Continuous-Time Trajectory Optimization for Decentralized Multi-Robot Navigation

09/05/2019 ∙ by Shravan Krishnan, et al. ∙ SRM University 0

Multi-robot systems have begun to permeate into a variety of different fields, but collision-free navigation in a decentralized manner is still an arduous task. Typically, the navigation of high speed multi-robot systems demands replanning of trajectories to avoid collisions with one another. This paper presents an online replanning algorithm for trajectory optimization in labeled multi-robot scenarios. With reliable communication of states among robots, each robot predicts a smooth continuous-time trajectory for every other remaining robots. Based on the knowledge of these predicted trajectories, each robot then plans a collision-free trajectory for itself. The collision-free trajectory optimization problem is cast as a non linear program (NLP) by exploiting polynomial based trajectory generation. The algorithm was tested in simulations on Gazebo with aerial robots.



There are no comments yet.


page 2

page 3

This week in AI

Get the week's most popular data science and artificial intelligence research sent straight to your inbox every Saturday.

1 Introduction

Multi-robotics is currently an emerging field with numerous applications and potential in several cross-cutting fields. However, the navigation of a group of robots in complex environments is still a daunting task. In such scenarios, it is essential that every robot is able to autonomously plan collision-free trajectory while also making sure that it replans the generated trajectory appropriately based on the dynamic nature of environment. In this paper, We present a decentralized multi-robot trajectory optimization algorithm which accepts only current states and non-interchangeable end poses of other robots as inputs and generates smooth trajectories. The algorithm exploits the differential flatness property of the robots (Mellinger and Kumar, 2011) (Ryu and Agrawal, 2011); a property that allows planning of trajectories in the space of flat variables and their derivatives.

Trajectory generation for a fleet of robots has received wide spread attention recently. A plethora of different solutions have been proposed to solve this problem. However, a majority of them are centralized methods (Tang et al., 2018)(Solovey et al., 2016). Centralized methods have recently branched into decentralized methods that are capable of planning trajectories appropriately in a decentralized manner (Bekris et al., 2012) (Zhou et al., 2017). These approaches with exception of (Tang et al., 2018)

(which is also centralized) are discrete-time and provide the robots with discrete commands and thereby overlook probable collisions. Moreover, discrete approaches are sampling based

(Solovey et al., 2016)(Bekris et al., 2012) or search based (Liu and Narayanan, 2011) that incrementally look through the space.

An another class of multi-robot algorithms utilize the concept of velocity obstacles to plan velocities appropriately by formulating admissible velocities that robots can be at without colliding (Van den Berg et al., 2008; Alonso-Mora et al., 2018; Van Den Berg et al., 2011; Rufli et al., 2013; Snape et al., 2010). The advantage of such approaches is that they are easily adaptable for decentralized implementations, but restricted to only circular shaped robots. Recently, some extensions have been proposed that allow their usages for heterogeneous robots (Bareiss and van den Berg, 2015). Building on these works, (Alonso-Mora et al., 2018) proposed a collaborative collision avoidance for non-holonomic robots with re-planning, while respecting the vehicular constraints and also accounting for potential tracking error bounds of the robot. They have also proposed an extension of the same to aerial robots in (Alonso-Mora et al., 2015).

Figure 4: A sequence of images showing robots during different transitions. The red curves are the planned trajectories. A video of the simulations is available at

Recently, a centralized method for generating collision-free trajectories for a swarm of quadrotors in known obstacle filled environments was proposed (Hönig et al., 2018). The method utilized a three step process with first step for generating sparse roadmaps, second step for planning discrete schedules and third step for generating bezier curve based time parametrized smooth trajectories. Additionally, the duration of the trajectory is not directly optimized but scaled to satisfy the dynamics of robots.

Methods based upon sequential convex programming have also been proposed for multi-robot systems (Chen et al., 2015)(Augugliaro et al., 2012). However, these methods are centralized and use a discrete approach for planning trajectories. Furthermore, they use only second order dynamics with jerk constrained as rate of change between two subsequent time steps. Nevertheless, the algorithms do not have any dimensional restrictions. Additionally, utilizing such approaches is tractable only if the end times are known or end times are scaled accordingly beforehand.

