基于IMU和地磁传感器的捷联惯导系统

摘要

本文介绍了我们使用模拟设备的惯性测量单元（IMU）传感器ADIS16470和PNI的地磁传感器RM3100构建的捷联惯性导航系统（SINS）。实现了基于磁力，角速率和重力（MARG）的SINS的一些基本过程，包括电磁罗盘校准，使用扩展卡尔曼滤波器（EKF）的姿态和航向参考系统（AHRS）以及跟踪跟踪。还实现了使用最小平方误差（MSE）方法松散耦合传感器融合。显示了每个过程步骤中的算法和实验设置。本文最后讨论了结果分析和提高精度的方法。

简介

随着服务机器人市场和技术的发展，导航成为一项热门研究和应用点。与车辆，船舶或飞机相比，服务机器人体积小，成本低，因此它们的导航系统应该是捷联和低成本的。传统的稳定平台导航系统通常涉及单独的加速器和光纤或基于激光的陀螺仪，所有部件都机械且刚性地安装在与移动车辆隔离的稳定平台上。这导致尺寸大，可靠性差和成本高的缺点。相反，在捷联导航系统中，惯性传感器直接固定在车辆的车身上，这意味着传感器与车辆一起旋转。这种捆绑方法消除了稳定平台导航的缺点。但是，平台导航的准确性通常高于SINS。平台导航通常可以达到战略等级（0.0001°/小时陀螺仪偏差，1μ g 加速器偏压）或军用级（0.005°/ hr陀螺仪偏差，30μ g 加速器偏差），而大多数SINS只能达到导航等级（0.01°/ hr陀螺仪偏差，50μ g 加速器偏压）或战术等级（10°/ hr陀螺仪偏差，1 m g < / em>加速器偏差）。对于大多数服务机器人或AGV导航应用来说，这已足够。

有多种导航方法，包括机器视觉，GPS，UWB，带SLAM的激光雷达等。惯性导航始终是导航的重要组成部分，使用IMU的地方。然而，由于这种传感器的限制 - 例如偏置误差，横轴误差，噪声，特别是偏置不稳定性 - 惯性导航通常需要伙伴传感器定期给它一个参考或校准，这称为传感器融合这里。有许多传感器可以与IMU融合，例如摄像机和里程表，但在这些传感器中，地磁传感器是与IMU一起获得态度的低成本方式。

在本文中，我们使用ADI的IMU，ADIS16470和地磁传感器，用于开发平台和算法，以实现捷联惯性导航系统。但是，传感器只能提供姿态信息。对于航位推算或距离测量，我们只能使用IMU中的加速度传感器。

ADIS16470 IMU简介

ADI公司的ADIS16470是一款集成了3轴陀螺仪的微型MEMS IMU和一个3轴加速度计。它为陀螺仪提供8°/ hr的偏置稳定性，为加速度计提供13μ g 偏置稳定性，同时其关键参数经过工厂校准。此外，ADIS16470的低成本价格在同级产品中具有吸引力，并被许多客户广泛使用。在本文中，我们使用微控制器与ADIS16470通过SPI接口进行通信。

地磁传感器介绍

地磁传感器是一种用于测量罗盘体内地磁场的传感器坐标（即框架），它为标题提供绝对参考。其x，y和z分量值从本地地磁场投射。这种传感器有两个主要缺点 - 一个是它的精度和分辨率不高 - 例如，霍尼韦尔常用的罗盘传感器HMC5883L只有12位分辨率。另一个缺点是传感器容易受到周围环境的干扰，因为地磁场非常弱，从毫高斯到8高斯。

尽管有这些缺点，它仍然可以在许多情况下使用，例如开放场，低EMI环境等。通过将地磁传感器松散地耦合到IMU，我们可以在大多数环境中使用它。

< p>在本文中，我们使用高性能罗盘传感器，PNI传感器公司的RM3100，提供24位分辨率。 PNI使用主动激励方法来提高抗噪声能力。

罗盘传感器的校准

在使用罗盘传感器之前，需要对其进行校准以消除两个主要误差。一个是偏移误差，这最初是由传感器和电路的偏移误差引起的。另一个是比例误差。这两种误差都容易受到周围磁环境的干扰。例如，如果向传感器施加x轴方向外部磁场，则会发出外部x轴偏移误差。同时，x轴刻度也将与y轴和z轴不同。

通常用于校准磁传感器的方法是将传感器旋转为xy平面中的圆，然后提取数据。一个地方的地磁场强度是一个常数值，因此绘制的数据应该是一个圆;然而，事实上，我们将看到一个椭圆形，这意味着我们需要将椭圆移动并重新缩放到以零为中心的圆。

上面提到的2D校准方法有一些缺点，需要一个加速器衡量它的倾向。我们使用3D球面拟合方法来校准罗盘传感器。首先，我们需要将传感器旋转到x-y-z空间中的每个方向，并在3D坐标中绘制其值。然后我们需要使用最小平方误差（MSE）方法将数据拟合为椭圆体。

