2基于图搜索路径规划-深蓝学院

Graph Search basis

@TOC
remark

T:what
Y:why
W:How

configuration space

Y : 在配置空间中进行规划
T:所有点的位姿描述,由自由空间和障碍空间构成
W:将工作空间的planning变为配置空间上的planning，机器人相当于一个质点，障碍物按照机器人的体积进行膨胀，路径规划便在配置空间从起始点到终止点的规划问题，从而避开了机器人体积问题。
• Robot configuration: a specification of the position of all points of the robot
• Robot degree of freedom (DOF): The minimum number 𝑛 of real-valued coordinates
needed to represent the robot configuration
• Robot configuration space: a 𝑛-dim space containing all possible robot configurations,
denoted as C C-space
• Each robot pose is a point in the C-space

Workspace and Configuration Space Obstacle

collision detection（碰撞检测费时费力)

• planning in c-space
  c-space=(c-obstacle)U(c-free)
  
  在工作空间中，机器人有形状和尺寸，而在c-space中是一个点，
  规划中，障碍物会进行各向同性的膨胀，在C-space中表示障碍可能非常复杂。所以近似(但更保守,方便在实践中使用

Graph and Search Method

• State space graph: a mathematical representation of
a search algorithm
* 每个搜索问题都有一个状态空间图
* 图中节点之间的连通性由(有向或无向)边表示

The search always start from start state X S
• 搜索图产生搜索树
• Back-tracing a node in the search tree gives us a path from the start state to that node
• For many problems we can never actually build the whole tree, too large or inefficient – we only want to reach the goal node asap.

构建一个完整的搜索树较困难

• 算法执行一般步骤
  维护一个容器存储所有要访问的节点
  刚开始是装入目标Xs
  然后进入循环
  从容器中根据预先定义的指标或者目标函数弹出一个元素，也就是访问节点，扩展相邻节点，将这些节点放入容器，

这个循环会在没有任何节点装入，也就是容器空了，如果是一个回环，则需要另外一个容器装入访问过的节点。

关键如何访问节点，更快到达终点；尽可能减少节点扩展，到达目标

breadth First Search（BFS） vs Depth First Search

• BFS 使用队列
  策略，弹出/扩展最浅的节点
  如图P为S临近最浅的节点，需要弹出，然后加入q,再弹出e,d;……依次迭代，这样能够找到最优路径
  
  BFS是不断一层一层往外扩展，直到终点

• DFS使用堆栈 一般不是最短路径
  扩展容器中最深的节点
  需要自己定义一个顺序，如从左到右，或者顺逆时钟
  

• 二者对比

紫色代表其找寻的路径

Heuristic search（启发式算法）

Greedy Best First Search（heuristic）

T:对每个节点进行与终点的猜测；搜寻规定最短距离，目标性强。
merit：启发式引导你走向正确的方向，应该容易计算。
demerit：贪心算法对障碍物较敏感，不一定能找到最优路径

• Euclidean Distance & manhattan Distance
  
• 都是对实际最短距离的近似

如果图上的边的代价不同，需要找寻代价最小的路径，因而有了迪杰斯特拉算法和A* 算法

Dijkstra & A*

与BFS相比，Dijkstra从容器中弹出的规则不同。Dijkstra弹出的是从起点到某点的走过的距离最短的点。也就是说，容器不再使用队列，而是优先队列，弹出的总是走过路径最短的点。

Strategy: 扩展/访问，累积代价函数g(n)最小的节点
最优

• 更新节点“n”的所有未展开邻居“m”的累积代价g(m)
• 已扩展/访问的节点保证从开始状态开始的成本最小
• 伪代码：数据结构，优先级队列，弹出具有最小代价的节点

if the queue is empty ,return false; break
Remove the node “n” with the lowest g(n) from the priority queue
Mark node “n” as expanded
If the node “n” is the goal state, return TRUE; break;
else for all unexpanded neighbors 'm' of node 'n'
	if g(m)=infinite
		g(m)=g(n)+Cnm
		push node 'm' into the queue
	if g(m) > g(n) +Cnm
		g(m)=g(n)+Cnm
	end
Cnm累积代价	

如图，排序p的g()值最小先在前排（这是容器自动进行的）

the merit : 具有完备最优解
the demerit: 穷举搜索，时间长，只能看到累积到目前为止的代价(即统一代价)，需要在每个“方向”探索下一个状态。没有目标位置的信息

A*

在迪杰斯特拉算法的基础上加上启发式函数（贪心算法）,也就是A*容器对元素的排序依赖的是f(n)=g(n)+h(n)（路程+启发距离）
h(n)：heuristic，当前节点到目标点所估计的代价，一般根据欧氏距离或者曼哈顿距离
代码中是在弹出容器是根据f(n)来决定的

a的h为5=3+2
对于A*的最优解存在需要满足估计的h小于实际的代价

how to design heuristic function

条件
h(n)<h(n)* h(n)* 表示真实的h

根据不同的情况设计一个满足条件的启发式函数。

退而求其次，如果追求速度，则使用
权重A算法，对h过高估计，影响f
选择h(n)>h(n)
Anytime A->ARA->D*

f=ag+bh
网页测试

Engineering precautions

grid map
3维:由8连通变为26(8+8+6+4)连通

priority queue in C++

• std::multimap
• std::make_heap
• std::priority_queue

Optimal heuristic function

实际往往并不是最好，即难以达到理论最优

• Is Euclidean distance (L2 norm) admissible?
  Is Manhattan distance (L1 norm) admissible?
  Is L∞ norm distance admissible?
  Is 0 distance admissible？
  此时h(n)=h*(n)
  
  Y：因为他们都不紧，这意味着谁离得近，他们衡量的是真正最短的距离

为什么会有这么多节点被扩展?
因为欧氏距离离真正的理论最优解还很远
W：使用对角距离,减少扩展点

使用Diagonal Heuristic：因为用norm做的启发函数都过分的低估了实际距离，使得搜索的区域很大，导致A*算法效率往往存在很大的差距

tie breaker

打破路径的对称性
1.修改对h放大 h=h*(1+p)
p=(最小的代价)/(路径扩展的最大代价）

2.
加入一个倾向性，终点起点连线的的路径偏移量
cross
tie Breaker 会给轨迹生成带来麻烦
也可以通过JPS

Jump Point Search（JPS）

详细解释
特性：
系统性实现tie breaker算法
core :find symmetry and break （找到对称性，并打破),choose one path
根据当前父节点和障碍物来判断是否需要扩展
如图中x的下一个节点为5号，为啥？因为上一步4到其他节点都比经过x到达要短或者相等，在straight情形下，默认直走，但是在diagonal情形下则不同。

Rule

look ahead

Neighbor Pruning
白色为自然邻点，从4出发经过x到达5（x的邻居节点），到达2或者7节点经过x的距离和不经过x的距离一样。
Forced Neighbor

红色节点为强制邻点，此时可以对角扩展，x的邻接点存在障碍物时，x的父节点是4时，从4到达3经过x节点的距离是最短的，那么3被称为x在父节点为4时的强制节点

Jumping Rules

process

• Expand horizontally and vertically.

• Both jumps end in obstacles.

• Move diagonally.
  

• Remember: you can only jump straight or diagonally;never piecewise jump

• Vertically expansion end in obstacle.

• Right-ward expansion finds a node with a
  forced neighbor.
  
  逻辑：
  若x为终点，则x为跳点
  若x存在强制邻点，则x为跳点
  斜向搜索时，x在水平和垂直上有跳点，则x是为跳点

code

example

• 3D JPS
  Planning Dynamically Feasible Trajectories for Quadrotors using Safe Flight Corridors in 3-D Complex Environments,Sikang Liu, RAL 2017

weaks

机器人的视角有限，在空旷的环境下，JPS需要时间确定jump point,效率低于A*，A*的目的性很强
大多数时间，特别是在复杂的环境中，JPS更好，但远离“总是”。为什么？ JPS 减少了 open_list中的节点数量，但增加了状态查询的数量。

HW作业

matalbA算法编写
ros 使用C++编写A算法，在rviz中显示
reference