Reference Path Planning for UAVs Formation Flight Based on PH Curve

Abstract

Formation flight of Unmanned aerial vehicles (UAVs) has become a research hotspot in recent years. How to plan a flyable path for UAV formation when flying with a given configuration to the destination safely is an important technology. Therefore, this paper proposes a path planning method based on Pythagorean Hodograph (PH) curves and Delaunay triangulation to generate a flyable reference path for UAV formation. Firstly, the kinematic constraints of the formation path are derived, while the formation moves. Secondly, the Delaunay triangulation and the Warshall-Floyd algorithm are used to obtain the best waypoints array from starting point to destination. Thirdly, the PH curves are applied to connect each two neighbour waypoints for meeting the kinematic constraints. The multi-population hybrid particle swarm genetic algorithm is proposed to generate an optimal flyable formation path. Finally, simulations are carried out considering a formation with three UAVs in a complicated environment. The simulation results show that the paths planned are connected by several PH curves, and all the paths can meet the kinematic constraints, avoid the obstacles and threat zones. Furthermore, the single population particle swarm genetic algorithm is also applied in the same simulation, and the simulation results show that the multi-population hybrid particle swarm genetic algorithm proposed in this paper has faster convergence speed and better stability.

1 Introduction

Unmanned aerial vehicles (UAVs) are widely used in both civilian and military fields, such as search and rescue, environmental monitoring, surveillance and attacks. Based on the multi-agent theory and applications [1], a group of UAVs flying in a formation rather than a single UAV can obtain higher combating effectiveness, deliver greater coverage, and will have wider applications in the future. Therefore, UAV formation flight has become a research hotspot in recent years [2, 3]. How to plan a safe flyable path for UAV formation when flying with a given configuration to the destination is one of the most important technologies for UAV formation flight.

The current research about UAV formation flight rarely involves the planning of the formation reference path, and it is usually assumed that the formation reference path is known in advance [4, 5]. The path planning problem in commercial intercontinental aviation formation flight has been studied to determine the optimal formation scheduling plan. However, there is less research on path planning in the formation maintenance phase [6]. The motion equations of flexible virtual structure formation have been deduced when moving along a reference path as a whole [7]. However, the Dubins path used has discontinuous curvature. The comprehensively improved particle swarm optimization (PSO) algorithm has been proposed for UAV formation path planning under environmental and collision avoidance constraints in [8]. However, the UAVs’ kinematic constraints have not been considered.

The Pythagorean hodograph (PH) curve [9] is parameterized by polynomials as a function of the length and provides closed-form polynomials for curve length and curvature. In addition, the position and the direction at the initial and the final locations are directly considered as boundary conditions, trading off the curve length and its curvature to meet the maximum-curvature constraint. Therefore, the PH curve is gradually being used for UAV path planning research [10, 11].

In this paper, the focus is on generating a safe flyable reference path for UAV formation when flying with a given configuration to the destination. Based on the PH curves and Delaunay triangulation, the multi-population hybrid particle swarm genetic algorithm is proposed to generate an optimal flyable formation reference path that considers the path’s kinematic constraints, length, smoothness, stealth and safety. The rest of this paper is organized as follows: The problem formulation is presented in Sect. 2. The kinematic constraints of the formation path are derived in Sect. 3. Section 4 describes the proposed path planning method for the UAV formation reference path. The simulation results are presented in Sect. 5. The summary and conclusions are provided in Sect. 6.

2 Problem Formulation

2.1 Reference Path Planning for UAV Formation Flight

In this paper, the UAV formation configuration is defined with the virtual structure approach [12], which means the path of the virtual point $O_{f}$ (or formation center) can be defined as the formation’s reference path. Therefore, the formation reference path planning is equivalent to the virtual point path planning. Assume that the obstacles and the no-fly zones in the environment are described by rectangles, the radar threats are described by circles, and the threat information is known in advance.

