【自动驾驶规划】综述（一）
无人驾驶规划概要
- 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确实是目前无人车最困难也最有挑战的部分
- 本质是什么？
a r g m i n x f ( x ) argmin_{x}f(x) argminxf(x) - 搜索问题
- Google：Quary词，返回给最优结果
- 无人车：当前环境和当前状态，当前路径上最优选择
- 什么是好规划？
- "好"其实就是一个目标函数： f ( x ) f(x) 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
- 不关心速度，不关心走
- 周围固定
- 将问题简化成一个简单的问题：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 simple shortest path example :
无人车中的规划和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
- 实时性保证
- 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（二次规划）
- 凸优化问题最优化求解
- 公式表达
m i n i m i z e 1 2 X T Q X + c T X minimize \frac{1}{2}X^{T}QX+c^{T}X minimize21XTQX+cTX
s u b j e c t : E x = d , F x ≤ m subject:Ex=d,Fx\leq m subject:Ex=d,Fx≤m - 性质：在凸优化中的凸空间问题，用QP有最优解
- QP如何找到平滑曲线？
- m i n ∣ f ′ ∣ 2 min \left |f^{'} \right |^{2} min∣∣∣f′∣∣∣2
- m i n ∣ f ′ ′ ∣ 2 min \left |f^{''} \right |^{2} min∣∣∣f′′∣∣∣2
- m i n ∣ f ′ ′ ′ ∣ 2 min \left |f^{'''} \right |^{2} min∣∣∣f′′′∣∣∣2
- 其它的平滑曲线方法还有贝塞尔曲线、样条插值方法
刚体模型
前轮转向和Heading的关系
- 车轮是沿着切线方向行驶
- 前后轮是同一个旋转中心
- 左右轮的结构相同
Bicycle Model
- 曲率公式：
1 / R = k a p p a = ( tan ( w ) ) / L 1/R=kappa=(\tan (w))/L 1/R=kappa=(tan(w))/L
- 曲率公式：
无人车Planning
定义
- A点到B点，构建一个车辆运动轨迹，结合HDMap，Localization和Prediction
- 输入
- 输出：可行是轨迹，有一系列点组成
- 两个层面：导航层面；运动轨迹层面
Routing
导航一条A到B的全局路径
输入：地图（路网信息、交通状态）、当前位置、目的地（乘客决定）
输出：可行驶道路的连接线
搜索：地图数据转化成图网络
- 节点表示道路
- 边表示道路连接
A* 经典算法
最经典的路径查找算法
F ( n ) = G ( n ) + H ( n ) F(n)=G(n)+H(n) 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 合法
- 交通法规
- 人类约定俗成规则
- Collision 碰撞
Cost function 成本函数
- 真实情况下有多条可行轨迹
Cost由许多条件组成
- 道路偏离中心线距离
- 碰撞或者靠的太近
- 速度太快，超速
- Curvature太大，方向盘太急
不同场景我们可能有多个不同的Cost func
- 高速场景
- 停车场
- 不同车辆
Frenet 坐标系（车道坐标系）
一般我们用笛卡尔坐标系（世界坐标系）
- xy坐标无法知道我们车开了多远
- 有没有偏离中心线
- 也不知道道路在哪
更好的坐标系：Frenet
- 注意和Track坐标系的区别
- L方向不同
- Track是基于Road级别
- Frenet是基于Lane级别
- S表示了走了多远
- L表示距离车道有多偏
- 注意和Track坐标系的区别
Path vs Speed 解耦
Path Planning
- 生成可行轨迹路径
- Cost
- path 平滑性
- 安全性
- 道路中心偏移距离
选择出成本最低的一个Path Planning
Speed Planning
- 每个Path上选择一系列速度
- 生成速度轨迹
Path Planning
- 先生成道路网格（GridMap）
- 每个网格单元中随机采样（撒点）
- 每个网格选一个点
- 组成多条候选Path
- Cost Function对这些轨迹进行评估
- （找到成本最低的一个）
- 中心线距离 l ∗ a 0 l*a_{0} l∗a0
- 障碍物距离 d ∗ a 1 d*a_{1} d∗a1
- 速度变化率 a c c ∗ a 2 acc*a_{2} acc∗a2
- 曲率 k a p p a ∗ a 3 kappa*a_{3} kappa∗a3
- （为什么是kappa）
- F ( x ) = l ∗ a 0 + d ∗ a 1 + a c c ∗ a 2 + k a p p a ∗ a 3 + a 4 F(x)=l*a_{0}+d*a_{1}+acc*a_{2}+kappa*a_{3}+a_{4} F(x)=l∗a0+d∗a1+acc∗a2+kappa∗a3+a4
- （大家可以任意构思这个评估函数）
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 v=v_{C} v=vC
- a = 0 a=0 a=0
- 这种状态我们不需要采样
Following: 跟车行驶
- 需要对s和t同事采样
- 规定时间t跟在某个车的后面
- 保证安全距离
- v = v 1 v=v_{1} v=v1
- a = a 1 a=a_{1} a=a1
Stopping: 停止
- 对车辆在哪里停下来进行采样
- s和t同时采样
- a = 0 a=0 a=0
- v = 0 v=0 v=0
Lattice 对L方向进行优化
- 需要保证车辆以一个稳定的状态进行终点状态，比如与车道线保持平齐
- H ′ = 0 H^{'}=0 H′=0 H ′ ′ = 0 H^{''}=0 H′′=0
- S ′ = 0 S^{'}=0 S′=0 S ′ ′ = 0 S^{''}=0 S′′=0
合并ST和SL坐标
- 转化到Cartesian坐标系
- 生成XYTime一个三维轨迹
- 两个坐标系中都有S
- 找同一个S，对应的L和T
- 转化到Cartesian坐标系
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：是一种凸优化
- 如何计算最优解（DP + QP）
Path QP
- 问题抽象：根据当前驾驶信息和道路状况建立平滑的SL坐标轨迹
- 模型建立：合理优化目标函数和约束条件
- 优化方法：二次优化求解带约束的二次规划问题
L ( s ) = a r g m i n l ( s ) w 0 ∫ l ′ ′ ′ ( s ) 2 d s + w 1 ∫ l ′ ′ ( s ) 2 d s + w 2 ∫ ( l ( s ) − l r e f ) 2 d s + o t h e r 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 L(s)=argminl(s)w0∫l′′′(s)2ds+w1∫l′′(s)2ds+w2∫(l(s)−lref)2ds+other
a i ≤ l ( s i ) ≤ b i a_{i}\leq l(s_{i})\leq b_{i} ai≤l(si)≤biSpeed DP
- 问题抽象：使用ST图
- 模型建立：Cost函数（障碍物、曲率、无人车状态）
- 优化方法DP最优求解
- s ′ = v s^{'}=v s′=v
- s ′ ′ = a c c s^{''}=acc s′′=acc
- s ′ ′ ′ = j e r k s^{'''}=jerk s′′′=jerk 踩油门或刹车的速度
计算提速思路
- SL坐标系离散化处理
- 什么时候使用粗分辨率什么时候用细分辨率
- GPU并行计算
- 同时计算多条Reference Lane的结果
- QP Hotstart
- QP的性质（QP的原理是泰勒展开）
- 两帧之间差距不大
- 精通C++
- SL坐标系离散化处理
总结
- 什么是规划
- 机器人学基本规划思路
- 无人车规划特点
- 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
【自动驾驶规划】综述（一）相关推荐
- 自动驾驶规划方法综述
点击上方"小白学视觉",选择加"星标"或"置顶" 重磅干货,第一时间送达 仅作学术分享,不代表本公众号立场,侵权联系删除 转载于:本文来源: ...
- 自动驾驶规划术语与搜索空间的几种方法
1. 导读 目前,自动驾驶或自动驾驶汽车是学术界和汽车界研究的核心,因为它具有多方面的优势,包括提高安全性.减少拥堵.降低排放和提高机动性.其实软件是支持自动驾驶的关键驱动因素,在将乘客或货物从指定的 ...
- 自动驾驶规划控制（Ａ*、pure pursuit、LQR算法，使用c++在ubuntu和ros环境下实现）
文章目录 1 目录概述 2 算法介绍 2.1 Astart改进 2.2 ROS(Gazebo仿真) 2.2.1 使用Gazebo仿真需要安装的功能包 2.2.2 创建工作空间 catkin_ws 2. ...
- 自动驾驶前沿综述：基于深度强化学习的自动驾驶算法
©作者 | 陈道明 学校 | 布里斯托尔大学 研究方向 | 自动驾驶方向 这是 21 年的一篇综述文章,可以算得上是最前沿的自动驾驶技术综述.这几年随着深度表征学习的发展,强化学习领域也得到了加强.本 ...
- Apollo星火计划学习笔记——第七讲自动驾驶规划技术原理1
文章目录 前言 1. 规划技术功能概述 1.1 自动驾驶系统 1.2 规划的作用 1.3 规划主要功能 1.3.1 路由寻径(Routing) 1.3.2 行为决策 1.3.3 轨迹规划 2. Apo ...
- 【自动驾驶规划算法】方向盘角速度限速
背景 自动驾驶路测中经常出现规划和控制匹配的问题,规划的路径理论可执行,但是控制不能有效跟踪.究其原因是控制算法性能受到方向盘角速度限制.为解决这一问题,在速度规划层添加方向盘角速度限速项. 理论基础 ...
- 自动驾驶规划控制系列文章（一）——车辆数学模型
目录 一.车辆自行车运动学模型(kinematic bicycle model) 1.基于车辆重心的运动学模型 2. 以后轴中心为原点的运动学模型 二.车辆自行车动力学模型(kinematic bic ...
- MCTS自动驾驶规划
MCTS 参考:蒙特卡洛树搜索 首先,我们给MCTS下两个定义.第一,MCTS是一个决策时规划算法:第二,MCTS是一个rollout算法.不同之处在于MCTS中会部分的保存值函数,从而能够指导仿真产 ...
- Apollo星火计划学习笔记——第七讲自动驾驶规划技术原理2
文章目录 前言 1. 泊车系统介绍 1.1 泊车任务 1.2 关键 Use Cas--车位类型 1.3 关键 Use Cas--环境条件 1.4 泊车过程描述 1.5 泊车状态操作和用户接口定义 1. ...
最新文章
- Linux进程模型总结
- 蓝桥杯_风险度量（一题多解）
- 无线路由器和计算机怎么连接网络连接,华为无线路由器怎么连接宽带上网
- python 判断字符串是否为空用什么方法？
- 小结SQL Server连接失败错误及解决[引用]
- Hadoop报错：could only be replicated to 0 nodes, instead of 1
- IDEA项目目录里下找不到src，但是src确实存在的的解决方案
- python参考手册下载_python参考手册第4版
- CISCO 2811 路由器配置命令全集
- J1800N-D2H主板升级bios，安装Fedora20，并搭建开发环境
- cad移动时捕捉不到基点_CAD很难学吗？其实不然，掌握这10个知识你就能走天下！...
- 支付宝花呗提前还款————附操作图片
- win10访问不了SAMBA
- linux服务器上tcp有大量time_wait状态的解决方法和原因解释
- 在深夜加油站遇见哈利波特
- [zt] SAP 标准教材和自学方法
- 光电器件专题：COMSOL多物理场/FDTD时域有限差分/ RSoft光电器件仿真
- 2D（横版）游戏开发心得
- 带上玻璃婚戒，赢得钻石人生
- 深入解析浏览器加速问题。