sensors

Article
Research on Autonomous Vehicle Path Planning Algorithm
Based on Improved RRT* Algorithm and Artificial Potential
Field Method
Xiang Li , Gang Li * and Zijian Bian

School of Automobile and Traffic Engineering, Liaoning University of Technology, Jinzhou 121001, China;
[email protected] (X.L.); [email protected] (Z.B.)
* Correspondence: [email protected]

Abstract: For the RRT* algorithm, there are problems such as greater randomness, longer time con‑
sumption, more redundant nodes, and inability to perform local obstacle avoidance when encoun‑
tering unknown obstacles in the path planning process of autonomous vehicles. And the artificial
potential field method (APF) applied to autonomous vehicles is prone to problems such as local op‑
timality, unreachable targets, and inapplicability to global scenarios. A fusion algorithm combining
the improved RRT* algorithm and the improved artificial potential field method is proposed. First
of all, for the RRT* algorithm, the concept of the artificial potential field and probability sampling
optimization strategy are introduced, and the adaptive step size is designed according to the road
curvature. The path post‑processing of the planned global path is carried out to reduce the redundant
nodes of the generated path, enhance the purpose of sampling, solve the problem where oscillation
may occur when expanding near the target point, reduce the randomness of RRT* node sampling,
and improve the efficiency of path generation. Secondly, for the artificial potential field method, by
designing obstacle avoidance constraints, adding a road boundary repulsion potential field, and op‑
timizing the repulsion function and safety ellipse, the problem of unreachable targets can be solved,
unnecessary steering in the path can be reduced, and the safety of the planned path can be improved.
In the face of U‑shaped obstacles, virtual gravity points are generated to solve the local minimum
problem and improve the passing performance of the obstacles. Finally, the fusion algorithm, which
Citation: Li, X.; Li, G.; Bian, Z.
combines the improved RRT* algorithm and the improved artificial potential field method, is de‑
Research on Autonomous Vehicle
signed. The former first plans the global path, extracts the path node as the temporary target point
Path Planning Algorithm Based on
of the latter, guides the vehicle to drive, and avoids local obstacles through the improved artificial po‑
Improved RRT* Algorithm and
Artificial Potential Field Method.
tential field method when encountered with unknown obstacles, and then smooths the path planned
Sensors 2024, 24, 3899. https:// by the fusion algorithm, making the path satisfy the vehicle kinematic constraints. The simulation
doi.org/10.3390/s24123899 results in the different road scenes show that the method proposed in this paper can quickly plan a
smooth path that is more stable, more accurate, and suitable for vehicle driving.
Academic Editors: Jun Chen and
Wen‑Chiao Lin
Keywords: autonomous vehicle; RRT* algorithm; path planning; fusion algorithm; curvature;
Received: 15 May 2024 artificial potential field method
Revised: 13 June 2024
Accepted: 13 June 2024
Published: 16 June 2024

1. Introduction
The casualties and economic losses caused by road traffic accidents are increasing
Copyright: © 2024 by the authors.
year by year. Every year, there are traffic accidents caused by improper driving behavior
Licensee MDPI, Basel, Switzerland. and other related reasons [1]. Driverless cars can well reduce road congestion and greatly
This article is an open access article reduce traffic accidents caused by human errors [2].
distributed under the terms and One of the crucial technologies for self‑driving vehicles is path planning [3]. It plans
conditions of the Creative Commons the safe driving path according to the map information, sensor data, and target location
Attribution (CC BY) license (https:// information and controls the steering and speed of the vehicle to ensure that the vehicle
creativecommons.org/licenses/by/ runs safely and efficiently to the destination [4]. The commonly used path‑planning algo‑
4.0/). rithms are the intelligent method, the random sampling method, the curve interpolation

Sensors 2024, 24, 3899. https://doi.org/10.3390/s24123899 https://www.mdpi.com/journal/sensors

Sensors 2024, 24, 3899 2 of 31

method, and so on. It can also be classified by global path planning and local path plan‑
ning. The global path planning algorithm has two methods: random sampling and graph‑
based search, such as the Dijkstra algorithm [5], the A* algorithm [6], PRM algorithm [7],
and the RRT* algorithm [8]. Among them, the RRT* algorithm finds extensive applica‑
tion in the realm of mobile robots and unmanned vehicles, and many researchers have
improved the RRT* algorithm. The RRT* algorithm proposed by Karaman et al. [9] intro‑
duces the functions of a reselection parent node and reconnection to optimize the search
results. However, its search efficiency is suboptimal. Cong et al. [10] proposed an RRT*
algorithm based on a hybrid sampling strategy, which effectively reduced the sampling
randomness of the algorithm. The HBAI‑RRT* algorithm proposed by Lin et al. [11] in
their study combines the greedy heuristic method with an adaptive adjustment strategy to
reduce the sampling area and improve search efficiency. Local path planning algorithms
more frequently employ the artificial potential field (APF) method, which can lead to local
minima and goal unreachability issues during the planning process [12]. After conducting
an in‑depth study, Li et al. [13] adopted the concept of the artificial potential field and im‑
proved strategies such as adding distance adjustment factors to address the limitations of
the traditional artificial potential field method. Xu [14] engineered a spring mechanism in
the repulsive velocity potential field to effectively eliminate the flutter phenomenon near
the obstacle. Zhai [15] and others added an adjustment factor and a judgement coefficient
to the potential field to optimize the potential field function and improve driving comfort
and the safety of the planned path. Many other researchers have proposed fusion algo‑
rithms combining the advantages of the two algorithms. Huang [16] introduced a fusion
algorithm that merges the RRT algorithm with the artificial potential field method. The
expansion of the RRT random tree incorporates probability values and a gravity compo‑
nent. With this method, simulation results demonstrate notable improvements in time
efficiency, path length, and iteration count. Zhang [17] introduced a novel path‑planning
algorithm that enhances obstacle avoidance speed and quality by combining the A* algo‑
rithm with the artificial potential field method. This fusion also effectively addresses the
inefficiencies of the A* algorithm when dealing with complex scenes. Wu [18] proposed a
path planning algorithm that integrates the artificial potential field method with the par‑
ticle swarm optimization algorithm. This approach allows for the real‑time generation of
obstacle avoidance paths and significantly improves the vehicle’s stability when avoiding
obstacles. Dasiah et al. [19] proposed an improved RRT* algorithm that realizes directional
fast search based on sampling angle constraints, which can effectively find better paths in
complex environments and significantly improve the convergence rate of the algorithm.
In terms of optimality and optimization in path planning problems, Nguyen et al. [20]
proposed a research method that integrates formation control and optimal control, taking
into account the kinematic and dynamic models of each vehicle. Experiments showed the
effectiveness of the control strategy. Shi et al. [21] studied the distributed time‑varying
output formation tracking problem of heterogeneous multi‑agent systems with different
dimensions and parameters. They designed an optimal tracking controller by adopting
model‑free reinforcement learning technology and designing the compensation input for
each follower. The simulation results show that the output formation tracking error even‑
tually approaches zero.
Although the above scholars have improved the path planning algorithm, there are
still problems that cannot deal with the unknown obstacles emerging in the environment
and cannot be applied to structured roads. Although some scholars consider real‑time ob‑
stacle avoidance, the vehicle collision range is not taken into account, and the final path
is not optimized. After consulting the literature, there is little research on the path plan‑
ning algorithm in the curved structured road scene, and there are some problems, such
as low search efficiency. Compared with the optimization of the path tracking control
in study [20] and study [21], this paper mainly studies the efficiency and quality of the
planned path of an unmanned vehicle, which provides a high‑quality path for tracking
control and improves the efficiency and stability of tracking. Therefore, this paper designs
Sensors 2024, 24, 3899 3 of 31

a method of vehicle driving guided by global path nodes and local planning to avoid ob‑
stacles. In order to solve the above path planning problems, this is necessary. The RRT*
global path planning algorithm with more sampling nodes is adopted to ensure that more
nodes play a guiding role, and the APF local path planning algorithm, which is better at
considering the collision range of obstacles and structured road constraints, is adopted to
solve the path planning problem of unknown obstacles on structured curved roads. This
paper proposes a fusion algorithm that combines the Improved RRT* algorithm and the
Improved Artificial Potential Field method. The objective is to tackle the path planning
challenge that arises when encountering a prominently unknown obstacle on a structured
road. By introducing the concepts of gravitational and repulsive fields into RRT* node
sampling, adding probabilistic sampling optimization, and enhancing the purposefulness
of node sampling, we improve the RRT* algorithm. Considering the curvature of the road
and designing the adaptive step size, which makes the sampling more efficient and time‑
consuming and solves the problem of possible oscillation when expanding near the target
point, subsequently, the nodes of the planned path undergo branching and constraints
to finalize the path post‑processing phase, enhancing the overall path quality. Enhance‑
ments to the artificial potential field method include the design of obstacles, optimization
of obstacle avoidance constraints, addition of a road boundary repulsive potential field
and optimization of the repulsive function, and the introduction of virtual gravitational
points. The goal unreachability problem is solved, and the local optimum is reduced to
improve the quality of locally planned paths. Finally, the fusion algorithm significantly
improves the performance of obstacle avoidance on structured roads. The planned roads
are smoothed to ensure that they are suitable for vehicles.

2. Traditional Algorithm
2.1. RRT* Algorithm
The RRT algorithm conducts random sampling in unoccupied space and continu‑
ously expands random tree branches until a branch contains or approaches the target
point, or until the algorithm reaches its iteration limit, at which point the planning pro‑
cess concludes [22].
From reference [9], it is known that the RRT* algorithm has the following theorems:

Theorem 1. (asymptotic optimality of the RRT* algorithm): when the number of iterations tends
to infinity, the path quality of the RRT* algorithm tends to be optimal; that is, the algorithm has
asymptotic optimality.

Theorem 2. (probabilistic completeness of the RRT* algorithm): if there is a feasible path from the
starting point to the end point, the RRT algorithm will almost certainly find a feasible path with an
infinite number of iterations.

In the RRT algorithm, the root node of the random tree is the starting point xstart , and
a random point xrand is generated in the blank area of the map. Then, the whole random
tree is searched, and xnearest is selected as the nearest node to xrand . Connect xrand and
xnearest , and expand a new node on the connected straight line at a set step size of xnew .
Next, we subject the new node to collision detection. If it does not collide with an obstacle,
we add it to the randomly expanded tree. If a collision occurs, then we delete the new
node. Continue the described steps until either the target node is appended to the random
tree node or the search concludes upon reaching the specified number of iterations. The
planned path is derived by retracing from the target point to the starting point. The random
sampling process is illustrated in Figure 1.
 Sensors2024,
Sensors 2024,Sensors
24,3899
24, x FOR PEER
2024, 24, xREVIEW
FOR PEER REVIEW 4 of 33 4 of
4 of 31

Figure1.
Figure 1. The
The node1.extension
node
Figure extension process.
The nodeprocess.
extension process.

The RRT*
The RRT* The
algorithm
algorithm enhances the
enhances
RRT* algorithm the RRT algorithm
RRT
enhances algorithm by incorporating
the RRT algorithm strate-
improved improved
by incorporating strate‑ strat
gies for parent
gies for parent node
node
gies for selection
selection
parent and
nodeand reconnecting
reconnecting
selection when sampling
when sampling
and reconnecting new
when new nodes
nodesnew
sampling [23], which
[23],nodes
which [23], whic
reduces the
reduces the generated
generated
reduces redundantredundant
redundant
the generated paths and
paths andpaths
makesand
makes themakes
the planned
planned paths
thepaths better.
better.
planned paths proce-The proc
Thebetter.
proce‑
durefor
dure forreselecting
reselecting parent
parent nodes
nodes is
is depicted
depicted in
in Figure
Figure 2.
2.
dure for reselecting parent nodes is depicted in Figure 2.

(a) (a) (b) (b)

Figure 2. The process
Figure of reselecting
2. The the parent node:
process of the
reselecting (a) the
the parent constrained
node: search range
(a) the constrained when reselect-
search
Figure 2. The process of reselecting parent node: (a) the constrained search range whenrange when resele
reselect‑
ing parent nodes;
ing (b)
parent the node
nodes; situation
(b) the nodeafter reselecting
situation after the parent
reselecting node.
the parent node.
ing parent nodes; (b) the node situation after reselecting the parent node.
With node xnew node as thex center of the circle, theform a constrained search range within
With nodeWith xnew as the center new as ofthethecenter
circle, of form acircle, form asearch
constrained constrained search
range within range with
the
the defined
defined radius, radius, as
as shown
the defined shown in
in Figure
radius, Figure
as shown2a. The2a. The neighboring
neighboring
in Figure 2a. Thenodes nodes in it are
in it are nodes
neighboring used as
used asincandidatescandi-
it are used as cand
dates
for thefor the
parent
datesparent
node ofnode
for the new
of
xparent
. x
When
node
new . When
solving
of x solving
for
. When for
taking taking
each
solving each
candidate
for candidate
taking as
eacha as
parent a parent
candidate node, as a pare
new
node,
the costthefrom
cost from
the the
startingstarting
point
node, the cost from the startingpoint
to x to isx
new point is
calculated
new calculated
and and
comparedcompared
with with
the the
cost
to xnew is calculated and compared with the cost cost
of of
the
original path;path;
the original the candidate nodenode with with
the lowest cost iscost
chosen as theas newtheparent node.
the original path; the candidate node lowest
the candidate the with the is chosen
lowest cost is chosen new asparent
the new pare
This
node. results in the least
Thisnode.
results in thecostly
least path
costlywithin
path the
within constrained
the radius size
constrained at this
radius size point
at in point
this time,
This results in the least costly path within the constrained radius size at this poi
and the candidate
in time, and node for nodethe path at this point in time isinselected in place of the original
in time, and the candidate node for the path at this point in time is selected inthe
the candidate for the path at this point time is selected in place of place of th
node [24].node
original The[24].
pathThecost withcost the candidate point as point
the parent node is shown isinshown
Table in1,
original nodepath[24]. The with path thecostcandidate
with the candidate as the parent
point node
as the parent node is shown
and
Tablethe1,parent
and thenode should
parent node beshould
updated to node 5,toas
be should
updated shown
node asinnode
5, to Figurein
shown 2b.
Table 1, and the parent node be updated 5, as Figure
shown 2b. in Figure 2b.
Table
Table1.1. Constrained
Constrained range
range path
pathcost.
cost. path cost.
Table 1. Constrained range
CandidatePoint
Candidate Point
Candidate Point Trails Trails Path
PathDistance
Distance
Path Distance
5 5 5 0-5-9
0‑5‑9 0-5-9 0-5-9
0‑5‑9==88 0-5-9 = 8
66 6 0-4-6-9
0‑4‑6‑9 0-4-6-9 0-4-6-9
0‑4‑6‑9==16
16
0-4-6-9 = 16
88 8 0-5-8-9
0‑5‑8‑9 0-5-8-9 0-5-8-9
0‑5‑8‑9==990-5-8-9 = 9

