Overview

• 什么是规划
  • 规划的本质
  • 如何解决一个规划问题
• 传统的规划方法
  • 机器人学基础
  • 经典算法
• 无人车的规划
  • Routing
  • Planning
  • Lattice Planner
• APOLLO如何求解规划问题
  • EM Planner
  • DP、QP求解

What is motion planning?

• Planning确实是目前无人车最困难也最有挑战的部分
• 本质是什么？
  $$argmi{n}_{x}f(x)$$
• 搜索问题
  • Google：Quary词，返回给最优结果
  • 无人车：当前环境和当前状态，当前路径上最优选择
  • 什么是好规划？
• "好"其实就是一个目标函数：$f(x)$
  • f(x)的最优解

Motion planning 的三个领域

• Robotics Fileds:
  • 生成轨迹实现目标
  • RRT，A* ，D* ，D* Lite
• Control Theory:
  • 动态系统理论实现目标状态
  • MPC，LQR
• AI: 生成状态和Action的一个映射
  • Reinforcement learning, Imitation Learning

  Cited by motion planning by Steve Lavelle : http://planning.cs.uiuc.edu/par1.pdf

如何解决一个Motion Planning问题？

• 找一个简单的突破口

  • 将问题简化成一个简单的问题：Path Finding Problem
    • 不关心速度，不关心走
    • 周围固定

• 简言之就是：路径选择问题

  • A simple shortest path example :

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

  • 什么样的Path是最好的Path？这是重点
    • 路径最短
      • BFS & DFS
      • Dijkstra
  • 刚刚看到的Search属于Non-informative search，效率比较低
  • A* search：基于Dijkstra的改进算法
    • 大概知道了终点位置
    • Heuristic func（启发函数）

    https://www.redblobgames.com/pathfinding/a-star/introduction.html

• 无人车中的规划和A* Search相差多远？

  • 部分感知
  • 动态障碍物
  • 复杂环境：交通规则、碰瓷
  
  • A* 本身是一个Global Algorithm
    • Global routing

• Partial observed situation

  • 贪心算法

    • incremental search（增量式）：目前状态求解到最优
    

  • D* star

    • 部分环境信息的一个Search
    • Apollo登月小车
    • D* Lite

  • 这样可以求解全局最优？

    • 有难度
    • 一定有必要全局最优？

    Stentz Anthony, “Optimal and Efficient Path Planning for Partially-Known Environments”, 1994

• Informative & Non-informative search

  • Global & Partial observed

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

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

• 我们还缺什么？

  • 处理动态障碍物，动态环境
    • 静止环境
  • 处理交通规则
    • 公共安全
  • 实时计算
    • 100ms~150ms
    • 人一般反应时间300~500ms
    • 酒驾 1000ms
    • 有限时间内找到最优解
    • C++

• 给无人车motion planning下一个定义：

  • Safely

  • Smoothly

  • Achieve to destination

  • X, Y, Time: 3D trajectory optimization problem

  • 无人车硬件系统

    • 定位设备
    • 感知设备

  • 无人车软件信息

    • 动态信息
    • 静态信息
      • HD Map
        • 实时性保证

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

    • 路径Path
    • 速度Speed
    

• 经典参考书籍：

  • Steve Lavelle, Motion Planning Algorithms
  • Principles of Robot Motion: Theory, Algorithms, and Implementations

• 经典文献：

  • A Review of Motion Planning Techniques for Automated Vehicles

基本Planning方法

• 经典基于环境建模的方法

  • RRT
  • Lattice（网格化）

• 现代无人车Planning方法

  • Darpa
  • Lattice in Frenet Frame
  • Spiral polynomial（解决平滑问题的思路）
  

  A Review of Motion Planning Techniques for Automated Vehicles

• 质点模型

  • 物体看成一个质点
  

  http://people.duke.edu/~kh269/teaching/b659/schedule.htm

  • 点与点不碰撞

• 刚体问题

  • BycicleModel
  • XY Heading
  • Collision

• Planning限制条件

  • 避免碰撞
  • 边界阈值

• 连续空间问题怎么解？

  • 离散化
  • 网格化

传统的机器人基础

• PRM(Probabilistic RoadMap Planning)

  • 非常常用的一个方法（撒点）
  
  • 连续空间离散化
    • 随机撒点
    • Obstacle 上的点删除
  • 连接可行点，行成可行空间
  • A*

• RRT(Incremental version of PRM)

  • 使用增量搜索的方式来进行
  
  • 找附近可行点的最优点
    • F(x)最小，Cost最小
    • 走过程中也不能有阻碍使Cost小
  • 走过程中，还可能碰到障碍物
  
  • 撒点搜索距离不能太远
    • 一步一步移动
  

• Lattice方法

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

• DP（动态规划）

  • 减少搜索空间
    • 复用已有结果
  
  • Lattice DP的平滑度够吗？
    • 曲率连续
    • 曲率导数不一定连续

