CN112109732A - 一种智能驾驶自适应曲线预瞄方法 - Google Patents

Abstract

本发明属于转向自动驾驶操纵领域，具体涉及一种智能驾驶自适应曲线预瞄方法。包括以下步骤：根据离散化后的目标路径和作业平台的绝对坐标，利用提出的自适应曲线预瞄方法确定预瞄点在大地坐标系下的位置；通过坐标转换得到预瞄点相对于作业平台的横向偏移量；根据横向偏移量与方向盘转角之间的关系，表征出方向盘目标转角并作为转向操纵控制器的跟踪目标，同时作业平台输出运动状态信息并计算其绝对坐标。本发明的方法，能够适用于各种具有自动驾驶功能的作业平台，实现的转向操纵能够在速度和路径曲率发生变化时，通过改变预瞄点的位置达到自适应调整的目标。

Description

一种智能驾驶自适应曲线预瞄方法

技术领域

本发明属于转向自动驾驶操纵领域，具体涉及一种智能驾驶自适应曲线预瞄方法。

背景技术

预瞄式横向操纵策略作为智能驾驶中的一项核心和关键技术，依据不同的要求和目标，可以应用到各类作业平台上，包括但不仅限于对底盘进行改装的无人驾驶作业平台、驾驶机器人操纵的作业平台、具备自动驾驶功能的工程机械等。区别于非预瞄式横向操纵策略中最小化横向偏差或航向偏差的做法，预瞄式横向操纵策略以作业平台前方预瞄点处的位姿误差为输入，横向稳定性好且不存在转向过度的问题，但是以单点预瞄为主的传统预瞄方法在速度变化和大曲率转向情况下的精度会急剧下降，如何优化预瞄式横向操纵策略在上述工况下的表现是完善预瞄方法的关键所在。

专利CN107097785B根据车辆的运动信息以及参考路径曲率计算预瞄点处的横向误差和方向误差，并将上述误差按一定权重组成综合误差，通过最小化综合误差的方式从而达到提高跟踪精度和操纵稳定性的目的。在预瞄距离的设计环节中，通过加入预瞄距离补偿，从而达到预瞄距离自适应的目标，但是补偿方法属于一种滞后性的操纵，并没有从根源上对预瞄距离进行设计。

专利CN107264621B提出了一种预瞄距离计算方法，其值的计算以固定步长向前延伸，并乘以相应的权重，最后累加得到。在权重设计中，距离车辆越远，航向角累积变化越大，权重越小。该方法通过累积航向角来设置权重的方式，充分考虑了路径曲率变化的影响，具有较好的路径曲率自适应性，但是对于速度变化的适应能力却没有研究。

专利CN106882185A综合了道路信息和驾驶员视野特性来设计预瞄区域，根据离预瞄焦点的距离为预瞄焦点附近的区域设置不同权重。通过权重的设计，给出了融合预瞄路径上所有点信息的横向误差计算方式，然后再计算预瞄偏差量并作为方向盘目标转角的设计依据。该方法提高了弯曲道路情况下的跟踪精度，但是预瞄焦点与车辆质心之间的距离相对固定，没有考虑到速度的影响。

专利CN102358287A公开了一种用于车辆自动驾驶机器人的轨迹跟踪控制方法，该方法在单点预瞄方法的基础上，根据预设的速度阈值为预瞄距离设置了分段固定增益，有效地解决了以往单点预瞄方法在速度变化时适应性差的问题，但是在进行大曲率路径跟踪时会产生较大的横向误差。

专利CN104571112B公开了一种根据估计转弯曲率获取预瞄距离的方法，首先基于道路的曲率设定预瞄距离和两个预瞄点，然后计算最优转弯曲率和方向盘转角控制量，并输出到自动驾驶仪以完成车辆的横向控制。该方法充分考虑了道路曲率对跟踪精度的影响，通过估计道路曲率来动态调整预瞄距离，有效提高了跟踪的精度，但是没有涉及速度变化对预瞄效果的影响这一方面的研究。

