从系列:自主导航
布莱恩•道格拉斯
本视频介绍了粒子过滤器的高级理解,并展示了如何在蒙特卡洛定位中使用它来确定建筑物内移动机器人的姿态。
我们将讨论为什么粒子滤波器比传统卡尔曼滤波器更适合解决这类问题,因为它能够处理非高斯概率分布。
在第一个视频中,我们讨论了什么是自动导航以及如何将其应用于不同类型的自动驾驶车辆。在这段视频中,我们将讨论自主导航问题的一部分,并演示如何使用粒子滤波器估计移动机器人的位置和方向。我们将通过相同的定位方法演示MATLAB例子,Localize TurtleBot使用蒙特卡罗定位。这个例子模拟了一个在办公大楼中移动的TurtleBot,测量环境并估计它的位置。如果你不熟悉粒子过滤器,你可能会感到困惑,为什么解决方案会出现一堆蓝点,并在每个时间步跳,然后以某种方式汇聚到TurtleBot的真实位置。这个过滤器在做什么?这些看似随机的点是如何帮助我们确定机器人的位置的?这就是我们要讲的内容,所以我希望你们能留下来听。我是Brian,欢迎来到MATLAB技术讲座。
让我们建立本地化问题。我们有一个机器人,龟形机器人,它在一栋办公大楼里闲逛。机器人会得到一份建筑地图,所以它知道墙壁和家具的总体布局,但最初它并不知道自己在地图上的位置——它可能在任何地方。因此,问题是机器人需要使用它的传感器和运动模型来估计它的姿态,也就是确定它在建筑中的位置和方向。
这个机器人有一个激光雷达传感器,可以返回视野范围内物体的距离。所以,如果传感器正在观察一个角落和两扇门,它可能会返回像这样的点云。但测量是有噪声的,所以它实际上可能看起来像这样。现在我们可以将这个模式与环境地图进行匹配,并确定机器人的位置。
现在,除了噪声测量外,机器人还有一种利用里程计数据计算自己位置的方法。Deadreckoning是指使用过去的位置和相对测量值(如速度和角速度)来计算未来的位置。如果你知道机器人正面临北和移动1米/秒,3秒之后你可以估计,北部的位置是3米,它曾经是,你可以有信心是这种情况,而不必再核对一下环境。Deadreckoning可以在较短的时间内使用,但由于相对测量中的噪声,随着时间的推移,误差会增长,需要通过测量它与环境的相对位置来进行修正。为了更深入地了解deadreckoning,请查看传感器融合和跟踪视频3,链接如下,在视频中,我们将GPS的绝对测量与IMU的相对测量相结合。
所以我们有两种可能的方法来确定位置;我们有一个嘈杂的激光雷达传感器,可以感知到我们可以与环境地图相比较的特征,我们有嘈杂的里程测量数据,可以估计机器人是如何移动的。现在,如果你熟悉卡尔曼滤波器,你可能会认识到混合一个有噪声的测量和一个有噪声的过程模型正是它的用途。
如果你不熟悉卡尔曼滤波器,你可以通过看MATLAB关于它们的技术演讲系列来加快速度,我在下面的描述中给出了链接,但你不需要太多的经验来理解这个视频的其余部分。
卡尔曼滤波器的一个重要缺点是它期望概率分布是高斯分布。因此,对于我们的问题,随机估算噪声需要是高斯噪声,激光雷达测量噪声需要是高斯噪声。两者都可能是,或者至少接近到仍然使用卡尔曼滤波器。但重要的是,机器人估计状态的概率分布也必须是高斯分布,我们一会儿就会看到这在我们的定位例子中绝对不是这样。因此,我们需要一个可以处理非高斯概率分布的估计滤波器。这就是粒子过滤器的作用。
为了理解粒子过滤器是如何解决定位问题的,我们应该试着把自己想象成机器人,并弄清楚我们将如何使用激光雷达来确定我们在一个非常简单的地图上的位置。一开始,我们对自己在房间里的位置毫无头绪。我们可以说,同样有可能的是,我们在地图上的任何地方,面对着任何方向。现在,我们用激光雷达传感器测量场景并接收结果。看起来我们在看一堵墙,因为所有的测量值基本上都在一条线上,从测量值我们可以知道我们离墙有多远。仅凭这一点我们无法确定我们的确切位置因为这个房间里有很多墙,我们可能正对着其中的任何一面墙,或者沿着每面墙的任何地方。但我们可以看出,我们可能不是在看一个角落或门,我们可能不是在房间的中间,因为墙很近。
第一个测量限制了我们的位置和方向。然而,由于传感器有一些噪声和误差,我们可能会声称我们可以比测量状态更近或更远一些。因此,我们可以相应地扩展可能的位置,并说我们最有信心我们在这些位置中的一个。现在,我们不能肯定地说我们不是在房间的中间,因为有可能我们看到的是一个暂时的平面障碍,但地图上没有。这种情况发生的概率很低,但仍然是可能的。
因此,在第一次测量之后,我们可以说我们所处位置的概率分布是这样的。较亮的黄色位置表示概率较高,较暗的位置表示概率较低。这已经是非高斯分布了所以,你可以看到我们很容易陷入非高斯情况当我们处理这样的本地化时。
从直觉上来说,我们不能确定我们从单一的测量中处在什么位置。假设这是我们在房间里的位置。同样,我们仍然只是看着这面平坦无特征的墙。作为一个人,如果我们四处移动去看不同的特征,我们就可以开始缩小我们所处的位置,通过移除那些不满足我们一路上看到的所有测量值的位置。所以,如果我们向左转,看到一个角落,然后会继续转动,看到另一个角落里,然后我们看到了门,我们可以使用我们的记忆开始拼凑我们基于我们所看到的在地图上,激光雷达测量,以及我们如何在环境中移动的估计——即我们转向的方向和我们走了多远。
在这张地图上只有一个地方像这样的运动可以看到两个角,然后在一扇门处结束。所以这肯定是我们曾经走过的路,这肯定是我们现在所处的位置。
所以,这些都很直观,但我们如何为机器人编写这种理解代码呢?它是如何记住多次连续的测量数据,并随着时间的推移精确定位的呢?正如你所期望的,因为我已经告诉过你,一个流行的方法是用粒子过滤器。
我们将会对粒子过滤器的工作原理有一个基本的了解。本节的目的是让您对过滤器有一些直觉,而不是深入到任何数学。如果你想要更多关于过滤器的细节,我在描述中留下了一些很棒的资源。好了,我们开始吧。
记住,一开始机器人只有房间的地图,没有其他的所以机器人在地图上的任何地方,面向任何方向的概率是相等的。所以,我们不是试图用一些平滑定义的函数来捕获这个概率,我们可以通过随机生成一些离散的姿态来近似概率分布。这里我使用50个不同的姿势均匀地随机化x和y的位置以及均匀地随机化方向。这捕获了整个地图上的均匀概率分布。
每一个姿态或粒子都保存在过滤器中,作为状态的可能估计。粒子越多,它们就越能代表真实的概率分布,但所有的计算都要花更多的时间。所以,在准确性和速度之间有一个权衡。在这个思考练习中,50个粒子就足够了,但你可能需要数百个或数千个粒子,这取决于地图的大小和你试图表示的概率分布的形状。
现在,一堆随机的系统状态猜测似乎对我们没有多大帮助,因为很多猜测显然都是错误的。但我们可以开始消除一些错误的猜测,因为在这一点上,我们得到了激光雷达测量,它返回的图案,看起来像一堵墙。现在,过滤器可以逐一检查我们的每一个粒子然后用地图来确定,如果那个姿势是真实的,激光雷达传感器会返回什么。然后它可以比较两个场景,并确定该粒子是真实系统状态的可能性有多大。场景越接近,概率就越高。例如,这个粒子会返回一个场景,比我们看到的更接近,这个粒子会。
我将放大正确概率高的粒子以便我们能更好地看到它们,我将缩小概率低的粒子。就像我们之前看到的在一次测量之后我们的概率分布不再是均匀的,而是向这些更可能的姿势倾斜。
现在,我们不想把所有的低概率粒子都保留在过滤器中因为那将在不太可能的姿态上消耗计算资源我们也可能会陷入一种情况它们会导致过滤器永远不收敛。因此,我们将根据新的概率分布对粒子进行重新采样。这将在姿势中放置更多的粒子,而在其他地方放置更少的粒子。这种概率分布的随机离散重采样就是为什么这种类型的局部化也被称为蒙特卡罗局部化。
此时,机器人在房间里移动,它沿着我之前展示的圆形路径移动,它通过机载里程计估算了它的相对位置。所以机器人能够感知激光雷达的测量结果。我们可以将这个估计的运动应用到过滤器中的每一个粒子上。基本上,我们是在预测每一个可能的姿势,如果它们以与机器人相同的相对方式移动。
当然,我提到过,deadreckoning是一个有噪声的过程,所以我们在机器人的运动估计中存在一定程度的不确定性。因此,我们并不是用估计的运动精确地传播每个粒子,而是根据过程噪声添加额外的随机运动。有些粒子可能移动得快一些,有些慢一些,有些转得多一些,有些转得少一些。所以,如果过程噪声是非高斯的,粒子滤波器可以处理它,至少只要有足够的粒子来精确地表示分布。
现在,我们可以做和之前一样的步骤。我们用激光雷达传感器测量场景,并找出哪些新粒子最有可能产生类似的测量结果。机器人现在正在看一个角落,这很好,因为这是地图中相对独特的特征。但它并没有完全缩小范围。你可以看到这个粒子在看一个角而这个粒子在看另一个角。所以,我们现在还不能确定哪个是真正观测到的角。我再一次放大概率最大的粒子,缩小概率较小的粒子。我们可以知道概率分布是如何变化的。现在,它是双模态的,机器人有两个主要可能的位置,其他地方的可能性就小得多。
再一次,我们可以根据这个概率分布,通过生成一个新一批的姿态来转移到下一代粒子。但我们可以开始变得更聪明一点。在前两代中,我使用了50个粒子,这样我就可以准确地表示大而复杂的分布。然而,现在我们正在打磨机器人的真实位置,分布变得越来越窄,它不需要太多的粒子来完全表示它们。我们仍然可以使用50,但这只会减慢过滤速度,可能不会增加太多的价值。因此,有了这种自适应蒙特卡罗定位的想法,或称AMCL,它在每一代之后重新计算粒子的数量,这样你就不会浪费计算资源。你必须小心粒子的数量过低,因为过滤器可能会对它的解决方案过于自信,并错误地表示真实的概率分布,导致它收敛到错误的姿态。所以,这里有一个权衡。MATLAB的TurtleBot例子使用了这种自适应蒙特卡罗定位,如果你想知道这个调整是如何完成的细节,下面有一个链接。
现在每一代都和以前一样。我们使用里程计对机器人的运动建模,将其应用到粒子上,进行另一种激光雷达测量,并确定哪些粒子最有可能。然后我们根据这个新的概率分布重新抽样,重新开始。这个迭代过程就是我们如何利用粒子随着时间的推移收敛到机器人的实际姿态上。即使在一个像这个矩形一样平凡的房间里。
现在如果我们重新审视这个视频的MATLAB的例子从一开始,它应该更多的意义,为什么这些蓝点在这里,他们是粒子,和他们为什么每次迭代移动,为什么粒子的数量随着时间的推移越来越小,因为它是收敛的。这是自适应蒙特卡罗定位方法中的粒子滤波器。
现在,我认为一个更好地理解这些是如何工作的,以及如何在MATLAB中实现它的好方法,就是看看这个例子。你可以改变一些过滤器参数,看看它如何影响解决方案,你可以检查不同的功能,实现我们刚刚谈到的每个想法。然后,当你去学习它背后的数学知识时,你就会对过滤器是如何工作有一个直观的感觉,希望数学变得更简单。
在这个视频中,我们只讨论了本地化假设我们已经有了可以信任的地图。但如果不是这样呢?如果我们没有地图,或者环境在不断变化呢?好吧,在下个视频中,我们将讨论SLAM问题。同时进行本地化和映射。
所以,如果你不想错过未来的Tech Talk视频,别忘了订阅这个频道。如果你想看看我的频道,控制系统讲座,我也会讲更多控制理论的话题。感谢收看,我们下次节目再见。
你也可以从以下列表中选择一个网站:
选择中国网站(中文或英文)以获得最佳网站性能。其他MathWorks国家站点没有针对您所在位置的访问进行优化。