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

Abstract

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

Description

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

技术领域

本发明涉及一种无人车横向控制方法，尤其是一种基于转弯曲率估计的无人车横向控制方法。

背景技术

无人驾驶车辆是未来智能车辆的发展趋势之一。无人车的研究也将推动智能交通和智慧城市的发展，具有重大的战略意义。为此，国内外众多机构都展开了对无人车的研发工作，如中国发明专利CN 102495631B于2013年8月21日公告的一种无人驾驶车辆跟踪预定轨迹的智能控制方法。该方法基于加装的车载计算机、GPS-INS组合定位系统和受车载计算机控制的车辆转向与速度伺服控制系统，采用路点坐标序列描述预定轨迹、等采样周期的计算机控制方式，先建立车身坐标系，确定车辆自身位置信息，再依据弧度值计算道路曲率特性的隶属度函数，定义模糊规则计算输出决策速度，之后按行驶1.5～2.5秒的距离在预定轨迹的坐标序列中搜索大于该距离并距车辆最近的轨迹点，使用自适应比例-微分-积分控制算法公式计算车辆运动的曲率，最后判断当前控制周期目标点是否为路点序列中被选中的最后一点：若是，则向刹车伺服控制系统输出信号启动刹车使车辆减速，若否，则使用控制器局域网模块输出电压信号保持控制油门开度使车辆继续前进。这种控制方法虽也能够使无人驾驶车辆实现对预定轨迹的跟踪功能，且对道路曲率特性变化具有一定的自适应能力，然其仅可近似的描述车辆附近的道路形状，而不能精确地描述道路的曲率，其在转向控制时，采用单点预瞄的横向误差PID(比例微分-积分)控制使得控制量受预瞄点横向误差的影响较大，当预瞄点存在跳动时，很容易引起方向盘转角的突变，导致控制量不够平滑。

发明内容

本发明要解决的技术问题为克服现有技术中的不足之处，提供一种在保证跟踪精度的同时，对无人驾驶车辆的控制更加平滑的基于转弯曲率估计的无人车横向控制方法。

为解决本发明的技术问题，所采用的技术方案为：基于转弯曲率估计的无人车横向控制方法包括预定轨迹的设定和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,& {\mathrm{\ρ}}_{\mathrm{near}}>{\mathrm{\ρ}}_{\mathrm{far}}\\ d_0-0.1v,& {\mathrm{\ρ}}_{\mathrm{near}}\≤{\mathrm{\ρ}}_{\mathrm{far}}\end{array}\right.,$

$d_{\min }=\left\{\begin{array}{cc}20,& 0\&le;{\mathrm{\&rho;}}_{\mathrm{far}}<0.01\\ 15,& 0.01\&le;{\mathrm{\&rho;}}_{\mathrm{far}}<0.03\\ 8,& 0.03\&le;{\mathrm{\&rho;}}_{\mathrm{far}}<0.05\\ 6,& {\mathrm{\&rho;}}_{\mathrm{far}}\&GreaterEqual;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\mathrm{\&pi;}}{180}(Y_A-Y_P)\cos \frac{\mathrm{\&pi;}{x}_A}{180}$

$y=\frac{6371004\mathrm{\&pi;}}{180}(X_A-X_P)$

x_A＝ycosa+xsina

y_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)，经下式计算得到最优转弯曲率

