A Lane-Change Path Planner and its application with a monocular camera

by   Yunlong Huang, et al.

Human drivers utilize the visual cues from the road to performance some fundamental driving tasks, e.g. lane keeping and lane change, for the complex driving maneuvers. Lane keeping and lane change can be generalized as one task, because both of them are to drive a vehicle onto a target lane. In this paper, we first design a lane-change path planner based on HD (High-Definition) map for autonomous driving systems using control theory. Later, applying the similar idea, a lane change controller using a monocular camera is designed.



There are no comments yet.


page 1

page 2

page 3

page 4


Introducing Liveness into Multi-lane Spatial Logic lane change controllers using UPPAAL

With Multi-lane Spatial Logic (MLSL) a powerful approach to formally rea...

Learning Personalized Discretionary Lane-Change Initiation for Fully Autonomous Driving Based on Reinforcement Learning

In this article, the authors present a novel method to learn the persona...

ONCE-3DLanes: Building Monocular 3D Lane Detection

We present ONCE-3DLanes, a real-world autonomous driving dataset with la...

Hybrid tracker based optimal path tracking system for complex road environments for autonomous driving

Path tracking system plays a key technology in autonomous driving. The s...

Lane Departure Prediction Based on Closed-Loop Vehicle Dynamics

An automated driving system should have the ability to supervise its own...

Exploring OpenStreetMap Availability for Driving Environment Understanding

With the great achievement of artificial intelligence, vehicle technolog...

Phase Space Reconstruction Network for Lane Intrusion Action Recognition

In a complex road traffic scene, illegal lane intrusion of pedestrians o...
This week in AI

Get the week's most popular data science and artificial intelligence research sent straight to your inbox every Saturday.

I Introduction

Human drivers utilize various visual cues to perform driving tasks, e.g. lane-keeping and lane-change, which are the fundamentals for some more complex driving maneuvers. Researchers from different areas have investigated this visual-motor behavior for a long period. However, with the development of related research, it turned out that using visual cues for lane-keeping (most research focused on lane-keeping alone) is a complex sensory-motor coupling as different visual cues are leveraged instantaneously in a compound way. For instance, researches stated that all optical flow, bearing and splay angle are related to human driving.

Besides the research results from psychological experiments, there is an engineering challenge from control theory: we need to design a lane-change path planner which needs to be optimal, time-critical and adaptive to different conditions of the lanes (straight or curvy) and different vehicle states. In today’s most research and autonomous driving systems, the path planners for lane change are usually model-free. They generate a parameterized-curve from one lane to the other and this curve is optimized in the space of the parameters. In this kind of path planners, the choice of the ”control point” will have important impact over the properties of the planned paths, which makes the path planner inadaptive to different road geometries and vehicle states.

In Section II, the vehicle dynamics and some fundamental geometry knowledge about orthonormal frame will be introduced. We will design the lane-change planner using HD map and its properties will be shown in some simulation in section III. In section IV, we will apply the idea from section III to design a lane change controller using a monocular camera. The conclusion will be drawn and some further development of the ideas in this paper will be proposed in the last section.

Ii Vehicle Dynamics and Orthonormal frame

Ii-a Vehicle Dynamics

In this paper, to balance modelling accuracy and computation complexity in path planning, we use kinematic bicycle model:


where is the coordinate of the vehicle’s center of gravity, is the orientation of the vehicle frame, is the slip angle of the center of gravity, is the angle of the front-wheel relative to the body frame, is the turning rate of the front-wheel which is also the control input and () is the length from the center of gravity to the front-axle (rear-axle) of the vehicle.

Ii-B Orthonormal frame

Using the HD map, the center-line of each lane will be provided to us. We would like to use it as the reference line for path planning. Given the reference line and the vehicle state, assuming that the point on the reference line which is closest to the vehicle is unique (which is usually called the “shadow point”), let denote the position of the shadow point, let

denote the unit tangent vector along the reference line at the shadow point and let

denote the unit normal vector. We use the convection that a unit normal vector completes a right-handed orthonormal frame with the corresponding unit tangent vector. The reference line can be described as


where is the speed which can be planned by a longitudinal planner along the reference line and is the curvature.

Let denote the position of the vehicle, let denote the unit tangent vector, let denote the unit normal vector, is the speed of the vehicle, is the angular velocity of the orientation of the vehicle velocity. We have the orthonormal frame about the vehicle:


In fact, this orthonormal frame of the vehicle is due to the projection from the state space of vehicle to a , where






If is small, in the admissible range of front angle steering, can be fully controlled by . We define


