Distributed Multi-Mobile Robot Path Planning and Obstacle Avoidance Based on ACO–DWA in Unknown Complex Terrain

Abstract

Multi-robot systems are popularly distributed in logistics, transportation, and other fields. We propose a distributed multi-mobile robot obstacle-avoidance algorithm to coordinate the path planning and motion navigation among multiple robots and between robots and unknown territories. This algorithm fuses the ant colony optimization (ACO) and the dynamic window approach (DWA) to coordinate a multi-robot system through a priority strategy. Firstly, to ensure the optimality of robot motion in complex terrains, we proposed the dual-population heuristic functions and a sort ant pheromone update strategy to enhance the search capability of ACO, and the globally optimal path is achieved by a redundant point deletion strategy. Considering the robot’s path-tracking accuracy and local target unreachability problems, an adaptive navigation strategy is presented. Furthermore, we propose the obstacle density evaluation function to improve the robot’s decision-making difficulty in high-density obstacle environments and modify the evaluation function coefficients adaptively by combining environmental characteristics. Finally, the robots’ motion conflict is resolved by combining our obstacle avoidance and multi-robot priority strategies. The experimental results show that this algorithm can realize the cooperative obstacle avoidance of AGVs in unknown environments with high safety and global optimality, which can provide a technical reference for distributed multi-robot in practical applications.

1. Introduction

Mobile robots have been increasingly widely used in many fields, such as logistics and transportation, daily services, and industrial manufacturing, for their autonomous navigation and flexible movement. Path planning is an integral and essential part of many technologies for mobile robots [1], aiming to plan a safe optimal or suboptimal route, from the starting point to the endpoint, for the robot in a complex environment. We divide it into global and local path-planning algorithms depending on the robot’s perception of the environment during the movement.

1.1. Global Path-Planning Algorithm

The global path-planning algorithms can be divided into traditional and intelligent algorithms according to their development. Traditional methods include the A* algorithm [2], simulated annealing algorithm [3], fast random tree method [4], etc.; intelligent methods mainly include the ant colony algorithm, particle swarm algorithm [5], genetic algorithm [6], etc. The ant colony algorithm [7] is a kind of bionic algorithm that is based on ant foraging; it has the advantages of positive feedback, better adaptability, robustness, and strong optimization ability. The practical application in path planning is prone to problems such as blindness of the preliminary search, low search efficiency, and easily falling into local optimum and deadlock. Accordingly, the initial pheromone distribution rule, heuristic information function, and pheromone update rule are the improvements that have received a lot of attention. Luo et al. [8] improved the initial pheromone assignment method based on the starting and ending points, as well as the ant location relationship, not only avoiding the initial search blindness of the algorithm but also further improving the search speed. Inspired by the A* algorithm, Dai et al. [9] used the A* algorithm as the heuristic information for path search to improve the convergence speed of ACO and reduce the cumulative number of turning points and cumulative bending angle by adding a bending suppression operator to this heuristic function. Miao et al. [10] proposed a high-quality ant pheromone update rule which only updated pheromones for those ants that could reach the target point successfully and discarded the ants stuck in a deadlock. Heuristic information and adaptive pheromone volatility factor adaptive adjustment strategies were introduced to balance the convergence and global search ability of ACO. Wang et al. [11] solved the ant deadlock problem of 3D path planning with a fallback strategy and set the pheromone of the deadlocked region to 0, which was simple and effective. Zhang et al. [12] designed a free pathfinding-pruning strategy for the complex environment with severe ant colony deadlock, allowing ants to reselect the next grid when a deadlock occurs and removing the redundant paths if they escape from the deadlocked region.

1.2. Local Path-Planning Algorithm

Some local path-planning algorithms that receive much attention include the Artificial Potential Field (APF) [13] and the Dynamic Window Approach (DWA). The APF drives the robot motion by the gravitational and repulsive forces of the virtual potential field, which is widely used in real-time obstacle-avoidance research, as it has a simple structure and good real-time performance; however, it is prone to dilemma near the force equilibrium point. DWA is a classical method proposed by Fox et al. [14] that converts the position constraint of a mobile robot into a velocity constraint problem and obstacle avoidance into an optimal velocity execution problem. Chang et al. [15] made improvements to the unreasonable dynamic window approach by Q-learning and modified and extended the original evaluation function to improve the navigation action, thus demonstrating the higher navigation efficiency and success rate of this method in an unknown environment. In some cases, there is a contradictory relationship between the heading angle evaluation function and the velocity evaluation function, and this leads to problems such as frequent direction changes and untimely evasion when the robot is on the optimal trajectory selected. For this reason, Liu et al. [16] introduced a direction-change-suppression evaluation factor into the evaluation function of DWA that significantly reduced the excessive influence of environmental factors on the evaluation function and the unnecessary steering frequency generated by the robot.

1.3. Fusion Algorithm Applied to Single-Robot Path Planning

Based on the above studies and discussions, the global path-planning algorithm combined with the local obstacle avoidance algorithm has been a fascinating topic for many scholars, enhancing robot path optimality and obstacle-avoidance capability. Zhong et al. [17] combined the secure A* algorithm with DWA to perform path-planning experiments in a large-scale dynamic environment, which achieved real-time motion planning of the robot with both path tracking and obstacle avoidance. Gao et al. [18] combined adaptive DWA with the A* algorithm to achieve robot motion planning, while avoiding DWA from falling into local optimal solutions by using the globally optimal path as a guide and adaptively adjusting the weights of the evaluation function. Fewer scholars have focused on the combined effects of dynamic obstacles and sea state conditions on USVs. Chen [19] et al. proposed an A* hybrid DWA path-planning algorithm. In contrast to most studies, the evaluation coefficients of the robot are adaptively adjusted according to the sea state, where the weights of velocity are reduced and the weights of distance are increased, as the sea state is poor; for example. Lin et al. [20] proposed a two-layer path-planning method based on the improved APF for global path planning and the enhanced DWA with fuzzy control for local path planning. The collision-risk index and relative distance of the robot and the obstacle are considered to dynamically adjust the weights of the evaluation function and finally obtain a reasonable motion path. Kashyap et al. [21] proposed a strategy to fuse DWA with teaching and learning optimization algorithms with promising results in a multi-robot system under dynamic environments.

1.4. Fusion Algorithm Applied to Multi-Robot Path Planning

With the development of technology, it has become difficult for a single mobile robot to complete numerous tasks in the face of more demanding requirements and more complex environments, and multi-mobile robots have entered the vision of researchers. Compared with single robots, multi-mobile robotic systems (MRSs) have the advantages of good flexibility and a wide exploration range, and they can accomplish complex tasks faster and better than a single robot. Therefore, they are used in various environments widely, such as performing sea rescue, formation patrol, mapping tasks, cargo handling, area search, etc.

Planning an optimal motion path for every mobile robot is essential to improve efficiency, but it is more important to ensure the autonomy and coordination of multiple robots during motion [22]. Nazarahari et al. [23] presented a novel multi-objective Enhanced Genetic Algorithm (EGA) to find the optimal path in terms of path length, smoothness, and safety in a continuous environment that is not limited by the grids in a discrete gridded environment. Moreover, an innovative Artificial Potential Field (APF) method was presented to build a set of feasible initial paths in an efficient deterministic fashion. Finally, a new version of the proposed algorithm was introduced to handle the multi-robot cooperative path problem. Bae et al. [24] proposed a deep Q-learning combined with CNN for a multi-robot path-planning algorithm, where the CNN was used for analyzing the relevant image information of the environment and the robot navigated based on deep Q-learning, experimentally demonstrating the flexibility and efficiency of the robot in various settings. Inspired by multi-objective particle swarm optimization, Thabit et al. [25] proposed a new method for multi-robot path planning in unknown environments, with the objectives of path optimality, safety, and smoothness to find the best path for the robot, also introducing probabilistic window combined with sensors to obtain motion information as a way to select a higher adaptive path. Yang et al. [26] proposed a multi-robot path-planning method for ACO that fuses the leader–follower approach, establishes a formation model for leader and follower ants, and updates the pheromone based on the overall characteristics of the formation; the effectiveness of the method was verified based on simulation experiments in MATLAB and ROS. Das et al. [27] added the consideration of path deviation and energy consumption optimization by embedding the social and cognitive behavior of an improved particle swarm algorithm (IPSO) into the Newtonian gravity of an enhanced gravity search algorithm (IGSA). They proposed IPSO–IGSA to implement path planning for multiple robots in dynamic environments and improve search capability by simultaneously updating particle positions, using IPSO velocity and IGSA acceleration. In subsequent research, the authors [27] also investigated the multi-robot collision-free planning problem by mixing improved particle swarm optimization (IPSO) and evolutionary operators (EOPs) [28]. In addition, some scholars [29] set different priorities for each robot by prioritization methods, thus reducing the possibility of robot collisions.

