一种自动驾驶车辆路径规划及跟踪控制系统及方法

本发明涉及自动驾驶汽车决策控制领域，尤其涉及一种自动驾驶车辆路径规划及跟踪控制系统及方法。

背景技术：

1、随着自动驾驶技术近年以来的突飞猛进，自动驾驶汽车在交通出行上逐渐有了崛起之势，在科研方面也成为了车辆工程研究的重点领域，自动驾驶汽车的路径跟踪技术为汽车自动驾驶的核心控制技术之一，更是受到了国内外专家学者的普遍及重点关注。但是，当前路径跟踪研究多是局限于提高跟踪精度，从而将车辆稳定性忽略，而且路径跟踪精度的优化多是通过对控制算法误差的降低来实现的，同时提高精度会导致控制算法复杂化而降低算力和效率，进一步降低了自动驾驶车辆实际路径跟踪的可行性。一方面，路径跟踪过程本身就影响了车辆系统的稳定性，且跟踪精度的提高可能会导致车辆稳定性的恶化，极端工况下甚至会影响到驾驶员的安全性；另一方面，现有的路径跟踪优化算法通常局限于控制方法的改变及复杂化，忽略了道路曲率本身对路径跟踪精度的影响。

2、自动驾驶车辆的路径跟踪控制算法是车辆实现自动驾驶功能的重要一步，作为车辆执行器的控制环节，路径跟踪控制理应随着自动驾驶领域的不断发展而提高。高精度自动驾驶需要能适应行车道路环境的变化以及车辆自身变化，因此对车辆路径跟踪控制的跟踪精度、稳定性、安全性等有着更高的要求。

技术实现思路

1、为了解决现有技术中存在的不足，本技术提出了一种自动驾驶车辆路径规划及跟踪控制系统及方法，从曲率优化和车身舒适性、操纵稳定性的角度去改善跟踪效果，提高路径跟踪精度的同时改善智能车辆行驶的稳定性和平顺性。

2、本发明所采用的技术方案如下：

3、一种自动驾驶车辆路径规划及跟踪控制方法，包括如下步骤：

4、s1、实时采集参考路径所输入的目标路径信息、车辆实时位姿信息、车辆运动状态信息；

5、s2：根据车辆当前的位置在参考路径中找到最近参考点以及参考点对应的车辆实时位姿以及曲率；

6、s3：根据车辆当前的位置、车辆运动状态信息以及参考点的信息(坐标、横向误差和曲率)分别计算出三自由度动力学离散模型的车辆运动状态信息、参考路径跟踪误差，实时更新mpc控制器的单元数据；

7、s4：设置跟踪精度阈值、速度阈值以及加速度阈值，且作为是否需要路径再规划的判断标准；基于s3获得的数据进行判定，即将实际车速vf、加速度af、跟踪误差ey分别与设定对应速度阈值vdefault、加速度阈值adefault和跟踪精度阈值edefault进行比较；若任何一个指标超出限制值，转步骤s5进行路径再规划；若三个指标均未超出限制值，则输出当前规划的路径；

8、s5：进行路径再规划，遗传算法求解路径规划问题；

9、s6：构建贝塞尔曲线，拟合规划路径并输出新路径。

10、进一步，s2的步骤如下：

11、s2.1：参考路径上寻找离车辆当前位置最近的点作为参考点，并提取该参考点的坐标；根据参考点的坐标分别计算参考点的航向角当前车辆的位姿信息，参考路径的参考点初始坐标(x0,y0)；

12、s2.2：基于所选取的参考点坐标，并计算横向误差；

13、s2.3：基于所选取的参考点坐标，计算参考点的曲率。

14、进一步，s3的步骤包括：

15、s3.1：构建三自由度的车辆动力学模型；

16、s3.2：计算未考虑道路跟踪误差的三自由度动力学离散模型的状态量，包括道路曲率、参考路径跟踪误差、车辆的航向偏差变化率；

17、s3.3：对所构建的三自由度的车辆动力学模型进行离散化处理得到三自由度动力学离散模型。

18、进一步，s4中判断过程如下：

19、s4.1：获取车辆自身的实际车速vf，加速度af，计算跟踪误差ey。

