Abstract
Estimating the fastest trajectory is one of the main challenges in autonomous vehicle research. It is fundamental that the vehicle determines its path not only to minimize travel time, but to arrive at the destination safely by avoiding any obstacles that may be in collision route. In this paper, we consider estimating the trajectory and acceleration functions of the trip simultaneously with an optimization objective function. By approximating the trajectory and acceleration function with B-splines, we transform an infinite-dimensional problem into a finite-dimensional one. Obstacle avoidance and kinematic constraints are carried out with the addition of a penalization function that penalizes trajectories and acceleration functions that do not satisfy the vehicles’ constraints or that are in a collision route with other obstacles. Our approach is designed to model observations of the obstacles that contain measurement errors, which incorporates the realistic stochasticity of radars and sensors. We show that, as the number of observations increases, the estimated optimization function converges to the optimal one where the obstacles’ positions are known. Moreover, we show that the estimated optimization function has a minimizer and that its minimizers converge to the minimizers of the optimization function involving the true threat zones.
Similar content being viewed by others
References
Atta D, Subudhi B (2013) Decentralized formation control of multiple autonomous underwater vehicles. Int J Robot Autom 28:303–310
Awerbuch B, Betke M, Rivest RL, Singh M (1999) Piecemeal graph exploration by a mobile robot. Inf Comput 152:155–172
Barraquand J, Latombe J (1993) Nonholonomic multibody mobile robots: controllability and motion planning in the presence of obstacles. Algorithmica 10:121–155
Belkouche F (2009) Reactive path planning in a dynamic environment. IEEE Trans Robot 25:902–911
Bodin P, Villemoes LF, Wahlberg B (2000) Selection of best orthonormal rational basis. SIAM J Control Optim 38(4):995–1032
Borenstein J, Koren Y (1989) Real-time obstacle avoidance for fast mobile robots. IEEE Trans Syst Man Cybern 19:1179–1187
Borenstein J, Koren Y (1991) The vector field histogram-fast obstacle avoidance for mobile robots. IEEE Trans Robot Autom 7:278–288
Borrelli F, Falcone P, Keviczky T, Asgari J, Hrovat D (2005) Mpc-based approach to active steering for autonomous vehicle systems. Int J Veh Auton Syst 3:265–291
Bouthemy P (1989) A maximum likelihood framework for determining moving edges. IEEE Trans Pattern Anal Mach Intell 11:499–511
Brooks RA (1982) Solving the find path problem by representing free space as generalized cones. Massachusetts Institute Technology, A.I Memo No 674
Chiang H-T, Malone N, Lesser K, Oishi M, Tapia L (2015a) Aggressive moving obstacle avoidance using a stochastic reachable set based potential field. In: Springer tracts in advanced robotics book series, Algorithmic foundations of robotics XI, pp 73–89
Chiang H-T, Malone N, Lesser K, Oishi M, Tapia L (2015b) Path-guided artificial potential fields with stochastic reachable sets for motion planning in highly dynamic environments. In: IEEE international conference on robotics and automation
Chiang H-T, Rackley N, Tapia L (2015c) Stochastic ensemble simulation motion planning in stochastic dynamic environments
Coutinho WP, Battarra M, Fliege J (2018) The unmanned aerial vehicle routing and trajectory optimisation problem, a taxonomic review. Comput Ind Eng 120:116–128
de Boor C (1978) A practical guide to splines, applied mathematical sciences. Springer, New York
de Ponte Muller F (2017) Survey on ranging sensors and cooperative techniques for relative positioning of vehicles. Sensors (Basel) 17:271
DeVore R, Petrova G, Temlyakov V (2003) Best basis selection for approximation in l p. Found Comput Math 3(2):161–185
Dias R, Garcia N, Zambom AZ (2010) A penalized nonparametric method for nonlinear constrained optimization based on noisy data. Comput Optim Appl 45:521–541
Dias R, Garcia N, Zambom AZ (2012) Monte Carlo algorithm for trajectory optimization based on Markovian readings. Comput Optim Appl 51:305–321
Donald B (1984) Motion planning with six degrees of freedom. Massachusetts Institute Technology Artificial Intelligence Laboratory, Technical report AIM-791
Elnagar A, Gupta K (1998) Motion prediction of moving objects based on auto regressive model. IEEE Trans Syst Man Cybern 28:803–810
Ferguson D, Stentz A (2006) Using interpolation to improve path planning: the field d* algorithm. J Field Robot 23:79–101
Fiorini P, Shiller Z (1993) Motion planning in dynamic environments using the relative velocity paradigm. In: Proceedings of IEEE international conference on robotics and automation, pp 560–565
Fujimura K, Samet H (1989) A hierarchical strategy for path planning among moving obstacles. IEEE Trans Robot Autom 5:61–69
Galceran E, Carreras M (2013) A survey on coverage path planning for robotics. Robot Auton Syst 61:1258–1276
Giyanani A, Bierbooms W, van Bussel G (2015) Lidar uncertainty and beam averaging correction. Adv Sci Res 12:85–89
Gong C, Tully S, Kantor G, Choset H (2012) Multi-agent deterministic graph mapping via robot rendezvous. In: IEEE international conference on robotics and automation
Gonzalez J, Dornbush A, Likhachev M (2012) Using state dominance for path planning in dynamic environments with moving obstacles. In: Proceedings of the IEEE international conference on robotics and automation, pp 4009–4015
Goulden T, Hopkinson C (2013) Quantification of LiDAR measurement uncertainty through propagation of errors due to sensor sub-systems and terrain morphology. In: AGU fall meeting abstracts
Hsu D, Kindel R, Latombe J-C, Rock S (2002) Randomized kinodynamic motion planning with moving obstacles. Int J Robot Res 21:233–255
Hsu D, Latombe J, Motwani R (1997) Path planning in expansive configuration spaces. In: Proceedings of IEEE international conference on robotics and automation, pp 2719–2726
Huang C, Shi S, Wang X, Chung W (2005) Parallel force/position controls for robot manipulators with uncertain kinematics. Int J Robot Autom 20:158–168
Kala R, Shukla A, Tiwari R (2012) Robot path planning using dynamic programming with accelerating nodes. Paladyn 3:23–24
Kambhampati S, Davis LS (1986) Multiresolution path planning for mobile robots. Int J Robot Res 5:90–98
Karlin S (1973) Some variational problems on certain Sobolev spaces and perfect splines. Bull Am Math Soc 79:124–128
Kavraki L, Svestka P, Latombe J, Overmars M (1996) Probabilistic roadmaps for path planning in high-dimensional configuration spaces. IEEE Trans Robot Autom 12:566–580
Koenig S, Likhachev M (2002) D* lite. In: Proceedings of the AAAI conference of artificial intelligence, pp 476–483
Krishnan J, Rajeev U, Jayabalan J, Sheela D (2017) Optimal motion planning based on path length minimisation. Robot Auton Syst 94:245–263
Kruse T, Pandey AK, Alami R, Kirsch A (2013) Human-aware robot navigation: a survey. Robot Auton Syst 61:1726–1743
Kuwata Y, Teo J, Fiore G, Karaman S, Frazzoli E, How JP (2009) Real-time motion planning with applications to autonomous urban driving. IEEE Trans Control Syst Technol 17:1105–1118
Lachout P, Liebscher E, Vogel S (2005) Strong convergence of estimators as \(\epsilon _n\)-minimisers of optimization problems. Ann Inst Stat Math 57:291–313
LaValle S (1998) Rapidly-exploring random trees: a new tool for path planning. Technical Report 98-11, Computer Science Dept., Iowa State University
Lindemann S, LaValle S (2009) Simple and efficient algorithms for computing smooth, collision-free feedback laws over given cell decompositions. Int J Robot Res 28:600–621
Luus R (2007) Choosing grid points in solving singular optimal control problems by iterative dynamic programming. In: Proceedings of the 10th IASTED international conference on intelligent systems and control, p 592
Macrina G, Di Puglia Pugliese L, Guerriero F, Laporte G (2020) Drone-aided routing: a literature review. Transport Res Part C Emerg Technol 120:102762
Mahdavian M, Shariat-Panahi M, Yousefi-Koma A, Ghasemi-Toudeshki A (2015) Optimal trajectory generation for energy consumption minimization and moving obstacle avoidance of a 4dof robot arm. In: 3rd RSI international conference on robotics and mechatronics, pp 353–358
Malone N, Lesser K, Oishi M, Tapia L (2014) Stochastic reachability based motion planning for multiple moving obstacle avoidance. In: Proceedings of the 17th international conference on hybrid systems: computation and control (New York, NY, USA), HSCC ’14. ACM, pp 51–60
Matveev AS, Wang C, Savki AV (2012) Real-time navigation of mobile robots in problems of border patrolling and avoiding collisions with moving and deforming obstacles. Robot Auton Syst 60:769–788
Milford MJ (2008) Robot navigation from nature: simultaneous localisation, mapping, and path planning based on hippocampal models. Springer Tracts in Advanced Robotics, Berlin, p 41
Mitteta M-A, Nouiraa H, Roynarda X, Goulettea F, Deschauda J-E (2016) Experimental assessment of the quanergy m8 lidar sensor. In: The international archives of the photogrammetry, remote sensing and spatial information sciences, XXIII ISPRS Congress, Prague XLI-B5, pp 12–19
Nakhaei A, Lamiraux F (2008) A framework for planning motions in stochastic maps. In: 2008 10th international conference on control, automation, robotics and vision, pp 1959–1964
Nanao M, Ohtsuka T (2010) Nonlinear model predictive control for vehicle collision avoidance using c/gmres algorithm. In: IEEE international conference on control applications, Yokohama
Naranjo JE, Gonzalez C, Garcia R, Pedro T (2008) Lane-change fuzzy control in autonomous vehicles for the overtaking maneuver. IEEE Trans Intell Transport Syst 9:438–450
Nash A, Daniel K, Koenig S, Felner A (2007) Theta*: any-angle path planning on grids. In: Proceedings of the national conference on artificial intelligence, pp 1177–1183
Ni J, Wu W, Shen J, Fan X (2014) An improved vff approach for robot path planning in unknown and dynamic environments. In: Computational Intelligence Approaches to Robotics, Automation, and Control, vol 2014, Article ID 461237. https://doi.org/10.1155/2014/461237
Pandey A, Parhi DR (2017) Optimum path planning of mobile robot in unknown static and dynamic environments using fuzzy-wind driven optimization algorithm. Def Technol 13:47–58
Park J, Kim D, Yoon Y, Kim HJ, Yi K (2009) Obstacle avoidance of autonomous vehicles based on model predictive control. Proc IMechE Part D J Automob Eng 223:1499–1516
Park J, Kim HJ (2016) Generation of locally optimal trajectories against moving obstacles using gaussian sampling. In: 16th international conference on control, automation and systems, pp 273–277
Petres C, Pailhas Y, Patron P, Petillot Y, Evans J, Lane D (2007) Path planning for autonomous underwater vehicles. IEEE Trans Robot 23:331–341
Qiang H, Yokoi K, Kajita S, Kaneko K, Arai H, Koyachi N, Tanie K (2001) Planning walking patterns for a biped robot. IEEE Trans Robot Autom 17:280–289
Reif U (1997) Orthogonality of cardinal b-splines in weighted Sobolev spaces. SIAM J Math Anal 28:1258–1263
Reif U (1997) Uniform b-spline approximation in Sobolev spaces. Numer Algorithms 15:1–14
Rubagotti M, Vedova M, Ferrara A (2011) Time-optimal sliding mode control of a mobile robot in a dynamic environment. IET Control Theory Appl 5:1916–1924
Sahraei A, Manzuri MT, Razvan MR, Tajfard M, Khoshbakht S (2007) Real-time trajectory generation for mobile robots. In: Proceedings of the 10th congress of the Italian association for artificial intelligence on AI*IA, artificial intelligence and human-oriented computing, Rome
Sen PK, Singer JM (1993) Large sample methods in statistics. an introduction with applications. Chapman & Hall, New York
Sgorbissa A, Zaccaria R (2012) Planning and obstacle avoidance in mobile robotics. Robot Auton Syst 60:628–638
Shiller Z, Large F, Sekhavat S (2001) Motion planning in dynamic environments: obstacles moving along arbitrary trajectories. In: Proceedings of IEEE international conference on robotics and automation
Silverman B (1986) Density estimation for statistics and data analysis. Chapman and Hall, London
Stentz A (1994) Optimal and efficient path planning for partially-known environments. In: Proceedings of the 1994 IEEE international conference on robotics and automation, pp 3310 – 3317
Takano G, Obayashi M, Uto K (2015) Path planning for autonomous car to avoid moving obstacles by steering using tangent-arc-tangent-arc-tangent model. In: IEEE conference on control applications, pp 1702–1709
Tazir M, Azouaoui O, Hazerchi M, Brahimi M (2015) Mobile robot path planning for complex dynamic environments. In: International conference on advanced robotics, pp 200–206
Tiwari A, Chandra H, Yadegar J, Wang J (2007) Constructing optimal cyclic tours for planar exploration and obstacle avoidance: a graph theory approach. Advances in variable structure and sliding mode control. Springer, Berlin
Toit NED, Burdick JW (2011) Robot motion planning in dynamic, uncertain environments. IEEE Trans Robot 28:101–115
van den Berg J, Lin M, Manocha D (2008) Reciprocal velocity obstacles for real-time multi-agent navigation. In: Proceedings of IEEE international conference on robotics and automation, pp 1928–1935
Vidakovic B (1999) Statistical modeling by wavelets. Wiley series in probability and statistics: applied probability and statistics. Wiley-Interscience, New York
Volos CK, Kyprianidis M, Stouboulos N (2012) A chaotic path planning generator for autonomous mobile robots. Robot Auton Syst 60:651–656
Wooden DT (2006) Graph-based path planning for mobile robots. Dissertation—Georgia Institute of Technology
Yang K, Sukkarieh S (2008) Real-time continuous curvature path planning of UAVs in cluttered environments. In: Proceedings of the 5th international symposium on mechatronics and its applications, pp 1–6
Ye C, Yung NHC, Wang D (2003) A fuzzy controller with supervised learning assisted reinforcement learning algorithm for obstacle avoidance. IEEE Trans Syst Man Cybern B 33:17–27
Yoon Y, Shin J, Kim HJ, Park Y, Sastry S (2009) Model-predictive active steering and obstacle avoidance for autonomous ground vehicles. Control Eng Pract 17:741–750
Yu J, LaValle SM (2016) Optimal multirobot path planning on graphs: complete algorithms and effective heuristics. IEEE Trans Robot 32:1163–1177
Zambom AZ, Seguin B, Zhao F (2018) Robot path planning in a dynamic environment with stochastic measurements. J Glob Optim. https://doi.org/10.1007/s10898-018-0704-4
Author information
Authors and Affiliations
Corresponding author
Additional information
Communicated by Joerg Fliege.
Publisher's Note
Springer Nature remains neutral with regard to jurisdictional claims in published maps and institutional affiliations.
Appendices
Appendix
Proof of Lemma 3
Proof
We need to show that for all \({\varvec{\omega }}_0 =({\varvec{\theta }}_0 ,{\varvec{\beta }}_0 )\in D_0(v_0)\subset {\mathbb {R}}^{K_1}\times {\mathbb {R}}^{K_2}\)
By performing a change of variables, (7), which is used to implicitly define \(p_{t1}({\varvec{\omega }})\), can be rewritten in the form
where
Notice that G is continuous and \(G(u,{\varvec{\theta }})\ge 1\). Moreover, since \(\tau \) is continuous, so is F.
Since the functions \(B_k\) are only defined on [0, b] and the functions \(C_k\) are only defined on [0, 1], \(G(u,{\varvec{\theta }})\) is only defined for \(0\le u\le b\) and \(F(s,{\varvec{\omega }})\) is only defined for \(0 \le s\le \tau ({\varvec{\omega }})\). For the purposes of this proof, let us extend the B-spline basis functions \(B_k\) and \(C_k\), so that they are zero past their original domains of definition. Doing so means that \(G(u,{\varvec{\theta }})\) will be defined for all \(u\ge 0\) and \(F(s,{\varvec{\omega }})\) will be defined for all \(s\ge 0\). Moreover, this will result in \(F(s,{\varvec{\omega }})\) being constant for \(s\ge \tau ({\varvec{\omega }})\). Notice that \(F(\tau ({\varvec{\omega }}),{\varvec{\omega }})\) is related to the horizontal speed of the vehicle at the time it reaches the destination. Since the trajectory of the vehicle is determined by a function, it must approach the destination from the left. Hence, it follows that \(F(\tau ({\varvec{\omega }}),{\varvec{\omega }})\ge 0\). Putting this together with the fact that \(F(s,{\varvec{\omega }})\) is constant for \(s\ge \tau ({\varvec{\omega }})\), we obtain
These extensions allow us to define \(q_t({\varvec{\omega }})\) implicitly through
for any \(t\ge T_0\). For \(t\in [T_0,T_0+\tau ({\varvec{\omega }})]\), \(q_t({\varvec{\omega }})=p_{t1}({\varvec{\omega }})\); however, for \(t\ge T_0+\tau ({\varvec{\omega }})\), by (20) and (21), \(q_t({\varvec{\omega }})\ge p_{1t}({\varvec{\omega }})\). We will begin by showing that for any \(M>T_0\)
Fix \(t\ge T_0\) and \(({\varvec{\theta }},{\varvec{\beta }}),({\varvec{\theta }}_0,{\varvec{\beta }}_0)\in D_0(v_0)\). Under the assumption that \(q_{t}({\varvec{\theta }}_0 ,{\varvec{\beta }}_0 )\ge q_{t}({\varvec{\theta }}_0 ,{\varvec{\beta }})\), since \(G\ge 1\), by (21), we have
A similar argument shows that this inequality also holds if \(q_{t}({\varvec{\theta }}_0 ,{\varvec{\beta }}_0 )< q_{t}({\varvec{\theta }}_0 ,{\varvec{\beta }})\). Similarly, under the assumption that \(q_{t}({\varvec{\theta }}_0 ,{\varvec{\beta }}_0 )\ge q_{t}({\varvec{\theta }},{\varvec{\beta }}_0 )\), we have
A similar argument shows that this inequality also holds if \(q_{t}({\varvec{\theta }}_0 ,{\varvec{\beta }}_0 )< q_{t}({\varvec{\theta }},{\varvec{\beta }}_0 )\).
The above inequalities show that to establish (22), it suffices to show that
To obtain (23), notice that
where \(M_2=\sup _{u\ge 0 } |\mathbf{B}(u)|\). The result (23) then immediately follows from the fact that the function \(t\mapsto q_t({\varvec{\theta }}_0 ,{\varvec{\beta }}_0 )\) is continuous. To obtain (24), write F in the form
where
are continuous functions of \({\varvec{\omega }}\). For all \({\varvec{\omega }}\), we have
where \(M_3=\sup _{u\ge 0 } |\mathbf{C}(u)|\). It follows that:
and so, (24) holds, since e and \({\varvec{\eta }}\) are continuous.
Since \(p_{t1}({\varvec{\omega }})=q_t({\varvec{\omega }})\) for \(t\in [T_0,T_0+\tau ({\varvec{\omega }})]\) and \(p_{t1}({\varvec{\omega }})\le q_t({\varvec{\omega }})\) for \(t>T_0+\tau ({\varvec{\omega }})\), it can be shown that
and so, from (22), we have
for all \(M>T_0\). Since \(\tau \) is continuous, there is a \(M>0\), such that for all \({\varvec{\omega }}\in D_0(v_0)\) satisfying \(|{\varvec{\omega }}-{\varvec{\omega }}_0 |<1\) we have \(T_0+\tau ({\varvec{\omega }})\le M\). Using this fact and the fact that \(p_{t1}({\varvec{\omega }})\) is constant for \(t\ge T_0+\tau ({\varvec{\omega }})\), it follows that:
To finish the proof, we must now establish a result of the form (26) with the first component of \(\mathbf{p}_t\) replaced by the second component [see (6)]. However, this follows from the fact that the B-spline basis functions \(B_k\) are continuously differentiable and that \(p_{t1}\) satisfies (26).\(\square \)
Proof of Lemma 4
Proof
Fix \({\varvec{\omega }}_0 =({\varvec{\theta }}_0 ,{\varvec{\beta }}_0 )\in D_0(v_0)\). Since \(\tau \) is continuous, find \(\delta >0\), such that if \({\varvec{\omega }}\in D_0(v_0)\) and \(|{\varvec{\omega }}-{\varvec{\omega }}_0|<\delta \), then \(0<|\tau ({\varvec{\omega }})-\tau ({\varvec{\omega }}_0)|<\tau ({\varvec{\omega }}_0)/2\). Now, consider \({\varvec{\omega }}=({\varvec{\theta }},{\varvec{\beta }})\in D_0(v_0)\) with \(|{\varvec{\omega }}_0 -{\varvec{\omega }}|<\delta \). By the triangle inequality and (4), we have for \(t\in [T_0,\infty )\)
where here we use a constant extension of \(C_k\) past the interval [0, 1] to ensure that \(a_{\varvec{\omega }}(t)\) is constant past the interval \([T_0,T_0+\tau ({\varvec{\omega }})]\). We will investigate three different regimes for t. If \(t\ge T_0+\max \{\tau ({\varvec{\omega }}),\tau ({\varvec{\omega }}_0)\}\), then
Now, assume that \(t\le T_0+\min \{\tau ({\varvec{\omega }}),\tau ({\varvec{\omega }}_0)\}\). Since the B-splines \(C_k\) are smooth functions, there is a constant \(M_4>0\), such that \(|C'_k(s)|\le M_4\) for all \(s\in [0,1]\) and all \(k=1,\dots ,K_2\). Thus, it follows from the mean value theorem that:
For the final case, we consider t between \(T_0+\tau ({\varvec{\omega }}_0)\) and \(T_0+\tau ({\varvec{\omega }})\). Here, we assume that \(\tau ({\varvec{\omega }}_0)\le \tau ({\varvec{\omega }})\). The case where \(\tau ({\varvec{\omega }})\le \tau ({\varvec{\omega }}_0)\) is treated in a similar manner. Since \(T_0+\tau ({\varvec{\omega }}_0)\le t\le T_0+\tau ({\varvec{\omega }})\), we can use the mean value theorem again to find that
Putting these cases together, we find that there is a constant \(M_5\), such that for all \(t\ge T_0\)
Since the \(C_k\) are bounded functions, by possibly enlarging \(M_5\), we may assume that \(|C_k(s)|\le M_5\) for all \(s\in [0,1]\) and all \(k=1,\ldots ,K_2\). Putting these facts together yields
for all \(t\in [T_0,\infty )\). Since \(\tau \) is continuous, it follows that \(a_{{\varvec{\omega }}}(t)\) is equicontinuous in t.
The equicontinuity in t of \(\int _0^t a_{{\varvec{\omega }}}(u)du\) follows from the equicontinuity in t of \(a_{{\varvec{\omega }}}(t)\). \(\square \)
1.1 Derivation of the maximum-likelihood estimators
The partial derivatives of \(L({\varvec{\eta }}_0^\ell , \mathbf{V}^\ell , \mathbf{W}^\ell |\mathbf {N}^\ell )\) with respect to the parameters \({\varvec{\eta }}_0^\ell , \mathbf{V}^\ell \), and \(\mathbf{W}^\ell \) are
Equating these to zero, we obtain the following system of equations:
where \(c_j = \sum _{t=1}^T t^j\). Substituting \({\varvec{\eta }}_0^\ell \) from the first equation into the second equation, and the resulting \({\widehat{\mathbf{V}}}^\ell \) into the third equation, we obtain
which in sequence define the maximum-likelihood estimators.
Derivation of Var\(({\hat{{\varvec{\eta }}}}_t^\ell )\)
After a lengthy calculation, one can write
where \(a = c_1c_2/T - c_3\), \(b = c_2-c_1^2/T\), \(d = c_4-c_2^2/T - a^2/b\), and
Rights and permissions
About this article
Cite this article
Zambom, A.Z., Seguin, B. Fastest motion planning for an unmanned vehicle in the presence of accelerating obstacles. Comp. Appl. Math. 40, 119 (2021). https://doi.org/10.1007/s40314-021-01511-9
Received:
Revised:
Accepted:
Published:
DOI: https://doi.org/10.1007/s40314-021-01511-9