CN104181923A - 一种基于直线控制器的智能车辆曲线跟踪方法 - Google Patents

Abstract

一种基于直线控制器的智能车辆曲线跟踪方法，首先对规划路径进行离散化，得到一系列首尾相连的直线段，并进行障碍物检测，在不存在障碍物的情况下，依次以各直线段为预瞄路径进行处理，包括初始化后，判断当前驾驶员关注点转换的标识符的值，如果Flag为1关注车辆与预瞄路径的距离，如果当前关注距离的相应变量于等于初始值的二分之一，车辆就转换关注点；如果Flag为0则关注车辆的朝向，包括计算车辆前轮的转向角δ并判断车辆当前航向角和路径方向角的角度差，如果小于或等于设定的相应阈值则更换关注点。本发明的路径跟踪方法可以应用于任意形状路径的跟踪，具有良好的适应性和高跟踪精度，鲁棒性好。

Description

一种基于直线控制器的智能车辆曲线跟踪方法

技术领域

本发明涉及车辆电子控制技术，尤其涉及一种基于直线控制器的智能车辆曲线跟踪方法。

背景技术

在智能车辆控制领域，路径跟踪技术联系着上层的路径规划和车辆底层的自动转向控制，主要目的是使智能车辆能准确、安全地沿规划的路径行驶，因此，路径跟踪算法是非常重要的。路径跟踪算法的目标是通过计算控制输入量以确保车辆能参照生成的路径曲线进行跟踪。

为了实现路径跟踪，需要设计跟踪控制器来跟踪生成路径曲线离散化之后的直线，路径跟踪领域应用的控制器主要可以分为两大类：几何控制器与基于误差的控制器。

几何控制器包含矢量跟踪、纯追随等，并不基于待跟踪路径与实际路径的误差来进行控制，鲁棒性好，但是控制精度不高。

基于误差的控制器包括经典控制、智能控制等。目前经典控制的应用越来越少，大多数学者更钟情于智能控制，例如模糊控制、神经网络、遗传算法，这些算法的实时性好，而且即使在路径不是很完美的情况下也可以达到很高的控制精度。但是这些智能控制算法植入困难，而且鲁棒性一般。

在进行路径跟踪控制时，参数的变化对跟踪效果影响很大，其中纵向速度变化的影响尤其明显，传统的路径跟踪控制器常采用PID(比例、积分、微分)控制器，将机器人的航向角误差和误差变化率作为控制器的输入，控制器的输出作为转向角。而在实际过程中，航向角还与速度、转动惯量、重心位置、前后轮侧偏系数等因素有关，这就使PID控制器参数的全局整定极为困难，PID的控制参数要根据系统参数特别是某些敏感参数发生变化来重新设定，鲁棒性差。

发明内容

本发明的目的是为了提高路径跟踪精度，简化路径跟踪的参数设置，通过模拟人类的驾驶习惯，提出一种拟人化的、精度高、鲁棒性好的路径跟踪技术方案。

本发明技术方案提供一种基于直线控制器的智能车辆曲线跟踪方法，包括首先利用道格拉斯-普克算法对规划路径进行离散化，得到一系列首尾相连的直线段，并进行障碍物检测，在不存在障碍物的情况下，依次以各直线段为预瞄路径，执行以下步骤，

步骤1，对变量Flag、D、D_pre、D_change、δ和i全部初始化为0，并初始化Error，

其中，

Flag为驾驶员关注点转换的标识符，

D是车辆当前位置与预瞄路径之间的最短距离；

D_pre是上一次迭代所得D值；

D_change是指D的变化率；

Error是车辆当前航向角θ_CurrentHeading和路径方向角θ_pre的角度差；

δ是控制输入量，表示车辆前轮的转向角；

i为循环变量，用于标识当前迭代次数；

步骤2，判断当前Flag的值，如果Flag为1，转向步骤5；如果Flag为0，则转向步骤3；

