An optimal hierarchical framework of the trajectory

Vehicle System Dynamics International Journal of Vehicle Mechanics and Mobility

ISSN: 0042-3114 (Print) 1744-5159 (Online) Journal homepage: http://www.tandfonline.com/loi/nvsd20

An optimal hierarchical framework of the trajectory following by convex optimisation for highly automated driving vehicles Haotian Cao, Song Zhao, Xiaolin Song, Shan Bao, Mingjun Li, Zhi Huang & Chuan Hu To cite this article: Haotian Cao, Song Zhao, Xiaolin Song, Shan Bao, Mingjun Li, Zhi Huang & Chuan Hu (2018): An optimal hierarchical framework of the trajectory following by convex optimisation for highly automated driving vehicles, Vehicle System Dynamics, DOI: 10.1080/00423114.2018.1497185 To link to this article: https://doi.org/10.1080/00423114.2018.1497185

Published online: 26 Jul 2018.

Submit your article to this journal

View Crossmark data

Full Terms & Conditions of access and use can be found at http://www.tandfonline.com/action/journalInformation?journalCode=nvsd20

VEHICLE SYSTEM DYNAMICS https://doi.org/10.1080/00423114.2018.1497185

An optimal hierarchical framework of the trajectory following by convex optimisation for highly automated driving vehicles Haotian Cao a,b , Song Zhaoc , Xiaolin Songa , Shan Baob , Mingjun Lia , Zhi Huang and Chuan Hu d

a

a State Key Laboratory of Advanced Design and Manufacturing for Vehicle Body, Hunan University, Changsha,

Hunan, People’s Republic of China; b Human Factors Group, University of Michigan Transportation Research Institute, Ann Arbor, MI, USA; c Optimal CAE Inc., Plymouth, MI, USA; d Department of Mechanical Engineering, University of Texas at Austin, TX, USA ABSTRACT

ARTICLE HISTORY

This paper presents a hierarchical trajectory following framework for SAE-level 2/3 automatic driving by convex optimisation, which aims for daily common driving manoeuvres with low traffics on the highway, such as free driving, adaptively following and overtaking. More specifically, it includes several combined functionalities of lane keeping, adaptive following, active obstacle avoidance and speed adaptation. The trajectory-planning module would generate the optimal path and speed profiles for the structure road (e.g. highway) with different geometric shapes, by using the natural cubic spline and convex optimisation approach. While modules of trajectory following or adaptive following are realised by a linearised time-varying model predictive control method. It is expected that the proposed framework could improve the driving safety, as well as the driving efficiency and driving comforts simultaneously. Thus, typical scenarios are designed to verify the effectiveness of the proposed trajectory following framework. Simulation results demonstrate that it could successfully deal with daily manoeuvres on a straight or curved highway without human driver’s interventions.

Received 19 October 2017 Revised 24 May 2018 Accepted 26 June 2018 KEYWORDS

Trajectory planning; convex optimisation; linearised MPC; nonlinear vehicle dynamics; automated driving

1. Introductions Problems and motivation: According to the reports of the World Health Organization (WHO) in 2015, road traffic fatalities are a leading cause of death worldwide. Additionally, road traffic injuries are estimated to be the ninth leading cause of death across all age groups globally, and it is predicted to become the seventh leading cause of death by 2030 [1,2]. Further, according to Fatality Analysis Reporting System (FARS) of the USA Transportation’s National Highway Traffic Safety Administration (NHTSA), there were 22,441 passenger-vehicle occupants died in traffic crashes and 2.18 million passenger vehicle occupants injured in 2015 [3]. Thus, great concerns are expressed to public traffic safety issues by the government, institute and industry field. However, most traffic accidents could be CONTACT Xiaolin Song State Key Laboratory of Advanced Design and Manufacturing for [email protected] Vehicle Body, Hunan University, Yuelu District, Changsha, Hunan Province 410082, People’s Republic of China © 2018 Informa UK Limited, trading as Taylor & Francis Group

2

H. CAO ET AL.

preventable. There exists considerable shreds of evidence on interventions that are effective at making roads safer, one of the solutions for reducing road fatalities is developing the intelligent transportation systems, or automatic driving vehicle. Now, one line of the vast majority of research projects centred on Advanced Driver Assistance Systems, the next step in the development of intelligent vehicles or transportation systems points toward the fully automatic driving vehicles [4]. Nevertheless, mimicking human driving is a challenging task for the automatic driving vehicle, and trajectory following is a vital one among numerous subsystems of the automatic driving vehicle, since it ensures that the ego vehicle could safely follow the planned path. Trajectory following has already received a broad of attention since from the 1980s in last century, which is usually considered as a local optimal problem [5]. There already exists various control methods for the trajectory tracking, and preview control strategy is well known and widely applied. For example, MacAdam presents an optimal single-point preview steering controller to minimise the lateral tracking errors [6,7], while Sharp et al. [8,9] present a concept of multi-point preview via linear quadratic regulator approach. What’s more, an LQ (linear quadratic)-preview control law for lateral steering of a passenger vehicle is proposed in [10]. However, there still exist several aspects to improve the trajectory following model: (i) First, when the vehicle is in free driving, generally, raw reference path profiles are directly adopted in most of the previous studies, there is usually no optimisation processing for the target path profiles. Nevertheless, path quantities such as the curvature could significantly affect the path tracking quality. (ii) Second, most known path-tracking controllers in automobile applications are simply based on a 2DOF linear bicycle model with the longitudinal speed remaining unchanged [11,12]. In order to deal with a situation where velocity changes with time, a linear time varying (LTV) MPC model based on 2DOF vehicle model is derived for the tracking problem [13–16]. However, a nonlinear vehicle model could be utilised to offer better state prediction, especially when the vehicle is in a high nonlinearity situation (e.g. double lane change (DLC) manoeuvre with high speed). (iii) Third, there is usually no velocity-planning modules integrated in the trajectory following, and velocity planning usually appears in applications where longitudinal control is needed, such as Adaptive Cruise Control, Collision Mitigation System with evasive manoeuvre assist [17] or at intersection left turn [18], but it is quite few for lateral tracking as far as we knew. Since the vehicle velocity would vary with traffic scenes (e.g. the vehicle usually decreases the speed when entering a circular lane), it matters for the vehicle moves at a proper speed. Related work: Currently, model predictive control (MPC) is a mainstream approach for autonomous vehicle’s trajectory following, more research work focusing on trajectory tracking via MPC could be found in [19,20]. What’s more, Lefèvre et al. [21] proposed a learning-based driver model that can represent human driving control strategies on the highway, where the driver model is MPC-based. Liu et al. [22] utilised a nonlinear MPC for obstacle avoidance for high-speed autonomous ground vehicles in unstructured environments. Ghazali et al. [23] integrate the rollover prevention into the MPC path following controller via front steering and braking. MPC could be also implemented in models of

VEHICLE SYSTEM DYNAMICS

3

intermittency in cognitive processing while examining the compensatory component of a human driver’s steering control [24]. It is even applied to the shared control strategy to deal with semi-automatic driving or assistance driving, for example, Na et al. focus on modelling of strategic interactions between the human driver and the vehicle active front steering (AFS) controller in a path-following task by LQ game and non-cooperative MPC methods [25,26]. In addition, control strategy with modelling uncertainty for trajectory following is more and more valued recently, for example, Guo et al. [27] presents an adaptive fuzzysliding mode control (SMC) strategy used for lateral control of vision-based automated vehicles; while Cao et al. [28] develop an adaptive preview time SMC steering controller when considering with model parameter uncertainties. Furthermore, trajectory planning is also an essential part because it obtains feasible trajectory profiles for the trajectory following controller. Nilsson et al. [29] show that the obstacle avoidance path planning problem could be also formulated as a convex optimisation problem within a receding horizon control framework (e.g. MPC), as the minimisation of the deviation from a desired velocity and lane, subject to a set of constraints introduced to avoid collision with surrounding vehicles. There also exists some other methods for trajectory planning, for example, Li et al. [30] utilised the cubic B-spline curve to interpolate the reference path, and adopted trapezoidal velocity profiles for autonomous urban driving. You et al. [31] proposed a polynomial-based method for trajectory planning, additionally, collision detection was mapped into a parameter space by adopting infinite dynamic circles. Werling et al. [32] utilised discretised terminal manifolds to deal with the trajectory generation problem faced by an autonomous vehicle in moving traffic. Moreover, Junsoo et al. [33] present a probabilistic approach for improving the motion planning performance of autonomous driving. Contributions and organisations: Based on the motivations and related work introduced above, it is reasonable to come up with ideas for trajectory following in a more completed framework. More specifically, we intend to present a hierarchical architecture of trajectory following for automatic driving vehicle to deal with daily common driving scenes, such as free driving, adaptive following or overtaking scenarios, on different shapes of the structured road (e.g. highway). It targets to enhance the driving safety, improve the driving efficiency and driving comforts as well. Apparently, the proposed framework includes trajectory planning and following. Main contributions and novelties in our study are as follows: (i) We proposed a novel trajectory planning (path and velocity profiles included) method that is based on the natural cubic spline and convex optimisation method. Theoretically, it could be adapted to any different shapes of structured road, and generate optimal trajectory profiles with low computation costs for vehicle’s tracking problem when dealing with manoeuvres such as lane keeping, overtaking etc. (ii) We developed a trajectory following controller based on linearised time-varying MPC technical for automatic driving. The novelty is that, different from most of the previous studies, the controller is based on a 5DOF nonlinear vehicle model, which is expected to offer a better state prediction accuracy. More importantly, it could track the target path and velocity simultaneously. Thus, there is no need to design more controllers to track them separately.