Normally, the starting pose (position and direction) at the initial location of UAV formation is known as $p_{s} = \left( {x_{s} ,y_{s} ,\chi_{s} } \right)$, and the formation pose $p_{d} = \left( {x_{d} ,y_{d} ,\chi_{d} } \right)$ at the destination is also known in advance. The formation reference path planning can be mathematically represented as:

$$ p_{s} = \left( {x_{s} ,y_{s} ,\chi_{s} } \right)\mathop{\longrightarrow}\limits^{{\mathop \coprod \limits_{\min J} r_{f} (q)}}p_{d} = \left( {x_{d} ,y_{d} ,\chi_{d} } \right) $$

(1)

where $r_{f} (q)$ is the formation reference path, $q$ is defined as a path parameter, and $\coprod$ represents the constraints including formation kinematic constraints and safety requirements, while J is the comprehensive performance index of the path, consisting of fuel consumption, smoothness, stealth, and safety.

2.2 Pythagorean Hodograph Curve

The PH curve is defined by a parameterized polynomial curve that has hodographs satisfying a Pythagorean condition. The definition is given as:

Definition 1.

(PH curve [9]). Suppose that ${\mathbf{r}}(q) = \{ x(q),y(q)\}$ is a polynomial curve parameterized by $q$. ${\mathbf{r}}(q)$ is a PH curve, if the first derivatives of its components satisfy the following Pythagorean condition:

$$ x^{{\prime}{2}} (q) + y^{{\prime}{2}} (q) = \sigma^{2} (q) $$

(2)

where $x(q)$, $y(q)$ and $\sigma (q)$ are polynomials about $q$.

In view of numerical stability, the PH curve can be written in the Bernstein-Bézier form [9] as:

$$ {\mathbf{r}}(q) = \sum\limits_{k = 0}^{n} {{\mathbf{p}}_{k} \left( {\begin{array}{*{20}c} n \\ k \\ \end{array} } \right)(1 - q)^{n - k} q^{k} } ,\begin{array}{*{20}c} {} & {q \in [0,1]} \\ \end{array} $$

(3)

where ${\mathbf{p}}_{k}$ represent the control points of the curve, and $n$ is the order of the curve. According to [13], the lowest order of PH curve that has an inflection point is the fifth, called the quintic PH curve. The inflection point provides sufficient flexibility in path shape to be appropriate for UAV’s path planning. Therefore, the quintic PH curve is applied for UAV formation path planning in this paper.

Generally, the poses $p_{s}$ and $p_{d}$ are known beforehand, namely boundary conditions $r(0)$, $r^{\prime}(0)$, $r(1)$ and $r^{\prime}(1)$. Then, a PH path is determined by $m_{0} = \left\| {{\mathbf{r}}^{\prime}(0)} \right\|$ and $m_{1} = \left\| {{\mathbf{r}}^{\prime}(1)} \right\|$, considered as the control variables. Hence, appropriate $m_{0}$ and $m_{1}$ are optimized to ensure that the formation maximum curvature constraints are satisfied. The detailed solving process of the PH curve can be found in [14]. The path length $L(q)$ and the curvature $\kappa (q)$ of the PH path can be solved by the following equations.

$$ L(q) = \int {\left\| {{\mathbf{r}}^{\prime}(q)} \right\|} \begin{array}{*{20}c} , & {q \in [0,1]} \\ \end{array} $$

(4)

$$ \kappa (q) = \frac{{\left| {{\mathbf{r}}^{\prime}(q) \times {\mathbf{r}}^{\prime\prime}(q)} \right|}}{{\left| {{\mathbf{r}}^{\prime}(q)} \right|^{3} }} $$

(5)

where ${\mathbf{r}}^{\prime}(q)$, and ${\mathbf{r}}^{\prime\prime}(q)$ are the first and the second derivatives of ${\mathbf{r}}(q)$, respectively. Obviously, the length and the curvature of a PH path are rational polynomials. To quantitatively describe the smoothness of a PH path, the elastic energy is defined as the integral of squares of the arc length, namely:

$$ E = \int {\kappa^{2} ds} $$

(6)

2.3 Delaunay Triangulation

The Delaunay triangulation, also named Delaunay graph, is widely used in battlefield environment modeling and path planning due to its good division characteristics of planes [15]. It has the following important properties:

• (1) Uniqueness: for a given vertex set, starting from any vertex in the region, the resulting Delaunay graph is unique.

• (2) Hollowness: the circumcircle of any triangle in the Delaunay triangulation does not contain other vertices in the vertex set.

• (3) Maximize the minimum angle characteristic: the triangle in the Delaunay triangulation is as close as possible to an equilateral triangle.

• (4) Regional: adding or moving a vertex will only affect the triangles adjacent to the vertex.

In the plane of formation flight, the Delaunay triangulation is constructed with the centers of the no-fly zones, obstacles and radar threats. Then, the connection diagram of the interconnection of each threat is obtained. Each connection is called Delaunay edge. The Delaunay edges with the shortest distance between the threats less than the diameter of the formation are directly eliminated.

In the traditional path planning research based on Delaunay triangulation, the midpoint of the Delaunay edge is used as an alternative waypoint. However, the generated waypoint may be too close to a larger threat. Therefore, the following improvements are made in the Delaunay triangulation: the alternative waypoint is located on the Delaunay edge, and the distances from the two threats connected to this waypoint are proportional to the size of the two threats. Figure 1 shows the distribution of alternative waypoints generated after adopting the improved strategy, where ‘○’ represents the generated alternative waypoints.

Fig. 1

Improved Delaunay triangulation

3 Kinematic Constraints of UAV Formation

Assume that the speed and the turning angle rate of each UAV in the formation are known to be constrained as $0 < V_{i,\min } \le V_{i} \le V_{i,\max }$ and $\left| {\omega_{i} } \right| \le \omega_{i,\max }$, with $i = 1,2, \cdots ,N$, respectively, while N is the number of UAVs in the formation. Since the formation can be approximated as a virtual rigid body when flying in a fixed configuration, to derive the kinematic constraints of the overall movement of the formation, namely, the minimum turning radius or maximum curvature constraints of the formation, the following assumptions are made:

Assumption 1.

Assume that any UAV formation in a fixed formation can be regarded as a virtual rigid body, and the instantaneous movement of the rigid body at any point on the reference path of the formation is a fixed axis rotation around a certain virtual axis, as shown in Fig. 2.

Fig. 2

Schematic diagram of formation instantaneous movement

where $O_{{\text{g}}} x_{{\text{g}}} y_{{\text{g}}}$ is the inertial system, $O_{f}$ is the formation center (or virtual point), $O_{f} x_{f} y_{f}$ is the motion coordinate system fixed to the virtual point, namely the formation system, and $(x_{{{\text{d}}fi}} ,y_{{{\text{d}}fi}} )$ is the desired relative position of UAV i in the formation system.

Generally, the formation reference path can be expressed as $r_{f} (q) = \{ x_{f} (q),y_{f} (q)\}$, where $q \in [0,1]$ is the path parameter, and the corresponding velocity and curvature types are $\{ V_{f} (q),\kappa_{f} (q)\}$. According to Assumption 1 and the desired formation configuration, the curvature $\kappa_{i} (q)$ and speed $V_{i} (q)$ of the UAV i can be obtained as:

$$ \kappa_{i} (q) = \frac{{{\text{sign}}(\kappa_{f} (q))}}{{\sqrt {({1 \mathord{\left/ {\vphantom {1 {\kappa_{f} (q)}}} \right. \kern-0pt} {\kappa_{f} (q)}} - y_{{{\text{d}}fi}} )^{2} + x_{{{\text{d}}fi}}^{2} } }} $$

(7)

$$ V_{i} (q) = \frac{{V_{f} (q)\kappa_{f} (q)}}{{\kappa_{i} (q)}} = V_{f} (q)\sqrt {(1 - \kappa_{f} (q)y_{{{\text{d}}fi}} )^{2} + (\kappa_{f} (q)x_{{{\text{d}}fi}} )^{2} } $$

