西安网站建设维护,做汽车导航仪在什么网站找客户,大渝网官网,wordpress标签自动生成插件下载0. Outline 1. Graph Search Basis
Configuration Space等概念 机器人配置: 指机器人位置和所有点的表示。 DOF: 指用于表示机器人配置所需的最小的实数坐标的数量n。 C-space: 包含机器人n维所有配置的空间。 在C-space中机器人的pose是一个点。 机器人在C-space中被表示为一…0. Outline 1. Graph Search Basis
Configuration Space等概念 机器人配置: 指机器人位置和所有点的表示。 DOF: 指用于表示机器人配置所需的最小的实数坐标的数量n。 C-space: 包含机器人n维所有配置的空间。 在C-space中机器人的pose是一个点。 机器人在C-space中被表示为一个点pose包含为Rt空间中的障碍物也需要映射到C-space中并且根据机器人的尺寸做膨胀该过程是one-time work可一次完成因为如果点碰不到膨胀边缘就不会碰到障碍物叫做C-obstacleS-space(C-obstacle) U (C-free)path是在C-free中从起点start到终点goal
eg如果将机器人建模为一个半径为 δ r \delta_r δr的球那么将obstacle向外膨胀 δ r \delta_r δr即可对一个点进行palnning
2. Graph and Search Method
无向图有向图加权图(权值根据具体问题定义可能是长度可能是消耗的能量可能是风险等等) 基于搜索的图有栅格本身就包含关系而基于采样的图则没有这种关系。 搜索的过程即构建搜索树的过程但该过程往往代价较大我们的目标是设计尽可能快且不失最优性的路径搜索算法。 DFS容器是栈BFS容器是队列。 图示中DFS先顺时针后逆时针访问最大的出栈入栈。 图始终BFS始终逆时针从小到大依次访问出队入队不断地一层一层地推进frontier。 BFS回溯可以找到最短路径而DFS不可以所以常用DFS。先有一个直观的印象
贪心算法 贪心算法依靠某种规则来挑选最优的节点叫做启发。启发即对于最近节点的一个猜测比如图中红色到紫色的距离有两种猜测欧式距离和曼哈顿距离。 无障碍物时贪心算法与BFS对比 贪心算法又快又准。 有障碍物时对比 贪心算法虽然快但是陷入局部最优而BFS虽然慢但是路径确实全局最优。
虽然贪心算法有时不是最优但仍然有很多我们可以借鉴的性质。 实际中需要考虑每小段路径的cost即C当每个C1时BFS最优但通常情况C不为1于是引入了Dijkstra和A*。
3. Dijkstra
算法流程可以看浙大的这节视频 dist最小可以使用priority_queue来实现按照键值来排序弹出键值最小的。 这里的priority queue就是open list已经扩展过的(已经收录的node)放在close list中(实际上不是方node本身只需使用容器管理这些node的下标)。 ProsDijkstra算法是完备且最优的完备即一定能找到解最优即解一定是最优的。 Cons由于没有goal的任何信息所以是盲目地均匀地向各个方向扩散的。 结合贪心算法和info和Dijkstra的单源带权搜索将二者的优点结合引出了A*相较于Dijkstra在g值的计算方面添加了对goal的估计h类似于DL中为loss添加正则项。
4. A*
4.1 算法介绍 A*与Dijkstra的唯一不同就是g值的计算由单纯的 g ( n ) g(n) g(n)变为 f ( n ) g ( n ) h ( n ) f(n)g(n)h(n) f(n)g(n)h(n)其中所有节点的 h ( n ) h(n) h(n)是在初始化阶段one-time计算好的如所有节点到goal的欧氏距离。 4.2 A*最优性讨论 上例说明A* 不一定是最优的 f A 7 , f G 5 f_A7,f_G5 fA7,fG5A*找到的路径为S-G
而实际cost g S G 5 , g S A G 4 g_{SG}5,g_{SAG}4 gSG5,gSAG4显然SAG最优。
A*满足最优的条件当所有节点的guess的h ≤ \leq ≤ 真实g。
用实际数据来验证 设 h s 6 h A 5 h_s6h_A5 hs6hA5则 S : f g h 0 6 6 S: fgh066 S:fgh066 A : f g h 1 5 6 A: fgh156 A:fgh156 G : f g h 5 0 5 G: fgh505 G:fgh505 输出S-G非最优。 设 h s 5 h A 4 h_s5h_A4 hs5hA4则 S : f g h 0 5 5 S: fgh055 S:fgh055 A : f g h 1 4 5 A: fgh145 A:fgh145 G : f g h 5 0 5 G: fgh505 G:fgh505 输出方式涉及到路径对称性问题在5.3节tie breaker中会详细讨论。 设 h s 5 h A 3 h_s5h_A3 hs5hA3均满足小于等于actual g g g则 S : f g h 0 5 5 S: fgh055 S:fgh055 A : f g h 1 3 4 A: fgh134 A:fgh134 G : f g h 5 0 5 G: fgh505 G:fgh505 pop A; pop G; 输出S-A-G最优。
于是重点就转移到了如何设计heuristic guess函数h 启发式函数需要满足 h ( n ) ≤ h ∗ ( n ) h(n)\leq h^*(n) h(n)≤h∗(n) 我们称这种启发式函数为admissible(可接受的、可容许的)的。 admissible heuristic function的本质为向量的范数以下为一些admissible heuristic function的列举
欧式距离向量2范数 ∣ ∣ x ∣ ∣ 2 ∑ i 1 n ∣ x i ∣ 2 ||x||_2\sqrt{\sum\limits_{i1}^{n} |x_i|^2} ∣∣x∣∣2i1∑n∣xi∣2 always admissible曼哈顿距离向量1范数 ∣ ∣ ∣ x ∣ ∣ 1 ∑ i 1 n ∣ x i ∣ |||x||_1\sum\limits_{i1}^{n} |x_i| ∣∣∣x∣∣1i1∑n∣xi∣Depends admissible向量的无穷范数 ∣ ∣ x ∣ ∣ ∞ m a x i ( x i ) ||x||_{\infty}\mathop{max}\limits_{i} (x_i) ∣∣x∣∣∞imax(xi)always admissibleOctile distance: 基于8个方向的直线移动和对角线移动的最短路径进行计算。具体而言Octile距离考虑了水平、垂直和对角线方向的移动。在直角移动水平或垂直时距离为1在对角线移动时距离为根号2即1.414。这样设计是为了模拟在规定的移动方向上的最短路径。在路径规划算法如A*算法中Octile距离经常用于启发式估计以帮助算法找到最优路径。 上例说明A*有了贪心算法的对于目标的引导性。
4.3 A*的改进
由 f f f中引入 h h h的结果可知h的引入也引入了贪心算法的特性强目的性faster。如果对h赋予权值则可以改变该引入特性的比重于是出现了weighted A*给h增减权重 ϵ ( ϵ 0 ) \epsilon(\epsilon0) ϵ(ϵ0)即 f g ϵ h fg\epsilon h fgϵh
当 ϵ 1 \epsilon1 ϵ1时贪心占比大算法强suboptimalfaster ϵ ∞ \epsilon\infty ϵ∞时忽略 g g g算法退化为贪心算法当 ϵ 1 \epsilon1 ϵ1时为正常A*solution suboptimal当 0 ϵ 1 0\epsilon1 0ϵ1时Dijkstra占比大更接近optimalslower极端情况 ϵ 0 \epsilon0 ϵ0退化为Dijkstra算法solution optimal
weighted A*改进的点 ϵ 1 \epsilon1 ϵ1实际上是使用solution的optimality来换取speed速度提升是A*的数量级等级(几十倍等级别)学界已证明weighted A*的 ϵ \epsilon ϵ-suboptimal即 c o s t ( s o l u t i o n ) ≤ ϵ ∗ c o s t ( o p t i m a l c o s t ) cost(solution)\leq\epsilon*cost(optimal \ cost) cost(solution)≤ϵ∗cost(optimal cost) 还有weight A的改进Anytime AARA*D*等研究本课程不做介绍可自行拓展。
更改权值的对比 该网页可以调参进行对比 http://qiao.github.io/PathFinding.js/visual/
以下是我的实验结果
AlgorithmLengthTime(ms)OperationsBest-First-Search(a0,b1)29.561200340weightedA*(a1,b5)27.0717000370weighted A*(a1,b1)26.2413000495Dijkstra(a1,b0)26.24420002670
可以看出贪心算法operation最少耗时最短但solution为suboptimal(path较长)Weight A*(a1,b1)和Dijkstra的solution为optimal但Weight A*(a1,b1)明显耗时更少此实验中耗时提升了2.23倍大规模问题上提升应该会更大。 5. Engineering considerations
工程实现对于算法性能影响可能有10倍100倍都不止。
5.1 地图表示
以Occupancy grid map为例 2Dgrid-map的表示8连通( 2 3 − 1 2^3-1 23−1)3D为26连通( 3 3 − 1 3^3-1 33−1)
数据结构在搜索过程中其实就是在找到该节点的相邻节点时需要用到。 推荐使用stl中的priority_queue和multimap对vector排序可以使用make_heap方法。(VINS-MONO中用unordered_map较多)。
5.2 the best heuristic
前面降到可以使用Euclidean distance作为heuristic尽管Euclidean distance满足 h ( n ) ≤ h ∗ ( n ) h(n)\leq h^*(n) h(n)≤h∗(n)即可找到optimal solution但是在搜索过程中会扩展很多不必要的节点 h ( n ) ≤ h ∗ ( n ) h(n)\leq h^*(n) h(n)≤h∗(n)小于的非常多即Euclidean distance不够tight 对于结构化的地图和结构化的移动rule在2D情况下可以推导出任何一个节点到终点的heuristic的estimate可直接用作我们的heuristic我们称之为Diagonal Heuristic对角Heuristic TODO这个2D Heuristic estimate结果怎么推出来的
上例中DIagonal Heuristic和L2 Heuristic的solution path的cost即f相同即出现了路径对称性问题因为在计算f时他们之间没有difference于是引入了Tie breaker。
5.3 Tie breaker
直观理解是打破等号。
Tie breaker用于打破5.2上述的对称性问题核心思想是 在cost相同的path中找到preference。
目的是减少不必要的node的拓展提高搜索效率。 方法1在计算Heuristic时加入一个系数 p 一步的最小 c o s t 可能的 p a t h 的最大 c o s t ( 比如矩形地图的斜对角线 c o s t ) p\frac{一步的最小cost}{可能的path的最大cost(比如矩形地图的斜对角线cost)} p可能的path的最大cost(比如矩形地图的斜对角线cost)一步的最小cost这里还不是很理解为什么加入一个 p p p就能使f不同需要结合代码具体理解。 这样会轻微打破admissible条件因为h有小幅的增大admissible heuristic取等号时是在没有任何障碍物情况下而实际工程中由于存在较多obstacle计算出来的h通常不会达到tight heuristic。 方法2定义rule当f相同时取h更小的。 方法3在f或者g上加一个deterministic random numbers(提前确定好的随机数确定之后不能变使用hash映射)工程实现可能较复杂但效果会比前面的好。 方法4倾向于走直线同样是slightly change heuristic给heuristic加上一个corss让其更倾向走符合直观感受的对角线path。
还有很多其他方法可以看论文。
但是Tie breaker也会带来一些负面影响我们最终给到robotic的是一条可执行的轨迹trajectorytrajectory是由我们的规划出的path来生成的会涉及到traj光滑的处理如下图当我们使用Tie breaker在有obsacle的情况下生成了一条path理想的trajectory是如图所示的红色光滑曲线使用tie breaker生成了虚线所示的path在trajectory generation过程中就比较难得到红色光滑traj反而是矩形的path更易得到光滑traj所以使用什么tie breaker是有讲究的针对这种情况引入了本章的第3块内容JPS。
6. JPS(Jump Point Search)
JPS是一种系统性的打破路径平衡性的图搜索方法其core idea是找到对称性并打破它。
6.1 look ahead rule(pruning rule) Look ahead rule以例子来介绍这样一个规则
对于无障碍物straight到达儿子节点的情况
从某点(4)到达另一点(1)不需要经过儿子节点(x)时的path cost为 f A f_A fA经过儿子节点的path cost为 f B f_B fB则 - 当 f B f A f_{B}f_{A} fBfA时才考虑从儿子节点到达该节点称该点为natural neighbors - 当 f B ≥ f A f_{B}\geq f_{A} fB≥fA不考虑从x到达该节点(即neighbor pruningdiscard)称该节点为inferior neighbors(劣性节点)。
按照上述定义 ( f B f 4 → x → 1 1 2 ) ( f A f 4 → 1 1 ) (f_{B}f_{4\to x \to 1}1\sqrt{2}) (f_A f_{4\to1}1) (fBf4→x→112 )(fAf4→11) 所以扩展儿子节点 x x x时不考虑节点1
从4走到3是否需要经过x ( f B f 4 → x → 3 1 2 ) ( f A f 4 → 2 → 3 2 1 ) (f_{B}f_{4\to x \to 3}1\sqrt{2}) (f_A f_{4\to2\to3}\sqrt{2}1) (fBf4→x→312 )(fAf4→2→32 1)相等所以不考虑节点3
同理123678均不需要考虑只需要考虑5。
对于无障碍物diagonal对角线到达儿子节点的情况rule稍微有些改动
当 f B f A f_{B}f_{A} fBfA时consider当 f B f A f_{B}f_{A} fBfA时discard
eg1到达1 ( f A f 6 → 4 → 1 2 ) ( f B f 6 → x → 1 2 2 ) (f_Af_{6\to4\to1}2)(f_Bf_{6\to x \to 1}2\sqrt{2}) (fAf6→4→12)(fBf6→x→122 )故discard 1
eg2到达2 ( f A f 6 → 4 → 2 2 1 ) ( f B f 6 → x → 2 2 1 ) (f_Af_{6\to4\to2}\sqrt{2}1)(f_Bf_{6\to x \to 2}\sqrt{2}1) (fAf6→4→22 1)(fBf6→x→22 1) 相等diagonal相等时需要consider详细参考论文 http://users.cecs.anu.edu.au/~dharabor/data/papers/harabor-grastien-aaai11.pdf Equation 1/2 有障碍物straight时3号节点必须被考虑称为forced neighbors有障碍物diagonal时1号节点必须被考虑。
以上有/无障碍物的straight和diagonal情况可以涵盖所有情况向上下左右向左上左下右上右下均类似。
6.2 Jumping rule 为方便说明将look ahead rule标记为①~④jumping的原则是先递归straight jump再递归diagonal jump发现force neighbor则会递归直线recall最上面一个node不允许折线recall 例子1 从 p ( x ) p(x) p(x)出发进行JPS扩展
起始点使用④绿框部分需要被considerw为force beighbor 在x处向上向右运用①最终都碰到障碍物边界也算障碍物jump失败直到递归diagonal jump到y时对y向右递归straight jump到z触发了③绿框蓝框是consider蓝框为force neighbor故z为特殊节点jump成功递归返回到最上层发现z的节点即y将y加入到open list 递归返回到x结合第1步的图x节点的上、右递归straight jump以及右上递归diagonal结束该右下的force neighbor的递归diagonal jumpw天然是x的force neighbor所以将w加入到open list中。
至此完成了对x节点的所有JPS扩展将x节点加入到close list中。 例子2 水平竖直jump无事发生对角线跳一步 向右jump时发现了特殊于是直线recall不允许折线recall将蓝框的点加入到open list绿色的起点即可加入close list退出历史舞台。 例子3 注意当在扩展时发现了goal视为发现了force neighbor要recall并加入node到open list
最终path 例子4
6.3 Extension
3D JPS本质上和2D JPS相同只是3D rules更复杂一些Sikang之前在深蓝讲过公开课可以看看。
大多数情况下JPS比A* 更优因为JPS能够在复杂环境中迅速找到一个个jump point省去了中间不必要的加入openlist的搜索过程。
但是在机器人的FOV内看不到障碍物后的点当存在大量空旷场景时JPS为了搜索jump point会不断地collision query虽然对于open list的操作变少了但是大量的collision query耗时会增加可能不一定比A*更优。
我的实验结果 这种情况下明显A* 更优。
6.4 JPS summary JPS小结
look ahead rule遵照①~④进行拓展举一反三jumping rule 先水平、竖直straight扩展再diagonal扩展遇obstacle或map border都视为该方向上扩展失败直至发现满足look ahead rule中的force neighbor直线recall到jump的起始节点(不允许折线recall)加入到open list中当所有方向都扩展完毕之后将source node加入close list中。当在扩展时发现了goal视为发现了force neighbor要recall并加入node到open list A* 与JPS的唯一区别在于如何选取neighbors前者选取几何neighbors后者根据上述2条rules选取neighbors故用在A*上的trick均可用于JPS(diagonal heuristicTie breaker等)JPS 和A* 都依赖的一个点是force point一定是最优路径需要经过的点即jump point将起点到终点过程中的所有jump points连接起来就得到了最优path只不过JPS通过他的规则过滤掉了很多不需要拓展的点而A* 是所有几何neighbors都拓展。JPS的jump point和外层循环所维护的最小cost是不一样的前者是JPS’s rules后者是JPS和A* 的算法框架。在复杂环境中JPS通常博A* 更优但远不是always更优JPS降低了对于open list的操作修改键值排序等但增加了status query的次数JPS的局限性仅仅在uniform grid map中才能使用(11 2 \sqrt2 2 这类几何特征的grid map)对于更general的图搜索问题如带权值的非grid map,A* 可以用而JPS无法使用。
7. 论文推荐
http://users.cecs.anu.edu.au/~dharabor/data/papers/harabor-grastien-aaai11.pdfPlanning Dynamically Feasible Trajectories for Quadrotors using Safe Flight Corridors in 3-D Complex Environments, Sikang Liu, RAL 2017
