农机具姿态倾角测量系统设计与试验

黄培奎，张智刚，罗锡文，刘兆朋，王 辉，林志健，高维炜



农机具姿态倾角测量系统设计与试验

黄培奎，张智刚※，罗锡文，刘兆朋，王 辉，林志健，高维炜

（1. 华南农业大学南方农业机械与装备关键技术教育部重点实验室，广州 510642；2. 华南农业大学工程学院，广州 510642）

农机具姿态倾角测量技术是实现农机装备精准作业的关键技术之一。为进一步提高农机装备作业质量，以ADIS16445微惯性MEMS传感器和STM32F446核心处理器搭建硬件平台，以欧拉角法解算姿态，建立卡尔曼滤波模型融合加速度计与陀螺仪信息，实现农机具姿态倾角的精准测量。融合算法模型考虑陀螺仪零偏特性，并根据MEMS微传感器运动特性，自适应模型误差协方差矩阵与，适应不同工况下农机具姿态倾角测量。采用SGT320E三轴多功能转台与BD982双天线定位测姿模块对系统进行测试与验证。三轴多功能转台试验结果表明，ADIS16445内置陀螺仪与加速度计性能合格，满足系统设计硬件要求；卡尔曼滤波融合模型精准有效，倾角静态测量误差精度为0.15°，动态测量精度典型值为0.3°，最大测量误差为0.5°。田间作业试验结果表明，自适应模型能保证农机具姿态倾角测量系统在不同工况下的测量精度，更稳定可靠，测量平均误差为0.55°。该文研究的农机具姿态倾角测量系统可满足农机装备精准作业要求。

机械化；算法；设计；农机具；倾角测量；多传感器融合；自适应卡尔曼滤波；欧拉角法

0 引 言

中国农业生产将向规模化与精细化方向发展[1-5]，规模化农业的发展必将带动宽幅农机装备更加广泛的应用，宽幅农业机械的精准导航行走控制与宽幅机具的调平控制均依赖于农机具姿态倾角的准确测量[6-8]。姿态倾角也是农机具运动学建模与农业机械安全预警研究的关键参数之一[9-15]。

农机具姿态倾角测量多采用惯性传感器、卫星导航系统和图像处理等方法获取，也有部分学者研究基于多传感器融合技术进行姿态倾角监测[16-20]。其中基于惯性传感器的农机具姿态倾角测量是目前应用最广泛的方法，其经济性、稳定性和适应性指标相比其他测量方法都具有突出优势[21-22]。

赵祚喜等[7-8,16]通过MEMS陀螺仪与加速度计信息进行了农机具的姿态倾角融合。其中赵祚喜等[7]将姿态倾角测量系统用于测定水田激光平地机平地铲实时倾角，融合算法测量误差一般不超过1°，能准确地检测平地铲倾角。但存在融合算法对加速度计与陀螺仪进行姿态倾角的切换不够精确、未能消除惯性传感器本身固有的偏移量影响等一些不足。马超等[16]将姿态倾角测量系统用于测定农田环境信息采集平台倾角并通过控制步进电机进行补偿修正，在田间颠簸路况也能保持最大误差在3.0°以内。但其使用互补滤波的融合算法过于简单，仅靠一个权重系数的调节难以满足实际作业测量需要，测量精度有限。胡炼等[8]将姿态倾角测量系统用于测定水田插秧机底盘横滚倾角并进行平地铲的调平控制，提升了激光平地机的控制精度。但融合算法中的误差协方差矩阵均为经验值，难以保证高速行走与转弯等工况下的测量精度、且其融合倾角为单轴，适应性有限。

为进一步提高农机具姿态倾角的测量精度及适应性，提升农机具作业质量，本文采用MEMS惯性传感器ADIS16445与STM32F446 ARM处理器设计农机具姿态倾角测量系统硬件平台，以欧拉角算法进行姿态解算，考虑陀螺仪零偏与漂移特性，建立卡尔曼滤波模型，自适应误差协方差矩阵与，实现各种工况下农机具姿态倾角的精准融合解算。并通过三轴多功能转台试验与田间试验对系统进行分析验证。

1 基本原理

1.1 欧拉角空间姿态解算算法

