machines

Article
An Effective Dynamic Path Planning Approach for Mobile
Robots Based on Ant Colony Fusion Dynamic Windows
Liwei Yang * , Lixia Fu *, Ping Li, Jianlin Mao and Ning Guo

School of Information Engineering and Automation, Kunming University of Science and Technology,
Kunming 650093, China; [email protected] (P.L.); [email protected] (J.M.);
[email protected] (N.G.)
* Correspondence: [email protected] (L.Y.); [email protected] (L.F.)

Abstract: To further improve the path planning of the mobile robot in complex dynamic environments,
this paper proposes an enhanced hybrid algorithm by considering the excellent search capability
of the ant colony optimization (ACO) for global paths and the advantages of the dynamic window
approach (DWA) for local obstacle avoidance. Firstly, we establish a new dynamic environment
model based on the motion characteristics of the obstacles. Secondly, we improve the traditional ACO
from the pheromone update and heuristic function and then design a strategy to solve the deadlock
problem. Considering the actual path requirements of the robot, a new path smoothing method is
present. Finally, the robot modeled by DWA obtains navigation information from the global path, and
we enhance its trajectory tracking capability and dynamic obstacle avoidance capability by improving
the evaluation function. The simulation and experimental results show that our algorithm improves
the robot’s navigation capability, search capability, and dynamic obstacle avoidance capability in
unknown and complex dynamic environments.



Keywords: mobile robot; path planning; ant colony optimization; dynamic window approach;


deadlock problem; dynamic obstacle avoidance
Citation: Yang, L.; Fu, L.; Li, P.; Mao,
J.; Guo, N. An Effective Dynamic
Path Planning Approach for Mobile
Robots Based on Ant Colony Fusion
Dynamic Windows. Machines 2022,
1. Introduction
10, 50. https://doi.org/10.3390/ Mobile robots have various applications in various fields, and their autonomous
machines10010050 navigation in ambient space is crucial [1]. When robots have a priori information about
the environment, they can plan a global path from the starting point to the endpoint and
Academic Editor: Dan Zhang
optimize some certain goals, an ability which has received much attention [2]. However,
Received: 7 December 2021 it is difficult for robots to have a priori environmental information, especially about the
Accepted: 7 January 2022 dynamically changing factors, such as climatic conditions [3], unknown obstacles [4], and
Published: 9 January 2022 unfamiliar terrain [5]. The robot needs to detect the surrounding environment in real-time
Publisher’s Note: MDPI stays neutral
and make multiple plans to obtain a feasible, safe path. Good results have been achieved in
with regard to jurisdictional claims in path planning research for solving unknown static environments [6–9], while the unknown
published maps and institutional affil- dynamic factors [3–5,10] constrain the reliable and robust motion of the robot in general
iations. environments and present a significant challenge.
As the complexity of the environment and the difficulty of robot tasks increase, tradi-
tional path planning methods are challenging to achieve the desired results. Ant colony
optimization (ACO) has strong robustness and adaptability for solving global path planning
Copyright: © 2022 by the authors. problems [11]. In recent years, related scholars have proposed many improvement strate-
Licensee MDPI, Basel, Switzerland. gies and methods. Luo et al. [12] introduced optimal and worst solutions in pheromone
This article is an open access article updating to expand the influence of high-quality ants and weaken the power of worst
distributed under the terms and ants, which accelerates the algorithm’s convergence. Dai et al. [13] proposed a smoothing
conditions of the Creative Commons
ACO that optimizes the number of path turns and path length. You et al. [14] designed a
Attribution (CC BY) license (https://
new heuristic operator to improve the diversity and convergence of the population search.
creativecommons.org/licenses/by/
To improve the solution accuracy of the algorithm, Xu et al. [15] proposed a mutually
4.0/).

Machines 2022, 10, 50. https://doi.org/10.3390/machines10010050 https://www.mdpi.com/journal/machines

Machines 2022, 10, 50 2 of 29

collaborative two-layer ant colony algorithm, using the outer ant colony for global search
and the inner ant colony for local search. Ao et al. [16] took the motion characteristics of
the USV into account for smoothing the paths, the continuous functions in orientation and
curvature achieved, which reduced the fuel consumption and space-time overhead of USV
to a certain extent.
In theory, such algorithms ensure an optimal global path obtained, but it may not be of
high practical value due to the unpredictability of local information. Local path planning
based on real-time sensor information is efficient and highly adaptable to local environments,
such as the BUG algorithm [9], DWA [10], the fuzzy logic algorithm [11], and the artificial
potential field algorithm [17]. Due to the lack of global information, it is challenging to
guarantee accessibility and path optimality. Hybrid path planning methods have received
extensive research and attention [6], with global paths as guides and local planning executed
online. The fusion algorithms A*–DWA [7,18,19] and Dijkstra–DWA [20–22] are the most
widely studied hybrid algorithms. To achieve global path tracking and local unknown
obstacle avoidance for the robot in complex and unfamiliar environments, Chi et al. [7]
use the improved A* algorithm to plan the robot’s navigation path and the DWA for local
obstacle avoidance. Some scholars [18] also considered obstacles with motion properties in
the fusion algorithm. The mechanism of Dijkstra-DWA studied by Liu et al. [22] is similar to
Chi et al. [7]. Differently, Liu et al. [22] take the mobile objects interfering robot’s motion
and verify the algorithm’s obstacle avoidance performance in a natural environment. Some
scholars [23,24] have recently fused ACO with DWA for dynamic obstacle avoidance studies.
Shao et al. [23] exploit improved ACO to plan the robot’s path and use DWA for dynamic
obstacle avoidance. Still, the robot uses a priority strategy for obstacle avoidance where
obstacle passage is prioritized. If there are many mobile obstacles in the environment, the
robot’s safety cannot be guaranteed. Jin et al. [24] consider a variety of motion states obstacles
to interfere with the robot’s motion without assuming their volume and size. Although
a good obstacle avoidance effect has been achieved, it is impractical. In addition, the
hybrid algorithm for Rapidly-exploring Random Trees (RRT)–DWA has been investigated
by several scholars [25].
With the rise of artificial intelligence, this type of path planning has received much at-
tention. Chen et al. [26] proposed a bidirectional neural network to solve the path planning
problem in an unknown environment. Wu et al. [27] transformed the path planning task
into an environment classification task, using Convolutional Neural Network (CNN) to
perform path planning. Reinforcement learning is a class of algorithms applied to unknown
environments. As one of the three major branches of machine learning, reinforcement
learning [28] does not need to provide data, unlike supervised and unsupervised learning.
All the learning material will be obtained from the environment. By continuously exploring
the environment and learning the model based on the other feedback generated by various
actions, the intelligence will eventually complete the task in the specified environment
with the optimal strategy. Since V. Mnih et al. [29] proposed Deep Q-Network (DQN),
deep reinforcement learning has continued to make breakthroughs, and some researchers
now try to solve path planning problems by deep reinforcement learning. In the grid
environment, Piotr Mirowski et al. [30] exploit multimodal perceptual information as input
and make decisions by reinforcement learning to accomplish navigation tasks in grid space.
Panov et al. [31] utilize the Neural Q-Learning algorithm to achieve the path planning
task. Lei et al. [32] combined CNN with DDQN to investigate path planning in dynamic
environments. Lv et al. [33] proposed an improved DQN-based learning strategy that
builds an experience-valued evaluation network in the beginning phase and uses a parallel
exploration structure when the path roaming phenomenon occurs. The study also con-
sidered exploring other points than the roaming points to improve the experience pool’s
breadth further.
Robots in unknown and complex dynamic environments require global navigation
and dynamic obstacle avoidance capabilities. This paper proposes an effective ACO hybrid
DWA dynamic path planning algorithm, and the planning ability and dynamic obstacle
 stacle avoidance ability of the hybrid algorithm in complex environments have b
ther enhanced by the following improvements:
• A new dynamic environment construction method is proposed to address th
Machines 2022, 10, 50
practical dynamic factors in current path planning research. 3 of 29
• The robot’s navigation trajectory is planned by the improved ACO, which i
the robot’s adaptability to complex environments in the grid map. We propo
uniform
avoidance abilityinitial pheromone
of the hybrid method
algorithm to avoid
in complex the blindness
environments have beenof the ants’ initi
further
enhanced
andbyimproved
the following improvements:
heuristic functions using corner suppression factors to enh
• A smoothing
new dynamicability
environment
of theconstruction
ants’ paths method is proposed to
of exploration. To address
improvethe lack
theofalgorith
practical dynamic factors in current path planning research.
vergence, we updated the pheromone hierarchically according to the quali
• The robot’s navigation trajectory is planned by the improved ACO, which improves
theants. In adaptability
robot’s addition, the deadlock
to complex problem is
environments solved
in the grid by
map. theWeretraction
propose a mecha
designed. initial
non-uniform Considering
pheromone themethod
actualtopath requirements
avoid the blindness of of the
theants’
robot, we smoot
initial
optimized the paths.
search and improved heuristic functions using corner suppression factors to enhance
• the smoothing ability
Constructing the of the ants’
robot modelpaths of exploration.
based on improved To improve
DWA,theour algorithm’s
primary focus
convergence, we updated the pheromone hierarchically according to the quality of
lize the global path planned by IACO as the robot’s navigation
the ants. In addition, the deadlock problem is solved by the retraction mechanism we
information,
alyze and
designed. improvethe
Considering the robot’s
actual pathsampling
requirementswindow and we
of the robot, evaluation
smoothedfunction
and to
the paththetracking
optimized paths. capability, dynamic obstacle avoidance capability, and mo
• Constructing the
bility. Finally, robot
wemodel
havebased on improved
verified DWA, our primary
the effectiveness of the focus
fusionis toalgorithm
utilize thr
the global path planned by IACO as the robot’s navigation information, then analyze
tensive simulation experiments.
and improve the robot’s sampling window and evaluation function to enhance the
path tracking capability, dynamic obstacle avoidance capability, and motion stability.
2. Environmental ModeltheConstruction
Finally, we have verified effectiveness of the fusion algorithm through extensive
simulation experiments.
Current research on mobile robot path planning is usually based on more id
2.ronments that Model
Environmental lack consideration
Construction of certain dynamic environmental factors. Th
proposes
Current aresearch
new approach to environment
on mobile robot path planningmodel construction
is usually to ideal
based on more further
en- exten
search on dynamic path planning.
vironments that lack consideration of certain dynamic environmental factors. This paper
proposes a new approach to environment model construction to further extend the research
on dynamic
2.1. path planning.
Modelling of the Known Environment
A standard
2.1. Modelling method
of the Known for describing
Environment the working environment of a mobile rob
gridA method, which divides
standard method the environment
for describing into a gridofofa Mrows
the working environment and isNcolum
mobile robot
the grid method, which divides the environment into a grid of Mrows and Ncolumns,
the black grids representing the obstacle areas that make the robot impassable and
with the black grids representing the obstacle areas that make the robot impassable and a
grid representing the areas where the robot can pass. Based on the previous id
white grid representing the areas where the robot can pass. Based on the previous ideal
ronment, this
environment, thispaper
paper will addthe
will add thedynamic
dynamic factor
factor environment
environment shownshown in 1,Figure 1
in Figure
ing unknown
including unknown static
staticobstacle (theorange
obstacle (the orange grid)
grid) and and unknown
unknown dynamic dynamic obstacle (t
obstacle (the
circular mobile obstacle), after the global path planning process has been implemented.
lar mobile obstacle), after the global path planning process has been implemente

Unknown static obstacle

Unknown dynamic obstacle

Figure
Figure 1. The
1. The natural
natural working
working environment
environment of
of the robot. the robot.

2.2. Modeling of the Unknown Environment

The robot’s movement is usually disturbed by other unknown obstacles in
ronmental space. Better research results on unknown environments focus on u
Machines 2021, 9, x FOR PEER REVIEW
Machines 2022, 10, 50 4 of 29

static obstacles [7,8]. In contrast, research on unknown dynamic environm

2.2. Modeling of the Unknown Environment
contains problems such as lack of consideration of moving objects’ speed an
The robot’s movement is usually disturbed by other unknown obstacles in its envi-
and unreasonable
ronmental space. Betterobstacle avoidance
research results mechanisms
on unknown environmentsin focus
the robots
on unknownthemselves
static obstacles
the above [7,8]. Inacontrast,
issues, research onenvironment
new dynamic unknown dynamic environments
modeling [19,24,25] is pr
requirement
contains problems such as lack of consideration of moving objects’ speed and volume size
better approach to the robot’s obstacle avoidance strategy will be presented
and unreasonable obstacle avoidance mechanisms in the robots themselves. Considering
To issues,
the above facilitate
a newthe expansion
dynamic of themodeling
environment volumerequirement
size of theis mobile
proposed,obstacles,
and a w
better approach to the robot’s obstacle avoidance strategy will be presented
a circular obstacle instead of a square obstacle [23] limited by the grid met following.
To facilitate the expansion of the volume size of the mobile obstacles, we will consider
a red threat circle with a size of n times the radius 𝑅_𝑜𝑏 of the mobile obsta
a circular obstacle instead of a square obstacle [23] limited by the grid method and build a
significantly
red improve
threat circle with a size ofthe robot’s
n times recognition
the radius R_ob of thearea of obstacle,
mobile blocks which
and enhance
can o
significantly
ance (we improve
verify thewith robot’s
therecognition area of blocks
corresponding and enhance obstacle
experiments). Where: 𝑅_𝑆𝑜𝑏 = 𝑛𝑅
avoidance
lute velocity vector is (𝑣 𝑐𝑜𝑠𝜃，𝑣 𝑠𝑖𝑛𝜃), and 𝜃 is the directional angle of th
(we verify with the corresponding experiments). Where: R_Sob = nR_ob, the absolute
velocity vector is (vb cosθ, vb sinθ ), and θ is the directional angle of the velocity vector vb .
The 𝑣 . The
tor model model
of the mobile of the mobile
obstacle is shown obstacle
in Figure 2.is shown in Figure 2.

Figure 2. Mobile obstacle.

