. However, collision avoidance in the presence of uncertainty of such systems has been sparsely reported. All the more so, when state and control noise (that includes velocity and actuator noise) are characterized by non-parametric models, which is typically encountered in real-world and practical scenarios. Recently[13, 8] details collision avoidance, as well as trajectory tracking under the duress of non-parametric noise but wherein the kinematic evolution can be expressed in the form of linear models. In this paper, we propose a formulation to kinematically constrained systems wherein the state evolves according to the popular unicycle kinematics .
This paper proposes a novel MPC formulation that outputs actuator controls in the form of changes in linear and angular velocities when subject to non-parametric state, velocity, control and lane boundary noise of both the ego agent and other dynamic/static participants. The MPC formulation takes the form of matching the moments of a certain desired collision cone distribution (whose samples are all collision-free) and the current distribution of the ego agent’s collision cone vis-a-vis other dynamic participants. We also define a desired distribution for velocity samples that avoid the lane boundaries , the MPC formulation also tries to match the moments of this distribution to the current distribution due to ego agent’s current state. By collision cone, we indicate the analytical counterpart of the popular velocity obstacle that has been successfully employed in many works [1, 18, 6]. The collision cone (2) distribution is non-parametric as well since it is composed of agent and obstacle state and velocities whose distributions are non-parametric. The matching of higher-order moments of two non-parametric distributions can be accomplished by embedding these distributions into the Reproducing Kernel Hilbert Space (RKHS) [14, 15], as shown in Figure 1 through the Kernel trick. Those actuator controls that minimize this distance in the RKHS space are selected by the proposed MPC formulation from a set of feasible controls.
In particular the paper contributes in the following manner
It proposes a novel method utilizing RKHS based collision avoidance  for kinematically constrained robots/agents whose state evolution can be modelled by unicycle kinematics under the duress of non-parametric noise. Typically the dynamic obstacle avoidance problem is solved for a holonomic robot and then a non-holonomic controller is used to best track this trajectory. We circumvent this by sampling achievable velocities in the space of the non holonomic robot, and then choosing an optimum sample.
We also provide a framework for collision avoidance in a realistic setting by bringing in lane/corridor constraints with non parametric noise which provide boundaries to the robots configuration space as typical work-spaces are always bounded. We formulate lane keeping also into a distribution matching problem similar to our obstacle avoidance problem stated above bringing in a unified approach to deal with non parametric uncertainty.
Moreover, the RKHS formulation provides for an easily tunable parameter through which a trade-off between cost and robustness is possible. We showcase this through ablations on this parameter in the results section.
Favourable comparisons in terms of trajectory length, control costs as well as qualitative evaluations vis-a-vis many dynamic participants vindicate the efficacy of the proposed framework in Sections V-C and V-D respectively. It is to be noted that performance gain over other distribution matching frameworks such as KLD was established in our previous works . The proposed method also compares favourably against two variants which approximates the non-parametric noise through Gaussian models, one which linearizes the Gaussian approximations of state and velocities of all participants to obtain linearized collision cone distribution, the other which does not subject the Gaussian models to such linearizations. Both these variants are posed as Chance Constraint Optimization along the lines reported in [7, 6]. We further also showcase real-time performance in the control of a non holonomic robot in a realistic gazebo setting under the duress of non parametric noise.
Ii Related Work
While literature abounds in collision avoidance formulations of dynamic obstacles from both single-agent and multi-agent standpoints [5, 18] the literature is sparse when it comes to collision avoidance under uncertainty. The trade-off comes in the form of just avoiding the mean evolution of dynamic obstacles with heightened collision chances vis-a-vis avoiding the entire uncertainty distribution that gives rise to highly conservative maneuvers. Earlier methods tended to err on the side of caution by growing obstacles by the size of the uncertainty, often referred to as bounding volume methods [9, 17]. However, the limitations of such approaches were exemplified in  where it was shown chance-constrained formulations offer tunable trade-offs.
Posing the problem as a robust MPC with probabilistic chance constraints are by nature intractable. The primary difficulty lies in computing the analytical form for the chance constraints. Notable exceptions exist in the case in which the random variables under consideration have Gaussian distribution and the chance constraints are defined over affine inequalities[3, 2]. Formulations along the lines of [12, 11, 6] have developed reliable surrogates that give tight approximations to these chance constraints defined over non-linear inequalities(could be non-convex also). However, all these algorithms require a fundamental assumption on the nature of uncertainty of the random variables (state and actuation of the robot) involved. In general closed-form, surrogates can only be derived if the random variables belong to a Gaussian distribution. Extensions to the non-Gaussian and non-parametric case are very complex and a very active field of research.
The challenge of robust MPC in the presence of non-parametric uncertainty for collision avoidance systems was first approached in  wherein an RKHS embedding of moment matching cost was folded into the original MPC cost function. These approaches were inspired by the formulations of [14, 15] and showcased their efficacy on systems whose state evolution was linear with respect to the current state.
In this paper, we represent the vectors in bold letters,x, matrices in capital, , and sets in mathcal, . denotes the Euclidean norm of x. For a random variable x, denotes the mean.
denotes the probability of an event whiledenotes the probability density function. The subscript indicates the value at time while the superscript indicates the robot or obstacle. Some of the commonly used symbols and notations are summarized in the table I. We also define some notations in the first place of their use.
|(Position, Velocity) of the robot at time|
|(Heading, Angular velocity) of the robot at time|
|(Position, Velocity) of the obstacle at time|
|Control input to the robot at time|
|Deterministic collision avoidance constraint|
|Distribution of under uncertainty|
|Probability of satisfaction of the chance constraint|
Iii-a Robot State Estimation
The state estimates of the robot at timeis given by . We model and
as random variables following a non parametric probability distribution. We define the state evolution over time in (1).
where, is a random variable described by an unknown non-parametric probability distribution and denotes the uncertainty in executing the given control. is the magnitude of the velocity of the robot at time while is the angular velocity. is the heading of the robot at time . Though the distribution of is unknown, we assume that we have access to the samples of at every instance of . We apply a particle filter-based approach on (1) to propagate the noise in time space.
Iii-B Obstacle State Estimation
We assume that we have access to the state estimates of the obstacles via a sensor like a camera or a LiDAR. The sensors are prone to noise and hence is also a random variable described by an unknown non-parametric probability distribution. We estimate using another particle filter to track the obstacles.
Iii-C Collision Avoidance Chance Constraints
Iii-C1 Collision avoidance
Consider the ego and obstacle modeled as circular disks with radii and respectively. We express the collision avoidance constraint at a given time based on the velocity obstacle method  in (2).
Iii-C2 Chance Constraints
According the state estimations described in sections III-A and III-B, the positions and velocities of the robot and obstacles are random variables described by unknown probability distributions. Hence, the collision avoidance constraints described in (2) should be satisfied in a probabilistic manner given by (3).
Ie. a control u is required which moves the distribution to the left of zero. Figure 2 shows how the shape of the distribution can be manipulated by u to obtain a desired distributional shift.
Iii-D Corridor/lane Constraints
Iii-D1 Keeping within the corridor
The corridor is defined as two linear boundaries within which the robot has to be in at all times. The corridor is defined as 2 lines
the collision avoidance condition at time t to avoid collision with the corridor boundary is defined as:
Iii-D2 Chance Constraints for corridor
Iii-E Problem Formulation
We formulate the collision avoidance problem as an optimization problem. For every obstacle, we satisfy the chance constraints while optimizing a trajectory following cost.
where, is some desired velocity that the robot needs to attain at time and is the set of feasible control commands.
We now present the method to sample the set of feasible controls, , for a non-holonomic robot. We also describe a method to solve the chance constraints presented in (3). In general, the chance constraint of the form (8b) is intractable and is replaced by surrogate constraints. For non-parametric state and control noise, we model the surrogate constraint as distribution matching problem , . From this, we reformulate (8) as
where, is the weight for , is a metric of similarity between the two given distributions, and . is a desired distribution which avoids the collision with . Here, we fold the chance constraint into the main cost function and manifest a surrogate distribution matching cost , . It should be noted that we do not have access the parametric form of the distributions of and but we assume access to the samples of the same. We elaborate on the desired distribution in section IV-A. We describe the method to sample the feasible set of controls in section IV-B.
Iv-a Desired Distribution
A control input that can shape the distribution of constraints to have an appropriate shape, e.g, appropriate mass to the left of zero as shown in the Figure 2 is a solution of the chance constrained optimization. The solution distribution should be as similar as possible to the desired distribution in aspects such as the similarity around the tail of the distribution. To put it more simply, the desired distribution is an estimate of how the solution distribution might look like. Thus, given a feasible chance-constrained optimization there always exists a desired distribution. However, the essential question one should ask is whether we can always construct such a desired distribution. The construction of the desired distribution is equivalent to solving the following deterministic optimization problem for .
The different computed for obtained from (10a)-(10e) are precisely the samples of the desired distribution . A desired distribution can be constructed as long as we can solve (10a)-(10e). In other words, the construction guarantee of the desired distribution is tied to the existence guarantee of solution of (10a)-(10e). Now, since represents the velocity obstacle function, (10a)-(10e) represents a non-convex optimization problem for which absolute solution guarantees are intractable to obtain. Thus, instead we focus on the conditions under which a solution to (10a)-(10e) is likely to be obtained.
Assumption 1: A exists such that , , and .
Assumption 1 essentially means that a collision avoidance control can be obtained if we disregard the uncertainty and just consider the mean. This is a fair assumption since velocity obstacle is indeed known to produce collision-free motion in the deterministic case with a very high success rate . Now, the solution computed with respect to the mean would always result in some non-zero probability. For example, if the uncertainty is Gaussian, the control input computed based on the mean of uncertainty will lead to probability of collision avoidance. This, in turn, implies that there would always be samples around the mean that will be collision-free with respect to the control input computed considering just the mean of the uncertainty. Thus, it is likely that if we choose samples , within a ball of radius of the mean uncertainty, we would obtain a solution for (10a)-(10e) and hence can construct the desired distribution.
The desired distribution can be constructed as long as the solution to the deterministic counterpart of (8) can be obtained by solely the deterministic version of MPC without the surrogate cost. Further, the small perturbations around the mean state and control are considered and solved for. As long as a solution exists to the mean MPC and perturbations around the mean, a desired distribution can be obtained. The distribution of , and are all functions of the state of the robot and obstacle as well as the control input to the robot, which are all, in turn, random variables.
Iv-B Sampling feasible controls
We solve the optimization problem in (8) by sampling a set of feasible control commands and choosing the best control from the sampled set. The sampling approach makes solving (8) independent of the motion model of the robot. For simplicity, we sample a set of feasible controls using a differential drive motion model. Algorithm 1 describes how the feasible sample set is populated.
Essentially, we take steps between the bounds on the magnitude of angular velocities and the bounds of the magnitude of linear velocities to get samples of linear velocities and samples of angular velocities. All combinations of the sampled linear and angular velocities are used to generate the set of feasible controls . The number of steps is chosen to ensure adequate coverage of the sample space of velocities that the ego vehicle can take.
Iv-C Solving Collision Avoidance Chance Constraints
Iv-C1 Distribution matching
A probability distribution is defined by its moments. From , for given random variables x and y, the distributions of x and y become more similar as the higher-order moments become similar.
where, refers to the order up to which the moments of and are similar. To the best of our knowledge, there is no method to deduce the exact mapping between the similarity of the distribution and order of moment to match starting from the first moment. Taking the concepts from ,  formulates a workaround using the concept of embedding distributions in Reproducing Kernel Hilbert Space (RKHS) and Maximum Mean Discrepancy (MMD) distance.
where, and are number of samples from respective distributions of the random variables x and y. represents a polynomial kernel of degree . and are the weight vectors corresponding to and . For samples, and take the values given in (13).
Iv-C2 Collision Avoidance
Algorithm 2 describes the procedure to obtain an optimal control, , that avoids the obstacles under uncertainty at time .
Essentially, we calculate the distributions , and for each sample control in the feasible sample set for all noise samples. Then the desired distributions , , and are calculated. Finally we calculate the cost defined in (9) as and find the optimal control as which yields the minimum cost .
V Results and Discussion
All simulations were carried out on a commodity laptop with MATLAB as the simulation engine. The robot’s state was evolved according to unicycle kinematics. Non-Gaussian and at times multi-model distributions were used to model the robot’s state, velocity and control noise and the state and velocity noise of the dynamic obstacles and participants.More details about the noise distributions can be found here https://robotics.iiit.ac.in/publications/2020/non-holonomic-collision-avoidance-hilbert-space.html. Twenty-five samples for robot and obstacle each were used to model the uncertainty in the state, velocity and actuator controls where the samples were drawn from a Pearson distribution.
V-a In-feasibility of an optimization based approach
Figure 3 shows that the cost function surface has many kinks and valleys where a gradient based optimisation technique may get stuck or cause delays in the time to converge or may even converge to a sub-optimal solution 3. Instead by sampling in the space we are able to converge very close to an optimal solution and are able to maintain tight bounds on the frequency of control output which is very important for realtime control.
V-B Qualitative Results
Figure 4 shows the various stages in an obstacle avoidance in the presence of five obstacles. For every snapshot where we are performing an avoidance manoeuvre we show below it the desired distribution of collision cones in black and the current distribution in blue for the closest obstacle. When a majority of samples of the robot are colliding with samples of the obstacles this distribution tends to be on the right side of the co-ordinate axis. Progressively as the obstacle gets avoided the collision cone distribution moves to the left of the axis and as can be seen in Figure 4 coincides largely in its moments (such as the mean) with the desired distribution. Also shown in the top part of the distribution plots, is a horizontal bar whose length shows the number of samples of the obstacle distribution that are avoided. By showcasing the ability of the algorithm where many samples of the ego robot is able to avoid many samples of the obstacles we verify its efficiency. Figure 6 shows the how we can change the behaviour of the avoidance maneuver with changes in the tunable parameter in the presence of static obstacles and lanes/corridors. With we are able to pass into tight spaces while prioritises safer maneuvers and more avoidance. In these figures the obstacle is shown in a shade of orange, the robot in blue; the mean sample is shown dense in their respective colours whereas the other samples are shown by the circles whose interiors are unfilled. We also show the proposed method working in a real-time simulation in gazebo in the presence of lane/corridor constraints and stationary and dynamic obstacles at https://robotics.iiit.ac.in/publications/2020/non-holonomic-collision-avoidance-hilbert-space.html.
V-C Ablation Studies
As the RKHS hyper-parameter increases higher order moments of the distributions strive to match each other as shown in 4(d), 4(e), 4(f). This results in enhanced collision avoidance at the cost of increasing trend in conservativeness of the maneuver. We show this for a robot avoiding single obstacle in Figures 4(a) 4(b) 4(c). Figure 4(a) depicts the scenario for where the agent gets close enough to the obstacle and not all samples of the obstacles are avoided. As increases to , more samples of the obstacles are avoided by the robot samples as seen in Figures 4(b), 4(c). However the conservativeness of the maneuver depicted in terms of deviation from the optimal path (straight line) also increases with increasing as shown in the bar chart of Figures 7. We further compare with the following 2 formulations. In the first one the collision cone distribution is obtained by further linearization of the Gaussian distributions 
. In the second one the mean and variance are derived from the Gaussian approximation of the noise. Then we construct the surrogate chance constraint along the lines of[7, 6]. We consistently perform better than them with respect to both the control cost and the deviation in 7.
V-D Quantitative Comparisons
In this section, we compare the proposed RKHS formulation vis a vis with a chance constrained linearized formulation which approximates the non-parametric distribution by Gaussian models as described in Section V-C. The comparisons are shown in the bar plots of Figure 7(b) which compares over deviations in trajectory length from the optimal trajectory whereas Figure 7(a) compares over the L2 norm of control or actuator costs. The bar coloured in blue in each bar plot represents the proposed formulation for , while the bar coloured in orange is for the method of linearization. Figures 7(a), 7(b) and 7(c) show the respective comparisons for the cases of one, two and three obstacles between the proposed method and the linearized method.
It is evident from these plots that the RKHS formulation outperforms our baseline on both trajectory deviations and control costs. Additional results based on the proposed method can be found at https://robotics.iiit.ac.in/publications/2020/non-holonomic-collision-avoidance-hilbert-space.html.
This paper proposed a novel formulation based on Reproducing Kernel Hilbert Space embedding of non-parametric agent and obstacle distributions and gainfully employed it to solve the problem of the ego agent avoiding several dynamic obstacles under the duress of non-parametric state, velocity, lane boundary, and actuator noise. The formulation was validated for a kinematically constrained agent whose state evolves according to unicycle kinematics. Performance gain across various parameters such as trajectory length and time were shown with respect to two formulations first where the collision cone distribution is obtained by further linearization of the Gaussian distributions with respect to state and velocity variables and the second method where the collision cone distribution is not analytically computed but its mean and variance are derived from the Gaussian approximation which is then used to construct the surrogate chance constraint. To the best of the authors’ knowledge, this is the first such paper to have attacked this problem.
-  (2014) Shared control of autonomous vehicles based on velocity space optimization. In 2014 IEEE International Conference on Robotics and Automation (ICRA), pp. 1639–1645. Cited by: §I, §I.
-  (2010) A probabilistic particle-control approximation of chance-constrained stochastic predictive control. IEEE transactions on Robotics 26 (3), pp. 502–517. Cited by: §II.
-  (2008) Stochastic programming. Lecture Notes, Stanford University. Cited by: §II.
-  (1998) Feedback control of a nonholonomic car-like robot. In Robot motion, planning and control, J.-P. Laumond (Ed.), Lecture Notes in Control and Information Sciences 229. Cited by: §I.
-  (1998) Motion planning in dynamic environments using velocity obstacles. The International Journal of Robotics Research 17 (7), pp. 760–772. Cited by: §I, §II.
-  (2017) PRVO: probabilistic reciprocal velocity obstacle for multi robot navigation under uncertainty. In Intelligent Robots and Systems (IROS), 2017 IEEE/RSJ International Conference on, pp. 1089–1096. Cited by: §I, §I, §II, §II, §V-C.
-  (2015) 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. Cited by: §I, §V-C.
-  (2018) Solving chance constrained optimization under non-parametric uncertainty through hilbert space embedding. External Links: Cited by: §I, §IV, §IV.
-  (2012) Multi-robot collision avoidance with localization uncertainty. In Proceedings of the 11th International Conference on Autonomous Agents and Multiagent Systems-Volume 1, pp. 147–154. Cited by: §II.
-  (2000) Moments determine the tail of a distribution (but not much else). The American Statistician 54 (4), pp. 248–251. External Links: Cited by: §IV-C1.
-  (2010) Chance constrained rrt for probabilistic robustness to environmental uncertainty. In AIAA guidance, navigation, and control conference, pp. 8160. Cited by: §II.
-  (2014-06) Stochastic nonlinear model predictive control with probabilistic constraints. pp. 2413–2419. External Links: Cited by: §II.
-  (2020-04) Reactive navigation under non-parametric uncertainty through hilbert space embedding of probabilistic velocity obstacles. IEEE Robotics and Automation Letters 5 (2), pp. 2690–2697. External Links: Cited by: item 1, §I, §I, §II, §IV-C1, §IV, §IV.
-  (2015) Computing functions of random variables via reproducing kernel hilbert space representations. Statistics and Computing 25 (4), pp. 755–766. Cited by: §I, §II.
-  (2016) Consistent kernel mean estimation for functions of random variables. In Advances in Neural Information Processing Systems, pp. 1732–1740. Cited by: §I, §II.
-  (2016) Consistent kernel mean estimation for functions of random variables. In Advances in Neural Information Processing Systems 29, D. D. Lee, M. Sugiyama, U. V. Luxburg, I. Guyon, and R. Garnett (Eds.), pp. 1732–1740. Cited by: §IV-C1.
-  (2011) The hybrid reciprocal velocity obstacle. IEEE Transactions on Robotics 27 (4), pp. 696–706. Cited by: §II.
-  (2008-05) Reciprocal velocity obstacles for real-time multi-agent navigation. In 2008 IEEE International Conference on Robotics and Automation, Vol. , pp. 1928–1935. External Links: Cited by: §I, §II, §IV-A.
-  (2009) Generalized velocity obstacles. In 2009 IEEE/RSJ International Conference on Intelligent Robots and Systems, pp. 5573–5578. Cited by: §I.
-  (2019-04) Chance-constrained collision avoidance for mavs in dynamic environments. IEEE Robotics and Automation Letters 4 (2), pp. 776–783. External Links: Cited by: §V-C.