综上所述，上述预瞄式横向操纵策略的发明专利，没有兼顾路径曲率和速度的自适应性需求，采用补偿的方式来修正预瞄距离存在一定的滞后性，引入权重来设计预瞄距离的方案以及增加预瞄点的方案会极大地增加预瞄的复杂程度。本专利通过改变预瞄距离的定义方式，同时为预瞄距离引入关于速度和预瞄时间的一阶校正项，从而使提出方法对于路径曲率和速度具有较好的自适应能力。

发明内容

本发明的目的在于提供一种自适应曲线预瞄方法，通过引入自适应曲线预瞄方法，可以使得弯曲道路和速度变化情况下的跟踪效果得到明显优化。

实现本发明目的的技术解决方案为：一种智能驾驶自适应曲线预瞄方法，包括如下步骤：

步骤(1)：根据稳态圆周工况下的横向动力学响应，求解前轮目标转角；

步骤(2)：在单点预瞄方法的基础上，提出自适应曲线预瞄方法，所述自适应曲线预瞄方法以作业平台的质心为原点C，建立相对坐标系xCy，作业平台在行驶中的任一时刻均看作沿着曲率半径为R，曲率中心为G的轨迹L行驶，假设沿着x轴正方向有一个参考点T，T’为GT连线与轨迹L的交点，d为T到C点的距离；在自适应曲线预瞄中，预瞄点P为L上一点，满足弧CP的长度等于d，ε*为P到x轴的距离，即预瞄点横向偏移量，预瞄距离d模型为：

d＝d_p+ξt_pu

式中：d_p为基础预瞄距离；ξ为时间系数；t_p为预瞄时间；

步骤(3)：将目标路径离散化，根据步骤(2)提出的自适应曲线预瞄方法，联立作业平台的位置信息，用绝对坐标表征出预瞄点横向偏移量；

步骤(4)：根据步骤(3)的计算结果求解方向盘目标转角。

进一步的，所述步骤(1)中的“根据稳态圆周工况下的横向动力学响应，求解前轮目标转角”具体为：

仅考虑作业平台的横向运动和横摆运动，得到稳态圆周工况下的横向动力学方程为：

式中：k₁和k₂为前后轮的侧偏角刚度；m为作业平台整备质量；β为质心侧偏角；a为前轴到作业平台质心距离；b为后轴到作业平台质心距离；ω_r为横摆角速度；δ为前轮转角；u为纵向速度。

求解上述动力学方程可以得到对应曲率半径R的前轮目标转角为：

式中：

进一步的，所述步骤(3)预瞄点横向偏移量的求解过程如下：

步骤(31)：用相对坐标表征预瞄点横向偏移量；

步骤(32)：用绝对坐标表征预瞄点横向偏移量。

进一步的，所述步骤(31)用相对坐标表征预瞄点横向偏移量具体为：

利用几何知识分析自适应曲线预瞄过程，预瞄点横向偏移量用相对坐标系下的转弯中心与作业平台质心纵坐标之差减去转弯中心与预瞄点纵坐标之差求得，用相对坐标表征出预瞄点横向偏移量为：

考虑到d<<R，有

预瞄点横向偏移量ε*简化为

ε*≈R cos β-R cos β+d sin β＝d sin β

由于

再根据步骤(1)求得的前轮目标转角δ*，最终得出相对坐标系下，δ*和ε*之间的关系为

进一步的，所述步骤(32)用绝对坐标表征预瞄点横向偏移量具体为：

对目标路径离散化(X_R,Y_R)→(X_R,i,Y_R,i,S_R,i)，其中X_R和Y_R表示大地坐标系下连续的目标路径横纵坐标，X_R,i和Y_R,i表示大地坐标系下离散化后目标路径横纵坐标，S_R,i表示点(X_R,i,Y_R,i)到起始点(X_R,0,Y_R,0)的曲线长度；利用线性差值，确定预瞄点P(X_P,Y_P,S_P)的信息为：

式中：X_P和Y_P为大地坐标系下预瞄点P的横纵坐标；S_P为预瞄点P到作业平台当前实际位置曲线长度。