Barrier functions based methods have been proposed for collision avoidance (Wang et al., 2017). Recently, model predictive control based methods have been used for multi-robot collision avoidance (Morgan et al., 2014)(Kamel et al., 2017). (Cheng et al., 2017) proposed a Model Predictive Control scheme based on (Van Den Berg et al., 2011).

A majority of these methods account for lower order dynamics of the robot and/or do not guarantee a continuous time trajectory that also accounts for collision avoidance in continuous time. Moreover, the algorithms do not consider time as a coupled optimization parameter.

In this work, a higher order dynamics(three) is used for the robots rather than two which is often found in literature. Additionally, trajectories are generated in continuous-time for multi-robot systems with different robots rather than sampling based or discrete time methods.

The major contributions of this paper are:

  1. A closed form solution for generating minimum time-jerk squared smooth trajectories given current state and desired end position.

  2. A decentralized algorithm for generating collision-free continuous-time trajectories for multi-robot systems in dimensions.

  3. Extensive simulations of the proposed algorithm using a variety of different aerial robots in three dimensional spaces

The proposed algorithm involves polynomials in each of the robots. polynomials denote the predicted trajectories of other robots in dimensions and polynomials represent the collision-free trajectory.

Based upon the polynomials, a two-step process is used to generate collision-free trajectories. The first step generates the polynomials representing the continuous-time predicted trajectory of other robots in the environment. The second step formulates a non linear optimization problem(NLP) with objectives of minimizing jerk and time while not exceeding the dynamic limits and avoiding collision(with respect to the trajectories from the previous step). The solution of the NLP provides the coefficients of the collision-free trajectory and the duration of the trajectory.

Furthermore, replanning of trajectories is done online for two reasons; one, the number of robots may not be known beforehand and two, the trajectory prediction does not account for robot-robot interaction and therefore the predicted trajectory diverges from the planned trajectories of other robots considerably during longer durations. To allow for efficient use of the previous trajectory, the algorithm is implemented in a receding horizon manner, i.e, a part of the trajectory is applied after which the trajectory is re-planned.

The rest of the paper is organized as: Section 2 showcases the Trajectory prediction method. Section 3 details the trajectory optimization problem. Section 4 showcases the performance of the algorithm in simulations and Section 5 concludes the paper

2 Trajectory Prediction

Knowing the trajectories of other robots in the environment improves the efficiency of the generated trajectory in avoiding collision. In this section, we derive a closed-form solution for efficiently predicting minimum time and jerk smooth trajectories for other robots in the environment. The dynamic limits of other robots are unknown and hence neglected. The trajectory prediction algorithm was inspired by the closed form solutions for efficient trajectory generation for quadcopters shown by (Mueller et al., 2015) and is discussed below.

Commonly used dynamic models for mobile robots or multirotors are differentially flat. This allows us to formulate the input of those robots as the th derivative of a th order robot. Additionally, The generated trajectory should be represented by at least a order polynomial (Tang et al., 2018).

2.1 State model

The robots are modelled as a third order system with state and input , where is position, is velocity, is acceleration and is jerk. Hence, the dynamics is represented as:

Figure 5: A schematic explaining the overall system. The white curve indicates the planned trajectory by robot 1 from it’s current state to it’s end state . Similarly, the black curve indicates the planned trajectory by robot 2 from it’s current state to it’s end state . The green and red arrows indicate the communication and the data being shared during the communication

2.2 Objective Function

The objective of the trajectory prediction is to find smooth and minimum-time trajectory that moves the robot from it’s current state to the partially defined end state in minimal time. The objective can be represented as ∫_0^T ∥u∥ ^2 +1   dt

The objective is coupled only by the end time in each dimension. This allows looking at each dimension individually. Hence, for the sake of brevity and readability, the trajectory is derived for a single dimension and the dimensions are coupled at end once a closed-form solution for is found.

The system represented in Eq.(1) is a linear system. Utilizing Pontygarin’s maximum principle (Bertsekas, 2005), the system can be solved. We derive the closed form solution for the system with a third order model, which has been used for a variety of classes of robots ranging from multirotors (Mueller et al., 2015) to Autonomous ground vehicles (Altché et al., 2017).The Hamiltonian of the third order system is:


