城市道路汽车自动驾驶路径生成算法＊

赵倬祺，张引，张琨，王严

（华晨汽车工程研究院，辽宁 沈阳 110141）

前言

汽车自动驾驶研究中，应用了大量自动控制及机器人控制算法，但由于应用场景的差异以及对动态性的高度需求，出现了许多适用于特定场景的路径生成算法。例如泊车工况下，要求车辆可以倒退；非结构化道路下，仅有终点状态和障碍物约束，不限制中间运动状态；而结构化道路要求车辆以相对平稳的状态在车道内行驶或换道。

非结构化场景工况常用的算法包括启发式搜索、维诺图、快速扩展随机树[5]、人工势场[7]等。其中 A*[6]和快速扩展随机树，以及其优化算法和得到了广泛应用，并在其原模型基础上加入了阿克曼转向约束。结构化道路受到约束较少，主要以离散化车辆可能行驶路径再进行筛选的方式生成路径；也有一些路径生成算法使用了迭代优化的方式。

在车辆可能行驶路径离散化过程中，受道路曲率或曲线表达式的影响，为保证离散过程中的鲁棒性，算法需要许多特殊处理。本文使用的路径生成算法使用离散化车辆相对参考路径的偏移量的方式，提高算法鲁棒性。同时将人工势场加入路径的代价函数计算中，实现避障。

1 路径生成

1.1 候选路径离散化

汽车行驶过程中，实际候选路径的集合是一个平面，其边缘受到车辆转向约束，因此需要对候选路径进行离散化。本文中的候选路径离散化方法主要考虑结构化道路对车辆的约束，通常可使用道路中心线作为参考路径，同时利用车辆自身转向约束，过滤部分候选路径，以减小代价函数计算量。需要离散化的变量包括：路径终点与参考路径的横向偏差，和达到路径终点时经过的参考路径弧长，如图1所示。到达路径终点车辆的航向也是候选路径的重要因素，但考虑运算量和车辆实际行驶的情况，本文在建模中保留了车辆终点的所有航向，在运算时仅取与参考路径切线方向相同的一个目标方向。

1.2 候选路径生成

决定候选路径的参数共四个，即起点(x0,y0)和终点(x1,y1)的位置及方向。为了使曲线更平滑，限定终点处的二阶导数为 0，起点处曲线二阶导数值为当前车辆前轮转角形成的车身角变化量。以上六个参数可确定一条关于x、y的五次多项式曲线。

设由未知量构成的向量：

假设有矩阵A由多项式系数以及该多项式的一、二阶导函数系数构造而成，则：

则可解得矩阵A，从而得到五次多项式的系统向量：

式中：x、y为平面坐标下的横、纵坐标。

1.3 坐标转换

在算法实现过程中，参考路径点集{Pref}内的横纵坐标可能存在一对多关系，例如U型弯，使用曲线表达式表示路径会使程序鲁棒性严重降低，因此此处多项式公式仅表示候选路径各点与参考路径的距离[1]，再通过弗莱纳标架将候选该距离转换到参考路径坐标下，生成候选路径。

在弗莱纳标架下单位切向量：

式中：r为参考路径弧长， 为法向量方向的单位向量，d为候选路径与参考路径的距离。

通过该坐标转换，可以将横向位移形成的路径转换到实际坐标系下，如图2所示。

1.4 起始点映射

路径生成依据的参考路径是离散的路网点，理想状况下路网点能较好的表现道路曲率，但限于路网数据存储量和传输速度，路网点密度不能达到理想状态，从而表现出折线形式，需要对其进行平滑处理。通常的方法是使用样条线[2]、贝塞尔曲线[4]等，对路网点进行拟合并插值，最终逼近曲线。

坐标转换需要将图1中（0,0）点映射到路网坐标系下，即车辆在参考路径上的位置。文献[3]使用牛顿法计算参考路径曲线上距离车辆最近的点，认为该点为车辆到曲线的垂足，即与该点的距离能表征横向位移。因此可以把该点作为路径起始点。

图1 车辆横向运动分量规划

图2 横向运动分量转换坐标得到的参考路径

2 代价函数

路径规划中，实际可能形成的路径有很多，例如但不限于图3所示，因此需要对路径进行评价。影响规划的因素表示为候选路径的代价。通常影响因素包括候选路径相对参考路径的偏移，路径弯曲程度，路径长度及障碍物。其中：

候选路径偏移使用距离平方的积分：

路径弯曲程度考虑曲线三阶导函数平方的积分：