• QP（二次规划）

  • 凸优化问题最优化求解
  • 公式表达
    $$minimize\frac{1}{2}{X}^{T}QX+c^{T}X$$
    $$subject:Ex=d,Fx\le m$$
  • 性质：在凸优化中的凸空间问题，用QP有最优解
  • QP如何找到平滑曲线？
    • $min{\left|f^{{}^{\prime }}\right|}^2$
    • $min{\left|f^{{}^{\prime \prime }}\right|}^2$
    • $min{\left|f^{{}^{\prime \prime \prime }}\right|}^2$
  • 其它的平滑曲线方法还有贝塞尔曲线、样条插值方法

• 刚体模型

  

  • 前轮转向和Heading的关系

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

  • Bicycle Model

    
    • 曲率公式：
      $$1/R=kappa=(\tan (w))/L$$

无人车Planning

• 定义

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

• Routing

  • 导航一条A到B的全局路径

  • 输入：地图（路网信息、交通状态）、当前位置、目的地（乘客决定）

  • 输出：可行驶道路的连接线

  • 搜索：地图数据转化成图网络

    • 节点表示道路
    • 边表示道路连接
    

• A* 经典算法

  • 最经典的路径查找算法

  • $F(n)=G(n)+H(n)$

    
    • Fn表示道路的routing的总Cost
    • Gn表示起始点到候选点的Cost
    • Hn表示候选点通过启发函数得到的目标点Cost

  • 真实地图中的应用

    
    • 左转Cost最大
    

• Motion Planning

  
  • Planning理解为高精度、低级别的一个search，trajectory planning
  • 轨迹点：XY、Time、Velocity

• 规划的约束条件

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

• Cost function 成本函数

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

  • Cost由许多条件组成

    • 道路偏离中心线距离
    • 碰撞或者靠的太近
    • 速度太快，超速
    • Curvature太大，方向盘太急
    

  • 不同场景我们可能有多个不同的Cost func

    • 高速场景
    • 停车场
    • 不同车辆

• Frenet 坐标系（车道坐标系）

  • 一般我们用笛卡尔坐标系（世界坐标系）

    
    • xy坐标无法知道我们车开了多远
    • 有没有偏离中心线
    • 也不知道道路在哪

  • 更好的坐标系：Frenet

    
    • 注意和Track坐标系的区别
      • L方向不同
      • Track是基于Road级别
      • Frenet是基于Lane级别
    • S表示了走了多远
    • L表示距离车道有多偏

• Path vs Speed 解耦

  • Path Planning

    • 生成可行轨迹路径
    • Cost
      • path 平滑性
      • 安全性
      • 道路中心偏移距离

  • 选择出成本最低的一个Path Planning

  • Speed Planning

    • 每个Path上选择一系列速度
    • 生成速度轨迹

• Path Planning

  • 先生成道路网格（GridMap）
  • 每个网格单元中随机采样（撒点）
  • 每个网格选一个点
  • 组成多条候选Path
  
  • Cost Function对这些轨迹进行评估
    • （找到成本最低的一个）
    • 中心线距离 $l\ast a_0$
    • 障碍物距离 $d\ast a_1$
    • 速度变化率 $acc\ast a_2$
    • 曲率 $kappa\ast a_3$
      • （为什么是kappa）
  • $F(x)=l\ast a_0+d\ast a_1+acc\ast a_2+kappa\ast a_3+a_4$
    • （大家可以任意构思这个评估函数）

• Speed Planning

  • ST图

    

  • S 表示Path上纵向距离

  • T 表示运动时间

• 如何规划ST轨迹

  • 连续空间离散化(grid map)
  • 单元格内速度保持不变
  

  • 把障碍物投影进来

    • 挡住我们Path轨迹的部分画进ST图中
    • 因此必须要有良好的预测轨迹
    • （下图，t0, t1时刻障碍物会在我们的path轨迹中挡住s0, s1部分）
    

  • 速度曲线不能碰撞这个区域

    

  • 凸优化来求解得到最优的速度曲线

    • Search
    • 限制条件：速度限制、距离限制（安全距离）、车辆动力学限制（车的加速度、刹车性能）
    

• 如何优化？

  • 对不平滑的速度线优化
    • QP
  

  • 我们的这个方法很大程度依赖于连续空间离散化

  • 网格、单元格方法

    • 但是并不平滑

  • Quadratic Programming 二次规划

    • 将平滑的非线性曲线与这些线段进行拟合
    • 有现成的工具 qpOASES

    https://projects.coin-or.org/qpOASES/wiki

• 轨迹规划

  • 实例：遇到一辆速度很慢的车我们如何超车
  
  • 生成很多轨迹线（撒点采样）
  
  • Cost function对其进行评估，选择Cost最小的一条
  
  • 生成一个ST图来表述速度规划
  • 生成多条速度曲线
  
  • 使用优化工具对多条速度采样进行最优化求解（Cost func，Constraints）
    • 让整个路线和速度曲线变得平滑
  

  • 最后将每个轨迹点（跟我们自己定义的轨迹点Resolution）的Path、Speed合并得到最终结果

    