MEMS惯性陀螺仪经数值积分运算可获得传感器坐标系下敏感轴的姿态角，常用的姿态解算算法有欧拉角法、方向余弦法和四元数法[14,23]。四元素法解算需要进行泰勒展开，通常忽略其高阶项将非线性转化成线性进行姿态估算，存在误差。方向余弦法共有9个参数，计算量大，不适宜工程应用。欧拉角法是载体姿态信息解算最为直接的方式，通过3个微分方程直接解算，适用于地面作业农机具姿态倾角解算（不存在俯仰角奇异点情况）[24]。

刚体在三维空间的姿态变化可理解为从一个坐标系到另一个坐标系的变化，通过绕不同坐标系3次连续转动实现，即欧拉角法原理[25-26]。如图1所示，姿态测量系统原始坐标系为，当绕X轴转动时产生横滚倾角Roll，传感器坐标系变为11；当绕Y轴转动时产生俯仰倾角Pitch，传感器坐标系变为22。

图1 欧拉角法姿态解算原理图

根据欧拉角法的坐标旋转原理，欧拉角随时间的传递关系可利用MEMS陀螺仪测得的角速率表示。

式中ωωω分别为、、轴旋转角速率，rad/s；分别为横滚角、俯仰角与偏航角，rad。整理式（1）可推导出欧拉角算法的微分方程为

故根据欧拉角算法，只需求解相应的微分方程便可得到相应的姿态角。本文研究的农机具姿态倾角具体指的是横滚角与俯仰角，不包含偏航角。

1.2 准静态条件下加速度计测量倾角算法

MEMS加速度计处于准静态（静止或无外部加速度时）条件下时，只感应到重力加速度[27-29]。对于单轴加速度计，如图2a所示。

式中为测量倾角，(°)；与分别为单轴加速度测量值与重力加速度值，m/s²。故准静态时，由单轴加速度计可测量平面倾角。同理，通过三轴加速度计可对刚体在准静态条件下的空间姿态倾角变化进行测量。如图2b所示，假设三轴加速度计输出值分别为a、a、a，则准静态时有

式中a为三轴合加速度，m/s2；准静态时值为1 g，即本地重力加速度。

注：θ为测量倾角，(°)；a与g分别为单轴加速度测量值与重力加速度值，m·s-²；aX、aY、aZ、ag分别为三轴加速度计输出值与三轴合加速度，m·s-²。

结合欧拉角姿态解算算法对刚体空间姿态倾角的定义，对应图2b三轴加速度计模型有

1.3 卡尔曼滤波融合算法

卡尔曼滤波算法是一种最优化自回归的数据处理方法，能简单、高效地处理离散数据线性滤波的问题[30-33]。1960年由卡尔曼首次提出后不断改进，现已广泛应用于控制、图像处理、传感器信息融合等领域。卡尔曼滤波算法由系统状态方程与系统观测方程组成

式中x、u、z分别代表系统在时刻的状态向量、输入向量与测量向量；分别为系统的状态矩阵、输入矩阵与输出矩阵；w与v分别代表系统在（-1）时刻的状态噪声与时刻的测量噪声。系统的状态噪声和测量噪声彼此独立且定义为零均值且符合正态分布的高斯白噪声，其协方差矩阵Q和R分别定义为

卡尔曼滤波算法执行主要由时间更新和测量更新两部分组成，因此也被称作“预估—更正”法。具体执行流程如下：

2）时间更新，即“预估”法；

①计算先验状态估计

②计算先验误差协方差

3）测量更新，即“更正”法；

①计算卡尔曼增益

式中K为卡尔曼时刻增益，为状态输出矩阵。

②用测量值更新状态估计

③更新误差协方差

重复“预估—更正”过程，直到算法结束。

2 系统硬件和软件设计

2.1 系统总体设计

农机具姿态倾角测量系统由硬件平台与软件算法两部分组成，如图3所示。MEMS惯性传感器输出的三轴加速度计、三轴陀螺仪与温度等测量信息输入至核心处理器STM32F446RC中，进行软件算法的计算，包括初始化、卡尔曼滤波与倾角估计三部分，最后输出融合横滚角与俯仰角。

图3 系统总计设计

2.2 硬件平台设计

系统的硬件部分主要包括AMR核心处理器与MEMS微惯性传感器稳压电路，工作电路和串口通讯电路等。

2.2.1 ARM核心处理器

根据系统的设计要求，选用ST公司STM32F446RC 作为系统的核心处理器。该ARM核心芯片为32位处理器，具有180 MHz的最高处理频率可满足姿态倾角数据处理要求；具有512 kB闪存和双模四SPI接口、16位DMA与低功耗等特点[34]。

