从系列中:自主导航
这个视频提供了一些关于姿态图优化的直观感受,这是一个解决自主导航中同步定位和绘图(SLAM)问题的流行框架。
我们将讨论为什么车辆传感器和状态估计的不确定性使得构建环境地图变得困难,以及姿态图优化如何处理它。我们还将简要介绍作为表示环境模型的一种方法的占用网格地图。
在上一个视频中,我们讨论了如果我们已经有了一个模型或环境地图,我们如何估计一辆自动驾驶汽车的姿势,也就是它的位置和方向。在本视频中,我们不会提前绘制地图。我们必须建立一个,同时使用一个称为SLAM的过程计算出车辆在地图中的位置;同时定位和映射。
SLAM算法有很多,但主要可以分为两类;过滤和平滑。滤波,如扩展卡尔曼滤波或粒子滤波,将问题建模为在线状态估计,机器人状态(可能是环境的一部分)随着新测量的可用而不断更新。
另一方面,平滑技术通过一整套测量来估计整个机器人轨迹,而不仅仅是新的测量。如果我们使用基于图的公式来处理这些方法,那么理解这些方法会更加直观。事实上,最近几年,一个特定的框架,姿态图优化(或者更一般的,因子图优化)已经成为大多数现代SLAM软件解决方案(如g2o或GTSAM)事实上的标准。万博 尤文图斯在这个视频中,我们将专注于理解什么是姿态图优化以及它为什么有效。所以,我希望你能留下来。我是Brian,欢迎来到MATLAB技术讲座。
让我们来解决这个问题。我们有一个自动驾驶车辆,一个机器人,它能够在环境中移动。我们给它激光雷达来感应到附近障碍物的距离和角度,我们给它一种通过里程计来推算其相对位置的方法。在这种情况下,它使用车轮编码器来计算障碍物的数量每个车轮在行驶过程中都会产生旋转,并由此估算出自上一个已知位置以来,它已经走了多远以及转动了多少。
所以,问题是,我们如何使用这些传感器来理解环境地图的样子,并计算出机器人随时间的姿势?好吧,让我们从一个简单的地图问题开始。
这里,我把机器人放在一个长方形的房间里,角落里有一个圆形的障碍物。我们可以看到这张地图,看看机器人在哪里,但它什么都不知道。对它来说,它只是一个未知的空间。让我们从一个非常理想的情况开始,在这种情况下激光雷达或里程计测量没有不确定性或误差,它们非常完美。在这种情况下,开发环境地图是相对简单的。机器人可以进行激光雷达测量,我们有信心它测量的是真正的障碍物位置。它可以在全球地图上保存下来,然后继续前进。在行驶了一段距离之后,由于完美的里程表,我们可以精确地知道,它需要进行另一次测量。这两种测量结果会在全球地图上很好地对齐,因为我们的系统中没有错误,通过这种方式,我们可以在周围环境中驾驶,创建一个完美的地图,同时知道我们在任何时候都在哪里。
当然,这种情况是不现实的。激光雷达测量和距离测量都存在误差,因此机器人的姿态估计和到被测障碍物的距离存在一定的不确定性。那么,让我们再试一次绘制这个房间的地图,但是这次我们说激光雷达仍然是相当准确的,但是里程表测量中有很大的不确定性。也许其中一个轮子一直在滑动,所以机器人认为它在朝不同的方向移动。
那么,让我们看看这对我们的机器人是如何起作用的。我们得到了一个初始的激光雷达测量,我们假设那里有一个障碍物,就像以前一样,然后我们开了一点车。不过,这次估计的姿势与实际姿势不同。因此,激光雷达返回了一些真实机器人看到的墙壁的相对距离,但我们只能假设它是相对于估计的机器人姿势的,因为这是我们所知道的全部。
这将两个可测量的障碍放在不同的框架中,而不是在全球环境框架中,所以它们不会对齐。如果我们继续这个过程当我们驾驶机器人在房间里转的时候,测程的误差会导致我们估计的姿势与真实的姿势有更大的偏差最后我们得到的是,这个障碍物的地图它与真实的环境不一样。所以,这就是为什么我们不能仅仅测量环境并把结果贴在地图上的原因之一。系统中的不确定性会把事情搞砸。但这正是SLAM算法可以帮助我们的地方。
现在,有几种不同的方法来解决SLAM问题。正如我在开始时所说,在本视频中,我们将重点关注姿势图优化。我想提供一点算法背后的直觉,让你在没有一堆数学知识的情况下感受它是如何工作的,但是如果你想了解更多关于数学的知识,我在详细介绍中留下了一些很好的资源。
就像以前一样,我们有真实的和估计的机器人姿势,这些姿势一开始就在真实环境地图中相互重叠。但是现在,在右边,还有一个空白区域。这就是我们要建立姿势图的地方。
我们的机器人做的第一件事是测量环境。这个测量与当前估计的机器人姿态相关联,我们可以将两者添加到姿态图中。本质上,我们所做的是说这个姿势被定义为一个X和Y位置和一个旋转角度,同时我们保存了距离和角度到感知到的障碍物。现在,这个姿势入口也有不确定性,但我马上就会讲到。
好的,我们在post图中有了第一个条目,让我们看第二个。再次,机器人行驶了一段时间,我们估计的姿势开始偏离真实姿势。环境的另一个度量,与新估计的姿势关联,此组合现在保存在姿势图中。
因此,我们有两个姿势,每个姿势都有自己对障碍物位置的局部估计。即使我们不知道这两种姿势在环境中的位置,我们也知道它们之间的距离。在我们的例子中,这来自于计算车轮的旋转,但是这个SLAM算法背后的想法并不与特定的一组传感器有关。相对姿势距离可能来自另一个内部测量源,如IMU,或者我们可以计算出机器人从其他外部源(如GPS或可见里程计)移动的距离。关键是我们对这两种姿势之间的距离有一些最好的猜测,以及我们对这种猜测的信心。
这样,我们就可以对这两个姿势之间的相对距离设置约束。理想情况下,它们会保持如此远的距离,因为这是我们最好的估计,但由于里程测量过程中的不确定性,如果我们将这两个姿势相对移动,可能会更好。例如,如果有一个机制能够移动两个姿势来对齐当前未对齐的障碍物,那就太好了。嗯,有!约束是实现这一目标的关键。
现在,为了可视化一个约束,我认为想象一个连接两个姿势的橡胶条是很有帮助的。钢筋的标称长度是我们估计它们之间的距离。在这些姿势上没有外力的情况下,杆只会将它们保持在此固定距离。但是,如果我固定一个姿势,并将另一个姿势移近或移远,它会压缩或拉伸橡皮筋,并且会产生一种力,希望将姿势恢复到其标称距离。橡胶条的强度取决于我们对距离估计的信心。如果我们有更多的信心,或者我们有一个非常好的里程测量过程,那么这是一个非常强的条,使得很难改变这个标称距离。如果我们几乎没有信心,那么这根橡皮筋非常脆弱,几乎没有恢复力。
所以,在姿势图术语中,我们可以说姿势是图形的节点,约束,或者说橡皮筋,是一条边。当然,该约束在所有三个姿势维度(X和Y)以及旋转中都起作用,始终试图将其恢复到其估计距离。好的,我们不能用两个节点和一条边做很多事情,所以让我们继续。
就像之前一样,机器人继续行驶并测量环境。在测量之后,我们可以得到估计的姿势,以及它对局部障碍的测量,并将它放在我们的姿势图中,并添加一个约束。现在我们有三个节点和两条边。我们可以继续这样做,一次一个姿势填满我们的姿势图直到我们有一些看起来像我们之前建立的不正确的地图,除了有约束连接所有的姿势。
现在,我们仍然无法利用这些信息,因为你可以想象所有的酒吧都处于其标称长度,所有的东西都想保持在原来的位置。然而,我们正处在一个有趣的时刻。我们的第一个姿势和当前姿势都在观察环境中的相同特征。这意味着我们可以在这两个节点之间建立一条新的边,一个新的约束。我们只需要了解这两个特征是如何对齐的,以确定这两个姿势之间的相对位置。在这个例子中,它们必须在完全相同的位置。
所以,现在我们可以添加一个新的约束来连接第一个和最后一个姿态并结束循环。这个杆的名义长度为0,因为这两个姿势想要在彼此的正上方。这根橡皮棒的强度取决于你对外部测量的信心以及你将两组数据关联为同一特征的能力。如果你真的很自信,就像我们在这个例子中所做的,这个杆非常有力,它真的想把这两个姿势放在一起。
所以,希望你能开始看到一个循环闭包是使所有这些工作的东西。没有一个结束语,所有的约束都得到了满足。但是有了闭环,我们就有办法给这个图注入张力。蓝色条想把这两个节点拉到一起紫色条都想保持相对距离不变。让这个图达到平衡,或者平衡每个约束施加的所有力是姿态图优化的优化部分。
最酷的是,通过优化姿势图,我们不仅可以更好地估计当前姿势和环境模型,还可以更好地估计机器人在过去的位置,因为所有过去的姿势都被更新了。因此,我们从这个单循环闭包中获得了很多价值。当然,我们没有一个完美的环境模型,也没有机器人的姿势,但这没关系,因为我们可以通过做更多的循环闭包来继续改进我们的估计。
机器人可以继续驾驶,通过里程计添加新的姿势,并在能够使用外部数据确定关系时关闭新的回路。在本例中,假设我们能够将这三个姿势联系在一起,即使房间的这一区域相对没有特色。
在这一点上,之前的所有约束都与之前相同,并且直到我们添加这些循环闭包,图形才处于平衡状态。但是现在我们已经添加了新的循环闭包,这就产生了更多的张力,需要在所有姿势上进行更多的拖拉和推拉,因此我们需要再次运行优化,并允许图形重新调整。
通过这种方式,您可以看到在每次循环结束后,图形将如何继续改进其环境和过去机器人姿势的全局模型。
也就是说,只要你连接的两个姿势确实是在观察相同的环境特征。如果你建立了一个错误的关联,并且你建立了一条带有不真实约束的边,你可以想象它将如何在你的图中持续存在,并在每次你运行它的时候继续扭曲你的优化过程。它总是把这些节点拉向远离真理的方向。因此,对你所做的任何循环闭包都有信心是非常重要的。事实上,错过一个真正的循环闭包要比添加一个假的好。你总是可以再开几次车,然后在未来再做一次了结,一旦坏的了结了,就很难再移除了。
现在,重要的是要注意,环路闭合需要对环境进行绝对测量,就像我们使用激光雷达,或者你可以使用可视摄像头,或者其他环境传感器。你不能使用像IMU或车轮编码器这样的相对传感器来关闭循环,因为没有办法将这些信息与旧的姿势联系起来。您可能认为您已经接近过去的节点,但如果不检查环境,您就无法确定。
好的,我们有我们的姿势图,我们已经对它进行了优化,现在我们如何从所有这些数据点构建环境地图?嗯,有几种不同的方法来表示环境模型,但我将快速介绍二进制占用网格。其背后的想法是,环境被分解成一个由细胞组成的网格。如果您相信它占用的单元格,则将该值设置为1,而不占用则将其设置为0。这里我假设整个环境是空的,填充我认为有障碍的单元格。你也可以假设相反的情况,当你的传感器显示那里什么都没有时,你把所有的电池都占满了,然后把它们设为零。你们可以看到,仅用1和0的I,我们就生成了一个相当精确的矩形房间和部分圆形障碍物的模型。有一些漏洞,我们可以通过多开几圈来填补,但总体上看起来还可以。
现在,也有概率占用网格,其中单元格不必完全占用或不占用,相反,它占用的概率在0到1之间。使用此模型,您对黑白单元格充满信心,其他任何地方都有灰色阴影,这取决于您的不确定性。
我们有了,使用姿势图优化的SLAM。希望现在您可以看到环境地图和机器人的姿态是如何通过这种方法同时确定的。现在我们有了一个居住网格地图,我们可以开始规划这个环境的未来轨迹。机器人不再需要漫无目的地四处游荡,而是可以利用这张地图来规划它想去的地方。下节课我们会讲到规划的一些方法。
但是,在我结束这个之前,我想提醒你们,学习这些东西的一个很好的方法就是练习并自己尝试。有一个MATLAB的例子,使用导航工具箱称为实现SLAM与激光雷达扫描,建立一个占用网格地图的环境只使用激光雷达,不需要相对里程计过程。从本质上讲,环境具有足够的独特特征,因此激光雷达测量的每一个姿势都可以通过数据关联或特征匹配与其他过去的姿势相关联。这是值得一试的。如果你想了解更多关于这个例子的信息,我已经链接到下面的另一个视频了。
好了,这节课就讲到这里。如果你不想错过任何未来的Tech Talk视频,不要忘记订阅这个频道。如果你想看看我的频道,控制系统讲座,我也会讲更多控制理论的话题。感谢收看,我们下次节目再见。
你也可以从以下列表中选择一个网站:
选择中国站点(中文或英文)以获得最佳站点性能。其他MathWorks国家/地区网站未针对您所在地的访问进行优化。