从系列:理解传感器融合和跟踪
布莱恩•道格拉斯
本视频通过展示我们如何使用GPS和IMU来估计物体的方向和位置,继续我们使用传感器融合进行定位和定位的讨论。我们会讲解算法的结构,并向你们展示GPS和IMU是如何对最终解决方案做出贡献的,这样你们就会对这个问题有一个更直观的理解。
让我们继续讨论使用传感器融合进行定位和定位。在上一个视频中,我们将IMU中的传感器组合在一起来估计物体的方向,并演示了如何使用加速度计和磁力计的绝对测量值来校正陀螺的漂移。在这个视频中,我们将做类似的事情,但我们将添加一个GPS传感器。GPS可以测量位置和速度,因此我们可以扩展融合算法来估计它们。就像上一个视频一样,我们的目标不是完全描述融合算法;这对一个视频来说太多了。相反,我主要想回顾一下算法的结构,并直观地向您展示GPS和IMU对最终解决方案的贡献,以便您对问题有一个更直观的理解。所以我希望你能留下来。我是Brian,欢迎来到MATLAB技术讲座。
现在,如果你想知道某物相对于地球表面的位置,使用GPS似乎是显而易见的。只要在你的系统上绑上一个GPS传感器,你就能得到纬度、经度和高度,就搞定了。在某些情况下,这是完全可以的。比如,当系统加速和改变方向的速度相对较慢时,你只需要定位精度到几米。这可能是一个决定你汽车方向的系统的情况。只要GPS把你定位在距离你实际地点几米的范围内,地图应用程序就能知道你在哪条路上,从而知道要去哪里。
另一方面,想象一下,如果系统需要几英尺或更短的位置信息,它需要以每秒数百次的速度更新位置,以跟上系统的快速运动。比如,尝试用无人机沿着快速轨迹穿过障碍物。在这种情况下,GPS可能需要与额外的传感器配对,比如IMU中的传感器,以获得所需的精度。
为了让你更直观地理解我在这里谈论的内容,让我们运行一个来自MATLAB传感器融合和跟踪工具箱的例子,称为来自异步传感器的姿态估计。这个例子使用GPS, accel,陀螺仪和磁力计来估计姿态,这是方向和位置,以及一些其他的状态。脚本生成系统遵循的真实路径和方向配置文件。真正的方向是红色立方体,真正的位置是红色钻石。现在还看不清楚,但一开始就清楚了。现在,姿态算法是使用可用的传感器来估计方向和位置,它的结果显示为蓝色立方体和蓝色钻石。这就是我们想看的。蓝色物体跟红色物体的距离有多近?如果你想看到更定量的结果,右边的图表描绘了误差。
关于这个很酷的事情是,当脚本运行时,界面允许我们改变每个传感器的采样率或将它们从解决方案中移除,这样我们就可以看到这是如何影响估计的。让我们先把除了GPS以外的所有传感器都拿掉我们会每秒读取5次GPS。
默认的轨迹是沿着一个半径约为15米的圆。你可以看到它绕着这个圆非常缓慢地运动。正如你所预料的那样,方位估计是错误的,因为我们没有任何主动的方位传感器。但对头寸的估计还不算太糟。当算法确定并消除初始偏差后,我们看到每个轴上的位置误差大约在±2米。
现在我再加入IMU传感器看看结果是否有所改善。它需要几秒钟的时间来收敛方向,但是你可以看到它正在慢慢地调整自己回到真实的方向。而且,对位置的估计也是差不多的。正负2米,可能比这个小一点。因为这是一个相对缓慢的运动,而且它是一个如此大的轨迹,IMU传感器在这里建模只是一个很小的改进,比GPS单独。GPS速度测量足以预测物体在两次测量之间的0.2秒内如何移动,因为物体没有加速得太快。这种设置有点类似于开车时用手机上的GPS从地图上获取方向。增加这些额外的传感器并没有多大帮助。
现在,我们往相反的方向走,创造一个更快的轨迹。在轨迹生成脚本中,我将加速物体绕圆的速度从2.5米/秒到12.5米/秒。这会在更短的时间内产生更多的角加速度。为了强调我在这里要强调的一点,我将把GPS采样时间降低到每秒一次。让我们试一试。
好的,这里发生的是当我们得到GPS测量,我们得到位置和速度。所以每一秒,我们会得到一个新的位置更新,把估计值放到距离真实值几米的范围内,但我们也会得到当前的速度。在一秒钟内,算法将速度向前传播,以预测物体在两次测量之间的表现。如果速度在这一秒内接近恒定,这个方法很有效,但就像你看到的,当速度快速变化时,效果就很差。
这种类型的情况类似于无人机,制作快速转动,避免障碍,这是这里的乌兹别克斯坦伊斯兰运动将帮助,因为我们不需要依赖传播一个静态速度一秒钟,我们可以使用乌兹别克斯坦伊斯兰运动估计速度和旋转传感器。
现在,为了看到改进,我把两个不同的运行放在一起。左边是我们刚刚看到的GPS,右边是IMU的附加。你可以看到,至少在视觉上,带有IMU的GPS和单独的GPS是不同的。它能够更密切地跟踪对象的位置,并创建一个圆形的结果,而不是一个锯片。因此,增加一个IMU似乎有助于估计位置。为什么会这样,算法是如何结合这些传感器的呢?
我们可以直观地想象IMU可以让我们在GPS更新之间计算系统的状态,就像我们在上个视频中使用陀螺仪在magg和accel更新之间计算一样。这是真的,只是它不是一成不变的;这比你想象的要复杂得多。要理解为什么会出现这种情况,我们需要稍微研究一下代码。
该融合算法为连续-离散扩展卡尔曼滤波算法。这个特殊的传感器被设置为异步接受传感器测量,这意味着每个传感器都可以以自己的速率读取。这是有益的,如果你想运行,你的陀螺仪在100hz,你的mag和加速计在50hz,和你的GPS在1hz。您将在下面看到这是如何处理的。
这里我想指出的是,这是一个巨大的卡尔曼滤波器。状态向量中有28个同时被估计的元素。有一些明显的状态,比如方向、角速度、线性位置、速度和加速度。但该滤波器也估计传感器偏差和磁场矢量。估计传感器的偏差是非常重要的,因为偏差会随时间变化。这意味着,即使您在操作系统之前计算传感器偏差,并将校准值硬编码到软件中,它也不会长期准确。任何我们不消除的偏差都会被整合,当我们依赖传感器时,会导致估计偏离真相。
如果你在启动系统时对传感器偏差没有一个很好的初始估计,那么你就不能打开过滤器然后马上相信它。你必须给它一些时间,不仅要估计你关心的主要状态,比如位置和速度,还要估计一些次要状态,比如偏差。通常你让卡尔曼滤波器收敛于正确的解决方案当系统是静止的,而不是控制,或者当你控制它使用不同的估计算法,或者也许你只是让它运行,你不在乎,系统表现欠佳,而滤波器收敛。所以当我们讨论给卡尔曼滤波器足够的时间来收敛时,这是它的一部分。
我们需要考虑的另一件事是如何初始化过滤器。这是一个EKF,它可以估计非线性系统的状态。它通过线性化当前估计的模型,然后使用线性模型来预测未来的状态。因此,如果滤波器初始化不够接近真实状态,线性化过程可能会离真实状态太远,导致滤波器永远不会真正收敛。这对这个例子来说不是问题,因为基本真理在模拟中是已知的,所以过滤器被初始化为接近真理的状态。但在一个真实的系统中,当你不知道真相时,你需要考虑如何初始化过滤器。
通常,这可以通过直接使用传感器的测量来实现。比如使用上次的GPS读数来初始化位置和速度,使用陀螺仪来初始化角速度,等等。
初始化过滤器后,我们就可以开始运行它了。每个卡尔曼滤波都包含相同的两步过程:预测和校正。
理解为什么我们可以这样想,如果我们要估计的状态为例,它在哪里,或者多快去那里是两个一般方法:我们可以直接测量它,或者我们可以使用我们的动力学和运动学知识来预测它在哪里。
例如,想象有一辆车在路上行驶,我们想知道它的位置。我们可以用GPS直接测量它的位置。这是一种方法。但如果我们知道它从哪里开始,它的平均速度,我们也可以预测一段时间后它会在什么位置,具有一定的准确性。将这些预测与测量结合起来可以得到更好的估计。所以问题可能是,为什么我们不能完全相信我们的测量呢?这可能比我们估计的要好。举个极端的例子,如果你看表,表上显示现在是下午3点,然后你等了几秒钟,再看一次,表上显示的是下午4点。你不会因为你的测量结果就自动假定一个小时过去了。你对时间有基本的理解,对吧? That is, you have an internal model that you can use to predict how much time has passed, and that would cause you to be skeptical of your watch if you thought seconds passed and it said an hour. On the other hand, if you thought about an hour had passed but the watch said 65 minutes, you would probably be more inclined to believe the watch over your own estimate since you’d be less confident in your prediction. And sensors have errors and uncertainty associated with them and you can improve the state estimate by including a prediction even if your sensor is pretty good.
这正是卡尔曼滤波器的作用。它预测状态将如何随着时间的推移而变化基于它已有的模型和状态,它还跟踪基于你给出的过程噪声的预测的可靠性。过滤器预测状态的时间越长,它对结果的信心就越低。然后,每当有新的测量值出现时,该滤波器将预测值与测量值进行比较,然后根据两者的相对置信度对其进行校正。
这就是剧本所做的。模拟以100hz的频率运行,在每个时间步长,它预测状态的估计。然后,如果有来自任何传感器的新测量,它运行卡尔曼滤波器的更新部分,根据预测和具体测量的相对置信度调整状态。通过这种方式,过滤器可以运行异步测量。
现在,在我们开始的唯一GPS解决方案中,预测步骤只能假设速度不会在1秒内改变,因为没有更新来纠正这个假设,估计会大大偏离事实。然而,在IMU中,过滤器每秒更新100次,通过加速度计看到速度实际上是在变化的。因此,通过这种方式,与GPS较慢的更新相比,IMU的快速更新可以使滤波器更快地对变化的状态做出反应。一旦滤波器收敛,它对传感器偏差有一个很好的估计,那就会给我们一个更好的预测,因此,一个更好的整体状态估计。这就是传感器融合的力量。
现在,这个解释可能不是很清楚,可能有点快,但我认为通过观看视频很难真正理解这个主题。所以我鼓励你们尝试这个例子,打开和关闭传感器,改变它们的速率,噪声特性,和轨迹看看估计是如何影响你自己的。您甚至可以深入到代码中,看看EKF是如何实现的。我发现放置断点和暂停脚本的执行很有帮助,这样我就可以看到不同的函数如何更新状态向量。
好了,我就讲到这里。在下个视频中,当我们讨论跟踪算法时,我们会开始考虑估计其他物体的状态。
所以,如果你不想错过这个和未来的Tech Talk视频,不要忘记订阅这个频道。如果你愿意,你可以看看我的频道,控制系统讲座,在那里我也涵盖了更多的控制理论主题。谢谢收看。
你也可以从以下列表中选择一个网站:
选择中国网站(中文或英文)以获得最佳网站性能。其他MathWorks国家站点没有针对您所在位置的访问进行优化。