2.2.2 MEMS微惯性传感器

姿态倾角测量系统的测量精度首先取决于所选取的惯性传感器精度，然后才是倾角融合的处理。本文选取具有6自由度的ADIS公司16445作为惯性测量模块，内置一个三轴陀螺仪和一个三轴加速度计，均具备动态补偿功能；集成温度传感器和SPI通讯[35]。其中陀螺仪的最大测量范围为±250°/s，相应分辨率为0.01°/s/LSB；加速度计测量范围为±5 g，分辨率为0.25 mg/LSB；温度计分辨率为0.073 86℃/LSB。程序设置传感器的采样频率为100 Hz，能保证田间地形高低起伏；农机具振动、晃动、高速作业等复杂工况下横滚与俯仰倾角的准确测量。姿态倾角硬件平台实物图如图4所示。

图4 硬件平台实物图

2.3 软件算法设计

农机具作业时带动姿态倾角测量系统在三维空间运动，可利用陀螺仪与加速度计解算农机具实时姿态倾角。由MEMS微惯性传感器原理所限，如前文1.2所述，准静态情况下，通过三轴加速度计可以准确测量农机具姿态倾角。但农机具实际作业时难以保证匀速运动，向心加速度等外部加速度会影响姿态倾角解算。对于MEMS陀螺仪，除了固有零偏外还存在漂移特性，在动态、频率较高情况下通过角速度积分可获得较准确的倾角，但陀螺仪漂移的积分会随时间累积产生积分误差。因此实际应用常将2种传感器组合使用，通过滤波融合算法实现倾角的准确估计。常用的融合算法有互补滤波、梯度下降法与卡尔曼滤波算法[19,21]，其中基于卡尔曼滤波器的姿态融合算法应用最为广泛。

式中Δ为系统采样间隔0.01 s。

农机具姿态倾角测量系统具有动态时变特性，系统的状态噪声与测量噪声均跟随系统动态变化。为了提高姿态倾角融合精度，需根据系统运动特性自适应调整系统状态噪声协方差矩阵与测量噪声协方差矩阵。定义如下

MEMS微惯性传感器的带宽有限，系统状态噪声可随系统旋转运动大小（式（17））变化，故动态过程可将系统状态噪声与旋转大小做近似线性处理如下[34]

式中为轴陀螺仪输出，代表当前时刻。

对于本文系统，根据ADIS16445传感器特性，有max=433°/s；2high=0.05；2low=0.01。

系统的测量噪声具有时变特性，取决于系统的测量状态，系统实际作业时产生的外部加速度会造成测量噪声的增加。对于卡尔曼滤波模型，系统的初始化会对倾角融合精度产生影响。由于实际农机具田间作业前均需经过系统设计或启动预热阶段（此时为准静态），故可保证由三轴加速度计解算得到准确的姿态倾角初始值，模型初始化有效。此后可将系统测量噪声与外部干扰加速度做近似线性处理如下[31,36]

3 试验与结果分析

3.1 试验设备及方法

为验证农机具姿态倾角测量系统的精确性与适用性，本文采用SGT320E三轴多功能转台、BD982双天线定位测姿模块与雷沃ZP9500型高地隙喷雾机等设备进行试验验证。三轴转台是测试角运动参数的标准设备, 通过设置三轴运动参数模拟多种运动状态进而实现惯性传感器标定与姿态测量系统模型验证。BD982双天线定位测姿模块是由美国天宝公司生产的高精度定位测姿系统，姿态测量稳定性与测量精度高，广泛运用于各种工程测量领域，本文将其作为田间试验姿态测量对照。雷沃ZP9500型是一种高地隙宽幅喷杆喷雾机，喷杆长11.5 m，田间作业时喷杆姿态倾角变化大，适宜作为农机具姿态倾角测量系统田间试验平台。

3.2 三轴转台试验与结果

SGT320E型三轴多功能转台由机械台体、电控系统及连接电缆组成。转台台体结构采用U-O-O结构，三轴均可连续无限旋转，其速率分辨率为0.000 1°/s[37]。SGT320E型具有位置模式、速率模式和摇摆模式等3种转台运动模式，支持外同步触发串口输出，可实现转台数据与传感器数据的同步记录。试验中分别采用三轴转台位置模式对ADIS16445三轴加速度计进行标定校验；采用速率模式对ADIS16445三轴陀螺仪进行标定校验；采用摇摆模式对农机具姿态倾角测量系统的融合算法模型进行验证并对其融合横滚角与俯仰角的测量精度进行测试。如图5所示，将农机具姿态倾角测量模块安装在内轴中心，保证ADIS16445的3个敏感轴分别与三轴转台的3个转动轴平行。

