CN112904858B - 一种曲率连续的路径规划方法、系统及设备 - Google Patents

Abstract

本发明公开一种曲率连续的路径规划方法、系统及设备，方法包括以下步骤：获取原始路点，生成曲率变化率连续的自然参数曲线，定义局部规划使用的坐标架；获取机器人在笛卡尔坐标系的位姿，基于所述位姿得到机器人在所述坐标架中的对应点；进一步解算出机器人在以所述坐标架为横轴的曲线坐标系下的位姿；在曲线坐标系下采样多个目标点，利用所述多个目标点和作为边界条件解算出多条路径的参数曲线，再将每一条路径从曲线坐标系转换回笛卡尔坐标系；采用损失函数对每一条路径进行评价，损失最小的路径作为当前最优路径，在曲线坐标系中进行规划仍然可以保证规划路径的曲率连续性，可以在顺应全局导航路径趋势的同时，实现局部避障。

Description

一种曲率连续的路径规划方法、系统及设备

技术领域

本发明属于自主移动机器人技术领域，具体涉及一种曲率连续的路径规划方法、系统及设备。

背景技术

轮式机器人的路径规划算法往往分为全局规划和局部规划两个部分。相比于直接利用全局规划信息进行局部规划，将全局规划路径作为进行局部规划可以适应不同道路。这种方式也称之为曲线坐标系下的规划。然而，当前在曲线坐标系下的路径规划算法无法实现规划路径曲率的连续，这会对机器人的行驶造成不利影响。

全局规划路径为机器人提供导航，它可以来源于地图的先验信息，也可以由A星等算法生成。使用螺线或参数曲线生成的局部路径往往难以拟合全局路径。为此，研究者们提出了将全局路径作为坐标架进行局部规划的方法。在这种方式下，机器人的位置、朝向和曲率将被转换到以全局路径为横坐标的曲线坐标系下，作为局部路径规划的初始条件。待曲线坐标系下的局部路径生成完毕后，再将路径转换到笛卡尔坐标系下，提供给控制模块。但是，问题在于从笛卡尔坐标系到曲线坐标系的曲率转换公式并没有被推导出来。为此，如果放弃使用机器人当前曲率作为局部路径规划的初始条件，使用三次样条进行曲线坐标系中的局部路径生成；这种方法会使得生成的局部路径初始曲率与机器人当前曲率不一致，导致曲率的不连续。

发明内容

为了解决现有技术中存在的问题，本发明提供一种在曲线坐标系下能够规划出曲率连续路径的方法，本发明获取从笛卡尔坐标系到曲线坐标系的位置、朝向、曲率转换公式，以及从曲线坐标系到笛卡尔坐标系的位置、朝向、曲率转换公式，基于所推导出的公式，包含了全局规划和局部规划两个部分。全局规划生成曲率变化率连续的路径作为坐标架，局部规划在该坐标架中利用二阶连续参数曲线进行局部路径生成，对每一条局部路径的损失进行计算，损失最小的就是最终生成的路径。

为了实现上述目的，本发明采用的技术方案是：一种曲率连续的路径规划方法，包括以下步骤：

获取原始路点，生成曲率变化率连续的自然参数曲线，定义局部规划使用的坐标架

获取机器人在笛卡尔坐标系的位姿I_a，基于所述位姿I_a得到机器人在所述坐标架

中的对应点R；根据机器人在笛卡尔坐标系的位姿I_a以及机器人在所述坐标架

中的对应点R解算出机器人在以所述坐标架

为横轴的曲线坐标系下的位姿I_u；

在曲线坐标系下采样多个目标点，利用所述多个目标点和I_u作为边界条件解算出多条路径的参数曲线，再将每一条路径从曲线坐标系转换回笛卡尔坐标系；

采用损失函数对每一条路径进行评价，损失最小的路径作为当前最优路径。

原始路点来自高精地图的道路中心线或搜索算法得到的初始路径点。

基于原始路点，使用四次样条

