引用本文

袁静妮, 杨林, 唐晓峰, 陈傲文. 基于改进RRT* 与行驶轨迹优化的智能汽车运动规划. 自动化学报, 2022, 48(12): 2941−2950 doi: 10.16383/j.aas.c190607

Yuan Jing-Ni, Yang Lin, Tang Xiao-Feng, Chen Ao-Wen. Autonomous vehicle motion planning based on improved RRT* algorithm and trajectory optimization. Acta Automatica Sinica, 2022, 48(12): 2941−2950 doi: 10.16383/j.aas.c190607

http://www.aas.net.cn/cn/article/doi/10.16383/j.aas.c190607

关键词

改进RRT* 算法，概率采样，路径简化，贝塞尔曲线，轨迹优化 

摘要

针对传统快速扩展随机树算法 (Rapidly-exploring random tree, RRT)搜索较慢、规划路径曲折、平顺性差等问题, 提出了一种结合改进RRT* 与贝塞尔曲线控制点优化的智能车辆运动规划方法. 该方法通过在给定概率分布下采样, 结合基于方向相似性的多步扩展与路径简化, 使用贝塞尔曲线拟合生成规划问题初始解, 最后使用序列二次规划优化曲线控制点, 从而在动态障碍物环境中生成兼具安全性与驾驶舒适性的车辆行驶轨迹. 在仿真实验中将本文算法与常规RRT及曲线拟合方法进行了比较, 结果显示本文算法在搜索速度、平顺性、安全性等方面有较大提升.

文章导读

为了提高出行效率和保障行车安全, 辅助驾驶与自动驾驶技术逐渐成为研究热点[1], 车辆运动规划是自动驾驶中的重要环节. 为提升规划路径的安全性、舒适性与计算效率, 研究者提出并改进了各类运动规划算法或局部路径/速度规划算法[2-3], 主要包括曲线规划、图搜索、人工势场法、采样方法、数值优化与数据驱动规划算法等.

曲线规划常用于泊车或局部路径平滑[4], 文献[5]通过枚举轨迹曲线, 并根据成本函数择优, 但可行路径无法穷尽, 在复杂场景中不可用. 图搜索[6]生成的轨迹不满足车辆动力学、交通规则等各类约束, 且其计算效率限制了网格划分密度与搜索空间维数. 人工势场法[7]计算量小, 在实时避障和路径平滑方面广泛应用, 但不合理的势场构建会使行驶路径陷入局部最优或产生振荡. 采样方法[8]实时性好且能够在采样过程中考虑各类约束, 但存在随机性, 相同问题规划结果可能不同. 数值优化[9-10]可以综合考虑障碍物约束、车辆动力学模型、交通规则、路径曲率、能耗等各种评价指标, 可将最优控制问题转换为非线性规划求解, 但该方法对初始解要求较高且计算时间长; 也可将模型线性化后使用二次规划求解, 但线性化会降低求解精度. 基于深度学习的端到端运动规划算法[11], 输入传感器信号或感知结果, 模型输出运动规划结果或车辆控制信号, 该方法可输出接近人类驾驶习惯的规划结果, 但较难泛化到极端场景, 且存在规划结果帧间跳变问题. 基于强化学习的运动规划算法[12]可以使模型自动学习最优规划, 但在仿真场景中训练时, 障碍物行为与实际路上行为存在一定差异, 从而影响算法最终的实车表现.

综合考虑以上方法的优缺点, 采样方法计算效率高、对于各类行驶场景通用性好, 适合车载实时应用, 但需对其采样、扩展策略进行调整使之满足车辆行驶过程中的各类复杂约束. 采样方法中, 概率地图方法(Probabilistic roadmaps, PRM)[13]或者基于PRM的快速行进树算法(Fast marching tree, FMT)[14]是概率不完备的, 当采样点较少或分布不合理时可能无法求得可行解. 快速扩展随机树算法(Rapidly-exploring random tree, RRT)计算速度较快且概率完备. 机器人领域使用的RRT方法[15]生成的路径曲折且搜索方向不受引导, 无法直接应用于车辆, 因此研究者们针对车辆运动规划问题特性提出了很多改进RRT算法. 文献[16]提出的RRT* 方法使路径在搜索与重连接过程中渐近最优. 文献[17]对直道、弯道变道超车场景分别建立高斯分布采样模型以生成符合交通规则的行驶轨迹, 该方法局限于特定场景且未考虑动态障碍物. 文献[18]添加转向角约束, 文献[19]采用B样条曲线平滑RRT* 搜索的路径, 但均未考虑动态障碍物, 且直接将路径节点用作曲线控制点可能会导致曲线超出曲率上限或因偏离原路径提高碰撞风险. 闭环RRT (Closed-loop RRT, CL-RRT)[20]能够生成满足车辆动力学约束的轨迹, 但由于未采用路径简化与局部路径优化, 可能会导致车辆在直路上蛇形行驶, 文献[8]基于规则模板集进行路径扩展, 并使用CL-RRT中的闭环预测方法求解可行驶的轨迹, 尽管轨迹可行驶, 但并未针对行驶轨迹的舒适性、安全性和能耗进行优化.

