自动驾驶偏移的横向轨迹规划系统及方法与流程

本发明用于自动驾驶领域，具体涉自动驾驶偏移的横向轨迹规划系统及方法。

背景技术：

自动驾驶偏移功能通过本车的横向偏移，使本车远离目标行进轨迹上的大型车辆以及对本车产生压迫的小型车辆，一定程度上实现“防御性驾驶”，增强驾乘人员的驾乘信心感与安全感。以往相关自动驾驶偏移的横向轨迹规划一般集中在单纯的横向位置插值偏移响应缓慢以致车速差较大时本车驶过障碍物时尚未偏移到位或者在短时间内频繁偏移时可能出现的轨迹跳变未进行有效处理，如申请号为cn201811119580.0的专利“基于车道线平行偏移进行避障及超车的动态轨迹规划方法”，利用五次多项式实时对轨迹平行偏移进行插值处理，来实现避障及超车的动态轨迹规划，未涉及航向角及曲率变化率的规划。

因此，有必要开发一种用于自动驾驶偏移的横向轨迹规划系统及方法，快速稳定的偏移到位。

技术实现要素：

本发明公开的自动驾驶偏移的横向轨迹规划系统，在横向控制激活的状态下，根据环境认知计算的目标偏移量及期望偏移过渡时间以及本车车速情况将偏移量规划成曲线叠加在道路中心线上，输出给横向控制模块，实现偏移及时响应。

本发明还公开了自动驾驶偏移的横向轨迹规划方法，在横向控制激活的状态下，根据环境认知计算的目标偏移量及期望偏移过渡时间以及本车车速情况将偏移量规划成曲线叠加在道路中心线上，输出给横向控制模块，实现偏移及时响应。

本发明公开的自动驾驶偏移的横向轨迹规划系统，包括自动驾驶横向控制状态模块、环境认知模块、本车车速模块、触发偏移模块、最终的偏移过渡时间模块、偏移轨迹规划模块、道路中心线系数模块、横向控制最终轨迹计算模块和横向控制执行模块；

所述自动驾驶横向控制状态模块提供当前横向控制的状态信号，并将横向控制的状态信号传送给触发偏移模块；

所述环境认知模块，根据周围的场景计算出相对于危险物体的横向目标偏移量以及期望偏移过渡时间；将横向目标偏移量传送给触发偏移模块、偏移轨迹规划模块；将期望偏移过渡时间传送给最终的偏移过渡时间模块；

所述触发偏移模块，接收横向控制的状态信号，环境认知模块的横向目标偏移量，判断是否触发偏移轨迹规划模块启动偏移规划；

所述最终的偏移过渡时间模块，根据期望偏移过渡时间进行限值生成最终偏移过渡时间并输出给偏移轨迹规划模块；

所述偏移轨迹规划模块，接收触发偏移模块的触发偏移规划信号、最终的偏移过渡时间模块的最终偏移过渡时间信号、环境认知模块的横向目标偏移量以及期望偏移过渡时间信号，将横向目标偏移量规划成能够直接叠加至道路中心线上的偏移轨迹系数final_delta0、final_delta1、final_delta2；

所述道路中心线系数模块，提供道路的中心线系数a0、a1、a2、a3；

所述横向控制最终轨迹计算模块，在道路的中心线系数上叠加偏移轨迹系数；

所述横向控制执行模块，执行道路中心线叠加偏移轨迹系数后用于控制跟踪的轨迹。

进一步地，将横向目标偏移量规划成下列二次曲线，并通过曲线插值求的偏移轨迹系数；y＝final_delta0+final_delta1*x+final_delta2*x²；其中，x为相对于本车的纵向距离，y为相对于本车的横向距离。

进一步地，曲线插值按如下方式进行：

步骤1)建立插值公式，

delta0＝c(1)*t³+c(2)*t⁴+c(3)*t⁵+deltoffset+offset；

delta1＝(3*c(1)*t²+4*c(2)*t³+5*c(3)*t⁴)/speed；

delta2＝(6*c(1)*t+12*c(2)*t²+20*c(3)*t³)/(2*speed)；

其中：

deltoffset＝offset-offsetprev，offset为横向目标偏移量，offsetprev为上一次插值横向目标偏移量，speed为车速；

