路径规划基础

· 2020-07-18 · # astar # c++ # bfs # rrt

路径规划基础

前言

该文档为无人机路径规划的基础文档。

TODO LIST：

[x] 完成学习文档
[ ] 完成部分基础算法C++的实现
[ ] 在二维仿真空间上进行算法验证
[ ] 在三维仿真空间上进行算法验证

1. 常见名词

拓扑图

拓扑图是对实体图形的简单化与规则化表示，他记录了物体之间的相对位置。
ESDF（欧几里何距离场）

记录了空间中物体距离当前位置的欧几里何距离信息。
欧几里何距离（Euclidean Distance）

又称欧式距离，是指m维空间中两个点之间的真实距离，或者向量的自然长度。

在欧几里何空间内，两点之间距离为：
$$
d(x, y)=\sqrt{\left(x_{1}-y_{1}\right)^{2}+\left(x_{2}-y_{2}\right)^{2}+\cdots+\left(x_{n}-y_{n}\right)^{2}}=\sqrt{\sum_{i=1}^{n}\left(x_{i}-y_{i}\right)^{2}}
$$
向量的自然长度为：
$$
\|\vec{x}\|_{n}=\sqrt{\left|x_{1}\right|^{2}+\cdots+\left|x_{n}\right|^{2}}
$$
标准化欧式距离（Standardized Euclidean Distance）

标准化欧式距离将数据各个维度的分量都标准化到均值、方差相等。标准化变量的数学期望为1，方差为0。标准化过程用公式来描述为：
$$
X^{*}=\frac{X-m}{s}
$$
N维空间内两点$ a\left(x_{11}, x_{12}, \cdots, x_{1 n}\right) $, $ b\left(x_{21}, x_{22}, \cdots, x_{2 n}\right)$的标准欧式距离为：
$$
d_{ab}=\sqrt{\sum_{k=1}^{n}\left(\frac{x_{1 k}-x_{2 k}}{s_{k}}\right)^{2}}
$$
曼哈顿距离（Manhattan Distance）

曼哈顿距离标明了两个点在标准坐标系上的绝对轴距之总和。

对于N维空间内的两个点$ a=\left(x_{1}, x_{2}, \ldots, x_{n}\right) $, $ b=\left(y_{1}, y_{2}, \ldots, y_{n}\right) $,他们的曼哈顿距离为：
$$
d_{a b}=\left|x_{1}-y_{1}\right|+\left|x_{2}-y_{2}\right|+\ldots . .+\left|x_{n}-y_{n}\right|
$$
切比雪夫距离（Chebyshev Distance）

切比雪夫距离描述了各个坐标轴差的最大值。

对于N维空间内的两个点$ a=\left(x_{1}, x_{2}, \ldots, x_{n}\right) $, $ b=\left(y_{1}, y_{2}, \ldots, y_{n}\right) $,他们的切比雪夫距离为：
$$
d(x, y):=\max _{i}\left(\left|x_{i}-y_{i}\right|\right)
$$
闵可夫斯基距离（Minkowski Distance）

闵可夫斯基距离简称闵式距离，他是一组距离的定义。

设空间内两点为$ a=\left(x_{1}, x_{2}, \ldots, x_{n}\right) $, $ b=\left(y_{1}, y_{2}, \ldots, y_{n}\right) $

那么闵可夫斯基距离为：
$$
\left(\sum_{i=1}^{n}\left|x_{i}-y_{i}\right|^{p}\right)^{1 / p}
$$
当$ p=1 $ 即为曼哈顿距离：
$$
\left(\sum_{i=1}^{n}\left|x_{i}-y_{i}\right|^{1}\right)^{\frac{1}{T}}=\sum_{i=1}^{n}\left|x_{i}-y_{i}\right|
$$
当$ p=2 $即为欧几里何距离：
$$
\left(\sum_{i=1}^{n}\left|x_{i}-y_{i}\right|^{2}\right)^{\frac{1}{2}}=\sqrt{\sum_{i=1}^{n}\left(x_{i}-y_{i}\right)^{2}}
$$
当$ p \rightarrow \infty $即为切比雪夫距离：
$$
\lim _{p \rightarrow \infty}\left(\sum_{i=1}^{n}\left|x_{i}-y_{i}\right|^{p}\right)^{\frac{1}{p}}=\max _{i=1}^{n} \mid x_{i}-y_{i}
$$

2. 路径搜索算法

2.1 广度优先算法（BFS）

2.1.1 算法简介

广度优先算法通过不断向周围扩散，因此搜索出来的路径一定是最优的。但是需要向周围无差别搜索，因此最终需要的计算量很高，效率很低（它几乎是一种纯粹的暴力搜索）。

2.1.2 算法实现

