自动驾驶轨迹追踪方法、系统及存储介质技术方案

本发明专利技术公开了一种自动驾驶轨迹追踪方法、系统及存储介质，其中方法包括如下步骤：S100：根据轨迹上距自车最近轨迹点的横向误差及朝向误差，计算前馈控制量；S200：根据自车距预瞄点的横向误差，利用纯跟踪方法计算反馈控制量；S300：将自车距预瞄点的横向误差作为超前滞后控制器的输入误差，计算超前滞后控制量；S400：将前馈控制量、反馈控制量级超前滞后控制量相加获得车轮转角总输出。根据上述技术方案的自动驾驶轨迹追踪方法，通过引入由自车距最近轨迹点的横向误差及朝向误差计算的前馈控制量和超前滞后控制器到纯跟踪方法中，解决了输出转角震荡的问题，提高了自动驾驶系统的稳定性及控制精度。

【技术实现步骤摘要】
自动驾驶轨迹追踪方法、系统及存储介质
本专利技术涉及智能驾驶领域，特别涉及一种自动驾驶轨迹追踪方法、系统及存储介质。
技术介绍
随着科技的发展，各大汽车及互联网厂商都开始智能驾驶技术的研发。其中车辆横向控制技术是只能驾驶技术中的关键技术之一，其性能好坏会对实际的驾乘体验有着十分重要的影响。现有低速下常用的路径跟踪方法包括纯跟踪方法及Stanley方法。纯跟踪方法适用于低速、转角不大的场景，适用于大偏差和道路曲率不连续的工况，鲁棒性较高。现有的纯跟踪方法的改进与结合存在输出转角存在震荡、收敛效果较差的问题。
技术实现思路
专利技术目的：本专利技术的目的是提出一种自动驾驶轨迹追踪方法，可以解决输出转角震荡的问题，提高系统的稳定性及控制精度。本专利技术的另一目的是提出一种可以实施上述方法的系统及存储有上述方法的指令或者计算机程序的存储介质。技术方案：本专利技术所述的自动驾驶轨迹追踪方法，具体包括如下步骤：S100：根据轨迹上距自车最近轨迹点的横向误差及朝向误差，计算前馈控制量；S200：根据自车距预瞄点的横向误差，利用纯跟踪方法计算反馈控制量；S300：将自车距预瞄点的横向误差作为超前滞后控制器的输入误差，计算超前滞后控制量；S400：将前馈控制量、反馈控制量级超前滞后控制量相加获得车轮转角总输出。进一步的，所述步骤S400后还包括：步骤S500：利用低通滤波器对车轮转角总输出进行滤波。进一步的，所述步骤S100包括：>S110：对轨迹上的轨迹点进行分段处理，决定车辆的行驶方向；S120：对各分段轨迹进行延长处理。进一步的，所述步骤S110包括：S111：将当前第i个轨迹点的下一点坐标(Xi+1,Yi+1,Yawi+1)从当前的ENU坐标系转换至当前点(Xi,Yi,Yawi)坐标系之下得到(X′i+1,Y′i+1,Yawi)；S112：若X’i+1大于0，则沿当前方向前进；若X’i+1小于0，则倒退，每出现一次前进或倒退的转折，记录转折对应的轨迹点的索引i，并记录转折点数n加1；S113：重复步骤S111至S112直至轨迹上最后一个轨迹点，发生转折的轨迹点即为轨迹分段点，轨迹被轨迹分段点分为n+1段。6、进一步的，所述步骤S120包括：S121：确定轨迹延长距离Extend_dis＝V_car*T_extend，式中V_car为当前车速，T_extend为延长时间；S122：确定延长轨迹分段点数Extend_num＝Extend_dis/dt_extend式中dt_extend为分段间隔时间；S123：在轨迹分段点i处，计算延长曲率第j个延长轨迹分段点的坐标x和y分别为temp(j).x＝path(i).x+dt*cos(temp.heading)*(j+1)，temp(j).y＝path(i).y+dt*sin(temp.heading)*(j+1)，其中path(i).x和path(i).y分别为轨迹分段点i的坐标x和y，j＝1、2、3……Extend_num；S124：循环步骤S123直至最后一个轨迹分段点。进一步的，所述步骤S300包括：S310：计算预瞄距离Lf＝Lmin+V_car*T_lf，式中Lf为预瞄距离，Lmin为车辆的最小预瞄距离，V_car为当前自车车速，T_lf为预瞄时间；S320：从横向位置距离自车最近的轨迹点开始，计算到后续未经过的各轨迹点的距离，选择大于等于预瞄距离的点作为预瞄点；S330：计算自车位置与预瞄点的横向误差，并带入到纯跟踪方法计算反馈控制量。进一步的，所述低通滤波器的表达式如下：wa＝2.0*PI*cutoff_freq；alpha＝wa*ts/2.0；alpha_sqr＝alpha*alpha；tmp_term＝std::sqrt(2.0)*alpha+alpha_sqr；gain＝alpha_sqr/(1.0+tmp_term)；denominators(0)＝1；denominators(1)＝(2.0*(alpha_sqr-1.0)/(1.0+tmp_term)；denominators(2)＝(1.0-std::sqrt(2.0)*alpha+alpha_sqr)/(1.0+tmp_term)；numerators(0)＝gain；numerators(1)＝2*gain；numerators(2)＝gain；式中PI为用来deg转rad的转换单位，cutoff_freq为设计截止频率，ts为控制器运行周期，wa,alpha,alpha_sqr,tmp_term,gain都为低通滤波器设计时产生的临时参数，denominators和numerators为低通滤波器的传递函数的分母和分子的系数项。进一步的，所述步骤S500中滤波后获得的车轮转角值Steer_filter＝(numerators(0)*X_values(0)+numerators(1)*X_values(1)+numerators(2)*X_values(2)-denominators(1)*Y_values(0)-denominators(2)*Y_values(1))/denominators(0)，式中X_values(0)、X_values(1)、X_values(2)分别为当前周期、前一周期和前二周期的车轮转角总输出值，Y_values(0)、Y_values(1)、Y_values(2)分别为前一周期、前二周期及前三周期滤波后获得的车轮转角值。本专利技术所述的自动驾驶轨迹追踪系统，包括：轨迹处理模块，用于对轨迹进行分段和延长处理，获得与自车最近的轨迹点及当前自车位置的预瞄点；前馈控制模块，用于根据轨迹上距自车最近点的横向误差及朝向误差，计算前馈控制量；反馈控制模块，用于根据自车距预瞄点的横向误差，利用纯跟踪方法计算反馈控制量；超前滞后控制模块，用于用于根据自车距预瞄点的横向误差，计算超前滞后控制量；低通滤波器，用于对所述前馈控制量、所述反馈控制量及所述超前滞后控制量的和进行滤波，获得车轮转角值。本专利技术所述的存储介质，存储有计算机程序，其特征在于，所述计算机程序被设置为运行时执行上述自动驾驶轨迹追踪方法。有益效果：与现有技术相比，本专利技术具有如下优点：解决了输出转角震荡的问题，并对输出结果进行低通滤波处理，提高了系统的稳定性及控制精度。附图说明图1为本专利技术实施例的自动驾驶轨迹追踪方法的流程图；图2为本专利技术实施例的轨迹延长循环示意图；图3为本专利技术实施例的自动驾驶轨迹追踪系统的原理框图。具体实施方式下面结合附图对本专利技术的技术方案作进一步说明。参照图1，根据本专利技术实施例的自动驾驶轨迹追踪方法，包括如下步骤：S100：本文档来自技高网...

【技术保护点】
1.一种自动驾驶轨迹追踪方法，其特征在于，包括如下步骤：/nS100：根据轨迹上距自车最近轨迹点的横向误差及朝向误差，计算前馈控制量；/nS200：根据自车距预瞄点的横向误差，利用纯跟踪方法计算反馈控制量；/nS300：将自车距预瞄点的横向误差作为超前滞后控制器的输入误差，计算超前滞后控制量；/nS400：将前馈控制量、反馈控制量级超前滞后控制量相加获得车轮转角总输出。/n

【技术特征摘要】
1.一种自动驾驶轨迹追踪方法，其特征在于，包括如下步骤：
S100：根据轨迹上距自车最近轨迹点的横向误差及朝向误差，计算前馈控制量；
S200：根据自车距预瞄点的横向误差，利用纯跟踪方法计算反馈控制量；
S300：将自车距预瞄点的横向误差作为超前滞后控制器的输入误差，计算超前滞后控制量；
S400：将前馈控制量、反馈控制量级超前滞后控制量相加获得车轮转角总输出。


2.根据权利要求1所述的自动驾驶轨迹追踪方法，其特征在于，所述步骤S400后还包括：
步骤S500：利用低通滤波器对车轮转角总输出进行滤波。


3.根据权利要求1所述的自动驾驶轨迹追踪方法，其特征在于，所述步骤S100包括：
S110：对轨迹上的轨迹点进行分段处理，决定车辆的行驶方向；
S120：对各分段轨迹进行延长处理。


4.根据权利要求3所述的自动驾驶轨迹追踪方法，其特征在于，所述步骤S110包括：
S111：将当前第i个轨迹点的下一点坐标(Xi+1,Yi+1,Yawi+1)从当前的ENU坐标系转换至当前点(Xi,Yi,Yawi)坐标系之下得到(X′i+1,Y′i+1,Yawi)；
S112：若X’i+1大于0，则沿当前方向前进；若X’i+1小于0，则倒退，每出现一次前进或倒退的转折，记录转折对应的轨迹点的索引i，并记录转折点数n加1；
S113：重复步骤S111至S112直至轨迹上最后一个轨迹点，发生转折的轨迹点即为轨迹分段点，轨迹被轨迹分段点分为n+1段。


5.根据权利要求3所述的自动驾驶轨迹追踪方法，其特征在于，所述步骤S120包括：
S121：确定轨迹延长距离Extend_dis＝V_car*T_extend，式中V_car为当前车速，T_extend为延长时间；
S122：确定延长轨迹分段点数Extend_num＝Extend_dis/dt_extend式中dt_extend为分段间隔时间；
S123：在轨迹分段点i处，计算延长曲率
第j个延长轨迹分段点的坐标x和y分别为temp(j).x＝path(i).x+dt*cos(temp.heading)*(j+1)，temp(j).y＝path(i).y+dt*sin(temp.heading)*(j+1)，其中path(i).x和path(i).y分别为轨迹分段点i的坐标x和y，j＝1、2、3……Extend_num；
S124：循环步骤S123直至最后一个轨迹分段点。


6.根据权利要求1所述的自动驾驶轨迹追踪方法，其特征在于，所述步骤S300包括：
S310：计算预瞄距离Lf＝Lmin+V_car*T_lf，式中Lf为预瞄距离，Lmin为车辆的最小预瞄距离，V_car为当前自车车速，T_lf为预瞄时间；
S320：从横向位置距离自车最近的轨迹点开始，计算到后续未经过的各轨迹点的距离，选择大于等于预瞄距离的点作为预瞄点；
S330：计...

【专利技术属性】
技术研发人员：吴昊，
申请(专利权)人：的卢技术有限公司，
类型：发明
国别省市：江苏;32