自动驾驶车辆避障轨迹规划

徐靖贤

（长安大学 汽车学院，陕西 西安 710064）

随着人工智能不断发展，作为无人驾驶重要模块的轨迹规划已经逐渐成为研究的热点。轨迹规划大致分为全局规划和局部规划，其中局部规划的主要作用是如何考虑自车与周围障碍物位置关系找出避免碰撞的路径。对于换道等避障场景，无人驾驶车辆需要从当前车道变更到相邻车道或偏置行驶，输出一条安全无碰撞的轨迹同时兼顾时效性、舒适性，常利用圆弧、正弦函数生成换道轨迹，但是这些方法无法满足车辆实际行驶过程中的换道避障要求。刘亚龙利用BP神经网络特性进行自学习，通过车辆实际换道轨迹数据建立换道轨迹模型[1]。陈伟为应对环境信息的变化采用了滚动窗口的局部规划方法，在局部规划过程中采用启发式算法进行换道关键路径点处理，再利用过渡圆弧平滑生成变道轨迹[2]。李玮等针对换道轨迹曲率不连续现象引入B样条来进行重规划，从而重新建立自由变道模型[3]。CHEN J等利用八叉树数据结构中的有效操作在线生成有大型重叠三位网格组成的自由空间飞行走廊，然后基于生成满足高阶动力学约束的平滑轨迹[4]。DANIEL M等基于优化将路径规划问题构造成典型二次规划问题，能够实时生成三维最优路径并且满足输入约束从而较好解决了紧密约束环境下的轨迹生成问题[5]。DING W C等使用统一的方法来描述环境语义信息，建立一种新颖的无碰撞立方体组成的动态约束即时空语义走廊结构，并且使用分段贝塞尔曲线参数化来保证生成轨迹的安全无碰撞且符合动力学要求[6]。本文从车辆避障出发，考虑车辆形状与周围障碍物位置关系提出一种考虑车辆形状的障碍物边界处理方法，并基于运动走廊采用贝塞尔曲线进行轨迹优化从而确保生成轨迹的安全性和可行性。

1 车辆模型建模

1.1 使用笛卡尔坐标系

SL坐标系也叫做Frenet Frame，如图1所示以道路中心线为参考，S表示道路中心线的方向，L表示与道路中心线垂直的方向。在结构化道路上行驶的时候，SL坐标系能够降低求解维度简化问题，道路边界限制更容易转化为线性约束，比XY坐标系更加贴合实际需求，在目前的轨迹规划中得到了广泛的运用。但是由于SL坐标系不能保证唯一性，位置点可能会在曲线上产生多个投影，所以在投影时需要额外的限制和处理。此外SL坐标系下无法对自我车辆的真实运动学进行建模，无法确保轨迹的连续性[7]，这些缺陷很容易导致违反轨迹曲率限制，当道路曲率较大时这一情况会更加明显，在避障场景下选用笛卡尔坐标系则可以避免这些问题[7]。

图1 笛卡尔坐标系和SL坐标系

1.2 车辆模型建立

轨迹规划任务中的约束通常包括运动学约束、两点边界约束和防撞约束。车辆运动学模型可表示为

(x,y)表示车辆后轴中心位置，其他参数含义如表1所示。

表1 车辆运动学模型相关参数及含义

自车形状一般表示为矩形，如图2所示，导致防撞约束在计算中不可微分，判断矩形外观与障碍物是否有交集计算量很大，如果只是考虑后轴中心点，则很难考虑到车辆外形特征，因此，考虑将车辆用两个圆形来代替，这种方法既可以减少计算量又可以保证冗余[7]。

图2 车辆模型示意图

2 构建运动走廊

2.1 障碍物边界预处理

轨迹规划需要考虑自车与周围障碍物的位置关系，即除考虑自车车辆位置与形状，还需要对环境建模。Apollo EM planner 利用撒点的方式基于动态规划的路径步骤提供一条粗略的路径信息，缺点在于需要逐个对障碍物进行碰撞检测复杂度较高（EM planner）。如果能提前将障碍物提前合并处理，虽然降低了解空间，但是也相应避免了碰撞检测，因此，本文基于运动走廊提出一种障碍物合并方法，当存在多个障碍物时求出所有需要关注障碍物的边界信息并进行排列，将位置有重叠的障碍物进行合并，如图3所示障碍物1由两个重叠的障碍物进行合并，分别取障碍物边界最值作为合并组合的边界，依此类推合并完成后可以得到互不重叠的障碍物组合，每个障碍物组合的起点和终点处分别产生通道节点，三个障碍物组合共生成6个通道节点以及对应的障碍物，这样就得到了需要关注的障碍物的边界组合和潜在行驶通道，从而降低周围环境的复杂度便于构建运动走廊。

图3 障碍物合并示意图

2.2 运动走廊生成

