Decentralized motion planning for multi quadrotor with obstacle and collision avoidance

Abstract

In this paper, a decentralized motion planning approach for multi-quadrotor autonomous navigation is developed in environments populated with obstacles. We propose a priority-based RRT algorithm, which generates an online global path for each quadrotor with obstacle avoidance. Furthermore, the front-end paths uncross by assigning priorities and sharing global states in the swarm. The security and clearance of the trajectory are improved by B-spline optimization, where inter-collision-free is achieved by formulating the collision risk as a penalty term of the cost function. The efficiency of the developed algorithm is verified through numerical simulation.

1 Introduction

In recent years, unmanned aerial vehicles (UAVs) are becoming popular in various applications such as industrial inspection, intelligence, surveillance, search-and-rescue missions (Saikin et al. 2020; Augugliaro et al. 2014; Mohta et al. 2018). To achieve full autonomy in these scenarios, the motion planning module plays an essential role in generating safe and smooth reference motion. However, computational power and payload become the limiting factors when planning a mission with small UAVs. Multi-UAV collaboration is often considered, aiming to coordinate multiple agents to complete the mission within a limited time or resources budget.

Several related works have promoted the development of multi-robot planning. In Fiorini and Shiller (1998), Van den Berg et al. (2008), Van Den Berg et al. (2011), the collision avoidance trajectory of multiple agents can be safely generated. However, these methods assume that there are no other obstacles in the environment. Even considering obstacles in the environment, the information about global maps are fully known (Park et al. 2019; Burri et al. 2015; Usenko et al. 2017). To obtain optimal trajectories for a large team of robots, the trajectory generation problem is solved in a centralized manner (Mellinger et al. 2012; Augugliaro et al. 2012; Tang et al. 2018). Nevertheless, it can not meet the requirements of real-time planning for large-scale robots. Decentralized approaches have been proposed in Liu et al. (2018), while the dynamic model of robots are ignored. To sum up, the motion planning for multi-robot is still an open question.

In this paper, we propose a decentralized motion planning approach for multi-quadrotor autonomous navigation in obstacle-dense previously unknown environments, where the quadrotors can steer away from obstacles and avoid collisions between them. A priority-based RRT algorithm is presented to find a guidance path for each quadrotor, which avoids obstacle and path crossing between multiple UAVs. Then, we take a piecewise polynomial trajectory to smooth the initial path. Finally, the trajectory is represented as a uniform B-spline, which refines carefully the initial polynomial trajectory.

The main contributions of the present work are summarized as follows.

• 1. A priority-based RRT algorithm is proposed for multi-UAV navigation to find an obstacle-avoidance and inter-vehicle collision-free piecewise line segment path with asymptotic optimality. Then B-spline optimization (Usenko et al. 2017) is extended for reciprocal collision avoidance with the cost function.

• 2. A decentralized multi-UAV motion planning approach is proposed for generating a safe, smooth, and dynamically feasible trajectory to each UAV.

• 3. A multi-UAV simulation environment is established in Gazebo, which can be used to verify the feasibility of the multi-UAV motion planning algorithm. And an extensive simulation evaluation of the proposed method is conducted.

The rest of this paper is given as follows. Section 2 reviews the related works, while system overview and problem formulation are presented in Sect. 3. The developed path searching method and trajectory optimization method are detailed in Sect. 4. The system settings and simulation verification are presented in Sect. 5. The paper is concluded in Sect. 6.

2 Related work

For the problem of motion planning for multi-robot, there have been a lot of researches and applications. In this section, we give a brief overview of prior work in trajectory generation of robot swarm, which can be grouped into two broad approaches, centralized and decentralized trajectory generation.