步骤3，计算车辆前轮的转向角δ，δ＝Kh/Velocity×Error，Kh为增益系数，Velocity为当前车辆的速度，Error是车辆当前航向角和路径方向角的角度差；

步骤4，判断Error的值，如果小于或等于设定的相应阈值，则令Flag＝1并转向步骤7，否则直接进入步骤7；

步骤5，计算变量D、D_pre、D_change、δ以及当前车辆和预瞄路径的相对位置S的值，更新当前迭代次数，所用式子如下，

D＝Distance(P_current,Line)；

D_change＝(D-D_pre)/T；

D_pre＝D；

S＝LOCATION(P_current,Line)；

S[i]＝S；

S_saved＝S[0]；

i++；

$\mathrm{\δ}=\mathrm{Kp}\×\stackrel{\→}{D}+\mathrm{Kd}/\mathrm{Velocity}\×\stackrel{\→}{D}\_\mathrm{change};$

其中，

Distance(P_current,Line)为车辆当前位置P_current与预瞄路径Line之间的最短距离；

D_pre是上一次迭代所得D值，T为当前与上一次迭代时间间隔；

LOCATION(P_current,Line)为预瞄路径Line起点、车辆当前位置P_current所在点以及预瞄路径Line终点所形成的三角形面积值，当预瞄路径Line起点、车辆当前位置P_current所在点、预瞄路径Line终点顺时针排列时，S<0，当预瞄路径Line起点、车辆当前位置P_current所在点、预瞄路径Line终点逆时针排列时，S>0；sign(S)为S的符号；

为矢量， $\stackrel{\&RightArrow;}{D}=\sin g\left(S\right)\&times;D;$

为矢量， $\stackrel{\&RightArrow;}{D}\_\mathrm{change}=\sin g\left(S\right)\&times;D\_\mathrm{change};$

S_saved为S的初始值；

Kp、Kd为增益系数；

步骤6，如果当前变量S小于等于初始值S_saved×C，C为预设的阈值比例，令Flag＝0并转向步骤7，否则直接进入步骤7；

步骤7，如果前轮转角大于等于预设的转向角阈值A1，则将此时的车辆前轮的转向角δ重新赋值，令δ＝π/6并转向步骤9；否则进入步骤8；

步骤8，如果前轮转角小于等于预设的转向角阈值A2，A2＝-A1，则将此时的车辆前轮的转向角δ重新赋值，令δ＝-π/6并转向步骤9；否则直接进入步骤9；

步骤9，判断当前车辆是否到达预瞄路径的终点，如果到达终点，结束对当前预瞄路径的相应处理流程；如果没有到达终点，则转向步骤2。

而且，步骤3中，利用比例控制器计算车辆前轮的转向角δ。

而且，步骤5中，利用比例微分控制器计算变量D、D_pre、D_change、δ以及当前车辆和预瞄路径Line的相对位置S的值。

本发明相对现有技术的优点与积极效果在于：

(1)本发明的路径跟踪方法可以应用于任意形状路径的跟踪，具有良好的适应性和高跟踪精度，引入GIS领域中的道格拉斯-普克算法作为曲线离散化的基础算法，很好的符合了控制器的要求；

(2)本发明的路径跟踪方法偏重实用性，可植入性，支持利用简单的PID控制器组合成能够精确实现复杂路径跟踪过程的算法；

(3)本发明的路径跟踪方法通过模拟人类的驾驶习惯提出，为了达到目标位置首先关注车子的朝向，等朝向与目标朝向误差小于一定的阈值时，再去关注与目标位置的距离，并结合经典控制的PID算法，相对于原始PID的综合考虑朝向和距离，本发明将目标与距离分开单独控制，并且不断的根据实际情况进行转换，这符合驾驶员的驾驶习惯，每次只关注一个量。并考虑了速度对路径跟踪的影响，具有精度高、鲁棒性好等优点。

附图说明

