CN114604259B - 一种拟人化车辆轨迹跟踪控制方法 - Google Patents

Abstract

本发明涉及一种拟人化车辆轨迹跟踪控制方法，使用的轨迹跟踪控制系统包括计算单元，计算单元包括预瞄模块、预瞄偏差计算模块、反馈校正模块和转角计算模块；其中，预瞄模块用于根据目标轨迹参数LanePars和车速VehSpd计算出预瞄距离PrvDis，预瞄偏差计算模块用于根据目标轨迹参数LanePars和预瞄距离PrvDis计算出预瞄点到轨迹的横向偏差PrvYErr，反馈校正模块用于根据横向偏差PrvYErr计算出前轮转角反馈控制量DetafFedBak，转角计算模块用于根据前轮转角反馈控制量DetafFedBak计算出期望的方向盘转角ExpSW并输出。本发明根据侧向加速度及其变化率动态调整反馈控制器参数，模拟人感知汽车运动响应后的驾驶操控特性，拟人化程度高，可有效提高自动驾驶操控的舒适度和可信度效果。

Description

一种拟人化车辆轨迹跟踪控制方法

技术领域

本发明属于自动驾驶的技术领域，具体涉及一种拟人化车辆轨迹跟踪控制方法。

背景技术

随着汽车智能化的高速发展，越来越多的汽车上搭载了自动驾驶功能，人们对于自动驾驶操控品质的要求也越来越高，期望汽车的自动驾驶操控品质更接近真实驾驶员，以获得更为舒适、可信的驾乘体验。自动驾驶中，轨迹跟踪控制是实现L3级及以上自动驾驶车辆横向运动控制的基本途径之一，而现有的轨迹跟踪控制方法的拟人化程度不够，一方面很少考虑方向盘转角控制频率是否符合真人的开车习惯，容易导致方向盘来回摆动，影响驾乘人员驾乘感受，另一方面对真实驾驶员操控行为的模拟不够，仅仅从数学和控制论的角度实现路径跟踪控制，导致对自动驾驶控制器的信心较低。

中国专利CN201910743104.4基于深度强化学习的自动驾驶智能车轨迹跟踪控制策略，该方案采用的基于深度学习的控制策略需要进行大量的场景训练，算法的泛化能力有待验证，在实际应用中一旦发生故障，难以溯源，且难以实现复杂交通环境下的自动驾驶。中国专利CN202010701188.8一种基于横纵向协调的自动驾驶汽车动态轨迹规划及跟踪方法，该方案在轨迹跟踪阶段，采用Brush轮胎模型设计自动驾驶车辆动力学操纵极限约束规则，利用模型预测控制(MPC)完成自动驾驶车辆主动转向(AFS)与直接横摆力矩(DYC)集成轨迹跟踪控制；中国专利CN201910846823.9一种自动驾驶车辆轨迹跟踪方法，该方案也是利用模型预测控制(MPC)完成自动驾驶车辆轨迹跟踪控制；人是通过观察汽车运动趋势是否朝着自己预瞄的位置靠近而下意识地转动方向盘，而模型预测控制(MPC)算法是通过大量的大型矩阵运算得出转向相关参数，拟人化程度较差。

发明内容

针对现有技术的上述不足，本发明要解决的技术问题是提供一种拟人化车辆轨迹跟踪控制方法，解决目前自动驾驶中轨迹跟踪控制拟人化程度较低的问题，取得提高自动驾驶操控的舒适度和可信度效果。

为解决上述技术问题，本发明采用如下技术方案：

一种拟人化车辆轨迹跟踪控制方法，包括轨迹跟踪控制系统，轨迹跟踪控制系统包括轨迹跟踪控制器，轨迹跟踪控制器包括计算单元，计算单元包括预瞄模块、预瞄偏差计算模块、反馈校正模块和转角计算模块；

