arXiv:1608.05829v1 [cs.RO] 20 Aug 2016  Chance constraint based multi agent navigation under uncertainty  Bharath Gopalakrishnan 1 , Arun Kumar Singh 2 , Meha Kaushik 1 , K. Madhava Krishna 1   and Dinesh Manocha 3   ⋆⋆⋆⋆ ⋆ ⋆   †  Abstract.   We present Probabilistic Reciprocal Velocity Obstacle or PRVO as a general algorithm for navigating multiple robots under per- ception and motion uncertainty. PRVO is defined as the space of veloc- ities 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 com- putationally 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 sim- ulations where we highlight the importance of incorporating the effect of motion uncertainty and the advantages of PRVO over existing for- mulations which handles the effect of uncertainty by using conservative bounding volumes.  1   Introduction  Ensuring collision free navigation of multiple robots between given start and goal positions is an important problem in robotics, swarm and crowd simulation. One commonly used approach to solve this problem performs local collision avoidance between multiple robots using velocity obstacles (VO) [1], [2], [3]. This involves repeatedly solving a set of non-convex inequalities to characterize the space of collision free velocities available to each robot at a given instant. Some faster techniques use a conservative formulation and reduce the local velocity computation to a linear programming problem [4]. Most algorithms for multi-robot navigation focus primarily on the determin- istic setting where it is assumed that the robot can perfectly estimate the states  ⋆   The research was supported  ⋆⋆   1   RRC, IIIT-Hyderabad  ⋆ ⋆ ⋆   2   NTU, Singapore  †   3   University of North Carolina, Chappel Hill
of the neighbouring robots and execute the computed avoidance maneuver with- out 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 avoid- ance velocities. As one would expect, the complexity of the probabilistic variant tends to be much higher than the deterministic counterpart, as now the reason- ing 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 avoid- ance which explicitly takes into account, the perception and motion uncertainty of each robot. Our approach combines ideas from velocity obstacle based multi- robot 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 uncer- tainty [13]. These chance constraints ensure that the probability of a constraint being satisfied is greater than a specified threshold. In our approach, we for- mulate chance constraints over the conditions defined by the deterministic Re- ciprocal 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 com- plex 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 con- straints are satisfied. Finally, a time scaling based methodology can be adopted to efficiently solve the surrogate constraints. In this paper, we extend the ap- proach 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.  1.1   Summary of Main Results  On the computational side, we show that the complexity of the PRVO is com- parable 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 associ- ated 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.  1.2   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 in- troduces 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 mo- tivations 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  2   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.  2.1   Uncertainty Model  Our methodology of modeling uncertainty in the system is similar to that pre- sented 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 treat- ing 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. How- ever, it does not consider the uncertainty in the error between the computed and executed avoidance maneuver,or in short the actuation uncertainty.  2.2   Dynamic Collision Avoidance Constraints under Uncertainty  At a conceptual level, our modeling of dynamic collision avoidance under un- certainty 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 col- lision 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 colli- sion 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.  3   Symbols and Notations  We used bold faced small case letters with superscripts to describe vectors asso- ciated 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 sym- bol   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.  4   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
 ̇ x i   =   v i x   ⇒   x i ( t   +   ∆t ) =   x i ( t ) +   v i x ∆t,    ̇ y i   =   v i y   ⇒   y i ( t   +   ∆t ) =   y i ( t ) +   v i y   ∆t   (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].  f   RV O i j   ( p j   ,   v j   )   ≥   0   (2)  f   RV O i j   ( p j   ,   v j   ) =   ‖ r ij   ‖ 2   −   ( r ij   ) T   (2 v i rvo   −   v i   −   v j   )  ‖ 2 v i rvo   −   v i   −   v j   ‖ 2   −   R 2  It is straightforward to observed that (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.  5   RVO in the Probabilistic Domain  Let us start by representing the current trajectory of each robot as the following random variables.  p i   ≈   N   ( μ i  p ,   ( σ i  p ) 2 ) ,   p j   ≈   N   ( μ j  p ,   ( σ j  p ) 2 )   (3)  v i   ≈   N   ( μ i  v ,   ( σ i  v ) 2 ) ,   v j   ≈   N   ( μ j  v ,   ( σ j  v ) 2 ) ,   (4) where   μ i  p ,   ( σ i  p ) 2 , μ i  v ,   ( σ i  v ) 2   and similarly others represents position and veloc- ity 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 ap- proach 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  v i rvo   ≈   N   ( v i rvo ,   ( σ i  v rvo   ) 2 ) ,   (5) 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 constant ( σ i  v rvo   ) 2  Now, in light of definitions (3)-(5),   f   RV O i j   becomes a multivariate function of random variables and thus, consequently a random variable itself. Thus, math- ematically, (2) does not make sense. Instead, a more well defined alternative would be to consider the following inequality  P   ( f   RV O i j   ( p j   ,   v j   )   ≥   0)   ≥   η.   (6) Constraint (6) ensures that the probability of RVO based collision avoidance condition (2) being satisfied is greater than some lower bound   η . It is straight- forward 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, effi- cient 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   .  5.1   Expectation and Variance of   f   RV O i j   ( . )  Expectation of a multivariate function   g   in terms of variables   z 1 , z 2 ...z n   is given by the following expression.
E [ g ( z 1 , z 2 ...z n )] =   (7)  ∫   ∞ −∞  ...  ∫   ∞ −∞  g ( z 1 , z 2 ...z n ) h ( z 1 , z 2 ...z n ) dz 1 dz 2 ..dz n  Using (7), the expectation of   f   RV O i j   ( . ) can be obtained in the following man- ner.  E [ f   RV O i j   ] =   μ f   RV Oi j   =  ∫   ∞ −∞  ...  ∫   ∞ −∞  f   RV O i j   ( . ) P   ( . ) dx i dy i dx j   dy j   dv xi dv y i dv xj   dv y j   d ( v i rvo ) x , d ( v i rvo ) y  (8)  In (8)   P   ( . ) represents the joint probability distribution of the random vari- ables ( x i , y i , x j   , y j   , v xi , v y i , v xj   , v y j   ,   ( v i rvo ) x ,   ( v i rvo ) y   ). Integral (8) can be com- puted symbolically using packages like MATHEMATICA [21] and can be even- tually 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 follow- ing manner. The right hand side of equality (9), when computed symbolically, reduces to a fourth order polynomial in terms of   v i rvo  ( σ f   RV Oi j   ) 2   =   E [( f   RV O i j   −   E [ f   RV O i j   ]) 2 ]   (9)  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 .  5.2   Approximations for PRVO  Using (8) and (9), we can replace (6) with the following family surrogate con- straints.  μ f RV Oi j  −   kσ f RV Oi j  ≥   0 ,   k   ≥   0   (10) As shown in our earlier work [14], inequality (10) represents a strip of width  μ f RV Oi j  −   kσ f RV Oi j  from the distribution of   f RV O i j   . Thus, solving (10) with in- creasing 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 con- straints (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.  η   ≥   k 2  1 +   k 2   (11)
6   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).  6.1   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  v ( τ   ) =   v ( t ) s,   s   =   dt  dτ   (12) 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  s v i rvo   =   s v i   (13) Substituting (13) into (8) and (9) results in the following time scaled variant of the expectation and variance of   f   RV O i j   .  μ s   f   RV Oi j   =   as 2   +   bs   +   c   (14)  σ s   f   RV Oi j   =  √  E [( s f   RV O i j   −   E [ s f   RV O i j   ]) 2 ] =   √ ds 4   +   es 3   +   f s 2   +   gs   +   h   (15) Where,   a ( . ) , b ( . ) ...h ( . ) are function of parameters of distribution of random variables i. e,   μ i  p ,   ( σ i  p ) 2 , μ j  p ,   ( σ j  p ) 2 , μ i  v ,   ( σ i  v ) 2   etc. Please note the additional super- script ” s ” representing that we are now dealing a time scaled variant of   μ f RV Oi j  and   σ f RV Oi j  derived in (8) and (9) Now, we perform one final simplification of (15) to obtain quadratic approx- imation based on second order Taylor series expansion around a point   s ∗  σ s   f   RV Oi j   =   σ s   ∗ f   RV Oi j   +   σ ′  s   ∗ f   RV Oi j  ( s   −   s ∗ ) +   σ ′′  s   ∗ f   RV Oi j  ( s ∗   −   s ∗ ) 2  2   (16)
Using (14) and (16), the final form of the time scaled variant of (10) can be obtained in the following manner.  as 2   +   bs   +   c   −   k ( σ s   ∗   f   RV Oi j   +   σ ′  s   ∗   f   RV Oi j  ( s   −   s ∗ ) +   σ ′′  s   ∗   f   RV Oi j  ( s   −   s ∗ ) 2  2   )   ≥   0   (17) 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 Oi j   ≥   0 [14]. It is easy to note that solving it is similar to solving (17).  7   Analysis of PRVO  7.1   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 con- straints (10) which approximates the PRVO constraints (6) is a quartic inequal- ity. Thus, on the surface PRVO seems significantly more complicated than RVO. However, the time scaling based reformulations discussed in section 6 does pro- vide a significant simplification. Essentially as explained in section 6, solving the surrogate constraints and consequently PRVO has been reduced to gener- ating 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.  7.2   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.  f   ORCA i j   =   z 1 v i orca   −   z 2   ≥   0   (18)
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]  P   ( f   ORCA i j   ≥   0)   ≥   η,   z 1   ≈   N   ( μ z 1   , σ z 1   )   (19)  ⇒   z 1 v i orca   −   z 2   −   η   1  2   (( v i orca ) 2   −   2 z 2 μ z 1   +   z 2 2   )   1  2   ≥   0   (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 con- straints 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.  8   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 proba- bility   η . (2) Showing the importance of incorporating the motion uncertainty. (3) Comparing solution space computed through PRVO with that obtained from [16] and similar approaches.  8.1   Validating Mapping between the Surrogate Constraints and the PRVO constraints  Figure 1(a) shows a collision scenario with three robots. In line with the time scaling based methodology described in section 6, each robot generates multiple candidate trajectories (figure 1(b)) and then solves time scaled variant of the surrogate constraints (17) along them. Depending on the value of   k , a particular candidate trajectory and its corresponding time scale is chosen. Figures, 2(a)- 3(c) summarizes these results. Consider, figure 2(a)-2(c), where each robot solves (10) for   k   = 1. According to Canetlli’s based bounds, discussed in section 5.2, this would mean, that PRVO constraints (6) should be satisfied with atleast probability 0 . 5. We validate this by sampling position and velocity samples from the uncertainty ellipses of the robots and evaluating what percentage of these samples satisfy the deterministic RVO constraints (2). As shown in the figures, the minimum probability observed agrees with Cantelli’s bounds. Figures 3(a)- 3(c) solves (10) for   k   = 1 . 5 and as shown, the minimum probability with which
0   5   10   15   20  0  5  10  15  20  x[m]  y[m]  Robo2  Robo3  Robo1  Uncertainty ellipse  (a)  0   5   10   15   20  −5  0  5  10  15  20  x[m]  y[m]  Various candidate trajectories of robo 3  Various candidate trajectories of robo 1  Various candidate trajectories of robo 2  (b)  Fig. 1.   (a): Collision scenario involving three robots. (b). For computing avoidance maneuvers, each robot generates various candidate trajectories and then solves the time scaled variant of the surrogate constraints (17) along them. Computing an avoidance maneuver would essentially mean choosing a particular candidate trajectory and a corresponding time scale for it.  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.  8.2   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 ig- nored, 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.
9   9.5   10   10.5   11   11.5   12  7  8  9  10  11  12  x[m]  y[m]  case for k=1, minimum 50% avoiding, P(f RVO 3 1 >0)>0.5,and P(f RVO 2 1 >0)>0.5  Colliding samples (black)  Robo 1  Robo 3  Non−Colliding samples (Red)  Cand.traj 5  Robo 2  (a)  8.5   9   9.5   10   10.5   11   11.5   12  7  8  9  10  11  12  x[m]  y[m]  case for k=1, minimum 50% avoiding, P(f RVO 3 2 >0)>0.5 and P(f RVO 1 2 >0)>0.5  Colliding samples (black)  Robo 3  Non−Colliding samples (Red)  Cand.traj 6  Robo 1  Robo 2  (b)  9   9.5   10   10.5   11   11.5   12   12.5  7  7.5  8  8.5  9  9.5  10  10.5  11  11.5  x[m]  y[m]  Robo 2  Robo 1  Colliding samples (black) Non−Colliding samples (Red)  Cand.traj 5  Robo 3 case for k=1, minimum 50% avoiding, P(f RVO 2 3 >0)>0.5 and P(f RVO 1 3 >0)>0.5  (c)  Fig. 2.   (a): Each robot chooses a candidate trajectory and then solves the time scaled variant of the surrogate constraints (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 .  9   9.5   10   10.5   11   11.5   12  7  8  9  10  11  12  x[m]  y[m]   Colliding samples (black)  Non−Colliding samples (Red)  case for k=1.5, minimum 70% avoiding, P(f RVO 2 1 >0)>0.7 and P(f RVO 3 1 >0)>0.7  Robo 1  Cand.traj 4  Robo 3  Robo 2  (a)  8.5   9   9.5   10   10.5   11   11.5   12  7  7.5  8  8.5  9  9.5  10  10.5  11  11.5  x[m]  y[m]  Cand traj 5  Robo 3  case for k=1.5, minimum 70% avoiding, P(f RVO 1 2 >0)>0.7 and P(f RVO 3 2 >0)>0.7  Robo 1  Robo 2  Colliding samples (black)  Non−Colliding samples (Red)  (b)  9   9.5   10   10.5   11   11.5   12   12.5  7  7.5  8  8.5  9  9.5  10  10.5  11  11.5  x[m]  y[m]  case for k=1.5, minimum 70% avoiding, P(f RVO 1 3 >0)>0.7 and P(f RVO 2 3 >0)>0.7  Colliding samples (black)  Non−Colliding samples (Red)  Robo 2  Robo 1  Cand traj4 (robo3)  (c)  Fig. 3.   (a)-(c): Results are same as figure 2(a)-2(c), but are now obtained for   k   = 1 . 5. With this value of   k , the probability with which (6) should get satisfied for each robot should be 0.70, which as shown in figure is indeed the case. The validation was done through sampling procedure similar to figure 2.  8.3   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]
0   5   10   15   20  8  10  12  14  16  18  20  x[m]  y[m]  Original trajectory Robo1  Cand. Traj 2(k=1.8, 77% confidence)  Cand. Traj 4(k=1.5, 69% confidence)  Cand. Traj 5(k=1, 50% confidence)  (a)  0   2   4   6   8   10   12   14  0  5  10  15  20  x[m]  y[m]  Cand. Traj 6(k=1, 50% confidence)  Cand. Traj 5(k=1.5, 69% confidence)  Cand. Traj 3(k=1.8, 77% confidence)  Original trajectory Robo2  (b)  8   10   12   14   16   18   20  −5  0  5  10  15  x[m]  y[m]   Cand. Traj 6(k=1, 50% confidence)  Cand. Traj 5(k=1.5, 69% confidence)  Cand. Traj 3(k=1.8, 77% confidence)  Original trajectory Robo3  (c)  Fig. 4.   (a)-(c) Collision avoidance maneuvers for various values of   k   or in other various probabilities   η   of PRVO constraints (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.  Table 1.   Comparison of the solution space obtained from the proposed formulation and that by the method of enlarging the robot’s radius for agent 1  Formulation   68 % contour   80 % contour  Expanding the radius by a desired confidence contour   [0 0 . 59]   [0 0 . 4]  Solution space of surrogate functions   [0 0 . 67]   [0 0 . 5]  Table 2.   Comparison of the solution space obtained from the proposed formulation and that by the method of enlarging the robot’s radius for agent 2  Formulation   68 % contour   80 % contour  Expanding the radius by a desired confidence contour   [0 0 . 35]   ∪   [1 . 1   ∞ )   [0 0 . 23]   ∪   [1 . 1   ∞ )  Solution space of surrogate functions   [0 0 . 37]   ∪   [1 . 1   ∞ )   [0 0 . 27]   ∪   [1 . 1   ∞ )
0   5   10   15   20  0  5  10  15  20  x[m]  y[m]  Robo2  Robo1  Uncertainty ellipse(grey)  Uncertainty ellipse(grey)  (a)  9   9.5   10   10.5   11   11.5   12  10  10.5  11  11.5  x[m]  y[m]  Non−Colliding samples (Red)  Robo 1 Robo 2  Colliding samples (black)  Cand.traj 6, s=0.8  case for k=1, P(f RVO 2 1 >0)<0.5, When we do not account into robo1’s uncertainty  (b)  8.5   9   9.5   10   10.5   11   11.5   12  9.5  10  10.5  11  11.5  x[m]  y[m]  Robo 2  Cand. traj 6, s=1   Robo 1  Colliding samples (black)  case for k=1, P(f RVO 1 2 >0)<0.5, When we do not account into robo2’s uncertainty  Non−Colliding samples (Red)  (c)  9   9.5   10   10.5   11   11.5   12  10  10.2  10.4  10.6  10.8  11  11.2  11.4  11.6  x[m]  y[m]  case for k=1, P(f RVO 2 1 >0)>0.5, When we account into robo1’s uncertainty  cand traj 5  Robo 2  Colliding samples (black)  Non−Colliding samples (Red)  Robo 1  (d)  8.5   9   9.5   10   10.5   11   11.5   12  9.5  10  10.5  11  11.5  x[m]  y[m]  Cand. traj 6, s=0.4  Colliding samples (black)  Non−Colliding samples (Red)  Robo 2  Robo 1  case for k=1, P(f RVO 1 2 >0)>0.5, When we account into robo2’s uncertainty  (e)  Fig. 5.   (a)-(e),(b) and (c) illustrates the effect of not considering the effect of motion uncertainty while computing collision free velocities, it can be observed from the num- ber 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  9.1   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 con- sideration of ego and estimation uncertainties further increases its complex- ity.This paper attempts to approximate such a chance constraint through a fam- ily 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.
9.2   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 map- ping 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.  References  1. Fiorini, Paolo, and Zvi Shiller. ”Motion planning in dynamic environments us- ing velocity obstacles.” The International Journal of Robotics Research 17, no. 7 (1998): 760-772. 2. Van den Berg, Jur, Ming Lin, and Dinesh Manocha. ”Reciprocal velocity obstacles for real-time multi-agent navigation.” In Robotics and Automation, 2008. ICRA 2008. IEEE International Conference on, pp. 1928-1935. IEEE, 2008. 3. Alonso-Mora, Javier, Andreas Breitenmoser, Martin Rufli, Paul Beardsley, and Roland   Siegwart.   ”Optimal   reciprocal   collision   avoidance   for   multiple   non- holonomic robots.” In Distributed Autonomous Robotic Systems, pp. 203-216. Springer Berlin Heidelberg, 2013. 4. Van Den Berg, Jur, Stephen J. Guy, Ming Lin, and Dinesh Manocha. ”Recipro- cal n-body collision avoidance.” In Robotics research, pp. 3-19. Springer Berlin Heidelberg, 2011. 5. Kluge, B., and Prassler, E. (2006, January). Recursive probabilistic velocity obsta- cles for reflective navigation. In Field and Service Robotics (pp. 71-79). Springer Berlin Heidelberg. 6. Kim, S., Guy, S. J., Liu, W., Wilkie, D., Lau, R. W., Lin, M. C., and Manocha, D. Predicting Pedestrian Trajectories for Robot Navigation. In Proc. of the workshop Crossing the Reality Gap: Control, Human Interaction and Cloud Technology for Multi- and Many- Robot Systems 2014 IEEE ICRA 2014. Hong Kong, China, May 31 - June 7, 2014 7. Fulgenzi, C., Tay, C., Spalanzani, A., and Laugier, C. (2008, September). Proba- bilistic navigation in dynamic environment using rapidly-exploring random trees and gaussian processes. In Proc. IEEE IROS 2008 (pp. 1056-1062). 8. Rios-Martinez,   J.,   Spalanzani,   A.,   and   Laugier,   C.   (2011).   Probabilistic   au- tonomous navigation using risk-rrt approach and models of human interaction. In Proc. IEEE IROS 2011 . 9. Kushleyev, A., and Likhachev, M. Time-bounded lattice for efficient planning in dynamic environments. In Proc. of IEEE ICRA 2009 (pp. 1662-1668). 10. Trautman, Peter, and Andreas Krause. ”Unfreezing the robot: Navigation in dense, interacting crowds.” In Intelligent Robots and Systems (IROS), 2010 IEEE/RSJ International Conference on, pp. 797-803. IEEE, 2010. 11. Aoude, Georges S., Brandon D. Luders, Joshua M. Joseph, Nicholas Roy, and Jonathan P. How. ”Probabilistically safe motion planning to avoid dynamic ob- stacles with uncertain motion patterns.” Autonomous Robots 35, no. 1 (2013): 51-76.
12. Luders, Brandon, Mangal Kothari, and Jonathan P. How. ”Chance constrained RRT for probabilistic robustness to environmental uncertainty.” In AIAA guidance, navigation, and control conference (GNC), Toronto, Canada. 2010. 13. Schildbach, Georg, Lorenzo Fagiano, Christoph Frei, and Manfred Morari. ”The scenario approach for stochastic model predictive control with bounds on closed- loop constraint violations.” Automatica 50, no. 12 (2014): 3009-3018. 14. Gopalakrishnan, Bharath, Arun Kumar Singh, and K. Madhava Krishna. ”Closed form characterization of collision free velocities and confidence bounds for non- holonomic robots in uncertain dynamic environments.” In Intelligent Robots and Systems (IROS), 2015 IEEE/RSJ International Conference on, pp. 4961-4968. IEEE, 2015. 15. Nagariya, Akhil, Bharath Gopalakrishnan, Arun Kumar Singh, Krishnam Gupta, and K. Madhava Krishna. ”Mobile robot navigation amidst humans with intents and uncertainties: A time scaled collision cone approach.” In 2015 54th IEEE Conference on Decision and Control (CDC), pp. 2773-2779. IEEE, 2015. 16. Snape, Jamie, and Dinesh Manocha. ”Goal velocity obstacles for spatial navigation of multiple virtual agents.” In Proceedings of the 2013 international conference on Autonomous agents and multi-agent systems, pp. 1191-1192. International Foun- dation for Autonomous Agents and Multiagent Systems, 2013. 17. Hennes, Daniel, Daniel Claes, Wim Meeussen, and Karl Tuyls. ”Multi-robot colli- sion avoidance with localization uncertainty.” In Proceedings of the 11th Interna- tional Conference on Autonomous Agents and Multiagent Systems-Volume 1, pp. 147-154. International Foundation for Autonomous Agents and Multiagent Sys- tems, 2012. 18. Claes, Daniel, Daniel Hennes, Karl Tuyls, and Wim Meeussen. ”Collision avoid- ance under bounded localization uncertainty.” In 2012 IEEE/RSJ International Conference on Intelligent Robots and Systems, pp. 1192-1198. IEEE, 2012. 19. Nemirovski, Arkadi. ”On safe tractable approximations of chance constraints.” European Journal of Operational Research 219, no. 3 (2012): 707-718. 20. Boyd, Stephen. ”Chance-constrained optimization.” (2015). 21. Wolfram, S. (2009). Mathematica. Wolfram Research, Champaign. 22. Snape, J., van den Berg, J., Guy, S. J., and Manocha, D. (2011). The hybrid reciprocal velocity obstacle. Robotics, IEEE Transactions on, 27(4), 696-706. 23. Gopalakrishnan, Bharath, Arun Kumar Singh, and K. Madhava Krishna. ”Time scaled collision cone based trajectory optimization approach for reactive planning in dynamic environments.” In 2014 IEEE/RSJ International Conference on Intel- ligent Robots and Systems, pp. 4169-4176. IEEE, 2014.