使用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和罗盘处于静止状态的同时，通过测量陀螺仪和加速器的交流均方根值的平方得到。我们设

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

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

图4. 用于更新姿态的卡尔曼滤波器流程图。

基于MSE的传感器融合

在上一节中，观测变量是

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

我们将各变量定义如下：

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

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

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

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

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

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

通过求方程8的最小值：

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

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

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

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

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

松耦合

如前所述，我们经常遇到无法使用罗盘传感器的情况。如果磁数据受到干扰，则求解的姿态精度将比仅使用IMU时更差。因此，我们使用松耦合来判断磁传感器是否可用。当磁传感器不可用时，我们只用IMU来求解姿态；当磁传感器可用时，我们将使用融合算法找到姿态，如图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中减去，因此更准确的表达式应为：

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