图1为本发明实施例使用直线控制器的跟踪算法流程图；

图2为本发明实施例S的几何意义说明图；

图3为本发明实施例正弦函数在速度为1m/s的速度鲁棒性示意图；

图4为本发明实施例正弦函数在速度为3m/s的速度鲁棒性示意图；

图5为本发明实施例正弦函数在速度为5m/s的速度鲁棒性示意图；

图6为本发明实施例正弦函数在速度为10m/s的速度鲁棒性示意图。

具体实施方式

下面结合附图和实施例对本发明作进一步的详细说明。

本发明设计了一种拟人化的跟踪控制器来实现曲线路径跟踪，即利用简单的PID控制器组合成能够精确实现复杂路径跟踪过程的算法，具体实施时本领域技术人员可采用计算机软件技术实现自动运行。实施例的流程图见图1，具体实现过程如下：

一、首先，利用道格拉斯-普克算法对已生成的规划路径曲线进行离散化，将曲线离散成一系列首尾相连的直线段，并进行障碍物检测：

实施例首先利用道格拉斯-普克算法对已生成的曲线进行离散化，将曲线路径离散成一系列首尾相连的直线段；

为便于实施参考起见，提供经典的道格拉斯-普克算法步骤如下：

1)在曲线首尾两点A、B之间连接一条直线AB，该直线为曲线的弦；

2)得到曲线上离该直线段距离最大的点C，计算其与AB的距离d；

3)比较该距离与预先给定的阈值的大小，如果小于等于阈值，则该直线段作为曲线的近似，该段曲线处理完毕，如果大于阈值，则进入4)。具体实施时，本领域技术人员可自行设定相应阈值。

4)此时距离大于阈值，则用C将曲线分为两段AC和BC，并分别对两段继续进行前三步的处理。

5)当所有曲线都处理完毕时，依次连接各个分割点形成的折线，即可以作为曲线的近似。

该算法的优势在于它能在弯曲比较小的曲线上生成比较稀疏的点，在弯曲比较大的曲线上生成比较密集的点，这正是控制器离散化所需要的情况，也符合车辆的驾驶习惯，在直线道路上仅需要微动方向，保持车辆平稳，在弯曲的道路上则要根据实际情况不断的调整方向。

由于最初生成的路径是避开障碍物的，但是经过道格拉斯普克算法离散化成一段段小直线之后，与最初路径存在少量不同，但是由于生成路径时已经将障碍物和车辆本身增加和缓冲距离，大多数情况下离散后的路径应该是安全的。出于安全性的考虑，对于离散化后的路径需要再次进行障碍物检测。障碍物用多边形来表示，离散化后的路径用直线段来表示，障碍物检测可通过判断各直线段和多边形是否相交实现。如果发现某条直线段存在碰撞的现象，那么就要考虑在该路段之前重新进行路径规划生成路径，如果仍然无法通过，则需要紧急制动。在不存在障碍物的情况下，进入后续处理。

二、对离散化所得每段直线段，依次作为预瞄路径(即当前处理的直线路径)，执行以下步骤：

步骤1：对变量Flag、D、D_pre、D_change、δ和i全部初始化为0，并初始化Error。其中Flag为驾驶员关注点转换的标识符，Flag为0时驾驶员关注车辆的朝向，Flag为1时驾驶员关注车辆与预瞄路径的侧向偏移；D是车辆当前位置与预瞄路径之间的最短距离，可通过车辆上的距离传感器获得；D_pre是上一次迭代所得的D值，初始时也取0；D_change是指D的变化率；Error是车辆当前航向角θ_CurrentHeading和路径方向角θ_pre的角度差，车辆当前航向角可以由车辆上的惯导系统直接得到，预瞄路径方向角可以由预瞄路径的直线斜率得到；δ是控制输入量，表示车辆前轮的转向角；i为循环变量，用于标识当前迭代次数。

即对变量Flag、D、D_pre、D_change、Error、δ全部初始化，

