一种基于多段回旋线的轨迹规划的方法、设备及存储设备与流程

1.本发明涉及智能驾驶中的路径规划领域，尤其涉及一种基于多段回旋线的轨迹规划的方法、设备及存储设备。


背景技术：

2.智能驾驶按软件架构可分为感知、规划、控制、定位等模块。其中路径规划的目的是规划出一条安全、舒适的可行驶轨迹，作为控制系统的输入，用于控制车辆沿规划的轨迹行驶。目前常用的轨迹规划的方法有基于几何的算法、基于采样搜索的算法和基于优化的算法。其中基于几何的算法和基于采样搜索的算法可能存在着规划出的轨迹曲率不连续或曲率过渡不够平滑等问题，如reeds-shepp曲线；基于优化的算法可能存在着求解时间过长或不存在可行解，以及复用性差等问题，因此亟需研究一种新的方法来解决上述问题。


技术实现要素：

3.为了解决上述问题，本发明提供了一种基于多段回旋线的轨迹规划的方法、设备及存储设备，一种基于多段回旋线的轨迹规划的方法主要包括以下步骤：
4.s1：假设规划的车辆运动曲线的曲率一阶线性变化，构建曲率连续的回旋线方程：
5.0≤s≤l
6.k(s)＝k0 k
′s[0007][0008][0009][0010]
其中，l为曲线总长度，s为曲线位置，为初始航向角，k0为初始曲率，k
′
为常数，表示曲率变化率，k(s)为曲线上长度s处的曲率，为曲线上长度s处的航向角，x(s)为曲线上长度s处的x坐标，y(s)为曲线上长度s处的y坐标，x0和y0分别为起始位置处的横坐标和纵坐标；
[0011]
s2：基于上述曲线方程，定义两段连续的回旋线，规划起点为规划终点为从规划起点开始，得到经过两段连续的回旋线后的终点位置：
[0012]kcal1
＝k0 k
′1l1[0013]kcal2
＝k
cal1
k
′2l2[0014][0015]
[0016][0017][0018][0019][0020]
其中，为从规划起点开始，经过第一段回旋线后的结束位置，为从第一段回旋线的结束位置开始，经过第二段回旋线后的结束位置，l1和l2分别为两段回旋线的长度，k
′1和k
′2分别为两段回旋线的曲率变化率；
[0021]
s3：为了使得第二段回旋线的结束位置尽可能的逼近规划终点构建如下目标函数：
[0022][0023]kcal1
＜k
max
[0024]kcal2
＜k
max
[0025]
其中，wi为各项的权重系数，i＝1,2,3,4，k
max
为车辆运动的最大曲率；
[0026]
s4：求解上述目标函数，得出使目标函数f(l1,l2,k
′1,k
′2)最优的两段回旋线参数(l1,l2,k
′1,k
′2)，将(l1,l2,k
′1,k
′2)带入回旋线方程中，即可求得满足规划条件的回旋线。
[0027]
进一步地，构建的回旋线的曲率是连续变化的，满足车辆运动学对曲率连续的约束。
[0028]
进一步地，所述两条回旋线规划出的轨迹需满足以下条件：
[0029]
x(0)＝x0[0030]
x(l)＝x1[0031]
y(0)＝y0[0032]
y(l)＝y1[0033][0034][0035]
k(0)＝k0[0036]
k(l)＝k1[0037]
其中，x(0)为曲线起点处的x坐标，x(l)为曲线终点处的x坐标，y(0)为曲线起点处的y坐标，y(l)为曲线终点处的y坐标，为曲线起点处的航向角，为曲线终点处的航向角，k(0)为曲线起点处的曲率，k(l)为曲线终点处的曲率，表示第二段回旋线后的结束
位置处的目标航向角，k1表示第二段回旋线后的结束位置处的目标曲率，x1和y1分别表示第二段回旋线后的结束位置处的目标横坐标和目标纵坐标。
[0038]
一种存储设备，所述存储设备存储指令及数据用于实现一种基于多段回旋线的轨迹规划的方法。
[0039]
一种基于多段回旋线的轨迹规划的设备，包括：处理器及所述存储设备；所述处理器加载并执行所述存储设备中的指令及数据用于实现一种基于多段回旋线的轨迹规划的方法。
[0040]
本发明提供的技术方案带来的有益效果是：通过使用分段的回旋线进行轨迹规划，具备曲率连续、曲率变化率稳定、满足车辆运动学约束和求解速度快等优点。
附图说明
[0041]
下面将结合附图及实施例对本发明作进一步说明，附图中：
[0042]
图1是本发明实施例中一种基于多段回旋线的轨迹规划的方法的流程图。
[0043]
图2是本发明实施例中硬件设备工作的示意图。
具体实施方式
[0044]
为了对本发明的技术特征、目的和效果有更加清楚的理解，现对照附图详细说明本发明的具体实施方式。
[0045]
本发明的实施例提供了一种基于多段回旋线的轨迹规划的方法、设备及存储设备，用于解决从车辆起始位置到车辆目标位置两点之间构造出一条曲率连续、曲率变化平滑、满足车辆运动学约束的轨迹，一方面可直接作为一些简单驾驶场景的路径规划算法；另一方面可作为几何规划算法中的过渡曲线或采样搜索算法中的最小拼接段。
[0046]
请参考图1，图1是本发明实施例中一种基于多段回旋线的轨迹规划的方法的流程图，具体包括如下步骤：
[0047]
1.车辆运动约束
[0048]
对于满足阿克曼转向模型的车辆而言，车辆运动时的曲率k满足公式其中l为车辆轴距，δ为前轮转角。若要确保规划出的曲线是车辆实际可行驶的，就需要曲线满足曲率连续的条件。因此为了满足车辆运动学对曲率连续的约束，构建如下所述的回旋线，该回旋线的曲率是连续变化的。
[0049]
2.回旋线的定义
[0050]
为保证曲线满足曲率连续的条件，假设曲线的曲率一阶线性变化，然后根据曲线的曲率方程，可以推导出曲线的位置和航向信息，因此可构建如下回旋线方程：
[0051]
0≤s≤l
[0052]
k(s)＝k0 k
′s[0053][0054]
[0055][0056]
其中，l为曲线总长度，s为曲线位置，为初始航向角，k0为初始曲率，k
′
为常数，表示曲率变化率，k(s)为曲线上长度s处的曲率，为曲线上长度s处的航向角，x(s)为曲线上长度s处的x坐标，y(s)为曲线上长度s处的y坐标。
[0057]
3.规划曲线求解
[0058]
假设规划起点为规划终点为因此规划出的轨迹需满足以下条件：
[0059]
x(0)＝x0[0060]
x(l)＝x1[0061]
y(0)＝y0[0062]
y(l)＝y1[0063][0064][0065]
k(0)＝k0[0066]
k(l)＝k1[0067]
其中，x(0)为曲线起点处的x坐标，x(l)为曲线终点处的x坐标，y(0)为曲线起点处的y坐标，y(l)为曲线终点处的y坐标，为曲线起点处的航向角，为曲线终点处的航向角，k(0)为曲线起点处的曲率，k(l)为曲线终点处的曲率，表示第二段回旋线后的结束位置处的目标航向角，k1表示第二段回旋线后的结束位置处的目标曲率，x1和y1分别表示第二段回旋线后的结束位置处的目标横坐标和目标纵坐标。
[0068]
由于一段回旋线无法同时满足以上条件，因此定义两段连续的回旋线，假设两段回旋线的长度分别为l1、l2,两段回旋线的曲率变化率分别为k
′1、k
′2。从规划起点开始，经过两段连续的回旋线后，回旋线的终点位置如下：
[0069]kcal1
＝k0 k
′1l1[0070]kcal2
＝k
cal1
k
′2l2[0071][0072][0073][0074][0075]
[0076][0077]
其中，为从规划起点开始，经过第一段回旋线后的结束位置，为从第一段回旋线的结束位置开始，经过第二段回旋线后的位置，因此规划的目标是求解出一组(l1,l2,k
′1,k
′2)，使得第二段回旋线后的位置尽可能的逼近规划终点
[0078]
因此可构建如下目标函数：
[0079][0080]kcal1
＜k
max
[0081]kcal2
＜k
max
[0082]
其中，wi为各项的权重系数，k
max
为车辆运动的最大曲率。
[0083]
通过求解以上非线性二次规划问题，可得出使目标函数f(l
1 l
2 k
′1k
′2)最优的两段回旋线参数：(l1,l2,k
′1,k
′2)，将(l1,l2,k
′1,k
′2)带入回旋线方程中，即可求得满足规划条件的回旋线。
[0084]
请参见图2，图2是本发明实施例的硬件设备工作示意图，所述硬件设备具体包括：一种基于多段回旋线的轨迹规划的设备401、处理器402及存储设备403。
[0085]
一种基于多段回旋线的轨迹规划的设备401：所述一种基于多段回旋线的轨迹规划的设备401实现所述一种基于多段回旋线的轨迹规划的方法。
[0086]
处理器402：所述处理器402加载并执行所述存储设备403中的指令及数据用于实现所述一种基于多段回旋线的轨迹规划的方法。
[0087]
存储设备403：所述存储设备403存储指令及数据；所述存储设备403用于实现所述一种基于多段回旋线的轨迹规划的方法。
[0088]
本发明的有益效果是：通过使用分段的回旋线进行轨迹规划，具备曲率连续、曲率变化率稳定、满足车辆运动学约和求解速度快等优点。
[0089]
以上所述仅为本发明的较佳实施例，并不用以限制本发明，凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。