Multirobot systems have been studied thoroughly for solving a variety of complex tasks such as search and rescue [kantor2003distributed], sensor coverage [cortes2004coverage] and environmental exploration [burgard2005coordinated]. Global coordinated behaviors result from executing local control laws on individual robots interacting with their neighbors [ogren2002control], [olfati2007consensus]. Typically, the local controllers running on these robots are a combination of a task-based controller responsible for completion of a primary objective and a reactive collision avoidance controller. However, including a hand-engineered safety control no longer guarantees that the original task will be satisfied [borrmann2015control]. This problem becomes all the more pronounced when the number of robots increases. Motivated by this bottleneck, this paper focuses on algorithmic analysis of the performance-safety trade-offs that result from augmenting a task-based controller with a collision avoidance constraints as done using CBF based quadratic programs (QPs) [ames2017control]. Although CBF based QPs are not hand-engineered, rather they mediate between safety and performance in a rigorous way, yet ultimately they are reactive controllers. They locally (in a decentralized way) explore the acceleration/velocity space from which one admissible control is selected at every time instant. These local approaches exhibit a lack of look-ahead, which causes the robots to be trapped in deadlocks as noted in [petti2005safe, o1989deadlockfree, wang2017safety].
Deadlock occurs because robots reach a state where conflict becomes inevitable, i.e. a control favoring goal stabilization will violate safety (see solid red dot in fig. 1). Hence, the only feasible strategy is to remain static. Although once in deadlock, small perturbations can steer the system away from deadlock, there is no theoretical guarantee that the robots will not fall back in deadlock. To circumvent these issues, this work addresses the following technical questions:
What are the characteristics of a system in deadlock?
How can we leverage these characteristics to synthesize a controller that provably makes the system exit deadlock and reach its goal?
As a first step towards addressing these questions, in Section III, we review technical definitions for CBF based QPs [wang2017safety] to synthesize controllers for collision avoidance and goal stabilization. In Section IV, we recall the definition of deadlock and analyze the KKT conditions for the QP when a robot is in deadlock. The mechanics interpretation of KKT conditions indeed reveals the intrinsic force-balance on a robot in deadlock (section V-A). We motivate a set theoretic interpretation of deadlock for the case of robots, treating it as a subset of the state space, with an eye towards designing a strategy that evades/exits this set. In Section V, we focus on the easier to analyze case for two robots and analyze the mathematical properties of the deadlock set. We prove that this set is on the boundary of the safe set and that it is non-empty, bounded and of measure zero. We believe that these properties offer insight for the characteristics of deadlock in the general robot case. Finally, in Section VI, we use these properties to derive a provably-correct three-phase control strategy to make the robots exit deadlock and make them reach their goals. We demonstrate this strategy on different robot models in simulation, and experimentally on Khepera-IV robots. Finally, we conclude with directions for future work.
Ii Prior Work
Several existing approaches provide inspiration for the results presented here. Of these, two are especially relevant to our work. In the first category, we describe existing methods for collision avoidance and in the second we focus on prior work on deadlock resolution.
Ii-a Prior Work on Avoidance Control
Avoidance control is a well-studied problem with immediate applications for planning collision-free motions for multirobot systems. Classical avoidance control assumes a worst case scenario with no cooperation between robots [leitmann1977avoidance, leitmann1983note]. Cooperative collision avoidance is explored in [stipanovic2007cooperative] where avoidance control laws are computed using value functions. A robust control formulation of this work is presented in [hokayem2010coordination] for robot models described by Lagrangian dynamics and bounded noise in actuator torques. Velocity obstacles have been proposed in [fiorini1998motion] for motion planning in dynamic environments. They select avoidance maneuvers outside of robot’s velocity obstacles to avoid static and moving obstacles by means of a tree-search. While this method is prone to undesirable oscillations, the authors in [van2008reciprocal, van2011reciprocal, wilkie2009generalized] propose reciprocal velocity obstacles that are immune to such oscillations. More recently, control barrier function based controllers have been used in [borrmann2015control, wang2017safety] to mediate between safety and performance using QPs. The QP computes a controller closest to a task-based control that guarantees collision avoidance.
Ii-B Prior Work on Deadlock Resolution
The importance of coordinating motions of multiple robots while simultaneously ensuring safety, performance and deadlock prevention has been acknowledged in works as early as in [o1989deadlockfree]. Here, authors proposed scheduling algorithms to asynchronously coordinate motions of two manipulators to ensure that their trajectories remain collision and deadlock free in addition to reaching their goals. However, their approach relies on prior knowledge of the explicit geometric path in the workspace each robot should take (without assuming knowledge of the other robot’s path). In the context of mobile robots, [yamaguchi1999cooperative] identified the presence of deadlocks in a cooperative scenario using mobile robot troops. To the best of our knowledge, [jager2001decentralized] were the first to propose algorithms for deadlock resolution specifically for multiple mobile robots. Their strategy for collision avoidance modifies planned paths by inserting idle times and resolves deadlocks by asking the trajectory planners of each robot to plan an alternative trajectory until deadlock is resolved. Authors in [li2005motion] proposed coordination graphs to resolve deadlocks in robots navigating through narrow corridors. Authors in [rodriguez2016guaranteed] added perturbation terms to their optimal controllers for avoiding deadlock. Similarly, authors in [wang2017safety] use consistent perturbations and modify the QP to synthesize controllers which evade deadlock.
Differently from these works, we characterize the analytical properties of states in deadlock. We explicitly analyze controls from CBF based QPs and demonstrate that intuitive explanations for system in deadlock are indeed obtained from analysis using duality. We do not consider additive perturbations for resolving deadlocks since there are no formal guarantees for such methods. Instead, we use feedback linearization to synthesize a provably correct controller that ensures safety, performance and deadlock resolution.
Iii AVOIDANCE CONTROL WITH CBFs: REVIEW
In this section, we review CBF based QPs used for synthesizing controllers that mediate between safety (collision avoidance) and performance (goal-stabilization) for multirobot systems. We refer the reader to [wang2017safety] for a comprehensive treatment on this subject, since our work builds on top of their approach. Assume that we have mobile robots, each of which follows double-integrator dynamics:
, where represents the position of robot , represents its velocity and represents the acceleration (i.e. control). The collective state of robot is denoted by and the collective state of the multirobot system is denoted as . Assume that each robot has maximum allowable acceleration limits that represent actuator constraints. The problem of goal stabilization with avoidance control requires that each robot must reach a goal while avoiding collisions with every other robot .
For reaching a goal, assume that there is a prescribed PD controller with . Although this controller by itself ensures exponential stabilization of each robot to its goal, there is no guarantee that the resulting trajectories will be collision free.
Based on [wang2017safety], a pairwise safety constraint is formulated for every pair of robots to ensure mutual collision free motions. This constraint is mathematically posed by defining a safety function that maps the joint state space of a pair of robots to a real-valued safety index i.e. . This index is defined as [wang2017safety],
Robots are considered to be collision-free if their states are such that . We define the “safe set” as the zero level super set of i.e. }. The boundary of the safe set is defined as:
Assuming that the initial positions of robots are in the safe set , we would like to synthesize controls that ensure that future states of the robots also stay in . This can be achieved by ensuring that
where we choose, . For the given choice of , constraint eq. 4 can be rewritten as compactly
This constraint is distributed on robots as:
Therefore, any and that satisfy eq. 7 are guaranteed to ensure collision free trajectories for robots in the multirobot system. Note that these constraints are linear in and for a given state . Therefore, the feasible set of controls is convex. Assuming robot wants to avoid collisions with its neighbors, there will be collision avoidance constraints. To mediate between safety and goal stabilization objective, a quadratic program is posed that computes a controller closest to the PD control (in 2-norm) and satisfies the collision avoidance constraints.
This QP has linear constraints ( from collision avoidance with neighbors and four from acceleration limits). Each robot executes a local version of this QP and computes its optimal . The control generated by this QP represents a reactive collision avoidance mechanism. At every time step, depends on the state of the ego robot and its neighbors. As long as the QP remains feasible, the generated control ensures collision avoidance of robot with its neighbors. In the next section, we use this method to analyze the incidence of deadlocks due to this technique.
Iv Analysis for Robot Deadlock
We reviewed the problem formulation of multirobot collision avoidance and goal stablization using the framework of CBF based QPs. In this section, we will show that this approach can result in deadlocks (depending on the initial conditions and goals of robots). Additionally, we want to analyze qualitative properties of a robot in deadlock. Towards that end, we will investigate the KKT conditions of the problem in eq. 8. Our goal is to use these properties to devise a strategy to make the system exit deadlock.
From our numerical simulations, we observed that one scenario when deadlock occurs is when the initial states of a pair of robots and their goals are symmetric with respect to one another. That is to say that one scenario prone to deadlock in robots is when the initial position of robot is the desired goal for robot and vice versa. To illustrate this point, Fig. 2 shows the states of two robots executing controllers based on eq. 8 . Notice from fig. 2(a) that the positions of robots have converged, but not to their respective goals. Therefore, the outputs from the prescribed PD controller will still be non-zero after convergence. Additionally, the corresponding velocities have converged to zero (fig. 2(b)), and the control inputs from eq. 8 have converged to zero as well (fig. 2(c)). Following these observations, deadlock is defined as follows
[wang2017safety] A robot is said to be in deadlock if , , and
This definition states that for a robot to be in deadlock, the robot should be static i.e. its velocity should be zero, and the output from the QP based controller should also be zero, even though the prescribed PD controller reports non-zero acceleration since the robot is not at its intended goal. We now review the KKT conditions for the optimization problem in eq. 8 and then specifically analyze them in deadlock.
Iv-a KKT Conditions
As explained in Section III, we seek control inputs that guarantee collision avoidance and goal stabilization. Recall that each robot computes a control by solving a local QP as in eq. 8. For the following discussion, define and . To simplify notation, we will drop subscript and implicitly assume that the QP is being solved for the ego robot. Therefore, we rewrite eq. 8 as:
Let denote the rows of and denote the elements of . The KKT conditions are necessary and sufficient (in this case) for a global optimum of the problem in eq. 9. The Lagrange dual function is
Let be the optimal primal-dual solution to problem eq. 9. The KKT conditions [boyd2004convex] for this problem are
Define the set of active and inactive constraints as follows:
Using complementary slackness from eq. 15, we deduce
Therefore, we can restrict the summation in item 1 to the set of active constraints only i.e.
Iv-B KKT Conditions for the deadlock case
Since in deadlock, the acceleration input to the robot i.e. is also obtained from QP, KKT conditions can still be used to analyze the characteristics of the system in deadlock. From Def.1, we know that in deadlock, , and . We conclude that:
i.e. the solution to the QP is not equal to the prescribed PD controller (which means that is infeasible in deadlock i.e. ).
. This implies that at least the last four constraints in are inactive i.e. .
Using these observations, we can rewrite the KKT conditions for the deadlock case as follows:
Based on these conditions, we will now motivate a set-theoretic interpretation of deadlock. Assume that the state of ego robot is and it has neighboring robots denoted as . Define and appropriately to extract the position and velocity components from i.e. and . Finally, define as:
The set is defined as the set of all states of the ego robot which satisfy the criteria of being in deadlock. We have combined the conditions of deadlock into a set theoretic definition. Additionally note that for each robot, its set of deadlock states depends on the states of its neighboring robots. This is because the Lagrange multipliers depend on the states of all robots. To highlight this dependence, we use the notation .
The motivation behind stating this definition is to interpret deadlock as a bonafide set in the state space of the ego robot and derive a control strategy that makes the robot evade/exit this set. We now rewrite this definition in more easily interpretable conditions. From eq. 19 and eq. 20, note that
Since , we rewrite eq. 25 as:
We will use this condition to replace the criterion in the def. of in (IV-B). Secondly, we know that prescribed controller is a PD controller. Define the goal state as . Noting that and in deadlock:
This criterion is merely restating that in deadlock the ego robot is not at its goal. Finally, we note that the third condition is that the velocity of the ego robot is zero i.e. Combining these conditions, we rewrite the definition of the deadlock from section IV-B as follows:
The Lagrange multipliers , in general, are a nonlinear function of the state. The exact values of can only be determined on the basis of the set of active constraints. If there are neighboring robots i.e. collision avoidance constraints per ego robot, there will be possible cases of active/inactive constraints. Therefore, for further analysis, we will restrict the discussion to the special case of two robots.
V Analysis for Two Robots
In section IV, we proposed a set-theoretic definition of deadlock for a specific robot in an robot system. In this section, we will refine the KKT conditions derived in Sec. IV-B for the case of two robots in the system. We restrict to two robots not only because this case is easier to analyze, but also because it reveals several important underlying characteristics of the system in deadlock that will extend to the robot case. Specifically, we believe that deadlock analysis for robots can be decomposed into subproblems involving deadlock analysis involving pairs of robots. We will explore this future work. One key feature of a two-robot system is that a single robot by itself cannot be in deadlock i.e. either both robots are in deadlock simultaneously or neither. This is because the sole collision avoidance constraint is symmetric in a system with just two robots. Keeping this in mind, we define system deadlock set to be the set of states for which both robots are in deadlock. To define this set, we modify eq. 25; specifically for this case there is no summation since the ego robot avoids collision only with the other robot i.e.
Assuming are the states of the two robots, we modify section IV-B and define as:
where . To gain more insight into this system deadlock, we derive analytical expressions for the Lagrange multipliers . Depending on whether the collision avoidance constraint is active/inactive at the optimum, there are two cases:
Case 1: The constraint is active at .
Case 2: The constraint is inactive at . i.e. . From complementary slackness, it follows that i.e. and hence . However, this contradicts the def. of deadlock. Hence, we conclude that case 2 can never arise when the two robots are in deadlock.
V-a Mechanics interpretation of KKT conditions
Recall eq. 29 which is one of the conditions for two robots in deadlock. The left hand side of this equation is . The right hand side of this equation is . Writing eq. 29 another way, we get . We interpret the first term as an attractive force that pulls the ego robot towards the goal . Since , we interpret the second term as a repulsive force pushing the ego robot away from its neighbor. Thus, deadlock occurs when the net force due to attraction to goal and repulsion from the other robot vanishes. We illustrate this equilibrium in Fig. 3.
V-B Characteristics of two-robot deadlock
We are now interested in analyzing qualitative properties of the system deadlock set with an eye towards synthesizing controllers that enable the robots to get out of deadlock. We will show that when deadlock occurs, (1) the two robots are separated by the safety distance, (2) deadlock set is non-empty and (3) deadlock set is bounded and of measure zero.
In section V, we proved that the collision avoidance constraint is active in deadlock. Since both robots are in deadlock, we know that both of their constraints are active i.e. and and . This implies . Using eqs. 7 and III and that in deadlock, we get
Recall from eq. 2 and using that , we get
Therefore, . Since robots remain safe (assuming QP is feasible), we disregard . Therefore, . Additionally, recalling the definition from from eq. 3 we deduce that, in deadlock, i.e. ∎
This result confirms our intuition, because if the robots are separated by more than the safety distance, then they will have space to move towards each other, albeit with very small non-zero velocity, which however would contradict the definition of deadlock.
We now propose a family of states that are always in the deadlock set section V.
a family of states and such that . Moreover, these states are such that the robots and their goals are all collinear.
To prove this theorem, we first propose a set of candidate states and show that they satisfy the definition of deadlock section V. See Fig. 3 for an illustration of quantities referred to in this proof.
Let and where and . Note that are collinear by construction. Let and . Then we will show that Note that
From definition, where is the distance between the goals. Therefore, we have
Hence, in section V-B, we have shown that which is one condition in the definition of deadlock set. Similarly, we can show that . Also note that in section V-B we have shown that the Lagrange multiplier is positive, which is another condition in section V. We can similarly show that . Finally, note that for our choice of states, and we have restricted so we ensure that Hence, the proposed states are always in deadlock. ∎
Although in this theorem, we proposed a candidate set of states and verified that they are in deadlock, one can check by explicitly solving the equations in section V that these are in fact, the only states that satisfy the criteria of deadlock.
is bounded and of measure zero.
Following the definition of and from theorem 1 and 2, we can show that when two robots are in deadlock, their positions satisfy the following constraint:
This can be verified by straightforward substitution. From this constraint it is evident, that the deadlock set is not “large” and of measure zero. That is why, random perturbations are one feasible way to resolve deadlock. However, whether or not deadlock gets resolved depends on the stability of this set. We explore resolution of deadlock in the next section. ∎
Vi Deadlock Resolution
We are now interested in using the properties derived above to synthesize a corrective strategy that (a) gets the robots out of deadlock, (b) ensures safety and (c) makes the robots converge to their respective goals. One approach to accomplish these three objectives is to detect the incidence of deadlock while the QP based controller is running on the robots. Once detected, any small non-zero perturbation to control will instantaneously give a non-zero velocity to the robots. Thereafter, the QP based controller can take charge again and we can hope that using this controller the system state will come out of deadlock at-least for a short time. This approach has two limitations however, firstly, since it was the QP based controller that led the system to deadlock in the first place, there is no guarantee that the system will not fall in deadlock again. Secondly, perturbations can violate safety.
We propose a provably-correct controller which ensures that goal stabilization, avoidance control and deadlock resolution are satisfied. Refer to Fig. 4 for a schematic of our approach. This algorithm is described here:
Given initial position of the robots, the algorithm starts by executing controls derived from CBFs based QP in Phase 1. This ensures movement of robots to the goals and safety by construction. To detect the presence of deadlock, we continuously compare against small thresholds. If satisfied, we switch control to phase 2, otherwise, phase 1 continues to operate.
In this phase, we use any controller to ensure that the (1) robots rotate around each other to swap positions while (2) maintaining the safe distance. Feedback linearization (FL) is one viable approach that can generate such a controller, and moreover, FL based control can be easily decentralized. By ensuring and rotation () , we ensure that safety is maintained (i.e ). Once the robots swap their positions (which is guaranteed by design), their new positions will ensure that prescribed PD controllers will be feasible in the future.
After convergence of Phase 2 (which happens in finite time), control switches to Phase 3, which simply uses the prescribed PD controllers. This phase guarantees that the distance between robots is non-decreasing and safety is maintained as we prove in theorem 4.
Fig. 5 shows simulation and experimental results from running this strategy on a pair of double-integrator (Fig. 5(a)) and nonholonomic robots (Fig. 5(b),5(c)). Videos of simulations and experiments are available at https://tinyurl.com/y4ylzwh8. Note that for nonholnomic robots, we noticed from simulations and experiments that deadlock only occurs if the body frames of both robots are perfectly aligned with one another at . Since this alignment is difficult to establish in experiments, we simulated a virtual deadlock at i.e. assumed that the initial position of robots are ones that are in deadlock. We next prove the correctness of our three-phase strategy:
Assuming that PD controllers are overdamped and , this strategy ensures that (1) the robots will never fall in deadlock and (2) converge to their goals.
We would like to show that once phase three control begins, the robots will never fall back in deadlock. We will do this by showing that the distance between the robots is non-decreasing, once phase three control starts.
We break this proof into three parts consistent with the three phases:
Phase 1 Phase 2:
Let be the time at which phase 1 ends (and phase 2 starts) i.e. when robots fall in deadlock. In theorem 1 we showed that in deadlock , and in theorem 2 we showed that the positions of the two robots and their goals are collinear. So at the end of phase 1,
. The goal vector. Moreover, since the robots are static in deadlock,
Phase 2 Phase 3:
The initial condition of phase two is the final condition of phase one i.e. and . In phase two, we rotate the assembly of the robots making sure that the distance between them stays at the safety distance, until the orientation of the vector aligns with the orientation of . Assuming that the control strategy accomplishes this, i.e. , a time at which . Additionally, at this time, the robots are no longer moving, hence their velocities are zero. So at this time, and . These states mark the final condition for phase 2 and initial for phase 3.
In this phase, the initial conditions are and . Also, note that the dynamics of phase 3 control are specified by the prescribed PD controllers. The dynamics of relative positions and velocities are:
Now, we will do a coordinate change as described next. Let and . The initial conditions in these coordinates are and i.e. . The dynamics in new coordinates are:
Using these coordinates, note that . Note from the dynamics and the initial conditions for the components of relative position and velocities that the only solution is the zero solution i.e. and . As for the component, we can compute the solution to be and where
and and . After substituting these values, we get, . Now, from the assumptions that PD controllers are overdamped i.e and , it follows that and .
Finally, note that . Hence, the distance between the robots is non-decreasing i.e. the robots never fall in deadlock and safety is guaranteed. Additionally, since the robots use a PD-type controller, their positions exponentially stabilize to their goals. ∎
In this paper, we analyzed the characteristic properties of deadlock that results from using CBF based QPs for avoidance control. We demonstrated how to interpret deadlock as a subset of the state space and proved that in deadlock, the robots are on the verge of violating safety. Additionally, we showed that this set non-empty and bounded. We developed a three-phase control algorithm to force the robots out of deadlock and ensure task completion while staying safe. There are several directions we would like to explore in future. Firstly, our analysis for deadlock was restricted to the case of two robots. We found that the exponential increase in the possible values of Lagrange multipliers limit the scalability of deriving characteristics of deadlock. However, we still claim that the problem of robot deadlock can be decomposed into subproblems of pairwise deadlock in two-robots. We will pursue this approach and extend our analysis to the robot case. Secondly, we would like to compute the basin of attraction of the deadlock set i.e. the formally characterize the set of initial conditions that lead the robots to deadlock. We envision that tools from backwards reachability set calculation can be used to formally characterize the basin of attraction. Finally, we focused on CBF based QPs for analysis. In future, we would like to extend this to other reactive methods such as velocity obstacles and tools using value functions, and explore the properties that make a particular algorithm immune to deadlock.
Authors would like to thank Michael Cheng and Fan Jia for help with experiments. This research was supported by the DARPA Cooperative Agreement HR00111820051.