避障功能函数-无人驾驶车辆模型预测控制

在无人驾驶车辆行驶环境中，障碍物信息通常由激光雷达的障碍物点给出。当这些障碍物点相互靠近时，可以通过聚类算法将它们形成一个大的障碍物，而障碍物点相互远离时，就可以将其看成多个离散的小障碍物。因此，避障功能函数可以针对每个单独的障碍物进行设计。惩罚函数的基本思路是通过障碍物点与目标点的距离偏差来调节函数值的大小，且距离越近，函数值越大。综合车辆速度与目标函数中惩罚函数的比重对避让障碍物的影响，选择如下形式的避障功能函数：

其中，Sobs为权重系数，是障碍物点在车身坐标系下的位置坐标，(x0，y0)是车辆质心坐标，ζ为较小的正数，用于防止出现分母为0的现象。给定各参数后，函数值随障碍物相对坐标变化的规律如图6.4(a)所示。

为了使轨迹规划的结果实际可行，需要根据车身尺寸对障碍物进行膨胀处理。本书参考文献[1]中的膨胀方法，以无人驾驶车辆运动中心的内切圆和外接圆半径为尺寸对障碍物膨胀。当障碍物尺寸较大时，还需要按照一定的分辨率对障碍物进行分割处理，防止出现车辆从障碍物中间穿越的现象，如图6.4(b)所示。

图6.4　惩罚函数与障碍物膨胀

(a)惩罚函数示意；(b)障碍物膨胀与分割

将上述避障功能函数代入式(6.4)中，设权重系数Sobs分别为10、100和1 000，给定不同初始状态后的规划结果如图6.5所示。当规划层接收到障碍物信息后(图6.5中点1和点2)，规划出避开障碍物的新轨迹，权重系数的增加会使规划结果趋于保守；而当车辆没有接收到障碍物信息(图6.5中点3)时，权重系数不会对规划结果产生任何影响。当车辆状态测量或者估计存在较大误差时，可以采用较大的权重系数，但同时也带来了跟踪偏差增加的问题。(www.xing528.com)

图6.5　不同权重系数下的规划结果

轨迹重规划层中的控制目标是尽量减少与全局参考路径的偏差，并且实现对障碍物的避让。对障碍物的避让以惩罚函数的方式实现。轨迹重规划层的模型预测控制器具体形式如下：

其中，Jobs，i为采样时刻i的避障函数。

由于规划层的实时性要求比控制层低，点质量模型相对于非线性动力学模型也进行了较大程度的简化，具备更高求解精度的非线性模型预测控制算法完全能够满足轨迹重规划的要求，而非线性目标函数的采用也能给后续惩罚函数的设计带来便利。因此，对于式(6.5)不再进行线性化，而是直接基于非线性模型求解。相关解法已在第3章中介绍，此处不再重复。