[发明专利]一种曲率连续的路径规划方法、系统及设备

权利要求书

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的计算为路径采样顶点曲率平方的求和。