进行插值生成曲率变化率连续的自然参数曲线，具体如下：建立分段函数

结合

满足G³连续性条件以及s与u的关系，解算出

对应的自然参数曲线

再根据自然参数曲线

计算出其朝向

曲率κ(s)＝θ′(s)和曲率变化率σ(s)＝κ′(s)，其中，Arg为计算向量朝向的函数，得到局部规划的坐标架

基于车辆在笛卡尔坐标系的位姿为I_a＝(x,y,β_a,α_a)，通过牛顿迭代求解表达式：

得到s_c，将s_c代入

中求解得到车辆在坐标架的对应点R＝(X,Y,θ,κ,σ)；根据车辆在笛卡尔坐标系下的位姿I_a＝(x,y,β_a,α_a)和坐标架中的对应点R＝(X,Y,θ,κ,σ)，求解出车辆在曲线坐标系下的位姿I_u＝(l,r,β_u,α_u)，车辆从笛卡尔坐标系到曲线坐标系下的位姿转换公式为：

其中，Sgn为符号函数，且Q＝-2+rκ(2-rκ)+rκ(-2+rκ)cos(2β_a-2θ)，x、y、β_a和α_a分别为笛卡尔坐标系中横坐标、纵坐标、朝向和曲率，X、Y、θ、κ和σ分别为横坐标、纵坐标、朝向、曲率，表示曲率变化率。

采样过程中将每一条局部路径的每一个采样顶点从曲线坐标系转换到笛卡尔坐标系，具体为：当前需要进行坐标转换的定位位姿为(l,r,β_u,α_u)，坐标架

中对应点为R＝(X,Y,θ,κ,σ)，采样顶点在笛卡尔坐标系下的坐标为(x,y,β_a,α_a)，从曲线坐标系到笛卡尔坐标系的位姿转换公式为：

其中

x、y、β_a和α_a分别为笛卡尔坐标系中横坐标、纵坐标、朝向和曲率，X、Y、θ、κ和σ分别为横坐标、纵坐标、朝向、曲率，表示曲率变化率。

以10Hz的频率进行重规划，不断更新当前最优路径。

所述损失函数为：

cost＝cost_obs+cost_smo

其中，cost_obs用于衡量路径的碰撞风险，cost_smo用于评价路径的平滑性；cost_obs用于判断每一条路径是否与障碍物发生碰撞，之后利用离散高斯卷积计算碰撞路径对于相邻路径的影响，每一条路径将自己受到碰撞路径的影响进行叠加就可以得到自己的碰撞风险损失，cost_smo的计算为路径采样顶点曲率平方的求和。

一种曲率连续的路径规划系统，包括坐标架生成模块、位姿转换模块、路径规划模块以及评价输出模块；

坐标架生成模块获取原始路点，生成曲率变化率连续的自然参数曲线，定义局部规划使用的坐标架

位姿转换模块用于获取机器人在笛卡尔坐标系的位姿I_a，基于所述位姿I_a得到机器人在所述坐标架

中的对应点R；根据机器人在笛卡尔坐标系的位姿I_a以及机器人在所述坐标架

中的对应点R解算出机器人在以所述坐标架

为横轴的曲线坐标系下的位姿I_u；

路径规划模块在曲线坐标系下采样多个目标点，利用所述多个目标点和I_u作为边界条件解算出多条路径的参数曲线，再将每一条路径从曲线坐标系转换回笛卡尔坐标系；

评价输出模块采用损失函数对每一条路径进行评价，损失最小的路径作为当前最优路径并输出。

一种计算机设备，包括一个或多个处理器以及存储器，存储器用于存储计算机可执行程序，处理器从存储器中读取部分或全部所述计算机可执行程序并执行，处理器执行部分或全部计算可执行程序时能实现本发明所述自主移动机器人路径规划方法。

一种计算机可读存储介质，计算机可读存储介质中存储有计算机程序，所述计算机程序被处理器执行时，能实现本发明所述的自主移动机器人路径规划方法。

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

