2017 11th Asian Control Conference (ASCC)

Gold Coast Convention Centre, Australia

December 17-20, 2017

RHO-based Convex Optimization Method Applied to Cooperative

Trajectory Planning for Multiple UAVs
Hui Zhou, Zhihao Cai, Jiang Zhao and Yingxun Wang, Beihang University

Abstract— This paper presents an optimization algorithm for

II. FUNDAMENTALS
trajectory planning of multiple unmanned aerial vehicles
(UAVs). The goal is to generate three-dimensional trajectories In this section, the model for the cooperative trajectory
satisfying the initial and terminal state constraints. The planning of UAVs is established, including dynamics
framework of cooperative trajectory planning is developed based equations and constraint conditions.
on receding horizon optimization (RHO). The solution of
trajectory optimization is obtained by using the convex
A. Trajectory Dynamics
optimization method, which serves as a convenient tool for
dealing with equality and inequality constraints. The obstacle The trajectory planning dynamics equation for multiple
avoidance is also achieved by transforming the non-convex UAVs are：
constraints to convex constraints. The effectiveness of the
trajectory planning algorithm is tested by numerical simulations. vi [k  1]  vi [k ]  hai [k ] (1)

I. INTRODUCTION
With the rapid development of unmanned aerial vehicles
(UAVs) technology in the world today, the need for UAVs has
increased, and the use of UAVs has become more and more
complicated. UAVs’ cooperative flight planning is an
important part of UAVs autonomous flight research, which
mainly means that the optimal flight trajectories are generated
to satisfy the UAVs’ maneuverability and battlefield
environment constraints under the conditions of knowing
takeoff point and terminal point.
There are many different methods to get the optimal design
for UAVs cooperative trajectory planning. The gauss
pseudo-spectrum method (GPM) is proposed to solve the
problem which is a nonlinear optimal control problem with
trajectory and boundary constraints in Refs [1-2]. A
three-dimensional trajectory planning algorithm of UAVs
which based on improved A* algorithm is proposed in order to
solve problem of trajectory planning for UAVs in complex
environment in Refs [3-5]. Refs [6-7] propose two different
improved PSO algorithms to generate three-dimensional
cooperative trajectories of multiple UAVs. And convex
optimization can solve the problem of UAVs about dealing
with the constraints more quickly, and can solve the trajectory
planning by using the inherent toolkit. Convex optimization
algorithm for aircraft trajectory planning with avoidance
constraints can be found in Refs [8-10], which also uses
simplified dynamics and constraints
On the basis of Ref [10], we add the idea of receding
horizon optimization (RHO) to solve the three-dimensional
cooperative trajectory planning better. This paper is organized
as follows: In Section II, we establish the dynamics equations
and trajectory constraints of UAVs. In Section III, the
framework of RHO-based convex optimization method is
developed. In Section IV, we mainly propose four groups of
examples and analyze the results.
The authors are with the School of Automation Science and Electrical
Engineering, Beihang University, Beijing, China.
h2
pi [k  1]  pi [k ]  hvi [k ]  ai [k ] (2)
2
we denote with pi [k ]  R3 the position of the UAV
i  {1,..., N } at discrete times k  {1,..., K } . Its acceleration
and velocity are indicated by ai [k ] and vi [k ] , respectively.
And h is the discretization time step.
B. Trajectory Constraints
We can get the equality constraints through (1) - (2), which
include the initial state and the terminal state of the UAVs. In
addition to the equality constraints of each UAV, inequality
constraints in trajectory planning also need to be considered,
such as position constraints, acceleration constraints, velocity
constraints, jerk constraints, obstacle avoidance constraints.
Initial state and terminal state: The initial state and the
terminal state of UAVs are as followed:
pi [1]  p0 , pi [ K ]  pK , vi [1]  v0 , vi [ K ]  vK (3)
Position constraints: Limit the flyable area of the UAVs.
pmin,l  pi ,l [k ]  pmax,l , l {x, y, z} (4)
Acceleration constraints: The UAVs’ thrust is limited,
adding acceleration constraints as follows:
amin,l  ai ,l [k ]  amax,l , l {x, y, z} (5)
Velocity constraints: On a UAV, the collective thrust is
limited by a minimum and a maximum thrust value, denoted by
vmin and vmax , respectively. Taking UAV dynamics into
consideration, the condition reads as:
vmin,l  vi ,l [k ]  vmax,l , l {x, y, z} (6)
Jerk constraints: The purpose of introducing the jerk is to
make the acceleration change smoother and closer. Bounded

978-1-5090-1573-3/17/$31.00 ©2017 IEEE 1572