图5 三轴转台试验现场图

3.2.1 ADIS16445标定

参考文献[37]所列举的6位置加速度计标定方法，采用三轴多功能转台位置模式对ADIS16445的三轴加速度计进行标定。传感器预热完成后采集试验数据，每个位置采集5 min，分别统计在6个不同位置下三轴加速度计输出的均值，结果如表1所示。

表1 加速度计各敏感轴取向与加速度输出

采用最小二乘法求解出ADIS16445三轴加速度计的标定系数矩阵为

由标定结果可知ADIS16445三轴加速度计的零偏值均在参考值0.075 mg（系数矩阵第4行为三轴加速度计零偏值）以内，加速度计性能符合试验要求。

参考文献[24], MEMS陀螺仪误差模型可表示为

式中为标度因素误差；B为与重力加速度无关的零偏；为陀螺仪零平均值随机零偏；为陀螺仪实际输出值。采用三轴多功能转台速率模式对进行ADIS16445的三轴陀螺仪标定。传感器预热完成后采集试验数据，设置转台以速率方式转动，每次运行5 min，每个敏感轴分别采集在速率为−200至200°/s（三轴转台的外框最高转动速率）。间隔为20°/s的ADIS16445陀螺仪数据，共计63组。采用数据拟合方法进行求解，如式（23）所示。

ADIS16445的三轴陀螺仪静态偏移（B）均在参考值0.15 °/s范围内，陀螺仪性能符合试验要求。

3.2.2 融合算法验证

采用信号发生器产生幅值为5 V，频率为10 Hz的方波同时触发三轴转台与农机具姿态倾角测量模块，并分别设置转台内轴与中轴以摇摆幅度为4°、摇摆频率为1 Hz摆动，模块预热完成后由静态零位置到摇摆运动模式模拟农机具实际作业时产生的横滚角与俯仰角，每组测试时长5 min，验证融合算法动静态测量精度，同时与普通陀螺积分方法进行对比。图6a为内轴摇摆产生横滚角，准静态测量误差的情况；图6b为中轴摇摆产生俯仰角，动态测量误差情况。

注：摇摆幅度为4°，摇摆频率为1 Hz。

由图6可知，在农机具实际作业时的倾角典型值（摇摆幅度为4°，频率1 Hz）的三轴转台摇摆测试中，利用普通的积分方法计算倾角在准静态时存在明显累计误差，陀螺仪漂移引起的误差随时间增长；动态误差普遍大于1°。如表2所示，基于卡尔曼滤波的融合算法能有效抑制有陀螺仪零偏与漂移引起的累计误差，倾角测量稳定精准。静态最大测量偏差为0.15°，动态最大测量偏差为0.5°。三轴转台试验结果即验证本文提出算法的优越性与有效性。

表2 基于卡尔曼滤波的融合算法的三轴转台倾角测试结果

注：典型值为多次试验统计最大偏差的平均值，下同。

Note: Typical value is the average value of multiple trial statistics max error，same as below.

3.3 田间试验与结果

雷沃ZP9500型自走式高地隙喷杆喷雾机（简称高地隙喷雾机）是雷沃重工股份有限公司2016年推向市场的一款植保机械，四轮驱动、最小转弯半径3.2 m，最高离地间隙1.1 m，喷幅11.5 m，广泛运用于水稻、小麦等种植全过程田间植保管理。

美国天宝公司生产的定位测姿模块BD982支持高精度定位、姿态和航向输出，稳定性高，动态响应快，当主从天线基线长度为10 m时其横滚/俯仰角动态测量精度可达0.05°。本文试验基线长度为1.4 m测量精度可达0.1°。广泛运用于工程机械，汽车，农业机械等领域。

田间试验于2017年6月在华南农业大学增城教学试验基地进行，采用高地隙喷雾机平台进行田间试验，并以BD982作为农机具倾角测量参照。如图7所示，将农机具姿态倾角测量模块与BD982安装于高地隙喷雾机顶部，农机具姿态倾角测量模块位于两天线中心。

1.ZP9500型高地隙喷雾机 2. 农机具姿态倾角测量系统 3. BD982双天线定位测姿模块