Flag＝0；

D＝0；

D_pre＝0；

D_change＝0；

Error＝θ_pre-θ_CurrentHeading；

δ＝0；

i＝0；

步骤2：判断当前Flag的值，如果Flag为1，则当前关注车辆与预瞄路径的距离，并转向步骤5；如果Flag不为1(为0)，则当前关注车辆的朝向，转向步骤3。

实施例设置Flag是为了模拟人类的驾驶习惯提出的标识，为了达到目标位置首先关注车子的朝向，等朝向能够保证可以到达目标位置之后，再去关注与目标位置的距离。

步骤3：利用比例(P)控制器计算车辆前轮的转向角δ，δ＝Kh/Velocity×Error，Kh为增益系数，具体实施时，本领域技术人员可自行设定取值，可采用经验值或实验值；Velocity为当前车辆的速度，Error是车辆当前航向角和路径方向角的角度差。

步骤4：判断Error的值，如果小于或等于设定的相应阈值(具体实施时，本领域技术人员可自行设定相应阈值，例如实施例取0.05)，则认为当前车子的航向角与预瞄方向一致，已经可以满足车子到达预瞄点，驾驶员的关注点就从车子的航向角转换为车子与预瞄点的距离，令Flag＝1并转向步骤7；如果Error不满足设定的相应阈值，直接进入步骤7。

预瞄路径中，直线段的两端点分别为跟踪的起点和终点，作为车辆当前跟踪目标终点的端点即为预瞄点，如图2中的终点就是车辆在当前直线段的预瞄点，如图2的起点就是上一段直线段的预瞄点。

步骤5：是模拟驾驶员控制车辆与预瞄路径之间的距离的过程，利用PD(比例、微分)控制器来模拟人类的决策行为，包括计算D、D_pre、D_change、δ以及当前车辆和路径Line的相对位置S的值。

实施例中首先计算车辆当前位置P_current与预瞄路径之间的最短距离D；则D的变化率D_change就由车辆当前位置与预瞄路径的最短距离D与上一次迭代所得距离D_pre之间的差再除以当前和上一次迭代的时间间隔T得到；把当前的D值赋给D_pre；具体来说，第一次执行步骤5时，D_pre为初始化所得0，T采用当前和初始化时刻的时间间隔；后续执行步骤5时，D_pre为上一次迭代执行步骤5所得距离D，T采用当前和上一次迭代执行步骤5的时间间隔。

变量S表示当前车辆和预瞄路径Line的相对位置，S的值为预瞄路径Line起点、车辆当前位置P_current所在点以及预瞄路径Line终点(即预瞄点)所形成的三角形面积值，该值是利用向量的方式计算出来，因此是带有符号的矢量值，当预瞄路径Line起点、车辆当前位置P_current所在点、预瞄路径Line终点顺时针排列时，S<0，并且S的值参见图2中三角形①的面积，反之当预瞄路径Line起点、车辆当前位置P_current所在点、预瞄路径Line终点逆时针排列时，S>0，并且S的值参见图2中三角形②的面积。

由于距离是标量，不是矢量，对于D来说必须考虑车辆在所预瞄路径的左侧或者右侧，才能作为一个变量来反馈信息。本发明通过引入S的符号来将距离D转化为矢量

$\stackrel{\&RightArrow;}{D}=\sin g\left(S\right)\&times;D;$

当车辆处于预瞄路径的左侧时，S<0，随着距离的增大车辆朝向角减小；当处于右侧时，S>0，随着距离的增大车辆朝向角增大。同理D_change和D一样，转化为矢量

$\stackrel{\&RightArrow;}{D}\_\mathrm{change}=\sin g\left(S\right)\&times;D\_\mathrm{change};$

通过利用PD控制器，车辆前轮的转向角δ为增益系数Kp与矢量的积，再加上增益系数Kd与为矢量的积再除以当前车辆的速度Velocity。具体实施时，本领域技术人员可自行设定增益系数Kp、Kd的取值，可采用经验值或实验值。

