Abstract
The main contribution of this paper is a novel method for assessing the safety of trajectories by means of their collision probability in dynamic and uncertain environments. The future trajectories of the robot are represented as directed graphs and the uncertain states of the obstacles are represented by probability distributions. Instead of evaluating the safety of the graph by determining the route with the smallest collision probability, the optimal policy minimizing the collision probability is used. The policy allows one to replan the route depending on the future probability distributions of the obstacles. Since these distributions are unknown at the time point of the assessment, they are simulated and represented by compound probability distributions. These compound distributions represent all possible future distributions of the obstacles. It is shown that this novel method is always less conservative than previous approaches. Two example implementations are presented, one using Gaussian distributions and one using motion patterns for representing the uncertain states of the obstacles. Simulation scenarios are used for validating the proposed concept.
Similar content being viewed by others
References
Althoff, D., Althoff, M., Wollherr, D., & Buss, M. (2010). Probabilistic collision state checker for crowded environments. In Proceedings of the IEEE international conference on robotics and automation (pp. 1492–1498).
Althoff, D., Buss, M., Lawitzky, A., Werling, M., & Wollherr, D. (2012). On-line trajectory generation for safe and optimal vehicle motion planning. In: Autonomous Mobile Systems, Informatik aktuell (pp. 99–107). Berlin Heidelberg: Springer.
Althoff, D., Kuffner, J. J., Wollherr, D., & Buss, M. (2011). Safety assessment of robot trajectories for navigation in uncertain and dynamic environments. Springer Autonomous Robots, 32, 285–302. SI Motion Safety for Robots.
Amato, N. M., Bayazit, O. B., Dale, L. K., Jones, C., & Vallejo, D. (2000). Choosing good distance metrics and local planners for probabilistic roadmap methods. IEEE Transactions on Robotics & Automation, 16, 442–447.
Aoude, G., Luders, B., Joseph, J., Roy, N., & How, J. (2013). Probabilistically safe motion planning to avoid dynamic obstacles with uncertain motion patterns. Autonomous Robots, 35(1), 51–76.
Bennewitz, M. (2004). Mobile robot navigation in dynamic environments. PhD thesis, University of Freiburg, Department of Computer Science.
Berthelot, A., Tamke, A., Dang, T., & Breuel, G. (2012). A novel approach for the probabilistic computation of time-to-collision. In Intelligent Vehicles Symposium (IV), 2012 IEEE (pp. 1173–1178).
Besse, C., & Chaib-draa, B. (2009). Quasi-deterministic partially observable markov decision processes. In C. Leung, M. Lee, & J. Chan (Eds.), Neural information processing. Lecture notes in computer science (Vol. 5863, pp. 237–246). Berlin, Heidelberg: Springer.
Blackmore, L., Ono, M., Bektassov, A., & Williams, B. C. (2010). A probabilistic particle-control approximation of chance-constrained stochastic predictive control. IEEE Transactions on Robotics, 26(3), 502–517.
Bonet, B. (2009). Deterministic pomdps revisited. In Proceedings of the twenty-fifth conference on uncertainty in artificial intelligence (pp. 59–66).
Box, G. E. P., & Tiao, G. C. (1973). Bayesian inference in statistical analysis. New York: Wiley.
Brechtel, S., Gindele, T., & Dillmann, R. (2014). Probabilistic decision-making under uncertainty for autonomous driving using continuous pomdps. In 2014 IEEE 17th international conference on intelligent transportation systems (ITSC) (pp. 392–399).
Broadhurst, A., Baker, S., & Kanade, T. (2005). Monte carlo road safety reasoning. In Proceedings of the IEEE intelligent vehicle symposium (pp. 319–324).
Carmo, M. P. D. (1976). Differential geometry of curves and surfaces. London: Pearson.
Chan, E., & Yaya, Y. (2009). Shortest path tree computation in dynamic graphs. IEEE Transactions on Computers, 58(4), 541–557.
de Nijs, R., Julia, M., Mitsou, N., Gonsior, B., Kühnlenz, K., Wollherr, D., & Buss, M. (2011). Following route graphs in urban environments. In Proceedings of the IEEE international symposium on robot and human interactive communication (pp. 363–368).
Dechter, R., & Pearl, J. (1985). Generalized best-first search strategies and the optimality of a*. Journal of ACM, 32(3), 505–536.
Doucet, A., Freitas, N. D., & Gordon, N. J. (2001). Sequential Monte Carlo methods in practice. Berlin: Springer.
Eidehall, A., & Petersson, L. (2008). Statistical threat assessment for general road scenes using monte carlo sampling. IEEE Transactions on Intelligent Transportation Systems, 9, 137–147.
Fulgenzi, C. (2009). Autonomous navigation in dynamic uncertain environment using probabilistic models of perception and collision risk prediction. PhD thesis, INRIA Rhône-Alpes.
Gelman, A., Carlin, J. B., Stern, H. S., & Rubin, D. B. (2003). Bayesian data analysis (2nd ed.). London: Chapman and Hall.
Geraerts, R., & Overmars, M. (2004). Sampling techniques for probabilistic roadmaps planners. In Proceedings of the international conference on intelligent autonomous systems (pp. 600–609).
González-Sieira, A., Mucientes, M., & Bugarín, A. (2014). A state lattice approach for motion planning under control and sensor uncertainty. In M. A. Armada, A. Sanfeliu, & M. Ferre (Eds.), ROBOT2013: First Iberian robotics conference, volume 253 of advances in intelligent systems and computing (pp. 247–260). Berlin: Springer International Publishing.
Gottschalk, S., Lin, M. C., & Manocha, D. (1996). Obb-tree: A hierarchical structure for rapid interference detection. Proceedings on ACM Siggraph, 96, 171–180.
Greytak, M., & Hover, F. (2010). Motion planning with an analytic risk cost for holonomic vehicles. In Proceedings of the 48th IEEE conference on decision and control (pp. 5655–5660).
Grubbström, R. W., & Tang, O. (2004). The moments and central moments of a compound distribution. European Jourbal of Operational Research, 170(1), 106–119.
Guibas, L. J., Hsu, D., Kurniawati, H., & Rehman, E. (2008). Bounded uncertainty roadmaps for path planning. In Proceedings of the international workshop on the algorithmic foundations of robotics.
Hermes, C., Wohler, C., Schenk, K., & Kummert, F. (2009). Long-term vehicle motion prediction. In IEEE intelligent vehicles symposium (pp. 652–657).
Jansson, J. (2005). Collision avoidance theory with application to automotive collision mitigation. PhD thesis, Linköping University.
Kavraki, L. E., Svestka, P., Latombe, J. C., & Overmars, M. H. (1996). Probabilistic roadmaps for path planning in high-diemnsional configuration spaces. IEEE Transactions on Robotics & Automation, 12, 566–580.
Kneebone, M. (2009). Navigation planning in probabilisitic roadmaps with uncertainty. In Proceedings of the nineteenth international conference on automated planning and scheduling (pp. 209–216).
Kurniawati, H., Bandyopadhyay, T., & Patrikalakis, N. (2011a). Global motion planning under uncertain motion, sensing, and environment map. In Proceedings of the robotics science and systems VII.
Kurniawati, H., Du, Y., & Lee, W. S. (2011b). Motion planning under uncertainty for robotic tasks with long time horizons. The International Journal of Robotic Research, 30(3), 308–323.
Kushleyev, A., & Likhachev, M. (2009). Time-bounded lattice for efficient planning in dynamic environments. In Proceedings of the international conference on robotics and automation (pp. 1662–1668).
Lambert, A., Gruyer, D., & Pierre, G. (2008a). A fast monte carlo algorithm for collision probability estimation. In The 10th international conference on control, automation, robotics and vision.
Lambert, A., Gruyer, D., Pierre, G. S., & Ndjeng, A. N. (2008b). Collision probability assessment for speed control. In Proceedings of the international IEEE conference on intelligent transportation systems (pp. 1043—1048).
Laugier, C., Paromtchik, I., Perrollaz, M., Mao, Y., Yoder, J., Tay, C., et al. (2011). Probabilistic analysis of dynamic scenes and collision risk assessment to improve driving safety. Intelligent Transportation Systems Journal, 3(4), 4–19.
LaValle, S. M. (2006). Planning algorithms. Cambridge, UK: Cambridge University Press.
LaValle, S. M., & Kuffner, J. J. (2001). Randomized kinodynamic planning. The International Journal of Robotics Research, 20, 378–401.
Lefevre, S., Bajcsy, R., & Laugier, C. (2013). Probabilistic decision making for collision avoidance systems: Postponing decisions. In 2013 IEEE/RSJ international conference on intelligent robots and systems (IROS) (pp. 4370–4375).
Levinson, J., Askeland, J., Becker, J., Dolson, J., Held, D., Kammel, S., Kolter, J. Z., Langer, D., Pink, O., Pratt, V., Sokolsky, M., Stanek, G., Stavens, D. M., Teichman, A., Werling, M., & Thrun, S. (2011). Towards fully autonomous driving: Systems and algorithms. In Intelligent vehicles symposium (pp. 163–168).
Missiuro, P. E., & Roy, N. (2006). Adapting probabilistic roadmaps to handle uncertain maps. In IEEE internation conference on robotics and automation (pp. 1261–1267).
Patil, S., van den Berg, J., & Alterovitz, R. (2012). Estimating probability of collision for safe motion planning under gaussian motion and sensing uncertainty. In Proceedings of the international conference on robotics and automation (pp. 3238–3244).
Philippsen, R. (2004). Motion planning and obstacle avoidance for mobile robots in highly cluttered dynamic environments. PhD thesis, ETH Zürich, Institute of Robotics and Intelligent Systems.
Platt Jr., R., Tedrake, R., Kaelbling, L., & Lozano-Perez, T. (2010). Belief space planning assuming maximum likelihood observations. In Proceedings of the robotics science and systems VI.
Puterman, M. L. (2005). Markov decision processes: Discrete stochastic dynamic programming. New York: Wiley.
Rubin, D. B. (1984). Bayesianly justifiable and relevant frequency calculations for the applies statistician. Annals of Statistic, 12(4), 1151–1172.
Rubinstein, R., & Kroese, D. (1981). Simulation and the Monte Carlo method. New York: Wiley-Interscience.
Sisbot, E. A., & Alami, R. (2012). A human-aware manipulation planner. IEEE Transactions on Robotics, 28(5), 1045–1057.
Smith, T., & Simmons, R. G. (2005). Point-based POMDP algorithms: Improved analysis and implementation. In Proceedings of the international conference on uncertainty in artificial intelligence (UAI) (pp. 542–549).
Thrun, S., Burgard, W., & Fox, D. (2005). Probabilistic robotics. Cambridge, MA: MIT Press.
Toit, N. E. D. & Burdick, J. W. (2010). Robotic motion planning in dynamic, cluttered, uncertain environments. In Proceedings of the IEEE international conference on robotics and automation (pp. 966–973).
Urmson, C., Baker, C., Dolan, J., Rybski, P., Salesky, B., Whittaker, W. L., et al. (2009). Autonomous driving in traffic: Boss and the urban challenge. AI Magazine, 30, 17–29.
van den Berg, J., Abbeel, P., & Goldberg, K. (2010). Lqg-mp: Optimized path planning for robots with motion uncertainty and imperfect state information. In Proceedings of the robotics science and systems VI.
Werling, M., Kammel, S., Ziegler, J., & Gröll, L. (2011). Optimal trajectories for time-critical street scenarios using discretized terminal manifolds. International Journal of Robotics Research, 31(3), 346–359.
Werling, M., Ziegler, J., Kammel, S., & Thrun, S. (2010). Optimal trajectory generation for dynamic street scenarios in a frenét frame. In Proceedings of the international conference on robotics and automation (pp. 987–993).
Acknowledgments
The authors gratefully acknowledge partial financial support of this work by the Deutsche Forschungsgemeinschaft (German Research Foundation) within the excellence initiative research cluster Cognition for Technical Systems – CoTeSys (http://www.cotesys.org), the EU-STREP project Interactive Urban Robot – IURO (http://www.iuro-project.eu), the ERC Advanced Grant project Seamless Human Robot Interaction in Dynamic Environments – SHRINE (http://www.shrine-project.eu), the BMBF Bernstein Center for Computational Neuroscience Munich (http://www.bccn-munich.de) and the Institute for Advanced Study – IAS, Technische Universität München (http://www.tum-ias.de).
Author information
Authors and Affiliations
Corresponding author
Appendix
Appendix
Proof of Proposition 1
In every step of our algorithm the number of vertices or edges is reduced by at least one. So there can be \(n+m\) steps at most.
In the following the convergence is proven by contradiction. Assume that the algorithm has not converged, so there has to be nodes
because if \({\text {deg}}^+(\mathbf {v})=1\) the node would have been removed from the graph in any step by rule (5). Choose \(\mathbf {v}_i\) as the latest such node, i.e. \(t_i > t, \forall t \in \mathbf {v} \in \mathcal {V}^+\). The outgoing edges cannot be multi-edges because they would have been removed in any step. Thus, there has to be an edge \(\mathbf {e}_{ij}\) to \(\mathbf {v}_j \ne \mathbf {v}_g\), with \(t_j > t_i\). As \(\mathbf {v}_j\) was not removed in a previous step, \({\text {deg}}^+(\mathbf {v}_j) >1\) must hold. Therefore, \(\mathbf {v}_j\) was not the latest node with \({\text {deg}}^+>1\). \(\square \)
Proposition 2
Let \(X_1\) and \(X_2\) be independent random variables on the support interval \([x_l, x_u]\) with distribution \(f_i\) and cumulative distribution function (cdf) \(F_i\), with \(i\in \{1,2\}\). Let \(Y=\min \{X_1,X_2\}\), with the minimum distribution \(F_Y(x) = 1-(1-F_1(x))(1-F_2(x))\). Then
where \({\text {E}}\) is the expectation value.
Proof
The expectation value is computed as
Thus, with \(F_i(x) = \int _{x_l}^{x}{f_i(\xi )\, \mathrm {d}\xi }\)
where the third term can be reformulated by Fubini’s Proposition to
with substituting x by \(\xi \) and vice versa. Finally, this leads to
The statement follows, as the integral over positive functions is again positive. The same holds for \({\text {E}}[X_2]\) as the indices can be interchanged. \(\square \)
Proposition 3
Let \(X_i\) be a set of independent random variables with distribution \(f_i(x)\) and cdf \(F_i(x)\) for \(i\in \{1,\ldots ,n\}\) and \({Y=\min \{X_1,\ldots ,X_n\}}\). Then
Proof
By complete induction over n.
Base case \(n=2\): Proven by Proposition 2.
Step \(n-1 \rightarrow n\): Let \(Z=\min \{X_1,\ldots ,X_{n-1}\}\) then \({Y=\min \{Z, X_n\}}\). The statement follows with Proposition 2. \(\square \)
Proposition 4
Let the collision probability \(P(C|\tilde{u}, \mathbf {b}_{t_{i}})\) be defined like in Sect. 6.2 for a given trajectory \(\tilde{u}\) and obstacle distribution \(f^{t_{i}}(\mathbf {p},t)=\mathbf {b}_{t_{i}}\) which is defined as a compound distribution like in (1). Then
Proof
The expectation value is computed as
\(\square \)
Proposition 5
Let X be a random variable of a one dimensional Gaussian compound distribution
with the mean value \(\mu _c\) and variance \(\sigma ^2_c\)
Then a valid parameterized distribution \(f(X|\theta )\) is a Gaussian distribution \(\mathcal {N}(\mu _p,\sigma _p)\) with the variance \(\sigma _p\) and the mean value as the parameter \(\mu _p = \theta \) distributed according to \(f(\mu _p)\) with
Proof
We will proof, that the Gaussian compound distribution has the expected mean and variance. The variance of the compound distribution can be determined as
by the law of total variance. Since the covariance of the parameterized distribution is constant \(\sigma ^2_p=\mathrm {const}\) this can be rewritten as
with the variance of the compound distributions being \({\sigma _p^2 + (\sigma _c^2-\sigma _p^2)}\).
The expectation of the compound distribution is defined as
according to the law of total expectation. Since \({\text {E}}[f(X|\theta )] = \mu _p \sim \mathcal {N}(\mu _c,\sigma _c^2-\sigma ^2_p)\) one can see that \({\text {E}}[X] = \mu _c\). \(\square \)
Proposition 6
Let the Gaussian compound distribution \(f(\mathbf {X})\) be defined like in (1) with the mean value \(\varvec{\mu }_c\) and variance \(\varvec{\varSigma }_c\), \({\mathbf {X} = [X_1,X_2] \sim \mathcal {N}_2(\varvec{\mu }_c,\varvec{\varSigma }_c)}\). Then a valid parameterized distribution \(f(\mathbf {X}|\varvec{\theta })\) is a Gaussian distribution \(\mathcal {N}_2(\varvec{\mu }_p,\varvec{\varSigma }_p)\) with the mean value \({\varvec{\mu }_p \sim f(\varvec{\theta }) = \mathcal {N}_2(\varvec{\mu }_c,\varvec{\varSigma }_c-\varvec{\varSigma }_p)}\) and the variance \(\varvec{\varSigma }_p\).
Proof
The Covariance matrix of the compound distribution is defined as
Applying Proposition 5 to each element will show
\(\square \)
Rights and permissions
About this article
Cite this article
Althoff, D., Weber, B., Wollherr, D. et al. Closed-loop safety assessment of uncertain roadmaps. Auton Robot 40, 267–289 (2016). https://doi.org/10.1007/s10514-015-9452-1
Received:
Accepted:
Published:
Issue Date:
DOI: https://doi.org/10.1007/s10514-015-9452-1