Trajectory Planning for an Autonomous Vehicle in Spatially Constrained Environments

Road shoulders and slopes often appear in unstructured environments. They make 2.5D vehicle trajectory planning commonly seen in our daily life, which lies on a 2D manifold embedded in a 3D space. The height difference of these terrains brings spatially dependent constraints on vehicle maneuvers, such as the limit on vehicle steering for vehicle tire protection when a vehicle approaches a road shoulder edge. These constraints have an ``if-else'' structure since they are activated only when the vehicle passes through the local area with a height difference, making the restriction on variables coupled with the judgment of variables. This makes the application of state-of-art optimization-based planners challenging. To solve this problem, we devise an approximation formulation for these constraints in the trajectory planning optimization problem, whose solution depends on a proper initial guess for the optimizer. We propose a two-stage trajectory planning framework, where the first stage improves the hybrid A* algorithm by adding spatially dependent constraints into node expansion to provide the initial guess. Then, the optimization problem with the formulated spatially dependent constraints is solved for further trajectory smoothness and quality. Finally, the simulation results validate the fast and high-quality planning performance of our proposed framework.