4780 IEEE TRANSACTIONS ON INTELLIGENT VEHICLES, VOL. 9, NO.

4, APRIL 2024

A Safe Motion Planning and Reliable Control

Framework for Autonomous Vehicles
Huihui Pan , Senior Member, IEEE, Mao Luo , Jue Wang , Tenglong Huang ,
and Weichao Sun , Senior Member, IEEE

Abstract—Accurate trajectory tracking is unrealistic in real- brain, which processes the perception information obtained from
world scenarios, however, which is commonly assumed to facilitate sensors to generate a safe motion or trajectory without collisions.
motion planning algorithm design. In this paper, a safe and reliable The control level [10] designs a suitable controller to track the
motion planning and control framework is proposed to handle the desired motion or trajectory. To simplify motion planning algo-
tracking errors caused by inaccurate tracking by coordinating the rithm design, it is commonly assumed that the planned trajectory
motion planning layer and controller. Specifically, motion space
is divided into safe regions and risky regions by designing the
can be tracked with complete accuracy [11]. Because tracking
movement restraint size dependent on tracking error to construct errors caused by inaccurate tracking are difficult to obtain in
the repulsive potential field. The collision-free waypoint set can advance, it leads to collision risk caused by tracking errors in the
then be obtained by combining global search and the proposed planning layer that are hard to avoid. In this paper, it is expected
waypoint set filtering method. The planned trajectory is fitted by to design a motion planning and control scheme that allows the
an optimization-based approach which minimizes the acceleration system to be safe and collision-free under inaccurate tracking
of the reference trajectory. Then, the planned trajectory is checked by coordinating motion control and planning algorithms in the
and modified by the designed anti-collision modification to ensure presence of tracking errors.
safety. Using invertible transformation and adaptive compensation The collision-free [12], [13] safe trajectory generation from
allows the transient trajectory tracking errors to be limited within the start point to the target point generally contains two stages:
the designed region even with actuator faults. Because tracking
error is considered and margined at the planning level, safety
searching the global path and performing local smoothing [11].
and reliability can be guaranteed by the coordination between Classical motion planning methods for global path search in-
the planning and control levels under inaccurate tracking and clude Dijkstra [14], A∗ [15], [16], [17] algorithms and arti-
actuator faults. The advantages and effectiveness of the proposed ficial potential field (APF) approach [18], etc. The APF can
motion planning and control method are verified by simulation and model the motion space intuitively, however, the traditional APF
experimental results. tends to fall into local optimality. The heuristic search, such as
Dijkstra and A∗ algorithms, can search the optimal path. A∗
Index Terms—Inaccurate tracking, safe motion planning,
actuator faults, reliable control, autonomous vehicles.
algorithm is used to search for the original point set of the
global path in this paper. The global path search algorithms are
mature, and this part is not the focus of this paper. In contrast,
I. INTRODUCTION many interesting algorithms have emerged for local trajectory
smoothing. The challenge of how to ensure the safety of planned
UTONOMOUS vehicles [1], including autonomous robot
A vehicles [2], unmanned vehicles [3], and surface vehi-
cles [4], performing a basic navigation task generally require
trajectories while performing local smoothing is an important
and attractive one that needs further research. It is worth noting
that Bezier-type [19] motion planning methods, such as the
perception, motion planning, and control to work together [3], classical Bezier algorithm and the B-Spline algorithm [20], can
[5]. The motion planning [6], [7], [8], [9] layer is taken as the significantly improve trajectory smoothness. Bezier algorithm
can ensure that the generated trajectories are arbitrary-order
Manuscript received 14 January 2024; accepted 27 January 2024. Date of differentiable. Polynomial-type trajectory planning methods,
publication 31 January 2024; date of current version 12 June 2024. This work including cubic spline [21], [22] and quintic spline, can generate
was supported in part by the National Natural Science Foundation of China under time-dependent motion trajectories based on the location of
Grant 62173108, in part by the Heilongjiang Post Doctoral Science Foundation the start and target points, desired velocity, and desired ac-
under Grant LBH-TZ2111, in part by the Major Scientific and Technological
Special Project of Heilongjiang Province under Grant 2021ZX05A01, and in part celeration. Further, optimization-based polynomial trajectory
by the Fundamental Research Funds for the Central Universities under Grant planning algorithms [23], including minimum velocity, mini
HIT.OCEF.2022012. (Corresponding author: Huihui Pan.) acceleration, minimum jerk [24], and minimum snap algorithms,
Huihui Pan, Mao Luo, Tenglong Huang, and Weichao Sun are with take the first to fourth-order derivatives of the trajectory as
the Research Institute of Intelligent Control and Systems, Harbin Insti-
tute of Technology, Harbin 150001, China (e-mail: [email protected];
cost. It is worth noting that, to produce a sufficiently smooth
[email protected]; [email protected]; [email protected]). reference trajectory, Bezier-type motion planning methods do
Jue Wang is with the Ningbo Institute of Intelligent Equipment Technology not guarantee the generated planned trajectory passes through
Company Ltd., Ningbo 315200, China, and also with the Department of Au- waypoints. Polynomial-type algorithms can constrain the refer-
tomation, University of Science and Technology of China, Hefei 230027, China ence trajectory to pass through the waypoints, and optimization-
(e-mail: [email protected]).
Color versions of one or more figures in this article are available at based polynomial planning algorithms can further optimize
https://doi.org/10.1109/TIV.2024.3360418. trajectory performance. However, by fitting the trajectory ac-
Digital Object Identifier 10.1109/TIV.2024.3360418 cording to the original waypoints in safety areas, the generated

2379-8858 © 2024 IEEE. Personal use is permitted, but republication/redistribution requires IEEE permission.
See https://www.ieee.org/publications/rights/index.html for more information.

Authorized licensed use limited to: Harbin Institute of Technology. Downloaded on July 10,2024 at 11:30:58 UTC from IEEE Xplore. Restrictions apply.
PAN et al.: SAFE MOTION PLANNING AND RELIABLE CONTROL FRAMEWORK FOR AUTONOMOUS VEHICLES 4781

fitted trajectory is still at risk of collision with the obstacle.

Meanwhile, the original global waypoints may lack useful path
information. The expectation is that by streamlining the useless
waypoints while adjusting the original risky waypoints to enable
the generation of collision-free planned trajectories. Meanwhile,
the collision risk caused by inaccurate tracking is also taken into
account. Therefore, this paper proposes a safe motion planning
method to obtain a collision-free and safe trajectory by waypoint
fitting in the case of inaccurate tracking.
An interesting and novel perspective of ensuring system se-
curity is the control barrier function (CBF) technology [25],
[26] which considers safety directly in the control layer, instead
of coordinating the motion planning layer. The CBF approach
can ensure system safety by directly dealing with safety con-
straints and potential control logic conflicts in the controller.
The application in scenarios with multiple irregular obstacles
can be further investigated. In contrast to the safety-oriented Fig. 1. Mathematical model for autonomous robot vehicles. CG denotes the
CBF approach, this paper constrains the tracking errors to pre- center of mass.
scribed regions by introducing invertible transformations, while
designing planning algorithms to ensure safety according to
the prescribed constraints [27]. It is worth noting that effective the planning layer and controller under actuator faults and
ideas, including the designed safety planner in [28], [29] and inaccurate tracking.
the motion planning method based on temporal logic [30], [31],
provide interesting solutions for dealing with tracking errors. II. PROBLEM STATEMENT
Coordinating the planning and control layers to ensure safety is
The primary objective of this paper is to develop a framework
more intuitive and provides more degrees of freedom to enhance
for safe motion planning and reliable tracking control of au-
safety.
tonomous vehicles, even in the presence of actuator failures and
In addition to system safety [32], the reliability of the system
trajectory tracking errors. The proposed framework addresses
is also crucial [33]. Actuator faults [34] due to wear and tear,
the critical issue of ensuring the safety and reliability of au-
and aging of system actuator components, including loss-of-
tonomous robot vehicles while performing complex tasks. Fig. 1
effectiveness fault and bias fault, can lead to system performance
depicts the mathematical model of autonomous robot vehicles.
degradation and even system instability. Actuator faults can
The position of the robot in the Cartesian coordinate system is
cause system states to deviate from the desired trajectory and
denoted by (x, y), while the yaw angle is represented by ϕ. The
lead to collisions. Further consideration of actuator faults to
corresponding reference trajectory is indicated by (xr , yr ), with
improve system reliability is therefore of great significance to
the reference yaw angle denoted by ϕ.
system stability and safety. In summary, this paper proposes
Defining xe and ye as the tracking errors in the x and y
a safe motion planning and control framework to enhance the
directions, respectively, and de and ϕ as the distance error and
safety and reliability of autonomous vehicles under inaccurate
yaw tracking error, as illustrated in Fig. 1, the corresponding
tracking in the presence of tracking errors and actuator faults.
errors [35] can be calculated by
Specifically, the contributions of this paper are summarized 
as follows: xe = xr − x, ye = yr − y, de = x2e + ye2 , ϕe = ϕ − ϕr
r In the motion planning layer, this paper designs a move- (1)
ment restraint size based on the prescribed constraints Referring to the kinematic relationship illustrated in Fig. 1, we
and geometric characteristics of autonomous vehicles to can obtain
allow a margin for tracking error. The movement restraint    
ẋ cos(ϕ) 0  
size is introduced to construct the repulsive potential field, v
ẏ = sin(ϕ) 0 (2)
dividing the motion space into safe and risky regions. 
r For the original point set generated based on the classical ϕ̇ 0 1
A∗ algorithm, this paper designs a waypoint set filtering where v and  represent the linear velocity and angular veloc-
method, which refines and adjusts the original point set by ity, respectively, while u1 = v and u2 =  denote the control
the proposed selecting feature points, transforming corner inputs. The possible fault [34] occurring in the i-th actuator can
points, and removing redundant points, to facilitate further be modeled as follows
smoothing.
r This paper introduces an optimization-based polynomial uif = αil ui + αib , i = 1, 2 (3)
trajectory fitting method that minimizes trajectory accel- where 0 < αil ≤ 1 denotes an unknown constant that indicates
eration. An anti-collision modification is presented by the loss-of-effective (LOE) fault occurring in the i-th actuator.
adjusting and adding auxiliary waypoints to ensure that To ensure controllability, αil > 0 is considered in this paper.
the planned trajectory is safe and collision-free. αil = 1 signifies the absence of actuator LOE fault. Meanwhile,
r A fault-tolerant controller is presented to constrain tracking αib is another unknown constant that represents the actuator
errors to prescribed regions by employing transformation bias fault, with αib = 0 indicating that the bias fault does not
functions and the adaptive backstepping technique. Safety exist. Actuator faults can arise from various sources such as
and reliability can be guaranteed by the coordination of actuator wear, aging, and mechanical stress. It is worth noting

