IVO: Inverse Velocity Obstacles for Real Time Navigation

05/04/2019
by   P. S. Naga Jyotish, et al.
IIIT Hyderabad
IIT Roorkee
0

In this paper, we present "IVO: Inverse Velocity Obstacles" an ego-centric framework that improves the real time implementation. The proposed method stems from the concept of velocity obstacle and can be applied for both single agent and multi-agent system. It focuses on computing collision free maneuvers without any knowledge or assumption on the pose and the velocity of the robot. This is primarily achieved by reformulating the velocity obstacle to adapt to an ego-centric framework. This is a significant step towards improving real time implementations of collision avoidance in dynamic environments as there is no dependency on state estimation techniques to infer the robot pose and velocity. We evaluate IVO for both single agent and multi-agent in different scenarios and show it's efficacy over the existing formulations. We also show the real time scalability of the proposed methodology.

READ FULL TEXT VIEW PDF
POST COMMENT

Comments

There are no comments yet.

Authors

page 5

11/02/2018

Toward Verifiable Real-Time Obstacle Motion Prediction for Dynamic Collision Avoidance

Next generation Unmanned Aerial Vehicles (UAVs) must reliably avoid movi...
02/26/2021

V-RVO: Decentralized Multi-Agent Collision Avoidance using Voronoi Diagrams and Reciprocal Velocity Obstacles

We present a decentralized collision avoidance method for dense environm...
04/04/2019

Intent-Aware Probabilistic Trajectory Estimation for Collision Prediction with Uncertainty Quantification

Collision prediction in a dynamic and unknown environment relies on know...
05/30/2020

A real-time multi-constraints obstacle avoidance method based on LiDAR

Obstacle avoidance is one of the essential and indispensable functions f...
12/24/2021

Non Holonomic Collision Avoidance of Dynamic Obstacles under Non-Parametric Uncertainty: A Hilbert Space Approach

We consider the problem of an agent/robot with non-holonomic kinematics ...
12/23/2021

Missing Velocity in Dynamic Obstacle Avoidance based on Deep Reinforcement Learning

We introduce a novel approach to dynamic obstacle avoidance based on Dee...
09/10/2021

SMARRT: Self-Repairing Motion-Reactive Anytime RRT for Dynamic Environments

This paper addresses the fast replanning problem in dynamic environments...
This week in AI

Get the week's most popular data science and artificial intelligence research sent straight to your inbox every Saturday.

1. Introduction

Autonomous navigation has gained a lot of attention in the recent years. They find applications in the fields like self-driving cars, crowd simulations, rescue operations, payload transferring etc. All these applications require a collision avoidance scheme for a safe navigation of the system to the goal. There have been quite a few approaches like (van2008reciprocal, )(van2011reciprocal, )

which present collision avoidance schemes but are computationally complex due to the non-convex nature of collision avoidance constraint. Also these schemes generally estimate whether the agent is on collision course with the other participants based on the states of the agent and the participants. A slight variance in the state estimation can lead to false detection which keeps propagating and can lead to system failure. In this paper, we present a novel methodology for collision avoidance that removes the reliance on the state of the robot. Our approach stems from the concepts of Velocity Obstacle

(fiorini1998motion, ) and ego-centric motion planning.

1.1. Contribution and Main Results

The principal contribution of the present work is the construction of efficient collision avoidance scheme for autonomous navigation called Inverse Velocity Obstacles (IVO). Our approach is a variant of Velocity Obstacle method presented in (fiorini1998motion, ), which is widely used technique for collision avoidance in a dynamic environment. Our method inherits all the salient features and incorporates capability to handle the uncertainty in collision detection that occur due to the error in state estimation. This is achieved by implementing the algorithm in an ego-centric framework. Due to the very nature of the implementation, it can be easily extended to multi-agent collision avoidance problem by implicitly assigning each agent with the same collision avoidance scheme. We also show that the low computational complexity and lower noise in collision detection of the approach significantly improves the chances for real time implementations as there is dependency on the state estimation techniques for inferring the self states of each agent.

On implementation side, we show the efficacy of Inverse Velocity Obstacles method by evaluating it in various scenarios for both single and multi-agent systems. Our simulations show that even for the agents as high as 50 can generate safe motions. We also show the variance of false collision detection is reduced significantly compared to a Velocity Obstacle approach. We have also show the real time potential of the presented approach by implementing it on real drone and also the approach can be easily parallelized as each agent computation is independent.

