一种考虑路径曲率的新型几何学路径跟踪算法的制作方法

文档序号：18408771发布日期：2019-08-10 00:43阅读：1646来源：国知局

本发明涉及无人驾驶汽车路径跟踪的技术领域，更具体地，涉及一种考虑路径曲率的新型几何学路径跟踪算法。

背景技术：

汽车的横向控制是实现无人驾驶汽车自主行驶的关键技术之一。其中，路径跟踪是通过自主转向控制车辆始终沿着期望路径行驶，同时保证车辆的乘坐舒适性和行驶安全性，是连接智能车上层软件与底层硬件的桥梁，是实现无人驾驶的必备环节。考虑曲率的路径跟踪算法，即根据路径曲率推测前轮转角以模仿路径变化趋势，再利用负反馈来消除横向偏差，利用航向偏差负反馈来抑制前轮转角大小，防止超调。

目前，国内外许多研究者针对无人驾驶汽车的路径跟踪控制，大都使用横向偏差和航向偏差作为控制器的输入，使用反馈控制方法实现无人驾驶汽车路径跟踪。最常用的路径跟踪方法之一是几何学控制器，它利用路径与车辆以及车辆自身的几何关系，提出路径跟踪问题的控制解决方案。最具有代表性的几何学控制器是纯追踪算法，由于纯追踪算法模型简单、计算量小且具有较好的跟踪效果而被广泛的应用。

纯追踪算法的关键在于前视距离的合理选择。前视距离过大，将导致跟踪的横向误差消除缓慢且在转弯路段出现明显的抄近路现象；前视距离过小，在极低速情况下有较好的跟踪效果，但在高速情况下出现极其严重的震荡现象，导致跟踪效果恶化。因此，很多国内外学者基于纯追踪算法提出了很多改进算法以扩展纯追踪算法的稳定性。anibalollero提出了一种基于模糊逻辑调整前视距离的跟踪算法；mit团队提出了一种根据汽车速度自适应调节前视距离参考点的跟踪算法。由于以上控制策略没有考虑路径曲率，在路径曲率连续变化尤其是曲率较大时跟踪出现抄近路现象。

技术实现要素：

为解决上述问题，本发明公开了一种考虑路径曲率的新型几何学路径跟踪算法，根据参考路径的曲率计算对应的汽车前轮转角以模仿参考路径变化趋势，利用负反馈消除模仿过程中的横向偏差，利用航向偏差抑制超调，最终实现路径跟踪，能够提高跟踪精度以及跟踪稳定性。

为达到上述目的，本发明的技术方案如下：

一种考虑路径曲率的新型几何学路径跟踪算法，包括以下步骤：

s1.利用gps以固定频率采集路径离散坐标点得到参考路径点集p；

所述gps定位精度为厘米级，所述的参考路径点集p由平面直角表示，是通过gps输出的经纬度进行坐标转换为utm坐标(通用横轴墨卡托坐标)所得，目的是便于计算路径曲率以及将位于全局坐标系xglobal-yglobal的路径点转换到移动的车体坐标系x_v-x_v。

s2.查找离当前汽车位置最近的路径点pi(pi∈p)；

参考路径点集中的数据是按照顺序排列的，只有第一次查找最近路径点时需要全局查找，之后只需进行局部查找，优势是节省计算资源以及提高查找速度。

s3.利用最小二乘法计算路径点pi处对应的路径曲率ρi；

分别选取点pi(xi,yi)前后各3个路径点，共7个点pi(xi-3,yi-3)，pi(xi-2,yi-2)，pi(xi-1,yi-1)，pi(xi,yi)，pi(xi+1,yi+1)，pi(xi+2,yi+2)，pi(xi+3,yi+3)。其中i>3

利用最小二乘法进行三次曲线y＝ai+bi·x+ci·x²+di·x³拟合,即根据a·x＝b求矩阵b，由于上述超定方程组无解，所以利用最小二乘原理求近似解由得

其中：

得到三次曲线y＝ai0+bi0·x+ci0·x²+di0·x³

再利用曲率公式计算点pi(xi,yi)点的曲率ρi。

需要指出的是，所述的曲线拟合方法，通过判断有序路径点x坐标是否单调变化(递增/递减)，决定曲线拟合方向(沿x轴/y轴)，具体为：

如果满足xi-3＞xi-2＞xi-1＞xi＞xi+1＞xi+2＞xi+3，则有序路径点的x坐标为单调递减，如果满足xi-3＜xi-2＜xi-1＜xi＜xi+1＜xi+2＜xi+3，则有序路径点的x坐标为单调递增，以上两种情况下，沿x轴方向进行y＝ai+bi·x+ci·x²+di·x³曲线拟合；

否则，有序路径点的x坐标不单调，此时，沿y轴方向进行x＝ai+bi·y+ci·y²+di·y³曲线拟合。

当曲线拟合沿y轴时，曲率求解以及路径点航向求解也需作出相应的调整。

需要强调的是，在局部路径内，有序路径点的x坐标不单调时，其y坐标必定的单调。

s4.根据阿克曼转向几何模型计算ρi对应的前轮转角θρi以及前轮方向角

根据阿克曼转向原理，前轮转角与转弯曲率之间的几何关系可以表示为：

tan(θρi)＝l·ρi

于是得：

θρi＝arctan(l·ρi)

进一步，得前轮方向角

其中，θρi为前轮转向角，l为汽车轴距，ρi为路径点pi处的曲率，为前轮方向角，yawcurrent为当前汽车航向角。

s5.计算汽车当前位置与路径点pi处的航向偏差yawerr、横向偏差laterr；

对步骤s3中拟合的三次曲线进行一阶求导可得路径参考点的航向角yawi即

yawi＝y′(xi)

于是得航向偏差：

yawerr＝yawi-yawvehicle

其中，yawerr为航向偏差，yawi为路径参考点航向角，yawvehicle为汽车航向角。

为求得横向偏差，将路径参考点在全局坐标系下的坐标(xr,yr)转换到车体坐标系下的坐标(xr,yr)，转换公式可以表示为：

其中(xvehicle,yvehicle)为汽车在utm坐标下的位置坐标。

于是得横向偏差：

其中sign()为符号函数。

s6.根据前轮方向角航向偏差yawerr和横向偏差laterr设计路径跟踪控制器，计算前轮期望方向角进而求得前轮期望转角θt。

路径跟踪控制器可以表示为：

其中为期望前轮方向角(前轮与正北方向的夹角)，用以模仿路径变化趋势，

k1为横向偏差校正系数，用以消除横向偏差，

k2为航向偏差校正系数，用以抑制前轮转角，防止超调。

最后得出前轮期望转角

本发明的有益效果为：

针对无人驾驶汽车提出一种考虑曲率的新型几何学路径跟踪算法，利用路径曲率计算前轮转角模仿路径变化趋势，利用负反馈调节横向偏差以及利用横向偏差负反馈抑制横向偏差超。实验证明，本发明所提出的路径跟踪算法对于曲线路径有较好的适用性。本发明算法原理简单，计算复杂度低，不仅适用于仿真技术领域，还能够在实车嵌入式控制器上实现，有很好的应用前景。

附图说明

图1为本发明的算法流程图。

图2为阿克曼转向模型示意图。

图3、4、5为本发明路径跟踪控制器仿真结果。

具体实施方式

下面结合附图和具体实施方式，进一步阐明本发明，应理解下述具体实施方式仅用于说明本发明而不用于限制本发明的范围。

本发明的总体设计思路是：根据参考路径的曲率计算对应的汽车前轮转角以模仿参考路径变化趋势，利用负反馈消除模仿过程中的横向偏差，利用航向偏差抑制超调，最终实现路径跟踪。

本发明的实施例包含以下步骤：