、目标点 
和多边形障碍物的各顶点(设 
是所有障碍物的顶点构成的集合)进行组合连接，要求起始点和障碍物各顶点之间、目标点和障碍物各顶点之间以及各障碍物顶点与顶点之间的连线均不能穿越障碍物，即直线是“可视的”。给图中的边赋权值，构造可见图 
。其中点集 
， 
为所有弧段即可见边的集合。然后釆用某种优化算法搜索从起始点 到目标点 的最优路径，那么根据累加和比较这些直线的距离就可以获得从起始点到目标点的最短路径。
。
、
分别是障碍物 
、
的顶点，但 与 连线与障碍物其它顶点连线相交，故 、 之间不可见；而实线所示的 
与 
连线不与障碍物其它顶点连线相交，故 、 之间可见。
，遍历当前的快速随机搜索树T，找到T上距离 最近的节点 
，考虑机器人的动力学约束从控制输入集 
中选择输入 
，从状态 开始作用，经过一个控制周期 
到达新的状态 
。满足 与 的控制输入 
为最佳控制量。将新状态 添加到快速随机搜索树T中。按照这样得到方法不断产生新状态，直到到达目标状态G。完成搜索树构建后，从目标点开始，逐次找到父节点直到到达初始状态，即搜索树的根节点。
代表随机树中两个位姿节点间的路径代价， 
代表随机树中两个位姿节点间的欧几里德距离。类似于A*算法，本算法为随机树中每个节点定义一个估价函数： 
。其中 
是随机节点 到树中节点 
所需的路径代价。 
为启发估价函数，这里取随机节点 到目标点 
的距离为估价值， 
。
表示从节点 经随机节点 到目标节点 的路径估计值。遍历滚动窗口内随机树T，取估价函数最小值的节点 ，有 
。这使得随机树沿着到目标节点估价值 最小的方向进行扩展。 总是选择离目标最近的节点，这可能会使随机树遇到局部极小值问题。因此随机树生长的新节点 必须要克服这个问题，引导随机树更好的探索未知空间。 与其父节点 的距离小于 与扩展树上其他任意节点的距离，即 
，则选择该节点为随机树新生节点。下图解释了新节点的判断过程。
； 距离最近的节点 ； ，使机器人状态由 到 ； 是否符合回归分析，不符合则回到第4步； 作为随机树T的一个新节点， 则被记录在连接节点 和 的边上。
。 是当前滚动窗口中的子树中估价函数最小的点。确定子目标后，机器人前进到子目标点，进行下一轮的滚动RRT规划。如此反复，直到到达目标点G。
是目标产生的引力势场； 
是各个障碍物产生的斥力势场之和，即： 
。
表示引力势的相对影响； 
表示第 
个障碍物的斥力势的相对影响， 表示机器人当前位置， 表示目标点位置， 
表示机器人距目标的距离， 
的作用是在机器人距离目标较远时，削弱目标引力势的作用， 
表示机器人距离第 个障碍物的距离， 
表示第 个障碍物的斥力势作用范围。 和 对势场形状的影响很大，适当的增大 能够增强引力势场的作用，有助于减少产生局部极小点的可能，并加快机器人向目标运动。 影响机器人在障碍物附近的运动特性， 比较大可以使机器人距离障碍物更远，运动路径更安全； 比较小，机器人在避开障碍物时运动比较平滑。
是一个很小的正数，其含义是机器人受到的虚拟合力接近0。这是最直接局部极小点判断方法。条件2中 
为0,1之间某一正数， 
为机器人运动过程中某一状态， 
表示机器人从 到达当前位置 的总路程，条件2成立意味着机器人在运动很长路程后，位移很小。用来检测机器人在局部极小点附近发生的振动和圆周运动。
和 
表示；机器人在 
时刻的位置表示为 
； 
表示连接机器人位置 和目标点的直线。
。若没有探测到障碍物，那么机器人就沿着 向目标直行，直到到达目标点或者遇到障碍物。当遇到障碍物时，记下当前位置 
。然后机器人环绕障碍物直到又一次到达 ，找到环绕路线上距离目标最近的点 
，并沿着障碍物边界移动到该点。 更新，机器人继续沿直线向目标运动。如果沿这条直线运动时还会遇到该障碍物，那么机器人不能到达目标点。否则算法不断循环直到机器人到达目标点或者规划器认为机器人无法到达目标点。
是连接初始点和目标点的直线，在计算过程中保持不变。当机器人在点遇到障碍物时，机器人开始绕行障碍物，如果机器人在绕行过程中在距离目标更近的点再次遇到直线 ，那么就停止绕行，继续沿着直线 向目标直行。如此循环，直到机器人到达目标点 。如果机器人在绕行过程中未遇到直线 上与目标更近的 点而回到了 点，那么得出结论，机器人不能到达目标。
是机器人位置 和传感器扫描角度 
的函数，具体点说， 是沿着 的射线以角度 到达最近障碍物的距离，
。 ， 
被传感器视野内的障碍物分割成多个连续区间。如下图所示。 出现不连续或者到达传感器最大测量范围的角度就是这些连续区间的端点。TangentBUG算法利用者些区间的端点避开工作空间中的障碍物。
不连续的情况做出说明（如图17所示）：点 ， ， 
， 
， 
， 
， 
和 
与障碍物的不连续性相关；请注意，这里的射线与障碍物相切。点 是不连续的，因为障碍物边界落在传感器的范围之外。 和 ， 和 ， 和 ， 和 之间的free space边界上的点集是连续性的间隔（加粗线部分）。
，障碍物 在传感器视野内，但是不阻碍机器人的运动。机器人在刚刚探测到障碍物时，传感器视野圆与障碍物边界相切。随着机器人继续向前移动，这个切点分裂成两个交点 和 （ 和 ）， 和 之间的障碍物边界区间与机器人运动直线相交。
。在没有关于障碍物的其他信息时，可以将探索距离 定义为从 经过一个交点到目标点的折线长度，即： 
。
，并选择向 移动，因为 
。 时，它无法知道 阻挡了从 到目标 的路径。 但目标 不同时，它具有足够的传感器信息来得出结论： 确实阻挡了从 到目标 的路径，从而朝 行驶。 行驶刚开始的时候可能使得 
最小化，而不是向 行驶，但是planner有效地为 
分配无限大的成本代价，因为它有足够的信息来推断任何通过 的路径都不是最理想的。
时，机器人还没有探测到障碍物，因而它向目标作直线运动；当 
时，机器人开始探测到障碍物，并朝向障碍物探索距离近的一侧运动；当 
和 
时，机器人继续移动，同时更新探测区域，在这个过程中探索距离不断减小。
的最近距离 
。机器人按照原来的方向环绕障碍物运动，同时机器人更新当前探测的障碍物边界与目标的最近距离 
。当发现 
时，机器人停止障碍物环绕行为，继续向目标运动。
点后，探索距离就不再减小，即 点是机器人探索距离在障碍物边界上的局部极小点。机器人开始沿着障碍物边界进行环绕，图中虚线路径就是机器人环绕障碍物时所走的路径。当机器人探测到与目标距离相比 点更近的点时，重新开始接近目标的运动。转载自知乎@搬砖的旺财，文中观点仅供分享交流，不代表本公众号立场，如涉及版权等问题，请您告知，我们将及时处理。
-- END --