针对以上问题, 本文针对自动驾驶车辆局部路径、速度规划问题, 提出了一种高效的自动驾驶车辆运动规划求解方法, 首先使用改进的采样算法快速求解接近最优解的启发解, 然后基于启发解优化得到可行驶、舒适、节能的规划轨迹. 本文算法结合改进的RRT* 算法与曲线控制点优化, 在动态障碍物环境中生成可行驶的初始局部路径, 并优化行驶轨迹以提升平顺性、安全性及降低能耗. 相比基于采样的规划算法, 本文算法规划的轨迹更平顺、节能且控制可执行; 相比基于优化的规划算法, 本文算法中的启发解提高了优化的求解成功率和求解速度. 本文提出的改进RRT* 算法, 在传统RRT* 算法[16]基础上结合了概率采样、多步节点扩展与基于航向相似性的路径简化方法. 首先, 改进RRT* 根据驾驶场景设置采样概率分布引导平面坐标点采样与车速采样, 从而提高搜索效率与路径合理性; 其次, 改进RRT* 在节点扩展中考虑车辆非完整性约束, 并采用多步节点扩展的方式抑制控制量波动、减少计算量; 再次, 提出基于航向相似性的路径简化算法对初始路径剪枝, 相比基于贪心算法的路径简化, 提高驾驶舒适性; 最后, 采用三维空间贝塞尔曲线拟合路径, 基于改进RRT* 算法生成的简化路径计算贝塞尔曲线控制点优化问题的初始解, 并优化控制点坐标使行驶轨迹满足曲率、航向、加减速、起止状态约束, 相比直接采用路径点为控制点的B样条曲线拟合以及通过预瞄控制闭环跟踪路径的CL-RRT方法, 提升了平顺性与安全性, 且准确的初始解降低了优化迭代计算量. 算法流程如图1所示. 本文第1节和第2节分别介绍了改进的RRT* 算法与行驶轨迹曲线控制点优化, 第3节为算法的仿真实验验证, 第4节为分析与总结.

图 1  算法结构

图 2  候选节点

图 3  多步节点扩展策略

本文提出了一种改进的RRT* 与贝塞尔曲线控制点优化结合的车辆运动规划方法, 能够在动态障碍物环境中生成兼具安全性与驾驶舒适性的可行驶路径. 在改进的RRT* 方法中, 建立用于引导搜索的采样概率分布, 对纵横向位移、车速三个维度进行概率采样; 改进了节点扩展方式, 在扩展时考虑方向相似性、加减速、能耗等, 并采用多步扩展以加速搜索; 提出了基于航向相似性的路径简化方法; 最后根据搜索的初始轨迹拟合空间贝塞尔曲线, 并通过优化曲线控制点使行驶轨迹满足各类约束的同时, 提高平顺性与安全性.

由于RRT存在随机性, 因此无法保证相同场景下每次规划的轨迹均相同, 希望通过进一步的研究, 在实时计算中使规划结果稳定并趋近于全局最优解.

作者简介

袁静妮

上海交通大学机械与动力工程学院博士研究生. 主要研究方向为混合动力汽车预测性能量管理策略与无人驾驶车辆运动规划.E-mail: lxrbhzj@outlook.com

杨林

上海交通大学教授. 主要研究方向为混合动力、纯电动、燃料电池等新能源汽车动力系统及控制, 电动汽车动力电池集成与管理系统, 智能网联汽车节能与安全控制、智能驾驶. 本文通信作者.E-mail: yanglin@sjtu.edu.cn

唐晓峰

工学博士, 上海交通大学机械与动力工程学院博士后. 主要研究方向为智能车辆, 混合增强智能, 智能空间. E-mail: 407699568@163.com

陈傲文

上海交通大学机械与动力工程学院硕士研究生. 主要研究方向为混合动力能量管理, 整车控制, 无人驾驶车辆的运动规划和控制.E-mail: aowenchen@sjtu.edu.cn