Lane changing trajectory planning and tracking control for intelligent vehicle on curved road

This paper explores lane changing trajectory planning and tracking control for intelligent vehicle on curved road. A novel arcs trajectory is planned for the desired lane changing trajectory. A kinematic controller and a dynamics controller are designed to implement the trajectory tracking control. Firstly, the kinematic model and dynamics model of intelligent vehicle with non-holonomic constraint are established. Secondly, two constraints of lane changing on curved road in practice (LCCP) are proposed. Thirdly, two arcs with same curvature are constructed for the desired lane changing trajectory. According to the geometrical characteristics of arcs trajectory, equations of desired state can be calculated. Finally, the backstepping method is employed to design a kinematic trajectory tracking controller. Then the sliding-mode dynamics controller is designed to ensure that the motion of the intelligent vehicle can follow the desired velocity generated by kinematic controller. The stability of control system is proved by Lyapunov theory. Computer simulation demonstrates that the desired arcs trajectory and state curves with B-spline optimization can meet the requirements of LCCP constraints and the proposed control schemes can make tracking errors to converge uniformly.

near-minimal energy-optimal method to determine the real-time collision-free path for a car-like vehicle. Chen et al. (2012) and Montes et al. (2007) used the Bézier curve to plan trajectory. A continuous smooth trajectory can be obtained by Bézier curve. Shang and Peng (2012) put forward two-lane cellular automaton model which was applied to lane changing with the turn signal effect. Horng et al. (2014) assumed that the vehicle can receive location information about nearby vehicles to perform analysis for lane changing. It simulated the traffic flow with different numbers of lanes. Feng et al. (2015) adopted the polynomial method to plan trajector. Moreover, collision detection was mapped into a parameter space by adopting infinite dynamics circles.
In process of lane changing, the vehicle can get state information according to different types of sensors. Then the kinematic controller and dynamics controller are constructed according to reference velocity and acceleration. Lin and Cook (1997) designed the vehicle lane changing controller which was based on two degrees of freedom dynamics model. The validity of controller was proved by optimal control theory. Naranjo et al. (2008) designed a fuzzy controller for vehicle lane changing under overtaking situation based on high precision GPS positioning system. The lane changing control of vehicle collision avoidance maneuver was explored by Kim et al. (2009). Rastelli and Penas (2015) applied the fuzzy logic technology to the vehicles' steering control. Hsu and Liu (2008) used lane changing control strategy of vehicle team based on vehicle kinematic model.
The current research on lane changing of intelligent vehicle is mainly based on straight road under the condition that the outside lane curvature and inside lane curvature are both zero, ignoring the effect of longitudinal velocity variation on trajectory. There is less research on lane changing of intelligent vehicle on curved road. This paper aims to study the problem of trajectory planning and tracking control about lane changing on curved road. Assuming that the outside lane curvature and inside lane curvature are greater than zero, the arcs trajectory can be designed. The backstepping technology is used to design the kinematic controller, and the dynamics controller based on slidingmode method is adopted to extend the kinematic controller. Finally, the effectiveness of the control scheme can be proved by simulation.

Kinematic model of vehicle
The intelligent vehicle kinematic model is shown in Fig. 1 in which the world XOY coordinate and local IMJ coordinate are established. M and N are the centroid of vehicle.
(x, y) is the actual position of vehicle in the world coordinate, (x r , y r ) is the reference position, δ is the angle between X-axis and I-axis which represents the motion orientation of the vehicle. We use q = x y δ T to represent the actual posture. Suppose that the vehicle is rolling purely without sideslip motion in the world coordinate, the constraint of the vehicle's motion (Jiang and Nijmeijer 1997) is described as Equation (1) can be represented by the form of constraint matrix where A(q) = sin δ − cos δ 0 is a vector associated with non-holonomic constraints, q = ẋẏδ T . On this basis, the kinematic model equation is established (Kanayama et al. 1990) where S(q) is the velocity transformation matrix, u = v ω T is the input velocity vector, v ∊ R and ω ∊ R denote the linear velocity and angular velocity respectively. Differentiating (3), we can obtain