Authorized licensed use limited to: Harbin Institute of Technology. Downloaded on July 10,2024 at 11:30:58 UTC from IEEE Xplore. Restrictions apply.
4782 IEEE TRANSACTIONS ON INTELLIGENT VEHICLES, VOL. 9, NO. 4, APRIL 2024

a grid that intersects or is obstructed by an irregular obstacle

is designated as a part of Aobst . Additionally, it is possible to
account for the obstacle size and tracking error by generating
a repulsive potential field. This potential field is utilized to
partition the remaining region into safe and risky areas, which
will be discussed further in the subsequent section. The next
step is to search the waypoint set through path planning based
on the previously established region divisions as depicted in
Fig. 2. Then, a trajectory smoothing algorithm is presented and
implemented to generate a collision-free reference trajectory, as
described in detail below.

B. Movement Restraint Size

In practical scenarios, it is unrealistic for autonomous robot
vehicles to track the planned reference trajectory with absolute
accuracy. Trajectory tracking errors commonly occur, and it
Fig. 2. Sensor data pre-processing to obtain grid map. is significant to account for them when generating the desired
trajectory to ensure collision-free navigation with obstacles, de-
spite inaccurate tracking. The motion restraint size is determined
that multiple actuators may fail simultaneously, and multiple not only by the physical size of the robot and map resolution but
forms of faults may coexist, as considered in this paper. also by the potential magnitude of tracking errors. It is evident
Meanwhile, this paper considers the collision risks caused by that the motion restraint size is proportional to the robot size and
inaccurate tracking at the motion planning level to ensure safety. the magnitude of errors. The coarser map resolution requires a
In practice, reference trajectory accurate tracking is not realistic. larger margin to ensure collision-free. The motion restraint size
Generally, it is assumed that accurate tracking can be achieved Se presented in this paper can be calculated as follows:
to simplify the challenges in motion planning.
This paper aims to develop a motion planning [36] and control Se = w1 ∗ r0 ∗ (w2 ∗ Ae + w3 ∗ W ) (4)
framework that guarantees collision-free navigation, even in
the presence of tracking inaccuracies. Specifically, the motion where Ae denotes the magnitude of the tracking error, and W
planning algorithm designed in this paper aims to achieve the is the robot size. r0 represents the width Cell of a single grid in
following properties: the map. w1 , w2 , and w3 serve as weighting factors. The motion
1) Safety and collision-free. Most motion planning algo- restraint size Se is calculated based on the selected weights,
rithms assume accurate reference trajectory tracking to which serve as a foundation for collision-free motion planning.
simplify the challenge; however, the motion planning pro- From (4), it can be seen that the robot size W and tracking
posed in this paper guarantees safety and collision avoid- error Ae act on Se , and the search region is divided by combining
ance even in the presence of trajectory tracking errors. the repulsive potential field in the next subsection to generate the
2) Reliability. In the presence of actuator faults, including final planning trajectory. Different weighting parameters in (4)
bias and loss-of-effectiveness faults, the autonomous ve- can regulate the role of different factors for Se . A larger tracking
hicle is still able to track reference values. The system is error margin Ae or robot size W leads to a reduction of the safe
stable and can maintain safety and avoid collisions even region. Conversely, it leads to an increase in the safe region.
in the event of actuator faults. This matches the actual situation, and we will further analyze it
3) Satisfy transient and steady-state performance constraints. in Section V.
Users can design both transient and steady-state perfor-
mance constraints [37] to meet practical requirements C. Repulsive Potential Field Generation
while ensuring safety and reliability. To ensure collision-
free operation, it is necessary to restrict trajectory tracking Using the constructed motion restraint size Se , the repulsive
errors within a predefined performance function. potential field is created to obtain the feasible region. The
repulsive potential field value Ure at Xc (x, y) is given as

III. MOTION PLANNING ηre − are × ηre × ρ2 , ρ ≤ ρ0
Ure (Xc ) = (5)
The motion planning algorithm presented in this paper takes 0, ρ > ρ0
into account the collision risks due to inaccurate tracking of the
planned trajectory, ensuring safety. This differs from traditional where ηre > 0 denotes the repulsive coefficient,
planning algorithms, which assume that the planned trajectory Xobst(x0 , y0 ) represents the nearest obstacle, and
can be tracked accurately. ρ = (x − xo )2 + (y − yo )2 is the shortest distance
from Xc (x, y) to obstacles. ρ0 > Se > 0 represents the
A. Grid Map Pre-Processing maximum repulsive range, while 0 < are < ρ12 is the decay
0
The sensor data is firstly pre-processed to generate a grid coefficient. The safety threshold Ure (Se ) can be determined by
map, as plotted in Fig. 2. The construction of the grid map [38] Ure (Se ) = ηre − are × ηre × Se2 . If Ure (Xc ) < Ure (Se ), the
can be facilitated by utilizing the given square grid width position Xc (x, y) is considered safe; otherwise, it is considered
Cell. The obstacle area Aobst can be classified based on the risky. Fig. 3 illustrates how the repulsive potential field can be
locations and shapes of the obstacles. Fig. 2 demonstrates that generated using (5). The constructed motion restraint size Se
Authorized licensed use limited to: Harbin Institute of Technology. Downloaded on July 10,2024 at 11:30:58 UTC from IEEE Xplore. Restrictions apply.
PAN et al.: SAFE MOTION PLANNING AND RELIABLE CONTROL FRAMEWORK FOR AUTONOMOUS VEHICLES 4783

Fig. 3. Repulsive potential field.

