从系列:理解传感器融合和跟踪
布莱恩·道格拉斯
本视频通过展示如何使用GPS和IMU来估计对象的方向和位置,继续我们关于使用传感器融合进行定位和定位的讨论。我们将介绍算法的结构,并向您展示GPS和IMU如何为最终解决方案做出贡献,以便您对问题有更直观的理解。
让我们继续讨论使用传感器融合进行定位和定位。在上一个视频中,我们将IMU中的传感器组合在一起来估计物体的方向,并演示了如何使用加速度计和磁力计的绝对测量值来校正陀螺的漂移。在这个视频中,我们将做类似的事情,但我们将添加一个GPS传感器。GPS可以测量位置和速度,因此我们可以扩展融合算法来估计它们。就像上一个视频一样,我们的目标不是完全描述融合算法;这对一个视频来说太多了。相反,我主要想回顾一下算法的结构,并直观地向您展示GPS和IMU对最终解决方案的贡献,以便您对问题有一个更直观的理解。所以我希望你能留下来。我是Brian,欢迎来到MATLAB技术讲座。
现在,如果你想知道某物相对于地球表面的位置,使用GPS似乎是显而易见的。只要在你的系统上绑上一个GPS传感器,你就有了纬度、经度和高度,你就完成了。在某些情况下,这是完全正确的。比如当系统相对缓慢地加速和改变方向时,你只需要几米的定位精度。对于确定汽车方向的系统来说,可能就是这种情况。只要GPS将你定位在距离实际地点几米的范围内,地图应用程序就可以确定你在哪条道路上,从而确定你要去哪里。
另一方面,想象一下,如果系统需要几英尺或更少的位置信息,并且需要每秒数百次的位置更新来跟上系统的快速运动。比如说,试图用无人机沿着障碍物的快速轨迹飞行。在这种情况下,GPS可能必须与其他传感器配对,如IMU中的传感器,以获得所需的精度。
为了让您更直观地了解我在这里讨论的内容,让我们运行一个来自MATLAB传感器融合和跟踪工具箱的示例,称为异步传感器的姿势估计。本例使用GPS、加速度计、陀螺仪和磁强计来估计姿态,即方向和位置,以及其他一些状态。脚本生成系统遵循的真实路径和方向配置文件。真实方向是红色立方体,真实位置是红色菱形。现在有点难看,但开始时会更清楚。现在,姿态算法使用可用的传感器来估计方向和位置,并将其结果分别显示为蓝色立方体和蓝色菱形。这就是我们想看的。蓝色物体跟红色物体的距离有多近?如果您希望看到更定量的结果,右侧的图表将绘制错误。
关于这个很酷的事情是,当脚本运行时,界面允许我们改变每个传感器的采样率或将它们从解决方案中移除,这样我们就可以看到这是如何影响估计的。让我们先把除了GPS以外的所有传感器都拿掉我们会每秒读取5次GPS。
默认的轨迹是沿着一个半径约为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更新之间计算系统的状态,就像我们在上个视频中使用陀螺仪在magg和accel更新之间计算一样。这是真的,只是它不是一成不变的;这比你想象的要复杂得多。要理解为什么会出现这种情况,我们需要稍微研究一下代码。
该融合算法是一种连续离散扩展卡尔曼滤波器。这个特定的传感器被设置为异步接受传感器测量,这意味着每个传感器都可以以自己的速率读取。这是有益的,如果你想运行,比如说,你的陀螺仪在100赫兹,你的磁和加速度计在50赫兹,你的GPS在1赫兹。您将在下面看到如何处理此问题。
这里我想指出的是,这是一个巨大的卡尔曼滤波器。状态向量中有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国家/地区网站未针对您所在地的访问进行优化。