CN112498368B - 自动驾驶偏移的横向轨迹规划系统及方法 - Google Patents
自动驾驶偏移的横向轨迹规划系统及方法 Download PDFInfo
- Publication number
- CN112498368B CN112498368B CN202011335081.2A CN202011335081A CN112498368B CN 112498368 B CN112498368 B CN 112498368B CN 202011335081 A CN202011335081 A CN 202011335081A CN 112498368 B CN112498368 B CN 112498368B
- Authority
- CN
- China
- Prior art keywords
- offset
- module
- final
- transverse
- transition time
- 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
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
Landscapes
- Engineering & Computer Science (AREA)
- Automation & Control Theory (AREA)
- Human Computer Interaction (AREA)
- Transportation (AREA)
- Mechanical Engineering (AREA)
- Traffic Control Systems (AREA)
- Control Of Driving Devices And Active Controlling Of Vehicle (AREA)
Abstract
本发明公开的自动驾驶偏移的横向轨迹规划系统及方法,系统包括自动驾驶横向控制状态模块、环境认知模块、本车车速模块、触发偏移模块、最终的偏移过渡时间模块、偏移轨迹规划模块、道路中心线系数模块、横向控制最终轨迹计算模块和横向控制执行模块;通过曲线拟合方式拟合偏移轨迹系数,在道路的中心线系数上叠加偏移轨迹系数,执行重新规划的轨迹,能够实现偏移及时响应。
Description
技术领域
本发明用于自动驾驶领域,具体涉自动驾驶偏移的横向轨迹规划系统及方法。
背景技术
自动驾驶偏移功能通过本车的横向偏移,使本车远离目标行进轨迹上的大型车辆以及 对本车产生压迫的小型车辆,一定程度上实现“防御性驾驶”,增强驾乘人员的驾乘信心 感与安全感。以往相关自动驾驶偏移的横向轨迹规划一般集中在单纯的横向位置插值偏 移响应缓慢以致车速差较大时本车驶过障碍物时尚未偏移到位或者在短时间内频繁偏移 时可能出现的轨迹跳变未进行有效处理,如申请号为CN201811119580.0的专利“基于车道线平行偏移进行避障及超车的动态轨迹规划方法”,利用五次多项式实时对轨迹平行偏移进行插值处理,来实现避障及超车的动态轨迹规划,未涉及航向角及曲率变化率的规划。
因此,有必要开发一种用于自动驾驶偏移的横向轨迹规划系统及方法,快速稳定的偏 移到位。
发明内容
本发明公开的自动驾驶偏移的横向轨迹规划系统,在横向控制激活的状态下,根据 环境认知计算的目标偏移量及期望偏移过渡时间以及本车车速情况将偏移量规划成曲线 叠加在道路中心线上,输出给横向控制模块,实现偏移及时响应。
本发明还公开了自动驾驶偏移的横向轨迹规划方法,在横向控制激活的状态下,根 据环境认知计算的目标偏移量及期望偏移过渡时间以及本车车速情况将偏移量规划成曲 线叠加在道路中心线上,输出给横向控制模块,实现偏移及时响应。
本发明公开的自动驾驶偏移的横向轨迹规划系统,包括自动驾驶横向控制状态模块、 环境认知模块、本车车速模块、触发偏移模块、最终的偏移过渡时间模块、偏移轨迹规划模块、道路中心线系数模块、横向控制最终轨迹计算模块和横向控制执行模块;
所述自动驾驶横向控制状态模块提供当前横向控制的状态信号,并将横向控制的状 态信号传送给触发偏移模块;
所述环境认知模块,根据周围的场景计算出相对于危险物体的横向目标偏移量以及 期望偏移过渡时间;将横向目标偏移量传送给触发偏移模块、偏移轨迹规划模块;将期望偏移过渡时间传送给最终的偏移过渡时间模块;
所述触发偏移模块,接收横向控制的状态信号,环境认知模块的横向目标偏移量,判断是否触发偏移轨迹规划模块启动偏移规划;
所述最终的偏移过渡时间模块,根据期望偏移过渡时间进行限值生成最终偏移过渡 时间并输出给偏移轨迹规划模块;
所述偏移轨迹规划模块,接收触发偏移模块的触发偏移规划信号、最终的偏移过渡 时间模块的最终偏移过渡时间信号、环境认知模块的横向目标偏移量以及期望偏移过渡 时间信号,将横向目标偏移量规划成能够直接叠加至道路中心线上的偏移轨迹系数Final_DeltA0、Final_DeltA1、Final_DeltA2;
所述道路中心线系数模块,提供道路的中心线系数A0、A1、A2、A3;
所述横向控制最终轨迹计算模块,在道路的中心线系数上叠加偏移轨迹系数;
所述横向控制执行模块,执行道路中心线叠加偏移轨迹系数后用于控制跟踪的轨迹。
进一步地,将横向目标偏移量规划成下列二次曲线,并通过曲线插值求的偏移轨迹 系数;y=Final_DeltA0+Final_DeltA1*x+Final_DeltA2*x2;其中,X为相对于本车的纵向 距离,Y为相对于本车的横向距离。
进一步地,曲线插值按如下方式进行:
步骤1)建立插值公式,
DeltA0=c(1)*t3+c(2)*t4+c(3)*t5+DeltOffset+Offset;
DeltA1=(3*c(1)*t2+4*c(2)*t3+5*c(3)*t4)/Speed;
DeltA2=(6*c(1)*t+12*c(2)*t2+20*c(3)*t3)/(2*Speed);
其中:
DeltOffset=Offset-Offsetprev,Offset为横向目标偏移量,offsetprev为上一次插值横向 目标偏移量,Speed为车速;
c=Tmatrix\yout;
t=tlast+dt,触发插值时t为0,tlast为上一次插值时间,dt为每次插值时间计算步长;
当t大于T时或者横向控制退出时,DeltA0等于Offset,DeltA1、DeltA2为0。
步骤2)条件判断,
偏移规划触发时间间隔大于等于上次偏移规划触发时的期望偏移过渡时间,则Fnial_DeltA0等于DeltA0,Fnial_DeltA1等于DeltA1,Fnial_DeltA2等于DeltA2;
偏移规划触发时间间隔小于上次偏移规划触发时的期望偏移过渡时间,则Fnial_DeltA0等于DeltA0;DeltA1、DeltA2分别进行二次插值规划处理:
Final_DeltA1=c(1)*t3+c(2)*t4+c(3)*t5+DeltOffset+DeltA1;
其中,DeltOffset=DeltA1-DeltA1prev,DeltA1prev为上一次插值DeltA1值;
Final_DeltA2=c(1)**t3+c(2)*t4+c(3)*t5+DeltOffset+DeltA2;
其中,DeltOffset=DeltA2-DeltA2prev,DeltA2prev为上一次插值DeltA2值。
进一步地,将中心线规划成下列三次多项式:
y=A0+A1*x+A2*x2+A3*x3;
其中,x为本车坐标系下的纵向距离,y为本车坐标系下的横向距离,A0、A1、A2、 A3为轨迹系数,A0对应于本车坐标原点处的横向位置,A1对应于本车坐标原点处的航 向角,2×A2对应于本车坐标原点处的曲率,6×A3对应于本车坐标原点处的曲率变化 率。
进一步地,横向控制最终轨迹计算模块,在道路的中心线系数上叠加偏移轨迹系数。
进一步地,最终的偏移过渡时间模块根据横向目标偏移量及车速查找标定的能够实 现正常功能的最小时间Tmin标定表以及能保证舒适性的舒适性过渡时间Tcom标定表;
当横向目标偏移量等于0时,最终偏移过渡时间T等于Tcom;否则,当期望偏移过渡时间小于最小时间Tmin时,最终偏移过渡时间T等于最小时间Tmin;否则,最终偏 移过渡时间T等于期望偏移过渡时间。
本发明还公开了自动驾驶偏移的横向轨迹规划方法,涉及自动驾驶横向控制状态模 块、环境认知模块、本车车速模块、触发偏移模块、最终的偏移过渡时间模块、偏移轨 迹规划模块、道路中心线系数模块、横向控制最终轨迹计算模块和横向控制执行模块;
按如下步骤工作:
步骤1)环境认知模块根据当前目标车等障碍物横向距离,相对车速及道路边界情况 规划出能够安全驶过的横向目标偏移量Offset,以及期望偏移过渡时间Texp;
步骤2)当车辆横向控制处于激活状态,且横向目标偏移量Offset存在变化时触发偏 移模块置1,进入步骤3);
步骤3)最终的偏移过渡时间模块根据横向目标偏移量Offset及本车车速模块的车速 根据期望偏移过渡时间Texp进行限值生成最终偏移过渡时间;
步骤4)偏移轨迹规划模块,接收触发偏移模块的触发偏移规划信号、最终的偏移过 渡时间模块的最终偏移过渡时间信号、环境认知模块的横向目标偏移量以及期望偏移过 渡时间信号,将横向目标偏移量规划成曲线,并通过曲线插值求的偏移轨迹系数;
步骤5)道路中心线系数模块提供道路中心线系数A0、A1、A2、A3;
步骤6)横向控制最终轨迹计算模块在道路的中心线系数上叠加偏移轨迹系数Fnial_DeltA0、Fnial_DeltA1、Fnial_DeltA2;
步骤7)横向控制执行模块执行道路中心线叠加偏移轨迹系数后用于控制跟踪的轨 迹。
进一步地,步骤4)将横向目标偏移量规划成下列二次曲线,并通过曲线插值求的偏 移轨迹系数;
步骤41)
y=Final_DeltA0+Final_DeltA1*x+Final_DeltA2*x2;其中,X为相对于本车的纵向距 离,Y为相对于本车的横向距离;
DeltA0=c(1)*t3+c(2)*t4+c(3)*t5+DeltOffset+Offset;
DeltA1=(3*c(1)*t2+4*c(2)*t3+5*c(3)*t4)/Speed;
DeltA2=(6*c(1)*t+12*c(2)*t2+20*c(3)*t3)/(2*Speed);
其中,
DeltOffset=Offset-Offsetprev,Offset为横向目标偏移量,offsetprev为上一次插值横向 目标偏移量,Speed为车速;
c=Tmatrix\yout;
t=tlast+dt,触发插值时t为0,tlast为上一次插值时间,dt为每次插值时间计算步长, 当t大于T时或者横向控制退出时,DeltA0等于Offset,DeltA1、DeltA2为0;
步骤42)
两次偏移规划触发时间间隔大于等于上次偏移规划触发时的期望偏移过渡时间,则 Fnial_DeltA0等于DeltA0,Fnial_DeltA1等于DeltA1,Fnial_DeltA2等于DeltA2;两次偏 移规划触发时间间隔小于上次偏移规划触发时的期望偏移过渡时间,则Fnial_DeltA0等 于DeltA0;
DeltA1、DeltA2分别进行二次插值规划处理,
Final_DeltA1=c(l)*t3+c(2)*t4+c(3)*t5+DeltOffset+DeltA1
其中,DeltOffset=DeltA1-DeltA1prev,DeltA1prev为上一次插值DeltA1值;
Final_DeltA2=c(1)*t3+c(2)*t4+c(3)*t5+DeltOffset+DeltA2
其中,DeltOffset=DeltA2-DeltA2prev,DeltA2prev为上一次插值DeltA2值。
进一步地,
步骤5)中,道路的中心线以三次多项式体现:
y=A0+A1*x+A2*x2+A3*x3;
其中,x为本车坐标系下的纵向距离,y为本车坐标系下的横向距离,A0、A1、A2、 A3为输出给控制的轨迹系数,A0对应于本车坐标原点处的横向位置,A1对应于本车坐 标原点处的航向角,2xA2对应于本车坐标原点处的曲率、6xA3对应于本车坐标原点处 的曲率变化率。
进一步地,步骤3)中,最终的偏移过渡时间模块查找标定的能够实现正常功能的最小 时间Tmin标定表以及能保证舒适性的舒适性过渡时间Tcom标定表,当横向目标偏移量 Offset等于0时,最终偏移过渡时间T等于Tcom;否则,当期望偏移过渡时间小于最小 时间Tmin时,最终偏移过渡时间T等于最小时间Tmin;否则,最终偏移过渡时间T等 于期望偏移过渡时间。
本发明有益技术效果为:
在道路的中心线系数上叠加偏移轨迹系数,使本车及时远离目标行进轨迹上的大型车 辆以及对本车产生压迫的小型车辆,一定程度上实现“防御性驾驶”,增强驾乘人员的驾 乘信心感与安全感。
附图说明
图1为自动驾驶偏移的横向轨迹规划系统架构图;
图2为自动驾驶偏移的横向轨迹规划方法流程图;
图3为偏移轨迹系数与时间关系图。
具体实施方式
下面结合附图对本发明做详细说明。
如图1至图3所示,在自动驾驶控制系统中通常采用多次多项式来表达横向需跟踪的 轨迹,本发明专利以三次多项式为列:
y=A0+A1*x+A2*x2+A3*x3
其中,x为本车坐标系下的纵向距离,y为本车坐标系下的横向距离,A0、A1、A2、 A3为输出给控制的轨迹系数,A0对应于本车坐标原点处的横向位置,A1对应于本车坐 标原点处的航向角,2XA2对应于本车坐标原点处的曲率、6XA3对应于本车坐标原点处 的曲率变化率。
本发明公开的自动驾驶偏移的横向轨迹规划系统,包括:自动驾驶横向控制状态模 块1、环境认知模块2、本车车速模块3、触发偏移模块4、最终的偏移过渡时间模块5、 偏移轨迹规划模块6、道路中心线系数模块9、横向控制最终轨迹计算模块7和横向控制 执行模块8。
自动驾驶横向控制状态模块提供当前横向控制的状态信号,并将横向控制的状态信 号传送给触发偏移模块。
环境认知模块,根据周围的场景计算出相对于危险物体的横向目标偏移量以及期望 偏移过渡时间;将横向目标偏移量传送给触发偏移模块、偏移轨迹规划模块;将期望偏移过渡时间传送给最终的偏移过渡时间模块。
触发偏移模块,接收横向控制的状态信号,环境认知模块的横向目标偏移量,判断是否触发偏移轨迹规划模块启动偏移规划。
最终的偏移过渡时间模块,根据期望偏移过渡时间进行限值生成最终偏移过渡时间 并输出给偏移轨迹规划模块;最终的偏移过渡时间模块根据横向目标偏移量及车速查找 标定的能够实现正常功能的最小时间Tmin标定表以及能保证舒适性的舒适性过渡时间Tcom标定表。
当横向目标偏移量等于0时,最终偏移过渡时间T等于Tcom;否则,当期望偏移过渡时间小于最小时间Tmin时,最终偏移过渡时间T等于最小时间Tmin;否则,最终偏 移过渡时间T等于期望偏移过渡时间。
偏移轨迹规划模块,接收触发偏移模块的触发偏移规划信号、最终的偏移过渡时间 模块的最终偏移过渡时间信号、环境认知模块的横向目标偏移量以及期望偏移过渡时间 信号,将横向目标偏移量规划成能够直接叠加至道路中心线上的偏移轨迹系数 Final_DeltA0、Final_DeltA1、Final_DeltA2。
将横向目标偏移量规划成下列二次曲线,并通过曲线插值求的偏移轨迹系数; y=Final_DeltA0+Final_DeltA1*x+Final_DeltA2*x2;其中,X为相对于本车的纵向距离,Y为相对于本车的横向距离。
曲线插值按如下方式进行:
步骤1)建立插值公式,
DeltA0=c(1)*t3+c(2)*t4+c(3)*t5+DeltOffset+Offset;
DeltA1=(3*c(1)*t2+4*c(2)*t3+5*c(3)*t4)/Speed;
DeltA2=(6*c(1)*t+12*c(2)*t2+20*c(3)*t3)/(2*Speed);
其中:
DeltOffset=Offset-Offsetprev,Offset为横向目标偏移量,offsetprev为上一次插值横向 目标偏移量,Speed为车速;
c=Tmatrix\yout;
t=tlast+dt,触发插值时t为0,tlast为上一次插值时间,dt为每次插值时间计算步长;
当t大于T时或者横向控制退出时,DeltA0等于Offset,DeltA1、DeltA2为0;
步骤2)条件判断,
偏移规划触发时间间隔大于等于上次偏移规划触发时的期望偏移过渡时间,则Fnial_DeltA0等于DeltA0,Fnial_DeltA1等于DeltA1,Fnial_DeltA2等于DeltA2;
偏移规划触发时间间隔小于上次偏移规划触发时的期望偏移过渡时间,则Fnial_DeltA0等于DeltA0;DeltA1、DeltA2分别进行二次插值规划处理:
Final_DeltA1=c(1)*t3+c(2)*t4+c(3)*t5+DeltOffset+DeltA1;
其中,DeltOffset=DeltA1-DeltA1prev,DeltA1prev为上一次插值DeltA1值;
Final_DeltA2=c(1)*t3+c(2)*t4+c(3)*t5+DeltOffset+DeltA2;
其中,DeltOffset=DeltA2-DeltA2prev,DeltA2prev为上一次插值DeltA2值。
道路中心线系数模块,提供道路的中心线系数A0、A1、A2、A3;
将中心线规划成下列三次多项式:
y=A0+A1*x+A2*x2+A3*x3;
其中,x为本车坐标系下的纵向距离,y为本车坐标系下的横向距离,A0、A1、A2、 A3为轨迹系数,A0对应于本车坐标原点处的横向位置,A1对应于本车坐标原点处的航 向角,2×A2对应于本车坐标原点处的曲率,6×A3对应于本车坐标原点处的曲率变化 率。
横向控制最终轨迹计算模块,在道路的中心线系数上叠加偏移轨迹系数;
横向控制执行模块,执行道路中心线叠加偏移轨迹系数后用于控制跟踪的轨迹。
本发明还公开了自动驾驶偏移的横向轨迹规划方法,涉及自动驾驶横向控制状态模 块、环境认知模块、本车车速模块、触发偏移模块、最终的偏移过渡时间模块、偏移轨 迹规划模块、道路中心线系数模块、横向控制最终轨迹计算模块和横向控制执行模块;
按如下步骤工作:
步骤1)环境认知模块根据当前目标车等障碍物横向距离,相对车速及道路边界情况 规划出能够安全驶过的横向目标偏移量Offset,以及期望偏移过渡时间Texp;
步骤2)当车辆横向控制处于激活状态,且横向目标偏移量Offset存在变化时触发偏 移模块置1,进入步骤3);
步骤3)最终的偏移过渡时间模块根据横向目标偏移量Offset及本车车速模块的车速 根据期望偏移过渡时间Texp进行限值生成最终偏移过渡时间;最终的偏移过渡时间模块 查找标定的能够实现正常功能的最小时间Tmin标定表以及能保证舒适性的舒适性过渡时间Tcom标定表,当横向目标偏移量Offset等于0时,最终偏移过渡时间T等于Tcom; 否则,当期望偏移过渡时间小于最小时间Tmin时,最终偏移过渡时间T等于最小时间 Tmin;否则,最终偏移过渡时间T等于期望偏移过渡时间;
步骤4)偏移轨迹规划模块,接收触发偏移模块的触发偏移规划信号、最终的偏移过 渡时间模块的最终偏移过渡时间信号、环境认知模块的横向目标偏移量以及期望偏移过 渡时间信号,将横向目标偏移量规划成二次曲线,并通过曲线插值求的偏移轨迹系数;
步骤41)
y=Final_DeltA0+Final_DeltA1*x+Final_DeltA2*x2;其中,X为相对于本车的纵向距 离,Y为相对于本车的横向距离;
DeltA0=c(1)*t3+c(2)*t4+c(3)*t5+DgltOffset+Offset;
DeltA1=(3*c(1)*t2+4*c(2)*t3+5*c(3)*t4)/Speed;
DeltA2=(6*c(1)*t+12*c(2)*t2+20*c(3)*t3)/(2*Spegd);
其中,
DeltOffset=Offset-Offsetprev,Offset为横向目标偏移量,offsetprev为上一次插值横向 目标偏移量,Speed为车速;
c=Tmatrix\yout:
t=tlast+dt,触发插值时t为0,dt为每次插值时间计算步长,当t大于T时或者横向控制退出时,DeltA0等于Offset,DeltA1、DeltA2为0;
步骤42)
两次偏移规划触发时间间隔大于等于上次偏移规划触发时的期望偏移过渡时间,则 Fnial_DeltA0等于DeltA0,Fnial_DeltA1等于DeltA1,Fnial_DeltA2等于DeltA2;两次偏 移规划触发时间间隔小于上次偏移规划触发时的期望偏移过渡时间,则Fnial_DeltA0等 于DeltA0,
DeltA1、DeltA2分别进行二次插值规划处理,
Final_DeltA1=c(1)*t3+c(2)*t4+c(3)*t5+DeltOffset+DeltA1
其中,DeltOffset=DeltA1-DeltA1prev,DeltA1prev为上一次插值DeltA1值;
Final_DeltA2=c(1)*t3+c(2)*t4+c(3)*t5+DeltOffset+DeltA2
其中,DeltOffset=DeltA2-DeltA2prev,DeltA2prev为上一次插值DeltA2值。
步骤5)道路中心线系数模块提供道路中心线系数A0、A1、A2、A3,道路的中心线 以三次多项式体现:
y=A0+A1*x+A2*x2+A3*x2;
其中,x为本车坐标系下的纵向距离,y为本车坐标系下的横向距离,A0、A1、A2、 A3为输出给控制的轨迹系数,A0对应于本车坐标原点处的横向位置,A1对应于本车坐 标原点处的航向角,2xA2对应于本车坐标原点处的曲率、6xA3对应于本车坐标原点处 的曲率变化率;
步骤6)横向控制最终轨迹计算模块在道路的中心线系数上叠加偏移轨迹系数Fnial_DeltA0、Fnial_DeltA1、Fnial_DeltA2;
步骤7)横向控制执行模块执行重新规划的轨迹。
以偏移量0.3米,车速20m/s为例,规划的偏移曲线如图3所示,其中第2s-5s为一个完整的偏移过程,第6s开始进行偏移回正规划处理,第7s尚未完成时再一次触发轨迹 规划,各轨迹系数能够连续规划输出。
Claims (10)
1.自动驾驶偏移的横向轨迹规划系统,其特征在于:包括自动驾驶横向控制状态模块(1)、环境认知模块(2)、本车车速模块(3)、触发偏移模块(4)、最终的偏移过渡时间模块(5)、偏移轨迹规划模块(6)、道路中心线系数模块(9)、横向控制最终轨迹计算模块(7)和横向控制执行模块(8);
所述自动驾驶横向控制状态模块提供当前横向控制的状态信号,并将横向控制的状态信号传送给触发偏移模块;
所述环境认知模块,根据周围的场景计算出相对于危险物体的横向目标偏移量以及期望偏移过渡时间;将横向目标偏移量传送给触发偏移模块、偏移轨迹规划模块;将期望偏移过渡时间传送给最终的偏移过渡时间模块;
所述触发偏移模块,接收横向控制的状态信号,环境认知模块的横向目标偏移量,判断是否触发偏移轨迹规划模块启动偏移规划;
所述最终的偏移过渡时间模块,根据期望偏移过渡时间进行限值生成最终偏移过渡时间并输出给偏移轨迹规划模块;
所述偏移轨迹规划模块,接收触发偏移模块的触发偏移规划信号、最终的偏移过渡时间模块的最终偏移过渡时间信号、环境认知模块的横向目标偏移量以及期望偏移过渡时间信号,将横向目标偏移量规划成能够直接叠加至道路中心线上的偏移轨迹系数Final_DeltA0、Final_DeltA1、Final_DeltA2;
所述道路中心线系数模块,提供道路的中心线系数A0、A1、A2、A3;
所述横向控制最终轨迹计算模块,在道路的中心线系数上叠加偏移轨迹系数;
所述横向控制执行模块,执行道路中心线叠加偏移轨迹系数后用于控制跟踪的轨迹。
2.如权利要求1所述的自动驾驶偏移的横向轨迹规划系统,其特征在于:将横向目标偏移量规划成下列二次曲线,并通过曲线插值求的偏移轨迹系数;y=Final_DeltA0+Final_DeltA1*x+Final_DeltA2*x2;其中,x为相对于本车的纵向距离,y为相对于本车的横向距离。
3.如权利要求2所述的自动驾驶偏移的横向轨迹规划系统,其特征在于:曲线插值按如下方式进行:
步骤1)建立插值公式,
DeltA0=c(1)*t3+c(2)*t4+c(3)*t5+DeltOffset+Offset;
DeltA1=(3*c(1)*t2+4*c(2)*t3+5*c(3)*t4)/Speed;
DeltA2=(6*c(1)*t+12*c(2)*t2+20*c(3)*t3)/(2*Speed);
其中:
DeltOffset=Offset-Offsetprev,Offset为横向目标偏移量,Offsetprev为上一次插值横向目标偏移量,Speed为车速;c(1)为插值公式的三阶项系数,c(2)为插值公式的四阶项系数,c(3)为插值公式的五阶项系数;
c=Tmatrix\yout;
t=tlast+dt,触发插值时t为0,tlast为上一次插值时间,dt为每次插值时间计算步长;当t大于T时或者横向控制退出时,DeltA0等于Offset,DeltA1、DeltA2为0;
步骤2)条件判断:
偏移规划触发时间间隔大于等于上次偏移规划触发时的期望偏移过渡时间,则Fnial_DeltA0等于DeltA0,Fnial_DeltA1等于DeltA1,Fnial_DeltA2等于DeltA2;
偏移规划触发时间间隔小于上次偏移规划触发时的期望偏移过渡时间,则Fnial_DeltA0等于DeltA0;DeltA1、DeltA2分别进行二次插值规划处理:
Final_DeltA1=c(1)*t3+c(2)*t4+c(3)*t5+DeltOffset+DeltA1;
其中,DeltOffset=DeltA1-DeltA1prev,DeltA1prev为上一次插值DeltA1值;
Final_DeltA2=c(1)*t3+c(2)*t4+c(3)*t5+DeltOffset+DeltA2;
其中,DeltOffset=DeltA2-DeltA2prev,DeltA2prev为上一次插值DeltA2值。
4.如权利要求3所述的自动驾驶偏移的横向轨迹规划系统,其特征在于:将中心线规划成下列三次多项式:
v=A0+A1*x+A2*x2+A3*x3;
其中,x为本车坐标系下的纵向距离,y为本车坐标系下的横向距离,A0、A1、A2、A3为轨迹系数,A0对应于本车坐标原点处的横向位置,A1对应于本车坐标原点处的航向角,2×A2对应于本车坐标原点处的曲率,6×A3对应于本车坐标原点处的曲率变化率。
5.如权利要求4所述的自动驾驶偏移的横向轨迹规划系统,其特征在于:横向控制最终轨迹计算模块,在道路的中心线系数上叠加偏移轨迹系数。
6.如权利要求5所述的自动驾驶偏移的横向轨迹规划系统,其特征在于:
最终的偏移过渡时间模块根据横向目标偏移量及车速查找标定的能够实现正常功能的最小时间Tmin标定表以及能保证舒适性的舒适性过度时间Tcom标定表;
当横向目标偏移量等于0时,最终偏移过渡时间T等于Tcom;否则,当期望偏移过渡时间小于最小时间Tmin时,最终偏移过渡时间T等于最小时间Tmin;否则,最终偏移过渡时间T等于期望偏移过渡时间。
7.自动驾驶偏移的横向轨迹规划方法,其特征在于:涉及自动驾驶横向控制状态模块、环境认知模块、本车车速模块、触发偏移模块、最终的偏移过渡时间模块、偏移轨迹规划模块、道路中心线系数模块、横向控制最终轨迹计算模块和横向控制执行模块;
按如下步骤工作:
步骤1)环境认知模块根据当前目标车等障碍物横向距离,相对车速及道路边界情况规划出能够安全驶过的横向目标偏移量Offset,以及期望偏移过渡时间Texp;
步骤2)当车辆横向控制处于激活状态,且横向目标偏移量Offset存在变化时触发偏移模块置1,进入步骤3);
步骤3)最终的偏移过渡时间模块根据横向目标偏移量Offset及本车车速模块的车速根据期望偏移过渡时间Texp进行限值生成最终偏移过渡时间;
步骤4)偏移轨迹规划模块,接收触发偏移模块的触发偏移规划信号、最终的偏移过渡时间模块的最终偏移过渡时间信号、环境认知模块的横向目标偏移量以及期望偏移过渡时间信号,将横向目标偏移量规划成曲线,并通过曲线插值求的偏移轨迹系数;
步骤5)道路中心线系数模块提供道路中心线系数A0、A1、A2、A3;
步骤6)横向控制最终轨迹计算模块在道路的中心线系数上叠加偏移轨迹系数Fnial_DeltA0、Fnial_DeltA1、Fnial_DeltA2;
步骤7)横向控制执行模块执行道路中心线叠加偏移轨迹系数后用于控制跟踪的轨迹。
8.如权利要求7所述的自动驾驶偏移的横向轨迹规划方法,其特征在于:
步骤4)将横向目标偏移量规划成下列二次曲线,并通过曲线插值求的偏移轨迹系数;
步骤41)
y=Final_DeltA0+Final_DeltA1*x+Final_DeltA2*x2;其中,x为相对于本车的纵向距离,y为相对于本车的横向距离;
DeltA0=c(1)*t3+c(2)*t4+c(3)*t5+DeltOffset+Offset;
DeltA1=(3*c(1)*t2+4*c(2)*t3+5*c(3)*t4)/Speed;
DeltA2=(6*c(1)*t+12*c(2)*t2+20*c(3)*t3)/(2*Speed);
其中,
DeltOffset=Offset-Offsetprev,Offset为横向目标偏移量,Offsetprev为上一次插值横向目标偏移量,Speed为车速;c(1)为插值公式的三阶项系数,c(2)为插值公式的四阶项系数,c(3)为插值公式的五阶项系数;
c=Tmatrix\yout;
t=tlast+dt,触发插值时t为0,dt为每次插值时间计算步长,当t大于T时或者横向控制退出时,DeltA0等于Offset,DeltA1、DeltA2为0;
步骤42)
两次偏移规划触发时间间隔大于等于上次偏移规划触发时的期望偏移过渡时间,则Fnial_DeltA0等于DeltA0,Fnial_DeltA1等于DeltA1,Fnial_DeltA2等于DeltA2;两次偏移规划触发时间间隔小于上次偏移规划触发时的期望偏移过渡时间,则Fnial_DeltA0等于DeltA0;
DeltA1、DeltA2分别进行二次插值规划处理,
Final_DeltA1=c(1)*t3+c(2)*t4+c(3)*t5+DeltOffset+DeltA1
其中,DeltOffset=DeltA1-DeltA1prev,DeltA1prev为上一次插值DeltA1值;
Final_DeltA2=c(1)*t3+c(2)*t4+c(3)*t5+DeltOffset+DeltA2
其中,DeltOffset=DeltA2-DeltA2prev,DeltA2prev为上一次插值DeltA2值。
9.如权利要求8所述的自动驾驶偏移的横向轨迹规划方法,其特征在于:
步骤5)中,道路的中心线以三次多项式体现:
y=A0+A1*x+A2*x2+A3*x3;
其中,x为本车坐标系下的纵向距离,y为本车坐标系下的横向距离,A0、A1、A2、A3为输出给控制的轨迹系数,A0对应于本车坐标原点处的横向位置,A1对应于本车坐标原点处的航向角,2xA2对应于本车坐标原点处的曲率、6xA3对应于本车坐标原点处的曲率变化率。
10.如权利要求9所述的自动驾驶偏移的横向轨迹规划方法,其特征在于:步骤3)中,最终的偏移过渡时间模块查找标定的能够实现正常功能的最小时间Tmin标定表以及能保证舒适性的舒适性过渡时间Tcom标定表,当横向目标偏移量Offset等于0时,最终偏移过渡时间T等于Tcom;否则,当期望偏移过渡时间小于最小时间Tmin时,最终偏移过渡时间T等于最小时间Tmin;否则,最终偏移过渡时间T等于期望偏移过渡时间。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202011335081.2A CN112498368B (zh) | 2020-11-25 | 2020-11-25 | 自动驾驶偏移的横向轨迹规划系统及方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202011335081.2A CN112498368B (zh) | 2020-11-25 | 2020-11-25 | 自动驾驶偏移的横向轨迹规划系统及方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN112498368A CN112498368A (zh) | 2021-03-16 |
CN112498368B true CN112498368B (zh) | 2022-03-11 |
Family
ID=74958418
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202011335081.2A Active CN112498368B (zh) | 2020-11-25 | 2020-11-25 | 自动驾驶偏移的横向轨迹规划系统及方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN112498368B (zh) |
Families Citing this family (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN113815608B (zh) * | 2021-09-24 | 2023-08-29 | 上汽通用五菱汽车股份有限公司 | 车道保持方法、装置和计算机可读存储介质 |
CN116729384B (zh) * | 2023-06-27 | 2024-01-09 | 广州小鹏自动驾驶科技有限公司 | 车道保持状态下的绕行规划方法、装置及车辆 |
Citations (10)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN104239628A (zh) * | 2014-09-10 | 2014-12-24 | 长安大学 | 一种检测车辆横向偏移对车辙深度误差的模拟分析方法 |
CN106371439A (zh) * | 2016-09-13 | 2017-02-01 | 同济大学 | 一种统一的自动驾驶横向规划方法与系统 |
CN107632602A (zh) * | 2017-09-01 | 2018-01-26 | 上海斐讯数据通信技术有限公司 | Agv小车运行轨道纠偏方法及系统、地标二维码获取装置 |
WO2018032933A1 (zh) * | 2016-08-17 | 2018-02-22 | 国网山东省电力公司电力科学研究院 | 变电站巡检机器人导航控制系统及方法 |
EP3377953A1 (en) * | 2016-10-31 | 2018-09-26 | Magneti Marelli S.p.A. | Adaptive control method and system in a terrestrial vehicle for tracking a route, particularly in an autonomous driving scenario |
CN109324620A (zh) * | 2018-09-25 | 2019-02-12 | 北京主线科技有限公司 | 基于车道线平行偏移进行避障及超车的动态轨迹规划方法 |
CN110597245A (zh) * | 2019-08-12 | 2019-12-20 | 北京交通大学 | 基于二次型规划和神经网络的自动驾驶换道轨迹规划方法 |
CN111196274A (zh) * | 2020-01-21 | 2020-05-26 | 重庆长安汽车股份有限公司 | 车辆倾斜道路对中控制的方法、系统、存储介质及车辆 |
DE102019003430B3 (de) * | 2019-05-15 | 2020-06-04 | Daimler Ag | Verfahren zur Durchführung eines automatisierten oder autonomen Fahrbetriebs eines Fahrzeugs |
CN111703436A (zh) * | 2020-08-18 | 2020-09-25 | 北京主线科技有限公司 | 一种自动驾驶车辆的控制方法及装置 |
Family Cites Families (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
WO2019218353A1 (en) * | 2018-05-18 | 2019-11-21 | Baidu.Com Times Technology (Beijing) Co., Ltd. | Drifting correction between planning stage and controlling stage of operating autonomous driving vehicles |
CN109795477B (zh) * | 2019-02-22 | 2020-11-06 | 百度在线网络技术(北京)有限公司 | 消除稳态横向偏差的方法、装置及存储介质 |
CN112602127A (zh) * | 2019-03-22 | 2021-04-02 | 深圳市大疆创新科技有限公司 | 用于车道监控和提供车道偏离警告的系统和方法 |
JP2020185968A (ja) * | 2019-05-17 | 2020-11-19 | 本田技研工業株式会社 | 車両制御装置、車両制御方法、およびプログラム |
-
2020
- 2020-11-25 CN CN202011335081.2A patent/CN112498368B/zh active Active
Patent Citations (10)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN104239628A (zh) * | 2014-09-10 | 2014-12-24 | 长安大学 | 一种检测车辆横向偏移对车辙深度误差的模拟分析方法 |
WO2018032933A1 (zh) * | 2016-08-17 | 2018-02-22 | 国网山东省电力公司电力科学研究院 | 变电站巡检机器人导航控制系统及方法 |
CN106371439A (zh) * | 2016-09-13 | 2017-02-01 | 同济大学 | 一种统一的自动驾驶横向规划方法与系统 |
EP3377953A1 (en) * | 2016-10-31 | 2018-09-26 | Magneti Marelli S.p.A. | Adaptive control method and system in a terrestrial vehicle for tracking a route, particularly in an autonomous driving scenario |
CN107632602A (zh) * | 2017-09-01 | 2018-01-26 | 上海斐讯数据通信技术有限公司 | Agv小车运行轨道纠偏方法及系统、地标二维码获取装置 |
CN109324620A (zh) * | 2018-09-25 | 2019-02-12 | 北京主线科技有限公司 | 基于车道线平行偏移进行避障及超车的动态轨迹规划方法 |
DE102019003430B3 (de) * | 2019-05-15 | 2020-06-04 | Daimler Ag | Verfahren zur Durchführung eines automatisierten oder autonomen Fahrbetriebs eines Fahrzeugs |
CN110597245A (zh) * | 2019-08-12 | 2019-12-20 | 北京交通大学 | 基于二次型规划和神经网络的自动驾驶换道轨迹规划方法 |
CN111196274A (zh) * | 2020-01-21 | 2020-05-26 | 重庆长安汽车股份有限公司 | 车辆倾斜道路对中控制的方法、系统、存储介质及车辆 |
CN111703436A (zh) * | 2020-08-18 | 2020-09-25 | 北京主线科技有限公司 | 一种自动驾驶车辆的控制方法及装置 |
Non-Patent Citations (2)
Title |
---|
基于驾驶员方向控制模型的汽车侧风稳定性虚拟试验研究;任秀欢等;《武汉理工大学学报(交通科学与工程版)》;20101215(第06期);全文 * |
拖拉机自动导航单因子控制系统设计;姚庆旺等;《农机化研究》;20181122(第08期);全文 * |
Also Published As
Publication number | Publication date |
---|---|
CN112498368A (zh) | 2021-03-16 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN111806467B (zh) | 一种基于车辆行驶规律的变速动态换道轨迹规划方法 | |
CN109649393B (zh) | 一种自动驾驶变换车道的路径规划方法及装置 | |
CN107128366B (zh) | 用于对拖曳车辆通过弯道提供校正的车道跟随路径的系统和方法 | |
US9229453B1 (en) | Unified motion planner for autonomous driving vehicle in avoiding the moving obstacle | |
US10988139B2 (en) | Vehicle position control method and device vehicle position control device for correcting position in drive-assisted vehicle | |
US8190330B2 (en) | Model based predictive control for automated lane centering/changing control systems | |
US9457807B2 (en) | Unified motion planning algorithm for autonomous driving vehicle in obstacle avoidance maneuver | |
EP3791239B1 (en) | A method for establishing a path for a vehicle | |
US9428187B2 (en) | Lane change path planning algorithm for autonomous driving vehicle | |
JP6905367B2 (ja) | 車両の走行制御装置 | |
CN112498368B (zh) | 自动驾驶偏移的横向轨迹规划系统及方法 | |
US20180129214A1 (en) | Determining a trajectory for a vehicle | |
CN111873998B (zh) | 一种自动驾驶车辆纵向加速度规划方法 | |
CN109164814B (zh) | 面向高速公路场景的自动驾驶控制系统 | |
US9573623B2 (en) | Collision avoidance control integrated with electric power steering controller and rear steer | |
EP3132244B1 (en) | Path control system | |
EP3782000A1 (en) | A method for controlling a string of vehicles | |
JP2000339600A (ja) | 自動追従走行システム | |
JP7315039B2 (ja) | 自動車の回避経路を求める方法 | |
US20230211786A1 (en) | Path-controlling module, associated path-controlling device and associated method | |
US20170240171A1 (en) | Method for modifying steering of an automated vehicle for improved passenger comfort | |
CN112224202B (zh) | 一种紧急工况下的多车协同避撞系统及方法 | |
CN111284486A (zh) | 车辆的行驶控制装置 | |
Fassbender et al. | An optimization approach to trajectory generation for autonomous vehicle following | |
JP2006335174A (ja) | 車両用走行制御装置 |
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 |