预瞄模块用于根据目标轨迹曲率和车速VehSpd计算出预瞄距离PrvDis并传输至预瞄偏差计算模块，预瞄偏差计算模块用于根据目标轨迹参数LanePars和预瞄距离PrvDis计算出预瞄点到轨迹的横向偏差PrvYErr并传输至反馈校正模块，反馈校正模块用于根据横向偏差PrvYErr计算出前轮转角反馈控制量DetafFedBak并传输至转角计算模块，转角计算模块用于根据前轮转角反馈控制量DetafFedBak计算出期望的方向盘转角ExpSW并输出。

进一步地，轨迹跟踪控制器还包括输入信号单元和输出信号单元，输入信号单元包括有效诊断模块和信息处理模块，输出信号单元包括安全限制模块和滤波处理模块；

有效诊断模块用于根据状态机信息决策是否激活计算单元，信息处理模块用于向预瞄模块传输目标轨迹参数LanePars和车速VehSpd，还用于向安全限制模块传输侧向加速度Ay、车速VehSpd和方向盘转角StrWhlAng；安全限制模块用于根据侧向加速度Ay、车速VehSpd和方向盘转角StrWhlAng对方向盘转角ExpSW进行赋值和速率限制得到符合功能安全的方向盘转角ExpSafSW并传输至滤波处理模块，滤波处理模块用于对方向盘转角ExpSafSW进行低通滤波得到最终的方向盘转角ExpFinalSW并输出。

进一步地，轨迹跟踪控制系统还包括状态机信息模块、轨迹规划信息模块和CAN总线模块；

状态机信息模块用于向有效诊断模块传输状态机信息，还用于接收安全限制模块输出的轨迹跟踪控制器的状态信息LATCtrlSts；轨迹规划信息模块用于向信息处理模块传输目标轨迹参数LanePars；CAN总线模块用于向信息处理模块传输侧向加速度Ay、车速VehSpd和方向盘转角StrWhlAng，还用于接收滤波处理模块输出的最终的方向盘转角ExpFinalSW。

进一步地，状态机信息包括自动驾驶状态ADSSts、偏移状态OffsetSts、换道状态LaneChgSts和电动助力转向系统状态EPSSts。

进一步地，目标轨迹参数LanePars包括目标到轨迹的横向偏差、航向角、目标轨迹曲率和目标轨迹曲率变化率，目标轨迹用三次多项式表达如下：

y＝A₀十A₁x十A₂x²十A₃x³

其中，y为横向坐标，x为纵向坐标，A₀～A₃分别为目标到轨迹的横向偏差、航向角、目标轨迹曲率和目标轨迹曲率变化率。

进一步地，预瞄模块首先根据目标轨迹曲率PrvCurv计算出预瞄时间PrvT，再根据车速VehSpd和预瞄时间PrvT计算出预瞄距离PrvDis；

预瞄时间PrvT的计算公式如下：

t_prv＝min(3,max(a×e^(-b×ρ)+c×e^(d×ρ),0.5))

其中，t_prv为预瞄时间PrvT，且上限为3秒，下限为0.5秒；ρ为目标轨迹曲率PrvCurv，a、b、c和d为系数，e为自然对数的底数；

预瞄距离PrvDis的计算公式如下：

d_prv＝v_x×t_prv

其中，d_prv为预瞄距离PrvDis，v_x为车速VehSpd，t_prv为预瞄时间PrvT。

进一步地，预瞄偏差计算模块根据目标轨迹参数LanePars和预瞄距离PrvDis计算预瞄点到轨迹的横向偏差PrvYErr的计算公式如下：

ey_prv＝A₀十A₁d_prv十A₂d_prv ²十A₃d_prv ³

其中，ey_prv为横向偏差PrvYErr，A0～A3分别为目标到轨迹的横向偏差、航向角、目标轨迹曲率和目标轨迹曲率变化率。

进一步地，反馈校正模块根据横向偏差PrvYErr计算前轮转角反馈控制量DetafFedBak采用的原型算法表示如下：