Dynamics model of vehicle
The four wheeled intelligent vehicle is a non-holonomic nonlinear system. It is assumed that there is a symmetrical power on both of the left and right sides, the Lagrange equation of three free degrees is established where m is the mass of vehicle, I z denotes the moment of inertia. The dynamics of nonholonomic mechanical systems can be described by the differential equation (Campion et al. 1991) Substituting (5) for (6), the dynamics equation (Fukao et al. 2000) can be defined as where M(q) ∊ R 3×3 is the symmetric inertia matrix, V (q,q) ∈ R 3×3 is the centripetal and Coriolis matrix associated with the velocity and position, B(q) ∊ R 3×2 is the input transformation matrix, τ = τ l τ r T stands for the torque control inputs vector generated by the right and left driven wheels, λ is the Lagrange multiplier, q and q represent velocity and acceleration vectors, respectively. According to (2) and (3), S T (q)A T (q) = 0 can be derived. If both ends of (7) are premultiplied by S T (q), the constraint term A T (p)λ can be eliminated. Meanwhile, substituting (3) and (4) for (7), we can get a dynamics equation in term of internal velocities where M (q) = S T (q)M(q)S(q) is the matrix of system state, V (q,q) = S T (q) M(q)Ṡ(q) + V (q,q)S(q) means the matrix of lateral deflection, B (q) = S T (q)B(q) denotes the input transformation matrix. In this paper we assume that the friction of surface is neglected and V (q,q) = 0, the variables in (8) are assumed as where r is the wheel's radius, b is half of the distance between two driven wheels.

Lane changing constraint
In recent years, researchers have divided the process of lane changing into two or three segments (Salvucci and Liu 2002;van Winsum et al. 1999). In the process of lane changing, if the starting position of the vehicle is fixed and the finishing position can be arbitrarily setting, it is called lane changing in theory (LCT). With the consideration of the intelligent vehicle dynamics constraints and the characteristics of lane changing on curved road, the constraints of lane changing on curved road in practice (LCCP) are proposed Constraint 1 The trajectory of vehicle, velocity and acceleration curve should be continuous smooth curves. An abrupt change of trajectory and state curve is not allowed. The examples of LCT and LCCP are shown in Fig. 2. O is the starting position while P is the finishing position. There are many paths from O to P. O → C → P is a path which has an abrupt curvature change. It does not meet the LCCP Constraint 1. In the extremum of curvature appears in the starting position. It does not meet the LCCP Constraint 2. O → A → P and O → B → P are the paths that can be achieved in practice. These two paths can satisfy the LCCP constraints. In addition, there are a great many other paths which can satisfy the LCCP constraints. In this paper, we mainly research the paths composed of two segments arcs which like

Trajectory planning
The desired lane changing trajectory is shown in Fig. 3. The position of the vehicle at the starting time is O and the finishing position is P. The inside and outside lane have the same instantaneous center O R . The curvature radius of outside lane is R 1 and the curvature radius of inside lane is R 2 . Assuming that the desired trajectory is composed of two segments arcs which have the equal curvature radius ρ. The arcs instantaneous center is O 1 and O 2 respectively. These two arcs are tangent in M, α is the rotation angel between OO R and PO R . Suppose that the centroid of the vehicle moves to the point C at time t along desired trajectory. If the C is in OM segment, the trajectory is shown in Fig. 4 where x r (t) and y r (t) represent the longitudinal and lateral displacement respectively, θ(t) is the angel of the vehicle centroid around instantaneous center O R , ψ(t) denotes the angel of vehicle centroid around instantaneous center O 1 , ψ(t) satisfies When the vehicle is in OM segment, the desired displacement, velocity, and acceleration can be calculated as

Fig. 4 Lane changing on OM segment
In desired trajectory state Eqs. (12)- (14), the θ(t), θ (t) and θ (t) are unknown functions. In this paper, polynomial method (Feng et al. 2015) is adopt to solve these functions. Assuming that θ(t) satisfies quantic polynomial where c i , i = 0, …, 5 represent undetermined coefficients. Taking the derivative of θ(t) with respect to time, the angular velocity and angular acceleration can be obtained Given θ(t), θ (t) and θ (t) in starting and finishing position satisfy equations where t 0 = 0 is the starting time and T is the finishing time, v x (·) and v y (·) are the lateral and longitudinal velocity respectively, a x (·) and a y (·) denote the lateral and longitudinal acceleration respectively. The coefficient vector C = [c 0 , c 1 , c 2 , c 3 , c 4 , c 5 ] T can be obtained by solving linear Eqs. (15) and (16) with given conditions (17)-(19). Then θ(t), θ (t) and θ (t) in (12)-(14) can be gotten.
If C is in MP segment, the trajectory is shown in Fig. 5. The given length between O 1 and O 2 is 2ρ, and the length between O R and O 2 is R 2 + ρ. According to this, the coordinates ofO 2 can be calculated as ((R 2 + ρ)sin α, R 1 − (R 2 + ρ) cos α). Assuming that the vehicle moves to M point at the time t 1 , ф(t) denotes the rotation angel of vehicle centroid around instantaneous center O 2 which satisfies ψ(t 1 ) and θ(t 1 ) can be calculated as where c i , i = 0, …, 5 have been solved in (15) and (16), x r (t 1 ) = ρ sin [ψ(t 1 )], y r (t 1 ) = ρ − ρ cos [ψ(t 1 )]. The given arc length L from O to M is ψ(t 1 ) × ρ where ψ(t 1 ) can be obtained by (21), and L also satisfies Furthermore, t 1 can be solved by (12), (21) and (22). When the vehicle is in MP segment, the desired displacement, velocity, and acceleration can be calculated as In (23)-(25), v x (t) and v y (t) are unknown functions. We also use the polynomial method to solve these functions. Assuming that v x (t) and v y (t) satisfies cubic polynomial where a i , i = 0, …, 3 and b i , i = 0, …, 3 represent the undetermined coefficients. Taking the derivatives of polynomials in (26) with respect to time, one can obtain , v y (T ), t 0 = 0 and T, the coefficient vector A = [a 0 , a 1 , a 2 , a 3 ] T and B = [b 0 , b 1 , b 2 , b 3 ] T can be obtained by solving polynomials (26) and (27). Then v x (t) and v y (t) in (23)-(25) can be gotten.
With the requirement of vehicle trajectory tracking, the reference yaw angle and yaw angular velocity which are adopted for the input of kinematic trajectory tracking controller can be determined as

