一种基于直线控制器的智能车辆曲线跟踪方法
【专利摘要】一种基于直线控制器的智能车辆曲线跟踪方法，首先对规划路径进行离散化，得到一系列首尾相连的直线段，并进行障碍物检测，在不存在障碍物的情况下，依次以各直线段为预瞄路径进行处理，包括初始化后，判断当前驾驶员关注点转换的标识符的值，如果Flag为1关注车辆与预瞄路径的距离，如果当前关注距离的相应变量于等于初始值的二分之一，车辆就转换关注点；如果Flag为0则关注车辆的朝向，包括计算车辆前轮的转向角δ并判断车辆当前航向角和路径方向角的角度差，如果小于或等于设定的相应阈值则更换关注点。本发明的路径跟踪方法可以应用于任意形状路径的跟踪，具有良好的适应性和高跟踪精度，鲁棒性好。
【专利说明】
一种基于直线控制器的智能车辆曲线跟踪方法
【技术领域】
[0001]本发明涉及车辆电子控制技术，尤其涉及一种基于直线控制器的智能车辆曲线跟踪方法。
【背景技术】
[0002]在智能车辆控制领域，路径跟踪技术联系着上层的路径规划和车辆底层的自动转向控制，主要目的是使智能车辆能准确、安全地沿规划的路径行驶，因此，路径跟踪算法是非常重要的。路径跟踪算法的目标是通过计算控制输入量以确保车辆能参照生成的路径曲线进行跟踪。
[0003]为了实现路径跟踪，需要设计跟踪控制器来跟踪生成路径曲线离散化之后的直线，路径跟踪领域应用的控制器主要可以分为两大类:几何控制器与基于误差的控制器。
[0004]几何控制器包含矢量跟踪、纯追随等，并不基于待跟踪路径与实际路径的误差来进行控制，鲁棒性好，但是控制精度不高。
[0005]基于误差的控制器包括经典控制、智能控制等。目前经典控制的应用越来越少，大多数学者更钟情于智能控制，例如模糊控制、神经网络、遗传算法，这些算法的实时性好，而且即使在路径不是很完美的情况下也可以达到很高的控制精度。但是这些智能控制算法植入困难，而且鲁棒性一般。
[0006]在进行路径跟踪控制时，参数的变化对跟踪效果影响很大，其中纵向速度变化的影响尤其明显，传统的路径跟踪控制器常采用PID(比例、积分、微分)控制器，将机器人的航向角误差和误差变化率作为控制器的输入，控制器的输出作为转向角。而在实际过程中，航向角还与速度、转动惯量、重心位置、前后轮侧偏系数等因素有关，这就使PID控制器参数的全局整定极为困难，PID的控制参数要根据系统参数特别是某些敏感参数发生变化来重新设定，鲁棒性差。
【发明内容】
[0007]本发明的目的是为了提高路径跟踪精度，简化路径跟踪的参数设置，通过模拟人类的驾驶习惯，提出一种拟人化的、精度高、鲁棒性好的路径跟踪技术方案。
[0008]本发明技术方案提供一种基于直线控制器的智能车辆曲线跟踪方法，包括首先利用道格拉斯-普克算法对规划路径进行离散化，得到一系列首尾相连的直线段，并进行障碍物检测，在不存在障碍物的情况下，依次以各直线段为预瞄路径，执行以下步骤，
[0009]步骤I,对变量Flag、D、D_pre、D_change、δ和i全部初始化为O,并初始化Error,
[0010]其中，
[0011]Flag为驾驶员关注点转换的标识符，
[0012]D是车辆当前位置与预瞄路径之间的最短距离；
[0013]D_pre是上一次迭代所得D值；
[0014]D_change是指D的变化率；
[0015]Error是车辆当前航向角Θ _CurrentHeading和路径方向角Θ _pre的角度差；
[0016]δ是控制输入量，表示车辆前轮的转向角；
[0017]i为循环变量，用于标识当前迭代次数；
[0018]步骤2，判断当前Flag的值，如果Flag为1，转向步骤5 ;如果Flag为0，则转向步骤3 ;
[0019]步骤3,计算车辆前轮的转向角δ，δ = Kh/VelocityXError, Kh为增益系数，Velocity为当前车辆的速度，Error是车辆当前航向角和路径方向角的角度差；
[0020]步骤4,判断Error的值,如果小于或等于设定的相应阈值,则令Flag = I并转向步骤7，否则直接进入步骤7 ;
[0021]步骤5,计算变量D、D_pre、D_change、δ以及当前车辆和预猫路径的相对位置S的值，更新当前迭代次数，所用式子如下，
[0022]D = Distance(P_current, Line)；
[0023]D_change = (D~D_pre) /T ；
[0024]D_pre = D ；
[0025]S = LOCAT1N(P_current, Line)；
[0026]S[i] = S ；
[0027]S_saved = S[0]；
[0028]i++ ；
[0029]δ = Kp xD + Kd / Velocity x D_change;
[0030]其中，
[0031]Distance (P_current, Line)为车辆当前位置P_current与预猫路径Line之间的最短距离；
[0032]D_pre是上一次迭代所得D值，T为当前与上一次迭代时间间隔；
[0033]LOCAT1N(P_current, Line)为预猫路径 Line 起点、车辆当前位置 P_current 所在点以及预瞄路径Line终点所形成的三角形面积值，当预瞄路径Line起点、车辆当前位置P_current所在点、预猫路径Line终点顺时针排列时，S〈0,当预猫路径Line起点、车辆当前位置P_current所在点、预瞄路径Line终点逆时针排列时，S>0 ;sign⑶为S的符号；
[0034]3 为矢量，Z)=,s7g/?(5‘)x Z);
[0035]D —change 为欠'丨丨.:，D _changc=sign(5) x D changc;
[0036]S_saved为S的初始值；
[0037]Kp、Kd为增益系数；
[0038]步骤6，如果当前变量S小于等于初始值5_8&%(1\(:，C为预设的阈值比例，令Flag = O并转向步骤7，否则直接进入步骤7 ；
[0039]步骤7，如果前轮转角大于等于预设的转向角阈值Al，则将此时的车辆前轮的转向角δ重新赋值，令δ = π/6并转向步骤9;否则进入步骤8;
[0040]步骤8，如果前轮转角小于等于预设的转向角阈值Α2，Α2 = -Al，则将此时的车辆前轮的转向角S重新赋值，令S =-η/6并转向步骤9;否则直接进入步骤9;
[0041]步骤9，判断当前车辆是否到达预瞄路径的终点，如果到达终点，结束对当前预瞄路径的相应处理流程；如果没有到达终点，则转向步骤2。
[0042]而且，步骤3中，利用比例控制器计算车辆前轮的转向角δ。
[0043]而且,步骤5中，利用比例微分控制器计算变量D、D_pre、D_change、δ以及当前车辆和预瞄路径Line的相对位置S的值。
[0044]本发明相对现有技术的优点与积极效果在于:
[0045](I)本发明的路径跟踪方法可以应用于任意形状路径的跟踪，具有良好的适应性和高跟踪精度，引入GIS领域中的道格拉斯-普克算法作为曲线离散化的基础算法，很好的符合了控制器的要求；
[0046](2)本发明的路径跟踪方法偏重实用性，可植入性，支持利用简单的PID控制器组合成能够精确实现复杂路径跟踪过程的算法；
[0047](3)本发明的路径跟踪方法通过模拟人类的驾驶习惯提出，为了达到目标位置首先关注车子的朝向，等朝向与目标朝向误差小于一定的阈值时，再去关注与目标位置的距离，并结合经典控制的PID算法，相对于原始PID的综合考虑朝向和距离，本发明将目标与距离分开单独控制，并且不断的根据实际情况进行转换，这符合驾驶员的驾驶习惯，每次只关注一个量。并考虑了速度对路径跟踪的影响，具有精度高、鲁棒性好等优点。
【专利附图】
【附图说明】
[0048]图1为本发明实施例使用直线控制器的跟踪算法流程图；
[0049]图2为本发明实施例S的几何意义说明图；
[0050]图3为本发明实施例正弦函数在速度为lm/s的速度鲁棒性示意图；
[0051]图4为本发明实施例正弦函数在速度为3m/s的速度鲁棒性示意图；
[0052]图5为本发明实施例正弦函数在速度为5m/s的速度鲁棒性示意图；
[0053]图6为本发明实施例正弦函数在速度为lOm/s的速度鲁棒性示意图。
【具体实施方式】
[0054]下面结合附图和实施例对本发明作进一步的详细说明。
[0055]本发明设计了一种拟人化的跟踪控制器来实现曲线路径跟踪，即利用简单的PID控制器组合成能够精确实现复杂路径跟踪过程的算法，具体实施时本领域技术人员可采用计算机软件技术实现自动运行。实施例的流程图见图1，具体实现过程如下:
[0056]一、首先，利用道格拉斯-普克算法对已生成的规划路径曲线进行离散化，将曲线离散成一系列首尾相连的直线段，并进行障碍物检测:
[0057]实施例首先利用道格拉斯-普克算法对已生成的曲线进行离散化，将曲线路径离散成一系列首尾相连的直线段；
[0058]为便于实施参考起见，提供经典的道格拉斯-普克算法步骤如下:
[0059]I)在曲线首尾两点A、B之间连接一条直线AB，该直线为曲线的弦；
[0060]2)得到曲线上离该直线段距离最大的点C，计算其与AB的距离d ；
[0061]3)比较该距离与预先给定的阈值的大小，如果小于等于阈值，则该直线段作为曲线的近似，该段曲线处理完毕，如果大于阈值，则进入4)。具体实施时，本领域技术人员可自行设定相应阈值。
[0062]4)此时距离大于阈值，则用C将曲线分为两段AC和BC，并分别对两段继续进行前三步的处理。
[0063]5)当所有曲线都处理完毕时，依次连接各个分割点形成的折线，即可以作为曲线的近似。
[0064]该算法的优势在于它能在弯曲比较小的曲线上生成比较稀疏的点，在弯曲比较大的曲线上生成比较密集的点，这正是控制器离散化所需要的情况，也符合车辆的驾驶习惯，在直线道路上仅需要微动方向，保持车辆平稳，在弯曲的道路上则要根据实际情况不断的调整方向。
[0065]由于最初生成的路径是避开障碍物的，但是经过道格拉斯普克算法离散化成一段段小直线之后，与最初路径存在少量不同，但是由于生成路径时已经将障碍物和车辆本身增加和缓冲距离，大多数情况下离散后的路径应该是安全的。出于安全性的考虑，对于离散化后的路径需要再次进行障碍物检测。障碍物用多边形来表示，离散化后的路径用直线段来表示，障碍物检测可通过判断各直线段和多边形是否相交实现。如果发现某条直线段存在碰撞的现象，那么就要考虑在该路段之前重新进行路径规划生成路径，如果仍然无法通过，则需要紧急制动。在不存在障碍物的情况下，进入后续处理。
[0066]二、对离散化所得每段直线段，依次作为预瞄路径(即当前处理的直线路径)，执行以下步骤:
[0067]步骤1:对变量Flag、D、D_pre、D_change、δ和i全部初始化为O,并初始化Error。其中Flag为驾驶员关注点转换的标识符，Flag为O时驾驶员关注车辆的朝向，Flag为I时驾驶员关注车辆与预瞄路径的侧向偏移；D是车辆当前位置与预瞄路径之间的最短距离，可通过车辆上的距离传感器获得；D_pre是上一次迭代所得的D值，初始时也取O ;D_change是指D的变化率；Error是车辆当前航向角Θ _CurrentHeading和路径方向角Θ _pre的角度差，车辆当前航向角可以由车辆上的惯导系统直接得到，预瞄路径方向角可以由预瞄路径的直线斜率得到；δ是控制输入量，表示车辆前轮的转向角；i为循环变量，用于标识当前迭代次数。
[0068]即对变量Flag、D、D_pre、D_change、Error、δ 全部初始化，
[0069]Flag = O ；
[0070]D = 0；
[0071]D_pre = O ；
[0072]D_change = 0 ；
[0073]Error = Θ _pre- Θ _CurrentHeading ；
[0074]5=0；
[0075]i = 0 ；
[0076]步骤2:判断当前Flag的值，如果Flag为1，则当前关注车辆与预瞄路径的距离，并转向步骤5 ;如果Flag不为1(为O)，则当前关注车辆的朝向，转向步骤3。
[0077]实施例设置Flag是为了模拟人类的驾驶习惯提出的标识，为了达到目标位置首先关注车子的朝向，等朝向能够保证可以到达目标位置之后，再去关注与目标位置的距离。
[0078]步骤3:利用比例⑵控制器计算车辆前轮的转向角δ，δ = Rh/Velocity X Error, Kh为增益系数,具体实施时，本领域技术人员可自行设定取值，可采用经验值或实验值；Velocity为当前车辆的速度，Error是车辆当前航向角和路径方向角的角度差。
[0079]步骤4:判断Error的值,如果小于或等于设定的相应阈值(具体实施时，本领域技术人员可自行设定相应阈值，例如实施例取0.05)，则认为当前车子的航向角与预瞄方向一致，已经可以满足车子到达预瞄点，驾驶员的关注点就从车子的航向角转换为车子与预猫点的距离，令Flag = I并转向步骤7 ;如果Error不满足设定的相应阈值,直接进入步骤7。
[0080]预瞄路径中，直线段的两端点分别为跟踪的起点和终点，作为车辆当前跟踪目标终点的端点即为预瞄点，如图2中的终点就是车辆在当前直线段的预瞄点，如图2的起点就是上一段直线段的预瞄点。
[0081]步骤5:是模拟驾驶员控制车辆与预瞄路径之间的距离的过程，利用ro(比例、微分)控制器来模拟人类的决策行为，包括计算D、D_pre、D_Change、δ以及当前车辆和路径Line的相对位置S的值。
[0082]实施例中首先计算车辆当前位置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的时间间隔。
[0083]变量S表示当前车辆和预瞄路径Line的相对位置，S的值为预瞄路径Line起点、车辆当前位置P_current所在点以及预瞄路径Line终点(即预瞄点)所形成的三角形面积值，该值是利用向量的方式计算出来，因此是带有符号的矢量值，当预瞄路径Line起点、车辆当前位置P_current所在点、预瞄路径Line终点顺时针排列时，S〈0，并且S的值参见图2中三角形①的面积,反之当预猫路径Line起点、车辆当前位所在点、预猫路径Line终点逆时针排列时，S>0，并且S的值参见图2中三角形②的面积。
[0084]由于距离是标量，不是矢量，对于D来说必须考虑车辆在所预瞄路径的左侧或者右侧，才能作为一个变量来反馈信息。本发明通过引入S的符号来将距离D转化为矢量
[0085]D =Sign(S) x D;
[0086]当车辆处于预瞄路径的左侧时，S〈0，随着距离的增大车辆朝向角减小；当处于右侧时，S>0，随着距离的增大车辆朝向角增大。同理D_change和D—样，转化为矢量
D —change:
[0087]￡) _changc=.s/g./?(S) x D changc;
[0088]通过利用H)控制器，车辆前轮的转向角δ为增益系数Kp与矢量3 I的积，再加上增益系数Kd与为矢量3—change的积再除以当前车辆的速度Velocity。具体实施时，本领域技术人员可自行设定增益系数Kp、Kd的取值，可采用经验值或实验值。
[0089]即:
[0090]D = Distance(P_current, Line)；
[0091]D_change = (D~D_pre) /T ；
[0092]D_pre = D ；
[0093]S = LOCAT1N(P_current, Line)；
[0094]S[i] = S ；
[0095]S_saved = S[0]；
[0096]i++ ；
[0097]δ = Kp X D + Kd / Velocity X D_changc;
[0098]S_saved为S的初始值，即当第一次执行步骤5时，i = 0，所得S存为S[0]，作为初始值供后续每次执行步骤6时判断使用。
[0099]步骤6:如果当前关注距离的相应变量S小于等于初始值S_savedXC,C为预设的阈值比例(具体实施时，本领域技术人员可自行设定C的取值，例如实施例取二分之一)，车辆就转换关注点，令Flag = O并转向步骤7，由当前的关注距离转换为关注朝向角；否则直接进入步骤7 ；
[0100]步骤7:限制车辆前轮的转向角δ的输出，包括如果前轮转角大于等于预设的转向角阈值Al，实施例取Al = /6，则将此时的车辆前轮的转向角δ重新赋值，令δ = ji/6并转向步骤9;否则进入步骤8;
[0101]步骤8:限制车辆前轮的转向角δ的输出，包括如果前轮转角小于等于预设的转向角阈值Α2，实施例取Α2 =-Ji/6，则将此时的车辆前轮的转向角δ重新赋值，令δ=-/6并转向步骤9 ;否则直接进入步骤9 ；
[0102]具体实施时，本领域技术人员可自行预设Al、Α2的取值，Al为正值，Α2 = -Al。
[0103]步骤9:判断当前车辆是否到达预瞄路径的终点，如果到达终点，结束对当前预瞄路径的相应处理流程；如果没有到达终点，则转向步骤2。
[0104]本发明采用经典的自行车模型进行仿真实验，取得了良好的效果:
[0105]X = vcos ( Θ )；
[0106]y = vsin ( Θ )；
[0107]Θ = V K ；
Γ πδ
[0108]K 二一；
[0109](X，y)为车辆的后轮轴中心的位置；θ为车辆的朝向；V为车辆的速度；K为曲率，范围[-0.190.19];车辆的初始位置[xy Θ ]为[000], δ为前轮转角；L为车前后轮轴距。仿真参数的设定如下表:
[0110]
【权利要求】
1.一种基于直线控制器的智能车辆曲线跟踪方法，其特征在于:包括首先利用道格拉斯-普克算法对规划路径进行离散化，得到一系列首尾相连的直线段，并进行障碍物检测，在不存在障碍物的情况下，依次以各直线段为预瞄路径，执行以下步骤， 步骤1，对变量Flag、D、D_pre、D_change、δ和i全部初始化为O,并初始化Error, 其中， Flag为驾驶员关注点转换的标识符， D是车辆当前位置与预瞄路径之间的最短距离； D_pre是上一次迭代所得D值； D_change是指D的变化率； Error是车辆当前航向角Θ _CurrentHeading和路径方向角Θ _pre的角度差； δ是控制输入量，表示车辆前轮的转向角； i为循环变量，用于标识当前迭代次数； 步骤2，判断当前Flag的值，如果Flag为1，转向步骤5 ;如果Flag为0，则转向步骤3 ； 步骤3,计算车辆前轮的转向角δ，δ = Kh/VelocityXError, Kh为增益系数，Velocity为当前车辆的速度，Error是车辆当前航向角和路径方向角的角度差； 步骤4,判断Error的值,如果小于或等于设定的相应阈值,则认令Flag = I并转向步骤7，否则直接进入步骤7; 步骤5,计算变量D、D_pre、D_change、δ以及当前车辆和预猫路径的相对位置S的值，更新当前迭代次数，所用式子如下，
D = Distance(P_current, Line)；
D_change = (D~D_pre)/T ；
D_pre = D ；
S = LOCAT1N(P_current, Line)； S[i] =S;
S_saved = S[0]；
i++ ；
δ 二 Kp X D + Kd / Velocity x D_change; 其中， Distance (P_current, Line)为车辆当前位置P_current与预猫路径Line之间的最短距离； D_pre是上一次迭代所得D值，T为当前与上一次迭代时间间隔； LOCAT1N(P_current, Line)为预猫路径Line起点、车辆当前位置P_current所在点以及预瞄路径Line终点所形成的三角形面积值，当预瞄路径Line起点、车辆当前位置P_current所在点、预猫路径Line终点顺时针排列时，S〈0,当预猫路径Line起点、车辆当前位置P_current所在点、预瞄路径Line终点逆时针排列时，S>0 ;sign⑶为S的符号； 3为矢量，D =sign(S) X D;
D changc 为矢量，D _changc=.s/g/7(5) x D_changc; S_saved为S的初始值； Kp、Kd为增益系数； 步骤6,如果当前变量S小于等于初始值S_savedXC,C为预设的阈值比例,令Flag =O并转向步骤7，否则直接进入步骤7 ； 步骤7，如果前轮转角大于等于预设的转向角阈值Al，则将此时的车辆前轮的转向角δ重新赋值，令δ =Al并转向步骤9;否则进入步骤8; 步骤8，如果前轮转角小于等于预设的转向角阈值Α2，Α2 = -Al，则将此时的车辆前轮的转向角δ重新赋值，令δ =Α2并转向步骤9;否则直接进入步骤9; 步骤9，判断当前车辆是否到达预瞄路径的终点，如果到达终点，结束对当前预瞄路径的相应处理流程；如果没有到达终点，则转向步骤2。
2.根据权利要求1所述基于直线控制器的智能车辆曲线跟踪方法，其特征在于:步骤3中，利用比例控制器计算车辆前轮的转向角δ。
3.根据权利要求1所述基于直线控制器的智能车辆曲线跟踪方法，其特征在于:步骤5中，利用比例微分控制器计算变量0、0_口代、0_01^1^6、δ以及当前车辆和预猫路径Line的相对位置S的值。
【文档编号】G05D1/02GK104181923SQ201410436835
【公开日】2014年12月3日 申请日期:2014年8月29日 优先权日:2014年8月29日
【发明者】李必军, 单云霄, 谢云, 郑玲, 周剑 申请人:武汉大学