Centralized trajectory generation In Mellinger et al. (2012), Augugliaro et al. (2012), Chen et al. (2015), the trajectory generation problems are formulated as mixed-integer quadratic programming (MIQP) or sequential convex programming (SCP) problems. However, the application of these methods is limited to small-scale teams due to the high computational overhead. To reduce the computation cost, Hamer et al. (2018) presented a parallel formulation that can fast generate collision-free trajectories for multi-agent. Hönig et al. (2018) proposed a method for multi-robot trajectory planning in known, obstacle-rich environments based on the concept of roadmaps, while it is not efficient enough. Wu et al. (2019) presented a novel approach to address the problem of multi-UAV trajectory planning only in the time domain, the original problem is decoupled into three stages to improve the efficiency. In summary, centralized trajectory generation methods attempt to jointly optimize the trajectories of all robots. Although the optimality and completeness are guaranteed, the complexities grow exponentially with the number of robots.

Decentralized trajectory generation Fiorini and Shiller (1998) pioneered a velocity obstacles (VO) approach, where a set of robot velocities that would result in a collision between the robot and an obstacle moving at a given velocity (RVO) are defined, and collision-avoidance is achieved by selecting robot’s velocities out of that set. Van den Berg et al. (2008) presented a new concept called the reciprocal velocity obstacle to overcome the shortcoming of the velocity obstacles approach that results in undesirable oscillatory motions. Liu et al. (2018) explored a search-based motion planning approach that solves the problem of planning in dynamic environments for multi-agent. While those algorithms ignore the dynamic model of robots. Based on the concept of optimal reciprocal collision avoidance (ORCA) and flatness-based model predictive control (MPC), Arul and Manocha (2020) presented a collision avoidance algorithm for navigating a swarm of quadrotors in obstacle-rich scenes. Nevertheless, only the planning was verified without integrating localization and mapping capabilities.

3 System overview

3.1 System architecture

The decentralized system architecture is proposed for quadrotor swarm motion planning, as shown in Fig. 1. To avoid conflict in the group, the global paths and states are shared. The mapping process in Usenko et al. (2017) is adopted to generate a local map with UAV’s moving. Then, the 3-D local map is projected into the 2-D grid map which is stored for global path planning.

Trajectory planning A priority-based RRT algorithm as the front-end of motion planning is proposed in this paper to generate a guidance path. The path can avoid the collision with obstacles in the environment and realize multiple UAVs collision-free in the local range. Then piecewise polynomial trajectory is adopted to smooth the initial path. Finally, We improve clearance and security of the trajectory by uniform B-spline optimization, which incorporates gradient information, collision avoidance constraints between multi-UAV and dynamic constraints.

Simulation environment The ground truth of UAVs can be accurately obtained by using the plugin in the Gazebo due to simulation verification. Besides, there is a VLP-16 lidar plugin with 16 lines on the quadrotor platform to obtain point cloud data, which is fused with the location information to build an occupancy map of the environment. We adopt the method proposed in Usenko et al. (2017) to generate a local environment map, which is a 3-D occupied voxel map. And the projection of the local environment map to the ground as a global map is stored, which is a 2-D map. As the paths traveled by UAVs increase, the global map is constantly expanding, while the UAV only maintains the current local map. For the controller of UAV, the dual-mode cascaded nonlinear controller proposed in our previous work is adopted (Shen et al. 2020), which ensures the hovering accuracy and trajectory tracking performance.

Fig. 1

The complete system architecture. The whole system is mainly composed of two parts, which are simulation environment (localization and mapping, control of a single UAV) and swarm motion planning. Suppose that UAV1 has the highest priority, UAV2 takes the second place, and UAV3 has the lowest priority. The dotted arrows in the figure indicate the information transfer between UAVs. ${d_{UA{V_{ij}}}}$ is the distance between UAVi and UAVj

3.2 Problem formulation

