CN104615889A - 基于回旋曲线追随的智能车辆路径跟踪方法及系统 - Google Patents

Abstract

本发明提供了基于回旋曲线追随的智能车辆路径跟踪方法及系统，包括步骤：步骤1，寻找跟踪路径中离智能车辆当前位置最近的GPS点；步骤2，遍历跟踪路径中GPS点；步骤3，生智能车辆当前位置向当前遍历点的回旋曲线并获得回旋曲线的相关参数；步骤4，根据回旋曲线相关参数获得预瞄目标点；步骤5，生成智能车辆当前位置向预瞄目标点的回旋曲线，根据回旋曲线相关参数获得智能车辆的方向盘转角。本发明可提高智能车辆路径跟踪精度，且能够使方向盘转动平稳，保证智能车辆行驶平稳。

Description

基于回旋曲线追随的智能车辆路径跟踪方法及系统

技术领域

本发明涉及车辆电子控制技术领域，特别涉及一种基于回旋曲线追随的智能车辆路径跟踪方法及系统。

背景技术

路径跟踪控制算法是指控制智能车沿着预定的参考路径安全、稳定、准确行驶的方法，其性能和控制精度将直接影响智能车自主行驶的执行能力。

早期的路径跟踪方法，如几何路径规划法、旋量法、滚动路径法等，仅适用于室内移动的机器人。由于智能车的非完整约束，且车辆体积尺寸较大，受到如最小转弯半径、最大角速度等动力学条件的约束，上述路径跟踪方法对于智能车而言是不适用的。近年来涌现出许多基于智能车研究的路径跟踪方法，这些方法在特定的环境中，如较高的速度、复杂的道路环境等，能够有较好表现。例如纯追随算法、Stanley方法等都能很好的完成跟踪既定路径，但是这些方法都与速度相耦合，在复杂道路上如果没有很好的控制相应的速度，这些方法就不能很好的完成路径跟踪。

发明内容

针对现有技术存在的不足，本发明提供了一种精度高、鲁棒性优、能稳定跟踪的、基于回旋曲线追随的智能车辆路径跟踪方法及系统。

为解决上述技术问题，本发明采用如下的技术方案：

基于回旋曲线追随的智能车辆路径跟踪方法，包括步骤：

步骤 1，寻找跟踪路径中离智能车辆当前位置最近的GPS点；

步骤2，从步骤1获得的GPS点开始，按智能车辆行驶方向遍历跟踪路径中GPS点，计算智能车辆当前位置和当前遍历点的距离，直至该距离大于当前最小预瞄距离，停止遍历，执行步骤3；所述的最小预瞄距离初始值取9~15米范围内任意值；

步骤3，以智能车辆当前位置为起点、当前遍历点为终点生成回旋曲线，并获得回旋曲线的起点曲率、曲率变化率和终点曲率；

步骤4，若起点曲率和终点曲率的绝对值中最大值小于曲率阈值，或者起点曲率和终点曲率的绝对值中最大值不小于曲率阈值、但当前最小预瞄距离为辅助最小预瞄距离，以当前遍历点为预瞄目标点；所述的曲率阈值取0.05-0.15内任意值；

步骤5，若起点曲率和终点曲率的绝对值中最大值不小于曲率阈值、但当前最小预瞄距离为最小预瞄距离初始值，以辅助最小预瞄距离为当前最小预瞄距离，然后执行步骤2；所述的辅助最小预瞄距离取5-7米内任意值；

步骤6，以智能车辆当前位置为起点、预瞄目标点为终点生成回旋曲线，根据回旋曲线的起点曲率、曲率变化率、终点曲率和长度获得智能车辆的方向盘转角。

步骤6中，采用阿卡曼原理获得智能车辆的方向盘转角。

上述基于回旋曲线追随的智能车辆路径跟踪方法对应的系统，包括：

第一模块，用来寻找跟踪路径中离智能车辆当前位置最近的GPS点；

第二模块，用来从第一模块获得的GPS点开始，按智能车辆行驶方向遍历跟踪路径中GPS点，计算智能车辆当前位置和当前遍历点的距离，直至该距离大于当前最小预瞄距离，停止遍历；所述的最小预瞄距离初始值取9~15米范围内任意值；