to be the vector from the vehicle pointing to the shadow point which can be understood as the “ray” cast from the vehicle to its shadow. We assume that initially , where stands for the inner product of vectors. The time derivative of is


As the shadow point is the closest point on the reference line to the vehicle, and if the reference line is on the right side of the vehicle:


and if the reference line is on the left side of the vehicle


As we will have an independent planner for the longitudinal speed along the reference line, we can decide the associated speed of the vehicle. Given


we have


as our vehicle speed based on the planned speed on the reference line.

Iii A Lane-change Path Planner

The intuition of this lane change planner is: the orientation of the vehicle velocity will be pointed to the center line of the target lane and the difference between the orientation of the vehicle velocity and the orientation of the shadow point’s tangent vector is related to the lateral deviation of the vehicle away from the target lane. If in orientation of the shadow point’s tangent vector is , given that the orientation of vehicle velocity is , this intuitive idea is:


Using the idea of sliding mode control, we have a manifold which we plan to keep our vehicle on and the manifold is


where is a coefficient and it will be shown that can be a piece-wise constant function which can be varied based on the speed of the vehicle.

Based on this manifold, we define a new state of the vehicle referring to the reference line and a new control associated to it where




It is seen that the new control is a function of the steering velocity of the front wheel . We can decompose into two parts:


where is the control to stabilize the orientation difference at zero and is the control to have to converge to zero. The stabilization control is defined as




We have a linear system with state and a control , where describe the deviation of the vehicle away from the manifold . To minimize this deviation, we have a simple LQR (Linear Quadratic Regulator) problem:


where is a balance coefficient between the error and the steering effort to reduce the error. With the solution of the ARE (Algebraic Riccati Equation), we have the optimal control


With this optimal control, we would like to have the convergence of the lateral deviation of the vehicle away from the target lane with the following theorem.

Theorem III.1

With the optimal control , if , the deviation of the vehicle away from the target reference line will converge to zero.

With the optimal control , we have , which indicates that converges to zero exponentially with the rate and , where is the initial value of the error. With converging to zero, we have the state of the vehicle converge to the manifold:


Meanwhile, with the feedback control, the dynamics of the orientation difference between the vehicle velocity and the tangent vector of the shadow point is:


and take the time derivative on both sides of above equation again we have:


It shows that the near the equilibrium point , dynamics of and

have two eigenvalues

and . As in the range , is the unique equilibrium point. The orientation of the vehicle will converge to the orientation of its shadow point. Going back to the manifold , when , = 0. Therefore, with converging to zero, on the manifold (14) the deviation of the vehicle away from the target lane will converge to zero.

We see there are two independent coefficients whose value we can choose independently. For , most lane-change maneuvers happened on highway have the duration of seconds. As governs the exponential convergence rate at which the system converges to the manifold and the manifold is only the necessary condition for the lane change, we choose the around . Once the value of is fixed and the speed is known to be inside a range in a time duration, the dynamics of the lateral deviation is


Based on the dynamics of , we have




If we choose , the error term on the right side of above equation will decay faster than the dynamics of . The slow dynamics will be


With this dynamics, we can get rid of the oscillation during the lane change. This potential hazard oscillation is significant at low speed scenarios . Here we provide one simulation on this hazard oscillation. Given that the vehicle speed is meter/second and , there are three trajectories of the vehicle’s lateral position of different values: , and in figure 1 which shows that all these trajectories converging to the target lane (). In the plot of the time derivatives of these trajectories in figure 2, the hazard oscillation with high value of turns more significant. Thus, to avoid this oscillation in path planning, given a range of and a value of , we will choose such that .

Fig. 1: Comparison of low speed lane-change trajectories at different s
Fig. 2: Time-derivatives of low speed lane-change trajectories at different s

In the second simulation which is shown in figure 3, the planner generates a lane change trajectory in a circular course of radius at a high longitudinal speed along the reference line () for the vehicle starting from and . and . The planned speed along the vehicle will adjust its speed based on (12).

Fig. 3: Lane-change trajectory in a circular course

In this section, we proposed a lane-change path planner. This idea originated from different previous work: the orthonormal frame has been well introduced in the work by krishna; the sliding mode idea for the trajectory tracking has been introduced in protugal. In our work, we first showed that through a projection of the state space of the vehicle, we can have the orthonormal frame. It can be shown different vehicle model–unicycle model, kinematic bicycle model, dynamic bicycle model or even more complex model, with the projection operation, we can always use this orthonormal frame for path planning. Second, by decomposing the steering input of the vehicle into two components–one is for stabilizing the orientation difference between that vehicle and the road at zero and the other is for steering the vehicle onto the target lane, it is shown that the optimal control of the lane-change path planning is a simple linear system optimal control problem. Thus, in the autonomous driving system, in lane-change path planning, an optimization problem of long horizon () can be avoided. Most important contribution of this work is that using this path planner, there is no overshoot in lane-change path planning and the lane-change time is simply controlled by two parameters and . The converging rate of the vehicle to the target lane is upper bounded by and lower bounded by . With a new constant and an upper bound of the speed in a time duration, we have


