自动驾驶 规划综述

Motion Planning

What is motion planning?

规划的本质是：

- 搜索问题

- “好”的规划就是一个目标函数：,求最优解

Motion Planning的三个领域

• Robotic Fields
  • 生成轨迹寻找目标
  • RRT， A*, D* ，D* Lite
• Control Theory
  • 动态系统理论实现目标状态
  • MPC模型预测控制， LQR线性二次型
• AI:状态到action的映射
  • 强化学习，模仿学习等

http://planning.cs.uiuc.edu/http://planning.cs.uiuc.edu/par1.pdf

如何解决一个Motion Planning问题?

我们需要找到一个简单的突破口，首先是一个path finding 问题，也就是路径选择问题。在这个问题中，我们不关心速度，不关心机器人如何运动，我们只关心路径的生成。

什么样的Path才是最好的path？

PathFinding.jshttps://qiao.github.io/PathFinding.js/visual/

• 路径最短
  • BFS & DFS
  • Dijkstra
• 基于启发式的搜索方法：A* * 大概知道了终点的位置
  • Heuristic function
  • Introduction to the A* Algorithmhttps://www.redblobgames.com/pathfinding/a-star/introduction.html

无人驾驶中的规划和A search有什么区别？*

• 无人驾驶场景下是部分感知

• 有动态障碍物

• 复杂环境：交通约束、碰瓷
  

• 而A* 是一个global algorithm

• Partial observed situation

  • 贪心算法
    increamental search:目前状态求解到最优
    

• D* * 部分环境信息的一种搜索算法，经典的apollo登月小车上就采用了这种算法。

  • D* Lite
  • 使用D*算法进行路径搜索不一定能够搜索到全局最优解，但是经过统计学的分析，是能够逼近全局最优解的。
    Stentz Anthony, "Optimal and Efficient Path Planning for Partially-Known Environments", 1994

至此，我们已经有了如下几个方法：

• 构造目标函数并且结合了平滑性和目标cost
• 使用通用的search方法来最小化Cost从而找到一个最优解
• 通过Partial observed information 来做局部planning

路径规划还需要什么？

• 处理动态障碍物，动态环境
• 遵守交通规则，公共安全
• 实时计算
  • 计算时间100ms-150ms
  • 人的一般反应时间300ms-500ms
  • 酒驾 1000ms
  • 有限时间内找到最优解
    这就是为什么很多大公司使用C++而不用python的原因，因为C++能够对代码进行更多的优化。

motion planning定义

• Safely

• Smootly

• Achieve to destination （能够到达目的地）

• 输出三维的轨迹 X，Y，Time ：3D trajectory optimization problem

• 无人车硬件系统

  • 定位设备
  • 感知设备

• 无人车软件信息

  • 动态信息
  • 静态信息
    - HD map（用来保证实时性，如果没有高精地图，仅仅通过感知的信息进行计算，那么很难保证实时性，因为这样会造成很大的计算量）

• 如何设置出一个合理的轨迹？

  • 路径Path
  • 速度Speed

• 经典参考书籍
  • Steve Lavelle, Motion Planning Algorithm
  • Principles of Robot Motion：Theory, Algorithm, and Implementations