在大地坐标系下，作业平台质心坐标为(X_C,Y_C)，参考点坐标为(X_T,Y_T)，预瞄点坐标为(X_P,Y_P)；P为目标路径上到质心路程恰好等于预瞄距离d的一点，利用坐标系旋转矩阵，可以得到P在相对坐标系xCy中的信息为：

求得P在相对坐标系中的坐标为：

预瞄点横向偏移量ε*即为y_P的相反数，因此ε*用绝对坐标表征为：

式中：X_C、Y_C和

分别表示作业平台在大地坐标系中的坐标和方位角。

进一步的，所述步骤(4)具体为：

根据步骤(31)给出的前轮目标转角和预瞄点横向偏移量之间的关系和步骤(32)中的预瞄点横向偏移量，最终求得方向盘目标转角为：

式中：i_sw为转向系传动比

本发明与现有技术相比，其显著优点在于：

通过输入作业平台实际位置、运动状态以及目标路径信息，利用提出的自适应曲线预瞄方法可以在线实时地给出前轮目标转角，且在目标转角的设计中充分考虑了路径曲率和速度的影响，并能够在路径曲率和速度发生变化时，对预瞄点做出自适应调整。

附图说明

图1为本发明的自适应曲线预瞄流程图。

图2为本发明的作业平台横向动力学模型图。

图3为相对坐标系下稳态行驶图。

图4为大地坐标系下的自适应曲线预瞄示意图。

图5为本发明双移线工况试验结果图；其中(a)为不同速度跟踪效果对比图，(b)为不同预瞄方法跟踪效果对比，(c)为不同预瞄方法横向误差对比。

图6为本发明蛇形工况试验结果；其中(a)为不同预瞄方法跟踪效果对比，(b)为不同预瞄方法横向误差对比。

具体实施方式

下面结合附图，详细说明本发明的具体实施方式以及相关的试验验证。

在阅读了本发明之后，本领域技术人员对本发明的各种等价形式的修改均落于本申请所附权利要求所限定的范围。

图1为本发明提出的自适应曲线预瞄方法在具备自动驾驶功能的作业平台上应用的预瞄流程图，描述了利用自适应曲线预瞄方法跟踪目标路径整个流程。

步骤1：根据稳态动力学响应求解前轮目标转角

仅考虑作业平台的横向运动和横摆运动，并限定横向加速度在0.4g以下，得到如图2所示的二自由度模型，在稳态圆周工况下，横向动力学可以用下式描述。

式中：k₁和k₂为前后轮的侧偏角刚度；m为作业平台整备质量；β为质心侧偏角；a为前轴到作业平台质心距离；b为后轴到作业平台质心距离；ω_r为横摆角速度；δ为前轮转角；u为纵向速度。

求解上述方程组可得对应曲率半径R的前轮目标转角为

式中：

步骤2：设计自适应预瞄距离

如图3所示，以作业平台的质心为原点C，建立相对坐标系xCy，作业平台在行驶中的任一时刻均可看作沿着曲率半径为R，曲率中心为G的轨迹L行驶。假设沿着x轴正方向有一个参考点T，T’为GT连线与轨迹L的交点，d为T到C点的距离；在自适应曲线预瞄中，预瞄点P为L上一点，满足弧CP的长度等于d，ε*为P到x轴的距离，即预瞄点横向偏移量，预瞄距离模型d由下式给出

d＝d_p+ξt_pu (3)

式中：d_p为基础预瞄距离；ξ为时间系数；t_p为预瞄时间。

步骤3：求解预瞄点横向偏移量

(3.1)用相对坐标表征预瞄点横向偏移量

根据图3，利用几何关系可以用相对坐标表征出预瞄点横向偏移量为

考虑到d<<R，有

ε*可以简化为

ε*≈R cos β-R cos β+d sin β＝d sin β (5)

(3.2)求解前轮目标转角和预瞄点横向偏移量之间的关系

于是根据式(2)和(5)，可以求得前轮目标转角δ*和预瞄点横向偏移量ε*之间的关系为

(3.3)确定作业平台位置

作业平台位置由X_C、Y_C和

确定，其中X_C和Y_C为大地坐标系下作业平台的横纵坐标，