Consider a team of $N \in {{{\mathcal {Z}}}}$ quadrotors operating in an obstacle-dense workspace, which is fully unknown. The environment is defined by ${{{\mathcal {W}}}} \subseteq {{\mathbb {R}}^3}$, while the obstacles that have been explored in the environment are denoted as ${\mathcal{O}}$. The free configuration space of the UAVs is given by ${\mathcal{F}}{{ = }}{{{\mathcal {W}}}}\backslash {{{\mathcal {O}}}}$. The state of a UAV, which is at position $x \in {{\mathbb {R}}^3}$, is defined by ${{{\mathcal {R}}}}\left( x \right)$. Let ${s_i} \in {{{\mathcal {F}}}}$ and ${g_i} \in {{{\mathcal {F}}}}$ represent the start and goal position of UAVi. To guarantee collision-free in the initial and terminal state for each UAV, ${{{\mathcal {R}}}}\left( {{s_i}} \right) \cap {{{\mathcal {R}}}}\left( {{s_j}} \right) = \emptyset$ and ${{{\mathcal {R}}}}\left( {{g_i}} \right) \cap \mathcal{R}\left( {{g_j}} \right) = \emptyset$ must be satisfied for all $i \ne j$. For the ${i{\mathrm{th}}}$ UAV, its trajectory is defined as ${f_i}$, where $pos\left( {{f_i}} \right) \in {f_i}$ denote the waypoint. There is no collision with obstacles when the following conditions are met:

$$\begin{aligned} {{{{\mathcal {R}}}}\left( {pos\left( {{f_i}} \right) } \right) \in \mathcal{F}},\,\,{\forall i} \end{aligned}$$

(1)

Furthermore, the collisions between any two UAVs should be avoided:

$$\begin{aligned} {{{{\mathcal {R}}}}\left( {pos\left( {{f_i}} \right) } \right) \cap {{{\mathcal {R}}}}\left( {pos\left( {{f_j}} \right) } \right) = \emptyset },\,\,{\forall i} \ne j \end{aligned}$$

(2)

In the swarm trajectory planning problem, we seek to generate kinodynamically feasible trajectories ${f_k}\left( {k = 1,2, \ldots N} \right)$ for quadrotor swarm, where obstacles and inter-vehicle collisions are avoided simultaneously.

4 Trajectory representation and optimization

4.1 Priority-based RRT

Simply applying RRT algorithm to search the initial path for the quadrotor swarm is unsafe. As illustrated in Fig. 2, the RRT algorithm only considers the collision avoidance between UAV and obstacles in the environment. Based on RRT, this paper proposes a priority-based RRT algorithm, which can generate swarm paths satisfying (1) and (2) by assigning priorities. When a UAV searches a global path, it only conducts collision checking with other UAVs that have higher priority than itself. To reduce unnecessary conflict checking, only when the distance between two UAVs (the distance between the UAVi and UAVj is expressed as ${d_{UA{V_{ij}}}}$, e.g.) is less than a certain threshold ${d_{che}}$, the conflict check will be triggered. Furthermore, the current UAV only checks the collision path conflict within a certain range to improve the efficiency. For the ${i{\mathrm{th}}}$ UAV, the range is a circle with the current UAV as the center and $r_{thr}^i$ as the radius.

Fig. 2

The RRT algorithm is applied to the front-end of three UAVs motion planning, and the search results are represented by dotted lines with different colors. The black solid blocks represent the obstacles in the environment. The hexagon stars represent the starting points, and the quadrangle stars represent the target points. Yellow, green, and red represent UAV1, UAV2, and UAV3, respectively

The procedure of priority-based RRT path generation for multi-UAV is shown in Fig. 3. Take 3 UAVs as an example and suppose their priorities are: UAV1 > UAV2 > UAV3. Firstly, three UAVs independently generate their paths using the RRT algorithm and get other paths with higher priority than itself for conflict checking. UAV1 need not check for path conflicts due to its highest priority, while the other two UAVs need to. If the current UAV finds a path conflict, it will re-search the path until there is no conflict. However, the re-search process may fail within the specified time. Then the higher priority UAV with path conflict will re-generate its path. This process is repeated until all UAVs find safe paths. The final path generation result is shown by the dotted line in Fig. 3a. Although the path of UAV3 conflicts with that of the other two UAVs, it will not re-search the path because the conflict path point is not within the range $r_{thr}^3$. When three UAVs fly forward for a while, they reach the positions shown in Fig. 3b. At this time, the path of UAV3 crosses the path of UAV1 and is within the conflict checking range of UAV3. UAV3 will re-search for a new path ensuring it will not conflict with the paths of UAV1 and UAV2 within the scope $r_{thr}^3$.