其中，u为控制输出，即前轮转角反馈控制量DetafFedBak，K_p为比例控制参数，k为抑制系数，e为横向偏差PrvYErr，为横向偏差PrvYErr的变化率，e_m,i为横向偏差PrvYErr的第i次峰值。

进一步地，反馈校正模块计算前轮转角反馈控制量DetafFedBak时，用e表示横向偏差PrvYErr，表示横向偏差PrvYErr的变化率，构建分别以横向偏差PrvYErr及其变化率作为横纵坐标的拟人化平面直角坐标系，并根据横向偏差PrvYErr及其变化率划分为六个模态，分别如下：

I)u＝sgn(e)·U_max，|e|≥e₁；

进一步地，转角计算模块用于根据前轮转角反馈控制量DetafFedBak计算期望的方向盘转角ExpSW的公式如下：

sw_exp＝δ_f×i×180/π

其中，sw_exp为期望的方向盘转角ExpSW，δ_f为前轮转角反馈控制量DetafFedBak，i为角传动比，π为圆周率。

相比现有技术，本发明的有益效果如下：

1、本发明所述一种拟人化车辆轨迹跟踪控制方法，根据本车侧向加速度及其变化率动态调整反馈控制器参数，模拟了人感知汽车运动响应后的驾驶操控特性，拟人化程度更高，可有效提高自动驾驶操控的舒适度和可信度效果。

2、本发明所述一种拟人化车辆轨迹跟踪控制方法，不需要复杂车辆动力学模型参数，计算量小、实时性好。

3、本发明所述一种拟人化车辆轨迹跟踪控制方法，所使用的轨迹跟踪控制系统采用模块化架构，方便单一模块算法更新而不必修改整个系统的程序。

附图说明

图1为实施例所述轨迹跟踪控制系统的组成示意图；

图2为实施例的一种拟人化车辆轨迹跟踪控制方法的场景示意图；

图3为实施例所述拟人化平面直角坐标系的示意图。

具体实施方式

下面结合附图对本发明的具体实施方式作进一步的详细说明。

实施例：

请参见图1，一种拟人化车辆轨迹跟踪控制方法，包括轨迹跟踪控制系统，轨迹跟踪控制系统包括状态机信息模块、轨迹规划信息模块、CAN总线模块和轨迹跟踪控制器；

轨迹跟踪控制器包括输入信号单元、计算单元和输出信号单元；输入信号单元包括有效诊断模块和信息处理模块，计算单元包括预瞄模块、预瞄偏差计算模块、反馈校正模块和转角计算模块，输出信号单元包括安全限制模块和滤波处理模块。

有效诊断模块的输入信号为来自状态机信息模块的状态机信息，包括自动驾驶状态ADSSts、偏移状态OffsetSts、换道状态LaneChgSts和电动助力转向系统状态EPSSts；有效诊断模块的输出信号为控制信号isLATact并发送至计算单元；即有效诊断模块的作用是根据状态机信息决策计算单元是否需要激活。

信息处理模块的输入信号为来自轨迹规划信息模块的目标轨迹参数LanePars和来自CAN总线模块的侧向加速度Ay、车速VehSpd和方向盘转角StrWhlAng，信息处理模块用于对上述数据进行解析和格式转换等，信息处理模块向预瞄模块传输目标轨迹参数LanePars和车速VehSpd，向安全限制模块传输侧向加速度Ay、车速VehSpd和方向盘转角StrWhlAng，向预瞄偏差计算模块传输目标轨迹参数LanePars；

目标轨迹参数LanePars包括目标到轨迹的横向偏差、航向角、目标轨迹曲率和目标轨迹曲率变化率，目标轨迹用三次多项式表达如下：

y＝A₀十A₁x十A₂x²+A₃x³

其中，y为横向坐标，x为纵向坐标，A₀～A₃分别为目标到轨迹的横向偏差、航向角、目标轨迹曲率和目标轨迹曲率变化率。