第三模块，用来以智能车辆当前位置为起点、当前遍历点为终点生成回旋曲线，并获得回旋曲线的起点曲率、曲率变化率和终点曲率；

第四模块，用来若起点曲率和终点曲率的绝对值中最大值小于曲率阈值，或者起点曲率和终点曲率的绝对值中最大值不小于曲率阈值、但当前最小预瞄距离为辅助最小预瞄距离，以当前遍历点为预瞄目标点；所述的曲率阈值取0.05-0.15内任意值；

第五模块，用来若起点曲率和终点曲率的绝对值中最大值不小于曲率阈值、但当前最小预瞄距离为初始最小预瞄距离，以辅助最小预瞄距离为当前最小预瞄距离，然后执行步骤2；所述的辅助最小预瞄距离取5-7米内任意值；

第六模块，用来以智能车辆当前位置为起点、预瞄目标点为终点生成回旋曲线，根据回旋曲线的起点曲率、曲率变化率和终点曲率获得智能车辆的方向盘转角。

和现有技术相比，本发明具有如下优点和有益效果：

（1）可提高智能车辆路径跟踪精度，且能够使方向盘转动平稳，保证智能车辆行驶平稳。

（2）对智能车辆速度没有很强的依赖性，即使速度控制没有能很好的反应道路曲率信息，也能保持很好的跟踪精度。

（3）简化了路径跟踪的参数输入，鲁棒性好，可实现性较强，可移植性好。

附图说明

图1为本发明方法的具体流程图；

图2为本发明实施例中跟踪路径示意图；

图 3为本发明施例中跟踪误差及其相应位置的关系图；

图 4为本发明实施例的偏航误差曲线。

具体实施方式

本发明主要包括寻找智能车辆预瞄目标点和基于回旋曲线的控制跟踪两部分，具体步骤如下：

步骤 1，寻找跟踪路径中离智能车辆当前位置最近的GPS点。

跟踪路径的获得属于本技术领域内的常规技术，是由事先采集的智能车辆经过位置的GPS点组成的路径线，即跟踪路径由一系列离散GPS点构成，各GPS点属性包括经度、纬度和方位角。

步骤 2，跟踪路径中GPS点的遍历。

从步骤1获得的GPS点开始，按智能车辆行驶方向遍历跟踪路径中GPS点，计算智能车辆和当前遍历点的距离，比较该距离与当前最小预瞄距离的大小，当该距离大于当前最小预瞄距离时，停止遍历，执行步骤3。

上述当前最小预瞄距离初始值取9~15米范围内任意值。

步骤3，生成回旋曲线并获得回旋曲线的相关参数。

以智能车辆当前位置为起点、当前遍历点为终点生成回旋曲线，并获得回旋曲线的起点曲率、曲率变化率和终点曲率。

本步骤为本技术领域内的常规技术，在此不做赘述。

步骤4，获得预瞄目标点。

当起点曲率和终点曲率的绝对值中最大值小于曲率阈值时，以当前遍历点为预瞄目标点；当起点曲率和终点曲率的绝对值中最大值不小于曲率阈值distri_cur、同时当前最小预瞄距离为辅助最小预瞄距离时，也以当前遍历点为预瞄目标点。获得预瞄目标点后执行步骤5。

当起点曲率和终点曲率的绝对值中最大值不小于曲率阈值，但当前最小预瞄距离为最小预瞄距离初始值时，将当前最小预瞄距离更改为辅助最小预瞄距离，然后重新执行步骤2。

曲率阈值为实践经验值，取0.05-0.15内任意值，辅助最小预瞄距离为实践经验值，取5-7米范围内任意值。

步骤 5，以智能车辆当前位置为起点、预瞄目标点为终点生成回旋曲线，根据回旋曲线的起点曲率、曲率变化率、终点曲率和长度获得智能车辆的方向盘转角。

具体实施中，依据Ackman原理（阿卡曼原理）计算智能车辆的方向盘转角。

下面结合附图和实施例对本发明技术方案作进一步说明。

本实施例基于武汉大学“途智号”无人车平台上进行实施，采用C++语言编程实现本发明方法，实施例的流程图参见图1，具体实施过程如下：

步骤一，寻找智能车辆的预瞄目标点。

Line表示智能车辆跟踪路径；CurPoint表示智能车辆当前位置；TargetPoint为待寻找的预瞄目标点；ds为当前遍历点与智能车辆当前位置间的距离，采用ds_flag判断是否需要计算ds，ds_flag初值为true；initial_min_distance为根据实践经验值设置的最小预瞄距离初始值，

assist_min_distance为根据实践经验值设置的辅助最小预瞄距离；k为回旋曲线的起点曲率，d_k为回旋曲线的曲率变化率，L为回旋曲线长度，k_end为回旋曲线终点曲率；distri_cur为根据实践经验值设置的曲率阈值；index为跟踪路径中离智能车辆当前位置最近GPS点的索引号；isfind为是否找到预瞄目标点的标识，找到为true，没找到为false；i为循环变量，用于标识当前的迭代次数。

本步骤中，跟踪路径Line和智能车辆当前位置CurPoint都是以高斯坐标为参考，具体流程如下：

1.1 在跟踪路径GPS点序列中寻找智能车辆当前位置的最邻近GPS点，返回该最邻近GPS点的索引号index。

1.2 将索引号index值赋给循环变量i，从i开始沿着智能车辆行驶方向遍历跟踪路径Line中GPS点，并计算智能车辆当前位置和当前遍历点的距离ds：

当距离ds小于当前最小预瞄距离时，继续遍历跟踪路径Line中GPS点，直到距离ds大于或等于当前最小预瞄距离，此时，将ds_flag置为false，然后继续执行1.3。

1.3 从智能车辆当前位置CurPoint到当期遍历点Line[i]生成回旋曲线，获得回旋曲线的起点曲率k、曲率变化率d_k和终点曲率k_end。

1.4 基于回旋曲线相关参数进行如下的判断，并调整当前最小预瞄距离：

（a）当起点曲率k和终点曲率k_end的绝对值中最大值小于曲率阈值distri_cur时，将当前遍历点作为预瞄目标点返回，并将isfind置为true。

（b）当起点曲率k和终点曲率k_end的绝对值中最大值不小于曲率阈值distri_cur时，且此时当前最小预瞄距离为最小预瞄距离初始值initial_min_distance时，将当前最小预瞄距离更改为辅助最小预瞄距离assist_min_distance，循环变量i重新置为index，ds_flag置为true，ds置为0，然后跳转至步骤1.2重新进行预瞄目标点的查找。

（c）当起点曲率k和终点曲率k_end的绝对值中最大值不小于曲率阈值distri_cur、且当前最小预瞄距离为辅助最小预瞄距离assist_min_distance时，将当前遍历点作为预瞄目标点返回，并将isfind的值置为true。

步骤二，基于回旋曲线的智能车辆控制追踪。

LWheel为智能车辆前后轮轴距，本具体实施中LWheel值为2.5米；Thegema为智能车辆车前轮转角，其值为回旋曲线起点曲率k与智能车辆前后轮轴距LWheel的反正切函数值；Gear_ratio为智能车辆传动系统的传动比，本具体实施中Gear_ratio值为15；AngleWheel为相对于方向盘正放位置时方向盘的转角，负值时向左转，正值时向右转；Initial_WheelAngle为方向盘正放时方向盘的默认初始角度值，本具体实施中Initial_WheelAngle值为550度。

本步骤中，当isfind为false值时，不满足控制追随执行条件，控制追随程序等待，直到isfind值为true；当isfind的值为true时，则执行控制追随，具体如下：

从智能车辆当前位置CurPoint到预瞄目标点TargetPoint生成回旋曲线，得到该回旋曲线的起点曲率k、曲率变化率dk、长度L和 终点曲率k_end，将上述参数代入公式（1）~（3），得出智能车辆方向盘转角s_TagWheelAngle：

                       （1）

               （2）

           （3）

表1为本实施例中设计的智能车辆不同动作时对应的速度范围。

