Trajectory optimization for robotics is a very common problem which has seen several advancements over time. From providing simple hand designed trajectories, to graph based search methods and random sampling methods, and finally the use of high level analytical and numerical optimization methods for optimizing state space trajectories. With the availability of computational resources, Model Predictive Control (MPC) has been recently used for online trajectory optimization. With the use of MPC, the extensive knowledge of the dynamics of various robots can now be used to perform model based optimization. This expands to a variety of systems with linear, non-linear and hybrid dynamics where MPC can be used to optimize trajectories in real time with robust constraint satisfaction.
Recently, MPC has also been used for multi-agent systems with the introduction of distributed MPC approach. One of the key challenges associated with trajectory optimization for multi-agent systems is efficient cooperation among the agents to maximize a global objective while satisfying local constraints like collision avoidance with other agents. This problem is hard to solve due to the non-convex objectives and constraints associated with it and due to scalability issues with higher number of agents. We consider the problem of a swarm of agents modelled as point masses with the objective of point to point transitions for each of the agent. This problem has a local objective for each of the agents while it also needs to satisfy global constraints like collision avoidance dependant on the dynamics of other agents. Several formulations and optimization methods have been discussed for similar problems. We model the problem with linearly separable objective and global coupled collision avoidance constraints and use distributed MPC for solving the global problem in a distributed fashion.
Distributed MPC based approaches aim to use consensus based distributed optimization algorithms that can help to divide the large complex problem in smaller sub-problems which can be handled by each agent separately. Optimization methods like Alternating Direction Method of Multipliers (ADMM) have been used to solve the distributed MPC problem and have proved to be computationally efficient for real time operations. In this work, we solve the multi-agent point to point transition problem using an ADMM based distributed MPC algorithm. The key contributions of this work is are (1) An extension of the optimization based collision avoidance constraints to multi-agent systems, (2) Formulation of a distributed optimization problem for multi-agent point to point transitions, and (3) an ADMM based distributed MPC approach to solve the given optimization problem.
We first discuss the problem formulation in the section III. Within the problem formulation, we first look at the system dynamics considered for our problem. Next we discuss the single agent objective and the obstacle avoidance constraints for each of the agent. Using this information we formulate the global optimization problem with linearly separable objective and coupled constraints. Next we present the formulation of the global problem in a distributed form and the distributed optimization method used to solve the problem. Section V discusses the details of the practical implementation and experiments conducted to show the effectiveness of the method and the analysis of the results in simulation. We finally discuss the conclusions and the future work in Section VI.
Ii Related Work
Trajectory optimization for multi-agent systems is a growing field of research attracting a lot of attention due to the significant applications of multi-agent systems of robots. The work related to the field spans across different problem statements and applications. Considerable work has been done on trajectory optimization for aerial robotic swarms in different scenarios. Works like [21, 3] focus on formation control and multi-agent path following tasks which are becoming increasingly popular. Several work like [22, 7] consider the problem of transporting heavy suspended payloads using a swarm of aerial robots which combines the use of geometric control and distributed optimal control. Various exploration and target searching applications also have considerable works like [2, 3]. A variety of distributed methods [15, 14] based on Alternating Direction Method of Multipliers (ADMM) and mixed integer programs have also come up on how to optimize such optimization problems.
Optimization based collision avoidance strategies require to incorporate the condition into a trajectory optimization problem like MPC. Several work [8, 24, 9] try to get rid of the non-convexity by modelling obstacles as repulsive potentials like gaussian and augmenting it to the objective function to penalize getting close to obstacles but such methods can lead to local optima and infeasible solutions a large number of times. Recent works model the obstacle collision condition as constraints . This helps to bring formal guarantees for collision avoidance at the cost of difficult optimization. The obstacles are generally modelled as convex polyhedrons or ellipsoids in order to mathematically formulate the constraints. Optimization methods like mixed integer programming 
and non linear programming[17, 13] are used to solve the optimization problems with collision avoidance constraints.
Several works focus on solving the multi-agent problem in a distributed fashion. The collision constraints between agents can be handled easily in such methods [5, 16] and the computational load reduces. Works like [4, 1] introduce guaranteed collision avoidance for different type of dynamic systems at the cost of conservativeness. Distributed MPC based methods have also been used in multi-agent trajectory optimization [15, 19, 12] for a range of problems. [10, 11] discuss the use of distributed MPC for the point to point transition problem along with obstacle avoidance.
Iii Problem Formulation
In this section we discuss the various aspects of the problem in hand and the adopted solution method along with certain caveats supplementing the current solution. We first have a look at the system and environment descriptions followed by the optimization objectives for each of the agent. We also look at the various constraint handling methods for the obstacle avoidance criterion. Further, we formulate the problem in a multi-agent scenario with global communication. Then we discuss the distributed formulation and the Alternating Direction Method of Multipliers (ADMM) used to solve the given distributed problem. Next, we discuss some implementation details required to efficiently solve the problem.
Iii-a System Dynamics
Within this work, we take into consideration two different set of systems for validating our solution methods. We consider a simple differential drive robot satisfying non holonomic constraints and navigating in a 2D environment with area constraint. Next, we consider a quadrotor aerial drone navigating in a 3D environment with volume constraint. Various complex dynamics model can be used to analyze these systems but for the simplicity of the problem, we model these systems using a double integrator in 2D and 3D respectively. We assume the availability of a position controller in both cases such that providing either accelerations as inputs or position points as the trajectory can work on the real robot.
where for and
and for and
We discretize the double integrator dynamics for our work using the first order Euler discretization in order to get the following system dynamics equations.
where are the respective continuous time matrices and is the order identity matrix for
Iii-B Single Agent Objective
We consider the problem of point to point transition for a single agent. Starting from an initial state, the goal of the agent is to reach the specified goal state while avoiding other agents and any obstacles. We use a weighted quadratic cost term to formulate the objective. We also add a regularization term to limit the actuator values.
The value function
is initially considered to be the normal quadratic value function similar to the discrete time LQR problem. The use of the value function here is to prevent the solution from diverging away from the global objective and acts as a heuristic for the global optimization problem. Pre-computed heuristics for the value function can also be incorporated within the method to improve overall performance.
Iii-C Obstacle Avoidance Constraint
Iii-C1 Obstacle Modelling
In this work, we model the obstacles as convex compact sets . We represent the obstacle space as
where , . Any polyhedral obstacle can be represented in the form shown above. As discussed in , this formulation is generalized for a convex pointed cone with non empty interior. For our work, we consider a second order cone and hence the general formulation converts to the common term.
For obstacle avoidance we need to ensure the following non differentiable constraint to satisfy
where is the position of the agent. This constraint is reformulated as given in  to preserve continuity and differentiability.
Iii-C2 Signed Distance Method
The signed distance method is a common method for obstacle avoidance. The sign of the signed distance function provides the indication of whether we have a collision with an obstacle. The signed distance function is defined as follows.
Here gives us the minimum distance from the polyhedron when the point is outside and gives the minimum distance from the polyhedron when the point is inside as shown in Fig. 1. For preventing collision with the obstacle, the . But as the is the result of an optimization problem, it leads to a bilevel optimization problem with the result of one constraining the other which is difficult to solve. So it needs to be reformulated before it can be included in the optimization problem.
Iii-C3 Collision Avoidance Reformulation
As discussed in , two different formulations for the collision avoidance constraint are provided by modelling the agent as point mass and the obstacle as discussed above. We discuss both the formulations ahead. The first formulation ensures that the distance between the obstacle and the agent is greater than a minimum threshold.
Here acts as a certificate for collision avoidance. Using this the optimal control problem for a single agent can be written as
The second formulation gives a less conservative approach to model the collision avoidance constraint. It is based on the logic of keeping the penetration depth of the agent inside the obstacle space less than a maximum threshold.
This formulation allows the signed distance function to be negative as well and hence provides less conservative results but the constraint adds non-convexity to the problem which leads to additional computational load and accurate initial guesses to be solved optimally. Using this minimum penetration reformulation, the optimal control problem can be written as,
where denotes the slack variable which must be very close to zero and should only become active when collision cannot be avoided and a minimum penetration trajectory is to be obtained. We use both the formulations to experiment between computational load and feasibility for different scenarios for our work.
Iii-D Distributed Formulation
In this section we formulate the problem for the multi-agent system and discuss the objective and constraints associated with the global problem for the entire swarm.
Iii-D1 Global Objective
The global objective is formulated in such a way that it is separable and can be solved in a distributed fashion. Taking the single agent objectives from section III-B, we use a simple summation of each of the single agent objectives to form the global objective.
where and are taken from Equation 5 for the th agent. We observe that this objective is separable and can be solved in a distributed fashion with each agent solving it’s own local objective.
Iii-D2 Modelling Agents As Dynamic Obstacles
For a distributed algorithm for a multi-agent system, each agent solves a finite time optimal control problem independently of the other agents. Exploiting this nature of the method, we model each agent as a cube around the spatial position of the agent as shown in Fig 2. This cube gives the obstacle space as discussed before in the single agent case. This can be written mathematically as
means the spatial position vector,is the threshold distance, and . For 2D system this can be also written as
and similar representation can be made in 3D as well. Now using the reformulation technique discussed before, we can formulate the obstacle collision avoidance between agents. For each agent, every other agent behaves as an obstacle cube represented by the equation 16. Using this we can formulate the obstacle collision avoidance for the th agent as
where and . Hence for each agent , there would be agents acting as obstacles which can be formulated as obstacle constraints similar to the above equation. We see that this formulation leads to a coupled constraint between neighbouring agents which is not separable. We intend to solve these problems with consensus based algorithm for distributed optimization.
Iii-D3 Distributed Optimal Control Problem
Having formulated the global objective and constraints, we can now write down a distributed problem with each agent solving the following finite time optimal control problem
where the objective depends only on the state of the agent but there is a presence of a coupling constraint between neighbouring states.
Iii-E ADMM Based Distributed Optimization
In this section we discuss the Alternating Direction Method of Multipliers (ADMM) for solving the Distributed MPC problem. The detailed proof and properties of the ADMM based Distributed MPC method can be found in 
Iii-E1 System Description and Notations
We first discuss the system description along with the notations that we use for the method. The ADMM approach that we follow requires each agent of the network to store a local copy of the optimization variables associated with other agents in the network. Hence the optimization vector for agent is defined as
where denotes the local copy of the optimization variable (states and actions over the horizon) of agent for the agent . denotes the set containing the index of all agents in the network. Augmenting the local optimization variable with copies of the optimization variable of other agent allows us to completely decompose the global problem into sub-problems for each agent and hence solve the problem in a distributed fashion.
Iii-E2 Consensus Constraint
After having divided the problem into sup-problems, we need to ensure that the values of the local copies and the global optimization variables match each other. In order to achieve this, a consensus constraint is introduced for each of the agent.
Here is defined as the network average variable which stores the average of all local copies of the optimization variables across different agents.
The values of are obtained after the optimization procedure for agent . Once the optimization procedure is completed for each of the agent, the resulting optimization variables are averaged to update the network average variable for each of the agent. The ADMM algorithm therefore iteratively alternates between local agent optimization and global averaging until consensus is achieved.
Iii-E3 Augmented Lagrangian Formulation
For the ADMM algorithm, the local optimization problem as discussed in Section III-D3 has to be modified. We have to formulate a augmented lagrangian objective that takes into consideration the consensus constraint and penalizes values too far away from the network average. The augmented objective can now be written as
where is as defined as in section III-B. denotes the Lagrange multiplier for agent and denotes the parameter that penalizes deviation from network average. Now at each iteration, we optimize the augmented lagrangian objective to obtain the local solution vector.
The optimal solution of the above problem is then transmitted to other agents in order to form the network average for the next iteration.
Iii-E4 ADMM Algorithm
The overall ADMM algorithm can now be written including the multiplier update step as shown in Algorithm 1.
Iii-F Pre-computing initial guess
Due to the non-convexity of the problem, the computational load is relatively high for the solution. We try to mitigate this by using pre-computed initial guess solutions as guiding trajectories for the solver. At every timestep, we run a sampling based motion planning scheme based on Rapidly Exploring Random Trees (RRT) from the current state to the goal state and pass the first states as the initial guess to the solver. This accelerates the optimization process and also prevents locally minimum solutions in many cases.
V Experiments and Results
For the simulation study, we consider agents to validate the algorithm. For the distributed MPC problem, we consider as the horizon length with a discretization time of . We vary the values of within a set of admissible values . The cost matrices as discussed in section III-B are defined as , and denoting the 2D and 3D system respectively.
V-B Simulation results
We are able to validate the algorithm on a simulation environment and the results shown in Fig. 3 suggest the success of the algorithm in achieving collision avoidance and goal reaching for each of the agent.
V-C Variation with
As observed in Fig. 5, the performance slightly deteriorates with an increase in the value of the parameter or the size of the cube around each agent. This is due to the conservative solutions associated with high value of the parameter. The effect of this parameter is more significant in dense environments leading to in-feasibility issues with a hard collision constraint formulation.
V-D Hard vs Soft constraints
We observe that soft constraints provide slightly sub-optimal solution as compared to the hard constraints as seen in Fig. 5. This is because it finds a sub-optimal solution which may not be feasible in the hard constraint formulation. Soft constraints are essential in dense environments as a conservative modelling of obstacles in tight spaces will lead to frequent infeasible solutions. Soft constraint formulation leads to elevated computational time as compared to hard formulation because of the non-convex equality constraint.
V-E Limitations and possible solutions
The current method has a relatively high computational load because of the collision avoidance constraint being solved at each time step. Incorporating an on-demand collision avoidance strategy as discussed in  can help to mitigate this for high frequency real time operations.
Vi Conclusions and Future Work
In this work, we present an extension to the work on optimization based collision avoidance for multi-agent point to point transition tasks. We model the agents as convex polyhedron obstacles and formulate the collision avoidance constraint using a reformulation method discussed in optimization based collision avoidance literature. Using the separable nature of the global objective, we are able to formulate the problem in a distributed manner with separable objective but coupled constraints among neighbouring agents. We solve the given problem using a consensus based ADMM algorithm.
We observe that the computational loads without code optimization is relatively high and hence we experiment with methods like online pre-computation of initial guess for the solver. Such modifications provide several advantages like lower computational times, high reliability and minimal deviation from optimal solution. We also observe the effects of parameter changes and changes in the reformulation methods on the nature of solution in terms of conservativeness, feasibility and optimality. All these ablations help us conclude on the usefulness of the method and the possibility of real-time application of such distributed methods for multi-agent robotic systems.
One of the key challenges not addressed by this work is handling of uncertainty within the system. For multi-agent systems and especially for aerial swarms, there are a lot of uncertainties associated with the system. When different UAVs come close together, the mutual interaction between them induces certain uncertainty in the dynamic model prediction. UAVs flying close to the ground also face uncertainty due to the downwash effect. Various problems involving aerial swarms also include suspended payloads to the UAVs which create more deviation from model predictions. All such uncertainties can be modelled as both random and systemic uncertainties. As a future work, we aim to model these uncertainties within the current formulation and develop Distributed Stochastic MPC based methods to solve the optimization problem robustly.
-  (2010) Optimal reciprocal collision avoidance for multiple non-holonomic robots. In DARS, Cited by: §II.
-  (2018) Distributed bayesian filtering using logarithmic opinion pool for dynamic sensor networks. ArXiv abs/1712.04062. Cited by: §II.
-  (2017) Tracking wildlife with multiple uavs: system design, safety and field experiments. 2017 International Symposium on Multi-Robot and Multi-Agent Systems (MRS), pp. 97–103. Cited by: §II.
-  (2011) Reciprocal collision avoidance with acceleration-velocity obstacles. 2011 IEEE International Conference on Robotics and Automation, pp. 3475–3482. Cited by: §II.
-  (2010) Distributed optimization with pairwise constraints and its application to multi-robot path planning. In Robotics: Science and Systems, Cited by: §II.
-  (2006) Optimal manipulator path planning with obstacles using disjunctive programming. 2006 American Control Conference, pp. 3 pp.–. Cited by: §II.
-  (2017) Fast trajectory optimization for agile quadrotor maneuvers with a cable-suspended payload. In Robotics: Science and Systems, Cited by: §II.
-  (1985) Real-time obstacle avoidance for manipulators and mobile robots. Proceedings. 1985 IEEE International Conference on Robotics and Automation 2, pp. 500–505. Cited by: §II.
-  (2016) BiRRTOpt: a combined sampling and optimizing motion planner for humanoid robots. 2016 IEEE-RAS 16th International Conference on Humanoid Robots (Humanoids), pp. 469–476. Cited by: §II.
-  (2019) Trajectory generation for multiagent point-to-point transitions via distributed model predictive control. IEEE Robotics and Automation Letters 4, pp. 375–382. Cited by: §II.
-  (2020) Online trajectory generation with distributed model predictive control for multi-robot motion planning. IEEE Robotics and Automation Letters 5, pp. 604–611. Cited by: §II, §V-E.
-  (2011) Minimum snap trajectory generation and control for quadrotors. 2011 IEEE International Conference on Robotics and Automation, pp. 2520–2525. Cited by: §II.
-  (2017) Real-time motion planning for aerial videography with real-time with dynamic obstacle avoidance and viewpoint optimization. IEEE Robotics and Automation Letters 2, pp. 1696–1703. Cited by: §II.
-  (2019) A distributed admm approach to non-myopic path planning for multi-target tracking. IEEE Access 7, pp. 163589–163603. Cited by: §II.
-  (2017) Distributed model predictive formation control with inter-vehicle collision avoidance. 2017 11th Asian Control Conference (ASCC), pp. 2399–2404. Cited by: §II, §II.
-  (2014) A decentralized cooperative control scheme with obstacle avoidance for a team of mobile robots. IEEE Transactions on Industrial Electronics 61, pp. 347–354. Cited by: §II.
-  (2017) Autonomous vehicle control: a nonconvex approach for obstacle avoidance. IEEE Transactions on Control Systems Technology 25, pp. 469–484. Cited by: §II.
-  (2017) ADMM-based distributed model predictive control: primal and dual approaches. 2017 IEEE 56th Annual Conference on Decision and Control (CDC), pp. 6598–6603. Cited by: §III-E.
-  (2017) Decentralized polynomial trajectory generation for flight formation of quadrotors. Proceedings of the Institution of Mechanical Engineers, Part K: Journal of Multi-body Dynamics 231, pp. 690 – 707. Cited by: §II.
-  (2014) Motion planning with sequential convex optimization and convex collision checking. The International Journal of Robotics Research 33, pp. 1251 – 1270. Cited by: §II.
-  (2018) Distributed simultaneous action and target assignment for multi-robot multi-target tracking. 2018 IEEE International Conference on Robotics and Automation (ICRA), pp. 1–9. Cited by: §II.
-  (2015) Mixed integer quadratic program trajectory generation for a quadrotor with a cable-suspended payload. 2015 IEEE International Conference on Robotics and Automation (ICRA), pp. 2216–2222. Cited by: §II.
-  (2017) Optimization-based collision avoidance. ArXiv abs/1711.03449. Cited by: §III-C1, §III-C1, §III-C3.
-  (2013) CHOMP: covariant hamiltonian optimization for motion planning. The International Journal of Robotics Research 32, pp. 1164 – 1193. Cited by: §II.