Design of kinematic controller
Assuming that the reference posture of vehicle is q r = x r y r δ r T and the actual posture of vehicle is q = x y δ T . The posture tracking error (Kanayama et al. 1990) can be described as Differentiating (30), a differential equation tracking error can be obtained The problem of trajectory tracking is to design the control input u c = v c ω c T to make sure that the posture tracking error q e = x e y e δ e T converge to zero, i.e. lim t→∞ [|x e (t)| + |y e (t)| + |δ e (t)|] = 0.
The nonlinear system can be decomposed into several subsystems. The Lyapunov function and virtual control variables for each subsystem can be defined and the vehicle kinematic controller is designed which makes the tracking errors to converge uniformly.

Theorem 1 Posture tracking error (30) will asymptotically converge to zero vectors if kinematic trajectory tracking controller
is applied where λ, k 1 , k 2 , k 3 and k 4 are positive constants, ω c and δ e are defined as ω c =ω r + 2k 3 v rẏe +v r y e cos δ e 2 − k 3 v r y eδe sin δ e 2 + 1 2 k 4δe cos δ e 2 Proof Since the variable y e in (30) is indirectly controlled, a virtual error x e is defined as where k 1 ∊ R + , λ ∊ R + and k 1 ω c 1+e −ωc y e is the virtual feedbacks. Differentiating (35), the following equation can be obtained where ẏ e = −ω c x e + v r sin δ e and ẋ e = ω c y e − v c + v r cos δ e . Let Lyapunov function is defined as Substituting (35), (36) for differentiation of (37), we obtain Substituting (32) for (38), we obtain It is obvious that the Lyapunov function V 1 ≤ 0. V 1 is a continuous differentiable uniformly function, and V 1 is a negative semi-definite continuous uniformly function. According to the Baralat's lemma (Brockett 1983), it will know that V 1 → 0 when t → ∞ such that x 2 e , ω 2 c 1+e −ωc y 2 e and sin 2 δ e 2 converge to a zero vector. Considering that the (34) δ e = 2k 3 v r y e cos δ e 2 + k 4 sin δ e 2 (35) k 3 sin 2 δ e 2 ω c does not converge to zero, it means that lim t→∞ ω 2 c 1+e −ωc y 2 e = 0 when lim t→∞ y e = 0. Because lim t→∞x 2 e = 0 and lim t→∞ y e = 0, it is obvious that lim t→∞ x e = k 1 ω c 1+e −ωc y e and lim t→∞ x e = 0. Taking into account the fact that δ e is the practical angle error, the periodicity can be neglected. Hence it can be known that δ e ∊ [0, 2π). Because lim t→∞ sin 2 δ e 2 = 0 and δ e ∊ [0, 2π), so lim t→∞ δ e = 0. Under the action of controller (32), the posture tracking error q e = x e y e δ e T is uniformly bounded and lim t→∞ [|x e (t)| + |y e (t)| + |δ e (t)|] = 0, system is asymptotically stable.