Authorized licensed use limited to: Imperial College London. Downloaded on February 22,2024 at 12:34:26 UTC from IEEE Xplore. Restrictions apply.
jerk implies continuity in the acceleration, which for UAVs
correspond to continuity in the attitude [1].
jmin,l  ji ,l [k ]  jmax,l , l {x, y, z}
where j[k ]  (a[k ]  a[k  1]) / h .
Obstacle avoidance constraints: The UAVs must avoid
collisions with known obstacles during the transition from the
initial states to the terminal states, so it’s necessary to give
obstacle avoidance constraints.
pi [k ]  pm 2
 Rm
C. Optimization Problem
The multi-UAV cooperative trajectory planning problem
can be considered as the convex optimization problem, which
can be transformed into a convex optimization form with
linear quadratic performance index, equality constraints and
inequality constraints. The standard form is:
min xT Qx  qT x  r
s.t. Ax  b
Gx  h
where the equation in form (9) contains the equality
constraints (3). Since the acceleration equality constraints
have been included in the position and velocity equality
constraints, it is not necessary to propose acceleration equality
constraints separately. Similarly, the inequation in form (9)
contains the inequality constraints (4) - (8).
III. RHO-BASED CONVEX OPTIMIZATION
This section mainly introduces the algorithm flow,
including RHO frame and convex optimization algorithm.
A. RHO Framework
We mainly combine RHO and convex optimization
algorithm to achieve the cooperative trajectory planning and
use the convex optimization algorithm to solve the optimal
solution in each receding horizon. The specific algorithm
framework is shown in Fig. 1.
Figure 1. The framework of RHO-based convex optimization method
Authorized licensed use limited to: Imperial College London. Downloaded on February 22,2024 at 12:34:26 UTC from IEEE Xplore. Restrictions apply.
The idea of RHO is known as model predictive
control(MPC). At each discrete time, the model of the system
is used to predict the future state evolution of the system, and
(7) an optimal problem is established by using the predicted state
of the system. The optimal problem is solved online and the
optimal control sequence is obtained. And at that time, only
the first control signal in the optimal control sequence is
actually applied to the system, and the latter items are
discarded. At the next moment, repeat the above process. We
use the RHO and the convex optimization to solve the UAVs’
trajectory planning. When the flight mission begins, the
(8) trajectories of the UAVs from the initial point to the
destination is progressively calculated over time. Using the
RHO strategy, each segment of the trajectories is obtained by
solving the convex optimization problem, which greatly
reduce the calculation time.
Using the framework of RHO-based convex optimization
algorithm above, select the receding horizon d (d  1, 2,..., K ) ,
and the calculation of the convex optimization is performed in
each receding horizon. The trajectory in the first receding
horizon is selected as the feasible solution. The d  1 point in
(9) this calculation is the initial point of the next convex
optimization. Repeat the process above until the whole
trajectory is planned (Fig. 2).
Figure 2. Flow chart of RHO
B. Convex Optimization Algorithm
This part we mainly focus on optimization variable,
approximation of non-convex constraints and the flows of
convex optimization algorithm, which have been presented in
Ref [10]. And the algorithm is almost totally independent of
the UAVs’ type. The goal is to generate cooperative
trajectories for N UAVs, allowing a transition in a given time
T from an initial set of states to a terminal set of states
(position, velocity, and acceleration). The trajectories must
satisfy equality constraints and inequality constraints.
The optimization variable   R3NK consists of the UAVs’
accelerations at each discretization time k . From (10) and
(11), it follows that the position and velocity of each UAV at
any discrete time k are affine functions of  .
vi [k ]  vi [1]  h(ai [1]  ai [2]  ai [3]  ...  ai [k  1]) (10)
pi [k ]  pi [1]  h(k  1)vi [1]
(11)
h2
 ((2k  3)ai [1]  (2k  5)ai [2]  ...  ai [k  1])
2
1573
 According to (10) and (11), the position, velocity and IV. SIMULATION EXAMPLES