At the beginning of the lane change , the reference line is switched from the current lane to a new target lane, there is a jerk in the steering input, it is seen that this difference is


where is the width of the lane. For the vehicle driving safety, the norm of the jerk should be bounded by which is the maximum jerk and also this should be bounded by maximum steering speed (where and are dependent on the road condition and the vehicle speed) as . The value of and are


By choosing different values of

and in different conditions of the road geometry and vehicle speed, we can have estimate the lane-change time easily. It is obvious, the lane-change trajectories generated by our path planner are asymmetric unlike the trajectories from the polynomials and this asymmetry is also consistent to human-drivers’ daily behavior.

Iv A Lane change controller using a Monocular Camera

Iv-a Splay Angle

Splay angle is the angle between the optical projection of the lane edge and a vertical line in the image plane. We use the ideas in the paper by Li Li. Please refer to that material for more details on the splay angles. In the scenario of straight lanes, we use the coordinate system where -axis is in parallel with the edges of the lanes, -axis is perpendicular to the edges of the lanes and -axis is orthogonal to the ground surface of the lanes. Given the -axis coordinate of the left(right) edge of a target lanes is , lateral deviations of the vehicle away from the left and right edges are and . The value of the splay angle of the left edge is


and value of the splay angle of the right edge of the lane is


where is the height of the observer and is the orientation angle of the vehicle and in the scenario of straight lanes it is the angle difference between the vehicle orientation and the path. The illustration of splay angles is in figure 4.

Fig. 4: Splay angles of left and right edges

The difference between the left and right splay angles can be indicated as


It shows that the difference between the splay angles are due to the difference between the vehicle’s lateral distances from the left and right edges of the target lane.

Iv-B Estimation of the vehicle pose

To utilize the same idea from last section to design a controller to change the lane, we need the estimation of the vehicle pose. In the scenario of the straight lanes, assuming the speed can be measured by other sensor and the vehicle parameters are known, the slip angle and the orientation of the vehicle are the information which we need to estimate based on the splay angles. As the splay angles are not time-stationary information, the time derivative of the splay angles can be utilized for this estimation.




where ( and are the -axis coordinate of the left and right edges of the target lane) and


Assuming and , and . We have the linear equation:


Based on this linear equation, the estimation of vehicle pose are:


Iv-C A Lane Change Controller

In lane-change, we have the target lane which we would like to drive on and the splay angles of the left and right edges are the guidance for the human drivers. To balance the left and right splay angles (i.e. ) by steering the front wheel, if which indicates , the vehicle will be steered to left edge (i.e. ) and if , the vehicle willbe steered to the right edge (). This indicates that is proportional to and the manifold is


Using the idea from the lane-change path planner, we define the new error state as


where are two positive coefficients and the steering front wheel control is

Theorem IV.1

With the control , if , when , we have the splay angles balanced and .

With the control (42), the dynamics of is


Assuming and , taking the derivative on both sides of above equation and linearizing the dynamics near ,


It is seen that when , , is the unique stable equilibrium of . Also, the dynamics of can be linearized as


When converges to zero, will also converge to zero, which indicates that , when . On the manifold (40), will approach to zero with .

Here we provide a simple simulation in figure 5 with the vehicle dynamics and the splay angles based on computation. The vehicle is at speed . The left edge of the target lane is at and the right edge is at . We have and . It is shown that the vehicle converges to the reference line .

Fig. 5: Lane-change trajectory using splay angles

V Conclusion

In this paper, we first proposed a lane-change path planner based on HD map. Later, a vision-based lane-change controller is designed using the similar idea. The path planner showed its versatility to different road geometric conditions and vehicle states. This path planner can be used in future research to investigate the safety in lane-change planning. Also, the vision-based controller showed that in some simple scenarios, using only monocular information as guidance, human can perform lane-change maneuvers. First, there is an engineering challenge to extend the lane departure warning system which is using monocular camera to a lane-change(lane-keeping) controller. Also, based on this controller, the psychological researchers can design new experiments to investigate whether human drivers also use this control law to do lane-change and what the values of the parameters are.