一种基于参数曲线优化的导航路径规划方法、系统
阅读说明：本技术 一种基于参数曲线优化的导航路径规划方法、系统 (Navigation path planning method and system based on parameter curve optimization ) 是由 郑南宁 简志强 张崧翌 张稼慧 陈仕韬 于 2021-09-06 设计创作，主要内容包括：本发明公开一种基于参数曲线优化的导航路径规划方法、系统,具体过程如下：根据定位、激光雷达、以及地图信息生成规划配置；根据规划配置,使用Lazy Theta星算法进行初始路径搜索,得到初始路径；对初始路径进行几何分析得到一部分关键点,对所述关键点进行扩增,得到剩下的关键点；对所有关键点进行三次样条插值,得到的参数曲线；定义优化目标函数,将所述参数曲线作为目标函数输入,关键点作为决策变量,输出为参数曲线碰撞风险和平滑性的测量值；通过数值优化的方法对目标函数进行数值优化,得到导航路径；对路径的碰撞风险和平滑性进行优化,从而得到最终路径,在优化过程中,结合数值优化与几何优化,以提升优化效果。(The invention discloses a navigation path planning method and a system based on parameter curve optimization, which comprises the following specific processes: generating planning configuration according to the positioning, the laser radar and the map information; according to the planning configuration, carrying out initial path search by using a Lazy Theta star algorithm to obtain an initial path; performing geometric analysis on the initial path to obtain a part of key points, and amplifying the key points to obtain the rest key points; performing cubic spline interpolation on all key points to obtain a parameter curve; defining an optimization objective function, inputting the parameter curve as an objective function, taking the key point as a decision variable, and outputting a measured value of the collision risk and the smoothness of the parameter curve; performing numerical optimization on the objective function by a numerical optimization method to obtain a navigation path; and optimizing the collision risk and the smoothness of the path to obtain a final path, and combining numerical optimization and geometric optimization in the optimization process to improve the optimization effect.)
技术领域
本发明属于自主移动机器人技术领域，具体涉及一种基于参数曲线优化的导航路径规划方法、系统。
背景技术
全局路径规划是自主移动机器人系统中的重要模块之一。其目的是生成可以引导机器人从当前位置前往目的地的路径，为局部行为规划提供导航。全局路径规划存在两个关键要素，第一是碰撞风险，第二是平滑性。
传统的全局路径规划方法分为两类，基于图搜索的规划方法和基于采样的规划算法。这些方法可以生成从当前位置到终点的路径，但是生成的路径无法满足高平滑性和最小化碰撞风险的要求，需要进一步的路径平滑和优化。
当前研究者提出了利用图搜索的规划算法生成初始路径，随后构建优化目标函数，利用共轭梯度下降的方法实现对于路径碰撞风险和平滑性的优化。虽然这些方法取得了不错的效果，但是依旧存在问题，并不能稳定对路径进行优化，有时甚至可能导致路径平滑性下降。
出现该问题的原因在于，这些方法将路径上全部点的坐标作为决策变量，进行目标函数的构建。这导致所定义的优化问题维度高，难以求解。同时，决策变量的过度冗余也导致部分变量可能对优化过程产生不利影响。
但是，这些方法难以稳定的在最小化路径的碰撞风险的同时提升平滑性。原因在于其将路径的全部采样点坐标作为优化目标，导致构建的优化问题维度过高。
发明内容
为了解决现有技术中存在的问题，本发明提供了一种新的全局路径规划方法及系统，首先利用Lazy Theta星算法进行初始路径的搜索，得到初始路径后，对路径进行关键点提取，根据关键点构建参数路径，并进一步得到优化的目标函数，对路径的碰撞风险和平滑性进行优化，从而得到最终路径，在优化过程中，不仅对关键点的坐标进行调整(数值优化)，还进行了关键点的添加和删除(几何优化)，以提升优化效果。
为了实现上述目的，本发明采用的技术方案是：一种基于参数曲线优化的导航路径规划方法，具体过程如下：
根据定位、激光雷达、以及地图信息生成规划配置；
根据规划配置，使用Lazy Theta星算法进行初始路径搜索，得到初始路径；
对初始路径进行几何分析得到一部分关键点，对所述关键点进行扩增，得到剩下的关键点；
对所有关键点进行三次样条插值，得到的参数曲线；
定义目标函数，将所述参数曲线作为目标函数的输入，关键点作为决策变量，输出为参数曲线碰撞风险和平滑性的测量值；通过几何优化和数值优化对目标函数进行优化，得到导航路径。
对初始路径进行几何分析得到一部分关键点时，使用道格拉斯-普客算法。
关键点扩增具体如下：
计算每一段参数曲线Si(u)与相应直线段的最大欧式距离di，同时，得到直线段上与曲线段Si(u)距离最大的点如果di大于给定的偏差容忍度σ，则将点作为新的关键点插入关键点和之间；
上述过程不断重复，直到没有新的关键点需要插入到中，得到关键点点和参数曲线
根据当前关键点进行三次样条插值，得到参数曲线是关于参数u的分段函数，参数曲线的第i段Si(u)的表达式如公式(1)所示：
其中，表示两个关键点和之间的欧式距离，l0＝0，和等是参数曲线中的未知参量；求解这些未知参量后就可以得到参数曲线确切的表达式。
所述目标函数具体为：
其中，为参数曲线碰撞风险的测量，为参数曲线平滑性的测量，为参数曲线两个端点与规划的起点和终点的偏移量，wc，ws和wo为预设的权重。
对参数曲线进行离散化：通过采样点来对参数曲线进行表示，其中t+1为采样点的数量；计算碰撞风险测量的计算如公式(3)所示
其中，表示离最近的障碍物点，FilterMean为自定义函数，γ为预设的参数，满足0≤γ≤1；
数曲线平滑性的测量的计算如公式(4)所示：
公式(4)计算的是每一个采样点与其前后采样点连线中心点的欧式距离；
参数曲线两个端点与规划的起点和终点的偏移量的计算如公式(5)所示：
其中，和分别为初始路径的起点和终点，τ(s)和τ(g)分别为起点和终点容忍的偏移量。
将数值优化和几何优化相结合的方法对参数曲线进行优化，具体如下：首先利用数值优化方法对关键点进行优化，判断优化后关键点之间的距离，如果距离小于预设值则删除相应关键点，对剩下的关键点进行三次样条插值，得到新的参数曲线如果此时得到的参数曲线无碰撞，则结束优化过程，反之，判断参数曲线的每一段Si(u)是否发生碰撞，如果碰撞，则找到Si(u)与障碍物最近的点将其插入关键点中；以上过程不断重复，直到达到最大迭代次数it，得到优化后的参数曲线即是最终的全局路径。
另一方面，本发明提供一种基于参数曲线优化的导航路径规划系统，包括规划配置生成模块、初始路径获取模块、关键点获取模块、参数曲线获取模块以及路径计算模块；
规划配置生成模块用于根据定位、激光雷达、以及地图信息生成规划配置；
初始路径获取模块用于根据规划配置，使用Lazy Theta星算法进行初始路径搜索，得到初始路径；
关键点获取模块用于对初始路径进行几何分析得到一部分关键点，对所述关键点进行扩增，得到剩下的关键点；
参数曲线获取模块用于对所有关键点进行三次样条插值，得到的参数曲线；
路径计算模块定义定义目标函数，将所述参数曲线作为目标函数的输入，关键点作为决策变量，输出为参数曲线碰撞风险和平滑性的测量值；通过几何优化和数值优化对目标函数进行优化，得到导航路径。
一种计算机设备，包括处理器以及存储器，存储器用于存储计算机可执行程序，处理器从存储器中读取所述计算机可执行程序并执行，处理器执行计算可执行程序时能实现本发明所述基于参数曲线优化的导航路径规划方法。
一种计算机可读存储介质，计算机可读存储介质中存储有计算机程序，所述计算机程序被处理器执行时，能实现本发明所述的基于参数曲线优化的导航路径规划方法。
与现有技术相比，本发明至少具有以下有益效果：
提出了一种新的全局路径规划算法，该方法通过优化参数化表征的全局路径，能够有效降低路径的碰撞风险，提升路径的平滑性；定义了一种新的优化目标函数，使对于目标函数的数值优化能够充分提升路径的表现，在路径优化的过程中，将数值优化和几何优化进行了结合，并提出了新的优化目标函数，提升了优化效果；基于本发明所述方法，并不是实时重规划全局路径，只有在当前全局路径发生碰撞，或当前全局路径评价不如新规划的全局路径时，才对全局路径进行更新。
附图说明
图1为路径导航、规划方法框架示意图。
图2为关键点的添加与删除示意图。
图3为移动机器人及其系统架构图。
具体实施方式
本发明所提出方法的流程如图1所示，可以分为四步。第一步，根据定位、激光雷达、以及地图信息生成规划的配置。第二步，根据规划配置，使用Lazy Theta星算法进行初始路径搜索，得到如图1中红色线所示的初始路径。第三步，对初始路径进行几何分析得到一部分关键点，之后对这些关键点进行扩增，得到剩下的关键点。第三步中得到的全部关键点如图1中红色点所示。对这些关键点进行三次样条插值，得到的参数曲线如图1中绿色曲线所示。第四步，定义优化目标函数，将参数曲线作为目标函数输入，关键点作为决策变量，进行数值优化。在优化过程中，同时进行关键点的插入和删除：判断两个关键点间是否需要插入新的关键点；判断每一个关键点是否冗余，如果冗余则将其删除。上述过程不断重复直到满足终止条件。调整后的关键点如图1中蓝色点所示，得到优化后的曲线如图1中蓝色曲线所示。
使用Lazy Theta星算法后，可以得到一条初始路径但此时得到的路径并不是曲率连续的，本发明希望能够得到一条与初始路径相似，且满足曲率连续要求的曲线。本发明采用的方法是在初始路径的基础上提取关键点利用关键点进行三次样条插值，得到参数曲线以上过程要求得到的参数曲线与路径相似(防止后续路径优化初始值过差)，并且关键点数量越少越好(降低后续优化的问题)。
为实现以上目的，本发明使用的方法如算法1所示。算法分为了两个部分：初始路径几何学分析(GeometryAnalysis)和关键点扩增(KeyPointsIncrement)。前者是为了使用尽可能少的关键点进行初始路径的表征，后者是防止插值得到的参数曲线与初始路径差异过大。
几何学分析使用的是道格拉斯-普客算法，关键点扩增如算法2所示。
根据当前关键点进行三次样条插值，得到参数曲线是关于参数u的分段函数。参数曲线的第i段Si(u)的表达式如公式1所示。
其中，表示两个关键点和之间的欧式距离，l0＝0，和等是参数曲线中的未知参量。求解这些未知参量后就可以得到参数曲线确切的表达式，求解的方法为解如公式2所示的方程组。
其中，li与公式1中含义相同。
得到参数曲线后，计算每一段参数曲线Si(u)与相应直线段的最大欧式距离di。同时，得到直线段上与曲线段Si(u)距离最大的点如果di大于给定的偏差容忍度σ，则将点作为新的关键点插入关键点和之间。上述过程不断重复，直到没有新的关键点需要插入到中，关键点扩增结束。至此，可以得到如图1中点所示的关键点和曲线所示的参数曲线
在上一步中得到的参数曲线虽然在效率上近似最优，且满足曲率连续性。但是，它在碰撞风险和平滑性上方面并不是最优。为了优化参数曲线的碰撞风险和平滑性，一个经典的思路是进行数值优化。
详细来讲，定义一个目标函数目标函数的输入为参数曲线目标函数的输出为参数曲线碰撞风险和平滑性的测量值。本发明只需要通过数值优化的方法对目标函数进行优化，就能够优化参数曲线的碰撞风险和平滑性。此外，由于参数曲线是由关键点唯一确定的，上述优化问题的决策变量实际上是因此，目标函数也可以记作为
经过理论计算分析、多次试验和测试，最终确定的目标函数如公式3所示。
其中，为参数曲线碰撞风险的测量，为参数曲线平滑性的测量，为参数曲线两个端点与规划的起点和终点的偏移量，wc，ws和wo为预设的权重。为了简化目标函数的计算，将参数曲线进行了离散化：通过一些采样点来对参数曲线进行表示，即假定其中t+1为采样点的数量。
的计算如公式4所示。
其中，表示离最近的障碍物点。FilterMean为自定义函数，如算法3所示。γ为预设的参数，满足0≤γ≤1。
利用FilterMean函数，在计算碰撞风险测量的过程中，只考虑离曲线最近的一部分障碍物对曲线的影响，而并没有考虑全部障碍物。其原因在于，碰撞风险主要来源于近距离障碍物。如果将全部障碍物进行考虑，曲线在优化过程中可能会无法远离近距离障碍物，反而会远离远距离障碍物，违背我们的期望。当前方法解决这一问题的方式是设定安全距离阈值，一旦障碍物与路径距离大于阈值，则不进行考虑。这种方法是存在问题的：如果阈值设置过大，则在狭窄场景难以起到效果；如果阈值设置过小，则优化过会受到阈值的制约，难以达到理想情况。相比之下，本发明所述方法则不存在上述问题。
数曲线平滑性的测量的计算如公式5所示。
公式(5)计算的是每一个采样点与其前后采样点连线中心点的欧式距离。也就是说，当采样点处于其前后采样点连线中心点处时，则认为达到了理想的平滑性，本发明使用FilterMean函数的目的也是为了让优化聚焦于曲线最不平滑的部分。
参数曲线两个端点与规划的起点和终点的偏移量的计算如公式6所示。
其中，和分别为初始路径的起点和终点，τ(s)和τ(g)分别为起点和终点容忍的偏移量。在优化过程中，没有将起点和终点看作为约束，而是将其作为优化项，能避免起点或终点条件太差影响参数曲线整体的优化。
至此，整体给出了目标函数的计算方法。对于该目标函数的优化是一个非线性无约束优化问题，并且目标函数的导数不存在解析解。因此，可以采用COBYLA算法进行优化。
但是，仅仅通过数值优化的方式对参数曲线进行优化是不够的。例如，在图2上方所示场景中，仅使用数值优化方法调整关键点的坐标就难以达到期望的规划路径。而在图2下方所示场景中，过近的关键点会造成曲线抖动，这也是通过数值优化难以解决的。在解决这些问题的过程中，关键点的插入和删除对解决这个问题有巨大的帮助，如图2中所示。
因此，本发明提出了一种将数值优化和几何优化(插入和删除关键点)相结合的方法对参数曲线进行优化，过程如算法4所示。首先利用数值优化方法对关键点进行优化。判断优化后关键点之间的距离，如果距离太小则删除相应关键点。对剩下的关键点进行三次样条插值，得到新的参数曲线如果此时得到的参数曲线无碰撞，则结束优化过程。反之，判断参数曲线的每一段Si(u)是否发生碰撞。如果碰撞，则找到Si(u)与障碍物最近的点将其插入关键点中。以上过程不断重复，直到达到最大迭代次数it，结束优化过程。此时，可以得到优化后的参数曲线也即是最终的全局路径。
本发明所述方法在自行研发的移动机器人上进行了实现，并集成到了整个机器人系统中。本发明所使用的机器人及其系统如图3所示。该机器人底盘搭载轮速计和单线激光雷达，机器人的计算单元使用的是NVIDIA Xavier NX。该机器人的系统如图3中所示，使用AMCL进行定位，使用了TEB规划器进行局部规划，并且对局部规划输出的速度进行了平滑。在此框架中，全局规划器并不是实时重规划的。只有在当前全局路径发生碰撞，或当前全局路径评价不如新规划的全局路径时，才对全局路径进行更新。
另外，本发明提供一种基于参数曲线优化的导航路径规划系统，包括规划配置生成模块、初始路径获取模块、关键点获取模块、参数曲线获取模块以及路径计算模块；
规划配置生成模块用于根据定位、激光雷达、以及地图信息生成规划配置；
初始路径获取模块用于根据规划配置，使用Lazy Theta星算法进行初始路径搜索，得到初始路径；
关键点获取模块用于对初始路径进行几何分析得到一部分关键点，对所述关键点进行扩增，得到剩下的关键点；
参数曲线获取模块用于对所有关键点进行三次样条插值，得到的参数曲线；
路径计算模块定义定义目标函数，将所述参数曲线作为目标函数的输入，关键点作为决策变量，输出为参数曲线碰撞风险和平滑性的测量值；通过几何优化和数值优化对目标函数进行优化，得到导航路径。
本发明还可以提供一种计算机设备，包括处理器以及存储器，存储器用于存储计算机可执行程序，处理器从存储器中读取部分或全部所述计算机可执行程序并执行，处理器执行部分或全部计算可执行程序时能实现本发明所述基于参数曲线优化的导航路径规划方法。
另一方面，本发明提供一种计算机可读存储介质，计算机可读存储介质中存储有计算机程序，所述计算机程序被处理器执行时，能实现本发明所述的基于参数曲线优化的导航路径规划方法。
所述计算机设备可以采用车载计算机、笔记本电脑、桌面型计算机或工作站。
处理器可以是中央处理器(CPU)、数字信号处理器(DSP)、专用集成电路(ASIC)或现成可编程门阵列(FPGA)。
对于本发明所述存储器，可以是笔记本电脑、桌面型计算机或工作站的内部存储单元，如内存、硬盘；也可以采用外部存储单元，如移动硬盘、闪存卡。
计算机可读存储介质可以包括计算机存储介质和通信介质。计算机存储介质包括以用于存储诸如计算机可读指令、数据结构、程序模块或其他数据等信息的任何方法或技术实现的易失性和非易失性、可移动和不可移动介质。计算机可读存储介质可以包括：只读存储器(ROM，Read Only Memory)、随机存取记忆体(RAM，Random Access Memory)、固态硬盘(SSD，Solid State Drives)或光盘等。其中，随机存取记忆体可以包括电阻式随机存取记忆体(ReRAM,Resistance Random Access Memory)和动态随机存取存储器(DRAM，Dynamic Random Access Memory)。