acceleration of each UAV are defined at discretization time k = In the previous section, we discuss the problem of
1 and k = K. So the constraints (3) can be rewritten as the multi-UAV cooperative trajectory planning. In this section, we
constraints (12). verify the effectiveness of this method by simulation. All our
Aeq  R9 N 3 NK , beq  R9 N (12) simulation examples were carried out on a PC running
Windows 10, and equipped with an Intel Core i3 CPU 550 @
In order to minimize the thrust of UAVs, the performance 3.20Ghz and 4GB of RAM.
indicators can be expressed as: The mission of the multi-UAV cooperative trajectory
N K 2 planning simulation is as followed: multiple UAVs fly from
f   a [k ] (13) their initial position to the assembly area. The initial state and
i 1 k 1 i 2
terminal state of UAVs are shown in Table I and Table II. The
Equation (13) can be expressed as the quadratic function of start time of the initial position is 0s ( k  1 ), and the UAVs
the optimization variable  : reach the terminal position at 49s ( h  1s, k  50, T  49s ).
We select the receding horizon is d  5 . During the assembly
f ( )   T  (14) process, UAVs need to avoid the known obstacle in the
Convex optimization algorithm is a method that allows us environment. And the obstacle radius is R=1000m, the
to approximate non-convex constraints by using convex ones. coordinates of obstacle are (3500, 3000, 2000) m.
Since the obstacle avoidance constraints (8) are non-convex
TABLE I. INITIAL POSITION
constraints, we linearize the (8) at the discrete point
corresponding to the reference trajectories. And the obstacle UAV ID Position(m) Velocity(m/s)
avoidance constraints can be converted to the following affine UAV-1 (0,0,0) (10,10,10)
function (15): UAV-2 (0,0,2000) (10,20,30)
UAV-3 (0,0,-1000) (30,30,30)
( pi [k ]  pm )T UAV-4 (1000,2000,1000) (30,40,50)
 ( pi [k ]  pi [k ])  pi [k ]  pm  Rm (15)
pi [k ]  pm
TABLE II. TERMINAL POSITION

The performance index function f (  ) of the convex UAV ID Position(m) Velocity(m/s)

optimization problem is convex quadratic and the constraint UAV-1 (5000,4000,3000) (0,0,0)
UAV-2 (5000,4000,3000) (0,0,0)
condition is an affine function, so the problem can be UAV-3 (5000,4000,3000) (0,0,0)
transformed into Quadratic Programming (QP). For QP, the UAV-4 (5000,4000,3000) (0,0,0)
standard form can be expressed as:
According to the conditions above, we made four sets of
min  T  simulations in Table III and the constraints are shown in Table
s.t Aeq   beq (16) IV.
Ain   bin TABLE III. SIMULATION EXAMPLES

The approximation of the non-convex constraints needs to No. Simulation scenarios

be based on the previous solution 0 , and the previous solution Example 1 Convex optimization without obstacle avoidance
Example 2 Convex optimization with obstacle avoidance constraints
0 is chosen as the solution of (16) without the constraints Example 3 RHO-based convex optimization without obstacle avoidance
(15). This is motivated by the fact that the resulting trajectories Example 4 RHO-based convex optimization with obstacle avoidance
of the problem without obstacle avoidance constraints are
often close to the ones of the actual problem. The specific TABLE IV. CONSTRAINTS
convex optimization steps applied to multi-UAV trajectory Constraints Min values Max values
planning are shown below: Position(m) (-1000,-1000,-2000) (6000,5000,4000)
Velocity(m/s) (-200,-200,-200) (200,200,200)
Step 1: Select the appropriate discretization step h .For a Acceleration(m/s²) (-20,-20,-20) (20,20,20)
given prediction horizon T , we have K  T / h  1 , where h
is chosen such that K  N . A. Example 1
The trajectory planning results of convex optimization
Step 2: Solve the previous solution 0 . According to the
without obstacle avoidance are shown in Fig. 3 - Fig. 6. As can
previous solution 0 , the convex optimization form described be seen from Fig. 5 - Fig. 6, the acceleration and velocity of
in (16) is solved and the corresponding trajectories and  are UAVs satisfy the constraints. Since there are no obstacle
obtained. avoidance conditions, we can observe from Fig. 3 - Fig. 4 that
Step 3: If there is a solution (the cooperative trajectories the trajectories pass through the threat area and reach the
satisfy all constraints), output the current results and exit. if terminal position at the same time T .
there is no solution, re-select the discretization step h and
return to step 2.

1574
Authorized licensed use limited to: Imperial College London. Downloaded on February 22,2024 at 12:34:26 UTC from IEEE Xplore. Restrictions apply.
 B. Example 2
On the basis of Example 1, we add the obstacle avoidance
constraints and the trajectory planning results of convex
optimization with obstacle avoidance constraints are shown in
Fig. 7 - Fig. 10. We can clearly observe the UAVs bypass the
threat source and reach the terminal position at the same time
T from Fig. 7 - Fig. 8. And we can observe the acceleration
and velocity are changed to satisfy the corresponding
constraints from Fig. 9 - Fig. 10.

Figure 3. The three-dimensional trajectories in Example 1

Figure 7. The three-dimensional trajectories in Example 2

Figure 4. The distance from obstacle in Example 1

