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
- 质点模型
- 刚体问题
- 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
-
曲率公式
-
- 前轮转向和heading的关系
无人车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最大
- 左转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轨迹
-
连续空间离散化(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站【全】无人驾驶系列知识入门到提高