自动驾驶 规划综述

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
• 质点模型
  • 物体看成一个质点
    CS B659: Principles of Intelligent Robot Motionhttps://people.duke.edu/~kh269/teaching/b659/schedule.htm
  • 点与点不碰撞
• 刚体问题
  • 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

    • 曲率公式 

      

无人车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最大
    

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图
  •  S-T图表示在path上的速度规划，S表示Path上的纵向距离，T表示运动时间。
  • 斜率越大，表示速度越快。

• 如何规划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站【全】无人驾驶系列知识入门到提高