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