(8)

Definition 2.

(Flyable formation reference path). For any given desired formation configuration $\{ (x_{{{\text{d}}fi}} ,y_{{{\text{d}}fi}} ),i = 1,2, \cdots ,N\}$, if there is a path $r_{f} (q)$ with continuous curvature that satisfies $\left| {\omega_{i} (q)} \right| \le \omega_{i,\max }$ and $0 < V_{i,\min } \le V_{i} (q) \le V_{i,\max }$, $i = 1,2, \cdots ,N$, $q \in [0,1]$, at any point q on the path, then the path is called a flyable formation reference path.

• (1) To satisfy $\left| {\omega_{i} (q)} \right| \le \omega_{i,\max }$, $i = 1,2, \cdots ,N$, $q \in [0,1]$, it can be known from the assumption that:

  $$ \left| {V_{f} (q)\kappa_{f} (q)} \right| \le \min \{ \omega_{1,\max } , \cdots ,\omega_{N,\max } \} $$

  (9)

  Let $\omega_{\max } = \min \{ \omega_{1,\max } , \cdots ,\omega_{N,\max } \}$, $V_{f\max } = \mathop {\sup }\limits_{q} V_{f} (q)$, then Eq. (9) is rewritten as:

  $$ \left| {V_{f} (q)\kappa_{f} (q)} \right| \le \left| {V_{f\max } } \right|\left| {\kappa_{f} (q)} \right| \le \omega_{\max } $$

  (10)

  For fixed-wing UAV formations, $V_{f} (q) > 0$, so the following can be obtained:

  $$ \left| {\kappa_{f} (q)} \right| \le \kappa_{\max 1} = \frac{{\omega_{\max } }}{{V_{f\max } }},\begin{array}{*{20}c} {} & {q \in [0,1]} \\ \end{array} $$

  (11)

• (2) To satisfy $0 < V_{i,\min } \le V_{i} (q) \le V_{i,\max }$, $i = 1,2, \cdots ,N$, $q \in [0,1]$, from Eq. (8), the following can be obtained:

  $$ V_{i,\min } \le V_{f} (q)\sqrt {(1 - \kappa_{f} (q)y_{{{\text{d}}fi}} )^{2} + (\kappa_{f} (q)x_{{{\text{d}}fi}} )^{2} } \le V_{i,\max } $$

  (12)

Let $x_{{{\text{d}}fi}} = D_{fi} \cos \varphi_{fi}$, $y_{{{\text{d}}fi}} = D_{fi} \sin \varphi_{fi}$, $D_{fi} \ge 0$, $\varphi_{fi} \in [ - \pi ,\pi ]$, and substitute them into Eq. (12):

$$ \left( {\frac{{V_{i,\min } }}{{V_{f} (q)}}} \right)^{2} - 1 \le (\kappa_{f} (q)D_{fi} )^{2} - 2\kappa_{f} (q)D_{fi} \sin \varphi_{fi} \le \left( {\frac{{V_{i,\max } }}{{V_{f} (q)}}} \right)^{2} - 1 $$

(13)

Assume that the safety radius of the formation $R_{{{\text{uavs}}}}$ is much smaller than the turning radius of the formation $R_{f} (q)$, namely:

$$ D_{fi} \le R_{{{\text{uavs}}}} \ll R_{f} (q) = {1 \mathord{\left/ {\vphantom {1 {\kappa_{f} (q)}}} \right. \kern-0pt} {\kappa_{f} (q)}} $$

(14)

Then $\kappa_{f} (q)D_{fi} \ll 1$, ignoring the second-order small quantity in Eq. (13), the following can be obtained:

$$ 1 - \left( {\frac{{V_{i,\max } }}{{V_{f} (q)}}} \right)^{2} \le 2\kappa_{f} (q)y_{{{\text{d}}fi}} \le 1 - \left( {\frac{{V_{i,\min } }}{{V_{f} (q)}}} \right)^{2} $$

