【运动规划】BFP搜索Best-First Planner及填充势场Local minima

Robot Motion Planning: A Distributed Representation Approach & Numerical Potential Field Techniques for Robot Path Planning

续上一篇笔记【运动规划】人工势场构造扩展 & 多点人工势场组合控制高自由度机器人

---

4. 利用势场的搜索算法及其应对Local Minima的措施

4.1 Best-First Planner（BFP）-- 针对低自由度机器人

4.1.1 算法描述

作者提出，BFP即用C域势场的值作为启发式代价函数，从起点开始，不断搜索邻域，寻找邻域最低的Potential，循环搜索最终到达目标点。

每一次搜索时，需要搜索当前节点的K-neighbor，k的大小与Configuration的维数有关，即与机器人的自由度有关。

作者给出了K-neighbor的计算方式：

	1-neighbor	2-neighbor	3-neighbor
一维	2	2	2
二维	4	8	8
三维	6	18	26
…	…	…	…
n维	$2n$	$2n^2$	$3^n-1$

这里我直接理解K邻数量=该维空间内与该节点距离≤$sqrt(K)$的节点数，是符合上述公式的。

可以看到，一维的1-neighbor是搜索“左右两个点”，二维的2-neighbor是搜索附近八个点（3x3去除自己），三维的3-neighbor是搜索3x3x3立方体内去除自己后的点。对于n维来说，必须要搜索n-neighbor才能满足要求，数量的量级将很大，给搜索增加负担。所以，高DOF机器人的Configuration维数太高，不适合这种搜索算法。

4.1.2 应对Local Minima的措施

沿梯度下降搜索时，若发现被困在Local Minima时（这很好判断），作者提出，可以把Local Minima的坑“填平”（Fill up the local minima well），直到形成鞍点（saddle point，一个方向极小，另一个方向极大），这时即可从Local Minima处逃逸，等到离开了Local Minima，再次沿梯度下降方向搜索。

鞍点：

我的理解是这样的：

联系人工势场中的目标引力与重力势能，可以将这种引力抽象成一个大坑，将物体放在坑边，它自然地就会沿着坑陷入坑底（目标处），而障碍物的斥力则可以抽象成一个个小山丘，在不施加外力的情况下，物体是没有自己爬坡的趋势的。这两种结构就会引导机器人产生目标导向和自动避障的行为。


我们只希望坑只有一个最低点，这个坑底就是目标点，这是保障机器人最终抵达目标的重要标准。但是想象一种情境，在物体沿着大坑的边缘下滑的时候，它遇到了一个小坑，虽然这个小坑不是整个环境里的最低点，但是当你沿着梯度下降的趋势落到里面时，再想沿梯度下降继续向目标运动时，发现此时已经处于一个局部环境里的势场极小值了，这就是Local Minima，通常由一些U型障碍物或复杂障碍物产生。

那么，我们可以沿来时的路线做“填充”，即增大势场值。考虑这条来时的路线，边填充自己脚下的势场，它就会比上一步的势场大，这时候你可以沿着梯度下降的方向回退，实际上就是在边“垫高自己脚下的地形”边回退，那么回退到什么时候呢？当发现不用垫高自己，周围就是比自己势场更低的地方时，就完成了“逃逸”，已经把来时的路填平了，这时就可以继续沿着梯度下降搜索了。

把上面的图想象称三维的，只“垫高”了来时的路线，是一条线，而坑在其他方向上还是存在的。最终，把自己垫到了一个周围势场更低的地方，也就是你来到了一个极大值处，而从其他方向看，这个小坑还是一个极小值，这也出现了所谓的“鞍点”。

4.1.3 补充说明

作者提到，C域势场是选定了机器人的几个控制点形成的，而无法完全确保机器人整体不触碰障碍物，所以在路径规划的过程中要注意碰撞检测。

4.2 Random-Path Planner（RPP）-- 针对高自由度机器人

暂时不接触机械臂之类的机器人，这部分略过。