Fig. 3

Schematic diagram of priority-based RRT algorithm. The priority is UAV1>UAV2>UAV3. The radius of yellow, green, and red circles are $r_{thr}^1$, $r_{thr}^2$, and $r_{thr}^3$, respectively. a The final path search results of three UAVs. Each UAV uncross paths with others within its conflict checking scope. b When UAV3 finds a path conflict with UAV1 in the conflict check range, it will search the path again, and the new path will not conflict with the path of others within the range $r_{thr}^3$

The whole process of priority-based RRT is summarized in Alg. 1. Line 3–12 is a process of RRT, where n represents the preset maximum number of iterations. When the relative distance between UAVs is less than $d_{che}$, path collision checking will be triggered (line 13). Then the UAV will check whether its path crosses with others, which have a higher priority, within the range of the circle with its current position as the center and radius $x_{init}$ (line 14). If no path crossing is found, the search process is successful (line 17). Otherwise, search again until success (line 15–16).

4.2 Local trajectory planning and reciprocal collision avoidance

For each UAV, a piecewise polynomial is leveraged to represent the trajectory. The trajectory is parameterized to the time variable t in each dimension $\lambda$ out of x, y, z. The ${N{\mathrm{th}}}$ order M-segment trajectory of one dimension can be written as follows (Zhou 2019):