常规的BFS算法需要维护一个visited数组来记录已经被访问的点。但在本算法中为了可以从终点回溯到起点，我们需要将visited数组改为came_from。比如从A扩散到B那么came_from[B]=A。

伪代码

frontier = Queue()
frontier.put(start )
came_from = dict()
came_from[start] = None

while not frontier.empty():
   current = frontier.get()

   if current == goal: 
      break           

   for next in graph.neighbors(current):
      if next not in came_from:
         frontier.put(next)
         came_from[next] = current

2.2 Dijkstra算法

2.2.1 算法介绍

在BFS算法中，我们是不考虑路径中的成本问题的，即我们只考虑起点和终点的最短距离。但是在实际情况中，这显然是不合理的，例如在无人机的运动过程中，进行高度爬升和左右移动所花费的成本显然是不同的。因此我们需要考虑一种成本最低的路线，这里的成本不在仅仅是距离的最短，他还包括其他可能的因素。

以下是使用Dijkstra算法和BFS算法对同一环境进行搜索的过程，路径上方块的颜色表示不同的成本。

2.2.2 算法实现

为了记录路径的总成本，我们需要添加一个新的变量$cost\_so\_far$来记录从起点到当前路径的移动成本，并且维护一个优先级队列来记录最佳的路径。

伪代码

frontier = PriorityQueue()
frontier.put(start, 0)
came_from = dict()
cost_so_far = dict()
came_from[start] = None
cost_so_far[start] = 0

while not frontier.empty():
   current = frontier.get()

   if current == goal:
      break

   for next in graph.neighbors(current):
      new_cost = cost_so_far[current] + graph.cost(current, next)
      if next not in cost_so_far or new_cost < cost_so_far[next]:
         cost_so_far[next] = new_cost
         priority = new_cost
         frontier.put(next, priority)
         came_from[next] = current

2.3 启发式算法

2.3.1 算法介绍

通过上面的介绍，我们应该可以看出BFS和Dijkstra是一种呈辐射状搜索的过程。但是既然我们已经知道了起点和终点的位置，那么我们可不可以通过一个方向的引导，让搜索时优先搜索指定方向上的点呢？这肯定是可行的,我们通过起点和终点的坐标位置来计算他们的曼哈顿距离，我们优先查找曼哈顿距离最小的点，因此就有了下面的启发式的搜索。

下面是启发式搜索和常规BFS的对比：

在这里看启发式搜索似乎很nice！但是你看！

在这种情况下虽然启发式搜索比BFS搜索花费的时间更短，但是他搜索出来的路径已经不是最优的了！因此这种算法在障碍物多且复杂的情况下显然是不可取的。

2.3.2 算法实现

该算法使用了Dijkstra算法的优先级队列，但是已经没有cost_so_far变量了。

伪代码

def heuristic(a, b):
   # Manhattan distance on a square grid
   return abs(a.x - b.x) + abs(a.y - b.y)
frontier = PriorityQueue()
frontier.put(start, 0)
came_from = dict()
came_from[start] = None

while not frontier.empty():
   current = frontier.get()

   if current == goal:
      break

   for next in graph.neighbors(current):
      if next not in came_from:
         priority = heuristic(goal, next)
         frontier.put(next, priority)
         came_from[next] = current

2.4 A*算法

2.4.1 算法介绍

Dijkstra算法可以很好的找到最优的路径，但是会耗费更大的计算量。启发式搜索可以减小计算量，但是搜索出的路径不一定是最优的，并且在复杂路况中的表现也不是很好。那么有没有一种鱼和熊掌都兼得的方法呢？

显然是有的，它就是A 算法，A算法使用了开始到终点的实际距离和估计距离来进行搜索。它是对启发式搜索的一种改进，只要启发式搜索估计的不太夸张，它都能搜索到最佳的路径。

下面是三种搜索算法的对比：

2.4.2 算法实现

A*算法和Dijkstra算法在实现上十分相近。

伪代码

frontier = PriorityQueue()
frontier.put(start, 0)
came_from = dict()
cost_so_far = dict()
came_from[start] = None
cost_so_far[start] = 0

while not frontier.empty():
   current = frontier.get()

   if current == goal:
      break

   for next in graph.neighbors(current):
      new_cost = cost_so_far[current] + graph.cost(current, next)
      if next not in cost_so_far or new_cost < cost_so_far[next]:
         cost_so_far[next] = new_cost
         priority = new_cost + heuristic(goal, next)
         frontier.put(next, priority)
         came_from[next] = current

C++实现

#include <iostream>
#include <list>
#include <vector>
#include <memory>
#include <cmath>
struct Point {
  int x, y;
  int F, G, H;

