理解传感器融合和跟踪,第3部分:融合GPS和IMU来估计姿态
从系列中:理解传感器融合和跟踪
布莱恩•道格拉斯
本视频继续讨论如何使用传感器融合进行定位和定位,展示我们如何使用GPS和IMU来估计物体的方向和位置。我们将讨论算法的结构,并向您展示GPS和IMU如何为最终解决方案做出贡献,以便您对问题有更直观的理解。
让我们继续讨论使用传感器融合进行定位和定位。在上一集视频中,我们结合IMU中的传感器来估计物体的方向,并展示了如何使用加速度计和磁力计的绝对测量来纠正陀螺的漂移。在这个视频中,我们要做类似的事情,但是我们要添加一个GPS传感器。GPS可以测量位置和速度,所以通过这种方式我们可以扩展融合算法来估计它们。就像上个视频一样,我们的目标不是完全描述融合算法;对于一个视频来说太多了。相反,我主要想回顾一下算法的结构,并直观地向您展示GPS和IMU是如何为最终解决方案做出贡献的,以便您对问题有更直观的理解。所以我希望你们能坚持下去。我是Brian,欢迎来到MATLAB技术讲座。
现在,如果你想知道某物相对于地球表面的位置,使用GPS似乎是显而易见的。只要把GPS传感器绑在你的系统上,你就有了纬度、经度和海拔,你就完成了。在某些情况下,这是完全没问题的。比如,当系统加速并相对缓慢地改变方向时,你只需要精确到几米的位置。这可能是一个在你的汽车中确定方向的系统的情况。只要GPS把你的位置定位在离你实际位置几米之内,地图应用程序就能知道你在哪条路上,从而知道你要去哪里。
另一方面,想象一下,如果系统需要几英尺或更短的位置信息,并且它需要以每秒数百次的速度更新位置,以跟上系统的快速运动。例如,试图用无人机沿着快速轨迹通过障碍物。在这种情况下,GPS可能必须与额外的传感器配对,比如IMU中的传感器,以获得所需的精度。
为了让您更直观地了解我在这里谈论的内容,让我们运行一个来自MATLAB传感器融合和跟踪工具箱的示例,称为来自异步传感器的姿态估计。本例使用GPS、加速度仪、陀螺仪和磁力计来估计姿态,即方向和位置,以及其他一些状态。该脚本生成系统遵循的真实路径和方向配置文件。真正的方向是红色方块,真正的位置是红色菱形。现在还看不清楚,但开始的时候会更清楚。现在,姿态算法正在使用可用的传感器来估计方向和位置,它分别显示蓝色立方体和蓝色菱形的结果。这就是我们想看的。蓝色物体跟红色物体的距离有多近?如果你想看到更定量的结果,右边的图表显示了误差。
很酷的一点是,当脚本运行时,界面允许我们改变每个传感器的采样率,或者将它们从解决方案中完全删除,以便我们可以看到这是如何影响估计的。让我们从移除所有的传感器开始,除了GPS,我们将每秒读取GPS 5次。
默认轨迹是沿着一个半径约15米的圆飞行。你可以看到它绕着这个圆移动得很慢。正如你所预料的那样,方向估计是错误的,因为我们没有任何方向传感器。但这个位置估计还不算太糟。在算法确定并去除初始偏差后,我们看到每个轴上的位置误差约为正负2米。
现在让我重新加入IMU传感器,看看我们的结果是否有所改善。方向收敛需要几秒钟,但你可以看到它在慢慢地自我修正回到真实的方向。同样,位置估计也是一样的。正负2米,可能比这个小一点。因为这是一个相对缓慢的运动,而且是一个很大的轨迹,这里建模的IMU传感器只比GPS单独做出了微小的改进。GPS速度测量足以预测物体在两次测量之间0.2秒内的运动方式,因为物体的加速度不是太快。这个设置有点类似于当你开车时使用GPS从你手机上的地图上获得方向。增加这些额外的传感器并没有多大帮助。
现在,让我们往相反的方向走,创造一个更快的轨迹。在轨迹生成脚本中,我将加速物体绕圈的速度从2.5米/秒到12.5米/秒。这会在更短的时间内产生更大的角加速度。为了强调我要强调的一点,我将GPS采样时间放慢到每秒一次。让我们试一试。
这里发生的是当我们得到GPS测量值时,我们得到了位置和速度。所以每隔一秒,我们就会得到一个新的位置更新,使估计值与真实值相差几米,但我们也得到了当前的速度。在一秒钟内,算法将速度向前传播,以预测物体在两次测量之间的运动。如果速度在那一秒内接近恒定,这个方法很有效,但你可以看到,当速度快速变化时,效果就很差了。
这是一种类似于无人机必须快速转弯和避开障碍物的情况,在这里,IMU的加入将有所帮助,因为我们不必依赖于传播静态速度一秒钟,我们可以使用IMU传感器估计速度和旋转。
现在,为了看到改进,我将两个不同的运行放在一起。左边是我们刚刚看到的GPS,右边是IMU的加入。你可以看到,至少在视觉上,有IMU的GPS和单独的GPS是不一样的。它能够更紧密地跟随物体的位置,并创建一个圆形的结果,而不是一个锯片。所以增加IMU似乎有助于估计位置。那么,为什么会出现这种情况,算法是如何结合这些传感器的呢?
好吧,再一次,直观地我们可以想象IMU允许我们在GPS更新之间估算系统的状态,类似于我们如何在上个视频中使用陀螺仪来估算mag和加速更新之间的状态。这是真的,只是它不像那样老生常谈;这比你想象的要复杂得多。为了理解为什么会出现这种情况,我们需要稍微研究一下代码。
融合算法是一种连续-离散扩展卡尔曼滤波器。这个特殊的传感器被设置为异步接受传感器测量,这意味着每个传感器都可以以自己的速率读取。如果你想让陀螺仪以100赫兹运行,磁场和加速度计以50赫兹运行,GPS以1赫兹运行,这是非常有益的。您将在下面看到这是如何处理的。
我想在这里指出的是,这是一个巨大的卡尔曼滤波器。状态向量中有28个元素同时被估计。有一些明显的状态,比如方向,角速度,线性位置,速度和加速度。但滤波器也估计传感器偏差和磁场矢量。估计传感器偏差是非常重要的,因为偏差会随时间漂移。这意味着,即使你在操作系统之前计算了传感器偏差,并将校准值硬编码到软件中,它也不会准确太久。当我们依赖于传感器时,任何我们不移除的偏差都会被整合,导致估计偏离事实。
现在,如果你在启动系统时没有对传感器偏差进行良好的初始估计,那么你就不能立即打开过滤器并信任它。你必须给它一些时间,不仅要估计你关心的主要状态,比如位置和速度,还要估计一些次要状态,比如偏差。通常你会让卡尔曼滤波器收敛到正确的解当系统是静止的,不受控制的时候,或者当你用不同的估计算法控制它的时候,或者你只是让它运行你不关心当滤波器收敛时系统的性能很差。所以当我们讨论给卡尔曼滤波器足够的时间收敛时,这是一部分。
我们需要考虑的另一件事是如何初始化筛选器。这是一个EKF,它可以估计非线性系统的状态。它通过围绕当前估计线性化模型,然后使用该线性模型预测未来的状态来实现这一点。所以,如果过滤器的初始化没有足够接近真实状态,线性化过程可能会太远,导致过滤器永远不会收敛。对于这个例子来说,这并不是一个问题,因为在模拟中,ground truth是已知的,所以过滤器只是被初始化为一个接近truth的状态。但在实际系统中,您需要考虑在不知道真值时如何初始化筛选器。
通常,这可以通过直接使用传感器的测量结果来实现。比如使用最后的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.
这就是卡尔曼滤波器的作用。它会根据已有的模型预测状态如何随时间变化,同时,它还会根据你给出的过程噪声跟踪预测的可信度。过滤器预测状态的时间越长,它对结果的信心就越低。然后,每当一个新的测量值进来,它有自己的测量噪声相关,滤波器比较预测和测量值,然后根据两者的相对置信度修正其估计。
这就是脚本所做的。模拟运行在100 Hz,在每一个时间步长,它预测预估的状态。然后,如果有来自任何传感器的新测量值,它会运行卡尔曼滤波器的更新部分,根据预测和特定测量值的相对置信度调整状态。通过这种方式,过滤器可以与异步测量一起运行。
现在,我们用GPS唯一的解决方案开始,预测步骤只能假设速度在1秒内没有变化,因为没有更新来纠正这个假设,估计会大大偏离事实。然而,对于IMU,过滤器每秒更新100次,观察加速度计,看到速度实际上在变化。因此,通过这种方式,过滤器可以更快地对状态变化做出反应,通过IMU的快速更新,而不是GPS的缓慢更新。一旦滤波器收敛,它就能很好地估计传感器偏差,然后就能给我们一个更好的整体预测,因此,一个更好的整体状态估计。这就是传感器融合的力量。
现在,这个解释可能不是很清楚,可能有点快,但我认为通过看视频很难真正理解这个主题。所以我鼓励你们玩一下这个例子,打开和关闭传感器,改变它们的速率,噪声特性,和轨迹看看评估是如何影响自己的。您甚至可以进一步深入代码,了解如何实现EKF。我发现设置断点和暂停脚本的执行很有帮助,这样我就可以看到不同的函数如何更新状态向量。
好吧,我要把这个放在这里。在下一集视频中,当我们讨论跟踪算法时,我们将开始研究估计其他对象的状态。
所以如果你不想错过本期节目和未来的Tech Talk视频,不要忘记订阅这个频道。而且,如果你愿意,你可以看看我的频道,控制系统讲座,在那里我涵盖了更多的控制理论主题。感谢收看。
您也可以从以下列表中选择一个网站:
如何获得最佳的网站性能
选择中国站点(中文或英文)以获得最佳站点性能。其他MathWorks国家站点没有针对您所在位置的访问进行优化。