系列文章目录

---

文章目录

• 系列文章目录
• 定义
• Routing
• A*算法
• Motion Planning
• • 规划的约束条件
  • Cost Function
  • Frenet坐标系
  • Path Planning
  • • 步骤
    • Cost Function
  • Speed Planning
  • • ST图
    • 如何规划ST轨迹
    • 如何优化
    • 轨迹规划

---

定义

无人车路径规划的定义：

1. A点到B点 ，构建一个车辆运动轨迹，结合HDMap，Localization 和Prediction
2. 输出：可行驶轨迹，有一系列点组成
3. 两个层面：导航层面; 运动轨迹层面

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

Motion Planning

导航信息相当于给了粗略的路径信息，而Planning相当于一个高精度，低级别的search

规划的约束条件

• Collision 碰撞（躲避任何障碍物）
• Comfortable 舒适 （路径点必须平滑，速度也要平滑）
• 运动学约束（高速转弯，掉头曲率角度）
• Illegal合法 （交通法规）

Cost Function

• cost function由许多部分组成
  • 道路偏离中心线
  • 碰撞或者靠得太近
  • 速度太大，超速
  • Curvature太大，方向盘太急
    针对不同的场景，我们可能有多个不同得cost

Frenet坐标系

一般情况下我们会用笛卡尔坐标系（世界坐标系），但是表征的东西并不全面。因此在道路形式方面，我们采用Frenet坐标系，能够更好地表征偏离道路中心线的距离。

【注】Frenet坐标系和Track坐标系的区别
* L方向不同
* Track是基于Road级别
* Frenet是基于Lane级别

Path 和 Speed解耦

• Path和Speed解耦能够让我们将motion planning问题转化为多个凸优化问题。

Path Planning

步骤

• 先生成道路网格
• 每个网格单元中随机采样
• 每个网格选一个点
• 组成多条候选Path

Cost Function

需要cost最低的path，也就是最优path，cost的设计往往是planning的重点

• 中心线距离 l*a0
• 障碍物距离 d*a1
• 速度变化率 acc * a2
• 曲率 kappa * a3
• F(x) = la0 + da1 + acc * a2 + acc * a3 + a4
  【思考】为什么线性加权可以在一定程度上解决所有问题呢？

Speed Planning

ST图

S-T图表示在path上的速度规划，S表示Path上的纵向距离，T表示运动时间。
斜率越大，表示速度越快。

如何规划ST轨迹

1. 连续空间离散化（grid map)

2. 单元格内速度不变

3. 把障碍物投影进来

   • 将挡住我们Path轨迹的部分画进ST图中
   • 因此必须要有良好的轨迹预测
   • 例如下图中，to-t1时刻障碍物会在我们的Path轨迹中挡住s0,s1部分，（如何理解黄色部分？ 相当于t0-t1时刻，s0-s1这块区域是不能有车通行的）
     

4. 速度曲线不能碰到这个区域

• 有多个车的情况
  

如何优化

由于折线并不平滑，我们需要将不平滑的折线优化成平滑的线性曲线。

• QP （Quadratic Programming) 二次规划
• 这个方法很大程度上依赖于线性空间离散化
• 将平滑的非线性曲线与这些线段进行拟合
• 现成的工具：qpQASES

轨迹规划

实例

• 遇到一辆速度很慢的车我们如何超车？
  

• 生成很多轨迹（撒点采样）
  

• 用cost function对其进行评估，选择cost最小的一条
  

• 生成一个ST图来表述速度规划

• 生成多条速度曲线
  

• 使用优化工具对多条速度采样进行最优化求解

  • 让整个线路变得平滑
    

• 最后将每个轨迹点（跟我们自己定义的轨迹点Resolution）的Path，Speed合并得到最终结果。

标签：轨迹,cost,Motion,Planning,Path,第二章,规划,坐标系
来源： https://blog.csdn.net/weixin_43794327/article/details/118654902