Skip to main content
Log in

Closed-loop safety assessment of uncertain roadmaps

  • Published:
Autonomous Robots Aims and scope Submit manuscript

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.

This is a preview of subscription content, log in via an institution to check access.

Access this article

Price excludes VAT (USA)
Tax calculation will be finalised during checkout.

Instant access to the full article PDF.

Institutional subscriptions

Fig. 1
Fig. 2
Fig. 3
Fig. 4
Fig. 5
Fig. 6
Fig. 7
Fig. 8
Fig. 9
Fig. 10
Fig. 11
Fig. 12
Fig. 13

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.

    Article  Google Scholar 

  • 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.

    Article  Google Scholar 

  • 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.

    Google Scholar 

  • 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.

    Article  Google Scholar 

  • 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.

    MATH  Google Scholar 

  • 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.

    MATH  Google Scholar 

  • Chan, E., & Yaya, Y. (2009). Shortest path tree computation in dynamic graphs. IEEE Transactions on Computers, 58(4), 541–557.

    Article  Google Scholar 

  • 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.

    Article  MathSciNet  MATH  Google Scholar 

  • Doucet, A., Freitas, N. D., & Gordon, N. J. (2001). Sequential Monte Carlo methods in practice. Berlin: Springer.

    Book  MATH  Google Scholar 

  • 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.

    Article  Google Scholar 

  • 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.

    Google Scholar 

  • 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.

    Chapter  Google Scholar 

  • Gottschalk, S., Lin, M. C., & Manocha, D. (1996). Obb-tree: A hierarchical structure for rapid interference detection. Proceedings on ACM Siggraph, 96, 171–180.

    Google Scholar 

  • 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.

    Article  Google Scholar 

  • 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.

    Article  Google Scholar 

  • 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.

    Article  Google Scholar 

  • 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.

    Google Scholar 

  • 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.

    Article  Google Scholar 

  • 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.

    Google Scholar 

  • Rubin, D. B. (1984). Bayesianly justifiable and relevant frequency calculations for the applies statistician. Annals of Statistic, 12(4), 1151–1172.

    Article  MATH  Google Scholar 

  • Rubinstein, R., & Kroese, D. (1981). Simulation and the Monte Carlo method. New York: Wiley-Interscience.

    Book  MATH  Google Scholar 

  • Sisbot, E. A., & Alami, R. (2012). A human-aware manipulation planner. IEEE Transactions on Robotics, 28(5), 1045–1057.

    Article  Google Scholar 

  • 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.

    MATH  Google Scholar 

  • 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.

    Google Scholar 

  • 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.

    Article  Google Scholar 

  • 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).

Download references

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

Authors

Corresponding author

Correspondence to Daniel Althoff.

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

$$\begin{aligned} \mathcal {V}^+=\{\mathbf {v} \in \mathcal {V}/ \{\mathbf {v}_s, \mathbf {v}_g\} \mid {\text {deg}}^+(\mathbf {v})>1\}, \end{aligned}$$

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

$$\begin{aligned} {\text {E}}[Y] \le {\text {E}}[X_i],\quad i\in \{1,2\} \end{aligned}$$

where \({\text {E}}\) is the expectation value.

Proof

The expectation value is computed as

$$\begin{aligned} {\text {E}}[Y] = \int \limits _{x_l}^{x_u}{\bigl (f_1(x)(1-F_2(x)) + f_2(x)(1-F_1(x))\bigr )x \, \mathrm {d}x}. \end{aligned}$$

Thus, with \(F_i(x) = \int _{x_l}^{x}{f_i(\xi )\, \mathrm {d}\xi }\)

$$\begin{aligned} \begin{aligned} {\text {E}}[Y]&= \underbrace{\int \limits _{x_l}^{x_u}{f_1(x)x\, \mathrm {d}x}}_{{\text {E}}(X_1)} - \int \limits _{x_l}^{x_u}{\int \limits _{x_l}^x{f_2(\xi )\, \mathrm {d}\xi }\;f_1(x)x\, \mathrm {d}x}\\&\ + \int \limits _{x_l}^{x_u}{\int \limits _x^{x_u}{f_1(\xi )\, \mathrm {d}\xi }\;f_2(x)x\, \mathrm {d}x}, \end{aligned} \end{aligned}$$

where the third term can be reformulated by Fubini’s Proposition to

$$\begin{aligned} \int \limits _{x_l}^{x_u}{\int \limits _{x_l}^\xi {f_1(\xi )f_2(x)x\, \mathrm {d}x}\, \mathrm {d}\xi } =\int \limits _{x_l}^{x_u}{\int \limits _{x_l}^x{f_2(\xi )\xi \, \mathrm {d}\xi }\;f_1(x)\, \mathrm {d}x} \end{aligned}$$