Design of dynamics controller
The sliding-mode control (Utkin 1977) is a kind of nonlinear control method which can overcome the external disturbance. The state-feedback control law can switch from one continuous structure to another based on the current position in the state space. Because of its good robustness, the sliding-mode control technique is suit to track the trajectory of intelligent vehicle. In this paper, the sliding-mode dynamics controller is designed to make the actual velocities of intelligent vehicle converge to the control velocities generated from the kinematic controller. The error between the actual velocity and kinematic control input is introduced where u = [v, ω] T represents the actual velocity, u c = [v c , ω c ] T is the control input associated with the kinematic controller. We select PI-type sliding surface s(t) as where β > 0 is the sliding-surface integral parameter. Taking the derivative of s(t) with respect to time, one can be obtained It is desired that the controller can make nonlinear system reach the sliding surface s(t) = 0. By using sliding-mode surface function, the trajectory tracking controller is derived.
Theorem 2 Considering posture tracking error (30) and dynamics Eq. (8). If we design controller the closed-loop system is asymptotically stable, where ρ 1 , ρ 2 and k 5 are positive constants.
Proof The dynamics Lyapunov function can be defined as  (46) it can be concluded that V 2 is the negative semi-definite. It is noted in (41) that u e = −β t 0 u e dt if the system is on the sliding surface s(t) = 0. It is obvious that the tracking error lim t→∞ u e = 0 as β > 0. By dynamics controller (43), the system is asymptotically stable. This completes the proof of the theorem.

Simulation of trajectory planning
In order to verify the effectiveness of the trajectory planning, simulation experiments are performed. The simulation environment is based on Matlab. Assuming that the parameters of the vehicle are set as follows: the curvature radius of the outside lane R 2 = 121 m and the curvature radius of the inside lane R 1 = 100 m, the beginning time t 0 = 0 s and the finishing time T = 18 s, the curvature radius of trajectory arcs ρ = 60 m, α = 0.7 rad . The initial state values of vehicle are shown in Table 1.
The desired trajectory, velocity and acceleration curves are shown in Fig. 6. Figure 6a and b are the displacement along X-axis and Y-axis, Fig. 6c and d are the velocity curve along X-axis and Y-axis and Fig. 6e, f is the acceleration curve.
The solid line in the Fig. 6c-f is the actual velocity and acceleration curve and the dotted line is the optimized velocity and acceleration curve using for B-spline. The red 'O' represents the B-spline control points. The actual velocity and acceleration curve have abrupt curvature change in the process of lane changing. It does not meet the LCCP constraint 1. We adopt the B-spline control theory to create a smoothness curve. In 2003, Kano et al. put forward the B-spline theory which can be used for curve-fitting and numerical differentiation. The piecewise continuous B-spline function can be defined as  where k = 0, 1, …, m, P j , j = 0, 1, …, n + m are the control points, u ∊ [0, 1], F k,n (u)is the basis function. The B-spline function (47) means the k piece n-degree B-spline curve.
Noting that the desired trajectory state Eqs. (12)- (14) and (23)-(25) are third-degree derivable, the cubic spline function can be set as the basis function which can be represented by the form of matrix According to (47) and (48), the B-spine optimization curve can be calculate as where P 0,i,x … P 3,i,x are the horizontal coordinates of the B-spline control points, P 0,i,y … P 3,i,y are the vertical coordinates of the B-spline control points. After optimization of the spline function, the velocity and acceleration curve can avoid the occurrence of the abrupt curvature change which meets the requirements of the intelligent vehicle dynamics.
The lane changing trajectory is shown in Fig. 6g. The dotted line is the center line of the two lanes and the solid line is the actual lane changing trajectory.

Simulation of trajectory tracking
In this section, lane changing trajectory is applied to the reference trajectory which is designed in "Trajectory planning" section. Simulation is designed to show the effectiveness of designed kinematic controller (32) and dynamics controller (43).
Simulation results are shown in Fig. 7. Figure 7a describes the actual and reference trajectory. The two trajectories are almost coincident by means of the kinematic and dynamics controller. The tracking errors are shown in Fig. 7b which converges to zero asymptotically. The torque control input of the left and right wheels are shown in Fig. 7c and d. The torque control input converges to a stable range. The linear velocity and angular velocity are shown in Fig. 7e, f. From these results, we can confirm the effectiveness of the proposed method. P 0,i,x P 0,i,y P 1,i,x P 1,i,y P 2,i,x P 2,i,y P 3,i,x P 3,i,y   

Conclusion
This paper considers lane changing trajectory planning and tracking control for intelligent vehicle on curved road. With the consideration of vehicle dynamics constraints, LCCP constraints are proposed to validate the effectiveness of desired trajectory. Then the arc trajectory with the same curvature is designed to implement lane changing on curved road. Kinematic controller based on backstepping method and sliding-mode dynamics controller are designed to implement the trajectory tracking control. The stability of the control system is proved by Lyapunov stability theory. Simulation results show that the controller can guarantee the convergence of the tracking error.