  std::shared_ptr<Point> parent;
  Point(int _x, int _y) : x(_x), y(_y), F(0), G(0), H(0), parent(nullptr) {

  }
};
class Astar {
 private:
  std::vector<std::vector<int>> map;
  std::list<std::shared_ptr<Point>> openList;
  std::list<std::shared_ptr<Point>> closeList;
  const int kCost1 = 10;//水平移动的成本
  const int kCost2 = 12;//斜向移动的成本
 public:
  void initAstar(const std::vector<std::vector<int>> &map) {
    this->map = map;
  }
  std::list<std::shared_ptr<Point>> getPath(const std::shared_ptr<Point> &start, const std::shared_ptr<Point> &end) {
    std::shared_ptr<Point> result = findPath(start, end);
    std::list<std::shared_ptr<Point>> path;
    while (result) {
      path.push_front(result);
      result = result->parent;
    }
    openList.clear();
    closeList.clear();
    return path;
  }
  std::shared_ptr<Point> getLeastFPoint() {
    if (!openList.empty()) {
      auto resPoint = openList.front();
      for (auto &point:openList) {
        if (point->F < resPoint->F) {
          resPoint = point;
        }
      }
      return resPoint;
    }
    return nullptr;
  }
  std::shared_ptr<Point> isInList(const std::list<std::shared_ptr<Point>> &list, const std::shared_ptr<Point> &point) {
    for (auto p:list) {
      if (p->x == point->x && p->y == point->y) {
        return p;
      }
    }
    return nullptr;
  }
  bool isCanReach(const std::shared_ptr<Point> &point, const std::shared_ptr<Point> &target) {
    if (target->x < 0 || target->x > map.size() - 1
        || target->y < 0 || target->y > map[0].size() - 1
        || map[target->x][target->y] == 1
        || target->x == point->x && target->y == point->y
        || isInList(closeList, target)) {
      return false;
    } else {
      if (abs(point->x - target->x) + abs(point->y - target->y) == 1) {
        return true;
      } else {
        return map[point->x][point->y] == 0 && map[target->x][point->y] == 0;
      }
    }
  }
  int calcG(std::shared_ptr<Point> &point, std::shared_ptr<Point> &target) {
    int extraG = (abs(point->x - target->x) + abs(point->y - target->y)) == 1 ? kCost1 : kCost2;
    int parentG = point->parent == nullptr ? 0 : point->parent->G;
    return parentG + extraG;
  }
  int calcH(const std::shared_ptr<Point> &point, const std::shared_ptr<Point> &target) {
    return std::sqrt((double) (target->x - point->x) * (double) (target->x - point->x)
                         + (double) (target->y - point->y) * (double) (target->y - point->y)) * kCost1;
  }
  int calcF(const std::shared_ptr<Point> &point) {
    return point->G + point->H;
  }
  std::vector<std::shared_ptr<Point>> getSurroundPoints(const std::shared_ptr<Point> &point) {
    std::vector<std::shared_ptr<Point>> surroundPoints;
    for (int x = point->x - 1; x <= point->x + 1; x++) {
      for (int y = point->y - 1; y <= point->y + 1; y++) {
        if (isCanReach(point, std::make_shared<Point>(x, y))) {
          surroundPoints.push_back(std::make_shared<Point>(x, y));
        }
      }
    }
    return surroundPoints;
  }
  std::shared_ptr<Point> findPath(const std::shared_ptr<Point> &start, const std::shared_ptr<Point> &end) {
    openList.push_back(std::make_shared<Point>(start->x, start->y));
    while (!openList.empty()) {
      auto curPoint = getLeastFPoint();
      openList.remove(curPoint);
      closeList.push_back(curPoint);
      auto surroundPoints = getSurroundPoints(curPoint);
      for (auto &target : surroundPoints) {
        if (!isInList(openList, target)) {
          target->parent = curPoint;
          target->G = calcG(curPoint, target);
          target->H = calcH(target, end);
          target->F = calcF(target);
          openList.push_back(target);
        } else {
          int tempG = calcG(curPoint, target);
          if (tempG < target->G) {
            target->parent = curPoint;
            target->G = tempG;
            target->F = calcF(target);
          }
        }
        std::shared_ptr<Point> resPoint = isInList(openList, end);
        if (resPoint) {
          return resPoint;
        }
      }
    }
    return nullptr;
  }

};
int main() {
  std::vector<std::vector<int>> map{
      {0, 0, 0, 0, 0, 0, 0, 0},
      {0, 0, 0, 1, 0, 0, 0, 0},
      {0, 0, 0, 1, 0, 0, 0, 0},
      {0, 0, 0, 1, 0, 0, 0, 0},
      {0, 0, 0, 0, 0, 0, 0, 0}};
  Astar astar;
  std::shared_ptr<Point> start = std::make_shared<Point>(2, 1);
  std::shared_ptr<Point> end = std::make_shared<Point>(2, 6);
  astar.InitAstar(map);
  std::list<std::shared_ptr<Point>> path = astar.getPath(start, end);
  for (auto &p:path) {
    std::cout << '(' << p->x << ',' << p->y << ')' << std::endl;
  }
  return 0;
}

