2. Vehicle dynamics modelling

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 spatialbased 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–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

Path Planning for Autonomous Vehicles - Ensuring Reliable Driverless Navigation…

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 spatiotemporalbased trajectory planning and control method by considering the off-road scenario. The major innovative part of this chapter is the development of the spatiotemporal-

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.

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

constraints are satisfied.

vehicle.

30

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.

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

$$
\dot{m}\dot{v}\_x = mv\_\gamma r + \left(F\_{x\emptyset} + F\_{x\emptyset r} + F\_{x\emptyset} + F\_{xrr}\right) + mg\sin\theta\_s \tag{1}
$$

Lateral motion:

$$
\dot{m}\dot{v}\_{\gamma} = -m v\_{\rm x} r + \left(F\_{\gamma \rm l} + F\_{\gamma \rm fr} + F\_{\gamma \rm l} + F\_{\gamma \rm rr}\right) + m \mathbf{g} \sin \theta\_b \tag{2}
$$

Yaw motion:

$$I\_x \dot{r} = l\_f (F\_{yfl} + F\_{yfr}) - l\_r (F\_{yrl} + F\_{yrr}) + \frac{b\_f}{2} \left( F\_{xfl} - F\_{xfr} \right) + \frac{b\_r}{2} (F\_{xrl} - F\_{xrr}) \tag{3}$$

Roll motion:

$$I\_x \ddot{\phi} = -me\_r \dot{\nu}\_y - me\_r \nu\_x r + mge\_r \sin \phi - K\_\phi \phi - C\_\phi \dot{\phi} \tag{4}$$

Pitch motion:

$$I\_{\mathbf{y}}\ddot{\boldsymbol{\rho}} = -m\boldsymbol{e}\_{p}\dot{\boldsymbol{v}}\_{\mathbf{x}} - m\boldsymbol{e}\_{p}\boldsymbol{v}\_{\mathbf{y}}\boldsymbol{r} + m\boldsymbol{g}\boldsymbol{e}\_{p}\sin\boldsymbol{\varrho} - \boldsymbol{K}\_{\boldsymbol{\varrho}}\boldsymbol{\varrho} - \boldsymbol{C}\_{\boldsymbol{\varrho}}\dot{\boldsymbol{\rho}}\tag{5}$$

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

where vx, vy and r are the vehicle longitudinal velocity, lateral velocity and yaw rate. θ<sup>s</sup> shows the road slope, and θ<sup>b</sup> represents the road bank angle. bf and br represent the front and rear track width. lf is the length of front wheel base, and lr is the length of rear wheel base. Iz represents the moment of yaw inertia, and m is vehicle mass. Fxfl and Fxfr represent the longitudinal tyre force of front left and front right tyre, while Fxrl and Fxrr present the longitudinal tyre force of rear left and rear right wheel. Fyfl and Fyfr present the lateral tyre force of front left and front right tyre, while Fyrl and Fyrr present the lateral tyre force of rear left and rear right wheel. ϕ and φ represent the vehicle roll angle and pitch angle, respectively. er is the distance from the vehicle centre of gravity (CG) to the roll centre, and ep is the distance from the vehicle CG to the pitch motion centre. K<sup>ϕ</sup> is the roll axis torsional stiffness, and C<sup>ϕ</sup> is the roll axis torsional damping. K<sup>φ</sup> is the pitch axis torsional stiffness, and C<sup>φ</sup> is the pitch axis torsional damping.

The tyre side force Fsi and traction or brake force Fti can be transferred to the longitudinal force Fxi and the lateral tyre force Fyi as follows:

$$\begin{aligned} F\_{xi} &= F\_{ti}\cos\delta\_i - F\_{si}\sin\delta\_i\\ F\_{yi} &= F\_{ti}\sin\delta\_i + F\_{si}\cos\delta\_i \end{aligned} \tag{6}$$

where ω<sup>i</sup> presents the wheel angular velocity of each wheel and Ti presents the traction or brake torque of each wheel. R<sup>ω</sup> is the wheel radius, and I<sup>ω</sup> is the wheel

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

<sup>v</sup>\_ yh � ger sin <sup>ϕ</sup> � � <sup>þ</sup>

<sup>v</sup>\_ yh � ger sin <sup>ϕ</sup> � � <sup>þ</sup>

<sup>v</sup>\_yh � ger sin <sup>ϕ</sup> � � � <sup>1</sup>

<sup>v</sup>\_yh � ger sin <sup>ϕ</sup> � � � <sup>1</sup>

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]. 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

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

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,

� � (14)

� � (15)

!

!

1 2

1 2

2

2

gep sin φ lf þ lr

gep sin φ lf þ lr

gep sin φ lf þ lr

gep sin φ lf þ lr

� �<sup>2</sup>

� �<sup>2</sup>

� �<sup>2</sup>

� �<sup>2</sup>

(12)

(13)

following equations by including the load transfer model:

Path Planning for Autonomous Vehicle in Off-Road Scenario

DOI: http://dx.doi.org/10.5772/intechopen.85384

<sup>v</sup>\_xh � lr bf

<sup>v</sup>\_xh � lf br

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

lr bf

moment of inertial.

Fzfl <sup>¼</sup> <sup>m</sup>

Fzfr <sup>¼</sup> <sup>m</sup>

Fzrl <sup>¼</sup> <sup>m</sup>

Fzrl <sup>¼</sup> <sup>m</sup>

advance.

33

lf þ lr

lf þ lr

lf þ lr

lf þ lr

1 2 glr � <sup>1</sup> 2 v\_xh þ

1 2 glr � <sup>1</sup> 2

1 2 glf þ 1 2 v\_xh þ lf br

1 2 glf þ 1 2

3. Upper-level trajectory planner

level task planner based on digital map [22–24].

3.1 Generate the candidate trajectory set

where i ¼ fl, fr, rl and 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\_{\rm si} = \frac{\mathbf{C}\_a \tan \alpha\_i}{\mathbf{1} - \mathbf{s}\_i} f(\lambda\_i) \tag{7}$$

Tyre traction or brake force:

$$F\_{ti} = \frac{\mathbf{C}\_{t}\mathbf{s}\_{i}}{\mathbf{1} - \mathbf{s}\_{i}}f(\lambda\_{i})\tag{8}$$

λ<sup>i</sup> in Eqs. (7) and (8) can be determined by the following equation:

$$\lambda\_i = \frac{\mu F\_{xi} \left[ 1 - \varepsilon\_r \mu\_i \sqrt{s\_i^2 + \tan^2 \alpha\_i} \right] (1 - s\_i)}{2 \sqrt{C\_s^2 s\_i^2 + C\_a^2 \tan^2 \alpha\_i}} \tag{9}$$

fð Þ λ<sup>i</sup> in Eqs. (7) and (8) can be determined by the following equation:

$$f(\lambda\_i) = \begin{cases} \lambda\_i (2 - \lambda\_i)(\lambda\_i < 1) \\ \mathbf{1} \left( \lambda\_i > 1 \right) \end{cases} \tag{10}$$

where C<sup>α</sup> represents the lateral cornering stiffness and Cs is the longitudinal cornering stiffness. The tyre-road friction coefficient can be represented as μ, and Fzi represents the individual wheel vertical load. α<sup>i</sup> represents the lateral side-slip angle, and si is the longitudinal slip ratio. ui represents the vehicle longitudinal velocity in the individual wheel plane. ε<sup>r</sup> is the road adhesive reduction factor, which is a constant value.

The following equation shows the wheel rotation dynamics:

$$I\_{oo}\dot{o}\_{i} = -R\_{o}F\_{ti} + T\_{i} \tag{11}$$

Path Planning for Autonomous Vehicle in Off-Road Scenario DOI: http://dx.doi.org/10.5772/intechopen.85384

where vx, vy and r are the vehicle longitudinal velocity, lateral velocity and yaw

The tyre side force Fsi and traction or brake force Fti can be transferred to the

Fxi ¼ Fti cos δ<sup>i</sup> � Fsi sin δ<sup>i</sup> Fyi ¼ Fti sin δ<sup>i</sup> þ Fsi cos δ<sup>i</sup>

where i ¼ fl, fr, rl and rr, which represents the front left wheel, front right

Fsi <sup>¼</sup> <sup>C</sup><sup>α</sup> tan <sup>α</sup><sup>i</sup> 1 � si

> Fti <sup>¼</sup> Cssi 1 � si

> > C2 s s 2 <sup>i</sup> <sup>þ</sup> <sup>C</sup><sup>2</sup>

fð Þ λ<sup>i</sup> in Eqs. (7) and (8) can be determined by the following equation:

�

ffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffi

ffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffi

λið Þ 2 � λ<sup>i</sup> ð Þ λ<sup>i</sup> < 1

<sup>i</sup> þ tan <sup>2</sup>α<sup>i</sup> h i <sup>p</sup> ð Þ <sup>1</sup> � si

<sup>α</sup> tan <sup>2</sup>α<sup>i</sup>

s 2

1 ð Þ λi>1

where C<sup>α</sup> represents the lateral cornering stiffness and Cs is the longitudinal cornering stiffness. The tyre-road friction coefficient can be represented as μ, and Fzi represents the individual wheel vertical load. α<sup>i</sup> represents the lateral side-slip angle, and si is the longitudinal slip ratio. ui represents the vehicle longitudinal velocity in the individual wheel plane. ε<sup>r</sup> is the road adhesive reduction factor,

λ<sup>i</sup> in Eqs. (7) and (8) can be determined by the following equation:

μFzi 1 � εrui

fð Þ¼ λ<sup>i</sup>

The following equation shows the wheel rotation dynamics:

2

The non-linear Dugoff tyre model is used in this chapter [19], and tyre traction

(6)

(10)

fð Þ λ<sup>i</sup> (7)

fð Þ λ<sup>i</sup> (8)

<sup>q</sup> (9)

Iωω\_ <sup>i</sup> ¼ �RωFti þ Ti (11)

rate. θ<sup>s</sup> shows the road slope, and θ<sup>b</sup> represents the road bank angle. bf and br represent the front and rear track width. lf is the length of front wheel base, and lr is the length of rear wheel base. Iz represents the moment of yaw inertia, and m is vehicle mass. Fxfl and Fxfr represent the longitudinal tyre force of front left and front right tyre, while Fxrl and Fxrr present the longitudinal tyre force of rear left and rear right wheel. Fyfl and Fyfr present the lateral tyre force of front left and front right tyre, while Fyrl and Fyrr present the lateral tyre force of rear left and rear right wheel. ϕ and φ represent the vehicle roll angle and pitch angle, respectively. er is the distance from the vehicle centre of gravity (CG) to the roll centre, and ep is the distance from the vehicle CG to the pitch motion centre. K<sup>ϕ</sup> is the roll axis torsional stiffness, and C<sup>ϕ</sup> is the roll axis torsional damping. K<sup>φ</sup> is the pitch axis torsional

Path Planning for Autonomous Vehicles - Ensuring Reliable Driverless Navigation…

stiffness, and C<sup>φ</sup> is the pitch axis torsional damping.

wheel, rear left wheel and rear right wheel.

Tyre traction or brake force:

λ<sup>i</sup> ¼

Tyre side force:

which is a constant value.

32

longitudinal force Fxi and the lateral tyre force Fyi as follows:

or brake force and side force of each wheel are described by:

where ω<sup>i</sup> presents the wheel angular velocity of each wheel and Ti presents the traction or brake torque of each wheel. R<sup>ω</sup> is the wheel radius, and I<sup>ω</sup> 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\_{\rm xfl} = \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 \phi \right) + \frac{1}{2} g e\_p \sin \phi \left( l\_f + l\_r \right)^2 \right) \tag{12}$$

$$F\_{\pi\circ r} = \frac{m}{l\_f + l\_r} \left(\frac{1}{2}g l\_r - \frac{1}{2}\dot{\nu}\_x h - \frac{l\_r}{b\_f} \left(\dot{\nu}\_y h - g e\_r \sin\phi\right) + \frac{1}{2} g e\_p \sin\phi \,\Big|\,\rho \left(l\_f + l\_r\right)^2\right) \tag{13}$$

$$F\_{xrl} = \frac{m}{l\_f + l\_r} \left(\frac{1}{2}gl\_f + \frac{1}{2}\dot{v}\_x h + \frac{l\_f}{b\_r} \left(\dot{v}\_y h - g e\_r \sin\phi\right) - \frac{1}{2}g e\_p \sin\rho \left(l\_f + l\_r\right)^2\right) \tag{14}$$

$$F\_{xrl} = \frac{m}{l\_f + l\_r} \left(\frac{1}{2}gl\_f + \frac{1}{2}\dot{v}\_x h - \frac{l\_f}{b\_r} \left(\dot{v}\_y h - g e\_r \sin\phi\right) - \frac{1}{2}g e\_p \sin\rho \left(l\_f + l\_r\right)^2\right) \tag{15}$$

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].

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–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,

\_

DOI: http://dx.doi.org/10.5772/intechopen.85384

Path Planning for Autonomous Vehicle in Off-Road Scenario

For the acceleration of candidate trajectory:

where M1ðÞ¼ t

calculated as follows:

where M1ð Þ¼ 0

with late arrival of final states.

35

€

1 t t<sup>2</sup> 012t 00 2

c<sup>345</sup> ¼

100 010 002

proposed optimisation cost function in the next section.

min <sup>d</sup>1, <sup>τ</sup>

3.2 Determine the optimisation cost function

2 6 4

2 6 4 d<sup>1</sup> ¼ c<sup>1</sup> þ 2c2t þ 3c3t

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

3 7

c<sup>012</sup> ¼

c3 c4 c5

3 7 5 3 7

2 6 4

d<sup>1</sup> ¼ 2c<sup>2</sup> þ 6c3t þ 12c4t

tory and τ ∈ ½ � 0 T . T is the longest time required to complete the motion.

<sup>5</sup>, <sup>M</sup>2ðÞ¼ <sup>t</sup>

c0 c1 c2

and ξ<sup>0</sup> ¼

3 7

2 6 4 <sup>2</sup> <sup>þ</sup> <sup>4</sup>c4<sup>t</sup>

with c0, c1, …, c<sup>5</sup> ∈R and t ∈½ � 0 τ . τ is the terminal time of the candidate trajec-

t <sup>3</sup> t

2 6 4

The coefficients c<sup>012</sup> and c<sup>345</sup> of the quintic state trajectory in Eq. (19) can be

3t <sup>2</sup> 4t

<sup>5</sup> <sup>¼</sup> <sup>M</sup>1ð Þ <sup>0</sup> �<sup>1</sup>

d0 \_ d0 € d0 3 7 5:

2 6 4

After the coefficients c<sup>012</sup> and c<sup>345</sup> are calculated, the vehicle trajectory can be described as d1ð Þt 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

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:

where this cost function has two optimization variables, the ending position d<sup>1</sup> and terminal time τ. This cost function also includes two terms, and k<sup>τ</sup> and kd 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. dr is the reference vehicle ending state. dr � d1ð Þτ 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

6t 12t

<sup>3</sup> <sup>þ</sup> <sup>5</sup>c5<sup>t</sup>

<sup>2</sup> <sup>þ</sup> <sup>20</sup>c5<sup>t</sup>

ξtðÞ¼ t M1ð Þt c<sup>012</sup> þ M2ð Þt c<sup>345</sup> (19)

<sup>4</sup> t 5 3 7 5

<sup>3</sup> 5t 4

<sup>2</sup> 20t 3

<sup>5</sup> <sup>¼</sup> <sup>M</sup>2ð Þ<sup>τ</sup> �<sup>1</sup> <sup>ξ</sup>½ � <sup>t</sup> � <sup>M</sup>1ð Þ<sup>τ</sup> <sup>c</sup><sup>012</sup> (21)

<sup>J</sup><sup>1</sup> <sup>¼</sup> <sup>k</sup>ττ <sup>þ</sup> kdð Þ dr � <sup>d</sup>1ð Þ<sup>τ</sup> <sup>2</sup> (22)

<sup>4</sup> (17)

<sup>3</sup> (18)

and ξtðÞ¼ t

ξ<sup>0</sup> (20)

d1ð Þt \_ d1ð Þt € d1ð Þt 3 7 5.

2 6 4

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

lateral position and lateral velocity). The start state is assumed as d<sup>0</sup> \_ <sup>d</sup><sup>0</sup> € d0 h i, and the desired ending state is assumed as d<sup>1</sup> \_ <sup>d</sup><sup>1</sup> € d1 h i. <sup>d</sup><sup>0</sup> is the initial vehicle position, and d<sup>1</sup> is a group of offset positions from reference ending position, and these offset positions are constrained within the road boundary. \_ d<sup>0</sup> and € d<sup>0</sup> present the initial velocity and acceleration, while \_ d<sup>1</sup> and € d<sup>1</sup> 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<sup>0</sup> 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 d1<sup>i</sup> and terminal time τ<sup>j</sup> can be generated [17], where i, j means that the number of i � j trajectories will be generated by the trajectory planner. d1<sup>i</sup> represents i number of final positions and will close to the target ending position when d1<sup>i</sup> ! d1. τ<sup>j</sup> represents the j number of candidate terminal time. The optimisation algorithm presented in the later section will choose the best trajectory from these i � j trajectories.

It can be assumed that the candidate vehicle trajectory dð Þτ 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 \tag{16}$$

For the velocity of candidate trajectory:

Path Planning for Autonomous Vehicle in Off-Road Scenario DOI: http://dx.doi.org/10.5772/intechopen.85384

$$\dot{d}\_1 = c\_1 + 2c\_2t + 3c\_3t^2 + 4c\_4t^3 + 5c\_5t^4 \tag{17}$$

For the acceleration of candidate trajectory:

$$
\ddot{d}\_1 = 2c\_2 + 6c\_3t + 12c\_4t^2 + 20c\_5t^3 \tag{18}
$$

with c0, c1, …, c<sup>5</sup> ∈R and t ∈½ � 0 τ . τ is the terminal time of the candidate trajectory and τ ∈ ½ � 0 T . T is the longest time required to complete the motion.

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

$$\mathfrak{E}\_{\mathbf{t}}(t) = \mathbf{M}\_1(t)\mathbf{c}\_{012} + \mathbf{M}\_2(t)\mathbf{c}\_{345} \tag{19}$$

