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

说明书

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

技术领域

本发明涉及自动驾驶运动规划的技术领域，尤其涉及一种基于双层启发搜索共轭梯度下降的轨迹规划方法及基于双层启发搜索共轭梯度下降的轨迹规划系统。

背景技术

近年来在图论中，应用最广泛的就是搜索算法了，比如深度优先搜索、广度优先搜索等。除了深度优先、广度优先这种暴力搜索算法，还有一些最短路算法也可以求得最短路径，并且效率比深度、广度优先搜索高。在一些大型网络游戏中，往往存在一种场景，游戏中的你需要从当前位置走到目的地，而你只需要在地图中找到目标位置，点击即可自动寻路走过去。在实际场景中，游戏地图通常会很大，而且其中的路，障碍物等都不是很规则，并不能简单的将它们抽象成节点和线段。在寻路的过程中需要绕过所有障碍物，并且找到一条不会绕路的路径。似乎最短路是最符合要求的，但是如果地图很大，节点很多很多，那么求最短路的算法执行效率还是太低了。我们日常生活中地图软件的路线规划，其实都不需要找到那条最短路，而是在权衡效率和路线质量的情况下，找到一个次优解即可，A-star算法就是可以平衡效率和路线质量，找到次优路线的搜索算法。

但现有A-star算法是在离散空间搜索，生成的轨迹不是连续的，未考虑车辆运动学约束，轨迹是针对质点模型运动，在实际车辆行驶中，路径不具备可行性，且轨迹未进行优化，轨迹点存在震荡问题。

发明内容

本部分的目的在于概述本发明的实施例的一些方面以及简要介绍一些较佳实施例。在本部分以及本申请的说明书摘要和发明名称中可能会做些简化或省略以避免使本部分、说明书摘要和发明名称的目的模糊，而这种简化或省略不能用于限制本发明的范围。

鉴于上述现有存在的问题，提出了本发明。

因此，本发明解决的一个技术问题是：提出一种作为本发明所述的基于双层启发搜索共轭梯度下降的轨迹规划方法，考虑车辆运动学约束，对轨迹进行优化，降低轨迹点的震荡。

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

作为本发明所述的基于双层启发搜索共轭梯度下降的轨迹规划方法的一种优选方案，其中：所述选择最小代价值包括构建总代价函数，所述总代价函数等于历史代价函数和启发代价函数之和，求得最小的总代价作为当前所需的最小代价值。

作为本发明所述的基于双层启发搜索共轭梯度下降的轨迹规划方法的一种优选方案，其中：所述计算判断包括，根据欧式距离或者曼哈顿距离求出当前拓展节点到父节点的值；选取最小的所述值；记录此时的节点作为拓展节点。

作为本发明所述的基于双层启发搜索共轭梯度下降的轨迹规划方法的一种优选方案，其中：所述车辆运动模型满足车的运动学约束，包括如下搭建步骤，

定义初始模型：

联立上式可得：