论文阅读笔记-《无人驾驶车辆局部路径规划的时间一致性与鲁棒性研究》
[TOC]
本文为《无人驾驶车辆局部路径规划的时间一致性与鲁棒性研究》的阅读笔记.
学校: 北理
作者: 姜岩, 王琦等
关键词: 局部规划, 时间一致性(temporary consistency), 鲁棒性(robustness)
引言
无人车与机器人的区别:
- 不具备已知的环境地图, 环境信息主要来自于车载环境感知系统的实时感知结果. 而环境感知系统只能提供局部不完整的环境信息. 因此在车辆行驶过程中, 即使环境是静态的, 规划系统面对的局部环境信息也是在不断更新的;
个人感触: 这就是很多基于 A star 搜索的改进算法无法应用到无人车的原因.
- 环境中可能存在较多的动态障碍物, 易造成规划结果不可行;
- 以较高速度行驶的无人驾驶车辆不允许停车等待规划系统完成规划, 因此实时性要求更高.
需要满足的约束种类:
- 环境约束 (体现为安全性)
- 系统运动学约束 (体现为可行性)
- 系统动力学约束 (体现为平顺性和稳定性)
- 特定的优化指标约束 (如最短时间或最短距离)
因此主流的思路是: 一般在全局路径规划之外增加局部路径规划提高整个规划系统的实时性. 局部规划在跟踪全局期望路径的前提下具有一定的避障能力, 节省了全局重规划所需的运算时间.
局部路径规划方法主要分为 3 大类:
1. 将全局规划方法应用于局部规划.
例如 RRT, 启发式搜索 A star. 根据全局期望路径上的目标点计算局部路径. 在路径生成过程中集中体现避障、路径跟踪和运动学约束等要求.
难点:
- 如何在每个规划周期中确定全局期望路径上合适的目标位姿.
- 全局规划算法要求对行驶环境有着较为完整的感知结果.
2. 局部路径规划算法是基于模型预测原理 MPC
核心是求解多约束条件下的最优化问题(通过对车辆运动微分方程进行滚动优化完成路径生成).
难点:
- 对于初始位姿和目标位姿的选取较为敏感, 当目标位姿与初始位姿偏差较大时, 易得到无法预料的规划结果.
3. 局部路径规划方法由路径生成和路径选择两部分组成
路径生成功能并不考虑环境约束, 而是生成一组能够跟踪全局期望路径, 且满足系统动力学和运动学等约束的可行路径. 完成路径生成后, 通过对可行路径进行避障分析, 在安全的可行路径中通过路径选择获得待执行路径, 从而体现环境约束.
简单来说: 规定采样点后求解一族曲线, 然后根据约束/安全性进行最优选择.
路径是基于高阶参数化方程曲线的优化求解方式(根据目标状态和当前状态确定的边界条件, 以距离最短或者曲率最小为优化条件). 反对的方法是: 触须方法(Tentacle 方法或者Morphine 方法) 用固定前轮偏角对应的圆弧曲线生成可行路径. 该方法实时性强, 能够胜任低速下的机器人行驶任务. 然而圆弧曲线上并没有体现前轮偏角的变化, 当速度较快时易产生大的超调从而造成路径跟踪失败.
难点:
曲线生成方法不具有时间一致性, 不同规划周期生成的曲线无法保持一致. 时间不一致的结果: 如果规划曲线与车辆实际行驶路径不一致,则意味着局部规划的安全性分析无效.
为考虑误差带来的影响: 即使曲线生成满足时间一致性的要求, 无人驾驶车辆行驶中的过程误差也将造成车辆实际行驶路径偏离规划结果, 从而造成安全性分析失效. 过程误差主要来自于控制误差和定位误差. 现有规划方法一般通过过度膨胀障碍物方式体现过程误差.
本文的特点:
基于前向预测 (Forward prediction) 的曲线生成方法用于局部规划. 该方法能够在不依赖精确全局定位信息的情况下生成具有时间一致性的曲线. 事实上局部规划最重要的要求是在满足车辆安全可行的前提下跟踪全局期望路径, 而规划结果并不必须是最优的.
在曲线生成过程中加入误差的考虑, 实现具有鲁棒性的局部路径规划.
基于前向预测生成时间一致性的可行曲线
时间一致性
概念: 时间不一致指在 $t_1$ 时刻按照最优化原则制定了将在 $t_n$ 时刻执行的计划 $p$, 然而当时间到达 $t_n$时 $p$ 已经不再是最优的.
图解如下:
问题分析
时间不一致是基于优化求解的曲线生成方法普遍存在的问题. 原因是这类方法通过最优化计算求解连接车辆当前位置和目标位置的曲线. 如果不考虑路径跟踪控制的误差, 在每个周期进行规划时, 规划起点位于上一周期的规划结果上, 但是规划终点随车辆向前移动. 这种边界条件的变化将导致最优化计算结果的变化, 从而使得新规划结果逐渐偏离原始规划结果. 一种折中的方法是根据 Belleman 优化原理, 锁定目标位置不变保证了前后曲线一致. 当车辆接近目标位置时, 周期性的切换到下一个目标位置重新生成路径. 然而为了维护目标状态的全局位置不变, 必须使用精确的车辆全局定位信息, 这将导致无人驾驶系统对高精度定位系统的依赖.
基于前向预测实现具有时间一致性的曲线生成
前向预测 (Forward prediction) 根据控制量序列对车辆未来的状态进行前馈估计. 而车辆的未来状态实际上等同于局部路径规划结果.
具体算法过程解释如下:
基于前向估计的曲线生成算法是一种迭代算法. $x_t$ 表示 $t$ 时刻车辆的位置. $P_{r_t}$ 表示全局期望路径相对于 $x_t$ 的坐标. 在每一步迭代中, 首先根据车辆的实时位置计算车辆坐标系下的全局期望路径 $P_{r_i}$ (行 3)), 根据控制算法生成控制量 $U_t$ (行 4)), 以及控制量和车辆模型对车辆新的实时位置 $x_{t+1}$ 进行估计 (行 5)), 将 $x_{t+1}$ 放入可行路径的末端. 如果可行路径的长度超过预设阈值则路径生成完成, 否则将在 $x_{t+1}$ 继续下一次迭代.
优点:
- 整个算法流程不需要任何的全局定位信息. 更准确地说, 当环境感知系统能够直接输出车辆局部坐标系下的全局期望路径时 (比如车道线检测系统输出的车道线可直接用作车身坐标系下的期望路径), 整个算法流程不需要任何的全局定位信息.
- 根据车辆运动模型增量式生成期望路径, 体现了车辆运动学和动力学约束, 保证了路径的可行性.
- 保证了时间一致性.
控制算法的选择
为了解决全局期望路径突变造成控制量突变的问题, 控制算法需要考虑实时前轮偏角的影响. 本文中采用对高阶参数化方程优化求解的方法获得控制量.
$$
x(u) = x_0 + x_1 u + x_2 u^2 + x_3 u^3 + x_4 u^4 + x_5 u^5 \\
y(u) = y_0 + y_1 u + y_2 u^2 + y_3 u^3 + y_4 u^4 + y_5 u^5
$$
以车辆的当前位姿、当前前轮偏角、目标位姿和目标前轮偏角作为边界条件, 按照长度最短和最大曲率最小对参数方程进行最优化求解.
生成曲线后, 以曲线上一个规划周期后的曲率作为前馈控制量. 由于生成控制量时考虑了车辆的当前前轮偏角状态, 因此即使全局期望路径发生突变, 实际期望控制量也不会产生剧烈变化.
车辆运动模型的更新策略
二自由度阿克曼转向模型进行位置更新:
$$
\left{\begin{array}{l}
\dot{x}=v \cos (\theta) \\
\dot{y}=v \sin (\theta) \\
\dot{\theta}=v \frac{\tan (\delta)}{l}
\end{array}\right.
$$
问题: 不同车速下生成的期望路径可能不同, 这将影响规划算法的时间一致性.
解放办法: 为了消除速度对曲线生成的影响, 运动更新采用定距更新的方式.
$$ \left\{\begin{array}{l} \theta_{k+1}=\theta_{k}+\frac{s \tan (\delta)}{l} \\\\ x_{k+1}=x_{c}+r \sin \left(\theta_{k+1}\right) \\\\ y_{k+1}=y_{c}-r \cos \left(\theta_{k+2}\right) \end{array}\right. $$$(x_c, y_c)$ 是转动中心, $s$ 是定距规划的距离阈值. 采用定距更新后, 只要在一个规划周期中车辆行驶距离不超过定距规划的距离阈值, 就能够保证曲线的一致性.
鲁棒的局部规划
常规做法: 增加膨胀尺寸无法避免车辆驶入障碍物区域:
本论文做法: 假设路径生成的每个迭代过程中期望转向控制量为 $δ$, 而系统的过程误差为 $∆δ$. 用 $δ + ∆δ$ 和 $δ − ∆δ$ 代替 $δ$ 进行运动状态更新. 以通行区域的形式代替原本不具有宽度的期望路径进行避障分析. 这种方法与过度膨胀障碍物效果相同, 可以使车辆远离障碍物以保证安全. 可通行区域是由车辆当前位置逐渐扩张开来的, 避免了车辆进入过度膨胀区域而不可行的问题.
实验验证
三种方法对比:
- 微分平坦的高阶参数方程优化求解,
- 基于模型预测控制原理 (MPC) 的曲线生成
- 基于前向预测的曲线生成
仿真方法:
V-Rep 仿真软件与 Visual Studio 联合仿真
阿克曼模型车辆模型 Manta
单线激光作为传感器
场景:
两级障碍物设定.
benchmark 算法的效果:
然而经过第 1 层障碍后, 再要通过第 2 层两个障碍之间的通道, 留给车辆的可行驶空间已经非常有限了. 虽然一开始规划结果能够得到可通行路径, 但是由于算法的时间不一致问题以及过程误差的影响, 车辆实际行驶路径逐渐偏移规划结果. 起初车辆距离障碍物较远时这种偏移还不足以使车辆无法通行, 但随着车辆接近障碍物, 规划程序最终无法计算出能够通过通道的结果.
小结
- 时间一致性的结果是 replan 的必要性与策略, 本文提出的思想是将车辆动力学约束加入到局部路径规划的曲线拟合优化求解过程中. 与 Apollo 的做法有些神似, 都是发送给控制一小段曲线与更长的轨迹规划, 保证光滑性与时间一致性.
- 本文学到最大的点是作者们对概念的总结很是不错, 实际调试过程中遇到的问题都用比较精确的概念总结了下来.