1.2. Layout of the paper

The rest of the paper is organized as follows, Section 2 presents a brief overview of the previous works. Section 3 reviews the concepts of Velocity Obstacle. In Section 4 we present our approach, Inverse Velocity Obstacles and derive its formulation. Section 5 describes the implementation details for the navigation of single and multi-agent systems. In Section 6 we evaluate our method in different scenarios and demonstrate the performance in real time. We conclude our work in Section 7.

2. Related Work

In this section, we present an overview of the approaches on collision avoidance and navigation in dynamic environment. Quite a few approaches (borenstein1991vector, ), (faverjon1987local, ), (fox1997dynamic, ), (kanehiro2008local, ) assume that the obstacles are static and plan for the control to avoid the collision. In case of moving obstacles these replan based on the updated positions of the obstacles. But these fail to generate the safe trajectories around the fast moving obstacles. In (fulgenzi2007dynamic, ), (de1994avoidance, ), (hsu2002randomized, ), (martinez2009collision, ) the future position of obstacles are computed by extrapolating with the current velocity to handle high velocities. But these approaches cannot handle the reactive nature of the other agents. Many works like (pettre2006real, ), (treuille2006continuum, ), (sud2008real, ), (gayle2007reactive, ) have focused on crowd simulation in which each agent considers the other agents as obstacles and navigates independently.

Centralized planning scheme on a given configuration space in the case of multiple agents is presented in (lavalle1998optimal, ), (sanchez2002using, ). These works majorly focus on optimal coordination and cannot be scaled up for real time implementation. A method called Velocity Obstacle based on velocity is presented in (fiorini1998motion, ) for moving obstacles which provides the robot a condition to avoid collision with obstacle with a known velocity. A variant called Recursive Velocity Obstacles (kluge2004reflective, ) is proposed, which considers the reactive behaviour of the other participants. However, this approach leads to the oscillations of the agents which sometimes may not converge. To address issue a extension to the Velocity obstacle called Reciprocal Velocity Obstacle (RVO)(van2008reciprocal, ) is presented, where both the agents which are on the course of collision select the velocities that bring them outside the RVO which is generated by the other agent. But this requires the knowledge of current pose and velocity of the obstacle which might bottleneck the update rates during real time implementation. They are several other extensions of Velocity Obstacle like (singh2013reactive, )(kumar2018novel, ).

To address this in this paper, we present an ego-centric based framework called Inverse Velocity Obstacles (IVO), which does not require the knowledge of robot’s pose and velocity. This eliminates the state estimation layer reducing the computational time (for state estimation) and false collision detection which aids in real time implementation.

3. Preliminaries

3.1. Velocity Obstacle

In this section, we briefly review the original concept of Velocity Obstacle and analyze its behaviour in in the presence of state, actuation and perception uncertainties.

3.1.1. Definition

Consider a mobile robot (our agent) and an obstacle, both taking the shape of a disc of radius and respectively, be denoted by and . The velocity obstacle for robot induced by obstacle , denoted by , is the set of velocities of which can result in a collision with at some point in the future. Let and represent the centres of and respectively. The robot and obstacle are geometrically modified such that the robot takes the form of a point object and the obstacle grows its radius to . If is a static obstacle, a cone can be constructed with the vertex on and the edges touching as shown in the figure 1. This cone represents the set of velocities of which lead to a collision. In case the obstacle is in motion, it is assumed to be static by taking a relative velocity of .

Figure 1. Velocity obstacle for agent induced by obstacle

3.1.2. Implementation problems

The obvious assumption from the definition of the velocity obstacle is that we need to track the velocity of the robot along with the position and velocity of the obstacle. In case of planning trajectories on a global frame, we also need to track the positions of robot and obstacle with respect to a global frame. Though we can plan trajectories in robot’s frame, this still needs us to have an estimation of the velocity of the robot. Generally, we take the instantaneous velocity from a sensor. This accounts for an additional noise in estimation of the velocity of the robot apart from the noise we end up having in the states of the obstacle. Other prominent methods include state estimation using SLAM which is not as reliable as the feed from the sensor since SLAM methods tend to break when complex maneuvers are involved.

