作者
下面这篇文章可以说是我之前的一篇文章的延续。 文章在那里你可以熟悉RRT算法的概念和在C++中的实现。
这篇文章扩展了之前的讨论，并介绍了如何在三维空间中进行机器人路径规划（工业机器人、无人机）。正如你所认为的，以前的文章和RRT算法的软件实现主要用于在二维空间运行的机器人（例如移动机器人）。
这篇文章的源代码可以在我的Github上找到。
正如在之前的相关文章中所展示的那样，为了实现结果的可视化，我使用了matplotlib库。
C++代码你可以按以下方式编译和运行。
头文件（用于绘图库）必须和你的cpp放在同一个文件夹中。
//compile
//run
//folder tree
这篇文章没有讨论理论方法，因为RRT算法的原理已经在前面展示过了。
一般机器人运动控制系统
关于机器人运动控制的一般概述，特别是本文中的RRT算法的 "逻辑位置"，如下图所示。
请注意，执行重复性工作的工业机器人（根据程序）不包括执行RRT、A*、Dijkstra或其他路径规划算法的 "模块"。
路径是根据任务规范离线编程的。
作者
当机器人可以改变路径时，因为机器人环境可能是动态的（可改变的），并且可能出现障碍物，机器人运动控制系统可能包括带有所述路径规划算法的路径规划器。
在这种情况下，只要前一个路径不能被利用（被障碍物阻挡），路径规划器算法就会搜索替代路径。
当机器人自由路径被计算和平滑后，基于现有机器人模型（运动学和动力学）的轨迹插值器 计算出机器人控制器的参考值。你可以把插值器想象成一个轨迹优化器，它考虑到计算的自由路径、编程的速度，并将它们与实际的机器人性能（可用的扭矩、机器人结构约束、机器人尺寸等）联系起来。 来自机器人的反馈，如来自编码器、加速度计或力传感器的位置信号，被用于整个系统的控制环。
本文讨论的RRT算法可以（实时）使用来自三维摄像机、激光雷达或其他信号（用于障碍物检测）。
请注意，上述描述只是对真正的机器人运动控制系统的简化，它通常是针对机器人和应用的。
三维RRT方法
在本节中，我将描述二维和三维RRT算法的主要实施差异。
你可以假设三维需要考虑额外的维度，所以节点结构之后的RRT类已经扩展到Z方向的维度。这里我们需要注意的是，对三维空间的扩展需要更多的计算能力（这里是CPU），因为预期空间更大。
根据所应用的障碍物、三维空间的大小、起点和终点（目标），算法的迭代次数必须延长。讨论算法随机地产生新的节点，因此可能发生的情况是，在正确定义的初始条件下，RRT算法在寻找自由道路（起点-目标）方面 "令人失望"。
由于目前的RRT算法是在三维空间中运行的，所以在计算当前节点和随机生成的节点之间的距离时也要考虑Z空间（欧几里得距离）。我们还需要应用额外的三维线方程，因为我们必须验证新的随机生成的节点是否超出了最大的允许扩展距离，如果是的话，最终要减少距离。
三维线的方程可以被指定如下。
使用上述公式（查看我的代码），我们可以绘制示例线。
作者
正如我上面提到的，三维线在三维空间中穿过两个点的方程式是必要的，因为我们正在寻找以前的节点，它位于离随机生成的新节点最近的地方（我们正在寻找新生成的节点和树的现有节点之间的最小距离）。
正如你所记得的，最小距离（现有的树节点-新的随机生成的节点）与我们可以扩展的最大距离相比。如果所考虑的节点之间的计算距离超过了允许的消耗距离，新的节点就会被移到距离条件得到满足的位置（在三维空间）。
在这里，三维线条方程开始发挥作用。方程为计算节点的新位置和确保扩展距离条件提供了极好的机会。你可以假设最后一步是验证新节点（通往新节点的线）是否与障碍物发生碰撞。如果是这样，随机的节点就会被牺牲掉。
3D RRT的C++实现，你可以在我的Github上找到。下面我描述了三维空间中两个定义目标的预期测试结果。
请注意，我无法生成3D形状（障碍物），所以障碍物只在2D空间中标出。
作者
作者
谢谢您的阅读。
机器人路径规划器的3D RRT算法（C++）》最初发表在《极客文化》杂志上，人们在这里通过强调和回应这个故事来继续对话。