图2是本实施例的跟踪路径示意图，跟踪路径包含了智能车辆的一些基本动作：换道、左转弯、U型掉头、右转弯等。图3是本实施例的跟踪误差示意图，偏航误差即本发明控制的智能车辆路径和跟踪路径的差运算结果，X轴和Y轴表示高斯坐标，Z轴为偏航误差（单位：米），从图中可以明显地看出，本发明在直线和变道路段的跟踪误差较小，在转弯和U型掉头路段的跟踪误差相对直线和变道路段的跟踪误差稍大。图4是本实施例的偏航误差图，横坐标表示跟踪路径起点到车辆当前位置的路程，从图中可以看出在直线和变道路段，本发明偏航误差不超过0.1；其在转弯和U型掉头时的误差最大不超过0.5米。

表1 智能车辆不同动作时的速度范围

执行动作	速度范围（㎞/h）
		直线行驶	20-30
换道行驶	20-30
		左右转弯	15-20
U型掉头	5-10

本文中所描述的具体实施例仅仅是对本发明精神作举例说明。本发明所属技术领域的技术人员可以对所描述的具体实施例做各种各样的修改或补充或采用类似的方式替代，但并不会偏离本发明的精神或者超越所附权利要求书所定义的范围。

Claims (3)

1.基于回旋曲线追随的智能车辆路径跟踪方法，其特征在于，包括步骤：

步骤 1，寻找跟踪路径中离智能车辆当前位置最近的GPS点；

步骤2，从步骤1获得的GPS点开始，按智能车辆行驶方向遍历跟踪路径中GPS点，计算智能车辆当前位置和当前遍历点的距离，直至该距离大于当前最小预瞄距离，停止遍历，执行步骤3；所述的最小预瞄距离初始值取9~15米范围内任意值；

步骤3，以智能车辆当前位置为起点、当前遍历点为终点生成回旋曲线，并获得回旋曲线的起点曲率、曲率变化率和终点曲率；

步骤4，若起点曲率和终点曲率的绝对值中最大值小于曲率阈值，或者起点曲率和终点曲率的绝对值中最大值不小于曲率阈值、但当前最小预瞄距离为辅助最小预瞄距离，以当前遍历点为预瞄目标点；所述的曲率阈值取0.05-0.15内任意值；

步骤5，若起点曲率和终点曲率的绝对值中最大值不小于曲率阈值、但当前最小预瞄距离为最小预瞄距离初始值，以辅助最小预瞄距离为当前最小预瞄距离，然后执行步骤2；所述的辅助最小预瞄距离取5-7米内任意值；

步骤6，以智能车辆当前位置为起点、预瞄目标点为终点生成回旋曲线，根据回旋曲线的起点曲率、曲率变化率、终点曲率和长度获得智能车辆的方向盘转角。

2.如权利要求1所述的基于回旋曲线追随的智能车辆路径跟踪方法，其特征在于：

步骤6中，采用阿卡曼原理获得智能车辆的方向盘转角。

3.基于回旋曲线追随的智能车辆路径跟踪系统，其特征在于，包括：

第一模块，用来寻找跟踪路径中离智能车辆当前位置最近的GPS点；

第二模块，用来从第一模块获得的GPS点开始，按智能车辆行驶方向遍历跟踪路径中GPS点，计算智能车辆当前位置和当前遍历点的距离，直至该距离大于当前最小预瞄距离，停止遍历；所述的最小预瞄距离初始值取9~15米范围内任意值；

第三模块，用来以智能车辆当前位置为起点、当前遍历点为终点生成回旋曲线，并获得回旋曲线的起点曲率、曲率变化率和终点曲率；

第四模块，用来若起点曲率和终点曲率的绝对值中最大值小于曲率阈值，或者起点曲率和终点曲率的绝对值中最大值不小于曲率阈值、但当前最小预瞄距离为辅助最小预瞄距离，以当前遍历点为预瞄目标点；所述的曲率阈值取0.05-0.15内任意值；

第五模块，用来若起点曲率和终点曲率的绝对值中最大值不小于曲率阈值、但当前最小预瞄距离为初始最小预瞄距离，以辅助最小预瞄距离为当前最小预瞄距离，然后执行步骤2；所述的辅助最小预瞄距离取5-7米内任意值；

第六模块，用来以智能车辆当前位置为起点、预瞄目标点为终点生成回旋曲线，根据回旋曲线的起点曲率、曲率变化率和终点曲率获得智能车辆的方向盘转角。