一种基于多点跟踪的无人驾驶汽车路径跟踪控制装置的制作方法

本实用新型涉及一种无人驾驶汽车路径跟踪控制装置，特别涉及一种基于多点跟踪的无人驾驶汽车路径跟踪控制装置。

背景技术：

路径跟踪是无人驾驶汽车中执行规划决策层指令必不可少的一个环节，路径跟踪能力的好坏直接影响无人驾驶汽车行驶安全性。对于路径跟踪方法而言，跟踪参考点选择的好坏与否对于路径跟踪能力有很大的影响，其将影响路径跟踪精度，甚至影响无人驾驶汽车的安全性。

与传统的路径跟踪采用单点预瞄方法的不同，动态调整的多点预瞄路径跟踪方法不仅可以根据车辆状态变化调整预瞄点的位置，还可以根据车辆运行信息来调整预瞄点的数目和各个预瞄点的对应输出权重。随着无人驾驶技术的逐渐发展，针对多点预瞄的路径跟踪研究也逐渐深入，以此提高路径跟踪精度，提高无人驾驶车辆的舒适性和安全性。

中国专利cn201810409217.6公开了一种基于纯追踪模型的工业机器人路径跟踪方法，根据纯跟踪的方法，使用目标点集内所有点的角度偏差和位置偏差的平均值，其本质上还是单点预瞄，没有根据车辆行驶状态和道路参数来动态调整目标点集的位置，很可能达不到理想的跟踪精度。

中国专利cn201711183868.x公开了一种无人车路径跟踪前馈控制方法，进行路径跟踪的控制方法中，在传统车辆路径跟踪系统模型中加入道路曲率作为干扰输入项。另外，该专利中,仅仅以横向位置偏差趋于零作为目标函数设计了一种前馈控制方法。没有考虑航向角偏差而引起的跟踪误差，这将会在某些道路条件下造车车辆“画龙”现象。

中国专利cn200910160909.2公开了一种智能车辆路径跟踪前轮转角补偿控制方法，使用了改进的purepursuit算法，在其路径跟踪控制器中前视距离通过纵向车速模糊控制器和路径曲率信息在线自适应得到，本质上是单点预瞄，不能够全面的考虑车辆行驶过程中预瞄的远点轨迹和近点轨迹对于路径跟踪的同时共同影响。

技术实现要素：

本实用新型的目的就在于针对上述现有技术的不足，提供一种基于多点跟踪的无人驾驶汽车路径跟踪控制装置，从提高路径跟踪精度的角度出发，采用动态调整的多点预瞄方法，提高车辆的路径跟踪精度和安全性。

本实用新型的目的是通过以下技术方案实现的：

一种基于多点跟踪的无人驾驶汽车路径跟踪控制装置，由gps数据采集模块1、车辆速度采集模块2、数据预处理模块3、计算模块4和线控执行系统5组成。所述gps数据采集模块1和车辆速度采集模块2分别经数据预处理模块3、计算模块4与线控执行系统5相连。

所述gps数据采集模块1和车辆速度采集模块2为数据预处理模块3提供原始数据，所述数据预处理模块3将处理好的信息传递给计算模块4，所述计算模块的输出与线控执行系统5相连。

所述gps数据采集模块1和车辆速度采集模块2采集数据与获取数据的周期均为t1，计算模块4的信号输出周期为t2；gps数据采集模块1实时采集和获取车辆的位置和姿态信息，并采集需跟踪路径上的gps坐标点，车速采集模块2采集车速信息。

所述数据预处理模块3将的跟踪路径上的坐标点进行预处理，将预处理好的gps坐标路径点作为路径跟踪的参考点。

所述计算模块4计算跟踪路径参考点的选择预瞄窗口，即预瞄最远距离和预瞄最近距离；确定预瞄间隔，过滤选择若干预瞄点；计算当前车辆坐标点选定好的若干预瞄点的航向角偏差和位置偏差；计算出每一个预瞄坐标点相对应的前轮转角；根据道路状况信息和车辆状态信息确定每一个预瞄坐标点相对应的前轮转角的权重，最后各前轮转角按照之前的权重进行加和输出。

与现有技术相比，本实用新型的有益效果在于：本实用新型根据车辆的运行状态和道路环境的信息动态调整预瞄窗口的上下限，并且动态调整了预瞄窗内预瞄间隔，使预瞄点的确定更加合理，对不同行驶状态和不同的路况都有较大的适应性。本发明进一步还使用航向偏差和位置偏差的共同作用，使前轮转角的计算时考虑的信息更加全面，最后通过各预瞄点按等权重计算出了最终输出的方向盘转角。

附图说明

图1为基于多点跟踪的无人驾驶汽车路径跟踪控制方法流程示意图；

图2为基于多点跟踪的无人驾驶汽车路径跟踪控制装置示意图；

图3为航向角偏差和横向位置偏差示意图。

图中，1.gps数据采集模块2.车辆速度采集模块3.数据预处理模块4.计算模块5.线控执行系统。

具体实施方式

下面结合实施例对本实用新型作进一步说明，应理解这些实施例仅用于说明本实用新型而不用于限制本实用新型的范围，在阅读了本实用新型之后，本领域技术人员对本实用新型的各种等同形式的修改均落于本申请所附权利要求所限定的范围内。

