基于双层启发搜索共轭梯度下降的轨迹规划方法及系统技术方案

本发明专利技术公开了基于双层启发搜索共轭梯度下降的轨迹规划方法及系统，包括以下步骤，初始化阶段创建保存起点信息的队列；开启列表获得起始点、目标点以及车辆运动模型；从所述开启列表中，选择最小代价值的运动模式作为父节点；基于所述父节点对周围节点进行计算判断，满足预设条件后，记该节点为拓展节点并生成轨迹；使用共轭梯度下降法对生成的所述轨迹进行优化。本发明专利技术的有益效果：基于非完整无障碍启发搜索，以解决错误方向的搜索代价消耗，指引轨迹朝着目标点行进；基于完整性有障碍启发搜索，以解决在障碍物或者在死胡同区域，指引轨迹远离这些区域；基于共轭梯度下降法，对生成的轨迹进行优化，解决轨迹震荡问题。

Trajectory planning method and system based on double heuristic search conjugate gradient descent

【技术实现步骤摘要】
基于双层启发搜索共轭梯度下降的轨迹规划方法及系统
本专利技术涉及自动驾驶运动规划的
，尤其涉及一种基于双层启发搜索共轭梯度下降的轨迹规划方法及基于双层启发搜索共轭梯度下降的轨迹规划系统。
技术介绍
近年来在图论中，应用最广泛的就是搜索算法了，比如深度优先搜索、广度优先搜索等。除了深度优先、广度优先这种暴力搜索算法，还有一些最短路算法也可以求得最短路径，并且效率比深度、广度优先搜索高。在一些大型网络游戏中，往往存在一种场景，游戏中的你需要从当前位置走到目的地，而你只需要在地图中找到目标位置，点击即可自动寻路走过去。在实际场景中，游戏地图通常会很大，而且其中的路，障碍物等都不是很规则，并不能简单的将它们抽象成节点和线段。在寻路的过程中需要绕过所有障碍物，并且找到一条不会绕路的路径。似乎最短路是最符合要求的，但是如果地图很大，节点很多很多，那么求最短路的算法执行效率还是太低了。我们日常生活中地图软件的路线规划，其实都不需要找到那条最短路，而是在权衡效率和路线质量的情况下，找到一个次优解即可，A-star算法就是可以平衡效率和路线质量，找到次优路线的搜索算法。但现有A-star算法是在离散空间搜索，生成的轨迹不是连续的，未考虑车辆运动学约束，轨迹是针对质点模型运动，在实际车辆行驶中，路径不具备可行性，且轨迹未进行优化，轨迹点存在震荡问题。
技术实现思路
本部分的目的在于概述本专利技术的实施例的一些方面以及简要介绍一些较佳实施例。在本部分以及本申请的说明书摘要和专利技术名称中可能会做些简化或省略以避免使本部分、说明书摘要和专利技术名称的目的模糊，而这种简化或省略不能用于限制本专利技术的范围。鉴于上述现有存在的问题，提出了本专利技术。因此，本专利技术解决的一个技术问题是：提出一种作为本专利技术所述的基于双层启发搜索共轭梯度下降的轨迹规划方法，考虑车辆运动学约束，对轨迹进行优化，降低轨迹点的震荡。为解决上述技术问题，本专利技术提供如下技术方案：一种基于双层启发搜索共轭梯度下降的轨迹规划方法，包括以下步骤，初始化阶段创建保存起点信息的队列；开启列表获得起始点、目标点以及车辆运动模型；从所述开启列表中，选择最小代价值的运动模式作为父节点；基于所述父节点对周围节点进行计算判断，满足预设条件后，记该节点为拓展节点并生成轨迹；使用共轭梯度下降法对生成的所述轨迹进行优化。作为本专利技术所述的基于双层启发搜索共轭梯度下降的轨迹规划方法的一种优选方案，其中：所述选择最小代价值包括构建总代价函数，所述总代价函数等于历史代价函数和启发代价函数之和，求得最小的总代价作为当前所需的最小代价值。作为本专利技术所述的基于双层启发搜索共轭梯度下降的轨迹规划方法的一种优选方案，其中：所述计算判断包括，根据欧式距离或者曼哈顿距离求出当前拓展节点到父节点的值；选取最小的所述值；记录此时的节点作为拓展节点。作为本专利技术所述的基于双层启发搜索共轭梯度下降的轨迹规划方法的一种优选方案，其中：所述车辆运动模型满足车的运动学约束，包括如下搭建步骤，定义初始模型：联立上式可得：假设车辆的方向变化率等于车辆的角速度，则车辆的角速度为代入所述角速度可得：在惯性坐标系XY下，得到所述车辆运动学模型：作为本专利技术所述的基于双层启发搜索共轭梯度下降的轨迹规划方法的一种优选方案，其中：所述车辆运动学模型包括输出δf、δr和V，且包括模型的输出滑移角β为，作为本专利技术所述的基于双层启发搜索共轭梯度下降的轨迹规划方法的一种优选方案，其中：所述开启列表包括，起始点push进OPEN优先队列；取OPEN优先队列的头节点；设置所述节点的index；若所述节点index位于clsoed表中，则进行下一次遍历、若所述节点index位于closed表在Open表中，则进行扩展；所述节点进入closed表中并从Open表中pop；检测当前节点是否为终点或者是否超出了解算最大时间；如节点当前是向前走的，计算从当前点到终点的dubins曲线，能够到达，则返回该当前节点。作为本专利技术所述的基于双层启发搜索共轭梯度下降的轨迹规划方法的一种优选方案，其中：所述扩展节点包括以下设置步骤，创建下一个扩展节点；设置扩展节点index标识；判断所述扩展节点是否满足约束，能否进行遍历；确定新扩展的节点不在closed表中或者未进行遍历过；更新G值；若所述扩展节点不在Open表中或者找到更短G值的路径；更新H值；若扩展节点与当前节点在同一个栅格且cost值更大则略过，否则保留所述扩展节点并更新前置节点。作为本专利技术所述的基于双层启发搜索共轭梯度下降的轨迹规划方法的一种优选方案，其中：所述轨迹进行优化包括，建立优化模型；确定优化方向；选取优化步长；最终简化，包括优化方向简化、步长简化和梯度计算简化。作为本专利技术所述的基于双层启发搜索共轭梯度下降的轨迹规划方法的一种优选方案，其中：所述优化模型包括，初始化定义x(k)表示第k次迭代的解向量、d(k)表示第k次迭代的方向向量、r(k)表示第k次迭代的残差向量；则x(0)＝0、d(k)＝0、r(0)＝0；进行第k次迭代、计算残差向量、计算步长以及更新解向量。本专利技术解决的一个技术问题是：提出一种作为本专利技术所述的基于双层启发搜索共轭梯度下降的轨迹规划系统，考虑车辆运动学约束，对轨迹进行优化，降低轨迹点的震荡。为解决上述技术问题，本专利技术提供如下技术方案：一种基于双层启发搜索共轭梯度下降的轨迹规划系统，包括初始化模块、运算模块、轨迹生成模块和优化模块；所述初始化模块用于创建保存起点信息的队列；所述运算模块与所述初始化模块连接，用将初始化后的队列中获得起始点、目标点以及车辆运动模型以及从所述开启列表中，选择最小代价值的运动模式作为父节点；所述轨迹生成模块与所述运算模块连接，用于根据所述运算模块的计算结果并基于所述父节点对周围节点进行计算判断生成轨迹；所述优化模块用于对生成的所述轨迹进行优化。本专利技术的有益效果：基于非完整无障碍启发搜索，以解决错误方向的搜索代价消耗，指引轨迹朝着目标点行进；基于完整性有障碍启发搜索，以解决在障碍物或者在死胡同区域，指引轨迹远离这些区域；基于共轭梯度下降法，对生成的轨迹进行优化，解决轨迹震荡问题。附图说明为了更清楚地说明本专利技术实施例的技术方案，下面将对实施例描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本专利技术的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其它的附图。其中：图1为本专利技术第一种实施例所述基于双层启发搜索共轭梯度下降的轨迹规划方法的算法流程示意图；图2为本专利技术第一种实施例所述的轨迹生成效果示意图；图3为本专利技术第一种实施例所述超窄障碍环境下的效果示意图；图4为本专利技术第二种实施例所述基于双层启发搜索共轭梯度下降的轨迹规划系统的整体原理结构示意图。具体实施方式为使本专利技术的上述目的、本文档来自技高网...