$$\begin{aligned} {p_\lambda }\left( t \right) = \left\{ {\begin{array}{*{20}{c}} {\sum \nolimits _{j = 0}^N {{\sigma _{1j}}{{\left( {t - {T_0}} \right) }^j}} }&{},&{}{{T_0} \le t \le {T_1}}\\ {\sum \nolimits _{j = 0}^N {{\sigma _{2j}}{{\left( {t - {T_1}} \right) }^j}} }&{},&{}{{T_1} \le t \le {T_2}}\\ \vdots &{},&{} \vdots \\ {\sum \nolimits _{j = 0}^N {{\sigma _{Mj}}{{\left( {t - {T_{M - 1}}} \right) }^j}} }&{},&{}{{T_{M - 1}} \le t \le {T_M}} \end{array}} \right. \end{aligned}$$

(3)

where ${{\sigma _{ij}}}$ is the ${j{\mathrm{th}}}$ order polynomial coefficient of the ${i\mathrm{th}}$ segment of the trajectory. ${T_1},{T_2}, \ldots ,{T_M}$ are the end time of each segment, and the total time ${T_{total}}$ is ${T_{total}} = {T_M} - {T_0}$.

We follow Mellinger and Kumar (2011) to construct the cost function of the piecewise polynomial trajectory. The cost function penalizing the squares of the derivatives of ${i\mathrm{th}}$ segment can be written as:

$$\begin{aligned} {J_i}({T_{total}})= & {} \int _0^{{T_{total}}} {{\sigma _{i0}}{{\left( t \right) }^2} + {\sigma _{i1}}{{\left( t \right) }^2}} + {\sigma _{i2}}{\left( t \right) ^2} \nonumber \\&+ \cdots + {\sigma _{iN}}{\left( t \right) ^2}dt = \varvec{\sigma ^T}Q\left( {{T_{total}}} \right) \varvec{\sigma }\end{aligned}$$

(4)

The M-polynomial segments can be jointly optimized by connecting their cost matrices in a block diagonal way, which is written as follows:

$$\begin{aligned} {J_{total}} = {\left[ {\begin{array}{*{20}{c}} {{\varvec{\sigma }_{{1}}}}\\ \vdots \\ {{\varvec{\sigma }_M}} \end{array}} \right] ^T}\left[ {\begin{array}{*{20}{c}} {{Q_1}\left( {{T_{{1}}}} \right) }&{}{}&{}{}\\ {}&{} \ddots &{}{}\\ {}&{}{}&{}{{Q_M}\left( {{T_M}} \right) } \end{array}} \right] \left[ {\begin{array}{*{20}{c}} {{\varvec{\sigma }_{{1}}}}\\ \vdots \\ {{\varvec{\sigma }_M}} \end{array}} \right] \end{aligned}$$

(5)

In this paper, we minimize the snap (${{{4}}\mathrm{th}}$ derivative of the position) of the UAV to obtain a smooth trajectory, and the order N of the polynomial is selected as 9. Using the method proposed in Richter et al. (2016), a mapping matrix and a selection matrix are introduced to map the polynomial coefficients to the derivatives at each segment points of the piecewise polynomial. Then the coefficients of the polynomial can be obtained by the closed-form method.

Fig. 4

Polynomial trajectories and B-spline trajectories

However, the trajectory produced by the polynomial is often close to obstacles and other UAVs since distance information in the free space is ignored, as shown in Fig. 4. Therefore, we improve the security and clearance of the initial trajectory using uniform B-spline optimization for the trajectory function $p\left( t \right)$. To initialize the control points, we sample the piecewise polynomial trajectory at equal intervals. The value of a B-spline of degree $k - 1$ can be evaluated using the following equation:

$$\begin{aligned} p\left( t \right) = \sum \limits _{i = 0}^n {{p_i}} {B_{i,k}}\left( t \right) \end{aligned}$$

(6)

A B-spline trajectory is parameterized by time t, where $t \in \left[ {{t_{k - 1}},{t_{m - k + 1}}} \right]$. ${t_j}$ $\left( {j = 0,1, \ldots ,m} \right)$ is the node value, which constitutes the node vector of the B-spline curve. ${p_i} \in {\mathbb {R}}{^n},\left( {i = [0,1, \ldots n]} \right)$ are control points and $m = n + k$, while ${B_{i,k}}\left( t \right)$ are basis functions that can be computed using the DeBoor – Cox recursive formula (Qin 2000). Uniform B-splines have a fixed time interval $\Delta t$ between their control points, which simplifies computation of the basis functions. To evaluate the position at time $[t \in [{t_l},{t_{l + 1}}) \subset \left[ {{t_{k - 1}},{t_{m - k + 1}}} \right]$, we transform time to a uniform representation $u\left( t \right) = {{\left( {t - {t_l}} \right) } \big / {\Delta t}}$. Then the position can be evaluated using the following matrix representation (Zhou 2019):

$$\begin{aligned} {\varvec{p}}\left( {u\left( t \right) } \right)&= \varvec{u}{\left( t \right) ^T}{{\varvec{M}}_k}{\varvec{p}} \end{aligned}$$

(7)

$$\begin{aligned} {\varvec{u}}\left( t \right)= & {} {\left[ {\begin{array}{*{20}{c}}1{} & {} {u\left( t \right) }{} & {} {{u^2}\left( t \right) }{} & {} \cdots{} & {} {{u^{k - 1}}\left( t \right) }\end{array}} \right] ^T} \end{aligned}$$

(8)

$$\begin{aligned} {\varvec{p}}= & {} {\left[ {\begin{array}{*{20}{c}}{{p_{l - k + 1}}}{} & {} {{p_{l - k + 2}}}{} & {} {{p_{l - k + 3}}}{} & {} \cdots{} & {} {{p_l}}\end{array}} \right] ^T} \end{aligned}$$

(9)

where ${M_k}$ is a constant matrix determined by k. In the case of our implementation, k is set as 6 since the quintic B-splines is used.

Refer to Usenko et al. (2017), the local replanning problem is represented as an optimization problem with smoothness cost, collision cost, quadratic derivative cost, and derivative limit cost. Moreover, an additional constraint on reciprocal collision avoidance is proposed in this paper. The overall cost function is defined as:

$$\begin{aligned} {Q_{total}} = {Q_s} + {Q_o} + {Q_c} + {Q_q} + {Q_l} \end{aligned}$$

(10)

where ${Q_s}$ is a smoothness cost function. ${Q_o}$ and ${Q_c}$ are the cost function of collision with static obstacles and reciprocal collision avoidance, respectively. ${Q_q}$ is the cost function of the integral over the squared derivatives (acceleration, jerk, snap), while ${Q_l}$ is a soft constraint on velocity, acceleration, jerk and snap. The detailed expressions of ${Q_s}$, ${Q_o}$, ${Q_q}$, and ${Q_l}$ can be found in Usenko et al. (2017).

The cost function of reciprocal collision avoidance penalizes the trajectory points whose distance among drones is within a certain threshold ${d_{UAV}}$. Assuming there are N UAVs in total. For the ${i\mathrm{th}}$ UAV, the cost is:

$$\begin{aligned} Q_c^i = \left\{ {\begin{array}{*{20}{c}} {\sum \nolimits _{j = 0,j \ne i}^N {\lambda _c^{ij}\int _{{t_{k - 1}}}^{{t_{m - k + 1}}} {{e^{\frac{{{d_{UAV}}^2 - {d_{UA{V_{ij}}}}^2}}{\tau }}}} dt} }&{}{{d_{UA{V_{ij}}}} \le {d_{UAV}}}\\ 0&{}{{d_{UA{V_{ij}}}} > {d_{UAV}}} \end{array}} \right. \end{aligned}$$

(11)

where ${d_{UA{V_{ij}}}}$ is the distance between the ${i\mathrm{th}}$ and the ${j\mathrm{th}}$ UAV, ${\lambda _c^{ij}}$ is a weighting parameter. In this paper, we take the same value for ${\lambda _c^{ij}}$ no matter what the values of i and j are. $\tau$ controls the rising speed of the function. The cost is negatively correlated to the distance value. The cost function of reciprocal collision avoidance is non-convex. Nevertheless, the local minimum can be avoided due to the guidance path generated from priority-based RRT.

To allow for changes in the global paths, we adopt a receding horizon planning strategy for running the local replanning algorithm in real-time. As shown in Fig. 5, the planning trajectory is divided into the execution part and planning part. For each iteration, we set the planning time horizon as $[{t_{\min }} - 2\Delta t,{t_{\max }})$. This horizon is decided by the number of control points that need to be optimized. The execution horizon is set as $[{t_{\min }} - 3\Delta t,{t_{\min }} - 2\Delta t)$. Then, after local replanning, the execution part of the local optimal trajectory is sent to the controller.

Fig. 5

The local planning strategy. The red geometry path indicates the initial path searched by the priority-based RRT algorithm and the pink curve is the polynomial trajectory. The cyan-blue curve and the green curve is the execution trajectory and planning trajectory, respectively

5 Simulation verification

5.1 System settings

The multi-UAV motion planning method proposed in this paper is implemented in C++11 with a general non-linear optimization solver NLopt. And all algorithms are executed using the robot operating system (ROS) in the Ubuntu16.04 system and run with a desktop, which has a six-core Intel i5-8500 processor running at 3.00 GHz with 8 GB RAM. The 3-D lidar module, which outputs the dense point cloud map, runs at 10 Hz and the local planner (B-spline curve) module runs at 2 Hz. The generated reference trajectory is input to the controller, and the frequency is 100 Hz. It is worth noting that the parameter settings of all drones are the same. A multi UAV simulation environment in the Gazebo simulation platform of the ROS system is built, as shown in Fig. 6.

Fig. 6

Multi-UAV simulation environment in Gazebo

5.2 Comparative testing

Comparison of front-end We compare the proposed front-end path searching algorithm with RRT. In the comparison, the starting point, endpoint, and the test environment of each UAV are set to be the same. The results are shown in Fig. 7. Compared with RRT, the paths obtained by priority-based RRT are more secure with taking a little more time since they uncross each other within the scope of their respective conflict checks. Benefit from the front-end paths not crossing, the computation burden from reciprocal collision avoidance constraint in the B-spline optimization process will be reduced.

Fig. 7

a The initial paths of three UAVs are searched by RRT algorithm. b The initial paths of three UAVs are searched by priority-based RRT algorithm, where the side length of each grid is set to 1 m, $r_{thr}^1 = r_{thr}^2 = r_{thr}^3 = 5m$. The average search time is 12.425 and 19.707 ms, respectively

Comparison of back-end A comparison of our back-end and method (Usenko et al. 2017) is conducted. The velocity limit is 1.5m/s. For fairness, we use the same guidance path generated by priority-based RRT as the front-end. The comparison results are shown in Fig. 8. When the guidance paths of the two UAVs are close, the optimization method proposed in Usenko et al. (2017) may lead to collision (Fig. 8a, b). Since reciprocal collision as a constraint is considered, the resulting trajectories optimized by our approach are safer (Fig. 8c, d). In addition, an inverse point-to-point transition is designed to make inter-vehicle collision avoidance inevitable during flight. The simulation results are summarized in Table 1. Despite the sacrifice of a few milliseconds and costs, reciprocal collision avoidance can be guaranteed and the aggressiveness of the trajectories is maintained.

Fig. 8

Trajectory optimization results. The red, pink, cyan-blue, and green curves represent the guidance path, polynomial trajectory, execution trajectory, and planning trajectory respectively. To make the trajectories clearly visible, the voxels of obstacles in b and d are not displayed. a, b The trajectories are optimized by the method (Usenko et al. 2017). c, d The trajectories are optimized by our method

Table 1 Comparison of trajectory optimization

5.3 Simulation flight test

Flight tests of three UAVs in the Gazebo simulation environment are conducted. In the test, all UAVs are planned online in a distributed manner, and they are commanded to fly to their respective targets. Each UAV obtains the pose information of the other two UAVs so that the relative distance information between the UAVs can be calculated. State estimation, mapping, control, and trajectory generation are all performed online without any prior information about the environments. Representative figures which record the flight are shown in Figs. 9 and 10, where each UAV generates a safe and smooth trajectory and travels through complex environments autonomously. In Figs. 9 and Fig. 10, we assume that the top trajectory is generated by UAV1, the middle is generated by UAV2, and the bottom is generated by UAV3. The cyan-blue curve and the green curve is the execution trajectory and planning trajectory, respectively.

Fig. 9

Three UAVs fly in simulation. a UAV1 and UAV3 can successfully avoid obstacles. b UAV1 and UAV2 can successfully avoid the inter-vehicle collision

Fig. 10

Complete global map and trajectories with obstacle and collision avoidance of the three UAVs

6 Conclusion and future work

In this paper, a decentralized motion planning approach for multi-UAV autonomous navigation is developed. The online motion planning problem for multi-UAV is decoupled as a front-end path searching and a back-end nonlinear trajectory optimization. A priority-based RRT, as a front-end, is proposed in this paper to generate an online global path for each quadrotor without reciprocal chiasmata and no collision with obstacles. Then uniform B-spline curve, as a back-end, is adopted to improve the security and clearance of the trajectory with the initial value of polynomial sampling trajectory. To further improve the security, reciprocal collision as a constraint is added to the cost function to be optimized. The efficiency of the developed algorithm is validated in Gazebo.

In future work, we plan to transplant the algorithm into the fully autonomous UAV platform for an experiment in the real-world.