[发明专利]基于双层启发搜索共轭梯度下降的轨迹规划方法及系统

权利要求书

1.一种基于双层启发搜索共轭梯度下降的轨迹规划方法，其特征在于：包括以下步骤，

初始化阶段创建保存起点信息的队列；

开启列表获得起始点、目标点以及车辆运动模型；

从所述开启列表中，选择最小代价值的运动模式作为父节点；

基于所述父节点对周围节点进行计算判断，满足预设条件后，记该节点为拓展节点并生成轨迹；

使用共轭梯度下降法对生成的所述轨迹进行优化；

所述双层启发搜索包括，第一层是采用欧式距离作为基准，第二层是采用dubins曲线作为H值依据；

所述轨迹进行优化包括，

建立优化模型；

确定优化方向；

选取优化步长；

最终简化，包括优化方向简化、步长简化和梯度计算简化；

所述计算判断包括，

根据欧式距离求出当前拓展节点到父节点的值；

选取最小的所述值；

记录此时的节点作为拓展节点；

所述车辆运动模型满足车的运动学约束，包括如下搭建步骤，

定义初始模型：

联立上式可得：

假设车辆的方向变化率等于车辆的角速度，则车辆的角速度为

代入所述角速度可得：

在惯性坐标系XY下，得到所述车辆运动学模型：

所述车辆运动学模型包括输出δ_f、δ_r和V，且包括模型的输出滑移角β为，

所述优化模型包括，

初始化定义x(k)表示第k次迭代的解向量、d(k)表示第k次迭代的方向向量、r(k)表示第k次迭代的残差向量；

则x(0)＝0、d(k)＝0、r(0)＝0；

进行第k次迭代、计算残差向量、计算步长以及更新解向量；

所述扩展节点包括以下设置步骤，

创建下一个扩展节点；

设置扩展节点index标识；

判断所述扩展节点是否满足约束，能否进行遍历；

确定新扩展的节点不在closed表中或者未进行遍历过；

更新G值；

若所述扩展节点不在Open表中或者找到更短G值的路径；

更新H值；

若扩展节点与当前节点在同一个栅格且cost值更大则略过，否则保留所述扩展节点并更新前置节点；

所述更新H值的步骤包括，

第一层采用欧式距离作为H值基准，根据欧式距离求出当前拓展节点到父节点的值；

第二层采用dubins曲线作为H值依据，计算从当前节点到目标节点的dubins曲线，

对两层H值进行加权融合，得到最终的H值。

2.如权利要求1所述的基于双层启发搜索共轭梯度下降的轨迹规划方法，其特征在于：所述选择最小代价值包括构建总代价函数，所述总代价函数等于历史代价函数和启发代价函数之和，求得最小的总代价作为当前所需的最小代价值。

3.如权利要求1所述的基于双层启发搜索共轭梯度下降的轨迹规划方法，其特征在于：所述开启列表包括，

起始点push进OPEN优先队列；

取OPEN优先队列的头节点；

设置所述节点的index；

若所述节点index位于clsoed表中，则进行下一次遍历、若所述节点index位于closed表在Open表中，则进行扩展；

所述节点进入closed表中并从Open表中pop；

检测当前节点是否为终点或者是否超出了解算最大时间；

如节点当前是向前走的，计算从当前点到终点的dubins曲线，能够到达，则返回该当前节点。

4.一种基于双层启发搜索共轭梯度下降的轨迹规划的系统，应用于如权利要求1所述的一种基于双层启发搜索共轭梯度下降的轨迹规划方法，其特征在于：包括初始化模块(100)、运算模块(200)、轨迹生成模块(300)和优化模块(400)；

所述初始化模块(100)用于创建保存起点信息的队列；

所述运算模块(200)与所述初始化模块(100)连接，用将初始化后的队列中获得起始点、目标点以及车辆运动模型以及从所述开启列表中，选择最小代价值的运动模式作为父节点；

所述轨迹生成模块(300)与所述运算模块(200)连接，用于根据所述运算模块(200)的计算结果并基于所述父节点对周围节点进行计算判断生成轨迹；

所述优化模块(400)用于对生成的所述轨迹进行优化。