为作业平台的航向角。X_C、Y_C和

这三个参数和质心侧偏角β、横摆角速度ω_r以及速度V之间的关系可以通过如下方程式确定。

(3.4)确定预瞄点绝对坐标

将目标路径离散化，这里采用微元法，用直线来代替每一小段曲线的弧长，求和得到最终的曲线长度。具体地，首先定义目标路径上各点的信息为(X_R,i,Y_R,i,S_R,i)，其中X_R,i和Y_R,i表示大地坐标系下离散化后目标路径横纵坐标，S_R,i表示点(X_R,i,Y_R,i)到起始点(X_R,0,Y_R,0)的曲线长度。当离开起始点后，假设它始终沿着规划路径行驶，行驶路程为S。点集上必将存在且唯一存在相邻的两点(X_R,i,Y_R,i,S_R,i)和(X_R,i-1,Y_R,i-1,S_R,i-1)，使得S_R,i-1≤S+d≤S_R,i，利用线性差值，确定预瞄点P(X_P,Y_P,S_P)的信息为

(3.5)用绝对坐标表征预瞄点横向偏移量

如图4所示，作业平台质心坐标为(X_C,Y_C)，参考点坐标为(X_T,Y_T)，预瞄点坐标为(X_P,Y_P)；P为目标路径上到质心路程恰好等于预瞄距离d的一点，利用坐标系旋转矩阵，可以得到P在相对坐标系xCy中的信息为

根据上式，P在相对坐标系中的纵坐标为

预瞄点横向偏移量ε*即为y_P的相反数，因此ε*可以表示为

步骤4求解方向盘目标转角

联立式(6)和式(11)，最终求得前轮方向盘目标转角为

式中：i_sw为转向系传动比；X_C,Y_C,

和X_P,Y_P分别由式(7)和(8)给出。

步骤5：多工况条件下跟踪效果验证

在得到式(12)所示的方向盘目标转角的表达式之后，本专利还在多工况条件下对提出的方法进行了仿真测试。首先设计了双移线工况下的转向操纵仿真试验。通过改变试验速度和换用不同的预瞄方法，得到如图5所示的结果。可以看出，在速度变化时，实际行驶轨迹变化不大且基本同目标路径重合，说明本发明提出的自适应曲线预瞄方法对速度具有较好的适应能力。在单点预瞄与自适应曲线预瞄的对比中，两种方法在拐点处均出现较大的横向误差，其中单点预瞄法还存在转向过度的问题，总体上本发明提出的自适应曲线预瞄方法的误差小于单点预瞄法。

然后设计了蛇形工况下的转向操纵仿真试验，其中标桩间距30m，速度控制在65km/h，通过仿真试验得到了如图6所示的结果。可以看出，在更为复杂的蛇形工况下，单点预瞄法的精度明显低于提出的自适应曲线预瞄，最大误差高出约50％，而提出的自适应曲线预瞄法的最大误差在0.6m左右，能够较好地适应路径曲率的变化。

本申请的方法对速度和路径曲率变化的适应能力较强，形式简单，可以应用于各种具备智能驾驶功能的作业平台，包括但不仅限于驾驶机器人操纵的作业平台、对底盘进行改装的无人驾驶作业平台、具有自动驾驶功能的工程机械等。

相比于单点预瞄，本发明提出方法的预瞄点可根据路径曲率和速度做出自适应调整，具体的，在路径曲率变大时，预瞄点横向偏移量变大，纵向偏移量变小；路径曲率变小时，上述变化趋势相反；而在速度变大时，预瞄距离变大；速度变小时，预瞄距离变小；通过引入自适应曲线预瞄方法，可以使得弯曲道路和速度变化情况下的跟踪效果得到明显优化。

Claims (6)

1.一种智能驾驶自适应曲线预瞄方法，其特征在于，包括如下步骤：

步骤(1)：根据稳态圆周工况下的横向动力学响应，求解前轮目标转角；