式中：xr为平面坐标系下的横坐标。

路径长度使用弧长的积分：

式中：roption为候选路径弧长。

障碍物使用分段函数：

为统一计算模型，将障碍物代价表示为：

式中：db为路径距离某一障碍物的距离；dc为发生碰撞的最大距离；ds为障碍物影响距离，可以使用驾驶员主观感受的障碍物安全距离；F为障碍物人工势场对路径的斥力。

所以完整的路径代价为：

图3 候选路径

3 人工势场

人工势场是机器人路径规划领域较成熟的算法，主要思想是通过为障碍物添加虚拟力场，得到目标在虚拟力场作用下的运动路径，作为规划算法生成的目标路径。本文仅使用斥力场计算障碍物对候选路径的代价值。

由斥力势能公式：

式中：pobs为障碍物位置。

4 结论

4.1 试验结论

实验程序使用在Ubuntu环境下使用Python实现，主要验证该算法下车辆行驶路径的表现以及代价函数的参数对路径选取的影响形式。

如图4所示，在结构化道路上，当车辆距离参考路径有一定距离，且行驶方向偏离车道时，受距离项影响，车辆快速到达参考路径上。

在图4相同的起始条件下，增大路径平顺系数以及长度系数，车辆以较平缓的方式接近参考路径，如图 5；继续增大以上两个系数，由于距离项系数占比重减小，车辆平稳的沿参考路径平行的路径行驶，而不必须回到参考路径上，如图6。

在路径前方有障碍的情况下，如图7中蓝色区域所示，区域的中心为障碍物，深色范围为考虑车身尺寸，可能发生碰撞的距离范围，浅色区域为考虑误差及驾驶员感受，障碍物可能影响的范围，在图7 中车辆绕过障碍物继续沿路径方向行驶，增大障碍物代价系数，路径受到浅蓝色区域势场力影响，如图8所示，车辆以更远的距离绕过障碍物。

4.2 后续研究

通常路径生成算法需要配合速度规划，很多相关研究采用了路径和速度相互独立的方式，也有部分将速度、加速度及导数和车辆位置与姿态整合成为完整的状态空间[1]，此方法需要行为决策模块给出更明确的状态信息。可以在弗莱纳坐标下对速度进行整合。

图4 车辆快速靠近参考路径

图5 车辆缓慢靠近参考路径

图6 车辆快速到达参考路径

图7 有障碍物时的路径规划

图8 车辆尽量远离障碍物时的路径规划

在工程化方面，需要调整路径选择的优化算法，目前通过选取适当的前视距离以及贪心算法，使车辆选取的路径在当前状态下选择局部最优，而对于更复杂的路况，则可以使用动态规划算法对其进行优化，以完成向非结构化道路算法的过渡。

[1] Moritz, Werling. Optimal Trajectory Generation for Dynamic Street Scenarios in a Frenét Frame[C]. Anchorage, AK, USA：2010 IEEE International Conference on Robotics and Automation, 2010. 987-993.

[2] K, Chu. Local Path Planning for Off-Road Autonomous Driving With Avoidance of Static Obstacles[J].IEEE Transactions on Intellig-ent Transportation Systems, 2012, 13(4)： 1599-1616.

[3] H. Wang, J. Kearney, and K. Atkinson, “Robust and efficient comput-ation of the closest point on a spline curve,” in Proc. 5th Int. Conf.Curves Surf.,2002, pp. 397-406.

[4] L.Han, H.Yashiro, H.T.N.Nejad, D.Q.Huy, S.Mita," Bezier Curve Based Path Planning for Autonomous Vehicle in Urban Environ-ment," IEEE Intelligence Vehicle symposium, San Diego, June 21-24, 2010.1036-1042.

[5] J. a. S. M. L. James J.Kuffner, "RRT-Connect： An Effficient Approach to Single-Query Path Planning,"Proceedings -IEEE International Conference on Robotics and Automation： Inst. Of Electrical and Ele-ctronics Engineers, New York, vol. 2, pp. 995-1001,2000.

[6] Likhachev, M.; Ferguson, D.; Gordon, G.; Stentz, A.; and Thrun,S.2005. Anytime Dynamic A*： The Proofs. Technical Report CMURI-TR-05-12, Carnegie Mellon School of Computer Science.

[7] J. Barraquand, B. Langlois, and J.C.Latombe. Numerical potential fie-ld techniques for robot planning.IEEE Transactions on Man and Cybernetics, 22(2)：224-241, Mar/Apr 1992.