Figure 2. Mobile obstacle.
For the ensuing study, we define the mobile robot and its dynamic environment
For the ensuing study, we define the mobile robot and its dynamic
as follows: en
(1) The starting point, endpoint, and global environmental information are known.
follows:
(2) The mobile robot has sensors that can acquire external information and sense the
(1) actual
The volume
starting point, endpoint, and global environmental information ar
size and speed of movement of the moving obstacle.
(2) The
(3) The mobile
mobile robotrobot
is basedhason sensors
a two-wheelthat can acquire
differential model external information
with the dynamics
constraints considered.
actual volume It has
size anda well-developed power system
speed of movement ofand
thesufficient
moving energy to
obstacle.
accelerate, decelerate, and avoid obstacles.
(3) The mobile robot is based on a two-wheel differential model with the d
(4) Considering the uncontrollable action of mobile barriers, this paper will focus on the
straints
uniform considered.
velocity motion andItsethas a well-developed
its movement power
speed as less than systemlinear
the maximum and suffic
accelerate, decelerate, and avoid obstacles.
speed of the robot.
(5) The route of the mobile obstacles motion can be obtained by the security A* algo-
(4) rithm
Considering the uncontrollable action of mobile barriers, this paper wi
[34], and the dynamic environmental factors are ignored in the planning process.
uniform
Based on this,velocity motion
we will set and set itsmotion
the corresponding movement
speed forspeed as less
the mobile than the ma
obstacles
speed
and obtainofthetheposition
robot.information for each moment in advance for the robot to
collect data and execute the obstacle avoidance strategy.
(5) The route of the mobile obstacles motion can be obtained by the security
[34],
3. Global and
Path the dynamic environmental factors are ignored in the plan
Planning
3.1. Ant
Based on this, we(ACO)
Colony Optimization will set the corresponding motion speed for the mo
Ant colony optimization
and obtain the positionis an efficient heuristic algorithm
information for eachthat uses distributed
moment com- for th
in advance
putation. The ants choose the direction of movement mainly based on the accumulated
lect data and execute the obstacle avoidance strategy.

3. Global Path Planning

3.1. Ant Colony Optimization (ACO)
Machines 2022, 10, 50 5 of 29

pheromones on the path. Assuming that the ant k is located at the node i at time t, the
probability of selecting the next node j is determined by Equation (1).

[τij (t)] a [ηij (t)] β
, j ∈ allowedi


pijk (t) = ∑ [τij (t)] a [ηij (t)] β (1)
 j ∈ allowedi
0, otherwise


ηij (t) = 1/d jE (2)

where allowedi denotes the set of optional adjacent grids of ant k in grid i; τij is the
pheromone concentration; ηij is the heuristic function and d jE denotes the Euclidean dis-
tance from grid j to the endpoint E; and α and β denote the relative importance degree of τij
and ηij , respectively. The ant’s search for the optional path is influenced by the pheromone
update strategy, which is traditional.
(
Q
k
∆τij (t) = Lk , j ∈ allowedi (3)
0, otherwise

m
τij (t + 1) = (1 − ρ)τij (t) + ∑ ∆τijk (t) (4)
k =1

where Q is the pheromone intensity; ∆τijk denotes the pheromone increment of the path;
Lk denotes the total length of the path traveled by the ant k; ρ is the pheromone volatility
factor; and m denotes the total number of ants.

3.2. Improved Ant Colony Optimization (IACO)

3.2.1. Initial Non-Uniform Pheromone
The initial pheromone of the traditional ACO is constant, and the ants tend to move to
the grid closer to the endpoint, which may lead the ants in a wrong direction and then con-
tinue searching. To improve the quality of the ants’ search paths and to prevent them from
falling into dead ends effectively, this paper proposes a method for non-uniformly assign-
ing pheromones based on obstacle density concerning the non-uniform initial pheromone
strategy proposed by Luo et al. [12] based on the distance relationship as follows:

τi,j (0) = C · f (i ) (5)

f (i ) = card( allowedi )/8 (6)

where C is an amplification factor of the feasibility function, which is taken as needed; f (i )
is the feasibility function; and card indicates the number of feasible neighborhood grids
for the grid i, with more selectable grids demonstrating greater path feasibility, more
pheromones are allocated, and conversely, to a lesser extent. In addition, the pheromone
concentration at the map boundary is set to a small constant. Figure 3 shows the ini-
tial pheromone concentration of one map, where C is a constant 10 and the boundary
pheromone concentration is set to 2, the brighter-colored areas indicate higher pheromone
concentrations and more excellent path selectivity.
 borhood grids for the grid 𝑖, with more selectable grids demonstrating greater pa
bility, more pheromones are allocated, and conversely, to a lesser extent. In add
pheromone concentration at the map boundary is set to a small constant. Figure
the initial pheromone concentration of one map, where 𝐶 is a constant 10 and th
Machines 2022, 10, 50 ary pheromone concentration is set to 2, the brighter-colored areas indicate
6 of 29 highe

mone concentrations and more excellent path selectivity.

Figure
Figure 3. Diagram
3. Diagram of initial
of initial pheromone
pheromone allocation.allocation.

3.2.2. Design of Heuristic Functions

3.2.2. Design of Heuristic Functions
The traditional heuristic function is shown in Equation (2), which guides the ant to
explore the path by the distance relationship from the ant’s current position to the endpoint.
The attraction effect becomes more pronounced as the ants get closer to the endpoint.
Conversely, the attraction effect is insignificant for parts far from the endpoint. Ant may
suffer severe deadlocks and produce more redundant turning points due to the lack of
guidance from favorable inspirational information [35]. For this reason, we considered
both the heuristic effects of the starting point, the current node, and the endpoint, and then
we added a corner suppression factor as shown in Equation (8) to enhance the smoothing
ability during ant exploration to reduce redundant nodes. The heuristic function we
designed is shown in Equation (7).

dSj
ηij = · Eturn (7)
dij + d jE

u1 /Lgrid,
√ turn( J ) = turn( J − 1)
Eturn = (8)
u1 / 2 Lgrid, otherwise
where dSj is the distance from the starting point S to the next node j; dij is the distance
from the current node i to the point j; d jE is the distance from the point j to the endpoint E;
Eturn is the corner suppression factor; Lgrid is the side length of the grid, and we default
to 1; u1 is the smoothness weight, taken according to the actual situation; and turn( J ) and
turn( J − 1) are the current and previous directions to be transferred by the ant k in the
eight-neighborhood range, respectively. The most beneficial effect of our heuristic function
is that when turn( J ) is the same as turn( J − 1), it will increase the probability of ant k
choosing this path, thus reducing the number of redundant turning points and improving
the smoothness of the path. To further demonstrate the effectiveness of the improved initial
pheromone strategy and the designed heuristic function, we analyzed it in Section 6.1.1
using first-generation ant populations with no pheromone update disturbance.

3.2.3. Improving Pheromone Updates

Pheromone update refers to accumulation and volatilization, mainly local and global
update processes. This paper improves the pheromone update rule by the optimal-worst
ant system and the elite ant system. After each round of iteration, we will divide the
optimal layer ants, the worst layer ants, and the ordinary layer ants by path length. Then,
perform local pheromone updating based on Equations (10)–(12) to increase the role of
the optimal solution in guiding and weakening the part of the worst solution. We have
Machines 2022, 10, 50 7 of 29

also introduced the enhancement factor of the elite ant of Equation (14) into the global
pheromone update to speed up the algorithm’s convergence further.

∆τ best + ∆τ worst + ∆τ other , j ∈ allowedi


∆τijk (t) = (9)
0, otherwise

Q1
∆τ best = , Q = bQ (10)
Lbest 1
Q2
∆τ worst = , Q2 = Q/w (11)
Lworst
Q3
∆τ other = , Q3 = Q (12)
L
where ∆τ best , ∆τ worst , ∆τ other are the local pheromone increments of the optimal, worst
and other paths in the current iteration, respectively; Lbest , Lworst , L correspond to the
lengths of the three tracks, respectively; b and w are the number of optimal ants and worst
ants in the current iteration, respectively; and Q1 , Q2 , Q3 are the pheromone intensity of
three types of ants, respectively.
AS the decisions of the ants in the current iteration will be influenced by the previous
ants, if the path obtained by the aforementioned method is not optimal, it will mislead
the following ants. Therefore, we select elite ants from within i iterations according to
Equation (14) and introduce an increase factor in the global pheromone update to amplify
the influence of the elite ants, which speeds up the convergence of the algorithm to some
extent. The improved global pheromone update is as follows.
m
τij (t + 1) = (1 − ρ)τij (t) + ∑ ∆τijk (t) + q(t) (13)
k =1
(
NCmax
δ NC · L ∗ (i )
, L∗ (i ) = min( L)
q(t) = (14)
0, other
where m denotes the total number of ants; δ is the influence value of elite ants; NC and
NCmax are the current and maximum number of iterations, respectively; min( L) is the
optimal path length of the current iteration; and L∗ (i ) is the path length of the elite ants
within i iterations.

3.2.4. Improving the Pheromone Volatility Factor

The optimal-worst ant system and the elite ant system speed up the algorithm’s
convergence. To avoid the positive feedback mechanism of the ant colony leading to the
optimum local problem, we improve the pheromone volatility factor ρ to enhance the global
searchability. If there is no new elite ant, the pheromone volatilization factor is adjusted;
otherwise, it is a constant ρ0 . The improvement strategy is as follows:

ρ(t)(1 + NC/NCmax ), L∗ (i ) = min( L)


ρ ( t + 1) = (15)
ρ0 , otherwise

3.2.5. Deadlock Handling Strategy

When the robot’s environment is complex (e.g., an environment with U-shaped ob-
stacles), the presence of the forbidden table mechanism may lead to a deadlock situation
where ants do not have a suitable grid to move. Ant retraction and ant death strategies
are common approaches to solving this problem [36]. However, when the environment
consists of many or large traps, the ant retraction strategy causes the ants to constantly
mark and judge their surroundings, with forwarding and backward situations, resulting in
algorithm inefficiency. Similarly, the ant death strategy causes ants with deadlock to die
 Machines 2022, 10, 50 8 of 29

naturally, which means the pheromone is updated inefficiently and poorly in algorithm
accuracy.
In this paper, a deadlock measure is proposed based on the ant retreat strategy. As
shown in Figure 4, when an ant falls into a deadlocked state at grid D, it is allowed to retreat
to the previous path grid T3 and fill the grid D with a virtual obstacle (the blue grid). If the
ant is still in a deadlocked state, we continue the backtracking and filling operations until
the ant is outside the trap entirely. The beneficial effects of the strategy are (1) ensuring the
diversity of the ant colony and improving the adaptability of the algorithm effectively to
2021, 9, x FOR PEER REVIEW complex environments and (2) using virtual obstacles to fill in the deadlocked region and8 of 28
then prevent subsequent ants from falling into that part again, improving the algorithm’s
solution speed effectively.

T3

T2

S T1

(a) (b)
Figure 4. Deadlock handling
Figure 4. Deadlock strategy
handling diagram;
strategy (a)(a)Ant
diagram; Antdeadlock diagram;
deadlock diagram; (b)(b) Improved
Improved fallback
fallback
strategy. strategy.
3.2.6. Path Smoothing Optimisation
3.2.6. Path Smoothing Optimisation
Paths obtained by both the ACO and A* algorithm consist of path nodes in the
Pathsgrid
obtained by both
center. The paths,the ACO
in this case,and
have A*more
algorithm consist
invalid nodes, ofthe
and path nodes isinalso
curvature the grid
discontinuous, which does not satisfy the actual path requirements of the
center. The paths, in this case, have more invalid nodes, and the curvature is also discon- robot. To this, we
design a new path smoothing method shown in Figure 5, where the initial path consists of
tinuous, which does not satisfy the actual path requirements of the robot. To this, we de-
a concatenation of grid centroids, and the original route is a black dashed line with many
sign a new path smoothing
redundant nodes. We method shown
optimize the tracksin Figure
which 5, where
no longer limitthe initialcenter
the grid’s pathandconsists
take of a
concatenation of grid centroids, and the original route is a black dashed line with many
the safety distance d into account, as follows:
redundant 1. nodes.
IterateWe optimize
through theoftracks
all nodes which
the path. If theno longer
current nodelimit
is onthe
the grid’s center
same line and take
with two
the safety distance 𝑑 into account, as follows:
adjacent nodes, the current node is removed;
2. Iterate through the starting point and the turning point. From the starting point,
each node will be connected to the following turning point as the alternative path
in succession and judge the relationship between the distance di of each path to the
obstacle grid and the safety distance d. If di ≤ d, the alternative path is ignored;
if di > d the redundant nodes of the original route are removed to generate this
optimized path (the blue dashed line). In this paper, the safety distance d for the paths
of the robot and mobile obstacles are 0.707 and 1.414, respectively.
3. The three-time B-sample curve has second-order continuity, which can better satisfy
the continuity of the mobile robot in terms of velocity and acceleration (the kinetic
characteristics are not considered).
(1) Extract the critical nodes as control nodes Pi (i = 0, 1, . . . , n) after removing the
redundant nodes.
(2) According to Equation (16),(a) the segmental B-sample smoothing [37] is per-
Original
formed on the turning point to generate the B-sample
Optimized
di
curve.
(3) To control the B-spline curve to avoid crossing obstacles, Huang et al. [38]
adjusted the curve based on the convexity of the curve wrapping. The B-spline
curve is retained when the barrier is outside the characteristic triangle formed
by the control point Pi . Suppose the obstacle intersects the characteristic
 S T1

(a) (b)
Machines 2022, 10, 50 9 of 29
Figure 4. Deadlock handling strategy diagram; (a) Ant deadlock diagram; (b) Improved fallback
strategy.
triangle, and the B-spline curve crosses the obstacle. In that case, a set of
3.2.6. Path Smoothing Optimisation
triangles with a similarity ratio k is selected within the characteristic triangle
Paths obtained by both
(k from 1 to 0,the
theACO and
scale is A* algorithm
chosen according consist of path nodes
to the experiment). Thisin the grid
triangle
is utilized to generate new control points to determine the shortest
center. The paths, in this case, have more invalid nodes, and the curvature is also discon- B-spline
curve
tinuous, which does notthat will not
satisfy thecross the path
actual obstacle.
requirementsof
  the 
robot. To this, we de-
1 4 1 0 P0
sign a new path smoothing method shown in Figure 5, where the initial path consists of a
1   −3 0 3 0   Pdashed
 1 
concatenation ofC0,3grid
(u) =centroids,
1 u and u2 the
u3 original
 route is a black , 0 ≤ line
u ≤ 1with (16)
many
6  3 −6 3 0  P2  
redundant nodes. We optimize the tracks which −1 no 3 longer
−3 limit
1 the
P3 grid’s center and take
the safety distance 𝑑 into account, as follows:

(a)
Original
Optimized
di

(b) (c)
FigureFigure
5. Schematic diagram
5. Schematic diagram of pathsmoothing
of path smoothing optimisation;
optimisation; (a) Editable
(a) Editable node determi-
node determination; (b)
The path of mobile robot; and (c) The path of mobile obstacle.
nation; (b) The path of mobile robot; and (c) The path of mobile obstacle.
Figure 5a is a schematic diagram of control point selection, in which the characteristic
1. Iterate through all nodes of the path. If the current node is on the same line with two
triangle consists of control points { p0 , p1 , p2 }. There is no collision between the character-
adjacent
istic trianglenodes,
and the current in
the obstacle node is removed;
the left figure, save the curve. In the right figure, there are
2. Iterate through
collisions, and the
the starting
measure point
is to and
generate thenew
turning point.
B-sample Frombased
curves the starting point,
on control each
points
n 8 8
nodep0 , will
p1 , p2be determined
connected by to similar
the following
trianglesturning point as further
the alternative
collisions.path in 5b
suc-
o
until there are no Figure
cession and judge the relationship between the distance 𝑑𝑖 of each
is the mobile robot path planning diagram, where IACO planned the black path, the route path to the obsta-
cle grid and
marked theissafety
in blue distance
optimized 𝑑. If1 𝑑𝑖
by steps and≤d, the alternative
2, and the red tracepath is ignored;
is obtained if 𝑑𝑖
by step 3. >
𝑑Figure
the redundant nodes of the original route are removed to generate
5c shows the dynamic obstacle motion path diagram, where the black dashed line is this optimized
path
received(thebyblue dashed A*
the security line). In this[34],
algorithm paper, theblue
and the safety line is𝑑optimized
distance
dashed for the paths of 1the
by steps
and 2.
robot and mobile obstacles are 0.707 and 1.414, respectively.
4. Local Path Planning
4.1. Dynamic Window Approach (DWA)
DWA is a local path planning method for predictive control that samples multiple
velocities (linear and angular velocities) in the velocity space and simulates the robot’s
trajectory at these velocities within a specific time interval. Based on the evaluation metrics,
we selected the best speed from multiple sets of simulated trajectories to drive the robot’s
motion [19].
 Machines 2022, 10, 50 10 of 29

4.1.1. Kinematics Model

The two-wheel differential speed model of the mobile robot is shown in Figure 6,
where: L is the width of the robot. In the experiment, we can ensure the safety of the
robot movement by setting the safety radius Rsa f e ; v(t) and ω (t) are the linear and angular
velocities, respectively. The motion state of the robot during the time interval of the
sampling period ∆t is:

 x (t) = x (t − 1) + v(t)∆t cos(θ (t − 1))



Machines 2021, 9, x FOR PEER REVIEW y(t) = y(t − 1) + v(t)∆t sin(θ (t − 1)) (17)
θ (t) = θ (t − 1) + w(t)∆t


where x (t), y(t), θ (t) denote the position and angle of the robot in the x, y directions at
the time t, respectively; the linear velocity v(t) range of variation depends on the nearest
distance to the obstacle and the maximum linear deceleration; the content of ang
distance to the obstacle and the maximum linear deceleration; the content of angular
locity 𝜔(𝑡)variation
velocity ω (t) variation is is determined
determined by the
by both both the closest
closest distancedistance to the obstruction
to the obstruction and
maximum
the maximum angular deceleration.
angular deceleration.

Figure
Figure 6. Robot
6. Robot kinematics
kinematics model.model.

4.1.2. Determination of Robot Motion Velocity Range

4.1.2. Determination of Robot Motion Velocity Range
In the velocity space, DWA describes the robot’s obstacle avoidance problem as
In the velocity
an optimization problemspace, DWA describes
with constraints, includingthe robot’s obstacle
incomplete avoidance
motion constraint on proble
optimization problem with constraints, including incomplete motion constrain
the differential robot, dynamics constraint on the robot structure, and environmental
constraints.
differential robot, dynamics constraint on the robot structure, and environmen
(1) The maximum and minimum speed constraints for the robot are:
straints.
(1) The maximum vs =and minimum

v, w | v ∈ [speed

constraints
vmin , vmax ], w ∈ [wmin ,for the
wmax ] robot are: (18)

(2) {(
Motor acceleration and deceleration ) [ ,
∣v ∈ vmin asvmax
vs = v, w constraints: ]
different [
, w ∈motors ,
wmin whave]}
max different
performances, the acceleration of the robotis also different, and the robot is constrained
(2) byMotor acceleration
the speed and deceleration
that can be reached in the nexttimeconstraints:
interval. as different motorshave
n performances,
h the acceleration
i h of the robotis also different,
io and the robo
min max min max
vc − av by∆t,
vd = (v, w) | v ∈strained the speed
vc + av ∆t w ∈ be
that, can − aw ∆t,inwthe
wc reached ∆t
c + anexttime
w interval.(19)
where {
vd = v(c v, , wwc∣
)arev ∈ [ c − av
the vlinear
moment, respectively; amin , a
min
Δt , vc and
velocity ]
+ avangular
max
[
w ∈ wcof−robot
Δt , velocity aw min Δ
att ,the
min are the minimum linear deceleration and minimum
]}
+ aw max Δt
wc current
v w
angular deceleration, respectively;
where 𝑣 , 𝑤 are the linear velocity amax
v and, amax are the maximum
w angular linear
velocity of robotacceleration
at the current
and maximum angular acceleration, respectively; and ∆t is the sampling time.
respectively;𝑎 , 𝑎 are the minimum linear deceleration and minimum angul
eration, respectively; 𝑎 ,𝑎 are the maximum linear acceleration and maxim
gular acceleration, respectively; and ∆𝑡 is the sampling time.
(3) Braking distance constraint: the entire robot trajectory can be subdivided int
linear or circular motions. To ensure the robot’s safety, the current speed sh
celerate to 0 before hitting the obstacle in the maximum deceleration condit

{ ) v 2dist (v, w) av min , w 2dist (v, w) aw min

va = (v, w∣ }
Machines 2022, 10, 50 11 of 29

(3) Braking distance constraint: the entire robot trajectory can be subdivided into several
linear or circular motions. To ensure the robot’s safety, the current speed should
decelerate to 0 before hitting the obstacle in the maximum deceleration conditions.
 q q 
v a = (v, w) | v 6 2dist(v, w) av min , w 6 2dist(v, w) aw min (20)

where dis(v, w) is the distance between the simulated trajectory and the nearest ob-
stacle. From the expression 0 − v2a = −2dist(v, w) amin
v and 0 − w2a = −2dist(v, w) amin
w ,
the simulated velocity must satisfy the condition of Equation (20) to ensure the safety
of the robot moves to a greater extent.
The above constraints limit the robot to a certain speed of motion, which we can
represent by a velocity set {Vr |Vr = vs ∩ vd ∩ v a }.

4.1.3. Evaluation Function

We sampled multiple velocities from the velocity space and then selected the optimal
trajectory from them by designing an appropriate evaluation function. The essential criteria
are that the robot in local navigation should efficiently avoid obstacles and move quickly
Machines 2021, 9, x FOR PEER REVIEW
and steadily towards the target. The conventional evaluation functions as follows: 11 of 28

G (v, w) = σ [ xheading(v, w) + ydist(v, w) + zvel (v, w)] (21)

where ℎ𝑒𝑎𝑑𝑖𝑛𝑔(𝑣, 𝑤)is the azimuth evaluation function and to evaluate the azimuth de-
where
viationheading (v, w
between ) isend
the thedirection
azimuth of
evaluation function
the simulated and toand
trajectory evaluate the azimuth
the target devi-
at the current
ation between the end direction of the simulated trajectory and the target at the current
speed; 𝑑𝑖𝑠𝑡(𝑣, 𝑤) is the function to evaluate the distance from the trajectory to the obsta-
speed; dist(v, w) is the function to evaluate the distance from the trajectory to the obsta-
cle; 𝑣𝑒𝑙(𝑣, 𝑤) is the current speed magnitude evaluation function;𝜎is the normalization
cle; vel (v, w) is the current speed magnitude evaluation function; σ is the normalization
operation, and 𝑥, 𝑦, 𝑧 are the weighting factor of each evaluation function.
operation, and x, y, z are the weighting factor of each evaluation function.
4.2.Improved
4.2. ImprovedDynamic
DynamicWindow
WindowApproach
Approach(IDWA)
(IDWA)
ConventionalDWA
Conventional DWAlackslacksglobal
globalpath
pathguidance
guidanceand
andisisprone
pronetotolocal
localoptimality,
optimality,for
for
which IACO planned the navigation path. Conventional DWA has good
which IACO planned the navigation path. Conventional DWA has good robustness and robustness and
goodobstacle
good obstacleavoidance
avoidancein instatic
staticenvironments,
environments,but
butititmay
maybe beineffective
ineffectivewhen
whenhighly
highly
moving obstacles arise in the motion space. To enhance the effectiveness of dynamic
moving obstacles arise in the motion space. To enhance the effectiveness of dynamic path path
planning, the following improvements are made to the evaluation
planning, the following improvements are made to the evaluation function. function.

4.2.1.Modifying
4.2.1. Modifyingthe ℎ𝑒𝑎𝑑𝑖𝑛𝑔(𝑣,
theheading (v, w𝑤) Function
) Function
Thisfunction
This functionisisa anavigation
navigationfunction.
function. Considering
Considering thethe situation
situation shown
shown in Figure
in Figure 7, it7,
is clear that the turning trend of trajectory 1 is more desirable. However, according to theto
it is clear that the turning trend of trajectory 1 is more desirable. However, according
the original
original evaluation
evaluation function
function to calculate
to calculate the angle
the angle byend
by the the end position
position oftrajectory,
of the the trajectory,
we
find λ1 >𝜆1λ2,
we find > which
𝜆2, which
showsshows
thatthat trajectory
trajectory 2 has2 has a higher
a higher rating
rating than
than trajectory
trajectory 1, which
1, which is
is not
not realistic
realistic [39].
[39]. In In this
this paper,
paper, wewe changed
changed the
the navigationtarget
navigation targetpoint
pointofofthe
therobot
robotafter
after
severaltime
several timeintervals
intervals onon the predicted trajectory
trajectory we found 𝜃1
we found θ1 <<𝜃2,θ2,
with
withtrajectory 1 re-
trajectory 1
ceiving aahigher
receiving higherrating.
rating.

Figure ℎ𝑒𝑎𝑑𝑖𝑛𝑔(𝑣,
Figure7.7.heading (v, w)𝑤) mechanistic
mechanistic analysis.
analysis.

Most scholars have optimized the global path and extracted key nodes as the navi-
gation points of the robot, but most of them have not considered that the selection of nav-
igation points is not significant if the navigation points are too far from the current posi-
tion of the robot. In this paper, the heading function is improved according to the robot’s
Machines 2022, 10, 50 12 of 29

Most scholars have optimized the global path and extracted key nodes as the nav-
igation points of the robot, but most of them have not considered that the selection of
navigation points is not significant if the navigation points are too far from the current
position of the robot. In this paper, the heading function is improved according to the
robot’s state of motion so that the navigation points are selected concerning the robot’s
current position. First, the average distance ∆s between adjacent path nodes after three
B-sample optimizations is calculated by Equation (22), and the desired move-out distance
ds is determined, then the navigation point tar is estimated by Equation (24):

length( B_spline)
∆s = (22)
size( B_spline)

ds
nds = f loor ( ) (23)
∆s
tar = B_spline(n + nds ) (24)
where length() is the path length of the B_spline curve; size() is the number of nodes for
calculating the path; f loor is the downward rounding function; n is the sequence of nodes
of the current navigation point in the original global path, and the information about the
robot’s present moment of navigation point tar is obtained by the expected increase in the
number of nodes nds .
As shown in Equation (25), the modified heading(v, w) function will calculate the
deviation angle in terms of the position of the predicted trajectory after several node
intervals nds and give a score accordingly. The robot’s trajectory tracking and navigation
capabilities have been improved to a certain extent.

heading0 (v, ω ) = 180◦ − |Θ(r, tar ) − Θ(r )| (25)

where Θ(r, tar ) is the predicted angle at which the robot’s position r at the next moment
points to the navigation target point tar, obtained with its current linear velocity v and
angular velocity ω; and Θ(r ) is the predicted direction of the robot’s motion.
To ensure the stability of the robot movement and to avoid acceleration and decelera-
tion of the robot wandering between navigation points, the following navigation point tar ∗
information is obtained from Equations (22)–(25) when the robot is moving towards the
current navigation point tar and the condition of Equation (26) is satisfied.

tar ∗ = B_spline(n + 2nds ), dis(r, tar ) < d1

(26)
||[min(dis(r, obs)) < d2 &dis(tar, min_obs) < d3 ]

where dis(r, tar ) is the distance from the predicted position r to the target point tar;
min(dis(r, obs)) is the shortest distance from the expected position r to the obstacle obs in
the robot motion space, and the obstacle is noted as min_obs; dis(tar, min_obs) is the dis-
tance from the target point tar to the obstacle min_obs; and d1 , d2 and d3 are the reference
distances, respectively, and taken as needed.

4.2.2. Modifying the dist(v, ω ) Function

The function is an obstacle avoidance function. The traditional obstacle list infor-
mation [40] is shown in Equation (27), including global known obstacles obs_closed and
unknown static obstacles obs_static, which lacks the consideration of unknown dynamic ob-
stacles obs_dynamics(t). For this reason, this paper optimizes the obs to update dynamically
over time, as shown in Equation (28):

obs = [obs_closed, obs_static] (27)

obs(t) = [obs(t−1), obs_dynamics(t)] (28)

 Machines 2022, 10, 50 13 of 29

Considering the situation shown in Figure 8, it is clear that the trend of obstacle avoid-
ance for trajectory 1 is more desirable. The robot needs to estimate the state information
of the mobile obstacle after ∆t seconds in advance to simulate a path in velocity space
that will avoid the obstacle. Based on the line velocity vobi (i = 1, 2, . . . , n) of the mobile
barriers and the current speed v of the robot, we define the prediction time ∆t as follows:
v
∆t = k1 int( ) + k2 (29)
vob
vob = min(vob1 , vob2 , . . . , vobn ) (30)
where int is an upward rounding function; k1 , k2 are the correction factors, to be taken as
Machines 2021, 9, x FOR PEER REVIEW
required; and vob is the minimum perceived velocity of the mobile obstacle in the robot’s
motion space.

Figure
Figure 8. 𝑑𝑖𝑠𝑡(𝑣,
8. dist 𝜔) mechanistic
(v, ω ) mechanistic analysis. analysis.

The modified
The dist(v,𝑑𝑖𝑠𝑡(𝑣,
modified w) function will calculate
𝑤) function willthecalculate
distance from
thethe current simulated
distance from the curre
trajectory point to the predicted movement of the moving obstacles after ∆t time, and
lated trajectory point to the predicted movement of the moving obstacles after
discard those trajectories whose shortest distance from the obstacle is less than or equal
toand
the discard those
robot’s safety trajectories
radius Rsa f e . In whose
addition,shortest distance
the function has anfrom
upperthe obstacle
limit on the is less
equal to
obstacle the robot’s
distance safety
Dmax , which radius
ignores 𝑅
barriers . In are
that addition, the the
too far from function has
robot, as an upper
shown in lim
obstacle distance 𝐷 , which ignores barriers that are too far from the robot, as s
Equation (31).
Equation (31). min[ Dmin (r, obs(t + ∆t)), Dmax ], Dmin (r, obs(t + ∆t)) > Rsa f e
0
dist (v, ω ) = (31)
discarded, Dmin (r, obs(t + ∆t)) ≤ Rsa f e
′
 min [ Dmin ( r , obs (t + Δt ) ) , Dmax ] , Dmin ( r , obs (t + Δt )) > Rsafe
dist ( v , ω ) = 
where Dmin is the shortest distance from the robot’s simulated trajectory r to all obstacles,
 discarded , Dmin ( r , obs (t + Δt )) ≤ Rsafe
and the calculation of the motion state of all moving obstacles is advanced by ∆t seconds.

where
4.2.3. 𝐷 isthe
Modifying thevelshortest distance from the robot’s simulated trajectory 𝑟 to all o
(v, w) Function
andThe
thefunction
calculation
makesof the
the motion
robot movestate of all
fast, and themoving obstacles
score is only relatedis the robot’s by ∆𝑡
toadvanced
linear velocity. In general, fluctuations in angular momentum tend to cause oscillations,
and excessive
4.2.3. volatility
Modifying thein 𝑣𝑒𝑙(𝑣,
angular𝑤)velocity can affect the stability of the robot’s motion [41].
Function
Considering the situation shown in Figure 9, we assume that the linear speeds of trajectories
1 and 2The function
are the makes1the
same. Trajectory has arobot move fast,
more uniform and
angular the score
variation and aismore
only related to th
desirable
linear trend.
turning velocity. In general,
Referring fluctuations
to the heuristic functionin angular momentum
improvement of IACO in thistend to the
paper, cause osc
and excessive volatility in angular velocity can affect the stability of the robot’s
speed score and vel ( v, w ) function are improved as follows:
[41]. Consideringthe situation shown in Figure 9, we assume that the linear s
ω1 ≤ |(ωt−2 − ωt−1 ) − (ωt−1 − ωt )| ≤ ω2
u2 , √
p = 2 are
trajectories 1 and (32) variati
u2 / the same. Trajectory 1 has a more uniform angular
2, otherwise
more desirable turning trend. Referring to the heuristic function improvement of
0
this paper, the speed score and 𝑣𝑒𝑙(𝑣,
velocity (v, ω )𝑤)
= vfunction
+p (33)
are improved as follows:

u2 , ω 1 ≤ (ωt − 2 − ωt −1 ) − (ωt −1 − ωt ) ≤ ω 2

p=
u2 / 2 , otherwise

velocity ′(v, ω ) = v + p
 u2 , ω 1 ≤ (ωt − 2 − ωt −1 ) − (ωt −1 − ωt ) ≤ ω 2
p=
u2 / 2 , otherwise
Machines 2022, 10, 50 14 of 29
velocity ′(v, ω ) = v + p

where
where P is𝑃the
is the angular
angular velocity
velocity score; ωscore; 𝜔 , 𝜔 and 𝜔 are the angular velocity
t−2 , ωt−1 and ωt are the angular velocity of the
bot at
robot atthethefirst
first
twotwo moments,
moments, first
first one one moment
moment and moment,
and the current the current moment, resp
respectively;
and 𝑢 , 𝜔1 and 𝜔2 are the correction factor.
and u 2 , ω1 and ω2 are the correction factor.

Figure
Figure 9. vel 𝑣𝑒𝑙(𝑣,
9. (v, 𝑤) mechanistic
w) mechanistic analysis. analysis.
5. Hybrid Path Planning
In this paper, ACO and DWA are fused to obtain the robot’s global navigation path
using IACO and construct a robot’s kinematic model for dynamic path planning using
IDWA. The basic process of the hybrid path planning method is as follows:
Step 1. Build a grid map for the mobile robot.
Step 2. Plan a global path based on the IACO with smoothing optimization.
Step 3. Extract the nodes of the global path and obtain the navigation points of the
robot according to Equations (22)–(24). The distance and azimuth angle from the current
position to the local target will be calculated in the IDWA.
Step 4. When the robot moves towards the position of the local target point, once a
new obstacle appears in the environment and is within the detection range of the robot, the
information (position, volume size, and speed of movement) will be sensed by the robot
and the corresponding strategy will be executed to avoid the obstacle.
Step 5. When the robot approaches the local target point, or a new obstacle appears
on the original path that prevents the robot from approaching the local target point, a new
target point will be reacquired according to Equation (26).
Step 6. The path planning and movement process will stop when the mobile robot
reaches the global target point or when the global target point is unreachable due to
obstacles occupying.
A flow chart of the hybrid path planning algorithm is shown as Figure 10:
 target point will be reacquired according to Equation (26).
Step 6. The path planning and movement process will stop when the
reaches the global target point or when the global target point is unreacha
Machines 2022, 10, 50 stacles occupying. 15 of 29

A flow chart of the hybrid path planning algorithm is shown as Figure

Start

Establish the environment model and

initialize the starting and ending
positions

Use the improved ACO to plan a global

optimal path
Initialize the current
position
Extract the nodes of the global path and
obtain the navigation points of the
robot according to Equations (22~24). Speed sampling

Determine the starting point and sub- Generate simulation

target location of the current path trajectory based on
planning kinematics mode

Select the speed and angular velocity

corresponding to the optimal trajectory

Move the robot along the optimal trajectory

Identify the new No

local target points Can the local goal
by Equation (26) be reached?

Yes

No Reach global
destination？

Yes
End

Figure
Figure 10.10. Flowchart
Flowchart of thealgorithm.
of the hybrid hybrid algorithm.
6. Simulation Experimental Analysis
6. Simulation Experimental
This paper uses Analysis
Matlab 2016a software to conduct experiments on a Windows 10
computer
Thiswith a 2.2 GHz
paper processor
uses Matlab and2016a
8 GB of software
RAM. to conduct experiments on a
computer
6.1. with
Performance a 2.2
Analysis GHz
of the processor and 8 GB of RAM.
IACO
6.1.1. Initial Population Validity Analysis
6.1.We
Performance Analysis
propose an initial of theapproach
pheromone IACO considering smoothness and a heuristic
function with a corner suppression factor, making the paths be fewer turning points and
6.1.1.smoothness
higher Initial Population Validity Analysis
in a natural environment. To exemplify the effects of population diver-
sity and path smoothing, we conduct experimental analyses in a conventional environment
through the first generation of ant colonies without pheromone update disturbances. The
number of ants is 50; the initial pheromone concentration of the traditional ACO is 10;
the initial pheromone concentration in this paper is assigned according to Equations (5)
and (6), with C set to 20 and the pheromone concentration on the boundary set to 2. The
experimental results are shown in Table 1 and Figures 11 and 12.
 Equations (5) and (6), with C set to 20 and the pheromone concentration on the boundary
Table set
1. Initial population
to 2. The algorithm
experimental performance
results are showncomparison table.
in Table 1 and Figures 11 and 12.
Evaluation Criteria Traditional ACO
Table 1. Initial population algorithm performance comparison table.
This Paper
Average path length /m
Machines 2022, 10, 50 47.9479 42.9443 16 of 29
Evaluation Criteria Traditional ACO This Paper
Worst path length /m 82.0760 64.8660
Average path length /m 47.9479 42.9443
Optimal path length 1./m 32.0380
Initial population algorithm performance comparison table. 32.1400
Worst path Table
length /m 82.0760 64.8660
Number of turns for optimalEvaluation
path Criteria 12 4 This Paper
Optimal path length /m 32.0380 Traditional ACO 32.1400
Number
Number of of ants
turns fordead Averagepath
optimal path length /m 15
12 47.9479 40 42.9443
Worst path length /m 82.0760 64.8660
Optimal search
Number timedead
of ants /s
Optimal path length /m
0.3287
15 32.0380
0.1822
0 32.1400
Number of turns for optimal path 12 4
Optimal search time /s
Number of ants dead
0.3287 15
0.1822 0
Optimal search time /s 0.3287 0.1822

(a) (a) (b)

(b)
Figure 11. Diagram
Figure of the
11. Diagram optimal
ofFigure
the path
optimal ofofof
path
11. Diagram ACO;
ACO;
the (a)
(a)Optimal
optimal Optimal
path path(a)diagram;
path
of ACO; diagram; (b)diagram;
(b)
Optimal path Allants’
All ants’ paths
paths
(b) dia-
dia-
All ants’ paths
gram.gram. diagram.

(a) (b)
(a) (b)
Figure 12. Diagram of the optimal path of this paper; (a) Optimal path diagram; (b) All ants’ paths
Figure 12. Diagram of theFigure
diagram. 12. Diagram
optimal path ofof the
thisoptimal
paper;path
(a)ofOptimal
this paper;path
(a) Optimal path diagram;
diagram; (b) All(b) All ants’
ants’ pathspaths
diagram.
diagram.
The experimental simulation results
The experimental show that
simulation the first
results showgeneration ofgeneration
that the first traditional of ACO
traditional
finds paths based onsimulation
The experimental ACOheuristic functions
finds paths based
results showunderthat
on the same
heuristic conditions
functions
the first under the
generation of pheromone
same conditions concen-
of
of traditional pheromone
ACO
concentration, resulting in more concentrated paths with poor population diversity,
is notwhich
findstration, resulting
paths based on in more concentrated
heuristic functions underpaths with poor population
the same conditions diversity,
of pheromonewhichconcen-
is not conducive to the exploration ability of the algorithm. In this paper, IACO has a
conducive
tration, to in
resulting themore
exploration ability paths
concentrated
better
ofthethe
guiding effect on
algorithm.
with
first poor of
generation
Inants,
this
population paper, IACO hasants
and the diversity,
a better
distribution ofwhich is not
in the whole
guiding effect on the first
space generation
is more of ants,
uniform. The and diversity
population the distribution of ants
is better, which in likely
is more the whole
to guide the
conducive to the exploration ability of the algorithm. In this paper, IACO has a better
subsequent ants to achieve the globally optimal path. From Table 1, we can see that our
guiding effect on the first generation of ants, and the distribution of ants in the whole
initial population pathfinding effectiveness is significantly better than the traditional ACO.
In subsequent experiments, we will utilize the improved pheromone update strategy to
further demonstrate our algorithm’s effectiveness in solving the contradictory population
diversity and convergence speed.
 space is more uniform. The population diversity is better, which is more likely to guide
the subsequent ants to achieve the globally optimal path. From Table 1, we can see that
our initial population pathfinding effectiveness is significantly better than the traditional
ACO. In subsequent experiments, we will utilize the improved pheromone update strat-
Machines 2022, 10, 50 egy to further demonstrate our algorithm’s effectiveness in solving the contradictory pop-
17 of 29
ulation diversity and convergence speed.

6.1.2.Analysis
6.1.2. Analysisofofthe
theEffectiveness
EffectivenessofofDeadlock
DeadlockSolutions
Solutions
ToToverify
verifythe
theeffectiveness
effectiveness of of the
the deadlock
deadlock problem-solving
problem-solvingstrategies
strategiesininthis paper,
this pa-
the traditional ant-death strategy and the retraction strategy are chosen
per, the traditional ant-death strategy and the retraction strategy are chosen to conduct to conduct com-
parative experiments
comparative experiments in ainspecific large-scale
a specific deadlock
large-scale deadlockmapmap environment. AfterAfter
environment. extensive
ex-
experiments in this environment, the following parameters are selected
tensive experiments in this environment, the following parameters are selected for the for the retraction
and death
retraction andstrategies, respectively:
death strategies, 𝛼 = 1,α𝛽=
respectively: = 1,
7, 𝜌β==0.7,7, 𝑄ρ =
=10, 𝑚Q
0.7, = 50 ； 𝛼m==1, 50;
= 10, 𝛽=
α 15, 𝜌 =β 0.9,
= 1, 𝑄=
= 15, ρ 10, 𝑚 =Q50.
= 0.9, = The
10, mparameters in this paper
= 50. The parameters in are
this shown in shown
paper are Table 2.inThe
Tablepath
2.
andpath
The iteration diagramdiagram
and iteration of all ants
of are added
all ants aretoadded
reflecttothe levelthe
reflect of level
pathfinding for different
of pathfinding for
strategies,
different and theand
strategies, results
the of the simulation
results experiments
of the simulation are shown
experiments in Tablein3Table
are shown and Figures
3 and
13–15.
Figures 13–15.

Table
Table 2. 2. Main
Main parameters
parameters ofof the
the simulation
simulation experiment.
experiment.
Algorithm
Algorithm α β𝜶 𝜷Q 𝑸ρ 𝝆 c 𝒄 u 𝒖𝟏
1 δ 𝛅
Traditional ACO 1 10 100 0.3 - - -
Traditional ACO 1 10 100 0.3 - - -
Luo etLuo et al. [12] 1.1
al. [12] 1.1
7 7
100 100
0.2 0.2 - - - - - -
Dai etDai et al. [13] 1
al. [13] 101 10
50 500.3 0.3 - - - - - -
You etYou
al. [14]
et al. [14] 1 21 21 10.2 0.2 - - - - - -
This paper 1 7 10
This paper 1 7 100.7 0.720 20 1 1 10
10

Table
Table 3. 3. Performance
Performance comparison
comparison table
table ofof algorithms
algorithms forfor deadlock
deadlock environment.
environment.
Evaluation Criteria Retraction Strategy Death Strategy This Paper
Evaluation Criteria Retraction Strategy Death Strategy This Paper
Length of path/m 125.1840 37.2100 32.7260
Length of path/m 125.1840 37.2100 32.7260
Number of turns for optimal
Number of turns for optimal path path 25 25 19 19 3 3
Number
Number of iterations
of iterations —— —— 13 13 5 5
Number
Number of ofant
antdeaths
deaths before
before the the algo-
0 108 108 0 0
algorithm 0
rithmreaches
reachesconvergence
convergence
Optimal search time /s 24.5955 2.4646 1.3852
Optimal search time /s 24.5955 2.4646 1.3852

300

250

200

150

100

50
0 10 20 30 40 50
Number of iterations

(a) (b) (c) (d)

Figure13.
Figure
Machines 2021, 9, x FOR PEER REVIEW
13.Retraction
Retractionstrategy
strategyoptimisation
optimisationpath
pathdiagram;
diagram;(a)
(a)Optimal
Optimalpath
pathdiagram;
diagram;(b)
(b)All
All
17
ants’
ants’
of 28
paths diagram; (c) Iterative diagram of the optimal path; and (d) Iterative diagram of ants’ paths.
paths diagram; (c) Iterative diagram of the optimal path; and (d) Iterative diagram of ants’ paths.

44

41

38

35
0 10 20 30 40 50
Number of iterations

(a) (b) (c) (d)

Figure 14. Death Strategy Optimisation Path Diagram; (a) Optimal path diagram; (b) All ants’ paths
Figure 14. Death Strategy Optimisation Path Diagram; (a) Optimal path diagram; (b) All ants’ paths
diagram; (c) Iterative diagram of the optimal path; and (d) Iterative diagram of ants’ paths.
diagram; (c) Iterative diagram of the optimal path; and (d) Iterative diagram of ants’ paths.
 38

35
0 10 20 30 40 50
Number of iterations

(a) (b) (c) (d)

Machines 2022, 10, 50 18 of 29
Figure 14. Death Strategy Optimisation Path Diagram; (a) Optimal path diagram; (b) All ants’ paths
diagram; (c) Iterative diagram of the optimal path; and (d) Iterative diagram of ants’ paths.

(a) (b) (c) (d)

Figure
Figure 15.15. Diagram
Diagram of the
of the optimal
optimal path
path of this
of this paper;
paper; (a) (a) Optimal
Optimal path
path diagram;
diagram; (b)(b)
AllAll ants’
ants’ paths
paths
diagram; (c) Iterative diagram of the optimal path; and (d) Iterative diagram of ants’ paths.
diagram; (c) Iterative diagram of the optimal path; and (d) Iterative diagram of ants’ paths.

TheThe simulation
simulation results
results show show
thatthat the retraction
the retraction strategy
strategy preserves
preserves all theall
ants’thesolutions.
ants’ solu-
tions. Still, the ants move forward and backward repeatedly
Still, the ants move forward and backward repeatedly in the deadlocked region, leaving in the deadlocked region,
leaving a large amount of interference pheromone for subsequent
a large amount of interference pheromone for subsequent ants. The pathfinding ability ants. The pathfinding
of ability of ants worse
ants becomes becomes and worse
worse and worse
as the as the progresses,
iteration iteration progresses,
showing that showing that the re-
the retraction
traction
strategy is strategy
not suitableis not
forsuitable for the large-scale
the large-scale trap environment.
trap environment. Although the Although the death
death strategy
strategy
is more is morethan
effective effective than the retraction
the retraction strategy in strategy
termsin ofterms
path of path and
length length and iteration,
iteration, the
the number
number of antsofthat
ants thatdue
died died to due to deadlock
deadlock before before the algorithm
the algorithm achieved achieved convergence
convergence is as
is as
high as high
35.8%, asnegatively
35.8%, negatively
impacting impacting
the abilitythetoability
solve tothesolve the solution.
optimal optimal solution.
The death The
death strategy can achieve better paths than the retraction strategy,
strategy can achieve better paths than the retraction strategy, but it is trapped in a local but it is trapped in a
local optimum
optimum and cannotand be cannot be improved
improved by changing
by changing the pheromone
the pheromone volatilityvolatility factor any-
factor anymore.
more.
This paperThis paper enhances
enhances the algorithm the algorithm
based on the based on the of
advantage advantage of thestrategy,
the retraction retraction strat-
which
canegy,
preserve
whichants’ survival and
can preserve ants’improve
survivalthe and solution
improve effect.
the From theeffect.
solution experimental
From the results,
experi-
themental
ants inresults,
this paper
the retreat
ants in from the deadlocked
this paper retreat from region in time and region
the deadlocked add a virtual
in timebarrier
and add
grid to the deadlocked
a virtual barrier grid areato the todeadlocked
prevent subsequent ants from
area to prevent falling into
subsequent antsthis
frompart, which
falling into
improves the path search quality of the following ants. Our strategy can
this part, which improves the path search quality of the following ants. Our strategy can obtain better paths
in obtain
large-scale
betterdeadlock
paths in environments, and the search
large-scale deadlock time is also
environments, andshorter, which
the search is more
time is also
suitable forwhich
shorter, path planning of mobile
is more suitable forrobots in a complex
path planning environment.
of mobile robots in a complex environ-
ment.
6.2. Simulation Experiment Analysis of IACO
6.2.ASimulation
general environment of 20 × 20
Experiment Analysis scale and a U-shaped environment are chosen for
of IACO
comparison
A general environment of 20 × 20 scaleofand
experiments, with a population 50 ants and theenvironment
a U-shaped number of iterations setfor
are chosen
to comparison
50. experiments, with a population of 50 ants and the number of iterations set to
50. General Environment
6.2.1.
To verify
6.2.1. General the effectiveness and superiority of our IACO, the traditional ACO, algo-
Environment
rithms from Luo et al. [12], and Dai et al. [13] are selected for comparative experimental
analysisToinverify the effectiveness
a conventional and superiority
environment that is based of on
ourLuo
IACO,
et al.the traditional
[12]. The resultsACO, algo-
of the
rithms from Luo et al. [12], and Dai et al. [13] are selected for comparative
simulation experiments are shown in Table 4 and Figure 16, where: the black dashed line experimental
in analysis
Figure 16ain aisconventional
the initial path environment that is based
of our algorithm, on Luo
the blue et al.line
dashed [12].isThe
the results of the
path after
simulation
optimizing theexperiments are shown
redundant nodes, in Table
and the solid 4red
and Figure
line is the16, where:
path the times
of three black B-sample
dashed line
in Figure 16a is the initial path of our algorithm, the blue dashed
smoothing; the paths in Figure 16b marked with black, purple and blue are implemented line is the path after
optimizing the redundant nodes, and the solid red line
by Dai et al. [13], Luo et al. [12], and traditional ACO, respectively. is the path of three times B-sample
As shown in Figure 16, both the traditional ACO and the Luo et al. [12] can find the
shortest path of 30.9690 m, where the Luo et al. [12] algorithm converges faster, and the
number of turns for both is 15, which is more than this paper and Dai et al. [13]. Too
many turns for the mobile robot will inevitably consume more energy and time. In this
paper and Dai et al. [13], the path length and smoothing factors are considered as the
objective of the search, where the length and the number of turning points of the initial
path obtained by our algorithm are 33.3137 m and 7, respectively, which are slightly better
than the 34.4840 m and 8 of Dai et al. [13]. The B-spline curve is obtained by optimizing
the initial path with 2.3023 m shorter than Dai et al. [13], and it is significantly better than
the other three algorithms in terms of smoothness and curvature continuity suitable for
the robot. We consider hierarchical optimization for our pheromone update strategy, so
Machines 2022, 10, 50 19 of 29

Machines 2021, 9, x FOR PEER REVIEW 18 of 28

the convergence of our algorithm is the fastest, as can be seen in Figure 16c. The downside
is that as we consider the optimization path twice, the total program run time is 2.755 s,
slightly morethe
smoothing; than others.
paths in Figure 16b marked with black, purple and blue are implemented
by Dai et al. [13], Luo et al. [12], and traditional ACO, respectively.
Table 4. General environment algorithm performance comparison table.
Table 4. General environment algorithm performance comparison table.
This Paper
Evaluation Criteria This Paper Luo et al. [12] Dai et al. [13] Traditional ACO
Evaluation Criteria
Original Optimized Spline Luo et al. [12] Dai et al. [13] Traditional ACO
Original Optimized B-Spline
Path length/m 33.3137 33.0226 32.1817 30.9680 34.4840 30.9680
Path length/m 33.3137 33.0226 32.1817 30.9680 34.4840 30.9680
Number of turns for
Number of path
turns for optimal 7 path 7 7 7 0 0 1515 88 15
15
optimal
NumberNumber of iterations 3
of iterations 3—— —— ———— 66 99 28
28
OptimalOptimal
search time /s time1.772
search /s 0.603
1.772 0.603 0.3800.380 1.816
1.816 1.619
1.619 2.084
2.084

Original
Optimized Traditional ACO
B-spline 45 IACO in reference 13
IACO in reference 12
This paper
40

35

30
0 5 10 15 20 25 30 35 40 45 50
Number of iterations
(a) (b) (c)
Figure 16.
Figure 16. Comparison
Comparisondiagram
diagramofofpath
pathplanning; (a) (a)
planning; Optimal pathpath
Optimal diagram of this
diagram of paper; (b) Opti-
this paper; (b)
mal path diagrams of other algorithms; and (c) Optimal path iteration diagram.
Optimal path diagrams of other algorithms; and (c) Optimal path iteration diagram.

6.2.2. As shown in
U-Shaped Figure 16, both the traditional ACO and the Luo et al. [12] can find the
Environment
shortest path of 30.9690m,
To verify the effectiveness where the Luo et of
and superiority al.our
[12]algorithm
algorithm inconverges
a U-shapedfaster, and the
environment,
number of turns for both is 15, which is more than this paper and Dai
the traditional ACO, Dai et al. [13], and You et al. [14] algorithms are chosen for comparative et al. [13]. Too many
turns for the analysis,
experimental mobile robotwithwill inevitably
the map consumebased
environment moreon energy
You etand time.The
al. [14]. In this paper
results of
andsimulation
the Dai et al. [13], the path length
experiments are shownand in smoothing
Table 5 and factors
Figureare 17,
considered
where: the as the
blueobjective
grid in
of the search,
Figure 17a is the where theobstacle
virtual length and gridthe
usednumber
to handleof turning points strategy
the deadlock of the initial
in thispath ob-
paper;
tained by our algorithm are 33.3137m and 7, respectively, which
the paths in Figure 17b are marked with black, purple and blue are implemented by Dai are slightly better than
the
et al.34.4840m
[13], You andet al.8[14],
of Daiandettraditional
al. [13]. TheACO,B-spline curve is obtained by optimizing the
respectively.
initial
FrompathFigure
with 2.3023m
17, we can shorter thansome
see that Dai et al. [13],
ants in theand it is significantly
pathfinding processbetter than the
of traditional
otherin
ACO three algorithms in
the deadlocked termshave
region of smoothness and curvature
a death situation, making the continuity
optimalsuitable for the
path is caught
robot.
in We consider
the local optimum.hierarchical
The algorithm optimization
in this paper for our pheromone
obtains the optimalupdatepathstrategy, so the
in the current
convergence which
environment, of our isalgorithm is thethan
slightly better fastest,
the as can be seen
algorithms in Figure
of Dai 16c.and
et al. [13], TheYou downside
et al. [14]is
that
in as we
terms consider
of path length,the optimization
where: our initialpath
pathtwice,
length the total program
is 28.6274 m, 28.1842 run timeremoving
m after is 2.755s,
slightly more
redundant than and
nodes, others.the final B-sample curve is 27.9103 m, which is 4.4% and 2.6%
reduced compared to the path length of Dai [13] et al. and You [14] et al., respectively.
6.2.2. importantly,
More U-Shaped Environment
the initial path planned by our algorithm has only four turns, which
is better than the algorithms
To verify the effectiveness of and
Dai superiority
et al. [13], andof ourYou et al. [14]
algorithm in with five and
a U-shaped seven
environ-
turns, respectively. At a particular time cost, the quality of our
ment, the traditional ACO, Dai et al. [13], and You et al. [14] algorithms are chosen for paths is much better
after two smoothing
comparative operations.
experimental In addition,
analysis, with the map we perform
environmenta hierarchical
based onoptimization
You et al. [14]. of
the
Thepheromone
results of the update,
simulationand as seen in Figure
experiments 17c, our
are shown algorithm
in Table 5 and converges in the 6th
Figure 17, where: the
generation, outperforming other algorithms.
blue grid in Figure 17a is the virtual obstacle grid used to handle the deadlock strategy in
this paper; the paths in Figure 17b are marked with black, purple and blue are imple-
mented by Dai et al. [13], You et al. [14], and traditional ACO, respectively.

Table 5. Performance comparison table for U-shaped environment algorithms.

This Paper
Evaluation Criteria Dai et al. [13] You et al. [14] Traditional ACO
Original Optimized B_Spline
Path length/m 28.6274 28.1842 27.9103 29.2100 28.6557 29.4520
Machines 2022, 10, 50 20 of 29

Table 5. Performance comparison table for U-shaped environment algorithms.

Machines 2021, 9, x FOR PEER REVIEW 19 of 28
This Paper
Evaluation Criteria Dai et al. [13] You et al. [14] Traditional ACO
Original Optimized B_Spline
Path length/m
Path length/m 28.6274 28.1842 28.1842
28.6274 27.9103
27.9103 29.2100
29.2100 28.6557
28.6557 29.4520
29.4520
Number of turns for
Number of turns for optimal
optimal path
4 path 44 4 0 0 5 5 77 88
NumberNumber of iterations6
of iterations 6—— —— —— —— 2323 12
12 22
22
OptimalOptimal
search time /s time1.494
search /s 0.543
1.494 0.543 0.2550.255 1.299
1.299 1.355
1.355 2.047
2.047

Original 50
Optimized
Traditional ACO
B-spline
IACO in reference 13
45
IACO in reference 14
This paper
40

35

30

0 5 10 15 20 25 30 35 40 45 50
Number of iterations

(a) (b) (c)

Figure 17. Comparison diagram of path planning; (a) Optimal path diagram of this paper; (b) Opti-
Figure 17. Comparison diagram of path planning; (a) Optimal path diagram of this paper; (b)
mal path diagrams of other algorithms; and (c) Optimal path iteration diagram.
Optimal path diagrams of other algorithms; and (c) Optimal path iteration diagram.

From Figure
6.3. Experimental 17, weofcan
Analysis thesee that some ants in the pathfinding process of traditional
IACO-IDWA
ACO in the deadlocked region
To validate the performance of our have a death situation,
improved ACO making
fusionthe optimal DWA,
improved path is thecaught
A*
in the local optimum. The algorithm in this paper obtains the optimal
fusion DWA algorithm [7], which is currently widely studied for hybrid path planning, is path in the current
environment,
chosen which is analysis
for experimental slightly better than the
of algorithm algorithms
comparison inof Dai et al.static
unknown [13],and
andunknown
You et al.
[14] in terms of path length, where: our initial path length is 28.6274m,
dynamic environments. Chi et al. [7] use the improved A* to plan global paths with a high 28.1842m after
removing redundant nodes, and the final B-sample curve is 27.9103m,
degree of safety (maintaining a safe distance from obstacles), combined with the traditional which is 4.4% and
2.6% reduced compared to the path length of Dai [13] et al. and
DWA for local path planning. The standard parameters of our DWA and the DWA of Chi You [14] et al., respectively.
etMore
al. [7]importantly, the initial
are: the maximum path planned
velocity is 1 m/s;bythe our algorithmangular
maximum has only four turns,
velocity is 20owhich
/s; theis
better than
velocity the algorithms
resolution of Dai
is 0.01 m/s; theetangular
al. [13],velocity
and Youresolution
et al. [14]iswitho
1 /s; five
theand seven turns,
acceleration is
respectively.
0.2 At a particular
m/s2 ; the angular timeiscost,
acceleration the
50o /s quality
2 ; the of our paths
parameters of the is much better
evaluation after are:
function two
xsmoothing
= 0.1, y = operations. In the
0.05, z = 0.2; addition, we perform
prediction period is a hierarchical
3s; the safetyoptimization
radius Rsa f eof them;
= 0.7 phero-
the
mone update, and as seen in Figure 17c, our algorithm converges
desired displacement distance ds is 5m; the navigation point reference distance d1 , d2 andin the 6th generation,
doutperforming other algorithms.
3 are 2m; the prediction time (moving obstacle) parameter: k1 is 10, k2 is 0; and the angular
velocity scoring parameter: u2 is 0.1 , ω1 is 0, and ω2 is 0.1 rad.
6.3. Experimental Analysis of the IACO-IDWA
6.3.1. ToUnknown
validateStatic Obstacle Environments
the performance of our improved ACO fusion improved DWA, the A*
OurDWA
fusion algorithm is compared
algorithm withisChi
[7], which et al. [7],
currently and the
widely resultsfor
studied arehybrid
shownpath in Figures 18–20is
planning,
and Tablefor
chosen 6. In this case, theanalysis
experimental black dotted line represents
of algorithm the global
comparison path of each
in unknown algorithm,
static and un-
where
knownthe originalenvironments.
dynamic path length planned by [7]
Chi et al. Chiuse et al.
the[7] is 36.5210
improved A*m, to and
planthis paper
global is
paths
35.4772
with a m. After
high the global
degree path
of safety planning is completed,
(maintaining a safe distancewe addfromunknown
obstacles),static obstacles
combined to
with
the
thecommon partDWA
traditional of thefor
path,
localwhere:
path the paths marked
planning. with blue
The standard and purple
parameters ofrepresent
our DWAlocal and
paths of Chi et al. [7] and our algorithm, respectively.
the DWA of Chi et al. [7] are: the maximum velocity is 1 m/s; the maximum angular ve-
In is
locity Simulation
20o /s; theExperiment 1, whenisa random
velocity resolution 0.01 m/s;obstacle is added
the angular to the
velocity map, both
resolution is 1Chi
o
/s;
etthe
al.acceleration
[7] and our algorithm
is 0.2 m/s can
2
; theplan a path
angular from the starting
acceleration is 50 /spoint
o 2
; the (top left corner)
parameters toeval-
of the the
target
uation point (bottom
function are:right
𝑥 = corner).
0.1，𝑦 =The 0.05actual
，𝑧 =path length
0.2; the of the robot
prediction perioddriving
is 3s; in
thethis paper
safety ra-
isdius
35.8240m,
𝑅 which is 2.2% optimized compared to Chi et al. [7]. As the
= 0.7m; the desired displacement distance 𝑑𝑠 is 5m; the navigation point refer- initial path of Chi
et al. [7] maintains a safe distance from the obstacles and reduces the constraints of barriers
ence distance 𝑑1 , 𝑑2 and 𝑑3 are 2m; the prediction time (moving obstacle) parameter: 𝑘1 is
on the robot pathfinding space, the algorithm takes lesser time than our algorithm. As
10, 𝑘2 is 0; and the angular velocity scoring parameter: 𝑢2 is 0.1, 𝜔1 is 0, and 𝜔2 is 0.1rad.
shown in Table 6, the robot still could avoid the obstacles better and more safely to reach
6.3.1. Unknown Static Obstacle Environments
Our algorithm is compared with Chi et al. [7], and the results are shown in Figures
18–20 and Table 6. In this case, the black dotted line represents the global path of each
algorithm, where the original path length planned by Chi et al. [7] is 36.5210m, and this
paper is 35.4772m. After the global path planning is completed, we add unknown static
 obstacles
representtolocal
the common partetofal.the
paths of Chi [7]path, where:
and our the paths
algorithm, marked with blue and purple
respectively.
represent
obstacles local
to thepaths of Chi
common et al.
part [7] and
of the our
path, algorithm,
where: respectively.
the paths marked with blue and purple
represent local paths of Chi et al. [7] and our algorithm, respectively.
Table 6. IACO- IDWA path planning performance table.
Table 6. IACO- IDWA path planning performance table.
Number of Unknown Static Table 6. IACO- IDWA Path Length/m
path planning performance table. Optimal Search Time/s
Number
Machines of
2022, 10, Unknown Static
50
Obstacles Path
Chi et al. [7] Length/m
This Paper Chi etOptimal
al. [7] Search Time/s
This Paper21 of 29
NumberObstacles
of Unknown
1 Static et al.Path
Chi36.6681 [7] Length/m
This Paper
35.8240 Optimal
Chi402.3918
et al. [7] Search Time/s
This Paper
409.8574
Obstacles
12 Chi et al.
36.6681
37.1823 [7] This Paper
35.8240
36.1200 Chi et
402.3918al.
419.0973 [7] This Paper
409.8574
417.4700
213 36.6681
37.1823
the target point
37.7814 35.8240
by increasing 36.1200
the number of random
36.5060 402.3918
419.0973 409.8574
417.4700
obstacles, with the driving
447.5673 path length
422.6949
32 also growing, but the path length
37.1823
37.7814 of our algorithm447.5673
36.1200
36.5060 is always better than Chi
419.0973 et al. [7].
417.4700
422.6949
3 37.7814 36.5060 447.5673 422.6949
6
5
64
53 0.6
42
36 0.6
2 50
49
0 38 0.4
0.6
9 27
86 0.4
7 05
6 94
5 83 0.2
0.4
4 72
36 Improved Astar-DWA in reference 7
2 50
0.2
49 This paper
0 38 Improved Astar-DWA in reference 7
9 27
0.20 This paper
86 0 200 400 600 800
7 05 0 Improved Astar-DWA in reference 7
Number of control points
6 94 0 This200
paper 400 600 800
5 83
47 0 Number of control points
36 0 200 400 600 800
5
4 Number of control points
Figure
Figure 18.
18. Comparison
Comparison of
3
of path
path planning
planning results
results for
for unknown
unknown static
static environment
environment 1;
1; (a)
(a) Local
Local path
path
Figure 18. [7]
planning Comparison of path
(b) This paper; andplanning resultscomparison
(c) Line speed for unknown static environment 1; (a) Local path
diagram.
planning [7] (b) This paper; and (c) Line speed comparison diagram.
planning [7]Comparison
Figure 18. (b) This paper; and (c)
of path Line speed
planning comparison
results diagram.
for unknown static environment 1; (a) Local path
planning [7] (b) This paper; and (c) Line speed comparison diagram.

(a) (b) (c)

(a) (b) (c)
Figure 19. Comparison of path planning results for unknown static environment 2; (a) Local path
(a) (b) (c)
Figure 19. Comparison of path planning results for unknown static environment 2; (a) Local path
planning [7] (b) This paper; and (c) Line speed comparison diagram.
Figure 19. Comparison of path planning results for unknown static environment 2; (a) Local path
planning [7]Comparison
Figure 19. (b) This paper; and (c)
of path Line speed
planning comparison
results diagram.
for unknown static environment 2; (a) Local path
planning [7] (b) This paper; and (c) Line speed comparison diagram.
planning [7] (b) This paper; and (c) Line speed comparison diagram.

(a) (b) (c)

(a) (b) (c)
Figure 20. Comparison of path planning results for unknown static environment 3; (a) Local path
(a) (b) (c)
Figure 20. Comparison of path planning results for unknown static environment 3; (a) Local path
planning [7] (b) This paper; and (c) Line speed comparison diagram.
planning [7]Comparison
Figure 20. (b) This paper; and (c)
of path Line speed
planning comparison
results diagram.
for unknown static environment 3; (a) Local path
Figure 20. Comparison of path planning results for unknown static environment 3; (a) Local path
In Simulation
planning Experiment
[7] (b) This paper; and (c) 1, when
Line a random
speed obstacle
comparison is added to the map, both Chi
diagram.
planning [7] (b) This paper; and (c) Line speed comparison diagram.
et al.In[7]
Simulation Experiment
and our algorithm can1,plan
when a random
a path obstacle
from the is added
starting to the
point (top map,
left bothtoChi
corner) the
et al. [7] and our algorithm
In Simulation can plan
Experiment a path
1, when from the
a random startingispoint
obstacle added(top left map,
to the corner) to the
both Chi
Table 6. IACO- IDWA path planning performance table.
et al. [7] and our algorithm can plan a path from the starting point (top left corner) to the
Number of Unknown Path Length/m Optimal Search Time/s
Static Obstacles Chi et al. [7] This Paper Chi et al. [7] This Paper
1 36.6681 35.8240 402.3918 409.8574
2 37.1823 36.1200 419.0973 417.4700
3 37.7814 36.5060 447.5673 422.6949
Machines 2022, 10, 50 22 of 29

In Simulation Experiments 2 and 3, the algorithm of Chi et al. [7] has more seek time
than our algorithm as the new random obstacle is placed at a more critical location on
the global path, then the robot is influenced by the more enormous spatial constraints
of the environment. The DWA used in Chi et al. [7] is based on the turning point of the
global path as the key navigation point, and when the navigation point is less helpful in
guiding the robot, it is costly for the robot to search for a path that can bypass this obstacle.
Therefore, in Figures 19c and 20c, we can observe that the movement speed of the robot
in Chi et al. [7] varies dramatically and more persistently than our robot between 200–300
and 400–600 control nodes. The algorithm in this paper calculates the deviation angle with
the desired trajectory node position by a modified heading(v, w) function, which is not
limited to the traditional DWA that only considers the path turning point case. Moreover,
in this paper, when the robot moves towards the current navigation point and satisfies the
condition of Equation (26), the following navigation point is obtained in advance, which
avoids the acceleration and deceleration situation where the robot wanders between local
target points, and the stability of robot motion more than Chi et al. [7]. In summary, the
hybrid algorithm in this paper can achieve obstacle avoidance in an unknown environment
and better fit the globally optimal path.

6.3.2. Unknown Dynamic Obstacle Environments

In the previous section, we verified the effect of the hybrid path planning of the
improved algorithm. Still, due to the difference in the path planned by the global algorithm,
the obstacle avoidance effect of our algorithm cannot be fully explained. Therefore, in
this section, we verify the dynamic obstacle avoidance effect of the algorithm by a mobile
obstacle in the 10 × 10 obstacle-free environment. Consider three types of moving blocks
with different characteristics (size, speed), as shown in Table 7. In the experiment, the actual
radius of the mobile obstacle is extended twice to generate a threat circle which expanded
threat circle can increase the robot’s recognition range of barriers and play a certain degree
of buffering effect on the robot’s braking. The start point coordinates of the mobile robot
are (3.5, 7.5), and the endpoint coordinates are (6.5, 2.5). The start point coordinates of the
mobile obstacle are (6.5, 4.5), and the endpoint coordinates are (3.5, 6.5).

Table 7. Mobile obstacle movement states.

Simulation Experiment Actual Radius R_ob /m Radius R_Sob of Threat Circle Speed of Movement of m/s
1 0.15 0.3 0.1
2 0.3 0.6 0.1
3 0.3 0.6 0.4

To facilitate the study, we modify the obstacle list of the function in Chi et al. [7] to
be dynamic through Equation (28). Usually, most scholars study DWA in a way similar to
Chi et al. [7], that is, for the detection distance Dmin in Equation (31) taken as the distance
from the robot to the geometric center of the obstacle. A reasonable safety radius Rsa f e
is set for the robot, so the predicted trajectory will not consider those areas where the
detection distance is less than the safety radius Rsa f e . Considering the limited space for
robot movement, if the volume of mobile obstacles is too large, a more effective safety
radius Rsa f e needs to be chosen to ensure the simulated trajectory process, for which we
improved it and added a set of experiments based on Chi et al. [7]. The detection distance
considered in the improved algorithm [7] and this paper will be the distance from the robot
to the surface of the obstacle threat circle.
The mobile obstacle in Simulation Experiment 1 is considered smaller in size and
lower in motion speed. From Table 8 and Figure 21, we can see that all three methods
avoid the obstacles, and the robot also travels a similar path length, with the algorithm
in Chi et al. [7] taking the least time to avoid the obstacle. In Simulation Experiment 2,
the volume size of the moving obstacle is double that of Experiment 1, and the robot in
 improved it and added a set of experiments based on Chi et al. [7]. The detection distance
considered in the improved algorithm [7] and this paper will be the distance from the
robot to the surface of the obstacle threat circle.
The mobile obstacle in Simulation Experiment 1 is considered smaller in size and
lower in motion speed. From Table 8 and Figure 21, we can see that all three methods
Machines 2022, 10, 50 23 of 29
avoid the obstacles, and the robot also travels a similar path length, with the algorithm in
Chi et al. [7] taking the least time to avoid the obstacle. In Simulation Experiment 2, the
volume size of the moving obstacle is double that of Experiment 1, and the robot in Chi et
Chi et al.
al. [7] [7] collides
collides with thewith the threat
threat circle shown
circle shown in Figurein Figure
22a, but22a, afterbut after improving
improving the
the obstacle
obstacle
detection detection
method method
of Chi etofal.Chi
[7],etitsal.
can [7],successfully
its can successfully
avoid theavoid obstaclethesafely.
obstacleThesafely.
algo-
The
rithmalgorithm in this
in this paper paper the
predicts predicts
motion theofmotion
the movingof the moving
block for ∆𝑡block for ∆t
periods periods
and and
pre-avoids
pre-avoids the obstacle at position (4, 6.2), with better safety in obstacle
the obstacle at position (4, 6.2), with better safety in obstacle avoidance than the improved avoidance than the
improved algorithm [7]. Simulation Experiment 3 further increased
algorithm [7]. Simulation Experiment 3 further increased the speed of obstacle motion to the speed of obstacle
motion
0.4 m/s.toFrom
0.4 m/s.
Figure From Figure
23, we 23, that
can see we canin a see that inenvironment
dynamic a dynamic environment with a
with a larger volume
larger volume
and more and more
enormous enormous
motion speedmotion
mobilespeed mobile
obstacle, obstacle,
the robot the et
in Chi robot in collided
al. [7] Chi et al.with
[7]
collided with it, and the improvement crossed the threat circle, nearly
it, and the improvement crossed the threat circle, nearly colliding with the mobile obstacle. colliding with the
mobile obstacle. The
The theoretical theoretical
maximum linearmaximum
velocity linear
of 1m/s velocity
that weofset1 m/s thatrobot
for the we setis for
notthe robot
achieved
isdue
not to
achieved
the robot receiving motion constraints, dynamics constraints, and obstacle and
due to the robot receiving motion constraints, dynamics constraints, con-
obstacle
straints. Hence, ourHence,
constraints. our robot successfully
robot successfully searched for searched
a path for a paththe
to avoid to obstacle
avoid theatobstacle
the cost
atofthe costfor
time ofpath
time search.
for pathFrom
search.theFrom
above theexperiments,
above experiments,
it is clearit is clear
that ourthat our algorithm
algorithm is suit-
isable
suitable
for a wide range of dynamic environments, and the following experiments will will
for a wide range of dynamic environments, and the following experiments vali-
validate the algorithm in more complex dynamic environments.
date the algorithm in more complex dynamic environments.
Table 8. Performance analysis of IACO-IDWA for path planning.
Table 8. Performance analysis of IACO-IDWA for path planning.
Simulation Experiment
Simulation Experiment Chi Chi
et al.et[7]al. [7] Improved Algorithm
Improved Algorithm[7][7] This
ThisPaper
Paper
Length
Length of the robot’s
of the robot’s driving path/m
driving path/m 5.8415
5.8415 5.9656
5.9656 6.0727
6.0727
1 1 Optimal search time /s
Optimal search time /s 73.3436
73.3436 89.7258
89.7258 81.2701
81.2701
Length of the robot’s
Length driving path/m
of the robot’s driving path/m 5.8415
5.8415 6.0905
6.0905 6.2377
6.2377
2 2 Optimal search time
Optimal /s time /s
search 106.3393
106.3393 125.5351
125.5351 76.1026
76.1026
Length of the robot’s
Length driving path/m
of the robot’s driving path/m 5.9284
5.9284 6.4851
6.4851 7.0157
7.0157
3 3 Optimal search time /s 82.5480 99.0592 124.8771
Optimal search time /s 82.5480 99.0592 124.8771

11 11 11

10 10 6
10 6
6

5 5
9 5 9 9

4 4
4 8
8 8
3
3
3 4 5 6 7
4 5 6 7
7 4 5 6 7 7 7

6 6 6

5 5 5

4 4 4

3 3 3

2 2 2

1 1 1
1 2 3 4 5 6 7 8 9 10 11 1 2 3 4 5 6 7 8 9 10 11 1 2 3 4 5 6 7 8 9 10 11

(a) (b) (c)

Machines 2021, 9, x FOR PEER REVIEWFigure21.
Figure 21. Comparison
Comparison of
ofpath
pathplanning
planningresults
resultsforfor
unknown dynamic
unknown environment
dynamic 1; (a)
environment 1; Chi
(a)ofet
23 Chi al.
28
et[7];
al. (b)
[7];Improved algorithm
(b) Improved [7]; and
algorithm (c) This
[7]; and paper.
(c) This paper.

11 11 11

10 10 10
7
7
7
9 9 9 6

6 6
5
8 8 8
5 4
5

7 7 7 3
4 4 3 4 5 6 7

6 3 6 6
3
3 4 5 6 7 3 4 5 6 7

5 5 5

4 4 4

3 3 3

2 2 2

1 1 1
1 2 3 4 5 6 7 8 9 10 11 1 2 3 4 5 6 7 8 9 10 11 1 2 3 4 5 6 7 8 9 10 11

(a) (b) (c)

Figure 22. Comparison of path planning results for unknown dynamic environment 2; (a) Chi et al.
Figure 22. Comparison of path planning results for unknown dynamic environment 2; (a) Chi
[7]; (b) Improved algorithm [7]; and (c) This paper.
et al. [7]; (b) Improved algorithm [7]; and (c) This paper.
11 11 11

10 10
7 10 8
7

9 9 9
6 7
6

8 8 8 6
5
5
 4 4 4

3 3 3

2 2 2

1 1 1
1 2 3 4 5 6 7 8 9 10 11 1 2 3 4 5 6 7 8 9 10 11 1 2 3 4 5 6 7 8 9 10 11

(a) (b) (c)

Machines 2022, 10, 50 24 of 29
Figure 22. Comparison of path planning results for unknown dynamic environment 2; (a) Chi et al.
[7]; (b) Improved algorithm [7]; and (c) This paper.

11 11 11

10 10
7 10 8
7

9 9 9
6 7
6

8 8 8 6
5
5

7 7 7 5
4 2 3 4 5
3 4 5 6 4
2 3 4 5 7
6 6 6
6

5 5 5 5

4 4 4 4
2 3 4 5

3 3 3

2 2 2

1 1 1
1 2 3 4 5 6 7 8 9 10 11 1 2 3 4 5 6 7 8 9 10 11
1 2 3 4 5 6 7 8 9 10 11

(a) (b) (c)

Figure 23.
Figure 23. Comparison
Comparisonofofpath
pathplanning results
planning forfor
results unknown dynamic
unknown environment
dynamic 3; (a)3;Chi
environment (a)etChi
al.
[7]; (b) Improved algorithm [7]; and (c) This paper.
et al. [7]; (b) Improved algorithm [7]; and (c) This paper.

6.3.3. Complex Dynamic Environments

The above experiments have verified the effectiveness of IACO global path planning
and the obstacle avoidance capability of IDWA. This simulation further demonstrates the
dynamic obstacle
dynamic obstacleavoidance
avoidancefeasibility
feasibilityofofthe
thefusion
fusion algorithm
algorithm in in complex
complex environments
environments by
by setting
setting up up different
different levels
levels of unknown
of unknown complex
complex environments,
environments, illustrated
illustrated by abysimulation
a simula-
example below.
tion example below.
In this example, firstly,
firstly, aa static
static environment
environment for for the
the robot
robot isisestablished
established in 40×
in aa40 × 40
grid environment, and the optimal global path with a path length of 55.5667m from the
starting point (1.5, 40.5) to the endpoint (40.5, 1.5) is planned based on IACO, as shown
by the black dashed line in in Figure
Figure 24.
24. Next, setting unknown dynamic obstacles in this
environment,
environment, the robot moves along the global path, judges the surrounding environment
in real-time, and avoids obstacles according to their conditions. The experimental results
Machines 2021, 9, x FOR PEER REVIEW 24 of 28
are
are shown
shownininTable
Table9 and
9 and Figures
Figures24–26, andand
24–26, duedueto the
to difference in computer
the difference performance,
in computer perfor-
the algorithm
mance, runningrunning
the algorithm time doestimenot
doesrepresent the actual
not represent robot robot
the actual movement time.time.
movement In this
In
case, the number of control nodes of the robot represents its motion
this case, the number of control nodes of the robot represents its motion time, and every time, and every ten
Simulation
tenExperiment
nodes simulate
nodes 41s in1snatural
simulate 58.4793 environments. 1248
environments.
in natural 524.4448

Table 9. Comparison table of robot paths.

Number of Robot
Path Length/m Optimal Search Time/s
Control Nodes
Static path planning 55.5667 —— 2.2415
Simulation Experiment 1 56.2028 1206 357.1979
Dynamic path
Simulation Experiment 2 56.6224 1215 417.6458
planning
Simulation Experiment 3 57.3136 1228 437.0259

Figure24.
Figure 24.Global
Globaloptimal
optimalpath
pathofofthe
therobot.
robot.

0.6
0
0.4 Attitude angle
Line speed
Angular velocity -0.3
0.2
-0.6
0 -0.9
-0.2 -1.2

-0.4 -1.5
0 200 400 600 800 1000 1200 0 200 400 600 800 1000 1200
Number of control points Number of control points

(a) (b)
Figure 25. Variation of robot motion parameters in a dynamic environment 4; (a) Diagram of the
Machines 2022, 10, 50 25 of 29

Figure 24. Global optimal path of the robot.

Figure 24. Global optimal path of the robot.
0.6
0
0.4 0.6 Line speed Attitude angle
-0.3 0
Angular velocity Attitude angle
0.2 0.4 Line speed
Angular velocity -0.6 -0.3
0 0.2 -0.9 -0.6
-0.2 0
-1.2 -0.9

-0.4 -0.2 -1.5 -1.2

0 200 400 600 800 1000 1200 0 200 400 600 800 1000 1200
-0.4 Number of control points -1.5 Number of control points
0 200 400 600 800 1000 1200 0 200 400 600 800 1000 1200
(a)
Number of control points (b)
Number of control points

(a) (b)
Figure
Figure25.25.Variation
Variationofof
robot motion
robot motionparameters
parameters inin
a dynamic
a dynamic environment
environment4; 4;
(a)(a)
Diagram
Diagramofof
the
the
Figure
robot’s 25. Variation
linear and of robot
angular motion
velocity parameters
variation; (b) in a dynamic
Diagram of the environment
robot’s angular 4; (a) Diagram of the
variation.
robot’s linear and angular velocity variation; (b) Diagram of the robot’s angular variation.
robot’s linear and angular velocity variation; (b) Diagram of the robot’s angular variation.

Machines 2021, 9, x FOR PEER REVIEW 25 of 28

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

(c) (d)
Figure
Figure 26.
26. Robot
Robotpath
pathplanning
planningprocess
processin
inaadynamic
dynamicenvironment;
environment;(a)
(a)Simulation
SimulationExperiment
Experiment1;1;(b)
(b)
Simulation Experiment 2; (c) Simulation Experiment 3; and (d) Simulation Experiment 4.
Simulation Experiment 2; (c) Simulation Experiment 3; and (d) Simulation Experiment 4.

In Simulation Experiment 1, the robot performs real-time path planning along the
Table 9. Comparison table of robot paths.
global path, and the path is essentially the same as the original. Once again, IDWA’s track
tracking capability is proven. The robot passed safelyofunder
Number Robotthis pathOptimal
withoutSearch
colliding
Path Length/m
any obstacles, and the final path length is 56.2028 m, taking
Control Nodes120.6 s. Time/s
Static path planning
Simulation Experiment 2 is
55.5667
based on Experiment ——
1. We set up a mobile obstacle with
2.2415
an actual radius 𝑅_𝑜𝑏 of 0.3m, a threat circle 𝑅_𝑆𝑜𝑏 of 0.6 m, and an unknown static block.
Simulation Experiment 1 56.2028 1206 357.1979
We designed the mobile barrier to move in opposite directions to the robot, where: the
Dynamic path Simulation Experiment 2 56.6224 1215 417.6458
planning starting
Simulation point 3coordinates 57.3136
Experiment are (10.5, 33.5); the endpoint
1228 coordinates are437.0259
(3.5, 38.5); the
movement
Simulation speed
Experiment 4 is 0.16m/s; and the trajectory is marked
58.4793 1248 in black dashed. 524.4448
At the moment
of process 1, the robot finds the mobile obstacle moving towards itself with a speed of
0.16m/s through the sensor, executes the obstacle avoidance strategy, and completes the
obstacle avoidance at the moment of process 2. An unknown static obstacle is sensed at
position (14.5, 29.5) during the movement towards the local target point, and the obstacle
avoidance strategy is executed again. By the improved ℎ𝑒𝑎𝑑𝑖𝑛𝑔(𝑣, 𝑤) function, the robot
eventually moves to the endpoint, and the final path length is 56.6224 m, taking 121.5 s.
Machines 2022, 10, 50 26 of 29

In Simulation Experiment 1, the robot performs real-time path planning along the
global path, and the path is essentially the same as the original. Once again, IDWA’s track
tracking capability is proven. The robot passed safely under this path without colliding
any obstacles, and the final path length is 56.2028 m, taking 120.6 s.
Simulation Experiment 2 is based on Experiment 1. We set up a mobile obstacle with
an actual radius R_ob of 0.3 m, a threat circle R_Sob of 0.6 m, and an unknown static
block. We designed the mobile barrier to move in opposite directions to the robot, where:
the starting point coordinates are (10.5, 33.5); the endpoint coordinates are (3.5, 38.5); the
movement speed is 0.16 m/s; and the trajectory is marked in black dashed. At the moment
of process 1, the robot finds the mobile obstacle moving towards itself with a speed of
0.16 m/s through the sensor, executes the obstacle avoidance strategy, and completes the
obstacle avoidance at the moment of process 2. An unknown static obstacle is sensed at
position (14.5, 29.5) during the movement towards the local target point, and the obstacle
avoidance strategy is executed again. By the improved heading(v, w) function, the robot
eventually moves to the endpoint, and the final path length is 56.6224 m, taking 121.5 s.
Simulation Experiment 3 added a further mobile obstacle 2 with an actual radius R_ob
of 0.2 m and a threat circle radius R_Sob of 0.4 m and larger volumes of unknown static
obstacles to Experiment 2. We designed the mobile obstacle 2 to move in the same direction
as the robot, where: the starting coordinates are (7.5, 34.5), the ending coordinates are (15.5,
27.5), and the speed of movement is 0.08 m/s. After avoiding the mobile obstacle 1, the
robot detects a mobile obstacle 2 ahead occupying the original path at a rate of 0.08 m/s and
again executes the obstacle avoidance strategy and overtakes it at the moment of process 3.
The unknown static obstacles are sensed at position (25, 20), which occupies the robot’s
path, and the robot executes the obstacle avoidance strategy until the end. The final path
length is 57.3136 m, and the time taken is 122.8 s.
Simulation Experiment 4 further builds on Experiment 3 by adding two mobile obsta-
cles and several unknown static obstacles, where: mobile obstacle 3 has an actual radius
R_ob of 0.4 m and a threat circle radius R_Sob of 0.8 m, and we designed it to come from
the side of the robot with starting coordinates of (39.5, 18.5) and ending coordinates of (16.5,
25.5), moving at a speed of 0.288 m/s; the actual radius R_ob of mobile obstacle 4 is 0.375 m
and a threat circle radius R_Sob of 0.75 m, and we design it in the robot’s global path, with
the starting point coordinates are (38.5, 13.5) and the ending point coordinates are (34.5,
9.5), moving at a speed of movement is 0.09 m/s. After avoiding mobile obstacle 1 and
mobile obstacle 2, the robot detects a mobile obstacle 3 coming towards itself at 0.288 m/s
on its side at process 4 and executes an obstacle avoidance strategy to avoid it after process
5 and 6 successfully. During the local exploration process, mobile obstacle 4 is detected to
have stopped moving, and the robot treated it as an unknown static obstacle to avoid and
eventually moved safely to the end position. The final path length is 58.4793 m, and the
time taken is 124.8 s.
The changes in linear velocity, angular velocity, and attitude angle of the robot in the
Simulation Experiment 4 have been recorded in Figure 25. We can see that the robot’s
motion is generally stable after improving the evaluation function, and it can maintain
a uniform speed throughout the motion, and the change of angular velocity is relatively
smooth.

7. Conclusions and Future Work

This paper investigates a new hybrid algorithm for dynamic path planning in view
of the ant colony algorithm’s excellent robustness and search capability and the dynamic
window approach’s local obstacle avoidance advantages. To further refine the dynamic
factors of the robot in the natural environment, a new dynamic environment construction
method is proposed. The robot obtains a global reference trajectory based on an improved
ant colony algorithm in unknown territory and uses mounted sensors to gain information
about the strange environment for dynamic obstacle avoidance.
Machines 2022, 10, 50 27 of 29

We are taking into account the poor adaptability of traditional ACO to complex
environments and the difference between the path and the actual requirements of the robot.
We propose a non-uniform initial pheromone method to avoid the blindness of the ant’s
initial search. Then, the smoothing heuristic function with corner suppression and the
improved retraction strategy is used to optimize the ant’s ability to explore paths in complex
environments. To address the inefficiencies of the ACO, pheromones are updated in layers.
Moreover, we present a path smoothing method to satisfy the motion requirements of the
robot better in a grid environment.
Considering the poor navigation capability and poor dynamic obstacle avoidance
capability of the traditional DWA, we construct the robot model with kinematic constraints
using IDWA and utilize the global path planned by IACO as the robot’s navigation infor-
mation. Then we improve the sampling window and evaluation function, which eventually
enhances the robot’s path tracking capability, dynamic obstacle avoidance capability, and
motion stability. The experiment shows that the proposed method enables the robot to
efficiently and safely navigate in global static environments and better dynamic obstacle
avoidance in complex dynamic environments.
In this paper, the robot is kinematically constrained, but this is neglected when plan-
ning global paths, and we will consider it in path planning and path smoothing processes
in the subsequent studies. Moreover, the dynamic environment considered in this paper is
limited to a two-dimensional environment, and it is hoped that our proposed approach
will be of some use to subsequent researchers in studying three-dimensional dynamic
environments.

Author Contributions: Conceptualization, L.Y. and P.L.; methodology, L.Y.; software, L.Y.; validation.
L.Y. and L.F.; formal analysis, L.F. and P.L.; resources, J.M.; writing—original draft preparation, L.Y.;
and writing—review and editing, L.F. and N.G. All authors have read and agreed to the published
version of the manuscript.
Funding: This work was supported by the National Natural Science Foundation of China (61163051)
and the Yunnan Provincial Key R&D Program Project “Research on key technologies of industrial
robots and its application demonstration in intelligent manufacturing”(202002AC080001). The APC
was funded by Associate Professor Lixia Fu.
Institutional Review Board Statement: Not applicable.
Informed Consent Statement: Not applicable.
Data Availability Statement: Not applicable.
Conflicts of Interest: The authors declare that there are no conflicts of interests regarding the publi-
cation of this article.

References
1. Pandey, A.; Parhi, D.R. Optimum path planning of mobile robot in unknown static and dynamic environments using Fuzzy-Wind
Driven Optimization algorithm. Def. Technol. 2017, 13, 47–58. [CrossRef]
2. Pandini, M.M.; Spacek, A.D.; Neto, J.M.; Ando, O.H. Design of a didatic workbench of industrial automation systems for
engineering education. IEEE Lat. Am. Trans. 2017, 15, 1384–1391. [CrossRef]
3. Sidoti, D.; Avvari, G.V.; Mishra, M.; Zhang, L.; Nadella, B.K.; Peak, J.E.; Hansen, J.A.; Pattipati, K.R. A multiobjective path-
planning algorithm with time windows for asset routing in a dynamic weather-impacted environment. IEEE Trans. Syst. Man
Cybern. Syst. 2016, 47, 3256–3271. [CrossRef]
4. Roy, D.; Chowdhury, A.; Maitra, M.; Bhattacharya, S. Geometric Region-Based Swarm Robotics Path Planning in an Unknown
Occluded Environment. IEEE Trans. Ind. Electron. 2020, 68, 6053–6063. [CrossRef]
5. Josef, S.; Degani, A. Deep Reinforcement Learning for Safe Local Planning of a Ground Vehicle in Unknown Rough Terrain. IEEE
Robot. Autom. Lett. 2020, 5, 6748–6755. [CrossRef]
6. Montiel, O.; Orozco-Rosas, U.; Sepúlveda, R. Path planning for mobile robots using Bacterial Potential Field for avoiding static
and dynamic obstacles. Expert Syst. Appl. 2015, 42, 5177–5191. [CrossRef]
7. Chi, X.; Li, H.; Fei, J.Y. Research on random obstacle avoidance method for robots based on the fusion of improved A~* algorithm
and dynamic window method. Chin. J. Sci. Instrum. 2021, 42, 132–140. [CrossRef]
Machines 2022, 10, 50 28 of 29

8. Qi, J.; Yang, H.; Sun, H. MOD-RRT*: A sampling-based algorithm for robot path planning in dynamic environment. IEEE Trans.
Ind. Electron. 2020, 68, 7244–7251. [CrossRef]
9. Sivaranjani, S.; Nandesh, D.A.; Raja, R.K.; Gayathri, K.; Ramanathan, R. An Investigation of Bug Algorithms for Mobile Robot
Navigation and Obstacle Avoidance in Two-Dimensional Unknown Static Environments. In Proceedings of the 2021 International
Conference on Communication information and Computing Technology (ICCICT), Mumbai, India, 25–27 June 2021; pp. 1–6.
[CrossRef]
10. Lee, D.H.; Lee, S.S.; Ahn, C.K.; Shi, P.; Lim, C.-C. Finite Distribution Estimation-based Dynamic Window Approach to Reliable
Obstacle Avoidance of Mobile Robot. IEEE Trans. Ind. Electron. 2021, 68, 9998–10006. [CrossRef]
11. Yang, L.; Fu, L.; Li, P.; Mao, J.; Guo, N.; Du, L. LF-ACO: An effective formation path planning for multi-mobile robot. Math. Biosci.
Eng. 2022, 19, 225–252. [CrossRef] [PubMed]
12. Luo, Q.; Wang, H.; Zheng, Y.; He, J. Research on path planning of mobile robot based on improved ant colony algorithm. Neural
Comput. Appl. 2020, 32, 1555–1566. [CrossRef]
13. Dai, X.; Long, S.; Zhang, Z.; Gong, D.W. Mobile robot path planning based on ant colony algorithm with A* heuristic method.
Front. Neurorobotics 2019, 13, 15. [CrossRef] [PubMed]
14. You, X.; Liu, S.; Zhang, C. An improved ant colony system algorithm for robot path planning and performance analysis. Int. J.
Robot. Autom. 2018, 33, 527–533. [CrossRef]
15. Xu, K.; Lu, H.; Huang, Y.; Hu, S.J. Robot path planning based on double-layer ant colony optimization algorithm and dynamic
environment. Acta Electonica Sin. 2019, 47, 2166. [CrossRef]
16. Ao, B.Q.; Yang, S.; Ye, Z.H. Improved ant colony algorithm for unmanned surface vehicle smooth path planning. Control. Theory
Appl. 2021, 38, 1006–1014. [CrossRef]
17. Lazarowska, A. Discrete artificial potential field approach to mobile robot path planning. IFAC-PapersOnLine 2019, 52, 277–282.
[CrossRef]
18. Zhong, X.; Tian, J.; Hu, H.; Peng, X.F. Hybrid Path Planning Based on Safe A* Algorithm and Adaptive Window Approach for
Mobile Robot in Large-Scale Dynamic Environment. J. Intell. Robot. Syst. 2020, 99, 65–77. [CrossRef]
19. Yuan, X.; Yuan, X.; Wang, X. Path Planning for Mobile Robot Basedon Improved Bat Algorithm. Sensors 2021, 21, 4389. [CrossRef]
20. Chen, T.J.; Sun, Y.; Dai, W.; Tao, W.; Liu, S. On the Shortest and Conflict-Free Path Planning of Multi-AGV System Based on
Dijkstra Algorithm and the Dynamic Time-Window Method. Adv. Mater. Res. 2013, 645, 267–271. [CrossRef]
21. Zhang, H.; Zhang, C.; Yang, W.; Chen, C.-Y. Localization and navigation using QR code for mobile robot in indoor environment. In
Proceedings of the IEEE International Conference on Robotics & Biomimetics, Zhuhai, China, 6–9 December 2015; pp. 2501–2506.
[CrossRef]
22. Liu, L.-S.; Lin, J.-F.; Yao, J.-X.; He, D.-W.; Zheng, J.-S.; Huang, J.; Shi, P. Path Planning for Smart Car Based on Dijkstra Algorithm
and Dynamic Window Approach. Wirel. Commun. Mob. Comput. 2021, 2021, 1–12. [CrossRef]
23. Shao, L.; Li, Q.; Li, C.; Sun, W.T. Mobile Robot Path Planning Based on Improved Ant Colony Fusion Dynamic Window Approach.
In Proceedings of the 2021 IEEE International Conference on Mechatronics and Automation (ICMA), Takamatsu, Japan, 8–11
August 2021; pp. 1100–1105. [CrossRef]
24. Jin, Q.; Tang, C.; Cai, W. Research on Dynamic Path Planning Based on the Fusion Algorithm of Improved Ant Colony
Optimization and Dynamic Window Method. IEEE Access 2021. [CrossRef]
25. Zhang, X.; Chen, X. Path Planning Method for Unmanned Surface Vehicle Based on RRT* and DWA. In Proceedings of the
International Conference on Multimedia Technology and Enhanced Learning, Virtual Event, 8–9 April 2021; pp. 518–527.
[CrossRef]
26. Chen, Y.-W.; Chiu, W.-Y. Optimal robot path planning system by using a neural network-based approach. In Proceedings of the
2015 International Automatic Control Conference (CACS), Yilan, Taiwan, 18–20 November 2015; pp. 85–90. [CrossRef]
27. Wu, P.; Cao, Y.; He, Y.Q.; Li, D.C. Vision-based robot path planning with deep learning. In International Conference on Computer
Vision Systems; Springer: Cham, Switzerland, 2017; pp. 101–111. [CrossRef]
28. Wu, Q.; Zhang, Y.; Guo, K.; Wang, X. LSTM combined with reinforcement learning dynamic environment path planning algorithm.
J. Chin. Comput. Syst. 2021, 42, 334–339. [CrossRef]
29. Mnih, V.; Kavukcuoglu, K.; Silver, D.; Graves, A.; Antonoglou, L.; Wierstra, D.; Riedmiller, M. Playing atari with deep reinforce-
ment learning. arXiv 2013, arXiv:1312.5602.
30. Mirowski, P.; Pascanu, R.; Viola, F.; Soyer, H.; Ballard, A.J.; Banino, A.; Denil, M.; Goroshin, R.; Sifre, L.; Kavukcuoglu, K.; et al.
Learning to navigate in complex environments. arXiv 2016, arXiv:1611.03673.
31. Panov, A.I.; Yakovlev, K.S.; Suvorov, R. Grid path planning with deep reinforcement learning: Preliminary results. Procedia
Comput. Sci. 2018, 123, 347–353. [CrossRef]
32. Lei, X.; Zhang, Z.; Dong, P. Dynamic path planning of unknown environment based on deep reinforcement learning. J. Robot.
2018, 2018, 1–10. [CrossRef]
33. Lv, L.; Zhang, S.; Ding, D.; Wang, Y.X. Path planning via an improved DQN-based learning policy. IEEE Access 2019, 7,
67319–67330. [CrossRef]
34. Chen, R.N.; Wen, C.C.; Peng, L.; You, C.Z. Application of improved A~* algorithm in robot indoor path planning. Comput. Appl.
2019, 39, 1006–1011. [CrossRef]
Machines 2022, 10, 50 29 of 29

35. Wu, X.; Wei, G.; Song, Y.; Huang, X. Improved ACO-based path planning with rollback and death strategies. Syst. Sci. Control.
Eng. 2018, 6, 102–107. [CrossRef]
36. Zhang, H.; He, L.; Yuan, L.; Ran, T. Path planning for mobile robots based on improved two-layer ant colony algorithm. Control.
Decis. Mak. 2021, 1–10. [CrossRef]
37. Yang, H.; Qi, J.; Miao, Y.; Sun, H.; Li, J. A New Robot Navigation Algorithm Based on a Double-Layer Ant Algorithm and
Trajectory Optimization. IEEE Trans. Ind. Electron. 2019, 66, 8557–8566. [CrossRef]
38. Huang, D.J.; Lei, X.; Jiang, C.F.; Chen, Y.M.; Ding, Y.D. Global path planning for film group animation based on improved JPS
algorithm. J. Shanghai Univ. Nat. Sci. Ed. 2018, 24, 694–702. [CrossRef]
39. Chang, L.; Shan, L.; Dai, Y.W.; Qi, Z.D. Improved DWA-based multi-robot formation control in unknown environments. Control.
Decis. Mak. 2021, 1–10. [CrossRef]
40. Liu, Z.; Liu, H.; Lu, Z.; Zeng, Q. A Dynamic Fusion Pathfinding Algorithm Using Delaunay Triangulation and Improved A-Star
for Mobile Robots. IEEE Access 2021, 9, 20602–20621. [CrossRef]
41. Chang, L.; Shan, L.; Jiang, C.; Dai, Y.W. Reinforcement based mobile robot path planning with improved dynamic window
approac h in unknown environment. Auton. Robot. 2021, 45, 51–76. [CrossRef]