简介
随着服务机器人市场和技术的发展，导航已成为研究和应用中的一个热点。与车辆、船舶或飞机相比，服务机器人体积小，成本低，因此它们的导航系统应该具有捷联和低成本的特点。传统的稳定平台导航系统通常要采用独立的加速度计和光纤或激光陀螺仪，所有传感器都机械且刚性地安装在与正在移动的车辆隔离的稳定平台上。这导致了尺寸大、可靠性差、成本高的缺点。相反，在捷联惯导系统中，惯性传感器直接固定在车辆本体上，这意味着传感器会与车辆一起旋转。这种捷联方法消除了稳定平台惯导的缺点。然而，平台惯导的准确性通常高于SINS。平台惯导往往可以达到战略级（0.0001°/时的陀螺仪偏置，1μg的加速器偏置）或军用级（0.005°/时的陀螺仪偏置，30μg的加速器偏置），而多数SINS只能到达导航级（0.01°/时的陀螺仪偏置，50μg的加速器偏置）或战术级（10°/时的陀螺仪偏置，1mg的加速器偏置）。对于大多数服务机器人或AGV导航应用，这一精度足够了。
导航方法很多，包括机器视觉、GPS、UWB、SLAM型激光雷达等。基于IMU的惯性导航始终是导航的重要组成部分。然而，由于这种传感器的限制——例如偏置误差、轴间误差、噪声，特别是零偏不稳定性——惯性导航通常需要采用一个伙伴传感器，定期为它提供参考或校准，本文将这种情况称为传感器融合。许多传感器都可以与IMU融合，例如摄像头和里程表，但在这些传感器中，地磁传感器是一种低成本的方案，可与IMU配合获得姿态信息。
在本文中，我们使用ADI的IMU ADIS16470和地磁传感器来开发平台和算法，实现捷联惯性导航系统。但是，地磁传感器只能提供姿态信息。对于航位推算或距离测量，我们只能使用IMU中的加速度传感器。
ADIS16470 IMU简介
ADI公司的ADIS16470是一款微型MEMS IMU，集成了3轴陀螺仪和3轴加速度计。其陀螺仪零偏稳定性为8°/时，加速计零偏稳定性为13μg 其关键参数都经过出厂校准。此外，ADIS16470的 低价格在同级产品中具有吸引力，得到了许多客户的广泛使用。在本文中，我们使用微控制器与ADIS16470通过SPI接口进行通信。
地磁传感器介绍
地磁传感器是用于测量罗盘体坐标（即坐标系）中的地磁场的传感器，可为航向提供绝对参考。其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）方法将数据拟合为椭球面。
椭球方程可以表示为
其中，X、Y和Z是罗盘输出在三个方向上的地磁分量。将这些值拟合为椭球面意味着，我们需要得到一组最优系数解。我们将系数定义为：
在拟合时，我们定义向量：
所以我们需要计算最优σ，并使用公式2来找出最小值：
这样我们就可以得到图1所示的拟合结果。
图1. 原始罗盘数据分布（左）和使用椭球拟合后的罗盘数据（右）。
为了校准传感器，我们需要拉伸或压缩拟合的椭球面并将其移至以零为中心的球面上。我们使用矩阵奇异值分解（SVD）方法来进行这种校准。校准后的球体如图2所示。1，2
图2. 用SVD方法进行球体校准后的罗盘数据。
校准后，我们可以看到，测得的磁场强度（球半径）几乎恒定不变，如图3所示。
图3. 校准前和校准后的磁场比较。
使用ADIS16470和罗盘的姿态和航向参考系统
AHRS由三个轴上的传感器组成，提供姿态信息，包括横滚角、俯仰角和偏航角。AHRS是一个来自飞机导航的概念。我们用它来描述方向，即姿态。
在介绍我们的方法之前，有必要首先解释为什么确定姿态需要进行融合。事实上，我们的系统现在有三种传感器：陀螺仪、加速器和罗盘（地磁传感器）。
陀螺仪提供围绕各轴的旋转角速度。通过角速率积分计算，我们可以得到旋转角度。如果我们知道初始航向，通过角度就始终能够得到航向姿态。积分将累积陀螺仪的不稳定零偏，这将导致角度误差。此外，来自陀螺仪的高斯分布噪声将积分成一个布朗运动过程，并导致随机游走误差。因此，我们很难长时 间使用陀螺仪，陀螺仪需要定期校准。
加速度计提供每个轴方向的移动加速度。在静态状态下，我们可以得到每个轴与重力加速度之间的角度。由于重力加速度在方向和值上恒定不变，我们可以获得相对于重力方向的航向姿态。然而，该方法使用重力加速度作为参考，因此不能解出围绕重力加速度旋转的角度。
罗盘提供从地磁场投影的每个轴的值。我们可以从每个轴与恒为常数向量的地磁场方向之间的关系推导出角度值。如前一节所述，由于对外部磁场的抗扰性较差，罗盘需要一个低干扰的环境。
从这一解释中，我们可以看到，很难靠一个传感器来找到姿态，我们需要组合使用两个或三个传感器并把信息融合起来。本文用加速度计、陀螺仪和地磁罗盘查找姿态。这种融合也被称为磁、角速率和重力（MARG）系统。
扩展卡尔曼滤波器的设计与传感器融合
有多种方法可以将IMU和罗盘数据融合起来，例如互补滤波器、统计学ARMA滤波器，卡尔曼滤波器等。我们在本文中使用的是扩展卡尔曼滤波器。
首先，我们需要介绍本文中使用的一些定义。
坐标定义
T航向或方向是两个坐标（即坐标系）之间的关系。一个坐标总在变化，另一个坐标保持不变。对于坐标定义方法，我们使用导航坐标和体坐标。与东北地（NED）坐标系或地理方法相反，我们将测量的初始体坐标值定义为导航坐标系，此后该坐标为恒定坐标。从体坐标到导航坐标的映射（投影）矩阵定义为
姿态定义
与欧拉角或方向余弦矩阵（DCM）不同，我们在这里使用四元数，定义为
常用于导航以避免奇异性。
用卡尔曼滤波器更新姿态
我们在本文中使用的运动学方程（即状态转移方程）是非线性微分方程，因此需要使用一个EKF，用于对该微分方程进行一阶近似。对于EKF设计，我们定义
一个1×7向量作为状态变量，其中
为角速率；
为姿态四元数。
一个1×7向量作为观测变量，与状态变量具有相同的分量。
一个7×7矩阵作为状态转移矩阵，其中，A的第一部分是角速率的数字化微分方程，第二部分是数字化四元数更新方程，后者从运动学方程推导而来。
一个7×7矩阵作为观察矩阵。
为误差协方差矩阵，这是一个7×7矩阵，其中
估计向量 x?真实值xx之间的误差我们在测试中将初始误差设为相对较小的值。该值会自动收敛到一个小值。
被设为状态转移噪声和观测噪声的协方差矩阵。我们得到它们的初始值，
在保持IMU和罗盘处于静止状态的同时，通过测量陀螺仪和加速器的交流均方根值的平方得到。我们设
根据以上定义，卡尔曼滤波器将通过以下五个步骤完成：
步骤1：使用公式3计算卡尔曼增益K
步骤2：计算误差协方差矩阵，P：
步骤3：输出估算状态x?：
步骤4：更新状态x?–：
步骤5：更新误差协方差矩阵P–：
该过程可以简单地描述为图4中的框图。
图4. 用于更新姿态的卡尔曼滤波器流程图。
基于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导航或停车定位，或者对于同一应用中的不同条 件，我们都需要全面了解其运动学模型。
责任编辑；zl