预瞄模块的输入信号为目标轨迹参数LanePars和车速VehSpd，输出信号为预瞄距离PrvDis；

具体地，请参见图2所示应用场景，预瞄模块首先根据目标轨迹曲率PrvCurv计算出预瞄时间PrvT，再根据车速VehSpd和预瞄时间PrvT计算出预瞄距离PrvDis；

本实施例中，预瞄模块根据目标轨迹曲率PrvCurv决策出相应的预瞄时间PrvT，目标轨迹曲率越大预瞄时间PrvT越小，预瞄时间PrvT的上限为3秒，下限为0.5秒；预瞄时间PrvT的计算公式如下：

t_prv＝min(3，max(a×e^(-b×ρ)+c×e^(d×p),0.5))

其中，t_prv为预瞄时间PrvT，ρ为目标轨迹曲率PrvCurv，a、b、c和d为系数，e为自然对数的底数；

预瞄距离PrvDis的计算公式如下：

d_prv＝v_x×t_prv

其中，d_prv为预瞄距离PrvDis，v_x为车速VehSpd，t_prv为预瞄时间PrvT。

预瞄偏差计算模块的输入信号为预瞄距离PrvDis和目标轨迹参数LanePars，输出信号为预瞄点到轨迹的横向偏差PrvYErr，计算公式如下：

ey_prv＝A₀十A₁d_prv+A₂d_prv ²+A₃d_prv ³

其中，ey_prv为横向偏差PrvYErr，A0～A3分别为目标到轨迹的横向偏差、航向角、目标轨迹曲率和目标轨迹曲率变化率。

反馈校正模块的输入信号为横向偏差PrvYErr，输出信号为前轮转角反馈控制量DetafFedBak，反馈校正模块用于模拟人开车的驾驶行为，根据预瞄点到轨迹的横向偏差PrvYErr的大小及其变化率实时调整反馈控制器参数，以获得较为精确地控制输出；

当偏差趋于增加或保持恒定不变时采用比例控制模态，由此产生强烈的闭环控制作用，当在偏差趋于减小或为零时取消强的比例控制作用，反馈校正模块根据横向偏差PrvYErr计算前轮转角反馈控制量DetafFedBak采用的原型算法表示如下：

其中，u为控制输出，即前轮转角反馈控制量DetafFedBak，K_p为比例控制参数，k为抑制系数，e为横向偏差PrvYErr，为横向偏差PrvYErr的变化率，e_m,i为横向偏差PrvYErr的第i次峰值；

具体地，请参见附图3所示，反馈校正模块计算前轮转角反馈控制量DetafFedBak时，构建分别以横向偏差PrvYErr及其变化率作为横纵坐标的拟人化平面直角坐标系，用e表示横向偏差PrvYErr，表示横向偏差PrvYErr的变化率，/>表示目标位置并用箭头标识出其移动轨迹；根据横向偏差PrvYErr及其变化率将运行控制级划分为六个模态，分别如下：

1)在区域①，偏差较大，采用比例模态控制，系统产生强烈的闭环控制作用，使误差尽快减小，对应的控制模态1：u＝sgn(e)·U_max，|e|≥e₁；

2)在区域②，偏差较大，在偏差减小过程中偏差变化速度较小，采用比例加积分控制模态；对应的控制模态2：u＝k_p2·e+k_i2∫edt

3)在区域③和④，在偏差减小过程中，若偏差变化速度大于预设速度，引入微分模态，采用比例加微分控制；对应的控制模态3：

4)在区域③和④，在偏差减小过程中，若偏差变化速度大于预设速度，引入微分模态，采用比例加微分控制；对应的控制模态4：

5)在区域⑤，在偏差减小过程中，若偏差变化速度小于等于预设速度，采用比例加积分控制模态；对应的控制模态5：u＝k_p5·e+k_i5∫edt，

6)在区域⑥，偏差和偏差变化速度均较小，则采用比例加微分加积分控制模态，以消除误差；对应的控制模态6：