椭球方程可以表示为

其中X，Y和Z是罗盘输出三个方向的地磁分量。将这些值拟合为椭圆体意味着我们需要获得系数的最佳值集。我们将系数定义为：

在拟合时，我们将向量定义为：

所以我们需要计算最佳σ，并使用公式2来找到最小值：

这样我们就可以得到如图1所示的拟合结果。

要校准传感器，我们需要拉伸和将拟合的椭球移动到以零为中心的球体。我们使用矩阵奇异值分解（SVD）方法进行校准。后校准球如图2所示。 1,2

校准后，我们可以看出测得的磁场强度（球半径）将保持几乎恒定的值，如图3所示。

使用ADIS16470和指南针的姿态和航向参考系统

AHRS由三个轴上的传感器组成，提供姿态信息，包括滚转，俯仰和偏航。 AHRS是飞机导航的概念。我们用它来描述方向;也就是说，态度。

在介绍我们的方法之前，有必要首先解释融合的原因。实际上，我们现在的系统中有三种传感器：陀螺仪，加速器和罗盘。

陀螺仪给出了围绕每个轴的旋转角速度。通过积分角速率，我们可以找到旋转角度。如果我们知道最初的标题，我们将始终获得标题态度。由于积分，陀螺仪的不稳定偏差将被累积，这将导致角度误差。此外，来自陀螺仪的高斯分布噪声将被整合到布朗运动过程中并导致随机行走误差。因此，我们很难长时间使用陀螺仪，并且陀螺仪需要定期校准。

加速器提供每个轴方向的加速度。在静态状态下，我们可以得到每个轴与重力加速度之间的角度。由于重力加速度在方向和值上是恒定的，我们可以获得相对于重力方向的航向姿态。然而，这种方法使用重力加速度作为参考，因此无法解决围绕重力加速度旋转的角度。

罗盘提供从地磁场投射的每个轴的值。我们可以从每个轴与地磁场方向之间的关系推导出角度值，地磁场方向也是一个常数向量。如前一节所述，由于对外部磁场的抗扰性较差，罗盘需要一个低干扰环境。

从这个解释中，我们可以看到很难依靠只有一个传感器来寻找态度，我们需要结合使用两个或三个传感器并融合信息。本文涉及加速器，陀螺仪和地磁罗盘以找到态度。这种融合称为磁，角速率和重力（MARG）系统。

扩展卡尔曼滤波器设计和传感器融合

将IMU和罗盘数据融合在一起有多种方法，例如互补滤波器，统计ARMA滤波器，卡尔曼滤波器等。我们在本文中使用扩展卡尔曼滤波器。

首先，我们需要介绍本文中使用的一些定义。

坐标定义

标题或方向是两个坐标之间的关系（是，框架）。一个坐标总是在变化，另一个坐标保持不变。对于坐标定义方法，我们使用导航坐标和身体坐标。与东北向下（NED）或地理方法相反，我们将测量的初始身体坐标值定义为导航坐标，之后是恒定坐标。从主体坐标到导航坐标的映射（投影）矩阵定义为

姿态定义

与欧拉角或方向余弦矩阵（DCM）矩阵不同，我们在这里使用四元数，定义为

，它通常用于导航以避免参数。 3

使用卡尔曼滤波器更新姿态< / h3>

运动学方程，即状态转移方程，我们在本文中使用的是偏差格式，它不是线性的，因此我们需要使用EKF和偏差方程的一阶线性逼近。对于EKF设计，我们定义

1×7向量作为状态变量，其中

是角速率，

是态度四元数。

一个1×7向量，是观察变量，它与状态变量具有相同的成分。

一个7×7矩阵，是状态转移矩阵，其中 A 中的第一部分是角速率的数字化微分方程，第二部分是数字化四元数更新方程，它是从运动学方程导出的。

7×7矩阵，是观察矩阵。

是误差协方差矩阵，一个7×7矩阵，其中

是估计的误差向量， x ，来自真实， X 。我们在测试中将初始误差设置为相对较小的值。它将自动收敛到一个小值。

被设置为状态转换噪声和观察噪声的噪声协方差。我们通过测量陀螺仪和加速器的交流有效值的平方来得到它们的初始值，

和

。在保持IMU和罗盘处于静止状态的同时，我们设置

根据这个定义，我们的卡尔曼滤波器将通过以下五个步骤完成：

步骤1：使用公式3计算卡尔曼增益 K ：

步骤2：计算误差协方差矩阵， P ：

步骤3：输出估计状态， x ：

步骤4：预测状态，< em> x - ：

步骤5：投影误差协方差矩阵 P - ：

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

基于MSE的传感器融合

在上一节中，观察变量是

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

我们将变量定义如下：

