Chance constraint based multi agent navigation under uncertainty

We present Probabilistic Reciprocal Velocity Obstacle or PRVO as a general algorithm for navigating multiple robots under perception and motion uncertainty. PRVO is defined as the space of velocities that ensures dynamic collision avoidance between a pair of robots with a specified probability. Our approach is based on defining chance constraints over the inequalities defined by the deterministic Reciprocal Velocity Obstacle (RVO). The computational complexity of the proposed probabilistic RVO is comparable to the deterministic counterpart. This is achieved by a series of reformulations where we first substitute the computationally intractable chance constraints with a family of surrogate constraints and then adopt a time scaling based solution methodology to efficiently characterize their solution space. Further, we also show that the solution space of each member of the family of surrogate constraints can be mapped in closed form to the probability with which the original chance constraints are satisfied and thus consequently to probability of collision avoidance. We validate our formulations through numerical simulations where we highlight the importance of incorporating the effect of motion uncertainty and the advantages of PRVO over existing formulations which handles the effect of uncertainty by using conservative bounding volumes.

of the neighbouring robots and execute the computed avoidance maneuver without any errors. However, in reality both motion and state estimation associated with each robot are imprecise and it is important to take the uncertainty in consideration. Thus, for robust implementation of multi robots in a particular task, it is imperative to make the transition to the probabilistic domain, where we explicitly consider the uncertainty in the system while computing the avoidance velocities. As one would expect, the complexity of the probabilistic variant tends to be much higher than the deterministic counterpart, as now the reasoning shifts from just collision avoidance to probability of collision avoidance. To be more precise, in the probabilistic setting, it is required to map each velocity in a given set to the resulting probability of collision avoidance. Alternatively, one needs to obtain the characterization for the space of velocities for a given probability of collision avoidance.
In this paper, we present a novel algorithm for multi-robot collision avoidance which explicitly takes into account, the perception and motion uncertainty of each robot. Our approach combines ideas from velocity obstacle based multirobot collision avoidance and robust control specifically robust Model Predictive Control (MPC) . In particular, we borrow the concept of chance constraints which are used in robust control to ensure constraint satisfaction under uncertainty [13]. These chance constraints ensure that the probability of a constraint being satisfied is greater than a specified threshold. In our approach, we formulate chance constraints over the conditions defined by the deterministic Reciprocal Velocity Obstacle (RVO) [2] and characterize the the resulting set of inequalities as Probabilistic Reciprocal Velocity Obstacle or PRVO.
We also provide a computationally efficient approach for solving these complex set of inequalities. To this end, we build upon our recent series of works [14], [15], where chance constraints are substituted with a more tractable family of surrogate constraints. The solution space of each member of the family can be mapped in closed form to the probability with each the original chance constraints are satisfied. Finally, a time scaling based methodology can be adopted to efficiently solve the surrogate constraints. In this paper, we extend the approach of [14], [15] to multiple decision making robots by expanding our model to incorporate the uncertainty that the robot would exhibit while executing the computed avoidance maneuver.

Summary of Main Results
On the computational side, we show that the complexity of the PRVO is comparable to its deterministic counterpart. This is primarily achieved by the use of surrogate constraints and a time scaling based methodology for their solution. Further, we show that the previous developed Cantellis inequality based bounds which relates solution space of surrogate constraints to the probability of collision avoidance performs well in case of maneuvering targets as well. On the practical side, we highlight the importance of explicitly including the uncertainty associated with execution of avoidance maneuvers. Moreover, the advantage of PRVO over existing works which incorporates the effect of uncertainty by enlarging the size of the robot [16], [22], [17], [18] is also presented.

Layout of the Paper
The rest of the paper is organized as follows. Section 2 contrasts the proposed formulation with the existing works. Section 3 summarizes the notations used in the paper. Section 4 presents the collision avoidance conditions as modeled Reciprocal Velocity Obstacle (RVO) in the deterministic setting. Section 5 introduces the model of the uncertainty used in the current work followed by the introduction of chance constraints over the inequalities defined by RVO. Section 6 presents a time scaling based methodology for efficient characterization of the solution space of the surrogate substitution of the chance constraints. Section 7 presents an analysis of computational complexity, probability bounds and motivations for choosing RVO as the basis for construction of chance constraints as compared to its convex approximation ORCA. The simulation results are presented in section 8

