Efficient Optimization-Based Trajectory Planning for Unmanned Systems in Confined Environments
Pages 18547 - 18560
Abstract
This research addresses the increasing demand for advanced navigation systems capable of operating within confined surroundings. A significant challenge in this field is developing an efficient planning framework that can generalize across various types of collision avoidance missions. Utilizing numerical optimal control techniques, this study proposes a unified optimization-based planning framework to meet these demands. We focus on handling two collision avoidance problems, i.e., the object not colliding with obstacles and not colliding with boundaries of the constrained region. The object or obstacle is denoted as a union of convex polytopes and ellipsoids, and the constrained region is denoted as an intersection of such convex sets. Using these representations, collision avoidance can be approached by formulating explicit constraints that separate two convex sets, or ensure that a convex set is contained in another convex set, referred to as separating constraints and containing constraints, respectively. We propose to use the hyperplane separation theorem to formulate differentiable separating constraints, and utilize the S-procedure and geometrical methods to formulate smooth containing constraints. We state that compared to the state of the art, the proposed formulations allow a considerable reduction in nonlinear program size and geometry-based initialization in auxiliary variables used to formulate collision avoidance constraints. Finally, the efficacy of the proposed unified planning framework is evaluated in two contexts, autonomous parking in tractor-trailer vehicles and overtaking on curved lanes. The results in both cases exhibit an improved computational performance compared to existing methods.
References
[1]
C. Dietz, S. Albrecht, A. Nurkanovic, and M. Diehl, “Efficient collision modelling for numerical optimal control,” in Proc. Eur. Control Conf. (ECC), Bucharest, Romania, Jun. 2023, pp. 1–7.
[2]
J. Karlsson, N. Murgovski, and J. Sjöberg, “Computationally efficient autonomous overtaking on highways,” IEEE Trans. Intell. Transp. Syst., vol. 21, no. 8, pp. 3169–3183, Aug. 2020.
[3]
X. Zhang, A. Liniger, and F. Borrelli, “Optimization-based collision avoidance,” IEEE Trans. Control Syst. Technol., vol. 29, no. 3, pp. 972–983, May 2021.
[4]
J. Schulman et al., “Motion planning with sequential convex optimization and convex collision checking,” Int. J. Robot. Res., vol. 33, no. 9, pp. 1251–1270, Aug. 2014.
[5]
V. Sunkara, A. Chakravarthy, and D. Ghose, “Collision avoidance of arbitrarily shaped deforming objects using collision cones,” IEEE Robot. Autom. Lett., vol. 4, no. 2, pp. 2156–2163, Apr. 2019.
[6]
S. Haddadin, A. De Luca, and A. Albu-Schäffer, “Robot collisions: A survey on detection, isolation, and identification,” IEEE Trans. Robot., vol. 33, no. 6, pp. 1292–1312, Dec. 2017.
[7]
S. Helling and T. Meurer, “Dual collision detection in model predictive control including culling techniques,” IEEE Trans. Control Syst. Technol., vol. 31, no. 6, pp. 2449–2464, Nov. 2023. 10.1109/TCST.2023.3259822.
[8]
B. Li, T. Acarman, Y. Zhang, L. Zhang, C. Yaman, and Q. Kong, “Tractor-trailer vehicle trajectory planning in narrow environments with a progressively constrained optimal control approach,” IEEE Trans. Intell. Vehicles, vol. 5, no. 3, pp. 414–425, Sep. 2020.
[9]
B. Li et al., “Optimization-based trajectory planning for autonomous parking with irregularly placed obstacles: A lightweight iterative framework,” IEEE Trans. Intell. Transp. Syst., vol. 23, no. 8, pp. 11970–11981, Aug. 2022.
[10]
P. F. Lima, “Optimization-based motion planning and model predictive control for autonomous driving: With experimental evaluation on a heavy-duty construction truck,” Ph.D. dissertation, Dept. Autom. Control, KTH Royal Inst. Technol., Stockholm, Sweden, 2018.
[11]
J. Zhou, B. Olofsson, and E. Frisk, “Interaction-aware motion planning for autonomous vehicles with multi-modal obstacle uncertainty predictions,” IEEE Trans. Intell. Vehicles, vol. 9, no. 1, pp. 1305–1319, Jan. 2024. 10.1109/TIV.2023.3314709.
[12]
J. Zhou, B. Olofsson, and E. Frisk, “Interaction-aware moving target model predictive control for autonomous vehicles motion planning,” in Proc. Eur. Control Conf. (ECC), 2022, pp. 154–161.
[13]
J. Hu, H. Zhang, L. Liu, X. Zhu, C. Zhao, and Q. Pan, “Convergent multiagent formation control with collision avoidance,” IEEE Trans. Robot., vol. 36, no. 6, pp. 1805–1818, Dec. 2020.
[14]
D. Ioan, I. Prodan, S. Olaru, F. Stoican, and S.-I. Niculescu, “Mixed-integer programming in motion planning,” Annu. Rev. Control, vol. 51, pp. 65–87, Feb. 2021.
[15]
I. E. Grossmann, “Review of nonlinear mixed-integer and disjunctive programming techniques,” Optim. Eng., vol. 3, pp. 227–252, Sep. 2002.
[16]
S. M. LaValle, Planning Algorithms. Cambridge, U.K.: Cambridge Univ. Press, 2006.
[17]
R. Deits and R. Tedrake, “Efficient mixed-integer planning for UAVs in cluttered environments,” in Proc. IEEE Int. Conf. Robot. Autom. (ICRA), Seattle, WA, USA, May 2015, pp. 42–49.
[18]
D. Mellinger, A. Kushleyev, and V. Kumar, “Mixed-integer quadratic program trajectory generation for heterogeneous quadrotor teams,” in Proc. IEEE Int. Conf. Robot. Autom., May 2012, pp. 477–483.
[19]
I. Prodan, F. Stoican, S. Olaru, and S. I. Niculescu, Mixed-Integer Representations in Control Design Mathematical Foundations and Applications. Cham, Switzerland: Springer. 2015.
[20]
J. P. Vielma, “Mixed integer linear programming formulation techniques,” SIAM Rev., vol. 57, no. 1, pp. 3–57, Jan. 2015.
[21]
X. Wang, M. Kloetzer, C. Mahulea, and M. Silva, “Collision avoidance of mobile robots by using initial time delays,” in Proc. 54th IEEE Conf. Decis. Control (CDC), Dec. 2015, pp. 324–329.
[22]
I. Haghighi, S. Sadraddini, and C. Belta, “Robotic swarm control from spatio-temporal specifications,” in Proc. IEEE 55th Conf. Decis. Control (CDC), Las Vegas, NV, USA, Dec. 2016, pp. 5708–5713.
[23]
J. Karlsson, N. Murgovski, and J. Sjoberg, “Comparison between mixed-integer and second order cone programming for autonomous overtaking,” in Proc. Eur. Control Conf. (ECC), Limassol, Cyprus, 2018, pp. 386–391.
[24]
F. Molinari, N. N. Anh, and L. Del Re, “Efficient mixed integer programming for autonomous overtaking,” in Proc. Amer. Control Conf. (ACC), Seattle, WA, USA, May 2017, pp. 2303–2308.
[25]
H. P. Williams, Model Building in Mathematical Programming. Hoboken, NJ, USA: Wiley, 2013.
[26]
X. Zhang, A. Liniger, A. Sakai, and F. Borrelli, “Autonomous parking using optimization-based collision avoidance,” in Proc. IEEE Conf. Decis. Control (CDC), Dec. 2018, pp. 4327–4332.
[27]
I. Ballesteros-Tolosana, S. Olaru, P. Rodriguez-Ayerbe, G. Pita-Gil, and R. Deborne, “Collision-free trajectory planning for overtaking on highways,” in Proc. IEEE 56th Annu. Conf. Decis. Control (CDC), Melbourne, VIC, Australia, Dec. 2017, pp. 2551–2556.
[28]
R. J. M. Afonso, R. K. H. Galvão, and K. H. Kienitz, “Reduction in the number of binary variables for inter-sample avoidance in trajectory optimizers using mixed-integer linear programming,” Int. J. Robust Nonlinear Control, vol. 26, no. 16, pp. 3662–3669, Nov. 2016.
[29]
F. Stoican, I. Prodan, and S. Olaru, “Hyperplane arrangements in mixed-integer programming techniques. Collision avoidance application with zonotopic sets,” in Proc. Eur. Control Conf. (ECC), Zurich, Switzerland, Jul. 2013, pp. 3155–3160.
[30]
A. Ganesan, S. Gros, and N. Murgovski, “Numerical strategies for mixed-integer optimization of power-split and gear selection in hybrid electric vehicles,” IEEE Trans. Intell. Transp. Syst., vol. 24, no. 3, pp. 3194–3210, Mar. 2023.
[31]
C. Kirches, Fast Numerical Methods for Mixed-Integer Nonlinear Model-Predictive Control. Cham, Switzerland: Springer, 2010.
[32]
H. Febbo, J. Liu, P. Jayakumar, J. L. Stein, and T. Ersal, “Moving obstacle avoidance for large, high-speed autonomous ground vehicles,” in Proc. Amer. Control Conf. (ACC), Seattle, WA, USA, May 2017, pp. 5568–5573.
[33]
U. Rosolia, S. De Bruyne, and A. G. Alleyne, “Autonomous vehicle control: A nonconvex approach for obstacle avoidance,” IEEE Trans. Control Syst. Technol., vol. 25, no. 2, pp. 469–484, Mar. 2017.
[34]
M. Geisert and N. Mansard, “Trajectory generation for quadrotor based systems using numerical optimal control,” in Proc. IEEE Int. Conf. Robot. Autom. (ICRA), Stockholm, Sweden, May 2016, pp. 2958–2964.
[35]
J. Zeng, B. Zhang, and K. Sreenath, “Safety-critical model predictive control with discrete-time control barrier function,” in Proc. IEEE Amer. Control Conf. (ACC), 2021, pp. 3882–3889.
[36]
A. Thirugnanam, J. Zeng, and K. Sreenath, “Safety-critical control and planning for obstacle avoidance between polytopes with control barrier functions,” in Proc. Int. Conf. Robot. Autom. (ICRA), Philadelphia, PA, USA, May 2022, pp. 286–292.
[37]
R. Chai, A. Tsourdos, A. Savvaris, S. Chai, Y. Xia, and C. L. P. Chen, “Design and implementation of deep neural network-based control for automatic parking maneuver process,” IEEE Trans. Neural Netw. Learn. Syst., vol. 33, no. 4, pp. 1400–1413, Apr. 2022.
[38]
J. Fan, N. Murgovski, and J. Liang, “Efficient collision avoidance for autonomous vehicles in polygonal domains,” 2023, arXiv:2308.09103.
[39]
J. Guthrie, “A differentiable signed distance representation for continuous collision avoidance in optimization-based motion planning,” in Proc. IEEE Conf. Decision Control (CDC), Sep. 2022, pp. 7214–7221.
[40]
Y. Zheng and K. Yamane, “Generalized distance between compact convex sets: Algorithms and applications,” IEEE Trans. Robot., vol. 31, no. 4, pp. 988–1003, Aug. 2015.
[41]
C. Landry, D. Hömberg, R. Henrion, and M. Gerdts, “Path planning and collision avoidance for robots,” Numer. Algebra, Control Optim., vol. 2, no. 3, pp. 437–463, Aug. 2012.
[42]
M. Lutz and T. Meurer, “Efficient formulation of collision avoidance constraints in optimization based trajectory planning and control,” in Proc. IEEE Conf. Control Technol. Appl. (CCTA), Aug. 2021, pp. 228–233.
[43]
V. Soltan, “Support and separation properties of convex sets in finite dimension,” Extracta Mathematicae, vol. 36, no. 2, pp. 241–278, Dec. 2021.
[44]
T. Rockafellar, Convex Analysis. Princeton, NJ, USA: Princeton Univ. Press, 1970.
[45]
S. Boyd and L. Vandenberghe, Convex Optimization. Cambridge, U.K.: Cambridge Univ. Press, 2009.
[46]
J. Tao, S. Scott, W. Joe, and D. Mathieu, “A geometric construction of coordinates for convex polyhedra using polar duals,” in Proc. Symp. Geom. Process., Vienna, Austria, pp. 181–186, 2005.
[47]
C. A. Hayes, “The Heine–Borel theorem,” Amer. Math. Monthly, vol. 63, no. 3, p. 180, Mar. 1956.
[48]
M. Kesäniemi and K. Virtanen, “Direct least square fitting of hyperellipsoids,” IEEE Trans. Pattern Anal. Mach. Intell., vol. 40, no. 1, pp. 63–76, Jan. 2018.
[49]
J. M. Aldaz, S. Barza, M. Fujii, and M. S. Moslehian, “Advances in operator cauchy-schwarz inequalities and their reverses,” Ann. Funct. Anal., vol. 6, no. 3, pp. 275–295, 2015.
[50]
I. Pólik and T. Terlaky, “A survey of the S-lemma,” SIAM Rev., vol. 49, no. 3, pp. 371–418, Jan. 2007.
[51]
R. A. Horn and C. R. Johnson, Matrix Analysis. Cambridge, U.K.: Cambridge Univ. Press, 1985.
[52]
J. Funke, M. Brown, S. M. Erlien, and J. C. Gerdes, “Collision avoidance and stabilization for autonomous vehicles in emergency scenarios,” IEEE Trans. Control Syst. Technol., vol. 25, no. 4, pp. 1204–1216, Jul. 2017.
[53]
J. Wang, Y. Yan, K. Zhang, Y. Chen, M. Cao, and G. Yin, “Path planning on large curvature roads using driver-vehicle-road system based on the kinematic vehicle model,” IEEE Trans. Veh. Technol., vol. 71, no. 1, pp. 311–325, Jan. 2022.
[54]
J. A. E. Andersson, J. Gillis, G. Horn, J. B. Rawlings, and M. Diehl, “CasADi: A software framework for nonlinear optimization and optimal control,” Math. Program. Comput., vol. 11, no. 1, pp. 1–36, Mar. 2019.
[55]
A. Wächter and L. T. Biegler, “On the implementation of an interior-point filter line-search algorithm for large-scale nonlinear programming,” Math. Program., vol. 106, no. 1, pp. 25–57, Mar. 2006.
[56]
A. C. Manav and I. Lazoglu, “A novel cascade path planning algorithm for autonomous truck-trailer parking,” IEEE Trans. Intell. Transp. Syst., vol. 23, no. 7, pp. 6821–6835, Jul. 2022.
[57]
E. Börve, N. Murgovski, and L. Laine, “Interaction-aware trajectory prediction and planning in dense highway traffic using distributed model predictive control,” 2023, arXiv:2308.13053.
[58]
X. Ruan, Y. Huang, Y. Wang, Z. Fu, L. Xiong, and Z. Yu, “An efficient trajectory planning method with a reconfigurable model for any tractor-trailer vehicle,” IEEE Trans. Transport. Electrific., vol. 9, no. 2, pp. 3360–3374, Sep. 2022.
[59]
A. Elawad, N. Murgovski, M. Jonasson, and J. Sjoberg, “Road boundary modeling for autonomous bus docking subject to rectangular geometry constraints,” in Proc. Eur. Control Conf. (ECC), Delft, The Netherlands, Jun. 2021, pp. 1745–1750.
[60]
Z. Zhang, L. Zhang, J. Deng, M. Wang, Z. Wang, and D. Cao, “An enabling trajectory planning scheme for lane change collision avoidance on highways,” IEEE Trans. Intell. Vehicles, vol. 8, no. 1, pp. 147–158, Jan. 2023.
[61]
S. Gilroy et al., “Autonomous navigation for quadrupedal robots with optimized jumping through constrained obstacles,” in Proc. IEEE 17th Int. Conf. Autom. Sci. Eng. (CASE), Lyon, France, Aug. 2021, pp. 2132–2139.
[62]
X. Shen, E. L. Zhu, Y. R. Stürz, and F. Borrelli, “Collision avoidance in tightly-constrained environments without coordination: A hierarchical control approach,” in Proc. IEEE Int. Conf. Robot. Autom. (ICRA), May 2021, pp. 2674–2680.
[63]
R. Verschueren et al., “Acados—A modular open-source framework for fast embedded optimal control,” Math. Program. Comput., vol. 14, no. 1, pp. 147–183, Mar. 2022.
[64]
G. Frison and M. Diehl, “HPIPM: A high-performance quadratic programming framework for model predictive control,” IFAC-PapersOnLine, vol. 53, no. 2, pp. 6563–6569, 2020.
Index Terms
- Efficient Optimization-Based Trajectory Planning for Unmanned Systems in Confined Environments
Index terms have been assigned to the content through auto-classification.
Recommendations
Toward Autonomous Exploration in Confined Underwater Environments
In this field note, we detail the operations and discuss the results of an experiment conducted in the unstructured environment of an underwater cave complex using an autonomous underwater vehicle AUV. For this experiment, the AUV was equipped with two ...
Comments
Please enable JavaScript to view thecomments powered by Disqus.Information & Contributors
Information
Published In
1524-9050 © 2024 IEEE. Personal use is permitted, but republication/redistribution requires IEEE permission. See https://www.ieee.org/publications/rights/index.html for more information.
Publisher
IEEE Press
Publication History
Published: 01 November 2024
Qualifiers
- Research-article
Contributors
Other Metrics
Bibliometrics & Citations
Bibliometrics
Article Metrics
- 0Total Citations
- 0Total Downloads
- Downloads (Last 12 months)0
- Downloads (Last 6 weeks)0
Reflects downloads up to 05 Feb 2025