首次得到从笛卡尔坐标系到曲线坐标系的曲率转换公式，并且改进了过去方法中从曲线坐标系到笛卡尔坐标系的曲率转换公式，从理论上证明了在曲线坐标系中进行规划仍然可以保证规划路径的曲率连续性。提出了一种在曲线坐标系中规划出曲率连续路径的方法，可以在顺应全局导航路径趋势的同时，实现局部避障。

附图说明

图1为规划方法的框架图。

图2为车辆位姿从笛卡尔坐标系转换到曲线坐标系示意图。

具体实施方式

本发明所提出规划方法的框架如图1所示，分为了四个步骤。第一步是全局规划，它在给定原始路点的条件下，生成曲率变化率连续的自然参数曲线。通过此自然参数曲线可以定义局部规划所使用的坐标架。后三步都是局部规划。第二步在给出机器人笛卡尔坐标系的位姿I_a＝(x,y,β_a,α_a)后(元组中x、y、β_a和α_a分别为横坐标、纵坐标、朝向和曲率)，找出其在坐标架中的对应点R＝(X,Y,θ,κ,σ)，元组的每一维度分别为：X-横坐标、Y-纵坐标、θ-朝向、κ-曲率，σ表示曲率变化率，第三步是在曲线坐标系下的路径规划，包含有以下部分：首先，根据I_a和R可以求解出机器人在以此坐标架为横轴的曲线坐标系下的位姿I_u＝(l,r,β_u,α_u)。其次，在曲线坐标系下采样多个目标点，利用目标点和I_u作为边界条件求解出路径的参数曲线。最后，对于每一条生成的路径，再将其从曲线坐标系转换回笛卡尔坐标系。第四步，根据定义的损失函数对每一条路径进行评价，损失最小的路径就是最终选中的路径。此外，局部规划以10Hz的频率进行重规划，不断更新输出到控制的路径，直到机器人到达目的地。

在全局规划中，输入信息是一系列路点

这些路点可以来源于高精地图的道路中心线，也可以是搜索算法得到的初始路径点。基于这些路点，我们希望使用参数曲线对其进行插值，生成对于全局路径参数化的描述。使生成的参数曲率满足G³连续性，坐标架满足G³连续性是在曲线坐标系下规划出曲率连续路径的必要条件，使用四次样条

进行插值。

是分段函数，

每一段的表达式为

并且存在

其中

求解(1)中的未知系数，由于

需要满足G³连续性，存在限制条件，所述限制条件可以实现对于(1)中的未知系数的求解。

但是，此时得到的四次样条

不是自然参数曲线，为了简化局部规划的计算，计算其对应的自然参数曲线

在此过程中，需要先得到s与u的关系，如

虽然我们无法通过(3)直接得到u(s)的闭式解，但是，根据牛顿迭代法与(3)，对于给出的任意s，可以得到对应的u；则

可以通过

和(3)进行求解。

得到

后，可以进一步计算出

的朝向

曲率κ(s)＝θ′(s)和曲率变化率σ(s)＝κ′(s)。其中，Arg为计算向量朝向的函数。通过以上过程，最终我们可以得到局部规划的坐标架，定义为

如图2所示，将车辆位姿从笛卡尔坐标系转换到以全局导航作为坐标架的曲线坐标系下的过程中，需要找到车辆位姿在坐标架上的对应点，即坐标架中与当前车辆位姿最近的点。假设车辆在笛卡尔坐标系的位姿为I_a＝(x,y,β_a,α_a)，则对应点可以通过牛顿迭代求解以下表达式得到：

经过验证，以这种方法计算出的s_c的误差可以保持在10^-8以下，将s_c代入

中，就可以得到车辆在坐标架的对应点，定义为R＝(X,Y,θ,κ,σ)。

根据车辆在笛卡尔坐标系下的位姿I_a和坐标架中的对应点R，可以求解出车辆在曲线坐标系下的位姿I_u＝(l,r,β_u,α_u)，其过程如下。

