（2）基于图的轨迹搜索

提纲

 1，图搜索算法基础
 2，Dijkstra和A*算法
 3，JPS跳点算法

一，图搜索算法基础

1.C空间

基本概念

工作空间：机器人的实际物理尺寸，形状，与环境组成的空间
机器人配置：机器人的各点位置说明
DOF自由度：最小驱动结构与机器人约束
机器人配置空间：包含机器人所有可能配置的n维空间（数学概念），简写为C空间，C-space
机器人的任意位姿都是C空间的一个点

工作空间向C空间转换

原因：工作空间中机器人有形状大小，计算开销大
方法：机器人缩小为一点，环境等值膨胀，膨胀尺寸为机器人的尺寸。一次计算，持续使用，复杂度O(1)
轨迹规划就是在C空间中从初始点Q_start到Q_end找到一条轨迹

C_space = C_obstacle 并上 C_free
联想ROS中导航时对机器人的大小，半径，膨胀层参数设置理解。

2.图搜索算法

图的分类

  无权重，有权重，有向，无向

状态空间图：

搜索算法的数学表达，对于每个搜索算法，都有一个状态空间图，通过线连接各个节点组成状态空间图

搜索算法通式：

1，通过搜索图产生搜索树（数据结构转换），
2，在搜索树中回溯节点寻找从出发到结束的轨迹，
3，通常算法会建立所有的搜索树，但是需要较长时间，一般都会尽可能搜索少的节点，

算法流程：

1，建立一个存储所有访问过的节点的容器（数据结构）
2，用起点来初始化容器
3，循环

  访问节点：按照预设规则从容器中弹出一个节点
  探索邻域：观察计算所有的临近点
  更新容器：将临近点压入容器

循环结束

基本问题

1，何时终止循环：当容器清空的时候（无可访问的节点）
2，如果图是闭环的：节点被弹出后，不会再次被加入容器中
3，如何能够尽可能少的扩展邻域的情况下尽快弹出正确的节点：且看下面分解

广度优先算法BFS

数据结构：队列结构，FIFO，先进先出
策略：从容器中弹出最浅的节点，更新容器，循环

深度优先算法DFS

数据结构：堆栈，LIFO，后进先出
策略：从容器中弹出最深的节点，更新容器，循环

算法对比

广度优先：兔子先吃窝边草，向外一圈圈探索
深度优先：一条路走到黑，然后换一条路走到黑
BFS和DFS都是仅仅靠数据结构决定如何探索，仅仅由先进还是后进决定，局限性比较大

启发式搜索（贪心搜索）

贪心算法是基于某种特定的规则（或许是最好的）进行节点的探索，成为启发式“heuristic”
启发式算法定义：猜测，你距离终点的距离。
猜测依据：

    欧式距离，点到点的几何平均数距离
    曼哈顿距离，点到点的x+y，沿坐标系距离之和

启发式算法的要求：

    能够引导探索的正确方向
    应该很容易计算（计算开销低）

在无障碍物的情况下，贪心算法获取的轨迹规划看起来很平滑，很迅速
但在有障碍物的情况下，因为算法中关于猜测的定义，导致对障碍物的躲避出现问题

二，Dijkstra和A*算法

动作代价

在实际问题中，从节点移动到他的邻域是需要代价的，如能量，时间，距离等
当所有节点之间的权重都置为1，BFS能够找到一个理想的轨迹
对于一般问题，如何找到最低代价的路径也是一个问题

Dijkstra算法

策略：访问具有最低代价和的节点（g(n)函数的和最低的节点）
g(n)：到达当前节点n的代价
为n的节点m更新g(m)的值
已经被访问过、扩展过的节点一定具有当前状态下的最低代价（最优性保证）