t为期望偏移过渡时间；

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)*t³+c(2)*t⁴+c(3)*t⁵+deltoffset+delta1；

其中，deltoffset＝delta1-delta1prev，delta1prev为上一次插值delta1值；

final_delta2＝c(1)**t³+c(2)*t⁴+c(3)*t⁵+deltoffset+delta2；

其中，deltoffset＝delta2-delta2prev，delta2prev为上一次插值delta2值。

进一步地，将中心线规划成下列三次多项式：

y＝a0+a1*x+a2*x²+a3*x³；

其中，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*x²；其中，x为相对于本车的纵向距离，y为相对于本车的横向距离；

delta0＝c(1)*t³+c(2)*t⁴+c(3)*t⁵+deltoffset+offset；

delta1＝(3*c(1)*t²+4*c(2)*t³+5*c(3)*t⁴)/speed；

delta2＝(6*c(1)*t+12*c(2)*t²+20*c(3)*t³)/(2*speed)；

其中，

deltoffset＝offset-offsetprev，offset为横向目标偏移量，offsetprev为上一次插值横向目标偏移量，speed为车速；

t为期望偏移过渡时间；

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)*t³+c(2)*t⁴+c(3)*t⁵+deltoffset+delta1

其中，deltoffset＝delta1-delta1prev，delta1prev为上一次插值delta1值；

final_delta2＝c(1)*t³+c(2)*t⁴+c(3)*t⁵+deltoffset+delta2

其中，deltoffset＝delta2-delta2prev，delta2prev为上一次插值delta2值。

进一步地，

步骤5)中，道路的中心线以三次多项式体现：

y＝a0+a1*x+a2*x²+a3*x³；

其中，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*x²+a3*x³

其中，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*x²；其中，x为相对于本车的纵向距离，y为相对于本车的横向距离。

曲线插值按如下方式进行：

步骤1)建立插值公式，

delta0＝c(1)*t³+c(2)*t⁴+c(3)*t⁵+deltoffset+offset；

delta1＝(3*c(1)*t²+4*c(2)*t³+5*c(3)*t⁴)/speed；

delta2＝(6*c(1)*t+12*c(2)*t²+20*c(3)*t³)/(2*speed)；

其中：

deltoffset＝offset-offsetprev，offset为横向目标偏移量，offsetprev为上一次插值横向目标偏移量，speed为车速；

t为期望偏移过渡时间；

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)*t³+c(2)*t⁴+c(3)*t⁵+deltoffset+delta1；

其中，deltoffset＝delta1-delta1prev，delta1prev为上一次插值delta1值；

final_delta2＝c(1)*t³+c(2)*t⁴+c(3)*t⁵+deltoffset+delta2；

其中，deltoffset＝delta2-delta2prev，delta2prev为上一次插值delta2值。

道路中心线系数模块，提供道路的中心线系数a0、a1、a2、a3；

将中心线规划成下列三次多项式：

y＝a0+a1*x+a2*x²+a3*x³；

其中，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*x²；其中，x为相对于本车的纵向距离，y为相对于本车的横向距离；

delta0＝c(1)*t³+c(2)*t⁴+c(3)*t⁵+dgltoffset+offset；

delta1＝(3*c(1)*t²+4*c(2)*t³+5*c(3)*t⁴)/speed；

delta2＝(6*c(1)*t+12*c(2)*t²+20*c(3)*t³)/(2*spegd)；

其中，

deltoffset＝offset-offsetprev，offset为横向目标偏移量，offsetprev为上一次插值横向目标偏移量，speed为车速；

t为期望偏移过渡时间；

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)*t³+c(2)*t⁴+c(3)*t⁵+deltoffset+delta1

其中，deltoffset＝delta1-delta1prev，delta1prev为上一次插值delta1值；

final_delta2＝c(1)*t³+c(2)*t⁴+c(3)*t⁵+deltoffset+delta2

其中，deltoffset＝delta2-delta2prev，delta2prev为上一次插值delta2值。

步骤5)道路中心线系数模块提供道路中心线系数a0、a1、a2、a3，道路的中心线以三次多项式体现：

y＝a0+a1*x+a2*x²+a3*x²；

其中，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尚未完成时再一次触发轨迹规划，各轨迹系数能够连续规划输出。