如图2所示，存在

l＝s_c (5)

(6)和(7)对时间t求导可得到

(8)和(9)再次对t求导可以得到

联立(8)、(9)、(10)和(11)可以得到

和

的表达式，更进一步的，可以得到β_u和α_u的表达式，如下

此时，β_u和α_u的表达式中存在一些未知量，根据输入条件对β_u和α_u的表达式中的未知量进行求解，以简化两者的表达式，由于坐标架

是自然参数曲线，因此存在

而另一方面，根据链式求导法则和质点运动在笛卡尔坐标下的分解，又存在

其中，v为车辆的线速度，是未知参数，将(14)和(15)代入β_u和α_u的表达式中可以消除全部未知参数。最终，可以得到车辆从笛卡尔坐标系到曲线坐标系下的位姿转换公式，如下：

其中，Sgn为符号函数，且

Q＝-2+rκ(2-rκ)+rκ(-2+rκ)cos(2β_a-2θ) (17)

当坐标架的曲率κ＝0和曲率变化率σ＝0时，曲线坐标系与笛卡尔坐标系为刚体变换。根据(16)可以得到，在这种情况下车辆在曲线坐标系下的朝向为β_a-θ，曲率为α_a，满足刚体变换的条件，佐证了(16)的正确性。

在曲线坐标系中进行路径生成，第一步是完成目标点的采样。首先可以得到多样化的采样点位姿

随后，为了保证曲率的连续性，可以使用五次样条对车辆当前位姿到目标点的路径

进行建模，表达式如下

其中，

为曲线

的弧长，通常可以使用弦长进行估计。以I_u和G_i作为边界限制，可以求解出(18)中的全部未知参数，得到局部路径的参数表示，不同的目标点可以得到不同的局部路径。最后，对每一条局部路径进行顶点采样得到该路径的非参数表示，如

在此过程中，需要将每一条局部路径的每一个采样顶点从曲线坐标系转换到笛卡尔坐标系。假设当前需要进行坐标转换的定位位姿为(l,r,β_u,α_u)。首先将l代入坐标架

中，找到对应点并定义为R＝(X,Y,θ,κ,σ)；接下来就是求解该顶点在笛卡尔坐标系下的坐标，设所述采样顶点在笛卡尔坐标系下的坐标为(x,y,β_a,α_a)。在这种情况下，(6),(7),(8),(9),(10)和(9)同样成立。并且存在

将(8),(9),(10)和(11)代入到(19)和(20)中，可以得到β_a和α_a的表达式，所述表达式中存在未知参数，以下条件可以作为求解这些未知参量的条件之一

其中，v为车辆在曲线坐标系的线速度，是未知参数。但是将(14)和(21)代入β_a和α_a的表达式，可以消除所有未知参量。最终，得到的从曲线坐标系到笛卡尔坐标系的位姿转换公式为

其中

当坐标架的曲率变化率σ＝0时，α_a的表达式为

此时，α_a的表达式与当前广泛使用的曲线坐标系到笛卡尔坐标系的曲率转换公式完全相同。该结果证明了，相比于当前的转换公式，(22)是一个更加广义的公式，适用于更多的情况。当前的转换公式只是(22)的一种特例。

当坐标架的曲率κ＝0和曲率变化率σ＝0时，笛卡尔坐标系与曲线坐标系为刚体变换。根据(22)可以得到，在这种情况下笛卡尔坐标系下的朝向为β_u+θ，曲率为α_u，满足刚体变换的条件，佐证了(22)的正确性。

另一方面，根据公式(22)，要在曲线坐标系下曲率连续的路径转换到笛卡尔坐标系下仍然保持曲率连续，则需要坐标架的曲率κ和曲率变化率σ连续，作为坐标架的全局路径满足G³连续性是保证在曲线坐标系下规划出曲率连续路径的必要条件。

通过公式(22)，可以将在曲线坐标系下生成的任意路径