m b 和 a b ：车身框架中的罗盘磁性值和加速度值。

m n 和 a n ：导航框中的罗盘磁性值和加速度值。

m n0 和 a n0 ：导航框架中的初始静态罗盘磁性值和加速度值。

是从身体框架到导航框架的姿态转换矩阵，用四元数表示，可以写成

，它给出了错误ε，导航框架中的初始值和实时从主体框架映射到导航框架的值。

根据之前的定义，MSE方法可用于获得最佳

通过最小化公式8：

通过推导 f（q）并使其等于零，

< img src>

我们将获得方差意义上的最佳 q 。我们使用Gauss-Newton方法用一阶梯度收敛来求解上面的非线性方程。

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

，它将罗盘数据和IMU数据融合在卡尔曼滤波器中。

过程可以是简单地用图5中的方框图描述。

松散耦合

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

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

航位推算（DR）使用ADIS16470

在导航中，航位推算是计算当前电流的过程通过使用先前确定的位置来定位，并基于解析周期内的已知或估计的速度或加速度推进该位置。这里将使用ADIS16470加速器。基于上一节中解决的态度，我们得到捷联系统的移动方向，然后我们需要计算方向上的距离，最终确定位置。

航位推算方法简介

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

其中 a e 是地球框架中的加速度， a b 是车身框架中的加速度， v e 是地球框架中的速度， s e 是地球框架中的距离， g e 是地球框架中的重力加速度，以 g 为单位为[0 0 1]。我们需要强调的是，地球框架与导航框架不同 - 地球框架是面向NED的。这个δ t 是解析周期。

第一个等式找到从IMU体框到地球框的加速度投影，如格式所示。

第二个方程将加速度积分或累加到速度中;然而，由于测量的加速度涉及重力分量，因此需要减去重力。

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

传统方法存在一些问题。

加速器输出总是有偏差，与重力相结合，使其难以在公式10中减去，因此更准确的表达应该是：
除非使用一些专业设备，例如作为分割头。

数值积分方法，传统方法，通常使用零阶持有者方法（前一个值）进行积分。但是，对于连续移动，这将引入重大错误。例如，让我们比较以下方法：

方法1：

（Zeroth-order holder）

方法2：

（线性插值）

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

基于前面的讨论，我们修改了传统特定力方程的两个点用于我们的应用：

我们不要将地球坐标用作导航框架。相反，正如我们在寻找先前态度时所做的那样，我们使用初始态度

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

虽然偏差和重力分量包含在初始姿态中，但我们这样做不需要将它们分开来获取每个组件，而是我们可以直接减去它们。

比较零阶保持器和一阶插值，我们使用一阶来获得更准确的积分结果。

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

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

要解决这个问题，我们需要找到一种方法来使用ZUPT重置速度技术。 ZUPT技术紧密依赖于应用，因此我们需要获得系统和应用的运动学特征，然后给出我们算法的一些规则。我们找到的运动学模式越多，我们的结果就越准确。

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

对于航位推算，导航框架中没有z轴移动。此限制仅适用于航位推算，不用于态度解决。显然，我们正在2D空间中移动系统。这有助于消除z轴误差。

停止后发生所有转弯。如果在移动时发生转弯，则会影响姿态分辨，因为将涉及额外的加速度。

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

加速度不能大于±1 m / s 2 。这个规则用于一些噪音过滤，这是基于我们在椅子上的拉力或推力，它不会很大。

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

基于第三个运动学假设，我们可以使用ZUPT来取消此错误。 ZUPT之后的积分速度如图9所示。

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

基线移位取消

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

我们不能完全取消使用ZUPT的偏差不稳定性误差和速度随机游走。

我们解决的态度有一些误差，将导致投影（从车身框架到导航框架）加速度误差。

以图10为例。图10中的左图是ADIS16470的原始数据（主体框架），图10中的右图是导航框架中投影的加速度。可以看出，当它停止移动时，预计的加速度不为零。由于它总是在变化，我们称之为基线变化。

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

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

可以使用图12中的框图简要描述航位推算过程。我们输入车身框架加速度 a b 和姿态转换矩阵（来自AHRS）

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

实验结果和结论

实验结果

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

通过将电路板和计算机放在转椅上实现实验在我们的实验室里把转椅推到一个圆圈里。

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

DR输出：航位推测结果XYZ位置和3D绘图如图15所示。

结论

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

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

使用里程表或UWB距离测量将加速器与IMU融合，以在DR中获得更好的距离。

使用更复杂的运动学模型，该模型涉及更多特征。 AHRS和DR中的传感器级别和系统级别，例如系统的振动，加速度和减速度模型，地面平整度等。这意味着为计算提供更多的边界条件，以获得更准确的结果。

使用更精确的数值计算方法，例如使用Simpson规则或三次样条插值在DR中进行积分，或者使用牛顿法代替Gauss-Newton方法来求解非线性MSE方程等。

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