在区域⑤偏差及偏差变化率均很小时，采用PID控制(比例积分微分控制)消除误差；在区域②偏差较大且保持常值时，在比例控制基础上引入弱微分控制；在区域④若偏差变化速度较大，对应区域③，则采用比例加强微分控制。

转角计算模块的输入信号为前轮转角反馈控制量DetafFedBak，输出信号为期望的方向盘转角ExpSW，计算公式如下：

sw_exp＝δ_f×i×180/π

其中，sw_exp为期望的方向盘转角ExpSW，δ_f为前轮转角反馈控制量DetafFedBak，i为角传动比，π为圆周率。

安全限制模块的输入信号为期望的方向盘转角ExpSW、侧向加速度Ay、车速VehSpd和方向盘转角StrWhlAng，安全限制模块的输出信号为状态信号LATCtrlSts，状态信号LATCtrlSts用于向状态机信息模块反馈轨迹跟踪控制器的状态，安全限制模块用于根据侧向加速度Ay、车速VehSpd和方向盘转角StrWhlAng对方向盘转角ExpSW进行赋值和速率限制得到符合功能安全的方向盘转角ExpSafSW并传输至滤波处理模块。

滤波处理模块的输入信号为方向盘转角ExpSafSW，输出信号为最终的方向盘转角ExpFinalSW，滤波处理模块用于对方向盘转角ExpSafSW进行低通滤波以平滑输出最终的方向盘转角ExpFinalSW。

最后说明的是，以上实施例仅用以说明本发明的技术方案而非限制，尽管参照较佳实施例对本发明进行了详细说明，本领域的普通技术人员应当理解，可以对本发明的技术方案进行修改或者等同替换，而不脱离本发明技术方案的宗旨和范围，其均应涵盖在本发明的权利要求范围当中。

Claims (8)

1.一种拟人化车辆轨迹跟踪控制方法，其特征在于：包括轨迹跟踪控制系统，轨迹跟踪控制系统包括轨迹跟踪控制器，轨迹跟踪控制器包括计算单元，计算单元包括预瞄模块、预瞄偏差计算模块、反馈校正模块和转角计算模块；

预瞄模块用于根据目标轨迹曲率和车速VehSpd计算出预瞄距离PrvDis并传输至预瞄偏差计算模块，预瞄偏差计算模块用于根据目标轨迹参数LanePars和预瞄距离PrvDis计算出预瞄点到轨迹的横向偏差PrvYErr并传输至反馈校正模块，反馈校正模块用于根据横向偏差PrvYErr计算出前轮转角反馈控制量DetafFedBak并传输至转角计算模块，转角计算模块用于根据前轮转角反馈控制量DetafFedBak计算出期望的方向盘转角ExpSW并输出；

轨迹跟踪控制器还包括输入信号单元和输出信号单元，输入信号单元包括有效诊断模块和信息处理模块，输出信号单元包括安全限制模块和滤波处理模块；

有效诊断模块用于根据状态机信息决策是否激活计算单元，信息处理模块用于向预瞄模块传输目标轨迹参数LanePars和车速VehSpd，还用于向安全限制模块传输侧向加速度Ay、车速VehSpd和方向盘转角StrWhlAng；安全限制模块用于根据侧向加速度Ay、车速VehSpd和方向盘转角StrWhlAng对方向盘转角ExpSW进行赋值和速率限制得到符合功能安全的方向盘转角ExpSafSW并传输至滤波处理模块，滤波处理模块用于对方向盘转角ExpSafSW进行低通滤波得到最终的方向盘转角ExpFinalSW并输出；

目标轨迹参数LanePars包括目标到轨迹的横向偏差、航向角、目标轨迹曲率和目标轨迹曲率变化率，目标轨迹用三次多项式表达如下：

y＝A₀+A₁x+A₂x²+A₃x³

