本发明涉及自动驾驶技术领域,具体涉及一种自动驾驶中车辆变道至目标车道前车之前的预测方法。
背景技术:
自动驾驶包括预测、决策、规划和控制四大模块,预测模块根据各车道的车辆行驶状态数据和车道环境数据预测车辆在未来时刻是否会变道,以及变道情况下的变道轨迹。变道包括左变道、右变道,左、右变道又包括变道至目标车道前车之前和变道至目标车道前车之后。决策模块根据预测模块的预测输出结果、环境信息、导航信息、本车驾驶员行为模块、本车动力学模型等计算本车的下一个期望状态;规划模块根据当前状态以及下一个期望状态规划本车行驶轨迹;控制模块根据规划的行驶轨迹计算相应的油门、制动和转向。
目前,一些互联网企业和汽车厂商的自动驾驶系统中,将预测出的所有感兴趣车辆(定义为可能对本车行驶带来干扰的周围车辆)均默认为在未来时刻会发生变道。这些车辆中不排除有部分车辆变道的意愿度较低的车辆,这里的意愿度为变道的意愿程度。这些变道意愿度较低的周围感兴趣车辆会干扰本车决策,还会增加本车对各车行驶状态时护具分析的计算量,增加自动驾驶系统运行难度和运行成本。
技术实现要素:
本发明提供一种自动驾驶中车辆变道至目标车道前车之前的预测方法,结合预变道车辆、预变道车辆所在车道的前方第一辆车、目标车道内位于预变道车辆前方第一辆车和目标车道内位于预变道车辆前方第二辆车这四辆车的行驶状态数据计算出预变道车辆变道至目标车道位于预变道车辆前方第一辆车之前的意愿度,根据意愿度来预测预变道车辆在未来时刻是否会变道至目标车道位前方第一辆车之前,有效排除周围车辆干扰本车决策的因素,提高本车决策的确信度;同时,有效降低自动驾驶系统中预测模块的数据分析计算量,降低自动驾驶系统的运行难度和运行成本,有效改善决策的滞后延时。
为了解决上述技术问题,本发明提供了一种自动驾驶中车辆变道至目标车道前车之前的预测方法,包括以下步骤,
获取预变道车辆、车辆一、车辆二、车辆三当前采样周期的行驶状态数据;其中,所述车辆一为预变道车辆所在车道的前方第一辆车;所述车辆二为目标车道内位于预变道车辆前方的第一辆车;所述车辆三为目标车道内位于预变道车辆前方的第二辆车;
结合各车辆的所述行驶状态数据计算出所述预变道车辆变道至所述车辆二之前的意愿度λ,0≤λ≤1;
当所述意愿度超过意愿度阈值时,预测出所述预变道车辆将在未来时刻变道至所述车辆二之前。
本发明一个较佳实施例中,进一步包括,所述预变道车辆向目标车道方向的横向速度小于零、且维持一定时间时,所述预变道车辆变道至目标车道的意愿度为零。
本发明一个较佳实施例中,进一步包括,计算所述意愿度λ包括,
存在一个时间t∈[0,tthres],tthres为能够调整的标定量;
存在一个时间t1,该时间内预变道车辆的行驶速度大于车辆一的行驶速度;
存在一个时间t2,该时间内所述车辆三位于车辆一的前方;
存在一个时间t3,该时间内所述车辆一位于车辆二的前方;
存在一个时间t4,该时间内所述预变道车辆位于车辆二的前方;
当时间t1、时间t2、时间t3和时间t4四者在时间t内有交集、且预变道车辆的行驶速度大于车辆二的行驶速度时,计算所述意愿度λ考虑预变道车辆实际加速度、预变道车辆和车辆一之间沿车道线方向间隔距离和预变道车辆与车辆一之间相对速度的影响、车道拥堵程度的影响、车辆三和车辆一之间纵向距离的影响、车辆三与车辆一两者行驶速度比的影响;否则,所述意愿度λ为零。
本发明一个较佳实施例中,进一步包括,计算所述意愿度λ包括,计算预变道车辆实际加速度影响下的意愿度λ1;
计算预变道车辆和车辆一之间沿车道线方向间隔距离影响下的意愿度λ2;
计算预变道车辆与车辆一之间相对速度影响下的意愿度λ3;
计算车道拥堵程度影响下的意愿度λ4;
计算车辆三和车辆一之间纵向距离的影响下的意愿度λ5;
计算车辆三与车辆一两者行驶速度比影响下的意愿度λ6;
λ=max{λ1,λ2,λ3,λ4,λ5,λ6}(式一);
根据式一计算预变道车辆变道至车辆二之前的意愿度λ。
本发明一个较佳实施例中,进一步包括,根据公式一计算所述意愿度λ1,
a表征预变道车辆的实际加速度;
k1表征预变道车辆实际加速度与意愿度λ1之间函数关系的直线斜率;
a1表征预变道车辆实际加速度所在加速度轴上的一个固定加速度值;
δλ表征预变道车辆的实际加速度超出加速度阈值持续一定时间的意愿度增量。
本发明一个较佳实施例中,进一步包括,计算所述意愿度λ2包括,
计算l1=a+b、a=s1-s2+v1*tr+d;
a表征预变道车辆以行驶速度v2行驶时与车辆一需要保持的距离;
b表征预变道车辆以减速度ax从行驶速度v1减速至行驶速度v2的行驶距离;
l1为预变道车辆和车辆一之间的初始间距;
v1为预变道车辆的行驶速度;
v2为车辆一的行驶速度;
s1为预变道车辆制动至停止的制动距离;
s2为车辆一制动至停止的制动距离;
tr为预变道车辆的制动反应时间;
d为预变道车辆和车辆一均制动至停止时两者之间的安全停车距离;
当b=(l1-a)≤0时,意愿度λ2=1;
当b=(l1-a)>0,且v1≤v2时,意愿度λ2=0;
当b=(l1-a)>0,且v1>v2时,根据式二计算预变道车辆的减速度ax:
根据公式二计算所述意愿度λ2:
其中,a表征预变道车辆的预设舒适制动减速度athres与减速度ax的差值;
k2表征减速度athres与减速度ax的差值与意愿度λ2之间函数关系的直线斜率;
a1表征减速度athres与减速度ax的差值所在减速度轴上的一个固定减速度值。
本发明一个较佳实施例中,进一步包括,根据公式三计算所述意愿度λ3,
其中,x表征预变道车辆与车辆一的相对速度;
k3表征预变道车辆与车辆一的相对速度与意愿度λ3之间函数关系的直线斜率;
x1表征预变道车辆与车辆一的相对速度所在速度轴上的一个固定速度值。
本发明一个较佳实施例中,进一步包括,根据公式四计算所述意愿度λ4,
其中,ρ1表征预变道车辆所在车道前方拥堵程度;
ρ2表征车辆二所在车道前方拥堵程度。
本发明一个较佳实施例中,进一步包括,计算所述意愿度λ5包括,
计算a=s1-s2+v1*tr+d
a表征预变道车辆以行驶速度v2行驶时需要与车辆一保持的距离;
v1为预变道车辆的行驶速度;
v2为车辆一的行驶速度;
s1为预变道车辆制动至停止的制动距离;
s2为车辆一制动至停止的制动距离;
tr为预变道车辆的制动反应时间;
d为预变道车辆和车辆一均制动至停止时两者之间的安全停车距离;
l1为预变道车辆和车辆一之间的初始间距;
当l1>a+δ1时,λ5=0;
当l1≤a+δ1时,根据公式五计算所述意愿度λ5,
其中,δ1表征一个固定的距离值;
e表征车辆一与车辆三之间的沿车道线距离;
k4表征车辆一与车辆三之间沿车道线距离与意愿度λ5之间函数关系的直线斜率;
e1和e2表征车辆一与车辆三之间沿车道线距离所在距离轴上的两个固定距离值。
本发明一个较佳实施例中,进一步包括,计算所述意愿度λ6包括,
计算a=s1-s2+v1*tr+d
a表征预变道车辆以行驶速度v2行驶时需要与车辆一保持的距离;
v1为预变道车辆的行驶速度;
v2为车辆一的行驶速度;
s1为预变道车辆制动至停止的制动距离;
s2为车辆一制动至停止的制动距离;
tr为预变道车辆的制动反应时间;
d为预变道车辆和车辆一均制动至停止时两者之间的安全停车距离;
l1为预变道车辆和车辆一之间的初始间距;
当l1>a+δ2时,λ6=0;
当l1≤a+δ2时,根据公式六计算所述意愿度λ6,
其中,δ2表征一个固定的距离值;
f表征车辆三行驶速度与车辆一行驶速度的比值;
f1和f2表征车辆三和车辆一两者行驶速度比所在轴上的两个固定值;
k5表征车辆三与车辆一行驶速度比与意愿度λ6之间函数关系的直线斜率;
λ*表征大于零小于等于1的一个固定值。
本发明的有益效果:
本发明自动驾驶中车辆变道至目标车道前车之前的预测方法,结合预变道车辆、预变道车辆所在车道的前方第一辆车、目标车道内位于预变道车辆前方第一辆车和目标车道内位于预变道车辆前方第二辆车这四辆车的行驶状态数据计算出预变道车辆变道至目标车道位于预变道车辆前方第一辆车之前的意愿度,根据意愿度来预测预变道车辆在未来时刻是否会变道至目标车道位于预变道车辆前方第一辆车之前,有效排除周围车辆干扰本车决策的因素,提高本车决策的确信度;同时,有效降低自动驾驶系统中预测模块的数据分析计算量,降低自动驾驶系统的运行难度和运行成本,有效改善决策的滞后延时。
附图说明
图1是本发明优选实施例中预测方法的流程图;
图2是本发明优选实施例中计算被预测车辆变道至目标车道前方第一辆车之前的路况示意图;
图3a是本发明优选实施例中计算意愿度λ1的曲线图;
图3b是本发明优选实施例中计算意愿度增量δλ的曲线图;
图4是本发明优选实施例中计算意愿度λ2的曲线图;
图5是本发明优选实施例中计算意愿度λ3的曲线图;
图6是本发明优选实施例中计算意愿度λ5的曲线图;
图7是本发明优选实施例中计算意愿度λ6的曲线图。
具体实施方式
下面结合附图和具体实施例对本发明作进一步说明,以使本领域的技术人员可以更好地理解本发明并能予以实施,但所举实施例不作为对本发明的限定。
实施例一
本实施例公开一种自动驾驶中车辆变道至目标车道前车之前的预测方法,参照图1所示,其包括以下步骤,
获取预变道车辆、车辆一、车辆二、车辆三当前采样周期的行驶状态数据,其中,上述车辆一为预变道车辆所在车道的前方第一辆车;上述车辆二为目标车道内位于预变道车辆前方的第一辆车;上述车辆三为目标车道内位于预变道车辆前方的第二辆车。参照图2所示,车辆4表征预变道车辆,车辆1表征车辆一,车辆2表征车辆二,车辆3表征车辆三。各车的行驶状态数据包括行驶速度、横向速度、各车之间沿车道线方向的距离(或称“纵向距离”)、加速度、减速度、横向加减速度等。当图2中某辆车不存在时,认为与该车相关的距离为无穷远。
结合预变道车辆、车辆一、车辆二、车辆三的行驶状态数据计算出车辆4变道至车辆二之前的意愿度λ,0≤λ≤1;
当意愿度λ超过意愿度阈值时,预测出车辆4将在未来时刻变道至车辆二之前。此处,根据驾驶员特性预设意愿度阈值,比如,预设意愿度阈值为0.6,或者0.65,或者0.7等。
计算上述意愿度λ包括排除以下否定项:
否定项1:车辆4向目标车道方向的横向速度vy小于零、且维持一定时间时,车辆4变道至车辆二所在车道的意愿度为零。比如,车辆4向车辆二所在车道方向的横向速度vy小于等于零、且持续2s时,车辆4变道至车辆二所在车道的意愿度为零。此处的持续时间为能够调整的标定量。需要说明的有:横向速度vy大于0,表征车辆4偏向目标车道;横向速度vy小于0,表征车辆4偏离目标车道。
否定项2:存在一个时间t∈[0,tthres],tthres为能够调整的时间标定量,比如,标定tthres为20s,或者25s;
存在一个时间t1,该时间内预变道车辆的行驶速度大于车辆一的行驶速度;
存在一个时间t2,该时间内所述车辆三位于车辆一的前方;
存在一个时间t3,该时间内所述车辆一位于车辆二的前方;
存在一个时间t4,该时间内所述预变道车辆位于车辆二的前方;
上述时间t1、时间t2、时间t3和时间t4四者在时间t内有交集、且车辆4的行驶速度大于车辆二的行驶速度时,计算上述意愿度λ考虑车辆4实际加速度、车辆4和车辆一之间沿车道线方向间隔距离和车辆4与车辆一之间相对速度的影响、车道拥堵程度的影响、车辆三和车辆一之间纵向距离的影响、车辆三与车辆一两者行驶速度比的影响;否则,上述意愿度λ为零。
计算意愿度λ包括,
计算车辆4实际加速度影响下的意愿度λ1;
计算车辆4和车辆一之间沿车道线方向间隔距离影响下的意愿度λ2;
计算车辆4与车辆一之间相对速度影响下的意愿度λ3;
计算车道拥堵程度影响下的意愿度λ4;
计算车辆三和车辆一之间纵向距离的影响下的意愿度λ5;
计算车辆三与车辆一两者行驶速度比影响下的意愿度λ6;
λ=max{λ1,λ2,λ3,λ4,λ5,λ6}(式一);
根据式一计算车辆4至车辆二之前的意愿度λ。
本实施例技术方案中,优选通过以下方式计算获得意愿度λ1、λ2、λ3、λ4、λ5和λ6:
(1)先根据车辆4当前的实际加速度a计算一个意愿度,然后根据加速度a持续的时间计算一个意愿度的增量δλ,该增量随持续时间的增加变化,并且不同加速度a对应的增量曲线斜率不同,加速度a值越大,斜率越大,意愿度λ4为实际加速度a对应的意愿度和意愿度增量之和,但两者之和应与1取小。
以车辆4当前的实际加速度a为横坐标,意愿度λ1为纵坐标建立坐标系,意愿度λ1与车辆4当前实际加速度a的函数关系参照图3(a)所示,
a表征车辆4的实际加速度;
k1表征车辆4实际加速度与意愿度λ4之间函数关系的直线斜率;
a1表征横坐标轴上的一个固定加速度值,比如,a1取0.1g。此处a1为能够调整的标定量。
δλ表征车辆4的实际加速度超出加速度阈值持续一定时间的意愿度增量。
参照图3(b)所示,以车辆4的实际加速度超出加速度阈值持续的时间为横坐标,意愿度增量δλ为纵坐标轴建立坐标系,车辆4的实际加速度超出加速度阈值持续的时间与意愿度增量δλ之间的函数关系参照图3(b)所示。
图3(a)和图3(b)中,除纵轴坐标最大值为1外,直线的折点横坐标值、纵坐标值、直线斜率均为能够调整的标定参数。
(2)计算所述意愿度λ2包括,
计算l1=a+b、a=s1-s2+v1*tr+d;
a表征车辆4以行驶速度v2行驶时与车辆一需要保持的距离;
b表车辆4以减速度ax从行驶速度v1减速至行驶速度v2的行驶距离;
l1为车辆4和车辆一之间的初始间距;
v1为车辆4的行驶速度;
v2为车辆一的行驶速度;
s1为车辆4制动至停止的制动距离;
s2为车辆一制动至停止的制动距离;
tr为车辆4的制动反应时间;
d为车辆4和车辆一均制动至停止时两者之间的安全停车距离;
当b=(l1-a)≤0时,意愿度λ2=1;
当b=(l1-a)>0,且v1≤v2时,意愿度λ2=0;
当b=(l1-a)>0,且v1>v2时,根据式二计算预变道车辆的减速度ax:
根据驾驶员特性预设舒适制动的减速度athres,参照图4所示,以减速度athres和减速度ax的差值为横坐标,意愿度λ2为纵坐标建立坐标系,意愿度λ2与减速度athres和减速度ax的差值之间的函数关系为:
其中,a表征车辆4的预设舒适制动减速度athres与减速度ax的差值;
k2表征减速度athres与减速度ax的差值与意愿度λ2之间函数关系的直线斜率;
a1表征横坐标轴上的一个固定减速度值,比如,a1取0.5g。其为能够调整的标定量。
图4中,除纵轴坐标最大值为1外,直线的折点横坐标值、纵坐标值、直线斜率均为能够调整的标定参数。
(3)以车辆4与车辆一的相对速度为横坐标,意愿度λ3为纵坐标建立坐标系,车辆4与车辆一的相对速度与λ3之间的函数关系参照图5所示,
其中,x表征车辆4与车辆一的相对速度;
k3表征车辆4与车辆一的相对速度与意愿度λ3之间函数关系的直线斜率;
x1表征横坐标轴上的一个固定速度值,比如,x1取v1,v1为车辆4的行驶速度。此处x1为能够标定的变量。
图5中,除纵轴坐标最大值为1外,直线的折点横坐标值、纵坐标值、直线斜率均为能够调整的标定参数。
(4)根据公式四计算所述意愿度λ4,
其中,ρ1表征预变道车辆所在车道前方拥堵程度;
ρ2表征车辆二所在车道前方拥堵程度。
此处计算拥堵程度时,以一定距离内的车数量表征,比如,前方100mm内有6辆车,计算出来的拥堵程度为
(5)计算所述意愿度λ5包括,
计算a=s1-s2+v1*tr+d
a表征车辆4以行驶速度v2行驶时需要与车辆一保持的距离;
v1为车辆4的行驶速度;
v2为车辆一的行驶速度;
s1为车辆4制动至停止的制动距离;
s2为车辆一制动至停止的制动距离;
tr为车辆4的制动反应时间;
d为车辆4和车辆一均制动至停止时两者之间的安全停车距离;
l1为车辆4和车辆一之间的初始间距;
当l1>a+δ1时,λ5=0;此处,δ1表征一个固定的距离值,其为能够调整的标定量。
当l1≤a+δ1时,意愿度λ5与车辆一和车辆三之间的沿车道线距离相关,以车辆一和车辆三之间沿车道线距离为横坐标,意愿度λ5为纵坐标建立坐标系,意愿度λ5与车辆一和车辆三之间的沿车道线距离相关之间的函数关系参照图6所示,
其中,e表征车辆一与车辆三之间的沿车道线距离;
k4表征车辆一与车辆三之间沿车道线距离与意愿度λ5之间函数关系的直线斜率;
e1和e2表征横坐标轴上的两个固定距离值,e1取2m,e2取0.5s*v1;e1和e2均为能够能够调整的标定量。
图6中,除纵轴坐标最大值为1外,直线的折点横坐标值、纵坐标值、直线斜率均为能够调整的标定参数.
(6)计算所述意愿度λ6包括,
计算a=s1-s2+v1*tr+d
a表征车辆4以行驶速度v2行驶时需要与车辆一保持的距离;
v1为车辆4的行驶速度;
v2为车辆一的行驶速度;
s1为车辆4制动至停止的制动距离;
s2为车辆一制动至停止的制动距离;
tr为车辆4的制动反应时间;
d为车辆4和车辆一均制动至停止时两者之间的安全停车距离;
l1为车辆4和车辆一之间的初始间距;
当l1>a+δ2时,λ6=0。此处,δ2表征一个固定的距离值,其为能够调整的标定量。
当l1≤a+δ2时,意愿度λ6与车辆三与车辆一的行驶速度比f相关,以车辆三与车辆一的行驶速度比为横坐标,意愿度λ6为纵坐标建立坐标系,参照图7所示,意愿度λ6与车辆三与车辆一的行驶速度比f的函数关系为:
其中,f表征车辆三行驶速度与车辆一行驶速度的比值;
f1和f2表征横坐标上的两个固定值。比如,f1取1,f2取1.5。此处,f1和f2均为能够调整的标定量。
k5表征车辆三与车辆一行驶速度比与意愿度λ6之间函数关系的直线斜率;
λ*表征大于零小于等于1的一个固定值,其为能够调整的标定量。
图6中,除纵轴坐标最大值为1外,直线的折点横坐标值、纵坐标值、直线斜率均为能够调整的标定参数。
以上,结合预变道车辆、预变道车辆所在车道的前方第一辆车、目标车道内位于预变道车辆前方第一辆车和目标车道内位于预变道车辆前方第二辆车这四辆车的行驶状态数据计算出预变道车辆变道至目标车道位于预变道车辆前方第一辆车之前的意愿度,根据意愿度来预测预变道车辆在未来时刻是否会变道至目标车道位前方第一辆车之前,有效排除周围车辆干扰本车决策的因素,提高本车决策的确信度;同时,有效降低自动驾驶系统中预测模块的数据分析计算量,降低自动驾驶系统的运行难度和运行成本,有效改善决策的滞后延时。
以上所述实施例仅是为充分说明本发明而所举的较佳的实施例,本发明的保护范围不限于此。本技术领域的技术人员在本发明基础上所作的等同替代或变换,均在本发明的保护范围之内。本发明的保护范围以权利要求书为准。