Figures
Abstract
When computing the trajectory of an autonomous vehicle, inevitable collision states must be avoided at all costs, so no harm comes to the device or pedestrians around it. In dynamic environments, considering collisions as binary events is risky and inefficient, as the future position of moving obstacles is unknown. We introduce a time-dependent probabilistic collision state checker system, which traces a safe route with a minimum collision probability for a robot. We apply a sequential Bayesian model to calculate approximate predictions of the movement patterns of the obstacles, and define a time-dependent variation of the Dijkstra algorithm to compute statistically safe trajectories through a crowded area. We prove the efficiency of our methods through experimentation, using a self-guided robotic device.
Citation: Hernández-Aceituno J, Acosta L, Piñeiro JD (2015) Application of Time Dependent Probabilistic Collision State Checkers in Highly Dynamic Environments. PLoS ONE 10(3): e0119930. https://doi.org/10.1371/journal.pone.0119930
Academic Editor: Yong Deng, Southwest University, CHINA
Received: July 25, 2014; Accepted: January 18, 2015; Published: March 23, 2015
Copyright: © 2015 Hernández-Aceituno et al. This is an open access article distributed under the terms of the Creative Commons Attribution License, which permits unrestricted use, distribution, and reproduction in any medium, provided the original author and source are credited
Data Availability: All simulation data used in our research was procedurally generated as explained within the text, based on the equations in "Probabilistic collision state checker for crowded environments", by Althoff D, Althoff M, Wollherr D and Buss M (2010, IEEE Int. Conf. Robotics and Automation. pp. 1492-1498). All prediction and pathfinding algorithms are explained in detail within our paper, so that researchers may reproduce our experiments. Additional generation scripts and experimental data can be found at https://github.com/jhaceituno/pcsdata.
Funding: The authors gratefully acknowledge the contribution of the Spanish Ministry of Economy and Competitiveness (www.mineco.gob.es) under Project STIRPE DPI2013-46897-C2-1-R. JHA’s research was supported by the Spanish Ministry of Science and Innovation (www.micinn.es), under the "Formación de Profesorado Universitario" grant FPU2012-3568. The funders had no role in study design, data collection and analysis, decision to publish, or preparation of the manuscript.
Competing interests: The authors have declared that no competing interests exist.
Introduction
In order for an autonomous vehicle to properly operate in an uncontrolled environment, safe trajectories must be ensured so it does not harm itself or pedestrians nearby. This is not a simple calculation, as decisions must be taken in real time to avoid accidents, while still trying to reach a target position as efficiently as possible.
The concept of inevitable collision state (ICS) [1] is defined as the configuration of an autonomous robotic vehicle for which the event of a collision with an obstacle is unavoidable. This definition is valid for environments with objects that remain static or that move following deterministic trajectories. However, as [2, 3] recognize, this is not a safe choice when these obstacles are, for instance, pedestrians.
This situation has been approached using Braking ICS [4], Rapidly-exploring Random Trees [5], Chance Constraints [6], Probabilistic Collision States [7, 8], or combinations thereof [9]. The Probabilistic Collision States method (PCS) assumes probable future occupation areas for each observed object, which depend on the geometry of both the object and the robot.
When first introducing PCS, Althoff et al. [8] considered two kind of objects: passive, when they completely ignored the situation of the approaching robot, and active, when they altered their trajectory in order to avoid collisions. All these objects were modeled to move following a uniformly accelerated motion, where the acceleration was not necessarily parallel to the initial velocity and was proportional to the effort the obstacle put into avoiding the robot. A passive object therefore moved with a uniform speed. These parameters were predicted using a random motion model, defined in [10, 11], following a normal distribution, as shown in [12] and its follow-up publication [13].
In order to avoid considering infinite time spans, Althoff et al. restricted the movement of the robot to a uniformly decelerated motion, so the acceleration direction was constant relative to the velocity direction, and always forming such an angle that produced a deceleration (their scalar product had to always be negative). This model was insufficient for a real life application, for it was restricted to braking motions and all objects were assumed to move uniformly. Furthermore, the consideration of obstacles actively avoiding a collision could be a dangerous assumption, since a robot should not expect an environment that varies favorably.
Althoff et al. also treated all possible locations without considering the evolution in time. Since the position of the obstacles varied during the procedure, the resulting trajectory would be faulty. This problem has been solved before by more computationally expensive planners [14–17].
To offer a safer and more efficient alternative to this model, in this paper we present a Bayesian time-dependent modification of the PCS system, which corrects all these flaws and meets the computational requirements to be implemented in a real autonomous vehicle. Our method relies on obstacle movement prediction and fast trajectory computing, and as such bears conceptual similarities to previous works [18–21]. These works, however, were presented as purely theoretical exercises, whereas our approach is backed up by experimental data obtained from tests on a real environment.
The following section of this paper defines the theoretical models and assumptions on which our calculations are based; we introduce how a collision probability matrix is calculated for a working area during a definite time window; afterwards, we explain how trajectories across this matrix are computed. Our proposal is put to the test in crowded areas, both simulated and real.
Methods
Our study defines the following basic elements:
Both the vehicle and the obstacles are assumed to exist within a working space W, defined as a two-dimensional grid. The area occupied by the robot is named AR ⊂ W, whereas each object i takes an area of Ai ⊂ W; the total number of obstacles will be denoted as N. For the sake of simplicity and more efficient calculations, all areas are considered circular and defined by radii rR and ri. For more complex shapes, these radii correspond to the circles in which the objects can be inscribed. In all the experiments in this paper, unless stated, W is a two-dimensional grid of 50×50 cells.
It is assumed that each object is observed during a certain time span before its future behavior can be modeled. We therefore consider an observed position vector for each obstacle i. The time interval between two observations is assumed to be as short as possible, and equal to or greater than the processing time required to execute a complete iteration of our algorithm.
Unlike previous works [8], we do not consider unbounded time spans, but definite overlapping time windows that allow the probable location of each object to be calculated for a certain period, similarly to [22, 23]. The length of these time windows, named T, can be selected to best suit the needs of each real case. The position of the observed obstacles is estimated for up to T time steps into the future. The present time step is named tP; the last time step for which predictions are made is therefore tT = tP+T.
The following subsection of this paper explains how to calculate a matrix of probable locations over the working area W for each obstacle, defined by its motion vector , for the chosen time window of size T. The global collision matrix M, which the robot must consider to properly design a trajectory, must take into account the added probable positions of every observed obstacle. A trajectory of the robot is named u(t) and is obtained by finding the optimal path through W with a minimum collision probability.
Probabilistic collision matrix
The probable location matrix of an individual object i at time t may follow any probability distribution required by each particular problem, but typically uses a Gaussian distribution, discretized into a grid model. In order to predict the future positions of the observed obstacles, we apply a sequential Bayesian model, which combines perceived location and previously calculated probabilistic locations. Although this sort of model has been used in the field of robot perception and planning before [24–26], its application to the calculation of the location matrices of dynamic obstacles, as far as we know, has not been presented in any previous works.
Given a sequence of observed positions , we wish to calculate the location matrix of object i at time t. The posterior probability for this parameter is given by Equation 1.
(1)
Observation likelihood.
The way to calculate the observation likelihood depends on the considered time step. All observations before the current time step tP are determined by the measurement devices, and affected by whichever data post-processing they may require and inaccuracies they may suffer. These are usually modeled as Gaussian functions.
However, future observations cannot be obtained, and therefore no valid locations may be inferred from measurements after tP. In this case, the measured location has to be modeled as uniform over the entire working area (Equation 2).
(2)
Motion prior.
The motion prior of each obstacle i is calculated as a convolution of its previous probable position. The movement of each object is modeled as a two-dimensional accelerated motion, such that Equation 3 is satisfied when δt is the time interval between two observations.
(3)
The xi values are not deterministic positions, but essentially continuous probabilistic distributions; this is because they are inferred from previous measurements, that may be noisy or otherwise impossible to define using the expected motion model. As such, vi and ai, which are in turn used to calculate new positions, will also be probabilistic.
In our work, the acceleration is modeled as a Gaussian function with mean and standard deviation , computed from the variation of the positions observed by the measurement devices. Both components of the two-dimensional movement are assumed independent and handled as different models.
In the case we present, the motion acceleration can also be modeled and rewritten as a function of position, time and velocity. Using Equation 3, we first define the variation between two positions as Equation 4.
(4)
Rearranging this equation and connecting it to the probability distribution which ai follows, we obtain Equation 5.
(5)
Applying a linear transformation to , we obtain the probability model for the position variation, Equation 6. Notice how the variance increases quadratically over time.
(6)
A convolution mask is created so that it defines the probabilistic relative region of space where an object would travel, considering the calculated acceleration and accumulated velocity and position, according to Equation 6. Applied to the posterior location matrix of the corresponding object, the prior matrix at the following time step is obtained.
Integration procedure.
This calculation however would consider both the obstacles and the robot as punctual. In order for our algorithm to work properly, the geometry of every element must be taken into account, as stated in [27].
To solve this flaw, the calculated location matrix for each object is integrated for a circular area of radius ri+rR, that is, its own radius plus the robot radius; for each navigable position w ∈ W, Equation 7 applies.
(7)
This way, the probability of a collision with a particular object is at least that of the closest point of said obstacle that the robot can impact.
This technique was tested along with those used in previous works, such as Minkowski sums [7, 8] and convolutions [28], and was found to be more efficient due to its greater computational simplicity: since our method depends exclusively on the radii of the involved objects, the integration boundaries remain constant during the entire execution, and can therefore be calculated beforehand.
Global collision probability matrix.
Each position of the global matrix M is expected to inform about the combined collision probability of every obstacle, for each step t of the time window; therefore, a union must be applied. In this paper, we consider the probable locations of different objects to be independent, since they do not attempt to avoid each other.
The union rule for a high number of events is inefficient, since the intersections for all possible event combinations must be calculated (Equation 8).
(8)
However, De Morgan’s laws simplify the calculation (Equation 9).
(9)
Fig. 1 shows a global collision matrix M for three moving obstacles. Unless stated, in all figures the integration radii ri+rR are equal to 1. The predicted trajectories are color coded, such that the hue value varies with time from tP to tT. In order to simplify the graphics, the observed trajectories for t < tP are not displayed from this point on.
The observed trajectories are shown as white lines, and the predicted location is shown as a colored area. For visualization purposes, motion noise has been omitted.
Although obstacle collisions are not considered in this work, it can be observed that objects 1 and 2 would overlap on position X, since the color of their trajectory coincides at that point. Objects 2 and 3 however would not collide at position Y, since their colors differ.
Optimal trajectory calculation
The result of the previous stage M is then used to generate a trajectory u(t) over W between two locations xstart and xgoal, with a minimum collision probability and respecting the motion physics of the robotic device. For the sake of simplicity, a non-holonomic Dubins-like car [29] is simulated (Equation 10).
(10)
However, the algorithm can be adapted to any kinematic model. By default, we use v = 1 and ω = π/4.
Time-dependent Dijkstra algorithm.
To compute a safe route between two positions, we developed a simple time-dependent variation of the Dijkstra algorithm, shown in algorithm in Table 1. The motion physics defined for the robotic device are taken into account, both when checking which poses are accessible at each time step, and when calculating the cost of each movement. The accumulated collision cost of reaching position y from position x is given by Equation 11, where ‖y−x‖ is the movement cost and Mt(y) is the collision probability of position y, at the moment when it is reached.
(11)
This method is arguably similar to the D* Lite algorithm [30], since the time of arrival at a cell does affect its cost, but our approach does not need to recalculate its route when finding an unexpected obstacle. This is so because all the steps of the evolution of the collision matrix are taken into account simultaneously during the computation of a route. However, in case of sequential execution, a precalculated route cannot be reused, since the collision matrix is completely reconfigured for each iteration of the algorithm.
Two examples of trajectories are shown in Fig. 2, where it can be observed that the vehicle avoids the paths of the objects only when they pose a risk.
Notice how the time step at which each location is reached does affect the trajectory.
The collision probability of the trajectory of the robot is finally computed as a union, such that the time at which each location is reached affects the result (Equation 12).
(12) Tu is time span required for the robot to reach its destination (equivalent to tA (xgoal) in the algorithm in Table 1). The spatial length of the trajectory will be denoted as ‖u‖ (Equation 13).
(13)
Minimum probability restriction.
The presented solution may sometimes be excessively cautious and take unnecessary detours. By setting a fixed minimum value ɛ, such that we define a new collision probability matrix as shown in Equation 14, more reasonable trajectories can be produced. The default probability matrix can still be used by considering ɛ = 0.
(14)
This restriction is similar to the chance constraints method [6], which specifies to which areas of the working space the autonomous vehicle must restrict its movement, and which areas it must avoid. This technique was combined with the probabilistic collision method in [9] by quantizing the resulting probability matrix around a fixed value for each observed obstacle.
However, in our case, ɛ works as a lower bound that declares a certain area of the working space to be safe enough to travel. No upper bound is defined, as our algorithm is expected to always return the trajectory with the lowest collision probability; if we were to establish a certain maximum probability value and it still were lower than that of our trajectory, there simply would not be any valid solutions.
Fig. 3 shows the results of using a ɛ value. In this example, a 0.01 minimum probability restriction produces a collision probability increase of only 3.17%, while the trajectory length is reduced by 22.037%, and the required time to reach the desired destination is lower.
The white segment in (a) is the section of the trajectory that exceeds time step tT, with T = 50.
Results
Simulation results
Our algorithm computes a trajectory with a minimum collision probability for different configurations of randomly moving objects. In order to compare the performance of our algorithm to previously available methods, we reproduce the examples given in [8] and force the robot to create a path through a probabilistically crowded area, as shown in Fig. 4, for both passive and active objects, along with the collision probabilities for the resulting trajectories. Since the original method did not take time into account, the results greatly differ: the robotic device is able to maneuver around the moving obstacles, instead of just choosing the optimal braking trajectory, and still shows a very low collision probability.
We use T = 35 and ɛ = 10−3; in both cases, Tu = 21 and ‖u‖ = 24.142.
More complex configurations were automatically generated in order to thoroughly test our algorithm. Objects were programmed to move following initial randomized curve trajectories, such that they would always affect the calculations of the robot. Table 2 shows the average results for 100 executions of our simulation, along with the chosen parameters. Fig. 5 shows four examples and their results.
Our algorithm produces very low collision probabilities for all cases, provided that the randomly obtained trajectories of the objects don’t pass through the initial and target positions, or completely fill the area in between them. In order to confirm the adaptability of our method, four additional kinematic models were programmed, and each one was tested over 100 instances of our simulation. As Table 3 shows, higher velocity values—both linear and angular—decrease the maneuverability of the robot, causing less efficient routes; however, the average collision probability remains similar to ɛ for all models.
Sequential execution.
Although it provides valid trajectories, our algorithm is heavily based on probabilistic assumptions of the future behavior of perceived pedestrians, which are bound to change every time step. A more complex approach is therefore required in order to apply this method to real cases.
To do this, the trajectory is only followed for a certain amount of time, ideally the length of a time step, while the obstacles are observed and a new trajectory is processed. Fig. 6 shows an example of a sequential execution of the algorithm.
We use T = 45, ɛ = 10−3 and rR + r1 = 2.
As can be seen, the expected future positions for the objects are more accurately calculated when more knowledge about their motion has been gathered. The limitations of the prediction procedure can also be observed: speed and acceleration are considered null or constant until enough observations have been collected.
Although this set of experiments assumes the robot moves at a constant speed, braking maneuvers can also be included in the motion model of the robot, so that more realistic trajectories are computed.
Experimental results
After verifying its efficiency theoretically, our algorithm was installed on an Adept Pioneer 3-AT device (Fig. 7). This was equipped with a customized on-board computer, which included a Intel Core2 Quad processor with four 2.66 GHz cores, 4Gb RAM, and a 64-bit Ubuntu 12.04 operative system with ROS Hydro Medusa. Visual information was provided by a zenithal GoPro Hero3 camera installed on the ceiling of the test area.
Our robot was released in a 4.25m×4.25m area in a crowded corridor of our faculty, and commanded to reach different target positions, following trajectories with minimum collision probability, which were computed sequentially as explained in the previous subsection. Fig. 8 shows an example of the calculation of one of these trajectories. Pedestrians were given an integration radius of one meter.
We use a workspace resolution of 50×50, a time window of T = 30 steps, and a minimum collision probability of ɛ = 0.1. The white dashed line represents the path that the robot has already followed, while the colored dashed lines show the observed movements of the obstacles.
We define two new variables: δu, which represents the relative increment between the traveled distance and the length of the minimum path between the starting and goal poses (Equation 15), and the processing time for a complete iteration (calculation of both a probabilistic collision matrix and an optimal trajectory), which we named τ.
(15)
In order to test the performance of our algorithm in real time, we measured the average values of these parameters, along with the average collision probability of the final trajectory pC (u,Mɛ), for 100 different configurations of workspace resolution, time window length T and minimum collision value ɛ.
The results are shown in Tables 4, 5 and 6. Notice that pC (u,Mɛ) and δu are not calculated over the output of each iteration, but over the final trajectory of the robot resulting from the whole execution.
Discussion
As we expected, experimental data shows that the processing time of the algorithm depends almost exclusively on the workspace resolution. Since every iteration of the procedure must also include the stages of data extraction from the visual media, as well as command delivery to the motion system, this parameter must be finely tuned in order to obtain precise enough trajectories in low enough time spans.
Increments of this value also cause shorter trajectories. This happens because a higher number of cells allows the algorithm to consider more accurate movements over the working area. However, the time cost of this precision can cause the robot to skip steps and calculate faulty trajectories. In our experiments, a workspace resolution of 50×50 cells resulted in a good enough avoidance behavior.
Higher values of ɛ result in shorter trajectories, along with higher collision probabilities—experimentally, we observed that the robot behaved more recklessly and moved closer to the obstacles. However, ɛ = 0 could cause extremely inefficient routes, with long detours and braking maneuvers. A value of ɛ = 0.1 was found to give good enough results for our particular case. Notice how, as our calculations are made a posteriori over the final result of the algorithm, the average collision probability of each trajectory closely resembles ɛ.
Finally, the size of the time window T was not observed to affect the processing time significantly, as the algorithm in Table 1 explores the whole workspace regardless of the available collision data. However, higher values were found to produce shorter trajectories and lower collision probabilities, since the accuracy of the method increases. Therefore, it is advised to make this value equal to the expected time span required for the robot to reach its goal.
Author Contributions
Conceived and designed the experiments: JHA LA. Performed the experiments: JHA. Analyzed the data: JHA JDP. Contributed reagents/materials/analysis tools: LA. Wrote the paper: JHA LA JDP.
References
- 1. Fraichard T, Asama H. Inevitable collision states. A step towards safer robots? IEEE/RSJ Int Conf Intell Robots and Systems. 2003;1: 388–393.
- 2.
Martinez-Gomez L, Fraichard T. An efficient and generic 2D inevitable collision state-checker. IEEE/RSJ Int Conf Intell Robots and Systems. 2008; 234–241.
- 3.
Parthasarathi R, Fraichard T. An inevitable collision state-checker for a car-like vehicle. IEEE Int Conf Robotics and Automation. 2007; 3068–3073.
- 4.
Bouraine S, Fraichard T, Salhi H. Relaxing the inevitable collision state concept to address provably safe mobile robot navigation with limited field-of-views in unknown dynamic environments. IEEE/RSJ Int Conf Intell Robots and Systems. 2011; 2985–2991.
- 5.
Fulgenzi C, Tay C, Spalanzani A, Laugier C. Probabilistic navigation in dynamic environment using rapidly-exploring random trees and gaussian processes. IEEE/RSJ Int Conf Intell Robots and Systems. 2008; 1056–1062.
- 6. Blackmore L, Ono M, Williams B. Chance-constrained optimal path planning with obstacles. IEEE Trans Robotics 2011;27: 1080–1094.
- 7.
Bautin A, Martinez-Gomez L, Fraichard T. Inevitable collision states: A probabilistic perspective. IEEE Int Conf Robotics and Automation. 2010; 4022–4027.
- 8.
Althoff D, Althoff M, Wollherr D, Buss M. Probabilistic collision state checker for crowded environments. IEEE Int Conf Robotics and Automation. 2010; 1492–1498.
- 9. Du Toit N, Burdick J. Probabilistic collision checking with chance constraints. IEEE Trans Robotics 2011;27: 809–815.
- 10.
Zhu Q. A stochastic algorithm for obstacle motion prediction in visual guidance of robot motion. IEEE Int Conf Systems Engineering. 1990; 216–219.
- 11. Benenson R, Fraichard T, Parent M. Achievable safety of driverless ground vehicles. Int Conf Control, Automation, Robotics and Vision. 2008;10: 515–521.
- 12. Miura J, Uozumi H, Shirai Y. Mobile robot motion planning considering the motion uncertainty of moving obstacles. IEEE Int Conf Systems, Man, and Cybernetics. 1999;4: 692–697.
- 13. Miura J, Shirai Y. Modeling motion uncertainty of moving obstacles for robot motion planning. IEEE Int Conf Robotics and Automation. 2000;3: 2258–2263.
- 14.
Alonso-Mora J, Breitenmoser A, Beardsley P, Siegwart R. Reciprocal collision avoidance for multiple car-like robots. IEEE Int Conf Robotics and Automation. 2012; 360–366.
- 15.
Patil S, van den Berg J, Alterovitz R. Estimating probability of collision for safe motion planning under gaussian motion and sensing uncertainty. IEEE Int Conf Robotics and Automation. 2012; 3238–3244.
- 16.
van den Berg J, Wilkie D, Guy S, Niethammer M, Manocha D. LQG-obstacles: Feedback control with collision avoidance for mobile robots with motion and sensing uncertainty. IEEE Int Conf Robotics and Automation. 2012; 346–353.
- 17.
Ziebart B, Ratliff N, Gallagher G, Mertz C, Peterson K, Bagnell, JA, et al. Planning-based prediction for pedestrians. IEEE/RSJ Int Conf Intelligent Robots and Systems. 2009; 3931–3936.
- 18.
Kim S, Guy SJ, Liu W, Lau RWH, Lin MC, Manocha D. Predicting pedestrian trajectories using velocity-space reasoning. In: Frazzoli E, Lozano-Perez T, Roy N, Rus D, editors. Algorithmic Foundations of Robotics X. Berlin / Heidelberg: Springer. 2013; 609–623.
- 19.
Luber M, Spinello L, Silva J, Arras KO. Socially-aware robot navigation: A learning approach. IEEE/RSJ Int Conf Intelligent Robots and Systems. 2012; 902–907.
- 20. Du Toit NE, Burdick JW. Robot motion planning in dynamic, uncertain environments. IEEE Trans Robotics. 2012;28: 101–115.
- 21.
Henry P, Vollmer C, Ferris B, Fox D. Learning to navigate through crowded environments. IEEE Int Conf Robotics and Automation. 2010; 981–986.
- 22. Fox D, Burgard W, Thrun S. The dynamic window approach to collision avoidance. IEEE Robotics Automation Magazine. 1997;4: 23–33.
- 23. Brock O, Khatib O. High-speed navigation using the global dynamic window approach. IEEE Int Conf Robotics and Automation. 1999;1: 341–346.
- 24. Oriolo G, Ulivi G, Vendittelli M. Fuzzy maps: A new tool for mobile robot perception and planning. Journal of Robotic Systems. 1997;14: 179–197.
- 25. Plagemann C, Fox D, Burgard W. Efficient failure detection on mobile robots using particle filters with gaussian process proposals. Int Joint Conf Artificial Intelligence. 2006;20.
- 26. Caron F, Davy M, Duflos E, Vanheeghe P. Particle filtering for multisensor data fusion with switching observation models: Application to land vehicle positioning. IEEE Trans Signal Processing. 2007;55: 2703–2719.
- 27.
Fraichard T. A short paper about motion safety. IEEE Int Conf Robotics and Automation. 2007; 1140–1145.
- 28. Guibas L, Ramshaw L, Stolfi J. A kinetic framework for computational geometry. Annual Symposium on Foundations of Computer Science. 1983;24: 100–111.
- 29. Dubins LE. On curves of minimal length with a constraint on average curvature, and with prescribed initial and terminal positions and tangents. American Journal of Mathematics 1957;79: 497–516.
- 30. Koenig S, Likhachev M. Fast replanning for navigation in unknown terrain. IEEE Trans Robotics 2005;21: 354–363.