其中，y为横向坐标，x为纵向坐标，A₀～A₃分别为目标到轨迹的横向偏差、航向角、目标轨迹曲率和目标轨迹曲率变化率。

2.根据权利要求1所述一种拟人化车辆轨迹跟踪控制方法，其特征在于：轨迹跟踪控制系统还包括状态机信息模块、轨迹规划信息模块和CAN总线模块；

状态机信息模块用于向有效诊断模块传输状态机信息，还用于接收安全限制模块输出的轨迹跟踪控制器的状态信息LATCtrlSts；轨迹规划信息模块用于向信息处理模块传输目标轨迹参数LanePars；CAN总线模块用于向信息处理模块传输侧向加速度Ay、车速VehSpd和方向盘转角StrWhlAng，还用于接收滤波处理模块输出的最终的方向盘转角ExpFinalSW。

3.根据权利要求1所述一种拟人化车辆轨迹跟踪控制方法，其特征在于：状态机信息包括自动驾驶状态ADSSts、偏移状态OffsetSts、换道状态LaneChgSts和电动助力转向系统状态EPSSts。

4.根据权利要求1所述一种拟人化车辆轨迹跟踪控制方法，其特征在于：预瞄模块首先根据目标轨迹曲率PrvCurv计算出预瞄时间PrvT，再根据车速VehSpd和预瞄时间PrvT计算出预瞄距离PrvDis；

预瞄时间PrvT的计算公式如下：

t_prv＝min(3,max(a×e^(-b×ρ)+c×e^(d×ρ),0.5))

其中，t_prv为预瞄时间PrvT，且上限为3秒，下限为0.5秒；ρ为目标轨迹曲率PrvCurv，a、b、c和d为系数，e为自然对数的底数；

预瞄距离PrvDis的计算公式如下：

d_prv＝v_x×t_prv

其中，d_prv为预瞄距离PrvDis，v_x为车速VehSpd，t_prv为预瞄时间PrvT。

5.根据权利要求1所述一种拟人化车辆轨迹跟踪控制方法，其特征在于：预瞄偏差计算模块根据目标轨迹参数LanePars和预瞄距离PrvDis计算预瞄点到轨迹的横向偏差PrvYErr的计算公式如下：

t_yprv＝A₀+A₁d_prv+A₂d_prv ²+A₃d_prv ³

其中，ey_prv为横向偏差PrvYErr，A0～A3分别为目标到轨迹的横向偏差、航向角、目标轨迹曲率和目标轨迹曲率变化率。

6.根据权利要求1所述一种拟人化车辆轨迹跟踪控制方法，其特征在于：反馈校正模块根据横向偏差PrvYErr计算前轮转角反馈控制量DetafFedBak采用的原型算法表示如下：

其中，u为控制输出，即前轮转角反馈控制量DetafFedBak，K_p为比例控制参数，k为抑制系数，e为横向偏差PrvYErr，为横向偏差PrvYErr的变化率，e_m,i为横向偏差PrvYErr的第i次峰值。

7.根据权利要求6所述一种拟人化车辆轨迹跟踪控制方法，其特征在于：反馈校正模块计算前轮转角反馈控制量DetafFedBak时，用e表示横向偏差PrvYErr，表示横向偏差PrvYErr的变化率，构建分别以横向偏差PrvYErr及其变化率作为横纵坐标的拟人化平面直角坐标系，并根据横向偏差PrvYErr及其变化率划分为六个模态，分别如下：

1)u＝sgn(e)·U_max，|e|≥e₁；

2)

3)

4)

5)

6)

8.根据权利要求1所述一种拟人化车辆轨迹跟踪控制方法，其特征在于：转角计算模块用于根据前轮转角反馈控制量DetafFedBak计算期望的方向盘转角ExpSW的公式如下：

sw_exp＝δ_f×i×180/π

其中，sw_exp为期望的方向盘转角ExpSW，δ_f为前轮转角反馈控制量DetafFedBak，i为角传动比，π为圆周率。