转换到笛卡尔坐标系。可以定义最终得到的笛卡尔坐标系下的局部待选路径为

在多样化的待选路径中选择一条最优路径，这一过程的关键在于对每一条路径进行评价。我们构造了如下的损失函数进行评价

cost＝cost_obs+cost_smo, (25)

其中，cost_obs用于衡量路径的碰撞风险，cost_smo用于评价路径的平滑性。

cost_obs的计算首先判断了每一条路径是否与障碍物发生碰撞，之后利用离散高斯卷积计算碰撞路径对于相邻路径的影响，每一条路径将自己受到碰撞路径的影响进行叠加就可以得到自己的碰撞风险损失。cost_smo的计算为路径采样顶点曲率平方的求和。最终损失最小的路径就是选择的最优路径。

一种计算机设备，包括一个或多个处理器以及存储器，存储器用于存储计算机可执行程序，处理器从存储器中读取部分或全部所述计算机可执行程序并执行，处理器执行部分或全部计算可执行程序时能实现本发明所述自主移动机器人路径规划方法。

一种计算机可读存储介质，计算机可读存储介质中存储有计算机程序，所述计算机程序被处理器执行时，能实现本发明所述的自主移动机器人路径规划方法。

所述计算机设备可以采用车载计算机、笔记本电脑、平板电脑、桌面型计算机、手机或工作站。

本发明还提供用于输出预测结果的输出装置，所述输出装置连接处理器的输出端，输出装置为显示器或打印机。

处理器可以是中央处理器(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)。

Claims (9)

1.一种曲率连续的路径规划方法，其特征在于，包括以下步骤：

获取原始路点，生成曲率变化率连续的自然参数曲线，定义局部规划使用的坐标架

获取机器人在笛卡尔坐标系的位姿I_a，基于所述位姿I_a得到机器人在所述坐标架

中的对应点R；根据机器人在笛卡尔坐标系的位姿I_a以及机器人在所述坐标架

中的对应点R解算出机器人在以所述坐标架

为横轴的曲线坐标系下的位姿I_u；

在曲线坐标系下采样多个目标点，利用所述多个目标点和I_u作为边界条件解算出多条路径的参数曲线，再将每一条路径从曲线坐标系转换回笛卡尔坐标系；

采用损失函数对每一条路径进行评价，损失最小的路径作为当前最优路径；基于机器人在笛卡尔坐标系的位姿为I_a＝(x,y,β_a,α_a)，通过牛顿迭代求解表达式：

得到s_c，将s_c代入

中求解得到机器人在坐标架的对应点R＝(X,Y,θ,κ,σ)；根据机器人在笛卡尔坐标系下的位姿I_a＝(x,y,β_a,α_a)和坐标架中的对应点R＝(X,Y,θ,κ,σ)，求解出机器人在曲线坐标系下的位姿I_u＝(l,r,β_u,α_u)，机器人从笛卡尔坐标系到曲线坐标系下的位姿转换公式为：

其中，Sgn为符号函数，且Q＝-2+rκ(2-rκ)+rκ(-2+rκ)cos(2β_a-2θ)，x、y、β_a和α_a分别为笛卡尔坐标系中横坐标、纵坐标、朝向和曲率，X、Y、θ、κ和σ分别为横坐标、纵坐标、朝向、曲率，表示曲率变化率。

2.根据权利要求1所述的曲率连续的路径规划方法，其特征在于，原始路点来自高精地图的道路中心线或搜索算法得到的初始路径点。

3.根据权利要求1所述的曲率连续的路径规划方法，其特征在于，基于原始路点，使用四次样条

进行插值生成曲率变化率连续的自然参数曲线，具体如下：建立分段函数

结合

满足G³连续性条件以及s与u的关系，解算出

对应的自然参数曲线

再根据自然参数曲线

计算出其朝向

曲率κ(s)＝θ′(s)和曲率变化率σ(s)＝κ′(s)，其中，Arg为计算向量朝向的函数，得到局部规划的坐标架

