系列文章目录

【无人驾驶——路径规划】第一章 基础知识

【无人驾驶——路径规划】第二章 无人车路径规划

定义

无人车路径规划的定义：

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合并得到最终结果。

1.BFS(广度优先搜索) && DFS（深度优先搜索）

• 广度优先遍历图的方式为，一次性访问当前顶点的所有未访问状态相邻顶点，并依次对每个相邻顶点执行同样处理。因为要依次对每个相邻顶点执行同样的广度优先访问操作，所以需要借助队列结构来存储当前顶点的相邻顶点。（队结构，先入先出）
• 深度优先遍历图的方式，同样会访问一个顶点的所有相邻顶点，不过深度优先的方式为，首先访问一个相邻顶点，并继续访问该相邻顶点的一个相邻顶点，重复执行直到当前正在被访问的顶点出度为零，或者不存在未访问状态的相邻顶点，则回退到上一个顶点继续按照该深度优先方式访问。因为存在回溯行为，所以需要借助栈结构保存顶点，或者直接利用递归调用产生的方法栈帧来完成回溯。(栈结构，先入后出)
• 当前路径规划相关的图搜索方法大多是基于广度优先搜索的原理来进行拓展的

1.1 BFS 实现方式

1. 选择起点放入队列中，并标记为已访问
2. 判断队列是否为空，当队列不为空时，从队列中取出顶点作为拓展点，随后将拓展点的所有相邻且未被访问过的节点放入队列，并标记为已访问；
3. 重复步骤2直至队列为空或搜索到目标点

1.2 DFS 实现方式

1. 选择起点放入栈中，并标记为已访问
2. 判断栈是否为空，当栈不为空时，将栈顶元素作为拓展点，随后搜索拓展点的相邻且未被访问过的节点，若存在，则将该节点入栈，否则则将拓展点出栈
3. 重复步骤2 直至栈为空或者搜索到目标点

由于BFS以及DFS都是较为基础的搜索算法，因此是较难满足无人车或者机器人的路径规划需求的。因此在此思想基础上，衍生出了众多基于栅格地图（grid map或costmap）的搜索算法。其中，包括了大家都熟知的Dijkstra算法以及A*算法。

2.Dijkstra算法

2.1 常规的Dijkstra方法介绍:

通常，在进行路径规划时，我们的所规划的栅格地图内可能存在栅格距离起点的move_cost不同的情况。譬如，地图内如果某片区域是草地，或者窄道，或者其他不易通行的区域，那么我们会自然将此区域距离起点的move_cost增加。当地图内所有的move_cost都相同的时候，此时的Dijkstra方法可以认为与BFS相同。

具体效果区别如下图所示：

• 其中，绿色区域为可行驶但是move_cost较高区域，深色区域为不可行驶区域
• 左图为BFS的搜索效果
• 右图为Dijkstra的搜索效果
• 可以看出Dijkstra更加考虑距离起点的cost(cost_so_far)

因此，在涉及具体存储的数据结构时，Dijkstra引入了优先队列用以存储节点的搜索信息。
Dijkstra算法步骤如下：

维护一个优先队列，用以存储所有需要被扩展的节点
2. 初始化起点，并将起点放入优先队列中
3. 设置起点的move_cost g($n_s$) = 0, 其余节点的g(n) = infinite
4. 执行以下循环：

• 如果优先队列为空，则返回false，终止搜索
• 弹出优先队列中 g值最小的节点n并将此节点标记为已拓展
• 如果该节点为目标点，return true，终止搜索
• 对于节点 n 的所有未拓展的邻节点m:

• 如果g(m) = infinite

• g(m) = g(n) + Cnm
• 将node m 放入优先队列中

• 如果g(m) > g(n) + Cnm :

• g(m) = g(n) + Cnm

2.2 ROS Navigation中Dijkstra源码解析

与常规Dijkstra的区别:

• 利用势场的方法来计算每个栅格的分数
• 采用倒叙(从终点向起点)的方法进行拓展
• 优点：生成的路径更加平滑
• 缺点：容易产生S型路径,且障碍物过多时会出现不必要的折线
• 缺点： 容易陷入局部最优无法脱困

costmap范围标记

/**
 * @实际的costmap坐标系可能原点坐标在左下角
 * @brief outline the boundary in the costmap
 * @param costarr: one dimension cost value array of costmap
 * @param nx: size of x in index coordinate
 * @param ny: size of y in index coordinate
 * @param value : in this function is 255 or lethal value = 254
 */
void OutlineMap(unsigned char* costarr, int nx, int ny, unsigned char value) {
    unsigned char* pc = costarr;
    for (int i = 0; i < nx; i++)
        *pc++ = value;              // in x direction in first column (from (0,0) to (10,0))
    pc = costarr + (ny - 1) * nx;   // move to last row first position((0,7))
    for (int i = 0; i < nx; i++)
        *pc++ = value;              // in x direction in last column (from (0,7) to (10,7) )
    pc = costarr;                   // (0,0)
    for (int i = 0; i < ny; i++, pc += nx)
        *pc = value;                // in y direction in first row (from (0,0) to (0,7))
    pc = costarr + nx - 1;          // in y direction in last row (10,0)
    for (int i = 0; i < ny; i++, pc += nx)
        *pc = value;                // in y direction in last row (from (10,0) to (10,7))
}

calculate potential/ 势场的计算
此处主要是基于ETH的一篇文献进行的优化，优化后的potential 扩散会由原来的矩形扩散优化为圆形扩散。
文献名称如下：

• A Light Formulation of the E∗Interpolated Path Replanner

最终的理论公式：
$$\begin{gathered} v=-0.2301dd+0.5307d+0.7040 \\ pot=ta+hf\ast v \end{gathered}$$
其中，变量的物理意义可以参考以下代码，代码中有对应的推导及说明

float CalculatePotential(int cell_index) {
  // get neighbors
  float u, d, l, r;
  l = potential_[cell_index - 1];
  r = potential_[cell_index + 1];
  u = potential_[cell_index - cell_x_];
  d = potential_[cell_index + cell_x_];

  // find lowest, and its lowest neighbor
  // ta lowest cost in same column, tc lowest cost in same row
  float ta, tc;
  if (l < r)
    tc = l;
  else
    tc = r;
  if (u < d)
    ta = u;
  else
    ta = d;

  float hf = getCost(charmap_, cell_index);  // traversability factor
  float dc = tc - ta;                   // relative cost between ta,tc
  if (dc < 0)                           // tc is lowest
  {
    dc = -dc;
    ta = tc;
  }

  // calculate new potential
  if (dc >= hf)  // if too large, use ta-only update
    return ta + hf;
  else  // two-neighbor interpolation update
  {
    // use quadratic approximation
    float d = dc / hf;
    float v = -0.2301 * d * d + 0.5307 * d + 0.7040;
    return ta + hf * v;
  }
}

双线性差值进行梯度计算
从终点开始逆向搜索，为了减少栅格对于路径精度的影响，这里采用了利用双线性差值的的方法来计算梯度从而达到搜索对应路径节点而不是栅格的目的。
具体步骤为：

1. 将终点设置为当前计算节点 node_cur，并更新world坐标与取整后坐标的dx,dy差距
2. 如果当前坐标与起点坐标相差小于预设阈值，则认为搜索成功
3. 计算node_cur的梯度，node_cur右侧相邻节点梯度，node_cur上侧相邻节点梯度，node_cur右上相邻节点梯度
4. 利用双线性差值原理计算实际计算节点world坐标对应的梯度值
5. 计算沿梯度搜索对应的实际坐标并更新node_cur以及dx,dy值
6. 重复第二步

补充知识点：

bilinear interpolation method