Fig. 4. Global path waypoint set Ωf search process. The start point PS is
facilitates the partitioning of the search space into the obstacle marked with a green pentagram grid, the target point PT with a purple pentagon
area Aobst , safe area As , and risky area Ar . grid, and the obstacles with orange grids. Both green and yellow grids are initial
waypoints and belong to the point set Ω0 . The yellow and red grids represent
feature points and corner points, respectively.
D. Waypoint Set Filtering
Waypoint set filtering and trajectory optimization are per-
formed within the safety region As determined by the con-
structed motion restraint size Se and repulsive potential field
Ure (Xc ). Waypoint set filtering aims to reduce the path length,
number of waypoints, and turns, thereby facilitating subsequent
trajectory smoothing. As illustrated in Fig. 5, the A∗ planning
algorithm is utilized to generate the initial waypoint set, denoted
as Ω0 . A heuristic distance is introduced into the search process
as follows:
f (Xc ) = g(Xc ) + h(Xc ) (6)
where Xc = (x, y) indicates the current position, f (Xc ) denotes
the search priority. Xc_f ather denotes the father point which is Fig. 5. Implementation of A∗ algorithm.
the same as the parent node in [16]. We can obtain
g(Xc ) = g(Xc_f ather ) + dis(g(Xc ), g(Xc_f ather )) (7)
h(Xc ) = |x − xtarget | + |y − ytarget | (8) set Ω0 {PS , A, B, C, D, E, F, G, H, I, J, K, L, M, PT } filter-
ing process is described as follows:
where dis(g(Xc ), g(Xc_f ather )) is the Euclidean distance, 1) Selecting Feature Points (SFP): The initial point set Ω0
Xtarget (xtarget , ytarget ) denotes the target point. The one-step contains numerous points that move in the same direction,
search process and the overall search process are illustrated in providing limited trajectory information. Thus, the key feature
Fig. 5(a) and (b), respectively. The search principle is identical points, specifically the waypoints where the direction of the path
to [15], [16]. By using the classical A∗ algorithm, Ω0 is obtained. −−→
changes, are extracted. As shown in Fig. 4(a), vector BC has
Then, Ω0 is refined by selecting feature points, transforming −−→
corner points, and removing redundant points to obtain the final a different direction than vector CD, indicating that point C
−−→ −−→
waypoint set Ωf . For clarity, the principle is illustrated with a is a feature point. Since vector CD and DE are in the same
simple scene search in Fig. 4. As depicted in Fig. 4(a), the A∗ direction, point D is not considered a feature point. The waypoint
algorithm is used to search the initial waypoint set Ω0 , which set Ω0 is simplified to Ω1 {PS , A, B, C, H, J, K, L, M, PT },
includes the yellow and green grids. The substantial number of resulting in a reduction of the number of waypoints by one-third
waypoints generated by the A∗ algorithm can pose challenges while preserving the feature points that contain critical path
for trajectory fitting and lead to poor tracking behavior. There- information.
fore, it is crucial to meticulously optimize and filter the initial 2) Transforming Corner Points (TCP): During the trajectory
point set Ω0 to guarantee that the resulting trajectory is both smoothing process, it is evident that corner points situated in
secure and well-suited for autonomous vehicles. The initial point close proximity to obstacles are prone to collision, such as
Authorized licensed use limited to: Harbin Institute of Technology. Downloaded on July 10,2024 at 11:30:58 UTC from IEEE Xplore. Restrictions apply.
4784 IEEE TRANSACTIONS ON INTELLIGENT VEHICLES, VOL. 9, NO. 4, APRIL 2024

waypoints K and L in Fig. 4(a). As depicted in Fig. 4(b), the 

corner points are transformed into convex points, with corner ¨ (τ̈ ) · δr
σ xr (τ ) = ẍr (τ ) = ∇
¨ (12)
points B and C being converted into convex point N , and points σ yr (τ ) = ÿr (τ ) = ∇(τ̈ ) · μr
K and L being replaced by point O. First, find the feature point
pairs like B, C among the feature points, namely, points B and The trajectory from the starting point PS to the target point
C are diagonally adjacent and the other two points sandwiched PT comprises k sub-trajectories. The i-th, i = 1, . . . , k sub-
in the middle of B, C one is collision-free and the other is the trajectory can be written as
obstacle. Satisfying the above conditions, we find a generation 
xir (τ ) = ∇(τ ) · δir , τi−1 ≤ τ < τi
condition for corner points, and we mark the corresponding
yir (τ ) = ∇(τ ) · μir ,τi−1 ≤ τ < τi
collision-free point as a corner point. Fig. 4(b) demonstrates
that the proposed transformation technique effectively avoids where τi−1 and τi denote the start and end moments of the i-th
collisions, thereby facilitating the generation of safer smooth sub-trajectory, respectively. τ0 refers to the initial moment, while
trajectories. In addition, the waypoint set Ω1 undergoes a trans- τk denotes its final moment. T = τk − τ0 is the total duration.
formation to yield set Ω2 {PS , A, N, H, J, O, M, PT }, resulting The coefficient matrices Θr and Λr , which describe the ref-
in a further reduction in the number of waypoints. erence trajectory, can be defined as
3) Removing Redundant Points (RRP): A∗ algorithm
T T T T
searches only in the eight surrounding directions, leading to the Θr = [δ1r , δ2r , . . . , δkr ] , Λr = [μT1r , μT2r , . . . , μTkr ]T (13)
generation of partially redundant waypoints. As illustrated in
Fig. 4(c), according to waypoint set Ω2 , the trajectory must The trajectory smoothing problem can be reformulated as a
sequentially traverse PS , A, N , H, J, O, M , and PT . It is search for Θr and Λr such that
−−→ r The trajectory is safe and collision-free;
evident that the vector N J does not intersect with obstacles. r The trajectory exhibit continuity and possess second-order
The intermediate point H is redundant for trajectory generation.
−−→ differentiability between two adjacent sub-trajectories;
By judging vector N O is collision-free, point J is marked as a r The trajectory intersects with the waypoints in the set Ωf ;
redundant point. In a similar vein, the waypoints in point set Ω2 r To enhance comfort, the second-order derivative of the
are filtered to remove redundancies, ultimately yielding the final trajectory is minimized.
point set, Ωf . For clarity, the optimization problem is presented for illustra-
The final planned trajectory can be fitted using the point set tive purposes in the x-direction, and likewise in the y-direction.
Ωf , as demonstrated in Fig. 4(d). Compared to the traditional A∗ The trajectory optimization problem can be converted into a
algorithm, the use of filtered waypoints leads to a reduction in the quadratic programming problem with equation constraints as
number of turns and promotes effective trajectory smoothing.
min f (Θr ) = min (ẍ(τ ))2 , s.t.A(4k+2)×6 k Θr = ξ (14)
E. Safe Trajectory Smoothing where the constraint determined by A(4k+2)×6 k and ξ is dis-
After obtaining the final point set Ωf as described above, a cussed below. By substituting (12), we can get
smooth reference trajectory is generated using trajectory opti-
k
mization in this section. Moreover, the reference trajectory is (ẍ(τ ))2 dτ = min (ẍ(τ ))2 dτ
τk τt
min (ẍ(τ ))2 = min τ0 τi−1
ensured to be safe and collision-free. i=1
1) Optimization-Based Polynomial Planning Approach: An k
optimization-based polynomial approach is employed to achieve = min Θir Qi Θir = min Θr T QΘr
T
i=1
smooth trajectory generation. The planned reference trajectory (15)
can be represented as a quintic polynomial [36] with respect to where Q =diag([Q1 , Q2 , . . ., Qk ]) and
τ in the x and y directions. Namely, the reference trajectory can  
τi
be formulated as ¨T × ∇ ¨ (τ̈ ) dτ = 02×2 02×4
 Qi = ∇ (16)
(τ̈ ) 04×2 N 4×4
xr (τ ) = δ0 + δ1 τ + δ2 τ 2 + δ3 τ 3 + δ4 τ 4 + δ5 τ 5 τi−1
(9)
yr (τ ) = μ0 + μ1 τ + μ2 τ 2 + μ3 τ 3 + μ4 τ 4 + μ5 τ 5 The (λ + 1)-th row and ( + 1)-th column element in Qi are
λ(λ−1)·(−1) (λ+−3) (λ+−3)
which can be rewritten in matrix form as follows: equal to (λ−2)+(λ−2)+1 τi − τi−1 . Subsequently,
 the matrix N 4×4 can be obtained.
xr (τ ) = ∇(τ ) · δr
(10) The constraints on position, velocity, and acceleration at PS
yr (τ ) = ∇(τ ) · μr
and PT can be formulated as
where δr = [δ 0 , δ 1 , δ 2 , δ 3 , δ 4 , δ 5 ]T and μr = [μ0 , μ1 , μ2 ,μ3 , [∇(τ0 ) , 06(k−1) ]Θr = xS , [06(k−1) , ∇(τk ) ]Θr = xT (17)
μ4 , μ5 ]T are coefficient vectors, ∇(∗) := [1, ∗, ∗2 , ∗3 , ∗4 , ∗5 ]
with its derivatives ∇˙ (˙∗) := [0, 1, 2∗, 3∗2 , 4∗3 , 5∗4 ] and ∇ ¨ (¨∗) := [∇ ˙ (τ̇ ) ]Θr = vxT
˙ (τ̇ ) , 06(k−1) ]Θr = vxS , [06(k−1) , ∇ (18)
0 k
2 3
[0, 0, 2, 6∗, 12∗ , 20∗ ] is a simplified vector expression. This ¨ (τ̈ ) ]Θr = axT
¨ (τ̈ ) , 06(k−1) ]Θr = axS , [06(k−1) , ∇
paper considers a time-dependent reference trajectory, namely [∇ 0 k
(19)
τ = t. The corresponding velocity v and acceleration σ can be where xS , vxS , and axS represent the position, velocity, and
expressed as acceleration, respectively, of PS in the x direction, while xT ,
 vxT , and axT are the corresponding variables of PT . The user
vxr (τ ) = ẋr (τ ) = ∇ ˙ (τ̇ ) · δr
˙ (τ̇ ) · μr (11) can assign these parameters based on their specific requirements.
vyr (τ ) = ẏr (τ ) = ∇ The trajectory needs to pass through the i-th waypoint (xi , yi )

Authorized licensed use limited to: Harbin Institute of Technology. Downloaded on July 10,2024 at 11:30:58 UTC from IEEE Xplore. Restrictions apply.
PAN et al.: SAFE MOTION PLANNING AND RELIABLE CONTROL FRAMEWORK FOR AUTONOMOUS VEHICLES 4785

Fig. 6. Trajectory constraint.

in the set Ωf , namely,

[06(i−1) , ∇(τi ) , 06(k−i−1) ]Θr = xi (20)
To ensure continuity of the reference trajectory at the i-th way-
point in the set Ωf , we consider the following constraints
[06(i−1) , ∇(τi ) , −∇(τi ) , 06(k−i−1) ]Θr = 0 (21)
˙ (τ̇ ) , −∇
[06(i−1) , ∇ ˙ (τ̇ ) , 06(k−i−1) ]Θr = 0 (22)
i i

¨ (τ̈ ) , −∇
[06(i−1) , ∇ ¨ (τ̈ ) , 06(k−i−1) ]Θr = 0 (23)
i i

To clarify, the trajectory constraint considered and handled to

generate the final planned trajectory from the obtained way-
points set Ωf is illustrated in Fig. 6. In summary, we can obtain Fig. 7. Modification operation to ensure collision avoidance.
⎡ ⎤
∇(τ0 ) , 06(k−1)
⎢ ∇˙ (τ̇ ) , 06(k−1) ⎥
⎢ 0 ⎥
⎢ ¨
∇(τ̈0 ) , 06(k−1) ⎥ is complete. Point E can then be added between P 1 and P 2,
⎢ ⎥
⎢ .. ⎥ to regenerate the planned trajectory. If a collision persists, point
⎢ . ⎥
⎢ ⎥ F can be found in the opposite direction of point E such that
⎢ 0 , ∇ , 0 ⎥
⎢ 6(i−1) (τi ) 6(k−i−1) ⎥ F D = r, as plotted in Fig. 7(c). Continuing with the process,
⎢ .. ⎥
A=⎢ ⎢ . ⎥ evaluate whether there is an obstacle at point F . Using a compa-
⎥ rable procedure, modifications are executed for collision points
⎢ 06(k−1) , ∇(τk ) ⎥
⎢ ⎥ within the generated trajectory until collision-free is achieved,
⎢ 06(k−1) , ∇(τ̇k )˙ ⎥
⎢ ⎥ as demonstrated in Fig. 7(d).
⎢ ¨
06(k−1) , ∇(τ̈k ) ⎥
⎢ ⎥ Remark 1: The proposed trajectory planning method can
⎢06(i−1) , ∇(τi ) , −∇(τi ) , 06(k−i−1) ⎥
⎢ ⎥ minimize the accelerations of the planned trajectory in the x
⎣06(i−1) , ∇˙ (τ̇ ) , −∇ ˙ (τ̇ ) , 06(k−i−1) ⎦ and y directions, which contributes to enhancing longitudinal
i i
¨ (τ̈ ) , −∇
06(i−1) , ∇ ¨ (τ̈ ) , 06(k−i−1) comfort. Moreover, the trajectory is safe and collision-free by
i i
employing anti-collision modification in the event of actuator
ξ = [xS , vxS , axS , . . . , xi , . . . , xT , vxT , axT , . . . , 0]T faults and inaccurate tracking.
Then, the planned trajectory can be obtained by solving the
equation-constrained optimization problem (14). IV. ADAPTIVE RELIABLE CONTROLLER DESIGN
2) Anti-Collision Modification: The trajectory generated us-
ing the waypoint set Ωf still poses a risk of collision, as By analyzing the motion relationship shown in Fig. 1 and (1),
shown in Fig. 7(a). An anti-collision modification approach is we can derive that
devised, which is delineated in this section. The procedures are  
xe ye
graphically depicted in Fig. 7. Starting from PS , the planned d˙e = − cos (ϕe ) v + ẋr + ẏr (24)
trajectory undergoes a thorough collision scan to detect any de de
potential obstacles along its path. Upon collision, the trajectory ye xe
undergoes a modification process. In this context, the collision ϕ̇e = w + 2 (ẋr − v cos(ϕ)) − 2 (ẏr − v sin(ϕ)) (25)
de de
location is denoted by point C, as illustrated in Fig. 7(b). The
waypoints P 1 and P 2, within the set Ωf on both sides of C, To ensure that the system remains collision-free even under
are located using the location information. Connect P 1 and P 2 inaccurate tracking, it is necessary to satisfy de < Ae , which is
to obtain the line segment marked as L. Draw a perpendicular determined by the movement restraint size Se . Meanwhile, it is
line from point C to line L to find the perpendicular point refer essential to satisfy de (t) > κ, − π2 < ϕe (t) < π2 with κ ∈ R+
as D. Let the length of the perpendicular CD be equal to 4r. to ensure the correct error definition. Therefore, we introduce
Consider a point E that lies in the opposite direction of CD invertible transformation functions as follows
and is situated such that the length of DE is equal to 2r. It is
necessary to check whether there exists any obstacle at point E. υ 1 e(ε1 ζ+κ1 ) + ς 1 e−(ε1 ζ+κ1 )
If no collision occurs, it indicates that the modification process G(ζ) = (26)
e(ε1 ζ+κ1 ) + e−(ε1 ζ+κ1 )

Authorized licensed use limited to: Harbin Institute of Technology. Downloaded on July 10,2024 at 11:30:58 UTC from IEEE Xplore. Restrictions apply.
4786 IEEE TRANSACTIONS ON INTELLIGENT VEHICLES, VOL. 9, NO. 4, APRIL 2024

υ 2 e(ε2 ϑ+κ2 ) − ς 2 e−(ε2 ϑ+κ2 ) where ki ∈ R+ , i = 1, 2, 3, 4, and Υ1 = 1 ( xdee ẋr + ye

de ẏr ) −
H(ϑ) = (27)
e(ε2 ϑ+κ2 ) + e−(ε2 ϑ+κ2 ) 1 dψ
e ψ̇
, Υ2 = −2 dye2 cos(ϕ) + 2 xd2e sin(ϕ), Υ3 = 2 dye2 ẋr −
e e e

where κ1 = 12 ln υ1 −ςθ1 −θ , κ2 = 12 ln υς22 , ς1 , ς2 , υ1 , υ2 , ε1 , ε2

xe
2 d2 ẏr − 2 ϕψe ψ̇ , Γ1 = −1 cos(ϕe ).
e
Theorem 1: By utilizing the designed controller given in (31)-
and θ are positive real numbers subject to ς1 + θ < υ1 . (33) and the adaptive laws described in (34)–(37), we can ensure
Meanwhile, the following monotonic function is introduced the stability of the system even with actuator faults. And the
tracking errors satisfy
ψ(t) = (ψ 0 − ψ ∞ ) e−ιt + ψ ∞ (28)
ς 1 ψ(t) < de (t) < υ 1 ψ(t), −ς 2 ψ(t) < ϕe (t) < υ 2 ψ(t) (38)
where ι > 0 and ψ 0 > ψ ∞ > 0 can be selected by the user.
Using the invertible transformation functions given in (26) for any t > 0. By choosing parameters ς1 , ς2 , υ2 , ψ0 and ψ∞
and ( 27), we can express the transformed errors ζ and ϑ as subject to ς1 ≥ ψΦ∞ , ς2 ≤ 2ψπ∞ , υ2 ≤ 2ψ
π
, we can obtain
0

ζ = G −1 (ω 1 (t)) π π
de (t) > Φ, − < ϕe (t) < (39)
1 2 2
= (ln (ω 1 (t) − ς 1 ) + ln (υ 1 − ς 1 −θ)
2ε1 where Φ is a positive constant. Meanwhile, the controller pa-
− ln (θυ 1 − θω 1 (t))) (29) rameters are adjusted to satisfy υ 1 ψ 0 < Ae , ensuring that the
system remains collision-free even in the presence of actuator
ϑ = H−1 (ω 2 (t)) failures and tracking errors. In summary, the motion planning
1 and control framework guarantees the safety and reliability of
= (ln (ω 2 (t)υ 2 + ς 2 υ 2 ) − ln (ς 2 υ 2 − ω 2 (t)ς 2 )) (30) the system.
2ε2 Proof: The following Lyapunov candidate function V is
2 2 k1 2
where the superscript −1 denotes the inverse function, ω 1 (t) = chosen as V = ζ2 + ϑ2 + 2β β̃ + k22 β̃12b + 2β
1 1
k3 2
β̃ + k24 β̃22b ,
2 2
de (t) ϕe (t) where ˜∗ := ˆ∗ − ∗ denotes the estimation error of ∗.
ψ(t) , and ω 2 (t) = ψ(t) .
The derivative of V can be derived as follows
Correspondingly, the derivatives of ζ and ϑ are written as
˙ ˙
k1 β̃ 1 β̂1 ˙ k3 β̃ 2 β̂2 ˙
xe ye de ψ̇ V̇ = ζ ζ̇ + + k2 β̃ 1b β̂1b + ϑϑ̇ + + k4 β̃ 2b β̂2b
ζ̇ = 1 (− cos (ϕe ) v + ( ẋr + ẏr ) − ) β1 β2
de de ψ
  Substituting the designed controller (31)–(33) and the adaptive
ye xe ϕe ψ̇
ϑ̇ = 2 w+ 2 (ẋr − v cos(ϕ))− 2 (ẏr − v sin(ϕ))− laws (34)–(37), we can obtain
de de ψ
k 1 k σ1 2 k 2 k σ2 2
V̇ ≤ − m1 ζ 2 − β̃ − β̃1b
where 1 = 1 1
− 1
, 2 = 1 2β1 1 2
2ε1 ψ ω 1 (t)−ς 1 ω 1 (t)−υ 1 2ε2 ψ
k 3 k σ3 2 k 4 k σ4 2
1
− 1
. − m2 ϑ 2 − β̃ − β̃2b + Cσ
ω 2 (t)+ς 2 ω 2 (t)−υ 2 2β2 2 2
The following adaptive reliable controllers are constructed
≤ − mσ V + C σ (40)
v=β̂ 1 ν 1 ,  = β̂ 2 ν 2 (31) where mσ = min(2m1 ,2m2 , kσ1 , kσ2 , kσ3 , kσ4 ), Cσ =
k1 kσ1 kσ2 k2 2 kσ3 k3 kσ4 k4 2
1 2 β1 + 2 β 1b + 2 β 2 + 2 β 2b .
ν1= − β̂1b Γ1 + Υ1 + m1 ζ (32)
Γ1 Thus, the system is ultimately uniformly bounded [39], [40],
and ζ and ϑ are bounded and accordingly, it is easy to obtain
1
ν2= − Υ2 β̂1b + Υ2 ν 1 + β̂2b ε2 + Υ3 + m2 ϑ (33) that de (t) ∈ (ς 1 ψ(t), υ 1 ψ(t)) and ϕe (t) ∈ (−ς 2 ψ(t), υ 2 ψ(t)).
ε2 By choosing the parameters as in Theorem 1, we can obtain
de (t) > Φ and ϕe (t) ∈ (− π2 , π2 ). Since we take into account
where β1 := α11 , β2 := α12 , β1b := α1b , β2b := α2b , and ˆ◦ de- the tracking error margin Ae in the movement restraint size Se ,
l l
notes the estimation of ◦. The adaptive laws are given by the system can be made safe and collision-free by adjusting the
parameters so that υ 1 ψ 0 < Ae holds. The proof of Theorem 1
˙ 1 is complete.
β̂ 1 = − (ζΓ1 ν 1 + ϑΥ2 ν 1 ) − kσ1 β̂ 1 (34)
k1 Remark 2: The controller developed in this paper can enhance
1 the system reliability by addressing actuator faults, including
˙
β̂1b = (ζΓ1 + ϑΥ2 ) − kσ2 β̂ 1b (35) actuator loss-of-effectiveness faults and bias faults. By con-
k2 sidering the tracking error caused by inaccurate tracking in
˙ 1 the controller design, the dynamic response can be constrained
β̂ 2 = − (ϑ2 ν 2 ) − kσ3 β̂ 2 (36) to safe performance boundaries to ensure system safety. The
k3
designed control method can be easily applied to autonomous
˙ 1 surface vehicles, mobile robots, etc. To enhance the proposed
β̂2b = (ϑ2 ) − kσ4 β̂ 2b (37)
k4 method, future work focuses on improving the system safety in

Authorized licensed use limited to: Harbin Institute of Technology. Downloaded on July 10,2024 at 11:30:58 UTC from IEEE Xplore. Restrictions apply.
PAN et al.: SAFE MOTION PLANNING AND RELIABLE CONTROL FRAMEWORK FOR AUTONOMOUS VEHICLES 4787

Fig. 8. Region division corresponds to different Se .

TABLE I
MOTION PLANNING PARAMETER SETTINGS

Fig. 9. Waypoint set Ωf generation.

TABLE II
BENEFITS OF WAYPOINT SET FILTERING

dynamic scenarios and reducing the traveling delay and energy

consumption to further improve the system performance.
Remark 3: By employing the grid map pre-processing
method in Section III-A, the map is pre-processed first. The cor-
responding repulsive potential field is generated with designed
motion restraint size Se , and then the generated grid map is
divided into obstacle regions and safe collision-free regions. The
initial collision-free candidate point set is then generated using
the A∗ algorithm, and the final point set Ωf is obtained using the
waypoint set filtering method proposed in Section III-D. Using
the trajectory smoothing and anti-collision modification method
in Section III-E, the planned smooth and safe trajectory can be
obtained. The reliable controller designed in Section IV is used
to achieve accurate tracking while enhancing the reliability of
autonomous vehicles.

V. SIMULATION RESULTS
Fig. 10. Collision avoidance by the proposed anti-collision modification.
In this section, a complex indoor simulation scenario com-
prising multiple irregular obstacles is constructed to validate the
effectiveness of the designed planning and control method. It is
assumed that the map information is available through sensors. part of the original set of AStarPoints. By selecting feature
To achieve collision-free motion planning, the map informa- points as described above, the set Ω1 is generated, namely the
tion is first pre-processed using the proposed map grid method. FeaturePoints in Fig. 9. The points in set Ω1 are then judged
Combined with the repulsive potential field, the region division and transformed to produce the corner point set Ω2 , which are
with different Se is provided in Fig. 8. The Se in Fig. 8(a), (b), denoted by the CornerPoints in Fig. 9. The redundant points that
and (c) are chosen as Se = 1, Se = 3, and Se = 5, respectively. do not contain valuable path information are removed to obtain
It can be seen that a larger Se , caused by a larger tracking error the final set Ωf , namely WayPoints in Fig. 9.
margin or robot size W , results in a smaller safety zone, which is Meanwhile, quantitative results of Dijkstra, A∗ , A∗ 2SFP (A∗ +
consistent with the actual situation. Then, the waypoint point set SFP), A∗ 2TCP (A∗ + SFP + TCP), and A∗ 2RRP (A∗ + SFP +
Ωf can be generated using the presented waypoint set filtering TCP + RRP) are provided in Table II to evaluate the proposed
strategy. For clarity, the parameter settings of motion planning waypoint set filtering method. From Table II, by combining
are provided and summarized in Table I. with the designed waypoint set filtering strategy, the generated
Specifically, the A∗ algorithm is used to search the map planned path obtains a shorter path length and fewer nodes.
space to find a feasible global path from PS to the PT . The Moreover, the number of path turns is reduced, which contributes
points involved and searched are represented as AStarExten- to generating and smoothing the planned trajectory. Based on
sionPoints in Fig. 9, and the final generated path Ω0 is indicated the proposed safe trajectory smoothing method, the planned
as AStarPoints. Note that the FeaturePoints in Fig. 9 are also trajectory is obtained by fitting the waypoints in set Ωf .

Authorized licensed use limited to: Harbin Institute of Technology. Downloaded on July 10,2024 at 11:30:58 UTC from IEEE Xplore. Restrictions apply.
4788 IEEE TRANSACTIONS ON INTELLIGENT VEHICLES, VOL. 9, NO. 4, APRIL 2024

TABLE III
PLANNING ALGORITHMS EVALUATION

TABLE IV
COMPARISON OF TRAJECTORY PERFORMANCE

Fig. 11. Comparison with classical trajectory smoothing algorithms.

As shown in Fig. 10(a), collision phenomena occur when the

planned trajectory is fitted directly using the final point set Ωf .
By modifying waypoint points using the proposed anti-collision
modification, safe trajectory smoothing can enable collision- because it minimizes the first-order derivative. Nonetheless, its
free trajectory fitting in complex environments, as in Fig. 10(b). planned trajectory may be subject to large accelerations which
The safety can be effectively enhanced by the proposed safe reduce comfort.
trajectory planning method. Further, we evaluate quantitatively the planning performance
Comparative simulation with the existing classical method of the different algorithms, and the comparative results are given
is added to highlight the advantages of the presented method, in Tables III and IV. As provided in Table III, the polynomial-
as shown in Fig. 11. The classical Minimum Snap (MS) algo- like algorithms (CS, QS, 7S), BL algorithm, BS algorithm,
rithm, the Minimum Jerk (MJ) algorithm, and the Minimum and the MJ without anti-collision modification can not en-
Velocity (MV) algorithm can minimize the 4th-order derivative, sure safety and avoid collisions with obstacles. CCR denotes
3rd-order derivative, and 1st-order derivative of the planned the root mean square (RMS) value of the rate of change of
trajectory, respectively. Trajectory generation is also performed curvature. The planned trajectory generated by OBP is reduced
and provided in Fig. 11 using the Cubic Spline (CS) algorithm, in terms of path length and the rate of change of trajectory
the Quintic Spline (QS) algorithm, the 7th Spline (7S) algo- curvature compared with MS and MJ. More importantly, by in-
rithm, the Bezier (BL) algorithm, the B-spline (BS) algorithm, corporating the anti-collision treatment, the optimization-based
and the proposed Optimization Based Polynomials(OBP) with (MS, MV, OBP) algorithm can guarantee collision-free and safe.
anti-collision modification algorithm. And the WPS denote way- The performance of the planned trajectory is quantified and
points. compared in the x and y directions as provided in Table IV. i-th,
As depicted in Fig. 11, the collision occurred for the planned i = 1, 2, 3 means that the trajectory is i-order differentiable. And
trajectory fitted by using the polynomial-like algorithms (CS, any-th means that it is differentiable to any order. All values in
QS, 7S) based on the final point set Af, which means that Table IV are the RMS values of the corresponding variables.
the safety of the planned trajectory is difficult to guarantee for To improve comfort, we introduced the acceleration of planned
the complex scenario. MJ without anti-collision modification, trajectory as cost into the optimization in OBP. It can be seen
BL and BS algorithms are more focused on the smoothness that the RMS of σxr the OBP algorithm is the smallest compared
of the planned trajectory, making the trajectory appear fre- to MS and MV. Because the CS, QS, 7S, BL, and BS algorithms
quent collisions with obstacles and making it may be unsafe. focus more on smoothness, smaller acceleration can be achieved.
Optimization-based methods (MS, MV, OBP) with the proposed However, these algorithms can not guarantee the corresponding
anti-collision modification can generate safe trajectories even in trajectories are safe. In summary, the designed OBP algorithm
complex scenes. can improve the comfort of the trajectory, by reducing the
Note that the MS and MJ optimize the higher order derivative acceleration of the reference trajectory, and more importantly, it
of the planned trajectory than the OBP, with correspondingly can guarantee the safety of the reference trajectory.
increased constraints, making it easy to produce unreasonable Finally, the final planning trajectory with the upper and lower
trajectories in order to meet the stricter constraints. Trajectory boundaries generated by Se is shown in Fig. 12. It can be
spins of MS and MJ can be observed in the enlarged part, which observed that, by introducing the designed motion constraint
is not reasonable for autonomous vehicle due to its geometric size Se and repulsive potential field, the generated planned
properties. In addition, except for the OBP, the MV algorithm trajectory has a safety margin against inaccurate tracking, which
also avoid the problems of the MS and MJ algorithms above can guarantee the safety of the planned trajectory.

Authorized licensed use limited to: Harbin Institute of Technology. Downloaded on July 10,2024 at 11:30:58 UTC from IEEE Xplore. Restrictions apply.
PAN et al.: SAFE MOTION PLANNING AND RELIABLE CONTROL FRAMEWORK FOR AUTONOMOUS VEHICLES 4789

Fig. 12. Final trajectory with movement restraint size Se . RT denotes the
final reference trajectory. LSPB and USPB are the lower boundary and upper Fig. 13. Tracking performance with various control methods. LB and UB are
boundary dependent on Se . the trajectory lower boundary and upper boundary. RT represents the reference
trajectory.

To highlight and verify the tracking performance and

reliability of the designed fault-tolerant controller, various
control algorithms are implemented, including: 1) Pure tracking
method (PP). 2) Proportional integral derivative (PID) controller.
3) Model predictive control (MPC) algorithm. 4) Prescribed
performance-based control without fault tolerance (PPC-WFC).
5) Prescribed performance-based fault tolerant control (PPC-
FC) with ψ0 = 0.2, ψ∞ = 0.05, ι = 2, ς 1 = 0.1, υ1 = 1, θ =
0.4, ς2 = 5, υ2 = 5, ε1 = 0.005, ε2 = 0.005, k1 = 100, k2
= 100, k3 = 100, k4 = 100, m1 = 100, m2 = 0.1, kσ1 =
0.01, kσ2 = 0.01, kσ3 = 0.01, kσ4 = 0.01, α1l = 0.8, α1b =
0.1, α2l = 0.8, α2b = 0.1. The actuator faults are set to when
t > 25, uif = αil ui + αib ; otherwise uif = ui , i = 1, 2.
The corresponding tracking comparison results are given in
Fig. 13. Note that the reference trajectory cannot be tracked
completely accurately and tracking errors exist in practice.
Although the planned trajectory is collision-free with the obsta-
cle, there is still a risk of collision when the planned trajectory
is tracked inaccurately, such as PP in Fig. 13. Most methods
decouple motion planning from control and assume that the Fig. 14. Transient performance and prescribed performance constraints.
desired trajectory is accurately tracked to generate the planned
trajectory, which in practice can still lead to unsafe collisions.
This is the significance of this research.
To analyze the transient performance, the actual transient be guaranteed. The designed PPC-FC not only improves the
responses of controllers and the corresponding performance reliability of the system but ensures its safety by combining the
constraints are provided in Fig. 14. It is worth mentioning that the Se in the planning layer.
PP clearly does not satisfy the desired performance constraints. To further visualize the tracking performance, comparative
Meanwhile, PID, MPC, and PPC-WFC can achieve accurate results of the tracking performance in the x and y directions are
tracking by tuning the parameters, however, the transient perfor- presented in Fig. 15(a) and (b), respectively. It can be observed
mance still may exceed the error constraints. Especially, when that excellent trajectory tracking performance can be achieved
actuator faults occur, the negative effect of faults may bring the for all controllers when no actuator faults occur. However,
transient responses out of the error bound, such as MPC and when actuator faults occur, the actual trajectories of MPC and
PPC-WFC in Fig. 14. PPC-WFC gradually deviate from the reference trajectory. In
In this case, the transient tracking error is possible to exceed contrast, the fault-tolerant controller PPC-FC still maintains
Ae in the movement restraint size Se , which indicates a potential good tracking performance in case of failure, which significantly
collision risk. The safety of the corresponding system cannot enhances the reliability of the system.

Authorized licensed use limited to: Harbin Institute of Technology. Downloaded on July 10,2024 at 11:30:58 UTC from IEEE Xplore. Restrictions apply.
4790 IEEE TRANSACTIONS ON INTELLIGENT VEHICLES, VOL. 9, NO. 4, APRIL 2024

TABLE V
RUNNING TIME AND TRACKING ERRORS

Fig. 15. Tracking performance in the x and y directions. XRV and YRV denote
the reference values in the x and y directions, respectively. Fig. 17. Experimental setup.

VI. EXPERIMENTAL RESULTS

The autonomous vehicle hardware platform and the ex-
perimental site are shown in Fig. 17(a) and (b), respectively.
The proposed algorithm is implemented in Jetson nano 4 GB
with Ubuntu 18.04 and ROS Melodic system and deployed
on the micro-autonomous vehicle illustrated in Fig. 17(a). The
micro-vehicle platform [41] with the same kinematic proper-
ties as actual autonomous vehicles can be used to validate the
effectiveness and advantages of the designed motion planning
and control framework. By employing SLAMTEC RPLIDAR
A1 and IMU sensors, the map information can be obtained
using simultaneous localization and mapping (SLAM), and
real-time localization information of the autonomous vehicle
can be collected. Based on the acquired perception information,
the proposed motion planning algorithm can generate a safe
collision-free planned trajectory. The control inputs are calcu-
lated by using the developed control algorithm and sent via com-
munication to the lower computer STM32F103VET6 to enable
Fig. 16. Control inputs.
tracking of the planned reference trajectory. The sensors and
hardware configuration of the autonomous vehicle are provided
in Fig. 17(a). The experimental scenario and the corresponding
The corresponding control inputs are shown in Fig. 16. Note map information are shown in Fig. 17(b).
that the proposed prescribed performance fault-tolerant control Similar to the simulation scenario, PP, PID, PP-WFC, and
method can reduce the negative effects caused by faults through PP-FC controllers are implemented to track the planned
adaptive adjustment. PPC-WFC cannot compensate for actuator trajectory and analyze the system performance, where
failures, which impairs the system reliability and thus makes the the parameters are set to ψ0 = 1, ψ∞ = 0.1, ι = 0.1, ς1
overall performance degrade, as shown in Figs. 14 and 15, where = 0.001, υ1 = 0.1, θ = 0.08, ς2 = 1.1, υ2 = 1.1, ε1 = 0.01, ε2
the tracking performance deteriorates significantly. = 0.01, k1 = 100, k2 = 100, k3 = 100, k4 = 100, m1 = 0.01,
The RMS values of tracking errors are given in Table V. m2 = 0.01, kσ1 = 0.01, kσ2 = 0.01, kσ3 = 0.01, kσ4 = 0.01,
It can be seen that PPC-FC still achieves excellent tracking α1l = 0.9, α1b = 0.021, α2l = 0.9, α2b = 0.021.
performance in the presence of an actuator failure. Meanwhile, The actuator faults are set to when t > 2.5, uif = αil ui +
MPC needs to solve the optimization problem, which leads to αib ; otherwise uif = ui , i = 1, 2. The comparative results of the
larger computational requirements than PPC-FC. Overall, the trajectory tracking performance obtained by different controllers
designed controller improves reliability and achieves good track- are plotted in Fig. 18. It can be observed that the reliability
ing performance even in the event of actuator faults. Moreover, of PPC-WFC and PP decreases significantly as the occurrence
the computation requirements are greatly reduced because the of actuator faults (3), gradually exceeds the desired constraint
optimization problem is avoided. boundaries. Compared to PPC-WFC, PPC-FC can adaptively

