2.793

2018影响因子

(CJCR)

  • 中文核心
  • EI
  • 中国科技核心
  • Scopus
  • CSCD
  • 英国科学文摘

留言板

尊敬的读者、作者、审稿人, 关于本刊的投稿、审稿、编辑和出版的任何问题, 您可以本页添加留言。我们将尽快给您答复。谢谢您的支持!

姓名
邮箱
手机号码
标题
留言内容
验证码

基于改进RRT*与行驶轨迹优化的智能汽车运动规划

袁静妮 杨林 唐晓峰 陈傲文

袁静妮, 杨林, 唐晓峰, 陈傲文. 基于改进RRT*与行驶轨迹优化的智能汽车运动规划. 自动化学报, 2020, 46(x): 1−10. doi: 10.16383/j.aas.c190607
引用本文: 袁静妮, 杨林, 唐晓峰, 陈傲文. 基于改进RRT*与行驶轨迹优化的智能汽车运动规划. 自动化学报, 2020, 46(x): 1−10. 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, 2020, 46(x): 1−10. doi: 10.16383/j.aas.c190607
Citation: 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, 2020, 46(x): 1−10. doi: 10.16383/j.aas.c190607

基于改进RRT*与行驶轨迹优化的智能汽车运动规划


DOI: 10.16383/j.aas.c190607
详细信息
    作者简介:

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

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

    工学博士. 主要研究方向为智能车辆, 混合增强智能, 智能空间. E-mail: 407699568@163.com

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

  • 基金项目:  国家自然科学基金(51875339)资助

