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/
- 路径最短
- 基于启发式的搜索方法: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(用来保证实时性,如果没有高精地图,仅仅通过感知的信息进行计算,那么很难保证实时性,因为这样会造成很大的计算量)
- 如何设置出一个合理的轨迹?
- 经典参考书籍
- 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方法
- 经典的基于环境建模的方法
- 现代无人车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 限制条件
- 连续空间问题
- 离散化,无人车路径规划问题是求解一个连续空间问题,那么常用的一个方法就是连续空间离散化。
- 网格化
传统的机器人基础
无人车Planning
定义:
- A点到B点 ,构建一个车辆运动轨迹
- 输入:HDMap,Localization 和Prediction
- 输出:可行驶轨迹,有一系列点组成
- 两个层面:导航层面; 运动轨迹层面
Routing
- routing的目标是导航一条A到B的全局路径,一条cost最小的路径。
- 输入:地图(路网信息,交通信息等)、当前位置、目的地(乘客决定)
- 输出:可行驶道路的连线
- 搜索:地图数据转化成图网络
什么情况下cost高?
- 权重规则:例如左转的权重相较于直行的权重更高,所以Node1到Node4的边权重大,Node1到Node3权重小。
- 拥堵情况:比如说Node1到Node3的道路很拥堵,那么它的cost就高;Node4的道路更堵,那么Node1到Node4的cost更高。
A*经典算法
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
- Speed Planning
- Path和Speed解耦能够让我们将motion planning问题转化为多个凸优化问题。
Path Planning(Littice DP)
Speed Planning
- ST图
- S-T图表示在path上的速度规划,S表示Path上的纵向距离,T表示运动时间。
- 斜率越大,表示速度越快。
轨迹规划
- 实例:遇到一辆速度很慢的车我们如何超车
Lattice Planning(网格规划)
- SL轨迹和ST轨迹
- Lattice将两个图合并处理,同事进行Path和Speed的采样
- 示例:如果我们遇到一个cut in场景
- 先对整个候选轨迹进行采样
- 设计一个合理的Cost
- 选择一个Cost最小的轨迹
- 条件检查和碰撞检查
- 检查失败则返回继续找Cost次优候选项
- 成功的输出结果
- Littice因为其采样计算量大,为了优化其采样效果,需要先进行场景化以简化计算量
课程:B站【全】无人驾驶系列知识入门到提高
本文内容由网友自发贡献,版权归原作者所有,本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容,请联系:hwhale#tublm.com(使用前将#替换为@)