路径规划基础
前言
该文档为无人机路径规划的基础文档。
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算法大体分为两个部分，他们分别是学习阶段和查询阶段。
-
学习阶段
在给定图的自由空间内随机撒点，构建一个路径网路图。
这些点和路径要遵循以下两点：
- 点不能与障碍物重合。
- 连接的路径不能与障碍物相交。
-
查询阶段
该阶段需要选择合适的搜索算法，通过学习阶段构建的路径网路图查询到一个从起点到终点的路径。
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}$的距离小于一个阈值