先进行横滚角试验，再将农机具姿态倾角测量模块水平旋转90°进行俯仰角试验（BD982双天线系统仅有1维倾角输出）。采用同步触发方式，在预热完成后开启高地隙喷雾机采集试验数据。试验结果分别如图8与图9所示。

图8 横滚角融合算法田间对比验证试验

图9 俯仰角融合算法田间对比验证试验

由图8与图9可知，带自适应的卡尔曼滤波算法能更好适应田间复杂工况下的应用，且具有更高的测量精度。田间试验以BD982作为农机具倾角测量参照，具体结果如表3所示。

表3 自适应卡尔曼滤波田间倾角测试结果

由表3可知，本文研制的农机具姿态倾角测量模块动态测量平均误差0.55°、最大误差小于0.91°，可满足农机具精准作业要求。

4 结论与讨论

1）本文设计了一种农机具姿态倾角测量系统，采用欧拉角算法进行姿态解算、自适应卡尔曼滤波算法融合加速度计和陀螺仪数据测量农机具实时倾角。

2）在三轴多功能转台上对MEMS加速度计与陀螺仪进行了标定验证，并测试倾角融合算法。测量静态最大偏差为0.15°，动态最大偏差为0.5°。

3）以雷沃ZP9500型高地隙喷雾机为平台、以BD982双天线定位测姿模块为参照进行田间试验。试验结果表明：带自适应的卡尔曼滤波算法能更好适应田间复杂工况下的应用，且具有更高的测量精度。动态测量平均误差0.55°、最大误差为0.91°，可满足农机具精准作业要求。

本文设计的农机具姿态倾角测量系统仍需进一步进行农机具实际作业倾角测量验证，并根据实际情况进一步优化融合算法。下一步研究还应考虑加速度计与陀螺仪本身误差模型，并尝试与视觉、全球导航卫星系统、激光雷达等传感器结合提高农机具姿态倾角测量精度。

[1] 中华人民共和国国务院，中共中央、国务院关于深入推进农业供给侧结构性改革加快培育农业农村发展新动能的若干意见[Z]. 2017-02-06.

[2] 李杨. 宽幅农业机械田间作业姿态监测方法[D]. 长春：吉林大学，2016.

Li Yang. An Attitude Monitoring Method for Broad Width Agricultural Machinery[D]. Changchun: Jilin University, 2016. (in Chinese with English abstract)

[3] 罗锡文，廖娟，胡炼，等. 提高农业机械化水平促进农业可持续发展[J]. 农业工程学报，2016，32(1)：1－11.

Luo Xiwen, Liao Juan, Hu Lian, et al. Improving agricultural mechanization level to promote agricultural sustainable development[J]. Transactions of the Chinese Society of Agricultural Engineering (Transactions of the CSAE), 2016, 32(1): 1－11. (in Chinese with English abstract)

[4] 罗锡文，廖娟，邹湘军，等. 信息技术提升农业机械化水平[J]. 农业工程学报，2016，32(20)：1－14. Luo Xiwen, Liao Juan, Zou Xiangjun, et al. Enhancing agricultural mechanization level through information technology[J]. Transactions of the Chinese Society of Agricultural Engineering (Transactions of the CSAE), 2016, 32(20): 1－14. (in Chinese with English abstract)

[5] Mousazadeh H. A technical review on navigation systems of agricultural autonomous off-road vehicles[J]. Journal of Terramechanics, 2013, 50(3): 211－232.

[6] 黄培奎，赵祚喜，靳俊栋，等. 基于AutoTrac的导航控制参数整定方法试验[J]. 农业工程学报，2015，31(增刊2)：100－106.

Huang Peikui, Zhao Zuoxi, Jin Jundong, et al. Navigation control parameter tuning method based on Auto Trac[J]. Transactions of the Chinese Society of Agricultural Engineering (Transactions of the CSAE), 2015, 31(Supp.2): 100－106. (in Chinese with English abstract)

[7] 赵祚喜，罗锡文，李庆，等. 基于MEMS惯性传感器融合的水田激光平地机水平控制系统[J]. 农业工程学报，2008，24(6)：119－124.

Zhao Zuoxi, Luo Xiwen, Li Qing, et al. Leveling control system of laser-controlled land leveler for paddy field based on MEMS inertial sensor fusion[J]. Transactions of the Chinese Society of Agricultural Engineering (Transactions of the CSAE), 2008, 24(6): 119－124. (in Chinese with English abstract)