即：

D＝Distance(P_current,Line)；

D_change＝(D-D_pre)/T；

D_pre＝D；

S＝LOCATION(P_current,Line)；

S[i]＝S；

S_saved＝S[0]；

i++；

$\mathrm{\&delta;}=\mathrm{Kp}\&times;\stackrel{\&RightArrow;}{D}+\mathrm{Kd}/\mathrm{Velocity}\&times;\stackrel{\&RightArrow;}{D}\_\mathrm{change};$

S_saved为S的初始值，即当第一次执行步骤5时，i＝0，所得S存为S[0]，作为初始值供后续每次执行步骤6时判断使用。

步骤6：如果当前关注距离的相应变量S小于等于初始值S_saved×C，C为预设的阈值比例(具体实施时，本领域技术人员可自行设定C的取值，例如实施例取二分之一)，车辆就转换关注点，令Flag＝0并转向步骤7，由当前的关注距离转换为关注朝向角；否则直接进入步骤7；

步骤7：限制车辆前轮的转向角δ的输出，包括如果前轮转角大于等于预设的转向角阈值A1，实施例取A1＝π/6，则将此时的车辆前轮的转向角δ重新赋值，令δ＝π/6并转向步骤9；否则进入步骤8；

步骤8：限制车辆前轮的转向角δ的输出，包括如果前轮转角小于等于预设的转向角阈值A2，实施例取A2＝-π/6，则将此时的车辆前轮的转向角δ重新赋值，令δ＝-π/6并转向步骤9；否则直接进入步骤9；

具体实施时，本领域技术人员可自行预设A1、A2的取值，A1为正值，A2＝-A1。

步骤9：判断当前车辆是否到达预瞄路径的终点，如果到达终点，结束对当前预瞄路径的相应处理流程；如果没有到达终点，则转向步骤2。

本发明采用经典的自行车模型进行仿真实验，取得了良好的效果：

x＝vcos(θ)；

y＝vsin(θ)；

θ＝vκ；

$\mathrm{\&kappa;}=\frac{\mathrm{\&delta;}}{L};$

(x,y)为车辆的后轮轴中心的位置；θ为车辆的朝向；v为车辆的速度；κ为曲率，范围[-0.190.19]；车辆的初始位置[xyθ]为[000]，δ为前轮转角；L为车前后轮轴距。仿真参数的设定如下表:

四种速率分别用三种增益系数都处理，可知不更改增益系数的情况下，速度从1m/s增加到10m/s，仍然能达到不错的跟踪效果，鲁棒性好。

在没有改变上表任何参数的情况下，用正弦函数来模拟S路，正弦函数为：

y＝40×sin(π/400×x)；

图3、4、5、6中横坐标X(m)为行驶距离，纵坐标Error(m)为跟踪误差。通过图3、4、5、6正弦函数在速度为1m/s、3m/s、5m/s、10m/s的跟踪误差可以看出，初始阶段的跟踪误差比较大，随着算法的植入，误差在不断的减小，最终在0附近震荡，由此可以看出该算法是稳定的收敛的。另外一个现象就是随着速度的增大误差，总体的跟踪误差是在增大的，尤其是在航向偏差比较大的初始阶段。这个现象反映出的问题与人类的驾驶是一致的，当路弯道比较多的时候，车辆的当前航向与预期航向的偏差比较大，这时驾驶员应降低速度，否则偏离车道的可能性就增大，也容易发生事故。

以上实施例仅供说明本发明之用，而非对本发明的限制，有关技术领域的技术人员，在不脱离本发明的精神和范围的情况下，还可以做出各种变换或变型，因此所有等同的技术方案，都落入本发明的保护范围。

Claims (3)

1.一种基于直线控制器的智能车辆曲线跟踪方法，其特征在于：包括首先利用道格拉斯-普克算法对规划路径进行离散化，得到一系列首尾相连的直线段，并进行障碍物检测，在不存在障碍物的情况下，依次以各直线段为预瞄路径，执行以下步骤，

步骤1，对变量Flag、D、D_pre、D_change、δ和i全部初始化为0，并初始化Error，

其中，

Flag为驾驶员关注点转换的标识符，

D是车辆当前位置与预瞄路径之间的最短距离；

D_pre是上一次迭代所得D值；

D_change是指D的变化率；

Error是车辆当前航向角θ_CurrentHeading和路径方向角θ_pre的角度差；

δ是控制输入量，表示车辆前轮的转向角；

i为循环变量，用于标识当前迭代次数；

步骤2，判断当前Flag的值，如果Flag为1，转向步骤5；如果Flag为0，则转向步骤3；

步骤3，计算车辆前轮的转向角δ，δ＝Kh/Velocity×Error，Kh为增益系数，Velocity为当前车辆的速度，Error是车辆当前航向角和路径方向角的角度差；

步骤4，判断Error的值，如果小于或等于设定的相应阈值，则认令Flag＝1并转向步骤7，否则直接进入步骤7；

步骤5，计算变量D、D_pre、D_change、δ以及当前车辆和预瞄路径的相对位置S的值，更新当前迭代次数，所用式子如下，

D＝Distance(P_current,Line)；

D_change＝(D-D_pre)/T；

D_pre＝D；

S＝LOCATION(P_current,Line)；

S[i]＝S；

S_saved＝S[0]；

i++；

$\mathrm{\&delta;}=\mathrm{Kp}\&times;\stackrel{\&RightArrow;}{D}+\mathrm{Kd}/\mathrm{Velocity}\&times;\stackrel{\&RightArrow;}{D}\_\mathrm{change};$

其中，

Distance(P_current,Line)为车辆当前位置P_current与预瞄路径Line之间的最短距离；

D_pre是上一次迭代所得D值，T为当前与上一次迭代时间间隔；

LOCATION(P_current,Line)为预瞄路径Line起点、车辆当前位置P_current所在点以及预瞄路径Line终点所形成的三角形面积值，当预瞄路径Line起点、车辆当前位置P_current所在点、预瞄路径Line终点顺时针排列时，S<0，当预瞄路径Line起点、车辆当前位置P_current所在点、预瞄路径Line终点逆时针排列时，S>0；sign(S)为S的符号；

为矢量， $\stackrel{\&RightArrow;}{D}=\sin g\left(S\right)\&times;D;$

为矢量， $\stackrel{\&RightArrow;}{D}\_\mathrm{change}=\sin g\left(S\right)\&times;D\_\mathrm{change};$

S_saved为S的初始值；

Kp、Kd为增益系数；

步骤6，如果当前变量S小于等于初始值S_saved×C，C为预设的阈值比例，令Flag＝0并转向步骤7，否则直接进入步骤7；

步骤7，如果前轮转角大于等于预设的转向角阈值A1，则将此时的车辆前轮的转向角δ重新赋值，令δ＝A1并转向步骤9；否则进入步骤8；

步骤8，如果前轮转角小于等于预设的转向角阈值A2，A2＝-A1，则将此时的车辆前轮的转向角δ重新赋值，令δ＝A2并转向步骤9；否则直接进入步骤9；

步骤9，判断当前车辆是否到达预瞄路径的终点，如果到达终点，结束对当前预瞄路径的相应处理流程；如果没有到达终点，则转向步骤2。

2.根据权利要求1所述基于直线控制器的智能车辆曲线跟踪方法，其特征在于：步骤3中，利用比例控制器计算车辆前轮的转向角δ。

3.根据权利要求1所述基于直线控制器的智能车辆曲线跟踪方法，其特征在于：步骤5中，利用比例微分控制器计算变量D、D_pre、D_change、δ以及当前车辆和预瞄路径Line的相对位置S的值。