(15)

Let $C_{i} = \mathop {\inf }\limits_{q} \min \left\{ {\begin{array}{*{20}c} {\left( {\frac{{V_{i,\max } }}{{V_{f} (q)}}} \right)^{2} - 1,} & {1 - \left( {\frac{{V_{i,\min } }}{{V_{f} (q)}}} \right)^{2} } \\ \end{array} } \right\}$, so if $\left| {2\kappa_{f} (q)y_{{{\text{d}}fi}} } \right| \le C_{i}$, then Eq. (15) is established. Further, if $y_{{{\text{d}}fi}} = 0$, then Eq. (15) is naturally established, if $y_{{{\text{d}}fi}} \ne 0$, then there is:

$$ \left| {\kappa_{f} (q)} \right| \le \frac{{C_{i} }}{{\left| {2y_{{{\text{d}}fi}} } \right|}},\begin{array}{*{20}c} {} & {i = 1,2, \cdots ,N} \\ \end{array} \begin{array}{*{20}c} , & {q \in [0,1]} \\ \end{array} $$

(16)

Let $\kappa_{\max 2} = \min \left\{ {\frac{{C_{1} }}{{\left| {2y_{{{\text{d}}f1}} } \right|}}, \cdots ,\frac{{C_{N} }}{{\left| {2y_{{{\text{d}}fN}} } \right|}}} \right\}$, the following can be obtained:

$$ \left| {\kappa_{f} (q)} \right| \le \kappa_{\max 2} \begin{array}{*{20}c} , & {q \in [0,1]} \\ \end{array} $$

(17)

Therefore, the maximum curvature constraint for the formation to maintain a given formation is:

$$ \left| {\kappa_{f} (q)} \right| \le \kappa_{\max } = \min \{ \kappa_{\max 1} ,\kappa_{\max 2} \} ,q \in [0,1] $$

(18)

It can be seen from the derivation that Eq. (18) is only applicable to the situation where the formation scale is much smaller than the turning radius of the formation and is not suitable for large-scale formation.

4 Formation Reference Path Planning

4.1 Intermediate Waypoints Search

After generating the candidate waypoints, the next step is to search for the optimal waypoint sequence from the start point to the destination point. Firstly, construct a path graph ${\text{G}}^{*} = ({\text{V}}^{*} {\text{,E}}^{*} )$ with the start point, end point and alternative waypoints as the vertex set ${\text{V}}^{*}$, and the connection of each vertex as the edge set ${\text{E}}^{*}$. Assign a cost value to each edge in the graph ${\text{G}}^{*}$ to evaluate the pros and cons of the path edge, and obtain the cost matrix ${\varvec{W}}^{*}$, where the cost value $w_{i,j}$ can be written as:

$$ w_{i,j} = \alpha d_{i,j} + \beta R_{i,j} + J_{{{\text{safe}},i,j}} $$

(19)

where $\alpha$ and $\beta$ are the weights, $d_{i,j}$ is the length of the side, $R_{i,j}$ and $J_{{{\text{safe}},i,j}}$ are the radar threat cost and the security penalty cost of the side, respectively, if the side passes through the no-fly zone or obstacle, $J_{{{\text{safe}},i,j}} = + \infty$, otherwise $J_{{{\text{safe}},i,j}} = 0$. Set the starting point as 1, and the destination point as n.

After obtaining the cost matrix ${\varvec{W}}^{*}$, the Warshall-Floyd algorithm is used to search for the optimal waypoint sequence from the start point to the destination point. The steps are as follows:

• Step 1. According to ${\varvec{W}}^{*}$, for all $i = 1,2, \cdots ,n$ and $j = 1,2, \cdots ,n$, set $c_{i,j} = w_{i,j}$, $k = 1$;