4.根据权利要求1所述的曲率连续的路径规划方法，其特征在于，采样过程中将每一条局部路径的每一个采样顶点从曲线坐标系转换到笛卡尔坐标系，具体为：当前需要进行坐标转换的定位位姿为(l,r,β_u,α_u)，坐标架

中对应点为R＝(X,Y,θ,κ,σ)，采样顶点在笛卡尔坐标系下的坐标为(x,y,β_a,α_a)，从曲线坐标系到笛卡尔坐标系的位姿转换公式为：

其中

x、y、β_a和α_a分别为笛卡尔坐标系中横坐标、纵坐标、朝向和曲率，X、Y、θ、κ和σ分别为横坐标、纵坐标、朝向、曲率，表示曲率变化率。

5.根据权利要求1所述的曲率连续的路径规划方法，其特征在于，以10Hz的频率进行重规划，不断更新当前最优路径。

6.根据权利要求1所述的曲率连续的路径规划方法，其特征在于，所述损失函数为：

cost＝cost_obs+cost_smo

其中，cost_obs用于衡量路径的碰撞风险，cost_smo用于评价路径的平滑性；cost_obs用于判断每一条路径是否与障碍物发生碰撞，之后利用离散高斯卷积计算碰撞路径对于相邻路径的影响，每一条路径将自己受到碰撞路径的影响进行叠加就可以得到自己的碰撞风险损失，cost_smo的计算为路径采样顶点曲率平方的求和。

7.一种曲率连续的路径规划系统，其特征在于，包括坐标架生成模块、位姿转换模块、路径规划模块以及评价输出模块；

坐标架生成模块获取原始路点，生成曲率变化率连续的自然参数曲线，定义局部规划使用的坐标架

位姿转换模块用于获取机器人在笛卡尔坐标系的位姿I_a，基于所述位姿I_a得到机器人在所述坐标架

中的对应点R；根据机器人在笛卡尔坐标系的位姿I_a以及机器人在所述坐标架

中的对应点R解算出机器人在以所述坐标架

为横轴的曲线坐标系下的位姿I_u；基于机器人在笛卡尔坐标系的位姿为I_a＝(x,y,β_a,α_a)，通过牛顿迭代求解表达式：

得到s_c，将s_c代入

中求解得到机器人在坐标架的对应点R＝(X,Y,θ,κ,σ)；根据机器人在笛卡尔坐标系下的位姿I_a＝(x,y,β_a,α_a)和坐标架中的对应点R＝(X,Y,θ,κ,σ)，求解出机器人在曲线坐标系下的位姿I_u＝(l,r,β_u,α_u)，机器人从笛卡尔坐标系到曲线坐标系下的位姿转换公式为：

其中，Sgn为符号函数，且Q＝-2+rκ(2-rκ)+rκ(-2+rκ)cos(2β_a-2θ)，x、y、β_a和α_a分别为笛卡尔坐标系中横坐标、纵坐标、朝向和曲率，X、Y、θ、κ和σ分别为横坐标、纵坐标、朝向、曲率，表示曲率变化率

路径规划模块在曲线坐标系下采样多个目标点，利用所述多个目标点和I_u作为边界条件解算出多条路径的参数曲线，再将每一条路径从曲线坐标系转换回笛卡尔坐标系；

评价输出模块采用损失函数对每一条路径进行评价，损失最小的路径作为当前最优路径并输出。

8.一种计算机设备，其特征在于，包括一个或多个处理器以及存储器，存储器用于存储计算机可执行程序，处理器从存储器中读取部分或全部所述计算机可执行程序并执行，处理器执行部分或全部计算可执行程序时能实现权利要求1～5中任一项所述曲率连续的路径规划方法。

9.一种计算机可读存储介质，其特征在于，计算机可读存储介质中存储有计算机程序，所述计算机程序被处理器执行时，能实现如权利要求1～5中任一项所述的曲率连续的路径规划方法。

Images