Related Work
In this section, we contrast our proposed formulation with the existing works in terms of uncertainty model, methodology for formulating and solving dynamic collision avoidance under uncertainty in the context of both single robot and multi-robot navigation.

Uncertainty Model
Our methodology of modeling uncertainty in the system is similar to that presented in works like [5], [6], [7], [8], [9], [10], [11], [12], [16], [17], [18]. in the sense that uncertainty at any time instant is represented as a normal random variable with a particular mean vector and covariance matrix. However, among these, [5]- [11] consider the case of a single robot moving among non-responsive dynamic obstacles and incorporates the effect of only perception uncertainty while treating the robot's motion through deterministic models. Algorithms presented in [12] considers the effect of both perception and motion uncertainty but only in the context of dynamic collision avoidance of a single robot. In [16], each robots motion uncertainty is taken into consideration while constructing the RVO. However, it does not consider the uncertainty in the error between the computed and executed avoidance maneuver,or in short the actuation uncertainty.

Dynamic Collision Avoidance Constraints under Uncertainty
At a conceptual level, our modeling of dynamic collision avoidance under uncertainty is similar to works like [5]- [12] in the sense that we are also primarily concerned with relating each velocity in some given set to a probability of collision avoidance. The differentiating factor however, with these cited works lies in the technical approach followed and in the fact that we deal with multiple maneuvering robots. [5]- [12] relies on sampling different velocities, using them to construct robot's trajectory over a short horizon and inferring probability of collision with respect to them. However, such sampling based approaches are difficult to extend for multiple robots due increase in complexity of the search space. In contrast, we propose a systematic decomposition of probabilistic collision avoidance constraints into simpler forms eventually leading to a close form approximation of the solution space.
The probabilistic approach followed in the current proposed work is also very different from those presented in [16], [17], [18] which account for uncertainty in dynamic collision avoidance by expanding the size of the velocity and radius of the robot, depending on the level of uncertainty in the system. In principle, this is equivalent to drawing a lot of samples from position, size and velocity uncertainty ellipse and writing collision avoidance with respect to each of them. Although very simple, this approach suffers from the drawback that it is blind to the probability of samples drawn and thus, samples which are closer to the mean are given same importance as samples farther from the mean. At the implementation level, one would have the same collision avoidance constraints with respect to both the samples. As we show later, this leads to a very conservative solution space.

Symbols and Notations
We used bold faced small case letters with superscripts to describe vectors associated with a particular robot. For example, the position and velocity of robot i is represented as p i = (p x , p y ) and v i = (v x , v y ). We use f RV O i j to represent the collision avoidance conditions computed through the concept of RVO. The symbol v i rvo represents collision avoiding velocities computed using the deterministic RVO algorithm. We use µ and σ 2 with suitable subscripts and superscripts to represent mean and variance of vectors or functions respectively. In section 6, we use additional superscript "s" to denote time scaled variants of vectors, functions or constraints.

Pre-Requsitie: Deterministic Velocity and Reciprocal Velocity Obstacle
In this section, we briefly review the concept of Reciprocal Velocity Obstacle (RVO). We do not go into details, rather present the general algebraic form for the collision avoidance constraints defined by RVO. For details, the readers are requested to refer to [2]. We consider disk shaped robots each modeled as following single integrator system in 2D Euclidean X − Y space (1) In (1), v i x and v i y respectively represents the x and y component of the velocity of the i th robot. It is clear from (1) that the trajectory of each robot is assumed to be composed of piece-wise straight line segments. Now, consider a collision scenario where two robots with radius R i and R j are moving with constant velocities v i and v j . The space of such velocities v i rvo which allow robot i to come out of the collision course with robot j can be characterized by the following set of inequalities derived from the concept of RVO [2]. (2) is a non-convex quadratic with respect to v i rvo and thus, computing a characterization of collision free velocities automatically becomes a challenging problem.
At this point, it is worth pointing out that the in the deterministic setting it is assumed that each robot can estimate its current state and the state of the other robot perfectly and thus, can construct inequalities (2) exactly. In the subsequent sections, we relax this assumption thereby extending RVO to the probabilistic domain.

RVO in the Probabilistic Domain
Let us start by representing the current trajectory of each robot as the following random variables.
and similarly others represents position and velocity level mean and variances, respectively. In the context of two robot collision scenario considered in the previous section, equations (3) and (4) model the fact that robot i has some uncertainty in the estimate of its current state and the state of the robot j. Although, we have assumed a Gaussian form, our approach can be easily extended to incorporate other representations as well and we discuss that briefly, later on in the paper.
Similarly, let us assume that each robot has an imperfect actuation and thus there is an inherent noise between the commanded and actual velocity.
This noise would result in some error between the avoidance maneuver that the robot intends to perform and actually performs. Moreover, this error itself would be a random variable. In the context of RVO, we account for this uncertainty associated with avoidance maneuver by assuming that v i rvo is drawn from a distribution. In other words, it is modeled as the following Gaussian random variable The above equation models the fact that when the robot commands a velocity v i rvo , the executed velocity can correspond to any sample drawn from a Gaussian distribution whose mean is the commanded velocity and the variance is some j becomes a multivariate function of random variables and thus, consequently a random variable itself. Thus, mathematically, (2) does not make sense. Instead, a more well defined alternative would be to consider the following inequality Constraint (6) ensures that the probability of RVO based collision avoidance condition (2) being satisfied is greater than some lower bound η. It is straightforward to note that (6) infact defines the space of velocities v i rvo for robot i which ensures satisfaction of RVO constraints with atleast probability η for the given robot j trajectory parameters p j and v j . We define (6) as probabilistic reciprocal velocity obstacle or PRVO.
Constraints having the general form as that of (6) are popularly known as "chance constraints" in the robust control literature [13], and in general are computationally intractable [19]. The primary difficulty lies in computing the analytical form for the chance constraints. One notable exception exists in the case when the random variables in consideration have Gaussian distribution and the chance constraints are defined over affine inequalities [20]. In such cases, efficient convex approximations for the chance constraints can be derived. However, as stated earlier, f RV O i j is non-convex quadratic in terms of random variables and thus the techniques proposed in [20] is not applicable in our case. In the next section, we present a novel substitution for (6), which exploits the fact that although it is intractable to obtain the analytical form for left hand side of (6), it is relatively straightforward to obtain symbolic expressions for expectation and variance for f RV O i j .

Expectation and Variance of
Expectation of a multivariate function g in terms of variables z 1 , z 2 ...z n is given by the following expression.
Using (7), the expectation of f RV O i j (.) can be obtained in the following manner.
In (8) P (.) represents the joint probability distribution of the random vari- (8) can be computed symbolically using packages like MATHEMATICA [21] and can be eventually reduced to a quadratic polynomial in terms of v i rvo . We can proceed to use (8) to compute the variance of f RV O i j (.) in the following manner. The right hand side of equality (9), when computed symbolically, reduces to a fourth order polynomial in terms of Equations having the general form similar to (8) and (9) were introduced in our earlier work [14]. However, in contrast to our earlier formulations, (8) and (9) is more complex as they depend on additional random variables pertaining to motion uncertainty of the robot i.
As shown in our earlier work [14], inequality (10) represents a strip of width Thus, solving (10) with increasing value of k ensures increasingly larger part of the distribution of f RV O i j to be above zero. This in turn leads to increasingly safer velocities. To put it mathematically, satisfaction of (10) ensures satisfaction of original chance constraints (6) with a lower bound probability dependent on the value of k, i.e η ≥ c(k) for some positive monontonic function c(k). In [14], we derived the following mapping based on Cantellis inequality, thus providing a closed form mapping between the solution space of each member of the family of constraints (10) and the probability with which the original chance constraints are satisfied.

Time Scaling based solution of Surrogate Constraints
As stated earlier, (8) and (9) are respectively quadratic and quartic in terms of variable v i rvo . Thus, (10) represents a non-convex polynomial inequality, which in general is computationally intractable. In this section, we present a time scaling based framework for approximating the solution space of (10). The basic idea is simple. We first compute the space of velocities v i rvo which the robot can reach by just changing the time scale of the current trajectory. Importantly, we show that this solution space can be characterized in closed form to obtain a set of formuale. Further, evaluating these formulae over multiple paths gives a good approximation of the complete solution space of (10).

Time Scaled Variant of Surrogate Constraints
Changing the time scale of a trajectory from t to τ does not alter the geometric path but results in following change in the velocity profile In (12), dt dτ is called the scaling function and decides the mapping between the time scales. Now, with slight abuse of notation, let us assume that the robot i is moving along a straight line trajectory characterized by a velocity v i . Let us denote by following the space of collision avoiding velocities that the robot can achieve by just changing the time scale of the current trajectory Substituting (13) into (8) and (9) Where, a(.), b(.)...h(.) are function of parameters of distribution of random variables i. e, µ i Please note the additional superscript "s" representing that we are now dealing a time scaled variant of µ f RV O i j and σ f RV O i j derived in (8) and (9) Now, we perform one final simplification of (15) to obtain quadratic approximation based on second order Taylor series expansion around a point s * σ s f Using (14) and (16), the final form of the time scaled variant of (10) can be obtained in the following manner.
It can be observed that (17) is a single variable quadratic inequality and thus, its solution space can be characterized in closed form [23]. Extending (17) to say n robots is also straightforward as in that case we would have n single variable quadratic inequalities, the solution space of which can again be characterized in closed form. The inequality (17) can be constructed along multiple paths to obtain a good characterization of the complete solution space.
It is worth pointing out that there are various heuristics for choosing the point s * . One of them which has been shown to result in low approximation errors is to choose s * from the solution space of µ s f RV O i j ≥ 0 [14]. It is easy to note that solving it is similar to solving (17).

Computational Complexity of RVO and PRVO
. The collision avoidance through deterministic RVO for a pair of robots takes the form of non-convex quadratic inequality (2). In contrast, the surrogate constraints (10) which approximates the PRVO constraints (6) is a quartic inequality. Thus, on the surface PRVO seems significantly more complicated than RVO. However, the time scaling based reformulations discussed in section 6 does provide a significant simplification. Essentially as explained in section 6, solving the surrogate constraints and consequently PRVO has been reduced to generating multiple candidate trajectories and evaluating the solution space of (17). Generating multiple candidate trajectories is simple; during implementation, we randomly generate some velocity samples and then use them to construct straight line trajectories. Solving (17) is straightforward and as explained can be infact done in closed form. Although it is difficult to derive an bounds on the number of samples, we employ some heuristics and often recover a solution space with mostly one or two samples. Thus, it can be concluded that kind of surrogate constraints that we have proposed coupled with time scaling based solution makes computational complexity of PRVO atleast comparable to RVO.

PRVO Vs PORCA
In the deterministic setting, the collision avoidance conditions modeled through ORCA for a pair of robots can be written as the following linear inequality for some z 1 and z 2 which are a function of robot j trajectory parameters.
In (18), v i orca is the collision avoidance velocity as modeled through ORCA. Now, since ORCA is an approximation of RVO, the space of collision avoidance velocities characterized by (18) is much smaller than that characterized by RVO (2). Now let us consider chance constraints defined over (18) and its surrogate constraints given by [20] Ineqaulity, 20 represents a convex second order cone constraint. Moreover, the first two terms in 20, is conjunction is nothing but the deterministic ORCA (18), while the third term is non-negative. It is clear from (20) that chance constraints essentially boils down to obtaining a even smaller subset from the already restrictive solution space of deterministic ORCA. Thus, at the implementation level, infeasibility of (20) can be of concern.
In light of the above arguments, it obviously makes more sense to start with the larger solution space of RVO such that feasibility can still be ensured when the solution space shrinks because of the application of chance constraints.

Results
The results presented in this section are grouped into following categories. (1) Validating that solving the surrogate constraints (10), with increasing value of k leads to satisfaction of original PRVO constraints (6) with increasing probability η. (2) Showing the importance of incorporating the motion uncertainty.
(3) Comparing solution space computed through PRVO with that obtained from [16] and similar approaches.  PRVO is satisfied for each robot increases to 0.75, which is again in accordance with the Cantellis bounds. Figures 4(a)-(4(c)) summarizes the avoidance maneuvers for various values of k or in other words for various probabilities η. As shown, with increase in k, the deviation from the current trajectory increases for each robot.