• Lattice Planning （网格规划）

  • SL轨迹和ST轨迹
  

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

  • 实例：如果我们遇到一个切车场景

    

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

    

  • 设计一个合理的Cost

  • 选择一个Cost最小的轨迹

    

  • 条件检查和碰撞检查

    
  

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

    

  • 成功则输出结果

    

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

• Cruising, Following and Stopping：对S方向进行优化

  • Cruising: 定速行驶

    • $v=v_C$
    • $a=0$
    • 这种状态我们不需要采样

  • Following: 跟车行驶

    
    • 需要对s和t同事采样
    • 规定时间t跟在某个车的后面
    • 保证安全距离
    • $v=v_1$
    • $a=a_1$

  • Stopping: 停止

    • 对车辆在哪里停下来进行采样
    • s和t同时采样
    • $a=0$
    • $v=0$
    

• Lattice 对L方向进行优化

  • 需要保证车辆以一个稳定的状态进行终点状态，比如与车道线保持平齐
  
  • $H^{{}^{\prime }}=0$ $H^{{}^{\prime \prime }}=0$
  • $S^{{}^{\prime }}=0$ $S^{{}^{\prime \prime }}=0$

• 合并ST和SL坐标

  • 转化到Cartesian坐标系
    
  
  • 生成XYTime一个三维轨迹
    • 两个坐标系中都有S
    • 找同一个S，对应的L和T

APOLLO 如何求解规划问题

• Constrains

  • soft contraints & hard constraints

  • 给一个实际例子
    

    

  • Constraints：

    • 交通规则：Hard constraints
    • 用QP或者Hard code方式精细处理

  • Decision：

    • 决策：Soft constraints
    • 用DP的方式来处理一些人为设置的软约束

  • 最优轨迹：

    • QP

• 如何换道

  • 无人车想要换道怎么办？

    

  • 换道考虑很多安全性问题

    • 给出两种轨迹结果，让后续模块判断

  • Reference Line Decider

    • 换道时是否安全
    • 拿到信息比Planning丰富
    • 做很多准备工作

• Apollo EM Planner

  • 什么叫EM（期望最大化）

    

  • 先生成一条Optimal Path

  • 再生成一条当前Path情况下的Optimal Speed

  • 再将目前的Speed返回去给Path进行一次Tuning

  • 将Tuning的Path返回来给Speed做优化

  • 最后迭代到最优解

  • 贪心算法：Local Optimum

    

• 三个关键步骤

  • 目标函数

    • 线性加权的Cost
    • （当然有更好的）

  • 限制条件

    • 交通规则
    • 碰撞条件（无穷大）
    • 动态特性（车辆能力）

  • 优化求解

    • 如何计算最优解（DP + QP）
      
    
    • DP：不要求问题是凸的
    • QP：是一种凸优化

• Path QP

  • 问题抽象：根据当前驾驶信息和道路状况建立平滑的SL坐标轨迹
  • 模型建立：合理优化目标函数和约束条件
  • 优化方法：二次优化求解带约束的二次规划问题
    
  

  $$L(s)=argmi{n}_{l(s)}{w}_0\int l^{{}^{\prime \prime \prime }}(s{)}^{2}ds+w_1\int l^{{}^{\prime \prime }}(s{)}^{2}ds+w_2\int (l(s)-l_{ref}{)}^{2}ds+other$$
  $$a_i\le l(s_i)\le b_i$$

• Speed DP

  • 问题抽象：使用ST图
  • 模型建立：Cost函数（障碍物、曲率、无人车状态）
  • 优化方法DP最优求解
    
  
  • $s^{{}^{\prime }}=v$
  • $s^{{}^{\prime \prime }}=acc$
  • $s^{{}^{\prime \prime \prime }}=jerk$ 踩油门或刹车的速度

• 计算提速思路

  • SL坐标系离散化处理
    • 什么时候使用粗分辨率什么时候用细分辨率
  • GPU并行计算
    • 同时计算多条Reference Lane的结果
  • QP Hotstart
    • QP的性质（QP的原理是泰勒展开）
    • 两帧之间差距不大
  • 精通C++

总结

• 什么是规划
• 机器人学基本规划思路
• 无人车规划特点
• Apollo中的EM Planner

Homework

• Reading：
  • A Review of Motion Planning Techniques for Automated Vehicles
  • Baidu Apollo EM Motion Planner
  • A* : https://www.redblobgames.com/pathfinding/a-star/introduction.html
• Coding：
  • PythonRobotics: https://github.com/AtsushiSakai/PythonRobotics
• Thinking：
  • Everything is trade-off?
  • No model is perfect, but useful