4

H. CAO ET AL.

(iii) Finally, we present a completed hierarchical trajectory following framework, which integrates the functionalities of adaptive following, lane keeping, active collision avoidance, and speed adaptation for automatic driving vehicles. It is composed of three layers, namely, the tactical layer, the planning layer, and the control layer. The proposed framework will be proved capable of dealing with easily encountered scenarios with low traffics on a highway. In addition, according to the SAE definition on automated driving [34], though contents in this study are mainly targeted to the level 2 or 3, however, the proposed framework could be easily modified and applied for even higher levels (such as level 4 or 5). The rest contents of this paper are reminded as follows: Section 2 introduces a detailed path planning and velocity planning method that is based on the natural cubic spline and convex optimisation, what’s more, a velocity planar for adaptively following is also introduced. Section 3 mainly focuses on the trajectory following controller design by an LTV–MPC approach. Moreover, Section 4 presents a competed hierarchical trajectory following framework for highly automatic driving vehicles, and the proposed framework would be validated in Section 5. Finally, conclusions and future work are drawn in Section 6.

2. Trajectory planning by convex optimisation The trajectory following architecture for automatic driving includes the path planning and speed planning. In this section, the trajectory planning is realised by the natural cubic spline and the convex optimisation method, which would be interpreted in details in the following contents. 2.1. Optimal path planning The emergence of global navigation satellite systems (e.g. GPS, Galileo, BeiDou etc.) and the usage of the digital map have enabled tremendous development in vehicular navigation for various applications. Global Positioning System (GPS) is one of the most frequently used for vehicle positioning. However, generally speaking, the raw GPS data have large estimation errors and cannot be directly used. Therefore, there has been much work conducted to improve the positioning accuracy of the GPS. For example, a method based on ANC principles and wavelet de-noising was proposed to decrease the random noise from GPS data [35]. Recently, multi-sensor information fusion methods are widely applied to afford accurate information of the road structure and obstacles. For example, considering visionbased sensing systems can provide relatively accurate lateral measurements and wealth of other information from images, Shi et al. [36] put forward a real-time GPS-Supported Simultaneous Localisation and Mapping (SLAM) algorithm with the camera. However, an inevitable issue of this method was the accumulated error with the time, and this challenge could be addressed by taking the map data into consideration [37]. The advantage of map data is that it could improve the perception of the surrounding environment and offer certain constraints for localisation results. As is shown in [38], the low-cost GPS data and map data were fused to determine a likely position of the vehicle, the longitudinal and lateral position information of higher accuracy was provided on the basis of traffic sign


5

Figure 1. Illustration of the road environment.

detected by cameras. Besides, the radar can detect all objects including vehicles, trees, and fences, and the useless objects can be eliminated to identify the required targets [39]. In addition, Dahmani et al. [40] proposed an approach to estimate vehicle dynamics and the road curvature in order to detect vehicle lane departures. Therefore, we assume that the road centreline, road width, and curvature could be obtained correctly by sensor fusion technical in our study. Supposing the road centreline could be constructed by an appropriate parametric function respecting to the travelled station s, then the position vector on centreline Rc could be donated by, fX (s) xc (s) = . (1) Rc = yc (s) fY (s) Define a unit vector in the road centreline normal-tangential coordinates system [41], which is, et =

y (s) x c (s) ex + c ey , Rc Rc

en = −

y c (s) x c (s) ex + ey . Rc Rc

(2)

Then as shown in Figure 1, an arbitrary point on the road could be expressed by, R(si ) = Rc (si ) + Li en (si ) = (xc (si ) + Li χi (si ))ex + (yc (si ) + Li ϑi (si ))ey , where Li represents one point’s nominal distance to the centreline, and, χi (si ) = −

y ci (si ) x ci (si ) , ϑ (s ) = . i i Rc Rc

(3)

6

H. CAO ET AL.

Assuming that the path profile is composed of n + 1 sets of (x, y) position data, which could be generated based on the road centreline. For equally distance spaced reference points along the road centreline, corresponding points’ position on the road could be donated as a matrix form, namely, x = xc + diag[χi ]L, y = yc + diag[ϑi ]L,

(4)

where L = [L0 , L1 , . . . , Ln ]T denotes a vector of each point’s nominal distance to the ceneerline, it is regarded as the control variable for this optimal path planning. In order to generate an optimal path profiles for driving, several criteria should be met in daily driving scenarios: (1) Lane keeping: it is one of the most common scenes for daily driving, and the vehicle would follow a path where it keeps a fixed lateral distance to the road centreline. (2) The shortest path: the covered distance of the vehicle would be the shortest. (3) Optimal curvature: the curvature of the path should be smooth and try to avoid large lateral acceleration occurred. (4) Path heading: the planned path should have a similar heading orientation with the road centreline. We will set them up with the following contents: Lane keeping: assuming that the vehicle is moving with a desired lateral distance to the centreline, which is denoted by Ld , thus, a quadratic term B2 could be defined as, B2 =

n

(Li − Ld )2 = LT L − 2LTd L + cost,

(5)

i=1

where cost is an extra term that is independent of the decision vector L. The shortest path: As the adjacent point interval could be calculated via, xi = (xc,i+1 − xc,i ) + (χi+1 Li+1 − χi Li ) = xci + α i L¯ i yi = (yc,i+1 − yc,i ) + (σi+1 Li+1 − ϑi Li ) = yci + β i L¯ i T α i = −χi χi+1 , β i = −ϑi ϑi+1 , L¯ i = Li Li+1 .

(6)

Then a squared term S2 in terms of the total length of the trajectory is, S2 =

n

((xi )2 + (yi )2 ) =

n T (L¯ i (α Ti α i + β Ti β i )L¯ i + 2Ti γ i L¯ i + Ti i ),

i=1

(7)

i=1

where i = xci

T

yci , γ i = α i

T β i , and neglect the constant term, then we have,

T

S2 = L¯ diag[α Ti α i + β Ti β i ]L¯ + 2T diag[γ i ]L¯ + T .

(8)

Set L¯ = gL, where g is a constant matrix is, finally, it is yielding: S2 = LT KL + ML + cost.

(9)


7

With K = g T diag[α Ti α i + β Ti β i ]g, M = 2g T diag[γ i ]g. Optimal curvature: In order to ensure the trajectory would be smooth and feasible, it is assuming that a natural cubic spline connects each adjacent points, namely, xri (s∗ ) = a0i + a1i s∗ + a2i s∗ 2 + a3i s∗ 3 , (10) yri (s∗ ) = b0i + b1i s∗ + b2i s∗ 2 + b3i s∗ 3 , where s∗ = (s − si0 )/(s) ∈ [0, 1]. The course curvature squared K2i is determined according to the following expression ⎡

2

2 ⎤ ∗ 4 2 2 ∗ ∗ ds ⎣ d xri (s ) + d yri (s ) ⎦ . Ki 2 (s) = (11) ds ds∗ 2 ds∗ 2 Neglecting the multiplier, then the whole segment’s curvature quality K2 is, ⎡

2

2 ⎤ n 2 2 ∗ ∗ ⎣ d xri (s ) + d yri (s ) ⎦ . K2 = ds∗ 2 ds∗ 2 i=1

(12)

Considering the second derivative for a natural cubic spline [42] computed at s∗ = 0, which could be donated by a linear form with a constant matrix H (refer Appendix 1 for details), such that, d2 x(s∗ ) = Hx = Hxc + Hdiag[χ i ]L. (13) ds∗ 2 ∗ s =0

Then, d2 x(s∗ ) ds∗ 2

∗

2

= xTc (HT H)xc + LT (HT H)diag[χ 2i ]L + 2(xTc HT H)diag[χ i ]L.

(14)

s =0

Similarly, we could obtain an expression for the y-coordinate, which is, 2 d2 y(s∗ ) = yTc (HT H)yc + LT (HT H)diag[ϑi2 ]L + 2(yTc HT H)diag[ϑi ]L. ds∗ 2 ∗

(15)

s =0