• 经典文献(一篇综述）：
  • A review of Motion Planning Techniques for Automated Vehicles
    • A. Graph search based planners(基于图搜索的规划方法)
      这种方法把状态空间表达成网格或者lattice的形式，然后在这些状态里面找到一个可达的path。这类方法主要有A* D* Dijkstra algorithm 算法。值得一提的还有state lattice算法，虽然这个图看起来和Apollo里面的lattice不一样，但是这个是爸爸，在这篇文章[1]里面提出了时空lattice，这个也就是后来Apollo算法里面用的
      
    • B. Sampling based planners(基于采样的规划方法)
      这个主要介绍了RRT算法

C. Interpolating Curve Planners(插值曲线规划)
介绍了几种曲线生成的方法，主要有羊角螺旋线（Clothoid Curves）多项式曲线（Polynomial Curves） 贝塞尔曲线（Be ́zier Curves）。并且分别介绍了这几类样条曲线在路径规划的优化过程中作用。

clothoid curves 曲线的曲率是线性变化的，又因为车辆轨迹的曲率和方向盘基本上成正比，也就是说这种线型出来的结果方向盘会非常顺滑。
贝塞尔曲线 计算简单 速度快。
多项式拟合也是一个比较好的方法。

复制代码

  *

D. Numerical Optimization(数值优化)

基本的Planning方法

• 经典的基于环境建模的方法
  • RRT
  • Lattice
• 现代无人车Planning方法
  • Darpa
  • Lattice in Frenet Frame
  • Spiral polynomial
• 质点模型

  • 物体看成一个质点
    CS B659: Principles of Intelligent Robot Motionhttps://people.duke.edu/~kh269/teaching/b659/schedule.htm

  • 点与点不碰撞

• 刚体问题
  • BycicleModel
  • XY Heading
  • Collision
• Planning 限制条件
  • 避免碰撞
  • 边界阈值
• 连续空间问题
  • 离散化，无人车路径规划问题是求解一个连续空间问题，那么常用的一个方法就是连续空间离散化。
  • 网格化

传统的机器人基础

• PRM（Probabilistic RoadMAP Planning）

  • 非常常用的方法
    

  • 连续空间离散化

    • 随即撒点
    • obstacles上的点删除

  • 连接可行点，形成可行空间

  • A*

• RRT（Incremental version of PRM）

  • 基于采样，使用增量搜索的方式来进行
    

  • 找到附近可行点的最优点

    • F(x)最小，cost最小
    • 走过程中也不能有障碍，使cost最小

  • 走的过程中还可能碰到障碍物
    

  • 所以撒点搜索不能太远，一步一步地移动
    

  • 形成的路径是一个折现，后面还需要平滑曲线

• Lattice
  • 改进了RRT的折现问题
  • 直接给出path的平滑曲线
  • 思路：网格化 ，每个采样个中都是用曲线连接
  • 指数级别的搜索算法（NP-Hard）

DP（动态规划）

* 减少搜索空间
* 复用已有结果

• Lattice DP

  • 平滑度够吗？
    • 曲率连续
    • 曲率导数不一定连续

• QP（二次规划）

  • 凸优化问题最优化求解

  • 公式表达
    

  • 性质：在凸优化中的凸空间问题，用QP中有最优解

  • QP如何找到平滑曲线？

    • 一阶导数最优
    • 二阶导数最优
    • 三阶导数最优

  • 其他平滑曲线的方法：贝塞尔曲线，样条插值

• 刚体模型

  • 前轮转向和heading的关系
    • 车轮是沿着切线方向行驶
    • 前后轮是同一个旋转中心
    • 左右轮的结构相同
  • Bbicycle Model

曲率公式

无人车Planning

定义：

• A点到B点 ，构建一个车辆运动轨迹
• 输入：HDMap，Localization 和Prediction
• 输出：可行驶轨迹，有一系列点组成
• 两个层面：导航层面; 运动轨迹层面

Routing

• routing的目标是导航一条A到B的全局路径，一条cost最小的路径。
• 输入：地图（路网信息，交通信息等）、当前位置、目的地（乘客决定）
• 输出：可行驶道路的连线
• 搜索：地图数据转化成图网络
  • 节点表示道路
  • 边表示路口
    

什么情况下cost高？

• 权重规则：例如左转的权重相较于直行的权重更高，所以Node1到Node4的边权重大，Node1到Node3权重小。
• 拥堵情况：比如说Node1到Node3的道路很拥堵，那么它的cost就高；Node4的道路更堵，那么Node1到Node4的cost更高。

A*经典算法

• 最经典的路径查找算法

  • 在rounting中目前A*算法的应用还是非常广泛的。

  • 公式： F(n) = G(n) + H(n)
    

  • F(n)表示道路的routing的总cost

  • G(n)表示起始点到候选点的cost

  • H(n)表示候选点通过启发函数得到的目标点cost

真实地图中的应用

* 左转Cost最大  

Planning

Motion Planning

• 导航信息相当于给了粗略的路径信息，而Planning相当于一个高精度，低级别的search。trajectory planning
• 轨迹点：XY、Time、Velocity
  

规划的约束条件

• Collision 碰撞（躲避任何障碍物）
• Comfortable 舒适 （路径点必须平滑，速度也要平滑）
• 运动学约束（高速转弯，掉头曲率角度）
• Illegal合法 （交通法规，人类约定俗成的规则）

Cost Function

真实情况下有多条可行轨迹

Cost由许多条件组成

道路偏离中心线距离

碰撞物靠的太近

速度太大，超速

Curvature太大，方向盘太急

• 针对不同的场景，我们可能有多个不同得cost
  • 高速场景
  • 停车场
  • 不同车辆

Frenet坐标系（车辆坐标系）

• 一般情况下我们会用笛卡尔坐标系（世界坐标系），但是表征的东西并不全面。
  • xy坐标无法知道我们车开了多远
  • 车有没有偏离车道中心线
  • 不知道道路在哪
• 更好的坐标系：Frenet
  • 在道路形式方面，我们采用Frenet坐标系，能够更好地表征偏离道路中心线的距离。
  • 【注】Frenet坐标系和Track坐标系的区别
    • L方向不同
    • Track是基于Road级别
    • Frenet是基于Lane级别
  • S表示走了多远
  • L表示距离车道中心线多远

Path 和 Speed解耦

• Path Planning
  • 生成可行轨迹
  • Cost（选最低的）
    • path平滑性
    • 碰撞安全性
    • 距离道路中心线的偏离性
• Speed Planning
  • 每个path上选择一系列速度
  • 生成速度轨迹
• Path和Speed解耦能够让我们将motion planning问题转化为多个凸优化问题。

Path Planning（Littice DP）

步骤

* 先生成道路网格（GridMap）
* 每个网格单元中随机采样（撒点）
* 每个网格选一个点
* 组成多条候选Path

Cost Function
需要cost最低的path，也就是最优path，cost的设计往往是planning的重点

* 中心线距离 l*a0
* 障碍物距离 d*a1
* 速度变化率 acc * a2
* 曲率 kappa * a3
* **F(x) = l*_a0 + d*_ a1 + acc * a2 + kappa * a3 + a4**  

【思考】为什么线性加权可以在一定程度上解决所有问题呢？

Speed Planning

• ST图

  • S-T图表示在path上的速度规划，S表示Path上的纵向距离，T表示运动时间。
  • 斜率越大，表示速度越快。

• 如何规划ST轨迹

连续空间离散化（grid map)