• Step 2. For all $i = 1,2, \cdots ,n$ and $j = 1,2, \cdots ,n$, if there is $c_{i,k} + c_{k,j} < c_{i,j}$, set $c_{i,j} = c_{i,k} + c_{k,j}$;

• Step 3. If $k = n$, go to Step 4, otherwise go to Step 2, $k = k + 1$;

• Step 4. Set $t = 1$, $P(t) = n$, $tt = n$;

• Step 5. For $i = 1,2, \cdots ,n$, set $e_{1,i} = c_{1,tt} - w_{i,tt}$, if $e_{1,i} = c_{1,i}$, then $P(t + 1) = i$, $tt = i$, $t = t + 1$;

• Step 6. If $tt = 1$, stop and output the optimal waypoint arrangement P, otherwise, go to Step 5.

4.2 Path Optimization

The path searched by the Warshall-Floyd algorithm is composed of a set of straight-line segments, so it is not the optimal flyable path for the formation. Thus, further optimization processing is required. Firstly, it is assumed that the searched intermediate waypoints can slide on the corresponding Delaunay side, and the multi-population hybrid particle swarm genetic algorithm (MHPSGA) is used to optimize the positions of the intermediate waypoints. Then, the PH curves are used to connect the waypoints, and the MHPSGA is used to optimize the PH curve parameters. The solution process of the MHPSGA is shown in Fig. 3.

Fig. 3

The solution process of the multi-population hybrid particle swarm genetic algorithm

The evolution mechanism of each subpopulation is the hybrid particle swarm genetic algorithm (HPSGA), which introduces the crossover and mutation operations from the genetic algorithm. This algorithm searches for the optimal solution by crossing the particles with the individual extremums or group extremums, as well as the particle mutation. In addition, the subpopulations evolve independently and only replace the worst individuals with the extreme individuals of other subpopulations every certain generation (immigration operation). The crossover and the mutation probabilities of each subpopulation are randomly generated in the interval $(0.7,0.9)$ and $(0.01,0.1)$ respectively.

4.2.1 Optimization of Intermediate Waypoint Position

Suppose the waypoint sequence searched by the Warshall-Floyd algorithm is $P = (p_{1} ,p_{2} , \cdots ,p_{n} )$, where $p_{1}$ and $p_{n}$ are the start and the end points, respectively. For the intermediate waypoints $p_{i}$, $i = 2, \cdots ,n - 1$, suppose the two threat bodies connected by the Delaunay side are $T_{i1}$ and $T_{i2}$. According to the formation configuration and the size of threat bodies, the coordinates of the two closest points on the edge close to the two threats can be obtained as $p_{i1}$ and $p_{i2}$. Therefore, the sliding interval of the waypoint must be between $p_{i1}$ and $p_{i2}$, and is described as:

$$ p_{i} = p_{i1} + t_{i} (p_{i2} - p_{i1} ),\begin{array}{*{20}c} {} \\ \end{array} t_{i} \in [0,1],\begin{array}{*{20}c} {} \\ \end{array} i = 2, \cdots ,n - 1 $$

(20)

According to Eq. (20), each group $\{ t_{i} ,i = 2, \cdots ,n - 1\}$ corresponds to a new straight path, so the particles to be optimized can be written as ${\varvec{z}} = (t_{2} ,t_{3} , \cdots ,t_{n - 1} )$.

The quality of the path is characterized by the fitness of the particles. The path performance indicators mainly include fuel consumption performance, smooth performance, stealth performance, and safety. Therefore, the particle fitness function can be designed as:

$$ J = {1 \mathord{\left/ {\vphantom {1 {(\alpha L + \beta E + \gamma J_{r} + J_{{{\text{safe}}}} )}}} \right. \kern-0pt} {(\alpha L + \beta E + \gamma J_{r} + J_{{{\text{safe}}}} )}} $$

(21)

where $\alpha$, $\beta$ and $\gamma$ are the weights, L is the length of the path, which is equal to the sum of the lengths of the straight lines connecting the waypoints, $E$ is the path bending cost that is related to the turning angle of each waypoint, $J_{r}$ is the radar threat cost and $J_{{{\text{safe}}}}$ is the penalty cost that is used to ensure the safety of each path segment. The path bending cost E can be obtained as:

$$ E = \sum\limits_{k = 1}^{n} {(1 - \cos \varphi_{k} )} $$

(22)

where $\varphi_{k}$ is the turning angle at waypoint k.

The radar threat cost of the entire formation is equal to the sum of the radar threat cost of each UAV. According to the radar threat characteristics [15], the radar threat cost can be expressed as:

$$ J_{r} = \sum\limits_{i = 1}^{{N_{r} }} {\sum\limits_{j = 1}^{N} {\int_{0}^{{S_{j} }} {\frac{1}{{d_{ij}^{4} (s)}}ds} } } $$

(23)

where $N_{r}$ is the number of radars, $N$ is the number of UAVs in the formation, $S_{j}$ is the path length of UAV j, and $d_{ij}$ is the distance from a point on the path to radar i. For simplification, each path segment is uniformly discretized into $N_{p}$ points. For each radar, only the threat cost of the path segment located in the threat circle is calculated. In addition, the crossover and the mutation operations in the algorithm adopt arithmetic crossover and uniform mutation, respectively.

4.2.2 Optimization of PH Curve Parameters for Each Waypoint

Assume that the optimal waypoint arrangement after optimization is $P^{\prime} = (p_{1} ,p^{\prime}_{2} , \cdots ,p^{\prime}_{n - 1} ,p_{n} )$, then the PH curves will be further used to connect the waypoints, and the PH curve parameters are optimized through the MHPSGA to ensure that the path meets the formation kinematics constraints.

To obtain a PH curve between any two waypoints, it is necessary to determine the direction $\chi$ and the direction vector modulus $m$ of each waypoint. Since the directions of the start and the end points are known, only the direction vector modulus needs to be determined. Therefore, the particle form of the formation PH path to be optimized is ${\varvec{z}} = (m_{1} ,m_{2} ,\chi_{2} , \cdots ,m_{n - 1} ,\chi_{n - 1} ,m_{n} )$. It should be noted that if $n = 2$, namely the path does not contain any intermediate waypoints, the particle will be ${\varvec{z}} = (m_{1} ,m_{2} )$.

To avoid blind search, a heuristic method is used to limit the search range of particles. The value ranges are given by the following formulas:

$$ m_{i} \in [0.05dis,dis],\,dis = \left\{ {\begin{array}{*{20}l} {d_{1,2} \begin{array}{*{20}c} , & {i = 1} \\ \end{array} } \hfill \\ {(d_{i - 1,i} + d_{i,i + 1} )\begin{array}{*{20}c} , & {i = 2, \cdots ,n - 1} \\ \end{array} } \hfill \\ {d_{n - 1,n} \begin{array}{*{20}c} , & {i = n} \\ \end{array} } \hfill \\ \end{array} } \right. $$

(24)

$$ \chi_{i} \in [\chi_{i}^{*} - {\pi \mathord{\left/ {\vphantom {\pi 4}} \right. \kern-0pt} 4},\chi_{i}^{*} + {\pi \mathord{\left/ {\vphantom {\pi 4}} \right. \kern-0pt} 4}]\begin{array}{*{20}c} , & {i = 2, \cdots ,n - 1} \\ \end{array} $$

(25)

where $d_{i,i + 1}$ is the straight-line distance between the waypoints $p^{\prime}_{i}$ and $p^{\prime}_{i + 1}$, and $\chi_{i}^{*}$ is the heading of the vector connecting the waypoints $p^{\prime}_{i - 1}$ and $p^{\prime}_{i + 1}$.

The particle fitness function has the same form as Eq. (21). The difference is that the path length L is equal to the sum of the path lengths of each PH curve, and the path bending cost E is equal to the sum of the bending energy of each PH curve. The penalty cost $J_{{{\text{safe}}}}$ not only guarantees path safety but also ensures the maximum curvature constraint of the path.

5 Simulation Results

To verify the effectiveness of the method proposed in this paper, simulations are carried out with three UAVs flying in formation. The programming environment is MATLAB 8.0. The obstacles, no-fly zones and radar threats are randomly placed in the flight environment. The desired formation configuration is $(x_{{{\text{d}}f1}} ,y_{{{\text{d}}f1}} ) = (0.5,0){\text{km,}}\begin{array}{*{20}c} {} \\ \end{array} (x_{{{\text{d}}f2}} ,y_{{{\text{d}}f2}} ) = ( - 0.3,0.4){\text{km,}}\begin{array}{*{20}c} {} \\ \end{array} (x_{{{\text{d}}f3}} ,y_{{{\text{d}}f3}} ) = ( - 0.3, - 0.4){\text{km}}$, and the formation safety radius is $R_{{{\text{uavs}}}} = 0.6\,{\text{km}}$. The speed of each UAV and the maximum turning angle rate are constrained to $80\,{\text{m/s}} \le V_{i} \le 125\,{\text{m/s}}$ and $\omega_{i,\max } = 0.1\,{\text{rad/s}}$, $i = 1,2,3$, respectively. Assume that the formation center maintains a constant speed $V_{f} = 100\,{\text{m/s}}$ in the path planning. Therefore, the maximum curvature constraint of the formation path can be obtained from Eq. (18) as $0.45\,{\text{km}}^{ - 1}$.

The number of subpopulations is $N_{{{\text{pop}}}} = 5$, the number of particles in each population is $M = 20$, and the maximum number of evolutions is $T = 50$. The migration operation is performed every 5 generations. The weights in Eq. (21) are $\alpha = 0.03$, $\beta = 0.03$, and $\gamma = 0.1$.

The simulation first performs formation reference path planning under 4 different starting and ending positions of the formation, as shown in Table 1. The planning results are shown in Fig. 4, where ■ and ★ represent the starting and the ending points, respectively. The solid straight lines connecting the starting and ending points are the optimal paths searched by the Warshall-Floyd algorithm. The thick black curves are the optimal flyable paths optimized by the MHPSGA combined with the PH curves, in which the optimized intermediate waypoint is represented by ●. Figure 4 shows that all the planned formation reference paths can avoid the no-fly zones and the obstacles in the environment, and can well avoid the radar threat zones. Figure 5 shows that all four paths can meet the maximum curvature constraint of the formation.

Table 1 Four different formation poses at starting and ending positions

Fig. 4

Formation reference paths under different starting and ending poses

Fig. 5

Curvature of the four paths

Furthermore, the single population particle swarm genetic algorithm is applied in the same simulation as path 1 for comparison, and five simulation experiments are conducted. The evolution process of the five simulation experiments is shown in Fig. 6. It can be seen from the figure that the MHPSGA evolves for 30 generations to obtain better results, while the single population algorithm requires 90 generations. This means that the MHPSGA converges faster than the single population algorithm.

Fig. 6

The evolution process of five simulation experiments with MHPSGA and HPSGA

6 Conclusion

This paper studies the problem of formation reference path planning in complicated environments, that is, planning a safe optimal flyable path for a UAV formation in a given configuration. Firstly, the formation is regarded as a virtual rigid body, and the kinematic constraints of the formation are deduced. Then the environment is modelled based on the improved Delaunay triangulation, and the Warshall-Floyd algorithm is used to search for the optimal waypoint sequence. Finally, the multi-population hybrid particle swarm genetic algorithm is used to optimize the position of the intermediate waypoints and the PH curves are used to connect the waypoints to meet the formation kinematics constraints. The simulation results demonstrate and verify the effectiveness of the method proposed in this paper. Compared with the single-population hybrid particle swarm genetic algorithm, the multi-population hybrid particle swarm genetic algorithm has a faster convergence rate and better searching stability. In future study, the authors will expand the proposed method to three-dimensional path planning problems.