Real-Time Safe Stop Trajectory Planning via Multidimensional Hybrid A*-Algorithm

A reliable autonomous driving system should be capable of performing safe stop when proceeding the normal driving becomes impossible. It is essential for safe stop planning to be able to provide a trajectory that leads the vehicle to a specific stopped goal in real-time. With this as a main challenge and distinction, the safe stop planning should still be able to avoid collision with static and dynamic obstacles like normal motion planning. To guarantee a meaningful solution and be real-time capable, we propose to utilize path-velocity decomposition, which provides a non-globally optimal solution but reduces the computational burden. Firstly, by sampling piecewise quintic polynomials that connect a series of sampled points from the current location to the goal, a set of path candidates in Frenét frame are generated. The path that meets kinematic constraints, maximizes comfort, and avoids static obstacles is selected. Afterward, we generate the length-time (ST) graph by projecting the dynamic obstacles on our driving corridor along the chosen path in space and time domain. The velocity planning is performed on the ST-graph with an extended multidimensional Hybrid A-Star $(\mathrm{A}^{*})$ Algorithm. Finally, our approach is evaluated in several simulation scenarios and also in CoInCar-Simulation framework, which shows a real-time capability and promising driving behaviors.