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