CN111591307A - 一种避障轨迹规划方法、***及车辆 - Google Patents
一种避障轨迹规划方法、***及车辆 Download PDFInfo
- Publication number
- CN111591307A CN111591307A CN202010297414.0A CN202010297414A CN111591307A CN 111591307 A CN111591307 A CN 111591307A CN 202010297414 A CN202010297414 A CN 202010297414A CN 111591307 A CN111591307 A CN 111591307A
- Authority
- CN
- China
- Prior art keywords
- track
- vehicle
- preset
- candidate
- point
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Granted
Links
- 238000000034 method Methods 0.000 title claims abstract description 52
- 238000012216 screening Methods 0.000 claims description 88
- 239000013598 vector Substances 0.000 claims description 78
- 230000004888 barrier function Effects 0.000 claims description 62
- 238000011156 evaluation Methods 0.000 claims description 38
- 239000006185 dispersion Substances 0.000 claims description 34
- 230000001133 acceleration Effects 0.000 claims description 16
- 239000011159 matrix material Substances 0.000 claims description 10
- 238000004364 calculation method Methods 0.000 abstract description 7
- 239000002131 composite material Substances 0.000 description 14
- 238000010586 diagram Methods 0.000 description 11
- 230000006870 function Effects 0.000 description 10
- NAWXUBYGYWOOIX-SFHVURJKSA-N (2s)-2-[[4-[2-(2,4-diaminoquinazolin-6-yl)ethyl]benzoyl]amino]-4-methylidenepentanedioic acid Chemical compound C1=CC2=NC(N)=NC(N)=C2C=C1CCC1=CC=C(C(=O)N[C@@H](CC(=C)C(O)=O)C(O)=O)C=C1 NAWXUBYGYWOOIX-SFHVURJKSA-N 0.000 description 6
- 238000004458 analytical method Methods 0.000 description 5
- 238000005516 engineering process Methods 0.000 description 4
- 230000008569 process Effects 0.000 description 4
- 230000035945 sensitivity Effects 0.000 description 3
- 230000008859 change Effects 0.000 description 2
- 230000007423 decrease Effects 0.000 description 2
- 239000002245 particle Substances 0.000 description 2
- 239000000126 substance Substances 0.000 description 2
- 238000006467 substitution reaction Methods 0.000 description 2
- 238000013459 approach Methods 0.000 description 1
- 230000006399 behavior Effects 0.000 description 1
- 238000010276 construction Methods 0.000 description 1
- 238000009795 derivation Methods 0.000 description 1
- 238000013461 design Methods 0.000 description 1
- 238000006073 displacement reaction Methods 0.000 description 1
- 238000013178 mathematical model Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 230000008447 perception Effects 0.000 description 1
- BULVZWIRKLYCBC-UHFFFAOYSA-N phorate Chemical compound CCOP(=S)(OCC)SCSCC BULVZWIRKLYCBC-UHFFFAOYSA-N 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 230000004044 response Effects 0.000 description 1
- 230000003068 static effect Effects 0.000 description 1
- 230000002194 synthesizing effect Effects 0.000 description 1
Images
Classifications
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B60—VEHICLES IN GENERAL
- B60W—CONJOINT CONTROL OF VEHICLE SUB-UNITS OF DIFFERENT TYPE OR DIFFERENT FUNCTION; CONTROL SYSTEMS SPECIALLY ADAPTED FOR HYBRID VEHICLES; ROAD VEHICLE DRIVE CONTROL SYSTEMS FOR PURPOSES NOT RELATED TO THE CONTROL OF A PARTICULAR SUB-UNIT
- B60W60/00—Drive control systems specially adapted for autonomous road vehicles
- B60W60/001—Planning or execution of driving tasks
- B60W60/0011—Planning or execution of driving tasks involving control alternatives for a single driving scenario, e.g. planning several paths to avoid obstacles
Landscapes
- Engineering & Computer Science (AREA)
- Automation & Control Theory (AREA)
- Human Computer Interaction (AREA)
- Transportation (AREA)
- Mechanical Engineering (AREA)
- Control Of Driving Devices And Active Controlling Of Vehicle (AREA)
Abstract
本发明提供了一种避障轨迹规划方法、***及车辆,应用于车辆,其中,所述方法综合考虑了动态障碍物与车辆第一候选轨迹上预设轨迹点之间的两个距离值、以及动态障碍物与车辆之间的合成速度因素的三元素数据,输入预设规避模型计算得到的威胁值,可以准确判断车辆是否会在上述预设轨迹点处与动态障碍物发生碰撞,由此可以将发生碰撞的预设轨迹点对应的第一候选轨迹筛除,从而得到安全、合理且可以适应全方位/动态/多移动障碍物的复杂实际交通工况的第二候选轨迹。
Description
技术领域
本发明涉及汽车技术领域,特别涉及一种避障轨迹规划方法、***及车辆。
背景技术
当前,自动驾驶技术已成为研究热点。
现阶段,自动驾驶技术主要包括感知层、规划层和控制层这三个组成部分。其中,感知层用于感知车辆及车辆周边环境信息;规划层用于根据自车状况、周边环境信息以及车辆所受约束、障碍物干扰、舒适性和轨迹长度等因素,通过优化所设计的权重函数,生成一条由当前状态到期望状态的安全、舒适且可行驶的局部最优轨迹,作为参考信号输入到控制层中;而控制层则用于根据规划层所提供的局部最优轨迹,控制车辆行驶。
但是,现有自动驾驶技术的轨迹规划中,将轨迹与速度分开规划,且其他车辆等障碍物的运动状态可能随时会发生变化,从而导致所规划的轨迹变得不合理,无法有效适应不断变化的实际驾驶环境,进而影响乘坐舒适性,甚至发生行车危险。
发明内容
有鉴于此,本发明旨在提出一种避障轨迹规划方法、***及车辆,以解决现有自动驾驶技术的轨迹规划方法不够合理,造成所规划的轨迹无法有效适应不断变化的实际驾驶环境的问题。
为达到上述目的,本发明的技术方案是这样实现的:
一种避障轨迹规划方法,应用于车辆,其中,所述方法包括:
基于车辆的当前行驶状态数据构建候选轨迹集,所述当前行驶状态数据包括车辆当前质心位置坐标、车辆当前质心速度大小和车辆当前质心运行方向,所述候选轨迹集包括多个第一候选轨迹;
将所述候选轨迹集输入预设规避模型,得到预设规避模型输出的目标轨迹集;其中,所述目标轨迹集包括多个第二候选轨迹,所述预设规避模型用于根据三元素数据,计算所述各个第一候选轨迹的威胁值,并将所述候选轨迹集中威胁值大于预设值的第一候选轨迹筛除,得到目标轨迹集;
从所述目标轨迹集中选取一条第二候选轨迹作为目标轨迹;
按照所述目标轨迹控制所述车辆运行;
其中,所述三元素包括第一候选轨迹上预设轨迹点到距离最近点之间的第一直线距离、潜在碰撞点到所述距离最近点的第二直线距离、动态障碍物的质心速度与所述预设轨迹点处对应的车辆规划质心速度的速度矢量和。
进一步地,所述的方法中,所述动态障碍物为车辆通过车辆传感器当前监测到的障碍物;所述距离最近点为合成轮廓按所述速度矢量和运行时,所生成的合成轨迹上距离所述预设轨迹点最近的点;所述合成轮廓为将车辆外径叠加到所述动态障碍物上所形成的轮廓;所述潜在碰撞点为参考直线与所述合成轮廓的交点中靠近所述最近碰撞点的点,所述参考直线为经过所述最近碰撞点且与所述速度矢量和的方向平行的直线。
进一步地,所述的方法中,所述预设规避模型包括威胁值确定子模型;所述将所述候选轨迹集输入预设规避模型,得到预设规避模型输出的目标轨迹集,包括:
获取所述三元素数据;
将所述三元素数据输入所述威胁值确定子模型,得到所述威胁值确定子模型输出的所述各个第一候选轨迹的威胁值;
将所述威胁值大于预设值的第一候选轨迹从候选轨迹集中筛除,得到目标轨迹集。
进一步地,所述的方法中,所述威胁值确定子模型包括:
其中,f为威胁值,w为所述第一直线距离,l为所述第二直线距离,vc为所述速度矢量和,a、b为常数,且a<0,b>0。
其中,fm为威胁值,w为所述第一直线距离,l为所述第二直线距离,vc为所述速度矢量和,a、α、b及γ均为常数,且[vc-aα≠0m/s]且-a(γ+bα)>0。
进一步地,所述的方法中,在将所述候选轨迹集输入预设规避模型之前,预先对所述动态障碍物进行威胁性判断,并忽略无碰撞威胁的障碍物。
进一步地,所述的方法中,所述预先对所述动态障碍物威胁性判断,并忽略无碰撞威胁的障碍物,具体包括:
若针对动态障碍物的所述速度矢量和,与预设轨迹点相对于所述动态障碍物质心的位置矢量之间的夹角大于90°,则确定所述动态障碍物为无碰撞威胁的障碍物,并将所述动态障碍物忽略。
进一步地,所述的方法中,所述基于车辆的当前行驶状态数据构建候选轨迹集,包括:
将所述当前行驶状态数据输入通过预设运动学模型,得到所述预设运动学模型输出的预设时刻范围内各个时刻的轨迹点状态值;
根据所述预设时刻范围内各个时刻的轨迹点状态值,确定所述第一候选轨迹,并得到候选轨迹集。
进一步地,所述的方法中,所述预设运动学模型包括:
公式Xh(t+k)=F(Xh(t+k-1))+G·u(t+k-1)、状态矩阵控制矩阵控制变量u(t+k-1)=[ω(t+k-1)a(t+k-1)]T,其中,0≤k≤Np,在t时刻时,为车辆当前位置的轨迹点状态值,且
其中,xh、yh分别为各个时刻的轨迹点在世界坐标系中的X轴坐标及Y轴坐标,Vh(t)=[xh(t) yh(t)]T为车辆当前质心位置坐标,ω为车辆在对应时刻的质心角速度,a为车辆在对应时刻的质心线加速度,vh为车辆在对应时刻的质心线速度,θh为车辆在对应时刻的质心线速度方向与世界坐标系X轴正方向的夹角,Np为所述预设时刻范围的最大值,Δt为两个相邻时刻之间的时间差,ωL及ωU分别表示车辆质心角速度的下限值与上限值,aL及aU分别表示车辆质心线加速度的下限值与上限值,及分别表示车辆质心线速度的下限值与上限值。
进一步地,所述的方法中,所述从所述目标轨迹集中选取一条第二候选轨迹作为目标轨迹,包括:
将所述目标轨迹集中各个第二候选轨迹输入预设筛选模型,得到预设筛选模型输出的目标轨迹;其中,所述预设筛选模型用于根据第二候选轨迹上的各个轨迹点的所述轨迹点状态值及所述三元素数据,计算各个第二候选轨迹的筛选值,并将所述目标轨迹集中筛选值符合预设筛选条件的第二候选轨迹,确定为所述目标轨迹。
进一步地,所述的方法中,所述预设筛选模型包括三元素分布子模型、舒适性筛选子模型、有效行驶路径筛选子模型及参考偏离量筛选子模型中至少一种子模型,并将使所述至少一种子模型的输出值之和达到最小的第二候选轨迹,确定为所述目标轨迹;
所述三元素分布子模型,用于根据所述第二候选轨迹上的各个轨迹点的所述三元素数据,计算各个轨迹点所对应的所述第一直线距离的第一离散度、计算各个轨迹点所对应的所述第二直线距离的第二离散度、以及计算各个轨迹点所对应的所述速度矢量和的第三离散度,并将所述第一离散度按第一预设权重、所述第二离散度按第二预设权重与所述第三离散度按第三预设权重求和,确定所述第二候选轨迹的总离散度;
所述舒适性筛选子模型,用于根据所述第二候选轨迹上的任意两个相邻时刻轨迹点之间的路径矢量及车辆质心运行距离,计算各个时刻的轨迹点的舒适度,并将所述各个时刻的轨迹点的舒适度按第四预设权重求和,确定所述第二候选轨迹的总舒适度;
所述有效行驶路径筛选子模型,用于根据所述第二候选轨迹上任意两相邻时刻轨迹点之间的车辆质心运行距离及相对行驶轨迹,计算所述任意两相邻时刻轨迹点之间的有效行驶路径评价值,并将所述任意两相邻时刻轨迹点之间的有效行驶路径评价值按第五预设权重求和,确定所述第二候选轨迹的总有效行驶路径评价值;其中,所述相对行驶轨迹长度为所车辆质心运行距离投影到道路坐标系下的长度;
所述参考偏离量筛选子模型,用于根据所述第二候选轨迹上各个时刻的轨迹点状态值及预设参考轨迹,计算所述各个时刻轨迹点与预设参考轨迹之间的偏离量评价值,并将所述各个时刻轨迹点的偏离量评价值按第六预设权重求和,确定所述第二候选轨迹的总偏离量评价值。
进一步地,所述的方法中,所述预设值为0。
本发明的另一目的在于提出一种避障轨迹规划***,应用于车辆,其中,所述***包括:
轨迹集构建模块,用于基于车辆的当前行驶状态数据构建候选轨迹集,所述当前行驶状态数据包括车辆当前质心位置坐标、车辆当前质心速度大小和车辆当前质心运行方向,所述候选轨迹集包括多个第一候选轨迹;
规避模块,用于将所述候选轨迹集输入预设规避模型,得到预设规避模型输出的目标轨迹集;其中,所述目标轨迹集包括多个第二候选轨迹,所述预设规避模型用于根据三元素数据,计算所述各个第一候选轨迹的威胁值,并将所述候选轨迹集中威胁值大于预设值的第一候选轨迹筛除,得到目标轨迹集;
选取模块,用于从所述目标轨迹集中选取一条第二候选轨迹作为目标轨迹;
控制模块,用于按照所述目标轨迹控制所述车辆运行;
其中,所述三元素包括第一候选轨迹上预设轨迹点到距离最近点之间的第一直线距离、潜在碰撞点到所述距离最近点的第二直线距离、动态障碍物的质心速度与所述预设轨迹点处对应的车辆规划质心速度的速度矢量和;
所述动态障碍物为车辆通过车辆传感器当前监测到的障碍物;所述距离最近点为合成轮廓按所述速度矢量和运行时,所生成的合成轨迹上距离所述预设轨迹点最近的点;所述合成轮廓为将车辆外径叠加到所述动态障碍物上所形成的轮廓;所述潜在碰撞点为参考直线与所述合成轮廓的交点中靠近所述最近碰撞点的点,所述参考直线为经过所述最近碰撞点且与所述速度矢量和的方向平行的直线。
进一步地,所述的***中,所述预设规避模型包括威胁值确定子模型;
所述规避模块包括:
获取单元,用于获取所述三元素数据;
威胁值确定单元,用于将所述三元素数据输入所述威胁值确定子模型,得到所述威胁值确定子模型输出的所述各个第一候选轨迹的威胁值;
第一筛选单元,用于将所述威胁值大于预设值的第一候选轨迹从候选轨迹集中筛除,得到目标轨迹集。
进一步地,所述的***中,所述威胁值确定子模型包括:
其中,f为威胁值,w为所述第一直线距离,l为所述第二直线距离,vc为所述速度矢量和,a、b为常数,且a<0,b>0。
进一步地,所述的***中,所述威胁值确定子模型包括:
其中,fm为威胁值,w为所述第一直线距离,l为所述第二直线距离,vc为所述速度矢量和,a、α、b及γ均为常数,且[vc-aα≠0m/s]且-a(γ+bα)>0。
进一步地,所述***还包括:
忽略模块,用于在将所述候选轨迹集输入预设规避模型之前,预先对所述动态障碍物进行威胁性判断,并忽略无碰撞威胁的障碍物。
进一步地,所述的***中,所述忽略模块,具体用于若针对动态障碍物的所述速度矢量和,与预设轨迹点相对于所述动态障碍物质心的位置矢量之间的夹角大于90°,则确定所述动态障碍物为无碰撞威胁的障碍物,并将所述动态障碍物忽略。
进一步地,所述的***中,所述构建模块包括:
轨迹点状态值确定单元,用于将所述当前行驶状态数据输入通过预设运动学模型,得到所述预设运动学模型输出的预设时刻范围内各个时刻的轨迹点状态值;
候选轨迹集确定单元,用于根据所述预设时刻范围内各个时刻的轨迹点状态值,确定所述第一候选轨迹,并得到候选轨迹集。
进一步地,所述的***中,所述预设运动学模型包括:
公式Xh(t+k)=F(Xh(t+k-1))+G·u(t+k-1)、状态矩阵控制矩阵控制变量u(t+k-1)=[ω(t+k-1)a(t+k-1)]T,其中,0≤k≤Np,在t时刻时,为车辆当前位置的轨迹点状态值,且
其中,xh、yh分别为各个时刻的轨迹点在世界坐标系中的X轴坐标及Y轴坐标,Vh(t)=[xh(t) yh(t)]T为车辆当前质心位置坐标,θh(t)为车辆当前质心线速度方向与世界坐标系X轴正方向的夹角,vh(t)表示车辆当前质心线速度,ω为车辆在对应时刻的质心角速度,a为车辆在对应时刻的质心线加速度,vh为车辆在对应时刻的质心线速度,θh为车辆在对应时刻的质心线速度方向与世界坐标系X轴正方向的夹角,Np为所述预设时刻范围的最大值,Δt为两个相邻时刻之间的时间差,ωL及ωU分别表示车辆质心角速度的下限值与上限值,aL及aU分别表示车辆质心线加速度的下限值与上限值,及分别表示车辆质心线速度的下限值与上限值。
进一步地,所述的***中,所述选取模块包括:
第二筛选单元,用于将所述目标轨迹集中各个第二候选轨迹输入预设筛选模型,得到预设筛选模型输出的目标轨迹;其中,所述预设筛选模型用于根据第二候选轨迹上的各个轨迹点的所述轨迹点状态值及所述三元素数据,计算各个第二候选轨迹的筛选值,并将所述目标轨迹集中筛选值符合预设筛选条件的第二候选轨迹,确定为所述目标轨迹。
进一步地,所述的***中,所述预设筛选模型包括三元素分布子模型、舒适性筛选子模型、有效行驶路径筛选子模型及参考偏离量筛选子模型中至少一种子模型,并将使所述至少一种子模型的输出值之和达到最小的第二候选轨迹,确定为所述目标轨迹;
所述三元素分布子模型,用于根据所述第二候选轨迹上的各个轨迹点的所述三元素数据,计算各个轨迹点所对应的所述第一直线距离的第一离散度、计算各个轨迹点所对应的所述第二直线距离的第二离散度、以及计算各个轨迹点所对应的所述速度矢量和的第三离散度,并将所述第一离散度按第一预设权重、所述第二离散度按第二预设权重与所述第三离散度按第三预设权重求和,确定所述第二候选轨迹的总离散度;
所述舒适性筛选子模型,用于根据所述第二候选轨迹上的任意两个相邻时刻轨迹点之间的路径矢量及车辆质心运行距离,计算各个时刻的轨迹点的舒适度,并将所述各个时刻的轨迹点的舒适度按第四预设权重求和,确定所述第二候选轨迹的总舒适度;
所述有效行驶路径筛选子模型,用于根据所述第二候选轨迹上任意两相邻时刻轨迹点之间的车辆质心运行距离及相对行驶轨迹,计算所述任意两相邻时刻轨迹点之间的有效行驶路径评价值,并将所述任意两相邻时刻轨迹点之间的有效行驶路径评价值按第五预设权重求和,确定所述第二候选轨迹的总有效行驶路径评价值;其中,所述相对行驶轨迹长度为所车辆质心运行距离投影到道路坐标系下的长度;
所述参考偏离量筛选子模型,用于根据所述第二候选轨迹上各个时刻的轨迹点状态值及预设参考轨迹,计算所述各个时刻轨迹点与预设参考轨迹之间的偏离量评价值,并将所述各个时刻轨迹点的偏离量评价值按第六预设权重求和,确定所述第二候选轨迹的总偏离量评价值。
进一步地,所述的***中,所述预设值为0。
相对于在先技术,本发明所述的避障轨迹规划方法及***具有以下优势:
在基于车辆的当前行驶状态数据构建候选轨迹集后,将所述候选轨迹集输入预设规避模型,得到预设规避模型输出的目标轨迹集,然后从所述目标轨迹集中选取一条第二候选轨迹作为目标轨迹,并按照所述目标轨迹控制所述车辆运行;其中,预设规避模型根据三元素数据,计算所述各个第一候选轨迹的威胁值,并将所述候选轨迹集中威胁值大于预设值的第一候选轨迹筛除,得到目标轨迹集。因为三元素综合了动态障碍物与第一候选轨迹上预设轨迹点之间的距离、以及动态障碍物与车辆之间的速度因素,因而通过由上述三元素数据输入预设规避模型计算得到的威胁值,可以准确判断车辆是否会在上述预设轨迹点处与动态障碍物发生碰撞,也即可以将会发生碰撞的预设轨迹点对应的第一候选轨迹筛除,从而得到安全、合理且可以适应不断变化的实际驾驶环境的第二候选轨迹。
本发明的再一目的在于提出一种车辆,其中,所述车辆包括所述的避障轨迹规划***。
所述车辆与上述一种避障轨迹规划方法、***相对于现有技术所具有的优势相同,在此不再赘述。
附图说明
构成本发明的一部分的附图用来提供对本发明的进一步理解,本发明的示意性实施例及其说明用于解释本发明,并不构成对本发明的不当限定。在附图中:
图1为本发明实施例所提出的避障轨迹规划方法的流程示意图;
图2为本发明实施例中将车辆及动态障碍物均简化为质点时的三元素分析示意图;
图3为本发明实施例中考虑车辆及动态障碍物外部轮廓时的三元素分析示意图;
图5为本发明实施例中w、l、vc三者的关系曲面图;
图6为本发明实施例中w-vc二者的关系曲线图;
图7为本发明实施例中w-l二者的关系曲线图;
图8为本发明实施例中l-vc二者的关系曲线图;
图9为本发明实施例中三元素分布示意图;
图10为本发明实施例中第二候选轨迹在道路坐标系上的车道偏离量计算原理图;
图11为本发明实施例所提出的避障轨迹规划***的结构示意图。
具体实施方式
下面将参考附图更详细地描述本申请的实施例。虽然附图中显示了本申请的实施例,然而应当理解,可以以各种形式实现本申请而不应被这里阐述的实施例所限制。相反,提供这些实施例是为了能够更彻底地理解本申请,并且能够将本申请的范围完整地传达给本领域的技术人员。
需要说明的是,在不冲突的情况下,本发明中的实施例及实施例中的特征可以相互组合。
下面将参考附图并结合实施例来详细说明本发明。
请参阅图1,示出了本发明实施例所提供的一种避障轨迹规划方法的流程示意图,应用于车辆,所述方法包括步骤S100~S400:
步骤S100、基于车辆的当前行驶状态数据构建候选轨迹集,所述当前行驶状态数据包括车辆当前质心位置坐标、车辆当前质心速度大小和车辆当前质心运行方向,所述候选轨迹集包括多个第一候选轨迹;
上述步骤S100中,即以车辆的当前行驶状态数据为起始状态,规划一个原始的轨迹集,也即上述候选轨迹集。该候选轨迹集中的各个第一候选轨迹,都包括从当前时刻开始至预设时刻范围内各个时刻的轨迹点状态值。其中,因为预设时刻范围内各个时刻的轨迹点状态值都是以车辆的当前行驶状态数据为基础规划构建的,具体是以车辆的当前行驶状态数据为基础,通过给定满足输入约束条件的输入变量规划得到的,因而可以贴合车辆的实际行驶状态。在实际应用中,该预设时刻范围的时间跨度较短,可以为半秒、1秒或数秒,具体可以根据轨迹规划精度和安全性的需要进行调整。也即上述候选轨迹集是按一个规划周期进行构建,在一个规划周期结束时,则继续基于车辆此刻的当前行驶状态数据规划新的规划周期对应的候选轨迹集,从而实现按时间持续根据车辆的当前行驶状态数据构建最新的候选轨迹集。
其中,第一候选轨迹中各个时刻的轨迹点都包含轨迹点状态值,该状态值包括轨迹点坐标及车辆规划质心速度,上述车辆规划质心速度为规划好的车辆经过轨迹点时的质心速度。
步骤S200、将所述候选轨迹集输入预设规避模型,得到预设规避模型输出的目标轨迹集;其中,所述目标轨迹集包括多个第二候选轨迹,所述预设规避模型用于根据三元素数据,计算所述各个第一候选轨迹的威胁值,并将所述候选轨迹集中威胁值大于预设值的第一候选轨迹筛除,得到目标轨迹集;其中,所述三元素包括第一候选轨迹上预设轨迹点到距离最近点之间的第一直线距离、潜在碰撞点到所述距离最近点的第二直线距离、动态障碍物的质心速度与所述预设轨迹点处对应的车辆规划质心速度的速度矢量和;
所述动态障碍物为车辆通过车辆传感器当前监测到的障碍物;所述距离最近点为合成轮廓按所述速度矢量和运行时,所生成的合成轨迹上距离所述预设轨迹点最近的点,即最近碰撞点;所述合成轮廓为将车辆外径叠加到所述动态障碍物上所形成的轮廓;所述潜在碰撞点为参考直线与所述合成轮廓的交点中靠近所述最近碰撞点的点,所述参考直线为经过所述最近碰撞点且与所述速度矢量和的方向平行的直线。
上述步骤S200中,上述预设轨迹点为第一候选轨迹上的任意轨迹点;威胁值用于表示车辆按第一候选轨迹行驶时,在预设轨迹点处与动态障碍物发生碰撞的可能性;上述预设值为预先设置的判断值,在上述威胁值大于预设值时,若车辆按第一候选轨迹行驶的话,很可能会与动态障碍物在该预设轨迹点处发生碰撞。
上述步骤中,因为最近碰撞点距离预设轨迹点最近,也即合成轮廓在该处与车辆发生碰撞的可能性最大;而根据第二直线距离及速度矢量和,可以确定合成轮廓从当前位置至接触最近碰撞点所需花费的时间,因而假设车辆按上述第一候选轨迹行驶时,可以利用第一候选轨迹上预设轨迹点对应的轨迹点处的上述三元素之间的几何关系建模,计算对应的威胁值,然后判断威胁值是否大于预设值,进而预判动态障碍物与车辆是否会在预设轨迹点处发生碰撞;若计算出的威胁值大于预设值,则预判动态障碍物与车辆可能会在预设轨迹点处发生碰撞,则对应的第一候选轨迹为危险轨迹,需要将该第一候选轨迹其筛除;而若计算出的威胁值不大于预设值,则预判动态障碍物与车辆不会在预设轨迹点处发生碰撞,则对应的第一候选轨迹为安全轨迹,无需将其筛除;通过上述方式即可以得到包括安全的第一候选轨迹的目标轨迹集,上述安全的第一候选轨迹也即第二候选轨迹。
请参阅图2,图2示出了将车辆及动态障碍物均简化为质点时的三元素分析示意图。
图2中,OXY为世界坐标系,下角标h和o分别表示预设轨迹点和动态障碍物,Vh=[xhyh]T和Vo=[xo yo]T分别表示预设轨迹点和动态障碍物的质心坐标,分别为动态障碍物质心速度、车辆规划质心速度,以及将车辆规划质心速度叠加到动态障碍物质心速度上二者的速度矢量和,θo和θh分别为动态障碍物和车辆规划质心速度方向与世界坐标系X轴正方向的夹角(即航向角),β为速度矢量和与预设轨迹点相对于动态障碍物质心的位置矢量间的夹角(0≤β≤π),pt为动态障碍物沿着速度矢量和的方向行驶时,所生成的合成轨迹上距离所述预设轨迹点最近的点。其中,因为此时动态障碍物由于简化为了质心,因为潜在碰撞点是指动态障碍物上最有可能与车辆发生相撞的点,由于障碍物简化成了质心,故质心即为其潜在碰撞点。将车辆规划质心速度叠加到动态障碍物上后,车辆相对于动态障碍物而言是静止不动的,也即相当于车辆保持在预设轨迹点处;而动态障碍物在沿着合成速度方向运动时,pt点所处位置也即是潜在碰撞点与车辆质心的直线距离最短的位置;w为pt到预设轨迹点的直线距离,l为动态障碍物质心到达pt的直线距离。图2中的w、l、即依次为上述三元素中的第一直线距离、第二直线距离和速度矢量和。
请参阅图3,图3示出了考虑车辆及动态障碍物外部轮廓时的三元素分析示意图。
图3中rh为车辆的外轮廓半径,o_i、o_j表示第i个动态障碍物和第j个动态障碍物,ro_i及ro_j依次为上述两个动态障碍物的外轮廓半径,也即下角标i和j分别表示针对第i个动态障碍物和第j个动态障碍物,i,j=1,…,N,N为动态障碍物个数;将车辆外轮廓半径叠加到动态障碍物上,则车辆简化为一个质点,5而动态障碍物外形扩大半径为rc_i(j)=ro_i(j)+rh的合成轮廓;τi_2和τj_3分别为质心在Vo_i和Vo_j处的动态障碍物的潜在碰撞点,对应的合成轮廓分别为A和B。其中,Vo_i坐标为(xo_i,yo_i),Vo_j坐标为(xo_j,yo_j)。其中,潜在碰撞点为扩展半径为rc_i(j)的合成轮廓,沿着速度矢量和的方向运动时,距离预设轨迹点Vh最近的点;为第i个障碍物与车辆的规划速度矢量和 方向与世界坐标系X轴正方向的夹角
此时与速度矢量和方向平行且经过预设轨迹点Vh的直线与合成轮廓的交点也有两个,而潜在碰撞点τj_3同样为距离最近碰撞点较近的点,其坐标为根据几何关系可知,此时的最近碰撞点就是自车质心点,也即是预设轨迹点Vh。其中,相应的第一直线距离wj=0m,而相应的第二直线距离lj为:
在实际应用中,因为动态障碍物为车辆通过车辆传感器当前监测到的障碍物,该动态障碍物可能是一个也可能是多个,因而上述步骤是针对车辆传感器当前监测到的所有障碍物进行分析,即假设车辆按上述第一候选轨迹行驶时,针对各个动态障碍物,依次利用第一候选轨迹上预设轨迹点处的上述三元素计算得到的各个威胁值,并确定是否存在大于预设值的情况。若各个威胁值中存在大于预设值的情况,则说明对应动态障碍物可能会与车辆在预设轨迹点处发生碰撞,则预判该第一候选轨迹为危险轨迹,需要将其筛除;若各个威胁值中不存在大于预设值的情况,则说明对应动态障碍物不会与车辆在预设轨迹点处发生碰撞,则预判该第一候选轨迹为安全轨迹。
步骤S300、从所述目标轨迹集中选取一条第二候选轨迹作为目标轨迹。
上述步骤S300中,因为目标轨迹集中的第二候选轨迹具有多条,而且每条都是安全的,车辆按第二候选轨迹行驶时不会与所检测到的任意动态障碍物发生碰撞,因而可以从所述目标轨迹集中选取一条第二候选轨迹作为目标轨迹。
在实际应用中,是通过构建不同评价指标组成的权重函数,选取使得权重函数取值最小的第二候选轨迹作为目标轨迹。
步骤S400、按照所述目标轨迹控制所述车辆运行。
上述步骤S400中,因为车辆按上述目标轨迹行驶时不会与车辆传感器当前检测到的障碍物发生碰撞,因而按照上述目标轨迹控制所述车辆运行,即可以实现避障行车的目的。在实际应用中,根据规划周期的不同,上述目标轨迹会在一个规划周期结束后进行实时更新。
相对于现有技术,本发明所述的避障轨迹规划方法具有以下优势:
在基于车辆的当前行驶状态数据构建候选轨迹集后,将所述候选轨迹集输入预设规避模型,得到预设规避模型输出的目标轨迹集,然后从所述目标轨迹集中选取一条第二候选轨迹作为目标轨迹,并按照所述目标轨迹控制所述车辆运动一段时间,直到新的目标轨迹产生;其中,预设规避模型根据三元素数据,计算所述各个第一候选轨迹的威胁值,并将所述候选轨迹集中威胁值大于预设值的第一候选轨迹筛除,得到目标轨迹集。因为三元素综合了动态障碍物与第一候选轨迹上预设轨迹点之间的距离、以及动态障碍物与车辆之间的速度因素,因而通过由上述三元素数据输入预设规避模型计算得到的威胁值,可以准确判断车辆是否可能会在上述预设轨迹点处与动态障碍物发生碰撞,也即可以将可能会发生碰撞的预设轨迹点对应的第一候选轨迹筛除,从而得到安全、合理且可以适应不断变化的实际驾驶环境的第二候选轨迹。
可选地,在一种实施方式中,上述步骤S100包括步骤S101~S102:
步骤S101、将所述当前行驶状态数据输入通过预设运动学模型,得到所述预设运动学模型输出的预设时刻范围内各个时刻的轨迹点状态值。
上述步骤S101中,即根据车辆的当前行驶状态并利用预设运动学模型,规划预设时刻范围内各个时刻的轨迹点状态值。其中,上述轨迹点状态值包括轨迹点坐标及车辆的对应行驶状态,该对应行驶状态包括车辆的速度矢量,即车辆规划质心速度,这样也就确定了从当前时刻开始至预设时刻范围结束时,车辆在各个时刻所处的位置以及以何种速度状态抵达上述位置。
在实际应用中,假设车辆的质心速度和航向角在每个预设周期内是线性变化的,则上述预设运动学模型可以包括:
公式Xh(t+k)=F(Xh(t+k-1))+G·u(t+k-1)、状态矩阵控制矩阵控制变量u(t+k-1)=[ω(t+k-1)a(t+k-1)]T,其中,0≤k≤Np,在t时刻时,为车辆当前位置的轨迹点状态值,且
其中,xh、yh分别为各个时刻的轨迹点在世界坐标系中的X轴坐标及Y轴坐标,Vh(t)=[xh(t) yh(t)]T为车辆当前质心位置坐标,ω为车辆在对应时刻的质心角速度,a为车辆在对应时刻的质心线加速度,vh为车辆在对应时刻的质心线速度,θh为车辆在对应时刻的质心线速度方向与世界坐标系X轴正方向的夹角,Np为所述预设时刻范围的最大值,Δt为两个相邻时刻之间的时间差,ωL及ωU分别表示车辆质心角速度的下限值与上限值,aL及aU分别表示车辆质心线加速度的下限值与上限值,及分别表示车辆质心线速度的下限值与上限值。
具体地,xh(t+k-1)、yh(t+k-1)分别为在t+k-1时刻时轨迹点在世界坐标系中的X轴坐标及Y轴坐标,Vh(t)=[xh(t) yh(t)]T为车辆当前质心位置坐标,θh(t)为车辆当前质心线速度方向与世界坐标系X轴正方向的夹角,vh(t)表示车辆当前质心线速度,ω(t+k-1)为车辆在t+k-1时刻时的质心角速度,a(t+k-1)为车辆在t+k-1时刻时的质心线加速度,vh(t+k-1)为车辆在t+k-1时刻时的质心线速度,θh(t+k-1)为在t+k-1时刻时的质心线速度方向与世界坐标系X轴正方向的夹角,Np为所述预设时刻范围的最大值,Δt为从t+k-1时刻至t+k时刻之间的时间差,ωL及ωU分别表示车辆质心角速度的下限值与上限值,aL及aU分别表示车辆质心线加速度的下限表示车辆质心线加速度的下限值与上限值,及分别表示车辆质心线速度的下限值与上限值。
可以看出,轨迹点状态值函数Xh(t+k)随着k变化,也即可以得到从当前时刻t开始至t+Np这Np个时刻内各个时刻的轨迹点状态值。其中,因为控制变量u(t+k-1)的存在,使得预设时刻范围内各个时刻的轨迹点状态值可以有多个,从而使得各个时刻对应的轨迹点也具有多个。
通过上述预设运动学模型即可以基于车辆的当前行驶状态及加速性能和转向能力,确定预设时刻范围内各个时刻的轨迹点状态值,也即车辆在各个时刻所处的位置以及以何种速度状态抵达上述位置。因为上述各个轨迹点状态值都是基于车辆的当前行驶状态及加速性能和转向能力构建的,因而若无其他障碍物的限制,车辆均可以按上述轨迹点状态值进行行车。
步骤S102、根据所述预设时刻范围内各个时刻的轨迹点状态值,确定所述第一候选轨迹,并得到候选轨迹集。
因为上述步骤S101中,规划好了从预设时刻范围内各个时刻的轨迹点状态值,也即好了轨迹坐标与时间的对应关系,将上述各个轨迹点按时刻先后顺序排序,即可以确定一条第一候选轨迹。其中,因为控制变量u(t)的存在,使得预设时刻范围内各个时刻的轨迹点状态值可以有多个,从而使得各个时刻对应的轨迹点也具有多个,从而得到包括多条第一候选轨迹的候选轨迹集。
可选地,在一种实施方式中,上述步骤S200中,所述预设规避模型包括威胁值确定子模型;所述将所述候选轨迹集输入预设规避模型,得到预设规避模型输出的目标轨迹集,包括步骤S201~S203:
步骤S201、获取所述三元素数据。
上述步骤S201中,即选取一条规划好的第一候选轨迹,针对该第一候选轨迹上的各个轨迹点,记为预设轨迹点,依次针对各个动态障碍物,计算上述预设轨迹点到最近碰撞点之间的第一直线距离w、潜在碰撞点到上述最近碰撞点之间的第二直线距离l、动态障碍物在相应时刻的质心速度与各个上述预设轨迹点处对应的车辆规划质心速度的速度矢量和vc;上述动态障碍物为车辆通过车辆传感器当前监测到的障碍物;
步骤S202、将所述三元素数据输入所述威胁值确定子模型,得到所述威胁值确定子模型输出的所述各个第一候选轨迹的威胁值。
上述威胁值确定子模型可以基于三元素数据之间的几何关系,计算用于判断发生碰撞概率大小的威胁值。第一候选轨迹的威胁值包括第一候选轨迹内各个轨迹点的威胁值。因而针对各个预设轨迹点,依次将针对各个动态障碍物的上述三元素输入上述威胁值确定子模型,即可以得到每一个预设轨迹点针对各个动态障碍物的威胁值,从而得到第一候选轨迹内各个轨迹点针对各个动态障碍物的威胁值,也即得到第一候选轨迹的威胁值。
可选地,在第一种具体实施方式中,上述威胁值确定子模型包括:
其中,f为威胁值,w为所述第一直线距离,l为所述第二直线距离,vc为所述速度矢量和,a、b为常数,且a<0,b>0。
在避让障碍物时,距离障碍物越远且速度越慢则越安全。在上述公式(8)中,利用可以计算出合成轮廓以速度矢量和从当前位置运行至最近碰撞点所需的时间,而则表示了合成轮廓以当前速度矢量和到达最近碰撞点时,动态障碍物不会对车辆形成碰撞威胁的第一安全距离;而w则表示了合成轮廓以当前速度矢量和到达最近碰撞点时,合成轮廓与预设轨迹点的实际距离,也即时动态障碍物与车辆的实际距离;通过的值与w的大小,则可以判断合成轮廓与预设轨迹点是否可能会在合成轮廓抵达最近碰撞点时发生碰撞,也即可以判断动态障碍物与车辆是否可能会发生碰撞。其中,可以理解的是,当的值与w相等时,则动态障碍物对车辆达到威胁临界状态,此时,即 此时的w即等于第一安全距离,可以得到作为第一安全距离函数式的式(9):
在式(9)中,若趋于0s,则此时w≈b,若b=0,则动态障碍物趋近于与车辆发生碰撞,因而为了使确定能够表示合成轮廓以当前速度矢量和到达最近碰撞点时,动态障碍物不会对车辆形成碰撞威胁,则需要保证b>0;另外,随着合成轮廓按速度矢量和移动,不断变小,意味着合成轮廓不断靠近最近碰撞点,其对车辆产生的碰撞威胁度不断增加,而为了保证驾乘人员的安全,需要增大相应的安全距离,即设置w值为l/vc的减函数,因而需要设置a<0。
由图4可知,对于三元素中的第一直线距离w的值、以及第二直线距离与速度矢量和的比值如果w与l/vc对应的点位于图4中的阴影部分区域,即f≤0m,则车辆相对安全;而若w与对应的点位于图4中的空白部分区域,即f>0,则障碍物对车辆有安全隐患,甚至可能发生碰撞。
在上述第一种具体实施方式,针对第一候选轨迹上的各个轨迹点,依次针对不同动态障碍物的三元素的数据代入式(8)中即可以计算得到对应的威胁值f,若存在一个威胁值f>0,即可以确定该第一候选轨迹为危险轨迹。
在第一种具体实施方式中,针对式(9),即w关于的函数,在l趋于0m时,w≈b且w对vc的变化不敏感。然而在实际应用中,在合成轮廓经过最近碰撞点附近的速度越快,对于相对静止在预设轨迹点处的车辆的威胁度就越高,而为了保证驾乘人员的安全,设置的第一安全距离也应越大,也即处于威胁临界状态时的w应越大。因而,第一种具体实施方式中利用式(8)计算威胁值的方式,存在第一安全距离固定,无法根据实际速度进行调整的问题,存在一定的安全隐患。
可选地,在第二种具体实施方式中,上述威胁值确定子模型包括:
其中,fm为威胁值,w为所述第一直线距离,l为所述第二直线距离,vc为所述速度矢量和,a、α、b及γ均为常数,且[vc-aα≠0m/s]且-a(γ+bα)>0。
针对第一候选轨迹上的各个轨迹点,依次针对不同动态障碍物的三元素的数据代入式(10)中即可以计算得到对应的威胁值fm,若存在一个威胁值fm>0,即可以确定该第一候选轨迹为危险轨迹。
对于式(10),同样具有一个第二安全距离,该第二安全距离为合成轮廓以当前速度矢量和到达最近碰撞点时,动态障碍物不会对车辆形成碰撞威胁的极限距离。令fm=0且l=0,此时动态障碍物达到对车辆的威胁临界状态,此时的w即等于第二安全距离,则可以得到作为第二安全距离函数式的式(11):
因为[vc-aα≠0m/s]且-a(γ+bα)>0,因而式(12)的值能够保持大于0的状态,也即在威胁临界状态下,第二安全距离随着vc的增大而增大,即第二安全距离函数为速度矢量和的单调递增函数。因而第二种具体实施方式可以克服第一种具体实施方式中存在第一安全距离固定,无法根据实际速度进行调整的问题。
相对于式(8),式(10)增加了αw+γ项。令fm=0,则可以确定w、l、vc三者的关系曲面,进而可以根据三元素所对应的点与该关系曲面之间的关系,确定对应的合成轮廓是否可能会与车辆发生碰撞,也即确实预设轨迹点是否安全。
例如,令α=10、γ=0m、a=-0.5m/s、b=4m,则根据式(10)确定的w、l、vc三者的关系曲面图w-l-vc如图5所示。如果三元素所对应的点位于该关系曲面内,则合成轮廓与位于预设轨迹点处的车辆之间处于威胁临界状态;如果三元素所对应的点位于该关系曲面上方,则合成轮廓不会与位于预设轨迹点处的车辆发生碰撞,即预设轨迹点为安全轨迹点;而如果三元素所对应的点位于该关系曲面下方,则合成轮廓会与对车辆构成威胁,甚至发生碰撞,即预设轨迹点是危险轨迹点。以w=1m、l=20m、vc=5m/s为例,代入式(10)中,得到fm=0,此时w=1m、l=20m、vc=5m/s所对应的点刚好位于关系曲面上,即可以确定当前所分析的预设轨迹点处于碰撞临界状态。
例如,令α=10、γ=0m、a=-0.5m/s、b=4m且l=0m,此时,即合成轮廓抵达了最近碰撞点,则可以根据式(10)确定的w-vc二者的关系曲线w-vc如图6中阴影交叉部分的曲线所示。通过图6可以看出,在l趋于0m时,保证行车安全所需的w随着vc的增大而增大,也即解决了第一种具体实施方式中,w对vc的变化不敏感的问题,从而保证了驾乘人员的安全,营造良好的驾乘体验;
例如,令α=10、γ=0m、a=-0.5m/s、b=4m且vc=5m/s,此时,则可以根据式(10)确定的w-l二者的关系曲线w-l如图7中阴影交叉部分的曲线所示。通过图7可以看出,在vc不变时,保证车辆处于安全状态所需的w随着l的增大而减小,这与实际驾驶原理也是一致的;
例如,令α=10、γ=0m、a=-0.5m/s、b=4m且w=1,此时,则可以根据式(10)确定的l-vc二者的关系曲线l-vc如图8中阴影交叉部分的曲线所示。通过图8可以看出,在w不变时,vc随着l的减小而减小,也即随着合成轮廓不断靠近最近碰撞点,控制vc不断减小,从而保证了驾乘人员良好驾乘体验,也使得车辆处于安全状态。
通过上述分析可以看出,利用式(10)所建立的威胁值确定子模型,不仅可以确定出对应预设轨迹点是否安全,还可以保证筛选出的安全轨迹点符合实际驾乘习惯,从而为驾乘人员提供良好的驾乘体验。
显然,上述两种具体实施方式所提供的威胁值确定子模型,均只是为了验证避障方法的合理性而建立的简化数学模型。在实际应用中,根据实际驾乘要求不同,可以通过调整f或fm的结构及其常系数,来拟合不同的安全范围,以建立出更贴近驾乘人员不同响应行为的期望模型。
步骤S203、将所述威胁值大于预设值的第一候选轨迹从候选轨迹集中筛除,得到目标轨迹集。
上述步骤S203中,即依次将各个第一候选轨迹内各个轨迹点的威胁值与预设值进行比较,若存在一个或多个威胁值大于上述预设值,则判断对应的第一候选轨迹为危险轨迹;而第一候选轨迹内各个轨迹点的威胁值均小于或等于预设值进行比较,则判断对应的第一候选轨迹为安全轨迹;由安全轨迹组成的轨迹集即上述目标轨迹。
其中,通过上述步骤S202中的两种具体实施方式可知,威胁值等于0时,对应的预设轨迹点为临界碰撞状态的轨迹点;而威胁值小于0时,对应的预设轨迹点为安全轨迹点。因而,为了保证将具有危险轨迹点的第一候选轨迹均筛除,需要设置预设值小于或等于0。当预设值为0时,刚好可以将全部具有危险轨迹点的第一候选轨迹均筛除;而当预设值小于0时,不仅可以将全部具有危险轨迹点的第一候选轨迹均筛除,还会将部分不具有危险轨迹点的第一候选轨迹也筛除掉。因而,可选地,上述预设值为0。
可选地,在一种实施方式中,上述步骤S200中,在将所述候选轨迹集输入预设规避模型之前,预先对所述动态障碍物进行威胁性判断,并忽略无碰撞威胁的障碍物。即先将完全不可能与车辆发生碰撞的动态障碍物筛选,从而减少后续计算量。
具体地,上述预先对所述动态障碍物威胁性判断,并忽略无碰撞威胁的障碍物,具体包括步骤S2001:
步骤S2001、若针对动态障碍物的所述速度矢量和,与预设轨迹点相对于所述动态障碍物质心的位置矢量之间的夹角大于90°,则确定所述动态障碍物为无碰撞威胁的障碍物,并将所述动态障碍物忽略。
上述步骤S2001中,若针对动态障碍物的所述速度矢量和,与预设轨迹点相对于所述动态障碍物质心的位置矢量之间的夹角大于90°,则说明该动态障碍物趋于远离预设轨迹点,也不会与车辆发生碰撞,因而可以确定该动态障碍物为无碰撞威胁的障碍物,也即可以将其忽略。
可选地,在一种实施方式中,上述步骤S300具体包括:
将所述目标轨迹集中各个第二候选轨迹输入预设筛选模型,得到预设筛选模型输出的目标轨迹;其中,所述预设筛选模型用于根据第二候选轨迹上的各个轨迹点的所述轨迹点状态值及所述三元素数据,计算各个第二候选轨迹的筛选值,并将所述目标轨迹集中筛选值符合预设筛选条件的第二候选轨迹,确定为所述目标轨迹。
在上述实施方式中,预设筛选条件为预设设置的、期望目标轨迹所能够满足的条件;而无法满足该预设筛选条件的第二候选轨迹则不会被选作目标轨迹。也即本实施方式,利用预设筛选模型对目标轨迹集中的第二候选轨迹进行筛选,筛选出符合预设筛选条件的第二候选轨迹作为目标轨迹。
可选地,上述步骤S300中,所述预设筛选模型包括三元素分布子模型、舒适性筛选子模型、有效行驶路径筛选子模型及参考偏离量筛选子模型中至少一种子模型,并将使所述至少一种子模型的输出值之和达到最小的第二候选轨迹,确定为所述目标轨迹。
其中,所述三元素分布子模型,用于根据所述第二候选轨迹上的各个轨迹点的所述三元素数据,计算各个轨迹点所对应的所述第一直线距离的第一离散度、计算各个轨迹点所对应的所述第二直线距离的第二离散度、以及计算各个轨迹点所对应的所述速度矢量和的第三离散度,并将所述第一离散度按第一预设权重、所述第二离散度按第二预设权重与所述第三离散度按第三预设权重求和,确定所述第二候选轨迹的总离散度。
通过上述三元素分布子模型,可以根据不同的驾乘人员对三个元素的敏感程度不同进行权重调节,进而筛选出符合不同的驾乘人员的驾乘习惯的目标轨迹。
具体地,所述三元素分布子模型包括:
公式:
式(13)中,k1、k2、k3分别为对应于w、l、vc的权重值,即依次为第一预设权重、第二预设权重与第三预设权重,而三元素相对于NA个障碍物的均值定义为和k指的是第二候选轨迹内t+k时刻所对应的轨迹点。
通过式(13)可以计算车辆传感器所监测到的障碍物针对第二候选轨迹内的全部轨迹点的三元素分布偏差值。其中,式(13)中,可以通过对三元素的权重值进行设置,可以刻画满足不同的驾乘人员对三个元素的敏感程度要求,即权重值越大,意味着车辆可以接受动态障碍物聚集在其对应元素的周边越紧密;而若权重值越小,意味着车辆可以接受动态障碍物聚集在其对应元素的周边越稀疏。
请参阅图9,图9为本实施方式中三元素分布示意图。如图9所示,以l-vc的投影面为例可知,l和vc两个元素中,权重函数设计对l的敏感度更高,因此“*”被压缩分布在均值“°”的l方向周围。
其中,所述舒适性筛选子模型,用于根据所述第二候选轨迹上的任意两个相邻时刻轨迹点之间的路径矢量及车辆质心运行距离,计算各个时刻的轨迹点的舒适度,并将所述各个时刻的轨迹点的舒适度按第四预设权重求和,确定所述第二候选轨迹的总舒适度。
具体地,上述舒适性筛选子模型,包括:
其中,为t+k时刻所对应的轨迹点相对世界坐标系原点的位置矢量,为t+k-1时刻所对应的轨迹点相对世界坐标系原点的位置矢量,vh(t+k)为t+k时刻所对应的轨迹点的车辆规划质心速度,Δt为相邻两时刻的轨迹点之间的时间间隔,t为当前时间,k4即第四预设权重,k表示第一候选轨迹内t+k时刻所对应的轨迹点。
式(16)及式(15)中,通过可以确定任意两个相邻时刻轨迹点之间的路径矢量,而vh(t+k)·Δt可计算得到相邻时刻轨迹点之间的车辆质心运行距离。因而利用式(16)可以计算得到第二候选轨迹上的任意两个相邻时刻轨迹点之间的路径矢量相对于行驶距离的单位量变化量,而利用式(15)可以近似轨迹曲率,使得通过上述式(14)即可以计算得到第二候选轨迹的总舒适度,而且式(14)综合考虑了轨迹点轨迹向量以及车辆规划质心速度的影响,因此通过设置该权重值k4,可以很好的平滑自车航向角和车速的变化,获得良好的行驶舒适性和平顺性。
其中,所述有效行驶路径筛选子模型,用于根据所述第二候选轨迹上任意两相邻时刻轨迹点之间的车辆质心运行距离及相对行驶轨迹,计算所述任意两相邻时刻轨迹点之间的有效行驶路径评价值,并将所述任意两相邻时刻轨迹点之间的有效行驶路径评价值按第五预设权重求和,确定所述第二候选轨迹的总有效行驶路径评价值;其中,所述相对行驶轨迹长度为所车辆质心运行距离投影到道路坐标系下的长度。
具体地,上述有效行驶路径筛选子模型,包括:
其中,vh(t+k)为t+k时刻所对应的轨迹点的车辆规划质心速度,Δt为相邻两时刻的轨迹点之间的时间间隔,t为当前时间,k5即第五预设权重,k表示第二候选轨迹内t+k时刻所对应的轨迹点,LL(t+k)为第二候选轨迹上t+k时刻对应的轨迹点与t+k-1时刻对应的轨迹点之间的车辆质心运行距离投影到道路坐标下的长度。
式(17)中,道路坐标系即基准坐标系,该坐标系由感知车道线的摄像头决定,vh(t+k)·Δt可以计算第二候选轨迹上t+k时刻与t+k-1时刻之间的车辆质心规划运行距离。利用可以计算第二候选轨迹上t+k时刻对应的轨迹点与t+k-1时刻对应的轨迹点之间的有效行驶路径评价值,而利用式(17)可以计算得到用于评估第二候选轨迹的轨迹有效率的总有效行驶路径评价值。该总有效行驶路径评价值越小,则说明车辆相对道路运行的效率越高,即利用较少的运行轨迹长度可以达到同样的位移。
请参阅图10,示出了世界坐标系下第二候选轨迹在道路坐标系上的车道偏离量计算原理图。图10中,Vh、Vh(t+k+1)及Vh(t+k+2)分别表示第二候选轨迹当前时刻、t+k+1时刻及t+k+2时刻的轨迹点,LL(t+k)及LL(t+k+1)则分别为t+k及t+k+1时刻的轨迹点对应的相对行驶轨迹长度。其中,因为在道路坐标系OLXLYL中,以预设参考轨迹如道路中间的前进方向为X轴方向,以与道路面平行且垂直预设参考轨迹的方向为Y轴方式,因而LL(t+k)为第二候选轨迹上t+k时刻对应的轨迹点与t+k-1时刻对应的轨迹点之间的车辆质心运行距离投影到道路坐标下OL-XL轴上的相对行驶轨迹。
所述参考偏离量筛选子模型,用于根据所述第二候选轨迹上各个时刻的轨迹点状态值及预设参考轨迹,计算所述各个时刻轨迹点与预设参考轨迹之间的偏离量评价值,并将所述各个时刻轨迹点的偏离量评价值按第六预设权重求和,确定所述第二候选轨迹的总偏离量评价值。
具体地,上述参考偏离量筛选子模型,包括:
其中,t为当前时间,k6即第六预设权重,k表示第二候选轨迹内t+k时刻所对应的轨迹点,SL(t+k)为第二候选轨迹上t+k时刻对应的轨迹点在道路坐标系与预设参考轨迹的相对偏移量。
利用(SL(t+k))2可以计算第二候选轨迹上t+k时刻对应的轨迹点在道路坐标系上与预设参考轨迹的偏离量评价值,而利用式(18)即可以计算第二候选轨迹上各个轨迹点与预设参考轨迹之间的总偏离量评价值。
OL-YL轴上与坐标原点的偏离量评价值,而利用式(18)即可以计算第二候选轨迹上各个轨迹点与预设参考轨迹之间的总偏离量评价值。
继续参阅图10,其中,SL(t+k)、SL(t+k+1)及SL(t+k+2)则分别为第二候选轨迹上当前时刻t+k、t+k+1时刻及t+k+2时刻的轨迹点对应的相对偏移量。在实际应用中,因为在道路坐标系OLXLYL中,以预设参考轨迹如道路中间的前进方向为X轴方向,以与道路面平行且垂直预设参考轨迹的方向为Y轴方式,因而SL(t+k)为第二候选轨迹上t+k时刻对应的轨迹点在道路坐标系下OL-YL轴上与坐标原点的相对偏移量。
另外,在道路坐标系中,坐标原点OL为车辆质心点Vh投影到车道中间上的点,而相对偏移量SL为Vh点到车道中间的最短距离。该OL点的求取,可采用两步的数值计算方法,即采用二次最小化方法先得到粗略的最近点位置,然后采用牛顿法获得更为准确的位置。
优选地,上述步骤S300中,所述预设筛选模型包括:
其中,各参数定义与前文一致。利用式(19)即可以将每一种控制变量u(t+k-1)所确定的第二候选轨迹的各个轨迹点的状态值、结合车辆传感器所检测到的障碍物输入式(19)进行计算,将使式(19)的输入值达到最小的控制变量u(t+k-1)所确定第二候选轨迹作为目标轨迹,该目标轨迹可以同时满足驾乘人员对三元素敏感程度、行车舒适度、有效轨迹长度及参考偏离量的综合要求。
另外,在本实施例中,对于未来位置不可预测动态障碍物,默认其下一时刻所处位置仍为当前时刻所处位置。
本发明的另一目的在于提出一种避障轨迹规划***,应用于车辆,其中,请参阅图11,图11示出了本发明实施例所提出的一种避障轨迹规划***的结构示意图,所述***包括:
轨迹集构建模块111,用于基于车辆的当前行驶状态数据构建候选轨迹集,所述当前行驶状态数据包括车辆当前质心位置坐标、车辆当前质心速度大小和车辆当前质心运行方向,所述候选轨迹集包括多个第一候选轨迹;
规避模块112,用于将所述候选轨迹集输入预设规避模型,得到预设规避模型输出的目标轨迹集;其中,所述目标轨迹集包括多个第二候选轨迹,所述预设规避模型用于根据三元素数据,计算所述各个第一候选轨迹的威胁值,并将所述候选轨迹集中威胁值大于预设值的第一候选轨迹筛除,得到目标轨迹集;
选取模块113,用于从所述目标轨迹集中选取一条第二候选轨迹作为目标轨迹;
控制模块114,用于按照所述目标轨迹控制所述车辆运行;
其中,所述三元素包括第一候选轨迹上预设轨迹点到距离最近点之间的第一直线距离、潜在碰撞点到所述距离最近点的第二直线距离、动态障碍物的质心速度与所述预设轨迹点处对应的车辆规划质心速度的速度矢量和;
所述动态障碍物为车辆通过车辆传感器当前监测到的障碍物;所述距离最近点为合成轮廓按所述速度矢量和运行时,所生成的合成轨迹上距离所述预设轨迹点最近的点;所述合成轮廓为将车辆外径叠加到所述动态障碍物上所形成的轮廓;所述潜在碰撞点为参考直线与所述合成轮廓的交点中靠近所述最近碰撞点的点,所述参考直线为经过所述最近碰撞点且与所述速度矢量和的方向平行的直线。
本发明实施例所述的***中,先由轨迹集构建模块111基于车辆的当前行驶状态数据构建候选轨迹集,再由规避模块112将所述候选轨迹集输入预设规避模型,得到预设规避模型输出的目标轨迹集,然后选取模块113从所述目标轨迹集中选取一条第二候选轨迹作为目标轨迹,并由控制模块114按照所述目标轨迹控制所述车辆运行;其中,预设规避模型根据三元素数据,计算所述各个第一候选轨迹的威胁值,并将所述候选轨迹集中威胁值大于预设值的第一候选轨迹筛除,得到目标轨迹集。因为三元素综合了动态障碍物与第一候选轨迹上预设轨迹点之间的距离、以及动态障碍物与车辆之间的速度因素,因而通过由上述三元素数据输入预设规避模型计算得到的威胁值,可以准确判断车辆是否可能会在上述预设轨迹点处与动态障碍物发生碰撞,也即可以将会发生碰撞的预设轨迹点对应的第一候选轨迹筛除,从而得到安全、合理且可以适应不断变化的实际驾驶环境的第二候选轨迹。
可选地,所述的***中,所述预设规避模型包括威胁值确定子模型;
所述规避模块包括:
获取单元,用于获取所述三元素数据;
威胁值确定单元,用于将所述三元素数据输入所述威胁值确定子模型,得到所述威胁值确定子模型输出的所述各个第一候选轨迹的威胁值;
第一筛选单元,用于将所述威胁值大于预设值的第一候选轨迹从候选轨迹集中筛除,得到目标轨迹集。
进一步地,所述的***中,所述威胁值确定子模型包括:
其中,f为威胁值,w为所述第一直线距离,l为所述第二直线距离,vc为所
述速度矢量和,a、b为常数,且a<0,b>0。
可选地,所述的***中,所述威胁值确定子模型包括:
其中,fm为威胁值,w为所述第一直线距离,l为所述第二直线距离,vc为所述速度矢量和,a、α、b及γ均为常数,且[vc-aα≠0m/s]且-a(γ+bα)>0。
可选地,所述***还包括:
忽略模块,用于在将所述候选轨迹集输入预设规避模型之前,预先对所述动态障碍物进行威胁性判断,并忽略无碰撞威胁的障碍物。
可选地,所述的***中,所述忽略模块,具体用于若针对动态障碍物的所述速度矢量和,与预设轨迹点相对于所述动态障碍物质心的位置矢量之间的夹角大于90°,则确定所述动态障碍物为无碰撞威胁的障碍物,并将所述动态障碍物忽略。
可选地,所述的***中,所述构建模块包括:
轨迹点状态值确定单元,用于将所述当前行驶状态数据输入通过预设运动学模型,得到所述预设运动学模型输出的预设时刻范围内各个时刻的轨迹点状态值;
候选轨迹集确定单元,用于根据所述预设时刻范围内各个时刻的轨迹点状态值,确定所述第一候选轨迹,并得到候选轨迹集。
可选地,所述的***中,所述预设运动学模型包括:
公式Xh(t+k)=F(Xh(t+k-1))+G·u(t+k-1)、状态矩阵控制矩阵控制变量u(t+k-1)=[ω(t+k-1)a(t+k-1)]T,其中,0≤k≤Np,在t时刻时,为车辆当前位置的轨迹点状态值,且
其中,xh、yh分别为各个时刻的轨迹点在世界坐标系中的X轴坐标及Y轴坐标,Vh(t)=[xh(t) yh(t)]T为车辆当前质心位置坐标,ω为车辆在对应时刻的质心角速度,a为车辆在对应时刻的质心线加速度,vh为车辆在对应时刻的质心线速度,θh为车辆在对应时刻的质心线速度方向与世界坐标系X轴正方向的夹角,Np为所述预设时刻范围的最大值,Δt为两个相邻时刻之间的时间差,ωL及ωU分别表示车辆质心角速度的下限值与上限值,aL及aU分别表示车辆质心线加速度的下限值与上限值,及分别表示车辆质心线速度的下限值与上限值。
具体地,xh(t+k-1)、yh(t+k-1)分别为在t+k-1时刻时轨迹点在世界坐标系中的X轴坐标及Y轴坐标,Vh(t)=[xh(t) yh(t)]T为车辆当前质心位置坐标,θh(t)为车辆当前质心线速度方向与世界坐标系X轴正方向的夹角,vh(t)表示车辆当前质心线速度,ω(t+k-1)为车辆在t+k-1时刻时的质心角速度,a(t+k-1)为车辆在t+k-1时刻时的质心线加速度,vh(t+k-1)为车辆在t+k-1时刻时的质心线速度,θh(t+k-1)为在t+k-1时刻时的质心线速度方向与世界坐标系X轴正方向的夹角,Np为所述预设时刻范围的最大值,Δt为从t+k-1时刻至t+k时刻之间的时间差,ωL及ωU分别表示车辆质心角速度的下限值与上限值,aL及aU分别表示车辆质心线加速度的下限表示车辆质心线加速度的下限值与上限值,及分别表示车辆质心线速度的下限值与上限值。
可选地,所述的***中,所述选取模块包括:
第二筛选单元,用于将所述目标轨迹集中各个第二候选轨迹输入预设筛选模型,得到预设筛选模型输出的目标轨迹;其中,所述预设筛选模型用于根据第二候选轨迹上的各个轨迹点的所述轨迹点状态值及所述三元素数据,计算各个第二候选轨迹的筛选值,并将所述目标轨迹集中筛选值符合预设筛选条件的第二候选轨迹,确定为所述目标轨迹。
可选地,所述的***中,所述预设筛选模型包括三元素分布子模型、舒适性筛选子模型、有效行驶路径筛选子模型及参考偏离量筛选子模型中至少一种子模型,并将使所述至少一种子模型的输出值之和达到最小的第二候选轨迹,确定为所述目标轨迹;
所述三元素分布子模型,用于根据所述第二候选轨迹上的各个轨迹点的所述三元素数据,计算各个轨迹点所对应的所述第一直线距离的第一离散度、计算各个轨迹点所对应的所述第二直线距离的第二离散度、以及计算各个轨迹点所对应的所述速度矢量和的第三离散度,并将所述第一离散度按第一预设权重、所述第二离散度按第二预设权重与所述第三离散度按第三预设权重求和,确定所述第二候选轨迹的总离散度;
所述舒适性筛选子模型,用于根据所述第二候选轨迹上的任意两个相邻时刻轨迹点之间的路径矢量及车辆质心运行距离,计算各个时刻的轨迹点的舒适度,并将所述各个时刻的轨迹点的舒适度按第四预设权重求和,确定所述第二候选轨迹的总舒适度;
所述有效行驶路径筛选子模型,用于根据所述第二候选轨迹上任意两相邻时刻轨迹点之间的车辆质心运行距离及相对行驶轨迹,计算所述任意两相邻时刻轨迹点之间的有效行驶路径评价值,并将所述任意两相邻时刻轨迹点之间的有效行驶路径评价值按第五预设权重求和,确定所述第二候选轨迹的总有效行驶路径评价值;其中,所述相对行驶轨迹长度为所车辆质心运行距离投影到道路坐标系下的长度;
所述参考偏离量筛选子模型,用于根据所述第二候选轨迹上各个时刻的轨迹点状态值及预设参考轨迹,计算所述各个时刻轨迹点与预设参考轨迹之间的偏离量评价值,并将所述各个时刻轨迹点的偏离量评价值按第六预设权重求和,确定所述第二候选轨迹的总偏离量评价值。
可选地,所述的***中,所述预设值为0。
本发明的再一目的在于提出一种车辆,其中,所述车辆包括所述的避障轨迹规划***。
所述车辆与上述一种避障轨迹规划方法、***相对于现有技术所具有的优势相同,在此不再赘述
综上所述,本申请提供的避障轨迹规划方法、***及车辆,在基于车辆的当前行驶状态数据构建候选轨迹集后,将所述候选轨迹集输入预设规避模型,得到预设规避模型输出的目标轨迹集,然后从所述目标轨迹集中选取一条第二候选轨迹作为目标轨迹,并按照所述目标轨迹控制所述车辆运行;其中,预设规避模型根据三元素数据,计算所述各个第一候选轨迹的威胁值,并将所述候选轨迹集中威胁值大于预设值的第一候选轨迹筛除,得到目标轨迹集。因为三元素综合了动态障碍物与第一候选轨迹上预设轨迹点之间的距离、以及动态障碍物与车辆之间的速度因素,因而通过由上述三元素数据输入预设规避模型计算得到的威胁值,可以准确判断车辆是否会在上述预设轨迹点处与动态障碍物发生碰撞,也即可以将会发生碰撞的预设轨迹点对应的第一候选轨迹筛除,从而得到安全、合理且可以适应不断变化的实际驾驶环境的第二候选轨迹。
所属领域的技术人员可以清楚地了解到,为描述的方便和简洁,上述描述的***、装置和单元的具体工作过程,可以参考前述方法实施例中的对应过程,在此不再赘述。
以上所述仅为本发明的较佳实施例而已,并不用以限制本发明,凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等,均应包含在本发明的保护范围之内。
以上所述,仅为本发明的具体实施方式,但本发明的保护范围并不局限于此,任何熟悉本技术领域的技术人员在本发明揭露的技术范围内,可轻易想到变化或替换,都应涵盖在本发明的保护范围之内。因此,本发明的保护范围应以权利要求的保护范围为准。
Claims (14)
1.一种避障轨迹规划方法,应用于车辆,其特征在于,所述方法包括:
基于车辆的当前行驶状态数据构建候选轨迹集,所述当前行驶状态数据包括车辆当前质心位置坐标、车辆当前质心速度大小和车辆当前质心运行方向,所述候选轨迹集包括多个第一候选轨迹;
将所述候选轨迹集输入预设规避模型,得到预设规避模型输出的目标轨迹集;其中,所述目标轨迹集包括多个第二候选轨迹,所述预设规避模型用于根据三元素数据,计算所述各个第一候选轨迹的威胁值,并将所述候选轨迹集中威胁值大于预设值的第一候选轨迹筛除,得到目标轨迹集;
从所述目标轨迹集中选取一条第二候选轨迹作为目标轨迹;
按照所述目标轨迹控制所述车辆运行;
其中,所述三元素包括第一候选轨迹上预设轨迹点到距离最近点的第一直线距离、潜在碰撞点到所述距离最近点的第二直线距离、动态障碍物的质心速度与所述预设轨迹点处对应的车辆规划质心速度的速度矢量和。
2.根据权利要求1所述的方法,其特征在于,所述动态障碍物为车辆通过车辆传感器当前监测到的障碍物;所述距离最近点为合成轮廓按所述速度矢量和运行时,所生成的合成轨迹上距离所述预设轨迹点最近的点;所述合成轮廓为将车辆外径叠加到所述动态障碍物上所形成的轮廓;所述潜在碰撞点为参考直线与所述合成轮廓的交点中靠近所述最近碰撞点的点,所述参考直线为经过所述最近碰撞点且与所述速度矢量和的方向平行的直线。
3.根据权利要求1所述的方法,其特征在于,所述预设规避模型包括威胁值确定子模型;所述将所述候选轨迹集输入预设规避模型,得到预设规避模型输出的目标轨迹集,包括:
获取所述三元素数据;
将所述三元素数据输入所述威胁值确定子模型,得到所述威胁值确定子模型输出的所述各个第一候选轨迹的威胁值;
将所述威胁值大于预设值的第一候选轨迹从候选轨迹集中筛除,得到目标轨迹集。
6.根据权利要求1所述的方法,其特征在于,在将所述候选轨迹集输入预设规避模型之前,预先对所述动态障碍物进行威胁性判断,并忽略无碰撞威胁的障碍物。
7.根据权利要求6所述的方法,其特征在于,所述预先对所述动态障碍物威胁性判断,并忽略无碰撞威胁的障碍物,具体包括:
若针对动态障碍物的所述速度矢量和,与预设轨迹点相对于所述动态障碍物质心的位置矢量之间的夹角大于90°,则确定所述动态障碍物为无碰撞威胁的障碍物,并将所述动态障碍物忽略。
8.根据权利要求1所述的方法,其特征在于,所述基于车辆的当前行驶状态数据构建候选轨迹集,包括:
将所述当前行驶状态数据输入通过预设运动学模型,得到所述预设运动学模型输出的预设时刻范围内各个时刻的轨迹点状态值;
根据所述预设时刻范围内各个时刻的轨迹点状态值,确定所述第一候选轨迹,并得到候选轨迹集。
9.根据权利要求8所述的方法,其特征在于,所述预设运动学模型包括:
公式Xh(t+k)=F(Xh(t+k-1))+G·u(t+k-1)、状态矩阵控制矩阵控制变量u(t+k-1)=[ω(t+k-1)a(t+k-1)]T,其中,0≤k≤Np,在t时刻时,为车辆当前位置的轨迹点状态值,且
10.根据权利要求8所述的方法,其特征在于,所述从所述目标轨迹集中选取一条第二候选轨迹作为目标轨迹,包括:
将所述目标轨迹集中各个第二候选轨迹输入预设筛选模型,得到预设筛选模型输出的目标轨迹;其中,所述预设筛选模型用于根据第二候选轨迹上的各个轨迹点的所述轨迹点状态值及所述三元素数据,计算各个第二候选轨迹的筛选值,并将所述目标轨迹集中筛选值符合预设筛选条件的第二候选轨迹,确定为所述目标轨迹。
11.根据权利要求10所述的方法,其特征在于,所述预设筛选模型包括三元素分布子模型、舒适性筛选子模型、有效行驶路径筛选子模型及参考偏离量筛选子模型中至少一种子模型,并将使所述至少一种子模型的输出值之和达到最小的第二候选轨迹,确定为所述目标轨迹;
所述三元素分布子模型,用于根据所述第二候选轨迹上的各个轨迹点的所述三元素数据,计算各个轨迹点所对应的所述第一直线距离的第一离散度、计算各个轨迹点所对应的所述第二直线距离的第二离散度、以及计算各个轨迹点所对应的所述速度矢量和的第三离散度,并将所述第一离散度按第一预设权重、所述第二离散度按第二预设权重与所述第三离散度按第三预设权重求和,确定所述第二候选轨迹的总离散度;
所述舒适性筛选子模型,用于根据所述第二候选轨迹上的任意两个相邻时刻轨迹点之间的路径矢量及车辆质心运行距离,计算各个时刻的轨迹点的舒适度,并将所述各个时刻的轨迹点的舒适度按第四预设权重求和,确定所述第二候选轨迹的总舒适度;
所述有效行驶路径筛选子模型,用于根据所述第二候选轨迹上任意两相邻时刻轨迹点之间的车辆质心运行距离及相对行驶轨迹,计算所述任意两相邻时刻轨迹点之间的有效行驶路径评价值,并将所述任意两相邻时刻轨迹点之间的有效行驶路径评价值按第五预设权重求和,确定所述第二候选轨迹的总有效行驶路径评价值;其中,所述相对行驶轨迹长度为所车辆质心运行距离投影到道路坐标系下的长度;
所述参考偏离量筛选子模型,用于根据所述第二候选轨迹上各个时刻的轨迹点状态值及预设参考轨迹,计算所述各个时刻轨迹点与预设参考轨迹之间的偏离量评价值,并将所述各个时刻轨迹点的偏离量评价值按第六预设权重求和,确定所述第二候选轨迹的总偏离量评价值。
12.一种避障轨迹规划***,应用于车辆,其特征在于,所述***包括:
轨迹集构建模块,用于基于车辆的当前行驶状态数据构建候选轨迹集,所述当前行驶状态数据包括车辆当前质心位置坐标、车辆当前质心速度大小和车辆当前质心运行方向,所述候选轨迹集包括多个第一候选轨迹;
规避模块,用于将所述候选轨迹集输入预设规避模型,得到预设规避模型输出的目标轨迹集;其中,所述目标轨迹集包括多个第二候选轨迹,所述预设规避模型用于根据三元素数据,计算所述各个第一候选轨迹的威胁值,并将所述候选轨迹集中威胁值大于预设值的第一候选轨迹筛除,得到目标轨迹集;
选取模块,用于从所述目标轨迹集中选取一条第二候选轨迹作为目标轨迹;
控制模块,用于按照所述目标轨迹控制所述车辆运行;
其中,所述三元素包括第一候选轨迹上预设轨迹点到距离最近点之间的第一直线距离、潜在碰撞点到所述距离最近点的第二直线距离、动态障碍物的质心速度与所述预设轨迹点处对应的车辆规划质心速度的速度矢量和。
13.根据权利要求12所述的***,其特征在于,所述动态障碍物为车辆通过车辆传感器当前监测到的障碍物;所述距离最近点为合成轮廓按所述速度矢量和运行时,所生成的合成轨迹上距离所述预设轨迹点最近的点;所述合成轮廓为将车辆外径叠加到所述动态障碍物上所形成的轮廓;所述潜在碰撞点为参考直线与所述合成轮廓的交点中靠近所述最近碰撞点的点,所述参考直线为经过所述最近碰撞点且与所述速度矢量和的方向平行的直线。
14.一种车辆,其特征在于,所述车辆包括如权利要求12~13任意所述的避障轨迹规划***。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202010297414.0A CN111591307B (zh) | 2020-04-15 | 2020-04-15 | 一种避障轨迹规划方法、***及车辆 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202010297414.0A CN111591307B (zh) | 2020-04-15 | 2020-04-15 | 一种避障轨迹规划方法、***及车辆 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN111591307A true CN111591307A (zh) | 2020-08-28 |
CN111591307B CN111591307B (zh) | 2021-10-01 |
Family
ID=72187563
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202010297414.0A Active CN111591307B (zh) | 2020-04-15 | 2020-04-15 | 一种避障轨迹规划方法、***及车辆 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN111591307B (zh) |
Cited By (7)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN113283401A (zh) * | 2021-07-20 | 2021-08-20 | 山东华力机电有限公司 | 基于云计算和图像处理的agv小车避障方法 |
CN113506237A (zh) * | 2021-05-17 | 2021-10-15 | 毫末智行科技有限公司 | 用于确定对象的边界的方法及对象检测方法 |
CN113823123A (zh) * | 2021-09-28 | 2021-12-21 | 合肥工业大学 | 一种基于离散点轨迹拟合的车辆避障预警方法及装置 |
CN115309168A (zh) * | 2022-10-11 | 2022-11-08 | 天地科技股份有限公司 | 一种井下无人车控制方法及装置 |
CN115456297A (zh) * | 2022-09-28 | 2022-12-09 | 远峰科技股份有限公司 | 自动泊车路径优化方法及装置 |
CN115657685A (zh) * | 2022-12-12 | 2023-01-31 | 中国南方电网有限责任公司超高压输电公司广州局 | 水上避障方法、装置、计算机设备和存储介质 |
CN118097972A (zh) * | 2024-04-26 | 2024-05-28 | 泰安九洲金城机械有限公司 | 一种车辆行驶轨迹的轨迹点优化方法及装置 |
Citations (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN102902269A (zh) * | 2012-09-21 | 2013-01-30 | 北京邮电大学 | 预选择最小距离指标冗余机器人动态避障方法 |
CN107672588A (zh) * | 2017-08-29 | 2018-02-09 | 广州小鹏汽车科技有限公司 | 一种自动泊车路径障碍物碰撞检测方法、装置及*** |
CN107702716A (zh) * | 2017-08-31 | 2018-02-16 | 广州小鹏汽车科技有限公司 | 一种无人驾驶路径规划方法、***和装置 |
CN109960261A (zh) * | 2019-03-22 | 2019-07-02 | 北京理工大学 | 一种基于碰撞检测的动态障碍物避让方法 |
WO2019206377A1 (de) * | 2018-04-24 | 2019-10-31 | Robert Bosch Gmbh | Verfahren und vorrichtung zum kooperativen abstimmen von zukünftigen fahrmanövern eines fahrzeugs mit fremdmanövern zumindest eines fremdfahrzeugs |
US20200110414A1 (en) * | 2018-10-05 | 2020-04-09 | Waymo Llc | Generating trajectories for autonomous vehicles |
-
2020
- 2020-04-15 CN CN202010297414.0A patent/CN111591307B/zh active Active
Patent Citations (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN102902269A (zh) * | 2012-09-21 | 2013-01-30 | 北京邮电大学 | 预选择最小距离指标冗余机器人动态避障方法 |
CN107672588A (zh) * | 2017-08-29 | 2018-02-09 | 广州小鹏汽车科技有限公司 | 一种自动泊车路径障碍物碰撞检测方法、装置及*** |
CN107702716A (zh) * | 2017-08-31 | 2018-02-16 | 广州小鹏汽车科技有限公司 | 一种无人驾驶路径规划方法、***和装置 |
WO2019206377A1 (de) * | 2018-04-24 | 2019-10-31 | Robert Bosch Gmbh | Verfahren und vorrichtung zum kooperativen abstimmen von zukünftigen fahrmanövern eines fahrzeugs mit fremdmanövern zumindest eines fremdfahrzeugs |
US20200110414A1 (en) * | 2018-10-05 | 2020-04-09 | Waymo Llc | Generating trajectories for autonomous vehicles |
CN109960261A (zh) * | 2019-03-22 | 2019-07-02 | 北京理工大学 | 一种基于碰撞检测的动态障碍物避让方法 |
Cited By (12)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN113506237A (zh) * | 2021-05-17 | 2021-10-15 | 毫末智行科技有限公司 | 用于确定对象的边界的方法及对象检测方法 |
CN113506237B (zh) * | 2021-05-17 | 2024-05-24 | 毫末智行科技有限公司 | 用于确定对象的边界的方法及对象检测方法 |
CN113283401A (zh) * | 2021-07-20 | 2021-08-20 | 山东华力机电有限公司 | 基于云计算和图像处理的agv小车避障方法 |
CN113283401B (zh) * | 2021-07-20 | 2021-10-08 | 山东华力机电有限公司 | 基于云计算和图像处理的agv小车避障方法 |
CN113823123A (zh) * | 2021-09-28 | 2021-12-21 | 合肥工业大学 | 一种基于离散点轨迹拟合的车辆避障预警方法及装置 |
CN113823123B (zh) * | 2021-09-28 | 2022-07-01 | 合肥工业大学 | 一种基于离散点轨迹拟合的车辆避障预警方法及装置 |
CN115456297A (zh) * | 2022-09-28 | 2022-12-09 | 远峰科技股份有限公司 | 自动泊车路径优化方法及装置 |
CN115456297B (zh) * | 2022-09-28 | 2024-05-21 | 远峰科技股份有限公司 | 自动泊车路径优化方法及装置 |
CN115309168A (zh) * | 2022-10-11 | 2022-11-08 | 天地科技股份有限公司 | 一种井下无人车控制方法及装置 |
CN115309168B (zh) * | 2022-10-11 | 2023-01-10 | 天地科技股份有限公司 | 一种井下无人车控制方法及装置 |
CN115657685A (zh) * | 2022-12-12 | 2023-01-31 | 中国南方电网有限责任公司超高压输电公司广州局 | 水上避障方法、装置、计算机设备和存储介质 |
CN118097972A (zh) * | 2024-04-26 | 2024-05-28 | 泰安九洲金城机械有限公司 | 一种车辆行驶轨迹的轨迹点优化方法及装置 |
Also Published As
Publication number | Publication date |
---|---|
CN111591307B (zh) | 2021-10-01 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN111591307B (zh) | 一种避障轨迹规划方法、***及车辆 | |
CN113386795B (zh) | 一种自动驾驶车辆智能决策及局部轨迹规划方法及其决策*** | |
EP3829958B1 (en) | Method for steering a vehicle and apparatus therefor | |
JP7150067B2 (ja) | 車両の制御システム、車両を制御する方法、及び非一時的コンピュータ可読メモリ | |
CN108445750B (zh) | 用于车辆运动规划的方法和*** | |
CN106080598B (zh) | 实时预期速度控制 | |
CN106030609B (zh) | 用于模仿前车的***和方法 | |
CN101837781B (zh) | 用于自动车道对正或换道的控制***的基于模型的预测控制 | |
EP3498555A1 (en) | Automatic driving device | |
CN112193244B (zh) | 基于线性约束的自动驾驶车辆运动规划方法 | |
CN112937608B (zh) | 一种基于轨迹预测的冰雪环境无人驾驶车辆一体化滚动决策方法、装置及存储介质 | |
CN107709058B (zh) | 基于轨迹的底盘控制 | |
CN113433947B (zh) | 基于障碍车估计与预测的交叉路口轨迹规划与控制方法 | |
EP3666612A1 (en) | Vehicle control device | |
CN114148348B (zh) | 自动驾驶车辆的停止控制方法、装置及计算机程序 | |
Bae et al. | Lane change maneuver based on Bezier curve providing comfort experience for autonomous vehicle users | |
CN114543827A (zh) | 一种路径规划方法及装置 | |
CN112305911A (zh) | 一种复杂环境下的反馈预测控制方法及其装置、车辆 | |
CN116654017A (zh) | 一种紧急工况下基于周车轨迹预测的智能汽车避撞决策与路径规划的方法、*** | |
CN117141489B (zh) | 一种基于最小作用量原理的智能车辆轨迹分层规划方法 | |
CN114442630A (zh) | 一种基于强化学习和模型预测的智能车规划控制方法 | |
CN116465427B (zh) | 一种基于时空风险量化的智能车辆换道避障路径规划方法 | |
CN115447607A (zh) | 用于规划车辆行驶轨迹的方法和装置 | |
CN115140048A (zh) | 一种自动驾驶行为决策与轨迹规划模型与方法 | |
Lienke et al. | Environment modeling for the application in optimization-based trajectory planning |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
PB01 | Publication | ||
PB01 | Publication | ||
SE01 | Entry into force of request for substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
GR01 | Patent grant | ||
GR01 | Patent grant |