机器人如何使用 RRT 进行路径规划？

作者：Justin Svegliato 编译：McGL

最近，我一直在和实验室的研究生一起研究移动机器人。我们通过尝试替换ROS中的一些默认包，学习了解了一个典型的机器人技术栈的各种算法。我的主要研究领域是规划和强化学习，而不是机器人学，所以学习曲线挺陡峭的。机器人需要知道如何在环境中定位自己，或者找到自己的位置，即时绘制环境地图，避开随时可能出现的障碍物，控制自己的电动机以改变速度或方向，制定解决任务的计划等等。

正如你所想到的，机器人真正重要的一环是，在环境地图已知的情况下， 规划从一个地点到另一个地点的路径的能力。它为什么要这么做？也许它必须穿过一个房间去送一些包裹，也许它必须护送一个人去某个建筑物。当机器人为了完成一项任务必须从一个起始位置到一个目标位置时，它必须为如何在周围环境中移动做出一个路径计划。在机器人技术的论文上，你经常会看到像下面这样的地图，它有一个起始位置和一个目标位置。这是移动机器人技术中的一个典型问题，我们通常称之为路径规划。换句话说，机器人如何才能找到一条从起点到目标点的路径？

在过去，我写了一些含彩色图表和冗长解释的文章。然而因为这样的帖子涉及大量的工作，我最终什么也没有发布出来。展望未来，我打算写一些简洁明了的文章，稍微粗糙一点，随意一点。为什么？好吧，这不仅让我更容易通过写文章来加强自己对一个概念的理解，而且我保证它们会像我的其他文章一样，提供有价值的信息，且没有那些花里胡哨的。言归正传......

一如既往，我们必须牢记一些微妙之处:

1. 路径规划应该在实际机器人上可行。如果路径规划需要机器人以极小角度转弯，但是机器人不能像汽车一样转动精准的角度，那么就不应该允许这个路径规划。

2. 路径规划应该尽可能接近最优解。能让机器人从一个起点到达一个目标点的路径规划，虽然找到任意一条已经不错了，但这还不够。我们想要更高效。它不仅能帮助机器人尽快完成任务，还能节省宝贵的电池电量。

3. 路径规划应避免与墙壁碰撞。这是不言而喻的。机器人可能相当昂贵，碰撞从来都不是一件好事。光是我的小机器人就花了我一千多美刀。

满足这些条件的路径规划算法，最流行之一是快速探索随机树(RRT)。一图胜千言，请看下面的图表。假定有一个简单的地图，没有任何障碍物， 机器人必须从一个起始位置(红点)到达一个目标位置(绿点)。我们从一棵树开始，它的根节点表示机器人的起始位置。之后我们将逐步把树建起来。怎么做到的？我们取一组随机的地图样本，为每个随机样本创建一个新节点，并以某种方式将每个新节点插入到树中。一旦树的节点足够接近机器人的目标位置，任务就完成了。

来源: 论文 RRT， 作者Steven LaValle

我知道这听起来很模糊，那么让我们为这个粗略的想法添加一些细节。首先，回顾一下需要发送给 RRT 的每个参数。

• 地图: 划分成障碍区域和无障碍区域的环境地图。它看起来就像我贴在上面的地图，障碍区域是紫色的，无障碍区域是白色的。
• 起始位置: 机器人在其环境中的起始位置。即地图中的红点。
• 目标区域: 机器人在其环境中的目标区域。即地图上的绿点。
• 迭代次数: RRT 执行的迭代次数。

让我们过一遍 RRT 的每个步骤。首先，初始化一棵空树。接下来，我们将表示起始位置的根节点插入到树中。这时我们就有了一棵树，它只有一个节点表示起始位置。最后，我们重复这些步骤，直到达到迭代次数或到达目标，先到为止。

1. 采样，从地图的无障碍区域采样一个随机位置。

2. 创建与随机位置相关联的节点。

3. 查找树中最接近随机位置的节点。

4. 计算一条从随机位置到节点位置的路径，这条路径在机器人上必须是可行的。

5. 继续，如果路径与某物体碰撞，则继续进行下一次迭代。

6. 插入，将与随机位置关联的节点插入到树中，与其最接近的节点作为其父节点。

7. 返回，一旦随机位置在目标位置的一定距离内，返回树。

提醒一下，如果在我们达到迭代次数之前，树还没有接近目标区域，那么我们就返回到目前为止构建好的树。

来源：Reddit

但是，一旦我们建好了树，我们怎样才能得到从起点到目标点的路径呢？我们所要做的就是从表示目标位置的节点开始，然后回溯，直到到达表示开始位置的节点。这样我们就可以知道机器人从起点到达终点的路径。很简单。

是否有可能在同一张地图中重用这棵树作为新的目标位置？当然！如果树中已经有一个接近新目标位置的节点，那么任务就达成了。然而，如果没有任何节点接近新的目标位置，我们可以继续采样，直到我们碰到一个接近它的节点。只要环境没有改变，你就可以为任何新目标位置在同一棵树上继续构建。

来源：Realtime Robotics

事不宜迟，这里有一个 RRT 算法的粗略版本！

function RRT(map, startPosition, goalRegion, numIterations):
    tree = initializeEmptyTree()
    
    insertRootNode(tree, startPosition)
    for i = 1 to numIterations:
        randomPosition = sample(map)
        randomNode = createNode(tree, randomPosition)
        nearestNode = findNearestNode(tree, randomPosition)
        path = calculatePath(nearestNode, randomNode)
        if (hasCollision(map, path)):
            continue
        
        insertNewNode(tree, nearestNode, randomNode)
        if (randomPosition is within goalRegion): 
            return tree
    return tree

还有一件重要的事情需要说明！前面，我提到我们正在寻找一条最优的在实际机器人上可行的路径，并可避障。虽然这条路径在一个真正的机器人上可行且能避障，但它是最佳的吗？不幸的是，RRT 并不能保证产生最优路径。在以后的博客文章中，我将讨论 RRT * ，一个 RRT 的改进版本，它最终会生成一个最优路径。

原文：https://towardsdatascience.com/how-does-a-robot-plan-a-path-in-its-environment-b8e9519c738b