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