Illustrating the need for incorporating motion(ego) uncertainty while computing collision free velocities
In this section, we highlight that if self motion uncertainties of robots are ignored, then it is difficult to reliably infer, the probability with which the RVO constraints are actually satisfied with each robot. To put it alternatively, the Cantellis bounds based mapping between the solution space of the surrogate constraints (10) and the PRVO (6) does not hold if self motion uncertainties of the robots are ignored. We start by deriving a variant of the surrogate constraints (10) but without considering equation (5), i.e, v i rvo is no longer treated as a random variable. Now for a configuration of robots shown in figure 5(a), consider figures 5(b)-5(c), where this variant is solved by each robot for k = 1. Based on Cantelli's bounds, this should translate to PRVO being satisfied with η ≥ 0.5. However, as shown, when evaluated through sampling, η turns out to be less than 0.5. In contrast, in figures 5(d)-(5(e)), where self motion uncertainties are considered, Cantellis bounds hold perfectly.  (17) along them for k = 1. Based on the Cantellis bounds discussed in section 5.2, this should lead to satisfaction of PRVO constraints (6) with atleast probability 0.5. As can be seen from the figures, we validate that this is indeed the case. To elaborate further, we take various samples from the position and velocity uncertainty ellipse of each robots and evaluate what percentage of these samples lead to satisfaction of RVO constraints (2), and this is shown in the form of black and red samples in each uncertainty ellipse, where black samples indicate that the RVO constraints are not satisfied, and the red samples indicate that the RVO constraints are satisfied .

Advantage of PRVO over existing works
One of the simplest approach to model the probabilistic variant of RVO is to grow the radius of the robots by a value corresponding to a particular confidence region of the uncertainty ellipses . This procedure has been illustrated in [16] (6). As can be seen, higher probabilities require each robot to take a larger deviation from the current trajectory.
and [22].Table1 and Table2 compares this method with our proposed formulation for two robots for a scenario similar to Figure 5(a). The solution space of the timescaled variant 17 of the surrogate functions for a particular value of 'k' that satisfies a 68 percentage and 80 percentage confidence contour of the uncertainty ellipse in the position space was obtained.This is then compared with the solution space that is obtained by enlarging the robot's radius by 68 percentage and 80 percentage confidence contours.While we enlarge the radius by the desired confidence region, it is also important to take into consideration the probability of velocities that the robots can take. So the solution spaces presented in tables 1 and 2 were evaluated for the most probable velocities (velocities that are very close to the mean velocities). It is clearly seen that the robots , especially robot 1 may tend to decelerate a bit higher if the solution space obtained by enlarging the radius is followed.  and (c) illustrates the effect of not considering the effect of motion uncertainty while computing collision free velocities, it can be observed from the number of colliding samples(black) that for k=1,the PRVO constraints (6) are not satisfied with a lower bound as 0.5, while (d) and(e) illustrate the effect of considering motion uncertainty, it can be seen here that for k=1, the PRVO constraints (6) are satisfied with atleast probability 0.5 9 Conclusions, Limitations and Future Work

Conclusions
In this paper, we have presented the probabilistic variant of RVO, defined as chance constraints 6 over the inequalities defined by the deterministic RVO. These chance constraints are generally computationally intractable,and the consideration of ego and estimation uncertainties further increases its complexity.This paper attempts to approximate such a chance constraint through a family of surrogate constraints 10 that have a closed form characterization of their solution space. Further a closed form mapping based on cantelli's inequalities is provided that maps the solution space of these surrogates to the probability of the chance constraint being satisfied.

Limitations
The computed avoidance maneuvers are piece-wise straight line trajectories with no velocity continuity or acceleration bounds. For practical implementation, these needs to be incorporated. The cantelli's inequality 11 represented here can act as a weak bound at times. Formulation of an efficient scheme for mapping the solution space of the surrogate functions to the probability of the chance constraint being satisfied is very important and would form the main crux of our future work.