After completing the parent node

After completing reselection, a reconnection process is process
also required
After completing the parent thenodeparent node reselection,
reselection, a reconnection
a reconnection process is also is also require
required
to prune the random tree [25], thereby further reducing the path length. Within theWithin
same
to prune the random tree [25], thereby further reducing the path length. Within the same the sam
to prune the random tree [25], thereby further reducing the path length.
constraints, as shown in Figure 3a, find all the neighboring nodes of node x with xnew with x
constraints,constraints,
as shown in as Figure
shown 3a,in Figure
find all3a,thefind all the neighboring
neighboring nodesxof
nodes of node with xxnew
newnode
new
new n
asthe
as theparent
parent ofeach
of
as the each neighboring
neighboring
parent nodeand
node
of each neighboring andfind
find the
nodetheand cost
cost ofthe
of
find each
each path,
path,
cost i.e.,ififpath,
i.e.,
of each thecost
the cost isless
i.e., is
if lesscost is le
the
thanthe
than thecost
cost
thanofofthe
thepath
pathofofthe
cost theoriginal
original
path parent
parent
of the ofof
originalthethe neighboring
neighboring
parent node,
node, update
update
of the neighboring its its
node, parent
parent
update to its pare
node xnew x.to
to node The
new . The
node reconnection
reconnection
xnew . The process
process is
reconnection is
shown shown
in
process in
Figure
is Figure
3.
shown 3.
From
in From
Table
Figure Table
2,
3.theFrom 2, the
parent parent
Table node
2, the pare
node
of nodeof6node
should 6 should
be be
updated updated
to nodeto node
9, as 9,
shownas shown
in in
Figure Figure
node of node 6 should be updated to node 9, as shown in Figure 3b.3b. 3b.
Sensors 2024,Sensors 2024, 24, x FOR PEER REVIEW
24, 3899 5 of 31 5 of 3

(a) (b)
Figure 3. Reconnection
Figure 3. Reconnection process: (a)process: (a) the constrained
the constrained search
search range range for reconnection;
for reconnection; (b) the post-nod
(b) the post‑node
situation after reconnection.
situation after reconnection.

Table 2. Reconnection
Table 2. Reconnection process
process path cost. path cost.
Neighboring
Neighboring Node Node Original Consideration
Original Consideration New Pathway New Price
New Pathway New Price
4 10 0-5-9-4 0-5-9-4 = 12
4 10 0‑5‑9‑4 0‑5‑9‑4 = 12
6 10 + 5 = 15 0-5-9-6 0-5-9-6 = 9
6 10 + 5 = 15 0‑5‑9‑6 0‑5‑9‑6 = 9
8 8 3+5+1=6 3 + 5 + 1 = 6 0‑5‑9‑8 0-5-9-8 0-5-9-8
0‑5‑9‑8 = 11 = 11