单元格内速度不变

把障碍物投影进来

将挡住我们Path轨迹的部分画进ST图中

因此必须要有良好的轨迹预测

例如下图中，to-t1时刻障碍物会在我们的Path轨迹中挡住s0,s1部分，（如何理解黄色部分？ 相当于t0-t1时刻，s0-s1这块区域是不能有车通行的）

* 速度曲线不能碰到这个区域  

* 有多个车的情况  

* 凸优化来求解得到最优的速度曲线 
  * search
  * 限制条件：速度限制、距离限制（安全距离）、车辆动力学限制（车的加速度、刹车性能）

• 如何优化
  • 对不平滑的速度线优化
    • QP优化速度、加速度、加速度导数
  • 我们的这个方法很大程度依赖于连续空间离散化
  • 网格、单元格方法
  • 由于折线并不平滑，我们需要将不平滑的折线优化成平滑的线性曲线。
    • QP （Quadratic Programming) 二次规划
    • 这个方法很大程度上依赖于线性空间离散化
    • 将平滑的非线性曲线与这些线段进行拟合
    • 现成的工具： qpQASEShttps://projects.coin-or.org/qpQASES/wiki

轨迹规划

• 实例：遇到一辆速度很慢的车我们如何超车
  

生成很多轨迹（撒点采样）

用cost function对其进行评估，选择cost最小的一条

生成一个ST图来表述速度规划

生成多条速度曲线

使用优化工具对多条速度采样进行最优化求解

* 让整个路线和速度曲线变得平滑  

Lattice Planning（网格规划）

• SL轨迹和ST轨迹
  

• Lattice将两个图合并处理，同事进行Path和Speed的采样

• 示例：如果我们遇到一个cut in场景
  

  • 先对整个候选轨迹进行采样
    

  • 设计一个合理的Cost

  • 选择一个Cost最小的轨迹
    

  • 条件检查和碰撞检查
    

  • 检查失败则返回继续找Cost次优候选项
    

  • 成功的输出结果
    

• Littice因为其采样计算量大，为了优化其采样效果，需要先进行场景化以简化计算量

课程：B站【全】无人驾驶系列知识入门到提高