[8] 胡炼，林潮兴，罗锡文，等. 农机具自动调平控制系统设计与试验[J]. 农业工程学报，2015，31(8)：15－20.

Hu Lian, Lin Chaoxing, Luo Xiwen, et al. Design and experiment on auto leveling control system of agricultural implements[J]. Transactions of the Chinese Society of Agricultural Engineering (Transactions of the CSAE), 2015, 31(8): 15－20. (in Chinese with English abstract)

[9] 李娜，姜海勇，张先鹏，等. 扇贝脏器气吸分离装置柔顺臂动力学特性分析[J]. 农业工程学报，2016，32(2)：244－251.

Li Na, Jiang Haiyong, Zhang Xianpeng, et al. Dynamic characteristic analysis of distributed-compliant arm in vacuum suction device for scallop viscera separation[J]. Transactions of the Chinese Society of Agricultural Engineering (Transactions of the CSAE), 2016, 32(2): 244－251. (in Chinese with English abstract)

[10] Kohanbash D, Bergerman M, Lewis K M, et al. A safety architecture for autonomous agricultural vehicles[C]// Dallas，Texas: American Society of Agricultural and Biological Engineers, 2012: 121337110.

[11] Wang Y, Li N, Chen X, et al. Design and implementation of an AHRS based on MEMS sensors and complementary filtering[J]. Advances in Mechanical Engineering, 2014, 2014(6): 1－11.

[12] Wang Y, Hussain A, Soltani M. A MEMS-based adaptive ahrs for marine satellite tracking antenna[J]. Ifac Papersonline, 2015, 48(16): 121－126.

[13] 邹波，张华，姜军. 多传感信息融合的改进扩展卡尔曼滤波定姿[J]. 计算机应用研究，2014，31(4)：1035－1038.

Zou Bo, Zhang Hua, Jiang Jun. Multi-sensor information fusion’s improved extended Kalman filter attitude determination[J]. Transations of Application Research of Computers, 2014, 31(4): 1035－1038. (in Chinese with English abstract)

[14] 刘佳妮. 基于MEMS器件的无人机姿态测量系统设计与实现[D]. 哈尔滨：黑龙江大学，2015.

Liu Jiani. Design and Implementation of UAV Attitude Measurement System Based on MEMS Device[D]. Harbin: Heilongjiang University, 2015. (in Chinese with English abstract)

[15] Burns R L. Dynamic safety envelope for autonomous-vehicle collision avoidance system: 6393362[P]. 2002-05-21.

[16] 马超，郑永军，谭彧，等. 基于MEMS传感器的两轴姿态调整系统设计与试验[J]. 农业工程学报，2015，31(增刊1)：28－37.

Ma Chao, Zheng Yongjun, Tan Yu, et al. Design of two-axis attitude control system based on MEMS sensors[J]. Transactions of the Chinese Society of Agricultural Engineering (Transactions of the CSAE), 2015, 31(Supp.1): 28－37. (in Chinese with English abstract)

[17] Li Y, Efatmaneshnik M, Dempster A G. Attitude determination by integration of MEMS inertial sensors and GPS for autonomous agriculture applications[J]. GPS Solutions, 2012, 16(1): 41－52.

[18] 王晓燕，陈媛，陈兵旗，等. 免耕覆盖地秸秆行茬导航路径的图像检测[J]. 农业机械学报，2009，40(6)：158－163.

Wang Xiaoyan,Chen Yuan, Chen Bingqi,et al.Detection of stubble row and inter-row line for computer vision guidance in no-till field[J]. Transactions of the Chinese Society for Agricultural Machinery, 2009, 40(6): 158－163. (in Chinese with English abstract)

[19] 胡静涛，高雷，白晓平，等. 农业机械自动导航技术研究进展[J]. 农业工程学报，2015，31(10)：1－10.

Hu Jingtao, Gao Lei, Bai Xiaoping, et al. Review of research on automatic guidance of agricultural vehicles[J]. Transactions of the Chinese Society of Agricultural Engineering (Transactions of the CSAE), 2015, 31(10): 1－10. (in Chinese with English abstract)

[20] Khot L R, Tang L, Steward B L, et al. Sensor fusion for improving the estimation of roll and pitch for an agricultural sprayer[J]. Biosystems Engineering, 2008, 101(1): 13－20.

[21] Crassidis J L, Markley F L, Cheng Y. Survey of nonlinear attitude estimation methods[J]. Journal of Guidance Control & Dynamics, 2012, 30(1): 12－28.