如图1和图2所示，基于多点跟踪的无人驾驶汽车路径跟踪控制装置，由gps数据采集模块1、车辆速度采集模块2、数据预处理模块3、计算模块4和线控执行系统5组成。所述gps数据采集模块1和车辆速度采集模块2为数据预处理模块3提供原始数据，所述数据预处理模块3将处理好的信息传递给计算模块4，所述计算模块的输出与线控执行系统5相连。结合图3，a为预瞄窗口内的一个预瞄点，b为预处理过的理想参考轨迹，c为车辆后轴中心线中点与预瞄点的横向偏差，d为车辆，e为车辆纵轴线与车辆后轴中心线中点和预瞄窗口上界参考点的连线的夹角，以车辆纵轴线为参考线以逆时针方向旋转至车辆后轴中心线中点和预瞄窗口上界参考点的连线的到的角度为正。

gps数据采集模块1和车辆速度采集模块2采集数据与获取数据的周期均为t1，本实施例中取0.002秒，计算模块4的信号输出周期为t2，本实例中取0.005秒。

gps数据采集模块1实时采集和获取车辆的位置和姿态信息，并采集需跟踪路径上的gps坐标点，车速采集模块2采集车速信息；

数据预处理模块3将的跟踪路径上的坐标点进行预处理，将预处理好的gps坐标路径点作为路径跟踪的参考点；

计算模块4计算跟踪路径参考点的选择预瞄窗口，即预瞄最远距离和预瞄最近距离；

计算模块4确定预瞄间隔，过滤选择若干预瞄点；

计算模块4计算当前车辆坐标点选定好的若干预瞄点的航向角偏差和位置偏差；

计算模块4计算出每一个预瞄坐标点相对应的前轮转角；

计算模块4根据道路状况信息和车辆状态信息确定每一个预瞄坐标点相对应的前轮转角的权重，最后各前轮转角按照之前的权重进行加和输出；

数据预处理模块3计算时，包含以下步骤：

第一步、将gps数据采集模块1采集的gps经纬度坐标信息转化为直角坐标，转化时忽略海拔高度上的因素，假定数据采集处理单元采集的gps坐标点在同一水平高度上，选择参考路径上的第一个点为直角坐标原点，得到直角坐标系下的跟踪路径轨迹坐标；

第二步、将转化得到的跟踪路径上的点用高斯滤波的方法滤掉路径上的毛刺点；

计算模块4计算跟踪路径参考点的选择的预瞄窗口，包括以下步骤：

第一步、预瞄窗口的上界距离的确定按下式计算：

其中，k1为预瞄窗口上界的车速系数，取值范围为0.1-3本实例中取3，vcar为车辆的实际速度，本实例计算时的取值为单位为m/s时的速度数值，k2为预瞄窗口上界的角度系数，取值范围为0-20，本实例取10，θ为车辆纵轴线与车辆后轴中心线中点和预瞄窗口上界参考点的连线的夹角，以车辆纵轴线为参考线以逆时针方向旋转至车辆后轴中心线中点和预瞄窗口上界参考点的连线的到的角度为正，k3为预瞄窗口上界的最小预瞄距离，取值为5-10，本实例中取8；

第二步、预瞄窗口的下界距离的确定按下式计算：

llow＝i1×vcar+i2×|sinθ|²+i3，

其中，i1为预瞄窗口下界的车速系数，取值范围为0.1-3，本实例中取1，vcar为车辆的实际速度，计算时的取值速度数值，i2为预瞄窗口上界的角度系数，取值范围为0-20，本实例中，取值为10，θ为车辆纵轴线与车辆后轴中心线中点和预瞄窗口上界参考点的连线的夹角，以车辆纵轴线为参考线以逆时针方向旋转至车辆后轴中心线中点和预瞄窗口上界参考点的连线的到的角度为正，i3为预瞄窗口上界的最小预瞄距离，取值为5-10，本实例中取5；

计算模块4计算预瞄间隔的确定及预瞄点，包含以下步骤：

步骤一、预瞄点间的间隔的确定按下式计算：

其中，l为相邻预瞄点之间的间隔，lup为预瞄窗口的上界距离，llow为预瞄窗口的下界距离，a为预瞄间隔系数本实例中取1，r为预瞄窗口中的弧线的最大曲率半径；

步骤二、预瞄窗口内预瞄点的确定，按照下面的方法确定：

如果l≥(lup-llow)，则只取lup，llow两个预瞄距离对应的预瞄点，具体的确定方法为，以车辆后轴中点为原点，以lup，llow为半径画圆，取与参考路径在车辆前方的交点为预瞄点；

如果l＜(lup-llow)，则各个预瞄点对应的预瞄距离为llow，llow+l,llow+2×l,…,lup，同样以车辆的后轴中心点为，为圆心，以llow，llow+l,llow+2×l,…,lup为半径画圆，取与参考路径在车辆前方的交点为预瞄点；

计算模块4计算出每一个预瞄坐标点相对应的前轮转角，计算公式如下：

式中，δ’为前轮转角，l车辆前轴与后轴之间的距离，α为航行角偏差，ld为预瞄距离，kk为横向偏差系数，本实例取值0.05，derr为横向位置偏差；

计算模块4根据道路状况信息和车辆状态信息确定每一个预瞄坐标点相对应的前轮转角的权重，最后各前轮转角按照等权重进行加和输出，计算公式如下：

式中，δ为输出前轮转角，δ’i为每一个预瞄点对应的输出的前轮转角，n为预瞄点个数。

本实施例提供了一种基于多点跟踪的无人驾驶汽车路径跟踪控制方法。依照本实施例的控制方法，能够针对不同的曲率的路径进行跟踪，以此提高无人驾驶车辆的转向安全性和路径跟踪精度。