IMU通过测量载体系(B系)相对惯性系(I系)的角速率和比力，获得载体相对于惯性空间的姿态与位置变化，并在导航系(N系)中进行姿态的数学表达。常用姿态更新方法包括：欧拉角、方向余弦矩阵(Direction Cosine Matrix，DCM)、四元数等。由于本研究仅考虑农机导航作业时的姿态角计算，其数值一般较小，不存在DCM姿态表达中的万向锁问题。定义B系至N系的变换矩阵为^[21]：

式中：α、β和γ分别为航向角、俯仰角和横滚角。

俯仰角和横滚角的表达式分别为：

其中：$C_{i j}$为$\boldsymbol{C}_{B}^{N}$的第$i$行、第$j$列元素。

设$I$系下重力加速度$\boldsymbol{g}^{N}=\left[\begin{array}{lll}0 & 0 & g_{e}\end{array}\right]$，其中$g_{e}$为当地重力加速度，则加速度计测量的重力加速度可通过下式计算：

由式(4)可知，$\boldsymbol{C}_{B}^{N}$最后一行不仅能计算$\beta$和$\gamma$，还可以基于当前状态量预测$B$系重力加速度$\tilde{\boldsymbol{g}}^{B}$，为构建线性KF滤波模型提供量测残差。选择$\boldsymbol{C}_{B}^{N}$最后一行向量${ }^z \boldsymbol{C}_k $作为$k$时刻的状态向量$\boldsymbol{x}_{k}$，即：

设k时刻陀螺仪和加速度计的量测输出为：

其中：$\boldsymbol{\omega}_{i b, k}^{b}、\boldsymbol{g}^{B}$为$k$时刻准确的角速率和当地重力加速度；$\boldsymbol{n}_{g, k}、\boldsymbol{n}_{a, k}$为对应传感器零均值高斯白噪声；$\boldsymbol{a}_{k}$为载体的外部加速度，采用一阶低通滤波白噪声过程表示为^[19]：

式中：$c_{a}$为0与1之间的常数；ε_k为载体变化的加速度建模误差。

设离散时间姿态滤波模型为：

其中：$\boldsymbol{x}_{k}$为$k$时刻状态向量；$\mathit{\boldsymbol{\varPhi}}_{k \mid k-1}$为系统状态转移矩阵；$\boldsymbol{z}_{k}$为量测向量；$\boldsymbol{H}_{k}$为系统量测矩阵；$\boldsymbol{w}_{k-1}$、$\boldsymbol{v}_{k}$分别为系统噪声和量测噪声向量。

基于陀螺仪输出进行滤波时间更新，采用一阶逼近求解DCM微分方程，即：

式中：$\Delta T$为陀螺仪采样周期；$\boldsymbol{I}_{3}$为3阶单位矩阵。

由于真实$\boldsymbol{\omega}_{i b, k}^{b}$无法获得，将式(6)代入式(11)得：

对比式(9)可得$\mathit{\boldsymbol{\varPhi}}_{k \mid k-1}=\boldsymbol{I}_{3}-\Delta T\left(\boldsymbol{\omega}_{i b, k}^{b} \times\right), \boldsymbol{w}_{k-1}=\Delta T\left(-{ }^z \boldsymbol{C}_{k-1} \times\right) \boldsymbol{n}_{g, k}$。设陀螺仪噪声方差$\boldsymbol{\Sigma}_{g g}= E\left(\boldsymbol{n}_{g, k} \boldsymbol{n}_{g, k}^{\mathrm{T}}\right)$，其中$E(a)$表示对变量$a$求期望，则系统噪声方差为：

基于加速度计量测进行量测更新，利用$\boldsymbol{g}^{B}=\left(\boldsymbol{C}_{B}^{N}\right)^{\mathrm{T}} \boldsymbol{g}^{N}=g_{e}{ }^{z} \boldsymbol{C}$，并将式(8)代入式(7)可得：

对比式(10)可得$\boldsymbol{z}_{k}=\boldsymbol{y}_{a, k}-c_{a} \boldsymbol{a}_{k-1}, \boldsymbol{H}_{k}=g_{e} \boldsymbol{I}_{3}, \boldsymbol{v}_{k}=\varepsilon_{k}+\boldsymbol{n}_{a, k}$，其中外加速度建模误差与加速度计量测噪声相互独立。式(12)和式(14)构成基于IMU姿态估计的KF滤波模型，其中加速度计量测中的外加速度建模误差通过系统量测方差$\boldsymbol{R}_{k}$影响量测更新新息权重，滤波估计必须动态调整以匹配加速度输出的不确定性。