【技术保护点】
1.一种基于双层启发搜索共轭梯度下降的轨迹规划方法，其特征在于：包括以下步骤，/n初始化阶段创建保存起点信息的队列；/n开启列表获得起始点、目标点以及车辆运动模型；/n从所述开启列表中，选择最小代价值的运动模式作为父节点；/n基于所述父节点对周围节点进行计算判断，满足预设条件后，记该节点为拓展节点并生成轨迹；/n使用共轭梯度下降法对生成的所述轨迹进行优化。/n

【技术特征摘要】
1.一种基于双层启发搜索共轭梯度下降的轨迹规划方法，其特征在于：包括以下步骤，
初始化阶段创建保存起点信息的队列；
开启列表获得起始点、目标点以及车辆运动模型；
从所述开启列表中，选择最小代价值的运动模式作为父节点；
基于所述父节点对周围节点进行计算判断，满足预设条件后，记该节点为拓展节点并生成轨迹；
使用共轭梯度下降法对生成的所述轨迹进行优化。


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


3.如权利要求1或2所述的基于双层启发搜索共轭梯度下降的轨迹规划方法，其特征在于：所述计算判断包括，
根据欧式距离或者曼哈顿距离求出当前拓展节点到父节点的值；
选取最小的所述值；
记录此时的节点作为拓展节点。


4.如权利要求3所述的基于双层启发搜索共轭梯度下降的轨迹规划方法，其特征在于：所述车辆运动模型满足车的运动学约束，包括如下搭建步骤，
定义初始模型：



联立上式可得：



假设车辆的方向变化率等于车辆的角速度，则车辆的角速度为
代入所述角速度可得：



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





5.如权利要求4所述的基于双层启发搜索共轭梯度下降的轨迹规划方法，其特征在于：所述车辆运动学模型包括输出δf、δr和V，且包括模型的输出滑移角β为，





6.如权利要求5所述的基于双层启发搜索共轭梯度下降的轨迹规划方法，其特征在于：所述开启列表包括，
起始点push进OPEN优先队列；
取OPEN优先队列的头节点；
设置所述节点的index；
若所述节点index位于clsoed表中，则进行下一次遍历、若所述节点index位于closed表在Open表中，则进行扩展；
所述节点进入closed表中并从Ope...

【专利技术属性】
技术研发人员：吴月路，
申请(专利权)人：的卢技术有限公司，
类型：发明
国别省市：江苏;32