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

Joel Li Van Yang

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

导航方法很多，包括机器视觉、GPS、UWB、SLAM型激光雷达等。惯性导航始终是导航的重要组成部分，采用的是IMU。然而，由于这种传感器的限制——例如偏置误差、横轴误差、噪声，特别是偏置不稳定性——惯性导航通常需要采用一个伙伴传感器，定期为它提供参考或校准，本文将这种情况称为传感器融合。许多传感器都可以与IMU融合，例如摄像机和里程表，但在这些传感器中，地磁传感器是一种低成本的方案，可与IMU配合获得姿态信息。

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

地磁传感器介绍

地磁传感器是用于测量罗盘体坐标（即坐标系）中的地磁场的传感器，可为航向提供绝对参考。其x、y和z分量值由本地地磁场投影而来。这种传感器有两个主要缺点：一是精度和分辨率不高。例如，常用的霍尼韦尔罗盘传感器HMC5883L的分辨率仅为12位。二是传感器容易受到周围环境的干扰。因为地磁场非常弱，强度范围为毫高斯到8高斯。

尽管存在这些缺点，仍然可以在许多情况下使用，例如户外、低EMI环境等。将地磁传感器松散地耦合到IMU，就可以在大多数环境中使用这类传感器。在本文中，我们使用高性能罗盘传感器（PNI传感器公司的RM3100）提供24位分辨率。PNI使用主动激励法来提高抗噪声能力。

罗盘传感器的校准

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

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

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

椭球方程可以表示为

aX+bY+eZ+2fXY+2gXZ+2hYZ+（1）

2gX+2qY+2rZ+d=0

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

σ=[abcfghpqrd]

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

β=[XYZ2XY2XZ2YZ2X2Y2Z]

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

min（σββσ）（2）

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

为了校准传感器，我们需要拉伸拟合的椭圆体并将其移至以零为中心的球体上。我们使用矩阵奇异值分解（SVD）方法来进行这种校准。校准后的球体如图2所示。

校准后，我们可以看到，测得的磁场强度（球半径）几乎恒定不变，如图3所示。

使用ADIS16470和罗盘的姿态和航向参考系统

AHRS由三个轴上的传感器组成，提供姿态信息，包括翻滚角、俯仰角和偏航角。AHRS是一个来自飞机导航的概念。我们用它来描述方向，即姿态。

在介绍我们的方法之前，有必要首先解释为什么确定姿态需要进行融合。事实上，我们的系统现在有三种传感器：陀螺仪、加速器和罗盘。

陀螺仪提供围绕各轴的旋转角速度。通过角速率积分计算，可以得到旋转角度。如果我们知道初始航向，就始终都能得到航向姿态。出于积分的原因，我们可以累计陀螺仪的不稳定偏置，这将导致角度误差。此外，来自陀螺仪的高斯分布噪声将积分成一个布朗运动过程，并导致随机行走误差。因此，很难长时间使用陀螺仪，陀螺儀需要定期校准。

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

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

从这一解释中可以看到，很难靠一个传感器来找到姿态，我们需要组合使用两个或三个传感器并把信息融合起来。本文用加速器、陀螺仪和地磁罗盘查找姿态。这种融合称为磁、角速率和重力（MARS）系统。

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

有多种方法可以将IMU和罗盘数据融合起来，例如互补滤波器、统计ARMA滤波器，卡尔曼滤波器等。我们在本文中使用的是扩展卡尔曼滤波器。首先，我们需要介绍本文中使用的一些定义。

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

姿态定义：与欧拉角或方向余弦矩阵（DCM）不同，在这里我们使用四元数，定义为q，常用于导航以避免争议。

用卡尔曼滤波器更新姿态

我们在本文中使用的运动学方程〔即状态转移方程）是偏差方程而非线性方程，因此需要使用一个EKF，用于求该偏差方程的一阶线性近似值。对于EKF设计，我们提出以下定义

x=[ω q]

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

ω=[ωxωyωz]为角速率;

q=[abcd]为姿态四元数。

z=[ωq]

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

A=[1/T（ω-ω）1/2qω]

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

H=[1001]一个×X7矩阵作为观察矩阵。

P=E[eTe]为误差协方差矩阵，这是一个7×7矩阵，其中e为从真实的x值估算而来的误差向量x。我们在测试中将初始误差设为相对较小的值。该值会自动收敛到一个小值。