with substituting x by \(\xi \) and vice versa. Finally, this leads to

$$\begin{aligned} {\text {E}}[Y] = {\text {E}}[X_1] - \int \limits _{x_l}^{x_u}{\int \limits _{x_l}^x{(x-\xi )}f_2(\xi )\, \mathrm {d}\xi \;f_1(x)\, \mathrm {d}x}. \end{aligned}$$

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

$$\begin{aligned} {\text {E}}[Y] \le {\text {E}}[X_i], \qquad \forall i\in \{1,\ldots ,n\}. \end{aligned}$$

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

$$\begin{aligned} P(C|\tilde{u}, \mathbf {b}_{t_{i}}) = {\text {E}}_{\theta _{t_i}}[P(C|\tilde{u}, \mathbf {b}_{t_{j}})],\quad t_i < t_j. \end{aligned}$$

Proof

The expectation value is computed as

$$\begin{aligned} \begin{aligned} {\text {E}}_{t_i}[P(C|\tilde{u}, t_{j})]&= \int \limits P(C|\tilde{u}, t_j) f(\varvec{\theta })\, \mathrm {d}\varvec{\theta }\\&= \int \limits \int \limits _{\mathcal {A}^{\mathrm {b}}(\tilde{u}(t))} f^{t_j}(\mathbf {p},t|\varvec{\theta }) \, \mathrm {d}\mathbf {p}\;f(\varvec{\theta })\, \mathrm {d}\varvec{\theta }\\&= \int \limits _{\mathcal {A}^{\mathrm {b}}(\tilde{u}(t))} \underbrace{\int \limits f^{t_j}(\mathbf {p},t|\varvec{\theta })f(\varvec{\theta })\, \mathrm {d}\varvec{\theta }}_{f^{t_i}(\mathbf {p},t)} \, \mathrm {d}\mathbf {p}\\&= P(C|\tilde{u}, t_{i}). \end{aligned} \end{aligned}$$

\(\square \)

Proposition 5

Let X be a random variable of a one dimensional Gaussian compound distribution

$$\begin{aligned} f(X) = {\text {E}}_{\theta } [f(X|\theta )] = \int f(X|\theta ) f(\theta ) \, \mathrm {d}\theta . \end{aligned}$$

with the mean value \(\mu _c\) and variance \(\sigma ^2_c\)

$$\begin{aligned} X \sim \mathcal {N}(\mu _c,\sigma ^2_c). \end{aligned}$$

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

$$\begin{aligned} \mu _p \sim \mathcal {N}(\mu _c,\sigma ^2_c-\sigma ^2_p). \end{aligned}$$

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

$$\begin{aligned} \begin{aligned} {\text {Var}}[X] = \int ((\mu _p - \mu _c)^2 + \sigma _p^2)f(\mu _p) \, \mathrm {d}\mu _p \end{aligned} \end{aligned}$$

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

$$\begin{aligned} \begin{aligned} {\text {Var}}[X] = \underbrace{\int f(\mu _p) \sigma _p^2 \, \mathrm {d}\theta }_{\sigma _p^2}+ \underbrace{\int f(\mu _p)(\mu _p - \mu _c)^2 \, \mathrm {d}\theta }_{{\text {Var}}(f(\mu _p))} \end{aligned} \end{aligned}$$

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

$$\begin{aligned} \begin{aligned} {\text {E}}[X] = \int {\text {E}}[f(X|\theta )]f(\theta ) \, \mathrm {d}\theta \end{aligned} \end{aligned}$$

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

$$\begin{aligned} \varvec{\varSigma }_c = \begin{bmatrix} \mathrm {E}[(X_1 - \mu _1)(X_1 - \mu _1)]&\mathrm {E}[(X_1 - \mu _1)(X_2 - \mu _2)] \\ \mathrm {E}[(X_2 - \mu _2)(X_1 - \mu _1)]&\mathrm {E}[(X_2 - \mu _2)(X_2 - \mu _2)] \end{bmatrix}. \end{aligned}$$

Applying Proposition 5 to each element will show

$$\begin{aligned} \varvec{\varSigma }_c = \varvec{\varSigma }_p + (\varvec{\varSigma }_c-\varvec{\varSigma }_p). \end{aligned}$$

\(\square \)

Rights and permissions

Reprints and permissions

About this article

Check for updates. Verify currency and authenticity via CrossMark

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

Download citation

  • Received:

  • Accepted:

  • Published:

  • Issue Date:

  • DOI: https://doi.org/10.1007/s10514-015-9452-1

Keywords

Navigation