Figure 8. The distance from obstacle in Example 2

Figure 5. The velocity in Example 1

Figure 9. The velocity in Example 2

Figure 6. The acceleration in Example 1

1575
Authorized licensed use limited to: Imperial College London. Downloaded on February 22,2024 at 12:34:26 UTC from IEEE Xplore. Restrictions apply.
 Figure 10. The acceleration in Example 2 Figure 13. The velocity in Example 3

C. Example 3
The trajectory planning results of RHO-based convex
optimization without obstacle avoidance are shown in Fig. 11
- Fig. 14. It can be seen that the trajectories are updated in each
receding horizon, the different color points of each trajectory
represent the optimal trajectory at different receding horizon.
And we can find that the trajectories in each receding horizon
are smooth. At the same time, the constraints are satisfied.

Figure 14. The acceleration in Example 3

Figure 11. The three-dimensional trajectories in Example 3
D. Example 4
The trajectory planning results of RHO-based convex
optimization with obstacle avoidance are shown in Fig. 15 -
Fig. 18. The trajectories also satisfy the whole constraints after
adding the obstacle avoidance constraints. Because of there
are no obstacle avoidance constraints in Example 3, it can be
seen from Fig. 15 that the UAVs pass through the threat area.
And we find that the performance index of Example 4 is
smaller than that of Example 2 in the simulations.

Figure 12. The distance from obstacle in Example 3

Figure 15. The three-dimensional trajectories in Example 4

1576
Authorized licensed use limited to: Imperial College London. Downloaded on February 22,2024 at 12:34:26 UTC from IEEE Xplore. Restrictions apply.

Figure 16. The distance from obstacle in Example 4
Figure 17. The velocity in Example 4
Figure 18. The acceleration in Example 4
V. CONCLUSION
In this paper, the framework of RHO-based convex
optimization method is given. The three-dimensional
cooperative trajectories for multiple UAVs are generated. And
the feasibility of the method is verified by numerical
simulations. Since we regard the UAV as a particle model, the
collision avoidance problem between the UAVs is not
considered, and this part will be researched in the future and
the online obstacle avoidance will also be studied.
1577
Authorized licensed use limited to: Imperial College London. Downloaded on February 22,2024 at 12:34:26 UTC from IEEE Xplore. Restrictions apply.
ACKNOWLEDGMENT
This research is supported by National Nature and Science
Foundation (NO. 6170020813).
REFERENCES
[1] R. G. Bai, X. Sun, Q. S. Chen, and X. W. Quan, "Multiple UAV
Cooperative Trajectory Planning Based on Gauss Pseudospectral
Method," Yuhang Xuebao/journal of Astronautics, vol. 35, pp.
1022-1029, 2014.
[2] D. Wang, W. Zhang and J. Shan, "Optimal trajectory planning for a
quadrotor via a Gauss Pseudo-spectrum Method," in International
Conference on Natural Computation, 2013, pp. 1666-1670.
[3] S. Zhang, L. I. Xue-Ren, P. Zhang, and L. I. Bo, "UAV path planning
based on improved A* algorithm," Flight Dynamics, 2016.
[4] T. Ren, R. Zhou, J. Xia, and Z. Dong, "Three-dimensional path
planning of UAV based on an improved A* algorithm," in Guidance,
Navigation and Control Conference, 2017.
[5] Q. Liu and J. Li, "Research on UAV flight path planning for dynamic
target based on sparse A* algorithm," Computer Era, 2016.
[6] J. Liu, T. Shi, P. Li, and X. Ren, "Trajectories planning for multiple
UAVs by the cooperative and competitive PSO algorithm," in
Intelligent Vehicles Symposium, 2015, pp. 107-114.
[7] X. Li, Y. Zhao, J. Zhang, and Y. Dong, "A Hybrid PSO Algorithm
Based Flight Path Optimization for Multiple Agricultural UAVs," in
IEEE International Conference on TOOLS with Artificial Intelligence,
2017, pp. 691-697.
[8] Z. Wang, L. Liu, T. Long, and Y. Wen, "Trajectory planning for
multi-UAVs using penalty sequential convex pro-gramming," 2016.
[9] X. Liu and P. Lu, "Solving Nonconvex Optimal Control Problems by
Convex Optimization," Journal of Guidance Control & Dynamics, vol.
37, pp. 750-765, 2014.
[10] F. Augugliaro, A. P. Schoellig and R. D'Andrea, "Generation of
collision-free trajectories for a quadrocopter fleet: A sequential convex
programming approach," in Ieee/rsj International Conference on
Intelligent Robots and Systems, 2012, pp. 1917-1922.