在上个视频中，我们讨论了如何估计自动驾驶汽车的姿态，也就是它的位置和方向，如果我们已经有了环境的模型或地图。在这个视频中，我们不会提前有地图。我们要建立一个同时通过一个叫做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 with Lidar扫描”的导航工具箱，仅使用Lidar构建环境的占用网格图，不需要相对里程测量过程。从本质上讲，环境中有足够多的独特特征，激光雷达对每个姿态的测量可以通过数据关联或特征匹配与过去的其他姿态相关联。这是值得研究和研究的。如果你想了解更多关于这个例子的信息，我在下面链接了另一个视频。
好了，这节课就讲到这里。如果你不想错过任何未来的Tech Talk视频，不要忘记订阅这个频道。你想看看我的频道，控制系统讲座，我也会讲更多的控制理论话题。谢谢收看，我们下期见。