轨迹规划不仅要对车辆进行建模，环境同样需要建模处理，上文将车辆简化为两个圆盘的模型，如果将车辆收缩半径对应地进行障碍物膨化，那么车辆的轨迹规划可以视为两个质点的避障轨迹。如图4所示，车辆转向过程中P1为距离前车最近的点即最容易发生角碰的危险点，由此画圆可以得到车辆避障过程中的危险域，半径为图2中车辆模型示意图中圆盘的半径，基于得到的障碍物边界，轨迹规划即可进行传统的全局搜索得到接近全局最优的路径粗解。全局路径规划的研究目前比较成熟，经典的栅格法、A*等算法有着广泛的运用，其中Hybrid A*算法作为A*算法的变种引入了角度维度考虑了车辆运动学特性，得到的路径能够较好的满足避障场景下对于车辆对于转向约束的要求，本文使用该算法得到初始轨迹点，并在此基础上进行扩展得到可行驶通道，Hybrid A*算法研究比较成熟且本文研究重点为运动走廊生成以及轨迹优化故不展开介绍。

图4 危险域划分示意图

全局路径规划得到初始轨迹点，扩展得到期望的运动走廊，即按照轨迹点依次向外扩张矩形直至接触到障碍物边界，多个矩形的联通区域就构成了安全行驶走廊。具体的做法是从初始轨迹点开始进行膨胀，即依次向周围四个方向扩展，每次扩展一定的距离并直到在膨胀方向上矩形框触及障碍物边界，接下来第二个轨迹点开始膨胀，直到所有全局规划得到的轨迹点膨胀完毕。为得到尽量稀疏的运动走廊，已经被膨胀区域覆盖的轨迹点不再进行扩展，如图5所示的膨胀区域即扩展矩形的组合就是所求的可行驶区域，这样就保证可行驶区域内没有障碍物存在，根据可行驶区域进行轨迹优化也就不用考虑障碍物避障，从而能够快速有效优化出安全无碰撞符合车辆运动的轨迹。

图5 运动走廊生成

3 硬约束轨迹优化

为了满足无人驾驶车辆运动特性，轨迹规划要求规划出的轨迹要尽量平滑，为得到最优的平滑轨迹，研究者通常将实际问题转化为带约束优化问题，即在可行轨迹中找到最优的特定轨迹，对得到的轨迹粗解进行优化。轨迹优化有利于路径平滑但是不利于避障，基于避免碰撞检测的思想，本文采用添加硬约束的方法进行轨迹优化[4]，也就是将轨迹平滑优化问题转化为二次优化问题然后添加硬约束进行求解，最后得到满足要求的轨迹。上文根据障碍物边界进行了全局路径规划，得到粗解后对初始轨迹点进行膨胀得到可行驶区域，在该区域内利用贝塞尔曲线的性质进行硬约束优化，就能在保证得到安全的平滑优化轨迹。

3.1 贝塞尔曲线的性质

贝塞尔曲线是由若干控制点按照一定权重组合而成的曲线，convex hull性质保证曲线始终被约束在控制点组成的凸包内，hodograph性质则保证曲线的各阶导数仍是贝塞尔曲线，可以确保规划的轨迹、速度、加速度、加速度连续，后续的优化问题构造都是基于这些性质的，其表现形式如下：

从起点到终点每段轨迹可以表示为

3.2 硬约束优化

轨迹优化问题实际上是最优控制问题，而Minimum Snap算法是经典的轨迹优化算法，主要思想是将轨迹规划问题构造成典型二次规划问题，从而施加约束进行求解，Minimum Snap算法的代价函数自然是snap值的平方，本文使用每段贝塞尔曲线的三阶导平方再求和，即期望过程中jerk最小，k=3，那么总的目标函数可以被写为

基于贝塞尔曲线的凸包性质，轨迹必然约束在凸包内，只要将凸包限制在运动走廊内，优化的轨迹就会在允许边界之内，以此可以得到安全性与运动学不等式约束，两段轨迹之间的连续性则带来等式约束，最终将上述代价函数和约束条件整合起来，构造成一个标准的二次优化问题来进行求解。图6为ros环境下进行的仿真试验，规划轨迹能够较好满足复杂环境下避障要求。

图6 运动走廊贝塞尔曲线优化轨迹

4 总结与展望

基于笛卡尔坐标系建立考虑车辆形状的自车模型，提出一种新的障碍物边界处理方法，根据自车与周围环境的位置关系排除危险行驶区域建立运动走廊，并利用贝塞尔曲线性质进行轨迹优化，通过仿真验证有效地避免与障碍物的碰撞，最大限度保证规划轨迹的安全性和舒适性。不过笛卡尔坐标系在周围环境复杂时计算需求量较大，如何在SL坐标系下建立满足轨迹曲率要求的运动走廊需要进一步探索，另外障碍物合并会减少解空间，如何在保证安全性的同时减少求解空间损耗仍需研究。