参考链接： https://blog.csdn.net/xbinworld/article/details/65660665
首先，参考线性插值原理：
已知坐标$(x_0,y_0),(x_1,y_1)$, 计算区间$[x_0,x_1]$内某一坐标$x$在直线上的$y $值, 可得公式：
$\frac{y-y_0}{x-x_0}=\frac{y_1-y_0}{x_1-x_0}$
推导上述公式可得：
$y=\frac{x_1-x}{x_1-x_0}{y}_0+\frac{x-x_0}{x_1-x_0}{y}_1$
由此类比，推出双线性插值原理：
双线性插值是有两个变量的插值函数的线性插值扩展，其核心思想是在两个方向分别进行一次线性插值，图例见下图：

由上图， 已知四点$a,b,c,d$及函数$f(x,y)$，首先在x方向上进行插值
$f(f)=\frac{x_2-x}{x_2-x_1}f(a)+\frac{x-x_1}{x_2-x_1}f(d)$
$f(e)=\frac{x_2-x}{x_2-x_1}f(b)+\frac{x-x_1}{x_2-x_1}f(c)$

接着在y方向上进行插值
$f(g)=\frac{y_2-y}{y_2-y_1}f(f)+\frac{y-y_1}{y_2-y_1}f(e)$

$f(g)=f(x,y)=\frac{(x_2-x)(y_2-y)}{(x_2-x_1)(y_2-y_1)}f(a)+\frac{(x-x_1)(y_2-y)}{(x_2-x_1)(y_2-y_1)}f(d)+\frac{(x_2-x)(y-y_1)}{(x_2-x_1)(y_2-y_1)}f(b)+\frac{(x-x_1)(y-y_1)}{(x_2-x_1)(y_2-y_1)}f(c)$

下一期将介绍图搜索算法中具有heuristic function的相关算法

上一篇博客 "无人车路径规划算法—（1）算法体系综述 " 中介绍了无人车路径规划通常采用的"前端生成路径->后端平滑优化"的pipeline，本文将为大家介绍路径规划算法常用地图格式，文章主要分为以下三个模块：

• 常用地图格式
• 栅格地图
• 拓扑地图与高精度地图

生成一个随机二维栅格地图方法的代码将开源在博主github主页

地图（MAP）

无人车的规划模块是联通感知模块，定位模块与控制模块的中间模块（架构图见图一），因此，基于搜索的规划算法通常是在能够包含无人车自身定位信息以及周边环境信息的数据结构体中进行运算，这种能够体现无人车周边环境信息的数据载体统称为地图（MAP），在介绍搜索具体的搜索算法之前，需要先引入搜索地图的概念以及常用的地图类型。

本部分将重点说明以下四种常见形式：

• Point Cloud Map（点云地图）
• Metric Map(度量地图)
• Topological Map(拓扑地图)
• Semantic Map(语义地图)

1. Point Cloud Map（点云地图）

说到自动驾驶的地图，大家常最先想起的就是激光雷达给出的点云地图，但是原始点云地图通常不会被拿来进行路径规划使用，主要是由于以下原因：

• 点云地图通常规模很大，需要大量的存储空间，同时点云地图提供了很多不必要的细节
• 处理多个点云重叠处的效果不好

因此，需要利用costmap或者octomap进一步的处理（见第二部分）。

2. Metric Map（度量地图）

度量地图框架是最常见的框架，它考虑的是目标对象的二维空间。用精确的坐标表示目标对象以及各个目标之间的相对关系。这种表示方法非常有效，但其对噪声敏感，并且很难计算距离精度。譬如在地图中，北京可以用固定的经纬度来表示，上海可以用另一个固定的经纬度来表示，两者之间的距离可以通过各自的经纬度推算出来。
通常我们用稀疏（Sparse）与稠密（Dense）对它们进行分类：1

