Sage Journals: Discover world-class research

Abstract

In this paper, a strategy based on model predictive control consisting of path planning and path tracking is designed for obstacle avoidance steering control problem of the unmanned ground vehicle. The path planning controller can reconfigure a new obstacle avoidance reference path, where the constraint of the front-wheel-steering angle is transformed to formulate lateral acceleration constraint. The path tracking controller is designed to realize the accurate and fast following of the reconfigured path, and the control variable of tracking controller is steering angle. In this work, obstacles are divided into two categories: static and dynamic. When the decision-making system of the unmanned ground vehicle determines the existence of static obstacles, the obstacle avoidance path will be generated online by an optimal path reconfiguration based on direct collocation method. In the case of dynamic obstacles, receding horizon control is used for real-time path optimization. To decrease online computation burden and realize fast path tracking, the tracking controller is developed using the continuous-time model predictive control algorithm, where the extended state observer is combined to estimate the lumped disturbances for strengthening the robustness of the controller. Finally, simulations show the effectiveness of the proposed approach in comparison with nonlinear model predictive control, and the CarSim simulation is presented to further prove the feasibility of the proposed method.

Keywords

Unmanned ground vehicle obstacle avoidance steering control model predictive control extended state observer

Introduction

The topic of unmanned ground vehicle (UGV)¹ has become increasingly popular in the field of intelligent transportation system. In UGV application, it is inevitable that the vehicle running fast in unknown environments always encounters obstacles,² and the general strategy is automatic braking or active steering. In emergency situations, human drivers are accustomed to braking. However, steering or steering combined with braking would be more frequent way for the optimal maneuver.³

Generally, obstacle avoidance steering problem is formulated as steering tracking control.⁴ Naranjo et al.⁵ suggest that there are two ways to design the steering controllers: imitating human drivers and using the dynamic model of the car. The former does not need detailed knowledge of vehicle dynamics, for example, proportional–integral–derivative (PID),^6,7 fuzzy control,^8–10 and neural network.^11,12 The latter relies on the accurate dynamic model of the vehicle to build controller, such as sliding mode control,^13–15 H-infinity,^16,17 feedback linearization,^18,19 and model predictive control (MPC).^20–23 In the steering control systems, control inputs commonly consist of the steering angle and the steering torque.²⁴

In practice, it is difficult to obtain such a safe pre-defined reference path in the unknown environment with obstacles.²⁵ Moreover, if the size of the vehicle is not explicitly considered, or the environment is complex and cluttered, the situation will become more serious. Thereby, path planning attracts a considerable attention in obstacle avoidance. The path planning methods are mainly classified into two categories,²⁶ that is, global planning method and local planning method. For the former, the UGV requires prior knowledge about the environment and assumes that the terrain is static. In contrast, the environment is partially known or completely unknown for the latter. Therefore, local path planning is more challenging. Shimoda et al.²⁷ propose a potential field-based path planning method for UGV to avoid static obstacles. Zhuge et al.²⁸ propose a dynamic obstacle avoidance algorithm using collision time histogram for UGV in uncertain dynamic environment, where the nonholonomic nature of the vehicle is embedded. Kuwata et al.²⁹ improve a rapidly exploring random tree approach with closed-loop prediction for obstacle avoidance navigation of the UGV in urban environment. Hao and Agrawal³⁰ propose a strategy based on A* search algorithm to produce the online safe path for multiple UGVs. An online autonomous navigation system of the UGV is proposed by Vilca et al.³¹ for more safety and smoothness. However, most of these approaches only consider the kinematic constraints of the vehicle, and the dynamics and size of the vehicle have not been fully investigated during path computation, which weakens the feasibility of the path.

In complex traffic environment, the robustness of the control system is closely related to the safe driving of the vehicle, so that disturbance and uncertainty cannot be ignored.³² Guo et al.³³ propose an adaptive fuzzy dynamic surface control strategy for UGV with parametric uncertainties and external disturbances. A multi-input multi-output disturbance observer is investigated for UGV steering control with multiple uncertainties.³⁴

Motivated by the safety and rapidity of obstacle avoidance of the UGV, a steering control strategy based on MPC, consisting of path planning and path tracking, is proposed in this paper. The path planning controller calculates the new path online according to the type of obstacles. When the obstacles are static, an optimal path reconfiguration combined with direct collocation (DC) is presented to keep the path feasible and smooth. According to Almayyahi et al.,³⁵ most existing works have been dedicated to the navigation in static environment. For the dynamic obstacle scenario, the receding horizon control (RHC)^36,37 is used for periodical path planning to ensure safety and decrease conservatism. In the road constraints, obstacle avoidance constraints and dynamic constraints are synthesized to form multiple constraint optimization problem, by which local optimal path is obtained periodically. In order to decrease the computation burden, the actual control input (front-wheel-steering angle) constraint is considered in path planning, so that the tracking design becomes an unconstrained optimization problem. To satisfy the real-time computation requirement, path tracking controller utilizes and enhances the continuous-time model predictive control (CTMPC) algorithm in Panchal et al.³⁸ and Lu.³⁹ Feedback linearization and appropriate functional expansion are used to predict the future output, and optimal control law is calculated analytically. For the lumped disturbances, the extended state observer (ESO) is introduced as estimator to guarantee the stability of the system. Finally, the simulations by MATLAB and CarSim are implemented to validate the effectiveness of the proposed method.

Comparing with the previous works, the contributions of this paper are as follows:

A judgment rule is presented to determine if there is a possibility of collision between UGV and obstacles.

The controller works more effectively on obstacle avoidance because the smooth and reasonable path has been calculated in advance.

For the computation complexity that the conventional optimization is solved at each sampling moment, the analytical control law is designed in path tracking since the path tracking constraints are taken into account in path reconfiguration priority. This greatly decreases the computation time of the tracking controller.

For the under-actuated system, CTMPC is introduced and enhanced to unify the control law with two dimensions in this paper, and the ESO is combined to reject the lumped disturbances.

This paper is organized as follows: section “UGV model” presents the bicycle dynamic model and the point mass model of UGV. In section “Obstacle avoidance steering control strategy design,” the obstacle avoidance steering control strategy is introduced. Section “Simulation” gives the simulations with static and dynamic obstacles. The conclusions are drawn in section “Conclusion.”

UGV model

Six-order dynamic model

In general, the four-wheeled dynamic model of the UGV is so complicated that precise modeling is very difficult. Hence, it is simplified into a bicycle model,²⁵ as shown in Figure 1. In this paper, the UGV is a front-wheel-steering vehicle, and the steering angle of the rear wheel remains unchanged.

Figure 1.

The bicycle dynamic model of UGV.

In Figure 1, $XOY$ is the inertial coordinate system, $xo' y$ is the UGV body coordinate system, and $o'$ is the centroid of the UGV, v is the velocity of the UGV, $φ$ is the yaw angle of the UGV over the centroid, and $β$ is the sideslip angle over the centroid. The dynamic model is described as

\begin{matrix} \begin{matrix} mv (\overset{\cdot}{β} + \overset{\cdot}{φ}) = - F_{x} \sin β + F_{y} \cos β \\ m \overset{\cdot}{v} = F_{x} \cos β + F_{y} \sin β \end{matrix} \\ I_{zz} \overset{\cdot\cdot}{φ} = M_{z} \end{matrix}

(1)

where m represents the mass of the UGV; $I_{zz}$ and $M_{z}$ denote the yaw inertia moment and yaw torque, respectively; and $F_{x}$ and $F_{y}$ are the forces over the centroid of the UGV. $F_{x}$ , $F_{y}$ , and $M_{z}$ are written as

\begin{matrix} F_{x} = - 2 \sin δ_{f} F_{sf} + 2 \cos δ_{f} F_{lf} + 2 F_{lr} \\ F_{y} = 2 \cos δ_{f} F_{sf} + 2 \sin δ_{f} F_{lf} + 2 F_{sr} \\ M_{z} = 2 l_{f} \cos δ_{f} F_{sf} - 2 l_{r} F_{sr} + 2 l_{f} \sin δ_{f} F_{lf} \end{matrix}

(2)

where $δ_{f}$ is the front-wheel-steering angle; $l_{f}$ and $l_{r}$ are the distances from the front and rear wheel axles to the centroid, respectively; $F_{sf}$ and $F_{sr}$ are the lateral tire forces of the front and rear wheels respectively; $F_{lf}$ and $F_{lr}$ are the longitudinal tire forces of the front and rear wheels, respectively. In general, the tire forces are presented as complex nonlinear functions using magic formula. In this paper, in order to decrease the calculation burden, their nonlinear models are linearized as

\begin{matrix} \begin{matrix} F_{sf} = C_{cf} α_{f} \\ F_{lf} = C_{lf} s_{f} \end{matrix} \\ F_{sr} = C_{cr} α_{r} \\ F_{lr} = C_{lr} s_{r} \end{matrix}

(3)