Notice that diag[χ i 2 ] + diag[ϑi 2 ] = diag[1], then K2 could be also expressed as a quadratic form that is dependent of the decision vector L, namely, K 2 = LT NL + TL + cost,

(16)

where, N = HT H T = 2(xTc HT H)diag[χ i ] + 2(yTc HT H)diag[ϑi ]. Path headings: The heading of the planned path should be consistent with the lane centreline’s heading, so that the vehicle can move forward in a proper direction, and the path

8

H. CAO ET AL.

transition could be smoother as well. The heading of the path could be approximated by, Li − Li−1 Li − Li−1 ≈ , (17) s s where s denotes the distance gap between two adjacent points, finally, a quadratic term P2 respecting to all planning points could be evaluated as, n

n n−1 n 1 2 2 2 2 θi = Li + Li − 2 Li Li−1 P = (s)2 i=1 i=1 i=0 i=1 θi = arcsin

=

LT (w1 − w2 )T (w1 − w2 )L = LT QL, s2

(18)

where Q = (w1 − w2 )T (w1 − w2 )/s2 , and w1 , w2 are constant matrixes to satisfy the following relationships, w1 L = [L1 , L2 , . . . , Ln ]T , w2 L = [L0 , L1 , . . . , Ln−1 ]T . Finally, the path planning problem while vehicle in free driving could be expressed as a convex optimisation problem, which is, Minimize 2 = λ1 B2 + λ2 S2 + λ3 K2 + λ4 P2 . Subjecting to boundary bounds, Dr ≤ L ≤ Dl ,

(19)

where Dr , Dl above denote the left road boundary and right road boundary, respectively, and λ1 , λ2 , λ3 , λ4 are the weight constants for quadratic terms. 2.2. Optimal speed planning in the non-following mode When the host vehicle moves in a non-following mode, then the objective of the speed planning is to generate speed profiles to achieve an optimal time consuming, as well as the driving comforts within the safety constraints. Similar with the optimal path planning in the previous section, we also assume that the velocity of the adjacent nodes was connected via natural cubic splines in order to obtain smooth and feasible speed profiles, namely, v(s∗ ) = c0i + c1i s∗ + c2i s∗ 2 + c3i s∗ 3 .

(20)

We consider following criterions to ensure the vehicle moves in a safe and comfortable status. Maximum allowed velocity: It keeps the host vehicle moving within the maximum allowed speed to save the travelling time, which could be donated by a squared term as, where v = v0 v1 curvature, namely,

V 2 = (v − v max )2 = v T v − 2v Tmax v + cost, (21) T v2 · · · vn−1 vn , and vmax is calculated based on the road v max

= (diag[K])−1 μg,

(22)

where K denotes the path curvature, g is the gravitational constant and μ is the road adhesion coefficient.


9

Minimum jerk: It ensures the derivative of the longitudinal acceleration is small enough to achieve a good driving comfort, which is also denoted by a squared term as,

2 n 2 ∗) v (s d xi A2 = (s2 Ai )2 = = v Tx HT Hv x . ∗2 ds i=1 i=1 n

(23)

Finally, the combined cost function for velocity planning is defined by, 2 = ε1 V 2 + ε2 A2 = v Tx v x − 2ε1 v Tmax v x ,

(24)

where = ε1 I + ε2 HT H, and ε1 , ε2 are weight constants for quadratic terms. Furthermore, in order to guarantee a pure rolling motion to avoid the wheel skidding phenomenon, it is necessary to verify that the forces transmitted to the ground is smaller than the ground friction force. For simplicity, the total tyre force Ft is evaluated by, 2 2 Ft = Fxt + Fyt = m a2x + (Ku2 )2 ≤ μmg,

(25)

where Ft denotes the tyre force, Fxt , Fyt denote the x-component and y-component of the tyre force, respectively. ax denotes the longitudinal acceleration of the host vehicle. Square both sides on (25) and simplify, yielding, a2x ≤ μ2 g 2 − K2 u4 .

(26)

According to the conclusions from Appendix 1, the first derivative for a natural cubic spline computed at s∗ = 0, which could be donated by a linear form with a constant matrix G, ∗ namely dv(s∗ 2 ) ∗ = Gv. Therefore, (26) could be expanded and expressed as a matrix ds s =0 form for all reference points, which is, (Gv x )2 − μ2 g2 + diag([Ki2 ])[vi4 ] ≤ 0.

(27)

Finally, the velocity planning could be interpreted by a convex optimisation problem as well, namely, Minimize 2 = v Tx v x − 2ε1 v Tmax v x .

(28)

Subjecting to v min ≤ v x = Wv x − u0 E ≤ v max Nonlinear constraints, (Gv x )2 − μ2 g2 + diag([Ki2 ])[vi4 ] ≤ 0 And boundary bounds, v min ≤ v x ≤ v max

⎡

1 ⎢−1 ⎢ Where W = ⎢ . ⎣ .. 0

0 1 .. .

... ... .. .

0 0 .. .

0

···

−1

⎤ 0 0⎥ ⎥ .. ⎥ .⎦

1

(n+1)×(n+1)

⎡ ⎤ 1 ⎢0⎥ ⎢ ⎥ , E = ⎢.⎥ ⎣ .. ⎦ 0

and the solution

(n+1)×1

search could be realised by fmincon function in Matlab [43], and active-set method could be applied for solving the convex optimisation problem (28).

10

H. CAO ET AL.

Figure 2. Illustration of adaptive following.

2.3. Speed planning in the following mode The velocity planning method introduced in Section 2.2 is only suitable when the host vehicle’s velocity is independent of surrounding vehicles on the road. Obviously, it will not work when the host vehicle follows a leading vehicle in front since the speed would change accordingly with the leading vehicle. Thus, we shall develop a velocity-planning algorithm for the following scenario. Technically, it would be very similar to the Adaptive Cruise Control. Assuming that the host vehicle detects a leading vehicle ahead, according to the kinematic relationship between the host vehicle and the leading vehicle as illustrated in Figure 2, the headway error d and relative speed error v could be determined by, d = ddes − (sp − sh ) = (Lv + vh τd ) − (sp − sh ), v = vp − vh ,

(29)

Where the length of the vehicle is Lv , vp , vh denote the longitudinal speed of the leading vehicle and the host vehicle, respectively, τd is the desired time headway to the leading vehicle, while sp , sh are the travelled distance of the leading vehicle and the host vehicle, respectively. Relationship of (29) could lead to a dynamical system as, ⎤ · ⎢d⎥ ⎢ ⎥= 0 ⎣ · ⎦ 0 v ⎡

τ −1 d 0 ah + + a , −1 v 0 1 p

(30)

where ap , ah denote the longitudinal acceleration of the leading vehicle and the host vehicle, respectively. Thus, a state-space form (31) could represent (30), namely, x˙ = f (x, u),

(31)

d is the state of the system, and u = ah represents the control input, while Where x = v the acceleration of the leading vehicle ap would be regarded as a disturbance to the system. In respective of the adaptive following control, the host vehicle is supposed to maintain a safe headway to the leading vehicle with minimum energy costs. Thus, a cost function related to the headway error, the relative speed error and the longitudinal acceleration of


11

the host vehicle is defined as, np n c −1 2 2 (w1 d (k + i|k) + w2 v (k + i|k)) + w3 a2h (k + i|k), Jf = i=1

(32)

i=0

where w1 , w2 , w3 are the weight constants for headway error, relative speed error and the longitudinal acceleration of the host vehicle, respectively; and np , nc denote the prediction horizon and control horizon in MPC algorithm, respectively. Finally, the solution finding for minimising (32) could be solved as a convex optimisation problem, namely, minimize Jf ah

(33)

s.t. x(k + i + 1|k) = f (k + i|k, k + i|ah ), i = 0, . . . , np − 1 amin ≤ ah ≤ amax amin ≤ ah ≤ amax .

3. Trajectory tracking controller design by a linearised time-varying MPC approach In this section, we would design a trajectory following controller based on MPC approach. MPC is a well-known, effective control technical, which is achieved by optimising a finite time-horizon while only implementing the current timeslot. It owns the ability to anticipate future events and take control actions accordingly. In this section, a detailed linearised time-varying MPC controller derived from a nonlinear 5DOF vehicle model is presented to track the target planned path and velocity simultaneously. 3.1. Coordinate system description When the driver follows a road, generally speaking, the driver would processes the road information in perspective of driver’ view, which means it would be processed in a local driver coordinate system, as shown in Figure 3. Then a preview point (x, y) in the world coordinate system could be transformed into the local driver coordinate system (x , y ) by, x cos ψ sin ψ x − xv = , (34) − sin ψ cos ψ y − yv y where ψ denotes the yaw angle of the host vehicle; and (xv , yv )represents the coordinate of the vehicle centre of geometry (CoG) in the world coordinate system. 3.2. Vehicle prediction model for MPC implementation As shown in Figure 3(b), a five DOF single-track rigid body model that includes the vehicle longitudinal speed u, lateral speed v, yaw motion ψ and front/rear wheel rolling motion ωf , ωr will be considered for prediction in MPC algorithm. According to the Newtown

12

H. CAO ET AL.

Figure 3. The coordinate system and vehicle view description. (a) Coordinate system and (b) vehicle top view.

second laws, the motions of the vehicle are governed by the following nonlinear dynamical system [44]: 1 u˙ = (Fxwf + Fxwr ) + vr, m 1 v˙ = (Fywf + Fywr ) − ur, m ψ˙ = r, (35) 1 (aFywf − bFywr ), Iz 1 ω˙ f = (Twf − Fxwf rw ), Jwf 1 (Twr − Fxwr rw ), ω˙ r = Jwr where the vehicle has a lumped mass as m, and moment of inertia corresponding to Z-axis of the vehicle Iz . Fywf , Fywr denote the lateral force of the front wheels and rear wheels, respectively, and Fxwf , Fxwr denote the longitudinal tyre force of front wheels and rear wheels, respectively. The length from the front/rear axle to the vehicle CoG is denoted by a and b, respectively. Jwf , Jwr denotes the equivalent inertia of the front wheel and rear wheel, respectively, rw denotes the effective rolling radius of the wheels. Twf , Twr denote the torque acting on the front wheels and rear wheels, respectively, which could be a driving torque when in accelerating or a braking torque while in braking, for a rear-drive vehicle, which is, τ Tw (1 − τ )Tw if braking if braking Twf = , Twr = , (36) 0 if accelerating Tw if accelerating r˙ =

where 0 ≤ τ ≤ 1 denotes the braking torque distribution factor between the front wheels and rear wheels. When in a working condition with a large lateral acceleration, the vehicle demands more lateral force from tyres, however, the system would become highly nonlinear since the main


13

cause of this non-linearity comes from a limited available tyre lateral force afforded by the road surface. One common way of modelling the tyre force versus slip is to use the analogous Magic Formula tyre models under pure slip conditions [45], such that, Fxj (κj ) = μFzj sin(Cx arctan(Bx κj /μ)) , j ∈ {f , r}, Fyj (αj ) = μFzj sin(Cy arctan(By αj /μ))

(37)

where Fzj , j ∈ {f , r} denote the vertical loads of the front wheel and rear wheel, respectively, and Cx , Bx , Cy , By are the parameters of the MF tyre model. However, the vehicle might deal with a combined slip situation where the longitudinal speed would change, then the tyre force is a function with longitudinal slip κ, lateral slip α, vertical loads Fz and ground adhesion constant μ, namely, Fxwj = fx (κj , αj , Fzf , μ) , j ∈ {f , r}, Fywj = fy (κj , αj , Fzf , μ)

(38)

where κ denotes the longitudinal slip, and α denotes the sideslip angle of the tyre. The vertical tyre loads for front tyre and rear tyre could be approximated by, mgb + εFdrag , a+b mga Fz r = + (1 − ε)Fdrag , a+b Fz f =

(39)

where ε is the wind drag-down force distribution factor, the wind drag-down force acting on the vehicle is calculated by Fdrag = 12 ρCz Ad u2 , where ρ is the air density, Cz is the lift coefficient, and Ad is the frontal area of the vehicle. What’s more, let us define the subcomponent of the combined slip as, κ , 1 + |κ| tan α , σy = 1 + |κ|

σx =

(40)

where κ denotes the tyre longitudinal slip and α denotes the tyre sideslip angle. Further, the tyre sideslip angle αf , αr for front and rear wheels could be approximated by, v + ar δsw − , G u v − br , αr = − u

αf =

(41)

where the steering wheel angle is denoted by δsw , through a steering gear ratio G to result in front wheel steer angles.

14

H. CAO ET AL.

In addition, we also define the longitudinal slip of the tyre as, ⎧ r ω−u ⎪ ⎨ w κ = rw ωu− u ⎪ ⎩ rw ω

if rw ω ≤ u, otherwise,

(42)

where ω denotes the wheel angular speed. Then, the combined slip σ is evaluated by, σ =

σx2 + σy2 .

(43)

Sill, we need derive the tyre force (38) based on the pure longitudinal/lateral tyre forces (37), and if we want to use the combined slip quantities σ , we must have available the pure slip characteristics with σx and σy as abscissa. This could be simply realised by the following relationships, which is,

σx Fσ xj (σx ) = Fxj 1 − |σx | Fσ yj (σy ) = Fyj arctan

σy 1 − |σx |

, j ∈ {f , r}.

(44)

Finally, the longitudinal force and the lateral force with combined slip are calculated by, ⎧ σxj ⎪ ⎪ ⎨ Fxwj (κi , αi ) = σj Fσ xj (σj ) , j ∈ {f , r}. σyj ⎪ ⎪ Fσ yj (σj ) ⎩ Fywj (κi , αi ) = σj

(45)

3.3. Optimisation In order to deploy MPC method, at first, we have to reform (35) into a discrete form with a sample time Ts . With an arbitrary working point (x0 , u0 ), the nonlinear vehicle dynamic model could be linearised as a discrete linear time-varying state-space form by taking the first-order term of its Taylor expansion, such that, x(k + 1) = f v (x0 (k), u0 (k)) + A(k)(x(k) − x0 (k)) + B(k)(u(k) − u0 (k)) z(k) = g v (x0 (k), u0 (k)) + C(k)(x(k) − x0 (k)),

(46)

where the state variables x = [u, v, ψ, r, ωf , ωr ]T , input variables u = [δsw , Tw ]T , and the outputs z = [y, u]T , which includes the lateral position y and the vehicle longitudinal velocityu. A, B, C denote the Jacobian matrixes evaluated at the working point. Then the predicted outputs Z respecting to the dynamical system (46) could be calculated sequentially using the set of future control vector U and its current states, namely, Z = Fx(k) + U,

(47)


15

where, ⎡

z(k + 1|k) z(k + 2|k) z(k + 3|k) .. .

⎤

⎡

u(k) u(k + 1) u(k + 2) .. .

⎤

CA ⎥ ⎢ CA2 ⎥ ⎢ 3 ⎥ ⎢ ⎥ , F = ⎢ CA ⎥ ⎢ .. ⎦ ⎣ .

⎥ ⎢ ⎢ ⎥ ⎢ ⎢ ⎥ ⎢ ⎢ Z=⎢ ⎥,U = ⎢ ⎥ ⎢ ⎢ ⎦ ⎣ ⎣ u(k + Nc − 1) z(k + Np |k) ⎡ CB 0 0 ⎢ CAB CB 0 ⎢ 2 ⎢ CAB CB = ⎢ CA B ⎢ .. .. .. ⎣ . . . CANp −1 B

CANp −2 B

⎡

CANp −3 B

⎤ ⎥ ⎥ ⎥ ⎥, ⎥ ⎦

CANp ... ... ... .. . ...

0 0 0 .. .

⎤ ⎥ ⎥ ⎥ ⎥. ⎥ ⎦

CANp −Nc B

And Np , Nc are the predictive horizon and control horizon, respectively. Recalling the objective of the trajectory following model is to track the planned path precisely, meanwhile, keep the vehicle moving at a desired speed as well. That means, for a given reference signal r(k) at one instant k, the objective of the MPC algorithm is to ensure the predicted outputs as precisely close to the reference signal within the prediction horizon. Then the cost quadratic function is defined by, J˜ =

Np

2

(k, k)(Y ref (k + i|k) − Z(k + i|k)) +

i=1

N c −1

(k, k)U(k + i|k)2 ,

(48)

i=0

where (k, k) is a diagonal subcomponent weight matrix for the errors between the reference signals and predicted outputs. Y ref is the reference signal including the vehicle’s lateral position and its longitudinal speed. (k, k) is the weight matrix component for the predicted control input. According to Camacho and Alba [46], the optimal problem for MPC could be converted to a standard quadratic programming problem with the decision variable denoted by U, which is, 1 ˜ +G ˜ T U, minmize J˜ = U T HU (49) 2 ˜ G ˜ are compatible matrices and vectors in the quadratic programming problem, where H, in our case, those matrices are, ˜ = (2ΛT )T with Λ = Fx(k) − Y ref . ˜ = 2( + T ), G H

(50)

The input constraints are expressed in terms of U, U min ≤ U ≤ U max .

(51)

The incremental input constraints are umin ≤ u(k) = u(k) − u(k − 1) ≤ umax , where u(k − 1) is the previous control input, which is known for current time slot.

(52)

16

H. CAO ET AL.

Figure 4. MPC-based trajectory tracking controller structure.

The solution for the quadratic program problem could be solved by quadprog in Matlab [47], and the active-set algorithm is chosen in this paper. Finally, once the optimal control inputs are calculated, the driving torque acting on the wheels should be converted to the engine throttle position γ through the powertrain system, In our paper, it is simply evaluated by looking up a 2D table of the engine throttle position respecting to the engine speed and engine output torque Tw , namely, γ = MAPγ

ig i0 Tw u× , rw ig i0 η

for Tw ≥ 0,

(53)

where ig denotes the ratio of the gearbox, η denotes the efficacy of the gearbox, i0 denotes the ratio of the differential, and rw is the radius of the wheels. In addition, Figure 4 shows a detailed picture of the proposed trajectory-tracking controller associating with a pathplanning module.

4. Hierarchical architecture of the trajectory following for automatic driving In this section, an online hierarchical trajectory following architecture is presented, which integrates functionalities of the path planning, speed planning, path following and speed tracking. Figure 5 shows a completed picture of this proposed trajectory following framework for automatic driving. It is composed of three layers, namely, the tactical layer which decides a specific manoeuvre to be performed by the host vehicle, the planning layer which generates optimal trajectory profiles (including path and speed) for the host vehicle for a specific manoeuvre, while the control layer ensure the host vehicle would follow the planned trajectory correctly [48,49]. A very common strategy for host vehicle travelling on the highway could be interpreted as follows. Assuming that the lane-keeping manoeuvre is set as a default mode, at the very beginning, the host vehicle uses the equipped onboard sensors (radar, camera etc.) to identify whether obstacles are detected. If there were obstacles ahead detected, then the adaptive following or overtaking manoeuvre will replace the


17

Figure 5. Trajectory following scheme for automated driving.

default mode. A detailed description of the decision-making and specific tactical operations including the adaptive following and overtaking manoeuvre will be discussed in the following contents. 4.1. Driving mode As introduced above, there exists three basic driving modes, namely, the lane keeping, adaptively following and overtaking. The driving mode could be transferred from one state to another one if the transferring condition is satisfied, as illustrated in Figure 5, the details would be interpreted as follows: (1) Lane keeping: it is set as a default mode when the host vehicle does not detect any obstacles within its forward sensors’ range, or the time headway is longer than 5s. Then the host vehicle would be in a free driving mode and will travel along with the lane centreline. (2) Adaptive following: As its name indicated, adaptive following mode would make the host vehicle keep a proper distance gap to the leading vehicle (the functionality is very similar to Adaptive Cruise Control system). In our case, when the time headway is not enough ( < 2.5 s) to perform a safe lane change manoeuvre, or the adjacent lane is not available for a lane changing (obstacle vehicles are detected on adjacent lane in a close range), then the host vehicle would stay in adaptive following state and also travel along with the lane centreline. (3) Overtaking: A completed overtaking includes a process that the host vehicle moves to its adjacent lane (lane changing), then pass the obstacle vehicle (lane keeping to pass), and finally return to its original lane (returning back). The criteria for triggering a lane change is that the time headway of the host vehicle is longer than 2.5 s; meanwhile, the adjacent lane is available for performing a lane change manoeuvre. Details on the overtaking process will be discussed in Section 4.2.

18

H. CAO ET AL.

Figure 6. Procedures of overtaking manoeuvre.

Table 1. Procedure of generating lane-changing trajectory profiles. Start Inputs: obstacle vehicle road-coordinates (sobst , Lobst ) and velocity vobst , host vehicle road-coordinates (shost , Lhost ) and velocity vhost Step-1: Calculate the planning distance DLC = TTC × vhost , and generate a sequence of reference points sLC staring from host vehicle’s position with an incremental interval s (e.g. sLC = shost : s : (shost + DLC )). Step-2: Generate a sequence of initial path points PLC (sLC ) via a linear interpolation based on{(shost , Lhost ), (shost + 20, Lhost ), (shost + DLC − 20, −Lobst ), (shost + DLC , −Lobst )}. Step-3: Use the path planning method introduced in Section 2 to generate an optimal path and velocity profiles. End Note: TTC-Time to Collision, which equals the distance gap by the relative speed.

4.2. Overtaking manoeuvre Overtaking is the act of one vehicle going past another slower moving vehicle on a road with travelling in the same direction. As depicted in Figure 6, a completed overtaking manoeuvre is composed of three sub-phases. (1) Phase-1 (Lane changing-L.C.): If an obstacle vehicle is detected ahead at the same lane, and the criteria for a lane changing is met, then the trajectory-planning module would be triggered, which generates the feasible path and velocity profiles by the convex optimisation method described in Section 2. Then the host vehicle follows the planned path to move to the adjacent lane. The procedure for generating a lane-change profile is listed in Table 1. (2) Phase-2 (Lane keeping to pass-L.K.P.). After the vehicle reaches to the adjacent lane, again, if no obstacle is detected ahead in the current lane or the time headway is larger than 5s, then the vehicle would keep its lane to pass the obstacle vehicle with a safe distance D. (3) Phase-3 (Returning back-R.B.). Similar to phase-1, the host vehicle makes a lane change in order to return to the original lane.


19

5. Simulation verifications In this section, at first, in order to demonstrate the superiorities of the proposed path-planning algorithm, we conduct comparative experiments with some other selected path planning approaches in Section 5.1, while a comparative study for the path following method is presented in Section 5.2. Then we designed several typical scenarios to verify this presented trajectory following architecture from Sections 5.3 to 5.5. The first one is a free driving scene on a challenging 90-degree curved course; while the other two are overtaking manoeuvres where the host vehicle intends to pass an obstacle vehicle ahead on a straight or a curved road. As described before, the decision-making module will decide a specific tactical manoeuvre (lane keeping, adaptive following or overtaking) based on the current traffic environment. In addition, the control plant is a full nonlinear vehicle model exported from a famous commercial software CarSim©® , whose configuration is listed in Appendix 2, Table A2. The simulation results include the host vehicle’s global positions, the tactical state, the time cost of the trajectory-following control system, the vehicle dynamical system inputs (steering wheel angle, throttle and braking torques), the lateral performance (yaw rate and lateral acceleration), as well as its longitudinal performance (travelling speed and longitudinal acceleration).

5.1. Test-1: Comparative study of the path planning method We will show the superiorities of the proposed path-planning algorithm that is based on the natural cubic spline and convex optimisation techniques in this part. And it will be compared with some other selected path planning algorithms targeting to the automatic driving vehicle, which includes the elastic band based on artificial potential [41], the improved velocity harmonic potential method [50], and the improved Rapidly-exploring Random Tree (RRT) variant method [51]. We select those methods (e.g. artificial potential, RRT) for comparisons because they origin and get inspired by the robotic path-planning applications, which have been well studied. Further, it is worthy to notice that path planning for an automatic driving vehicle is a little bit different from that on robotics. Because we have to consider the road boundary constraints when in planning, and ensure the smoothness of the planned path, so that the path could be safely and easily tracked by the host vehicle even in high speeds. Therefore, those mentioned methods have been well modified, improved and well designed for the automatic driving vehicle. The test scenario is a curved road whose centreline is constructed via a cubic curve. The starting position of the host vehicle lies in the centre of the right lane at the very beginning of the segment, and a static obstacle lies in front in the same lane, whose CoG keeps a distance as 100 m to the host vehicle, and its lateral distance to the centreline is 2 m. In addition, the goal position also lies in the same lane where the distance to the host vehicle is 200 m, and its lateral distance to the centreline is 2 m. The purpose of the path planning is to obtain a feasible, collision-free path to reach the goal position. Finally, the planning results are shown in Figure 7(a), as well as the statistical plot (box-plot) for path curvature change corresponding to the distance and computation costs (running 50 times on a desktop (CPU-i7 6700 K @4.0 GHz, 16 GB RAM)) are shown in Figure 7(b,c), respectively. Based on the statistical results of the path curvature change respecting to distance as shown in Figure 7(b), though the path planned by the improved harmonic velocity

20

H. CAO ET AL.

Figure 7. The comparative study of path planning algorithms (test-1). (a) Path planning via different methods, (b) curvature change of the path and (c) computation costs.

potential has the best performance, while the differences for other three methods seem few. However, as shown in Figure 7(a), we can easily observe that the proposed algorithm has a very natural shape with smooth transition along the overtaking path when comparing with other approaches. In addition, when setting up for a potential-based method (e.g. the elastic band, the improved harmonic velocity potential), we found that the planning results are very sensitive to the potential construction parameters, the path planning could even be failed with improper parameters, while our method in this study won’t suffer this issue. As for the improved RRT-variant method, due to a random search mechanism and spline technical for path smoothing, the path shape seems a little bit wavy. Another advantage for our proposed method is the computation efficiency is the highest among them, as shown in Figure 7(c), the average cost time is below 10 ms thanks to the well-studied quadratic programming techniques. The average cost for improved harmonic velocity potential method is around 0.6 s as it needs to build a hazard potential map, while the average cost for elastic band is over 1s, which is higher because it needs to build hazard potential map and solve nonlinear algebraic equation sets. As for the improved RRT-variant method, the time cost has a wide range from 10 ms to 2 s, which is not very stable due to the random search mechanism with searching angle constraints, it might take a long time to find a feasible path. Therefore, the proposed method has advantages on planning a feasible path with a smooth transition and excellent computation efficiency. 5.2. Test-2: Comparative study of the path following method In this section, the proposed trajectory following controller with the path-planning module would be compared with other selected methods. Compared methods include conventional linear MPC controller (based on 2dof linear vehicle model, without path planning), linearised time-varying MPC (based on 5dof nonlinear vehicle model, without path planning), as well as the well-known driver model in CarSim© (optimal control driver model by MacAdam [6,7]). The test scenario is a DLC manoeuvre [52] with a constant speed of 20 m/s. Path following results including the global position, lateral tracking errors, steering


21

Figure 8. The path following comparison study for DLC (test-2). (a) Path optimised for DLC manoeuvre and (b) path following result comparisons.

wheel angle and lateral acceleration are compared. As shown in Figure 8(b), we could easily observe that the conventional linear MPC controller without the path planning gets the largest tracking errors and lateral accelerations in the whole DLC manoeuvre. While the linearised MPC controller without the path planning gets smaller tracking errors, however, lateral accelerations are large. Moreover, CarSim© driver model gets smaller lateral accelerations; but its tracking errors are not very satisfying. That is because a precise tracking of an emergency path (e.g. DLC) at a high speed often leads a dissatisfactory in its lateral performance, which indicates there exists conflicts between vehicles’ tracking quality and lateral performance when the vehicle is in a high speed. Nevertheless, as shown in Figure 8(a), thanks to the proposed path-planning module, an optimal path based on the original DLC profile will be derived by the natural cubic spline and convex optimisation. The improved path profiles have little changes in its geometric shape, but its curvature is significantly improved when compared with the original one. Apparently, the improved path is more suitable for path tracking, because the linearised time-varying MPC controller with path planning has the lowest lateral accelerations and tracking errors among those controllers. 5.3. Test-3: Lane-keeping on a challenging course Test 3 is designed for validating the tracking quality of the proposed framework where the host vehicle performs a lane-keeping manoeuvre along with the lane centreline. As shown

22

H. CAO ET AL.

Figure 9. The trajectory following results of test-3. (a) Global path, (b) speed, (c) speed, (d) throttle and braking torque, (e) steer wheel angle and yaw rate and (f) longitudinal and lateral acceleration.

in Figure 9(a), the test course has a 90-degree turn with a turning radius as 50 m; the total length of the course is around 250 m. The driving task demands that the host vehicle should follow the lane centreline and finish the course as quickly as it can. For a safety purpose, the maximum speed should be less than 25 m/s, and the expected lateral accelerations should be less than 0.5 g. According to the results shown in Figure 9(a), we can easily observe that the host vehicle could follow the target path quite well. Especially the path curvature is optimised when compared with the original one, as depicted in Figure 9(b). Meanwhile, its travelling speed is shown in Figure 9(c), which indicates that the travelling speeds are always within the bounds. The host vehicle would slow down its speed to around 13 m/s to enter the bend, and it would accelerate to the maximum allowed speed gradually once it gets through. Moreover, the control inputs including the throttle, braking torques and steering wheel angle for the host vehicle are smooth, as shown in Figure 9(d) and (e). As for the lateral performance of the vehicle as depicted in Figure 9(e,f), the maximum absolute value of the yaw rate and lateral acceleration are around 18 °/s and 0.49 g, respectively, which are acceptable. While the longitudinal acceleration of the host vehicle, as shown in Figure 9(f), also lies in an acceptable interval of −2 to 1.5 m/s2 . Therefore, the presented framework works well in a free driving mode when dealing with a challenging course.


23

Figure 10. The trajectory following results of test-4. (a) Global paths of the host vehicle and obstacle vehicle, (b) tactical state, (c) computation cost, (d) speeds, (e) throttle and braking torque, (f) steer wheel angle and yaw rate and (g) longitudinal and lateral acceleration.

5.4. Test-4: Interacting with a moving vehicle on a straight road Test 4 is designed for validating whether the presented framework is capable of making the host vehicle to perform a successful overtaking manoeuvre on a straight road. As shown in the Figure 10(a), the total length of the straight road segment is around 450 m, and there is an obstacle vehicle ahead in the right lane, which is keeping a constant speed as 15 m/s. In addition, the initial speed of the host vehicle is 20 m/s, and the distance gap to the obstacle vehicle is 30 m. The driving task demands that the host vehicle could safely overtake the preceding vehicle. Similar to the previous scenario, the maximum speed is set to be less than 25 m/s, and the expected lateral accelerations are less than 0.5 g. The global trajectories of the host vehicle and obstacle vehicle are shown in Figure 10(a), while the tactical states of the host vehicle are shown in Figure 10(b). At the very first beginning, the adaptive following module is activated since the distance gap for lane changing is not enough. Once the criteria for a lane changing is met, the host vehicle would perform

24

H. CAO ET AL.

a lane change manoeuvre to the adjacent lane. Then it would keep in its current lane until the host vehicle passes the obstacle vehicle by a safe distance (20 m). Finally, the host vehicle would return to the original lane to complete the overtaking manoeuvre. In addition, the computation costs for the trajectory following module is shown in Figure 10(c), which are all within 10 ms for each iteration. Thus, the proposed trajectory following algorithm has a good computation efficiency, which is expected to get a high potential for a realtime application in future. Moreover, the travelling speed is shown in Figure 10(d), which exhibits that the host vehicle would slow down its speed to around 15 m/s to increase the time headway, so that it would satisfy the lane changing criteria. Then the host vehicle would gradually speed up to the maximum allowed speed until the overtaking manoeuvre is finished. The control inputs (steering wheel angle, throttle, and braking torques) for host vehicle as shown in Figure 10(e,f) are smooth as well. According to Figure 10(f,g), the maximum absolute value of the yaw rate and lateral acceleration are around 4 °/s and 0.18 g, respectively, which means the vehicle was moving in an excellent stable condition. As for the driving comfort, the longitudinal acceleration of the host vehicle lies in the acceptable interval of −2.9 to 1.0 m/s2 , as shown in Figure 10(g). 5.5. Test-5: Interacting with a moving vehicle on a curved road The final test is designed for testing whether the presented framework is effective as well when the host vehicle performs an overtaking manoeuvre on a curved road. As shown in Figure 11(a), the total length of the curved road is around 550 m. Similar to the test-4, there is also an obstacle vehicle ahead in the right lane, which keeps a constant speed as 15 m/s, and the distance gap to the host vehicle is 30 m. The initial speed of the host vehicle is 20 m/s, and the driving task demands that the host vehicle should safely overtake the preceding vehicle as quick as it can. Again, for safety considerations on a curved road, the maximum speed should be less than 20 m/s, and the expected lateral accelerations are less than 0.4 g. The global trajectories of the host vehicle and obstacle vehicle are shown in Figure 11(a), while the tactical states in the completely overtaking manoeuvre are shown in Figure 11(b). As we can see, the overtaking procedure is quite similar to the case on the straight road. The computation cost for the trajectory following module on a curved road is shown in Figure 11(c), which is within 25 ms for each iteration. The largest computation cost occurs at the place where the host vehicle started to perform a lane change away from the lane centreline, as well as the place where the host vehicle was beginning to return to the original lane centreline. However, the maximum time cost does not exceed 25 ms that is still acceptable. Therefore, we believe the proposed trajectory following algorithm for a curved road is also capable of high potential in a real-time application. What’s more, the travelling speed is depicted in Figure 11(d), which shows that the host vehicle would slow down its speed to around 13 m/s to satisfy the lane changing criteria. Then it would gradually accelerate to the maximum allowed speed (20 m/s) after reaching the other lane. In addition, as shown in Figure 11(e,f), the inputs (throttle and braking torques and steering wheel angle) for the host vehicle are also smooth. The maximum absolute value of the yaw rate and lateral acceleration is around 6 °/s and 0.20 g, respectively, as shown in Figure 11(f,g), which indicate the vehicle moves in an excellent stable condition on a curved road as well. The Figure 11(g) also shows that the longitudinal acceleration of the host vehicle lies in


25

Figure 11. The trajectory following results of test-5. (a) Global paths of the host vehicle and obstacle vehicle, (b) tactical state, (c) computation cost, (d) speeds, (e) throttle and braking torque, (f) steer wheel angle and yaw rate and (g) longitudinal and lateral acceleration.

the acceptable interval of −2.9–1.2 m/s2 . Therefore, the framework also works well on a curved road.

6. Conclusions and future work This paper presents a hierarchical trajectory following architecture for automatic driving vehicles. It designs for SAE-level 2 or 3 automatic driving, which includes several combined functionalities of lane keeping, adaptive following, active obstacle avoidance and speed adaptation. It is expected to relieve the driving loads of the driver, enhance the driving safety, and improve the driving efficiency and comforts simultaneously. The trajectory-planning module would generate optimal path and velocity profiles by a novel approach, which combines the natural cubic spline and the convex optimisation technical. Furthermore, a linearised time-varying MPC controller that is based on a 5DOF

26

H. CAO ET AL.

nonlinear vehicle model would realise the trajectory-tracking mission. Simulation results demonstrate that the automatic driving vehicle with the proposed trajectory following framework could successfully deal with daily driving manoeuvres (e.g. lane keeping, adaptively following, and overtaking) with low traffics on different shapes of structured road. Our future work would mainly include: (1) sensor fusion module would be added to our current framework to provide accurate information of vehicle’s surroundings. (2) An intelligent decision-making module that aims to determine proper tactical decisions would be added as well. (3) In addition, a steer-by-wire system with active steering would be considered, which is expected to further improve the vehicle’s handling qualities in critical circumstances.

Acknowledgements The authors thank the editor and the reviewers’ valuable and insightful suggestions for this paper.

Disclosure statement The authors reported no potential conflict of interest.

Funding This work is supported by National Science Foundation of China (NSFC) [grant numbers 51575169, 51575167], China Scholarship Council (CSC) [grant number 201606130075] and Natural Science Foundation of Hunan Province [grant number 2017JJ2032].

ORCID Haotian Cao http://orcid.org/0000-0001-5905-4116 Zhi Huang http://orcid.org/0000-0003-3748-9252 Chuan Hu http://orcid.org/0000-0001-5379-1561

References [1] World Health Organization. Global Status Report on Road Safety 2015; 2015 [cited 2017 Oct 1]. Available from: http://www.who.int/violence_injury_prevention/road_safety_status/2015/en/. [2] World Health Organization. Improving global road safety; 2016 [cited 2017 Oct 1]. Available from: http://www.who.int/roadsafety/about/resolutions/download/en/index.html [3] National Center for Statistics and Analysis. Passenger vehicles: 2015 data (Traffic Safety Facts. Report No. DOT HS 812 413). Washington, DC: National Highway Traffic Safety Administration; 2017. [4] Zhang RH, He ZC, Wang HW, et al. Study on self-tuning tyre friction control for developing main-servo loop integrated chassis control system. IEEE Access. 2017;5:6649–6660. [5] Sheridan TB. three models of preview control. IEEE Trans Hum Factors Electron. 1966;HFE7(2):91–102. [6] MacAdam CC. Application of an optimal preview control for simulation of closed-loop automobile driving. IEEE Trans Syst Man Cybern. 1981;11(6):393–399. [7] MacAdam CC. An optimal preview control for linear systems. J Dyn Syst Meas Control. 1980;102(3):188–190. [8] Sharp RS, Casanova D, Symonds P. A mathematical model for driver steering control, with design, tuning and performance results. Veh Syst Dyn. 2000;33(5):289–326.


27

[9] Sharp RS, Valtetsiotis V. Optimal preview car steering control. Veh Syst Dyn. 2001;35(Suppl. 1):101–117. [10] Saleh L, Chevrel P, Lafay JF. Optimal control with preview for lateral steering of a passenger car: design and test on a driving simulator. Sipahi R, Vyhlídal T, Niculescu S, Pepe P, editors. Time Delay Systems: Methods, Applications and New Trends, Vol. 423. Berlin, Heidelberg: Springer-Verlag; 2012. p. 173–185. [11] Cole DJ, Pick AJ, Odhams AMC. Predictive and linear quadratic methods for potential application to modelling driver steering control. Veh Syst Dyn. 2006;44(3):259–284. [12] Odhams AMC, Cole DJ. Application of linear preview control to modelling human steering control. Proc Inst Mech Eng D J Automob Eng. 2009;223(3):835–853. [13] Timings JP, Cole DJ. Vehicle trajectory linearisation to enable efficient optimisation of the constant speed racing line. Veh Syst Dyn. 2012;50(6):883–901. [14] Raff GV, Timings JP, Cole DJ. Minimum maneuver time calculation using convex optimization. J Dyn Syst Meas Control. 2013;135:031015-1:9. [15] Attia R, Orjuela R, Basset M. Coupled longitudinal and lateral control strategy improving lateral stability for autonomous vehicle. Proceedings of the 2012 American Control Conference (ACC); 2012 Jun 27–29; Montreal, QC, Canada; 2012. p. 6509-6514. [16] Attia R, Orjuela R, Basset M. Nonlinear cascade strategy for longitudinal control in automated vehicle guidance. Control Eng Pract. 2014;29(8):225–234. [17] Arikere A, Yang D, Klomp M, et al. Integrated evasive manoeuvre assist for collision mitigation with oncoming vehicles. Veh Syst Dyn. 2018: 1–27. DOI:10.1080/00423114.2017.1423091 [18] Nobukawa K, Gordon TJ, LeBlanc DJ. Anticipatory speed control model applied to intersection left turns. Veh Syst Dyn. 2012;50(11):1653–1672. [19] Falcone P, Borrelli F, Asgari J, et al. Predictive active steering control for autonomous vehicle systems. IEEE Trans Control Syst Technol. 2007;15(3):566–580. [20] Raffo GV, Gomes GK, Kelber CR, et al. A predictive controller for autonomous vehicle path tracking. IEEE Trans Intell Transp Syst. 2009;10(1):92–102. [21] Lefèvre S, Carvalho A, Gao Y, et al. Driver models for personalised driving assistance. Veh Syst Dyn. 2015;53(12):1705–1720. [22] Liu J, Jayakumar P, Stein JL, et al. A nonlinear model predictive control formulation for obstacle avoidance in high-speed autonomous ground vehicles in unstructured environments. Veh Syst Dyn. 2018;56(6):853–882. [23] Ghazali M, Durali M, Salarieh H. Path-following in model predictive rollover prevention using front steering and braking. Veh Syst Dyn. 2017;55(1):121–148. [24] Johns TA, Cole DJ. Measurement and mathematical model of a driver’s intermittent compensatory steering control. Veh Syst Dyn. 2015;53(12):1811–1829. [25] Na X, Cole DJ. Linear quadratic game and non-cooperative predictive methods for potential application to modelling driver–AFS interactive steering control. Veh Syst Dyn. 2013;51(2):165–198. [26] Ji X, Liu Y, Na X, et al. Research on interactive steering control strategy between driver and AFS in different game equilibrium strategies and information patterns. Veh Syst Dyn. 2018: 1–31. DOI:10.1080/00423114.2018.1435890 [27] Guo J, Li L, Li K, et al. An adaptive fuzzy-sliding lateral control strategy of automated vehicles based on vision navigation. Veh Syst Dyn. 2013;51(10):1502–1517. [28] Cao H, Song X, Zhao S, et al. An optimal model-based trajectory following architecture synthesising the lateral adaptive preview strategy and longitudinal velocity planning for highly automated vehicle. Veh Syst Dyn. 2017;55(8):1143–1188. [29] Nilsson J, Falcone P, Ali M, et al. Receding horizon maneuver generation for automated highway driving. Control Eng Pract. 2015;41:124–133. [30] Li X, Sun Z, Cao D, et al. Real-time trajectory planning for autonomous urban driving: framework, algorithms, and verifications. IEEE/ASME Trans Mechatronics. 2016;21(2):740–753. [31] You F, Zhang R, Guo L, et al. Trajectory planning and tracking control for autonomous lane change maneuver based on the cooperative vehicle infrastructure system. Expert Syst Appl. 2015;42(14):5932–5946.

28

H. CAO ET AL.

[32] Werling M, Kammel S, Ziegler J, et al. Optimal trajectories for time-critical street scenarios using discretized terminal manifolds. Int J Robot Res. 2012;31(3):346–359. [33] Junsoo K, Jo K, Lim W, et al. A probabilistic optimization approach for motion planning of autonomous vehicles. Proc Inst Mech Eng D J Automob Eng. 2017;232(5):632–650. [34] SAE On-Road Automated Vehicle Standards Committee. Taxonomy and definitions for terms related to on-road motor vehicle automated driving systems. SAE Standard. 2014;J3016:01–16. [35] Yi T, Li H. On GPS observation errors with ANC principles and wavelet denoise method. Geomat Inf Sci Wuhan Univ. 2006;31(11):995–998. [36] Shi Y, Ji S. GPS-supported visual SLAM with a rigorous sensor model for a panoramic camera in outdoor environments. Sensors. 2013;13(1):119–136. [37] Li H, Tsukada M. Multivehicle cooperative local mapping: a methodology based on occupancy grid map merging. IEEE Trans Intell Transp Syst. 2014;15(5):2089–2100. [38] Li H. Localization for intelligent vehicle by fusing mono-camera, low-cost GPS and map data. Proceedings of the 13th International IEEE Conference on Intelligent Transportation Systems; 2010 Sept 19–22; Funchal, Portugal; 2010. p. 1657–1622. [39] Yang M, Wang C. Laser radar based vehicle localization in GPS signal blocked areas. Int J Comput Intell Syst. 2011;4(6):1100–1109. [40] Dahmani H, Chadli M, Rabhi A, et al. Vehicle dynamics and road curvature estimation for lane departure warning system using robust fuzzy observers: experimental validation. Veh Syst Dyn. 2015;53(8):1135–1149. [41] Sattel T, Brandt T. From robotics to automotive: Lane-keeping and collision avoidance based on elastic bands. Veh Syst Dyn. 2008;46(7):597–619. [42] Weisstein EW. Cubic spline. From MathWorld – a Wolfram web resource; last updated 2018 [cited 2017 Oct 1]. Available from: http://mathworld.wolfram.com/CubicSpline.html [43] MATLAB Help Document, fminicon (Find minimum of constrained nonlinear multivariable function); last updated 2018 [cited 2017 October 1]. Available from: https://cn.mathworks. com/help/optim/ug/fmincon.html. [44] Rajamani R. Vehicle dynamics and control. Berlin: Springer Science & Business Media; 2011. [45] Pacejka H. Tire and vehicle dynamics. Oxford: Butterworth-Heinemann; 2006. [46] Camacho EF, Alba CB. Model predictive control. Berlin: Springer Science & Business Media; 2013. [47] MATLAB Help Documentation, quadprog (Quadratic programming); last updated 2018 [cited 2017 Oct 1]. Available from: https://www.mathworks.com/help/optim/ug/quadprog.html [48] Michon JA. A critical view of driver behavior models: what do we know, what should we do? In: Schwing R, Evans LA, editors. Human behavior and traffic safety. New York (NY): Plenum Press; 1986. p. 487–525. [49] Gordon TJ, Lidberg M. Automated driving and autonomous functions on road vehicles. Veh Syst Dyn. 2015;53(7):958–994. [50] Cao H, Song X, Huang Z, et al. Simulation research on emergency path planning of an active collision avoidance system combined with longitudinal control for an autonomous vehicle. Proc Inst Mech Eng D J Automob Eng. 2016;230(12):1624–1653. [51] Xiaolin S, Nan Z, Zhengyu H, et al. Application of improved RRT in local path planning for vehicle avoidance. J Hunan Univ. 2017;44(4):30–37. [52] ISO 3888-1:1999, Passenger cars – test track for a severe lane-change maneuver – Part 1: double lane-change; 1999.

Appendices Appendix 1. Natural cubic spline A cubic spline is a spline constructed of piecewise third-order polynomials that pass through a set of m control points. The second derivative of each polynomial is commonly set to zero at the endpoints, since this provides a boundary condition that completes the system of m−2 equations. This produces


29

a so-called ‘natural’ cubic spline and leads to a simple tridiagonal system that can be solved easily to give the coefficients of the polynomials. Consider 1-dimensional spline for a set of n + 1 points y = (y0 , y1 , . . . , yn )T , and let the ith piece of the spline be represented by Yi (t) = ai + bi t + ci t 2 + di t 3 ,

(A1)

where t is a parameter, t ∈ [0, 1]i = 0, 1, . . . , n − 1 and Yi (0) = yi = ai ,

(A2)

Yi (1) = yi+1 = ai + bi + ci + di . Taking the derivative of Yi in each interval then gives,

(A3)

Yi (0) = Di = bi ,

(A4)

Yi (1) = Di+1 = bi + 2ci + 3di . Solving Equations (A2)–(A5) for ai , bi , ci , di then gives ⎧ ai = yi , ⎪ ⎪ ⎪ ⎨ bi = Di , ⎪ ci = 3(yi+1 − yi ) − (2Di + Di+1 ), ⎪ ⎪ ⎩ di = 2(yi − yi+1 ) + Di + Di+1 .

(A5)

(A6)

Now require that the second derivatives also match at the points, so ⎧ Yi−1 (1) = yi , ⎪ ⎪ ⎪ ⎪ ⎨ Y (1) = Y (0), i i−1

(A7)

⎪ Yi−1 (1) = Yi (0), ⎪ ⎪ ⎪ ⎩ Yi (0) = yi .

For interior points, as well as that the endpoints satisfy, Y0 (0) = y0 ,

(A8)

Yn−1 (1) = yn .

This gives a total of 4(n − 1) + 2 = 4n − 2 equations for the 4n unknowns. To obtain two more conditions, require that the second derivatives at the endpoints be zero, so Y0 (0) = 0, (A9) Yn−1 (1) = 0. Rearranging all these equations leads to the following symmetric tridiagonal system ⎤⎡ ⎤ ⎡ ⎡ D0 2 1 (y1 − y0 ) 1 ⎥ ⎢ D1 ⎥ ⎢ 1 4 ⎢ (y2 − y0 ) ⎥⎢ D ⎥ ⎢ ⎢ (y − y ) 1 4 1 ⎥⎢ 2 ⎥ ⎢ ⎢ 3 1 ⎥ ⎢ ⎥ ⎢ ⎢ 1 4 1 ⎥ ⎢ D3 ⎥ = 3 ⎢ .. ⎢ ⎥⎢ . ⎥ ⎢ . . ⎢ . ⎢ ⎥ ⎢ . ⎢ .. ... ... ... ... ... ⎥ ⎥ ⎢ .. ⎥ ⎢ . ⎢ (yn−1 − yn−3 ) ⎦ ⎣ ⎦ ⎣ ⎣ (y − y ) 1 4 1 D n−1

1

2

Dn

n

n−2

⎤ ⎥ ⎥ ⎥ ⎥ ⎥. ⎥ ⎥ ⎥ ⎦

(yn − yn−1 )

Donate it as a matrix form, such that, MD = 3Ny,

(A10)

30

H. CAO ET AL.

Where M, N denote constant matrixes, Once D is determined, according to Equation (A6), we can have, dY(t) = D = (3M−1 N)y, (A11) dt t=0 d2 Y(t) = 6[yi+1 − yi ] − 2[2Di + Di+1 ] = 6Jy − 2PD = 6(J − PM−1 N)y, (A12) dt 2 t=0

where Jy = [y(i+1) − yi ], PD = [2Di + Di+1 ], i = 0, . . . , n − 1, J, P are constant matrix. Rewrite Equations (A11)–(A12) as a linear form of the control points y, ⎧ dY(t) ⎪ ⎨ dt t=0 = Gy . 2 ⎪ ⎩ d Y(2 t) = Hy dt t=0

With G =

3M−1 N, H

=

6(J − PM−1 N), and

G, H are constant matrixes.

Table A1. Parameters of the host vehicle and controllers. Parameters description Long. MF constants Bx , Cx Lateral MF constants By , Cy Length of the vehicle Lv Steering ratio G Vehicle mass m Vehicle inertia respecting to Z-axis Iz Wheel inertia Jwf , Jwr Distance from the CoG to the front axle a Distance from the CoG to the rear axle b Wheel effective radius rw Gear ratio of the differential ig Efficiency of the powertrain η Braking distribution factor τ Wind drag-down force distribution ε Frontal area Ad Air density ρ Lift coefficient Gravitational constant g Road adhesion constant μ MPC discretisation sample time Ts Constraints of the steering incremental input Umin /Umax Constraints of the steering input Umin /Umax Prediction/control horizontal length for steering Np , Nu Prediction/control horizontal length for adaptive following np , nu Desired time headway to the leading vehicle τd

Values (Unit) 16.480 (–), 1.217 (–) −11.270 (–),1.452 (–) 5m 18 (–) 1800 kg 3200 kg m2 2.9 kg m2 1.40 m 1.65 m 0.36 m 4.1 (–) 0.95 (–) 0.5 (–) 0.4 (–) 2.2 m2 1.206 kg/m3 0.18 (–) 9.80 m/s2 0.9 (–) 0.02 s (−15°, −500 Nm)/(15°, 300 Nm) (−150°, −3000 Nm)/(150°, 1600 Nm) 50 (–),50 (–) 100 (–),100 (–) 2.0 s

(A13)


Appendix 2

Table A2. Vehicle configurations in CarSim©® . Components

Values and description

Vehicle type Driven type Engine Transmission Differential Suspension Tyres Steering Braking

E-class Sedan Rear-driven Power 150 Kw, idle speed 750 rev/min AT, 5-speed gearbox Vicious, Ratio 4.1 Both independent for front/rear suspension 225/60 R18 Front steering, EPS on Four wheels braking with ABS

31