-
开放科学（资源服务）标识码（OSID）：
-
惯性测量单元(Inertial Measurement Unit，IMU)是农业机械(农机)导航定位与农机具测控系统的核心模块[1-3]。受农机田间作业振动状况和机具作业负载变化的影响，农机具的自适应调控对其姿态测量动态性能提出新需求，亟待基于低成本IMU研究高精度姿态测量方法[4-5]。载体姿态变化一般基于陀螺仪角速率积分或加速度计比力获得，前者存在未知初始姿态和陀螺仪偏置导致的姿态漂移问题，后者则受载体外部运动加速度的影响较大[6]。由于微机电系统(Micro-Electro-Mechanical System，MEMS)惯性器件噪声特性和环境漂移误差非常复杂，研究利用加速度计、磁力计与陀螺仪多源信息融合的载体姿态估计受到广泛关注[7-8]。
基于陀螺仪和加速度计姿态误差更新频率的互补特征，互补滤波(Complementary Filter，CF)在计算资源受限平台应用广泛[9-10]，由于采用固定增益系数进行信息融合，CF收敛速度受初始姿态值影响较大，且其在姿态动态估计方面的性能有待改善[11]。卡尔曼滤波(Kalman Filter，KF)及其扩展算法在姿态动态估计上较CF有显著优势，其采用加速度计和磁力计感知载体重力场和磁场信息，并与导航系理论模型导出量构造量测残差，补偿陀螺仪姿态更新漂移误差[12-13]。虽然CF运算量较小且易于部署，但KF模型匹配灵活更适用于载体动态姿态估计，因此本研究基于KF开展农机具振动工况下的姿态测量。
KF姿态估计受IMU偏置、时变量测噪声和外部加速度影响较大，尤其MEMS-IMU的随机噪声和偏置会造成姿态估计快速发散。为提高MARG模块姿态估计精度，林新华等[14]采用序贯Sage-Husa估计量测噪声抑制磁场干扰和载体线加速度对姿态估计的影响；乔美英等[15]基于滤波新息序列卡方检验构造模糊边界修正准则，自适应修正容积卡尔曼滤波(Cubature Kalman Filter，CKF)量测噪声与状态预测协方差。为降低IMU姿态滤波对外部加速度的敏感性，储开斌等[16]采用CF与扩展卡尔曼滤波(Extended Kalman Filter，EKF)级联方式抑制外部加速度干扰，同时采用Mahony滤波对陀螺仪、加速度计和磁力计输出进行预滤波，并以CF输出作为EKF时间更新初值；刘春等[17]在CF与EKF级联滤波中引入强跟踪滤波和噪声自适应，进一步改进EKF融合效果；Ding等[18]提出基于IMU输出序列滑动窗口检测的方法，根据设定阈值自适应调整加速度计量测KF更新中的权重，然而其采用单一尺度对加速度三轴外部干扰同步调整，同时受陀螺仪积分计算姿态漂移的影响，因此先验固定阈值无法适应长时间干扰存在的场景。
为解耦三轴加速度计受外加速度干扰的影响，Candan等[19]提出加速度计量测噪声多尺度自适应估计方法，利用滤波新息方差计算外加速度干扰对应的噪声方差，然而量测残差不仅来源于外加速度干扰，还包括陀螺仪偏置，因此随着KF的迭代更新，新息中外加速度干扰对应的不确定性低于预测状态偏差的对应值。农机具受到连续振动干扰时，受状态值或滤波新息不确定性逐渐增加的影响，现有自适应滤波方法难以用于长时间的外加速度干扰抑制。Javed等[20]利用地球磁场约束构造加速度计量测约束模型，提出了无陀螺仪载体姿态估计方法，但农机作业环境中无法避免磁场的干扰。受磁场量测约束模型的启发，本研究在加速度计噪声方差自适应估计的基础上，利用农机具运动约束构造MEMS-IMU姿态估计的级联卡尔曼滤波(Cascaded Kalman Filter，CaKF)结构，改善其姿态估计中外加速度干扰的补偿效果，并通过精密转台角度跟踪和拖拉机田间犁耕试验验证所提方法的有效性。
HTML
-
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}$ 影响量测更新新息权重，滤波估计必须动态调整以匹配加速度输出的不确定性。
1.1. 姿态计算
1.2. 姿态估计模型
-
由式(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所示
2.1. 平面运动约束建模
2.2. 级联自适应滤波框架
-
采用三轴连续转动转台对本研究提出的级联带约束抗干扰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°/s2做正弦摇摆运动，转台外轴旋转至角度为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能更好地抑制外部加速度的干扰。
3.1. 转台试验
3.2. 犁具姿态测量
-
针对农机作业复杂工况下机具高精度姿态估计的需求，提出一种基于平面加速度运动约束的姿态测量CaKF估计方法，分别采用三轴转台和拖拉机田间耕地试验验证CaKF的有效性。转台试验表明CaKF角度跟踪优于KF和RKF，其横滚角RMSE由RKF的0.53°降低为0.42°。拖拉机耕地作业试验表明CaKF较KF在俯仰角估计上性能提升32.2%，且较RKF能更好地抑制耕作犁具长时间外部加速度的干扰。后续将围绕犁具耕深测量与控制中不同干扰对应的姿态误差建模与补偿开展研究。
DownLoad: