[发明专利]一种考虑路径曲率的新型几何学路径跟踪算法

说明书

本发明公开了一种考虑路径曲率的新型几何学路径跟踪算法，包括以下步骤：S1.利用GPS以固定频率采集路径离散坐标点得到参考路径点集P；S2.查找离当前汽车位置最近的路径点p_i(p_i∈P)；S3.利用最小二乘法计算路径点p_i处对应的路径曲率ρ_i；S4.根据阿克曼转向几何模型计算ρ_i对应的前轮转角θ_ρi以及前轮方向角S5.计算汽车当前位置与路径点p_i处的航向偏差yawErr、横向偏差latErr；S6.根据前轮方向角航向偏差yawErr和横向偏差latErr设计路径跟踪控制器，计算前轮期望方向角进而求得前轮期望转角θ_t。本发明算法简单，不仅适用于仿真技术领域，还能够在实车嵌入式控制器上实现，有较好的应用前景。实验证明，本发明所提出的方法跟踪误差小，且具有较强的鲁棒性。

技术领域

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

背景技术

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

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

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

发明内容

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

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

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

S1.利用GPS以固定频率采集路径离散坐标点得到参考路径点集P；

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

S2.查找离当前汽车位置最近的路径点p_i(p_i∈P)；

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

S3.利用最小二乘法计算路径点p_i处对应的路径曲率ρ_i；