Authorized licensed use limited to: Harbin Institute of Technology. Downloaded on July 10,2024 at 11:30:58 UTC from IEEE Xplore. Restrictions apply.
PAN et al.: SAFE MOTION PLANNING AND RELIABLE CONTROL FRAMEWORK FOR AUTONOMOUS VEHICLES 4791

Fig. 18. Tracking performance comparative results. LB and UB are the trajec-
tory lower boundary and upper boundary. RT represents the reference trajectory. Fig. 20. Tracking performance in the x and y directions. XRV and YRV denote
the reference values in the x and y directions, respectively.

Fig. 19. Transient responses and performance constraints.

Fig. 21. Control inputs.

address actuator faults which significantly improves the relia- TABLE VI

bility and tracking performance. EXPERIMENTAL RUNNING TIME AND TRACKING ERRORS
The transient responses of de , ϕe , and the prescribed per-
formance constraints are illustrated in Fig. 19. PP, PPC-WFC,
and PID without actuator faults can obtain satisfactory tracking
performance by tuning controller parameters. However, after
actuator faults occur, the system gradually deviates from the
desired trajectory and the tracking performance and reliability
of the system degrades significantly. It is clear that the PPC-FC
designed in this paper can ensure the safety and reliability of the is clear that the PPC-FC considering actuator fault-tolerance can
autonomous vehicle under inaccurate tracking, and in practical significantly improve the reliability and tracking performance of
applications, users can select specific performance parameters the system.
to achieve the desired planning and control performance. The corresponding control inputs and quantitative results are
Meanwhile, to further analyze the tracking performance, com- provided in Fig. 21 and Table VI. By using the control inputs as in
parative results in the x and y directions are shown in Fig. 20(a) Fig. 21, it can be observed from Fig. 18 that the tracking perfor-
and (b), respectively. After actuator failures occur, the trajectory mance of the controller is significantly improved with adaptive
tracking capability of the PPC-WFC decreases considerably. It regulation. Compared with PID, PP, and PPC-WFC, it can be
Authorized licensed use limited to: Harbin Institute of Technology. Downloaded on July 10,2024 at 11:30:58 UTC from IEEE Xplore. Restrictions apply.
4792 IEEE TRANSACTIONS ON INTELLIGENT VEHICLES, VOL. 9, NO. 4, APRIL 2024