步骤(2)：在单点预瞄方法的基础上，提出自适应曲线预瞄方法，所述自适应曲线预瞄方法以作业平台的质心为原点C，建立相对坐标系xCy，作业平台在行驶中的任一时刻均看作沿着曲率半径为R，曲率中心为G的轨迹L行驶，假设沿着x轴正方向有一个参考点T，T’为GT连线与轨迹L的交点，d为T到C点的距离；在自适应曲线预瞄中，预瞄点P为L上一点，满足弧CP的长度等于d，ε*为P到x轴的距离，即预瞄点横向偏移量，预瞄距离d模型为：

d＝d_p+ξt_pu

式中：d_p为基础预瞄距离；ξ为时间系数；t_p为预瞄时间；

步骤(3)：将目标路径离散化，根据步骤(2)提出的自适应曲线预瞄方法，联立作业平台的位置信息，用绝对坐标表征出预瞄点横向偏移量；

步骤(4)：根据步骤(3)的计算结果求解方向盘目标转角。

2.根据权利要求1所述的方法，其特征在于，所述步骤(1)中的“根据稳态圆周工况下的横向动力学响应，求解前轮目标转角”具体为：

仅考虑作业平台的横向运动和横摆运动，得到稳态圆周工况下的横向动力学方程为：

式中：k₁和k₂为前后轮的侧偏角刚度；m为作业平台整备质量；β为质心侧偏角；a为前轴到作业平台质心距离；b为后轴到作业平台质心距离；ω_r为横摆角速度；δ为前轮转角；u为纵向速度。

求解上述动力学方程可以得到对应曲率半径R的前轮目标转角为：

式中：

3.根据权利要求2所述的方法，其特征在于，所述步骤(3)预瞄点横向偏移量的求解过程如下：

步骤(31)：用相对坐标表征预瞄点横向偏移量；

步骤(32)：用绝对坐标表征预瞄点横向偏移量。

4.根据权利要求3所述的方法，其特征在于，所述步骤(31)用相对坐标表征预瞄点横向偏移量具体为：

利用几何知识分析自适应曲线预瞄过程，预瞄点横向偏移量用相对坐标系下的转弯中心与作业平台质心纵坐标之差减去转弯中心与预瞄点纵坐标之差求得，用相对坐标表征出预瞄点横向偏移量为：

考虑到d<<R，有

预瞄点横向偏移量ε*简化为

ε*≈Rcosβ-Rcosβ+dsinβ＝dsinβ

由于

再根据步骤(1)求得的前轮目标转角δ*，最终得出相对坐标系下，δ*和ε*之间的关系为

5.根据权利要求4所述的方法，其特征在于，所述步骤(32)用绝对坐标表征预瞄点横向偏移量具体为：

对目标路径离散化(X_R,Y_R)→(X_R,i,Y_R,i,S_R,i)，其中X_R和Y_R表示大地坐标系下连续的目标路径横纵坐标，X_R,i和Y_R,i表示大地坐标系下离散化后目标路径横纵坐标，S_R,i表示点(X_R,i,Y_R,i)到起始点(X_R,0,Y_R,0)的曲线长度；利用线性差值，确定预瞄点P(X_P,Y_P,S_P)的信息为：

式中：X_P和Y_P为大地坐标系下预瞄点P的横纵坐标；S_P为预瞄点P到作业平台当前实际位置曲线长度；

在大地坐标系下，作业平台质心坐标为(X_C,Y_C)，参考点坐标为(X_T,Y_T)，预瞄点坐标为(X_P,Y_P)；P为目标路径上到质心路程恰好等于预瞄距离d的一点，利用坐标系旋转矩阵，可以得到P在相对坐标系xCy中的信息为：

求得P在相对坐标系中的坐标为：

预瞄点横向偏移量ε*即为y_P的相反数，因此ε*用绝对坐标表征为：

式中：X_C、Y_C和

分别表示作业平台在大地坐标系中的坐标和方位角。

6.根据权利要求5所述的方法，其特征在于，所述步骤(4)具体为：

根据步骤(31)给出的前轮目标转角和预瞄点横向偏移量之间的关系和步骤(32)中的预瞄点横向偏移量，最终求得方向盘目标转角为：

式中：i_sw为转向系传动比。

Images