一种基于参数曲线优化的导航路径规划方法、系统

技术领域

本发明属于自主移动机器人技术领域，具体涉及一种基于参数曲线优化的导航路径规划方法、系统。

背景技术

全局路径规划是自主移动机器人系统中的重要模块之一。其目的是生成可以引导机器人从当前位置前往目的地的路径，为局部行为规划提供导航。全局路径规划存在两个关键要素，第一是碰撞风险，第二是平滑性。

传统的全局路径规划方法分为两类，基于图搜索的规划方法和基于采样的规划算法。这些方法可以生成从当前位置到终点的路径，但是生成的路径无法满足高平滑性和最小化碰撞风险的要求，需要进一步的路径平滑和优化。

当前研究者提出了利用图搜索的规划算法生成初始路径，随后构建优化目标函数，利用共轭梯度下降的方法实现对于路径碰撞风险和平滑性的优化。虽然这些方法取得了不错的效果，但是依旧存在问题，并不能稳定对路径进行优化，有时甚至可能导致路径平滑性下降。

出现该问题的原因在于，这些方法将路径上全部点的坐标作为决策变量，进行目标函数的构建。这导致所定义的优化问题维度高，难以求解。同时，决策变量的过度冗余也导致部分变量可能对优化过程产生不利影响。

但是，这些方法难以稳定的在最小化路径的碰撞风险的同时提升平滑性。原因在于其将路径的全部采样点坐标作为优化目标，导致构建的优化问题维度过高。

发明内容

为了解决现有技术中存在的问题，本发明提供了一种新的全局路径规划方法及系统，首先利用Lazy Theta星算法进行初始路径的搜索，得到初始路径后，对路径进行关键点提取，根据关键点构建参数路径，并进一步得到优化的目标函数，对路径的碰撞风险和平滑性进行优化，从而得到最终路径，在优化过程中，不仅对关键点的坐标进行调整(数值优化)，还进行了关键点的添加和删除(几何优化)，以提升优化效果。

为了实现上述目的，本发明采用的技术方案是：一种基于参数曲线优化的导航路径规划方法，具体过程如下：

根据定位、激光雷达、以及地图信息生成规划配置；

根据规划配置，使用Lazy Theta星算法进行初始路径搜索，得到初始路径；

对初始路径进行几何分析得到一部分关键点，对所述关键点进行扩增，得到剩下的关键点；

对所有关键点进行三次样条插值，得到的参数曲线；

定义目标函数，将所述参数曲线作为目标函数的输入，关键点作为决策变量，输出为参数曲线碰撞风险和平滑性的测量值；通过几何优化和数值优化对目标函数进行优化，得到导航路径。

对初始路径进行几何分析得到一部分关键点时，使用道格拉斯-普客算法。

关键点扩增具体如下：

计算每一段参数曲线S

上述过程不断重复，直到没有新的关键点需要插入到

根据当前关键点

其中，

所述目标函数具体为：

其中，

对参数曲线

其中，

数曲线

公式(4)计算的是每一个采样点与其前后采样点连线中心点的欧式距离；

参数曲线

其中，

将数值优化和几何优化相结合的方法对参数曲线进行优化，具体如下：首先利用数值优化方法对关键点

另一方面，本发明提供一种基于参数曲线优化的导航路径规划系统，包括规划配置生成模块、初始路径获取模块、关键点获取模块、参数曲线获取模块以及路径计算模块；

规划配置生成模块用于根据定位、激光雷达、以及地图信息生成规划配置；

初始路径获取模块用于根据规划配置，使用Lazy Theta星算法进行初始路径搜索，得到初始路径；

关键点获取模块用于对初始路径进行几何分析得到一部分关键点，对所述关键点进行扩增，得到剩下的关键点；

参数曲线获取模块用于对所有关键点进行三次样条插值，得到的参数曲线；

路径计算模块定义定义目标函数，将所述参数曲线作为目标函数的输入，关键点作为决策变量，输出为参数曲线碰撞风险和平滑性的测量值；通过几何优化和数值优化对目标函数进行优化，得到导航路径。

