Efficient Cooperative Trajectory Planning Using Cognitive-Based Risk Models for Multi-Agent Systems in Non-Convex Environments

This letter focuses on the multi-agent trajectory planning (MATP) problem for car-like intelligent agents operating in high-density unstructured environments and structured scenarios (e.g., urban roads) containing heterogeneous traffic participants. The primary challenge in addressing such problems is obtaining a globally optimal solution rapidly while ensuring safety. The nonlinear kinematic constraints of vehicles, along with the collision constraints that scale with scenario complexity, significantly increase the computational difficulty. Furthermore, the quality of homotopy solutions critically impacts the feasibility of the final solution. To tackle these issues, we propose a hierarchical search and centralized optimization (HSCO) method. Initially, a priority-based search algorithm is employed to rapidly generate an initial guess, enhanced by a time-priority collision pair selection strategy that significantly improves search efficiency. Subsequently, we introduce a novel higher-order warped Gaussian collision risk model integrated with a driver perception field model, effectively reduces the potential collision risk caused by tracking performance uncertainty. Finally, the original MATP problem is linearized into a coupled convex quadratic programming formulation. The proposed collision risk model is seamlessly integrated into the separation plane of potential collision pairs, enabling efficient iterative solution processes that ensure safety, feasibility, and optimality. Experimental results demonstrate that HSCO outperforms existing MATP algorithms in large-scale unstructured scenarios. Moreover, in structured scenarios involving heterogeneous traffic participants, HSCO rapidly generates safe and optimal cooperative trajectories.