Autonomous Vehicle Motion Planning based on Improved RRT* Algorithm and Trajectory Optimization

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

    Fig.  1  Algorithm structure

    图  3  多步结点扩展策略

    Fig.  3  Multi-step node extension strategy

    图  2  候选结点

    Fig.  2  Candidate node selection

    图  4  RRT*方法与改进RRT*的比较

    Fig.  4  Comparison of basic RRT* and improved RRT*

    图  5  轨迹简化与曲线拟合结果

    Fig.  5  Path simplification and curve fitting result

    图  6  贝塞尔曲线控制点优化结果

    Fig.  6  Bezier curve optimization result

    图  7  曲线平滑方法比较

    Fig.  7  Comparison of curve smooth methods

  • [1] 胡云峰, 曲婷, 刘俊, 施竹清, 朱冰, 曹东璞. 智能汽车人机协同控制的研究现状与展望. 自动化学报, 2019, 45(7): 1261−1280

    Hu Yun-Feng, Qu Ting, Liu Jun, Shi Zhu-Qing, Zhu Bing, Cao Dong-Pu. Human-machine cooperative control of intelligent vehicle: recent developments and future perspectives. Acta Automatica Sinica, 2019, 45(7): 1261−1280
    [2] Yong SZ, Yershov D, Frazzoli E, Paden B, Michal C. A survey of motion planning and control techniques for self-driving urban vehicles. IEEE Transactions on Intelligent Vehicles, 2016, 1(1): 33−55 doi:  10.1109/TIV.2016.2578706
    [3] 李柏, 张友民, 邵之江. 自动驾驶车辆运动规划方法综述. 控制与信息技术, 2018, 456(6): 7−12

    Li Bai, Zhang You-Min, Shao Zhi-Jiang. Motion planning methodologies for automated vehicles: a critical review. Control and Information Technology, 2018, 456(6): 7−12
    [4] Kawabata K, Ma L, Xue J, Zhu C, Zheng N. A path generation for automated vehicle based on Bezier curve and via-points. Robotics and Autonomous Systems, 2015, 74(A): 243−252
    [5] Li X, Sun Z, Cao D, Liu D, He H. Development of a new integrated local trajectory planning and tracking control framework for autonomous ground vehicles. Mechanical Systems and Signal Processing, 2017, 87(B): 118−137
    [6] Oliveira R, Cirillo M, Jonas M. Combining lattice-based planning and path optimization in autonomous heavy duty vehicle applications. In: 2018 IEEE Intelligent Vehicles Symposium. Changshu, China: IEEE, 2018. 2090-2097
    [7] Bounini F, Gingras D, Pollart H, Gruyer D. Modified artificial potential field method for online path planning applications. In: 2017 IEEE Intelligent Vehicles Symposium. Los Angeles, CA, USA: IEEE, 2017. 180-185
    [8] Ma L, Xue J, Kawabata K. Efficient sampling-based motion planning for on-road autonomous driving. IEEE Transactions on Intelligent Transportation Systems, 2015, 16(4): 1961−1976 doi:  10.1109/TITS.2015.2389215
    [9] 韩月起, 张凯, 宾洋, 秦闯, 徐云霄, 李小川. 基于凸近似的避障原理及无人驾驶车辆路径规划模型预测算法. 自动化学报, 2018

    Han Yue-Qi, Zhang Kai, Bin Yang, Qin Chuang, Xu Yun-Xiao, Li Xiao-Chuan. Convex approximation based avoidance theory and path planning MPC for driver-less vehicles. Acta Automatica Sinica, 2018
    [10] 吴伟, 刘洋, 刘威, 吴国弘, 马万经. 自动驾驶环境下交叉口车辆路径规划与最优控制模型. 自动化学报, 2019

    Wu Wei, Liu Yang, Liu Wei, Wu Guo-Hong, Ma Wan-Jing. A novel autonomous vehicle trajectory planning and control model for connected-and-autonomous intersections. Acta Automatica Sinica, 2019
    [11] Kavraki L E, Svestka P, Latombe J C, Overmars M H. Probabilistic roadmaps for path planning in high-dimensional configuration spaces. IEEE Transactions on Robotics and Automation, 2002, 12(4): 566−580
    [12] Janson L, Schmerling E, Clark A, Pavone M. Fast marching tree?: a fast marching sampling-based method for optimal motion planning in many dimensions. The International Journal of Robotics Research, 2015, 34(7): 883−921 doi:  10.1177/0278364915577958
    [13] LaValle S M. Rapidly-exploring random trees: a new tool for path planning. 1998
    [14] Karaman S, Frazzoli E. Sampling-based algorithms for optimal motion planning. The International Journal of Robotics Research, 2010
    [15] 宋晓琳, 周南, 黄正瑜, 曹昊天. 改进RRT在汽车避障局部路径规划中的应用. 湖南大学学报(自然科学版), 2017, 44: 30−37

    Song Xiao-Lin, Zhou Nan, Huang Zheng-Yu, Cao Hao-Tian. An improved RRT algorithm of local path planning for vehicle collision avoidance. Journal of Hunan University (Natural Sciences), 2017, 44: 30−37
    [16] 贺伊琳, 高奇, 赵丹, 刘伟. 基于改进RRT算法的无人驾驶汽车轨迹规划. 西北大学学报(自然科学版), 2018, 48: 651−658

    He Yi-Lin, Gao Qi, Zhao Dan, Liu Wei. The trajectory planning of autonomous vehicle based on improved RRT algorithm. Journal of Northwest University (Natural Science Edition), 2018, 48: 651−658
    [17] Yoon S, Lee D, Jung J, Hyunchul D. Fast marching tree: a fast marching sampling-based method for optimal motion planning in many dimensions. Journal of Intelligent & Robotic Systems, 2015, 34(7): 883−921
    [18] Yoshiaki K, Justin T, Sertac K, Gaston F, Emilio F, Jonathan P H. Motion planning in complex environments using closed-loop prediction. In: 2008 AIAA Guidance, Navigation and Control Conference and Exhibit. Honolulu, Hawaii, USA: AIAA
    [19] Gong H, Yin C, Zhang F, Hou Z, Member S, Zhang R. A path planning algorithm for unmanned vehicles based on target-oriented rapidly-exploring random tree. In: 2017 11th Asian Control Conference. Gold Coast Convention Centre, Australia: IEEE
    [20] 陈慧岩, 陈舒平, 龚建伟. 智能汽车横向控制方法研究综述. 兵工学报, 2017, 38(6): 1203−1214 doi:  10.3969/j.issn.1000-1093.2017.06.021

    Chen Hui-Yan, Chen Shu-Ping, Gong Jian-Wei. A review on the research of lateral control for intelligent vehicles. Acta Armamentarii, 2017, 38(6): 1203−1214 doi:  10.3969/j.issn.1000-1093.2017.06.021
    [21] 李耀宇, 朱一凡, 李群. 基于Legendre伪谱法的UGV避障路径规划. 指挥控制与仿真, 2012, 34(4): 124−127

    Li Yao-Yu, Zhu Yi-Fan, Li Qun. Legendre pseudospectral path planning method for UGV obstacle avoidance. Command Control & Simulation, 2012, 34(4): 124−127
    [22] 王炳琪, 杨明, 王春香, 王冰. 一种基于最优状态点的无人车路径跟踪横向控制方法. 自动化学报, 2019, 45(10): 1883−1892

    Wang Bing-Qi, Yang Ming, Wang Chun-Xiang, Wang Bing. Path tracking lateral control of self-driving vehicles based on the optimal state point. Acta Automatica Sinica, 2019, 45(10): 1883−1892
  • [1] 恽鹏, 吴盘龙, 李星秀, 何山. 变分贝叶斯概率数据关联算法[J]. 自动化学报, doi: 10.16383/j.aas.c200407
    [2] 周宏宇, 王小刚, 单永志, 赵亚丽, 崔乃刚. 基于改进粒子群算法的飞行器协同轨迹规划[J]. 自动化学报, doi: 10.16383/j.aas.c190865
    [3] 孙燕, 张弛, 路兴龙, 王靖戈, 付俊. 具有不等式路径约束的微分代数方程系统的动态优化[J]. 自动化学报, doi: 10.16383/j.aas.c180302
    [4] 王霞, 王耀民, 施心陵, 高莲, 李鹏. 噪声环境下基于蒲丰距离的依概率多峰优化算法[J]. 自动化学报, doi: 10.16383/j.aas.c190474
    [5] 汤鹏杰, 王瀚漓, 许恺晟. LSTM逐层多目标优化及多层概率融合的图像描述[J]. 自动化学报, doi: 10.16383/j.aas.2017.c160733
    [6] 卜新苹, 苏虎, 邹伟, 王鹏, 周海. 基于非均匀环境建模与三阶Bezier曲线的平滑路径规划[J]. 自动化学报, doi: 10.16383/j.aas.2017.c160262
    [7] 陈灵, 王森, 胡豁生, 麦当劳-麦尔·克劳斯, 费敏锐. 保证智能轮椅平滑通过狭窄通道的路径曲率优化算法[J]. 自动化学报, doi: 10.16383/j.aas.2016.c160185
    [8] 张高阳, 金鑫, 张之敬. 步进驱动系统大振荡机理与轨迹优化控制[J]. 自动化学报, doi: 10.16383/j.aas.2015.c140154
    [9] 陈成, 何玉庆, 卜春光, 韩建达. 基于四阶贝塞尔曲线的无人车可行轨迹规划[J]. 自动化学报, doi: 10.16383/j.aas.2015.c140295
    [10] 胡云卿, 刘兴高. 处理动态优化问题中控制变量路径约束的方法[J]. 自动化学报, doi: 10.3724/SP.J.1004.2013.00440
    [11] 宋晓霞, 石光明. 满足重构概率约束的更少贝努利观测[J]. 自动化学报, doi: 10.3724/SP.J.1004.2013.00053
    [12] 张勇, 巩敦卫, 张婉秋. 一种基于单纯形法的改进微粒群优化算法及其收敛性分析[J]. 自动化学报, doi: 10.3724/SP.J.1004.2009.00289
    [13] 冀俊忠, 张鸿勋, 胡仁兵, 刘椿年. 一种基于独立性测试和蚁群优化的贝叶斯网学习算法[J]. 自动化学报, doi: 10.3724/SP.J.1004.2009.00281
    [14] 孙达, 唐降龙, 刘家锋, 黄剑华. 基于概率密度的兴趣点检测算法[J]. 自动化学报, doi: 10.3724/SP.J.1004.2008.00854
    [15] 吕哲, 王福利, 常玉清, 刘阳. 一种改进的骨架曲线串行多边形近似算法[J]. 自动化学报, doi: 10.3724/SP.J.1004.2008.01467
    [16] 唐昊, 奚宏生, 韩江洪, 袁继彬. 具有不确定性路径概率的闭排队网络鲁棒控制策略[J]. 自动化学报
    [17] 王明辉, 游志胜, 赵荣椿, 张建州. 强干扰环境下性能优化的相互作用多模型-概率数据关联算法[J]. 自动化学报
    [18] 周昆, 潘志庚, 石教英. 一种新的基于顶点聚类的网格简化算法[J]. 自动化学报
    [19] 汪自勤, 宋文忠, 冯纯伯. 一种计算性能指标对路径概率灵敏性的新方法[J]. 自动化学报
    [20] 孙熊岳. 一种简化的模糊控制算法[J]. 自动化学报
  • 加载中