$$\begin{aligned} \text{where } \mathbf{M\_1}(t) = \begin{bmatrix} 1 & t & t^2 \\ 0 & 1 & 2t \\ 0 & 0 & 2 \end{bmatrix}, \mathbf{M\_2}(t) = \begin{bmatrix} t^3 & t^4 & t^5 \\ 3t^2 & 4t^3 & 5t^4 \\ 6t & 12t^2 & 20t^3 \end{bmatrix} \text{ and } \mathbf{\dot{\xi}\_t}(t) = \begin{bmatrix} d\_1(t) \\ \dot{d}\_1(t) \\ \ddot{d}\_1(t) \end{bmatrix}. \end{aligned}$$

The coefficients c<sup>012</sup> and c<sup>345</sup> of the quintic state trajectory in Eq. (19) can be calculated as follows:

$$\mathbf{c\_{012}} = \begin{bmatrix} c\_0 \\ c\_1 \\ c\_2 \end{bmatrix} = \mathbf{M\_1}(\mathbf{0})^{-1} \mathbf{f\_0} \tag{20}$$

$$\mathbf{c\_{345}} = \begin{bmatrix} c\_3 \\ c\_4 \\ c\_5 \end{bmatrix} = \mathbf{M\_2}(\tau)^{-1} [\mathbf{\tilde{g}}\_t - \mathbf{M\_1}(\tau) \mathbf{c\_{012}}] \tag{21}$$

$$\text{where } \mathbf{M\_1}(\mathbf{0}) = \begin{bmatrix} \mathbf{1} & \mathbf{0} & \mathbf{0} \\ \mathbf{0} & \mathbf{1} & \mathbf{0} \\ \mathbf{0} & \mathbf{0} & \mathbf{2} \end{bmatrix} \text{ and } \mathbf{f\_0} = \begin{bmatrix} d\_0 \\ \dot{d}\_0 \\ \ddot{d}\_0 \end{bmatrix}.$$

After the coefficients c<sup>012</sup> and c<sup>345</sup> are calculated, the vehicle trajectory can be described as d1ð Þt 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:

$$\min\_{d\_{1\mathfrak{p}}\mathfrak{r}} J\_1 = k\_{\mathfrak{r}}\mathfrak{r} + k\_d (d\_{\mathfrak{r}} - d\_1(\mathfrak{r}))^2 \tag{22}$$

where this cost function has two optimization variables, the ending position d<sup>1</sup> and terminal time τ. This cost function also includes two terms, and k<sup>τ</sup> and kd 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. dr is the reference vehicle ending state. dr � d1ð Þτ 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.

lateral position and lateral velocity). The start state is assumed as d<sup>0</sup> \_

Path Planning for Autonomous Vehicles - Ensuring Reliable Driverless Navigation…

these offset positions are constrained within the road boundary. \_

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

<sup>d</sup><sup>1</sup> € d1

h i

d<sup>1</sup> and €

position, and d<sup>1</sup> is a group of offset positions from reference ending position, 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<sup>0</sup> in current section of

and will close to the target ending position when d1<sup>i</sup> ! d1. τ<sup>j</sup> represents the j number of candidate terminal time. The optimisation algorithm presented in the

later section will choose the best trajectory from these i � j trajectories.

d<sup>1</sup> ¼ c<sup>0</sup> þ c1t þ c2t

In each section of the road, when the initial and ending state values are determined, the candidate trajectories with different ending conditions d1<sup>i</sup> and terminal time τ<sup>j</sup> can be generated [17], where i, j means that the number of i � j trajectories will be generated by the trajectory planner. d1<sup>i</sup> represents i number of final positions

It can be assumed that the candidate vehicle trajectory dð Þτ in the optimisation of trajectory planning can be described by the following quintic state equations [17]:

<sup>2</sup> <sup>þ</sup> <sup>c</sup>3<sup>t</sup>

<sup>3</sup> <sup>þ</sup> <sup>c</sup>4<sup>t</sup>

<sup>4</sup> <sup>þ</sup> <sup>c</sup>5<sup>t</sup>

the desired ending state is assumed as d<sup>1</sup> \_

Figure 2.

34

the initial velocity and acceleration, while \_

road should be the ending state of previous section.

For the position of candidate trajectory:

For the velocity of candidate trajectory:

<sup>d</sup><sup>0</sup> € d0 h i

d<sup>0</sup> present

. d<sup>0</sup> is the initial vehicle

d<sup>0</sup> and €

<sup>5</sup> (16)

d<sup>1</sup> present the ending velocity and

, and

Path Planning for Autonomous Vehicles - Ensuring Reliable Driverless Navigation…

Furthermore, the vehicle longitudinal or lateral jerk (presented as d⃛ ð Þτ ) should be minimised to improve the smoothness of the trajectory. The total optimisation cost function J<sup>1</sup> of the trajectory planning can be augmented as:

$$\min\_{d\_{\mathfrak{U}}, \tau} J\_1 = k\_l \left(\ddot{d}\_1(\tau)\right)^2 + k\_r \tau + k\_d (d\_r - d\_1(\tau))^2 \tag{23}$$

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

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

The desired yaw angle φ<sup>d</sup> and longitudinal velocity vxd in the body-fixed coordinate system can be determined according to the following optimisation cost

<sup>J</sup><sup>2</sup> <sup>¼</sup> a vð Þ xdð Þ� <sup>k</sup> vxd�<sup>b</sup>ð Þ<sup>k</sup> <sup>2</sup> <sup>þ</sup> b vxd tan <sup>φ</sup>dð Þ� <sup>k</sup> vyd�<sup>b</sup>ð Þ<sup>k</sup> <sup>2</sup> <sup>þ</sup> <sup>c</sup>ð Þ <sup>φ</sup>dð Þ� <sup>k</sup> <sup>φ</sup>dð Þ <sup>k</sup> � <sup>1</sup> <sup>2</sup>

vxd�<sup>b</sup> ¼ vxd�<sup>g</sup> cos φ þ vyd�<sup>g</sup> sin φ (28) vyd�<sup>b</sup> ¼ vxd�<sup>g</sup> sin φ � vyd�<sup>g</sup> cos φ (29)

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 kð Þ, and k � 1 represents the time step t kð Þ � 1 . vxd�<sup>b</sup> and vyd�<sup>b</sup> represent the desired longitudinal velocity and lateral velocity in the body-fixed coordinate system, which can be calculated according to the desired longitudinal velocity vxd�<sup>g</sup> and lateral velocity vyd�<sup>g</sup> in the global coor-