• 稀疏地图：对地图信息进行一定程度的抽象，只保留需要关注的信息，对次要信息进行简化
• 稠密地图：对所有地图中所有见到的信息进行描述，对于无人车的路径规划模块来说，稠密地图是十分必要的，通常我们将所有的有效信息通过设定的分辨率，分割到每个固定的栅格中（二维的栅格称为Grid,三维的栅格称为Voxel）。每个栅格由占有，空闲，与未知三种状态构成以表述对应栅格的障碍物占据情况。但是我们也看到，这种地图需要存储每一个格点的状态，耗费大量的存储空间，而且多数情况下地图的许多细节部分是无用的。

在度量地图中，与无人车规划模块或机器人导航模块应用最广泛的则是占据栅格地图（Occupancy Grid Map）与代价地图（CostMap）

2.1 Occupancy Grid Map (占据栅格地图)

占据栅格地图是把环境划分成一系列栅格，其中每一栅格给定一个可能值，表示该栅格被占据的概率。占据栅格的构建主要是基于二值贝叶斯滤波原理利用多传感器（激光雷达，相机等）的输入判断地图内每个栅格的障碍物占据概率。
由于占据栅格地图通常是单层结构，更适用于简单的路径规划功能，当机器人或无人车对于环境信息精确度要求不高时可以利用此地图进行规划，当规划模块对精度与安全性要求较高时，更好的选择是利用代价地图（CostMap）进行规划。

2.2 CostMap (代价地图)

Costmap是无人车或者机器人融合多传感器信息后建立维护的二维或三维栅格地图，在地图中障碍物信息会被转换到对应的栅格中并且利用相应的规则对障碍物进行膨胀。ROS的官方Navigation库中最早利用costmap进行障碍物的位置判断以及在此基础上利用DWA（原理可参见我之前的系列博客）算法进行了局部规划，后来在日本的开源框架Autoware中也基于costmap实现了乘用车的局部规划功能（HybridAstar算法）。
ROS官方Navigation库中对于costmap说明文档如下：http://wiki.ros.org/costmap_2d/
官方的说明文档中有以下两幅图需要重点关注：

• 地图结构说明图 （见图一）
• 障碍物膨胀（inflation）与栅格分数关系图（见图二）

下图（图一）可以大致描述出costmap地图中的三个主要组成要素：

1. Footprint （红色多边形）
2. 障碍物的膨胀层 （蓝色栅格）
3. 障碍物位置 （红色栅格）

下图（图二）详细的解释了障碍物膨胀区栅格分数的取值依据，其中红色多边形为车辆轮廓：

1. Lethal：机器人的中心与障碍物所占据栅格重合，此时机器人必然与障碍物冲突
2. Inscribed：障碍物所占据栅格与机器人footprint内切圆重合，此时机器人必然与障碍物冲突
3. Possibly circumscribed：机器人外接圆与障碍物占据栅格相切或接近，因此是否碰撞取决于机器人的朝向等因素
4. Freespace：自由空间，无碰撞风险
5. Unknow：未知区域
6. 其他的栅格的分数可以根据与Lethal以及Freespace的距离来计算，也可以根据自身产品设计相应的衰减函数来推算

介绍完costmap自身构建原理之后，另外一个值得注意的重点则是栅格地图坐标系与world坐标系转换问题：

单层地图由许多个cell组成，一般可以认为单层地图有两个坐标系：位置坐标系(以m为单位)和索引坐标系(以cell为单位)。在地图构造完成后，会有索引坐标系到位置坐标系的转换关系，所以在定位某个cell的时候可以使用任意坐标系，不存在本质性的区别。

转换关系如下图所示2：

• 地图坐标系（map frame，坐标系原点即map center，其在world中的位置就是mapPosition，X正向上，Y正向左)
• 内存坐标系（原点为内存块的左上角，即第一个存储的cell，可以近似理解为图像坐标系，X正向下，Y正向右）

样例代码片段如下：