$\mathrm{\&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，先由下式计算得到方向盘转角控制量

$\mathrm{\&delta;}=k_p\frac{180}{\mathrm{\&pi;}}\mathrm{\&rho;I}(L+{\mathrm{Kv}}^2)+{\mathrm{\&delta;}}_0,$

式中的I为车辆转向系传动比、L为车辆轴距、K为车辆不足转向系数、δ₀为方向盘零位偏移、k_p为用于调节动态性能的增益系数、v为速度，再将方向盘转角控制量δ输出到自动驾驶仪后，进入下一控制周期。

作为基于转弯曲率估计的无人车横向控制方法的进一步改进：

优选地，预定轨迹为由m个目标点组成的序列，其中，每个目标点序列为一m×3维的数组描述，数组中每一行的三个元素分别代表轨迹中一点的纬度、经度和该点处的道路曲率。

优选地，GPS-INS组合定位系统的定位精度≤0.1m；利于确保控制的精度。

优选地，步骤8中的I＝14，L＝2.7m，K＝0.0003,δ₀＝5°,k_p＝1.25。

优选地，方向盘转角控制量δ经CAN总线输出到自动驾驶仪；使用现有的成熟技术，不仅稳定可靠，也利于降低成本。

优选地，进入下一控制周期前，先删除本控制周期位于车辆后方预定轨迹中的所有点；利于减小数据的冗余度，提高计算机的运行效率。

相对于现有技术的有益效果是：

其一，本发明基于先由道路的曲率设定预瞄距离，再由预瞄距离重新设定2个预瞄点A和B，之后，先将预瞄点A和B的经纬度变换到车辆坐标系，以计算出最优转弯曲率，再由最优转弯曲率计算出方向盘转角控制量，并输出到自动驾驶仪的方法对无人车的横向控制，既实现了对各种复杂轨迹的精确跟随，又避免了单点预瞄反馈控制造成的方向盘抖动，使无人车驾驶的控制量更加平滑。

其二，经对使用本发明的车辆轨迹的实际测量，当车速为20km/h时，其跟踪轨迹与预定轨迹几乎重合，最大跟踪误差仅为0.4m；且方向盘转动平稳，没有突变和毛刺。当掉头车速5km/h时，完成了非常复杂的轨迹跟随任务，且跟踪精度较高，最大跟踪误差仅0.6m。将其与掉头时单点预瞄PID控制量相比较，可看出本发明控制的方向盘转角明显地比PID控制时要平滑得多。

附图说明

图1是本发明的一种基本控制流程示意图。

图2是使用本发明的车辆轨迹跟随效果图，测试时的车速为20km/h。图中的虚线为预定轨迹、实线为实测的跟随轨迹；由图可看出，两者几乎重合。

图3是图2中方框的局部放大图。图中的虚线为预定轨迹、实线为实测的跟随轨迹；由图可看出，两者的误差很小，以20km/h的速度行驶，误差最大处仅0.4m。

图4是图2所示曲线对应的方向盘转角曲线。由图可看出，方向盘转动平滑，没有跳动和毛刺。

图5是使用本发明控制车辆掉头时的车辆轨迹跟随效果图，掉头时的车速为5km/h。图中的虚线为预定轨迹、实线为实测的跟随轨迹；可见在复杂的情况下，本方法依然有较高的跟踪精度。

图6是将掉头时的PID控制与本发明的方向盘转角进行比较的结果图。其中，图上部的曲线图为PID控制的方向盘转角、下部的曲线图为本发明的方向盘转角，可看出本发明的控制量明显地比PID的平滑。

具体实施方式

下面结合附图对本发明的优选方式作进一步详细的描述。

本实施例使用的车辆为中国芜湖奇瑞公司的“瑞虎”汽车，并于其上配接诺瓦泰(NovAtel)公司的型号为SPAN-CPT的GPS-INS组合定位系统和中科自控公司的型号为IPD1的自动驾驶仪，以及作为工控机的微型计算机。其中，GPS-INS组合定位系统用于获取车辆所在位置的经纬度、车辆速度信号和车辆航向角信号，以送往工控机；该系统在使用差分定位的情况下，其提供的定位精度为0.1m。自动驾驶仪通过CAN总线与工控机相连，用于接收工控机发出的车速控制信号和方向盘转角指令信号。

参见图1，基于转弯曲率估计的无人车横向控制方法的工作流程如下：

首先，采用路点坐标序列来描述预定轨迹。预定轨迹为由m个目标点组成的序列，其中，每个目标点序列为一m×3维的数组描述，数组中每一行的三个元素分别代表轨迹中一点的纬度、经度和该点处的道路曲率。预定轨迹事先存储于工控机中，以便程序运行时读取。

接着，以0.1s为控制周期，按以下具体步骤向车辆发出方向盘转角指令信号，以实现对车辆预定轨迹的跟踪：

步骤1，设定预瞄距离d₀和确定车辆的当前状态

先将上一控制周期的预瞄距离d作为初始预瞄距离d₀，若无预瞄距离d，即属起步，则初始预瞄距离d₀取值为15～30m。再采集GPS-INS组合定位系统的车辆位置P、速度v和航向a的信息。

步骤2，选定预定轨迹中的第一点A0并计算出第一点A0到车辆位置P的距离s

先选择预定轨迹中的第一点A0，并判断第一点A0是否为预定轨迹中的最后一点，若是，则结束控制，否则由车辆位置P、第一点A0和航向a得出向量以及向量与航向a的夹角。再判断向量与航向a的夹角是否>90度，若是，则因第一点A0在车辆的后方而选择下一点作为新的第一点A0并进行新的判断直至≤90度；否则，因此时的第一点A0在车辆的前方而需计算出第一点A0到车辆位置P的距离s。

步骤3，选取预瞄点

判断第一点A0到车辆位置P的距离s是否≥初始预瞄距离d₀，若是，则将第一点A0作为预瞄点，否则，选择下一点作为新的第一点A0并进行新的计算第一点A0到车辆位置P的距离s，以及再次判断s＜d₀，直至s≥d₀。

步骤4，由第一点A0处道路的曲率情况重新计算本控制周期的预瞄距离d

先从预定轨迹中找出距离车辆位置P的最近点D，并计算出最近点D到第一点A0的轨迹的近处曲率平均值ρ_near，以及第一点A0至其之后的3个点的轨迹的远处曲率平均值ρ_far。再由近处曲率平均值ρ_near和远处曲率平均值ρ_far按下式得到预瞄距离d，

$d_1=\left\{\begin{array}{cc}{d}_0,& {\mathrm{\&rho;}}_{\mathrm{near}}>{\mathrm{\&rho;}}_{\mathrm{far}}\\ d_0-0.1v,& {\mathrm{\&rho;}}_{\mathrm{near}}\&le;{\mathrm{\&rho;}}_{\mathrm{far}}\end{array}\right.,$

$d_{\min }=\left\{\begin{array}{cc}20,& 0\&le;{\mathrm{\&rho;}}_{\mathrm{far}}<0.01\\ 15,& 0.01\&le;{\mathrm{\&rho;}}_{\mathrm{far}}<0.03\\ 8,& 0.03\&le;{\mathrm{\&rho;}}_{\mathrm{far}}<0.05\\ 6,& {\mathrm{\&rho;}}_{\mathrm{far}}\&GreaterEqual;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和B

先以预瞄距离d的值作为预瞄距离，由预定轨迹中选择第一预瞄点A，并判断第一预瞄点A到车辆位置P的距离s是否≥预瞄距离d，若是，则将第一预瞄点A作为预瞄点，否则，选择下一点作为新的第一预瞄点A并进行新的计算第一预瞄点A到车辆位置P的距离s，以及再次判断s<d，直至s≥d。再以同样的方法，以预瞄距离d的值加长度为1m作为预瞄距离，由预定轨迹中选择出第二预瞄点B。

步骤6，以长度米为单位，将两个新的预瞄点A和B的经纬度变换到车辆坐标系

以GPS-INS组合定位系统的坐标原点为原点、x轴为沿车身中线指向车辆前方、y轴为指向车身左侧的车辆坐标系下，设第一预瞄点A的纬度、经度坐标为(X_A,Y_A)、以长度米为单位的车辆坐标为(x_A,y_A)、车辆位置P点的纬度、经度坐标为(X_P,Y_P)，按以下方法变换：

$x=\frac{6371004\mathrm{\&pi;}}{180}(Y_A-Y_P)\cos \frac{\mathrm{\&pi;}{x}_A}{180}$

$y=\frac{6371004\mathrm{\&pi;}}{180}(X_A-X_P)$

x_A＝ycosa+xsina

y_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)，经下式计算得到最优转弯曲率

$\mathrm{\&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，先由下式计算得到方向盘转角控制量

$\mathrm{\&delta;}=k_p\frac{180}{\mathrm{\&pi;}}\mathrm{\&rho;I}(L+{\mathrm{Kv}}^2)+{\mathrm{\&delta;}}_0,$

式中的I为车辆转向系传动比、L为车辆轴距、K为车辆不足转向系数、δ₀为方向盘零位偏移、k_p为用于调节动态性能的增益系数、v为速度；本实施例中，其取值分别为I＝14，L＝2.7m，K＝0.0003,δ₀＝5°,k_p＝1.25。再将方向盘转角控制量δ通过CAN总线输出到自动驾驶仪。

之后，先删除本控制周期位于车辆后方预定轨迹中的所有点，再进入下一控制周期。

显然，本领域的技术人员可以对本发明的基于转弯曲率估计的无人车横向控制方法进行各种改动和变型而不脱离本发明的精神和范围。这样，倘若对本发明的这些修改和变型属于本发明权利要求及其等同技术的范围之内，则本发明也意图包含这些改动和变型在内。

Claims (6)

1.一种基于转弯曲率估计的无人车横向控制方法，包括预定轨迹的设定和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,& {\mathrm{\&rho;}}_{\mathrm{near}}>{\mathrm{\&rho;}}_{\mathrm{far}}\\ d_0-0.1v,& {\mathrm{\&rho;}}_{\mathrm{near}}\&le;{\mathrm{\&rho;}}_{\mathrm{far}}\end{array}\right.,$

$d_{\min }=\left\{\begin{array}{cc}20,& 0\&le;{\mathrm{\&rho;}}_{\mathrm{far}}<0.01\\ 15,& 0.01\&le;{\mathrm{\&rho;}}_{\mathrm{far}}<0.03\\ 8,& 0.03\&le;{\mathrm{\&rho;}}_{\mathrm{far}}<0.05\\ 6,& {\mathrm{\&rho;}}_{\mathrm{far}}\&GreaterEqual;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\mathrm{\&pi;}}{180}(Y_A-Y_P)\cos \frac{{\mathrm{\&pi;x}}_A}{180}$

$y=\frac{6371004\mathrm{\&pi;}}{180}(X_A-X_P)$

x_A＝ycosa+xsina

y_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)，经下式计算得到最优转弯曲率

$\mathrm{\&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，先由下式计算得到方向盘转角控制量

$\mathrm{\&delta;}=k_p\frac{180}{\mathrm{\&pi;}}\mathrm{\&rho;I}(L+{\mathrm{Kv}}^2)+{\mathrm{\&delta;}}_0,$

式中的I为车辆转向系传动比、L为车辆轴距、K为车辆不足转向系数、δ₀为方向盘零位偏移、k_p为用于调节动态性能的增益系数、v为速度，再将方向盘转角控制量δ输出到自动驾驶仪后，进入下一控制周期。

2.根据权利要求1所述的基于转弯曲率估计的无人车横向控制方法，其特征是预定轨迹为由m个目标点组成的序列，其中，每个目标点序列为一m×3维的数组描述，数组中每一行的三个元素分别代表轨迹中一点的纬度、经度和该点处的道路曲率。

3.根据权利要求1所述的基于转弯曲率估计的无人车横向控制方法，其特征是GPS-INS组合定位系统的定位精度≤0.1m。

4.根据权利要求1所述的基于转弯曲率估计的无人车横向控制方法，其特征是步骤8中的I＝14，L＝2.7m，K＝0.0003,δ₀＝5°,k_p＝1.25。

5.根据权利要求1所述的基于转弯曲率估计的无人车横向控制方法，其特征是方向盘转角控制量δ经CAN总线输出到自动驾驶仪。

6.根据权利要求1所述的基于转弯曲率估计的无人车横向控制方法，其特征是进入下一控制周期前，先删除本控制周期位于车辆后方预定轨迹中的所有点。