where the desired longitudinal velocity vxd�<sup>g</sup> and lateral velocity vyd�<sup>g</sup> along the desired trajectory in the global coordinate system can be determined according to

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

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

(27)

3.3 Map planned trajectory into vehicle dynamics control targets

longitudinal velocity in the body-fixed coordinate system.

Path Planning for Autonomous Vehicle in Off-Road Scenario

DOI: http://dx.doi.org/10.5772/intechopen.85384

will increase a lot.

function:

dinate system:

Eqs. (17), (26).

37

controller in the next section.

4. Lower-level trajectory tracking controller

min <sup>φ</sup>d, vxd

where kJ is the scaling factor of the term related to longitudinal or lateral jerk. It can be noted that the target final velocity \_ d<sup>1</sup> or acceleration € d<sup>1</sup> can be used in (23) instead of d<sup>1</sup> 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 θ<sup>s</sup> and bank angle θ<sup>b</sup> along one particular candidate trajectory could be calculated as the following equation:

$$\overline{\theta}\_{\circ} = \frac{\sum\_{i=1}^{n} \theta\_{\circ}(\mathbf{x}\_{i}, \mathbf{y}\_{i})}{n} \tag{24}$$

$$\overline{\theta}\_b = \frac{\sum\_{i=1}^n \theta\_b(\mathbf{x}\_i, \mathbf{y}\_i)}{n} \tag{25}$$

where θ<sup>s</sup> xi; yi and <sup>θ</sup><sup>b</sup> xi; yi 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{aligned} \min\_{d\_{\mathcal{D}}, \tau} J\_1 &= k\_{\mathcal{I}} \left( \ddot{d}\_1(\tau) \right)^2 + k\_{\tau} \tau + k\_d (d\_{\tau} - d\_1(\tau))^2 + k\_{\tau} \overline{\theta}\_i(d\_1(\tau)) + k\_{sd} \dot{\overline{\theta}}\_i(d\_1(\tau)) \\ &+ k\_b \overline{\theta}\_b(d\_1(\tau)) + k\_{bd} \dot{\overline{\theta}}\_b(d\_1(\tau)) \end{aligned} \tag{26}$$

where this cost function have four additional cost function terms compared with cost function (23). The terms ksθsð Þ d1ð Þτ and kbθbð Þ d1ð Þτ are designed to minimise the road slope and bank angle along the selected trajectory. ksd \_ <sup>θ</sup><sup>s</sup> dx, <sup>y</sup>ð Þ<sup>τ</sup> and kbd \_ <sup>θ</sup><sup>b</sup> dx, <sup>y</sup>ð Þ<sup>τ</sup> are designed to prevent the abrupt change of road slope and bank angle. ks, ksd and kb, kbd are scaling factors of each term.

When the optimisation values of ending position d<sup>1</sup> and terminal time τ 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 Path Planning for Autonomous Vehicle in Off-Road Scenario DOI: http://dx.doi.org/10.5772/intechopen.85384

Furthermore, the vehicle longitudinal or lateral jerk (presented as d⃛

Path Planning for Autonomous Vehicles - Ensuring Reliable Driverless Navigation…

<sup>1</sup>ð Þ<sup>τ</sup> <sup>2</sup>

instead of d<sup>1</sup> if the final velocity or acceleration is required to be optimised.

angle between current and previous time step should be minimised.

<sup>θ</sup><sup>s</sup> <sup>¼</sup> <sup>∑</sup><sup>n</sup>

<sup>θ</sup><sup>b</sup> <sup>¼</sup> <sup>∑</sup><sup>n</sup>

θbð Þ d1ð Þτ

the road slope and bank angle along the selected trajectory. ksd \_

angle. ks, ksd and kb, kbd are scaling factors of each term.

along the candidate trajectory. n is the total number of discrete points along this

cost function J<sup>1</sup> of the trajectory planning can be augmented as:

J<sup>1</sup> ¼ kJ d⃛

min d1, <sup>τ</sup>

can be noted that the target final velocity \_

where θ<sup>s</sup> xi; yi

candidate trajectory.

J<sup>1</sup> ¼ kJ d⃛

min <sup>d</sup>1, <sup>τ</sup>

kbd \_

36

to Eqs. (16)–(18).

and <sup>θ</sup><sup>b</sup> xi; yi

<sup>1</sup>ð Þ<sup>τ</sup> <sup>2</sup>

<sup>þ</sup>kbθbð Þþ <sup>d</sup>1ð Þ<sup>τ</sup> kbd \_

be minimised to improve the smoothness of the trajectory. The total optimisation

where kJ is the scaling factor of the term related to longitudinal or lateral jerk. It

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

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 θ<sup>s</sup> and bank angle θ<sup>b</sup> along one particular candidate trajectory could be calculated as the following equation:

> <sup>i</sup>¼<sup>1</sup>θ<sup>s</sup> xi; yi

n

<sup>i</sup>¼<sup>1</sup>θ<sup>b</sup> xi; yi 

n

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

<sup>þ</sup> <sup>k</sup>ττ <sup>þ</sup> kdð Þ dr � <sup>d</sup>1ð Þ<sup>τ</sup> <sup>2</sup> <sup>þ</sup> ksθsð Þþ <sup>d</sup>1ð Þ<sup>τ</sup> ksd \_

where this cost function have four additional cost function terms compared with cost function (23). The terms ksθsð Þ d1ð Þτ and kbθbð Þ d1ð Þτ are designed to minimise

<sup>θ</sup><sup>b</sup> dx, <sup>y</sup>ð Þ<sup>τ</sup> are designed to prevent the abrupt change of road slope and bank

When the optimisation values of ending position d<sup>1</sup> and terminal time τ are determined based on (26), the desired best trajectory can be determined according

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

are the road slope and bank angle at a specific point

d<sup>1</sup> or acceleration €

ð Þτ ) should

(24)

(25)

(26)

θsð Þ d1ð Þτ