/**
*@ This Section Is For Transform pose from world frame to index frame
*@ This Is The First Method
*/
gemoetometry_msgs::Pose transformPose(const geometry_msgs::Pose& pose, const tf::Transform& tf)
{
    // convert ROS pose to TF pose
    tf::Pose tf_pose;
    tf::poseMsgToTF(pose, tf_pose);
    
    // transform pose
    tf_pose = tf * tf_pose;
    
    // convert TF pose to ROS pose
    geometry_msgs::Pose ros_pose;
    tf::poseTFToMsg(tf_pose, ros_pose);
    
    return ros_pose;
}

void poseToIndex(const geometry_msgs::Pose& pose, int* index_x, int* index_y) 
{
    tf::Transform orig_tf;
    tf::poseMsgToTF(costmap_info.origin, orig_tf);
    geometry_msgs::Pose pose_new = transformPose(pose, orig_tf.inverse());
    
    *index_x = pose_new.position.x / costmap_info.resolution;
    *index_y = pose_new.position.y / costmap_info.resolution;
}

/**
*@ This Section Is For Transform pose from world frame to index frame
*@ This Is The Second Method
*/
void poseToIndex(const geometry_msgs::Pose& pose, int* index_x, int* index_y)
{
    *index_x = (pose.position.x - costmap_.info.origin.position.x) / costmap_.info.resolution;
    *index_y = (pose.position.y - costmap_.info.origin.position.y) / costmap_.info.resolution;
}

/**
*@ This Section Is For Transform pose from index frame to world frame
*/
void indexToPose(int index_x , int index_y , geometry_msgs::Pose& pose) 
{
  double px, py;
  px = costmap_.info.origin.x + (*index_x + 0.5) * costmap_.info.resolution;
  py = costmap_.info.origin.y + (*index_y + 0.5) * costmap_.info.resolution;
  pose.position.x = px;
  pose.position.y = py;
}

2.3 开源库分享

grid_map 开源库：

1. grid_map库是设计有ROS接口的C++库，用于管理具有多个数据层的二维栅格地图。它是为移动机器人地图设计的，用于存储(海拔)高度、方差、颜色、摩擦系数、可通过性等数据。它用于为粗糙地形导航设计的以机器人为中心的高程地图包中。
2. 链接：https://github.com/ANYbotics/grid_map

Octomap 开源库：

1. Octomap是SLAM领域经常用到的地图结构，它以八叉树数据结构来存储三维环境的概率占据地图，采用此结构可以节省大量的存储空间同时可以实现地图的压缩与更新，并且地图的分辨率可调。
2. 链接：https://github.com/OctoMap/octomap

2.4 式例代码

编写了一个随机生成障碍物的二维地图 (跳票中。。。。最近活太多。。。正在努力填坑中，后面也会找时间把这段时间做的东西整理一下分享出来)

3 Topological Map （拓扑地图）

拓扑地图则更强调地图元素之间的关系。拓扑地图是一个图（Graph），由节点和边组成，只考虑节点间的连通性。它放松了地图对精确位置的需要，去掉地图的细节问题，是一种更为紧凑的表达方式。这种地图更适用于低纬度的路径规划，因此接触的不多，有经验的网友欢迎补充。

4 Semantic Map （语义地图）

语义地图个人理解可以分为ADAS MAP和高精度地图（HD MAP）,ADAS地图是级别低于高精度地图的一种包含语义信息的地图，我们这里还是主要介绍一下高精度地图。
高精度地图主要服务与自动驾驶车辆（用于机器人太奢侈，高精度地图成本太高），通过一套独特的导航体系，帮助自动驾驶解决系统性能问题，扩展传感器检测边界。它可以帮助车辆找到合适的行车空间，还可以帮助Planner确定不同的路线选择。如APOLLO中，高精度地图可以帮助车辆识别准确的道路中心线，这样在规划时就可以让车辆尽可能沿道路中心线行驶。同时在十字路口等交叉路口也可以帮助车辆缩小规划范围，以便节省计算成本与时间，在最有范围内选择出最佳路线。

参考文献：
1《视觉SLAM十四讲：从理论到实践》
2 https://github.com/FleverX/grid_map