由式(14)可知，用于姿态量测更新的加速度计数据，需要去除直接测量中的载体运动加速度。由于载体运动加速度无法直接测量，可以从量测残差中估计外部加速度，但其仅适用于短时载体外部加速度的补偿。为改善姿态估计算法在载体长时间变速运动时的可靠性，基于农机具运动约束提出级联滤波模型，将Z轴加速度计对应的量测更新从平面外的加速度数据扰动中隔离。载体静止或者无速度变化条件下，加速度计测量的重力加速度值为$g_{e}=9.81 \mathrm{~m} / \mathrm{s}^{2}$，即：

由于农机载体运动速度的变化多位于平面内，加速度计Z轴方向受外加速度的影响较小，存在：

加速度计输出量测为$\boldsymbol{y}_{a, k}=\left[\begin{array}{lll}y_{a x, k} & y_{a y, k} & y_{a z, k}\end{array}\right]^{\mathrm{T}}$，忽略$Z$轴加速度计的噪声影响，可构造约束方程为：

对比式(14)，当仅更新状态向量第3个元素$\boldsymbol{x}_{k}$(3)时，有：

可得：

类似地，可计算$\boldsymbol{R}_{k}=\boldsymbol{\Sigma}_{a z}+\boldsymbol{\Sigma}_{z z, k}$，其中$\boldsymbol{\Sigma}_{a z}$为$Z$轴加速度计的量测噪声，$\boldsymbol{\Sigma}_{z z, k}$为$Z$轴加速度计对应的外加速度建模不确定度。将更新后的$\boldsymbol{x}_{k}$（3）代入式（18）可得更新后的$\stackrel{\wedge}{y}_{a z, k}$。定义$g_{r e s}=g_{e}^{2}-\stackrel{\wedge}{y}_{a z, k}^{2}$并结合式(17)可得：

由$g_{x, k}=g_{e} \boldsymbol{x}_{k}$（1）、$g_{y, k}=g_{e} \boldsymbol{x}_{k}$（2）可得：

为建立预测量测$g_{r e s}$与状态向量的线性关系，将$f\left(\boldsymbol{x}_{k}\right)$在已知点$\boldsymbol{x}_{k}^{-}$处进行一阶泰勒展开：

对比式(10)可得：

式中：$\zeta_{k}$为泰勒展开引入的截断误差与重力常量比值。由于$\zeta_{k}$无法直接获得，需对其进行在线自适应估计。

基于KF的姿态滤波估计的实现步骤包括：

其中：式(25)、式(26)为时间更新，式(27)~式(29)为量测更新。泰勒展开数值截断误差通过$\boldsymbol{K}_{k}$影响量测新息在状态更新中的权重，且外加速度建模误差耦合到$\boldsymbol{x}_{k}^{-}$的计算中，必须根据量测噪声和外加速度变化对$\boldsymbol{R}_{k}$进行自适应调整。本研究采用文献[19]中提出的噪声方差多尺度自适应估计方法在线估计$\boldsymbol{R}_{k}$。定义新息$\boldsymbol{e}_{k}=\boldsymbol{z}_{k}-\boldsymbol{H}_{k} \boldsymbol{x}_{k}^{-}$，则有：

式中：$\boldsymbol{\Sigma}_{a a}$为加速度计量测噪声对应方差；$\Delta \stackrel{\wedge}{\boldsymbol{\Sigma}}_{k}$为噪声方差在线调整项。

当滤波信息发生显著变化时，即：

式中：$\operatorname{tr}(\boldsymbol{A})$表示取矩阵$\boldsymbol{A}$的迹。

利用历史新息序列计算平均新息方差以在线更新$\Delta \stackrel{\wedge}{\boldsymbol{\Sigma}}_{k}$，定义：

式中：$\mu$为新息序列长度。设新息方差与平均方差的偏差量为$\Delta$，由式(30)可得：

假设载体外加速度对平面内加速度计的量测值影响相互独立，则：

式中：$s_{i}=\max \left(0, \boldsymbol{S}_{i i}\right)$，其中$\max (a, b)$表示取$a$和$b$中的最大值，$\boldsymbol{S}_{i i}$为矩阵的第$i$个对角线元素，对应平面内加速度计的量测数据有i=1、2。

