上节内容：http://www.ganwen.net.cn/?/article/30

基于MSE的传感器融合

在上一节中，观测变量是

其中没有来自罗盘的信息。由于ω是角速率，我们只能使用四 元数来导入罗盘数据q. 我们使用MSE方法获得q， 即观测变量 中的组分。

我们将各变量定义如下：

mb和ab： 体坐标系里的罗盘磁值和加速度值。

mn和an： 导航坐标系里的罗盘磁值和加速度值。

mn0和an0： 导航坐标系里的初始静态罗盘磁值和加速度值。

为从体坐标系到导航坐标系的姿态转换矩阵，用四元数表示，可以写成

其给出了导航坐标系中初始值与实时从体坐标系映射到导航坐标系的值之间的误差ε。

根据之前的定义，MSE方法可用于求取最优值。

通过求方程8的最小值：

对f（q）求导并使其等于零，

我们将获得方差意义上的最优q。我们使用高斯-牛顿方法，用一阶梯度收敛来求解以上非线性方程。

通过组合角速率，我们将得到观测变量

其中融合了卡尔曼滤波器中的罗盘数据和IMU数据。

该过程可以简单地描述为图5中的框图。

图5. 使用MSE方法的传感器融合框图。

松耦合

如前所述，我们经常遇到无法使用罗盘传感器的情况。如果磁数据受到干扰，则求解的姿态精度将比仅使用IMU时更差。因此，我们使用松耦合来判断磁传感器是否可用。当磁传感器不可用时，我们只用IMU来求解姿态；当磁传感器可用时，我们将使用融合算法找到姿态，如图6所示。

图6. 姿态计算流程图。

在获得新数据之后或者在求解新的姿态时（在某些系统中，采样周期与姿态解算周期不同，但我们在此处进行的是单采样周期解算），我们计算加速度的大小，如果结果不等于1g， 我们 就不会使用加速器的输出进行姿态计算。然后我们计算罗盘输出的大小并将其与初始值进行比较。如果它们彼此不相等，我们就不会在此周期中使用地磁传感器的数据。当满足两个条件时，我们会使用卡尔曼滤波器并执行MSE融合。

使用ADIS16470进行航位推算（DR）

在导航中，航位推算是计算当前位置的过程，先使用先前确定的位置，然后在解算周期中基于已知或估计的速度或加速度更新该位置。这里将使用ADIS16470里的加速度计。基于上一节解出的姿态，我们可以得到捷联系统的移动方向，然后需要计算该方向上的距离，最后确定位置。

捷联航位推算需要使用基于加速度测量的比力方程来跟踪INS的位置。比力方程可以简单描述为等式10、等式11和等式12：

其中ae是地球坐标系里的加速度，ab 是体坐标系里的加速度，ve是地球坐标系里的速度，se是地球坐标系里的距离，ge 是 地球坐标系里的重力加速为［0 0 1］，单位为 g。需要强调的是，地球坐标系与导航坐标系不同——地球坐标系是基于NED的。该 δtt是解算周期。

用第一个等式可以得到从IMU体坐标系到地球坐标系的加速度映射，如格式

第二个等式将加速度积分或累加为速度；然而，由于测量的加速度包含了重力分量，所以需要减去重力。

与等式11类似，等式12将速度积分成距离。

传统方法存在几个问题。

● 加速度计输出总是有偏置，与重力相结合后，难以从公式10中减去，因此更准确的表达式应为：

除非是用一些专业设备来测量该偏置，例如分度头。

● 基于数值积分的实现方式，通常使用零阶保持器方法（前一个值）进行积分。但是，对于连续移动，这将带来重大的误差。例如，我们来比较以下方法：

方法1：

（零阶保持器）

方法2：

（线性插值）

在5秒内加速度为为0.5 m/s2时，位移最高将相差4m。仿真结果如图7所示。

图7. 速度计算中的零阶保持与一阶积分方法比较。

基于前面的讨论，基于应用，我们修改了传统比例方程中的两个地方：

X 我们不使用地球坐标作为导航坐标系。相反，正如我们在计算先前姿态时所做的那样，我们用初始姿态

作为导航坐标系。通过这种方式，偏置和重力都可以轻松取消，如公式14所示：

虽然姿态中同时包含了偏置和重力分量，但这样我们就不需要将它们分开成单独的分量，而是直接一起减去它们。

基于零阶保持器与一阶插值之间的比较，我们使用一阶方法来获得更准确的积分结果。

运动学模式和零速更新技术（ZUPT）

通过使用IMU的初始值作为导航坐标系，我们可以消除一部分加速度的偏置影响。然而，即使我们在使用设备之前能用分度头准确测量偏置，仍然很难取消，除非使用另一个精确的传感器来定期校准它。这主要是由两个原因引起的：一是偏置不稳定，这意味着我们之前测量的偏置不是现在的实际偏置。二是速度随机游走，由加速度噪声积分而来。前面提到的不良特性会使我们计算的距离显著漂移。即使我们停止移动并保持静止，从加速度积分而来的速度仍然存在，距离仍会增加。