R，Q被设为状态转移噪声和观察噪声的噪声协方差。我们得到它们的初始值，R和Q。

通过测量陀螺仪和加速器的交流均方根值的平方。在保持IMU和罗盘处于静止状态的同时，我们设R=Q

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

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

K=PH（HPH+R）（3）

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

P=（I-KH）P_k（4）

步骤3：输出估算状态X：

x=xK（z-Hx）

步骤4：投影状态x-：

x=Ax（6）

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

P=APA+Q（7）

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

基于MSE的传感器融合

在上一节中，观察变量是z=（ω q）

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

我们将各变量定义如下：

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

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

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

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

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

根据之前的定义，MSE方法可用于获得最优C（q）

通过求方程8的最小值：

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

f（q）/q=0（9）

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

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

z=[ω q]

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

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

松耦合

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

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

使用ADIS16470进行航位推算

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

航位推算方法介绍

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

a=（t）=Cab（t）

v（t+δt）=v+δt×（a（t+δt）-g（11）

s（t+δt）=s（t）+δt×（v（t+δt））（12）

其中，a是地球坐标系里的加速度，a是体坐标系里的加速度，v是地球坐标系里的速度，s。是地球坐标系里的距离，g是地球坐标系里的重力加速度，为[001]，单位为g。需要强调的是，地球坐标系与导航坐标系不同——地球坐标系是面向NED的。该δt是解析周期。

C用第一个等式可以得到从IMU体坐标系到地球坐标系的加速度投影，如格式C所示。

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

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

传统方法存在几个问题。

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

除非使用一些专业设备，例如分度头。

作为传统方法的数值积分方法通常使用零阶保持器方法（前一个值）进行积分。但是，对于连续移动，这将带来重大的误差。例如，

我们来比较以下方法：

方法1：

v=v+at

（零阶保持器）

方法2：

v=vat+a-a-1/2

（线性插值）

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

基于前面的讨论，我们根据应用要求，修改了传统比力方程的两个点：

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

基线偏移的消除

虽然使用了ZUPT，但有时仍然无法达到零加速度。这会导致两个问题：

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

我们求出的姿态有一些误差，结果将导致投影加速度误差。

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

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

图11是基线偏移消除前的加速度，底图中的绿色轨迹是我们计算的基线偏移，红色轨迹是基线偏移消除后的加速度。可以使用图12中的框图简要描述航位推算过程。我们将体坐标系加速度ab和姿态转换矩阵（来自AHRS）输入C到DR系统。完成后，我们将获得导航坐标系中的位置。

实验结果与结论

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

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

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

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

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

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

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

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

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

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

Silicon Labs和飞利浦秀推进智能照明

Silicon Labs和听诺飞（Signify），正在合作推广“秀之友（Friends of Hue）”的扩展项目，这使得生态系统合作伙伴能够更好地为飞利浦秀（Philips Hue）系统开发智能灯具开关。Silicon Labs提供Zigbee簇库、软件工具、参考应用和专业技术知识，以帮助由秀之友项目认证的成员公司快速设计和生产照明开关产品，确保与Philips Hue智能照明系统完美配合。

秀之友生态系统使得第三方供应商及品牌能够创建通过认证的、可互操作的智能照明产品，与领先的基于Zigbee的智能照明解决方案飞利浦秀（Philips Hue）无缝集成。SiliconLabs是秀之友的主要合作伙伴，与Signify密切合作，定义Zigbee簇并提供兼容软件以实现先进的照明开关功能，例如飞利浦秀应用（Philips Hue app）中对用户友好的设置以及通过飞利浦秀云部署固件更新。Silicon Labs的Zigbee软件开发套件支持秀之友规范，允许第三方供应商能够创建确保与飞利浦秀集成的认证设备。

通常当最终用户使用传统墙壁开关关闭智能灯时，灯的电源被关闭，并且不能使用应用程序、语音命令、日程安排或传感器进行控制。秀之友开关使得飞利浦秀智能照明控制可以直接从墙壁开关可靠控制，并保持飞利浦秀灯具处于“时刻准备好”的状态，同时提供人人都很熟悉的界面。Silicon Labs的秀之友软件开发套件支持各种智能灯开关设计，并为飞利浦秀用户添加了兼容性开关的选择。