I Introduction
Human drivers utilize various visual cues to perform driving tasks, e.g. lanekeeping and lanechange, which are the fundamentals for some more complex driving maneuvers. Researchers from different areas have investigated this visualmotor behavior for a long period. However, with the development of related research, it turned out that using visual cues for lanekeeping (most research focused on lanekeeping alone) is a complex sensorymotor 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 lanechange path planner which needs to be optimal, timecritical 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 modelfree. They generate a parameterizedcurve 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 lanechange 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
Iia Vehicle Dynamics
In this paper, to balance modelling accuracy and computation complexity in path planning, we use kinematic bicycle model:
(1)  
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 frontwheel relative to the body frame, is the turning rate of the frontwheel which is also the control input and () is the length from the center of gravity to the frontaxle (rearaxle) of the vehicle.
IiB Orthonormal frame
Using the HD map, the centerline 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 righthanded orthonormal frame with the corresponding unit tangent vector. The reference line can be described as(2)  
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:
(3)  
In fact, this orthonormal frame of the vehicle is due to the projection from the state space of vehicle to a , where
(4) 
and
(5) 
where
(6) 
If is small, in the admissible range of front angle steering, can be fully controlled by . We define
(7) 
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
(8) 
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:
(9) 
and if the reference line is on the left side of the vehicle
(10) 
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
(11) 
we have
(12) 
as our vehicle speed based on the planned speed on the reference line.
Iii A Lanechange 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:
(13) 
Using the idea of sliding mode control, we have a manifold which we plan to keep our vehicle on and the manifold is
(14) 
where is a coefficient and it will be shown that can be a piecewise 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
(15) 
and
(16) 
It is seen that the new control is a function of the steering velocity of the front wheel . We can decompose into two parts:
(17) 
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
(18) 
and
(19) 
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:
(20) 
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
(21) 
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:
(22) 
Meanwhile, with the feedback control, the dynamics of the orientation difference between the vehicle velocity and the tangent vector of the shadow point is:
(23) 
and take the time derivative on both sides of above equation again we have:
(24) 
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 lanechange 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
(25) 
Based on the dynamics of , we have
(26) 
and
(27) 
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
(28) 
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 .
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).
In this section, we proposed a lanechange 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 lanechange path planning is a simple linear system optimal control problem. Thus, in the autonomous driving system, in lanechange 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 lanechange path planning and the lanechange 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
(29) 
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
(30) 
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
(31)  
By choosing different values of
and in different conditions of the road geometry and vehicle speed, we can have estimate the lanechange time easily. It is obvious, the lanechange trajectories generated by our path planner are asymmetric unlike the trajectories from the polynomials and this asymmetry is also consistent to humandrivers’ daily behavior.
Iv A Lane change controller using a Monocular Camera
Iva 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
(32) 
and value of the splay angle of the right edge of the lane is
(33) 
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.
The difference between the left and right splay angles can be indicated as
(34) 
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.
IvB 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 timestationary information, the time derivative of the splay angles can be utilized for this estimation.
(35) 
and
(36) 
where ( and are the axis coordinate of the left and right edges of the target lane) and
(37)  
Assuming and , and . We have the linear equation:
(38) 
Based on this linear equation, the estimation of vehicle pose are:
(39)  
IvC A Lane Change Controller
In lanechange, 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
(40) 
Using the idea from the lanechange path planner, we define the new error state as
(41) 
where are two positive coefficients and the steering front wheel control is
(42) 
Theorem IV.1
With the control , if , when , we have the splay angles balanced and .
With the control (42), the dynamics of is
(43) 
Assuming and , taking the derivative on both sides of above equation and linearizing the dynamics near ,
(44) 
It is seen that when , , is the unique stable equilibrium of . Also, the dynamics of can be linearized as
(45) 
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 .
V Conclusion
In this paper, we first proposed a lanechange path planner based on HD map. Later, a visionbased lanechange 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 lanechange planning. Also, the visionbased controller showed that in some simple scenarios, using only monocular information as guidance, human can perform lanechange maneuvers. First, there is an engineering challenge to extend the lane departure warning system which is using monocular camera to a lanechange(lanekeeping) 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 lanechange and what the values of the parameters are.
References
 [1] M. Kardar, G. Parisi and Y.C. Zhang, “Dynamic Scaling of Growing Interfaces”, Phys. Rev. Lett. 56(9), 889–892 (Mar 1986).
 [2] K. Takeuchi, M. Sano, “Universal Fluctuations of Growing Interfaces: Evidence in Turbulent Liquid Crystals”, Phys. Rev. Lett., 104:230601 (2010).
 [3] X. Wang, W. Zheng, L. Gao, “The universality classes in growth of iron nitride thin films deposited by magnetron sputtering”, Mater. Chem. Phys., 82:254–257 (2003).
 [4] M. Degawa, T.J. Stasevich1, W.G. Cullen, A. Pimpinelli, T.L. Einstein, E.D. Williams, “Distinctive Fluctuations in a Confined Geometry”, Phys. Rev. Lett., 97:080601 (2006).
 [5] J .Wakita, H. Itoh, T. Matsuyama, M. Matsushita, “Selfaffinity for the growing interface of bacterial colonies”, J. Phys. Soc. Jpn., 66:67–72 (1997).
 [6] A. Hansen, E. L. Hinrichsen, S. Roux, “Roughness of crack interfaces”, Phys. Rev. Lett., 66:2476–2479 (1991).
 [7] C. Tracy, H. Widom, “Integral formulas for the asymmetric simple exclusion process, Comm. Math. Phys., 279:815–844 (2008).
 [8] C. Tracy, H. Widom, “A Fredholm determinant representation in ASEP”, J. Stat. Phys., 132:291–300 (2008).
 [9] J. Yong, X. Zhou, Stochastic Controls, Hamiltonian Systems and HJB Equations, (SpringerVerlag, New York, 1999).

[10]
V. Mnih, K. Kavukcuoglu, D. Silver, etc, “Humanlevel control through deep reinforcement learning”,
Nature, 518, 529–533 (2015).  [11] E. Theodorou, J. Buchli, S. Schaal, “A generalized path integral control approach to reinforcement learning”, JMLR, 11(Nov): 3137–3181 (2010).

[12]
J. Han, W. E, “Deep learning approximation for stochastic control problems”,
NIPS Deep Reinforcement Learning Workshop, (2016). 
[13]
S. Liang, R. Srikant, “Why deep neural networks for function approximation?”
ICLR conference (2017).  [14] C. Beck, W. E, A. Jentzen, “Machine learning approximation algorithms for highdimensional fully nonlinear partial differential equations and secondorder backward stochastic differential equations”, arXiv:1709.05963v1 [math.NA].
 [15] J. Han, A. Jentzen, W. E, “Overcoming the curse of dimensionality: Solving highdimensional partial differential equations using deep learning”, arXiv:1707.02568v1 [math.NA].
Comments
There are no comments yet.