Most scholars have focused on multi-robot navigation methods mostly in relation to reinforcement learning, neural networks, and deep learning [24,30,31,32], which suffer from harsh data sets and training models. More important, the robot dynamics constraints are ignored. Given this, this paper proposes a real-time motion-planning method for distributed multi-robots based on the ant colony algorithm fused with the dynamic windowing approach, without over-reliance on a priori information to decision-making, with the following main contributions:

(1)

Global path phase of ACO: Firstly, the concept of obstacle occupancy ratio is introduced to the initial pheromone allocation principle, avoiding the blindness of the ant’s search. We use the dual-population collaboration strategy with different heuristic functions to improve planning efficiency in complex environments. The sort strategy of the ant is proposed in the pheromone update rule to make full use of the quality of ants to improve the search speed and adjust the pheromone volatility coefficient adaptively to avoid local optimum. Considering the motion characteristics of the robot, the optimal navigation path is obtained by removing redundant path nodes.

(2)

Local path phase of DWA: Firstly, we construct a kinematic model of the robot with kinematic constraints and obstacle environment constraints based on DWA. Secondly, an adaptive navigation strategy is proposed to guide the mobile robot to move regarding the global path reasonably, improving the robot’s safety and path-tracking accuracy significantly in complex environments. Then, based on the traditional evaluation function, we propose a new obstacle density evaluation function to guide the robot reasonably through the complex terrain with high-density obstacles. In addition, to regulate the multiple indicator conflict problem rationally during robot motion and improve the judgment of an optimal path, we adjust the weight of the heading angle evaluation function and the speed evaluation function by combining the obstacle environment effectiveness.

(3)

Priority strategy to coordinate the multi-robot motion: We combine the above single-robot navigation and obstacle-avoidance strategies with the mainstream multi-robot priority strategy, resolving the conflicts between robots and robots, and between robots and unknown static obstacles.

This paper is organized as follows: Section 3 describes our ACO. Section 4 performs the DWA improvement. Section 5 describes the obstacle-avoidance rules among multiple robots. Section 6 discusses the conducted experiments. Section 7 concludes the whole paper and discusses future work.

2. Problem Description

2.1. Environment Modeling

Building an environment map that provides accurate robot motion information (mainly containing starting point, endpoint, obstacle information, etc.) is essential. The grid method is one of the most widely used methods in environmental modeling. The main idea is to divide the entire free space into a grid of uniform cells of the same size, where the model’s accuracy is determined by the actual environment. The grid method divides robots’ workspace into a passable free area and an impassable obstacle area, represented by the matrix $MAP$, with “0” representing the free grid, “1” representing the obstacle grid, and “2” representing the dynamic obstacle, as in Equation (1).

$$MAP={\left[\begin{array}{cccc}1& 0& \cdots & 0\\ 2& 1& \cdots & 0\\ \cdots & \cdots & \cdots & \cdots \\ 2& 2& \cdots & 1\end{array}\right]}_{n\times n}$$

(1)

As shown in Figure 1, each cell represents a kind of basic environment information, where white grid, ${\Gamma }^f$, and black grid, ${\Gamma }^o$, are both global environments known to the robot a priori, indicating the area with the free passage and the region with obstacles, respectively. In contrast, the red grid, ${\Gamma }^b$, is a local environment with a piece of unknown prior information. To make sure that the robot can obtain the global information, the sequence method is used to mark each grid, and the actual coordinates of the environment corresponding to the grid with serial number $N$ are as follows:

$$\left\{\begin{array}{c}x=\left\{\begin{array}{c}mod\left(N,M\right)-0.5 if mod\left(N,M\right)!=0\\ M+mod\left(N,M\right)-0.5 otherwise\end{array}\right.\\ y=M+0.5-ceil(\frac{N}{M})\end{array}\right.$$

(2)

2.2. Multi-Robot Motion Statement

First, considering a multi-robot model with kinematic constraints and environmental constraints $\left\{ROB1, ROB2,\dots , ROBn\right\}$, we set the starting grid, S, and the target grid, E, for each robot in the map, $MAP$, and defined a set of paths of motion $\left\{ROUT1, ROUT2,\dots , ROUTn\right\}$. This means that, in a static environment with known a priori information, our improved ant colony algorithm gradually generates a safe and passable initial path $\left\{Initial\text{\_}ROUT1, Initial\text{\_}ROUT2,\dots , Initial\text{\_}ROUTn\right\}$ from the starting grid, S, to the target grid, E, and uses the redundancy point deletion strategy to optimize the paths to obtain the most suitable routes $\left\{ROUT1, ROUT2,\dots ROUTn\right\}$ for multi-robot motion in this environment. Then we randomly set several unknown obstacles $\left\{UN\text{\_}Obstacle1, UN\text{\_}Obstacle2,\dots , UN\text{\_}Obstaclem\right\}$, which were then guided by our improved dynamic window approach in $\left\{ROUT1, ROUT2,\dots ,.ROUTn\right\}$ to refer $\left\{ROB1, ROB2,\dots , ROBn\right\}$ to their respective target points, E, and ensure that each robot did not collide with any obstacle during the motion. Moreover, we defined the priority of robot motion, which is necessary to solve the conflict problem with multi-robot systems.

3. Ant Colony Optimization

3.1. Principle of Ant Colony Optimization

Ant colony optimization (ACO) is a stochastic heuristic algorithm that simulates ants’ foraging. Ants select the next node by the pheromone concentration in the path, and the higher the pheromone concentration, the more likely the path is selected. As the ants keep searching, the ants will converge to the optimal path with the highest pheromone concentration. The probability of ant $k$ moving from node $i$ to node $j$ in $t$ iterations is shown in Equation (3).

$$P_{ij}^k\left(t\right)=\left\{\begin{array}{l}\frac{{\left[{\tau }_{ij}\left(t\right)\right]}^{\alpha }{\left[{\eta }_{ij}\left(t\right)\right]}^{\beta }}{{{\displaystyle \sum }}_{j\in allowed}{\left[{\tau }_{ij}\left(t\right)\right]}^{\alpha }{\left[{\eta }_{ij}\left(t\right)\right]}^{\beta }},j\in allowed\\ 0,\mathrm{otherwise}\hfill \end{array}\right.$$

(3)

where $\alpha$ is the information heuristic factor, and the larger the value, the less random the algorithm; $\beta$ is the expectation heuristic factor, and the value must be limited to a small range to avoid optimum local problems; $\mathrm{allowed}$ is the set of subsequent selectable nodes for ant; $P_{ij}^k\left(t\right)$ is the node transfer probability; ${\tau }_{ij}^k\left(t\right)$ is the pheromone concentration on the path; ${\eta }_{ij}^k\left(t\right)$ is the heuristic function; $d_{ij}$ is the Euclidean distance from node $i$ to node $j$, and the relationship between ${\eta }_{ij}$ and $d_{ij}$ can be defined as the following Equation (4):

$${\eta }_{ij}\left(t\right)=\frac{1}{d_{ij}}$$

(4)

After all ants complete one iteration, the pheromones on the path are updated and volatilized according to Equation (5):

$${\tau }_{ij}\left(t+1\right)=\left(1-\rho \right){\tau }_{ij}\left(t\right)+\Delta {\tau }_{ij}\left(t,t+1\right)$$

(5)

In the above equation, the increment of pheromone from $t$ iterations to $t+1$ iterations can be expressed by Equation (6):

$$\Delta {\tau }_{ij}\left(t,t+1\right)=\left\{\begin{array}{c}\frac{Q}{L},Ant k passed through path \left(i,j\right)\\ 0, otherwise\end{array}\right.$$

(6)

where $\rho$ is the pheromone volatility coefficient, $\Delta {\tau }_{ij}$ is the pheromone increment in ant pathfinding, $Q$ is the pheromone intensity, and $L$ denotes the total path length of ant $K$.

3.2. Improved Ant Colony Optimization

3.2.1. The Principle of Initial Pheromone Assignment

The pheromone distribution of the traditional ACO initial stage environment is the same value, which will lead to certain blindness of the ants in the initial path search, lowering the algorithm’s search efficiency and search time to a certain extent. We propose an improved initial pheromone distribution method to avoid the blindness of the ant colony’s initial search path, which reasonably assigns the pheromone values on the cell grid according to the percentage of obstacles around the node. Our method is implemented as follows:

$${\tau }_{ij}\left(0\right)=\epsilon \ast f\left(j\right)$$

(7)

$$f\left(j\right)=1-\frac{C_U\left({\mathrm{allowed}}_j\right)}{8}$$

(8)

where $\epsilon$ is the enhancement factor of the initial pheromone, $f\left(j\right)$ denotes the obstacle avoidance factor, $C_U$ is the complementary set symbol, and $U$ represents the set of neighboring grids of the current grid $I$. Using the $f\left(j\right)$ function, the percentage of free grids in the eight neighboring grids of the current grid can be calculated. When the number of obstacle grids in the neighboring is less, the initial pheromone of this grid is more significant, and conversely, the initial pheromone is smaller. In this way, the ants are guided to move forward, avoiding too much terrain pathfinding by the ants for obstacles and speeding up the convergence of our algorithm.

As shown in Figure 2, the lower initial pheromone concentration for the map edges is set to avoid the ants’ pathfinding toward the edges, leading to deviation from the target point. The initial pheromones of our grids are calculated according to Equation (7) and Equation (8), and it can be seen that the initial pheromone concentration is high at the locations with more minor obstacle grids (areas with high luminance in the figure). On the contrary, the initial pheromone concentration is low in the areas with more obstacles (regions with low luminance in the figure).

3.2.2. Double-Species Colony Heuristic Function

A two-layer ant colony was proposed in the literature [12,13] to enable the ant colony to find the optimal path quickly and efficiently. Accordingly, we divided the original number of $\mathrm{m}$ ant colonies in the traditional ACO into ordinary layer ants with the numbers of $\left[2/3m\right]$ and $\left[1/3m\right]$, respectively, and planning layer ants, where the ordinary layer ant colonies perform first and the planning layer ant colonies achieve the final path. The heuristic function of traditional ACO is shown in Equation (4), which considers only the local Euclidean distance between node $i$ and node $j$, and there is an optimum local problem. Therefore, we expanded the distance relationship between the node to be selected and the target point of the common layer ants and designed a new heuristic function so that our pathfinding operation has a high capability even on a map without initial pheromone assignment. Combined with our rules for multi-robot motion description in Section 2.2, it is necessary to plan more suitable paths for $\left\{ROB1, ROB2,\dots , ROBn\right\}$. We designed a corner suppression factor, $T,$ to the heuristic function of the planning layer ants to make the planning paths smoother. The heuristic functions of the ordinary layer ant colony and the planning layer ant colony are shown in Equations (9)–(11), respectively.

$${\eta }_1=\frac{\max \left(dis\right)-\min \left(dis\right)}{d_{JE}-\min \left(dis\right)}$$

(9)

$${\eta }_2=\frac{d_{SJ}}{d_{JE}}·T$$

(10)

$$T=\left\{\begin{array}{c}1, t\left(J\right)=t\left(J-1\right)\\ \frac{1}{\sqrt{2}},otherwise\end{array}\right.$$

(11)

where $\lfloor .\rfloor$ and $\lceil .\rceil$ are the round-down and round-up operations, respectively; $max\left(dis\right)$ and $min\left(dis\right)$ are the maximum and minimum distances from all the nodes to be selected to the end point, respectively; $d_{SJ}$ is the distance from the starting point to the current node; and $d_{JE}$ is the distance from the node to be selected to the end point. We propose a suppression factor, $T$, for the heuristic function of the planning layer in order to reduce the number of turning points of the path. When the direction of the grid to be selected is the same as the direction of the ant’s transfer at the previous moment, $T$ is 1; otherwise, $T$ is ${1/2}^{0.5}$.

3.2.3. The Pheromone Update Strategy

The traditional pheromone update strategy of ACO ignores the problem of differences in ant quality and adopts an undifferentiated update treatment, which may lead to the negative impact of pheromones updated by poorer ants on the subsequent algorithm seeking. Therefore, we improved both local and global pheromone updates with the following main contributions:

We introduced a ranking strategy to reward and penalize the local pheromone update in the local pheromone update. We sorted all ants in each iteration by path length $L_1\le L_2\le \cdots \le L_M$, and the contribution of ants to the hormone trajectory volume update was also weighted according to the ranking. If the current path length, $L_{km}$, is smaller than the contemporary mean path length, $mean\left(L\right)$, the pheromone is rewarded according to Equation (13); if $L_{km}$ is larger than $mean\left(L\right)$, the pheromone is penalized according to Equation (14). In addition, to prevent ants from falling into local optimum during the path search, we divided the pheromone volatile coefficient values and differential global pheromone update rules, as shown in Equation (12) to (14).

$${\tau }_{ij}\left(t+1\right)=1-{\rho }_0\left(1-e^{-\frac{t}{T}}\right){\tau }_{ij}\left(t\right)+\Delta {\tau }_{ij}\left(t\right)$$

(12)

$$\Delta {\tau }_{ij}\left(t+1\right)=\Delta {\tau }_{ij}\left(t\right)+\left(\frac{L_{km}}{mean\left(L\right)}\right)\ast \left(M-m+2\right)\ast \frac{Q}{L_{km}}$$

(13)

$$\Delta {\tau }_{ij}\left(t+1\right)=\Delta {\tau }_{ij}\left(t\right)-\left(\frac{L_{km}}{mean\left(L\right)}\right)\ast \left(M-m+2\right)\ast \frac{Q}{L_{km}}$$

(14)

where $M$ is the total number of ants; $m$ is the ant ranking; $t$ is the number of current iterations; $T$ is the total number of iterations; and ${\rho }_0$ is the initial value, which is taken as 0.9.

3.2.4. Redundant Point Deletion Strategy

The improved ACO described above improves the algorithm search efficiency and path quality, but there are still many unnecessary turns on the path that are not conducive to the robot for trajectory tracking. To improve the actual motion efficiency of our multi-robot system, we adopted the strategy of removing redundant points, eliminating redundant nodes, and keeping only the necessary turning points, as follows:

Eliminate the redundant points on the same line: Figure 3 shows the original path $Ori\text{\_}Route$ node sequence for $P1\to P2\to P3\to P4\to P5$ and the node $P2$ with the front node, $P1$, and the back node $P3$ in the same line. If you eliminate node $P2$, the path becomes $P1\to P3\to P4\to P5$. Then start from node $P3$ to determine the relationship with the front node, $P1$, and back node, $P4$, and then repeat the above process until traversing all nodes to get $Mid\text{\_}Route$.

Eliminate redundant points: the nodes in the path $Mid\text{\_}Route$ for $\left\{Pk\right| k=1, 2, \dots , n\}$. Take Figure 3b as an example. From the starting point, $P1$ is connected to the endpoint, $P4$; the line segment $P1P4$ distance is $dis\left(P1,P4\right)<\left[dis\left(P1,P3\right)+dis\left(P3,P4\right)\right]$, and it crosses the obstacle $\mathrm{obstacle}$ and gives up the connection. Then connect node $P1$ and node $P3$; line segment $P1P3$ distance is $dis\left(P1,P3\right)<\left[dis\left(P1,P2\right)+dis\left(P2,P3\right)\right]$, and it does not cross the obstacle. Then eliminate all the intermediate nodes in the path $Mid\text{\_}Route$, that is, $P2$. Then connect to the endpoint $P4$ with $P3$ as the starting point and repeat the above distance judgment and obstacle relationship judgment until all the nodes of the path $Mid\text{\_}Route$ are traversed and obtain the $New\text{\_}Route$.

4. Improved the Dynamic Window Approach

4.1. Principle of Dynamic Window Approach

The dynamic window approach (DWA) is a mainstream method in local path planning. This algorithm refers to sampling multiple sets of velocities in the velocity search space based on the motion model of the robot. The robot’s trajectory is analyzed and predicted for each group of velocities over a while. Then the velocity group corresponding to the optimal trajectory is selected according to the evaluation function, and the mobile robot is driven for local motion planning according to the velocity corresponding to the optimal trajectory.

4.1.1. Robot Motion Model

The DWA can sample the mobile robot’s linear velocity $v\left(t\right)$ and angular velocity $\omega \left(t\right)$ within the window, so we built the kinematic modeling of the two-wheel differential speed mobile robot, as shown in Figure 4. Assuming that the robot trajectory can be decomposed into multiple time slices, and within each time slice, $\Delta t$, the robot makes uniform linear motion, at which time the kinematic model of the robot can be expressed as follows:

$$\left\{\begin{array}{c}x\left(t\right)=x\left(t-1\right)+v\left(t\right)\Delta tcos\left(\theta \left(t-1\right)\right)\\ y\left(t\right)=y\left(t-1\right)+v\left(t\right)\Delta tsin(\theta \left(t-1\right)\\ \theta \left(t\right)=\theta \left(t-1\right)+\omega \left(t\right)\Delta t\end{array}\right.$$

(15)

where $x\left(t\right), y\left(t\right), \mathrm{and} \theta \left(t\right)$ denote the position and angle of the robot in the $x \mathrm{and} y$ directions at the time, $t$, respectively; the linear velocity $v\left(t\right)$ range of variation depends on the nearest distance to the obstacle and the maximum linear deceleration; and the content of angular velocity $\omega \left(t\right)$ variation is determined by both the closest distance to the obstruction and the maximum angular deceleration.

4.1.2. Robot Speed Constraint

In the velocity space, DWA describes the robot’s obstacle avoidance problem as an optimization problem with constraints, including incomplete motion constraint on the differential robot, dynamics constraint on the robot structure, and environmental constraints.

(1)

The maximum and minimum speed constraints for the robot are as follows [33]:

$$v_s=\left\{\left(v,\omega \right)|v\in \left[v_{min},v_{max}\left],\omega \in \right[{\omega }_{min},{\omega }_{max}\right]\right\}$$

(16)

(2)

Motor acceleration and deceleration constraints: Because different motors have different performances, the acceleration of the robot is also different, and the robot is constrained by the speed that can be reached in the next time interval.

$$v_d=\left\{\left(v,\omega \right)|v\in \left[v_c-a_v^{min}\Delta t,v_c+a_v^{max}\Delta t\left],\omega \in \right[{\omega }_c-a_{\omega }^{min}\Delta t,{\omega }_c+a_{\omega }^{max}\Delta t\right]\right\}$$

(17)

where $v_c \mathrm{and} w_c$ are the linear velocity and angular velocity of the robot at the current moment, respectively; $a_v^{min} \mathrm{and} a_w^{min}$ are the minimum linear deceleration and minimum angular deceleration, respectively; $a_v^{max} \mathrm{and} a_w^{max}$ are the maximum linear acceleration and maximum angular acceleration, respectively; and $\Delta t$ is the sampling time.

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

$$v_a=\left\{\left(v,\omega \right)|v\le \sqrt{2dist\left(v,\omega \right)a_v^{min}},\omega \le \sqrt{2dist(v,\omega )a_{\omega }^{min}}\right\}$$

(18)

where $dis\left(v,w\right)$ is the distance between the simulated trajectory and the nearest obstacle. From the expression $0-v_a^2=-2dist\left(v,w\right)a_v^{min}$ and $0-w_a^2=-2dist\left(v,w\right)a_w^{min},$ the simulated velocity must satisfy the condition of Equation (19) to ensure the safety of the robot in motion to a greater extent.

As an example, as shown in Figure 5, the above constraints limit the robot to a certain speed of motion, which we can represent by a velocity set as follows:

$$v_r=v_s\cap v_d\cap v_a$$

(19)

4.1.3. Evaluation Function

After velocity space search and sampling, DWA generates predicted motion trajectories for the robot within $N$s based on the sampled multiple sets of $velocity \left(v,\omega \right)$ simulations and then scores each trajectory according to the appropriate objective function. The part of traditional DWA contains three evaluation functions, $velocity$, $heading$, and $dist$, as shown in Equation (20).

$$G\left(v,\omega \right)=\sigma \left(A·heading\left(v,\omega \right)+B·dist\left(v,\omega \right)+C·velocity\left(v,\omega \right)\right)$$

(20)

where $G\left(v,\omega \right)$ is the evaluation value of the simulated trajectory, and the simulated trajectory group with the most significant value that corresponds to the optimal speed group $\left(v,\omega \right)$; $heading\left(v,\omega \right)$ is used to evaluate the consistency of the robot and the target direction, and there is a maximum value when the mobile robot travels straight to the target point, $heading\left(v,\omega \right)=\pi -\theta$, where $\theta$ is used to indicate the angle between the robot heading and the target line; $dist\left(v,\omega \right)$ is the distance between the robot and the nearest obstacle, and the more significant this value is, the smaller the risk of collision; $velocity\left(v,\omega \right)$ is used to evaluating the robot’s motion performance, and the greater the speed, the higher the score; $A, B, \mathrm{and} C$ are the weight factor of the corresponding evaluation function; and $\sigma$ is the normalization factor.

4.2. Improved DWA

Traditional DWA inevitably has the following problems:

(1)

In a complex terrain map with U-traps and so on, the robot is easily caught in the local optimum without global planning guidelines and cannot go to the endpoint.

(2)

Currently, the global algorithm hybrid DWA is the most widely studied, and the robot’s navigation, in the traditional way, depends on the turning points of the global path. However, the tracking accuracy of the robot will be low when more unknown static obstacles exist.

(3)

The evaluation function of traditional DWA is unreasonable, which leads to the robot’s obstacle-avoidance performance in complex environments not being optimal. To ensure the heading and speed, the traditional evaluation function will have a higher probability of selecting the motion trajectory with excessive speed, which makes the robot’s motion too close to the edge of the obstacle, and the robot’s safety is not guaranteed.

(4)

The evaluation function weight value of traditional DWA is fixed and does not change the size in real-time according to the environmental conditions around the robot. This will lead to the robot choosing to go around the periphery when it is in a dense cluster of obstacles, and the inability to pass between the dense obstacles will lead to more extended robot movement and time [34].

To solve the problems of traditional DWA, we propose the following improvement strategies to enable our multi-mobile robot to achieve efficient and safe motion in complex static and dynamic environments.

4.2.1. Fusion with Improved ACO

In maps with U-traps, traditional DWA planning paths lack guidance from global planning and tend to fall into local optimum. To address this drawback, we combined the global path information of the improved ACO planning with the robot motion to ensure that the local action follows the global path basically. The ACO hybrid DWA approach considers the advantages of both ACO and DWA, and it can be seen from Figure 6 that the robot has better safety and smoothness under the fusion algorithm and can finally reach the target point smoothly.

4.2.2. Adaptive Tracking of Target Points

Most scholars have based improved global path-planning algorithms, such as A* [35] and Dijkstra [36], on a hybrid with traditional DWA to simulate robot motion, which is the most popular approach. These studies mentioned above [17,18,35] have mentioned critical points of the global path to guide the robot for local motions, but most refer to the turning points of the global way. Considering turn points as local navigation points simply is not applicable for the environment with dense obstacles and complex unknown obstacles that we are conducting in the following paper. The main problems include the following:

• Using turn points as local target points does not sufficiently utilize the global path information, and the robot is prone to deviate and lose the correct direction in the dense obstacle environment.
• It is difficult to guarantee a high tracking accuracy for robots with slight deviations from the global path, especially for robots that need to strictly ensure global routes’ tracking accuracies, such as electronic inspection robots [37] and fire rescue robots [38]. More significant path deviations may produce more severe safety hazards.
• The traditional method does not consider the case that the robot is unreachable when a large number of unknown obstacles block the local target point.
• Furthermore, for our multi-robot system, we need to ensure that the multiple robots can continue to complete the accurate tracking motion of the local target point after completing mutual obstacle avoidance. Therefore, we propose an adaptive tracking method for local target points, as shown in the following equation:

  $$New\text{\_}path={\displaystyle \sum }_{i=2}^{m}Remake[Old\text{\_}path\left(i\right),Old\text{\_}path\left(i-1\right),n_{ds}]$$

  (21)

  $$local\text{\_}target=\left\{\begin{array}{c}New\text{\_}path\left(j\right),dis\text{\_}robot<1.7m or dis\text{\_}lacal<1.5m or dis\text{\_}obstacle<0.7m\\ goal, otherwise\end{array}\right.$$

  (22)

  where $Old\text{\_}path$ is the global path optimized by Section 3.2.4, $n_{ds}$ is the node distance of the global path expected to be optimized, and $Remake\left[.\right]$ is the function designed in this paper. Firstly, the adjacent path turning points $Old\text{\_}path\left(i\right)$ and $Old\text{\_}path\left(i-1\right)$ are concatenated one by one to form a straight line equation, and the sequence of nodes on the straight line is solved from the starting point, $Old\text{\_}path\left(i-1\right)$, according to the expected node,$n_{ds}$, starting. The path $New\text{\_}path$ is obtained by looping through all the linear equations of the path $Old\text{\_}path$; $\mathrm{m}$ is the number of critical nodes of $Old\text{\_}path$ (including starting point, ending point, and turning point); $dis\text{\_}robot$ is the distance between the robot and the local target point; $dis\text{\_}lacal$ is the distance from the end of the simulated trajectory to the local target point; and $dis\text{\_}obstacle$ is the closest distance from the local target point to that obstacle. When the robot and the local target point satisfy the relationship of Equation (22), the path node is extracted from $New\text{\_}path$ as the local navigation point, $local\text{\_}target$, where $j$ is the node sequence of $New\text{\_}path$; when the condition of Equation (22) is not satisfied, the final target point $\mathrm{goal}$ is selected as the navigation information point.

The improved local target point tracking method is shown in Figure 7. Firstly, the global path is divided into $\mathrm{n}$ points of equal distance, and if an unknown obstacle (red obstacle in the figure) is encountered, the surrounding situation needs to be judged: when the distance between the robot and the local target point is less than 1.7 m, or the distance from the endpoint of the simulated trajectory to the local target point is less than 1.5 m, or the distance from the local target point to the nearest obstacle is less than 0.7 m. When one of these three conditions is satisfied, it is determined that the point is desirable as the local target point; otherwise, the endpoint is set as the local target point.

4.2.3. New Obstacle Density Evaluation Function $I\left(v,\omega \right)$

We found through a large number of experiments that, when the mobile robot takes the traditional DWA for movement in complex terrain, as in Figure 8, for example, assuming that the robot gets five simulated trajectories with similar ratings, it is easy to choose the blue trajectory in the figure for movement. The robot has the risk of collision with obstacles. A reasonable trajectory (in red) is preferred by the robot if it considers the distribution of surrounding obstacles comprehensively during its movement. It not only makes the robot keep a certain safety distance from the obstacles, but also improves the degree of freedom of movement.

In reality, it is meaningful to avoid the collision problem when the robot turns or obstacle avoidance to significantly increase the safety of robot motion. Therefore, we propose a new evaluation function, $I\left(v,\omega \right)$, concerning obstacle density based on the three evaluation functions of traditional DWA. Consequently, we propose a new evaluation function regarding obstacle density based on the three evaluation functions of traditional DWA. It takes the end of the trajectory as the circle’s center, with a detection radius of $R$. The number and occupied area of the free grid within this detection range are considered comprehensively, and the distance index between the robot and obstacles is also added. In the environment, as shown in Figure 8, the robot with the added obstacle density evaluation function action is more inclined to choose the red simulated trajectory that can improve motion safety. The evaluation functions are expressed as follows:

$$I\left(v,\omega \right)=k_1·N+k_2·\frac{S_{all}-S_{obstacle}}{S_{all}}+k_3·\frac{1}{dis\left(r,obs\right)}$$

(23)

where $N$ is the number of free grids in the detection area; $S_{all}$ denotes the area of the detection area; $S_{obstacle}$ is the area of obstacles in the detection area; $\mathrm{dis}\left(·\right)$ means the closest distance between the robot, $r$, and obstacle, $obs$, in the detection circle; and $k_1, k_2, \mathrm{and} k_3$ are the weight of each component. The robot will score each simulated trajectory through the obstacles as a density function, $I\left(v,\omega \right)$, and the larger the value, the sparser the distribution of obstacles within the sensing window, and thus the more suitable the trajectory is for the robot to pass.

4.2.4. Adaptive Evaluation Function Weights

The conventional DWA prioritizes the trajectory that is further from the obstacle to ensure safety. As shown in Figure 9, when the robot approaches the target point, the algorithm prefers the trajectory far away from the obstacle since the distance between the target point and the obstacle is too close. However, it can be clearly seen that the trajectory close to the target point is the optimal trajectory. Considering that when the robot moves in a dense environment, the simulated trajectory without obstacles is retained, our newly added obstacle density evaluation function, $I\left(v,\omega \right)$, with navigation function, $heading\left(v,\omega \right)$, and velocity evaluation function, $velocity\left(v,\omega \right)$, will have multi-objective optimality-seeking constraints in this class of environment [39].

According to the experiment shown in Figure 9, we sacrificed the robot’s movement speed for the benefit of higher path tracking accuracy. Regarding the actual path length traveled by the robot, ours is 12.3806 m, which is 2% less than the conventional 12.6720 m. As for the global path-tracking error, the traditional robot is 0.5381 m, and ours is only 0.4179 m, which is 22.34% less. For the average motion speed, the conventional one is 0.4267 m/s, and ours is 0.3825 m/s.

To coordinate the robot seeking problem, this paper improves the weights $A$ and $C$ of the $heading\left(v,\omega \right)$ function and $velocity\left(v,\omega \right)$ function into an adaptive adjustment strategy, respectively, so that the robot adjusts the relationship between $A$ and $C$ according to the situation of obstacles in the environment it is in. When there are many obstacles, the trajectory point whose heading angle points to the direction of the target point is preferred, and the robot will not pick the trajectory far from the obstacles to form a detour before arriving at the target point. With few obstacles, the trajectory with superior speed evaluation is preferred to make the overall path smoother and the distance shorter. The adjustment method is shown as follows:

$$A^{\prime }=\left(\frac{S_{obstacle}}{S_{all}}+\mu \right)·A$$

(24)

$$C^{\prime }=\kappa \left[mean\left(\frac{S_{all}-S_{obstacle}}{S_{all}}\right)+\lambda \right]·C$$

(25)

where $\mu$, $\lambda$, and $\kappa$ are the augmented weight coefficients, as obtained by experimental testing; and $mean\left(·\right)$ is the average of the area occupied by the free grid within the probe circle for all simulated trajectories.

4.3. Fusion of Ant Colony Algorithm and Dynamic Window Approach

As a classical heuristic search algorithm, the ACO has good adaptability and robustness to a complex environment and is easy to integrate with other algorithms. The DWA fully considers the limitations of the mechanical characteristics of the mobile robots and the environments. The hybrid strategy of ACO–DWA has been well reported in some papers [40,41] recently. Based on the consideration of environmental characteristics, we propose a new fusion strategy and apply a multi-robot system. Firstly, the global path is obtained by improving the ACO, and the adaptive navigation scheme is proposed to use the global path as the motion information point of the robot. To make the robot fit the global path in local motion and consider the safety of movement, the obstacle density evaluation function, $I\left(v,\omega \right)$, is proposed, and the final evaluation function of robot obstacle avoidance is as follows:

$$G\left(v,\omega \right)=\sigma \left(A^{\prime }·heading\left(v,\omega \right)+B·dist\left(v,\omega \right)+C^{\prime }velocity\left(v,\omega \right)+D·I\left(v,\omega \right)\right)$$

(26)

where $D$ is the weight of the obstacle density evaluation function, $I\left(v,\omega \right)$. The flowchart of the fusion algorithm is shown in Figure 10.

5. Prioritized Obstacle Avoidance Strategy for Multi-Mobile Robots

The path-planning problem of multiple mobile robots is extended from the single robot, for which we improved the ACO to preplan a globally optimal path for each mobile robot in the grid environment. The DWA obstacle-avoidance strategy is mainly applicable to static environments or local small-scale dynamic environments (low-speed motion with disturbing obstacles, etc.) [42], but it works poorly for solving the phenomenon of motion conflict among multiple robots [43]. The prioritization method [44], as one of the current mainstream techniques for coordinated collision avoidance, can dissipate local conflicts to achieve collision avoidance coordination. To achieve a better result in global multi-robot motion planning, we combined the improved ACO, improved DWA strategy, and multi-robot priority strategy to simplify the path-planning problem into a dynamic path-planning problem with sequential order, which simplifies the complexity of the algorithm.

We set a corresponding priority level for each mobile robot. When a low-priority robot encounters a high-priority robot during its motion and a motion conflict is about to occur, the low-priority robot stops its action, and the high-priority robot performs obstacle-avoidance motion based on DWA. For example, as shown in Figure 11, three mobile robots are assigned priority, where Robot 1 has the highest priority, Robot 2 has the second-highest priority, and Robot 3 has the lowest priority. When three robots meet and potential motions conflict, Robot 1 maintains its activity without considering the movements of the other robots because it has the highest priority, while Robots 2 and 3 need to stop to give way. When Robot 1 leaves the conflict zone, the state of Robot 3 remains unchanged, and the conflict is wholly resolved after Robot 2 leaves the conflict zone.

The constraints introduced by the robot’s kinematic model and the environmental obstacles were considered in our study, making the challenge of the multi-machine priority obstacle-avoidance strategy focus on judging the conflict and coordinating the move. Taking two robots, AGV1 with higher priority, and AGV2 with lower priority, as an example, we investigated multiple conflict types in

Table 1. Multi-robot motion conflict detection and resolution scenarios.

Case	Movement Status	Angle Relationship	Potential Conflict or Not	-Conflict Resolution Strategies
1		$\left|\alpha -\beta \right|\ge 90°$	NO	——
2		$\left|\alpha -\beta \right|\ge 90°$	NO	——
3		$\left|\alpha -\beta \right|<90°$	YES	AGV1 passes first, AGV2 stops the movement
4		$\left|\alpha -\beta \right|\ge 90°$	NO	——
5		$\left|\alpha -\beta \right|<90°$	YES	AGV1 passes first, AGV2 stops the movement
6		$\left|\alpha -\beta \right|<90°$	YES	AGV1 passes first, AGV2 stops the movement

and obtained the following collision conflict judgments and solutions:

Step 1: If the actual distance, $d_{12}$, of two robots is less than the desired obstacle-avoidance distance, $d$, there is a potential collision risk.

Step 2: Establish a local coordinate axis with the lower-priority robot, AGV2, as the center; calculate the angle, $\alpha$, between the line of the two robots and the positive direction of the x-axis; and then calculate the relationship between the magnitude of $\alpha$ and the directional angle, $\beta$, of AGV1.

Step 3: Determine whether $\left|\alpha -\beta \right|$ is less than 90°; if this condition is satisfied, the risk of collision is extremely high. To significantly ensure the safety of multi-robot motion, the AGV2, which has lower priority, decelerates and stops in a short time. The AGV1, which has higher priority, treats AGV2 as an unknown static obstacle and performs obstacle-avoidance motion through DWA. When the distance relationship between the two robots is $d_{12}>d$ and $\left|\alpha -\beta \right|\ge 90°$, AGV2 resumes motion.

In conjunction with the research presented in this paper, most of the experiments we have conducted show that the proposed prioritized obstacle-avoidance strategy applies to most situations. This is mainly attributed to the fact that robots are defined as conflicting motions only when the above conditions are satisfied in both distance and angle relationships, and robots not in the conflict range perform the corresponding obstacle-avoidance strategies based on our DWA. What is worth mentioning is that our definition of the conflict range is consistent with the detection range of the obstacle density evaluation function, $I\left(v,\omega \right)$, proposed in Section 4.2.3.

6. Simulation Experiment and Result Analysis

The experimental conditions of this paper are shown in

Table 2. Description of the simulation experiment.

Chapters	Type of Experiment	Method	Purpose
6.1	Global path planning	Improved ACO	Verifying our improved ACO
6.2	Single robot	Improved ACO–DWA	Verifying the single-robot obstacle avoidance
6.3	Multi-robot	ACO–DWA combined with priority strategy	Verifying the distributed multi-robot obstacle avoidance

and

Table 3. Main parameters of simulation experiment.

Parameter	Value	Parameter	Value	Parameter	Value	Parameter	Value
$N_{max}$	50	$k_1$	0.3	$\lambda$	1	$B$	0.1
$M$	50	$k_2$	0.5	$v_{max}$	1	$C$	0.3
$\alpha$	1	$k_3$	0.5	${\omega }_{max}$	70	$D$	0.1
$\beta$	8	$n_{ds}$	0.09	${\dot{v}}_{max}$	0.2	Velocity resolution	0.02
$\rho$	0.1	$\mu$	2	${\dot{\omega }}_{max}$	50	Angular velocity resolution	5
$Q$	10	$\kappa$	2	$A$	0.15	Simulated prediction time	3

. MATLAB 2019a was used for the experiment, and the experimental environment was Windows 11 64-bit; processor, R5-5600H; and memory, 16 GB.

6.1. Comparative Experimental Analysis of Improved Ant Colony Algorithm

(1)

20 × 20 environment

To verify the relative superiority of our improved ACO and the improved strategy of Luo et al. [8] in a conventional environment, a comparison experiment was conducted on a 20 × 20 environment map. The parameter values of our ACO are shown in Table 2, the experimental results are shown in

Table 4. Comparison of the results of the two algorithms in global path planning.

Indicators of the Optimal Path	Luo et al. [8]	This Paper
		Before Optimization (Initial Path)	After Optimization (Optimized Path)
Optimal path length/m	30.968	30.968	30.7924
Number of iterations	6	4	—
Runtime/s	1.5301	1.2377	1.4918
Number of turns	15	13	12

, and the path-planning results and convergence curve trends of the two algorithms are shown in Figure 12, where the experimental data of Luo et al. [8] are from the original literature.

The results show that the optimal path length achieved by the improved ACO of Luo et al. [8] is 30.968 m and converges to the optimal path after six generations of population search, with the number of turns of the route being 15. The original path planned by our algorithm is the same as Luo et al. [8], with several turns of 13, but it undergoes only four population iterations to converge quickly, and after later optimization, the path length is 30.7924 m. The number of turning points was also reduced by one. The beneficial effect of the post-optimization is to break through the eight domain directions of the path in the grid environment and make the path more suitable for mobile robot motion. Our improved ACO is superior to the improved strategy of Luo et al. [8] in terms of population convergence speed and path quality.

(2)

30 × 30 environment

We conducted comparative experiments on a 30 × 30 environmental map, and the experimental results are shown in

Table 5. Comparison of the results of the two algorithms in global path planning.

Indicators of the Optimal Path	Luo et al. [8]	This Paper
		Before Optimization (Initial Path)	After Optimization (Optimized Path)
Optimal path length/m	44.522	44.522	43.5921
Number of iterations	13	9	—
Runtime/s	16.9595	6.0813	6.5187
Number of turns	14	13	11

, and the path planning results and convergence curves are shown in Figure 13.

The results show that the optimal path length achieved by the algorithm of Luo et al. [8] was 44.522 m, which converges to the optimal path after 13 iterations, and the number of path turning points is 14. Our ACO achieved an optimal path with 44.522 m, and after spending 0.4374s of late optimization time, the path length was reduced by 0.9299 m, and the number of path turning points was reduced by three compared to the path length of Luo et al. [8]. In a 30 × 30 environment, our algorithm took only nine populations to converge quickly to the optimal path, optimizing 2.1% and 22.3% in path length and number of turning points, respectively, compared to the literature [8]. The path validation of the robot based on two-wheel differential kinematics in several complex terrain environments will be performed in the experiments to be carried out subsequently.

6.2. Comparative Experimental Analysis of Improved DWA

We referred to the experiments of Yang et al. [45] to design seven more complex environments with high obstacle density for implementing the algorithm test to validate the positive effect of our improved DWA strategy on the robot’s obstacle avoidance. The parameters of our improved ACO and robot are shown in Table 2, where the traditional DWA and our DWA have the same global path and various parameters. The unknown static obstacles are randomly selected and added to the global path planned by our improved ACO for the experiments, and the experimental results are shown in Figure 14, Figure 15, Figure 16, Figure 17, Figure 18, Figure 19 and Figure 20. The blue dashed line in the figure indicates the global path, the red solid line in is the actual route of the conventional robot, and the pink solid line is the actual path of our robot. The green solid line is the linear velocity of the conventional robot, the dark blue solid line is the linear velocity of our robot, the purple solid line is the tracking error of the conventional robot, and the light blue solid line is the tracking error of our robot. The red grid and gray grid indicate unknown static obstacles. To reflect the effectiveness of our algorithm designed for the robot for complex terrain, the line-speed comparison graph and tracking-error comparison graph are added under each map.

What we can clearly conclude from Figure 14, Figure 15, Figure 16, Figure 17, Figure 18, Figure 19 and Figure 20 and

Table 6. Comparison of the results of the two algorithms in local path planning.

Indicators of the Optimal Path	Map No.	Map Name	Optimal Path Length	Robot Travel Time	Average Velocity	Tracking Deviation
Traditional DWA	1	Complex1	N/A
	2	X-shape	N/A
	3	Complex2	49.5960	85.3966	0.5808	0.5832
	4	Double U-shape	66.3800	133.28824	0.4969	0.3977
	5	Corridor	42.5320	77.4964	0.5488	0.5645
	6	Clasps	23.2980	45.2961	0.5143	0.3007
	7	Z-type	50.6060	81.7968	0.6187	0.4670
Our DWA	1	Complex1	49.3986	114.3048	0.4322	0.8473
	2	X-shape	42.2437	96.3881	0.4383	0.2609
	3	Complex2	41.7105	93.4432	0.4464	0.1176
	4	Double U-shape	64.7839	152.2820	0.4254	0.1697
	5	Corridor	38.8887	88.3286	0.4403	0.2295
	6	Clasps	23.1982	54.8197	0.4232	0.1799
	7	Z-type	44.9864	99.2984	0.4530	0.2671

is that benefiting from our improved DWA motion strategy results in a robot that takes into account the distance traveled, the stability of the motion, the degree of global path maintenance, and the safety of obstacle avoidance during the action, as analyzed below:

In the X-shape environment of Figure 15 and the Complex2 environment of Figure 16, the target point is set at a location closer to the surrounding obstacles. The mechanism of the evaluation function used for the traditional DWA forces the robot to choose a simulated trajectory away from the obstacles, which leads to a detour when the robot approaches the target point, and even in the X-shape environment, as in Figure 15, it fails to get to the endpoint. In contrast, our improved DWA benefits from the newly obstacle density evaluation function, the adapted weighting coefficients enable this robot to have a successful movement, and the path length of our robot is optimized by 15.8% compared to the Complex2 environment in Figure 16. The average error of our global path is only 0.1176 m, which is 0.4656 m less deviation than the traditional robot.

For the double U-shape environment in Figure 17, the traditional robot deviates from the global path after coming out of the narrow area to a more significant extent, resulting in a much less accurate trajectory tracking than our robot and a travel distance 1.5961 m more than our robot.

The Corridor environment in Figure 18 is a demonstration of how the traditional robot chooses trajectories with the fixed rating function to perform motion and results in collisions. What we can see from Figure 18c is that the traditional robot decelerates abruptly only when an obstacle is detected ahead at control point 350, failing to coordinate the trajectory rating function with the obstacle environment and causing a collision. Taking into account the robot’s motion-speed data in Table 6, it can be seen that, under the same parameters and kinematic and environmental constraints, the average speed of the conventional robot is 0.5488 m/s, while ours is only 0.4403 m/s. In addition, it can be seen from Figure 18c that our robot’s low uniform speed motion in a complex environment is better guaranteed in terms of safety than traditional robot motion.

As for the Clasps and Z-type environments in Figure 19 and Figure 20, both the traditional robot and ours reached the destination correctly. Although the passage time of the traditional robot is shorter than ours, the length of the movement distance of our robot is better than that of the traditional robot, mainly because of the excellent follow ability of our robot with the global path at the turns and when performing obstacle avoidance.

In addition, we found from the motion of robots in seven types of complex environments that our robot is better than the path-tracking error of the traditional one. Although the line speed of the traditional robot is incredible, our robot is more stable, which is more beneficial to reducing energy consumption in practice. The experimental data of the robot in complex terrain are shown in Table 6, with the data missing in Map1 and Map2 because the traditional robot broke down. In terms of the path length, our improved DWA outperforms the traditional DWA. Take the traditional DWA of Map3 as an example; the optimal path length is shortened by 15.9% compared to the traditional DWA. In terms of robot passage time, the traditional DWA is slightly better than our DWA, with a difference of about 10 s. Most of the differences are in the evaluation metrics, three for traditional DWA and four for ours, as shown in Equations (20) and (26), respectively. The complexity of our algorithm is greater, and the robot movement takes a longer time. The average linear speed of traditional DWA is better than our DWA, but the smoothness is poor. In addition, the tracking error of the global path was set to evaluate the robot’s tracking accuracy of the global path, and the tracking error with our improved algorithm was significantly better than the conventional DWA. Taking Map3 as an example, the tracking error was reduced by 79.84% compared with the traditional. In summary, we achieved an algorithm that takes into account the four metrics of the robot’s travel length, stability of motion, degree of global path retention, and obstacle avoidance safety with the above analysis and comparison of the metrics.

6.3. Multi-Mobile Robot Path Planning Experiment

To verify the ability of our above fusion algorithm with the priority method to solve conflict avoidance in the path planning of multi-mobile robots, experiments were conducted in environments with known a priori information and unknown factors. Considering that the primary research of this paper is to solve the conflict problem of multi-mobile robots during motion, three mobile robots were designed while considering the kinematic model constraint and obstacle environment constraint, where the priority is $\mathrm{AGV}1>\mathrm{AGV}2>\mathrm{AGV}3$. The optimal paths were planned in the global static environment, the environment with unknown static obstacles, and the large-scale environment with unknown static obstacles by improving ACO, respectively. The following three experiments were conducted by constructing the multi-robot kinematic model, using the improved DWA. In addition, every robot was equipped with a circular detection circle of radius, R, which achieves both conflict detection and obstacle density detection.

6.3.1. Environments Containing Known Static Obstacle

As shown in Figure 21a and

Table 7. Multi-robot path planning results in a known static environment.

Indicators of the Path	AGV1	AGV2	AGV3
Global path length/m	41.0609	40.9754	31.6413
Local path length/m	40.8980	40.7400	29.4940
The initial direction angle of robot/rad	−0.7854	0.7266	2.6422
Tracking deviation/m	0.1238	0.0937	0.0938

, we planned a globally optimal path for each robot with the improved ACO, where the blue, pink, and red lines are the global paths of AGV1, AGV2, and AGV3, respectively, with the starting and ending points of AGV1 being (1.5, 30.5) and (30.5, 1.5), AGV2 being (1.5, 1.5) and (30.5, 30.5) for AGV2, and (30.5, 15.5) and (1.5.5, 15.5) for AGV3; the initial direction angles of the robots are −0.7854, 0.7266, and 2.6422 m, respectively. Moreover, the robot motion parameters are shown in Table 2; the black grid is a static obstacle with a priori information. Figure 21b shows the changes of the robot in real-time during the planning process, Figure 22a,b shows the linear velocity and angular velocity variation curves of the robot, and Figure 22c shows the deviation of the robot’s local path planning from the global path, where AGV1 is the solid blue line, AGV2 is the solid pink line, and AGV3 is the solid red line.

What can be seen in Figure 21b and Figure 22 is that, when there is a motion conflict between the lowest-priority AGV3 and the highest-priority AGV1, AGV3 starts to decelerate at the 305th motion control node, stops entirely at 307th, and resumes motion at 342th, with the conflict released. Then AGV1 reencounters the lower-priority AGV2. AGV2 starts to decelerate from the 346th control node to a complete stop at 349th and resumes motion with the conflict resolved at 407th. AGV1 performs a reasonable obstacle avoidance based on our DWA avoidance strategy by treating AGV2 and AGV3 as unknown static obstacles.

With the highest priority, AGV1 performs more obstacle-avoidance processes than the lower-priority AGV2 and AGV3, and this produces more pronounced angular velocity changes and global path deviations, as seen in Figure 22, but the velocity fluctuations are not significant. In addition, combined with our study of multi-robot motion conflict in Section 4, the directional angle relationship between AGV2 and AGV3 is always greater than 90°, which corresponds to Case_4 of Table 1 and does not satisfy the motion conflict relationship. This multi-robot motion experiment takes 438.85 s. What can be seen in Table 7 is that the global path lengths of AGV1, AGV2, and AGV3 are 41.0609, 40.9754, 31.6413 m; the final travel distances are 40.898, 40.74, and 29.494 m, respectively; and the average error of motion deviation from the global path at each motion moment (0.1 s) is 0.1238, 0.0937, and 0.0938 m, respectively.

6.3.2. Environments Containing Unknown Static Obstacle

As shown in Figure 23a and

Table 8. Multi-robot path planning results in an unknown static environment.

Indicators of the Path	AGV1	AGV2	AGV3
Global path length/m	41.0609	40.9694	39.6227
Local path length/m	41.16	41.3900	39.8580
The initial direction angle of robot/rad	−0.7854	0.7266	−2.3562
Tracking deviation	0.2226	0.2874	0.2710

, the safe and optimal path is planned for each robot based on our improved ACO, and then four unknown static obstacles are added to these paths, where the blue, pink, and red lines are the path cases of three robots, AGV1, AGV2, and AGV3, respectively; the linear velocity, angular velocity, and tracking deviation of each robot are presented in Figure 24, with the solid blue line indicating AGV1, the solid pink line AGV2, and the solid red line AGV3. The starting and ending points of AGV1 are (1.5, 30.5) and (30.5, 1.5), respectively; the starting and ending points of AGV2 are (1.5, 1.5) and (30.5, 30.5), respectively; and the starting and ending points of AGV3 are (30.5, 29.5) and (1.5, 2.5), respectively. The black grids are static obstacles that the robots have a priori information about, and the red grids are static obstacles without a priori information; the initial direction angles of the robots are −0.7854, 0.7266, and −2.3562 m, respectively. Figure 23b shows a real-time variation of the robot during its motion; Figure 24a,b show the linear and angular velocity variation curves of the robot, respectively; and Figure 24c shows the deviation of the robot’s local path from the global path.

What we can see from Figure 23b and Figure 24 is that the motion of AGV1, which has the highest priority, and AGV2, which has a lower priority, near the unknown red obstacle, is rough, as follows: AGV2 travels from the lower left to the upper right, and AGV1 travels from the upper left to the lower right. When the distance between these two robots is less than the conflict detection radius, AGV2 starts to decelerate at the 350th motion control node and stops altogether at the 351st. Then AGV1 exits the conflict detection radius within a short time, and AGV2 immediately returns to the motion state. AGV1 continues to move toward the target point without conflict with AGV3, so each robot makes reasonable obstacle avoidance based on DWA for unknown obstacles and reaches its respective endpoints safely.

The velocity-change curve in Figure 24 shows that only a brief conflict occurred between AGV2 and AGV1, so the velocity of AGV2 drops to 0. The change of movement angle is the most obvious, and the global path deviation is the largest, but the whole movement is relatively smooth. What can be seen in Table 8 is that the multi-robot motion experiment takes 432.5953 s, and the global path lengths of AGV1, AGV2, and AGV3 are 41.0609, 40.9694, and 39.6227 m. The travel distances of AGV1, AGV2, and AGV3 are 41.16, 41.39, and 39.858 m, respectively, and the average error of motion deviation from the global path for each moment (0.1 s) is 0.2226, 0.2874, and 0.2710 m, respectively.

6.3.3. Marine Environment with Unknown Static Obstacles

As shown in Figure 25a and

Table 9. Multi-robot path planning results in an unknown static marine environment.

Indicators of the Path	AGV1	AGV2	AGV3
Global path length/m	42.7139	69.8566	72.1761
Local path length/m	47.3280	71.0220	72.1500
The initial direction angle of robot/rad	1.0304	1.2490	−1.5994
Tracking deviation	0.3319	0.7673	0.5376

, the blue, pink, and red lines are the paths of AGV1, AGV2, and AGV3, respectively, where the starting and ending points of AGV1 are (20.5, 45.5) and (44.5, 85.5), respectively; the starting and ending points of AGV2 are (44.5, 21.5) and (72.5, 84.5), respectively; and the starting and ending points of AGV3 are (65.5, 85.5) and (49.5, 16.5), respectively. The black grid is a static obstacle for which the robot has a priori information, and the red grid is an unknown obstacle; the initial direction angle of the robots are −0.7854, 0.7266, and −2.3562 m, respectively. Figure 25b shows the obstacle avoidance of the robot during the motion, Figure 26a,b shows the linear and angular velocity variations, and Figure 26c shows the deviation of the robot’s motion path from the global path during the single-step motion time, where the blue solid line is AGV1, the pink solid line is AGV2,and the red solid line is AGV3. Since we sampled a larger-scale sea area map in this example experiment, we made the following changes to improve the robot motion efficiency and program running time: the robot speed evaluation weight, $C$, is increased from 0.3 to 0.5; the robot sampling time, $\tau t$ (also the robot’s single-step motion time of Equation (16)), is changed from 0.1 to 0.3 s; the trajectory simulation prediction time is increased from 3 to 5 s; the maximum angular velocity is increased from 70 to 80 rad/s; and the angular velocity resolution is reduced from 5 to 2.

Figure 25b and Figure 26 show that the higher-priority AGV2 and the lowest-priority AGV3 are in a conflict during their motion. AGV3 starts to decelerate at the 183rd motion control node, stops entirely at the 185th, and resumes its motion after the conflict is released. In addition, we put a large number of unknown obstacles on each robot’s global path to interfere significantly with the action in this example, where the degree of interference is the greatest for AGV2. However, with our proposed adaptive navigation strategy, all the robots achieve obstacle avoidance and reach their respective endpoints safely.

We can find from Figure 26 that only a brief conflict occurred between AGV2 and AGV3, resulting in AGV3’s velocity decreasing to 0. The path of AGV2 is disturbed by unknown obstacles to the greatest extent, so the degree of deviation from the global path during the motion is the greatest, but there is still a relatively smooth robot motion velocity. Finally, the whole experiment of multi-robot action takes 705.8052 s, and Table 9 shows that the global path lengths of AGV1, AGV2, and AGV3 are 42.7139, 69.8566, 72.1761 m, and the travel distances are 47.328, 71.022, and 71.15 m, respectively. The average error of motion deviation from the global path for each moment (0.3 s) is 0.3319, 0.7673, and 0.5376 m, respectively.

7. Conclusions

To solve the navigation and obstacle avoidance problems of distributed multi-mobile robots in unknown complex environments, we fused ant colony optimization (ACO) with the dynamic window approach (DWA) to enhance the obstacle-avoidance capability of robots and introduce a priority strategy to coordinate path planning and motion navigation among multiple robots and unknown regions.

Improvements were made to solve the blindness of early search, slow convergence, and local optimal solutions of the ACO. Accordingly, the principle of non-uniform initial pheromone assignment, dual-population heuristic functions, weighting sort ant pheromone update rule, and pheromone volatile coefficient self-adjustment strategy were proposed. Finally, the global path was optimized by our redundant point streamlining approach to make the global path more compatible with robot motion. The simulations demonstrated the effectiveness of our improved ACO in global path planning.

We investigated the problems of poor global path-tracking ability and obstacle-avoidance effect of the robot in high-density obstacle and large-scale unknown static obstacle environments, as well as unreasonable obstacle-avoidance mechanisms. Accordingly, we propose an adaptive navigation strategy to enhance the guidance for robots by selecting reasonable local target points. Considering the effects of local environmental changes on robot motion comprehensively, a new obstacle density evaluation function and an adaptive weighting strategy based on the original evaluation functions were presented to smooth out the robot motion path. Finally, the effectiveness of this improvement was verified in seven complex environments.

The distributed multi-robot system was extended from our single-robot obstacle-avoidance algorithm. Then we discussed the situation of motion conflict among multi-mobile robots, proposed conflict judgment rules, and introduced a priority strategy to solve the collision problem. Finally, the effectiveness of our method was verified in three scenarios.

In this study, high-density obstacles and unknown static obstacles were the environments we focused on, and more complex factors (non-flat terrain, dynamic obstacles, etc.) will be gradually considered in future work. In addition, we can extend our study to a 3D environment; build accurate models based on sensor acquisition information (including environment model, complex oceanic factors, dynamic obstacle factors, etc.); and add evaluation functions, such as energy consumption and ocean currents, to the DWA to better coordinate the multi-agent system.