算法逻辑：
维持一个优先级队列的容器（从大到小排列的容器，从数据较小端弹出）
用初值初始化容器
假设g(0) = 0，其他所有节点g(n) = 无穷或者其他大数
循环

  从优先级队列中弹出具有最低代价的节点n
  标记n为已经扩展过
  对于n来说，所有未被探索过的节点m都有：
    如果g(m) = 无穷：
      g(m) = g(n) + cost(n=>m)
      将m加入优先级队列
    如果g(m) > g(n) +  cost(n=>m):
      g(m) = g(n) + cost(n=>m)
   直到优先级队列为空或n为终点

结束循环

算法优先：完备且最优，最优有保证
缺点：类似BFS，扩展所有节点的计算开销较大，盲目寻找终点

A*算法

回顾贪心算法，定义了一个代价方程，有准确的搜索问题解决方式
A*算法就是Dijkstra算法 + 贪心搜索
总和代价g(n)：同Dijkstra算法中的g(n)
启发Heuristic：h(n)：预估的从起点到终点的最低代价
最大的假设代价是f(n) = g(n) + h(n)

算法策略：
依赖最低f(n)进行节点扩展
更新n所有的节点m的f(n)
只要被扩展过或者访问过的节点都具有最低代价

算法逻辑：
维持一个优先级队列的容器（从大到小排列的容器，从数据较小端弹出）
为所有的节点建立预先定义的Heuristic函数f(n)
用初值初始化容器
假设g(0) = 0，其他所有节点g(n) = 无穷或者其他大数
循环

  从优先级队列中弹出具有最低 f(n) = g(n) + h(n) 代价的节点n
  标记n为已经扩展过
  对于n来说，所有未被探索过的节点m都有：
    如果g(m) = 无穷：
      g(m) = g(n) + cost(n=>m)
      将m加入优先级队列
    如果g(m) > g(n) +  cost(n=>m):
      g(m) = g(n) + cost(n=>m)
   直到优先级队列为空或n为终点

结束循环

A的最优性保证
我们需要所有几点的假设最小代价都低于实际代价，才能保证最优性
原因：在已知g(n)的情况下，当h(n)的值能够显著影响f(n)的时候，A就会选择诡异的路径，造成绕弯路，无法保证最优

容许的Heuristic函数
估计距离要小于实际距离，即为所有节点的 h(n) <= h(n)
如果Heuristic是容许的，那么A就是最优的
工程应用中通常提出可容许的Heuristic函数

Heuristic函数设计
欧拉距离函数是相容的
曼哈顿距离函数不一定是相容的（机器人可以斜向运动，就不相容了）
L_无穷(max(x,y,z))是相容的
0也是相容的（退化为Dijkstra算法）

Dijkstra 和 A*对比

Dijkstra算法探索扩展所有的方向，A*算法有目的性的向终点靠拢，具有贪心特性

如果使用不相容的Heuristic函数（超估计）

获得次优的路径，速度更快 =>权重A算法
f = g + $\varepsilon$ h ， $\varepsilon$ >1
就是在速度与最优之间获得一个取舍问题

更高级，更快的算法

权重A* -> Anytime A* -> ARA* -> D*

算法的线性叠加

f = $\alpha$ * g + $\beta$ * h
$\alpha$ =0 ， $\beta$ =1 最贪心算法
$\alpha$ =1 ， $\beta$ = $\varepsilon$ > 1 权重A算法
$\alpha$ =1 ， $\beta$ =1 A算法
$\alpha$ =0 ， $\beta$ =0 Dijkstra算法

工程应用

怎样表示图和网格
网格分为四连通或者八连通，对图建模无需表述，仅需在扩展邻域时进行调整即可

最优的Heuristic
在工程中没有最好的函数，因为没有网格是tight的，也就是紧的
首先找到结构化地图的理论最短距离（对于任一起点终点都可以求到的），这个最短距离是真的最短估计
使用这样的启发式函数得到的就是最优的Heuristic函数，被称为diagonal heuristic函数，能够优化20-50倍都是有可能的

破坏网格地图的对称性

网格地图对称性导致了轨迹的对称性，在一个地图中会存在若干具有同等长度的轨迹，并没有什么差异，但是让邻域探索工作量变大，容器中存在了若干具有相同代价的节点

