Integrating Higher-Order Dynamics and Roadway-Compliance into Constrained ILQR-based Trajectory Planning for Autonomous Vehicles (2309.14566v1)
Abstract: This paper addresses the advancements in on-road trajectory planning for Autonomous Passenger Vehicles (APV). Trajectory planning aims to produce a globally optimal route for APVs, considering various factors such as vehicle dynamics, constraints, and detected obstacles. Traditional techniques involve a combination of sampling methods followed by optimization algorithms, where the former ensures global awareness and the latter refines for local optima. Notably, the Constrained Iterative Linear Quadratic Regulator (CILQR) optimization algorithm has recently emerged, adapted for APV systems, emphasizing improved safety and comfort. However, existing implementations utilizing the vehicle bicycle kinematic model may not guarantee controllable trajectories. We augment this model by incorporating higher-order terms, including the first and second-order derivatives of curvature and longitudinal jerk. This inclusion facilitates a richer representation in our cost and constraint design. We also address roadway compliance, emphasizing adherence to lane boundaries and directions, which past work often overlooked. Lastly, we adopt a relaxed logarithmic barrier function to address the CILQR's dependency on feasible initial trajectories. The proposed methodology is then validated through simulation and real-world experiment driving scenes in real time.
- T. Gu, “Improved trajectory planning for on-road self-driving vehicles via combined graph search, optimization & topology analysis,” Ph.D. dissertation, 02 2017.
- B. Li, Y. Ouyang, L. Li, and Y. Zhang, “Autonomous driving on curvy roads without reliance on frenet frame: A cartesian-based trajectory planning method,” IEEE Transactions on Intelligent Transportation Systems, vol. 23, no. 9, pp. 15 729–15 741, 2022.
- J. Chen, W. Zhan, and M. Tomizuka, “Autonomous driving motion planning with constrained iterative lqr,” IEEE Transactions on Intelligent Vehicles, vol. 4, no. 2, pp. 244–254, 2019.
- J. Ma, Z. Cheng, X. Zhang, M. Tomizuka, and T. H. Lee, “Alternating direction method of multipliers for constrained iterative lqr in autonomous driving,” IEEE Transactions on Intelligent Transportation Systems, vol. 23, no. 12, pp. 23 031–23 042, 2022.
- Y. Shimizu, W. Zhan, L. Sun, J. Chen, S. Kato, and M. Tomizuka, “Motion planning for autonomous driving with extended constrained iterative lqr,” in Dynamic Systems and Control Conference, vol. 84270. American Society of Mechanical Engineers, 2020, p. V001T12A001.
- D. Gonzalez Bautista, J. Pérez, V. Milanes, and F. Nashashibi, “A review of motion planning techniques for automated vehicles,” IEEE Transactions on Intelligent Transportation Systems, vol. 17, pp. 1–11, 11 2015.
- M. McNaughton, C. Urmson, J. M. Dolan, and J.-W. Lee, “Motion planning for autonomous driving with a conformal spatiotemporal lattice,” in 2011 IEEE International Conference on Robotics and Automation, 2011, pp. 4889–4895.
- W. Xiao, C. G. Cassandras, and C. A. Belta, “Bridging the gap between optimal trajectory planning and safety-critical control with applications to autonomous vehicles,” Automatica, vol. 129, p. 109592, 2021.
- J. Ziegler and C. Stiller, “Spatiotemporal state lattices for fast trajectory planning in dynamic on-road driving scenarios,” in 2009 IEEE/RSJ International Conference on Intelligent Robots and Systems, 2009, pp. 1879–1884.
- H. Fan, F. Zhu, C. Liu, L. Zhang, L. Zhuang, D. Li, W. Zhu, J. Hu, H. Li, and Q. Kong, “Baidu apollo em motion planner,” 2018.
- O. Von Stryk and R. Bulirsch, “Direct and indirect methods for trajectory optimization,” Annals of Operations Research, vol. 37, pp. 357–373, 12 1992.
- J. Ziegler, P. Bender, T. Dang, and C. Stiller, “Trajectory planning for bertha — a local, continuous method,” in 2014 IEEE Intelligent Vehicles Symposium Proceedings, 2014, pp. 450–457.
- D. H. Jacobson, “New second-order and first-order algorithms for determining optimal control: A differential dynamic programming approach,” Journal of Optimization Theory and Applications, vol. 2, pp. 411–440, 1968. [Online]. Available: https://api.semanticscholar.org/CorpusID:122366806
- E. Todorov and W. Li, “A generalized iterative lqg method for locally-optimal feedback control of constrained nonlinear stochastic systems,” in Proceedings of the 2005, American Control Conference, 2005., 2005, pp. 300–306 vol. 1.
- Y. Tassa, T. Erez, and E. Todorov, “Synthesis and stabilization of complex behaviors through online trajectory optimization,” in 2012 IEEE/RSJ International Conference on Intelligent Robots and Systems. IEEE, 2012, pp. 4906–4913.
- J. Hauser and A. Saccon, “A barrier function method for the optimization of trajectory functionals with constraints,” in Proceedings of the 45th IEEE Conference on Decision and Control. IEEE, 2006, pp. 864–869.
- Y. Pan, Q. Lin, H. Shah, and J. M. Dolan, “Safe planning for self-driving via adaptive constrained ilqr,” in 2020 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). IEEE, 2020, pp. 2377–2383.
- A. G. Baydin, B. A. Pearlmutter, A. A. Radul, and J. M. Siskind, “Automatic differentiation in machine learning: a survey,” Journal of Marchine Learning Research, vol. 18, pp. 1–43, 2018.
- S. Xu, H. Peng, Z. Song, K. Chen, and Y. Tang, “Accurate and smooth speed control for an autonomous vehicle,” in 2018 IEEE Intelligent Vehicles Symposium (IV). IEEE, 2018, pp. 1976–1982.
- S. Zhu, S. Y. Gelbal, B. Aksun-Guvenc, and L. Guvenc, “Parameter-space based robust gain-scheduling design of automated vehicle lateral control,” IEEE Transactions on Vehicular Technology, vol. 68, no. 10, pp. 9660–9671, 2019.
- S. Xu, H. Peng, Z. Song, K. Chen, and Y. Tang, “Design and test of speed tracking control for the self-driving lincoln mkz platform,” IEEE Transactions on Intelligent Vehicles, vol. 5, no. 2, pp. 324–334, 2019.