4. Inverse Velocity Obstacle

In this section, we propose a new concept of ”Inverse Velocity Obstacle” to minimize the uncertainty in collision detection during the planning phase. This integrates into our optimization framework which provides controls leading to collision free and smooth trajectories.

4.1. Definition

The idea is simple - Instead of assuming that the obstacle is stationary, we assume that the robot is stationary and get a relative velocity vector for the obstacle. At this point, our robot is stationary at the origin (since we are in an ego-frame). We also make the obstacles point objects and grow the radius of the robot to

. Now, we find a relative velocity for our robot (which is stationary in the relative frame) which is outside the collision cone. A simple case is demonstrated in the figure 2, where and . We show that we can calculate the relative velocity of the obstacle as seen by the agent using the ego-centric observation of the obstacle by the agent at two consecutive time instance, here and , as shown in 4.1

For any time instance, suppose the global position of the obstacle moving with velocity and agent moving with velocity be and respectively. At the next time instance, the global positions of the obstacle and agent will be and respectively. The ego-centric observations of the obstacle by the agent for these instances is and for agent frame and respectively.

So, the global position of the obstacle at first instance is

Similarly for the second instance we have

Therefore the obstacle velocity in the global frame is

And hence the relative velocity of the obstacle with respect to the agent is

Figure 2. , denote the position and velocity of the agent while and denote the position and velocity of the obstacle in global frame. and denote the position and velocity of the obstacle as seen from the agent’s frame (agent is at origin and stationary in this frame).

Now, we write the collision cone using inverse velocity obstacles,

(1)

5. Navigating agents

5.1. Single Agent

Let us start with the case of a single agent that follows a holonomic motion model and obstacles that do not have a complex behaviour but move with some constant velocity. Now, consider the following optimization with variables as which represent the controls to the agent at a time instant . The goal position in the agent’s frame is denoted by and u is the control given to the agent, which in this case is the change in the velocity. r and v represent the position and velocity of the obstacle as seen by the agent. The smoothing factor can be adjusted based on the requirement. Let us assume that the maximum attainable velocity of the agent is .

(2a)
(2b)

The collision avoidance constraint, , exists for every possible pair of agent and obstacle within the sensor range of the agent. In section 6.1, we experimentally show that this formulation is valid and the agent successfully avoids the obstacles and reaches the goal.

5.2. Multiple Agents

Let us consider agents that use the optimization routine mentioned in equation 2. In this case, the obstacles may not necessarily move with constant velocity. For the sake of simplicity, we assume that every agent moves with some instantaneous velocity . Now, we scale the single agent problem to agents by considering every other agent to be an obstacle. Following this idea, a navigation algorithm for multi-agent scenario is described in Algorithm 1.

for  to  do
     for obstacle, , in obstacles in sensor range do
         
         
         
               
      return
Algorithm 1 Controls for agents in multi-agent setup

In section 6.2, we experimentally show that the algorithm works for multiple agents with large values of .

6. Experimental Results

To evaluate the performance of the presented methodology we have tested in both single agent and multi agent scenarios. All the simulations are performed on Intel i7 processor @ 3.2 GHz clock speed. The methodology is also validated on a real quadrotor. For this we used Parrot Bebop2. The detailed videos of all the simulations and real time implementations are available at [this link].

6.1. Single agent

First we validate our formulation in a single agent case. Figure (3) shows the scenario where single agent is among five dynamic obstacles. All the participants in the environment are of same radius and have same speed limits. As can be seen the agent executes safe trajectories to avoid all the obstacles and reaches the goal. The computation time for each cycle in this scenario is around 10ms making it achieve an update rate of 100Hz.

Figure 3. Blue disc represents the agent while rest are the obstacles with simple behaviour

6.2. Multiple agents

In this section, we evaluated the performance of our Inverse Velocity Obstacles in a multi-agent collision scenario. We first evaluate for a 6 agent scenario in an antipodal case. All the agents are of same radius and have same speed and acceleration limits. Figure 4 shows the scenario. All the agents plan independently considering all the other participants as potential obstacles. As can be seen all the agents generate safe motions avoiding each other and reach the goal. The computational time for each cycle in this scenario is 15ms with update rates of 66Hz.