基于Z轴加速度计量测级联更新的IMU带约束抗干扰的姿态估计流程如图 1所示

采用三轴连续转动转台对本研究提出的级联带约束抗干扰CaKF算法的有效性进行测试，其中转台倾角回转误差为±5″，角位置定位精度为±4″，最小角速率为0.001°/s。基于荷兰Xsens公司MTI-300型航姿参考系统(AHRS)验证滤波算法跟踪转台姿态的变化效果，其内置陀螺仪零偏稳定性为10°/h，加速度计零偏稳定性为40 μg，标称姿态估计精度为0.2° RMS。如图 2所示，将MTI-300固定在工装上并安置于转台台面，为模拟地面载体行驶过程中外部振动引发的加速度干扰，转台内轴(对应MTI-300的Y轴)先静止，然后以角速率3°/s、角加速度3°/s²做正弦摇摆运动，转台外轴旋转至角度为10°的位置。MTI-300测量的转台运动角度变化如图 3所示，同步记录转台与MTI-300的输出数据，数据采样频率为200 Hz，其中转台数据采用外同步触发获取。运用KF和文献[19]中提出的RKF对比分析CaKF的性能，除CaKF中的约束辅助引入外部参数之外，3种滤波器参数配置一致。

转台横滚角角度估计结果如图 4所示，其中MTI-300和CaKF能较好地跟踪转台姿态变化。KF与RKF在角度突变处出现明显的跟踪误差，其中RKF仅基于滤波新息对噪声方差进行自适应，无法满足角速度较大情况下的角度估计，其估计结果与KF无明显区别。以MTI-300输出角度为参考基准，3种姿态测量方法下横滚角的均方根误差(Root Mean Square Error，RMSE)如表 1所示，其中CaKF角度跟踪的RMSE为0.42°，较RKF改善了20.8%。采用MATLAB内置计时函数对3种方法的运算耗时进行比较(计算机主频为2.9 GHz)，结果表明：KF单采样点运算耗时为20.8 μs，RKF单采样点运算耗时为26.4 μs，CaKF单采样点运算耗时为35.6 μs，即由于CaKF增加级联滤波更新步骤，其运算量大于RKF。

为进一步验证CaKF在田间实际工况下的性能，搭建如图 5所示的东方红1104型拖拉机犁耕作业平台。基于STM32F429微控制器和北斗星通KY-IMU102N-A0型MEMS-IMU自主研发姿态测量板卡，其陀螺仪零偏稳定性为12°/h(10 s平滑)，加速度计零偏稳定性为60 μg。将自研板卡和MTI-300固定于悬挂犁具上，IMU与MTI-300采样频率均为100 Hz。拖拉机以正常作业速度进行犁耕作业，在线采集自研板卡IMU的原始输出并进行后处理，同时与MTI-300输出姿态角进行对比分析。

拖拉机耕地过程中的犁具振动会对其俯仰角的估计产生影响，不同滤波算法的俯仰角估计结果如图 6、图 7所示。由于KF未对振动引起的外加速度进行补偿，其与MTI-300俯仰角区别较大。RKF采用自适应量测噪声估计减少外加速度对陀螺仪预测姿态校正的影响，估计误差较KF有明显改善。然而随着振动时间的增加，RKF滤波新息逐渐被状态估计误差污染，导致其抗干扰能力逐渐减弱，表现在图 7中RKF俯仰角误差随采样点数的增加而逐渐增大。基于Z轴加速度计振动数据隔离的CaKF较RKF更能有效地抑制长时间的外部振动干扰，以MTI-300输出作为俯仰角参考值，不同滤波方法的俯仰角估计RMSE如表 2所示。由表 2可知，CaKF较KF在犁具俯仰角估计上改善了32.2%，其在振动工况下且持续时间较长时较RKF能更好地抑制外部加速度的干扰。

针对农机作业复杂工况下机具高精度姿态估计的需求，提出一种基于平面加速度运动约束的姿态测量CaKF估计方法，分别采用三轴转台和拖拉机田间耕地试验验证CaKF的有效性。转台试验表明CaKF角度跟踪优于KF和RKF，其横滚角RMSE由RKF的0.53°降低为0.42°。拖拉机耕地作业试验表明CaKF较KF在俯仰角估计上性能提升32.2%，且较RKF能更好地抑制耕作犁具长时间外部加速度的干扰。后续将围绕犁具耕深测量与控制中不同干扰对应的姿态误差建模与补偿开展研究。