where is the costate equation. The optimal costate equation can be formulated as:


The solution of costate differential equations is then represented as:


The optimal control input can be found by: u^*H(x^*(t),u^*(t),Λ^*(t))

On integrating


As the end state is partially defined (only position), the position can be substituted into Eq. (5) resulting in


The unknown coefficients can be found as the corresponding costates will be zero at the free endstates(Bertsekas, 2005). Thus, from (4), the appropriate equations(second and third) can be used to solve for the three unknown coefficients. This also allows a representation of the unknown coefficients as a function of end time, known initial states and end position. Resulting in:


Furthermore, due to the time being a variable to optimize, of the system is now zero instead of a constant (Bertsekas, 2005). Hence,


substituting Eq. (7) into Eq. (8) and simplifying results yields


Similarly, the other dimensions can also be lumped into the coefficients for finding the time. This eighth order polynomial can be solved for the roots. This is solved using (Horn, 1985, p.   146–147), which is implemented in numpy on python111 The trajectory with the least cost among all the real and positive solutions for the polynomial is utilized after the coefficients are found using Eq. (7

). There are scenarios where no real roots exist,then we assume a constant jerk and use Newton’s second equation of motion to solve for the end time(This assumption is used rather than fixing an end time due to the fact that fixing an end time beforehand reduces a degree of freedom in the solution). After getting the end time, we solve for the polynomial coefficients. Each of the polynomial represents the predicted trajectory in one dimension for one robot. Therefore, for every other robot that transmits it’s current state and desired state, the above method is used for predicting trajectories thereby generating a smooth trajectory representation for dynamic obstacles.

3 Trajectory generation

The trajectory generation problem can be formulated as: x∫_0^T C_d +∑_i=1^M C_c+C_l+ K_t   dt x(0)=x_0 x(T)=x_f

Where , and are costs for trajectory smoothness, collision with other robots and dynamic limits respectively. Please note the addition of at the end of the cost, which is added to minimize the time taken along the trajectory. Besides, the constraint in Eq. (3) can be defined for the complete state or partial state. Furthermore, we represent the trajectories by time parameterized polynomials of order five to allow trajectories to be similar in representation to the predicted trajectory. Therefore, for each dimension, the trajectory can be represented by:


The decision variables of the optimization problem are:


where the first six numbers represent the polynomial coefficients of the first dimension, the second six the second dimension. Lastly the represents the end time.

3.1 Dynamic Smoothness

To ensure that the generated trajectory is smooth, a smoothness constraint is added. This constraint is represented as:


is a weight for the dynamics. This derivative cost can be integrated analytically and solved for in terms of the optimization variables as shown in Eq. (11).

3.2 Collision Avoidance

To avoid collisions with other robots in the environment, an exponential barrier function based collision avoidance method is utilized. This function can be represented as


Where is collision avoidance weighing parameter,


is the collision avoidance cost and is the euclidean distance between the robot and obstacle and is a threshold distance beyond which the robots have to be for collision-free navigation. Thus, we model the robots as dimensional spheres and is the sum of the two radii(minimum distance to ensure collision avoidance). The robots are modeled as spheres as it is easier to compute distances for spheres than ellipses.

Figure 6: The trajectories for six Neos robots during a rest-to-rest experiment.

Furthermore, the velocity difference is added to the cost for two reasons, one, to account for the difference in velocity of the robots and two, analytical integration of the proposed cost for closed-form solutions. The analytically integrated cost is then utilized for collision avoidance. Moreover, in the case of other robots( whose trajectories are predicted as detailed in Section 2), the trajectories are represented by time parameterized polynomials. Therefore, this time parameterized trajectories of each of the robots can be directly incorporated into the objective in , allowing for continuous time evaluation.

3.3 Dynamic limits

As the algorithm attempts to minimize the time taken by the robots, it is important to consider the dynamic limits of the robot. To account for the limits, we utilize a soft constraint inspired by (Usenko et al., 2017) that allows for continuous time limit verification while also not adding constraints. The dynamic limits are represented as:


Where is the tuning weight for the dynamic limits,