一种计算机设备，包括处理器以及存储器，存储器用于存储计算机可执行程序，处理器从存储器中读取所述计算机可执行程序并执行，处理器执行计算可执行程序时能实现本发明所述基于参数曲线优化的导航路径规划方法。

一种计算机可读存储介质，计算机可读存储介质中存储有计算机程序，所述计算机程序被处理器执行时，能实现本发明所述的基于参数曲线优化的导航路径规划方法。

与现有技术相比，本发明至少具有以下有益效果：

提出了一种新的全局路径规划算法，该方法通过优化参数化表征的全局路径，能够有效降低路径的碰撞风险，提升路径的平滑性；定义了一种新的优化目标函数，使对于目标函数的数值优化能够充分提升路径的表现，在路径优化的过程中，将数值优化和几何优化进行了结合，并提出了新的优化目标函数，提升了优化效果；基于本发明所述方法，并不是实时重规划全局路径，只有在当前全局路径发生碰撞，或当前全局路径评价不如新规划的全局路径时，才对全局路径进行更新。

附图说明

图1为路径导航、规划方法框架示意图。

图2为关键点的添加与删除示意图。

图3为移动机器人及其系统架构图。

具体实施方式

本发明所提出方法的流程如图1所示，可以分为四步。第一步，根据定位、激光雷达、以及地图信息生成规划的配置。第二步，根据规划配置，使用Lazy Theta星算法进行初始路径搜索，得到如图1中红色线所示的初始路径。第三步，对初始路径进行几何分析得到一部分关键点，之后对这些关键点进行扩增，得到剩下的关键点。第三步中得到的全部关键点如图1中红色点所示。对这些关键点进行三次样条插值，得到的参数曲线如图1中绿色曲线所示。第四步，定义优化目标函数，将参数曲线作为目标函数输入，关键点作为决策变量，进行数值优化。在优化过程中，同时进行关键点的插入和删除：判断两个关键点间是否需要插入新的关键点；判断每一个关键点是否冗余，如果冗余则将其删除。上述过程不断重复直到满足终止条件。调整后的关键点如图1中蓝色点所示，得到优化后的曲线如图1中蓝色曲线所示。

使用Lazy Theta星算法后，可以得到一条初始路径

为实现以上目的，本发明使用的方法如算法1所示。算法分为了两个部分：初始路径几何学分析(GeometryAnalysis)和关键点扩增(KeyPointsIncrement)。前者是为了使用尽可能少的关键点进行初始路径的表征，后者是防止插值得到的参数曲线与初始路径差异过大。

几何学分析使用的是道格拉斯-普客算法，关键点扩增如算法2所示。

根据当前关键点

其中，

其中，l

得到参数曲线

在上一步中得到的参数曲线

详细来讲，定义一个目标函数

经过理论计算分析、多次试验和测试，最终确定的目标函数如公式3所示。

其中，

其中，

利用FilterMean函数，在计算碰撞风险测量

数曲线

公式(5)计算的是每一个采样点与其前后采样点连线中心点的欧式距离。也就是说，当采样点处于其前后采样点连线中心点处时，则认为达到了理想的平滑性，本发明使用FilterMean函数的目的也是为了让优化聚焦于曲线最不平滑的部分。

参数曲线

其中，

至此，整体给出了目标函数

但是，仅仅通过数值优化的方式对参数曲线进行优化是不够的。例如，在图2上方所示场景中，仅使用数值优化方法调整关键点的坐标就难以达到期望的规划路径。而在图2下方所示场景中，过近的关键点会造成曲线抖动，这也是通过数值优化难以解决的。在解决这些问题的过程中，关键点的插入和删除对解决这个问题有巨大的帮助，如图2中所示。

因此，本发明提出了一种将数值优化和几何优化(插入和删除关键点)相结合的方法对参数曲线进行优化，过程如算法4所示。首先利用数值优化方法对关键点

本发明所述方法在自行研发的移动机器人上进行了实现，并集成到了整个机器人系统中。本发明所使用的机器人及其系统如图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)。