Safety Trajectory Planning for Autonomous Vehicles in Unstructured Narrow Environments: A Perception Error Compatible Approach

Planning in unstructured narrow environments with perception errors is of great challenge, as inaccurately perceived obstacle positions may lead planners to generate collision-prone trajectories. Typically, constraints are applied on the collision probability of the potential trajectory to ensure safety. However, due to the oversimplified modeling of vehicles or obstacles in estimating collision probabilities, these methods are often unsuitable for unstructured environments. To address this issue, we propose a computationally efficient risk zone generation method and introduce a Gaussian error function-based evaluation method to the collision risk assessment. The resulting risk-aware cost term is integrated into a trajectory optimization framework based on optimal control theory, effectively enabling obstacle avoidance by explicitly penalizing collision risk along the trajectory. Additionally, to address the inherent limitations of penalty based obstacle avoidance, we introduce a geometric safety constraint rigorously derived with duality principles. In the simulation scenarios of the Trajectory Planning Competition for Automated Parking organized by the IEEE Intelligent Transportation Systems Conference 2022, we introduce perception uncertainty to better reflect real-world conditions. The results demonstrate that our algorithm significantly improves trajectory safety while maintaining an effective balance between safety and efficiency.