CN113386766B - 一种连续的、周期自适应同步的在线轨迹规划系统及方法 - Google Patents
一种连续的、周期自适应同步的在线轨迹规划系统及方法 Download PDFInfo
- Publication number
- CN113386766B CN113386766B CN202110671912.1A CN202110671912A CN113386766B CN 113386766 B CN113386766 B CN 113386766B CN 202110671912 A CN202110671912 A CN 202110671912A CN 113386766 B CN113386766 B CN 113386766B
- Authority
- CN
- China
- Prior art keywords
- track
- information
- vehicle
- planning
- current
- 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.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 34
- 230000001360 synchronised effect Effects 0.000 title claims abstract description 13
- 230000000737 periodic effect Effects 0.000 title claims abstract description 10
- 238000001514 detection method Methods 0.000 claims abstract description 13
- 230000001133 acceleration Effects 0.000 claims description 20
- 238000006073 displacement reaction Methods 0.000 claims description 20
- 238000005070 sampling Methods 0.000 claims description 11
- 230000004888 barrier function Effects 0.000 claims description 8
- 230000036461 convulsion Effects 0.000 claims description 6
- 230000002194 synthesizing effect Effects 0.000 claims description 5
- 230000003044 adaptive effect Effects 0.000 claims description 4
- 239000011159 matrix material Substances 0.000 claims description 3
- 230000009466 transformation Effects 0.000 claims description 3
- 230000008859 change Effects 0.000 description 26
- 230000008569 process Effects 0.000 description 12
- 238000013461 design Methods 0.000 description 3
- 230000000694 effects Effects 0.000 description 2
- 238000001914 filtration Methods 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 238000010586 diagram Methods 0.000 description 1
- 230000007613 environmental effect Effects 0.000 description 1
- 238000011156 evaluation Methods 0.000 description 1
- 230000007246 mechanism Effects 0.000 description 1
- 230000035772 mutation Effects 0.000 description 1
- 230000004044 response Effects 0.000 description 1
- 238000012216 screening Methods 0.000 description 1
- 238000012163 sequencing technique Methods 0.000 description 1
- 230000003068 static 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
- 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/18—Propelling the vehicle
- B60W30/18009—Propelling the vehicle related to particular drive situations
- B60W30/18163—Lane change; Overtaking manoeuvres
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B60—VEHICLES IN GENERAL
- B60R—VEHICLES, VEHICLE FITTINGS, OR VEHICLE PARTS, NOT OTHERWISE PROVIDED FOR
- B60R16/00—Electric or fluid circuits specially adapted for vehicles and not otherwise provided for; Arrangement of elements of electric or fluid circuits specially adapted for vehicles and not otherwise provided for
- B60R16/02—Electric or fluid circuits specially adapted for vehicles and not otherwise provided for; Arrangement of elements of electric or fluid circuits specially adapted for vehicles and not otherwise provided for electric constitutive elements
- B60R16/023—Electric or fluid circuits specially adapted for vehicles and not otherwise provided for; Arrangement of elements of electric or fluid circuits specially adapted for vehicles and not otherwise provided for electric constitutive elements for transmission of signals between vehicle parts or subsystems
- B60R16/0231—Circuits relating to the driving or the functioning of the vehicle
-
- 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
- B60W30/0953—Predicting travel path or likelihood of collision the prediction being responsive to vehicle dynamic parameters
-
- 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
- B60W30/0956—Predicting travel path or likelihood of collision the prediction being responsive to traffic or environmental parameters
-
- 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
- B60W2520/00—Input parameters relating to overall vehicle dynamics
- B60W2520/10—Longitudinal speed
-
- 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
- B60W2520/00—Input parameters relating to overall vehicle dynamics
- B60W2520/10—Longitudinal speed
- B60W2520/105—Longitudinal acceleration
-
- 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
- B60W2552/00—Input parameters relating to infrastructure
- B60W2552/50—Barriers
-
- 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
- B60W2552/00—Input parameters relating to infrastructure
- B60W2552/53—Road markings, e.g. lane marker or crosswalk
-
- 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)
- Mechanical Engineering (AREA)
- Transportation (AREA)
- Traffic Control Systems (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
本发明公开了一种连续的、周期自适应同步的在线轨迹规划方法,该方法包括以下步骤:1)确定车辆当前位置、轨迹终点信息和参考线信息;2)根据车辆当前位置在历史轨迹上一定范围内寻找匹配点,确定规划轨迹的初始点信息、终点信息;3)生成车辆的规划轨迹;4)每生成一条规划轨迹,对其进行代价函数值计算,若小于现有最小代价值,则保留该规划轨迹;5)将规划轨迹与历史轨迹进行拼接,得到完整轨迹规划时长的轨迹;6)对筛选出的完整轨迹规划时长的轨迹进行碰撞检测;7)对完整轨迹规划时长的轨迹基于车辆自行车模型进行坐标转换,以补偿周期滞后。本发明能有效解决由于规划周期较长而引起的轨迹滞后问题。
Description
技术领域
本发明涉及智能驾驶辅助技术,尤其涉及一种连续的、周期自适应同步的在线轨迹规划系统及方法。
背景技术
在高速公路驾驶场景下,道路形状较确定、车辆速度大、环境动态性强,留给智能驾驶辅助系统的响应时间较短。在这种情况下,要求局部轨迹规划能产生稳定、平滑的轨迹,使得运动跟踪模块能向执行机构发出平缓的控制指令,保证行驶过程中的安全性、舒适性。
发明内容
本发明要解决的技术问题在于针对现有技术中的缺陷,提供一种连续的、周期自适应同步的在线轨迹规划系统及方法。
本发明解决其技术问题所采用的技术方案是:一种连续的、周期自适应同步的在线轨迹规划方法,包括以下步骤:
1)根据车身CAN提供的车辆当前信息,感知模块提供的车道线信息和障碍物信息,接收决策模块发出的当前指令和历史指令,确定车辆当前位置、轨迹终点信息和参考线信息;
所述车辆当前信息包括车辆当前的速度和加速度;
2)根据车辆当前位置在历史轨迹上一定范围内寻找匹配点,根据轨迹重叠时间Tview确定规划轨迹的初始点信息、以及该轨迹重叠时间Tview内规划轨迹的终点信息;
3)根据步骤2)中的轨迹初始点信息和终点信息,生成车辆的规划轨迹;所述规划轨迹通过纵向一维轨迹集合和横向采样轨迹集合合成转化获得;
4)每生成一条规划轨迹,对其进行代价函数值计算,若其小于现有最小代价值,则保留该规划轨迹;
5)若规划轨迹通过碰撞检测,则将轨迹与历史轨迹进行拼接,得到完整轨迹规划时长的轨迹;
6)对筛选出的完整轨迹规划时长的轨迹进行碰撞检测,检测其是否能避开障碍物以及满足设计的各项指标要求;
7)对完整轨迹基于车辆自行车模型进行坐标转换,以补偿周期滞后;将轨迹点序列和多项式系数信息发布给下游运动跟踪模块。
按上述方案,所述步骤1)中参考线信息为目标车道的中心线信息按照一定纵向平行位移距离Δs,生成的参考线点序列{P1、P2、P3.....Pn},参考线点的信息包括Frenet坐标下纵向位移、车体坐标系下坐标,航向角、曲率、曲率微分。
按上述方案,所述步骤1)中轨迹终点信息获取如下:
根据障碍物信息和决策模块发出的当前指令和历史指令确定目标车道中心线;
其中,vcurrent为车辆当前的速度,a为车辆当前的加速度,Ttarget为轨迹规划时长,终点横向偏差设置为lTarget±lBound范围内,为预设值。
按上述方案,所述步骤2)中根据车辆当前位置在历史轨迹上一定范围内寻找匹配点,具体如下:
历史轨迹是点序列Traji,i=1,2,3…n;
即{Traj1、Traj2、Traj3、……、Trajn},第i个点的信息包括横向偏差、横向偏差一阶微分、纵向位移、速度、加速度信息;
依据车辆当前的Frenet坐标系中位置信息(SV,LV),与参考线序列点上的Si依次比较,当搜索到第一个SV>Si时,取index=i+1,如果车辆当前位置至历史轨迹点序列Trajindex的距离小于阈值distmax时,搜索结束,轨迹匹配点的下标为index,记为(Sindex,Lindex)。
按上述方案,所述步骤3)中生成车辆的规划轨迹,具体如下:
依据设定的间隔时间TimeInterval和轨迹时长Ttarget,采用五次多项式确定纵向一维轨迹S(t);
根据当次纵向轨迹S(t),采用五次多项式形式生成横向轨迹l(s);
通过纵向一维轨迹集合和横向采样轨迹集合合成转化获得车辆的规划轨迹。
间隔时间TimeInterval为规划程序运行的时间周期,根据系统架构和功能需要来决定。
按上述方案,所述步骤4)中代价函数为多目标代价函数:
J=W1*Goffset+W2*Gkappa+W3*Gdkappa+W4*GLength+W5*Gspeed+W6*GAcce+W7*GLongJerk+W8*GObj
上式中,Goffset为横向位移偏差代价函数,Gkappa为曲率代价函数,Gdkappa为曲率变化率代价函数,GLength为纵向位移长度代价函数,Gspeed为速度偏差代价函数,GAcce为纵向加速度代价函数,GLongJerk为纵向加加速度代价函数,GObj为障碍物代价函数,其中[W1,W2,W3,W4,W5,W6,W7,W8]为以上各代价函数对应的权重系数矩阵。
按上述方案,所述步骤7)中具体如下:
7.1)收集当前周期的参考线信息、规划轨迹点序列、车辆当前速度、横摆角速度信息;
7.2)将基于Frenet坐标系的规划轨迹转换为车体坐标系下的规划轨迹;
7.3)根据当前车辆信息,对车体坐标系进行一个周期的旋转,并在新的车体坐标系下更新规划轨迹和参考线点序列;
7.4)将车体系下的规划轨迹转换到Frenet坐标系下;
7.5)将完整轨迹规划时长的轨迹信息发布给跟踪控制模块,包括轨迹点序列信息、轨迹纵向多项式系数、横向多项式系数;
7.6)运动跟踪模块根据7.5)中发布的多项式系数,结合自身模块的控制周期进行轨迹生成,生成轨迹如下式所示,能避免规划模块、运动跟踪模块周期不同步引起的指令跳变;
S(t)=a0+a1*t+a2*t2+a3*t3+a4*t4+a5*t5;
l(s)=b0+b1*s+b2*s2+b3*s3+b4*s4+b5*s5;
其中,[a0,a1,a2,a3,a4,a5]为轨迹纵向多项式系数,[b0,b1,b2,b3,b4,b5]为横向多项式系数,s为一条完整轨迹中的相对纵向位移,t为一条完整轨迹中的相对运动时间。
本发明产生的有益效果是:
1、本发明方法通过当前轨迹与历史轨迹的匹配、拼接,通过运动跟踪模块控制周期要求生成连续轨迹,以上两种机制消除了轨迹状态发生大的转变时的轨迹抖动,以及不同模块周期引起指令突变,改善了轨迹运动跟踪性能。
2、本发明方法的轨迹补偿方式,能有效解决由于规划周期较长而引起的轨迹滞后问题。
3、本发明方法采用的轨迹评估函数中代价值排序方式,能减少计算量;
4、本发明方法基于运动跟踪控制周期生成连续轨迹的方法,细化每个控制周期内的目标轨迹,能改善轨迹运动跟踪性能。
附图说明
下面将结合附图及实施例对本发明作进一步说明,附图中:
图1是本发明实施例的方法流程图;
图2是本发明实施例的在线轨迹规划效果示意图。
具体实施方式
为了使本发明的目的、技术方案及优点更加清楚明白,以下结合实施例,对本发明进行进一步详细说明。应当理解,此处所描述的具体实施例仅用以解释本发明,并不用于限定本发明。
如图1所示,一种连续的、周期自适应同步的在线轨迹规划方法,包括以下步骤:
1)根据车身CAN提供的车辆当前信息,感知模块提供的车道线信息和障碍物信息,接收决策模块发出的当前指令和历史指令,确定车辆当前位置、轨迹终点信息和参考线信息;
所述车辆当前信息包括车辆当前的速度和加速度;
2)根据车辆当前位置在历史轨迹上一定范围内寻找匹配点,根据轨迹重叠时间Tview确定剩余轨迹的初始点信息、以及该轨迹重叠时间周期内的终点信息;
3)根据步骤2)中的轨迹初始点信息和终点信息,生成车辆的规划轨迹;所述规划轨迹通过纵向一维轨迹集合和横向采样轨迹集合合成转化获得;
4)每生成一条规划轨迹,对其进行代价函数值计算,若其小于现有最小代价值,则保留该规划轨迹;
5)若规划轨迹通过碰撞检测,则将轨迹与历史轨迹进行拼接,得到完整轨迹规划时长的轨迹;
6)对筛选出的完整轨迹规划时长的轨迹进行碰撞检测,检测其是否能避开障碍物以及满足设计的各项指标要求;指标为安全性、舒适性相关指标,包括最大加速度、最大加加速度、最大曲率、最大曲率微分;
7)对完整轨迹基于车辆自行车模型进行坐标转换,以补偿周期滞后;将轨迹点序列和多项式系数信息发布给下游运动跟踪模块。
实施例1:高速公路变道行驶
在高速公路变道行驶场景,根据车辆当前位姿,实时计算出车辆行驶轨迹,生成的轨迹需要满足安全性、舒适性相关指标,给跟踪控制模块合适的跟踪轨迹,控制模块据此计算出合理的横向、纵向控制指令,操纵车辆快速、平滑地完成整个变道过程,实施效果如图2所示,具体步骤如下:
1)决策模块发出变道指令和历史指令,车身CAN提供车辆当前信息,感知模块提供车道线信息、障碍物信息,规划模块对以上信息分析处理,确定车辆当前位置、轨迹终点信息和参考线信息。
1.1)根据感知模块提供的车道线系数计算参考线在车体坐标系中的坐标:
Y=C0+C1*X+C2*X2+C3*X3,X∈[0,Xrange];
方向角θ=atan(C1+2*C2*X+3*C3*X2),θ∈(-π/2,π/2);
曲率计算ρ=2*C2+6*C3*X;
障碍物信息,包括障碍物在车体坐标系中的位置、速度、加速度信息;采集车身CAN提供的车辆当前速度vcurrent、横摆角速度ω等。
1.2)决策模块根据1.1)中信息,筛选出关键障碍物,如果满足变道条件,发出变道指令、目标车道中心线(左侧车道或右侧车道)与上一周期历史指令(居中行驶/变道过程中/变道返回等状态);
1.3)轨迹规划模块根据1.2)中变道指令信息,若目标车道为左侧超车道,则以al做匀加速运动,若目标车道为右侧车道,则以ar做匀加速运动,终点速度为:vTerm=vcurrent+a*Ttarget,a=al,ar,vTerminal≤vmax;终点轨迹长度终点横向偏差设置为lTarget±lBound范围内,以Δl为采样间隔的数组。
1.4)将变道目标车道中心线作为参考轨迹,由1.1)中感知输出的车道线信息,按照一定纵向位移距离Δs,生成参考线点序列{P1、P2、P3.....Pi},式中,i=1,2,3…n,点上信息包括Frenet坐标下纵向位移、车体坐标系下X,Y坐标,航向角、曲率、曲率微分。
Frenet坐标系:使用道路的中心线作为参考线,使用参考线的切线向量和法线向量建立坐标系,纵向S,沿着参考线的距离,横向l,沿参考线的法向。
2)在历史轨迹上一定范围内寻找匹配点,依据运动跟踪模块提供的轨迹重叠时间Tview确定剩余轨迹的初始点、终点状态信息;若没找到匹配点,或根据1.2)中历史指令信息,上一周期为居中行驶,则以车辆当前位置为起点,重新规划。
2.1)历史轨迹是点序列{Traj1、Traj2、Traj3.....Traji},式中i=1,2,3…n,第i个点的信息包括横向偏差、横向偏差一阶微分、纵向位移、速度、加速度等信息;
2.2)依据车辆当前的Frenet坐标系中位置信息{SV,LV},与参考线序列点上的Si依次比较,当搜索到第一个SV>Si时,取index=i+1,如果车辆当前位置至第index点的距离小于阈值distmax时,搜索结束,轨迹初始点的下标为index。
3)纵向一维轨迹规划+横向采样生成规划轨迹。依据设定的间隔时间TimeInterval和轨迹时长Ttarget,采用五次多项式确定纵向一维轨迹S(t);根据当次纵向轨迹S(t),采用五次多项式形式生成横向轨迹l(s)。
4)每生成一条曲线,对其进行代价函数值计算,并于现有最小代价值进行比较,保留代价值小的轨迹。
4.1)变道过程的规划轨迹要满足舒适性需求,这些指标通过代价函数的形式体现,考虑横向偏差、曲率、曲率导数、纵向距离、纵向速度、加速度、加加速度、静态障碍物因素,多目标代价函数如下:
J=W1*Goffset+W2*Gkappa+W3*Gdkappa+W4*GLength+W5*Gspeed+W6*GAcce+W7*GLongJerk+W8*GObj
上式中,Goffset为横向位移偏差代价函数,Gkappa为曲率代价函数,Gdkappa为曲率变化率代价函数,GLength为纵向位移长度代价函数,Gspeed为速度偏差代价函数,GAcce为纵向加速度代价函数,GLongJerk为纵向加加速度代价函数,GObj为障碍物代价函数,具体表达方式如下表所示,其中[W1,W2,W3,W4,W5,W6,W7,W8]为以上各代价函数对应权重系数矩阵。
表1轨迹评估因素
4.2)计算当前生成轨迹的代价值Jtraj,如果Jtraj≤Jmin,则将当前轨迹信息拷贝到最优轨迹数组,否则,滤除当前轨迹。
5)如果规划轨迹通过碰撞检测,则将轨迹与4-b)中截取的历史段进行拼接,得到完整的Ttarget时长的轨迹。
6)对筛选出的最优轨迹进行碰撞检测,看其是否能避开障碍物以及满足设计的各项指标要求。
6.1)对筛选出的最优轨迹,依次遍历关键障碍物,计算障碍物至轨迹的距离,若最小距离小于设计阈值,则碰撞检测失败,给决策模块发出规划失败指令;若最小距离大于设计阈值,则碰撞检测成功;
6.2)对6.1)中通过碰撞检测的轨迹,检查最大加速度、最大加加速度、最大曲率、最大曲率微分均小于各指标阈值,则轨迹满足设计指标。
7)对完整轨迹进行滞后补偿,发布给下游运动跟踪模块
7.1)收集当前周期的参考线信息、规划轨迹点序列、车辆当前速度、横摆角速度等信息,并分析;
7.2)将基于Frenet坐标系的规划轨迹转换到车体坐标系下面;
7.3)依据当前车辆信息,对车体坐标系进行一个周期的旋转,并在新的车体坐标系下更新规划轨迹和参考线点序列;
7.4)将车体系下的规划轨迹转换到Frenet坐标系下;
7.5)将最优轨迹信息发布给跟踪控制模块,包括轨迹点序列信息轨迹纵向多项式系数Coffelon=[a0,a1,a2,a3,a4,a5]、横向多项式系数Coffelat=[b0,b1,b2,b3,b4,b5]。
7.6)运动跟踪模块根据7.5)中发布的多项式系数,结合自身模块的控制周期进行轨迹生成,如下式所示,能避免规划模块、运动跟踪模块周期不同步引起的指令跳变。
S(t)=a0+a1*t+a2*t2+a3*t3+a4*t4+a5*t5;
l(s)=b0+b1*s+b2*s2+b3*s3+b4*s4+b5*s5;
实施例2:高速公路变道返回场景
在高速公路变道过程中,若系统检测到风险,决策模块给出变道返回指令,局部轨迹规划模块根据车辆当前位姿、车道线、障碍物信息等实时计算出车辆变道返回行驶轨迹,生成的轨迹需要满足安全性、舒适性相关指标,给跟踪控制模块合适的跟踪轨迹,控制模块据此计算出合理的横向、纵向控制指令,操纵车辆快速、平滑地完成整个变道返回过程,具体步骤如下:
1.决策模块发出变道返回指令和历史指令(变道过程中/变道返回中),车身CAN提供车辆当前信息,感知模块提供车道线信息、障碍物信息,规划模块对以上信息分析处理,确定车辆当前位置、轨迹终点信息和参考线信息。
1.1)感知模块和车身CAN提供的信息处理同实施例1;
1.2)决策模块根据1.1)中信息,筛选出关键障碍物,如果不满足变道,但满足返回条件,则发出变道返回指令、目标车道中心线(左侧车道或右侧车道)与上一周期历史指令(变道过程中/变道返回中)。
1.3)轨迹规划模块根据1.2)中变道返回指令信息,若目标车道为左侧超车道,则以a′l做匀加速运动,若目标车道为右侧车道,则以a′r做匀加速运动,终点速度为:vTerm=vcurrent+a*T′targert,a=a′l,a′r,vTerminal≤vmax;终点轨迹长度终点横向偏差设置为lTarget±lBound范围内,以Δl为采样间隔的数组。
1.4)将变道返回目标车道中心线作为参考轨迹,由1.1)中感知输出的车道线信息,按照一定纵向位移距离Δs,生成参考线点序列{P1、P2、P3.....Pi},式中i=1,2,3…n,点上信息包括Frenet坐标下纵向位移、车体坐标系下X,Y坐标,航向角、曲率、曲率微分。
2)在历史轨迹中一定范围内寻找匹配点,依据运动跟踪模块提供的轨迹重叠时间T′view确定剩余轨迹的初始点、终点状态信息;若没找到匹配点,或根据1.2)中历史指令信息,上一周期为变道过程中,则以车辆当前位置为起点,重新规划。
2.1)历史轨迹是点序列{Traj1、Traj2、Traj3…Traji},式中i=1,2,3…n,第i个点的信息包括横向偏差、横向偏差一阶微分、纵向位移、速度、加速度等信息;
2.2)依据车辆当前的Frenet坐标系中位置信息{SV,LV},与参考线序列点上的Si依次比较,当搜索到第一个SV>Si时,取index=i+1,如果车辆当前位置至第index点的距离小于阈值distmax时,搜索结束,轨迹初始点的下标为index。
3)纵向一维轨迹规划+横向采样生成规划轨迹,依据设定的间隔时间TimeInterval和轨迹时长T′target,采用五次多项式确定纵向一维轨迹S(t);根据当次纵向轨迹S(t),采用五次多项式形式生成横向轨迹l(s)。
4)每生成一条曲线,对其进行代价函数值计算,并于现有最小代价值进行比较,保留代价值小的轨迹。
4.1)变道返回过程的多目标代价函数同实施例1,考虑到动态环境更复杂,将与安全性相关的障碍物代价函数GObj的权重系数W8的比值调高,优先保证返回安全性。
4.2)计算当前生成轨迹的代价值Jtraj,如果Jtraj≤J′min则将当前轨迹信息拷贝到最优轨迹数组,否则,滤除当前轨迹。
5)如果规划轨迹通过碰撞检测,则将轨迹与4-b)中截取的历史段进行拼接,得到完整的T′targert,时长的轨迹。
6)对筛选出的最优轨迹进行碰撞检测,看其是否能避开障碍物以及满足设计的各项指标要求,具体步骤同实施例1。
7)对完整轨迹进行滞后补偿,发布给下游运动跟踪模块,滞后补偿及轨迹发布具体信息同实施例1。
应当理解的是,对本领域普通技术人员来说,可以根据上述说明加以改进或变换,而所有这些改进和变换都应属于本发明所附权利要求的保护范围。
Claims (7)
1.一种连续的、周期自适应同步的在线轨迹规划方法,其特征在于,包括以下步骤:
1)根据车身CAN提供的车辆当前信息,感知模块提供的车道线信息和障碍物信息,接收决策模块发出的当前指令和历史指令,确定车辆当前位置、轨迹终点信息和参考线信息;
所述车辆当前信息包括车辆当前的速度和加速度;
2)根据车辆当前位置在历史轨迹上一定范围内寻找匹配点,根据轨迹重叠时间Tview确定规划轨迹的初始点信息、以及该轨迹重叠时间Tview内规划轨迹的终点信息;
3)根据步骤2)中的轨迹初始点信息和终点信息,生成车辆的规划轨迹;所述规划轨迹通过纵向一维轨迹集合和横向采样轨迹集合合成转化获得;
4)每生成一条规划轨迹,对其进行代价函数值计算,若其小于现有最小代价值,则保留该规划轨迹;
5)若规划轨迹通过碰撞检测,则将规划轨迹与历史轨迹进行拼接,得到完整轨迹规划时长的轨迹;
6)对筛选出的完整轨迹规划时长的轨迹进行碰撞检测,检测其是否能避开障碍物;
7)对完整轨迹规划时长的轨迹基于车辆自行车模型进行坐标转换,以补偿周期滞后;将轨迹点序列和多项式系数信息发布给下游运动跟踪模块。
2.根据权利要求1所述的连续的、周期自适应同步的在线轨迹规划方法,其特征在于,所述步骤1)中参考线信息为目标车道的中心线信息按照一定纵向平行位移距离Δs,生成的参考线点序列{P1、P2、P3.....Pn},参考线点的信息包括Frenet坐标下纵向位移、车体坐标系下坐标,航向角、该点处参考线的曲率和曲率微分。
4.根据权利要求1所述的连续的、周期自适应同步的在线轨迹规划方法,其特征在于,所述步骤2)中根据车辆当前位置在历史轨迹上一定范围内寻找匹配点,具体如下:
历史轨迹是点序列Traji,i=1,2,3…n;
即{Traj1、Traj2、Traj3、……、Trajn},第i个点的信息包括横向偏差、横向偏差一阶微分、纵向位移、速度、加速度信息;
依据车辆当前的Frenet坐标系中位置信息(SV,LV),与参考线序列点上的Si依次比较,当搜索到第一个SV>Si时,取index=i+1,如果车辆当前位置至历史轨迹点序列Trajindex的距离小于阈值distmax时,搜索结束,轨迹匹配点的下标为index,记为(Sindex,Lindex)。
5.根据权利要求1所述的连续的、周期自适应同步的在线轨迹规划方法,其特征在于,所述步骤3)中生成车辆的规划轨迹,具体如下:
依据设定的间隔时间TimeInterval和轨迹时长Ttarget,采用五次多项式确定纵向一维轨迹S(t);
根据当次纵向轨迹S(t),采用五次多项式形式生成横向轨迹l(s);
通过纵向一维轨迹集合和横向采样轨迹集合合成转化获得车辆的规划轨迹。
6.根据权利要求1所述的连续的、周期自适应同步的在线轨迹规划方法,其特征在于,所述步骤4)中代价函数为多目标代价函数:
J=W1*Goffset+W2*Gkappa+W3*Gdkappa+W4*GLength+W5*Gspeed+W6*GAcce+W7*GLongJerk+W8*GObj
式中,Goffset为横向位移偏差代价函数,Gkappa为曲率代价函数,Gdkappa为曲率变化率代价函数,GLength为纵向位移长度代价函数,Gspeed为速度偏差代价函数,GAcce为纵向加速度代价函数,GLongJerk为纵向加加速度代价函数,GObj为障碍物代价函数,其中[W1,W2,W3,W4,W5,W6,W7,W8]为以上各代价函数对应的权重系数矩阵。
7.根据权利要求1所述的连续的、周期自适应同步的在线轨迹规划方法,其特征在于,所述步骤7)中具体如下:
7.1)收集当前周期的参考线信息、规划轨迹点序列、车辆当前速度、横摆角速度信息;
7.2)将基于Frenet坐标系的规划轨迹转换为车体坐标系下的规划轨迹;
7.3)根据当前车辆信息,对车体坐标系进行一个周期的旋转,并在新的车体坐标系下更新规划轨迹和参考线点序列;
7.4)将车体坐标系下的规划轨迹转换到Frenet坐标系下;
7.5)将完整轨迹规划时长的轨迹信息发布给跟踪控制模块,包括轨迹点序列信息、轨迹纵向多项式系数、横向多项式系数;
7.6)运动跟踪模块根据7.5)中发布的多项式系数,结合自身模块的控制周期进行轨迹生成,生成轨迹如下式所示,能避免规划模块、运动跟踪模块周期不同步引起的指令跳变;
S(t)=a0+a1*t+a2*t2+a3*t3+a4*t4+a5*t5;
l(s)=b0+b1*s+b2*s2+b3*s3+b4*s4+b5*s5;
其中,[a0,a1,a2,a3,a4,a5]为轨迹纵向多项式系数,[b0,b1,b2,b3,b4,b5]为横向多项式系数,s为一条完整轨迹中的相对纵向位移,t为一条完整轨迹中的相对运动时间。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202110671912.1A CN113386766B (zh) | 2021-06-17 | 2021-06-17 | 一种连续的、周期自适应同步的在线轨迹规划系统及方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202110671912.1A CN113386766B (zh) | 2021-06-17 | 2021-06-17 | 一种连续的、周期自适应同步的在线轨迹规划系统及方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN113386766A CN113386766A (zh) | 2021-09-14 |
CN113386766B true CN113386766B (zh) | 2022-09-23 |
Family
ID=77621602
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202110671912.1A Active CN113386766B (zh) | 2021-06-17 | 2021-06-17 | 一种连续的、周期自适应同步的在线轨迹规划系统及方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN113386766B (zh) |
Families Citing this family (7)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN114291112B (zh) * | 2021-12-30 | 2023-08-04 | 同济大学 | 一种应用于自动驾驶汽车的决策规划协同增强方法 |
CN114578808A (zh) * | 2022-01-10 | 2022-06-03 | 美的集团(上海)有限公司 | 路径规划方法、电子设备、计算机程序产品及存储介质 |
CN115195743B (zh) * | 2022-09-16 | 2022-12-23 | 毫末智行科技有限公司 | 基于无人驾驶的车辆自动变道方法、装置、设备及介质 |
CN115892075B (zh) * | 2023-01-06 | 2023-06-09 | 阿里巴巴达摩院(杭州)科技有限公司 | 轨迹规划方法、自动驾驶设备及计算机存储介质 |
CN116088538B (zh) * | 2023-04-06 | 2023-06-13 | 禾多科技(北京)有限公司 | 车辆轨迹信息生成方法、装置、设备和计算机可读介质 |
CN116572994B (zh) * | 2023-07-10 | 2023-09-22 | 之江实验室 | 一种车辆速度规划方法、装置及计算机可读介质 |
CN117141520B (zh) * | 2023-10-31 | 2024-01-12 | 合肥综合性国家科学中心人工智能研究院(安徽省人工智能实验室) | 一种实时轨迹规划方法、装置和设备 |
Citations (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
WO2020121215A1 (en) * | 2018-12-12 | 2020-06-18 | Marelli Europe S.P.A. | Method and system for planning a path for a land vehicle for obstacle avoidance with fast reaction, particularly in an autonomous drive scenario |
CN111806467A (zh) * | 2020-07-27 | 2020-10-23 | 福州大学 | 一种基于车辆行驶规律的变速动态换道轨迹规划方法 |
CN112327830A (zh) * | 2020-10-19 | 2021-02-05 | 东风汽车集团有限公司 | 车辆自动驾驶变道轨迹的规划方法及电子设备 |
CN112810630A (zh) * | 2021-02-05 | 2021-05-18 | 山东大学 | 一种自动驾驶车辆轨迹规划方法及系统 |
Family Cites Families (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN106114507B (zh) * | 2016-06-21 | 2018-04-03 | 百度在线网络技术(北京)有限公司 | 用于智能车辆的局部轨迹规划方法和装置 |
KR102494364B1 (ko) * | 2017-09-26 | 2023-02-03 | 주식회사 에이치엘클레무브 | 차량 제어 시스템, 차량 외장형 전자 제어 유닛, 차량 제어 방법 및 애플리케이션 |
US11378961B2 (en) * | 2018-04-17 | 2022-07-05 | Baidu Usa Llc | Method for generating prediction trajectories of obstacles for autonomous driving vehicles |
-
2021
- 2021-06-17 CN CN202110671912.1A patent/CN113386766B/zh active Active
Patent Citations (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
WO2020121215A1 (en) * | 2018-12-12 | 2020-06-18 | Marelli Europe S.P.A. | Method and system for planning a path for a land vehicle for obstacle avoidance with fast reaction, particularly in an autonomous drive scenario |
CN111806467A (zh) * | 2020-07-27 | 2020-10-23 | 福州大学 | 一种基于车辆行驶规律的变速动态换道轨迹规划方法 |
CN112327830A (zh) * | 2020-10-19 | 2021-02-05 | 东风汽车集团有限公司 | 车辆自动驾驶变道轨迹的规划方法及电子设备 |
CN112810630A (zh) * | 2021-02-05 | 2021-05-18 | 山东大学 | 一种自动驾驶车辆轨迹规划方法及系统 |
Also Published As
Publication number | Publication date |
---|---|
CN113386766A (zh) | 2021-09-14 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN113386766B (zh) | 一种连续的、周期自适应同步的在线轨迹规划系统及方法 | |
CN110187639B (zh) | 一种基于参数决策框架的轨迹规划控制方法 | |
JP3714258B2 (ja) | 車両用推奨操作量生成装置 | |
CN113291308B (zh) | 一种考虑驾驶行为特性的车辆自学习换道决策系统及方法 | |
CN113204236B (zh) | 一种智能体路径跟踪控制方法 | |
JP6799150B2 (ja) | 車両制御装置 | |
CN109501799B (zh) | 一种车联网条件下的动态路径规划方法 | |
CN110361013B (zh) | 一种用于车辆模型的路径规划系统及方法 | |
Khodayari et al. | A historical review on lateral and longitudinal control of autonomous vehicle motions | |
US8190330B2 (en) | Model based predictive control for automated lane centering/changing control systems | |
CN113276848A (zh) | 一种智能驾驶换道避障轨迹规划、跟踪控制方法及系统 | |
CN110162046A (zh) | 基于事件触发型模型预测控制的无人车路径跟随方法 | |
CN112947469A (zh) | 汽车换道轨迹规划与动态轨迹跟踪控制方法 | |
Xu et al. | A nash Q-learning based motion decision algorithm with considering interaction to traffic participants | |
CN112965476A (zh) | 一种基于多窗口抽样的高速无人车轨迹规划系统及方法 | |
CN108791290A (zh) | 基于在线增量式dhp的双车协同自适应巡航控制方法 | |
CN113515038B (zh) | 一种车辆换道方法、装置、设备及可读存储介质 | |
CN112429004A (zh) | 一种车辆自动换道控制方法 | |
CN111081069A (zh) | 一种高速公路瓶颈区域的车辆轨迹控制方法 | |
CN114715193B (zh) | 一种实时轨迹规划方法及系统 | |
CN110703775B (zh) | 一种基于改进人工势场的商用车车道保持路径规划方法 | |
CN116486356A (zh) | 一种基于自适应学习技术的狭窄场景轨迹生成方法 | |
CN114877904A (zh) | 一种道路曲率的估计方法、测量装置和计算机设备 | |
CN114852085A (zh) | 基于路权侵入度的车辆自动驾驶轨迹规划方法 | |
Yalamanchi et al. | Long-term prediction of vehicle behavior using short-term uncertainty-aware trajectories and high-definition maps |
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 |