要解决这个问题，我们需要找到一种通过使用ZUPT技术重置速度的方法。ZUPT技术与具体的应用密切相关，因此我们需要获得系统和应用的运动学特征，然后给出一些算法规则。我们发现的运动学模式越多，结果就越准确。

我们通过移动带有SINS系统的转椅来进行实验。由于我们的研究不限于特定应用，我们使用以下运动学假设：

● 对于航位推算，导航坐标系中没有z轴移动。此限制仅适用于航位推算，不适用于姿态求解。显然，我们是在二维空间中移动系统。这有助于消除z轴误差。

● 所有转弯都发生在停止后。如果在移动时发生转弯，则会因为引入额外加速而干扰姿态求解。

● 如果系统正在移动，加速度不能保持不变超过500毫秒。速度不能保持不变超过2秒。由于我们在推动或拉动转椅，因此很难手动使力精确地保持不变超过500毫秒，并且个人很难以匀速持续推动转椅2秒以上。事实上，我们正是运用这一规则来实施ZUPT。

● 加速度不能大于±1 m/s2。该规则用于一些噪音过滤，后者基于我们施加于椅子上、不会很大的拉力或推力。

如图8所示，当系统在X方向上移动时（投影到导航坐标系后），Y方向也会产生加速度；积分后，Y方向速度不会为零，这意味着即使我们只是在X方向上移动，航位推算系统仍然会给我们带来Y分量。

图8. 导航坐标系中三个方向的加速度。

基于第三条运动学假设，我们可以使用ZUPT来消除此误差。经ZUPT处理之后的积分速度如图9所示。

图9. 导航坐标系中三个方向的速度。

虽然我们使用了第三条假设，如前所示，误差仍然无法完全取消。误差消除取决于设定的零加速度和零速度的判断阈值。但是，大多数误差已得到修正。

虽然使用了ZUPT，但有时仍然无法达到零速。这由两个因素导致：

● 我们无法用ZUPT完全消除偏置不稳定误差和速度随机游走。

● 我们求出的姿态有一些误差，结果将导致投影（从体坐标系到导航坐标系）后的加速度误差。

以图10为例。图10中的左图是ADIS16470的原始数据（体坐标系），图10中的右图是投影到导航坐标系的加速度。可以看出，停止移动时，投影加速度不为零。由于它总是在变化，我们此处称之为基线漂移。

图10. 体坐标系（左）和导航坐标系（右）的加速度。

为了消除基线漂移，我们需要实时连续获得偏移偏置并从投影加速度中减去该值。结果如图11所示

Figure图11. 基线漂移消除之前（上）和之后（下）的加速度。

上图是基线漂移消除前的加速度，下图中的绿色轨迹是我们计算的基线偏移，红色轨迹是基线偏移消除后的加速度。

可以使用图12中的框图简要描述航位推算过程。我们将体坐标系加速度ab和姿态转移矩阵（来自AHRS）输入

到DR系统。完成后，我们将获得导航坐标系中的位置。

图12. 航位推算流程图

实验结果与结论

实验结果

使用SPI端口，我们将ADIS16470评估板和RM3100罗盘评估板连接到ADI公司的ADuCM4050电路板，构建出我们的系统，如图13所示。ADuCM4050 调整数据格式并进行时间同步（因为IMU和罗盘的数据速率不同）。然后使用UART将捕获的数据传输到计算机。所有计算（包括校准、AHRS和DR在MATLAB）均在MATLAB®中执行。

图13. 实验平台设置。

将评估板和计算机放在转椅上，并在实验室中推着转椅绕圈。

● AHRS输出：姿态以四元数格式和DCM格式表示，如图14所示。

图14. 四元数格式（左）和DCM格式（右）的姿态。

● DR输出：带XYZ位置的航位推算结果和三维图如图15所示。

图15. 位置计算结果。

结论

本文介绍了使用ADI公司的IMU ADIS16470和地磁传感器RM3100构建捷联惯导系统的基本过程，介绍了我们使用的校准、AHRS和DR方法。在平台和实验环境等条件有限的情况下，很难进一步测试平台和算法。

有很多方法可用于改善结果，例如：

● 使用里程表或UWB距离测量方法与IMU中的加速度计融合，以在DR中获得更准确的距离值。

● 使用更复杂的运动学模型，从而在AHRS和DR中在传感器和系统层次引入更多特性，例如系统的振动、加速和减速模型、地面平整度等。这意味着为了提高导航结果的准确性，需要给出更多的边界条件。

● X 使用更精确的数值计算方法，比如用辛普森规则或三次样条插值在DR中进行积分，或者使用牛顿方法而非高斯-牛顿方法求解非线性MSE方程等。

最后但也是最重要的一点，我们在试验中发现INS与应用或运动学模型紧密相关。例如，我们在两个地方进行了实验：未铺地毯的实验室和铺有地毯的办公室。如果我们使用相同的参数集，DR结果会显示出巨大的差异。因此，无论哪种应用，例如患者跟踪、AGV导航或停车定位，或者对于同一应用中的不同条 件，我们都需要全面了解其运动学模型。