Trajectory Planning Method Based on Optimization in Complex Environment

Abstract

This paper's major objective is to rapidly and precisely plan the optimal trajectory in a complex environment. Because collisions must be avoided to ensure driving safety and compliance with kinematic constraints to enable precise tracking, this task can be characterized as an optimal control problem(OCP). The outcome of the optimal control problem is determined by the initial solution. A good first solution can reduce the time needed to solve the problem and enhance the effectiveness of the planning process. Hybrid A* is a well-known heuristic method that has a short planning time and satisfies vehicle but it is not complete. It offers a good initial solution for following optimal control problems. In order to achieve the ideal balance between optimality and solvability, we therefore suggest that, in the event that Hybrid A* planning fails, the complete two-dimensional A* planning result be employed as the initial solution of the optimal control problem. It is difficult and time-consuming to avoid contact with any obstacles when the initial solution indicates the homotopy path. The collision avoidance constraint is changed into a linear constraint by the addition of a space corridor, making it independent of the overall quantity of obstacles and solely related to obstacles that are approaching the trajectory. Soft constraints are utilized to further simplify the optimal control problem and guarantee its solvability. We conduct simulation experiments in multiple scenes to prove the feasibility of the algorithm.