CN114889643A - 一种运动障碍物的三元素自主避障方法 - Google Patents
一种运动障碍物的三元素自主避障方法 Download PDFInfo
- Publication number
- CN114889643A CN114889643A CN202210470261.4A CN202210470261A CN114889643A CN 114889643 A CN114889643 A CN 114889643A CN 202210470261 A CN202210470261 A CN 202210470261A CN 114889643 A CN114889643 A CN 114889643A
- Authority
- CN
- China
- Prior art keywords
- vehicle
- obstacle
- self
- point
- speed
- 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.)
- Pending
Links
- 238000000034 method Methods 0.000 title claims abstract description 56
- 238000004088 simulation Methods 0.000 claims abstract description 16
- 238000004422 calculation algorithm Methods 0.000 claims abstract description 14
- 230000006870 function Effects 0.000 claims description 88
- 230000008859 change Effects 0.000 claims description 39
- 238000010586 diagram Methods 0.000 claims description 12
- 238000004364 calculation method Methods 0.000 claims description 9
- 230000033001 locomotion Effects 0.000 claims description 9
- 230000001133 acceleration Effects 0.000 claims description 6
- 230000004888 barrier function Effects 0.000 claims description 6
- 230000000694 effects Effects 0.000 claims description 6
- 238000011156 evaluation Methods 0.000 claims description 6
- 238000013507 mapping Methods 0.000 claims description 6
- 238000012545 processing Methods 0.000 claims description 5
- 238000013461 design Methods 0.000 claims description 4
- 230000005540 biological transmission Effects 0.000 claims description 3
- 238000010276 construction Methods 0.000 claims description 3
- 238000002474 experimental method Methods 0.000 claims description 3
- 238000013178 mathematical model Methods 0.000 claims description 3
- 239000011159 matrix material Substances 0.000 claims description 3
- 238000012795 verification Methods 0.000 claims description 3
- 238000009825 accumulation Methods 0.000 claims description 2
- 238000005516 engineering process Methods 0.000 description 4
- 230000036962 time dependent Effects 0.000 description 3
- 230000003044 adaptive effect Effects 0.000 description 2
- 230000007613 environmental effect Effects 0.000 description 2
- 230000008569 process Effects 0.000 description 2
- 239000000956 alloy Substances 0.000 description 1
- 229910045601 alloy Inorganic materials 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000015572 biosynthetic process Effects 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 230000002349 favourable effect Effects 0.000 description 1
- 239000011664 nicotinic acid Substances 0.000 description 1
- 230000008447 perception Effects 0.000 description 1
- 238000005070 sampling Methods 0.000 description 1
- 238000003786 synthesis reaction Methods 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
-
- 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
- B60W30/00—Purposes of road vehicle drive control systems not related to the control of a particular sub-unit, e.g. of systems using conjoint control of vehicle sub-units
- B60W30/08—Active safety systems predicting or avoiding probable or impending collision or attempting to minimise its consequences
- B60W30/095—Predicting travel path or likelihood of collision
-
- 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
- B60W40/00—Estimation or calculation of non-directly measurable driving parameters for road vehicle drive control systems not related to the control of a particular sub unit, e.g. by using mathematical models
-
- 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/0015—Planning or execution of driving tasks specially adapted for safety
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
- Y02T10/00—Road transport of goods or passengers
- Y02T10/10—Internal combustion engine [ICE] based vehicles
- Y02T10/40—Engine management systems
Landscapes
- Engineering & Computer Science (AREA)
- Automation & Control Theory (AREA)
- Transportation (AREA)
- Mechanical Engineering (AREA)
- Human Computer Interaction (AREA)
- Physics & Mathematics (AREA)
- Mathematical Physics (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
本发明公开了一种运动障碍物的三元素自主避障方法,包括以下步骤,所述(1)首先通过GNSS设备采集期望路径的waypoint点,并在期望路径上构建Frenet坐标系;(2)通过Frenet坐标系在局部环境下划分自适应栅格点,有利于确定自车在局部环境下的具***置,然后引入速度障碍物的概念,障碍物的姿态信息通过激光雷达测出;(3)通过对障碍物进行圆形近似提出三元素的动态避障方法。本发明通过三元素原理结合自适应栅格法和MPC安全高效的规划出无碰撞安全路径,为局部动态避障提供了一种全新的方法,具有一定的应用前景,并且大大减少了计算量,仿真结果表明该算法能成功的避开动态障碍物,在自车较低速度时,算法能满足平顺性、安全性、实时性和道路交通规则的要求。
Description
技术领域
本发明涉及汽车自动驾驶技术领域,具体为一种运动障碍物的三元素自主避障方法。
背景技术
自动驾驶技术既能有效解决交通安全问题,还能缓解交通拥堵、提高交通效率,给人们生活带来便利,自动驾驶汽车是通过车载传感器检测和识别环境信息并进行分析判断,并利用算法控制车辆自主运动,构成自动驾驶的***一般包括环境感知、决策规划、控制执行等,路径规划是自动驾驶的关键技术之一,自动驾驶汽车要在道路环境中对可行使路线进行规划,实现规划路径的安全、平顺及高效,必须有高效的规划技术作保障,路径规划常分为全局路径规划与局部路径规划,全局路径规划是空间上的位置信息,是一条从起始点到终点的路径,但局部路径规划是一种二维的规划,依托车载传感器对局部环境信息的感知,还需要考虑时间和局部避障问题,目前常用的避障算法有图形搜索法、采样法、智能仿生法、人工势场法等,每一种单一方法对于速度较低的机器人避障有一定的效果,但这些算法都存在搜索范围广、计算量大、对动态障碍物避障不明显以及陷入局部极小值等情况,若在自动驾驶车辆上应用需要对其进行整合,对于动态障碍物避障问题,在有运动障碍物的道路环境中进行路径规划是比较复杂的,需要考虑到多种情况,涉及到空间上的路径规划以及时间域上的速度规划,为了使自动驾驶车辆在局部环境下安全高效的避开动态障碍物,必须开发一种新的算法来应对这种问题。
现有技术中运动障碍物自主避障方法存在的缺陷是:
1、专利文件CN113985896A公开一种自动驾驶的带避障的路径规划方法,包括“采集工作地块地理信息,获得多边形顺序坐标点,以规定安全边界距离将多边形向内部缩进,以当前工作车辆位置为参考,以与多边形顶点距离最近的为下一个目标点,以此点相邻较长边,生成路径待定点并生成路径;遇到障碍物通过平移待定点绕过障碍物,并能通过比较左右平移距离获得较优路径,通过平移路径待定点生成下一组路径,直至路径能够遍历整个工作地块”,但是该方法在使用时,需要遍历整个工作地块,计算量偏大。
发明内容
本发明的目的在于提供一种运动障碍物的三元素自主避障方法,以解决上述背景技术中提出的问题。
为实现上述目的,本发明提供如下技术方案:一种运动障碍物的三元素自主避障方法,包括以下步骤,所述(1)首先通过GNSS设备采集期望路径的waypoint点,并在期望路径上构建Frenet坐标系;(2)通过Frenet坐标系在局部环境下划分自适应栅格点,有利于确定自车在局部环境下的具***置,然后引入速度障碍物的概念,障碍物的姿态信息通过激光雷达测出; (3)通过对障碍物进行圆形近似提出三元素的动态避障方法;(4)接着构建各种约束的代价函数,根据权重值确定最终规划的路径点[1];(5)最后进行仿真验证;
优选的,所述步骤(1)局部栅格的划分,进行局部避障时先对局部道路环境进行栅格划分,首先引入Frenet坐标系,如附图12所示,附图12是车辆在大地坐标系与Frenet坐标系下的位置,该图形象描述了自车在两个坐标系之间的关系,自车在大地坐标系下的坐标是K(x2,y2),图中虚线是期望路径的参考线,F点为自车坐标点位置在参考线上的投影点,F点在全局坐标系下的坐标为F(x1,y1),根据计算可以得到F点在期望路径上的Frenet坐标(s,d),其中s为参考线上起点位置到F点的路径长,通过累加计算得到,d是F点到自车的横向距离。最终构建式中Tref为参考线;然后在Frenet 坐标系下进行栅格的划分,划分方法如下:
shange_a=-3.75:0.25:3.75
Shange_a和shange_b分别是Frenet坐标系中的S、L,但是S值需要加上自车当前位置的路径长度s(n_out),R_lj是道路曲率半径;道路曲率半径可以通过三点法计算,自车在期望路径上的投影点为中间点,选取此投影点前后各5m的位置有两个点,通过这三个点就可以确定道路曲率半径;
计算公式如下,已知三个点的坐标分别是,圆的方程一般可以写成二次多项式:
A(x2+y2)+Bx+Cy+D=0
将上式进行变形得到圆的标准方程:
再将三个坐标点带入到A(x2+y2)+Bx+Cy+D=0式中得到齐次线性方程组:
根据自适应栅格划分原则,在道路上进行栅格点的划分,附图3是自适应栅格画法,在道路曲率较小时,划分的前向距离较大,当经过大区率弯道时划分的前向距离较小。
优选的,所述步骤(2)建立三元素避障模型如附图4所示,附图4中自车为A所在的位置,障碍车为B所在位置,自车速度为Vh,障碍车的速度为航向角为将自车的速度映射到障碍车上。同时把自车A变成一个点,得到自车与障碍车合成的相对速度vc;其方向是过B点沿BC的方向,然后过自车A点作AD⊥BC交点为E点,构成ΔABE。BE线段的长为l,AE线段的长为W,然后建立避障的几何数学模型,得到如下公式:
当fg(w,l,vc)=0时,根据模型得出w的值越大自车与障碍车发生碰撞的概率就越小,但当l,vc的值接近于0时,威胁度也比较大;随着时间的推移,自车还是会和障碍车相撞;w是里面最关键的一个参数,当碰撞时间因子l/vc→0m时,此时vc的改变对w的影响非常大;最终得到速度障碍车的避障模型评价参数,w,l,vc三个参数数值的大小对碰撞其关键作用;然后建立优化后的多障碍物避障模型,优化后的避障模型附图5所示,图中A是自车,B、C是两个障碍车辆,根据碰撞锥模型,将三个车辆的外形轮廓形状近似成圆形,其圆形的半径根据传感器测得障碍物车的外形轮廓后得到,图中A、B、C三个车辆的轮廓最开始都是图中的实线圆,自车A的速度与航向角分别是障碍车B的位置和速度分别是 v0(x0,y0)、障碍车C的位置和速度分别是v1(x1,y1)、根据避障模型,考虑到自车与障碍车的外形轮廓,障碍车的最外圈虚线圆就是考虑到自车A的外形半径r所形成新的无碰撞外形轮廓;然后将自车A的速度分别映射到障碍车B、C上,分别得到B、C相对于A的合成速度在合成速度方向指引延长线上,画出两个障碍车对应速度方向上的外形轮廓线a、b和c、d;该轮廓线同时也是两虚线圆的切线。在靠近自车的那一边,外形轮廓的两个切点分别是f、g,通过自车质心分别向映射后的两个障碍车外形轮廓做垂线,得到垂足点m、n;此时得到mn、ng、mf三条线段,线段mn构成的长度是三元素模型中的w值,线段ng、mf的长度就是l值;最终得到w、l、vc三个参数,这三个参数是评价自车与障碍车之间发生碰撞的关键,这样就将障碍车的速度考虑进来,且可以进行多运动障碍物的避障,当有多个障碍车时,对应就有多组的w、l、vc参数;下面对威胁度权重函数进行设计,根据避障模型,对自车产生威胁的元素主要是w、l、vc,构建如下的代价函数:
式中k1/k2/k3是几何三元素w、l、vc对应的三个权重值,其中对于NA个障碍物进行了如下的定义:
优选的,所述步骤(3)设计基于MPC的代价函数,为了从空间中解决自动驾驶车辆最优行驶路线,需要重新定义一个评价路径优劣的准则,也就是代价函数。在自动驾驶中,路径决策的代价函数主要体现在行车的安全性、舒适性、高效性上;其中安全性是最重要的一个环节,它反映了车辆与障碍物之间的潜在危险,这是顺利通行的保证;舒适性体现在车辆横纵向速度的变化幅度,以及连续两个时间段的前轮转角的变化情况;高效性体现在车辆通行效率上,代价函数的设计如下所示:
1)左侧优先变道代价函数的设计
根据道路法和行车准则,中国道路上的车辆都是靠右行驶,当自车需要超车时,一般情况下需要向左变道,于是设计如下的权重函数:
k=0,1…N-1,klp为权重系数。SL是车道偏移量,是自车到道路边界的宽度;
2)逐次变道代价函数
由于上面的约束条件引入了不是连续变化的布尔变量作为权重函数,此种约束条件会导致最后的目标函数为非凸的情况,非凸的约束存在局部最优解不是全局最优解现象,需要对此进行如下的变形:
上式中kc为权重系数,ML是实验所在车道宽度;
3)侧向加速度代价函数
4)方向盘转角变化率代价函数
在自动驾驶汽车局部路径规划时,为了使跟踪的轨迹平滑,需要考虑方向盘转角变化率,防止程序在计算时,出现方向盘转角变化率过大导致车辆方向盘剧烈抖动失控的现象。仿真车方向盘与前轮转角的传动比为15.8。得到如下公式:
fcosSW(t+k)=kSW·15.8·[δf(t+k+1)-δf(t+k)]
其中kSW为权重系数,δf为前轮转角;
5)基于几何三元素的威胁度权重函数
根据推导的基于几何三元素的动态避障方法,得到动态避障的理论公式:
此代价函数可以解决多运动障碍物的避障情况,自车与障碍车越靠近时对应的栅格点的权重越大,所付出的代价越大;
6)车道对中代价函数
车道对中代价函数也叫路径偏移代价函数。在车辆正常行驶时,需要符合行驶规则,自车应该行驶在每个车道的最中间,所以在道路中间对应的点的权重应该小,而道路边缘的点所对应的权重应该较大;
7)安全度的代价函数
安全度是衡量有障碍物情况下的一个权重函数,与几何三元素的威胁度类似,需要提前知道自车与障碍车的速度,通过评估车辆的制动性能与制动距离,得到安全车距公式:
fs(t+k)=2.2VA、B(t+k)+D
VA、B=vA-vB表示自车与障碍车之间的相对速度,D根据经验一般取6m,表示当自车与障碍车停止时之间的安全距离,在实际行车时存在一个实际车距,然后得到如下公式:
上式是安全度代价函数公式,此代价函数只用在存在一个障碍车辆下;
8)前视点权重函数
将前视点权重函数加入到整个栅格点选取的评价中,越靠近此前视点的栅格点,代价越小,得到如下公式:
(xs,ys),(xq,yq)分别表示栅格点和前视点的坐标;
最终得到总的目标代价函数公式,该代价函数的设计都是基于Frenet坐标系,这样有效减少了无效栅格点的搜索时间,提高了算法的实时性,同时能准确定位自车在局部环境的具***置,可以有效的进行路径跟踪;
fcostfunction(i)=fcostp(t+k)+fcostc(t+k)′+fcosay(t+k)+fcosSW(t+k) +fd(t+k)+fz(t+k)+fan(t+k)+fqian(t+k)
得到总的权重函数之后通过min函数求出每个循环时刻权重最小的栅格点:
为了安全性加上硬约束:前轮极限转角约束,道路边界约束[2]:
-38°≤Steer≤38°
优选的,所述步骤(4)动态避障仿真实验:为了验证三元素方法的动态避障效果,进行了联合仿真;采用Uturn路线,同时在道路上设置一个运动障碍物,从距离起点25米处逆向以10km/h的速度往起点运动,自车从起点向终点按照曲率规划的速度进行运动;附图6是自车的行驶路线图,在程序里面设置了障碍物的运动轨迹之后,自车会根据三元素的避障算法,进行转向避障处理,在避开障碍物之后自车会迅速跟踪上期望路径;附图7是避障时车辆的航向角变化曲线,根据图示,5s之后,由于自车进行转向操作,所以航向角会有一个小范围的变化,如附图7中的第一个小的波峰所示;后面的一个凸字形波峰是过S弯的时自车两个极限转角对应的航向角变化曲线;自车先进行极限的左转,之后又进行右转,最后航向角回到0°;附图8是自车方向盘转角变化图,一开始自车就进行了避障处理,所以一开始自车的转角就从0°变化到120°左右,直线行驶又变为0°左右;然后在行驶了5s后进入第一个极限弯道,方向盘最高转到+310°,又经过15s的直线运动,进入第二个弯道,此时方向盘转角最大为-295°,最后进入直道方向盘转角又回到0°;将自车航向角与方向盘转角随时间的变化曲线画在一起就是附图9。附图10是整个程序的循环耗时,最高耗时为25.5ms最低9.5ms,总体的平均耗时13.5ms;附图11为仿真时自车跟踪规划的速度,最高规划速度为15km/h。
与现有技术相比,本发明的有益效果是:
1、本发明通过使用方法结合自适应栅格法和MPC原理安全高效的规划出无碰撞安全路径,为局部动态避障提供了一种全新的方法,具有一定的应用前景,并且大大减少了计算量,仿真结果表明该算法能成功的避开动态障碍物,在自车较低速度时,算法能满足平顺性、安全性、实时性和道路交通规则的要求。
附图说明
图1为本发明的整体步骤示意图;
图2为本发明的基于Frenet坐标系的栅格法示意图;
图3为本发明的自适应栅格法示意图;
图4为本发明的避障结合模型示意图;
图5为本发明的优化避障几何模型示意图;
图6为本发明的自车的行驶路线图示意图;
图7为本发明的避障时车辆的航向角变化曲线示意图;
图8为本发明的自车方向盘转角变化示意图;
图9为本发明的自车航向角与方向盘转角随时间的变化曲线示意图;
图10为本发明的整个程序的循环耗时示意图;
图11为本发明的仿真时自车跟踪规划的速度示意图;
图12为本发明的Frenet坐标系示意图。
具体实施方式
为使本发明实施例的目的、技术方案和优点更加清楚,下面将结合本发明实施例中的附图,对本发明实施例中的技术方案进行清楚、完整地描述,显然,所描述的实施例是本发明一部分实施例,而不是全部的实施例。基于本发明中的实施例,本领域普通技术人员在没有作创造性劳动前提下所获得的所有其他实施例,都属于本发明保护的范围。
在本发明的描述中,需要说明的是,除非另有明确的规定和限定,术语“安装”、“相连”、“连接”应做广义理解,例如,可以是固定连接,也可以是可拆卸连接,或一体地连接;可以是机械连接,也可以是电连接;可以是直接相连,也可以通过中间媒介间接相连,可以是两个元件内部的连通。对于本领域的普通技术人员而言,可以具体情况理解上述术语在本发明中的具体含义。
请参阅图1、图2、图3和图12,本发明提供一种实施例:一种运动障碍物的三元素自主避障方法;
实施例一:
包括以下步骤,(1)首先通过GNSS设备采集期望路径的waypoint点,并在期望路径上构建Frenet坐标系。
请参阅图4和图5所示,一种运动障碍物的三元素自主避障方法;
实施例二:(2)通过Frenet坐标系在局部环境下划分自适应栅格点,有利于确定自车在局部环境下的具***置,然后引入速度障碍物的概念,障碍物的姿态信息通过激光雷达测出,首先引入Frenet坐标系,如附图12所示,附图12是车辆在大地坐标系与Frenet坐标系下的位置,该图形象描述了自车在两个坐标系之间的关系,自车在大地坐标系下的坐标是K(x2,y2),图中虚线是期望路径的参考线,F点为自车坐标点位置在参考线上的投影点,F点在全局坐标系下的坐标为F(x1,y1),根据计算可以得到F点在期望路径上的 Frenet坐标(s,d),其中s为参考线上起点位置到F点的路径长,通过累加计算得到,d是F点到自车的横向距离;最终构建式中Tref为参考线。然后在Frenet坐标系下进行栅格的划分,划分方法如下:
shange_a=-3.75:0.25:3.75
Shange_a和shange_b分别是Frenet坐标系中的S、L,但是S值需要加上自车当前位置的路径长度s(n_out),R_lj是道路曲率半径;道路曲率半径可以通过三点法计算,自车在期望路径上的投影点为中间点,选取此投影点前后各5m的位置有两个点,通过这三个点就可以确定道路曲率半径;
计算公式如下,已知三个点的坐标分别是,圆的方程一般可以写成二次多项式:
A(x2+y2)+Bx+Cy+D=0
将上式进行变形得到圆的标准方程:
再将三个坐标点带入到A(x2+y2)+Bx+Cy+D=0式中得到齐次线性方程组:
根据自适应栅格划分原则,在道路上进行栅格点的划分,附图3是自适应栅格画法,在道路曲率较小时,划分的前向距离较大,当经过大区率弯道时划分的前向距离较小。
请参阅图1、图4和图5所示,一种运动障碍物的三元素自主避障方法;
实施例三:(3)通过对障碍物进行圆形近似提出三元素的动态避障方法,附图4中自车为A所在的位置,障碍车为B所在位置,自车速度为Vh,障碍车的速度为航向角为将自车的速度映射到障碍车上;同时把自车A 变成一个点,得到自车与障碍车合成的相对速度vc;其方向是过B点沿BC的方向,然后过自车A点作AD⊥BC交点为E点,构成ΔABE;BE线段的长为l,AE线段的长为W,然后建立避障的几何数学模型,得到如下公式:
当fg(w,l,vc)=0时,根据模型得出w的值越大自车与障碍车发生碰撞的概率就越小,但当l,vc的值接近于0时,威胁度也比较大。随着时间的推移,自车还是会和障碍车相撞;w是里面最关键的一个参数,当碰撞时间因子l/vc→0m时,此时vc的改变对w的影响非常大;最终得到速度障碍车的避障模型评价参数,w,l,vc三个参数数值的大小对碰撞其关键作用;然后建立优化后的多障碍物避障模型,优化后的避障模型附图5所示,图中A是自车,B、C是两个障碍车辆,根据碰撞锥模型,将三个车辆的外形轮廓形状近似成圆形,其圆形的半径根据传感器测得障碍物车的外形轮廓后得到,图中A、B、C三个车辆的轮廓最开始都是图中的实线圆,自车A的速度与航向角分别是障碍车B的位置和速度分别是v0(x0,y0)、障碍车C的位置和速度分别是v1(x1,y1)、根据避障模型,考虑到自车与障碍车的外形轮廓,障碍车的最外圈虚线圆就是考虑到自车A的外形半径r所形成新的无碰撞外形轮廓;然后将自车A的速度分别映射到障碍车B、C上,分别得到B、C相对于A的合成速度在合成速度方向指引延长线上,画出两个障碍车对应速度方向上的外形轮廓线a、b和c、d;该轮廓线同时也是两虚线圆的切线;靠近自车的那一边,外形轮廓的两个切点分别是f、g,通过自车质心分别向映射后的两个障碍车外形轮廓做垂线,得到垂足点m、n;此时得到mn、ng、mf三条线段,线段mn构成的长度是三元素模型中的w值,线段ng、mf的长度就是l值;最终得到w、l、vc三个参数,这三个参数是评价自车与障碍车之间发生碰撞的关键,这样就将障碍车的速度考虑进来,且可以进行多运动障碍物的避障,当有多个障碍车时,对应就有多组的w、l、vc参数。下面对威胁度权重函数进行设计,根据避障模型,对自车产生威胁的元素主要是w、l、vc,构建如下的代价函数:
式中k1/k2/k3是几何三元素w、l、vc对应的三个权重值,其中对于NA个障碍物进行了如下的定义:
请参阅图1所示,一种运动障碍物的三元素自主避障方法;
实施例四:(4)接着构建各种约束的代价函数,根据权重值确定最终规划的路径点[1],设计基于MPC的代价函数,为了从空间中解决自动驾驶车辆最优行驶路线,需要重新定义一个评价路径优劣的准则,也就是代价函数;在自动驾驶中,路径决策的代价函数主要体现在行车的安全性、舒适性、高效性上;其中安全性是最重要的一个环节,它反映了车辆与障碍物之间的潜在危险,这是顺利通行的保证;舒适性体现在车辆横纵向速度的变化幅度,以及连续两个时间段的前轮转角的变化情况;高效性体现在车辆通行效率上,代价函数的设计如下所示:
1)左侧优先变道代价函数的设计
根据道路法和行车准则,中国道路上的车辆是靠右行驶,当自车需要超车时,一般情况下需要向左变道,于是设计如下的权重函数:
k=0,1…N-1,klp为权重系数,SL是车道偏移量,是自车到道路边界的宽度。
2)逐次变道代价函数
由于上面的约束条件引入了不是连续变化的布尔变量作为权重函数,此种约束条件会导致最后的目标函数为非凸的情况,非凸的约束存在局部最优解不是全局最优解现象,需要对此进行如下的变形:
上式中kc为权重系数,ML是实验所在车道宽度。
3)侧向加速度代价函数
4)方向盘转角变化率代价函数
在自动驾驶汽车局部路径规划时,为了使跟踪的轨迹平滑,需要考虑方向盘转角变化率,防止程序在计算时,出现方向盘转角变化率过大导致车辆方向盘剧烈抖动失控的现象。仿真车方向盘与前轮转角的传动比为15.8。得到如下公式:
fcosSW(t+k)=kSW·15.8·[δf(t+k+1)-δf(t+k)]
其中kSW为权重系数,δf为前轮转角。
5)基于几何三元素的威胁度权重函数
根据推导的基于几何三元素的动态避障方法,得到动态避障的理论公式:
此代价函数可以解决多运动障碍物的避障情况,自车与障碍车越靠近时对应的栅格点的权重越大,所付出的代价越大。
6)车道对中代价函数
车道对中代价函数也叫路径偏移代价函数;在车辆正常行驶时,需要符合行驶规则,自车应该行驶在每个车道的最中间,所以在道路中间对应的点的权重应该小,而道路边缘的点所对应的权重应该较大。
7)安全度的代价函数
安全度是衡量有障碍物情况下的一个权重函数,与几何三元素的威胁度类似,需要提前知道自车与障碍车的速度,通过评估车辆的制动性能与制动距离,得到安全车距公式:
fs(t+k)=2.2VA、B(t+k)+D
VA、B=vA-vB表示自车与障碍车之间的相对速度,D根据经验一般取6m,表示当自车与障碍车停止时之间的安全距离,在实际行车时存在一个实际车距,然后得到如下公式:
上式是安全度代价函数公式,此代价函数只用在存在一个障碍车辆下。
8)前视点权重函数
将前视点权重函数加入到整个栅格点选取的评价中,越靠近此前视点的栅格点,代价越小,得到如下公式:
(xs,ys),(xq,yq)分别表示栅格点和前视点的坐标。
最终得到总的目标代价函数公式,该代价函数的设计都是基于Frenet坐标系,这样有效减少了无效栅格点的搜索时间,提高了算法的实时性,同时能准确定位自车在局部环境的具***置,可以有效的进行路径跟踪。
fcostfunction(i)=fcostp(t+k)+fcostc(t+k)′+fcosay(t+k)+fcosSW(t+k) +fd(t+k)+fz(t+k)+fan(t+k)+fqian(t+k)
得到总的权重函数之后通过min函数求出每个循环时刻权重最小的栅格点:
为了安全性加上硬约束:前轮极限转角约束,道路边界约束[2]:
-38°≤Steer≤38°
请参阅图1、图6-11所示,一种运动障碍物的三元素自主避障方法;
实施例四:(5)最后进行仿真验证,为了验证三元素方法的动态避障效果,进行了联合仿真。采用Uturn路线,同时在道路上设置一个运动障碍物,从距离起点25米处逆向以10km/h的速度往起点运动,自车从起点向终点按照曲率规划的速度进行运动。附图6是自车的行驶路线图,在程序里面设置了障碍物的运动轨迹之后,自车会根据三元素的避障算法,进行转向避障处理,在避开障碍物之后自车会迅速跟踪上期望路径。附图7是避障时车辆的航向角变化曲线,根据图示,5s之后,由于自车进行转向操作,所以航向角会有一个小范围的变化,如附图7中的第一个小的波峰所示。后面的一个凸字形波峰是过S弯的时自车两个极限转角对应的航向角变化曲线。自车先进行极限的左转,之后又进行右转,最后航向角回到0°。附图8是自车方向盘转角变化图,一开始自车就进行了避障处理,所以一开始自车的转角就从0°变化到120°左右,直线行驶又变为0°左右。然后在行驶了5s后进入第一个极限弯道,方向盘最高转到+310°,又经过15s的直线运动,进入第二个弯道,此时方向盘转角最大为-295°,最后进入直道方向盘转角又回到0°。将自车航向角与方向盘转角随时间的变化曲线画在一起就是附图9。附图10 是整个程序的循环耗时,最高耗时为25.5ms最低9.5ms,总体的平均耗时 13.5ms。附图11为仿真时自车跟踪规划的速度,最高规划速度为15km/h。
对于本领域技术人员而言,显然本发明不限于上述示范性实施例的细节,而且在不背离本发明的精神或基本特征的情况下,能够以其他的具体形式实现本发明。因此,无论从哪一点来看,均应将实施例看作是示范性的,而且是非限制性的,本发明的范围由所附权利要求而不是上述说明限定,因此旨在将落在权利要求的等同要件的含义和范围内的所有变化囊括在本发明内,不应将权利要求中的任何附图标记视为限制所涉及的权利要求。
Claims (5)
1.一种运动障碍物的三元素自主避障方法,包括以下步骤,其特征在于:所述(1)首先通过GNSS设备采集期望路径的waypoint点,并在期望路径上构建Frenet坐标系;(2)通过Frenet坐标系在局部环境下划分自适应栅格点,有利于确定自车在局部环境下的具***置,然后引入速度障碍物的概念,障碍物的姿态信息通过激光雷达测出;(3)通过对障碍物进行圆形近似提出三元素的动态避障方法;(4)接着构建各种约束的代价函数,根据权重值确定最终规划的路径点[1];(5)最后进行仿真验证。
2.根据权利要求1所述的一种运动障碍物的三元素自主避障方法,其特征在于:所述步骤(1)局部栅格的划分,进行局部避障时先对局部道路环境进行栅格划分,首先引入Frenet坐标系,附图12是车辆在大地坐标系与Frenet坐标系下的位置,该图形象描述了自车在两个坐标系之间的关系,自车在大地坐标系下的坐标是K(x2,y2),图中虚线是期望路径的参考线,F点为自车坐标点位置在参考线上的投影点,F点在全局坐标系下的坐标为F(x1,y1),根据计算可以得到F点在期望路径上的Frenet坐标(s,d),其中s为参考线上起点位置到F点的路径长,通过累加计算得到,d是F点到自车的横向距离;最终构建式中Tref为参考线;然后在Frenet坐标系下进行栅格的划分,划分方法如下:
shange_a=-3.75:0.25:3.75
Shange_a和shange_b分别是Frenet坐标系中的S、L,但是S值需要加上自车当前位置的路径长度s(n_out),R_lj是道路曲率半径;道路曲率半径可以通过三点法计算,自车在期望路径上的投影点为中间点,选取此投影点前后各5m的位置有两个点,通过这三个点就可以确定道路曲率半径;
计算公式如下,已知三个点的坐标分别是,圆的方程一般可以写成二次多项式:
A(x2+y2)+Bx+Cy+D=0
将上式进行变形得到圆的标准方程:
再将三个坐标点带入到A(x2+y2)+Bx+Cy+D=0式中得到齐次线性方程组:
根据自适应栅格划分原则,在道路上进行栅格点的划分,通过自适应栅格画法,在道路曲率较小时,划分的前向距离较大,当经过大区率弯道时划分的前向距离较小。
3.根据权利要求1所述的一种运动障碍物的三元素自主避障方法,其特征在于:所述步骤(2)建立三元素避障模型,附图4中自车为A所在的位置,障碍车为B所在位置,自车速度为Vh,障碍车的速度为航向角为将自车的速度映射到障碍车上;同时把自车A变成一个点,得到自车与障碍车合成的相对速度vc;其方向是过B点沿BC的方向,然后过自车A点作AD⊥BC交点为E点,构成ΔABE;BE线段的长为l,AE线段的长为W,然后建立避障的几何数学模型,得到如下公式:
当fg(w,l,vc)=0时,根据模型得出w的值越大自车与障碍车发生碰撞的概率就越小,但当l,vc的值接近于0时,威胁度也比较大;随着时间的推移,自车还是会和障碍车相撞;w是里面最关键的一个参数,当碰撞时间因子l/vc→0m时,此时vc的改变对w的影响非常大,最终得到速度障碍车的避障模型评价参数,w,l,vc三个参数数值的大小对碰撞其关键作用;然后建立优化后的多障碍物避障模型,优化后的避障模型附图5所示,图中A是自车,B、C是两个障碍车辆,根据碰撞锥模型,将三个车辆的外形轮廓形状近似成圆形,其圆形的半径根据传感器测得障碍物车的外形轮廓后得到,图中A、B、C三个车辆的轮廓最开始都是图中的实线圆,自车A的速度与航向角分别是障碍车B的位置和速度分别是v0(x0,y0)、障碍车C的位置和速度分别是v1(x1,y1)、根据避障模型,考虑到自车与障碍车的外形轮廓,障碍车的最外圈虚线圆就是考虑到自车A的外形半径r所形成新的无碰撞外形轮廓;然后将自车A的速度分别映射到障碍车B、C上,分别得到B、C相对于A的合成速度在合成速度方向指引延长线上,画出两个障碍车对应速度方向上的外形轮廓线a、b和c、d;该轮廓线同时也是两虚线圆的切线;靠近自车的那一边,外形轮廓的两个切点分别是f、g,通过自车质心分别向映射后的两个障碍车外形轮廓做垂线,得到垂足点m、n;此时得到mn、ng、mf三条线段,线段mn构成的长度是三元素模型中的w值,线段ng、mf的长度就是l值;最终得到w、l、vc三个参数,这三个参数是评价自车与障碍车之间发生碰撞的关键,这样就将障碍车的速度考虑进来,且可以进行多运动障碍物的避障,当有多个障碍车时,对应就有多组的w、l、vc参数;下面对威胁度权重函数进行设计,根据避障模型,对自车产生威胁的元素主要是w、l、vc,构建如下的代价函数:
式中k1/k2/k3是几何三元素w、l、vc对应的三个权重值,其中对于NA个障碍物进行了如下的定义:
4.根据权利要求1所述的一种运动障碍物的三元素自主避障方法,其特征在于:所述步骤(3)设计基于MPC的代价函数,为了从空间中解决自动驾驶车辆最优行驶路线,需要重新定义一个评价路径优劣的准则,也就是代价函数;在自动驾驶中,路径决策的代价函数主要体现在行车的安全性、舒适性、高效性上;其中安全性是最重要的一个环节,它反映了车辆与障碍物之间的潜在危险,这是顺利通行的保证;舒适性体现在车辆横纵向速度的变化幅度,以及连续两个时间段的前轮转角的变化情况,高效性体现在车辆通行效率上,代价函数的设计如下所示:
1)左侧优先变道代价函数的设计
根据道路法和行车准则,中国道路上的车辆都是靠右行驶,当自车需要超车时,一般情况下需要向左变道,于是设计如下的权重函数:
k=0,1…N-1,klp为权重系数;SL是车道偏移量,是自车到道路边界的宽度。
2)逐次变道代价函数
由于上面的约束条件引入了不是连续变化的布尔变量作为权重函数,此种约束条件会导致最后的目标函数为非凸的情况,非凸的约束存在局部最优解不是全局最优解现象,需要对此进行如下的变形:
上式中kc为权重系数,ML是实验所在车道宽度;
3)侧向加速度代价函数
4)方向盘转角变化率代价函数
在自动驾驶汽车局部路径规划时,为了使跟踪的轨迹平滑,需要考虑方向盘转角变化率,防止程序在计算时,出现方向盘转角变化率过大导致车辆方向盘剧烈抖动失控的现象,仿真车方向盘与前轮转角的传动比为15.8,得到如下公式:
fcosSW(t+k)=kSW·15.8·[δf(t+k+1)-δf(t+k)]
其中kSW为权重系数,δf为前轮转角;
5)基于几何三元素的威胁度权重函数
根据推导的基于几何三元素的动态避障方法,得到动态避障的理论公式:
此代价函数可以解决多运动障碍物的避障情况,自车与障碍车越靠近时对应的栅格点的权重越大,所付出的代价越大;
6)车道对中代价函数
车道对中代价函数也叫路径偏移代价函数,在车辆正常行驶时,需要符合行驶规则,自车应该行驶在每个车道的最中间,所以在道路中间对应的点的权重应该小,而道路边缘的点所对应的权重应该较大;
7)安全度的代价函数
安全度是衡量有障碍物情况下的一个权重函数,与几何三元素的威胁度类似,需要提前知道自车与障碍车的速度,通过评估车辆的制动性能与制动距离,得到安全车距公式:
fs(t+k)=2.2VA、B(t+k)+D
VA、B=vA-vB表示自车与障碍车之间的相对速度,D根据经验一般取6m,表示当自车与障碍车停止时之间的安全距离,在实际行车时存在一个实际车距,然后得到如下公式:
上式是安全度代价函数公式,此代价函数只用在存在一个障碍车辆下;
8)前视点权重函数
将前视点权重函数加入到整个栅格点选取的评价中,越靠近此前视点的栅格点,代价越小,得到如下公式:
(xs,ys),(xq,yq)分别表示栅格点和前视点的坐标;
最终得到总的目标代价函数公式,该代价函数的设计都是基于Frenet坐标系,这样有效减少了无效栅格点的搜索时间,提高了算法的实时性,同时能准确定位自车在局部环境的具***置,可以有效的进行路径跟踪;
fcostfunction(i)=fcostp(t+k)+fcostc(t+k)′+fcosay(t+k)+fcosSW(t+k)+fd(t+k)+fz(t+k)+fan(t+k)+fqian(t+k)
得到总的权重函数之后通过min函数求出每个循环时刻权重最小的栅格点:
为了安全性加上硬约束:前轮极限转角约束,道路边界约束[2]:
-38°≤Steer≤38°
5.根据权利要求1所述的一种运动障碍物的三元素自主避障方法,其特征在于:所述步骤(4)动态避障仿真实验:为了验证三元素方法的动态避障效果,进行了联合仿真;采用Uturn路线,同时在道路上设置一个运动障碍物,从距离起点25米处逆向以10km/h的速度往起点运动,自车从起点向终点按照曲率规划的速度进行运动;附图6是自车的行驶路线图,在程序里面设置了障碍物的运动轨迹之后,自车会根据三元素的避障算法,进行转向避障处理,在避开障碍物之后自车会迅速跟踪上期望路径;附图7是避障时车辆的航向角变化曲线,根据图示,5s之后,由于自车进行转向操作,所以航向角会有一个小范围的变化,如附图7中的第一个小的波峰所示;后面的一个凸字形波峰是过S弯的时自车两个极限转角对应的航向角变化曲线;自车先进行极限的左转,之后又进行右转,最后航向角回到0°;附图8是自车方向盘转角变化图,一开始自车就进行了避障处理,所以一开始自车的转角就从0°变化到120°左右,直线行驶又变为0°左右;然后在行驶了5s后进入第一个极限弯道,方向盘最高转到+310°,又经过15s的直线运动,进入第二个弯道,此时方向盘转角最大为-295°,最后进入直道方向盘转角又回到0°;将自车航向角与方向盘转角随时间的变化曲线画在一起就是附图9;附图10是整个程序的循环耗时,最高耗时为25.5ms最低9.5ms,总体的平均耗时13.5ms;附图11为仿真时自车跟踪规划的速度,最高规划速度为15km/h。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202210470261.4A CN114889643A (zh) | 2022-04-28 | 2022-04-28 | 一种运动障碍物的三元素自主避障方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202210470261.4A CN114889643A (zh) | 2022-04-28 | 2022-04-28 | 一种运动障碍物的三元素自主避障方法 |
Publications (1)
Publication Number | Publication Date |
---|---|
CN114889643A true CN114889643A (zh) | 2022-08-12 |
Family
ID=82719219
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202210470261.4A Pending CN114889643A (zh) | 2022-04-28 | 2022-04-28 | 一种运动障碍物的三元素自主避障方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN114889643A (zh) |
Cited By (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN116045991A (zh) * | 2023-01-05 | 2023-05-02 | 山东港口渤海湾港集团有限公司 | 一种车辆的外轮廓无碰撞路径规划方法及装置 |
-
2022
- 2022-04-28 CN CN202210470261.4A patent/CN114889643A/zh active Pending
Cited By (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN116045991A (zh) * | 2023-01-05 | 2023-05-02 | 山东港口渤海湾港集团有限公司 | 一种车辆的外轮廓无碰撞路径规划方法及装置 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
US11390300B2 (en) | Method for using lateral motion to optimize trajectories for autonomous vehicles | |
CN107702716B (zh) | 一种无人驾驶路径规划方法、***和装置 | |
CN109032131B (zh) | 一种应用于无人驾驶汽车的动态超车避障方法 | |
CN109976329B (zh) | 一种车辆避障换道路径的规划方法 | |
Chen et al. | Lane change path planning based on piecewise bezier curve for autonomous vehicle | |
CN110379203B (zh) | 一种行车转向碰撞预警方法 | |
US7522091B2 (en) | Road curvature estimation system | |
CN102591332B (zh) | 用于无人驾驶汽车局部路径规划的装置及方法 | |
CN112193244B (zh) | 基于线性约束的自动驾驶车辆运动规划方法 | |
CN109270933A (zh) | 基于圆锥曲线的无人驾驶避障方法、装置、设备及介质 | |
CN111830979A (zh) | 一种轨迹优化方法和装置 | |
CN104881025A (zh) | 一种地下矿用车辆的反应式导航控制方法 | |
CN112577506B (zh) | 一种自动驾驶局部路径规划方法和*** | |
CN113619603B (zh) | 一种双阶段自动驾驶车辆调头轨迹规划方法 | |
Said et al. | Local trajectory planning for autonomous vehicle with static and dynamic obstacles avoidance | |
CN112937606A (zh) | 用于循迹自动驾驶车辆的防撞路径规划与控制方法及*** | |
CN116331264A (zh) | 一种未知障碍物分布的避障路径鲁棒规划方法及*** | |
Silva et al. | Path planning at roundabouts using piecewise linear continuous curvature curves | |
CN115489548A (zh) | 智能汽车园区道路路径规划方法 | |
CN114889643A (zh) | 一种运动障碍物的三元素自主避障方法 | |
CN116465427B (zh) | 一种基于时空风险量化的智能车辆换道避障路径规划方法 | |
CN110502004A (zh) | 一种面向智能车辆激光雷达数据处理的行驶区域重要性权值分布建模方法 | |
Hu et al. | Decision-making system based on finite state machine for low-speed autonomous vehicles in the park | |
Zhu et al. | Trajectory planning algorithm using Gauss pseudo-spectral method based on vehicle-infrastructure cooperative system | |
CN117916682A (zh) | 使用时空凸形走廊的运动规划 |
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 |