Parent node reselection and reconnection strategies complement and interact wit
Parent node
each otherreselection and reconnection
to significantly improve thestrategies complement and interact with
path quality.
each other to significantly improve the path quality.
Although the parent node reselection and reconnection strategy in RRT* can improv
Although the parent
the path quality,node reselection
it increases the and
timereconnection
cost and stillstrategy in RRT*
has a slow can improve
convergence speed, whic
the path quality,
requires a large number of iterations to approach the better path. Whenwhich
it increases the time cost and still has a slow convergence speed, dealing with
requires a large number of iterations to approach the better path. When dealing with a
complex obstacle environment, especially in the case of a narrow or curved channel, ther
complex obstacle environment, especially in the case of a narrow or curved channel, there
are some problems, such as unstable path quality, low algorithm efficiency, even plannin
are some problems, such as unstable path quality, low algorithm efficiency, even planning
failure, redundant nodes, and an unsmooth path, and there is still a lack of targeted sam
failure, redundant nodes, and an unsmooth path, and there is still a lack of targeted sam‑
pling. Therefore, the improved strategy should be introduced into the RRT* algorithm
pling. Therefore, the improved strategy should be introduced into the RRT* algorithm
according to Lemma 1.
according to Lemma 1.
Lemma 1. (improving the efficiency of the RRT* algorithm): the improved RRT* algorithm reduce
Lemma 1. (improving the efficiency of the RRT* algorithm): the improved RRT* algorithm reduces
the number of invalid sampling points by introducing heuristic information or an optimizatio
the number of invalid sampling points by introducing heuristic information or an optimization
strategy, thus improving the efficiency of the algorithm.
strategy, thus improving the efficiency of the algorithm.
2.2. Traditional Artificial Potential Field Method
2.2. Traditional The
Artificial Potential
artificial Fieldfield
potential Methodmethod views the vehicle motion space as a virtual forc
field space, with the target point
The artificial potential field method views on thethevehicle
vehicleproducing
motion space gravitational
as a virtual forceforceand the ob
field space, with the target point on the vehicle producing gravitational force and the ob‑ of thes
stacle producing repulsive force. Therefore, the vehicle moves under the effect
two forces.
stacle producing repulsive force. Therefore, the vehicle moves under the effect of these
two forces. Within the virtual realm of a force field, the attraction of a target point to a vehicle i
Withindirectly proportional
the virtual realm oftoathe distance
force field, between
the attractionthem.ofThe equation
a target fortothe
point gravitational po
a vehicle
is directlytential field function
proportional is provided
to the distance below.them. The equation for the gravitational
between
potential field function is provided below.
1
U att (d ) = katt d 2 ( p, pg ) (1
1 2 2
Uatt (d) = k att d ( p, p g ) (1)
where katt is the gravitational potential 2 field gain coefficient, p is the current locatio
where k attof vehicle, p g potential
is the gravitational denotes the field gain coefficient,
position of the target p is the current
point, d ( p , p g )of is
and location thea vector o
vehicle, p gmagnitude
denotes the position of the target point, and d ( p, p g ) is a vector
representing the distance between the vehicle and the target point in the d of magnitude
representing the distance
rection between
of the vehicle the vehicle
pointing to theand
targetthepoint.
targetThe point in the direction
gravitational forceoffunction
the is ob
vehicle pointing to the target point. The gravitational force
tained from the above equation, as shown in the following equation. function is obtained from the
above equation, as shown in the following equation.
Fatt ( d ) = k att d ( p , p g ) (2
Fatt (d) = k att d( p, p g ) (2)
Sensors 2024, 24, 3899 6 of 31

When the vehicle is out of its repulsive potential field’s range, the obstacle exerts no
repulsive force. Only within the range of the repulsive potential field will the obstacle’s
repulsive force affect the vehicle; the greater the distance between the two, the smaller the
vehicle’s exposure to the repulsive potential energy value, and vice versa. The following
equation illustrates the repulsive potential field function.
(
1 2
1 1
2 k rep ( d( p,p0 ) − 1
d0 ) d2 ( p,p0 ) , 0 ≤ d( p, p0 ) ≤ d0
Ureq (d) = (3)
0, d( p, p0 ) > d0

where krep is the repulsive potential field gain coefficient, p is the current position of the
vehicle, p g denotes the position of the target point, and d( p, p g ) is a vector of magnitude
representing the Euclidean distance between the vehicle and the obstacle in the direction
of the vehicle pointing towards the obstacle. The repulsive force function on the vehicle
can be calculated from the above equation as follows:
(
1
krep ( d( p,p − d10 ), 0 ≤ d( p, p0 ) ≤ d0
Freq (d) = 0) (4)
0, d( p, p0 ) > d0

In the process of driving, the vehicle will be affected by both the gravitational field
and the repulsive field. The combined potential field to which it is subjected is as follows:
n
U (d) = Uatt (d) + ∑i=0 Urep (d) (5)

where n is the number of obstacles that have a repulsive effect on the vehicle.
From the combined potential field, we can obtain the combined force applied to
the vehicle during its motion, and the combined force expression is given in the
following equation:
n
F (d) = Fatt (d) + ∑i=0 Frep (d) (6)
However, the traditional artificial potential field method is prone to local minimum
problems and unreachable targets. As a result, the unmanned car may fall into a “trap
area” (where the resultant force on the vehicle is zero). For example, in the face of U‑
shaped obstacles, it is easy to fall into the local minimum area, so that the target point
cannot be reached. It is also possible that the unmanned vehicle cannot reach the target
position because the strong repulsive force will produce concussive behavior, which will
eventually lead to the failure of path planning.

3. Improved RRT* Algorithm

3.1. Introducing the Concept of Artificial Potential Fields
3.1.1. Introducing the Gravitational Component
In the RRT* algorithm, the gravitation between the random node and the target point
is increased to guide the random tree to grow to the target point more rapidly and reduce
the randomness. The schematic diagram is shown in Figure 4. In order to change the
resultant force of the growth direction, the gravity function G (n) is added to each new
node n as follows:
F1 (n) = Rd(n) + G (n) (7)
where F1 (n) is the function of the new node, Rd(n) is the random function of new nodes,
and G (n) is the objective gravity function.
The gravity function G (n) is as follows:

x goal − xnear
G (n) = ρ × g × (8)
x goal − xnear
 x near .
Random function Rd ( n ) :
xgoal − xnear
G(n) = ρ × g × xgoal − xnear (8)
Rd (n) = ρ × x − x
Sensors 2024, 24, 3899
xgoal
goal
− xnear
near 7 of (9)
31

where ρ is the step size, g is the gravity gain factor, x goal is the target position vector,
xgoal x− ρxnear
where newis the
is the
x
size, g is the
stepmagnitude gravity
of the gain factor,
geometric x goal
distance is the target
between node position
x goal andvector,
node
ρ rand

goal. − xnear is the magnitude of the geometric distance between node x goal and
xxnear
node
x goal xnear .
Random function Rd ( n ) :
Random function Rd(n):
xgoal − xnear
g *ρ
xnear
Rd (n) = ρ ×x goal − xnear (9)
Rd(n) = ρ × xgoal − xnear (9)
x goal − xnear

xnew
xrand
Figure 4. Random
ρ tree growth with increasing gravitational components.

3.1.2. Introduction of Repulsive Force Component

xgoal

When
g *ρ the RRT* algorithm expands the tree, it makes the random tree have a certain
x near
distance from the obstacle by increasing the repulsive component, as shown in Figure 5.
The repulsive force component is added to the new nodes generated in the process of
random tree expansion. Add the repulsion function T (n) at node n , as follows:

F2 (n) = Rd (n) + T (n) (10)

Figure 4.
Figure 4. Random
Random tree
tree growth
growth with
with increasing
increasing gravitational
gravitational components.
components.
where F2 ( n ) is the new node in the search process, Rd ( n ) is the random growth func-
3.1.2.
3.1.2. Introduction of Repulsive Force Component
tion, and T ( n ) is the repulsion function.
When
When the
the RRT*
The obstacleRRT* algorithm
algorithm
repulsion expands
expands
function the) istree,
T (nthe tree, it
it makes
as follows: makes the the random
random tree tree have
have a certain
certain
distance from the obstacle by increasing the repulsive component,
distance from the obstacle by increasing the repulsive component, as shown in as shown in Figure
Figure 5.5.
The repulsive force component
The repulsive force component  is added to the0, p
new( x ) > p
nodes generated in the process of ran‑
 is added to the new nodes generated in the process of
0

(n) =repulsion
Tthe 1 ∂T( (xnear
n)T−at
dom
random treetree
expansion. AddAdd
expansion. theρ k
 rep (
1 function
 repulsion
−
1 function
) 2
(xnobstacle
)nodeat) , node
n, as follows:
p ( x) ≤ pn
0
, as follows: (11)
 p ( x) p0 p( x) ∂xnear
F2 (nF)2 (= = Rd
n) Rd ) +TT((nn))
(n()n+ (10)
where k rep is the repulsive force gain coefficient, p ( x ) is the shortest distance from
where
xrand to
where F2F(the
2n( n) )obstacle,
is is the
the new
new node inin the
the search
search process,
process, Rd(Rd n)( nis) the
is the random
random growth
0 is the distance affected by the obstacle on the node, and xobstacle
pnode growth func-
function,
and
tion, T (
andn ) T
is (
then ) repulsion
is the function.
repulsion
is the position vector of the obstacle. function.
The obstacle repulsion function T (n) is as follows:

xnew  0, p( x) > p0

T ( n) =  1 1 1 ∂ ( xnear − xobstacle ) (11)
xrand
 ρ krep ( p ( x) − p ) p( x) 2 ∂xnear
, p ( x) ≤ p0
 0

where k rep is the repulsive force gain coefficient, p ( x ) is the shortest distance from
Obstacle
ρ
xrand to
K the
*ρ obstacle, p0 is the distance affected by the obstacle on the node, and xobstacle
rep

xnear
is the position vector of the obstacle.

xnew

Figure 5. Random treexrand

growth with increasing repulsion component.

The obstacle repulsion function T (n) is as follows:

Obstacle
ρ (
Krep *ρ 0, p( x ) > p0
xnearT ( n ) (11)
= 1 ∂( xnear − xobstacle )
ρkrep ( p(1x) − 1
p0 ) p ( x )2 ∂xnear , p ( x ) ≤ p0

where krep is the repulsive force gain coefficient, p( x ) is the shortest distance from xrand
to the obstacle, p0 is the distance affected by the obstacle on the node, and xobstacle is the
position vector of the obstacle.
The new node generating function F (n) is as follows:

F (n) = F1 (n) + F2 (n) (12)

 Figure 5. Random tree growth with increasing repulsion component.

The new node generating function F ( n) is as follows:

Sensors 2024, 24, 3899 F ( n ) = F1 ( n ) + F2 ( n ) (12)
8 of 31

where F1 ( n ) represents the new node function of the random tree in the gravitational
field, and
where 2 ( n ) represents
F1 (n)Frepresents the new thenode
newfunction
node function of the random
of the random tree
tree in the in the repulsion
gravitational field,
field. It is known
and F2 (n) represents the that F ( nnew
) is not only determined
node function of the randomby x , which avoids random field.
rand tree in the repulsion sam-
Itpling
is known that F (n) is not only determined by xrand , which avoids random sampling
of the RRT*.
of the RRT*.
3.2. Probabilistic Sampling Optimization
3.2. Probabilistic Sampling Optimization
Because of the strong randomness in sampling, the RRT* algorithm often causes ran-
dom trees to of
Because the strong
deviate from randomness
growth, which in sampling, the RRT*
seriously affects thealgorithm
efficiency oftenof thecauses
algorithm.ran‑
dom trees to deviate from growth, which seriously affects the efficiency
It can be seen from Theorem 2 that in order to find a feasible path faster, the random tree of the algorithm.
Itofcan
thebe seen fromcan
algorithm Theorem
grow more2 thatpurposefully,
in order to find a feasibleto
according path faster,1.the
Lemma random tree
Therefore, proba-of
the algorithm can grow more purposefully, according to Lemma
bilistic sampling optimization is designed to improve target orientation and reduce re- 1. Therefore, probabilis‑
tic sampling
dundant optimization
searches [26]. is designed to improve target orientation and reduce redundant
searches [26].
Firstly, the obstacle-free area of the target point is determined, that is, the minimum
Firstly, the obstacle‑free area of the target point is determined, that is, the minimum
distance from target x goal to all obstacles, which is called the convergence radius of target
distance from target x goal to all obstacles, which is called the convergence radius of target
point RR.g .As
point Asdepicted
depictedin in Figure
Figure 6,6, aa region
region circle
circle is
is generated
generatedwith with R R g asasthe
theradius
radius andand
g g
xxgoal as the center, and the inner circle is the convergence region of the target point. Its
goal as the center, and the inner circle is the convergence region of the target point. Its
characteristic
characteristic isis that that when
when thethe random
random treetree grows
grows within
within thisthis range,
range, itit can
can direct
direct itself
itself to
to
the end point.
the end point.

Figure6.
Figure 6. Schematic
Schematic diagram
diagram of
of convergence
convergence region
regionof
oftarget
targetpoint.
point.

In
In the
the strategy
strategy of target bias, a target bias bias probability
probability valuevalue pptarget
target
is randomly gen‑ gen-
erated by using the random number generating function, and then
erated by using the random number generating function, and then a random probability a random probability
value p pis obtained,
value which
is obtained, which is is
greater
greater than
than0 0andandless
lessthan When p p≥≥ pptarget
than1.1.When target
is
is obtained,
obtained,
then the sampling point is randomly generated. If p < ptarget is in the convergence region
of the target point, the random extension tree growsp <near ptarget is in the convergence region
then the sampling point is randomly generated. If
the target point with a certain
of the targetbypoint,
probability the random
randomly generatingextension
a point tree
as grows
a samplingnear the target
point. Thispoint
methodwithcana certain
accel‑
probability
erate by randomly
the convergence rategenerating
to the target a point
node.as aThe
sampling
sampling point. This method can
point‑generating accel-
function
erate
is the convergence rate to the target
as follows:  node. The sampling point-generating function is
as follows: Goal Area(), p < ptarget
Xrand = (13)
Sample(), p ≥ ptarget
GoalArea (), p < ptarget
where the GoalArea function represents X rand = the random generation of a sampling point in (13) the
 Sample (), p ≥ ptarget
convergence region of the target point, the sample function represents the random genera‑
tion
where of athe
sampling
GoalAreapoint in therepresents
function global range, and xrandgeneration
the random representsofthe randomlypoint
a sampling generated
in the
sampling point.
convergence region of the target point, the sample function represents the random gener-
The probabilistic sampling optimization of the global path search is carried out [27].
ation of a sampling point in the global range, and xrand represents the randomly gener-
When p ≤ ptarget , the target offset sampling is adopted, that is, a point is randomly gener‑
ated in
ated sampling point.
the target convergence domain as the sampling point. When ptarget < p ≤ p goal ,
there is uniform random sampling in the search space.
When a new node coordinate xnew is generated, it is determined whether the xnew is
in the target point convergence region. If so, the xnew is directly connected to the terminal
x goal , but the step size ρ may be set to be larger than the target convergence radius R g ,
causing the random tree to oscillate when it expands near the x goal . This will cause the
random tree to have limitations at the target point, as depicted in Figure 7.
 When a new node coordinate xnew is generated, it is determined whether the xnew
is in the target point convergence region. If so, the xnew is directly connected to the ter-
minal x goal , but the step size ρ may be set to be larger than the target convergence ra-
Sensors 2024, 24, 3899 dius R g , causing the random tree to oscillate when it expands near the x goal . This9 of
will
31
cause the random tree to have limitations at the target point, as depicted in Figure 7.

Figure7.
Figure 7. The
The limitation
limitation of
of the
the random
random tree
tree near
near the
thetarget
targetpoint.
point.

In
Inorder
ordertoto avoid thisthis
avoid situation and make
situation and make it get the optimal
it get path faster,
the optimal path when p ≥when
faster, p goal
occurs, all occurs,
of the random
all of the random tree nodes are traversed to find the node xk withcost
tree nodes are traversed to find the node x with the lowest
p ≥ p goal k the
of cmin , as shown in the following formula:
lowest cost of cmin , as shown in the following formula:


cmin = C k + d xk , x goal (14)
cmin = C k + d ( xk , xgoal ) (14)
k


where C is the actual path cost from xinit to xk , and d xk , x goal is the Euclidean distance
where
from nodeC k xistothetarget
actualpoint
pathxcost.from xinit x toand d ( xkperform
xk x, and, then , xgoal ) iscollision
the Euclidean dis-
k goal Connect k goal detection
tance
on the from node xkxktoto
path from target
x goal point
. If no x goal .occurs,
collision Connect xk isxkdirectly
and xusedgoal
, then perform
as the parent collision
node of
xdetection
goal , and then the path is generated. If a collision occurs, it is changed
on the path from xk to x goal . If no collision occurs, xk is directly used as the to uniform random
sampling, where ptarget and p goal are between 0 and 1.
parent node of x goal , and then the path is generated. If a collision occurs, it is changed to
In the initial target offset strategy, the target bias probability of the RRT* algorithm is
fixed, andrandom
uniform sampling,
its adaptability towhere
the scene ptarget and In
is poor. p goal
orderaretobetween
improve 0 and 1.
adaptability, a proba‑
bilityIn adaptive
the initialtarget biasoffset
target strategy is designed
strategy, the target to expand the tree growth
bias probability combined
of the RRT* with
algorithm
the gravity
is fixed, andcomponent constraints
its adaptability to the mentioned
scene is poor. above to shorten
In order the planning
to improve time [28].
adaptability, a prob-
abilityBefore generating
adaptive target abias
new sampling
strategy point, a decision
is designed to expand circle
theistree
generated
growth with the center
combined with
Sensors 2024, 24, x FOR PEER REVIEW 10 of 33
thexnew
of as the
gravity center andconstraints
component the connection between
mentioned xneartoand
above xnew as
shorten thethe radius R,
planning as [28].
time shown
in FigureBefore8. generating a new sampling point, a decision circle is generated with the center
of xnew as the center and the connection between xnear and xnew as the radius R , as
shown in Figure 8.

Figure8.8.Construct
Figure Constructthe
thedecision
decisionarea.
area.

In Figure 8,8, xnew

In Figure xnewis isthethecenter
centerofofthe circle,R is
thecircle, R the radius,
is the radius, S is Stheisarea
andand of the
the area of
obstacle in the circle. The proportional value P can be obtained from the area value S, as
the obstacle in the circle. The proportional value P can be obtained from the area value
shown in the following formula:
S , as shown in the following formula: S
P= (15)
(π ∗ RS)2
P= (15)
(π offset
∗ R ) probability value ptarget ; the formula
2
Then change the scale value P to the target
is as follows:
Then change the scale value pP to = the1−target 4.5) probability value ptarget ; the for-
e−( P∗offset (16)
target
mula is as follows:
In the process of determining circle generation, the proportional value P will change
e ( S, while
− P∗4.5)
with the change of radius R and occupied p area = 1−
target
value the probability value ptarget
(16)

In the process of determining circle generation, the proportional value P will

change with the change of radius R and occupied area value S , while the probability
value ptarget will change with the change of proportional value P , so as to realize the
self-adaptation of target bias probability. After the target bias probability value p is
Sensors 2024, 24, 3899 10 of 31

will change with the change of proportional value P, so as to realize the self‑adaptation of
target bias probability. After the target bias probability value ptarget is obtained and com‑
pared with the random probability value p, sample according to the probability sampling
optimization method mentioned above.
When the target offset sampling is adopted, the expansion tree grows to the target
point, and the random node xrand will appear in the direction of the x goal and xnear connec‑
tions. Set the step size to ρ, and generate the formula for the new node xnew as follows:

xrand − xnear
xnew = xnear + ∗ρ (17)
∥ xrand − xnear ∥

When uniformly randomly sampled in the search space, the extension tree grows in
the direction of the resultant force of random and gravitational components, so that the
expansion of the new node will not deviate from the shortest path and enhance the goal
bias of path sampling. The diagram of the new node is shown in Figure 4 above. The
following formula will generate the new node xnew :
 
x − x x goal − xnear
xnew = xnear +  ∗ g ∗ ρ
rand near
∗ρ+ (18)
∥ xrand − xnear ∥ x −x near
goal

3.3. Adaptive Step Size Strategy

In the process of global planning sampling, it can be seen from Theorem 1 that the
algorithm should speed up the search speed, make its iterative progress faster, and im‑
prove the path quality. Therefore, in this improvement, the adaptive step size should be
designed according to Lemma 1, so as to accelerate the sampling efficiency and improve
the path quality. Different road environments need different steps. If the step size is too
short, when there are few obstacles near the vehicle, or if the curvature of the road is small,
the search takes a long time and reduces the search efficiency. If the step size is too large,
there are many obstacles in the environment, or if the road has a large curvature, it may fall
into a stagnant state and be unable to generate a feasible path. In order to further optimize
the step size selection, an adaptive step size strategy is proposed. The step size ρ uses the
number of obstacles near the vehicle and the curvature of the road so as to improve the
adaptability of the vehicle to the environment. The formula for calculating curvature k is
as follows: ..
y
k=  3 (19)
.
2 2
1+ y

where y is the road function. As the vehicle runs, there is continuous acquisition of vehicle
location information and lane conditions, and the curvature of the lane is calculated.
The adaptive step size policy formula is as follows:

ρ = ρ0 e λ ( p − k − q ) (20)

where ρ0 is the initial step, p is the threshold of the number of obstacles, k is the curvature
of the road, q is the number of obstacles, and λ is a value between 0 and 1.
When the number of obstacles around vehicle q and the road curvature k are zero,
more than double the initial step size of ρ0 is selected to generate new nodes. When the
number of obstacles q is less than or equal to the threshold p and the curvature of the road is
small, it means that the vehicle is driving on a road similar to a straight line and the number
of obstacles around is small, and a new node is created by using a step close to the initial
step size of ρ0 . When the number of obstacles q is greater than the threshold p, it shows that
there are a large number of obstacles around the vehicle. When there is a large curvature
on the road, it means that the road environment where the vehicle is located is a large curve.
Consider that the vehicle cannot come into contact with obstacles; a new node is created
Sensors 2024, 24, 3899 11 of 31

by 0–1 times the initial step length of ρ0 . After the above process, the sampling step size
can automatically change the applicable value in different road environments, improving
the adaptability of the algorithm and enhancing the robustness of path planning.
When the target point is on the straight road and there are few obstacles nearby, a
longer step size should be selected, but using a larger expansion step size will produce the
oscillation phenomenon shown in Figure 7, resulting in an increase in path planning time
and redundant nodes. In the optimization of probabilistic sampling, although it is possible
to take x goal as the parent node near the target point, there is still probability. Therefore, it is
necessary to further optimize the algorithm’s performance. The distance Distance between
xnearest and x goal is calculated before each expansion. If Distance > ρ, the expansion step
size remains the same; otherwise, the expansion step size is adjusted to the Distance more
suitable for growing near x goal , and the oscillation problem is solved.

3.4. Global Path Post‑Processing

Sensors 2024, 24, x FOR PEER REVIEW After the above‑mentioned improvement strategies, the algorithm in this paper can33
12 of
quickly plan the initial path, but there may still be some tortuous paths and a large number
of unnecessary nodes [29]. First of all, for the redundant paths, the triangle principle is
used to design the optimization node strategy, and then for some of the tortuous paths,
3.4.1. Node Pruning Strategy
the constraint based on the minimum turning radius is established [30].
The principle of the node pruning strategy is depicted in Figure 9. The dashed line
3.4.1. Node the
represents Pruning
initialStrategy
path planned by the improved RRT* algorithm without incorporat-
ing aThenode pruningofstrategy.
principle the nodeThe planning
pruning path is formed
strategy depictedbyinthe nodes
Figure 9. in
Thethedashed
initial node
line
set {Q1 , Q2the
represents , Q3initial , Q6 } planned
, Q4 , Q5path , where by Q1 the
and Q6 are the
improved RRT*starting point
algorithm and target
without point, re-
incorporating
aspectively.
node pruning Afterstrategy.
introducing The planning
the node path is formed
pruning by the
strategy, takenodes in thepoint
the target initialQnode set
6 as the
{root
Q1 , Q2 , Q3 , Q4 , Q5 , Q6 }, where Q1 and Q6 are the starting point and target point, respec‑
node, and each node in the initial node set is connected successively based on Q ,
tively. After introducing the node pruning strategy, take the target point Q6 as the root6
and collision detection is carried out. It is found that line segment Q6 Q1 , line segment
node, and each node in the initial node set is connected successively based on Q6 , and col‑
Q6 Q2 detection
lision , and Q6 Qis3 carried
will collide with
out. It obstacles,
is found but there
that line is noQcollision
segment Q
6 1 , line when
segment Q6 Qis directly
6 Q2 , and
Qconnected to Q4 ,with
6 Q3 will collide so Q 4 is added
obstacles, buttothere
the set of collision
is no optimized nodes.
when Q6 Then, taking
is directly Q4 as the
connected to
Q , so
starting
4 Q 4point of collision detection, Q4 can only connect with 4Q3 without collision, of
is added to the set of optimized nodes. Then, taking Q as the starting point so
collision detection, Q can only connect with Q without collision,
Q3 is added to the set of optimized nodes. Connect each node in the original path in turn
4 3 so Q 3 is added to the
set of optimized nodes. Connect each node in the original path in turn and carry out a new
and carry out a new round of collision detection. We can know that when Q1 can be di-
round of collision detection. We can know that when Q1 can be directly connected to Q3 ,
rectlythe
when connected
starting pointto Q3is, added
when theto thestarting pointnode
optimized is added to the optimized
set, it indicates noderecon‑
that the node set, it
indicates
nection thatthat
ends, the is,
node
Q1 reconnection
is put into theends, that is,node
optimized Q1 is put
set. into the optimized
According to the order node set.
of the
nodes
According in thetooptimized
the ordernode of theset, the optimization
nodes in the optimized path node
is obtained.
set, theThe path is depicted
optimization path is
by the solidThe
obtained. linepath
in Figure 9. Compared
is depicted by the solidwithline
the inoriginal
Figurepath, it is observed
9. Compared with that the op‑
the original
timized path features a substantial reduction in node count and
path, it is observed that the optimized path features a substantial reduction in node count effectively decreases the
path’s length cost.
and effectively decreases the path’s length cost.

Figure9.9.Schematic
Figure Schematicdiagram
diagramof
ofnode
nodebranch
branchpruning
pruningstrategy.
strategy.

3.4.2.
3.4.2.Node
NodeOptimization
Optimization
After the path is treated by the node branch and shear, the joint angle of part of the
After the path is treated by the node branch and shear, the joint angle of part of the
path cannot meet the vehicle dynamics constraint, so it is necessary to optimize the route
path cannot meet the vehicle dynamics constraint, so it is necessary to optimize the route
with too large a bend angle to ensure that the turning angle at the turning point of the
with too large a bend angle to ensure that the turning angle at the turning point of the
path satisfies the vehicle kinematic constraints [31]. And the continuous folding angle is
optimized to make the turning angle more uniform and stable. The process is illustrated
in Figure 10.
 Figure 9. Schematic diagram of node branch pruning strategy.

3.4.2. Node Optimization

Sensors 2024, 24, 3899 After the path is treated by the node branch and shear, the joint angle of part of the 12 of 31
path cannot meet the vehicle dynamics constraint, so it is necessary to optimize the route
with too large a bend angle to ensure that the turning angle at the turning point of the
pathpath
satisfies the the
satisfies vehicle kinematic
vehicle constraints
kinematic [31].[31].
constraints AndAnd
the continuous folding
the continuous angle
folding is is
angle
optimized to make the turning angle more uniform and stable. The process is illustrated
optimized to make the turning angle more uniform and stable. The process is illustrated
in Figure 10. 10.
in Figure

(a) (b)
Figure
Sensors 2024, 24, x FOR PEER REVIEW 10. Schematic
Figure diagram
10. Schematic of node
diagram optimization
of node strategy:
optimization (a) before
strategy: nodenode
(a) before optimization; (b) after
optimization; (b)ofafter
13 33
node optimization.
node optimization.

As shown in Figure
As shown 10a,10a,
in Figure in the
in planned
the plannedpath, the angle
path, between
the angle node
between 2 and
node node
2 and 3 3
node
to
andand be
node optimized.
4 and nodeThis problem
5 does notnotis solved
conform by adding
to to
thethe auxiliary
vehicle nodes,
kinematic such as
constraint, nodes 3, 4,
sosoit itneeds and
node 4 and node 5 does conform vehicle kinematic constraint, needs to
5beinoptimized.
Figure 10b,This
which flatten the initial angle so as to meet the minimum turning
problem is solved by adding auxiliary nodes, such as nodes 3, 4, and radius
constraint
5 in Figureof10b,
the which
vehicle.flatten the initial angle so as to meet the minimum turning radius
constraint of the vehicle.
4. Improved Artificial Potential Field Method
4. Improved
4.1. ArtificialConstraint
Obstacle Avoidance Potential Field Method
4.1. Obstacle Avoidance Constraint
When modelling obstacles in the road environment, the threat in the longitudinal
When
direction modellinggreater
is generally obstacles
thaninthat
theinroad environment,
the horizontal the threat
direction, inlongitudinal
so the the longitudinaldis-
tance between the main vehicle and the environmental obstacles should longitudinal
direction is generally greater than that in the horizontal direction, so the be fully consid- dis‑
tancewhen
ered between the main obstacle
establishing vehicle and the environmental
avoidance constraints.obstacles
Therefore, should be fully considered
the surrounding obsta-
when
cles areestablishing
modelled asobstacle avoidance
ovals. The major axisconstraints. Therefore,
of the ellipse the surrounding
corresponds obstacles
to the longitudinal
are modelled
axis as ovals.
of the obstacle, andThe
themajor
short axis
axisofofthe
theellipse
ellipsecorresponds
correspondstotothe thelongitudinal
horizontal axisaxisofof
the obstacle, and the short axis of the ellipse corresponds to the horizontal
the obstacle. Considering that the obstacle has speed, this will impact the main vehicle’s axis of the ob‑
stacle. Considering
obstacle that the obstacle
avoidance performance. has speed,
Therefore, this will impact
a dynamic ellipsethe mainofvehicle’s
model obstacle
environmental
avoidance performance. Therefore, a dynamic ellipse model of environmental
obstacles, which can change with speed, is designed. The long axis length of the ellipse is obstacles,
which can with
associated change thewith speed,
velocity is designed.
parameter. Thus,Thevelocity
long axisis length
directlyofrelated
the ellipse is associated
to the length of
withdynamic
the the velocity parameter.
ellipse’s Thus,indicating
major axis, velocity isthat
directly related
a larger to the area
influence length of the dynamic
corresponds to a
ellipse’s major axis, indicating that a larger influence area corresponds
lower collision probability [32]. Environmental obstacles are depicted, as illustrated in to a lower collision
probability
Figure 11. [32]. Environmental obstacles are depicted, as illustrated in Figure 11.

a v

Figure 11.
Figure 11. Schematic
Schematicdiagram
diagramof
ofenvironmental
environmental obstacles.
obstacles.

The major
The major and
and minor
minor axes
axes of
of the
the dynamic
dynamic ellipse
ellipse are
are denoted
denoted as
as follows:
follows:

LL
a = ++kkaa++ krr v
a= (21)
(21)
22
W
b = W+ k b (22)
b =2 + kb (22)
where L represents the length of the obstacle, 2 W represents the width of the obstacle, v
denotesLtherepresents
where speed of the length ofk athe
theobstacle, W represents
k b are horizontal
andobstacle, and vertical security
the width redundancy
of the obstacle,
vdistances, the kspeed
denotesand r is the
ofsafety redundancy
the obstacle, ka andtime.kb are horizontal and vertical security re-
dundancy distances, and k r is the safety redundancy time.
In the above modelling, the collision can be judged by judging whether there is an
overlap between the main vehicle and the ellipse boundary. The boundary of the ellipse
is divided into many circles, and several circles are made on the boundary line of the el-
Sensors 2024, 24, 3899 13 of 31

In the above modelling, the collision can be judged by judging whether there is an
overlap between the main vehicle and the ellipse boundary. The boundary of the ellipse is
divided into many circles, and several circles are made on the boundary line of the ellipse
with a width of 1/2 of the vehicle as the radius, thus forming a new ellipse, that is, the ex‑
tended ellipse. It is equivalent to increasing the collision range by half a vehicle’s width on
Sensors 2024, 24, x FOR PEER REVIEW 14 of 33
the basis of the original obstacle ellipse. The distance between the vehicle and the obstacle
is increased, and the path safety is improved. The extended ellipse is shown in Figure 12.

ak

Rob

bk

Figure 12.
Figure 12. Extended
Extended ellipse.
ellipse.

The major and minor axes of an extended ellipse are expressed

expressed as
as follows:
follows:

L L
ak =ak = + k+ak+
a +
krkvr v++ R
Rob
ob (23)
(23)
2 2
W
bk = W+ k b + Rob (24)
bk =2 + kb + Rob (24)
2
When the vehicle is driving on the planned path, the point mass model is used to sim‑
plify When the vehicle
the vehicle, is driving
and collision on theisplanned
detection performed path,by the point mass
assessing model
the spatial is used to
relationship
simplify the
between vehicle,
the point andmodel
mass collision
anddetection
the extended is performed
ellipse. Ifby theassessing
center ofthemass spatial relation-
lies within or
ship
on thebetween
extended the point amass
ellipse, model
collision and thetoextended
is deemed have occurred,ellipse.andIf the
the center
collision of constraint
mass lies
within
is or onConversely,
not met. the extendedifellipse,
the centera collision
of mass is is
deemed
locatedtooutside
have occurred,
the extended and the collision
ellipse, no
constraintis is
collision not met. Conversely,
considered to have takenifplace, the center
and the of collision
mass is located
constraint outside the extended
is satisfied.
ellipse, no collision is considered to have taken place, and the collision constraint is satis-
4.2.
fied.Road Repulsion Potential Field
During main car operation on a two‑lane road, the lane line and lane boundary should
4.2. Road
cause Repulsion
repulsion Potential
to the moving Field
vehicle, and when there are no obstacles in the road, the ve‑
hicle During
should not main car operation lane
leave the current on aunless
two-laneit is necessary
road, thetolane change
line lanes
and when obstacles
lane boundary
are encountered. Vehicles are not allowed to press the line or cross
should cause repulsion to the moving vehicle, and when there are no obstacles in the road, the road boundary to
avoid trafficshould
the vehicle accidents.
not leave the current lane unless it is necessary to change lanes when
In theare
obstacles road repulsion potential
encountered. Vehiclesfield are model design,toensuring
not allowed press the that theorrepulsion
line cross thepoten‑
road
tial field at the lane boundary
boundary to avoid traffic accidents. is the highest serves to confine the vehicle within the lane
boundary [33]. While preserving the boundary repulsion potential
In the road repulsion potential field model design, ensuring that the repulsion poten- field, the lane repul‑
sion potential
tial field at thefield
laneisboundary
incorporated.
is the To ensure
highest smooth
serves lane changes
to confine for the
the vehicle mainthe
within vehicle,
lane
the
boundary [33]. While preserving the boundary repulsion potential field, the lane the
maximum value of the lane’s repulsive potential field must be lower than that of ob‑
repul-
stacle. A novel road repulsion potential field is formulated to prevent
sion potential field is incorporated. To ensure smooth lane changes for the main vehicle, vehicle lane changes
in
thethe absence of
maximum obstacles.
value The repulsion
of the lane’s repulsivepotential
potentialfieldfieldformust
a vehicle
be lowertravelling
than that along
of the
the
centerline of the current lane is minimized to constrain the vehicle’s
obstacle. A novel road repulsion potential field is formulated to prevent vehicle lane ability to remain in
the lane’s centerline. The road repulsion potential field U road is obtained
changes in the absence of obstacles. The repulsion potential field for a vehicle travelling by superposing
the
alonglane
theline potentialoffield
centerline Ul and the
the current laneroad boundary to
is minimized potential
constrain theUvehicle’s
field r . The formulaabilityfor
to
the road repulsion potential field Uroad is expressed as follows:
remain in the lane’s centerline. The road repulsion potential field U road is obtained by
superposing the lane line potential field Uroad U=l Uandl + Ur
the road boundary potential field U r .
(25)
The formula for the road repulsion potential field U road is expressed as follows:
The lane line potential field Ul can make the vehicle keep a certain distance from the
lane and make it move towards the center U road = Ulane.
of the l + U r It should be guaranteed to overcome (25)
The lane line potential field U l can make the vehicle keep a certain distance from
the lane and make it move towards the center of the lane. It should be guaranteed to over-
come the potential field when encountering obstacles, which requires that the lane poten-
tial field be small enough to ensure driving safety. The lane line potential field established
Sensors 2024, 24, 3899 14 of
Sensors 2024, 24, x FOR PEER REVIEW 15 of 31
33

the potential field when encountering obstacles, which requires that the lane potential field
−1
be small enough to ensure driving safety. TheNlslane line potential field established in this
U
paper is expressed by the following formula:
l =  U l ,i i
(26)

Nls −1
Ul = ∑ U ( yl,i− yc ,i ) 
2
(26)

U l ,i = Al expi −  (27)
 2σ 2 !
 
(y − yc,i )2
where Al represents the intensityU = A exp
l,i coefficient
l − of the (27)
potential field, yc ,i is the central
2σlane
2

position of the Gaussian function, the i lane coordinate is generally set on the centerline
where Al represents the intensity coefficient of the lane potential field, yc,i is the central
of the lane, and σ determines the range of the potential field, that is, the sensitivity of
position of the Gaussian function, the i lane coordinate is generally set on the centerline
the vehicle to the lane centerline.
of the lane, and σ determines the range of the potential field, that is, the sensitivity of the
Set the road boundary potential field U r to infinity to limit vehicle crossing, as
vehicle to the lane centerline.
shownSet in
thethe U rboundary
road expression:
potential field Ur to infinity to limit vehicle crossing, as shown
in the Ur expression: N
U r =NU r , j (28)
Ur = ∑ Uj r,j (28)
j
!2 2
U 1= 1 η  1 1  (29)
Ur,j =r , j η  (29)
2 2 y−y y−0,jy0, j 

whereη ηis the

where potential
is the field
potential factor
field of of
factor thethe
boundary
boundaryroad,
road, is ythe
y0,j yis0, jthe y coordinate
coordinate j
of the of
road boundary, and N represents the number of road boundaries.
the j road boundary, and N represents the number of road boundaries. Figure 13 Figure 13 shows the
road potential field diagram of the Dow repulsion.
shows the road potential field diagram of the Dow repulsion.

Figure 13. Road repulsion potential field diagram.

Figure 13. Road repulsion potential field diagram.

4.3.Increase
4.3. IncreaseDistance
DistanceFactor
Factor
ToToaddress
addressthetheissue
issuethat
thatthe the target
target pointpoint is unreachable,
is unreachable, it isitnecessary
is necessary to optimize
to optimize the
repulsion function
the repulsion and add
function and the
adddistance
the distance factor d2g [34].
factor d g2 The[34].improved
The improved repulsive potential
repulsive po-
field function is as follows:
tential field function is as follows:
  2 2

 12 k α krep
1  1 1 − 1 1 d2 2, 0 ≤ d ( p, p0 ) ≤ d0
kα krep
d(p,p0 ) d−0  dgg , 0 ≤ d ( p, p0 ) ≤ d 0
Urep (d, α) = 2  d ( p, p ) d  (30)
 0 
α ∈ (0, π )

0

U rep
( ) 
d , α = 0 , d( p,( p0 )) > d0
α ∈ 0, π (30)

 0 , d ( p, p0 ) > d 0
 2
After introducing the distance  factor d g , the repulsion from the obstacle and the at‑
traction from the target point will both diminish to zero only when the vehicle reaches
Afterpoint.
the target introducing the distance
This resolves factor
the issue d g2 , the
of the repulsion
target from the obstacle
being unreachable when and the at-
the obsta‑
traction from the target point will both diminish to zero only when the vehicle reaches the
 target point. This resolves the issue of the target being unreachable when the obstacle is
in close proximity to the target point. However, when there are multiple obstacles, they
Sensors 2024, 24, 3899 may lead to local minima because their repulsion and gravity may be in the opposite di-
15 of 31
rection. Therefore, the repulsion function of the obstacle is improved, and a repulsion term
pointing to the target point is added. The enhanced repulsion function is defined as fol-
lows:
cle is in close proximity to the target point. However, when there are multiple obstacles,
they may lead to local minima because  Frep1 +their ≤ d ( p, p0 ) ≤and
Frep 2 , 0repulsion α ∈ ( 0, π may
d 0 ∩gravity ) be in the opposite
direction. Therefore, theFrep ( d , α ) =  function of the obstacle is improved, and a repulsion
repulsion (31)
 0, d ( p, p0 ) > d 0
term pointing to the target point is added. The enhanced repulsion function is defined
The Frep1 and Frep 2 expressions
as follows:  are as follows:
Frep1 + Frep2 , 0 ≤ d( p, p0 ) ≤ d0 ∩ α ∈ (0, π )
Frep (d, α) = (31)
 0, d(1p, p0 ) >1 d0 2
Frep1 = kα krep  −  d g (32)
The Frep1 and Frep2 expressions are as follows:  d ( p, p0 ) d0 
 
1 1
1 − 1  dg
2
Frep1 = k αkkα rep  (32)
Frep 2 = krep d( p, p0 ) − d0  d g (33)
2  d ( p, p0 ) d0  
kα 1 1
krep
Frep2of=the obstacle’s − dg (33)
where d 0 represents the range 2 p0 ) d0 potential field, d denotes the
d( p, repulsive
distance
where d between thethe
represents obstacle
rangeand of the theobstacle’s d g represents
vehicle, repulsive the distance
potential from thethe
field, d denotes vehi-
dis‑
0
cle to the
tance targetthe
between point, k rep and
obstacle is thethe
repulsive
vehicle,gain coefficient,
d g represents thekαdistance
is the repulsive potential
from the vehicle to
the target point, k
field constraint factor,
rep is the repulsive gain coefficient, k
Frep1 is the repulsive force of the is the repulsive potential field
α obstacle pointing to the vehicle, con‑
straint factor, Frep1 is the repulsive force of the obstacle pointing to the vehicle, and Frep2
and Frep 2 is directed from the vehicle to the target point. Figure 14 shows the force anal-
is directed from the vehicle to the target point. Figure 14 shows the force analysis of the
ysis of the improved
improved vehicle. vehicle.

Obstacle

xnear

Frep1

Frep2

Frep
Fatt

F xgoal

Figure
Figure 14.
14. The
Theforce
forceof
ofthe
thevehicle
vehicleafter
afteroptimizing
optimizing the
the repulsion
repulsion function.
function.

4.4. Virtual
4.4. VirtualTarget
TargetPoint
Point
Inthe
In theartificial
artificialpotential
potentialfield
fieldmethod,
method,the
thevehicle
vehiclemoves
movesfrom
fromthe
thehigh
highpotential
potentialfield
field
to the low potential field, and the target point is the global minimum point of the potential
to the low potential field, and the target point is the global minimum point of the potential
field, so the vehicle should stop moving at the target point. When there are U‑shaped
field, so the vehicle should stop moving at the target point. When there are U-shaped ob-
obstacles in the road, the interior is the local minimum region, and the global minimum
stacles in the road, the interior is the local minimum region, and the global minimum is
is the target point. Therefore, the minimum point will not only be at the target point; in
the target point. Therefore, the minimum point will not only be at the target point; in this
this environment, the vehicle driving in the direction of low potential energy will enter
environment, the vehicle driving in the direction of low potential energy will enter the
the local minimum area of the U‑shaped obstacle, which will cause the vehicle to stop
local minimum area of the U-shaped obstacle, which will cause the vehicle to stop or os-
or oscillate around this area. The overall potential field featuring a U‑shaped obstacle is
cillate around this area. The overall potential field featuring a U-shaped obstacle is de-
depicted in Figure 15.
picted in Figure 15.
To address this issue, a virtual target point is introduced to steer the vehicle away from
obstacles. The potential field is adjusted to improve the passing capacity of the vehicle near
the local minimum so as to plan a more reasonable path. When setting the virtual target
point, the point is too far from the obstacle, which will lead to the planned path being
too long, resulting in an increase in the amount of calculation. The point too close to the
obstacle may reduce the safety of the path or even collide with the obstacle. The processing
flow for encountering a U‑shaped obstacle is shown in Figure 16.
 Sensors2024,
Sensors 2024,24,
24,3899
x FOR PEER REVIEW 17 of 31
16 of 33

Figure 15. The general potential field near a U-shaped obstacle.

To address this issue, a virtual target point is introduced to steer the vehicle away
from obstacles. The potential field is adjusted to improve the passing capacity of the vehi-
cle near the local minimum so as to plan a more reasonable path. When setting the virtual
target point, the point is too far from the obstacle, which will lead to the planned path
being too long, resulting in an increase in the amount of calculation. The point too close
to the obstacle may reduce the safety of the path or even collide with the obstacle. The
Figure
Figure 15.The
processing
15. The
flowgeneral potentialfield
for encountering
general potential field nearaaU‑shaped
a U-shaped
near U-shaped obstacle.
obstacleobstacle.
is shown in Figure 16.

To address this issue, a virtualStart target point is introduced to steer the vehicle away
from obstacles. The potential field is adjusted to improve the passing capacity of the vehi-
cle near the local minimum so as to plan a more reasonable path. When setting the virtual
target point, the point is too far from the obstacle, which will lead to the planned path
Encounter a U-shaped obstacle
being too long, resulting in an increase in the amount of calculation. The point too close
to the obstacle may reduce the safety of the path or even collide with the obstacle. The
processing flow for encountering
N
a U-shaped obstacle is shown in Figure 16.
Whether you encounter
a local minimum point

Start
Y

Introduce vir tual target point

Encounter a U-shaped obstacle

Move along the dir ection of N Whether to reach near

resultant for ce the virtual tar get point
N Whether you encounter
a local
Y minimum point

Replan the global path and

update the tem porary
Y target
point

Introduce vir tual target point

Continue to guide the vehicle

Move along the dir ection of N Whether to reach near

resultant for ce the End
virtual tar get point

Figure 16. The processing flow of U-shaped obstacles.

Y
Figure 16. The processing flow of U‑shaped obstacles.
Replan the global path and
update
Before introducing the virtual the tem
target porary near
point target the obstacle, first of all, judge whether
point
the vehicle meets the local minimum point or not, and the discrimination condition is
as follows:
n
Continue
F to+
guide the
att ∑
F vehicle< rep,j ε (34)
j =1

| x − x a | < αs a (35)
End
where j is the number of obstacles, ε is a small positive number, α is a positive number
between 0 to 1, x a is a certain state of the vehicle, and s a represents the total distance of the
Figure 16. The processing flow of U-shaped obstacles.
vehicle from x a to the current position x in the process of vehicle movement.
Sensors 2024, 24, 3899 17 of 31

When the determination condition is established, it means that the virtual resultant
force on the vehicle is close to 0, and the vehicle has moved for a long distance but its dis‑
placement is very small, so it can be considered that the vehicle stops or oscillates around
the local minimum [35]. When introducing the virtual target point, the role of the target
gravitational potential field is ignored until the vehicle successfully reaches the virtual
target point, so as to get rid of the predicament of falling into local optimization in the U‑
shaped obstacle. The virtual target point is selected where the distance from the obstacle
is L2 longitudinally and L1 horizontally. The calculation method is as follows:

L1 = λa (36)

L2 = µb (37)
q
L = L21 + L22 (38)

where λ and µ are the distance expansion coefficients. According to the danger degree of
the obstacle, the safe distance can be adjusted dynamically by changing the value of λ.
Through the above formula, the potential field function can be obtained:
q
Uvir = k vir | x − x0 − L |2 + | y − y0 |2 (39)

where k vir represents the coefficient representing the potential energy increase of the vir‑
tual target point.
Following the identification of the local minimum point, the vehicle becomes influ‑
enced by the potential field of the virtual target point. Considering that the target may
be unreachable, a circle with a radius of Rv is established as the virtual target area, with
the virtual target point as the center. Guided by the global path, the vehicle encounters
a U‑shaped obstacle and introduces the virtual target point. When the vehicle runs into
the area of the virtual target point, it will deactivate the virtual target point and proceed to
utilize either the original target point or the updated target point until reaching the 19
Sensors 2024, 24, x FOR PEER REVIEW global
of 33
target point. The virtual target point selection method is shown in Figure 17.

Figure17.
Figure 17.Virtual
Virtualtarget
targetpoint
pointselection
selectionmethod.
method.

On the road, the vehicle is mainly affected by the gravitational potential field, the
repulsive potential field, and the road potential field. If the virtual target point is intro-
duced, all the potential fields in the road environment are shown as follows:
U all = U att + U rep + U road + U vir (40)

4.5. Elliptical Groove Treatment

On structured roads, obstacles typically pose a greater threat in the longitudinal di-
Sensors 2024, 24, 3899 18 of 31
Figure 17. Virtual target point selection method.

On the road, the vehicle is mainly affected by the gravitational potential field, the
On the road, the vehicle is mainly affected by the gravitational potential field, the re‑
repulsive potentialfield,
pulsive potential field,and
andthe
theroad
road potential
potential field.
field. If the
If the virtual
virtual target
target point
point is intro-
is introduced,
duced, all the potential fields in the road environment are shown
all the potential fields in the road environment are shown as follows: as follows:
U all = U att + U rep + U road + U vir (40)
Uall = Uatt + Urep + Uroad + Uvir (40)

4.5.
4.5. Elliptical
Elliptical Groove
Groove Treatment
Treatment
On On structuredroads,
structured roads,obstacles
obstaclestypically
typicallypose
poseaa greater
greater threat in the
the longitudinal
longitudinaldirec‑
di-
tion than
rection thanininthe
thelateral
lateraldirection
direction [36], so so
[36], thisthis
paper
paperuses the the
uses obstacle avoidance
obstacle constraint
avoidance con-
mentioned
straint in Section
mentioned 3.1 for the
in Section 3.1 obstacle model. However,
for the obstacle when driving
model. However, when ondriving
a curvedonroad,
a
the extended
curved road, theelliptical
extended boundary of boundary
elliptical the obstacle ofcannot be deflected
the obstacle cannotbybethe curvature
deflected byof the
the
road, so it
curvature ofwill
the affect
road, theso ittrajectory
will affectofthe
the trajectory
main vehicle in the
of the lane,
main resulting
vehicle in lane,
in the unreasonable
result-
planning and even local minimums, as depicted in Figure 18a.
ing in unreasonable planning and even local minimums, as depicted in Figure 18a.

(a) (b)
Figure
Figure18.18.
AA comparison
comparison diagram ofof
diagram elliptical groove
elliptical treatment:
groove treatment:(a)(a)
a schematic diagram
a schematic ofof
diagram the lim-
the limi‑
itations of the repulsive domain of elliptical obstacles; (b) a schematic diagram of an elliptical
tations of the repulsive domain of elliptical obstacles; (b) a schematic diagram of an elliptical groove.
groove.
To address this issue, the extended ellipse boundary is processed by an elliptical
groove by considering the curvature of the road. The center of curvature of the road is
taken as the center of the two long boundaries of the elliptical groove, and the bending
degree of the two boundaries of the elliptical groove is consistent with the bending degree
of the lane line, and then the two long boundaries are closed and connected with two small
arcs. Finally, an elliptical groove is formed, as depicted in Figure 18b.

5. Fusion Algorithm
5.1. Analysis of the Limitation of the Algorithm
In addition to the local minimum situation encountered by the artificial potential field
method described above, the target is unattainable and there is the problem of facing U‑
shaped obstacles. There are also algorithmic limitations caused by road conditions.
As the distance between the vehicle and the target point increases, so does the gravi‑
tation in the potential field. Affected by the road conditions, when driving on the turning
road, A is the starting point and C is the target point. The distance AC between the main
car A and the target point C is less than the distance BC between the point B on the way
and the target point C. Therefore, the potential field of point B will be larger than that of
point A, and it is impossible to travel from the low potential field to the high potential field
in the APF potential field, so there will be a local minimum, as illustrated in Figure 19.
 road, A is the starting point and C is the target point. The distance AC between the main
car A and the target point C is less than the distance BC between the point B on the way
and the target point C. Therefore, the potential field of point B will be larger than that of
point A, and it is impossible to travel from the low potential field to the high potential
Sensors 2024, 24, 3899 field in the APF potential field, so there will be a local minimum, as illustrated in Figure
19 of 31
19.

Figure 19.
Figure 19. A
A schematic
schematic diagram
diagram of
of the
the limitations
limitations of
of the
the turning
turning road.
road.

In the actual
In actual driving
drivingscene,
scene,thetheenvironment
environmentis isusually
usuallypartially known,
partially known, butbut
there are
there
often
are unknown
often unknown obstacles. Therefore,
obstacles. pathpath
Therefore, planning needs
planning to consider
needs global
to consider andand
global locallocal
fac-
tors. Global
factors. path
Global planning
path planningalgorithms
algorithms can
canusually
usuallyfind
findfeasible
feasiblepaths
pathsininthe
the whole
whole envi-
envi‑
ronment but may not be able to deal with environmental changes and unknown obstacles.
ronment
The local
The local path
path planning
planning algorithm
algorithm can
can usually
usually quickly
quickly respond
respond to environmental
environmental changes
changes
and obstacle movement, but it only considers local factors, is likely to encounter
and obstacle movement, but it only considers local factors, is likely to encounter local opti‑ local op-
timization,and
mization, andsosoon.
on.Therefore,
Therefore,asasaalocal
localpath
pathplanning
planningalgorithm,
algorithm, the
the artificial
artificial potential
potential
field
field method often falls into the local minimum point in a complex environment and can- can‑
not
not reach
reach the
the target
target point
point smoothly
smoothly duedue to
to insufficient
insufficient global
globalinformation.
information. As a global
global
algorithm,
algorithm, the RRT* algorithm is effective
effective in static path planning when the road environ- environ‑
ment
ment information
information is is known.
known. However, it lacks real‑time
real-time obstacle avoidance capabilities
capabilities
when
when confronted
confronted with
with unknown
unknown obstacles.
obstacles.

5.2. Algorithm Fusion

5.2. Algorithm Fusion Strategy
Strategy
To
To simultaneously accomplish
simultaneously accomplish global
global path
path optimization
optimization and local avoidance
and local avoidance of
of un-
un‑
known obstacles, this paper proposes a hybrid path planning approach integrating
known obstacles, this paper proposes a hybrid path planning approach integrating en- en‑
hancements to the RRT* algorithm and the artificial potential field method.
hancements to the RRT* algorithm and the artificial potential field method. The RRT* The RRT* al‑
gorithm is improved by using the strategies of probability sampling optimization, adaptive
step size, and path post‑processing, and the artificial potential field concept is introduced
to expedite the discovery of the global path within the road environment. The artificial po‑
tential field method is a local obstacle avoidance algorithm for the real‑time generation of
obstacle avoidance paths that realizes path planning according to the motion state and en‑
vironment information of the controlled object. Therefore, combining the improved RRT*
algorithm and the enhanced artificial potential field approach suggests a fusion algorithm,
which can combine the benefits of both global and local path planning algorithms. Be‑
fore applying the artificial potential field method to dynamic path planning, the improved
RRT* algorithm is used for the global path planning of static known environment roads,
and the global path node sequence is extracted.
The initial and final points in the node sequence also serve as the starting and ending
points for vehicle path planning. Make each node of the node sequence a temporary tar‑
get point, and when the vehicle is driving along the path, the enhanced artificial potential
field method is used to guide the vehicle by taking each node in the node sequence as the
target point in turn. Once the vehicle reaches a node, the subsequent node is designated
as a temporary target point until the vehicle reaches the global target point. When un‑
known obstacles are encountered in the driving process, the enhanced artificial potential
field method is employed to locally plan the path, thereby accomplishing obstacle avoid‑
ance. Upon encountering a local minimum value, the improved RRT* global path is au‑
tomatically re‑planned, or the virtual target point is introduced to update the temporary
Sensors 2024, 24, 3899 20 of 31

target point position and direct the vehicle away from the local minimum region, thus
solving the local minimum problem of the artificial potential field method and enhancing
Sensors 2024, 24, x FOR PEER REVIEW 22 of 33
the vehicle’s obstacle avoidance performance. The final fusion algorithm’s workflow is
depicted in Figure 20.

Start

Improve RRT* global path planning

Extract the sequence of global path

nodes as temporar y target points

N
Wh ether th e APF target poin t is a
temp orary target poin t

Drive in the direction of temporary

target point

N
Do you encounter unknown obstacles?

Improved A PF algorithm for local path

planning

Y
Have you avoided the obstacles?

Replan the global path and update

the temporary target point

Y
Is it successful in avoiding obstacles?

Introducing virtual target points to

guide vehicles to avoid obstacles

Continue to follow the tempor ary

target point

N
Does it reach the finish line？

End

Figure
Figure 20.
20. Fusion
Fusion algorithm
algorithm flow.
flow.

Path Smoothing
5.3. Path Smoothing Strategy
Strategy
After the above
After above path
path optimization,
optimization, the path produced by the
the RRT*
RRT* algorithm
algorithm exhibits
exhibits
a turning angle, and the turning angle is too large, which is not suitable for vehicles. The
The
Bezier curve only needs a few control points to generate a more complex, smooth curve
[37]. The smooth processing of the path is realized by fitting the points on the path using
the n-order Bezier curve. The vehicle can run smoothly.
Given points P0, P1, … Pn, the nth-order Bezier curve is as follows:
Sensors 2024, 24, 3899 21 of 31

Bezier curve only needs a few control points to generate a more complex, smooth curve [37].
The smooth processing of the path is realized by fitting the points on the path using the
n‑order Bezier curve. The vehicle can run smoothly.
Given points P0 , P1 , … Pn , the nth‑order Bezier curve is as follows:
n
P(t) = ∑ Pi BiN (t), t ∈ [0, 1] (41)
i =1

where Pi is the vertex coordinate, BiN (t) represents the n‑degree Bernstein polynomial, and
its basis function is as follows:
n  
n
BiN (t) = ∑ (1 − t ) n −i t i (42)
i =0
i
 
n
where = i!(nn!−i)! ; Formula (42) represents the n‑th order formula of the Bezier curve
i
determined by point P0 P1 . . . Pn ; P0 , and Pn are the starting point and ending point, respec‑
tively; and P1 . . . Pn−1 is called the intermediate control point. Each Bezier curve is defined
by a set of intermediate control points. In order to make the connection of a multi‑segment
curve smooth, the end control point and the first end control point of the adjacent segment
should be selected so that the tangent direction and curvature of the connection point are
continuous and a smooth path can be generated [38].

6. Simulation Analysis
In order to verify whether the improved RRT* algorithm and the fusion algorithm can
search the target point and generate the path more effectively at the starting point of the
space, the simulation experiments of the improved RRT* algorithm and the fusion algo‑
rithm are carried out on the turning road. In order to further verify the performance of the
fusion algorithm, the simulation experiments are carried out in a more complex S‑bend.
The computer is configured with a Windows 11 operating system, an Intel (R) Core (TM)
i5‑8300H CPU in Dell, Texas, USA, a main frequency of 2.30 GHz, and a running memory
of 16.0 GB. The simulation environment is built in the MATLABR2022b environment, and
the simulation experiments of the improved RRT* algorithm and fusion algorithm are re‑
alized. In order to compare the experimental results, the simulation parameters of each
algorithm are unified. The following are the simulation experiment condition settings and
parameter selections:
The size of the simulation map in turn is 15 × 16. The starting node is (0, 0), and
the target node is (9, 0). The size of the simulation map in the S‑bend is 26 × 32. The
starting node is (0, 0), and the target node is (21, 0). The white dotted line and the white
solid line are the lanes that can be crossed by vehicles and the lanes that cannot be crossed
by vehicles. The grey road area is represented as a safe area, and the black boundary on
both sides means that vehicles are not allowed to touch the road boundary. The black
rectangular object and the black U‑shaped object are unknown obstacles and U‑shaped
unknown obstacles, respectively, and the red dotted line around the rectangular obstacle
is the influence area of the obstacle. The initial step size, the initial target bias probability,
the gravitational gain coefficient, the repulsion gain coefficient, the repulsive force range,
the maximum number of nodes, and the maximum number of iterations are 0.5, 1.05, 1.15,
and 1.25. The maximum number of nodes is 3000, and the maximum number of iterations
is 7000. The position of the starting point is represented by a blue solid dot, while the
position of the target point is represented by a red solid dot. Considering the randomness
of RRT series algorithms, 50 simulation experiments are carried out on each algorithm,
and the effectiveness of the improved algorithm is verified by comparing and analyzing
the four performance indexes, namely, the average path length, the average number of
nodes, the average simulation time, and the average number of iterations.
Sensors 2024, 24, 3899 22 of 31

6.1. Simulation Analysis of Improved RRT* Algorithm

This simulation experiment is divided into Experiments 1 and 2. Experiment 1 is the
simulation experiment of the improved RRT* algorithm before path post‑processing, and
Experiment 2 is the simulation experiment of path post‑processing.

6.1.1. Experiment 1

Sensors 2024, 24, x FOR PEER REVIEW

To evaluate the effectiveness of the improved RRT* algorithm proposed in this24 paper,
of 33
an analysis was conducted. Simulation experiments were conducted on the conventional
RRT algorithm, the RRT* algorithm, and the proposed improved algorithm outlined in
this paper. Among them, the traditional improved RRT algorithm, which introduces the
conceptThis ofsimulation experiment
potential fields, is usedistodivided
compare into Experiments
with the improved 1 and 2. Experiment
algorithm in this 1paper,
is the
simulation
which experiment
is commonly usedofinthe
theimproved
improvement RRT* algorithm
method of thebefore path post-processing,
RRT algorithm. The above fourand
Experiment
RRT algorithms2 is are
the simulated,
simulationrespectively,
experiment of path
and thepost-processing.
average path length, average number
of nodes, average simulation time, and average number of iterations are compared and
6.1.1. Experiment
analyzed. 1 confirm the efficacy of the enhanced algorithm, the improved algo‑
To further
rithmTo is evaluate
simulated theand comparedof
effectiveness with
the the Dijkstra
improved algorithm,
RRT* algorithm theproposed
ant colony in algorithm,
this paper,
the A* algorithm,
an analysis and the improved
was conducted. Simulation A*experiments
algorithm, and werethe four performance
conducted indicators
on the conventional
of each
RRT algorithmthe
algorithm, areRRT*
analyzed. Figureand
algorithm, 21 below displaysimproved
the proposed the simulation outcomes
algorithm of the
outlined in
aforementioned
this paper. Among fourthem,
algorithms. The solid
the traditional blue lineRRT
improved is the search path,
algorithm, whichandintroduces
the red solidthe
line is the
concept offinal search
potential path.is Table
fields, used to3 shows
compare thewith
comparison
the improvedof thealgorithm
experimental
in thisdata for
paper,
the average path length, average number of nodes, average simulation
which is commonly used in the improvement method of the RRT algorithm. The above time, and average
number
four RRT of algorithms
iterations ofarethesimulated,
four algorithms.
respectively, and the average path length, average
number of nodes, average simulation time, and average number of iterations are com-
Table
pared3.andComparative
analyzed. analysis table of
To further performance
confirm indexes of
the efficacy of RRT algorithm. algorithm, the im-
the enhanced
proved algorithm is simulated and compared with the Dijkstra algorithm, the ant colony
Average Path Average Number of Average Simulation Average
Algorithm algorithm, the Length
A* algorithm, and the improved A* algorithm,
Nodes Time (s)
and the fourIterations
performance
indicators of each algorithm are analyzed. Figure 21 below displays the simulation out-
RRT algorithm 37.92 81 29.61 6150
comes of the aforementioned four algorithms. The solid blue line is the search path, and
RRT* algorithm 34.05 42 20.09 5590
the red solid line33.89
Traditional improved RRT algorithm is the final search path. 47 Table 3 shows the comparison
2.24 of the experimental
575
data for the average
This paper improves the algorithm. 32.47 path length, average 12 number of nodes,1.21 average simulation279 time, and
average number of iterations of the four algorithms.

(a) (b)

Figure 21. Cont.

 2024, 24,
Sensors 2024, 24, 3899
x FOR PEER REVIEW 2523of
of 33
31

(c) (d)
Figure 21. Comparison diagram of improved RRT* algorithm: (a) simulation path of traditional RRT
Figure 21. Comparison diagram of improved RRT* algorithm: (a) simulation path of traditional
algorithm; (b) RRT* algorithm simulation path; (c) simulation path of traditional improved RRT
RRT algorithm;
algorithm; (b)paper
(d) this RRT*improves
algorithm simulation
RRT* path;
algorithm (c) simulation
simulation path. path of traditional improved
RRT algorithm; (d) this paper improves RRT* algorithm simulation path.
Compared to the traditional RRT algorithm, the improved RRT* algorithm presented
Compared to the traditional RRT algorithm, the improved RRT* algorithm presented
in this paper reduces the average path length by 14.37%, 4.64%, and 4.19% for the RRT*
in this paper reduces the average path length by 14.37%, 4.64%, and 4.19% for the RRT* al‑
algorithm and the traditional improved RRT algorithm. The reduction in path length is
gorithm and the traditional improved RRT algorithm. The reduction in path length is not
not obvious. This occurs due to the excessive randomness inherent in the RRT algorithm,
obvious. This occurs due to the excessive randomness inherent in the RRT algorithm, and
and there will still be redundant nodes. Moreover, the algorithm enhanced in this paper
there will still be redundant nodes. Moreover, the algorithm enhanced in this paper has
has yet
not notundergone
yet undergone the post-processing
the post‑processing stage stage soin
so far far insimulation
the the simulation experiment.
experiment. How-
However,
ever, in terms of the average number of nodes, compared with
in terms of the average number of nodes, compared with the RRT algorithm, the RRT* algo‑ the RRT algorithm, the
RRT* algorithm, and the traditional enhanced RRT algorithm,
rithm, and the traditional enhanced RRT algorithm, they were reduced by 85.19%, 71.43%, they were reduced by
85.19%,
and 71.43%,
77.47%, and 77.47%,
respectively. Thisrespectively.
suggests that This
thesuggests
enhanced that the algorithm
RRT* enhanced significantly
RRT* algorithm im‑
significantly impacts the optimization strategy for redundant
pacts the optimization strategy for redundant path branches, greatly reducing the path branches, greatly
number re-
ducing the number of redundant nodes. In terms of average
of redundant nodes. In terms of average simulation time, the average simulation time issimulation time, the average
simulation
reduced by time is reduced
95.91%, 93.98%, by and95.91%,
45.98%,93.98%, and 45.98%,
respectively, with therespectively,
RRT algorithm, with the
the RRT*
RRT al-al‑
gorithm, and the traditional improved RRT algorithm. At the same time, because thesame
gorithm, the RRT* algorithm, and the traditional improved RRT algorithm. At the RRT
time, because
algorithm is a the RRT search
random algorithm is a random
algorithm, it alsosearch
notably algorithm,
diminishesit also
the notably diminishes
path’s randomness
the path’s to
attributed randomness
time cost and attributed
can find tothe
time cost search
global and canpath findmore
the global
quickly. search path of
In terms more
the
quickly. In terms of the average number of iterations, compared
average number of iterations, compared to the RRT algorithm, the RRT* algorithm, and to the RRT algorithm, the
RRT* algorithm, and the traditional improved RRT algorithm,
the traditional improved RRT algorithm, the average number of iterations is reduced by the average number of it-
erations is reduced by 95.46%, 95%, and 51.48%, respectively, and
95.46%, 95%, and 51.48%, respectively, and the average number of iterations is significantly the average number of
iterationsindicating
reduced, is significantly
that the reduced, indicating
path generation thatof
speed thethepath generation
enhanced speedis of
algorithm the the
faster, en-
hanced
time costalgorithm
is lower, is and faster, the time costofisthe
the randomness lower,
RRT and the randomness
algorithm is reduced.ofCombined
the RRT algo-
with
rithm is21d,
Figure reduced.
it can Combined
be seen thatwith whileFigure 21d, ait safe
ensuring can distance
be seen that
at the while
roadensuring
boundary, a safe
the
distance atRRT*
improved the road boundary,
algorithm can the improved
generate RRT*more
the path algorithm canand
quickly generate the path
accurately, and more
the
quickly and
improved RRT* accurately,
algorithm and the improved
significantly reducesRRT*redundant
algorithmnodessignificantly reduces
in the path, redun-
minimizes
dant nodes
lateral in the path,
fluctuations, andminimizes
enhances pathlateral fluctuations,
stability. and enhances
Observation of Figure path stability.
21a–c showsObser-
that it
is obvious
vation that these
of Figure 21a–calgorithms
shows thathave many redundant
it is obvious that thesenodes, havehave
algorithms a large invalid
many search
redundant
range,
nodes, and
haveeven extend
a large invalidto the opposite
search lane.
range, and The performance
even extend to indicator
the opposite comparison
lane. Theofper-
the
four algorithms
formance indicator is presented
comparison in Table
of the3.four algorithms is presented in Table 3.
As depicted in Figure 22, the blue dotted line, purple dashed line, yellow dashed line,
black dashed line, and red solid line are the paths planned by the A* algorithm, the im‑
proved A* algorithm, the ant colony algorithm, the Dijkstra algorithm, and the improved
RRT* algorithm, respectively. it is evident that the improved RRT* algorithm can rapidly
 Traditional improved RRT algorithm 33.89 47 2.24 575
This paper improves the algorithm. 32.47 12 1.21 279

As depicted in Figure 22, the blue dotted line, purple dashed line, yellow dashed line,
Sensors 2024, 24, 3899 black dashed line, and red solid line are the paths planned by the A* algorithm, the 24 ofim-
31
proved A* algorithm, the ant colony algorithm, the Dijkstra algorithm, and the improved
RRT* algorithm, respectively. it is evident that the improved RRT* algorithm can rapidly
plan
planthetheglobal
globalpath
pathcompared
comparedto tothe
theother
otherfour
fouralgorithms
algorithmswhilewhilestill
still maintaining
maintaining aa cer‑ cer-
tain
tainsafety
safetymargin
marginfromfromthetheroad
roadboundary;
boundary;ititdoes
doesnotnotencroach
encroachon onthe
theopposite
oppositelane,lane,and
and
the
thevehicle
vehicletends
tendstowards
towardsthe thecenterline
centerlineofofthe thelane
laneininwhich
whichititisissituated,
situated,and andthetheplanned
planned
path has more accurate nodes, which play an accurate guiding role
path has more accurate nodes, which play an accurate guiding role for the following for the following fusionfu-
algorithm. Among them, the improved A* algorithm takes into
sion algorithm. Among them, the improved A* algorithm takes into account the lane account the lane boundary
constraints. Although the
boundary constraints. Dijkstra algorithm
Although the Dijkstra andalgorithm
the A* algorithm
and thehave a slightly smaller
A* algorithm have a
average
slightlypath length,
smaller they path
average do not take into
length, theyaccount the impact
do not take of the lane
into account boundary
the impact on lane
of the the
path. The planned
boundary path cannot
on the path. avoid driving
The planned path cannotalongavoid
the lane boundary
driving alongand the colliding
lane boundarywith
the
and lane boundary,
colliding withresulting in a risky path.
the lane boundary, Due to
resulting in the influence
a risky path. ofDuethetolevel of pheromone,
the influence of the
the path planned by the ant colony algorithm has some
level of pheromone, the path planned by the ant colony algorithm has somelimitations, such as a return path, a
limitations,
long
suchplanning
as a returntime, many
path, path turning
a long planning points,
time, and
many so path
on. The enhanced
turning points,A* and
algorithm
so on.con‑
The
siders the safe distance from the lane boundary and mitigates the
enhanced A* algorithm considers the safe distance from the lane boundary and mitigatesrisk of driving close to or
along theof
the risk boundary, but does
driving close to ornotalongconsider the opposite
the boundary, lane, not
but does resulting
considerin thetheencroachment
opposite lane,
ofresulting
the opposite
in the encroachment of the opposite lane; thus, the risk path appears. RRT*
lane; thus, the risk path appears. In comparison to the enhanced algo‑
In compar-
rithm outlined in this study, the other four algorithms obviously
ison to the enhanced RRT* algorithm outlined in this study, the other four algorithms show poor ability to deal
ob-
with the turning road. The comparison of performance indicators
viously show poor ability to deal with the turning road. The comparison of performance of the five algorithms is
depicted in Table 4.
indicators of the five algorithms is depicted in Table 4.

Figure 22. The improved algorithm in this paper and the overall effect simulation path of the four
Figure 22. The improved algorithm in this paper and the overall effect simulation path of the
algorithms.
four algorithms.

Table 4. Comparative analysis table of performance indexes of five algorithms.

Average Path Average Number of Average Simulation Average

Algorithm
Length Nodes Time (s) Iterations
Dijkstra algorithm 31.49 7 1.42 313
A* algorithm 31.65 6 1.37 175
Ant colony algorithm 32.93 16 3.47 ‑
Improved A* algorithm 31.78 4 1.15 116
This paper improves the algorithm. 32.47 12 1.21 279

6.1.2. Experiment 2
To evaluate the efficacy of the enhanced RRT* algorithm’s path post‑processing and
the effect of the node pruning strategy and node optimization, the enhanced RRT* algo‑
rithm and the enhanced RRT* algorithm with path post‑processing are both subjected to
simulation experiments within identical scenarios to facilitate comparative analysis.
 This paper improves the algorithm. 32.47 12 1.21 279

6.1.2. Experiment 2
To evaluate the efficacy of the enhanced RRT* algorithm’s path post-processing and
Sensors 2024, 24, 3899 the effect of the node pruning strategy and node optimization, the enhanced RRT* 25 algo-
of 31
rithm and the enhanced RRT* algorithm with path post-processing are both subjected to
simulation experiments within identical scenarios to facilitate comparative analysis.
According
AccordingtotoFigure
Figure23,23,ititisisevident
evidentthatthatthe
theaverage
averagenumber
numberof ofnodes
nodesafter
afterpath
pathpost‑
post-
processing
processingisis25%
25%less
lessthan
thanthat
thatofofthe theoriginal
originalpath,
path,andandthetheaverage
averagepath
pathlength
lengthexhibits
exhibits
aareduction
reductionof of 18.32%
18.32% compared
compared to the original path. path. There
Thereare arefewer
fewerredundant
redundantnodesnodesin
inthe
theoptimized
optimizedpath,
path,and
andthe
thecorner
cornerof of the
the original
original path becomes smoother in in the
the bend,
bend,
which adheres more closely to the vehicle’s kinematic constraints, and
which adheres more closely to the vehicle’s kinematic constraints, and the updated node the updated node
isismore
more accurately
accurately close towards the the centerline
centerlineof ofthe
thelane,
lane,which
whichlays
lays a better
a better founda‑
foundation
tion for the
for the future
future fusion
fusion algorithm.
algorithm. TheThe results
results indicate
indicate thatthat
the the
path path generated
generated by bythethe
en-
enhanced
hanced RRT*RRT*algorithm
algorithmwithwith path
path post‑processing
post-processing is is superior,
superior, which
which is more
is more suitable
suitable for
for guiding
guiding vehicles
vehicles ininthe
thefuture
futurefusion
fusionalgorithm
algorithmand and directly
directly verifies
verifies the
the effectiveness
effectivenessofof
path
pathpost‑processing.
post-processing.The comparative
The comparative analysis tabletable
analysis of pathof post‑processing performance
path post-processing perfor-
indicators is depicted in Table 5.
mance indicators is depicted in Table 5.

Figure 23. Path post-processing simulation path.

Figure 23. Path post‑processing simulation path.
Table5.5.Comparative
Table Comparativeanalysis
analysistable
tableofofperformance
performanceindex
indexofofpath
pathpost‑processing.
post-processing.

Average Path Average Number of

Algorithm Average Path Average Number of
Algorithm Length Nodes
Length Nodes
This paper improves the algorithm. 32.47 12
This paper improves the algorithm. 32.47 12
An improved algorithm after node optimization 26.52 9
An improved algorithm after node optimization 26.52 9

6.2. Simulation Analysis of Fusion Algorithm

6.2. Simulation Analysis of Fusion Algorithm
In order to verify the effectiveness of the improved RRT* and improved APF fusion
In order to verify the effectiveness of the improved RRT* and improved APF fusion
algorithms, the road environments of turning back and S turning are established, respec-
algorithms, the road environments of turning back and S turning are established, respec‑
tively, to verify the passing ability of the fusion algorithm for turning back and S turning
tively, to verify the passing ability of the fusion algorithm for turning back and S turning
and to set up sudden, unknown obstacles at different positions on the road. Further anal‑
ysis of the fusion algorithm ensures that it can pass through the turn while avoiding un‑
known obstacles smoothly. Finally, a path‑smoothing strategy is introduced to the planned
path. With or without unknown obstacles and unknown obstacles in straight roads or
bends and road scenes with complex obstacles, the passability and obstacle avoidance abil‑
ity of the fusion algorithm are analyzed and verified. Figure 24 show the simulation results
of experiments under different obstacles. Among them, the blue solid line, black point, and
red solid line are the paths planned by the improved RRT* algorithm, the fusion algorithm,
and the smooth fusion algorithm, respectively.
 not be disturbed by obstacles, and the vehicle can navigate the road smoothly. To further
validate the efficacy of the fusion algorithm, all obstacles are put on the same turning road.
As shown in Figure 24f, the fusion algorithm can avoid unknown obstacles in complex
scenes and satisfy collision constraints. The two U-shaped obstacles are also reasonably
Sensors 2024, 24, 3899 avoided. When reaching near the virtual target point, it can re-plan a global path and suc-26 of 31
cessfully guide to the target point. Simultaneously, the path planned by the above exper-
iment is smoothed to meet the requirements of vehicle tracking.

Sensors 2024, 24, x FOR PEER REVIEW 29 of 33

(a) (b)

(c) (d)

(e) (f)
Figure 24. Simulation analysis of fusion algorithm: (a) simulation path of fusion algorithm without
Figure 24. Simulation
obstacle; analysis
(b) simulation path ofofobstacle
fusion algorithm: (a) simulation
fusion algorithm on straightpath
road;of(c)fusion algorithm
limited simulationwithout
obstacle;
path of(b)
safesimulation path of in
ellipse of obstacles obstacle fusion
bends; (d) algorithm
simulation pathon straight
of fusion road; (c)
algorithm forlimited simulation
obstacles in
bends;
path (e) simulation
of safe ellipse of path of road
obstacles infusion
bends;algorithm with U-shaped
(d) simulation path of obstacles; (f) simulation
fusion algorithm forpath of
obstacles in
road fusion algorithm with complex obstacles.
bends; (e) simulation path of road fusion algorithm with U‑shaped obstacles; (f) simulation path of
road fusion algorithm with complex obstacles.
To further corroborate the effectiveness and robustness of the fusion algorithm, the
road scene is changed from a single U-turn to a complex S-bend. The simulation results
from Figure 25 show that no matter the barrier-free S-bend road, the simple obstacle S-
bend road, or the complex obstacle S-bend road, the fusion algorithm can still safely and
reasonably avoid random obstacles and reach the target point. In Figure 25c, when the last
obstacle is planned by the fusion algorithm, the improved RRT* is re-planned twice. In
Sensors 2024, 24, 3899 27 of 31

Through the above five groups of simulation experiments, from Figure 24a, it is ev‑
ident that when utilizing the fusion algorithm, the vehicle can navigate the curved road
more smoothly than with the traditional APF algorithm. On the road without unknown
obstacles, both the enhanced RRT* algorithm and the fusion algorithm can generate a path
from the initial point to the final destination. However, as can be seen from Figure 24b,e,
after the unknown obstacles appear on the road, the enhanced RRT* algorithm by itself can‑
not adequately evade abrupt unknown obstacles, while the fusion algorithm can evade the
unknown obstacles and satisfy the ellipse collision constraints. As depicted in Figure 24e,
when facing the first U‑shaped obstacle, the vehicle is trapped in the local optimal situation.
Through the action of the fusion algorithm, a virtual target point is introduced in proxim‑
ity to the obstacle, and after guiding the vehicle to near the virtual target point, the global
path re‑planning is realized, and the temporary target point is updated to keep the vehicle
going. For the second U‑shaped obstacle, it is because the global path is re‑planned. By
making a detour, the vehicle can be guided to avoid it; additionally, the fusion algorithm
demonstrates robustness in obstacle avoidance. As can be seen from Figure 24c,d, before
the elliptical slot processing is introduced into the algorithm, the obstacles have some lim‑
itations in the curve, which could impact the driving of the main vehicle and make the
planned path turn unnecessarily. It even makes the main car fall into the local optimal
situation. After the introduction of elliptical slot processing, the path will not be disturbed
by obstacles, and the vehicle can navigate the road smoothly. To further validate the effi‑
cacy of the fusion algorithm, all obstacles are put on the same turning road. As shown in
Figure 24f, the fusion algorithm can avoid unknown obstacles in complex scenes and sat‑
isfy collision constraints. The two U‑shaped obstacles are also reasonably avoided. When
reaching near the virtual target point, it can re‑plan a global path and successfully guide to
the target point. Simultaneously, the path planned by the above experiment is smoothed
to meet the requirements of vehicle tracking.
To further corroborate the effectiveness and robustness of the fusion algorithm, the
road scene is changed from a single U‑turn to a complex S‑bend. The simulation results
from Figure 25 show that no matter the barrier‑free S‑bend road, the simple obstacle S‑
bend road, or the complex obstacle S‑bend road, the fusion algorithm can still safely and
reasonably avoid random obstacles and reach the target point. In Figure 25c, when the last
obstacle is planned by the fusion algorithm, the improved RRT* is re‑planned twice. In
the first re‑planning, the path node falls within the scope of the collision constraint, so it is
eliminated. After the second re‑planning, the node is outside the constraint range, and the
vehicle is successfully guided to avoid the obstacle. At the same time, the above S‑bend
road simulation experiments ensure the smoothness of the planned path to fulfil the ve‑
hicle’s tracking requirements. All of the aforementioned experimental results underscore
the indispensability and superiority of the fusion algorithm.
Sensors
Sensors 2024,
2024, 24,
24, 3899
x FOR PEER REVIEW 28 of
30 of 31
33

(a) (b)

(c)
Figure 25. Simulation analysis of fusion algorithm: (a) simulation path of S-bend obstacle-free scene
Figure
fusion 25. Simulation
algorithm; analysis of path
(b) simulation fusion
ofalgorithm: (a) simulation
S-bend simple path of
obstacle scene S‑bend
fusion obstacle‑free
algorithm; scene
(c) simula-
fusion algorithm; (b) simulation path of S‑bend simple obstacle
tion path of S-bend complex obstacle scene fusion algorithm. scene fusion algorithm; (c) simulation
path of S‑bend complex obstacle scene fusion algorithm.
7. Conclusions
7. Conclusions
For the purpose of solving the path planning problem of highlighting unknown ob-
For the purpose of solving the path planning problem of highlighting unknown ob‑
stacles on the structured road, this paper takes the improved RRT* algorithm and the im-
stacles on the structured road, this paper takes the improved RRT* algorithm and the im‑
proved artificial potential field method as the main body of research, examines the path
proved artificial potential field method as the main body of research, examines the path
planning algorithm, improves the global path search efficiency and path quality of the
Sensors 2024, 24, 3899 29 of 31

planning algorithm, improves the global path search efficiency and path quality of the
RRT* algorithm, and solves the limitations of the artificial potential field method of local
path planning. Combined with the two algorithms, a fusion algorithm is devised for global
dynamic scene path planning.
(1) Targeting the deficiencies of the RRT* algorithm, such as strong randomness, slow
convergence speed, poor path feasibility, and poor ability to deal with corners, the
RRT* algorithm is improved, and the concepts of artificial potential field and proba‑
bilistic sampling optimization are introduced to make RRT* node sampling more pur‑
poseful, probabilistic sampling applicability stronger, and sampling efficiency better.
Considering the constraint imposed by the fixed step size, the adaptive step size is
designed according to the road curvature to solve the problem that oscillation may
occur near the target point, improve the adaptability to each road scene, and achieve
rapid convergence towards the target point accurately. The path planned by the im‑
proved RRT* is post‑processed to minimize the number of redundant nodes along
the path and optimize the global path quality.
(2) In view of the problems that the artificial potential field method is prone to local op‑
timization, the target is unreachable, and it is not suitable for the global scene, an
enhanced artificial potential field method is introduced, which adds obstacle avoid‑
ance constraints to obstacles and formulates a road boundary repulsion potential field
to delineate the risk boundaries within the road space. The distance factor is added
to the repulsion function to solve the problem of target unreachability. In the face of
U‑shaped obstacles, virtual gravity points are introduced to solve the local minimum
problem and improve the performance of obstacle avoidance. For the case where the
obstacle is located in the bend, the safety ellipse of the obstacle is treated with an
elliptical slot to reduce unnecessary steering on the planned path.
(3) Aiming at the emerging unknown obstacles, complex obstacles, and other road scenes,
a fusion algorithm of improved RRT* and an improved artificial potential field method
is proposed. The enhanced RRT* algorithm is employed for generating the global
path, and the nodes of the global path are extracted to serve as temporary target
points for the artificial potential field method, which guides the vehicle to drive, re‑
duces the occurrence of the local minimum, uses the artificial potential field method
to avoid the local path when encountered with unknown obstacles, and carries on the
path smoothing processing to the planned path to satisfy the vehicle’s driving. The
simulation results in different scenarios demonstrate that the fusion algorithm can
successfully plan a smoother and more feasible path and verify its effectiveness and
robustness.
Currently, this study has its limitations. In our future work, we will consider adding
the fusion algorithm to decision making in more complex road environments, such as cross‑
roads, and analyze its path‑planning effect.

Author Contributions: Conceptualization, X.L. and G.L.; methodology, X.L. and Z.B.; software, Z.B.
and X.L.; validation, X.L., G.L., and Z.B.; formal analysis, X.L.; investigation, Z.B.; resources, G.L. and
Z.B.; data curation, X.L. and Z.B.; writing—original draft preparation, X.L.; writing—review and edit‑
ing, X.L. and G.L.; visualization, X.L. and Z.B.; supervision, G.L.; project administration, G.L.; fund‑
ing acquisition, G.L. All authors have read and agreed to the published version of the manuscript.
Funding: This work was supported by key research projects of China Liaoning Provincial Depart‑
ment of Education (JYTZD2023081), China Liaoning Provincial Natural Fund Grant Program Project
(2022‑MS‑376) and Key Projects of China National Natural Science Foundation Joint Fund (U22A2043).
Institutional Review Board Statement: Not applicable.
Informed Consent Statement: Not applicable.
Data Availability Statement: The data used to support the findings of this study are available from
the corresponding author upon request.
Sensors 2024, 24, 3899 30 of 31

Conflicts of Interest: The authors declare no conflicts of interest.

References
1. Achiri, A.T.; Mbue, I.N.; Merlin, A.; Gerard, A. Automobile Crash Investigation Based on Vehicle System Related Causes: Sys‑
tematic Literature Review. World J. Eng. Technol. 2022, 10, 139–157. [CrossRef]
2. Gaba, S.; Pawar, S.; Chinta, P.P.; Sweta, S. A computational model of deep learning in self‑driving car. AIP Conf. Proc. 2023,
2755, 020019. [CrossRef]
3. Lin, S.‑L.; Li, X.‑Q.; Wu, J.‑Y.; Lin, B.‑C. Research on overtaking path planning of autonomous vehicles. In Proceedings of the 2021
IEEE International Future Energy Electronics Conference (IFEEC), Taipei, Taiwan, 16–19 November 2021; pp. 1–4. [CrossRef]
4. Reda, M.; Onsy, A.; Haikal, A.Y.; Ghanbari, A. Path planning algorithms in the autonomous driving system: A comprehensive
review. Robot. Auton. Syst. 2024, 174, 104630. [CrossRef]
5. Li, X. Path planning of intelligent mobile robot based on Dijkstra algorithm. J. Phys. Conf. Ser. 2021, 2083, 042034. [CrossRef]
6. He, Z.; Liu, C.; Chu, X.; Negenborn, R.R.; Wu, Q. Dynamic anti‑collision A‑star algorithm for multi‑ship encounter situations.
Appl. Ocean. Res. 2022, 118, 102995. [CrossRef]
7. Li, Q.; Xu, Y.; Bu, S.; Yang, J. Smart Vehicle Path Planning Based on Modified PRM Algorithm. Sensors 2022, 22, 6581. [CrossRef]
[PubMed]
8. Huang, G.; Ma, Q. Research on Path Planning Algorithm of Autonomous Vehicles Based on Improved RRT Algorithm. Int. J.
Intell. Transp. Syst. Res. 2021, 20, 170–180. [CrossRef]
9. Karaman, S.; Frazzoli, E. Sampling‑based algorithms for optimal motion planning. Int. J. Robot. Res. 2011, 30, 846–894. [CrossRef]
10. Cong, J.; Hu, J.; Wang, Y.; He, Z.; Han, L.; Su, M. FF‑RRT*: A sampling‑improved path planning algorithm for mobile robots
against concave cavity obstacle. Complex Intell. Syst. 2023, 9, 7249–7267. [CrossRef]
11. Lin, Y.; Zhang, L. An improved Quick Informed‑RRT* algorithm based on hybrid bidirectional search and adaptive adjustment
strategies. Intell. Serv. Robot. 2024. [CrossRef]
12. Gu, X.; Han, M.; Zhang, W.; Xue, G.; Zhang, G.; Han, Y. Intelligent vehicle path planning based on improved artificial potential
field algorithm. In Proceedings of the 2019 International Conference on High Performance Big Data and Intelligent Systems
(HPBD&IS), Shenzhen, China, 9–11 May 2019; pp. 104–109. [CrossRef]
13. Li, Y.; Yang, W.; Zhang, X.; Kang, X.; Li, M. Research on Automatic Driving Trajectory Planning and Tracking Control Based on
Improvement of the Artificial Potential Field Method. Sustainability 2022, 14, 12131. [CrossRef]
14. Xu, X.; Hu, Y.; Zhai, J.M.; Li, L.Z.; Guo, P.S. A novel non‑collision trajectory planning algorithm based on velocity potential field
for robotic manipulator. Int. J. Adv. Robot. Syst. 2018, 15, 1729881418787075. [CrossRef]
15. Zhai, S.; Pei, Y. The Dynamic Path Planning of Autonomous Vehicles on Icy and Snowy Roads Based on an Improved Artificial
Potential Field. Sustainability 2023, 15, 15377. [CrossRef]
16. Huang, S. Path planning based on mixed algorithm of RRT and artificial potential field method. In Proceedings of the 2021
4th International Conference on Intelligent Robotics and Control Engineering (IRCE), Lanzhou, China, 18–20 September 2021;
pp. 149–155. [CrossRef]
17. Zhang, W.; Wang, N.; Wu, W. A hybrid path planning algorithm considering AUV dynamic constraints based on improved A*
algorithm and APF algorithm. Ocean. Eng. 2023, 285, 115333. [CrossRef]
18. Wu, H.; Zhang, Y.; Huang, L.; Zhang, J.; Luan, Z.; Zhao, W.; Chen, F. Research on vehicle obstacle avoidance path planning
based on APF‑PSO. Proc. Inst. Mech. Eng. Part D J. Automob. Eng. 2022, 237, 1391–1405. [CrossRef]
19. Dasiah, N.B.J.a.p.a. Modified RRT* for Path Planning in Autonomous Driving. arXiv 2024, arXiv:arXiv:2402.12129. [CrossRef]
20. Nguyen, K.; Dang, V.T.; Pham, D.D.; Dao, P.N. Formation control scheme with reinforcement learning strategy for a group of
multiple surface vehicles. Int. J. Robust Nonlinear Control. 2023, 34, 2252–2279. [CrossRef]
21. Shi, Y.; Dong, X.; Hua, Y.; Yu, J.; Ren, Z. Distributed output formation tracking control of heterogeneous multi‑agent systems
using reinforcement learning. ISA Trans. 2023, 138, 318–328. [CrossRef] [PubMed]
22. Wei, K.; Ren, B. A Method on Dynamic Path Planning for Robotic Manipulator Autonomous Obstacle Avoidance Based on an
Improved RRT Algorithm. Sensors 2018, 18, 571. [CrossRef]
23. Xin, P.; Wang, X.; Liu, X.; Wang, Y.; Zhai, Z.; Ma, X. Improved Bidirectional RRT* Algorithm for Robot Path Planning. Sensors
2023, 23, 1041. [CrossRef]
24. Dong, M.; Chen, T.; Yang, J. Simulation Research on path Planning of Unmanned vehicle based on improved RRT algorithm.
Comput. Simul. 2019, 36, 96–100.
25. Zhuge, C.; Wang, Q.; Liu, J.; Yao, L. An Improved Q‑RRT* Algorithm Based on Virtual Light. Comput. Syst. Sci. Eng. 2021, 39,
107–119. [CrossRef]
26. Li, W.; Jin, J. Optimal path Convergence method based on artificial potential Field method and heuristic sampling. Comput. Appl.
2021, 41, 2912. [CrossRef]
27. Liu, X.; Cao, L. Research on path Planning of manipulator based on improved RRT* algorithm. J. Sichuan Univ. Sci. Eng. (Nat.
Sci. Ed.) 2024, 37, 61–70. [CrossRef]
28. Zhang, Y.; Gao, F.; Zhao, F. Research on Path Planning and Tracking Control of Autonomous Vehicles Based on Improved RRT*
and PSO‑LQR. Processes 2023, 11, 1841. [CrossRef]
Sensors 2024, 24, 3899 31 of 31

29. Wang, J.; Li, B.; Meng, M.Q.H. Kinematic Constrained Bi‑directional RRT with Efficient Branch Pruning for robot path planning.
Expert Syst. Appl. 2021, 170, 114541. [CrossRef]
30. Guo, Y.; Liu, H.; Fan, X.; Lyu, W.; Chen, H. Research Progress of Path Planning Methods for Autonomous Underwater Vehicle.
Math. Probl. Eng. 2021, 2021, 8847863. [CrossRef]
31. Liu, H. Research on Path Planning of RRT Algorithm Based on APF. Master’s Thesis, Jiangsu University, Zhenjiang, China, 2022.
[CrossRef]
32. Wang, W.; Li, G.; Liu, S.; Yang, Q. Trajectory Planning of a Semi‑Trailer Train Based on Constrained Iterative LQR. Appl. Sci.
2023, 13, 10614. [CrossRef]
33. Hongyu, H.; Chi, Z.; Yuhuan, S.; Bin, Z.; Fei, G.J.I.‑P. An improved artificial potential field model considering vehicle velocity
for autonomous driving. IFAC‑PapersOnLine 2018, 51, 863–867. [CrossRef]
34. Ruan, S. Research on Path Planning of Mobile Robot Based on Improved RRT* and Artificial Potential Field Method; Hangzhou University
of Electronic Science and Technology: Hangzhou, China, 2023. [CrossRef]
35. Xie, C.; Tao, T.; Li, J. Research on path Planning based on improved artificial potential Field method. J. Jilin Univ. (Inf. Sci. Ed.)
2023, 41, 998–1006. [CrossRef]
36. Li, Y.; Jin, R.; Xu, X.; Qian, Y.; Wang, H.; Xu, S.; Wang, Z. A Mobile Robot Path Planning Algorithm Based on Improved A*
Algorithm and Dynamic Window Approach. IEEE Access 2022, 10, 57736–57747. [CrossRef]
37. Lai, R.; Wu, Z.; Liu, X.; Zeng, N. Fusion Algorithm of the Improved A* Algorithm and Segmented Bézier Curves for the Path
Planning of Mobile Robots. Sustainability 2023, 15, 2483. [CrossRef]
38. Li, X.; Zheng, H.; Wang, J.; Xia, Y.; Song, H. A Global Path Planning Method for Unmanned Ground Vehicles in Off‑Road
Scenarios Based on Terrain Data. Soc. Sci. Res. Netw. 2024. [CrossRef]

Disclaimer/Publisher’s Note: The statements, opinions and data contained in all publications are solely those of the individual au‑
thor(s) and contributor(s) and not of MDPI and/or the editor(s). MDPI and/or the editor(s) disclaim responsibility for any injury to
people or property resulting from any ideas, methods, instructions or products referred to in the content.