基于图搜索与数值优化方法的分层轨迹规划方法

Hierarchical Trajectory Planning Method Based on Graph Search and Optimization Method

针对结构化道路场景中多约束的轨迹规划问题,提出一种路径和速度协同搜索、解耦优化的分层轨迹规划方法.上层初始轨迹规划器考虑动态障碍物风险场与时空信息,构造时空代价地图,通过三维A*算法搜索得到安全可行的初始轨迹,保证初始轨迹解的质量.下层运动轨迹规划器将轨迹规划解耦为路径规划和速度规划,以最小曲率、最大速度以及舒适性等为目标,采用数值优化算法构建路径和速度样条优化模型,使其在避障过程中能够充分发挥车辆的动力性能,同时保证驾乘舒适性,并以局部时空隧道思想简化约束条件,提高求解效率.通过试验验证本文提出的方法具有较好的行驶效率、舒适性以及实时性.

A hierarchical trajectory planning method with path and speed co-search and decoupling optimiza⁃tion is proposed to address the multi-constraint trajectory planning problem in structured road scenarios.To ensure the quality of the initial trajectory solution,the upper initial trajectory planner constructs the Spatio-temporal cost map by taking into account the risk field and spatio-temporal information of dynamic obstacles,and searches for the safe and feasible initial trajectory using a three-dimensional A*algorithm.The lower trajectory planner implements decoupling for the trajectory planning as path planning and velocity planning with the minimum curvature,maximum speed,and comfort as the goal.A numerical optimization algorithm is used to construct a spline optimization model for the path and speed,in order that in the process of obstacle avoidance it can give full play to the dynamic perfor⁃mance of the vehicle and at the same time improve the ride comfort,simplify the constraint conditions on local spacetime tunnel thought,and improve solving efficiency.The experiment results show that the proposed method has better driving efficiency,comfort,and real-time performance.