Path and speed planning with longitudinal and lateral motion coupling on arbitrary road profile for driving assistance and automated driving systems

Driver assistance and automated driving systems on general roads are crucial for reducing traffic accidents and ensuring the freedom of mobility in aging societies. To foster trust in these systems, it’s essential to achieve vehicle movements that people perceive as natural. This paper aims to incorporate human driving characteristics, such as jerk minimization and coupled longitudinal and lateral motions, into path and speed planning for natural driving assistance and automated driving. To simplify the complex equations typically required when implementing these characteristics in a vehicle-fixed moving coordinate system without compromising rigor, we propose using a local fixed coordinate system along the road geometry. Our approach sets longitudinal and lateral motions as time polynomials within a predetermined time horizon. This allows coupling conditions to be transformed into polynomial parameter equilibrium equations, enabling the comprehensive application of optimization requirements like jerk minimization. Consequently, optimal path and speed plans can be derived by solving a system of linear equations at each time step. We also develop a strategy for coupling gain management to suppress transient vehicle jerk fluctuations caused by lateral acceleration sign reversals. The resulting path and speed plans demonstrate features similar to experienced drivers’ “out-in-out” driving paths. This method enables smooth path and speed planning for various road geometries while minimizing jerk and maintaining coupled longitudinal and lateral motion. Future work will involve verifying the system’s effectiveness using prototype vehicles and/or driving simulators.