where $C_{lf}$ and $C_{cf}$ represent the longitudinal and lateral cornering stiffness of the front wheel, respectively; $C_{lr}$ and $C_{cr}$ are the longitudinal and lateral cornering stiffness of the rear wheel, respectively; $s_{f}$ and $s_{r}$ are the slip rates of the front and rear wheels, respectively; $α_{f}$ and $α_{r}$ are the sideslip angles of the front and rear wheels, respectively, which are the functions of $β$ , $\overset{\cdot}{φ}$ , v, and $δ_{f}$ ( $\overset{\cdot}{φ}$ is the yaw angular velocity), as follows

\begin{matrix} α_{f} = δ_{f} - \tan^{- 1} (\frac{v \sin β + l_{f} \overset{\cdot}{φ}}{v \cos β}) \\ α_{r} = - \tan^{- 1} (\frac{v \sin β - l_{r} \overset{\cdot}{φ}}{v \cos β}) \end{matrix}

(4)

Assumption 1

$β$ and $δ_{f}$ are small enough, and the velocity v of the UGV remains constant as follows

{\begin{matrix} β << 1 \\ \overset{\cdot}{v} = 0 \\ δ_{f} << 1 \end{matrix}

(5)

Correspondingly, $α_{f}$ and $α_{r}$ are transformed as

\begin{matrix} α_{f} = δ_{f} - \frac{v β + l_{f} \overset{\cdot}{φ}}{v} \\ α_{r} = - \frac{v β - l_{r} \overset{\cdot}{φ}}{v} \end{matrix}

(6)

The velocity is converted into the inertial coordinate system as

\begin{matrix} \overset{\cdot}{X} = v \cos β \cos φ - v \sin β \sin φ \\ \overset{\cdot}{Y} = v \cos β \sin φ + v \sin β \cos φ \end{matrix}

(7)

Similarly, equation (7) can also be simplified in terms of Assumption 1. The state variables and control input are defined as $ξ = [\begin{matrix} \begin{matrix} β & φ & \overset{\cdot}{φ} \end{matrix} & \begin{matrix} δ_{f} & X & Y \end{matrix} \end{matrix}]^{T}$ and $u = {\overset{\cdot}{δ}}_{f}$ , and the output is $η = [\begin{matrix} X & Y \end{matrix}]^{T}$ . In reality, the true control input is $δ_{f}$ , and ${\overset{\cdot}{δ}}_{f}$ is just a designed virtual control input. So, the UGV is a typical under-actuated system, and the six-order dynamic model of UGV is formulated as follows

\overset{\cdot}{ξ} = f (ξ, u) = {\begin{matrix} \begin{matrix} - \overset{\cdot}{φ} + \frac{2 (F_{sf} + F_{sr} + δ_{f} F_{lf})}{mv} \\ \overset{\cdot}{φ} \\ \frac{2 (l_{f} F_{sf} - l_{r} F_{sr} + δ_{f} l_{f} F_{lf})}{I_{zz}} \end{matrix} \\ \begin{matrix} u \\ v \cos φ - v β \sin φ \\ v \sin φ + v β \cos φ \end{matrix} \end{matrix}

(8)

In order to prove that the presented approximation model is the good fidelity of true UGV dynamics, the approximation degree by the CarSim simulation is tested as shown in Figure 2.

Figure 2.

(a) The path of UGV. (b) The front-wheel-steering angle. (c) The comparison of the sideslip angles of UGV over the centroid. (d) The comparison of the sideslip angles of the rear wheel. (e) The comparison of the yaw angles of UGV over the centroid. (f) The comparison of yaw angular velocities.

From Figure 2, it can be seen that the front-wheel-steering angles of the two models change synchronously in the same sinusoidal trend. Although the other kinematic characteristics of the approximation model deviate slightly from true model, the errors are acceptable.

The point mass model

Although incorporating the dynamic model can improve the accuracy, the computation complexity increases greatly. So, the point mass model (9) of the UGV is used to calculate the path. This model is equipped with the bicycle model, also satisfying Assumption 1

\begin{matrix} \begin{matrix} \overset{\cdot\cdot}{y} = a_{y} \\ \overset{\cdot\cdot}{x} = a_{x} \\ \overset{\cdot}{φ} = \frac{a_{y}}{\overset{\cdot}{x}} \end{matrix} \\ \begin{matrix} \overset{\cdot}{X} = \overset{\cdot}{x} \cos φ - \overset{\cdot}{y} \sin φ \\ \overset{\cdot}{Y} = \overset{\cdot}{x} \sin φ + \overset{\cdot}{y} \cos φ \end{matrix} \end{matrix}

(9)

where $\overset{\cdot}{x}$ and $\overset{\cdot}{y}$ are the longitudinal and lateral velocities of the UGV, respectively; $a_{x}$ and $a_{y}$ are the longitudinal and lateral accelerations, respectively; and X and Y represent two dimensional displacements of the UGV in the inertial coordinate system, respectively. The state variable vector is defined as $ξ_{p} = [\begin{matrix} \begin{matrix} \overset{\cdot}{y} & \overset{\cdot}{x} \end{matrix} & \begin{matrix} φ & Y & X \end{matrix} \end{matrix}]^{T}$ , the output variable vector is $η_{p} = [\begin{matrix} X & \begin{matrix} Y & φ \end{matrix} \end{matrix}]^{T}$ , and $u_{p} = a_{y}$ is the input variable. Because only steering control is investigated in this paper, $a_{x} = 0$ is fixed. The point mass model is formed as

{\overset{\cdot}{ξ}}_{p} = g (ξ_{p}, u_{p})

(10)

Obstacle avoidance steering control strategy design

In order to avoid obstacles safely and quickly, the complete control system is designed as shown in Figure 3.

Figure 3.

The architecture of the control system.

Generally, obstacles are divided into static case and dynamic case. When encountering obstacles, the UGV has to determine the type of obstacles at first. Then, the path planning controller is used to calculate the local path online, and the steering controller is designed to track the path precisely.

Judgment of collision possibility

In fact, the information of obstacles is unknown and uncertain. The perception technology using sensors in UGV such as lidar and camera is essential for the identification of obstacles. In addition, the multi-sensor information fusion algorithm, for example, information filter, is very useful to obtain the accurate states of the obstacle. Recently, deep learning has become the most attractive method for obstacle or target recognition. However, this paper focuses on the obstacle avoidance steering control strategy, so the states of obstacles are assumed to be known for the UGV. The following assumption is given.

Assumption 2

The position and velocity of obstacles on the road are known for UGV.

Before implementing obstacle avoidance steering control strategy, it is important to judge the collision possibility to prevent the UGV from unnecessary maneuver. The judgment rules are given as follows

{\begin{matrix} φ - \frac{1}{2} Ψ \leq θ \leq φ + \frac{1}{2} Ψ \Rightarrow b k_{j} = 1 \\ φ + \frac{1}{2} Ψ \leq θ \Rightarrow b k_{j} = 0 \\ φ - \frac{1}{2} Ψ \geq θ \Rightarrow b k_{j} = 0 \end{matrix}

(11)

where $b k_{j} = 1$ represents possible collision; $b k_{j} = 0$ represents no collision; $Ψ$ is the danger angle range depending on the size of the UGV, the size of the obstacles, and the distance between the UGV and the obstacles; and $θ$ is the angle of the line from the UGV to the obstacle in the inertial coordinate system. The judgment of the collision possibility is shown in Figure 4.

Figure 4.

Judgment of the threat of obstacle.

Path planning

In reality, static obstacles involve broken-down vehicles, rolling stones on the highway, potholes, and so on. Dynamic obstacles mean the moving obstacles such as passersby, motorcycles, and other slow-moving vehicles. In this paper, if the decision-making system of the UGV determines that obstacles are static, the path planning controller will utilize the optimal path reconfiguration based on DC to obtain a local optimal path online. But if obstacles are dynamic, the RHC algorithm will be used in path planning for more safety and flexibility. The local path will be calculated periodically for the movement of obstacles.

Static obstacle scenario

In this scenario, the potholes on the road are taken as static obstacles, as shown in Figure 5. The optimal path reconfiguration using DC is designed to compute the local path online.

Figure 5.

Static obstacle scenario.

The path is separated into N subintervals with equal time intervals T from $t_{0}$ to $t_{f}$ . The third-order Hermite polynomial is used to formulate the state variables and their derivatives in each subinterval. The middle points of the subintervals are taken as collocation points, and the two endpoints of one subinterval are called nodes. The following relationship between the ith collocation point and its nodes is satisfied

\begin{matrix} ξ_{pc} (i) = \frac{ξ_{p} (i) + ξ_{p} (i + 1)}{2} + \frac{T ({\overset{\cdot}{ξ}}_{p} (i) - {\overset{\cdot}{ξ}}_{p} (i + 1))}{8} \\ {\overset{\cdot}{ξ}}_{pc} (i) = \frac{3 (ξ_{p} (i + 1) - ξ_{p} (i))}{2 T} - \frac{{\overset{\cdot}{ξ}}_{p} (i) + {\overset{\cdot}{ξ}}_{p} (i + 1)}{4} \end{matrix}

(12)

where $i \in {1, 2, \dots, N}$ .

At each collocation point, the UGV must satisfy kinematic constraint

{\overset{\cdot}{ξ}}_{pc} (i) = g (ξ_{pc} (i), u_{pc} (i)), i \in {1, 2, \dots, N - 1}

(13)

where $u_{pc} = a_{ypc}$ is the input on the collocation point.

For the accuracy of the interpolation, the Defect vector needs to be 0, as follows

\begin{matrix} Δ = ξ_{p} (i) - ξ_{p} (i + 1) + \frac{T}{6} * \\ ({\overset{\cdot}{ξ}}_{p} (i) + 4 g (ξ_{pc} (i), u_{pc} (i)) + {\overset{\cdot}{ξ}}_{p} (i + 1)) = 0 \end{matrix}

(14)

where $ξ_{pc} = [\begin{matrix} \begin{matrix} {\overset{\cdot}{y}}_{pc} & {\overset{\cdot}{x}}_{pc} & φ_{pc} \end{matrix} & \begin{matrix} Y_{pc} & X_{pc} \end{matrix} \end{matrix}]^{T}$ is the state variable on collocation point.

Combining DC, diverse constraints should be considered:

1. The input variable $a_{y}$ must meet the dynamic constraint to prevent the UGV from sideslip or rollover

\begin{matrix} | a_{y} (i) | \leq μ g \\ | a_{ypc} (i) | \leq μ g \end{matrix}, i \in {1, 2, \dots, N}

(15)

where $μ$ is the friction coefficient between the tire and the road surface, and g is the gravitational acceleration.

2. The actual control input constraint $(δ_{f} \leq δ_{fmax})$ in the tracking controller design can be converted approximately to the inequality about $a_{y}$ . Hence, the constraint of $a_{y}$ is the intersection of the dynamic constraint and input constraint

\begin{matrix} | a_{y} (i) | \leq \min {μ g, δ_{fmax} \frac{v^{2}}{l}} \\ | a_{ypc} (i) | \leq \min {μ g, δ_{fmax} \frac{v^{2}}{l}} \end{matrix}, i \in {1, 2, \dots, N}

(16)

Assumption 3

There is only rolling at the junction of the tire and the road surface, and the lateral slippage does not exist. Each tire meets the rolling constraint and non-slipping constraint.

The pure rolling constraint of the rear wheel is

{\overset{\cdot}{x}}_{r} \cos φ + {\overset{\cdot}{y}}_{r} \sin φ = v

(17)

The non-slipping constraints of the front and rear wheels are

\begin{matrix} {\overset{\cdot}{x}}_{r} \sin (φ + δ_{f}) - {\overset{\cdot}{y}}_{r} \cos (φ + δ_{f}) - l \overset{\cdot}{φ} \cos δ_{f} = 0 \\ {\overset{\cdot}{x}}_{r} \sin φ - {\overset{\cdot}{y}}_{r} \cos φ = 0 \end{matrix}

(18)

where $(x_{r}, y_{r})$ is the center position of the rear axle, and l is the distance between the front and rear axles. Solve equations (17) and (18) to yield^40,41

\overset{\cdot}{φ} = \frac{v}{l} \tan δ_{f}

(19)

Combining with the point mass model and Assumption 1, the following equation holds

a_{y} = δ_{f} \frac{v^{2}}{l}

(20)

Therefore, the constraint of the front-wheel-steering angle $δ_{f}$ is converted to the constraint of the lateral acceleration $a_{y}$ , which decreases the calculation burden of the path tracking controller.

3. The UGV is not allowed to move across the solid lines of the lanes according to the traffic laws. Consequently, its lateral position Y should be constrained within a certain range

\begin{matrix} Y_{\min} \leq Y (i) \leq Y_{\max} \\ Y_{\min} \leq Y_{pc} (i) \leq Y_{\max} \end{matrix}, i \in {1, 2, \dots, N}

(21)

4. The UGV should always maintain a safe distance $(d_{safe})$ from obstacles

\begin{matrix} \sqrt{{(X (i) - X_{obs 1})}^{2} + {(Y (i) - Y_{obs 1})}^{2}} \geq d_{safe} \\ ⋮ \\ \sqrt{{(X (i) - X_{obsn})}^{2} + {(Y (i) - Y_{obsn})}^{2}} \geq d_{safe} \\ \sqrt{{(X_{pc} (i) - X_{obs 1})}^{2} + {(Y_{pc} (i) - Y_{obs 1})}^{2}} \geq d_{safe} \\ ⋮ \\ \sqrt{{(X_{pc} (i) - X_{obsn})}^{2} + {(Y_{pc} (i) - Y_{obsn})}^{2}} \geq d_{safe} \end{matrix}

(22)

where $i \in {1, 2, \dots, N}$ and n is the number of obstacles.

5. The UGV should arrive at the target position at time $t_{f}$

{\begin{matrix} X (t_{f}) = X_{target} \\ Y (t_{f}) = Y_{target} \end{matrix}

(23)

In order to smooth the path and complete the original task, the objective function of the optimal path reconfiguration method is formulated as

\begin{matrix} min J = \sum_{i = 1}^{N} (| | η_{p} (i) - η_{ref} (i) | |_{Q_{11}}^{2} + q_{12} {sum}_{obs}^{2} + r_{11} u_{p}^{2} (i) + r_{12} Δ u_{p}^{2} (i)) \end{matrix}

(24)

where $η_{ref} \in R^{3 \times 1}$ is the reference path; $Δ u_{p}$ is the control input increment; $Q_{11} \in R^{3 \times 3}$ is a symmetric and positive definite matrix; and $q_{12}$ , $r_{11}$ , and $r_{12}$ are positive constant coefficients. $r_{11}$ denotes the steerability, and $r_{12}$ characterizes the smoothness of the path. $su m_{obs}$ is a penalty term related to obstacle avoidance, as follows

su m_{obs} = \sum_{j = 1}^{n} \frac{v}{d_{obsj} + c}

(25)

where

d_{obsj} = \sqrt{{(X (i) - X_{obsj})}^{2} + {(Y (i) - Y_{obsj})}^{2}}

(26)

$(X_{obsj}, Y_{obsj})$ is the coordinate of the jth obstacle in the inertial coordinate system; c is a very small constant, which is used to avoid the singularity of equation (25).

By solving equation (24) under the constraints (14)–(16) and (21)–(23), the optimal path reconfiguration can obtain N path points, that is, $(X_{pl} (i), Y_{pl} (i))$ , $i \in {1, 2, \dots, N}$ . Since the continuous-time controller is developed to track the path, continuous path, that is, $(X_{pl} (t), Y_{pl} (t))$ , $t \in [0, t_{f}]$ , must be obtained by fitting method.

Dynamic obstacle scenario

In the dynamic obstacle scenario, the UGV is cruising at the center of the lane at a constant velocity. Simultaneously, some vehicles (such as motorcycles) are moving in the same direction with slower speeds in front of the UGV, as shown in Figure 6. To overtake the running obstacles safely, RHC is used to calculate the local optimal path during each planning period.

Figure 6.

Dynamic obstacle scenario.

First, the point mass model is discretized at time k, and the state variables in $n_{p}$ predictive steps are predicted by RHC. The value of $n_{p}$ determines the predictability of RHC and the dimension of the optimization problem.

Here, RHC is used to optimize the obstacle avoidance path, as follows. In Figure 7, the solid lines represent the parts of the path being tracked, and the dash lines are the parts not tracked. $T_{1}$ represents the time interval of RHC, $n_{p} T_{1}$ is the predictive horizon, $T'$ is the planning period, and $T ″$ is the computation time of RHC.

Figure 7.

The schematic of RHC path planner.

For the dynamic obstacles, RHC focuses on avoiding obstacles and maintaining the original task, but the item about $Δ u_{p}^{2}$ is not considered. In the optimization problem of the dynamic obstacle scenario, road constraints and dynamic constraints are same as those in the static obstacle scenario. The cost function for dynamic obstacles is presented as follows

\begin{matrix} min J = \sum_{i = 1}^{n_{p}} (‖ {\tilde{η}'}_{p} (k + i) - η_{ref} (k + i) ‖_{Q_{11}}^{2} + q_{12} {sum}_{obs}^{2}) + \sum_{j = 0}^{n_{m} - 1} r_{11} u_{p}^{2} (k + j) \end{matrix}

(27)

where $n_{m}$ is the control steps, and $su m_{obs}$ is correspondingly changed for different types of obstacles

\begin{matrix} su m_{obs} = \sum_{j = 1}^{n} b k_{j} \frac{v - v_{obsj}}{d_{obsj} + c} \end{matrix}

(28)

where

d_{obsj} = {({(\tilde{X} (k + i) - X_{obsj} (k + i))}^{2} + {(\tilde{Y} (k + i) - Y_{obsj} (k + i))}^{2})}^{\frac{1}{2}}

(29)

where $(\tilde{X} (k + i), \tilde{Y} (k + i))$ is the predictive position of the UGV in the inertial coordinate system; $v_{obsj}$ is the velocity of the jth obstacle; and $(X_{obsj} (k + i), Y_{obsj} (k + i))$ is the position of the jth obstacle. $b k_{j}$ is a Boolean variable representing collision possibility.

Furthermore, in order to reduce the calculation time, the reference path is directly taken as the output of the path planning controller if obstacles are far away from vehicles, that is, $d_{obsj} \geq d_{action}$ , $j = 1, 2, \dots, n$ . $d_{action}$ is a designed parameter depending on the relative speed between the UGV and obstacles.

By solving equation (27), the local optimal path can be obtained as the point set, that is, $(X_{pl} (i), Y_{pl} (i))$ , $i \in {1, 2, \dots, n_{p}}$ . Similarly, continuous path $(X_{pl} (t), Y_{pl} (t))$ is obtained by fitting method.

Path tracking

CTMPC

First, the following feedback linearization is implemented for dynamic model

{\begin{matrix} \overset{\cdot\cdot}{X} = L_{ξ} \overset{\cdot}{X} = v \overset{\cdot}{φ} (- β \cos φ - \sin φ) - v \overset{\cdot}{β} \sin φ \\ \overset{\cdot\cdot}{Y} = L_{ξ} \overset{\cdot}{Y} = v \overset{\cdot}{φ} (\cos φ - β \sin φ) + v \overset{\cdot}{β} \cos φ \end{matrix}

(30)

{\begin{matrix} \begin{matrix} \overset{. . .}{X} = L_{ξ} (L_{ξ} \overset{\cdot}{X}) = v {\overset{\cdot}{φ}}^{2} (β \sin φ - \cos φ) \\ - v \overset{\cdot\cdot}{φ} (β \cos φ + \sin φ) - 2 v \overset{\cdot}{φ} \overset{\cdot}{β} \cos φ - v \sin φ \overset{\cdot\cdot}{β} \end{matrix} \\ \begin{matrix} \overset{. . .}{Y} = L_{ξ} (L_{ξ} \overset{\cdot}{Y}) = - v {\overset{\cdot}{φ}}^{2} (\sin φ + β \cos φ) \\ + v \overset{\cdot\cdot}{φ} (\cos φ - β \sin φ) - 2 v \overset{\cdot}{φ} \overset{\cdot}{β} \sin φ + v \cos φ \overset{\cdot\cdot}{β} \end{matrix} \end{matrix}

(31)

where $\overset{\cdot\cdot}{X} (t)$ and $\overset{\dots}{X} (t)$ are the Lie derivatives of $\overset{\cdot}{X} (t)$ , $\overset{\cdot\cdot}{Y} (t)$ , and $\overset{\dots}{Y} (t)$ are the Lie derivatives of $\overset{\cdot}{Y} (t)$ , and $\overset{\cdot\cdot}{β}$ is calculated as follows

\overset{\cdot\cdot}{β} = \frac{2}{mv} (C_{cf} ({\overset{\cdot}{δ}}_{f} - \frac{v \overset{\cdot}{β} + l_{f} \overset{\cdot\cdot}{φ}}{v}) - C_{cr} \frac{v \overset{\cdot}{β} - l_{r} \overset{\cdot\cdot}{φ}}{v} + {\overset{\cdot}{δ}}_{f} C_{lf} s_{f}) - \overset{\cdot\cdot}{φ}

(32)

The control input appears in $\overset{\dots}{X}$ and $\overset{\dots}{Y}$ at the first time, which means the relative degree of the prediction model is 6, that is, exactly being equal to the order of the system. In fact, external interference $d_{u}$ is also involved in the linearization along with the control input. So, the system can be fully linearized in the following

{\begin{matrix} \begin{matrix} {\overset{\cdot}{X}}_{0} = X_{1} \\ {\overset{\cdot}{X}}_{1} = X_{2} \end{matrix} \\ \begin{matrix} {\overset{\cdot}{X}}_{2} = f_{1} (ξ) + g_{1} (ξ) (u + d_{u}) \\ = f_{1} (ξ) + g_{1} (ξ) u + d_{1} \\ X_{0} = X \end{matrix} \end{matrix}

(33)

{\begin{matrix} \begin{matrix} {\overset{\cdot}{Y}}_{0} = Y_{1} \\ {\overset{\cdot}{Y}}_{1} = Y_{2} \\ {\overset{\cdot}{Y}}_{2} = f_{2} (ξ) + g_{2} (ξ) (u + d_{u}) \end{matrix} \\ \begin{matrix} = f_{2} (ξ) + g_{2} (ξ) u + d_{2} \\ Y_{0} = Y \end{matrix} \end{matrix}

(34)

where $f_{1}$ , $f_{2}$ , $g_{1}$ , and $g_{2}$ are the nonlinear functions over $ξ$ . $d_{1}$ and $d_{2}$ are the lumped disturbances, that is, $d_{1} = g_{1} (ξ) d_{u}$ and $d_{2} = g_{2} (ξ) d_{u}$

\begin{matrix} f_{1} (ξ) = υ {\overset{\cdot}{φ}}^{2} (β \sin φ - \cos φ) - υ \overset{\cdot\cdot}{φ} (β \cos φ + \sin φ) \\ - υ \sin φ (\frac{2}{mv} (- C_{cf} \frac{υ \overset{\cdot}{β} + l_{f} \overset{\cdot\cdot}{φ}}{υ} - C_{cr} \frac{υ \overset{\cdot}{β} - l_{r} \overset{\cdot\cdot}{φ}}{υ}) - \overset{\cdot\cdot}{φ}) \\ - 2 υ \overset{\cdot\cdot}{φ} \overset{\cdot}{β} \cos φ \\ f_{2} (ξ) = - υ {\overset{\cdot}{φ}}^{2} (β \cos φ + \sin φ) + υ \overset{\cdot\cdot}{φ} (\cos φ - β \sin φ) \\ + υ \cos φ (\frac{2}{mv} (- C_{cf} \frac{υ \overset{\cdot}{β} + l_{f} \overset{\cdot\cdot}{φ}}{υ} - C_{cr} \frac{υ \overset{\cdot}{β} - l_{r} \overset{\cdot\cdot}{φ}}{υ}) - \overset{\cdot\cdot}{φ}) \\ - 2 υ \overset{\cdot\cdot}{φ} \overset{\cdot}{β} \sin φ \\ g_{1} (ξ) = - \frac{\sin φ (2 C_{cf} + 2 C_{l f} s_{f})}{m} \\ g_{2} (ξ) = \frac{\cos φ (2 C_{cf} + 2 C_{l f} s_{f})}{m} \end{matrix}

(35)

It is obvious that the UGV is an under-actuated system. Therefore, CTMPC^38,39 is introduced and enhanced in this paper.

The motion of the UGV is predicted using the Taylor expansion principle based on the linearized model

{\begin{matrix} X (t + tp) = X (t) + tp \overset{\cdot}{X} (t) + \frac{t p^{2}}{2!} \overset{\cdot\cdot}{X} (t) + \frac{t p^{3}}{3!} \overset{. . .}{X} (t) \\ Y (t + tp) = Y (t) + tp \overset{\cdot}{Y} (t) + \frac{t p^{2}}{2!} \overset{\cdot\cdot}{Y} (t) + \frac{t p^{3}}{3!} \overset{. . .}{Y} (t) \end{matrix}

(36)

where tp is the Taylor expansion time. In order to ensure good approximation performance, tp should be small enough.

In path planning, the control input constraint has been considered so that CTMPC becomes the unconstrained optimization problem. This makes the iterative computation of control input be the analytical calculation, which reduces the computation greatly. Different from the conventional CTMPC, the tracking performance of X and Y is synthesized to form the cost function

J (t) = \frac{q_{21} e_{X}^{2} (t + tp) + q_{22} e_{Y}^{2} (t + tp) + r_{21} u^{2} (t)}{2}

(37)

where $q_{21}$ , $q_{22}$ , and $r_{21}$ are the positive constant coefficients. $e_{X} (t + tp)$ and $e_{Y} (t + tp)$ are the components of the tracking error in the direction of X and Y at time $t + tp$ as follows

\begin{matrix} e_{X} (t + tp) = X (t + tp) - X^{*} (t + tp) \\ e_{Y} (t + tp) = Y (t + tp) - Y^{*} (t + tp) \end{matrix}

(38)

where $(X^{*} (t + tp), Y^{*} (t + tp))$ is the position variable of the nominal system at time $t + tp$ . It can also be obtained by the Taylor expansion principle and has the same model as equations (33) and (34). There exist the following relations for the local path planned

\begin{matrix} X^{*} (t) = X_{pl} (t) \\ Y^{*} (t) = Y_{pl} (t) \end{matrix}

(39)

The unique control input is obtained by solving $\partial J / \partial u = 0$ as follows, which can satisfy the tracking requirements of both X and Y

\begin{matrix} u = q (- q_{21} (e_{X} + tp {\overset{\cdot}{e}}_{X} + \frac{t p^{2}}{2} {\overset{\cdot\cdot}{e}}_{X} + \frac{t p^{3}}{6} (f_{1} + d_{1} - {\overset{. . .}{X}}_{pl})) \\ - q_{22} (e_{Y} + tp {\overset{\cdot}{e}}_{Y} + \frac{t p^{2}}{2} {\overset{\cdot\cdot}{e}}_{Y} + \frac{t p^{3}}{6} (f_{2} + d_{2} - {\overset{. . .}{Y}}_{pl}))) \end{matrix}

(40)

where

q = \frac{6}{t p^{3} (q_{21} g_{1} + q_{22} g_{2}) + 6 r_{21}}

(41)

Because $X_{pl}$ and $Y_{pl}$ are both continuous functions through fitting, it is easy to get the high-order derivatives of $X_{pl}$ and $Y_{pl}$ in equation (40). However, $d_{1}$ and $d_{2}$ are unknown disturbances, which should be estimated.

Disturbance observation by ESO

UGV hardware system is quite complex, and the environment is varying, which results in various disturbances on the control system. This may cause the UGV to sideslip or overturn. In this paper, the effect of the rough and uneven roads on the vehicle suspension system, the influence of wind resistance on the steering system, and the existence of gear meshing clearance are considered as the source of external disturbances. Therefore, ESO is designed to observe them. By taking the lumped disturbance as an extended state variable, the extended system is presented as

{\begin{matrix} \begin{matrix} {\overset{\cdot}{X}}_{0} = X_{1} \\ {\overset{\cdot}{X}}_{1} = X_{2} \\ {\overset{\cdot}{X}}_{2} = f_{1} + g_{1} u + X_{3} \end{matrix} \\ {\overset{\cdot}{X}}_{3} = Δ_{1} \end{matrix}

(42)

{\begin{matrix} \begin{matrix} {\overset{\cdot}{Y}}_{0} = Y_{1} \\ {\overset{\cdot}{Y}}_{1} = Y_{2} \\ {\overset{\cdot}{Y}}_{2} = f_{2} + g_{2} u + Y_{3} \end{matrix} \\ {\overset{\cdot}{Y}}_{3} = Δ_{2} \end{matrix}

(43)

where $Δ_{1}$ and $Δ_{2}$ are the derivatives of $d_{1}$ and $d_{2}$ . Then, the linear ESO is introduced for the above extended system

{\begin{matrix} \begin{matrix} {\overset{\cdot}{\hat{X}}}_{0} = {\hat{X}}_{1} + k_{11} (X - {\hat{X}}_{0}) \\ {\overset{\cdot}{\hat{X}}}_{1} = {\hat{X}}_{2} + k_{12} (X - {\hat{X}}_{0}) \\ {\overset{\cdot}{\hat{X}}}_{2} = f_{1} + g_{1} u + {\hat{X}}_{3} + k_{13} (X - {\hat{X}}_{0}) \end{matrix} \\ {\overset{\cdot}{\hat{X}}}_{3} = k_{14} (X - {\hat{X}}_{0}) \end{matrix}

(44)

{\begin{matrix} {\dot{\hat{Y}}}_{0} = {\hat{Y}}_{1} + k_{21} (Y - {\hat{Y}}_{0}) \\ {\dot{\hat{Y}}}_{1} = {\hat{Y}}_{2} + k_{22} (Y - {\hat{Y}}_{0}) \\ {\dot{\hat{Y}}}_{2} = f_{2} + g_{2} u + {\hat{Y}}_{3} + k_{23} (Y - {\hat{Y}}_{0}) \\ {\dot{\hat{Y}}}_{3} = k_{24} (Y - {\hat{Y}}_{0}) \end{matrix}

(45)

where ${\hat{X}}_{0}$ , ${\hat{X}}_{1}$ , and ${\hat{X}}_{2}$ are the observations of X, $\overset{\cdot}{X}$ and $\overset{\cdot\cdot}{X}$ ; ${\hat{Y}}_{0}$ , ${\hat{Y}}_{1}$ , and ${\hat{Y}}_{2}$ are the observations of Y, $\overset{\cdot}{Y}$ , and $\overset{\cdot\cdot}{Y}$ ; Similarly, ${\hat{X}}_{3}$ and ${\hat{Y}}_{3}$ are the observations of $d_{1}$ and $d_{2}$ . $k_{11}$ , $k_{12}$ , $k_{13}$ , $k_{14}$ , $k_{21}$ , $k_{22}$ , $k_{23}$ , and $k_{24}$ are the observer gains. When these gains are appropriately designed, the observation values of the disturbances can accurately approximate the actual disturbances.

With ${\hat{X}}_{3}$ and ${\hat{Y}}_{3}$ by ESO, the control input u becomes

\begin{matrix} u = q (- q_{21} (e_{X} + tp {\overset{\cdot}{e}}_{X} + \frac{t p^{2}}{2} {\overset{\cdot\cdot}{e}}_{X} + \frac{t p^{3}}{6} (f_{1} + {\hat{X}}_{3} - {\overset{. . .}{X}}_{pl})) \\ - q_{22} (e_{Y} + tp {\overset{\cdot}{e}}_{Y} + \frac{t p^{2}}{2} {\overset{\cdot\cdot}{e}}_{Y} + \frac{t p^{3}}{6} (f_{2} + {\hat{Y}}_{3} - {\overset{. . .}{Y}}_{pl}))) \end{matrix}

(46)

Assumption 4

$d_{1}$ and $d_{2}$ are bounded, and $Δ_{1}$ and $Δ_{2}$ are bounded. There exist positive constants that make $‖ {\overset{\cdot}{d}}_{1} ‖ = ‖ Δ_{1} ‖ \leq d_{M 1}$ and $‖ {\overset{\cdot}{d}}_{2} ‖ = ‖ Δ_{2} ‖ \leq d_{M 2}$ hold.

Theorem 1

If the lumped disturbances satisfy Assumption 4, the observation error of the ESO is bounded.

Proof

We can get the following differential equations about the observation errors at X from equations (42) and (44)

{\begin{matrix} \begin{matrix} {\overset{\cdot}{e}}_{0} = e_{1} - k_{11} e_{0} \\ {\overset{\cdot}{e}}_{1} = e_{2} - k_{12} e_{0} \end{matrix} \\ \begin{matrix} {\overset{\cdot}{e}}_{2} = e_{3} - k_{13} e_{0} \\ {\overset{\cdot}{e}}_{3} = Δ_{1} - k_{14} e_{0} \end{matrix} \end{matrix}

(47)

where

\begin{matrix} e_{i} = X_{i} - {\hat{X}}_{i}, i = 0, 1, 2 \\ e_{3} = d_{1} - {\hat{X}}_{3} \end{matrix}

(48)

The differential equations are transformed into the following

\begin{matrix} \overset{\cdot}{e} = [\begin{matrix} \begin{matrix} {\overset{\cdot}{e}}_{0} \\ {\overset{\cdot}{e}}_{1} \end{matrix} \\ \begin{matrix} {\overset{\cdot}{e}}_{2} \\ {\overset{\cdot}{e}}_{3} \end{matrix} \end{matrix}] = [\begin{matrix} \begin{matrix} - k_{11} & 1 \\ - k_{12} & 0 \end{matrix} & \begin{matrix} 0 & 0 \\ 1 & 0 \end{matrix} \\ \begin{matrix} - k_{13} & 0 \\ - k_{14} & 0 \end{matrix} & \begin{matrix} 0 & 1 \\ 0 & 0 \end{matrix} \end{matrix}] [\begin{matrix} e_{0} \\ e_{1} \\ \begin{matrix} e_{2} \\ e_{3} \end{matrix} \end{matrix}] \\ + [\begin{matrix} \begin{matrix} 0 \\ 0 \end{matrix} \\ 0 \\ 1 \end{matrix}] Δ_{1} = Ae + γ Δ_{1} \end{matrix}

(49)

where

A = [\begin{matrix} \begin{matrix} - k_{11} & 1 \\ - k_{12} & 0 \end{matrix} & \begin{matrix} 0 & 0 \\ 1 & 0 \end{matrix} \\ \begin{matrix} - k_{13} & 0 \\ - k_{14} & 0 \end{matrix} & \begin{matrix} 0 & 1 \\ 0 & 0 \end{matrix} \end{matrix}], γ = [\begin{matrix} \begin{matrix} 0 \\ 0 \end{matrix} \\ 0 \\ 1 \end{matrix}]

Solving equation (49), yields

e (t) = \exp (At) e (0) + \int_{0}^{t} \exp (A (t - τ)) γ Δ_{1} dt

(50)

We can obtain the eigenvalues of the matrix A, and mark them as $λ_{1}$ , $λ_{2}$ , $λ_{3}$ , and $λ_{4}$ . Suppose $λ_{i} \leq λ_{j} (i < j, i = 1, \dots, 4)$ .

According to Assumption 4, there is

\begin{matrix} ‖ e (t) ‖ \leq ‖ \exp (At) e (0) ‖ + ‖ \int_{0}^{t} \exp (A (t - τ)) γ Δ_{1} dt ‖ \\ \leq ‖ e (0) ‖ \exp (- λ_{1} t) + \frac{d_{M 1}}{λ_{1}} (1 - \exp (- λ_{1} t)) \\ \leq ‖ e (0) ‖ + \frac{d_{M 1}}{λ_{1}} \end{matrix}

(51)

which means that the observation errors of the ESO are bounded. By choosing the suitable observer gains, the observation errors can converge to the origin point $({\hat{X}}_{3} \approx d_{1})$ . Similarly, the observation errors at Y can also be guaranteed.

Assumption 5

${\overset{\dots}{X}}_{pl}$ and ${\overset{\dots}{Y}}_{pl}$ are continuous and bounded.

Theorem 2

If the lumped disturbances and the path variables satisfy Assumptions 4 and 5, and the controller parameters are selected properly, the path tracking error is bounded, and the whole control system is stable.

Proof

Substitute equation (46) into equations (33) and (34), and subtract the nominal system, then the error state equation is formulated as

\begin{array}{l} {\dot{e}}_{X 0} = e_{X 1} \\ {\dot{e}}_{X 1} = e_{X 2} \\ {\dot{e}}_{X 2} = (\begin{array}{l} - q_{21} (e_{x} + t p {\dot{e}}_{x} + \frac{t p^{2}}{2} {\overset{\cdot\cdot}{e}}_{x} + \frac{t p^{3}}{6} (f_{1} + \overset{⌢}{X} - {\overset{⃛}{X}}_{p l})) \\ - {q_{2}}_{2} (e_{Y} + t p {\dot{e}}_{Y} + \frac{t p^{2}}{2} {\overset{\cdot\cdot}{e}}_{Y} + \frac{t p^{3}}{6} (f_{2} + {\overset{⌢}{Y}}_{3} - {\overset{⃛}{Y}}_{p} l)) \end{array}) g_{1} q + f_{1} + d_{1} - {\overset{⃛}{X}}_{p l} \\ e_{X 0} = e_{X} \\ {\dot{e}}_{Y 0} = e_{Y 1} \\ {\dot{e}}_{Y 1} = e_{Y 2} \\ {\dot{e}}_{Y 2} = (\begin{array}{l} - q_{21} (e_{x} + t p {\dot{e}}_{x} + \frac{t p^{2}}{2} {\overset{\cdot\cdot}{e}}_{x} + \frac{t p^{3}}{6} (f_{1} + \overset{⌢}{X} - {\overset{⃛}{X}}_{p l})) \\ - {q_{2}}_{2} (e_{Y} + t p {\dot{e}}_{Y} + \frac{t p^{2}}{2} {\overset{\cdot\cdot}{e}}_{Y} + \frac{t p^{3}}{6} (f_{2} + {\overset{⌢}{Y}}_{3} - {\overset{⃛}{Y}}_{p} l)) \end{array}) g_{2} q + f_{2} + d_{2} - {\overset{⃛}{Y}}_{p l} \\ e_{Y 0} = e_{Y} \end{array}

(52)

$E = [\begin{matrix} \begin{matrix} e_{X 0} & e_{X 1} & e_{X 2} \end{matrix} & \begin{matrix} e_{Y 0} & e_{Y 1} & e_{Y 2} \end{matrix} \end{matrix}]^{T}$ is chosen as the error state variable, and equation (52) can be simplified as

\overset{\cdot}{E} = A' E + H

(53)

where

A' = [\begin{matrix} \begin{matrix} 0 & 1 & 0 \\ 0 & 0 & 1 \\ a_{11} & a_{12} & a_{13} \end{matrix} & \begin{matrix} 0 & 0 & 0 \\ 0 & 0 & 0 \\ a_{14} & a_{15} & a_{16} \end{matrix} \\ \begin{matrix} 0 & 0 & 0 \\ 0 & 0 & 0 \\ a_{21} & a_{22} & a_{23} \end{matrix} & \begin{matrix} 0 & 1 & 0 \\ 0 & 0 & 1 \\ a_{24} & a_{25} & a_{26} \end{matrix} \end{matrix}]

(54)

H = [\begin{matrix} \begin{matrix} 0 \\ 0 \\ h_{1} \end{matrix} \\ \begin{matrix} 0 \\ 0 \\ h_{2} \end{matrix} \end{matrix}]

(55)

{\begin{matrix} \begin{matrix} a_{i 1} = - g_{i} q q_{21} \\ a_{i 2} = - g_{i} qtp q_{21} \\ a_{i 3} = - \frac{1}{2} g_{i} qt p^{2} q_{21} \end{matrix} \\ \begin{matrix} a_{i 4} = - g_{i} q q_{22} \\ a_{i 5} = - g_{i} qtp q_{22} \\ a_{i 6} = - \frac{1}{2} g_{i} qt p^{2} q_{22} \end{matrix} \end{matrix}, i = 1, 2

(56)

\begin{array}{l} h_{1} = f_{1} + g_{1} q (- q_{21} (\frac{t p^{3}}{6} (f_{1} + {\overset{⌢}{X}}_{3} - {\overset{⃛}{X}}_{p l})) - q_{22} (\frac{t p^{3}}{6} (f_{2} + {\overset{⌢}{Y}}_{3} - {\overset{⃛}{Y}}_{p l}))) + d_{1} - {\overset{⃛}{X}}_{p l} \\ h_{2} = f_{2} + g_{2} q (- q_{21} (\frac{t p^{3}}{6} (f_{1} + {\overset{⌢}{X}}_{3} - {\overset{⃛}{X}}_{p l})) - q_{22} (\frac{t p^{3}}{6} (f_{2} + {\overset{⌢}{Y}}_{3} - {\overset{⃛}{Y}}_{p l}))) + d_{2} - {\overset{⃛}{Y}}_{p l} \end{array}

(57)

Select the appropriate parameters $(tp, q_{21}, q_{22}, r_{21})$ to ensure that the eigenvalues of $A'$ are located on the left half plane. According to Assumption 4, Assumption 5, and Theorem 1, we can conclude that the error state is bounded. So, the whole control system is stable.

Algorithm

The algorithm of the proposed obstacle avoidance steering control strategy is listed as follows:

Step 1. Judge the collision possibility of the obstacles. If there is a possibility of collision, go to next step, otherwise skip to Step 3.

Step 2. Use suitable planning methods for the different types of obstacles to optimize the local obstacle avoidance path online and then go to Step 4.

Step 3. Instead of planning a new local path, take the reference path directly as the output of the path planning controller.

Step 4. Use the CTMPC algorithm based on ESO to track the local path. At the next time, return to Step 1.

Simulation

The scenarios of the static obstacles and dynamic obstacles are used to demonstrate the steerability of the proposed obstacle avoidance control strategy. In this paper, the numerical simulations using MATLAB and CarSim are proposed to prove the effectiveness and feasibility of the proposed method. Table 1 shows the dynamic parameters of the UGV.

Table 1.

Dynamic parameters of the UGV.

Dynamic parameters	Values
m	1723 kg
$I_{zz}$	4175 kg m²
$C_{cf}$	66,900 N rad⁻¹
$C_{cr}$	62,700 N rad⁻¹
$C_{lf}$	66,900 N rad⁻¹
$C_{lr}$	62,700 N rad⁻¹
$l_{f}$	1.232 m
$l_{r}$	1.468 m
$s_{f}$	0.2
$s_{r}$	0.2
v	5 m/s

UGV: unmanned ground vehicle.

Static obstacles

In static obstacle scenario, there are two lanes marked as lane one and lane two, respectively, in the same direction. The length of the road is 50 m, and the width of each lane is 3.5 m. The origin of the inertial coordinate system is located at the starting point of the centerline of the lane one. There are two potholes at (10, 0) and (35, 3.5) marked as ① and ②, respectively, and their length and width are both 1.6 m. The width of the UGV is 2 m. Before encountering the obstacles, the UGV is cruising at lane one at 5 m/s. Simultaneously, the sinusoidal term $(d_{u} = 0.01 \sin t)$ is equivalently taken as the external disturbance.

The parameters related to the constraints are as follows: $Y_{\max}$ is 4.25 m; $Y_{\min}$ is −0.75 m; $d_{safe}$ is 2 m; $δ_{fmax}$ is 0.52 rad; the friction coefficient between the tire and the road surface is 0.42; and g is $9.8 m / s^{2}$ .

First, the UGV computes a local path using the optimal path reconfiguration based on DC as shown in Figure 8, in which N = 40. The red path represents the computed result using the point mass model. It can be seen that this path maintains the adequate safe distances from the obstacles and satisfies the road constraint. Moreover, the UGV keeps cruising on the lane one after avoiding the first obstacle. As shown in Figure 9, the input in the optimization problem also meets the dynamic constraints. It is obvious that there is no risk of sideslip or rollover, that is, the UGV is stable.

Figure 8.

Path planning in static obstacle scenario.

Figure 9.

Lateral acceleration.

Second, in the path tracking controller design, the Taylor expansion time is 0.45 s. The observer gains are chosen as shown in Table 2, and the lumped disturbances are observed as shown in Figure 10.

Table 2.

Observer gains in static obstacle scenario.

Observer for $d_{1}$		Observer for $d_{2}$
Parameters	Values	Parameters	Values
$k_{11}$	150	$k_{21}$	3
$k_{12}$	500	$k_{22}$	350
$k_{13}$	700	$k_{23}$	350
$k_{14}$	−75	$k_{24}$	10,000

Figure 10.

(a) The observation of d₁ in static obstacle scenario. (b) The observation of d₂ in static obstacle scenario.

We can see that both observers can converge to the actual disturbances. Figure 11 shows the tracking performance. The red lines represent the left and right outer boundaries of the UGV. The UGV adopts detour strategy after judging that the obstacle ① poses a risk to the current driving. However, the extra detour is not implemented after determining that the obstacle ② is not threatening. Furthermore, the designed control system can effectively suppress the disturbance and maintain the UGV stability. As shown in Figure 12, UGV always keeps the safe distance, that is, 2 m to two obstacles.

Figure 11.

Outer boundaries of UGV path tracking in static obstacle scenario.

Figure 12.

The distance from obstacles in static obstacle scenario.

In order to illustrate the effectiveness of the proposed control method, a comparison simulation with dynamic-model-based nonlinear model predictive control (NMPC)⁴² method is carried out as shown in Figures 13 and 14.

Figure 13.

Obstacle avoidance comparison at the centroid of UGV with NMPC in static obstacle scenario.

Figure 14.

Control input comparison with NMPC in static obstacle scenario.

The centroid coordinates in the course of vehicle driving are shown in Figure 13. Although the two methods can successfully avoid the obstacles, the vehicle does not swing in the proposed method after getting around the obstacles. Moreover, the proposed method spends 19.09 s in calculation, but the traditional NMPC takes 241.92 s under the same conditions. The constraint of $δ_{f}$ is less than $0.52 rad$ , but the constraint originated from dynamics is $δ_{f} < μ g (I / v^{2}) = 0.44 rad$ . The red dotted lines in Figure 14 show the two constraints, respectively. From Figure 14, we can see that the actual control inputs of the proposed approach and NMPC can be kept within the intersection of the two constraints, but control input changes more frequently and has larger amplitude in NMPC. It can be seen that the proposed control method has better control effect and real-time performance.

Dynamic obstacles

In dynamic obstacles scenario, the length of the road is still 50 m, and the width of each lane is 3.5 m; there are two motorcycles driving at 1 m/s in the same direction with the UGV in both lanes, whose initial positions are at (10, 0) and (35, 3.5); their length and width are 1.6 m and 0.7 m, marked as ① and ②, respectively; $Y_{\max}$ is 4.25 m and $Y_{\min}$ is −0.75 m. $d_{safe}$ is 1.6 m, the friction coefficient is 0.42, and g is 9.8 m/s². Similarly, $d_{u} = 0.01 \sin t$ is the disturbance.

In the path computation, the sampling period is 0.45 s; the predictive period is six steps, and the control period is four steps. The planning cycle is 1.8 s.

In the path tracking, the Taylor expansion time is 0.9 s. The CTMPC controller and ESO are designed. The observer gains are chosen as shown in Table 3. The disturbances are observed as shown in Figure 15.

Table 3.

Observer gains in dynamic obstacle scenario.

Observer for $d_{1}$		Observer for $d_{2}$
Parameters	Values	Parameters	Values
$k_{11}$	150	$k_{21}$	3
$k_{12}$	500	$k_{22}$	350
$k_{13}$	700	$k_{23}$	350
$k_{14}$	−70	$k_{24}$	10,000

Figure 15.

(a) The observation of d₁ in dynamic obstacle scenario. (b) The observation of d₂ in dynamic obstacle scenario.

It can be seen that after a certain time period, both observations can still converge to the actual disturbances like that in static obstacle scenario. Figure 16 shows the tracking result.

Figure 16.

Outer boundaries of UGV path tracking in dynamic obstacle scenario.

At time $t_{1}$ , the UGV takes overtaking action after judging that the motorcycle ① is posing a risk to the current driving. At time $t_{2}$ , the UGV is far away from the slow-moving motorcycle ① and returns to the original lane. At time $t_{3}$ , the UGV does not take extra detour after determining that the motorcycle ② does not collide with the UGV. At time $t_{4}$ , the UGV runs in the original direction. It can be seen that the designed control system can maintain stability of the UGV and ensure enough safe distance from obstacles. As shown in Figure 17, UGV always keeps the safe distance, that is, 1.6 m to two obstacles.

Figure 17.

The distance from obstacles in dynamic obstacle scenario.

The comparison between the proposed method and traditional NMPC in dynamic obstacle scenario is shown in Figures 18 and 19. In the traditional NMPC, the swing behavior of the UGV still exists. In this simulation, the proposed method spends 31.24 s, and NMPC uses 303.42 s.

Figure 18.

Obstacle avoidance comparison at the centroid of UGV with NMPC in dynamic obstacle scenario.

Figure 19.

Control input comparison with NMPC in dynamic obstacle scenario.

The control inputs are shown in Figure 22. The trend of the control input in the proposed method is much smoother than that of NMPC. That is the variance of the control input of NMPC is still larger. We can get the same conclusion as the static obstacle scenario from Figures 18 and 19.

CarSim simulation

In order to verify the performance of the proposed method in realistic environment, CarSim and MATLAB are combined to give the joint simulation results. The dynamic characteristics are established in CarSim. The position information of the obstacles is the same as the static and dynamic obstacle scenarios in MATLAB simulation. The vehicle model is chosen as B-class, Hatchback, which is consistent with the dynamic parameters in this paper. To clarify the difference between CarSim joint simulation and MATLAB simulation, the result of the former is called CarSim, and the latter is called MATLAB.

In static scenario, cones are used to take place of the potholes in Figure 20 for the reason that there are no potholes in the animator of CarSim. Figure 20 demonstrates the actual trajectory of the vehicle, and it is obvious that the vehicle succeeds in avoiding the static obstacles. Figure 21 shows the simulation comparison between using CarSim and MATLAB, which shows the effectiveness of the proposed method both in simulation and experiment scenarios. Figure 22 gives the control inputs using CarSim and MATLAB, respectively, where the control inputs have the similar behavior.

Figure 20.

Path tracking of UGV by CarSim in static obstacle scenario.

Figure 21.

Obstacle avoidance comparison at the centroid of UGV using CarSim and MATLAB in static obstacle scenario.

Figure 22.

Control input comparison using CarSim and MATLAB in static obstacle scenario.

Figures 23 –25 demonstrate the control performance of the proposed method using CarSim simulation in dynamic obstacle scenario. Figure 23 shows the real trajectory of the vehicle. It can be seen that the vehicle can avoid the moving obstacles in the road without collision. Figure 24 presents the simulation comparison between using CarSim and MATLAB. The UGV can successfully avoid the dynamic obstacles and keep 1.6 m distance from them, which satisfy the constraints. The control inputs are depicted in Figure 25, and they have similar curves. This verifies the effectiveness of the proposed method in more realistic scenario.

Figure 23.

Path tracking of UGV by CarSim in dynamic obstacle scenario.

Figure 24.

Obstacle avoidance comparison at the centroid of UGV using CarSim and MATLAB in dynamic obstacle scenario.

Figure 25.

Control input comparison using CarSim and MATLAB in dynamic obstacle scenario.

Conclusion

For more safety and flexibility, a strategy for the complex obstacle avoidance steering control problem of the UGV is designed in this paper. According to the type of obstacles, an optimal path reconfiguration based on DC and RHC are proposed, respectively. Different from the previous path planning algorithms, our algorithms take into account the constraint of the actual control input. The unconstrained CTMPC controller is designed to make the tracking system have better real-time performance. The ESO is introduced to observe the disturbances to guarantee the stability and robustness of the control system.

Footnotes

Declaration of conflicting interests

The author(s) declared no potential conflicts of interest with respect to the research, authorship, and/or publication of this article.

Funding

The author(s) disclosed receipt of the following financial support for the research, authorship, and/or publication of this article: This work was supported by the National Natural Science Foundation of China (Nos 61773279 and 61873340), the Open Project of Key Laboratory of Micro Opto-Electro Mechanical System Technology, Tianjin University, Ministry of Education (No. MOMST 2016-4), and Joint Science Foundation of Ministry of Education of China (No. 6141A0202304).

ORCID iD

Chaofang Hu

References

Abdelhamid

Hassanein

Takahara

. Vehicle as a resource (VaaR). IEEE Netw 2014; 29(1): 12–17.

Parent

. Advanced urban transport: automation is on the way. IEEE Intell Syst 2007; 22(2): 9–11.

Önkol

Kasnakoglu

. Adaptive model predictive control of a two-wheeled robot manipulator with varying mass. Meas Contr 2018; 51(1–2): 38–56.

Kanarachos

. A new min-max methodology for computing optimised obstacle avoidance steering manoeuvres of ground vehicles. Int J Syst Sci 2014; 45(5): 1042–1057.

Naranjo

Gonzalez

Garcia

, et al. Power-steering control architecture for automatic driving. IEEE Trans Intell Transp Syst 2005; 6(4): 406–415.

Marino

Scalzi

Netto

. Nested PID steering control for lane keeping in autonomous vehicles. Contr Eng Pract 2011; 19(12): 1459–1467.

Kadir

Zamzuri

Hudha

, et al. Optimisation of yaw rejection control for armoured vehicle using Taguchi method. Int J Heavy Veh Syst 2016; 23(1): 60–80.

Jin

Chen

Zhang

, et al. Longitudinal velocity estimation based on fuzzy logic for electronic stability control system. Adv Mech Eng 2017; 9(5): 1–12.

March

Shim

. Integrated control of suspension and front steering to enhance vehicle handling. Proc IMechE, Part D: J Automobile Engineering 2010; 221(4): 377–391.

10.

Sun

Guo

Cao

, et al. Study on integrated control of active front steering and direct yaw moment based on vehicle lateral velocity estimation. Math Prob Eng 2013; 2013(12): 1–8.

11.

Eski

Temürlenk

. Design of neural network-based control systems for active steering system. Nonlin Dynam 2013; 73(3): 1443–1454.

12.

Deng

Yan

, et al. Adaptive neural network automatic parking constrained control via anti-windup compensator. Adv Mech Eng 2017; 9(5): 1–10.

13.

Zhou

Wen

, et al. Intervention criterion and control research for active front steering with consideration of road adhesion. Vehicle Syst Dyn 2018; 56(4): 553–578.

14.

Yong

Gao

Ding

, et al. An integrated algorithm for vehicle stability improvement with the coordination of direct yaw moment and four-wheel steering control. Int J Vehicle Des 2017; 74(3): 231–256.

15.

Wong

Zhao

, et al. Multi-objective sliding mode control on vehicle cornering stability with variable gear ratio actuator-based active front steering systems. Sensors 2017; 17(1): 49.

16.

Zhang

Huang

, et al. Observer-based H∞ control for vehicle handling and stability subject to parameter uncertainties. Proc IMechE, Part I: J Systems and Control Engineering 2013; 227(9): 704–717.

17.

, et al. Robust H∞ output-feedback yaw control for in-wheel-motor driven electric vehicles with differential steering. In: 2015 54th IEEE conference on decision and control, Osaka, Japan, 15–18 December 2015, pp.1521–1526. New York: IEEE.

18.

Javid

Eghtesad

Khayatian

, et al. Experimental study of dynamic based feedback linearization for trajectory tracking of a four-wheel autonomous ground vehicle. Auton Robot 2005; 19(1): 27–40.

19.

Eghtesad

Dan

. Experimental study of the dynamic based feedback linearization of an autonomous wheeled ground vehicle. Robot Auton Syst 2004; 47(1): 47–63.

20.

Falcone

Borrelli

Asgari

, et al. Predictive active steering control for autonomous vehicle systems. IEEE T Contr Syst T 2007; 15(3): 566–580.

21.

Cao

Zhao

, et al. Model predictive control-based steering control of unmanned ground vehicle with tire blowout. J Tianjin Univ (Sci Technol) 2019; 52(5): 468–474.

22.

Adireddy

Shim

Rhode

, et al. Model predictive control (MPC) based combined wheel torque and steering control using a simplified tire model In: ASME 2010 dynamic systems and control conference, Cambridge, MA, 12–15 September 2010, pp. 165–172. New York: ASME.

23.

Falcone

Borrelli

Tseng

, et al. Linear time-varying model predictive control and its application to active steering systems: stability analysis and experimental validation. Int J Robust Nonlin 2008; 21(8): 862–875.

24.

Kou

Raksincharoensak

Nagai

. A study on shared control between the driver and an active steering control system in emergency obstacle avoidance situations. In: 19th IFAC world congress, Cape Town, South Africa, 24–29 August 2014, pp. 6338–6343. Geneva: IFAC.

25.

Yoon

Shin

Kim

, et al. Model-predictive active steering and obstacle avoidance for autonomous ground vehicles. Control Eng Pract 2009; 17(7): 741–750.

26.

Sedighi

Ashenayi

Manikas

, et al. Autonomous local path planning for a mobile robot using a genetic algorithm. In: 2004 congress on evolutionary computation, Portland, OR, 19–23 June 2004, pp. 1338–1345. New York: IEEE.

27.

Shimoda

Kuroda

Iagnemma

. High-speed navigation of unmanned ground vehicles on uneven terrain using potential fields. Robotica 2007; 25(4): 409–424.

28.

Zhuge

Cai

Tang

. A novel dynamic obstacle avoidance algorithm based on collision time histogram. Chin J Electron 2017; 26(3): 522–529.

29.

Kuwata

Teo

Fiore

, et al. Real-time motion planning with applications to autonomous urban driving. IEEE T Contr Syst T 2009; 17(5): 1105–1118.

30.

Hao

Agrawal

. Formation planning and control of UGVs with trailers. Auton Robot 2005; 19(3): 257–270.

31.

Vilca

Adouane

Mezouar

. Optimal multi-criteria waypoint selection for autonomous vehicle navigation in structured environment. J Intell Robot Syst 2016; 82(2): 1–24.

32.

Zhang

. Adaptive backstepping sliding mode control for wheel slip tracking of vehicle with uncertainty observer. Meas Contr 2018; 51(9–10): 396–405.

33.

Guo

Luo

. Integrated adaptive dynamic surface car-following control for nonholonomic autonomous electric vehicles. Sci China Technol Sc 2017; 60(8): 1221–1230.

34.

Güvenc

Karaman

. Robust MIMO disturbance observer analysis and design with application to active car steering. Int J Robust Nonlin 2010; 20(8): 873–891.

35.

Almayyahi

Wang

Hussein

, et al. Motion control design for unmanned ground vehicle in dynamic environment using intelligent controller. Int J Intell Comput Cybern 2017; 10(4): 530–548.

36.

Zhang

. Networked model predictive control based on neighbourhood optimization for serially connected large-scale processes. J Process Contr 2007; 17(1): 37–50.

37.

Ding

Cychowski

, et al. A synthesis approach for output feedback robust constrained model predictive control. Automatica 2008; 44(1): 258–264.

38.

Panchal

Kolhe

Talole

. Robust predictive control of a class of nonlinear systems. J Guid Control Dynam 2014; 37: 1437–1445.

39.

. Optimal predictive control of continuous nonlinear systems. Int J Contr 1995; 62(3): 633–649.

40.

Kurashiki

Fukao

Ishiyama

, et al. Orchard traveling UGV using particle filter based localization and inverse optimal control. In: 2010 IEEE/SICE international symposium on system integration, Sendai, Japan, 21–22 December 2010, pp. 31–36. New York: IEEE.

41.

Haytham

Elhalwagy

Wassal

, et al. Modeling and simulation of four-wheel steering unmanned ground vehicles using a PID controller. In: 2014 international conference on engineering and technology, Cairo, Egypt, 19–20 April 2014, pp. 1–8. New York: IEEE.

42.

Abbas

Milman

Eklund

. Obstacle avoidance in real time with nonlinear model predictive control of autonomous vehicles. Can J Elect Comput E 2017; 40(1): 12–22.

Steering control based on model predictive control for obstacle avoidance of unmanned ground vehicle

Abstract

Keywords

Introduction

UGV model

Six-order dynamic model

Assumption 1

The point mass model

Obstacle avoidance steering control strategy design

Judgment of collision possibility

Assumption 2

Path planning

Static obstacle scenario

Assumption 3

Dynamic obstacle scenario

Path tracking

CTMPC

Disturbance observation by ESO

Assumption 4

Theorem 1

Proof

Assumption 5

Theorem 2

Proof

Algorithm

Simulation

Static obstacles

Dynamic obstacles

CarSim simulation

Conclusion

Footnotes

Declaration of conflicting interests

Funding

ORCID iD

References