一种基于监控策略的无人车换道路径规划方法与流程

1.本发明涉及的无人车辆技术领域，具体涉及基于监控策略的无人车辆换道路径规划方法。


背景技术：

2.随着科学技术的不断发展，路径规划技术所面临的环境将更为复杂多变。这就要求路径规划算法要具有迅速响应复杂环境变化的能力。
3.运动规划作为一种具有多个非线性约束的高维优化问题，需要消耗大量的计算资源，并且由于场景的复杂性和实时性，不是总能快捷简单的得到最优轨迹，这对规划算法的鲁棒性和实用性有着较高的要求。
4.国家专利201710953896.9提出的一种仿优秀驾驶员的无人车换道路径规划方法，规划无人驾驶车辆行驶路径，但是在未能规划轨迹或者规划超时，影响车辆在换道过程中的稳定性和可行性。


技术实现要素：

5.为了解决现有技术中存在的不足，本技术提出了一种基于监控策略的无人车换道路径规划方法，换道轨迹规划是在三次b样条的方法基础上，引入了基于快速扩展随机树算法作为补充算法；同时还引入了监控轨迹的规划思想，通过减速规划以保持车辆实时运动的稳定性。在车辆行驶时，同时进行换道轨迹规划和监控轨迹规划，保证车辆换道的安全性和稳定性。
6.本发明所采用的技术方案如下：
7.一种基于监控策略的无人车换道路径规划方法，包括如下步骤：
8.步骤1：利用环境感知模块得到环境信息和车辆自身状态信息，并根据环境信息和车辆自身状态信息判断是否产生新的换道任务；
9.步骤2：对于产生新的换道任务，则采用基于b样条曲线的方法来规划连续曲率路径；并对基于b样条曲线得到的换道轨迹进行安全性判断；若符合安全性要求，则输出换道轨迹；
10.步骤3：当基于b样条曲线得到的换道轨迹不符合安全性要求，且路径存在时，添加基于快速扩展随机树的算法作为补充算法以获得换道轨迹；并对基于快速扩展随机树的算法获得的换道轨迹进行安全性判断；若符合安全性要求，则输出换道轨迹；
11.步骤4：如果步骤2和步骤3中的换道轨迹规划失败或者不存在换道任务，则输出监控轨迹。
12.进一步，步骤2中采用基于b样条曲线的方法来规划连续曲率路径的方法为：
13.步骤2.1换道路径规划可以被定义为换道过程中的两点边值问题：
14.minf(x)
15.s.t.gi(x)＝1,i＝1,...,n
16.hj(x)≤0,j＝1,...,n
17.其中，f(x)是目标函数，gi(x)代表起始点和目标点的约束，hj(x)代表安全距离约束，n为起始点和目标点之间的节点数；
18.步骤2.2为了满足车辆转向执行器的限制，生成的路径应该满足连续曲率、峰值曲率和最大曲率的约束。
19.进一步，b样条曲线的路径规划两点边值问题可以归结为：
[0020][0021][0022][0023]
α
min
≤αi≤α
max
,i＝1,2
[0024][0025]
δ
log-h(l
obs
,v
obs
,l0,l
fba
,v
ego
)≤0
[0026]
δ
lat-h(l
obs
,v
obs
,l0,l1,w,α1,l
fba
,v
ego
)≤0
[0027]
δ
lat-h(l
obs
,v
obs
,l0,l1,l2,w,α1,α3,l
fba
,v
ego
)≤0
[0028]
其中，lj为控制段j的长度，j＝0、1、2；αi是控制点pi处的夹角，i＝1、2；α
min
、α
max
是夹角αi取值的最小值与最大值，k
max
为峰值曲率，代表起始点，x0,y0,v0分别是初始点对应的横坐标，纵坐标，横摆角，车速；代表目标点，xg,yg,vg分别是起始点目标点对应的横坐标，纵坐标，横摆角，车速；xg,yg,vg分别是目标点目标点对应的横坐标，纵坐标，横摆角，δ
log
是纵向安全距离，δ
lat
是横向安全距离，l
obs
是目标车辆到障碍车辆的距离，v
ego
是自车速度，w是自车宽度，l
fba
是自车从后轴到头部的长度，v
obs
是障碍车辆的车速。
[0029]
进一步，步骤3中引入了最近邻域搜索方法减少采样的盲目性，根据欧几里德距离，节点n1是新采样节点n
new
的最近节点；然而，从节点n2到节点n1的转向角更大，导致路径s1和n1连接更为尖锐；为了使路径更利于车辆跟踪，由定义的传统度量函数被修改为式：
[0030][0031][0032]
其中，x1,x2分别为1,2节点，d
eu
表示度量函数，x1,x2分别为1,2节点的横坐标，y1,y2分别是1,2节点的纵坐标，θ为n1,n2连接n
new
的夹角。
[0033]
进一步，步骤4中输出监控轨迹的方法为：
[0034]
路径规划系统在规划监控轨迹时，如果监控轨迹的减速度为a
x
，则对于直线行驶：
[0035][0036]
其中a
min
是车辆最小加速度，v0是初始速度，vg是目标速度，l
0g
是减速长度。
[0037]
如果转弯半径为r，则对于直线行驶：
[0038][0039]
其中，μ为路面粗糙度；
[0040]
在路径生成后，应根据边界约束，执行器约束和稳定性约束计算速度曲线，如下所示：
[0041][0042]
其中，边界约束即起始速度和目标速度，执行器约束即最大速度和最大加速度，稳定性约束即最大侧向加速度，v
min
、v
max
是速度的最小值和最大值；a
min
、a
max
是加速度的最小值和最大值；ay(t)是侧向加速度，a
ymax
是最大侧向加速度。
[0043]
进一步，通过控制段与障碍物之间的距离来表示生成路径的安全性检测，将安全性检测表示为：
[0044]
(1)第一个控制段的纵向安全距离：
[0045]
l
obs
v
obs
t
0-(l0 l
fba
)≥δ
log
,t0＝l0/v
ego
[0046]
(2)第二个控制段的横向安全距离：
[0047]
l
obs
v
obc
t
1-l0 (l1 l
fba
)|cosα1| 0.5w|sinα1|≥δ
lat
,t1＝l1/v
ego
[0048]
(3)第三个控制段的横向安全距离：
[0049]
l
obs
v
obs
t
2-(x
2-x1 (l1 l
fba
))sinα
2-0.5w-(d
obs-(y
2-y1))cosα2≥δ
lat
,
[0050]
t2＝l2/v
ego
[0051]
根据自我车辆的位置，存在三个阶段发生碰撞风险，其中需满足各自的安全约束条件；其中，l
obs
是目标车辆到障碍车辆的距离，v
ego
是自车速度，v
obs
是障碍车辆的车速，t0是自车行驶过第一段距离的时间，t1是自车行驶过第二段距离的时间，t2是自车行驶过第二段距离的时间，l
fba
是自车从后轴到头部的长度，w是自车宽度，δlat是横向安全距离，δlog是纵向安全距离，α1是第一控制段与第二控制段间的夹角，α2是第二控制段与第三控制段间的夹角，d
obs
为障碍车辆的形成障碍的宽度，x1,x2分别是车辆在控制点p1,p2的横坐标，y1,y2分别是车辆在控制点p1,p2的纵坐标。
[0052]
进一步，所获取的环境信息包括：当前行驶前方障碍物信息、车道宽度、当前传感器检测盲区信息、当前行驶道路侧向障碍物信息；所述车辆自身状态信息包括：车辆速度、转向盘转角、车辆当前位置。
[0053]
进一步，所述环境感知模块包括用于获取环境信息的毫米波雷达与ccd工业相机；毫米波雷达设置为4个，其中1个分置在车辆前端保险杆中间位置，另外2个分别放置在两侧的前门和后门之间的中间位置，最后1个放置在车辆的尾部，用于检测车辆四个方向上的障碍物信息并传输给电子控制单ecu；ccd工业相机安装在车辆前挡风玻璃顶部上方，用于将
车辆前方的状况传输给电子控制单元ecu。
[0054]
本发明的有益效果：
[0055]
(1)本发明所提出的一种轨迹监控策略，同时进行换道轨迹规划和监控轨迹规划，在换道轨迹规划未能规划出轨迹或者规划超时，系统将以监控轨迹作为输出，保证车辆换道的稳定性和可行性。
[0056]
(2)监控轨迹遵循无碰撞要求，通过减速规划以保持车辆实时运动的稳定性。
[0057]
(3)为满足车辆运动约束以及实时性要求，提出基于b样条曲线凸特征的安全距离约束，可以通过控制段与障碍物之间的距离来表示生成路径的安全检测。同时基于rrt的方法可以作为补充算法尽可能找到路径。监控轨迹主要遵循无碰撞要求，通过减速规划以保持车辆实时运动的安全性和稳定性。
附图说明
[0058]
图1是本发明的方法流程图。
[0059]
图2是三次b样条曲线的控制点图。
[0060]
图3是安全距离约束示意图。
[0061]
图4是最近邻域搜索法度量示意图。
具体实施方式
[0062]
为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅用于解释本发明，并不用于限定本发明。
[0063]
如图1所示，本发明提供一种基于监控策略的无人车换道路径规划方法，包括以下步骤：
[0064]
步骤1：利用环境感知模块得到环境信息和车辆自身状态信息，并根据上述信息判断是否存在新的驾驶任务，即换道任务。
[0065]
所述环境感知模块包括用于获取环境信息的毫米波雷达与ccd工业相机；具体地，毫米波雷达设置为4个，其中1个分置在车辆前端保险杆中间位置，另外2个分别放置在两侧的前门和后门之间的中间位置，最后1个放置在车辆的尾部，用于检测车辆四个方向上的障碍物信息并传输给电子控制单ecu。ccd工业相机安装在车辆前挡风玻璃顶部上方，用于将车辆前方的状况传输给电子控制单元ecu。所获取的环境信息包括：当前行驶前方障碍物信息、车道宽度、当前传感器检测盲区信息、当前行驶道路侧向障碍物信息；所述车辆自身状态信息包括：车辆速度、转向盘转角、车辆当前位置。
[0066]
步骤2：如果产生新的驾驶任务，则采用基于b样条曲线的方法来规划连续曲率路径。
[0067]
所述步骤2中基于b样条曲线的方法来规划连续曲率路径具体过程如下：
[0068]
步骤2.1换道路径规划可以被定义为换道过程中的两点边值问题：
[0069]
minf(x)
[0070]
s.t.gi(x)＝1,i＝1,...,n
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(1)
[0071]hj
(x)≤0,j＝1,...,n
[0072]
其中，f(x)是目标函数，通常用最短时间或者路程来描述代价，gi(x)代表起始点和目标点的约束，hj(x)代表安全距离约束；n为起始点和目标点之间的节点数。其中如图2所示，安全距离约束要求是基于b样条曲线的凸特征，可以通过控制段与障碍物之间的距离来表示生成路径的安全性检测。根据自我车辆的位置，存在三个阶段发生碰撞风险，其中需满足各自的安全约束条件。假设自动车辆的避障是通过强制转向而不是改变速度来实现的，其目的在于使乘客感到舒适。轨迹安全性检测如下：
[0073]
(1)第一个控制段的纵向安全距离：
[0074]
l
obs
v
obs
t
0-(l0 l
fba
)≥δ
log
,t0＝l0/v
ego
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(2)
[0075]
(2)第二个控制段的横向安全距离：
[0076]
l
obs
v
obc
t
1-l0 (l1 l
fba
)|cosα1| 0.5w|sinα1|≥δ
lat
,t1＝l1/v
ego
ꢀꢀꢀꢀꢀ
(3)
[0077]
(3)第三个控制段的横向安全距离：
[0078][0079]
其中，l
obs
是目标车辆到障碍车辆的距离，v
ego
是自车速度，v
obs
是障碍车辆的车速，t0是自车行驶过第一段距离的时间，t1是自车行驶过第二段距离的时间，t2是自车行驶过第二段距离的时间，l
fba
是自车从后轴到头部的长度，w是自车宽度，δ
lat
是横向安全距离，δ
log
是纵向安全距离，α1是第一控制段与第二控制段间的夹角，α2是第二控制段与第三控制段间的夹角，d
obs
为障碍车辆的形成障碍的宽度，x1,x2分别是车辆在控制点p1,p2的横坐标，y1,y2分别是车辆在控制点p1,p2的纵坐标。
[0080]
步骤2.2为了满足车辆转向执行器的限制，生成的路径应该满足连续曲率，峰值曲率和最大曲率的约束，如图3所示，采用参数化的三次b样条曲线来规划路径，则路径表达式为：
[0081][0082]
其中xi(t)、yi(t)分别是车辆在控制点pi的横、纵坐标，αi是控制点pi处的夹角，lj为控制段j的长度。
[0083]
曲率表达式为：
[0084][0085]
其中为xi(t)、yi(t)的导数，为xi(t)、yi(t)的二次导数。
[0086]
为了满足车辆的峰值曲率约束，控制线的长度和夹角受下式约束：
[0087][0088]
其中，k
max
为峰值曲率。
[0089]
b样条曲线的路径规划两点边值问题可以归结为：
[0090][0091]
其中代表起始点，x0,y0,v0分别是初始点对应的横坐标，纵坐标，横摆角，车速；代表目标点，xg,yg,vg分别是目标点对应的横坐标，纵坐标，横摆角，车速。
[0092]
步骤3：当基于b样条的算法没有解决方案但路径存在时，添加基于快速扩展随机树的算法作为补充算法以尽可能地找到路径。
[0093]
进一步，所述步骤3具体如下，
[0094]
当基于b样条的算法没有解决方案但路径存在时，添加基于快速扩展随机树的算法以尽可能地找到路径。为了加快算法的求解速度，引入了一些措施来减少采样的盲目性，这里拟采用最近邻域搜索方法，其原理如图4所示，根据欧几里德距离，节点n1是新采样节点n
new
的最近节点。然而，从节点n2到节点n1的转向角更大，导致路径s1和n1连接更为尖锐。为了使路径更利于车辆跟踪，由定义的传统度量函数被修改为式：
[0095][0096]
其中，x1,x2分别为1,2节点，d
eu
表示度量函数，x1,x2分别为1,2节点的横坐标，y1,y2分别是1,2节点的纵坐标，θ为n1,n2连接n
new
的夹角。
[0097]
步骤4：如果路径规划失败或者不存在新的驾驶任务，即换道任务，则输出监控轨迹；否则输出换道轨迹。步骤4中输出监控轨迹具体过程如下：
[0098]
基于b样条的方法和基于快速扩展随机树的方法都可以产生曲率连续路径并满足运动限制，但两者都有可能超时或失败，则输出监控轨迹。路径规划系统在规划监控轨迹时，如果监控轨迹的减速度为α
x
，则对于直线行驶：
[0099][0100]
其中，a
min
是车辆最小加速度，v0是初始速度，vg是目标速度，l
0g
是减速长度。
[0101]
如果转弯半径为r，则对于直线行驶：
[0102][0103]
其中，μ为路面粗糙度。
[0104]
在路径生成后，应根据边界约束(起始速度和目标速度)，执行器约束(最大速度和最大加速度)和稳定性约束(最大侧向加速度)计算速度曲线，如下所示：
[0105][0106]
其中，v
min
、v
max
是速度的最小值和最大值；a
min
、a
max
是加速度的最小值和最大值；ay(t)是侧向加速度，a
ymax
是最大侧向加速度
[0107]
以上实施例仅用于说明本发明的设计思想和特点，其目的在于使本领域内的技术人员能够了解本发明的内容并据以实施，本发明的保护范围不限于上述实施例。所以，凡依据本发明所揭示的原理、设计思路所作的等同变化或修饰，均在本发明的保护范围之内。