Continuous-time Gaussian Process Trajectory Generation for Multi-robot Formation via Probabilistic Inference
In this paper, we extend a famous motion planning approach GPMP2 to work with multiple robots in formation, yielding a novel centralized trajectory generation method for the multi-robot formation. A sparse Gaussian Process model is employed to represent the continuous-time trajectories of all robots as a limited number of states, which brings high computational efficiency due to the sparsity. We add constraints to guarantee collision avoidance between individuals and formation maintenance, then all constraints and kinematics are formulated on a factor graph. By introducing a global planner, our proposed method can generate trajectories efficiently for adaptive formation change of multiple quadrotors which have to get through a width-varying area. Finally, we provide the implementation of an incremental replanning algorithm to demonstrate the online operation potential of our proposed framework. The experiments in simulation and real world illustrate the feasibility, efficiency and scalability of our approach.
Multi-robot teams have been popularized in a wide range of tasks, including surveillance, inspection and rescue. The multi-robot team is required to move in a proper formation in some scenarios, for instance, to survey an area collaboratively . Trajectory generation is an indispensable component in multi-robot systems . It is challenging for planning algorithms to efficiently compute a goal-oriented, collision-free trajectory while respecting kinematics and formation constraints because of a high number of robots sharing the same space.
Current multi-robot motion planning methods can be classified into two categories, namely decentralized methods and centralized methods . In decentralized methods , local interactions between neighbors are employed to achieve group behaviors, so decentralized methods have attracted much attention due to the reduced communication requiring and scalability. However, it is hard for them to impose constraints at either the individual or system level. By comparison, centralized approaches [5, 6] provide global guarantees and are reasonable about constraints, but they often scale poorly with the growing number of robots. In this paper, we present a centralized method with good scalability. Its computational cost increases cubically with the size of state, rather than exponentially as described in other literatures. We demonstrate that our method can compute the whole trajectories for 10 robots within 0.39s in a complex task where a multi-UAV team is required to get through a width-varying area in formation.
There is also a large body of work existing to address formation control and trajectory generation problems, including methods using reactive behaviors , potential fields , virtual structures , leader follower  and model predictive control . Some other researchers have formulated the multi-robot formation navigation problem as a constrained optimization. In [1, 5], a sequential convex programming is used to navigate a multi-robot formation to the goal while reconfiguring the formation to avoid obstacles. However, most existing work is limited to hold a fixed formation, or transition between several predefined formations. In contrast, given a map, our proposed method can adaptively compute proper rectangular formations and then allocate execution time intervals for every formation so that the multi-robot team can get through a width-varying area without human designers.
In this work we extend GPMP2 , a well-known motion planning algorithm using Gaussian Processes (GPs) and factor graphs, to the multi-robot formation case. We represent continuous-time trajectories of all robots in the formation as samples from a GP and then formulate the trajectory optimization problem as probabilistic inference expressed on a factor graph, which can be solved fast by exploiting the sparsity. Constraints on kinematics, obstacle avoidance, collision avoidance between individuals and formation maintenance are all formulated as factors deployed on the factor graph to ensure that the feasible trajectory for every robot in the team can be found by performing a non-linear least-square optimization.
The multi-robot team needs the ability to change the formation adaptively when moving through a width-varying area, as shown in Fig. 1. To this end, we introduce a global planner including two parts: formation planning and task assignment. Given a group of robots with known initial locations and the target point of the formation, as well as a map describing the environment, expected formations and their corresponding execution time intervals can be computed efficiently. Then a simple but effective task assignment method specific to rectangular formations is employed to allocate each robot to its unique position in the formation. In this way, conflicts during transitioning can be avoided, which guarantees the trajectory optimization to converge to the optimal solution. All these results are then used to define the formation constraints respected in the trajectory optimization.
In most cases, centralized approaches run offline , which means a known global map is required in advance. However, in many tasks, only a limited sensing range around robots is available, or the destination of the formation is varying. To this end, we implement an incremental replanning algorithm for multi-robot formation following iGPMP2  to illustrate the online operation potential of our framework.
Ii Global Planning
Ii-a Formation Planning
Inspired by [13, 14], we use Rectangular Safe Flight Corridor (RSFC) constraints to generate expected formations. Then we allocate the corresponding execution time intervals for each formation according to lengths of all path segments. To this end, we present three procedures: RSFC construction, formation generation, and path updating and time allocation. Note that we focus on rectangular formations because they can cover most of the practical tasks.
As shown in Fig. 2, obstacles are represented by gray shaded areas. We adopt the convention used in . A piece-wise linear path is donated as , where indicate points in the free space and are directed line segments, donated as . In our work, is initialized by the positions of width changes. The RSFC generated from is denoted as . In this step we find a collision-free RSFC which includes the segment . Robots are modeled as points with radius of and we expand the obstacle region with the thickness of to respect the volume of robots (darkgray shaped areas in Fig. 2). Then is computed in two steps: build an initial RSFC to derive the maximal width of the formation according to , and the number of robots in the team; translate the initial RSFC in the direction perpendicular to or shrink it iteratively until it just fit into the obstacle-free region, so that we find .
With and the expected distance between robots, we can calculate the number of robots that can be accommodated in each column of the formation. Note that we need to ensure the formation to be rectangular.
Path updating and time allocation
To guarantee obstacle avoidance during the formation transitioning, we update according to the length of the desired formation (to ensure that the formation transitioning is carried out in the wider area, see Fig. 2 (d)). Then we allocate the corresponding execution time interval for each formation according to percentage of updated in . In addition, we allocate a small time gap for the formation transitioning to achieve the smoothness of trajectories.
Ii-B Task Assignment
We introduce a task assignment algorithm for rectangular formations to allocate the unique position in the formation for each robot. By doing this, conflicts between individuals during the formation transitioning can be significantly reduced, so it is more likely and faster for the optimizer to find to the feasible trajectories. On the other hand, given the goal of the formation, the goal of each robot can be calculated automatically using the result of the task assignment.
Our strategy is to guarantee that the relative position changes of all robots during the transitioning are roughly in the same direction. In our convention, the ârow-majorâ formation indicates that the rectangular formation has more elements in each row than column, and the definition of âcolumn-majorâ is opposite. A specific example of eight robots is shown in Fig. 3. It can be seen that all robotsâ relative position changes are roughly in the same direction in both two cases (lower left for the case of ârow-majorâ to âcolumn-majorâ and upper right for the opposite). By doing this, all robots can reach their expected positions in the new formation in a conflict-free style. In the implementation, a matrix is used to store the position assignment scheme of each formation. Taking transitioning from âcolumn-majorâ to ârow-majorâ as an example, we iteratively cut the elements in the previous formation with diagonal lines (marked in green dashed lines) from top to bottom, and store them in a queue in ascending order by column indexes, then fill them into the matrix representing the new formation by row. Note that if the number of elements in the queue is more than that of vacancies in the current row of the new formation (e.g. Cut 3 in Fig. 3), we first fill the remaining vacancies with the elements at the end of the queue, then use the elements left at the front of the queue to fill in a new row. The opposite is true in the inverse process. It can be proven that our method can adapt to any rectangular formation.
Iii Trajectory Optimization
Our work builds on a well-developed motion planning algorithm GPMP2 . For the sake of completeness, we briefly review GPMP2 first. Then we introduce the new constraints we add in order to extend it to the multi-robot formation case. Finally, we also provide an incremental replanning method for multi-robot formation by following iGPMP2  to show the online operation potential of our proposed centralized method.
Iii-a Review of GPMP2
Planning as inference on factor graphs
GPMP2 treats the motion planning problem as probabilistic inference. The goal is to find the maximum a posterior (MAP) trajectory given a prior distribution on the space of trajectories encouraging smoothness and a likelihood function that encourages the trajectory to be collision-free , as shown in (1)
where is a set of random binary events of interest, for example, obstacle avoidance. The posterior distribution of given can be derived from the prior and likelihood by Bayes rule.
which can be represented as the product of a series of factors
where are factors on state subsets . It is shown in  that this MAP problem can be expressed on a factor graph and solved in high efficiency by exploiting sparsity.
The GP prior
A vector-valued Gaussian Process (GP) is employed to represent a continuous-time trajectory: , where is the mean and is the covariance, which is generated by a linear time-varying stochastic differential equation (LTV-SDE) defined as
where and are system matrices, is the control input and the white noise is with being the power-spectral density matrix and being the Dirac delta function. The first order moment (mean) and second order moment (covariance) can be derived from the solution to (4), given by
where is the state transition matrix and are respectively mean and covariance at . The Markov property of (4) results in the sparsity of the inverse kernel matrix which allows for fast inference. The proof of the sparsity can be found in . Then the GP prior can be written as
The likelihood function
GPMP2 formulates constraints as events that the trajectory has to obey. For example, the likelihood function of obstacle avoidance indicates the probability of being free from collisions with obstacles. All likelihood functions are defined as a distribution in the exponential family, given by
where is a vector-valued cost function and is the corresponding events. For the proof of sparsity of the likelihood in GPMP2, please see .
Iii-B Likelihood specific to the multi-robot formation case
On the basis of GPMP2, we add a formation constraint on the factor graph to enforce the robot team to maintain the expected formation.
Our definition of the formation is depicted in Fig. 4. The robot in the upper left corner is the origin point of the formation. The red points indicate the expected position of each robot, which is computed by the expected distance and the relative position to the origin point. The tolerant range of each robot is marked as yellow circles with the radius in Fig. 4. We achieve formation control by limiting the relative position of each robot to the origin. In contrast to giving an expected global position to every robot and controlling each alone to its goal, our strategy enables the robot team to act more flexibly, and their behaviors would be closer to a real intelligent swarm, instead of a set of individuals executing their own orders respectively.
Collision avoidance between individuals
Another constraint required in the multi-robot case is collision avoidance between each other during the movement, especially during the process of formation transitioning.
We prevent collisions between each other by checking the distances between every two robots in the multi-robot team. If any two robots get too close to be safe, it will cause a rapid increase in the value of the corresponding cost function. In this way, robots will show mutual repulsion and keep a safe distance with each other.
Iii-C Factor graph formulation
Having defined all factors, we now describe our graphical model for representing the problem of trajectory optimization for a multi-robot formation. An example is illustrated in Fig. 5. Compared with GPMP2, we add two types of unary factors: formation factors and collision factors (Note that collision factors here refer specifically to collisions between robots, those of static obstacle avoidance are indicated by obstacle factors), and their corresponding interpolated versions to the factor graph. Therefore, the useful sparsity exploited by GPMP2 is still available for our case. A simple explanation for interpolated factors is that we can compute the state of any time of interest between two support states and impose constraints on it (For more details about the GP interpolation, see [7, 17, 18]). Furthermore, this constraint can be equivalently allocated to two support vectors due to the property of GP (Fig. 5). It is worth noting that we apply two different formation factors on the trajectories in Fig. 5, meaning that we are allowed to use different formation configurations at any time of interest. In our work, we define the formation factors according to the result of the global planning, which includes a sequence of expected formations and their corresponding time intervals of execution.
Iii-D Incremental inference for replanning
We also provide the implementation of an incremental replanning algorithm on the basis of iGPMP2, which is necessary when the target point of the formation have been moved or new obstacles are found by robots due to the limited sensing scope.
Given a set of optimized trajectories and the changed condition, the replanning task is to efficiently recompute new feasible trajectories for the robot team to achieve its goal on the premise of safety. Following iGPMP2, we adopt an incremental style to update the current solution by using the Bayes Tree [19, 20] data structure, instead of resolving a new entire MAP inference from scratch. By doing this, we can update trajectories fast to achieve the online operation, because the main body of the original problem is unchanged. For a full treatment about how the incremental method works, see .
In the implementation, we use our proposed framework to solve the original trajectory generation problem. Then we update the factor graph according to the changed conditions and an incremental solver called iSAM2  is adopted to update the trajectories for all robots in the formation.
Iv Implementation Details
We implement our global planner in C++ from scratch. The C++ implementation of our trajectory optimization part in our proposed framework builds on open-source libraries: GPMP2  and GTSAM . Following GPMP2, we employ Levenberg-Marquardt algorithm to solve the non-linear least square problem and the default parameters are adopted. Similarly, our replanning implementation adopts iSAM2 incremental solver with default settings. It is worth noting that our trajectory optimizers are initialized by a constant-velocity straight line trajectory from the start to the goal for every robot, without any prior about the environment. Since it is straightforward for our method to be extended to 3D cases, here we discuss the 2D case for brevity.
Iv-a GP prior
We augment the state variable to include all robotsâ positions and velocities
where is the position of the robot in the group and is the number of robots.
Iv-B Obstacle avoidance likelihood
Similar to GPMP2, we compute a signed distance field (SDF)  with the map which indicates obstacles in the environment. Then we use the SDF to check the distance to the closest obstacle for every robot and impose a hinge loss
where is the distance to the closest obstacle for each robot . Then we have the cost function
Iv-C Collision avoidance likelihood
We check the distances between every two robots at each iteration and impose a hinge loss on them, given by
where indicates the distance between robot and . The cost function can be written as
Iv-D Formation constraints
We again employ a hinge loss to encourage robots to hold the expected formation when required.
where is the distance between robot and its expected position. Then we have the cost function
So far, we have defined all the cost functions depicted in Fig. 5. The weight of each term is defined by . Generally, a smaller correlates to a higher weight.
We test our proposed framework with a team of quadrotors on three common scenarios: formation maintenance, replanning for a changed destination and adaptive formation change for moving through a width-varying area. Experiments in the real world is conducted with a group of Crazyflie nano-quadrotors flying in a NOKOV motion capture system.
The global planning is applied to the cases involved formation change. The expected distance between quadrotors in the formation is . The thickness of the expansion in the obstacle area is and the time gap is .
As for the trajectory optimization, all GP trajectories are represented by 11 support states, and all tasks are required to be finished in the same time of . Quadrotors are modeled as point robots with radius of . Our parameter settings are as follows: , , . The parameters indicating the weight of each term of likelihood are shown in TABLE I. It can be seen that the priority of formation constraints is much higher in the cases involved formation change. It implies that the formation configurations provided by the global planning are strong priors encouraging the trajectory optimization to achieve the optimal solution.
Since our work focus on the trajectory generation problem, the output of our method is a set of trajectories represented by a sequence of discrete-time states. Therefore, we employ a post-process to convert trajectories to a set of polynomial curves with degree of 7, which can be deployed on Crazyflies. We use the CVXOPT package in Python to solve the quadratic programming. During experiments, converted trajectories are uploaded to Crazyflies in real time and executed by using Crazyswarm infrastructure support . Crazyflies receive messages from the motion capture system through CrazyRadio PA for localization.
V-a Formation maintenance of multiple quadrotors
In the first scenario, four quadrotors are expected to fly to the goal in a fixed square formation, while avoiding obstacles, see Fig. 6. It is worth noting that the quadrotor team makes trade-offs when necessary. In our case, the weight of formation constraints is smaller compared with the other two terms, so the formation has slight distortion during the turn but a smoother turn is achieved.
V-B Replanning for a changed destination
We then extend the first scenario by modifying the target point suddenly while quadrotors are flying to their original goals. We move the target point in the opposite direction to the original target at , as illustrated in Fig. 6. Our incremental replanning algorithm updates the trajectory to the new goal within 4ms, which meets the real-time requirement.
V-C Adaptive formation change for a width-varying area
In the last scenario, as depicted in Fig. 7, six quadrotors are set to move through a corridor with three different widths: 2.5m, 1.5m, 3.5m, the corresponding formations and execution time intervals calculated by the global planner are ; ; . In addition, we implement a demo of 10 quadrotors on a four fold map, see Fig. 8. The three widths are respectively: 4m, 2m, 7m, and the corresponding formation configurations are: ; ; .
V-D Runtime Evaluation
Finally, we present the result of runtime evaluation for all cases mentioned above, as shown in TABLE II. The configuration of the PC platform is Intel Core i7-10700F CPU @2.90GHz. Our method computes the full trajectories for 10 quadrotors with formation change within 0.39s, which is efficient for such a complex task. It can be seen that the trajectory optimization takes the most expensive computational cost, while growing cubically with the size of the state . There is a rapid increase in runtime from 6 quadrotors to 10 quadrotors, because their scenario configurations are totally different (a four-fold map and more complex transitioning), not just a growth in the dimension of states. Compared with GPMP2, we just add two new kinds of unary factors, and the useful sparsity is still available. Therefore, the description of scalability in  is still valid for our method. As for the global planning, formation planning and time allocation are independent of the number of robots, while task assignment is also insensitive to the scale of the quadrotor team.
In this paper, we propose a novel trajectory generation framework for the multi-robot formation. In the global planning part, we present a formation planning method and a task assignment approach specific to rectangular formations to compute the expected formation sequence and the corresponding assignment scheme, which provides strong priors to encourage the following trajectory optimization to converge to the optimal solution. Our trajectory optimization part is built on GPMP2 by adding new constraints specific to multi-robot formation cases, which has high efficiency and good scalability by exploiting the sparsity brought by GPs. Both simulations and real-world experiments where a team of quadrotors moving through a width-varying area show the efficiency and feasibility of our method. Additionally, a fast incremental replanning approach is implemented to illustrate the possibility of online operation.
- J. Alonso-Mora, S. Baker, and D. Rus, “Multi-robot formation control and object transport in dynamic environments via constrained optimization,” The International Journal of Robotics Research, vol. 36, no. 9, pp. 1000–1021, 2017.
- W. HÃ¶nig, J. A. Preiss, T. K. S. Kumar, G. S. Sukhatme, and N. Ayanian, “Trajectory planning for quadrotor swarms,” IEEE Transactions on Robotics, vol. 34, no. 4, pp. 856–869, 2018.
- C. E. Luis, M. Vukosavljev, and A. P. Schoellig, “Online trajectory generation with distributed model predictive control for multi-robot motion planning,” IEEE Robotics and Automation Letters, vol. 5, no. 2, pp. 604–611, 2020.
- B. E. Jackson, T. A. Howell, K. Shah, M. Schwager, and Z. Manchester, “Scalable cooperative transport of cable-suspended loads with uavs using distributed trajectory optimization,” IEEE Robotics and Automation Letters, vol. 5, no. 2, pp. 3368–3374, 2020.
- D. Mellinger, A. Kushleyev, and V. Kumar, “Mixed-integer quadratic program trajectory generation for heterogeneous quadrotor teams,” in IEEE International Conference on Robotics and Automation, 2012.
- J. Alonso-Mora, S. Baker, and D. Rus, “Multi-robot navigation in formation via sequential convex programming,” in 2015 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). IEEE, 2015, pp. 4634–4641.
- M. Y. Mukadam, J. Dong, X. Yan, F. Dellaert, and B. Boots, “Continuous-time gaussian process motion planning via probabilistic inference,” The International Journal of Robotics Research, 2018.
- T. Balch and R. C. Arkin, “Behavior-based formation control for multirobot teams,” IEEE transactions on robotics and automation, vol. 14, no. 6, pp. 926–939, 1998.
- T. Balch and M. Hybinette, “Social potentials for scalable multi-robot formations,” in Proceedings 2000 ICRA. Millennium Conference. IEEE International Conference on Robotics and Automation. Symposia Proceedings, vol. 1. IEEE, 2000, pp. 73–80.
- D. Zhou, Z. Wang, and M. Schwager, “Agile coordination and assistive collision avoidance for quadrotor swarms using virtual structures,” IEEE Transactions on Robotics, vol. 34, no. 4, pp. 916–923, 2018.
- W. Ren and N. Sorensen, “Distributed coordination architecture for multi-robot formation control,” Robotics and Autonomous Systems, vol. 56, no. 4, pp. 324–333, 2008.
- W. B. Dunbar and R. M. Murray, “Model predictive control of coordinated multi-vehicle formations,” in Proceedings of the 41st IEEE Conference on Decision and Control, 2002., vol. 4. IEEE, 2002, pp. 4631–4636.
- S. Liu, “MOTION PLANNING FOR MICRO AERIAL VEHICLES,” Ph.D. dissertation, University of Pennsylvania, 2018.
- J. Park, J. Kim, I. Jang, and H. J. Kim, “Efficient multi-agent trajectory planning with feasibility guarantee using relative bernstein polynomial,” in 2020 IEEE International Conference on Robotics and Automation (ICRA). IEEE, 2020, pp. 434–440.
- J. Dong, M. Mukadam, F. Dellaert, and B. Boots, “Motion planning as probabilistic inference using gaussian processes and factor graphs.” in Robotics: Science and Systems, vol. 12, 2016, p. 4.
- T. D. Barfoot, C. H. Tong, and S. SÃ¤rkkÃ¤, “Batch continuous-time trajectory estimation as exactly sparse gaussian process regression,” in Robotics: Science and Systems, vol. 10. Citeseer, 2014.
- S. Sarkka, A. Solin, and J. Hartikainen, “Spatiotemporal learning via infinite-dimensional bayesian filtering and smoothing: A look at gaussian process regression through kalman filtering,” IEEE Signal Processing Magazine, vol. 30, no. 4, pp. 51–61, 2013.
- X. Yan, V. Indelman, and B. Boots, “Incremental sparse gp regression for continuous-time trajectory estimation and mapping,” Robotics and Autonomous Systems, vol. 87, pp. 120–132, 2017.
- M. Kaess, V. Ila, R. Roberts, and F. Dellaert, The Bayes tree: An algorithmic foundation for probabilistic robot mapping. Springer, 2010, pp. 157–173.
- M. Kaess, H. Johannsson, R. Roberts, V. Ila, J. J. Leonard, and F. Dellaert, “isam2: Incremental smoothing and mapping using the bayes tree,” The International Journal of Robotics Research, vol. 31, no. 2, pp. 216–235, 2012.
- F. Dellaert, “Factor graphs and gtsam: A hands-on introduction,” Georgia Institute of Technology, Tech. Rep., 2012.
- M. Zucker, N. Ratliff, A. D. Dragan, M. Pivtoraiko, M. Klingensmith, C. M. Dellin, J. A. Bagnell, and S. S. Srinivasa, “Chomp: Covariant hamiltonian optimization for motion planning,” The International Journal of Robotics Research, vol. 32, no. 9-10, pp. 1164–1193, 2013.
- J. A. Preiss, W. Honig, G. S. Sukhatme, and N. Ayanian, “Crazyswarm: A large nano-quadcopter swarm,” in 2017 IEEE International Conference on Robotics and Automation (ICRA). IEEE, 2017, pp. 3299–3304.