Here is the maximum limit of the specific derivative that the robot is allowed and is the euclidean norm of the dimensions of the robot. Furthermore, in our implementation we utilize the euclidean norm of the derivatives to account for dynamic limits rather than each dimension individually. This is done as the dynamic limits of most robots are better represented by magnitude rather than decoupled limits on each dimension. We penalize the limits for the dynamics from the first derivative to derivative of the robot’s position. The usage of soft constraints is guided by it allowing for a continuous time dynamic limit checking and if necessary, the dynamic limits can be violated by a small margin by the algorithm.

Figure 7: The executed trajectory for Eight Fireflys from a hovering start to non zero end velocity. The robots as they start in a cluttered environment showcase some drastic changes and evasions, but as time progresses, they come to stand still
Figure 8: The velocity profile for the eight robot multiple transitions maneuver. The evasive velocity profiles as the robots navigate between the other robots initially. As the robots move further away from each other, the robots velocity profiles stabilize into a smooth curve. The dynamic limits are shown in dotted black lines

The NLP in Eq.(12) cannot be proven to be convex due to the equality constraints in Eq. (3) and Eq. (3). Hence, any solution that is generated is going to be locally optimal in general. We utilize Sequential Quadratic Program(SQP) to solve the NLP. The algorithm was implemented in (Johnson, 2014) using the method proposed in (Kraft, 1988). The initialization of the SQP is done using the previously optimized trajectory. For solving the problem during the first iteration, the algorithm was initialized using the method for trajectory prediction in Section 2

Figure 9: The trajectories for Eight aerial robots(Three Neos and Five Fireflys) during multiple tranisitions

4 Simulation Results

