移动机器人运动规划

常用地图结构

occupancy grid map - 栅格地图

• Most Dense;
• Structural;
• Direct Index Query;

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

• Sparse;
• Structural;
• Indirect Index Query;

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

• Most Sparse;
• Structural;
• Indirect Index Query;

point cloud map

• Un-ordered;
• No Index Query;

基于图搜索的算法

基础概念

• 机器人配置（Robot configuration）：机器人上所有点的位置的描述；
• 机器人自由度（Robot degree of freedom, DOF）：能够表示机器人配置空间最少需要的值的数量；
• 机器人配置空间（Robot configuration space）：包含机器人所有可能的配置的 n 维向量空间，又称为 C-space；
• 机器人任何一个可能的位置都是在配置空间中的一个点；

2.1 Graph Search Basis

算法核心思想

• Maintain a container to store all the nodes to be visited;
• The container is initialized with the start state \(X_s\) ;
• Loop:
  • Remove a node from the container according to some pre-defined score function;
    • Visit a node;
  • Expansion: Obtain all neighbors of the node;
    • Discover all its neighbors;
  • Push them (neighbors) into the container;
• End loop;

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: 优先移除和扩张容器中最深的节点；

Dijkstra 算法

• 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.

算法流程：

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

2.3 A star 算法

算法简介：

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

地图预处理：

1. 将地图栅格化，把每一个正方形格子的中央称为节点；
2. 确定栅格属性，即每一个格子有两种状态：可走和不可走；
3. 定义两个列表集合：OpenList 和 CloseList，OpenList 可以存储节点的 g 值；
4. 确定起始节点和目标节点。

A* 算法流程：

1. 初始时，定义起始节点为父节点，移入 CloseList 中；
2. 逐个检查与父节点 \(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\)，若这条路径不是更优，则不进行任何操作；
3. 计算 OpenList 中所有节点的 \(f\) 值：\(f\left(n\right)=g\left(n\right)+h\left(n\right)\)，从 OpenList 中取出 \(f\) 值最小的节点，放到 CloseList 中，并把它作为新的父节点 \(i\)；
   • 这里启发式函数 \(h\left(n\right)\)一般用曼哈顿距离或者欧氏距离来替代。
4. 重复步骤 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)\)，则：

• \(a=0\)，\(b=1\) 时为最贪心的路径；
• \(a=1\)，\(b=\epsilon>1\) 时为偏向贪心的路径；
• \(a=1\)，\(b=1\) 时为最优路径；
• \(a=1\)，\(b=0\) 时退化为 Dijkstra 算法；

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} \]

Jump Point Search 算法

与 A* 的区别：

• A* 在扩张时考虑自然的邻居节点，而 JPS 考虑的是跳跃的邻居点；
• JPS 可以在空旷的区域进行大范围的跳跃；

搜索流程：

Question：JPS 一定比 A* 好吗？
Answer：不是，如果在障碍物多的场景，JPS 具有较好的效果，但是如果在障碍物较少的场景，JPS 需要扩展大量的节点；

总结：

• 在大多数情况下，尤其是复杂环境，JPS 的效果比 A*好，因为 JPS 减少了加入到 Open List 中节点的数量，减少了内存消耗，和 Open List 中排序的次数等，但是 JPS 增加了对地图中节点状态（有无障碍物）查询的次数；
• JPS 只适用于 uniform grid map；