Path Planning for Autonomous Vehicle in Off-Road Scenario

Abstract

The road topography information, such as bank angle and road slope, can significantly affect the trajectory tracking performance of the autonomous vehicle, so this information needs to be considered in the trajectory planning and tracking control for off-road autonomous vehicle. In this chapter, a two-level real-time dynamically integrated spatiotemporal-based trajectory planning and control method for off-road autonomous vehicle is proposed. In the upper-level trajectory planner, the most suitable time-parameterised trajectory with the minimum values of road slope and bank angle can be selected from a set of candidate trajectories. In the lower-level trajectory tracking controller, the sliding-mode control (SMC) technique is applied to control the vehicle and achieve the desired trajectory. Finally, simulation results are presented to verify the proposed integrated trajectory planning and control method and prove that the proposed integrated method has better overall tracking control and dynamics control performance than the conventional method both in the highway scenario and off-road scenario. Furthermore, the four-wheel-independent-steering (4WIS) and four-wheel-independent-driving (4WID) vehicle shows better tracking control performance than vehicle based on two-wheel model.

1. Introduction

Nowadays, the off-road autonomous ground vehicle has been widely applied in various industries, such as military [1, 2] and space applications [3, 4]. Furthermore, this kind of vehicle also received focused attention in mining [5], agriculture and forestry sectors [6].

In order to improve the stability and safety of off-road autonomous vehicles, the path planning of these vehicles should be considered as the priority of current research. The path planning of autonomous vehicle includes two stages: the trajectory planning in the upper-level and trajectory tracking control in the lower-level. The upper-level trajectory planner considers the surrounding environment information according to the various sensors and selects the best desired trajectory, while the lower-level trajectory tracking controller controls the steering and driving actuators to achieve the desired trajectory.

In the current literature, the path planning of autonomous vehicle has attracted focused attention. Particularly, the spatial-based path planning methods are widely applied, but the time parameter is not considered [7]. For example, for the direct tracking method, the steering system is controlled to follow the pre-planned spatial-based desired path exactly at every time step [8, 9]. In the potential field method proposed in [10], the desired path is planned within a potential field with a tracking error tolerance along the road centreline. In this way, the autonomous vehicle does not need to strictly follow the road centreline, and smaller steering control effort is required compared with the direct tracking method. The spatiotemporal-based trajectory planning concept, on the other hand, considers the kinematic constraints and generates time-parameterised trajectories. Several typical spatiotemporal-based trajectory planning methods, such as the methods proposed in [11, 12, 13], aim to find the best suitable time-parameterised trajectory connecting the initial vehicle states with exactly defined goal states. These methods rely on discrete geometric structure, such as the rapidly exploring random trees (RRT) [14] and state lattice [13]. However, the generation of candidate trajectories requires large computational work. When the surrounding environment is unconstructed and complex, these methods may not be computational efficient. In [15, 16], the proposed trajectory planning strategies utilise ‘deliberated multiple final states’ method. This method deliberately generates multiple alternative final states which can respond to traffic changes very fast. In study [17], based on the concept of ‘deliberated multiple final states’, the combined trajectory planning of the longitudinal and lateral motion of autonomous vehicle are proposed, and the ‘deliberated multiple final states’ are described as the offset error values from the target reference final states. The most suitable trajectory which satisfies the initial and ending states with certain terminal time can be selected from candidate trajectory set, and the kinematic constraints are satisfied.

Motivated by the widely application of the off-road autonomous vehicle in various industries and based on above research studies on path planning, this chapter proposed a two-level real-time dynamically integrated spatiotemporal-based trajectory planning and control method by considering the off-road scenario. The major innovative part of this chapter is the development of the spatiotemporal-based trajectory planning method and considering the off-road topography information in trajectory planning. In the upper-level trajectory planner, a number of candidate spatiotemporal-based trajectories with various terminal times and state-ending conditions are generated. These candidate trajectories also include the road topography information—the bank angle and road slope. The best suitable trajectory can be selected from these candidate trajectories based on the optimised cost function which is used to minimise the tracking error, terminal time spent and the effect of road topography on the vehicle. After that, trajectory tracking controller in the lower-level is proposed based on the sliding-mode technique and vehicle dynamics model in order to track the selected best suitable trajectory. In addition, the vehicle dynamics model of this chapter is based on a four-wheel-independent-steering (4WIS) and four-wheel-independent-driving (4WID) electric vehicle. Due to a large number of available control actuators, the 4WIS-4WID electric vehicle shows advantages over the traditional vehicle. This chapter also discusses the advantage of 4WIS-4WID electric vehicle on trajectory planning and trajectory tracking control over traditional two-wheel vehicle.

In this chapter, Section 2 first discusses the vehicle dynamics model based on 4WIS-4WID electric vehicle. Then Section 3 describes the upper-level trajectory planner, and Section 4 shows the lower-level trajectory tracking control. After that, Section 5 presents the simulation results to verify the proposed trajectory planning and control method. Finally, the conclusion is given in Section 6.

2. Vehicle dynamics modelling

In this section, a 4WIS-4WID vehicle model is utilised first to describe the dynamic motion of an off-road autonomous vehicle [18]. The information of road slope and bank angle is included in the vehicle longitudinal and lateral dynamics equations. Furthermore, vehicle roll dynamics equation and pitch dynamics equation are included in the dynamics model to better present the effect of bank angle and road slope on the vehicle dynamics. The vector diagram of vehicle dynamics model is presented in Figure 1.

Figure 1.

The vector diagram of 4WIS-4WID vehicle dynamics model.

The equations of motion of this model are described as follows:

Longitudinal motion:

$$m{\dot{v}}_x=m{v}_{y}r+\left(F_{\mathit{xfl}}+F_{\mathit{xfr}}+F_{\mathit{xrl}}+F_{\mathit{xrr}}\right)+\mathit{mg}sin{\theta }_s$$E1

Lateral motion:

$$m{\dot{v}}_y=-m{v}_{x}r+\left(F_{\mathit{yfl}}+F_{\mathit{yfr}}+F_{\mathit{yrl}}+F_{\mathit{yrr}}\right)+\mathit{mg}sin{\theta }_b$$E2

Yaw motion:

$$I_z\dot{r}=l_f\left(F_{\mathit{yfl}}+F_{\mathit{yfr}}\right)-l_r\left(F_{\mathit{yrl}}+F_{\mathit{yrr}}\right)+\frac{b_f}{2}\left(F_{\mathit{xfl}}-F_{\mathit{xfr}}\right)+\frac{b_r}{2}\left(F_{\mathit{xrl}}-F_{\mathit{xrr}}\right)$$E3

Roll motion:

$$I_x\ddot{\varphi }=-m{e}_r{\dot{v}}_y-m{e}_r{v}_{x}r+\mathit{mg}{e}_{r}sin\varphi -K_{\varphi }\varphi -C_{\varphi }\dot{\varphi }$$E4

Pitch motion:

$$I_y\ddot{\phi }=-m{e}_p{\dot{v}}_x-m{e}_p{v}_{y}r+\mathit{mg}{e}_{p}sin\phi -K_{\phi }\phi -C_{\phi }\dot{\phi }$$E5

where $v_x,v_y\text{and}r$ are the vehicle longitudinal velocity, lateral velocity and yaw rate. ${\theta }_s$ shows the road slope, and ${\theta }_b$ represents the road bank angle. $b_f$ and $b_r$ represent the front and rear track width. $l_f$ is the length of front wheel base, and $l_r$ is the length of rear wheel base. $I_z$ represents the moment of yaw inertia, and $m$ is vehicle mass. $F_{\mathit{xfl}}$ and $F_{\mathit{xfr}}$ represent the longitudinal tyre force of front left and front right tyre, while $F_{\mathit{xrl}}$ and $F_{\mathit{xrr}}$ present the longitudinal tyre force of rear left and rear right wheel.$F_{\mathit{yfl}}$ and $F_{\mathit{yfr}}$ present the lateral tyre force of front left and front right tyre, while $F_{\mathit{yrl}}$ and $F_{\mathit{yrr}}$ present the lateral tyre force of rear left and rear right wheel. $\varphi$ and $\phi$ represent the vehicle roll angle and pitch angle, respectively.$e_r$ is the distance from the vehicle centre of gravity (CG) to the roll centre, and $e_p$ is the distance from the vehicle CG to the pitch motion centre. $K_{\varphi }$ is the roll axis torsional stiffness, and $C_{\varphi }$ is the roll axis torsional damping. $K_{\phi }$ is the pitch axis torsional stiffness, and $C_{\phi }$ is the pitch axis torsional damping.

The tyre side force $F_{\mathit{si}}$ and traction or brake force $F_{\mathit{ti}}$ can be transferred to the longitudinal force $F_{\mathit{xi}}$ and the lateral tyre force $F_{\mathit{yi}}$ as follows:

$$\begin{array}{c}{F}_{\mathit{xi}}=F_{\mathit{ti}}cos{\delta }_i-F_{\mathit{si}}sin{\delta }_i\\ F_{\mathit{yi}}=F_{\mathit{ti}}sin{\delta }_i+F_{\mathit{si}}cos{\delta }_i\end{array}$$E6

where $i=\mathit{fl},\mathit{fr},\mathit{rl}\text{and}\mathit{rr}$, which represents the front left wheel, front right wheel, rear left wheel and rear right wheel.

The non-linear Dugoff tyre model is used in this chapter [19], and tyre traction or brake force and side force of each wheel are described by:

Tyre side force:

$$F_{\mathit{si}}=\frac{C_{\alpha }tan{\alpha }_i}{1-s_i}f\left({\lambda }_i\right)$$E7

Tyre traction or brake force:

$$F_{\mathit{ti}}=\frac{C_s{s}_i}{1-s_i}f\left({\lambda }_i\right)$$E8

${\lambda }_i$ in Eqs. (7) and (8) can be determined by the following equation:

$${\lambda }_i=\frac{\mu F_{\mathit{zi}}\left[1-{\epsilon }_r{u}_i\sqrt{s_i^2+{tan}^2{\alpha }_i}\right]\left(1-s_i\right)}{2\sqrt{C_s^2{s}_i^2+C_{\alpha }^2{tan}^2{\alpha }_i}}$$E9

$f\left({\lambda }_i\right)$ in Eqs. (7) and (8) can be determined by the following equation:

$$f\left({\lambda }_i\right)=\left\{\begin{array}{c}{\lambda }_i\left(2-{\lambda }_i\right)\left({\lambda }_i<1\right)\\ 1\left({\lambda }_i>1\right)\hfill \end{array}\right.$$E10

where $C_{\alpha }$ represents the lateral cornering stiffness and $C_s$ is the longitudinal cornering stiffness. The tyre-road friction coefficient can be represented as $\mu$, and $F_{\mathit{zi}}$ represents the individual wheel vertical load. ${\alpha }_i$ represents the lateral side-slip angle, and $s_i$ is the longitudinal slip ratio. $u_i$ represents the vehicle longitudinal velocity in the individual wheel plane. ${\epsilon }_r$ is the road adhesive reduction factor, which is a constant value.

The following equation shows the wheel rotation dynamics:

$$I_{\omega }{\dot{\omega }}_i=-R_{\omega }{F}_{\mathit{ti}}+T_i$$E11

where ${\omega }_i$ presents the wheel angular velocity of each wheel and $T_i$ presents the traction or brake torque of each wheel. $R_{\omega }$ is the wheel radius, and $I_{\omega }$ is the wheel moment of inertial.

The load transfer model is considered here by adding the roll and pitch motion to better present the effect of road slope and bank angle on the vehicle vertical load distribution [20]. The vertical load of individual wheel can be presented by the following equations by including the load transfer model:

$$F_{\mathit{zfl}}=\frac{m}{l_f+l_r}\left(\frac{1}{2}g{l}_r-\frac{1}{2}{\dot{v}}_{x}h+\frac{l_r}{b_f}\left({\dot{v}}_{y}h-g{e}_{r}sin\varphi \right)+\frac{1}{2}g{e}_{p}sin\phi {\left(l_f+l_r\right)}^2\right)$$E12

$$F_{\mathit{zfr}}=\frac{m}{l_f+l_r}\left(\frac{1}{2}g{l}_r-\frac{1}{2}{\dot{v}}_{x}h-\frac{l_r}{b_f}\left({\dot{v}}_{y}h-g{e}_{r}sin\varphi \right)+\frac{1}{2}g{e}_{p}sin\phi {\left(l_f+l_r\right)}^2\right)$$E13

$$F_{\mathit{zrl}}=\frac{m}{l_f+l_r}\left(\frac{1}{2}g{l}_f+\frac{1}{2}{\dot{v}}_{x}h+\frac{l_f}{b_r}\left({\dot{v}}_{y}h-g{e}_{r}sin\varphi \right)-\frac{1}{2}g{e}_{p}sin\phi {\left(l_f+l_r\right)}^2\right)$$E14

$$F_{\mathit{zrl}}=\frac{m}{l_f+l_r}\left(\frac{1}{2}g{l}_f+\frac{1}{2}{\dot{v}}_{x}h-\frac{l_f}{b_r}\left({\dot{v}}_{y}h-g{e}_{r}sin\varphi \right)-\frac{1}{2}g{e}_{p}sin\phi {\left(l_f+l_r\right)}^2\right)$$E15

where $h$ is the height of the vehicle CG above the ground.

3. Upper-level trajectory planner

Figure 2 presents the whole structure of the proposed integrated trajectory planning and control method, which mainly includes the upper-level trajectory planner, the lower-level trajectory controller and the vehicle dynamics model [21].

Figure 2.

The whole structure of the proposed integrated trajectory planning and control method.

At the beginning, it is assumed that a behaviour layer planner exists and can determine the rough global reference path according to the digital map. This behaviour layer planner consists of a number of modules, such as digital map, perception and localisation system and behaviour level path planner [22]. The digital map provides real-time traffic information, and the real-time vehicle position on the digital map can be determined by the perception and localization system (such as the GPS combined with IMU and wheel encoder). When digital map and vehicle’s real-time position on the digital map are available, the behaviour planner can make deliberate manoeuvre task decisions, such as lane following, lane changing, vehicle following and overtaking, in complex street-driving scenario. Based on the manoeuvre task decisions, the global route planner in the behaviour planner can compute the rough reference path. This is a reasonable assumption because many studies in the literature have determined the rough reference path by behaviour level task planner based on digital map [22, 23, 24].

In the upper-level trajectory planner, according to the rough desired path determined by the behaviour planner, the desired vehicle initial and ending states of each section of the road along the rough reference path can be assumed to be known in advance.

3.1 Generate the candidate trajectory set

In each section of road, when the initial states are assumed to be available, the multiple target ending states can be defined as a group of offset state values from the reference state values (such as longitudinal position, longitudinal velocity, lateral position and lateral velocity). The start state is assumed as $\left[\begin{array}{cc}{d}_0& {\dot{d}}_0\end{array}{\ddot{d}}_0\right]$, and the desired ending state is assumed as $\left[\begin{array}{cc}\begin{array}{cc}{d}_1& {\dot{d}}_1\end{array}& {\ddot{d}}_1\end{array}\right]$. $d_0$ is the initial vehicle position, and $d_1$ is a group of offset positions from reference ending position, and these offset positions are constrained within the road boundary. ${\dot{d}}_0$ and ${\ddot{d}}_0$ present the initial velocity and acceleration, while ${\dot{d}}_1$ and ${\ddot{d}}_1$ present the ending velocity and acceleration. For the purpose of the guarantee of the continuities of the planned trajectory between each section of the road, the initial state $d_0$ in current section of road should be the ending state of previous section.

In each section of the road, when the initial and ending state values are determined, the candidate trajectories with different ending conditions $d_{1i}$ and terminal time ${\tau }_j$ can be generated [17], where $i,j$ means that the number of $i\times j$ trajectories will be generated by the trajectory planner. $d_{1i}$ represents $i$ number of final positions and will close to the target ending position when $d_{1i}\to d_1$. ${\tau }_j$ represents the $j$ number of candidate terminal time. The optimisation algorithm presented in the later section will choose the best trajectory from these $i\times j$ trajectories.

It can be assumed that the candidate vehicle trajectory $d\left(\tau \right)$ in the optimisation of trajectory planning can be described by the following quintic state equations [17]:

For the position of candidate trajectory:

$$d_1=c_0+c_{1}t+c_2{t}^2+c_3{t}^3+c_4{t}^4+c_5{t}^5$$E16

For the velocity of candidate trajectory:

$${\dot{d}}_1=c_1+2c_{2}t+3c_3{t}^2+4c_4{t}^3+5c_5{t}^4$$E17

For the acceleration of candidate trajectory:

$${\ddot{d}}_1=2c_2+6c_{3}t+12c_4{t}^2+20c_5{t}^3$$E18

with $c_0,c_1,\dots ,c_5\in R$ and $t\in \left[\begin{array}{cc}0& \tau \end{array}\right]$. $\tau$ is the terminal time of the candidate trajectory and $\tau \in \left[\begin{array}{cc}0& T\end{array}\right]$. $T$ is the longest time required to complete the motion.

Eqs. (16)–(18) can be rewritten as the following equation:

$${\mathbf{\xi }}_{\mathit{t}}\left(t\right)={\mathit{M}}_{\mathbf{1}}\left(t\right){\mathit{c}}_{\mathbf{012}}+{\mathit{M}}_{\mathbf{2}}\left(t\right){\mathit{c}}_{\mathbf{345}}$$E19

where ${\mathit{M}}_{\mathbf{1}}\left(t\right)=\left[\begin{array}{ccc}1& t& t^2\\ 0& 1& 2t\\ 0& 0& 2\end{array}\right]$, ${\mathit{M}}_{\mathbf{2}}\left(t\right)=\left[\begin{array}{ccc}{t}^3& t^4& t^5\\ 3t^2& 4t^3& 5t^4\\ 6t& 12t^2& 20t^3\end{array}\right]$ and ${\mathbf{\xi }}_{\mathit{t}}\left(t\right)=\left[\begin{array}{c}{d}_1\left(t\right)\\ {\dot{d}}_1\left(t\right)\\ {\ddot{d}}_1\left(t\right)\end{array}\right]$.

The coefficients ${\mathit{c}}_{\mathbf{012}}$ and ${\mathit{c}}_{\mathbf{345}}$ of the quintic state trajectory in Eq. (19) can be calculated as follows:

$${\mathit{c}}_{\mathbf{012}}=\left[\begin{array}{c}{c}_0\\ c_1\\ c_2\end{array}\right]={\mathit{M}}_{\mathbf{1}}{\left(0\right)}^{-1}{\mathit{\xi }}_{\mathbf{0}}$$E20

$${\mathit{c}}_{\mathbf{345}}=\left[\begin{array}{c}{c}_3\\ c_4\\ c_5\end{array}\right]={\mathit{M}}_{\mathbf{2}}{\left(\tau \right)}^{-1}\left[{\mathit{\xi }}_{\mathit{t}}-{\mathit{M}}_{\mathbf{1}}\left(\tau \right){\mathit{c}}_{\mathbf{012}}\right]$$E21

where ${\mathit{M}}_{\mathbf{1}}\left(0\right)=\left[\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\\ 0& 0& 2\end{array}\right]$ and ${\mathit{\xi }}_{\mathbf{0}}=\left[\begin{array}{c}{d}_0\\ {\dot{d}}_0\\ {\ddot{d}}_0\end{array}\right].$

After the coefficients ${\mathit{c}}_{\mathbf{012}}$ and ${\mathit{c}}_{\mathbf{345}}$ are calculated, the vehicle trajectory can be described as $d_1\left(t\right)$ in Eq. (16). In this way, candiadate trajectories in this section of the road can be determined, and the best trajectory can be selected based on the proposed optimisation cost function in the next section.

3.2 Determine the optimisation cost function

After the candidate trajectories have been determined in each section of the road, the next step is to determine the cost function to select the best suitable trajectory. The optimisation cost function is designed as the following equation:

$$\underset{d_1,\tau }{min}{J}_1=k_{\tau }\tau +k_d{\left(d_r-d_1\left(\tau \right)\right)}^2$$E22

where this cost function has two optimization variables, the ending position $d_1$ and terminal time $\tau$. This cost function also includes two terms, and $k_{\tau }$ and $k_d$ are the scaling factors of each term, which can be used to balance the term of total time cost and the term of offset error from the desired ending state. $d_r$ is the reference vehicle ending state. $d_r-d_1\left(\tau \right)$ presents the offset error from the desired reference ending state. The selection of total time cost can greatly affect the vehicle trajectory tracking behaviour: with the small total time cost, the vehicle can reach the final states early, while large time cost will make the vehicle movement slow and stable with late arrival of final states.

Furthermore, the vehicle longitudinal or lateral jerk (presented as $\stackrel{⃛}{d}\left(\tau \right)$) should be minimised to improve the smoothness of the trajectory. The total optimisation cost function $J_1$ of the trajectory planning can be augmented as:

$$\underset{d_1,\tau }{min}{J}_1=k_J{\left({\stackrel{⃛}{d}}_1\left(\tau \right)\right)}^2+k_{\tau }\tau +k_d{\left(d_r-d_1\left(\tau \right)\right)}^2$$E23

where $k_J$ is the scaling factor of the term related to longitudinal or lateral jerk. It can be noted that the target final velocity ${\dot{d}}_1$ or acceleration ${\ddot{d}}_1$ can be used in (23) instead of $d_1$ if the final velocity or acceleration is required to be optimised.

In optimisation cost function (23), the road topography information, such as the road slope and bank angle, has not been considered. However, road topography will greatly affect the trajectory planning and vehicle dynamics performance in off-road scenario. The trajectory planning optimisation cost function should consider the additional optimisation control target of road topography by selecting the trajectory with the smaller road slope and bank angle. Furthermore, in order to prevent the abrupt change of road slope and bank angle, the change of the road slope and bank angle between current and previous time step should be minimised.

The assumption is made that the topography information along each candidate trajectory is already known through various sensors equipped in the intelligent vehicle system. In this chapter, the topography information at a specific point can be obtained from a lookup table. The average road slope ${\overline{\theta }}_s$ and bank angle ${\overline{\theta }}_b$ along one particular candidate trajectory could be calculated as the following equation:

$${\overline{\theta }}_s=\frac{{\sum }_{i=1}^n{\theta }_s\left(x_i,y_i\right)}{n}$$E24

$${\overline{\theta }}_b=\frac{{\sum }_{i=1}^n{\theta }_b\left(x_i,y_i\right)}{n}$$E25

where ${\theta }_s\left(x_i,y_i\right)$ and ${\theta }_b\left(x_i,y_i\right)$ are the road slope and bank angle at a specific point along the candidate trajectory. $n$ is the total number of discrete points along this candidate trajectory.

After the road topography information is available, the road topography information can be included into the optimal cost function (23) as the following equation:

$$\begin{array}{c}\underset{d_1,\tau }{\min }{J}_1=k_J{\left({\stackrel{⃛}{d}}_1\left(\tau \right)\right)}^2+k_{\tau }\tau +k_d{\left(d_r-d_1\left(\tau \right)\right)}^2+k_s{\overline{\theta }}_s\left(d_1\left(\tau \right)\right)+k_{sd}{\dot{\overline{\theta }}}_s\left(d_1\left(\tau \right)\right)\\ +k_b{\overline{\theta }}_b\left(d_1\left(\tau \right)\right)+k_{bd}{\dot{\overline{\theta }}}_b\left(d_1\left(\tau \right)\right)\end{array}$$E26

where this cost function have four additional cost function terms compared with cost function (23). The terms $k_s{\overline{\theta }}_s\left(d_1\left(\tau \right)\right)$ and $k_b{\overline{\theta }}_b\left(d_1\left(\tau \right)\right)$ are designed to minimise the road slope and bank angle along the selected trajectory. $k_{\mathit{sd}}{\dot{\overline{\theta }}}_s\left(d_{x,y}\left(\tau \right)\right)$ and $k_{\mathit{bd}}{\dot{\overline{\theta }}}_b\left(d_{x,y}\left(\tau \right)\right)$ are designed to prevent the abrupt change of road slope and bank angle. $k_s,k_{\mathit{sd}}$ and $k_b,k_{\mathit{bd}}$ are scaling factors of each term.

When the optimisation values of ending position $d_1$ and terminal time $\tau$ are determined based on (26), the desired best trajectory can be determined according to Eqs. (16)–(18).

It is noted that the trajectory planning in this section can be divided as the longitudinal trajectory planning and lateral trajectory planning. Eqs. (16)–(26) merely provide the common mathematical equations to generate the candidate trajectory set and determine the best suitable trajectory according to optimisation cost function. These mathematical equations are only corresponding to one section of road. The predefined global desired path can have a number of sections of road, and a number of the optimisation calculations are implemented successively. In the ideal condition, the more sections the desired global path is divided, the more accurate the optimisation results would be. However, a large number of the divided sections of road require intensive computing efforts, and the computational cost will increase a lot.

3.3 Map planned trajectory into vehicle dynamics control targets

After the desired trajectory is planned and determined while satisfying certain position constraints and velocity constraints, the next step is to map the desired trajectory into vehicle dynamics control targets: desired yaw angle and desired longitudinal velocity in the body-fixed coordinate system.

The desired yaw angle ${\phi }_d$ and longitudinal velocity $v_{\mathit{xd}}$ in the body-fixed coordinate system can be determined according to the following optimisation cost function:

$$\underset{{\phi }_d,v_{\mathit{xd}}}{min}{J}_2=a{\left(v_{\mathit{xd}}\left(k\right)-v_{\mathit{xd}-b}\left(k\right)\right)}^2+b{\left(v_{\mathit{xd}}tan{\phi }_d\left(k\right)-v_{\mathit{yd}-b}\left(k\right)\right)}^2+c{\left({\phi }_d\left(k\right)-{\phi }_d\left(k-1\right)\right)}^2$$E27

where this cost function includes three terms, which are used to achieve the desired longitudinal velocity (the first term), desired lateral velocity (the second term) and avoid the abrupt change of the yaw angle between each time step and improve the smooth of the trajectory (the third term). $a$, $b$ and $c$ are scaling factors of each term.$k$ represents the time step $t\left(k\right)$, and $k-1$ represents the time step $t\left(k-1\right)$. $v_{\mathit{xd}-b}$ and $v_{\mathit{yd}-b}$ represent the desired longitudinal velocity and lateral velocity in the body-fixed coordinate system, which can be calculated according to the desired longitudinal velocity $v_{\mathit{xd}-g}$ and lateral velocity $v_{\mathit{yd}-g}$ in the global coordinate system:

$$v_{\mathit{xd}-b}=v_{\mathit{xd}-g}cos\phi +v_{\mathit{yd}-g}sin\phi$$E28

$$v_{\mathit{yd}-b}=v_{\mathit{xd}-g}sin\phi -v_{\mathit{yd}-g}cos\phi$$E29

where the desired longitudinal velocity $v_{\mathit{xd}-g}$ and lateral velocity $v_{\mathit{yd}-g}$ along the desired trajectory in the global coordinate system can be determined according to Eqs. (17), (26).

After the desired longitudinal velocity and yaw angle in the vehicle body-fixed coordinate system are determined, the desired tyre forces and yaw moment to achieve these desired control targets can be calculated by the lower-level trajectory controller in the next section.

4. Lower-level trajectory tracking controller

In this section, the lower-level two-layer trajectory tracking controller is proposed to control the autonomous vehicle to follow the desired planned trajectory [21]. In the first layer, according to the desired longitudinal velocity, desired zero lateral velocity and desired yaw angle, the desired longitudinal force, lateral force and yaw moment in the vehicle body-fixed coordinate system can be calculated. In the second layer, the individual steering and driving actuators are optimised and controlled to achieve the desired longitudinal force, lateral force and yaw moment.

4.1 Trajectory tracking controller in the first layer

First, the error dynamics equation of vehicle trajectory tracking including the longitudinal velocity error, lateral velocity error and yaw angle error is presented to calculate the feedback tyre force and moment, which can be presented by the following equation based on [25]:

$${\stackrel{\sim }{v}}_y=\left[v_{x}sin\stackrel{\sim }{\phi }+v_{y}cos\stackrel{\sim }{\phi }\right]-v_{\mathit{yd}}$$E30

$${\stackrel{\sim }{v}}_x=\left[v_{x}cos\stackrel{\sim }{\phi }-v_{y}sin\stackrel{\sim }{\phi }\right]-v_{\mathit{xd}}$$E31

$$\stackrel{\sim }{\phi }={\phi }_{\mathit{act}}-{\phi }_d$$E32

where ${\phi }_{\mathit{act}}$ is the actual measurement yaw angle. $v_x$ and $v_y$ are actual measurement feedback longitudinal and lateral velocity. ${\stackrel{\sim }{v}}_x$ and ${\stackrel{\sim }{v}}_y$ are longitudinal velocity error and lateral velocity error, respectively. In order to improve the vehicle stability, the desired lateral velocity $v_{\mathit{yd}}$ is assumed as zero value.

The feedback tyre force and moment can be determined according to the tracking error dynamics in Eqs. (30–32):

$$F_{x,\mathit{\text{feedback}}}=-K_1{\stackrel{\sim }{v}}_x$$E33

$$F_{y,\mathit{\text{feedback}}}=-K_{2p}{\stackrel{\sim }{v}}_y-K_{2d}{\dot{\stackrel{\sim }{v}}}_y$$E34

$$M_{z,\mathit{\text{feedback}}}=-K_{3p}\stackrel{\sim }{\phi }-K_{3d}\dot{\stackrel{\sim }{\phi }}$$E35

where $K_1,K_{2p},K_{2d},K_{3p}\text{and}{K}_{3d}$ represent feedback control gains.

The feedforward tyre force and moment can be calculated as:

$$F_{x,\mathit{\text{forward}}}=m{\dot{v}}_{\mathit{xd}}-m{\stackrel{\sim }{v}}_y{\dot{\phi }}_d$$E36

$$F_{y,\mathit{\text{forward}}}=m{v}_{\mathit{xd}}{\dot{\phi }}_d+m{\stackrel{\sim }{v}}_x{\dot{\phi }}_d$$E37

$$M_{z,\mathit{\text{forward}}}=I_z{\ddot{\phi }}_d$$E38

The vehicle total desired longitudinal force $F_{x,\mathit{\text{total}}}$, lateral force $F_{y,\mathit{\text{total}}}$ and yaw moment $M_{z,\mathit{\text{total}}}$ can be determined by adding up feedforward and feedback terms:

$$F_{x,\mathit{\text{total}}}=m{\dot{v}}_{\mathit{xd}}-m{\stackrel{\sim }{v}}_y{\dot{\phi }}_d-K_1{\stackrel{\sim }{v}}_x$$E39

$$F_{y,\mathit{\text{total}}}=m{v}_{\mathit{xd}}{\dot{\phi }}_d+m{\stackrel{\sim }{v}}_x{\dot{\phi }}_d-K_{2p}{\stackrel{\sim }{v}}_y-K_{2d}{\dot{\stackrel{\sim }{v}}}_y$$E40

$$M_{z,\mathit{\text{total}}}=I_z{\ddot{\phi }}_d-K_{3p}\stackrel{\sim }{\phi }-K_{3d}\dot{\stackrel{\sim }{\phi }}$$E41

4.2 Trajectory controller in the second layer

In this section, the individual steering and driving control actuators are allocated and controlled to achieve the desired total longitudinal tyre force, the desired total lateral tyre force and desired yaw moment determined in the first layer of trajectory controller. First the individual tyre forces are optimal allocated by the optimisation cost function, and then the allocated tyre forces can be mapped into the individual steering and driving control actuators.

The mathematical equation of cost function of this control allocation and optimisation problem can be shown as follows:

$$\underset{F_{\mathit{ti}},F_{\mathit{si}}}{min}{J}_3=\frac{F_{\mathit{tfl}}^2+F_{\mathit{sfl}}^2}{{\mu }^2{F}_{\mathit{zfl}}^2}+\frac{F_{\mathit{tfr}}^2+F_{\mathit{sfr}}^2}{{\mu }^2{F}_{\mathit{zfr}}^2}+\frac{F_{\mathit{trl}}^2+F_{\mathit{srl}}^2}{{\mu }^2{F}_{\mathit{zrl}}^2}+\frac{F_{\mathit{trr}}^2+F_{\mathit{srr}}^2}{{\mu }^2{F}_{\mathit{zrr}}^2}$$E42

with the constraints of:

$${\mathit{B}}_{\mathit{x}}\mathit{F}=F_{x,\mathit{\text{total}}}$$E43

$${\mathit{B}}_{\mathit{y}}\mathit{F}=F_{y,\mathit{\text{total}}}$$E44

$${\mathit{B}}_{\mathit{r}}\mathit{F}=M_{z,\mathit{\text{total}}}$$E45

where $\mathit{F}={\left[\begin{array}{cc}\begin{array}{cc}\begin{array}{cc}{F}_{\mathit{tfl}}& F_{\mathit{tfr}}\end{array}& \begin{array}{cc}{F}_{\mathit{trl}}& F_{\mathit{trr}}\end{array}\end{array}& \begin{array}{cc}\begin{array}{cc}{F}_{\mathit{sfl}}& F_{\mathit{sfr}}\end{array}& \begin{array}{cc}{F}_{\mathit{srl}}& F_{\mathit{srr}}\end{array}\end{array}\end{array}\right]}^T,$ ${\mathit{B}}_{\mathit{x}}=\left[\begin{array}{cc}\begin{array}{cc}cos{\delta }_{\mathit{fl}}& cos{\delta }_{\mathit{fr}}\end{array}& \begin{array}{cc}cos{\delta }_{\mathit{rl}}& cos{\delta }_{\mathit{rr}}\end{array}\end{array}\right.$ $\left.\begin{array}{cc}\begin{array}{cc}-sin{\delta }_{\mathit{fl}}& -sin{\delta }_{\mathit{fr}}\end{array}& \begin{array}{cc}-sin{\delta }_{\mathit{rl}}& -sin{\delta }_{\mathit{rr}.}\end{array}\end{array}\right]$

${\mathit{B}}_{\mathit{y}}=\left[\begin{array}{cc}\begin{array}{cc}sin{\delta }_{\mathit{fl}}& sin{\delta }_{\mathit{fr}}\end{array}& \begin{array}{cc}sin{\delta }_{\mathit{rl}}& sin{\delta }_{\mathit{rr}}\end{array}\end{array}\right.$ $\left.\begin{array}{cc}\begin{array}{cc}cos{\delta }_{\mathit{fl}}& cos{\delta }_{\mathit{fr}}\end{array}& \begin{array}{cc}cos{\delta }_{\mathit{rl}}& cos{\delta }_{\mathit{rr}}\end{array}\end{array}\right]$

$${\mathit{B}}_{\mathit{r}}=\left[\begin{array}{cc}{l}_{f}sin{\delta }_{\mathit{fl}}+0.5b_{f}cos{\delta }_{\mathit{fl}}& l_{f}sin{\delta }_{\mathit{fr}}-0.5b_{f}cos{\delta }_{\mathit{fr}}\end{array}\right.$$

$$\begin{array}{cc}-l_{r}sin{\delta }_{\mathit{rl}}+0.5b_{r}cos{\delta }_{\mathit{rl}}& -l_{r}sin{\delta }_{\mathit{rr}}-0.5b_{r}cos{\delta }_{\mathit{rr}}\end{array}$$

$$\begin{array}{cc}{l}_{f}cos{\delta }_{\mathit{fl}}-0.5b_{f}sin{\delta }_{\mathit{fl}}& l_{f}cos{\delta }_{\mathit{fr}}+0.5b_{r}sin{\delta }_{\mathit{fr}}\end{array}$$

$$\left.\begin{array}{cc}-l_{r}cos{\delta }_{\mathit{rl}}-0.5b_{r}sin{\delta }_{\mathit{rl}}& -l_{r}cos{\delta }_{\mathit{rr}}+0.5b_{r}sin{\delta }_{\mathit{rr}}\end{array}\right]$$

$$F_{\mathit{ti}}^2+F_{\mathit{si}}^2\le \mu F_{\mathit{zi}}^2$$E46

where the optimisation variables of this cost function are individual tyre forces $F_{\mathit{ti}}$, and $F_{\mathit{si}}$. $F_{x,\mathit{\text{total}}},F_{y,\mathit{\text{total}}}\text{and}{M}_{z,\mathit{\text{total}}}$ are the desired total longitudinal tyre force, lateral tyre force and yaw moment determined in the first layer controller. The effect of tyre friction circle is considered in (46). The constraints (43), (44) and (45) are used to achieve the desired total longitudinal tyre force, lateral tyre force and yaw moment. In order to overcome the model error due to the non-linear characteristic of the vehicle dynamics model, the sliding-mode controller (SMC) is applied and included in constraints (43), (44) and (45) to accurately track the desired total tyre forces and yaw moment. After applying the SMC control law, the following equations are proposed to replace the constraints (43), (44), (45):

$${\mathit{B}}_{\mathit{x}}\mathit{F}=F_{x,\mathit{\text{total}}}-K_{s1}sgn{S}_1$$E47

$${\mathit{B}}_{\mathit{y}}\mathit{F}=F_{y,\mathit{\text{total}}}-K_{s2}sgn{S}_2$$E48

$${\mathit{B}}_{\mathit{r}}\mathit{F}=M_{z,\mathit{\text{total}}}-K_{s3}sgn{S}_3$$E49

where $K_{s1}$,$K_{s2}$ and $K_{s3}$ are positive control gains of SMC. The sliding surface $S_1,S_2$ and $S_3$ in Eqs. (47)–(49) can be presented as follows:

$$S_1=\int {\mathit{B}}_{\mathit{x}}\mathit{F}-F_{x,\mathit{\text{total}}}$$E50

$$S_2=\int {\mathit{B}}_{\mathit{y}}\mathit{F}-F_{y,\mathit{\text{total}}}$$E51

$$S_3=\int {\mathit{B}}_{\mathit{r}}\mathit{F}-M_{z,\mathit{\text{total}}}$$E52

After the individual tyre forces have been optimised and allocated in (42), the controlled values of individual steering and driving actuators can be mapped from the individual tyre force according to the following equations:

$$T_i=F_{\mathit{ti}}{R}_{\omega }$$E53

$${\delta }_{\mathit{fl}}=\frac{F_{\mathit{sfl}}}{C_{\alpha }}+\frac{l_{f}r}{v_x}$$E54

$${\delta }_{\mathit{fr}}=\frac{F_{\mathit{sfr}}}{C_{\alpha }}+\frac{l_{f}r}{v_x}$$E55

$${\delta }_{\mathit{rl}}=\frac{F_{\mathit{srl}}}{C_{\alpha }}-\frac{l_{r}r}{v_x}$$E56

$${\delta }_{\mathit{rr}}=\frac{F_{\mathit{srr}}}{C_{\alpha }}-\frac{l_{r}r}{v_x}$$E57

This controlled actuator values can be input into actual electric vehicle to achieve desired vehicle trajectory.

5. Simulation results

In this section, two sets of simulation results are used to verify the effectiveness of proposed trajectory planner and controller in both highway and off-road scenarios. The simulation parameters are shown in Table 1.

Symbol	Definition	Values
$m$	Mass	1298.9 kg
$l_f$	Distance of CG from the front axle	1.3 m
$l_r$	Distance of CG from the rear axle	1.5 m
$b_f$	Front track width	1.6 m
$b_r$	Rear track width	1.6 m
$C_s$	Longitudinal stiffness of the tyre	50,000 N/unit slip
$C_{\alpha }$	Cornering stiffness of the tyre	30,000 N/unit slip
$I_z$	Vehicle moment of inertial about yaw axle	3900 kgm^2
$I_x$	Vehicle moment of inertial about longitudinal axle	765 kgm^2
$I_y$	Vehicle moment of inertial about lateral axle	3477 kgm^2
$R_{\omega }$	Wheel radius	0.3 m
$I_{\omega }$	Wheel moment of inertial	4 kgm^2
$e$	The distance between the vehicle roll centre and CG	0.4 m
$h$	Height of the vehicle centre of gravity	0.533 m
$K_{\varphi }$	The stiffness of roll axis	89,000

Table 1.

The simulation parameters [18].

In the first set of simulations, the controlled vehicle is overtaking the vehicle ahead in the same lane in the highway scenario. A slow vehicle (with the velocity of 15 m/s) is moving 100 metres ahead of the controlled vehicle (with the velocity of 20 m/s). In order to overtake the slow vehicle, the controlled vehicle first decelerates from 20 m/s into 15 m/s and then makes the lane change to the right lane. After that, the controlled vehicle accelerates from 15 m/s into 20 m/s to go ahead of the overtaken vehicle. Finally, the controlled vehicle goes back to the left lane. The details of this scenario are described in Figure 3(a), and the whole global desired path can be divided by 5 sections. For the purpose of comparison, the control performance of the potential field method based on [26] is also presented here. Furthermore, in order to show the advantage of 4WIS-4WID vehicle model, the proposed trajectory planning and control performance based on two-wheel model is presented and compared.

Figure 3.

(a) Vehicle overtaking scenario in the first set of simulations (unit: m). (b) The vehicle trajectory in the global coordinate system. (c) The relative distance between the overtaking vehicle and overtaken vehicle [21].

In Figure 3(b), the moving trajectory of the overtaking vehicle controlled by both the potential field method and the proposed method based on two-wheel model and 4WIS-4WID model is compared. The proposed method based on two-wheel model and 4WIS-4WID model shows good control performance, and the controlled vehicle is moving within the road boundary. Figure 3(c) shows that the overtaking vehicle and overtaken vehicle maintain the safety distance to avoid collision. Figure 4 demonstrates that the potential field method shows big lateral tracking error compared with the proposed methods based on two-wheel model and four-wheel model, while the longitudinal tracking error of potential filed method is smaller than the proposed method. Since the lateral tracking error is more important than longitudinal tracking error on highway overtaking scenario, the proposed method has better overall tracking performance than potential field method. It is also noted that the tracking error of proposed method based on two-wheel model is larger than four-wheel model, especially for the tracking error of the lateral position. This shows the advantages of 4WIS-4WID model.

Figure 4.

The tracking errors of vehicle trajectory in the first set of simulations: (a) longitudinal position and (b) lateral position [21].

In Figures 5(a) and 5(b), the longitudinal velocity and lateral velocity in the global coordinate system for both the potential field method and the proposed trajectory planning method are presented. $V_{\mathit{xd}1}$, $V_{\mathit{xd}2}$, $V_{\mathit{xd}3}$, $V_{\mathit{xd}4}\text{and}{V}_{\mathit{xd}5}$ are desired longitudinal velocities on each section of road, while $V_{\mathit{yd}1}$, $V_{\mathit{yd}2}$, $V_{\mathit{yd}3}$,$V_{\mathit{yd}4}\text{and}{V}_{\mathit{yd}5}$ are desired lateral velocities on each section of road. The potential field method can only roughly achieve the desired longitudinal velocity and lateral velocity, while the proposed method can accurately achieve desired values. This proves that the proposed method can not only achieve the desired ending positions but also achieve the desired ending velocities. Figure 5(c) and Figure 5(d) present the vehicle yaw rate and body side-slip angle responses, which proves that the proposed trajectory planning method can achieve much better handling and stability performance compared with potential field method.

Figure 5.

The vehicle state in the first set of simulations: (a) longitudinal velocity in the global coordinate system, (b) lateral velocity in the global coordinate system, (c) yaw rate and (d) body slip angle [21].

In the second set of simulations, the autonomous vehicle is assumed to move in the off-road scenario, and the road topography should be considered. Figure 6 presents the scenario in the second set of simulations: in a particular section of the road, the vehicle start position is (0, 0) and the target ending position is constrained by a certain boundary (90–110, 20–30); the initial and ending longitudinal velocity is 5 m/s, and the initial and ending lateral velocity is 0 and 3 m/s, respectively. The bank angle and road slope of this section of road is shown in Figure 7. The trajectory planner proposed in Eq. (26) will choose the best suitable ending position and vehicle trajectory by considering the road topography information (minimising the bank angle and road slope). The vehicle dynamics response of the trajectory planner which has not considered the road topography information proposed in Eq. (23) is also shown and compared. It is noted that trajectory planner without considering road topography is briefly called ‘trajectory planner 1’ and trajectory planner considering road topography is briefly called ‘trajectory planner 4’. Figure 8 compares the bank angle and road slope of the desired trajectories planned by trajectory planner 1 and trajectory planner 4 and proves that the trajectory planner 4 can generate the trajectory with smaller bank angle and road slope. Figure 9 shows the trajectory tracking performance when trajectory planner 4 applied is much improved compared with trajectory planner 1. Figure 10 shows the dynamics responses between trajectory planner 1 and trajectory planner 4. Figure 10(a) suggests that the undesired lateral velocity is reduced a lot when trajectory planner 4 has been applied. Figure 10(b) and Figure 10(c) prove that the autonomous vehicle has smoother roll angle and pitch angle response when trajectory planner 4 is applied since the road bank angle and road slope is minimised compared with the situation when trajectory planner 1 is applied.

Figure 6.

The vehicle off-road scenario in the second set of simulations (unit: mm).

Figure 7.

The vehicle (a) road slope and (b) bank angle of the one particular section of uneven road surface in the second set of simulations.

Figure 8.

The actual (a) bank angle and (b) road slope in the second set of simulations.

Figure 9.

The desired trajectory tracking performance in the second set of simulations.

Figure 10.

Vehicle dynamics responses in the second set of simulations: (a) lateral velocity, (b) roll angle and (c) pitch angle.

6. Conclusion

In this chapter, a dynamically integrated spatiotemporal-based trajectory planning and control method for the off-road autonomous vehicles is proposed. The upper-level trajectory planner can select the best time-parameterised trajectory among a group of the candidate trajectories by considering the road topography information. Then, the lower-level trajectory controller can control the motion of the vehicle and achieve the desired trajectory.

Simulation results have proved that the proposed trajectory planning and control method can successfully control the motion of autonomous vehicles and achieve the spatiotemporal-based desired trajectory while satisfying the target ending position and velocity. In the highway scenario, the proposed method has better overall position tracking control performance and can better achieve the desired longitudinal and lateral velocity compared with the conventional potential field method. In addition, the 4WIS-4WID vehicle shows better tracking control performance than traditional vehicle based on two-wheel model.

In the off-road scenario, the proposed trajectory planning method can successfully find a specific trajectory which can avoid the peak values of bank angle and road slope. Simulation results prove that the proposed trajectory planner when considering the road topography information can generate the trajectory with much smaller bank angle and road slope compared with trajectory generated by traditional trajectory planner. The actual trajectory tracking performance, roll stability and pitch stability performance can be improved by using the proposed trajectory planning method to minimise the effect of road topography on vehicle dynamics.

Conflict of interest

We declare that there is no conflict of interest in this chapter.