The algorithm was implemented in Python using Robot Operating System(ROS) with . The algorithm was tested on a workstation with Intel Xeon E5 1630v5 processor, 64GB of RAM and a Nvidia Quadro M4000 GPU. The algorithm was verified with different rotary winged flying robots of different sizes (Asctec Hummingbird222,Firefly333,Neo444 in Gazebo using RotorS (Furrer et al., 2016), a high fidelity multirotor simulator.

Figure 10: Five Fireflys and Five Neos’s trajectories during a rest-to-rest maneuver

Although aerial robots are fourth order systems, in our experiments we utilize third order system that minimizes the jerk of the robot similar to (Mueller et al., 2015). To track the generated trajectory, we use (Lee et al., 2010). Throughout the experiments, the yaw of the system is kept free as the rotation about yaw does not affect the translational motion. The trajectories are re-planned at a frequency of 8Hz.

4.1 Simulations

In Figure 6,7,9 and 10, the colored circles represent the starting point of the robots while the colored triangles represent the end points of the robots.

In a first simulation, six Neos were spawned randomly and tested for rest-to-free-end-state trajectory. The plotted trajectories are shown in Figure 6.

In a second simulation, eight Fireflys starting from a tightly scrunched space, with many robots stacked on top of each other were given a non-rest end state. This experiment also had the robots start off at distances closer than the allowed threshold. The trajectories of the robots during this experiment are shown in Figure 7.

In a third simulation, the algorithm’s performance with heterogeneous robots and multiple maneuvers was tested (The robots were first given a rest-to-rest state, and after that another non-rest desired pose was published). The experiment used five Fireflys and three Neos and consisted of four different transitions with the latter three transitions having non-rest end states (left free). The robots were able to maneuver safely in the experiment while ensuring the dynamic limits were not exceeded. The resulting trajectory is shown in Figure 9.

It can also be seen from the velocity profile that some of the robots do come to rest during the third transition.

In a final simulation, five Fireflys and five Neos were tested for their capabilities with rest-to-rest transitional maneuver. The resulting trajectories for different robots are shown in Figure 10.

The algorithm is capable of generating collision-free smooth trajectories for the robots to traverse across the environment. Furthermore, the utilization of soft constraints allows the robots to be able to violate the constraints if required. This is while being a boon for dynamic limits but is not so for collision avoidance.

5 Conclusion

A decentralized algorithm for replanning trajectories for multi-robots systems with third order dynamics was proposed in this paper. Each robot uses the algorithm to predict continuous-time trajectories for other robots in the environment and utilizes those trajectories for planning collision-free trajectory for itself. The algorithm was simulated extensively on Gazebo for up to ten robots with speeds of up to 2 m/s in three dimensional spaces. Furthermore, the algorithm utilizes continuous time trajectory parameterization and implements soft constraints for dynamic limits.

Future work is to implement the algorithm on real robotic platforms. For collision avoidance, the performance is dependent on the axis in which the robots are proximal to each other. This is an anomaly that we look to solve in the future. Another avenue for research is to develop a methodology that allows for adapting the weights online as hand tuning the cost function is tedious and time consuming. Besides, in most of the experiments, the algorithm predicts trajectories assuming non-zero end states, developing a method to decide whether to use free-end state or rest-end state is another point to look through in the future. Moreover, accounting for deviations in the predicted trajectory and actual trajectory can be incorporated into the collision avoidance to allow for better predictions.


  • Alonso-Mora et al. (2018) Alonso-Mora, J., Beardsley, P., and Siegwart, R. (2018). Cooperative collision avoidance for nonholonomic robots. IEEE Transactions on Robotics, 34(2), 404–420.
  • Alonso-Mora et al. (2015) Alonso-Mora, J., Naegeli, T., Siegwart, R., and Beardsley, P. (2015). Collision avoidance for aerial vehicles in multi-agent scenarios. Autonomous Robots, 39(1), 101–121.
  • Altché et al. (2017) Altché, F., Polack, P., and de La Fortelle, A. (2017). High-speed trajectory planning for autonomous vehicles using a simple dynamic model. In Intelligent Transportation Systems (ITSC), 2017 IEEE 20th International Conference on, 1–7. IEEE.
  • Augugliaro et al. (2012) Augugliaro, F., Schoellig, A.P., and D’Andrea, R. (2012). Generation of collision-free trajectories for a quadrocopter fleet: A sequential convex programming approach. In Intelligent Robots and Systems (IROS), 2012 IEEE/RSJ International Conference on, 1917–1922. IEEE.
  • Bareiss and van den Berg (2015) Bareiss, D. and van den Berg, J. (2015). Generalized reciprocal collision avoidance. The International Journal of Robotics Research, 34(12), 1501–1514.
  • Bekris et al. (2012) Bekris, K.E., Grady, D.K., Moll, M., and Kavraki, L.E. (2012). Safe distributed motion coordination for second-order systems with different planning cycles. The International Journal of Robotics Research, 31(2), 129–150.
  • Bertsekas (2005) Bertsekas, D.P. (2005). Dynamic programming and optimal control, volume 1. Athena scientific Belmont, MA.
  • Chen et al. (2015) Chen, Y., Cutler, M., and How, J.P. (2015). Decoupled multiagent path planning via incremental sequential convex programming. In Robotics and Automation (ICRA), 2015 IEEE International Conference on, 5954–5961. IEEE.
  • Cheng et al. (2017) Cheng, H., Zhu, Q., Liu, Z., Xu, T., and Lin, L. (2017). Decentralized navigation of multiple agents based on orca and model predictive control. In Intelligent Robots and Systems (IROS), 2017 IEEE/RSJ International Conference on, 3446–3451. IEEE.
  • Furrer et al. (2016) Furrer, F., Burri, M., Achtelik, M., and Siegwart, R. (2016). Rotors—a modular gazebo mav simulator framework. In Robot Operating System (ROS), 595–625. Springer.
  • Hönig et al. (2018) Hönig, W., Preiss, J.A., Kumar, T.S., Sukhatme, G.S., and Ayanian, N. (2018). Trajectory planning for quadrotor swarms. IEEE Transactions on Robotics, 34(4), 856–869.
  • Horn (1985) Horn, R.A. (1985). Cr johnson matrix analysis.
  • Johnson (2014) Johnson, S.G. (2014). The nlopt nonlinear-optimization package.
  • Kamel et al. (2017) Kamel, M., Alonso-Mora, J., Siegwart, R., and Nieto, J. (2017). Robust collision avoidance for multiple micro aerial vehicles using nonlinear model predictive control. In Intelligent Robots and Systems (IROS), 2017 IEEE/RSJ International Conference on, 236–243. IEEE.
  • Kraft (1988) Kraft, D. (1988). A software package for sequential quadratic programming. Forschungsbericht- Deutsche Forschungs- und Versuchsanstalt fur Luft- und Raumfahrt.
  • Lee et al. (2010) Lee, T., Leoky, M., and McClamroch, N.H. (2010). Geometric tracking control of a quadrotor uav on se (3). In Decision and Control (CDC), 2010 49th IEEE Conference on, 5420–5425. IEEE.
  • Liu and Narayanan (2011) Liu, F. and Narayanan, A. (2011). Real time replanning based on a* for collision avoidance in multi-robot systems. In Ubiquitous Robots and Ambient Intelligence (URAI), 2011 8th International Conference on, 473–479. IEEE.
  • Mellinger and Kumar (2011) Mellinger, D. and Kumar, V. (2011). Minimum snap trajectory generation and control for quadrotors. In Robotics and Automation (ICRA), 2011 IEEE International Conference on, 2520–2525. IEEE.
  • Morgan et al. (2014) Morgan, D., Chung, S.J., and Hadaegh, F.Y. (2014). Model predictive control of swarms of spacecraft using sequential convex programming. Journal of Guidance, Control, and Dynamics, 37(6), 1725–1740.
  • Mueller et al. (2015) Mueller, M.W., Hehn, M., and D’Andrea, R. (2015). A computationally efficient motion primitive for quadrocopter trajectory generation. IEEE Transactions on Robotics, 31(6), 1294–1310.
  • Rufli et al. (2013) Rufli, M., Alonso-Mora, J., and Siegwart, R. (2013). Reciprocal collision avoidance with motion continuity constraints. IEEE Transactions on Robotics, 29(4), 899–912.
  • Ryu and Agrawal (2011) Ryu, J.C. and Agrawal, S.K. (2011). Differential flatness-based robust control of mobile robots in the presence of slip. The International Journal of Robotics Research, 30(4), 463–475.
  • Snape et al. (2010) Snape, J., Van Den Berg, J., Guy, S.J., and Manocha, D. (2010). Smooth and collision-free navigation for multiple robots under differential-drive constraints. In Intelligent Robots and Systems (IROS), 2010 IEEE/RSJ International Conference on, 4584–4589. IEEE.
  • Solovey et al. (2016) Solovey, K., Salzman, O., and Halperin, D. (2016). Finding a needle in an exponential haystack: Discrete rrt for exploration of implicit roadmaps in multi-robot motion planning. The International Journal of Robotics Research, 35(5), 501–513.
  • Tang et al. (2018) Tang, S., Thomas, J., and Kumar, V. (2018). Hold or take optimal plan (hoop): A quadratic programming approach to multi-robot trajectory generation. The International Journal of Robotics Research, 0278364917741532.
  • Usenko et al. (2017) Usenko, V., von Stumberg, L., Pangercic, A., and Cremers, D. (2017). Real-time trajectory replanning for mavs using uniform b-splines and a 3d circular buffer. In Intelligent Robots and Systems (IROS), 2017 IEEE/RSJ International Conference on, 215–222. IEEE.
  • Van Den Berg et al. (2011) Van Den Berg, J., Guy, S.J., Lin, M., and Manocha, D. (2011). Reciprocal n-body collision avoidance. In Robotics research, 3–19. Springer.
  • Van den Berg et al. (2008) Van den Berg, J., Lin, M., and Manocha, D. (2008). Reciprocal velocity obstacles for real-time multi-agent navigation. In Robotics and Automation, 2008. ICRA 2008. IEEE International Conference on, 1928–1935. IEEE.
  • Wang et al. (2017) Wang, L., Ames, A.D., and Egerstedt, M. (2017). Safe certificate-based maneuvers for teams of quadrotors using differential flatness. In Robotics and Automation (ICRA), 2015 IEEE International Conference on, 3293–3298. IEEE.
  • Zhou et al. (2017) Zhou, D., Wang, Z., Bandyopadhyay, S., and Schwager, M. (2017). Fast, on-line collision avoidance for dynamic vehicles using buffered voronoi cells. IEEE Robotics and Automation Letters, 2(2), 1047–1054.


Partially defined end state

In case of end position only being available, substituting the velocity and acceleration terms from costate equation Eq.(4) and equating them to zero allows:


The solution to the linear system results in: