A Real-Time Optimization-Based Trajectory Planning Method in Dynamic and Uncertain Environments

Reducing conservatism while ensuring safety poses great difficulties for real-time trajectory planning in uncertain and cluttered environments. If we view trajectory planning as an optimization problem, the non-convex collision avoidance constraints with uncertain obstacles make trajectory planning challenging and time-consuming. Disjunctive chance constraint-based methods have been one of the most popular stochastic tools for this problem, for they can provide a tighter bound and lead to less conservative trajectories compared with other methods. However, previous work on disjunctive chance constraint-based trajectory planning adopts mixed-integer programming which has exponential complexity. Different from existing work, we propose a new optimization-based trajectory planning method with chance constraints, which turns uncertain obstacles into bounding boxes with tight upper bound collision avoidance constraints. Then, with a proposed time-varying convex feasible sets (TVCFS) algorithm, the original non-convex optimization problem is transferred into a series of convex problems, which can meet real-time requirements. Since the planned trajectory may be dynamically infeasible, we consider vehicle kinematics and formulate an optimal control problem to further smooth the planned trajectory and obtain desired control inputs. Simulation tests demonstrate the effectiveness of the proposed method.