<sup>θ</sup><sup>s</sup> dx, <sup>y</sup>ð Þ<sup>τ</sup> and

d<sup>1</sup> can be used in (23)

<sup>þ</sup> <sup>k</sup>ττ <sup>þ</sup> kdð Þ dr � <sup>d</sup>1ð Þ<sup>τ</sup> <sup>2</sup> (23)

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 φ<sup>d</sup> and longitudinal velocity vxd in the body-fixed coordinate system can be determined according to the following optimisation cost function:

$$\min\_{\rho\_{\text{dry}}} J\_2 = a \left( \upsilon\_{\text{xd}}(\mathbf{k}) - \upsilon\_{\text{xd}-b}(\mathbf{k}) \right)^2 + b \left( \upsilon\_{\text{xd}} \tan \rho\_d(\mathbf{k}) - \upsilon\_{\text{yd}-b}(\mathbf{k}) \right)^2 + c \left( \rho\_d(\mathbf{k}) - \rho\_d(\mathbf{k} - \mathbf{1}) \right)^2 \tag{27}$$

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 kð Þ, and k � 1 represents the time step t kð Þ � 1 . vxd�<sup>b</sup> and vyd�<sup>b</sup> represent the desired longitudinal velocity and lateral velocity in the body-fixed coordinate system, which can be calculated according to the desired longitudinal velocity vxd�<sup>g</sup> and lateral velocity vyd�<sup>g</sup> in the global coordinate system:

$$
\sigma\_{\text{xd}-b} = \upsilon\_{\text{xd}-\text{g}} \cos \rho + \upsilon\_{\text{yd}-\text{g}} \sin \rho \tag{28}
$$

$$
\sigma\_{yd-b} = \upsilon\_{\text{xd}-\text{g}} \sin \rho - \upsilon\_{\text{yd}-\text{g}} \cos \rho \tag{29}
$$

where the desired longitudinal velocity vxd�<sup>g</sup> and lateral velocity vyd�<sup>g</sup> 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]:

$$\widetilde{\boldsymbol{\nu}}\_{\mathcal{Y}} = \left[ \boldsymbol{v}\_{\text{x}} \sin \widetilde{\boldsymbol{\rho}} + \boldsymbol{v}\_{\text{y}} \cos \widetilde{\boldsymbol{\rho}} \right] - \boldsymbol{v}\_{\text{yd}} \tag{30}$$

$$\tilde{\boldsymbol{v}}\_{\mathbf{x}} = \left[ \boldsymbol{v}\_{\mathbf{x}} \cos \tilde{\boldsymbol{\rho}} - \boldsymbol{v}\_{\mathbf{y}} \sin \tilde{\boldsymbol{\rho}} \right] - \boldsymbol{v}\_{\mathbf{x}d} \tag{31}$$

$$
\tilde{\boldsymbol{\rho}} = \boldsymbol{\rho}\_{\rm act} - \boldsymbol{\varphi}\_{\rm d} \tag{32}
$$

The mathematical equation of cost function of this control allocation and opti-

þ F2 trl <sup>þ</sup> <sup>F</sup><sup>2</sup> srl

μ2F<sup>2</sup> zrl þ F2 trr <sup>þ</sup> <sup>F</sup><sup>2</sup> srr

μ2F<sup>2</sup> zrr

,

zi (46)

BxF ¼ Fx,total � Ks<sup>1</sup> sgn S<sup>1</sup> (47) ByF ¼ Fy,total � Ks<sup>2</sup> sgn S<sup>2</sup> (48) BrF ¼ Mz,total � Ks<sup>3</sup> sgn S<sup>3</sup> (49)

BxF � Fx,total (50)

ByF � Fy,total (51)

BrF � Mz,total (52)

BxF ¼ Fx,total (43) ByF ¼ Fy,total (44) BrF ¼ Mz,total (45)

(42)

�

μ2F<sup>2</sup> zfr

Bx <sup>¼</sup> cos <sup>δ</sup>fl cos <sup>δ</sup>fr cos <sup>δ</sup>rl cos <sup>δ</sup>rr � � sin <sup>δ</sup>fl � sin <sup>δ</sup>fr � sin <sup>δ</sup>rl � sin <sup>δ</sup>rr:

By <sup>¼</sup> sin <sup>δ</sup>fl sin <sup>δ</sup>fr sin <sup>δ</sup>rl sin <sup>δ</sup>rr � cos <sup>δ</sup>fl cos <sup>δ</sup>fr cos <sup>δ</sup>rl cos <sup>δ</sup>rr �

Br <sup>¼</sup> lf sin <sup>δ</sup>fl <sup>þ</sup> <sup>0</sup>:5bf cos <sup>δ</sup>fl lf sin <sup>δ</sup>fr � <sup>0</sup>:5bf cos <sup>δ</sup>fr �

�lr sin δrl þ 0:5br cos δrl �lr sin δrr � 0:5br cos δrr lf cos δfl � 0:5bf sin δfl lf cos δfr þ 0:5br sin δfr

�lr cos δrl � 0:5br sin δrl �lr cos δrr þ 0:5br sin δrr �

where the optimisation variables of this cost function are individual tyre forces Fti, and Fsi. Fx,total, Fy,total and Mz,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):

where Ks1, Ks<sup>2</sup> and Ks<sup>3</sup> are positive control gains of SMC. The sliding surface

S1, S<sup>2</sup> and S<sup>3</sup> in Eqs. (47)–(49) can be presented as follows:

39

S<sup>1</sup> ¼ ð

S<sup>2</sup> ¼ ð

S<sup>3</sup> ¼ ð si ≤μF<sup>2</sup>

F2 ti <sup>þ</sup> <sup>F</sup><sup>2</sup>

misation problem can be shown as follows:

DOI: http://dx.doi.org/10.5772/intechopen.85384

<sup>J</sup><sup>3</sup> <sup>¼</sup> <sup>F</sup><sup>2</sup>

tfl <sup>þ</sup> <sup>F</sup><sup>2</sup> sfl

where <sup>F</sup> <sup>¼</sup> Ftfl Ftfr Ftrl Ftrr Fsfl Fsfr Fsrl Fsrr � �<sup>T</sup>

