路径规划基础

路径规划基础

前言

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

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 算法简介

bfs

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

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算法对同一环境进行搜索的过程,路径上方块的颜色表示不同的成本。

Dijkstra

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的对比:

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

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

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

3.3 RRT*算法

3.3.1 算法介绍

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

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

3.3.2 算法实现

参考文献

添加新评论