基于Matlab通过惯性传感器融合估计方向（附源码）

2022-11-13 18:10 278人阅读评论(0)

此示例演示如何使用6轴和9轴融合算法来计算方向。有几种算法可以从惯性测量单元（IMU）和磁角速率重力（MARG）单元计算方向。此示例介绍了定向基础知识以及如何使用这些算法。

一、取向

对象的方向描述其相对于某个坐标系（有时称为父坐标系）的三维旋转。对于以下算法，使用的固定父坐标系为东北向下（NED）。NED有时被称为全局坐标系或参考系。在NED参考系中，X轴指向北方，Y轴指向东，Z轴指向下方。NED的X-Y平面被认为是地球的局部切平面。根据算法，北可以是磁北或真北。此示例中的算法使用磁北。如果指定，以下算法可以估计相对于东-北-上（ENU）父坐标系而不是 NED 的方向。可以将对象视为具有自己的坐标系，通常称为局部坐标系或子坐标系。此子坐标系随对象相对于父坐标系旋转。如果没有平移，则两个坐标系的原点重叠。

计算的方向量是将数量从父参考系带到子参考系的旋转。旋转由四元数或旋转矩阵表示。

二、传感器的类型

对于方向估计，通常使用三种类型的传感器：加速度计，陀螺仪和磁力计。加速度计测量适当的加速度。陀螺仪测量角速度。磁力计测量当地的磁场。不同的算法用于融合不同的传感器组合以估计方向。

2.1 传感器数据

在本例的大部分内容中，使用同一组传感器数据。当设备围绕三个不同的轴旋转时，记录加速度计、陀螺仪和磁力计传感器数据：首先围绕其本地 Y 轴旋转，然后围绕其 Z 轴旋转，最后围绕其 X 轴旋转。在实验期间，设备的X轴通常指向南方。

2.2 加速度计-磁力计融合

该功能融合了加速度计和磁力计数据。这是一种无需参数调整的无记忆算法，但该算法极易受到传感器噪声的影响。

请注意，该算法正确找到北方的位置。但是，由于函数是无记忆的，因此估计的运动并不平滑。该算法可以用作方向滤波器中的初始化步骤，或者可以使用使用四元数SLERP的低通滤波器定向中介绍的一些技术来平滑运动。

2.3 加速度计-陀螺仪融合

以下对象使用误差状态卡尔曼滤波器或互补滤波器估计方向。误差状态卡尔曼滤波器是标准估计滤波器，允许使用相应的噪声参数对系统的许多不同方面进行调谐。互补滤波器可用作具有内存限制的系统，并且具有最小的可调参数，从而可以更轻松地进行配置，但需要更精细的调整。

TheandSystem 对象融合了加速度计和陀螺仪数据。使用内部误差状态卡尔曼滤波器，并使用互补滤波器。滤波器能够消除陀螺仪随时间漂移的偏置噪声。

尽管与算法相比，算法对运动的估计明显更平滑，但它们不能正确估计北方的方向。它不处理磁力计数据，因此它只是假设设备的X轴最初指向北方。给出的运动估计相对于初始估计的方向。当属性设置为时，做出相同的假设。

2.4 加速度计-陀螺仪-磁力计融合

姿态和航向参考系统（AHRS）由一个 9 轴系统组成，该系统使用加速度计、陀螺仪和磁力计来计算方向。TheandSystem 对象™结合了以前最好的算法，以生成平滑变化的设备方向估计，同时正确估计北方的方向。使用与以前相同的互补滤波器算法，但增加了一个步骤来包括磁力计并改进方向估计。类似，算法也使用错误状态卡尔曼滤波器。除了陀螺仪偏置消除外，还具有检测和抑制轻度磁干扰的能力。

2.5 调整过滤器参数

根据所使用的指定传感器调整参数可以提高性能。参数和可以调整以改变每个传感器的测量值对方向估计的影响量。

TheandSystem 对象具有更多参数，这些参数可以使筛选器更紧密地匹配特定的硬件传感器。传感器的环境也很重要。控制滤波器对线性（平移）加速度的响应。晃动设备是添加线性加速度的简单示例。

它预计处于线性加速度较低的环境中，它更容易受到线性加速度的影响，如图前面的巨大变化所示。但是，由于它期望处于线性加速度较低的环境中，因此加速度计信号的信任度更高。因此，一旦震动结束，方向估计会迅速收敛回垂直方向。反之亦然。滤波器受晃动的影响较小，但当晃动停止时，方向估计需要更长的时间才能收敛到垂直方向。

该属性能够以大致相同的方式对磁扰动（非地磁噪声源）进行建模线性加速度。两个衰减因子属性（和）对噪声的变化率进行建模。对于缓慢变化的噪声源，请将这些参数设置为接近 1 的值。对于快速变化、不相关的噪声，请将这些参数设置为接近 0。较低的方向估计能够更快地找到“向下”。较低的方向估计可以更快地找到北方。非常大、短的磁扰动几乎完全被拒绝。考虑从固定传感器记录时施加的 [0 250 0] uT 脉冲。理想情况下，方向估计不应有变化。