seen that the transient responses are within the performance [10] F.-Y. Wang, “Parallel control and management for intelligent transporta-
constraints, which is theoretically guaranteed and verified by tion systems: Concepts, architectures, and applications,” IEEE Trans.
the experimental results in Fig. 19. This means that not only is Intell. Transp. Syst., vol. 11, no. 3, pp. 630–638, Sep. 2010.
[11] L. Chen et al., “Milestones in autonomous driving and intelligent vehicles:
the tracking performance and reliability improved as in Table VI, Survey of surveys,” IEEE Trans. Intell. Veh., vol. 8, no. 2, pp. 1046–1056,
but the safety of the system is ensured combined with motion Feb. 2023.
restraint size Se in the planning level. [12] B. Li et al., “From formula one to autonomous one: History, achieve-
ments, and future perspectives,” IEEE Trans. Intell. Veh., vol. 8, no. 5,
pp. 3217–3223, May 2023.
[13] S. Teng, L. Chen, Y. Ai, Y. Zhou, Z. Xuanyuan, and X. Hu, “Hierarchical
VII. CONCLUSION interpretable imitation learning for end-to-end autonomous driving,” IEEE
Trans. Intell. Veh., vol. 8, no. 1, pp. 673–683, Jan. 2023.
This paper presents a safe and reliable motion planning and [14] A. J. Rimmer and D. Cebon, “Planning collision-free trajectories for
control framework that can ensure the safety and reliability reversing multiply-articulated vehicles,” IEEE Trans. Intell. Transp. Syst.,
of autonomous vehicles in the presence of inaccurate tracking vol. 17, no. 7, pp. 1998–2007, Jul. 2016.
and actuator faults by coordinating the planning and control [15] P. E. Hart, N. J. Nilsson, and B. Raphael, “A formal basis for the heuristic
layers. The motion restraint size is introduced into the planning determination of minimum cost paths,” IEEE Trans. Syst. Sci. Cybern.,
vol. 4, no. 2, pp. 100–107, Jul. 1968.
layer to leave a safe margin for inaccurate tracking, and the [16] H. Pan, X. Chang, and W. Sun, “Multitask knowledge distillation guides
potential field based on Se is constructed to divide safe regions. end-to-end lane detection,” IEEE Trans. Ind. Inform., vol. 19, no. 9,
The original point set generated using A∗ algorithm in the safe pp. 9703–9712, Sep. 2023.
region is refined and modified by the proposed waypoint set [17] S. M. Persson and I. Sharf, “Sampling-based A* algorithm for robot path-
planning,” Int. J. Robot. Res., vol. 33, no. 13, pp. 1683–1708, 2014.
filtering operation. Then safe planned trajectory is obtained by [18] T. Huang, H. Pan, W. Sun, and H. Gao, “Sine resistance network-based mo-
combining the optimization-based polynomial planning method tion planning approach for autonomous electric vehicles in dynamic envi-
and anti-collision modification. ronments,” IEEE Trans. Transport. Electrific., vol. 8, no. 2, pp. 2862–2873,
Comparative results with classical planning and control algo- Jun. 2022.
rithms show that the proposed planning algorithm can generate a [19] J. Faigl and P. Váňa, “Surveillance planning with bézier curves,” IEEE
Robot. Automat. Lett., vol. 3, no. 2, pp. 750–757, Apr. 2018.
smooth trajectory that satisfies the given constraints, including [20] C.-C. Tsai, H.-C. Huang, and C.-K. Chan, “Parallel elite genetic algo-
position, velocity, and acceleration constraints at PS and PT . rithm and its application to global path planning for autonomous robot
Satisfactory tracking performance even under actuator faults can navigation,” IEEE Trans. Ind. Electron., vol. 58, no. 10, pp. 4813–4821,
be achieved by employing the proposed adaptive fault-tolerant Oct. 2011.
[21] A. M. Lekkas and T. I. Fossen, “Integral LOS path following for curved
controller. In future work, we focus on further improving the paths based on a monotone cubic hermite spline parametrization,” IEEE
proposed method to ensure the safety of autonomous vehicles in Trans. Control Syst. Technol., vol. 22, no. 6, pp. 2287–2301, Nov. 2014.
dynamic scenarios [18]. Meanwhile, reducing travel delay and [22] J. Wittmann and D. J. Rixen, “Time-optimization of trajectories using
energy consumption by using advanced techniques [42], such zero-clamped cubic splines and their analytical gradients,” IEEE Robot.
as reinforcement learning-based multi-objective optimization, Automat. Lett., vol. 7, no. 2, pp. 4528–4534, Apr. 2022.
[23] D. Mellinger and V. Kumar, “Minimum snap trajectory generation and
to further improve energy efficiency while ensuring safety is an control for quadrotors,” in Proc. IEEE Int. Conf. Robot. Automat., 2011,
interesting topic. pp. 2520–2525.
[24] A. Piazzi and A. Visioli, “Global minimum-jerk trajectory planning
of robot manipulators,” IEEE Trans. Ind. Electron., vol. 47, no. 1,
REFERENCES pp. 140–149, Feb. 2000.
[25] A. D. Ames, X. Xu, J. W. Grizzle, and P. Tabuada, “Control barrier function
[1] S. Park and S. Hashimoto, “Autonomous mobile robot navigation using based quadratic programs for safety critical systems,” IEEE Trans. Autom.
passive RFID in indoor environment,” IEEE Trans. Ind. Electron., vol. 56, Control, vol. 62, no. 8, pp. 3861–3876, Aug. 2017.
no. 7, pp. 2366–2373, Jul. 2009. [26] A. D. Ames, S. Coogan, M. Egerstedt, G. Notomista, K. Sreenath, and P.
[2] D. Dolgov, S. Thrun, M. Montemerlo, and J. Diebel, “Path planning for Tabuada, “Control barrier functions: Theory and applications,” in Proc.
autonomous vehicles in unknown semi-structured environments,” Int. J. IEEE 18th Eur. Control Conf., 2019, pp. 3420–3431.
Robot. Res., vol. 29, no. 5, pp. 485–501, 2010. [27] H. Pan, D. Zhang, W. Sun, and X. Yu, “Event-triggered adaptive asymptotic
[3] D. Cao et al., “Future directions of intelligent vehicles: Potentials, possi- tracking control of uncertain MIMO nonlinear systems with actuator
bilities, and perspectives,” IEEE Trans. Intell. Veh., vol. 7, no. 1, pp. 7–10, faults,” IEEE Trans. Cybern., vol. 52, no. 9, pp. 8655–8667, Sep. 2022.
Mar. 2022. [28] S. L. Herbert, M. Chen, S. Han, S. Bansal, J. F. Fisac, and C. J. Tom-
[4] D. K. M. Kufoalor, T. A. Johansen, E. F. Brekke, A. Hepsø, and K. lin, “FaSTrack: A modular framework for fast and guaranteed safe mo-
Trnka, “Autonomous maritime collision avoidance: Field verification of tion planning,” in Proc. IEEE 56th Annu. Conf. Decis. Control, 2017,
autonomous surface vehicle behavior in challenging scenarios,” J. Field pp. 1517–1522.
Robot., vol. 37, no. 3, pp. 387–403, 2020. [29] M. Chen, S. Herbert, and C. J. Tomlin, “Exact and efficient Hamilton-
[5] F.-Y. Wang et al., “Verification and validation of intelligent vehicles: Jacobi guaranteed safety analysis via system decomposition,” in Proc.
Objectives and efforts from China,” IEEE Trans. Intell. Veh., vol. 7, no. 2, IEEE Int. Conf. Robot. Automat., 2017, pp. 87–92.
pp. 164–169, Jun. 2022. [30] G. E. Fainekos, A. Girard, H. Kress-Gazit, and G. J. Pappas, “Temporal
[6] F.-Y. Wang, N.-N. Zheng, D. Cao, C. M. Martinez, L. Li, and T. Liu, logic motion planning for dynamic robots,” Automatica, vol. 45, no. 2,
“Parallel driving in CPSS: A unified approach for transport automation pp. 343–352, 2009.
and vehicle intelligence,” IEEE/CAA J. Automatica Sinica, vol. 4, no. 4, [31] H. Kress-Gazit, G. E. Fainekos, and G. J. Pappas, “Temporal-logic-based
pp. 577–587, Oct. 2017. reactive mission and motion planning,” IEEE Trans. Robot., vol. 25, no. 6,
[7] E. A. Sisbot, L. F. Marin-Urias, R. Alami, and T. Simeon, “A human pp. 1370–1381, Dec. 2009.
aware mobile robot motion planner,” IEEE Trans. Robot., vol. 23, no. 5, [32] W. Chung et al., “Safe navigation of a mobile robot considering visibility of
pp. 874–883, Oct. 2007. environment,” IEEE Trans. Ind. Electron., vol. 56, no. 10, pp. 3941–3950,
[8] T. Huang, J. Wang, and H. Pan, “Adaptive bioinspired preview suspension Oct. 2009.
control with constrained velocity planning for autonomous vehicles,” IEEE [33] C. J. Ostafew, A. P. Schoellig, and T. D. Barfoot, “Robust constrained
Trans. Intell. Veh., vol. 8, no. 7, pp. 3925–3935, Jul. 2023. learning-based NMPC enabling reliable mobile robot path tracking,” Int.
[9] H. Pan, Y. Hong, W. Sun, and Y. Jia, “Deep dual-resolution net- J. Robot. Res., vol. 35, no. 13, pp. 1547–1563, 2016.
works for real-time and accurate semantic segmentation of traffic [34] H. Pan, C. Zhang, and W. Sun, “Fault-tolerant multiplayer tracking control
scenes,” IEEE Trans. Intell. Transp. Syst., vol. 24, no. 3, pp. 3448–3460, for autonomous vehicle via model-free adaptive dynamic programming,”
Mar. 2023. IEEE Trans. Rel., vol. 72, no. 4, pp. 1395–1406, Dec. 2023.

