移动机器人运动规划

常用地图结构#

occupancy grid map - 栅格地图

octo-map - 栅格地图的优化，在障碍物存在的地方稠密，无障碍物的地方稀疏

voxel hashing - 将坐标哈希化，通过哈希表查询地图信息

基础概念

算法核心思想

Question 1：When to end the loop?
Answer：容器为空，或者目标节点已经被访问到；

Question 2：What if the graph is cyclic?
Answer：维护一个新的容器，存储被访问过的节点，避免节点被再次添加到容器中重复访问；

图的遍历

广度优先搜索（Breadth First Search, BFS）
- 先进先出，使用 queue；
- Strategy: 优先移除和扩张容器中最浅的节点；
深度优先搜索（Depth First Search, DFS）
- 先进后出，使用 stack；
- Strategy: 优先移除和扩张容器中最深的节点；

Strategy: expand/visit the node with cheapest accumulated cost $g(n)$ ;
- $g(n)$ ：The current best estimates of the accumulated cost from the start state to node $n$ ;
- Update the accumulated costs $g(m)$ for all unexpanded neighbors $m$ of node $n$ ;
- A node that has been expanded/visited is guaranteed to have the smallest cost from the start state.

算法流程：

初始时，S 集只包含起点 s，U 集包含除 s 外的其他节点，U 集中的节点 v 与起点 s 相邻，则该节点存储值为距起点 s 的距离，若与起点 s 不相邻，则距离为无限大；
从 U 集中选出距离起点最短的节点 k，并将节点 k 加入到 S 集中，同时从 U 集中移除节点 k；
更新 U 集中各个节点到起点 s 的距离。之所以更新 U 集中节点的距离，是因为由于上一步确定了节点 k 是求出最短路径的节点，从而可以利用节点 k 来更新其他节点的距离；例如，（s, v）的距离可能大于(s, k) + (k, v)的距离。
重复步骤 2 和步骤 3，直到遍历完所有节点。

算法简介：

A*算法是一种静态路网中求解最短路径最有效的直接搜索方法。广泛应用于室内机器人的路径搜索、游戏动画路径搜索等；
A*算法结合了贪心算法（深度优先）和 Dijkstra 算法（广度优先），是一种启发式的搜索算法；
路径优劣评价公式为： $f\left(n\right)=g\left(n\right)+h\left(n\right)$ ， $g\left(n\right)$ 是在状态空间中从初始状态到状态 $n$ 的实际代价， $h\left(n\right)$ 是从状态 $n$ 到目标状态的最佳路径的估计代价。
A*算法使用了两个状态表，分别称为OpenList和CloseList，OpenList存储待考察的节点，CloseList存储已考察过的节点。

地图预处理：

A* 算法流程：

初始时，定义起始节点为父节点，移入 CloseList 中；
逐个检查与父节点 $i$ 相邻的周围的节点 $j$ ，忽略不可走节点和已经存在于 CloseList 中的节点：
- 若节点 $j$ 不在 OpenList 中，则将节点 $j$ 的父节点记为节点 $i$ ，计算节点 $i$ 到节点 $j$ 的距离 $d$ ，计算 $g\left(j\right)=g\left(i\right)+d$ ，并把它们加入到 OpenList 中成为待考察的对象；
- 若节点 $j$ 已经在 OpenList 中，则检查这条路径是否更优，即经由节点 $i$ 到达节点 $j$ 是否有更小的 $g$ 值：计算节点 $i$ 到节点 $j$ 的距离 $d$ ，若 $g\left(i\right)+d$ 小于 OpenList 中节点 $j$ 存储的 $g\left(j\right)$ ，则这条路径更优，更新 OpenList 中节点 $j$ 存储的 $g$ 值 $g\left(j\right)=g\left(i\right)+d$ ，并将节点 $j$ 的父节点记为节点 $i$ ，若这条路径不是更优，则不进行任何操作；
计算 OpenList 中所有节点的 $f$ 值： $f\left(n\right)=g\left(n\right)+h\left(n\right)$ ，从 OpenList 中取出 $f$ 值最小的节点，放到 CloseList 中，并把它作为新的父节点 $i$ ；
- 这里启发式函数 $h\left(n\right)$ 一般用曼哈顿距离或者欧氏距离来替代。
重复步骤 2 和步骤 3，不断重复，直到搜索到目标节点，完成路径搜索。搜索出路径的结果可以直接遍历父节点得到。

Question 1：欧式距离（L2）作为启发式函数一定是最优的吗？
Amswer：是的

Question 2：曼哈顿（L1）作为启发式函数一定是最优的吗？
Amswer：不一定

Question 3： $L \infty$ 作为启发式函数一定是最优的吗？
Amswer：是

Question 4：常值 0 作为启发式函数一定是最优的吗？
Amswer：退化成 Dijkstra 算法，保证最优

思考：

如果启发式函数 $h(n) < h^*(n)$ ，即启发式函数估计的距离小于真实距离，则保证路径的最优性。如果设计出的 $h(n)\ge h^*(n)$ ，可以视为更加贪心，牺牲路径的最优性来换取计算时间，这就是 weight A*。

weighted A*：

将 A* 中的 $f(n)=g(n)+h(n)$ 替换为 $f(n)=g(n)+\epsilon \cdot h(n), \epsilon > 0$ ，以牺牲路径最优性来换取计算时间上的效率。

若有 $f(n)=a\cdot g(n)+b\cdot h(n)$ ，则：

Tie Breaker：

在寻找路径时，有时会出现多个节点的 $f(n)$ 相同的情况，这可能会在搜索的过程中导致 expand 很多节点，导致搜索效率低下。

解决：

方案一：当几个节点拥有相同的 $f(n)$ 值时，对 $h(n)$ 进行比较；
方案二：我们倾向于对角线抵达终点，在计算 $h(n)$ 时进行如下修正：
$\begin{align} dx1 &= abs(node.x-goal.x) \\ dy1 &=abs(node.y-goal.y) \\ dx2 &= abs(start.x-goal.x) \\ dy2 &= abs(start.y - goal.y) \\ cross &= abs(dx1\times dy2-dx2\times dy1) \\ h &= h+cross\times0.001 \end{align}$