计量
  • 文章访问数:  4
  • HTML全文浏览量:  6
  • 被引次数: 0
出版历程
  • 收稿日期:  2019-08-27
  • 录用日期:  2019-11-16

基于改进RRT*与行驶轨迹优化的智能汽车运动规划

doi: 10.16383/j.aas.c190607
    基金项目:  国家自然科学基金(51875339)资助
    作者简介:

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

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

    工学博士. 主要研究方向为智能车辆, 混合增强智能, 智能空间. E-mail: 407699568@163.com

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

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

English Abstract

袁静妮, 杨林, 唐晓峰, 陈傲文. 基于改进RRT*与行驶轨迹优化的智能汽车运动规划. 自动化学报, 2020, 46(x): 1−10. doi: 10.16383/j.aas.c190607
引用本文: 袁静妮, 杨林, 唐晓峰, 陈傲文. 基于改进RRT*与行驶轨迹优化的智能汽车运动规划. 自动化学报, 2020, 46(x): 1−10. 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, 2020, 46(x): 1−10. doi: 10.16383/j.aas.c190607
Citation: 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, 2020, 46(x): 1−10. doi: 10.16383/j.aas.c190607
  • 为了提高出行效率和保障行车安全, 辅助驾驶与自动驾驶技术逐渐成为研究热点[1], 车辆运动规划是自动驾驶中的重要环节. 为提升规划路径的安全性、舒适性与计算效率, 研究者提出并改进了各类运动规划算法[2][3], 主要包括曲线规划、图搜索、人工势场法、采样方法与数值优化等.

    曲线规划常用于泊车或局部路径平滑[4], 文献[5]通过枚举轨迹曲线, 并根据成本函数择优, 但可行路径无法穷尽, 在复杂场景中不可用. 图搜索[6]生成的轨迹不满足车辆动力学、交通规则等各类约束, 且其计算效率限制了网格划分密度与搜索空间维数. 人工势场法[7]计算量小, 在实时避障和路径平滑方面广泛应用, 但不合理的势场构建会使行驶路径陷入局部最优或产生震荡. 采样方法[8]实时性好且能够在采样过程中考虑各类约束, 但存在随机性, 相同问题规划结果可能不同. 数值优化[9][10]可以综合考虑障碍物约束、车辆动力学模型、交通规则、路径曲率、能耗等各种评价指标, 可将最优控制问题转换为非线性规划求解, 但该方法对初始解要求较高且计算时间长; 也可将模型线性化后使用二次规划求解, 但线性化会降低求解精度.

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

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

    图  1  算法结构

    Figure 1.  Algorithm structure

    • 本文算法流程如图1与Algorithm 1所示. RRT*中的搜索树定义为 $T = \left\{ {{{{n}}_i} \vert i = 1,2,3...N } \right\}$ , 其中 ${{{n}}_i} = [x_i ,y_i ,t_i ,v_i ,c_i ,idx_i ]$ 为第 $i$ 个轨迹结点, 包含坐标 $[x_i ,y_i ]$ 、时间 $t_i$ 、车速 $v_i$ 、成本 $c_i$ 、父结点编号 $idx_i$ . $env$ 为环境信息, $f_{samp}$ 为采样概率分布, $s$ 为采样点, ${{{n}}_{e,j}}$ 为当前步第 $j$ 个扩展结点, $a_e$ $\delta$ 分别为第一步扩展时计算的加速度和转向角, ${{path}}$ ${{path}}_{{s}}$ 分别为简化前后的RRT*规划路径, $ctrlpt$ 为以贝塞尔曲线表示的行驶轨迹控制点.

      Algorithm 1 改进RRT*与行驶轨迹优化

      $f_{samp}$ =CalSamplingProbability(env)

      while not NearGoal( $T)$ do

        $s$ = Sample(env, $f_{samp})$

        ${{{n}}_{i }}$ = FindBestNeighbor( $T, s)$

       ( ${{{n}}_{e,0}}, a_{e}$ , $\delta$ , T) = Extend( $T,{{{n}}_{i}}, s)$

        while not collision do

         $T$ =Rewire( $T, {{{n}}_{e,j}})$

        ( ${{{n}}_{e,j+1}}, T)$ =Extend( $T, {{{n}}_{e,j}}, a_{e}$ , $\delta$ )

         $j =j$ +1

        end while

      end while

      path = FindPath( $T)$

      ${{path}}_{s }$ = PathSimplification(path)

      ctrlpt = InitCurve( ${{path}}_{s})$

      (ctrlpt,traj) = Optimize(ctrlpt, ${{path}}_{s}$ , env)

    • 常规RRT与RRT*在整个搜索空间内采样[14], 采样不受约束, 导致搜索的路径无法满足交通规则约束, 如反复变道或压线行驶. 交通规则通常包括但不限于: 与周边车辆保持纵横向安全车距, 靠近车道中心线行驶, 减小横向偏移量及其波动, 避免反复变道, 避免压线行驶, 超车完成后尽快回到目标车道, 车速不超过道路限速, 避免频繁加减速影响后车行驶, 与交通信号灯相关的规则等. 本文采用平面坐标点概率采样与车速概率采样的方式引导RRT的采样与扩展, 图3中渐变背景为采样概率分布的示意图, 在概率分布中考虑交通规则与横向偏移约束等, 优先采样满足交通规则的点. 对于安全车距约束, 由于障碍物数量较多导致概率分布函数计算显著增加, 且在碰撞检测、轨迹优化中均考虑过避障, 因此在概率采样中不再添加障碍物距离函数引导采样; 对于减小横向偏移、避免频繁变道或压线相关的规则, 采样概率分布 $f_{samp} (x,y)$ 定义如式(1)-(4), 从而引导采样点靠近目标车道的中心线, 在路口等没有车道中心线的场景中可以参考轨迹坐标点串代替式(4); 对于车速相关的规则, 通过结点扩展时的车速概率采样使车速逐渐趋近于目标车速, 如1.2节中式(11)(12)所示; 与交通信号相关的规则一般设置为约束条件, 不在采样概率分布中考虑.

      图  3  多步结点扩展策略

      Figure 3.  Multi-step node extension strategy

      $$ f_{samp} (x,y) = \frac{J_r (x,y)}{\int\limits_y {\int\limits_x {J_r (x,y)dxdy} } } $$ (1)
      $$ J_r (x,y) = 1/(d_{lc} (x,y)/w_r )^{c_r } $$ (2)
      $$ d_{lc} (x,y) = y-y_l (x) $$ (3)
      $$ y_l (x) = c_{l0} +c_{l1} x+c_{l2} x^2+c_{l3} x^3 $$ (4)

      其中 $J_r (x,y)$ 为道路势场, $d_{lc} (x,y)$ 为点 $(x,y)$ 与车道中心线的距离, 采用当前点y坐标与车道中心线y坐标 $y_l (x)$ 之差近似计算, $w_r$ 为路宽, $c_r$ 为道路势场衰减系数, $c_{l0}$ $c_{l1}$ $c_{l2}$ $c_{l3}$ 分别为车道中心线在 $x{ = }0$ 处的横向偏移、航向、曲率及曲率变化率.

      根据上面定义的采样概率分布, 采样得到随机采样点 $s = (x_s ,y_s )$ . 该方法基于先验知识缩小搜索空间, 由于偏离车道中心线较远的采样点通常会导致扩展分支方向与车道航向接近垂直, 而采用上述概率采样方法时, 这些点的采样概率接近于0, 因此可减少规划器对于无效点的采样, 从而在更少的采样次数内搜索到可行解. 另外, 可基于驾驶员的驾驶行为或离线规划的最优路径建立各场景下的高斯过程模型, 用于设计采样概率函数.

    • 常规的RRT算法在结点扩展时, 选取与采样点 $s$ 距离最近的候选结点作为扩展结点, 未考虑转向、加减速约束. 本文将候选结点筛选改为寻找一定距离范围内满足转向、加减速约束的点, 并使用当前路径分支与扩展分支的航向间的夹角(即近似的转向角)、分支长度来判断是否满足约束, 如图2所示. 转向角约束阈值则是根据父结点的车速、加速度设定.

      图  2  候选结点

      Figure 2.  Candidate node selection

      使用综合距离 $d_c$ (式(5)-(10))衡量采样点 $s$ 与树结点 ${{{n}}_i} = [x_i ,y_i ]$ 之间的距离, ${{{n}}_i}$ 的父结点为 ${{{n}}_j} = [x_j ,y_j ]$ ,

      $$ d_c = d_{hs} +d_v +d_{ed} $$ (5)
      $$ d_{hs} = 1-\frac{{{{h}}_1} \cdot {{{h}}_2} }{\left| {{{{h}}_1} } \right|\left| {{{{h}}_2} } \right|} $$ (6)
      $$ {{{h}}_1} = [x_s -x_i ,y_s -y_i ] $$ (7)
      $$ {{{h}}_2} = [x_i -x_j ,y_i -y_j ] $$ (8)
      $$ d_v = (v_i -v_{op} )_{norm} $$ (9)
      $$ d_{ed} = \sqrt {(x_s -x_i )^2+(y_s -y_i )^2} $$ (10)

      其中 $d_{hs}$ 用于衡量待扩展分支相对于当前分支的航向变化量, ${{{h}}_1} \cdot {{{h}}_2} /\left| {{{{h}}_1} } \right|\left| {{{{h}}_2} } \right|$ 为航向相似度, ${{{h}}_1}$ 为待扩展分支航向向量, ${{{h}}_2}$ 为当前分支航向向量; $d_v$ 为结点车速 $v_i$ 与理想车速 $v_{op}$ 的偏差, 除以 $v_{norm}$ 以归一化, $v_{norm}$ 可取为自车在一段时间内的平均车速与 $v_{op}$ 中的较大值; $d_{ed}$ 为结点与采样点间的欧氏距离.

      根据上述方法, 计算出周边结点的综合距离值, 并在现有树结点中选出满足 $\left| {d_c } \right|<d_{thre}$ 的结点, 作为候选点集 $N_e = \{i\vert d_c (i)<d_{thre} \}$ , 用于后续的结点扩展. 对候选结点的筛选使扩展分支满足车辆运动约束, 减少后续优化计算量.

      为使规划车速趋近理想车速, 采用变长度扩展方法, 扩展长度 $l_e$ 正比于车速, 表示 $t_s$ 时间内车辆行驶的距离, 本文设定 $t_s$ 为1 s, 即 $l_e$ 大小等于车速. 扩展长度 $l_e$ 通过采样确定, 采样范围受理想车速、当前车速、加减速的约束, 有一定概率使车速趋近理想车速 $v_{op}$ , 其他情况随机采样. 对于第 $i$ 个结点, $l_e$ 计算如式(11)-(13),

      $$ \begin{array}{*{20}{l}} {r = u(0,1)}\\ {if \;{\rm{ }}r > 0.5{\rm{ }}}\\ {{\rm{ }}{a_e} = \max (\min (0.2({v_{op}} - {v_i}),{a_{max}}),{a_{min}})}\\ {else}\\ {{\rm{ }}{a_e} = u({a_{min}},{a_{max}})} \end{array} $$ (11)
      $$ v_e = v_i +a_e \cdot t_s $$ (12)
      $$ l_e = v_e \cdot t_s $$ (13)

      其中 $r$ 为(0,1)间均匀分布随机数, $v_i$ 为编号为 $i$ 的结点的车速, $[a_{min} ,a{ }_{max}]$ 为车辆加速度约束范围. 根据采样的加速度 $a_e$ 计算扩展点的车速 $v_e$ 与扩展长度 $l_e$ .

      为使路径趋近最优, RRT*采用重连接删除成本较高的分支, 本节中成本函数除路径长度外, 还加入能耗等指标. 扩展分支的成本函数 $c_l ({{{n}}_i} ,{{{n}}_e} )$ 定义如下,

      $$ c_l ({{{n}}_i} ,{{{n}}_e} ) = d_{hs} +d_{ed} +\left| {v_e -v_{op} } \right|_{norm} +f_e $$ (14)
      $$ \begin{array}{l} f_e = (ma_e +mg\mu _r +0.5C_d A\rho _{air} v_e ^2)\cdot v_e t_s ,\; \\ a_e \ge 0 \\ f_e = (r_{brk} ma_e +mg\mu _r +0.5C_d A\rho _{air} v_e ^2)\cdot v_e t_s ,\; \\ a_e <0 \end{array} $$ (15)

      其中 ${{{n}}_e}$ 为扩展结点, $d_{hs}$ $d_{ed}$ 定义如式(6)(10), $\left| {v_e -v_{op} } \right|_{op}$ 用于衡量扩展结点车速与理想车速的偏差, $f_e$ 为当前扩展分支的能耗, $r_{brk}$ $m$ $\mu _r$ $C_d$ $A$ $\rho _{air}$ 分别为制动回馈比例、整车质量、滚动阻力系数、风阻系数、迎风面积、空气密度. 通过对能耗、安全性等多项指标的优化以及结点扩展中对转向、加减速的约束, 使搜索的初始路径经过曲线平滑后可行驶且在重连接过程中渐近最优, 从而减少后续优化计算量.

      式(16)用于计算扩展结点总成本, 即在候选点集 $N_e = \{i\vert d_c (i)<d_{thre} \}$ 中, 选出使树结点成本 $c_i$ 加分支成本 $c_l ({{{n}}_i} ,{{{n}}_e} )$ 最低的结点 ${{{n}}_i}$ 用于扩展.

      $$ \mathop {\min }\limits_{i\in N_e } \;c_i +c_l ({{{n}}_i} ,{{{n}}_e} ) $$ (16)

      第一步扩展策略如式(17)-(19), 计算扩展方向向量 $h$ , 从 ${{{n}}_i}$ 沿 ${{{h}}}$ 方向扩展 $l_e$ 得到初始扩展点 $[x_{e,0} ,y_{e,0} ]$ .

      $$ {{{h}}} = \frac{[x_s -x_i ,y_s -y_i ]}{\sqrt {(x_s -x_i )^2+(y_s -y_i )^2} } $$ (17)
      $$ [x_{e,0} ,y_{e,0} ] = [x_i ,y_i ]+{{{h}}}\cdot l_e $$ (18)
      $$ v_{e,0} = l_e /t_s $$ (19)

      车辆运动控制中需避免控制量频繁变化, 在本问题中即为期望在一段时间内保持平稳的加速度和转向角, 可借鉴模型预测控制的思路, 在扩展时沿用第一步控制量, 连续扩展多次直至遇到障碍物或偏离目标方向或达到预定的扩展次数, 如图3所示, 即可减少控制量波动, 同时因为在后续结点扩展时无需重复采样与候选结点计算的步骤, 在搜索结束时扩展结点数量相同的前提下减少了计算量. 具体步骤如下, 求解出第一次扩展的加速度 $a_e$ 和转向角 $\delta$ , 使用旋转变换更新方向向量 ${{{h}}}$ (式(20)), 然后继续以同样的转向角和加速度行驶(式(21)-(25)), 依次得到 $N_p$ 个扩展分支和扩展点,

      $$ {{h}} = \left[ {\begin{array}{*{20}{c}} {\cos \delta }&{ - \sin \delta }\\ {\sin \delta }&{\cos \delta } \end{array}} \right]{{h}} $$ (20)
      $$ v_{e,k} = v_{e,k-1} +a_e t_s $$ (21)
      $$ l_e = v_{e,k} t_s $$ (22)
      $$ t_{e,k} = t_{e,k-1} +t_s $$ (23)
      $$ [x_{e,k} ,y_{e,k} ] = [x_{e,k-1} ,y_{e,k-1} ]+{{{h}}}\cdot l_e $$ (24)
      $$ c_{e,k} = c_{e,k-1} +c_l ({{{n}}_{e,k-1}} ,{{{n}}_{e,k}} ) $$ (25)

      其中 $v_{e,k}$ $t_{e,k}$ $x_{e,k}$ $y_{e,k}$ $c_{e,k}$ 分别为第 $k$ 个扩展点 ${{{n}}_{e,k}}$ 的车速、时间、 $x$ 坐标、 $y$ 坐标、成本.

      重连接过程与扩展相似, 对新结点附近每一个结点, 判断将该结点的父结点换为新扩展的结点是否会降低成本, 若能够降低, 则对该点进行重连接, 将其父结点替换为新扩展的结点.

    • 当搜索到若干个距离终点较近的结点, 可停止搜索, 选择终点附近成本最低的结点, 并反向回溯得到一条初始路径, 但路径可能存在冗余的转向等, 因此需要对路径进行简化, 常用方法为贪心算法[19], 但该方法简化的路径夹角不受约束, 可能会导致急转弯, 因此本节提出一种新的路径简化方法, 简化中考虑多种指标, 这些指标包括: 与车道中心线航向相似度、与前一分支的航向相似度、是否碰撞以及与障碍物的距离、简化后行驶距离减少的百分比. 通过对简化后的分支进行综合评价以筛选待简化的路径分支. 初始路径定义为 ${{path}} = [{{{n}}_1} ,{{{n}}_2} ,{{{n}}_3} ,... {{{n}}_N} ]$ , $N$ 为简化前路径结点数, 对于 $1<i<N$ 的点 ${{{n}}_i}$ , 使用 $J_i$ (式(26)-(29))衡量是否需要删除结点 $n_i$ 并直接连接点 ${{{n}}_{i-1}}$ 与点 ${{{n}}_{i+1}}$ ,

      $$ \begin{split} J_i=\; &\frac{{{{h}}_l} \cdot {{{h}}_i} }{\left| {{{{h}}_l} } \right|\cdot \left| {{{{h}}_i} } \right|}+\frac{{{{h}}_i} \cdot {{{h}}_{i,pre}} }{\left| {{{{h}}_i} } \right|\cdot \left| {{{{h}}_{i,pre}} } \right|}+d_{obs} \\ & +\frac{l_{i,i+1} +l_{i+1,i+2} -l_{i,i+2} }{l_{i,i+1} +l_{i+1,i+2} } \end{split} $$ (26)
      $$ {{{h}}_l} = [ 1, c_{l1} +2c_{l2} x_i +3c_{l3} x_i ^2 ] $$ (27)
      $$ {{{h}}_i} = [ x_{i+1} -x_{i-1} , y_{i+1} -y_{i-1} ] $$ (28)
      $$ {{{h}}_{i,pre}} = [ x_{i-1} -x_{i-2} , y_{i-1} -y_{i-2} ] $$ (29)

      其中 $J_i$ 的第一项表示车道中心线航向与简化后路径分支航向的相似度, 第二项表示简化后路径分支与前一分支的航向相似度, 第三项 $d_{obs}$ 表示简化后路径分支与障碍物的最小距离, 第四项表示简化后路径分支距离减少比例. ${{{h}}_l}$ 为点 ${{{n}}_i}$ 处的车道中心线航向向量, ${{{h}}_i}$ 为分支 ${{{n}}_{i-1}} {{{n}}_{i+1}}$ 航向向量, ${{{h}}_{i,pre}}$ 为分支 ${{{n}}_{i-2}}{ {{n}}_{i-1}}$ 航向向量, $l_{i,j}$ 为分支 ${{{n}}_i}{ {{n}}_j}$ 的长度.

      依据上式计算得到数组 $[J_2 ,J_3 ,...,J_{N-1} ]$ , 将 $J_i >J_{thre}$ 的点 ${{{n}}_i}$ 加入待删除结点集合, 其中 $J_{thre}$ 可以取为数组均值, 当相邻两点均满足该条件且同时删除会导致碰撞时, 仅删除 $J$ 值较大的点, 从而避免过度简化. 简化后路径结点序列为 ${{path}}_s = [{{{n}}_{s1}} ,{{{n}}_{s2}} ,{{{n}}_{s3}} ,...{{{n}}_{sN}} ]$ .

      为减少计算时间, 并减小连续两次规划结果的差异, 当车辆实际行驶轨迹与前一次规划轨迹接近且障碍物运动趋势不变时, 优先沿用前一次规划的路径, 并规划从前一次路径终点到当前规划终点的可行路径, 将这两部分路径连接后, 重复路径简化与控制点优化步骤, 得到下一时刻的规划轨迹.

    • 由于RRT*规划的路径不符合车辆动力学约束, 无法直接用于控制, 得到简化路径后需使用曲线拟合生成一条曲率连续的行驶轨迹. 使用最小二乘拟合多项式曲线耗时短, 但该方法求得的曲线不能满足曲率、航向、起点终点车速、加速度等各类约束, 同样无法用于控制. 若直接将路径结点作为贝塞尔曲线或B样条曲线的控制点[15]用于平滑行驶轨迹, 同样存在无法满足约束的问题. 使用CL-RRT方法生成的轨迹是可行驶的, 满足车辆动力学约束, 但并未对通行效率、舒适度、能耗等指标进行优化. 由于贝塞尔曲线的起点和终点即为行驶轨迹的起止点, 控制点的物理意义明确, 可以直接将部分约束转换为控制点的约束以减少计算量, 相比各类样条曲线优化变量个数更少, 因此使用优化贝塞尔曲线控制点的方法来求解规划路径, 将轨迹优化、拟合问题转换为控制点参数优化的问题, 其本质与优化多项式系数相同, 但更直观.

    • 高自由度车辆动力学模型无法应用于实时计算, 且侧向加速度较小时, 简化的二自由度车辆运动学模型的精度足够用于车辆控制[20]. 以车辆后轴中心为参考点建立二自由度车辆运动模型如式(30),

      $$ \begin{split} & \dot {x}(t) = v(t)\cos \theta (t) \\ & \dot {y}(t) = v(t)\sin \theta (t) \\ & \dot {\theta }(t) = v(t)\tan \delta (t)/L\end{split} $$ (30)

      其中 $x(t)$ $y(t)$ $t$ 时刻车辆后轴中心的 $x$ $y$ 坐标, $L$ 为车辆前后轴距, $\delta(t)$ 为转向角, $\theta (t)$ 为航向角.

      $n$ 阶贝塞尔曲线[4]定义如式(31),

      $$ \begin{split} B(\tau )\; &=\sum\limits_{i = 0}^n {P_i B_{i,n} (\tau )} \\ &=\sum\limits_{i = 0}^n {P_i \frac{n!}{(n-i)!i!}(1-\tau )^{n-i}\tau ^i} ,\tau \in [0,1] \end{split} $$ (31)

      其中 $P_i$ 为第 $i$ 个曲线控制点, $\tau$ 为自变量, $B_{i,n} (\tau )$ 为伯恩斯坦多项式. 对曲线求微分可得如下的一阶微分和 $r$ 阶微分递推关系(式(32)(33)), 可用于计算车速、加速度等.

      $$ \begin{split} & \dot {B}(\tau ) = \sum\limits_{i = 0}^{n-1} {P_i^{(1)} B_{i,n-1} (\tau )} ,\;\tau \in [0,1] \\ & P_i^{(1)} = n\left({P_{i+1} -P_i } \right) \end{split} $$ (32)
      $$ \begin{split} & B^{(r)}(\tau ) = \sum\limits_{i = 0}^{n-r} {P_i^{(r)} B_{i,n-r} (\tau )} ,\tau \in [0,1] \\ & P_i^{(r)} =\frac{n!}{(n-r)!}\sum\limits_{j = 1}^r {\left({-1} \right)^{r-j}} \frac{r!}{j!(r-j)!}P_{i+j} \end{split} $$ (33)

      根据之前RRT*规划的路径, 终点 ${{{n}}_{sN}}$ 对应时间 $t_{sN}$ , 设置终点时间 $t_d = t_{sN}$ . 将车辆后轴中心行驶轨迹表示为贝塞尔曲线方程(式(34)(35)), 并设置贝塞尔曲线中的 $\tau = t/t_d$ . 根据行驶轨迹方程, 结合车辆运动学模型, 可求得车速 $v$ 、加速度 $a$ 、航向角 $\theta$ 、曲率 $\kappa$ , 如式(36)-(39).

      $$ x(t) = \sum\limits_{i = 0}^n {x_i \frac{n!}{(n-i)!i!}(1-\tau )^{n-i}\tau ^i} $$ (34)
      $$ y(t) = \sum\limits_{i = 0}^n {y_i \frac{n!}{(n-i)!i!}(1-\tau )^{n-i}\tau ^i} $$ (35)
      $$ v(t) = \sqrt {\dot {x}(t)^2+\dot {y}(t)^2} /t_d $$ (36)
      $$ a(t) = \frac{\dot {x}(t)\ddot {x}(t)+\dot {y}(t)\ddot {y}(t)}{t_d ^2\sqrt {\dot {x}(t)^2+\dot {y}(t)^2} } $$ (37)
      $$ \theta (t) = \arctan(\dot {y}(t)/\dot {x}(t)) $$ (38)
      $$ \kappa (t) = \frac{\left| {\dot {x}(t)\ddot {y}(t)-\ddot {x}(t)\dot {y}(t)} \right|}{\left({\dot {x}(t)^2+\dot {y}(t)^2} \right)^{3/2}} $$ (39)

      其中 $(x_i ,y_i )$ 为控制点.

    • 建立优化问题模型, 在RRT*规划路径基础上, 以曲线控制点为优化变量, 求解安全、平顺的行驶轨迹. 优化目标定义如式(40), 包括路径偏差、障碍物距离、车速波动、曲率、能耗等,

      $$ \begin{split} & J=\int_0^{t_d } {J_e (t)} dt \\ & J_e (t) = \omega _1 ((x(t)-x_{ref} (t))^2+(y(t)-y_{ref} (t))^2) \\ & \qquad +\omega _2 d_{obs} (t)^2+\omega _3 a(t)^2+\omega _4 \kappa (t)^2+\omega _5 f_e (t) \end{split} $$ (40)

      其中 $x_{ref} (t)$ $y_{ref} (t)$ 为根据 ${{path}}_s$ 插值计算的参考轨迹, $f_e (t)$ 为式(15)计算的能耗, $\omega _i ,i = 1,2...5$ 为权重.

      优化问题的约束包括: 起点终点约束、动力学约束、障碍物与交通规则约束, 分别定义如下.

      ①起点、终点约束

      起点终点约束包括对起始位置、起始车速、起始航向、终点位置、终点车速、终点航向的约束. 起点处的约束即为当前时刻车辆状态为定值; 终点处的约束视场景而定, 例如在换道场景中, 要求终点航向与目标车道中心线航向一致.

      采用车身坐标系, 车辆起点坐标即为[0, 0], 则起点位置约束如式(41), 起始车速约束如式(42). 在起始状态给定时, 贝塞尔曲线中的第1、2个控制点, 即 $[x_0 ,y_0 ]$ $[x_1 ,y_1]$ , 可直接计算, 无需优化. 终点位置约束取决于具体场景, 则此处可设置终点坐标为RRT*规划终点坐标, 如式(43)所示, 据此计算控制点 $[x_n ,y_n ]$ .

      $$ x(0) = x_0 = 0,y(0) = y_0 = 0 $$ (41)
      $$ \begin{split} & \dot {x}(0) = n(x_1 -x_0 ) = v_{x,init} \cdot t_d , \\ &\dot {y}(0) = n(y_1 -y_0 ) = v_{y,init} \cdot t_d \end{split} $$ (42)
      $$ x(t_d ) = x_n = x_d ,y(t_d ) = y_n = y_d $$ (43)

      ②动力学约束

      动力学约束包括对车速、加速度、曲率的约束, 设置如下,

      $$ v(t)<v_{max} $$ (44)
      $$ a_{min} <a(t)<a_{max} $$ (45)
      $$ \kappa (t)<\kappa _{max} $$ (46)

      其中 $v_{max}$ 为车辆最高车速或道路限速, $[a_{min} ,a_{max} ]$ 为车辆加速度范围, $\kappa _{max}$ 为路径最大曲率限值, 其中 $[a_{min} ,a_{max} ]$ $\kappa _{max}$ 均与车速相关.

      ③障碍物与交通规则约束

      采用p-norm函数[21]描述障碍物, 障碍物形状近似为矩形, 考虑障碍物航向的p-norm函数定义为式(47), 当 $h_{obs}$ 大于距离阈值 $d_{thre}$ 时, 认为车辆与障碍物之间不存在碰撞.

      $$ \begin{split} & h_{obs} = \left({\left| {\frac{2d_x }{l_{obs} }} \right|^p+\left| {\frac{2d_y }{w_{obs} }} \right|^p} \right)^{1/p}-1>d_{thre} \\ & d_x = \frac{\left| {\cot(\theta _{obs} )\cdot (x-x_{obs} )+y-y_{obs} } \right|}{\sqrt {\cot (\theta _{obs} )^2+1} } \\ & d_y = \frac{\left| {\tan(\theta _{obs} )\cdot (x-x_{obs} )-y+y_{obs} } \right|}{\sqrt {\tan(\theta _{obs} )^2+1} } \end{split} $$ (47)

      其中 $x_{obs}$ $y_{obs}$ $t$ 时刻障碍物中心坐标, $\theta _{obs}$ $t$ 时刻障碍物航向, $l_{obs}$ $w_{obs}$ 为障碍物长宽, $d_x$ $d_y$ 是车辆与障碍物中心在障碍物纵轴和横轴方向上的距离.

      道路边界约束包括道路边沿的硬约束和虚线车道间隔线的软约束, 道路边沿的约束可以转换为不同纵向距离处的横向偏移约束, 车道线的软约束在目标函数中考虑, 在目标函数中添加偏离目标车道中心线惩罚 $Pr$ ,

      $$ Pr = \int\limits_t {k_r \left({\frac{y(t)-y_l (x(t))}{w_r }} \right)^2} dt $$ (48)

      其中 $y_l$ $x(t)$ 处车道中心线横向坐标, $w_r$ 为车道宽度, $k_r$ 为常数, 用于调节车道中心线惩罚对规划路径的影响程度.

      优化的计算速度与初始解相关, 当初始解接近最优解时, 可加速收敛. 本节根据RRT*规划的简化路径, 预先计算一组合理的初始控制点. 首先根据起点、终点的位置和速度, 根据前面式(41)-(43)的起点终点条件可计算第1、2个控制点与最后一个控制点, 从RRT*规划的路径中, 等时间间隔插值取出 $n$ -2个路径点, 不含起点终点, 路径点包含时间、位置等信息, 将其代入 $n$ 阶贝塞尔曲线方程可以计算得到剩余的 $n$ -2个控制点, 以这组控制点作为优化问题初始解. 最后使用序列二次规划(SQP)方法求解上述优化问题以优化这组控制点, 使最终的行驶轨迹收敛到局部最优解.

    • 在Intel I7-7500U笔记本电脑上使用MATLAB R2018a对本文提出的算法进行仿真实验验证. 以弯道行驶场景为例, 随机设置环境中障碍物的位置、速度, 使用改进的RRT*算法与曲线控制点优化进行车辆运动规划.

      仿真案例设置为多车道弯道环境下的障碍物避障轨迹规划, 其中起始状态设置为 $x_0 = 0\;m$ , $y_0 = 0\;m$ , $\theta _0 = 0\;rad$ , $v_0 = 4\;m/s$ , 目标车速设置为 $v_{op} = 10\;m/s$ , 规划终点设置为 $x_e = 90\;m$ , $y_e = -2\;m$ .

      图4(a)(c)为常规RRT*方法(对照策略)搜索的路径与车速, 图4(b)(d)为本文提出的改进RRT*算法搜索的初始路径与车速. 图中细实线表示搜索结束时的RRT搜索树状态. 运动规划的计算耗时包括两部分: RRT搜索树采样、扩展与简化的计算耗时, 以及曲线优化的计算量. 首先, 对照策略的采样与扩展完全随机, 产生了很多无效扩展结点, 而本文中的采样概率分布是根据人类驾驶员的驾驶习惯、交通规则设置的, 采样概率低的区域存在最优路径的概率较低, 因此减少对于该区域的搜索可以减少最终所需的采样与扩展次数. 其次, 对照策略中对扩展方向不做任何约束, 而不合理的初始解会导致后续曲线优化过程的迭代次数增加. 另外, 即使假设不同方法搜索到最终路径时, RRT搜索树需要扩展出相同数量级的分支和结点, 采用多步扩展方法可以减少数次采样与候选点的计算, 而改进RRT*在扩展与重连接中的单步计算量与原RRT*算法相等, 因此整体计算量减少. 综上, 本文提出的方法能够提高搜索效率, 并减少搜索与优化的整体计算耗时. 统计多次搜索结果, 本文算法搜索次数相比常规RRT*降低约71.68%, 计算耗时降低49.28%. 同时常规RRT*中车速随机采样导致最终轨迹呈现频繁加减速, 且常规RRT*方法的成本函数中未考虑能耗, 因此无法在重连接的过程中逐渐优化车速轨迹使能耗降低, 在该仿真场景中对照策略的能耗相比本文提出的策略高14.9%, 且车速随机采样导致平均车速与车辆通行效率较低.

      图  4  RRT*方法与改进RRT*的比较

      Figure 4.  Comparison of basic RRT* and improved RRT*

      图5(b)(c)比较了基于贪心算法的路径简化策略与本文提出的路径简化算法, 细实线表示RRT*搜索树, 包含搜索过程中扩展的所有结点与分支, 粗实线为简化的路径, 粗虚线为贝塞尔曲线拟合的路径. 基于贪心算法的简化不考虑转向角约束以及车道行驶的交通规则, 仅根据距离最短与碰撞检测简化路径, 而本文算法综合考虑车道中心线的航向、转向角与路径长度, 因此图5(c)中改进的简化算法计算的路径(粗实线)相比图5(b)中贪心算法简化路径(粗实线)更平滑, 转向角小, 图5(c)(d)中的虚线为贝塞尔曲线拟合的行驶轨迹和车速, 对应的控制点分布如图6所示. 由于改进RRT*搜索路径时已在成本函数中考虑了能耗、安全性、行驶距离等多项指标, 因此初始路径与最优解较接近, 图6中初始化的控制点与优化后的控制点非常接近, 因此迭代次数少.

      图  5  轨迹简化与曲线拟合结果

      Figure 5.  Path simplification and curve fitting result

      图  6  贝塞尔曲线控制点优化结果

      Figure 6.  Bezier curve optimization result

      图7中将本文提出的曲线控制点优化方法与常用的CL-RRT、三次B样条曲线平滑方法进行了比较. 给定相同的初始路径, CL-RRT方法通过预瞄与反馈控制生成一条可行驶的轨迹; B样条曲线直接采用包含时间信息的初始路径插值点作为控制点, 拟合得到 $x(t)$ , $y(t)$ 轨迹; 本文算法根据初始路径预估初始控制点后进一步优化控制点分布. 仿真结果如图7所示, 图7(a) $x$ - $y$ 平面内的规划路径, 图7(b)(c)为根据规划的车速轨迹计算的加速度和冲击度, 图7(d)为路径曲率. 三种方法均满足曲率连续、起始车速、起点、终点位置的约束; B样条拟合直接以路径点为控制点, 未考虑起点状态约束, 因此在起点处易出现路径曲率大、冲击度高的情况, 导致控制平顺性差, 且由于拟合时不再进行碰撞检测, 曲线控制点间隔较大时易导致碰撞, 而控制点间隔较小时曲率较大; 本文方法在优化目标中考虑了舒适性与经济性, 因此优化的行驶轨迹加速度与曲率变化更平缓, 冲击度更小, 平顺性好; 且因在控制点优化时考虑了安全性, 因此本文方法生成的曲线优化了避障轨迹, 距离障碍物更远, 相比仅考虑跟随和拟合的CL-RRT和B样条曲线, 提高了安全性. 使用基于二自由度动力学模型的预测控制算法控制车辆的纵横向运动[22], 使车辆跟踪规划的车辆轨迹, 平均横向跟随误差为0.05m, 平均车速跟随误差为0.08m/s, 表明规划的轨迹是可行驶的, 符合车辆运动特性.

      图  7  曲线平滑方法比较

      Figure 7.  Comparison of curve smooth methods

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

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

WeChat 关注分享

返回顶部

目录

    /

    返回文章
    返回