Authorized licensed use limited to: Harbin Institute of Technology. Downloaded on July 10,2024 at 11:30:58 UTC from IEEE Xplore. Restrictions apply.
PAN et al.: SAFE MOTION PLANNING AND RELIABLE CONTROL FRAMEWORK FOR AUTONOMOUS VEHICLES 4793

[35] W. Wang, J. Huang, and C. Wen, “Prescribed performance bound- Jue Wang received the B.S. degree in automation and
based adaptive path-following control of uncertain nonholonomic mo- the M.S. degree in pattern recognition and intelligent
bile robots,” Int. J. Adaptive Control Signal Process., vol. 31, no. 5, systems from Huaqiao University, Xiamen, China, in
pp. 805–822, 2017. 2016 and 2019, respectively, and the Ph.D. degree
[36] S. Teng et al., “Motion planning for autonomous driving: The state of from the Harbin Institute of Technology, Harbin,
the art and future perspectives,” IEEE Trans. Intell. Veh., vol. 8, no. 6, China, in 2023. She is currently a Postdoctoral Fel-
pp. 3692–3711, Jun. 2023. low with the Ningbo Institute of Intelligent Equip-
[37] C. P. Bechlioulis and G. A. Rovithakis, “Robust adaptive control of feed- ment Technology Company Ltd., Ningbo, China, and
back linearizable MIMO nonlinear systems with prescribed performance,” the University of Science and Technology of China,
IEEE Trans. Autom. Control, vol. 53, no. 9, pp. 2090–2099, Oct. 2008. Hefei, China. Her research interests include adap-
[38] X. Tao, N. Lang, H. Li, and D. Xu, “Path planning in uncertain environment tive control, fault-tolerant control, mechatronics, and
with moving obstacles using warm start cross entropy,” IEEE/ASME Trans. intelligent vehicles.
Mechatron., vol. 27, no. 2, pp. 800–810, Apr. 2022.
[39] N. Wang, S. Lv, M. J. Er, and W.-H. Chen, “Fast and accurate trajectory
tracking control of an autonomous surface vehicle with unmodeled dynam-
ics and disturbances,” IEEE Trans. Intell. Veh., vol. 1, no. 3, pp. 230–243,
Sep. 2016.
[40] P. Shi, X. Yu, X. Yang, J. J. Rodrguez-Andina, W. Sun, and H. Gao,
“Composite adaptive synchronous control of dual-drive gantry stage with
load movement,” IEEE Open J. Ind. Electron. Soc., vol. 4, pp. 63–74, 2023. Tenglong Huang (Graduate Student Member, IEEE)
[41] J. Hu, Y. Zhang, and S. Rakheja, “Adaptive lane change trajectory planning received the B.E. degree in automation from the
scheme for autonomous vehicles under various road frictions and vehicle Henan University of Technology, Zhengzhou, China,
speeds,” IEEE Trans. Intell. Veh., vol. 8, no. 2, pp. 1252–1265, Feb. 2023. in 2019. He is currently working toward the Ph.D.
[42] S. Lim, H. Balakrishnan, D. Gifford, S. Madden, and D. Rus, “Stochastic degree with the Research Institute of Intelligent Con-
motion planning and applications to traffic,” Int. J. Robot. Res., vol. 30, trol and Systems, Harbin Institute of Technology,
no. 6, pp. 699–712, 2011. Harbin, China. His research interests include motion
planning, vehicle dynamics control, adaptive control,
fault-tolerant control, and intelligent vehicles.
Huihui Pan (Senior Member, IEEE) received the
Ph.D. degree in control science and engineering from
the Harbin Institute of Technology, Harbin, China, in
2017, and the Ph.D. degree in mechanical engineering
from the Hong Kong Polytechnic University, Hong
Kong, in 2018. He is currently a Professor with the
Research Institute of Intelligent Control and Systems, Weichao Sun (Senior Member, IEEE) received the
Harbin Institute of Technology. His research interests Ph.D. degree in control science and engineering from
include nonlinear control, vehicle dynamic control, the Harbin Institute of Technology, Harbin, China, in
and intelligent vehicles. He is an Associate Editor for 2013. He is currently a Professor with the Research
IEEE TRANSACTIONS ON INTELLIGENT VEHICLES, Institute of Intelligent Control Systems, Harbin In-
IEEE TRANSACTIONS ON SYSTEMS, MAN, AND CYBERNETICS: SYSTEMS, and stitute of Technology. His research interests include
Mechatronics. adaptive robust control, mechatronics, robotics, and
autonomous vehicles. He is an Associate Editor
for IEEE/ASME TRANSACTIONS ON MECHATRON-
Mao Luo received the B.S. degree in automation and ICS, IEEE TRANSACTIONS ON SYSTEMS, MAN, AND
the M.S. degree in control science and engineering CYBERNETICS: SYSTEMS, and Journal of Dynamic
from the Harbin Institute of Technology, Harbin, Systems, Measurement and Control.
China, in 2021 and 2023, respectively. His research
interests include motion planning and intelligent ve-
hicles.

Authorized licensed use limited to: Harbin Institute of Technology. Downloaded on July 10,2024 at 11:30:58 UTC from IEEE Xplore. Restrictions apply.