Figure 4. Multi agent scenario: 6 agents

Next, we increased the number of agents in the same scenario with same settings to validate how IVO scales when the agents grow. Figure 5 presents the scenario with 10 agents that is evaluated. The computational time increases with the increase in the number of agents and for this scenario it is around 15ms for each cycle and has the update rates close to 50Hz. Even though the computational time is increasing with the increase in the number of agents, the update rates are high enough for aiding in a easy real time implementation.

(a)
(b)
(c)
(d)
(e)
(f)
Figure 5. Multiagent scenario: 10 agents

Additional simulation results are available at https://sites.google-.com/view/inverse-velocity-obstacle.

6.3. Real time Experiments

In this section, we evaluate the performance of Inverse Velocity Obstacles in real time implementation. For this we used Parrot Bebop2 quadrotor which accepts the yaw, pitch, roll angles as the control input. We have also developed a PID controller for the velocity control. This is integrated on top of the inbuilt controller for better performance for the validation of the algorithm as our algorithm is developed in velocity control space. This lets us pass velocity as a control command to the drone. We used April Tags (olson2011tags, ) of the family Tag36h11 for better state estimation of the other participants in the environment. We have completely bypassed the self state estimation module as our framework does not need the agents self state for collision detection and avoidance. Figures(5(a))-(5(f)) show the snapshots of the real time implementation of the proposed method on the quadrotor in a dynamic environment.

(a)
(b)
(c)
(d)
(e)
(f)
Figure 6. Real time implementation with one dynamic obstacle

6.4. Comparisons with Velocity Obstacle for Collision Detection

In this section we compare the presented approach with Velocity Obstacle and show that the collision detection for IVO is more reliable compared to the traditional Velocity Obstacle. For this the equation 1, is re-written in terms of controls in the following manner,

(3)

Similarly, the original Velocity Obstacle equation is rearranged into equation 3. In a real time scenario, the coefficients

take the form of a random variable. This introduces randomness into each coefficient due to the uncertainties in the state, actuation and perception.

denotes the PDF of . The advantage with IVO is that the random variables need not depend on and . In figure 7

, we compare the probability distributions of the error in collision cone for velocity obstacle as well as inverse velocity obstacle. The noise in agent and obstacle states were assumed to be Gaussian distributions with zero mean. The distributions clearly show a reduction in the noise. The 99% confidence region for inverse velocity obstacle is between 0 and 0.14 error range while it is between -0.03 and 0.56 error range for velocity obstacle. This provides a better scope for dealing with the noise just by increasing the radius of the obstacle.

Figure 7. Distributions for collision cone
Figure 8. Computational time for different number of obstacles

7. Conclusion

In this paper, we presented a new concept called Inverse Velocity Obstacles, for the safe navigation of autonomous agents in dynamic environments. In contrast to the previous works, we developed an ego-centric framework which eliminates the reliance on robot’s state for collision detection. This also decreases the computational complexity improving the real time implementation. The formulation presented is a natural extension of Velocity Obstacle and is easy to implement. We have also applied this to multi-agent navigation and we show its efficacy to generate natural paths for systems as high as 50 agents in very tight environment.

Our further work includes investigating the Inverse Velocity Obstacle application in the domains like crowd simulations and rescue works. Also we are exploring to extending the method to handle non-parametric uncertainty that arises due to perception and localization error.