2.5 总结

上面三种搜索算法在路径搜索上都有各自的特点，因此可以根据他们本身的特点来选择需要的算法：

如果想要遍历地图上的所有点，那么可以使用BFS或者Dijkstra算法。如果移动的成本不同，可以考虑Dijkstra算法。
如果想要快速查找最接近的路径可以选择启发式搜索或者A*算法。需要注意的是启发式搜索的路径不一定是最优的。

3. 路径规划算法

3.1 PRM（Probabilistic Road Map 随机路径图）算法

3.1.1 算法介绍

我们在进行路径规划的过程中，无可避免的需要将现实中的地图转化为数据的形式。那么转化过程中怎么才能保证准确度和效率呢？如果我们将所有点都纳入到路径规划的图中，显然是不合理的，因此就需要一种将图离散化的方法。

PRM就是一种基于图搜索的方法，他将连续的空间转换成离散的空间，再利用路径搜索算法（A*，Dijkstra等）来寻找路径，这样可以大大提高搜索的效率，并且能够有效解决高维空间和复杂约束中的路径规划问题。对于大多数问题，PRM能以较少的样本点来覆盖大部分可行的空间，并且最终能够寻找到路径的概率为1（随着采样数的增加，最终总可以找到一条路径）。因此PRM算法是概率完备且不是最优的。

PRM算法大体分为两个部分，他们分别是学习阶段和查询阶段。

学习阶段

在给定图的自由空间内随机撒点，构建一个路径网路图。

这些点和路径要遵循以下两点：
1. 点不能与障碍物重合。
2. 连接的路径不能与障碍物相交。
查询阶段

该阶段需要选择合适的搜索算法，通过学习阶段构建的路径网路图查询到一个从起点到终点的路径。

3.1.2 代码实现

3.2 RRT（(Rapidly-exploring Random Trees 快速拓展随机树）路径规划算法

3.2.1 算法介绍

传统的路径规划算法有人工势场法、模糊规则法、遗传算法、神经网络、模拟退火算法、蚁群优化算法等。这些算法需要在一个确定的空间内对障碍物进行建模，计算复杂度与机器人自由度呈指数关系，不适合解决多自由度机器人在复杂环境中的规划。而基于RRT的路径规划算法通过对状态空间内的采样点进行碰撞检测，避免了对空间的建模，能够有效的解决高维度空间和复杂约束的路径规划问题。该方法的特点是能够有效快速的搜索高维空间，通过对状态空间的随机采样点，把搜索导向空白区域，从而找到一条从起始点到目标点的路径规划，适合在多自由度机器人在复杂环境和动态环境中的路径规划。与PRM类似，这种方法是概率完备且不最优的。

RRT算法以一个初始点作为根节点，通过随机采样增加叶子节点的方式，生成一个随机拓展树，当随机树中的叶子节点包含了目标点或者进入了目标区域，便可以在随机树中找到一条由初始点到目标点的路径。简略的实现步骤如下：

初始化根节点$N_0$
随机从状态空间内选择一个采样点$N_r$
从随机树中选择一个离$N_r$最近的节点$N_n$
让$N_n$节点向$N_r$的方向拓展一段距离，得到一个新的节点$N_{new}$
若$N_{new}$没有与障碍物发生碰撞则放弃生长，否则将$N_{new}$加入到随机树中
重复2～5直至$N_n$和目标点$N_{goal}$的距离小于一个阈值。

从上面我们可以看出来RRT是一种纯粹的搜索算法，因此它的收敛速度是不稳定的（因为采样点的选取将决定他的生长情况）。并且在狭窄的空间内，RRT算法会进一步暴露出他的不稳定性。

3.3 RRT*算法

3.3.1 算法介绍

RRT*是对RRT算法的一个改进，他的基本思想和RRT相同，但在生长的过程中相较于RRT算法他的生长更有优势。接下来可以看看它的算法流程：

初始化根节点$N_0$
随机从状态空间内选择一个采样点$N_r$
从随机树中选择一个离$N_r$最近的节点$N_n$
以$N_r$为圆心，以$r$为半径画圆，选出圆内的所有潜在的父节点$N_{pi}$
计算$N_r$到$N_{pi}$的路径所花费的代价之和并与$N_r$到$N_{n}$的路径所花费的代价之和进行比较。如果更优则进行碰撞检测
如果没有碰撞则将$N_r$与$N_{pi}$相连，并且删除$N_r$与$N_n$之间的连线
重复4～6直至遍历完所有的潜在父节点
重复2～4直至$N_n$和目标点$N_{goal}$的距离小于一个阈值

壁立千仞，无欲则刚