解法1：让f函数稍微出现变化，让对称的网格出现极小的差异
h = h * (1.0 + p)
p < 每步最小cose / 期待的最长cost（或者非常大的常量）
这种方式会略微破坏h函数的相容性，但是不影响整体使用效果，因为实际环境下肯定有障碍物

解法2：当两个节点具有相同的f值的情况下，就对h值进行排序
或者在每个节点中加入一个预先设定好的hash表对应的值，
或者加上一个倾向性，比如加入一个沿着某个方向的倾向性
还有其他方法

有障碍物下的对称性打破

在有障碍物下，部分对称性打破的算法会导致路径能生成，但是轨迹无法生成，虽然更短或者更快，但是危害了轨迹规划

三，JPS跳点算法

是一种彻底消灭对称性的算法
核心思想就是找到图中对称性并打破

算法原理

通过父节点转移过来的过程中考虑子节点的临近节点
当子节点的某个临近节点可以由父节点到达，且几何距离代价更低或者相同（直线运动就是小于等于，对角运动就是小于），就不考虑这个节点了
称这些放弃的节点为劣性邻域节点，考虑的节点为自然邻域节点

当有障碍物时候
将由于障碍物的作用而不得不考虑时，就需要进行强制考察，有强制考察节点的节点称为关键节点

算法逻辑

首先重复不断的执行直线运动，一直移动到关键节点
其次进行对角运动

若是直线运动不断跳跃到障碍物处或者地图边界，都发现没有合理的道路，就认为直线运动失败了，需要进行对角运动，一直到一个关键节点处，然后标记关键节点的最大父节点，最大父节点就可以被加入优先级队列中了。

发现目标点就是发现了一个强制考察节点，直接选择就好了

算法流程图

和A*基本上完全相同

维持一个优先级队列的容器（从大到小排列的容器，从数据较小端弹出）
为所有的节点建立预先定义的Heuristic函数f(n)
用初值初始化容器
假设g(0) = 0，其他所有节点g(n) = 无穷或者其他大数
循环

  从优先级队列中弹出具有最低 f(n) = g(n) + h(n) 代价的节点n
  标记n为已经扩展过
  对于n来说，所有未被探索过的节点m都有（寻找探索节点的过程中使用JPS算法）：
    如果g(m) = 无穷：
      g(m) = g(n) + cost(n=>m)
      将m加入优先级队列
    如果g(m) > g(n) +  cost(n=>m):
      g(m) = g(n) + cost(n=>m)
   直到优先级队列为空或n为终点

结束循环

算法差异

A*算法寻找的是几何上的邻域节点
JPS算法寻找的是具有跳跃意义与价值的邻域节点

算法引申

用于三维条件下的JPS性能提高更多

迷宫环境

迷宫环境中，JPS是大部分情况下可以远超过A*的，但是对于机器人来说，如果有一个非常庞大的全局地图，因为机器人视野范围有限，JPS可能会慢很多

结论

在大多数情况下，即使在复杂环境中，JPS也会优化很多，因为它很少操作open list，
但是JPS会增加很多很多次是否有障碍物的膨胀查询，JPS也局限于规则网格化地图

（2）基于图的轨迹搜索

提纲

一，图搜索算法基础

1.C空间

基本概念

工作空间向C空间转换

2.图搜索算法

图的分类

状态空间图：

搜索算法通式：

算法流程：

基本问题

广度优先算法BFS

深度优先算法DFS

算法对比

启发式搜索（贪心搜索）

二，Dijkstra和A*算法

动作代价

Dijkstra算法

A*算法

Dijkstra 和 A*对比

如果使用不相容的Heuristic函数（超估计）

更高级，更快的算法

算法的线性叠加

工程应用

破坏网格地图的对称性

有障碍物下的对称性打破

三，JPS跳点算法

算法原理

算法逻辑

算法流程图

算法差异

算法引申

迷宫环境

结论

End