References

  • [1] Jur Van den Berg, Ming Lin, and Dinesh Manocha. Reciprocal velocity obstacles for real-time multi-agent navigation. In 2008 IEEE International Conference on Robotics and Automation, pages 1928–1935. IEEE, 2008.
  • [2] Jur Van Den Berg, Stephen J Guy, Ming Lin, and Dinesh Manocha. Reciprocal n-body collision avoidance. In Robotics research, pages 3–19. Springer, 2011.
  • [3] Paolo Fiorini and Zvi Shiller. Motion planning in dynamic environments using velocity obstacles. The International Journal of Robotics Research, 17(7):760–772, 1998.
  • [4] Johann Borenstein and Yoram Koren. The vector field histogram-fast obstacle avoidance for mobile robots. IEEE transactions on robotics and automation, 7(3):278–288, 1991.
  • [5] Bernard Faverjon and Pierre Tournassoud.

    A local based approach for path planning of manipulators with a high number of degrees of freedom

    .
    PhD thesis, INRIA, 1987.
  • [6] Dieter Fox, Wolfram Burgard, and Sebastian Thrun. The dynamic window approach to collision avoidance. IEEE Robotics & Automation Magazine, 4(1):23–33, 1997.
  • [7] Fumio Kanehiro, Florent Lamiraux, Oussama Kanoun, Eiichi Yoshida, and Jean-Paul Laumond. A local collision avoidance method for non-strictly convex polyhedra. Proceedings of robotics: science and systems IV, 2008.
  • [8] Chiara Fulgenzi, Anne Spalanzani, and Christian Laugier. Dynamic obstacle avoidance in uncertain environment combining pvos and occupancy grid. In Proceedings 2007 IEEE International Conference on Robotics and Automation, pages 1610–1616. IEEE, 2007.
  • [9] James Gil de Lamadrid. Avoidance of obstacles with unknown trajectories: Locally optimal paths and periodic sensor readings. The International journal of robotics research, 13(6):496–507, 1994.
  • [10] David Hsu, Robert Kindel, Jean-Claude Latombe, and Stephen Rock. Randomized kinodynamic motion planning with moving obstacles. The International Journal of Robotics Research, 21(3):233–255, 2002.
  • [11] Luis Martinez-Gomez and Thierry Fraichard. Collision avoidance in dynamic environments: an ics-based solution and its comparative evaluation. In 2009 IEEE International Conference on Robotics and Automation, pages 100–105. IEEE, 2009.
  • [12] Julien Pettré, Pablo de Heras Ciechomski, Jonathan Maïm, Barbara Yersin, Jean-Paul Laumond, and Daniel Thalmann. Real-time navigating crowds: scalable simulation and rendering. Computer Animation and Virtual Worlds, 17(3-4):445–455, 2006.
  • [13] Adrien Treuille, Seth Cooper, and Zoran Popović. Continuum crowds. ACM Transactions on Graphics (TOG), 25(3):1160–1168, 2006.
  • [14] Avneesh Sud, Erik Andersen, Sean Curtis, Ming Lin, and Dinesh Manocha. Real-time path planning for virtual agents in dynamic environments. In ACM SIGGRAPH 2008 classes, page 55. ACM, 2008.
  • [15] Russell Gayle, Avneesh Sud, Ming C Lin, and Dinesh Manocha. Reactive deformation roadmaps: motion planning of multiple robots in dynamic environments. In 2007 IEEE/RSJ International Conference on Intelligent Robots and Systems, pages 3777–3783. IEEE, 2007.
  • [16] Steven M LaValle and Seth A Hutchinson. Optimal motion planning for multiple robots having independent goals. IEEE Transactions on Robotics and Automation, 14(6):912–925, 1998.
  • [17] Gildardo Sanchez and J-C Latombe. Using a prm planner to compare centralized and decoupled planning for multi-robot systems. In Proceedings 2002 IEEE International Conference on Robotics and Automation (Cat. No. 02CH37292), volume 2, pages 2112–2119. IEEE, 2002.
  • [18] Boris Kluge and Erwin Prassler. Reflective navigation: Individual behaviors and group behaviors. In IEEE International Conference on Robotics and Automation, 2004. Proceedings. ICRA’04. 2004, volume 4, pages 4172–4177. IEEE, 2004.
  • [19] Arun Kumar Singh and K Madhava Krishna. Reactive collision avoidance for multiple robots by non linear time scaling. In 52nd IEEE Conference on Decision and Control, pages 952–958. IEEE, 2013.
  • [20] AVS Sai Bhargav Kumar, Adarsh Modh, Mithun Babu, Bharath Gopalakrishnan, and K Madhava Krishna. A novel lane merging framework with probabilistic risk based lane selection using time scaled collision cone. In 2018 IEEE Intelligent Vehicles Symposium (IV), pages 1406–1411. IEEE, 2018.
  • [21] Edwin Olson. AprilTag: A robust and flexible visual fiducial system. In Proceedings of the IEEE International Conference on Robotics and Automation (ICRA), pages 3400–3407. IEEE, May 2011.