μ2F<sup>2</sup> zfl þ F2 tfr <sup>þ</sup> <sup>F</sup><sup>2</sup> sfr

Path Planning for Autonomous Vehicle in Off-Road Scenario

min Fti, Fsi

with the constraints of:

where φact is the actual measurement yaw angle. vx and vy are actual measurement feedback longitudinal and lateral velocity. <sup>e</sup>vx and <sup>e</sup>vy are longitudinal velocity error and lateral velocity error, respectively. In order to improve the vehicle stability, the desired lateral velocity vyd 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\_{\mathbf{x}, \text{fend}} = -K\_1 \widetilde{\boldsymbol{\nu}}\_{\mathbf{x}} \tag{33}$$

$$F\_{y,feedback} = -K\_{2p} \widetilde{\boldsymbol{\upsilon}}\_{y} - K\_{2d} \dot{\widetilde{\boldsymbol{\upsilon}}}\_{y} \tag{34}$$

$$\mathcal{M}\_{x,feedback} = -\mathcal{K}\_{3p}\widetilde{\rho} - \mathcal{K}\_{3d}\dot{\widetilde{\rho}} \tag{35}$$

where K1, K2p, K2d, K3<sup>p</sup> and K3<sup>d</sup> represent feedback control gains. The feedforward tyre force and moment can be calculated as:

$$F\_{\mathbf{x},forward} = m\dot{\upsilon}\_{\mathbf{x}d} - m\widetilde{\upsilon}\_{\mathbf{y}}\dot{\rho}\_d \tag{36}$$

$$F\_{\text{y,forward}} = m v\_{\text{xd}} \dot{\rho}\_d + m \widetilde{\boldsymbol{\nu}}\_{\text{x}} \dot{\rho}\_d \tag{37}$$

$$\mathbf{M}\_{x,forward} = I\_x \ddot{\boldsymbol{\rho}}\_d \tag{38}$$

The vehicle total desired longitudinal force Fx,total, lateral force Fy,total and yaw moment Mz,total can be determined by adding up feedforward and feedback terms:

$$F\_{\mathbf{x},total} = m\dot{\upsilon}\_{\mathbf{x}d} - m\widetilde{\upsilon}\_{\mathbf{y}}\dot{\rho}\_d - K\_1\widetilde{\upsilon}\_{\mathbf{x}} \tag{39}$$

$$F\_{y,total} = m\upsilon\_{\rm xd}\dot{\rho}\_d + m\tilde{\upsilon}\_{\rm x}\dot{\rho}\_d - K\_{2p}\tilde{\upsilon}\_{\rm y} - K\_{2d}\dot{\tilde{\upsilon}}\_{\rm y} \tag{40}$$

$$\mathbf{M}\_{\mathbf{z}, \text{total}} = I\_x \ddot{\boldsymbol{\rho}}\_d - \mathbf{K}\_{\mathbf{3}p} \boldsymbol{\tilde{\rho}} - \mathbf{K}\_{\mathbf{3}d} \dot{\boldsymbol{\tilde{\rho}}} \tag{41}$$

#### 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:

$$\min\_{F\_{\text{irp}}, F\_{\text{ii}}} J\_3 = \frac{F\_{\text{tf1}}^2 + F\_{\text{sf1}}^2}{\mu^2 F\_{\text{xf1}}^2} + \frac{F\_{\text{tfr}}^2 + F\_{\text{sfr}}^2}{\mu^2 F\_{\text{xfr}}^2} + \frac{F\_{\text{trl}}^2 + F\_{\text{srt}}^2}{\mu^2 F\_{\text{xrl}}^2} + \frac{F\_{\text{trr}}^2 + F\_{\text{srr}}^2}{\mu^2 F\_{\text{xrr}}^2} \tag{42}$$

with the constraints of:

the second layer, the individual steering and driving actuators are optimised and controlled to achieve the desired longitudinal force, lateral force and yaw moment.

Path Planning for Autonomous Vehicles - Ensuring Reliable Driverless Navigation…

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

<sup>e</sup>vy <sup>¼</sup> vx sin <sup>φ</sup><sup>e</sup> <sup>þ</sup> vy cos <sup>φ</sup>e� � vyd

<sup>e</sup>vx <sup>¼</sup> vx cos <sup>φ</sup><sup>e</sup> � vy sin <sup>φ</sup>e� � vxd

where φact is the actual measurement yaw angle. vx and vy are actual measurement feedback longitudinal and lateral velocity. <sup>e</sup>vx and <sup>e</sup>vy are longitudinal velocity error and lateral velocity error, respectively. In order to improve the vehicle stabil-

The feedback tyre force and moment can be determined according to the track-

Fy,feedback ¼ �K2<sup>p</sup>evy � <sup>K</sup>2<sup>d</sup> \_

Mz,feedback ¼ �K3<sup>p</sup>φ<sup>e</sup> � <sup>K</sup>3<sup>d</sup> \_

The vehicle total desired longitudinal force Fx,total, lateral force Fy,total and yaw moment Mz,total can be determined by adding up feedforward and feedback terms:

Fy,total <sup>¼</sup> mvxdφ\_ <sup>d</sup> <sup>þ</sup> <sup>m</sup>evxφ\_ <sup>d</sup> � <sup>K</sup>2<sup>p</sup>evy � <sup>K</sup>2<sup>d</sup> \_

Mz,total <sup>¼</sup> Izφ€<sup>d</sup> � <sup>K</sup>3<sup>p</sup>φ<sup>e</sup> � <sup>K</sup>3<sup>d</sup> \_

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

where K1, K2p, K2d, K3<sup>p</sup> and K3<sup>d</sup> represent feedback control gains. The feedforward tyre force and moment can be calculated as:

� (30)

� (31)

<sup>φ</sup><sup>e</sup> <sup>¼</sup> <sup>φ</sup>act � <sup>φ</sup><sup>d</sup> (32)

Fx,feedback ¼ �K<sup>1</sup>evx (33)

Fx,forward <sup>¼</sup> mv\_xd � <sup>m</sup>evyφ\_ <sup>d</sup> (36) Fy,forward <sup>¼</sup> mvxdφ\_ <sup>d</sup> <sup>þ</sup> <sup>m</sup>evxφ\_ <sup>d</sup> (37)

