【自动驾驶规划】综述（一）

Overview什么是规划规划的本质如何解决一个规划问题传统的规划方法机器人学基础经典算法无人车的规划RoutingPlanningLattice PlannerAPOLLO如何求解规划问题EM PlannerDP、QP求解What is motion planning?Planning确实是目前无人车最困难也最有挑战的部分本质是什么？argminxf(x)argmin_{x}f(x)argmin

宇光_

921人浏览 · 2021-08-18 18:21:41

宇光_ · 2021-08-18 18:21:41 发布

无人驾驶规划概要

Overview
What is motion planning?
Motion planning 的三个领域
如何解决一个Motion Planning问题？
基本Planning方法
传统的机器人基础
无人车Planning
APOLLO 如何求解规划问题
总结
Homework

Overview

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

What is motion planning?

Planning确实是目前无人车最困难也最有挑战的部分
本质是什么？
$argmin_{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（二次规划）
- 凸优化问题最优化求解
- 公式表达
  $\frac{1}{2}X^{T}QX+c^{T}X$
  $subject:Ex=d,Fx\leq m$
- 性质：在凸优化中的凸空间问题，用QP有最优解
- QP如何找到平滑曲线？
  - $\left |f^{'} \right |^{2}$
  - $\left |f^{''} \right |^{2}$
  - $\left |f^{'''} \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*a_{0}$
  - 障碍物距离 $d*a_{1}$
  - 速度变化率 $acc*a_{2}$
  - 曲率 $kappa*a_{3}$
    - （为什么是kappa）
- $F(x)=l*a_{0}+d*a_{1}+acc*a_{2}+kappa*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^{'}=0$ $H^{''}=0$
- $S^{'}=0$ $S^{''}=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)=argmin_{l(s)}w_{0}\int l^{'''}(s)^{2}ds+w_{1}\int l^{''}(s)^{2}ds+w_{2}\int (l(s)-l_{ref})^{2}ds+other$
$a_{i}\leq l(s_{i})\leq b_{i}$
Speed DP
- 问题抽象：使用ST图
- 模型建立：Cost函数（障碍物、曲率、无人车状态）
- 优化方法DP最优求解
- $s^{'}=v$
- $s^{''}=acc$
- $s^{'''}=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