20、s4.2：将实际车速vf、加速度af、跟踪误差ey分别与设定的速度阈值vdefault、加速度阈值adefault和跟踪精度阈值edefault进行比较；

21、若任何一个指标超出限制值，则需要进行路径再规划；若三个指标均未超出限制值，则输出当前规划的路径。

22、进一步，s5中利用遗传算法求解路径规划的步骤如下：

23、s5.1：解码与编码，采用二进制编码，优化参数为参考路径的曲率k；

24、s5.2：对优化参数进行解码输出；

25、s5.3：初始化种群，随机产生一个初始种群h＝{γ1,γ2,......,γn}，包含n条染色体，每一条都为一个路径再规划的方案，染色体其结构表示为γi＝{u1,u2,......,up}，up为0或1；对选择的染色体进行优化，计算每次迭代种群内不同染色体之间的差异度；增加判断准则：在每次迭代中，只有种群的相似性指标低于阈值μ时，这些染色体才能进行下一次迭代；μ的初始值为(0，1)区间内的随机值，在选代过程中的值μ从初始值线性下降到0，衰减因子ω＝0.99代表衰减率，即μ的衰减公式为：μn+1＝ωμt，当t趋近于无穷时，μ趋近于0；

26、s5.4：选择适应度函数，寻找到距离参考路径最近的目标路径进行跟踪。

27、进一步，适应度函数表示为：

28、

29、其中，kf为优化路径的曲率，ey为跟踪误差，k为参考路径曲率。

30、进一步，优化曲率应该满足车身稳定性的要求：

31、

32、其中，ay为车身横向加速度，adefault为系统内置加速度阈值，kdefault为系统内置曲率阈值。

33、进一步，s6的过程如下：

34、s6.1：将环境地图以二值化进行处理，构建具有多个像素点的网格地图；当该网格是贝塞尔曲线的控制点时，其值取为1，不是贝塞尔曲线的控制点时，其值取为0；若障碍物覆盖网格处，则其值设为-1；若路径穿过障碍物，即穿过值为-1的网格点，则此路径点会在后续的遗传操作中通过避障操作移除；

35、s6.2：定义低阶连续性准则：连接起始点的一条线段具有0阶连续性；在两条线段的连接处，用等效切线保证一阶连续性；三阶以上则通过贝塞尔曲线保证其连续；

36、s6.3：在满足连续性的前提下，采取三阶贝塞尔曲线，表示为：

37、f(t)＝b0(1-t)3+3b1(1-t)2t+3b2(1-t)t2+b3t3

38、其中，t为参数变量，t＝0……1，bi为第i个贝塞尔曲线控制点。

39、s6.4：输出生成的路径，为下一次跟踪控制提供新规划的路径。

40、一种自动驾驶车辆路径规划及跟踪控制系统，包括数据采集模块、mpc控制器、误差处理单元、路径再规划模块；

41、数据采集模块获取参考路径，采集参考路径所输入的目标路径信息、车辆实时位姿信息、车辆运动状态信息；

42、mpc控制器包括mpc横向控制器和mpc纵向控制器，在mpc控制器中，实时更新的mpc控制器单元数据并将单元数据输入给mpc横向控制器；由mpc横向控制器输出车前轮转角，并将车前轮转角输入mpc纵向控制器计算汽车的纵向速度、纵向加速度；

43、误差处理单元分别连接数据采集模块、mpc控制器，误差处理单元对数据采集模块、mpc控制器的输出结果进行跟踪精度判定、速度判定及曲率判定；

44、路径再规划模块根据误差处理单元的判定结果，判断是否需要执行路径再规划。

45、进一步，车辆运动状态信息包括车辆横摆角速度ω、车辆纵向速度vx、车辆横向速度vy；参考路径为离散的参考路径点集；车辆实时位姿信息包括车辆在车身坐标系下的质心位置、车辆航向角目标路径信息为离散的参考路径点集，其中包含大地坐标系下的路径参考点的横纵坐标与曲率信息。

46、本发明的有益效果：

47、本发明的目的在于提供一种曲率优化的自动驾驶车辆路径再规划及路径跟踪控制方法，从曲率优化和车身舒适性、操纵稳定性的角度去改善跟踪效果，提高路径跟踪精度的同时改善智能车辆行驶的稳定性和平顺性。