Fx,total <sup>¼</sup> mv\_xd � <sup>m</sup>evyφ\_ <sup>d</sup> � <sup>K</sup><sup>1</sup>evx (39)

Mz,forward ¼ Izφ€<sup>d</sup> (38)

<sup>e</sup>vy (34)

<sup>φ</sup><sup>e</sup> (35)

<sup>e</sup>vy (40)

<sup>φ</sup><sup>e</sup> (41)

4.1 Trajectory tracking controller in the first layer

ity, the desired lateral velocity vyd is assumed as zero value.

following equation based on [25]:

ing error dynamics in Eqs. (30–32):

4.2 Trajectory controller in the second layer

steering and driving control actuators.

38

$$\mathcal{B}\_{\mathbf{x}}\mathbf{F} = F\_{\mathbf{x}, total} \tag{43}$$

$$\mathcal{B}\_{\mathcal{Y}}\mathcal{F} = F\_{\mathcal{Y},total} \tag{44}$$

$$\mathbf{B}\_{\mathbf{r}}\mathbf{F} = \mathbf{M}\_{\mathbf{r}, \text{total}} \tag{45}$$

$$\begin{aligned} \text{where } & \mathbf{F} = \begin{bmatrix} F\_{\tilde{q}l} & F\_{\tilde{r}r} & F\_{rl} & F\_{rr} & F\_{r\tilde{l}l} & F\_{r\tilde{r}l} & F\_{r\tilde{r}l} & F\_{rr} \end{bmatrix}^T, \\ \mathbf{B}\_x &= \begin{bmatrix} \cos\delta\_{\tilde{l}l} & \cos\delta\_{\tilde{r}r} & \cos\delta\_{rl} & \cos\delta\_{rr} & -\sin\delta\_{\tilde{l}l} & -\sin\delta\_{\tilde{r}r} & -\sin\delta\_{rr} & -\sin\delta\_{rr} \end{bmatrix}^T \\ \mathbf{B}\_y &= \begin{bmatrix} \sin\delta\_{\tilde{l}l} & \sin\delta\_{\tilde{r}r} & \sin\delta\_{rl} & \sin\delta\_{rl} & \cos\delta\_{\tilde{l}r} & \cos\delta\_{\tilde{r}r} & \cos\delta\_{rl} & -\sin\delta\_{rr} \end{bmatrix}^T \\ \end{aligned}$$

$$\mathbf{B}\_r = \begin{bmatrix} l\_f \sin\delta\_{\tilde{l}l} + 0.5b\_f \cos\delta\_{\tilde{l}l} & l\_f \sin\delta\_{\tilde{r}r} - 0.5b\_f \cos\delta\_{\tilde{r}r} \end{bmatrix}$$

$$-l\_r \sin\delta\_{rl} + 0.5b\_r \cos\delta\_{rl} \quad -l\_r \sin\delta\_{rr} - 0.5b\_r \cos\delta\_{\tilde{r}r}$$

$$l\_f \cos\delta\_{\tilde{l}l} - 0.5b\_f \sin\delta\_{\tilde{l}l} \quad l\_f \cos\delta\_{\tilde{r}r} + 0.5b\_r \sin\delta\_{\tilde{r}r} \end{bmatrix}$$

$$-l\_r \cos\delta\_{rl} - 0.5b\_r \sin\delta\_{rl} \quad -l\_r \cos\delta\_{rr} + 0.5b\_r \sin\delta\_{rr} \tag{46}$$

where the optimisation variables of this cost function are individual tyre forces Fti, and Fsi. Fx,total, Fy,total and Mz,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):

$$\mathbf{B\_{x}F} = F\_{\mathbf{x},total} - K\_{\text{r1}} \operatorname{sgn} \mathbf{S\_{1}} \tag{47}$$

$$\mathbf{B\_{y}F} = F\_{y,total} - K\_{i2} \operatorname{sgn} \mathbf{S\_2} \tag{48}$$

$$\mathbf{B}\_r \mathbf{F} = \mathbf{M}\_{\mathbf{z}, \text{total}} - K\_{r3} \operatorname{sgn} \mathbf{S}\_3 \tag{49}$$

where Ks1, Ks<sup>2</sup> and Ks<sup>3</sup> are positive control gains of SMC. The sliding surface S1, S<sup>2</sup> and S<sup>3</sup> in Eqs. (47)–(49) can be presented as follows:

$$\mathbf{S}\_1 = \int \mathbf{B}\_\mathbf{x} \mathbf{F} - F\_{\mathbf{x}, total} \tag{50}$$

$$\mathcal{S}\_2 = \int \mathcal{B}\_{\mathcal{Y}} \mathcal{F} - F\_{\mathcal{Y}, total} \tag{51}$$

$$\mathbf{S}\_3 = \int \mathbf{B}\_r \mathbf{F} - \mathbf{M}\_{x, total} \tag{52}$$

Path Planning for Autonomous Vehicles - Ensuring Reliable Driverless Navigation…

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\_{ti} R\_{ov} \tag{53}$$

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

Path Planning for Autonomous Vehicle in Off-Road Scenario

DOI: http://dx.doi.org/10.5772/intechopen.85384

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 twowheel 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 posi-

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. Vxd1, Vxd2, Vxd3, Vxd<sup>4</sup> and Vxd<sup>5</sup> are desired longitudinal velocities on each section of road, while Vyd1, Vyd2,

Vyd3, Vyd<sup>4</sup> and Vyd<sup>5</sup> 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.

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

tion. This shows the advantages of 4WIS-4WID model.

presented and compared.

41

$$\delta\_{\rm fl} = \frac{F\_{\rm sfl}}{C\_a} + \frac{l\_f r}{v\_{\rm x}} \tag{54}$$

$$\delta\_{\hat{r}} = \frac{F\_{\hat{r}r}}{\mathcal{C}\_a} + \frac{l\_f r}{\nu\_x} \tag{55}$$

$$
\delta\_{rl} = \frac{F\_{srl}}{C\_a} - \frac{l\_r r}{v\_\chi} \tag{56}
$$

$$
\delta\_{rr} = \frac{F\_{srr}}{C\_a} - \frac{l\_r r}{v\_\infty} \tag{57}
$$

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