基于转弯曲率估计的无人车横向控制方法

摘要

本发明公开了一种基于转弯曲率估计的无人车横向控制方法。它包括预定轨迹的设定和GPS‑INS组合定位系统数据的采集，以及向自动驾驶仪发出方向盘转角指令信号，特别是，基于先由道路的曲率设定预瞄距离，再由预瞄距离重新设定2个预瞄点A和B，之后，先将预瞄点A和B的经纬度变换到车辆坐标系，以计算出最优转弯曲率，再由最优转弯曲率计算出方向盘转角控制量，并输出到自动驾驶仪的方法实现了对无人车的横向控制。它经实测，当车速为20km/h时，最大跟踪误差仅为0.4m，当掉头车速5km/h时，最大跟踪误差仅0.6m，既实现了对各种复杂轨迹的精确跟随，又避免了单点预瞄反馈控制造成的方向盘抖动，使无人车驾驶的控制量更加平滑；可广泛地用于各种车辆的无人驾驶控制。

主权项

一种基于转弯曲率估计的无人车横向控制方法，包括预定轨迹的设定和GPS‑INS组合定位系统数据的采集，以及向自动驾驶仪发出方向盘转角指令信号，其特征在于主要步骤如下：步骤1，先将上一控制周期的预瞄距离d作为初始预瞄距离d₀，若无预瞄距离d，则初始预瞄距离d₀取值为15～30m，再采集GPS‑INS组合定位系统的车辆位置P、速度v和航向a的信息；步骤2，先选择预定轨迹中的第一点A0，并判断第一点A0是否为预定轨迹中的最后一点，若是，则结束控制，否则由车辆位置P、第一点A0和航向a得出向量以及向量与航向a的夹角，再判断向量与航向a的夹角是否大于90度，若是，则选择下一点作为新的第一点A0并进行新的判断直至小于等于90度；否则，计算出第一点A0到车辆位置P的距离s；步骤3，判断第一点A0到车辆位置P的距离s是否大于等于初始预瞄距离d₀，若是，则将第一点A0作为预瞄点，否则，选择下一点作为新的第一点A0并进行新的计算第一点A0到车辆位置P的距离s，以及再次判断s与d₀的大小关系，直至s≥d₀；步骤4，先从预定轨迹中找出距离车辆位置P的最近点D，并计算出最近点D到第一点A0的轨迹的近处曲率平均值ρ_near，以及第一点A0至其之后的2～10个点的轨迹的远处曲率平均值ρ_far，再由近处曲率平均值ρ_near和远处曲率平均值ρ_far按下式得到预瞄距离d，$d_1=\left\{\begin{array}{cc}{d}_0,& {\rho }_{near}>{\rho }_{far}\\ d_0-0.1v,& {\rho }_{near}\le {\rho }_{far}\end{array}\right.,$$d_{min}=\left\{\begin{array}{cc}20,& 0\le {\rho }_{far}<0.01\\ 15,& 0.01\le {\rho }_{far}<0.03\\ 8,& 0.03\le {\rho }_{far}<0.05\\ 6,& {\rho }_{far}\ge 0.05\end{array}\right.,$$d=\left\{\begin{array}{cc}{d}_1,& d_1>d_{min}\\ d_{min},& d_1\le d_{min}\end{array}\right.,$式中的d₀为初始预瞄距离、v为速度；步骤5，先以预瞄距离d的值作为预瞄距离，由预定轨迹中选择第一预瞄点A，并判断第一预瞄点A到车辆位置P的距离s是否大于等于预瞄距离d，若是，则将第一预瞄点A作为预瞄点，否则，选择下一点作为新的第一预瞄点A并进行新的计算第一预瞄点A到车辆位置P的距离s，以及再次判断s与d的大小关系，直至s≥d，再以同样的方法，在预瞄距离d上增加长度大于等于1m后的值作为预瞄距离，由预定轨迹中选择出第二预瞄点B；步骤6，以GPS‑INS组合定位系统的坐标原点为原点、x轴为沿车身中线指向车辆前方、y轴为指向车身左侧的车辆坐标系下，设第一预瞄点A的纬度、经度坐标为(X_A,Y_A)、以长度米为单位的车辆坐标为(x_A,y_A)、车辆位置P点的纬度、经度坐标为(X_P,Y_P)，按以下方法变换：$x=\frac{6371004\pi }{180}(Y_A-Y_P)cos\frac{{\mathrm{\pi x}}_A}{180}$$y=\frac{6371004\pi }{180}(X_A-X_P)$x_A＝ycosa+xsinay_A＝ysina‑xcosa，式中的a为航向，得到第一预瞄点A在车辆坐标系下的坐标(x_A，y_A)，再以同样的变换方法得到第二预瞄点B在车辆坐标系下的坐标(x_B，y_B)；步骤7，由第一预瞄点A和第二预瞄点B在车辆坐标系下的坐标(x_A,y_A),(x_B,y_B)，经下式计算得到最优转弯曲率$\rho =\frac{2(y_A^2+y_B^2)}{x_A^2{y}_A+x_B^2{y}_B+y_A^3+y_B^3};$步骤8，先由下式计算得到方向盘转角控制量$\delta =k_p\frac{180}{\pi }\rho I(L+{\mathrm{Kv}}^2)+{\delta }_0,$式中的I为车辆转向系传动比、L为车辆轴距、K为车辆不足转向系数、δ₀为方向盘零位偏移、k_p为用于调节动态性能的增益系数、v为速度，再将方向盘转角控制量δ输出到自动驾驶仪后，进入下一控制周期。