[22] Rico-Azagra J, Gil-Martínez M, Rico-Azagra R, et al. Low-Cost attitude estimation for a ground vehicle[C]//Robot 2015: Second Iberian Robotics Conference. Springer International Publishing, 2016: 121－132.

[23] Wu D, Wang Z. Strapdown inertial navigation system algorithms based on geometric algebra[J]. Advances in Applied Clifford Algebras, 2012, 22(4): 1151－1167.

[24] Titterton D, Weston J L. Strapdown Inertial Navigation Technology[M]. United Kingdom：Institution of Electrical Engineers, 2004:36－54.

[25] 刘星. 多维MEMS惯性传感器的姿态解算算法研究[D]. 哈尔滨：哈尔滨工程大学，2013.

Liu Xing. The Attitude Test Algorithm Based on Mems Multidimensional Inertial Sensors[D]. Harbin: Harbin Engineering University, 2013. (in Chinese with English abstract)

[26] 秦永元. 惯性导航[M]. 西安：科学出版社，2010：8－11.

[27] 徐涛，刘翠海，黄青斌. 水平定向钻进随钻测量系统研究与设计[J]. 仪器仪表学报，2009，30(9)：1976－1980.

Xu Tao, Liu Cuihai, Huang Qingbin. Research and design of a measurement-white-drilling system for horizontal directional drilling[J]. Transactions of Chinese Journal of Scientific Instrument, 2009, 30(9): 1976－1980. (in Chinese with English abstract)

[28] Guo M, YIN G, Tian X, et al. Research and design of tilt-angle sensor based on three-axis accelerometer[J]. Modern Electronics Technique, 2010, 8: 55.

[29] Merfeld D M, Zupan L, Peterka R J. Humans use internal models to estimate gravity and linear acceleration[J]. Nature, 1999, 398(6728): 615－618.

[30] Makni A, Fourati H, Kibangou A Y. Adaptive kalman filter for MEMS-IMU based attitude estimation under external acceleration and parsimonious use of gyroscopes[C]// Strasbourg, France: Control Conference (ECC), 2014 European. IEEE, 2014: 1379－1384.

[31] Huang Y H, Rizal Y, Ho M T. Development of attitude and

heading reference systems[C]//Yilan, Taiwan: Automatic Control Conference (CACS), 2015 International. IEEE, 2015: 13－18.

[32] Marina H G D, Pereda F J, Giron-Sierra J M, et al. UAV attitude estimation using unscented kalman filter and TRIAD[J]. IEEE Transactions on Industrial Electronics, 2012, 59(11): 4465－4474.

[33] 张晓兵. 独轮机器人姿态检测信息融合算法的研究[D]. 哈尔滨：哈尔滨工业大学，2015.

Zhang Xiaobing. Research on Information Fusion Algorithm for Attitude Estimation of Unicycle Robot[D]. Harbin: Harbin Engineering University, 2013. (in Chinese with English abstract)

[34] STMicroelectronics, STM32F446RC[Z]. http://www.st.com/ content/st_com/zh/products/microcontrollers/stm32-32-bit-arm- cortex-mcus/stm32f4-series/stm32f446/stm32f446rc.html.

[35] Analog Devices, ADIS16445_cn[Z]. http://www.analog.com/ media/cn/technical-documentation/data-sheets/ADIS16445_cn. pdf.

[36] Jurman D, Jankovec M, Kamnik R, et al. Calibration and data fusion solution for the miniature attitude and heading reference system[J]. Sensors & Actuators a Physical, 2007, 138(2): 411－420.

[37] 彭孝东，张铁民，李继宇，等. 三轴数字MEMS加速度计现场标定方法[J]. 振动·测试与诊断，2014(3)：544－548.

Peng Xiaodong, Zhang Tiemin, Li Jiyu, et al. Field calibration of three-axis mems digital acceleration[J]. Transactions of the Journal of Vibration, Measurement &. Diagnosis, 2014(3): 544－548. (in Chinese with English abstract)

黄培奎，张智刚，罗锡文，刘兆朋，王 辉，林志健，高维炜. 农机具姿态倾角测量系统设计与试验[J]. 农业工程学报，2017，33(22)：9－16. doi：10.11975/j.issn.1002-6819.2017.22.002 http://www.tcsae.org

Huang Peikui, Zhang Zhigang, Luo Xiwen, Liu Zhaopeng, Wang Hui, Lin Zhijian, Gao Weiwei. Design and test of tilt angle measurement system for agricultural implements[J]. Transactions of the Chinese Society of Agricultural Engineering (Transactions of the CSAE), 2017, 33(22): 9－16. (in Chinese with English abstract) doi：10.11975/j.issn.1002-6819.2017.22.002 http://www.tcsae.org

Design and test of tilt angle measurement system for agricultural implements

Huang Peikui, Zhang Zhigang※, Luo Xiwen, Liu Zhaopeng, Wang Hui, Lin Zhijian, Gao Weiwei

(1,510642,;2.510642,)

Agricultural implements tilt angle measurement is one of the key technologies to achieve agricultural implements and equipment precision operations. For example, the precision navigation control and the leveling control of agricultural implements are all dependent on the accurate measurement of tilt angle. What’s more, agricultural implements of tilt angle are one of the key parameters of agricultural mechanics modeling and agricultural implements safety warning learning. In order to further improve the quality of agricultural implements operation, we developed a new agricultural implement tilt angle measurement system in this paper and verified by tests on triaxial turntable platform and field. Modern micro-electromechanical systems (MEMS) technologies provide the moderate-cost and miniaturized solutions for the development of attitude reference system. Using highly-integrated inertial measurement units (IMUs) ADIS16445 provided by ADI company and micro ARM processor STM32F446 provided by ST company, we built the hardware platform. ADIS16445 ISensor® includes tri-axial gyroscopes and tri-axial accelerometers, the raw sensors data was sampled by STM32F446RC processor through SPI interface. The attitude calculation was carried out based on the Euler angle algorithm. The Kalman filter model with four state vectors and two observations was established to fuse the accelerometer and gyroscope information to achieve the accurate measurement of the tilt angle of agricultural implements. Considering the zero bias and drift characteristics of the gyroscope and the motion characteristics of the MEMS micro sensor, adaptive error covariance matrix Q and R rules were established to achieve precise tilt angle measurement of agricultural implements under different working conditions. Tests were conducted on SGT320E triaxial turntable platform and ZP9500 high level sprayer provided by LOVOL company dual in the field with the assistance of antenna positioning and attitude module BD982 provided by Trimble company. The SGT320E triaxial turntable platform was the standard equipment for testing the angular motion parameters and inertial systems. By setting the triaxial motion parameters to simulate a variety of motion states, it had speed, position and sine swing modes on all triaxial with a rate resolution of 0.0001°/s. In this paper, we used six position accelerometer calibration method and gyroscope error model to verify the performance of accelerometers and gyroscopes. Three-axis multi-function turntable test results showed that ADIS16445 built-in gyroscopes’ and accelerometers’ zero bias were under 0.15°/s and 0.075 mg, qualified to meet the system design hardware requirements. Kalman fusion algorithm were more accuracy and effective compare to simple integral by gyroscope and can solve the problem of zero bias and drift characteristics of the gyroscope with tilt static measurement error accuracy was 0.15°, typical dynamic measurement accuracy was 0.3°, maximum measurement error was less than 0.5°. The BD982 supports high precision positioning, attitude and heading output with high stability and fast dynamic response, which is widely used in construction implements, automobiles, agricultural implements and other fields, making it to be the leader of the industry. In this paper, the baseline length was 1.4 m with the measurement accuracy of 0.1°. Test results from high level sprayer showed that the average error of the attitude inclination was less than 0.55°, maximum measurement error was less than 0.91°, which satisfied the precise operation requirement of the agricultural equipment. Test results also verified that self-adaptive Kalman filter algorithm was more accuracy and stable than normal Kalman filter algorithm, which made the system development by this paper have more applicability. The agricultural implements tilt angle measurement system developed in this paper not only can reducing costs but also can improving the quality of agricultural implements operations.

mechanization; algorithms; design; agricultural implements; tilt angle measurement; multisensory fusion; adaptive Kalman filter; euler angle method

10.11975/j.issn.1002-6819.2017.22.002

S220.5; TP391

A

1002-6819(2017)-22-0009-08

2017-07-01

2017-10-25

国家国际科技合作专项（2015DFG12280）；国家科技部863项目（2013AA10230703）；广东省省级科技计划项目（2016B020205003）

黄培奎，博士生，主要从事农业机械姿态检测与导航控制。 Email：peikuihuang@stu.scau.edu.cn.

张智刚，副教授，博士，主要从事农业机械自动导航技术、精细农业。Email：zzg208@scau.edu.cn.