農(nóng)機具姿態(tài)傾角測量系統(tǒng)設(shè)計與試驗

黃培奎，張智剛，羅錫文，劉兆朋，王 輝，林志健，高維煒

?

農(nóng)機具姿態(tài)傾角測量系統(tǒng)設(shè)計與試驗

黃培奎，張智剛※，羅錫文，劉兆朋，王 輝，林志健，高維煒

（1. 華南農(nóng)業(yè)大學南方農(nóng)業(yè)機械與裝備關(guān)鍵技術(shù)教育部重點實驗室，廣州 510642；2. 華南農(nóng)業(yè)大學工程學院，廣州 510642）

農(nóng)機具姿態(tài)傾角測量技術(shù)是實現(xiàn)農(nóng)機裝備精準作業(yè)的關(guān)鍵技術(shù)之一。為進一步提高農(nóng)機裝備作業(yè)質(zhì)量，以ADIS16445微慣性MEMS傳感器和STM32F446核心處理器搭建硬件平臺，以歐拉角法解算姿態(tài)，建立卡爾曼濾波模型融合加速度計與陀螺儀信息，實現(xiàn)農(nóng)機具姿態(tài)傾角的精準測量。融合算法模型考慮陀螺儀零偏特性，并根據(jù)MEMS微傳感器運動特性，自適應(yīng)模型誤差協(xié)方差矩陣與，適應(yīng)不同工況下農(nóng)機具姿態(tài)傾角測量。采用SGT320E三軸多功能轉(zhuǎn)臺與BD982雙天線定位測姿模塊對系統(tǒng)進行測試與驗證。三軸多功能轉(zhuǎn)臺試驗結(jié)果表明，ADIS16445內(nèi)置陀螺儀與加速度計性能合格，滿足系統(tǒng)設(shè)計硬件要求；卡爾曼濾波融合模型精準有效，傾角靜態(tài)測量誤差精度為0.15°，動態(tài)測量精度典型值為0.3°，最大測量誤差為0.5°。田間作業(yè)試驗結(jié)果表明，自適應(yīng)模型能保證農(nóng)機具姿態(tài)傾角測量系統(tǒng)在不同工況下的測量精度，更穩(wěn)定可靠，測量平均誤差為0.55°。該文研究的農(nóng)機具姿態(tài)傾角測量系統(tǒng)可滿足農(nóng)機裝備精準作業(yè)要求。

機械化；算法；設(shè)計；農(nóng)機具；傾角測量；多傳感器融合；自適應(yīng)卡爾曼濾波；歐拉角法

0 引 言

中國農(nóng)業(yè)生產(chǎn)將向規(guī)模化與精細化方向發(fā)展[1-5]，規(guī)模化農(nóng)業(yè)的發(fā)展必將帶動寬幅農(nóng)機裝備更加廣泛的應(yīng)用，寬幅農(nóng)業(yè)機械的精準導(dǎo)航行走控制與寬幅機具的調(diào)平控制均依賴于農(nóng)機具姿態(tài)傾角的準確測量[6-8]。姿態(tài)傾角也是農(nóng)機具運動學建模與農(nóng)業(yè)機械安全預(yù)警研究的關(guān)鍵參數(shù)之一[9-15]。

農(nóng)機具姿態(tài)傾角測量多采用慣性傳感器、衛(wèi)星導(dǎo)航系統(tǒng)和圖像處理等方法獲取，也有部分學者研究基于多傳感器融合技術(shù)進行姿態(tài)傾角監(jiān)測[16-20]。其中基于慣性傳感器的農(nóng)機具姿態(tài)傾角測量是目前應(yīng)用最廣泛的方法，其經(jīng)濟性、穩(wěn)定性和適應(yīng)性指標相比其他測量方法都具有突出優(yōu)勢[21-22]。

趙祚喜等[7-8,16]通過MEMS陀螺儀與加速度計信息進行了農(nóng)機具的姿態(tài)傾角融合。其中趙祚喜等[7]將姿態(tài)傾角測量系統(tǒng)用于測定水田激光平地機平地鏟實時傾角，融合算法測量誤差一般不超過1°，能準確地檢測平地鏟傾角。但存在融合算法對加速度計與陀螺儀進行姿態(tài)傾角的切換不夠精確、未能消除慣性傳感器本身固有的偏移量影響等一些不足。馬超等[16]將姿態(tài)傾角測量系統(tǒng)用于測定農(nóng)田環(huán)境信息采集平臺傾角并通過控制步進電機進行補償修正，在田間顛簸路況也能保持最大誤差在3.0°以內(nèi)。但其使用互補濾波的融合算法過于簡單，僅靠一個權(quán)重系數(shù)的調(diào)節(jié)難以滿足實際作業(yè)測量需要，測量精度有限。胡煉等[8]將姿態(tài)傾角測量系統(tǒng)用于測定水田插秧機底盤橫滾傾角并進行平地鏟的調(diào)平控制，提升了激光平地機的控制精度。但融合算法中的誤差協(xié)方差矩陣均為經(jīng)驗值，難以保證高速行走與轉(zhuǎn)彎等工況下的測量精度、且其融合傾角為單軸，適應(yīng)性有限。

為進一步提高農(nóng)機具姿態(tài)傾角的測量精度及適應(yīng)性，提升農(nóng)機具作業(yè)質(zhì)量，本文采用MEMS慣性傳感器ADIS16445與STM32F446 ARM處理器設(shè)計農(nóng)機具姿態(tài)傾角測量系統(tǒng)硬件平臺，以歐拉角算法進行姿態(tài)解算，考慮陀螺儀零偏與漂移特性，建立卡爾曼濾波模型，自適應(yīng)誤差協(xié)方差矩陣與，實現(xiàn)各種工況下農(nóng)機具姿態(tài)傾角的精準融合解算。并通過三軸多功能轉(zhuǎn)臺試驗與田間試驗對系統(tǒng)進行分析驗證。

1 基本原理

1.1 歐拉角空間姿態(tài)解算算法

MEMS慣性陀螺儀經(jīng)數(shù)值積分運算可獲得傳感器坐標系下敏感軸的姿態(tài)角，常用的姿態(tài)解算算法有歐拉角法、方向余弦法和四元數(shù)法[14,23]。四元素法解算需要進行泰勒展開，通常忽略其高階項將非線性轉(zhuǎn)化成線性進行姿態(tài)估算，存在誤差。方向余弦法共有9個參數(shù)，計算量大，不適宜工程應(yīng)用。歐拉角法是載體姿態(tài)信息解算最為直接的方式，通過3個微分方程直接解算，適用于地面作業(yè)農(nóng)機具姿態(tài)傾角解算（不存在俯仰角奇異點情況）[24]。

剛體在三維空間的姿態(tài)變化可理解為從一個坐標系到另一個坐標系的變化，通過繞不同坐標系3次連續(xù)轉(zhuǎn)動實現(xiàn)，即歐拉角法原理[25-26]。如圖1所示，姿態(tài)測量系統(tǒng)原始坐標系為，當繞X軸轉(zhuǎn)動時產(chǎn)生橫滾傾角Roll，傳感器坐標系變?yōu)?1；當繞Y軸轉(zhuǎn)動時產(chǎn)生俯仰傾角Pitch，傳感器坐標系變?yōu)?2。

圖1 歐拉角法姿態(tài)解算原理圖

根據(jù)歐拉角法的坐標旋轉(zhuǎn)原理，歐拉角隨時間的傳遞關(guān)系可利用MEMS陀螺儀測得的角速率表示。

式中ωωω分別為、、軸旋轉(zhuǎn)角速率，rad/s；分別為橫滾角、俯仰角與偏航角，rad。整理式（1）可推導(dǎo)出歐拉角算法的微分方程為

故根據(jù)歐拉角算法，只需求解相應(yīng)的微分方程便可得到相應(yīng)的姿態(tài)角。本文研究的農(nóng)機具姿態(tài)傾角具體指的是橫滾角與俯仰角，不包含偏航角。

1.2 準靜態(tài)條件下加速度計測量傾角算法

MEMS加速度計處于準靜態(tài)（靜止或無外部加速度時）條件下時，只感應(yīng)到重力加速度[27-29]。對于單軸加速度計，如圖2a所示。

式中為測量傾角，(°)；與分別為單軸加速度測量值與重力加速度值，m/s2。故準靜態(tài)時，由單軸加速度計可測量平面傾角。同理，通過三軸加速度計可對剛體在準靜態(tài)條件下的空間姿態(tài)傾角變化進行測量。如圖2b所示，假設(shè)三軸加速度計輸出值分別為a、a、a，則準靜態(tài)時有

式中a為三軸合加速度，m/s2；準靜態(tài)時值為1 g，即本地重力加速度。

注：θ為測量傾角，(°)；a與g分別為單軸加速度測量值與重力加速度值，m·s-2；aX、aY、aZ、ag分別為三軸加速度計輸出值與三軸合加速度，m·s-2。

結(jié)合歐拉角姿態(tài)解算算法對剛體空間姿態(tài)傾角的定義，對應(yīng)圖2b三軸加速度計模型有

1.3 卡爾曼濾波融合算法

卡爾曼濾波算法是一種最優(yōu)化自回歸的數(shù)據(jù)處理方法，能簡單、高效地處理離散數(shù)據(jù)線性濾波的問題[30-33]。1960年由卡爾曼首次提出后不斷改進，現(xiàn)已廣泛應(yīng)用于控制、圖像處理、傳感器信息融合等領(lǐng)域?？柭鼮V波算法由系統(tǒng)狀態(tài)方程與系統(tǒng)觀測方程組成

式中x、u、z分別代表系統(tǒng)在時刻的狀態(tài)向量、輸入向量與測量向量；分別為系統(tǒng)的狀態(tài)矩陣、輸入矩陣與輸出矩陣；w與v分別代表系統(tǒng)在（-1）時刻的狀態(tài)噪聲與時刻的測量噪聲。系統(tǒng)的狀態(tài)噪聲和測量噪聲彼此獨立且定義為零均值且符合正態(tài)分布的高斯白噪聲，其協(xié)方差矩陣Q和R分別定義為

卡爾曼濾波算法執(zhí)行主要由時間更新和測量更新兩部分組成，因此也被稱作“預(yù)估—更正”法。具體執(zhí)行流程如下：

2）時間更新，即“預(yù)估”法；

①計算先驗狀態(tài)估計

②計算先驗誤差協(xié)方差

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

①計算卡爾曼增益

式中K為卡爾曼時刻增益，為狀態(tài)輸出矩陣。

②用測量值更新狀態(tài)估計

③更新誤差協(xié)方差

重復(fù)“預(yù)估—更正”過程，直到算法結(jié)束。

2 系統(tǒng)硬件和軟件設(shè)計

2.1 系統(tǒng)總體設(shè)計

農(nóng)機具姿態(tài)傾角測量系統(tǒng)由硬件平臺與軟件算法兩部分組成，如圖3所示。MEMS慣性傳感器輸出的三軸加速度計、三軸陀螺儀與溫度等測量信息輸入至核心處理器STM32F446RC中，進行軟件算法的計算，包括初始化、卡爾曼濾波與傾角估計三部分，最后輸出融合橫滾角與俯仰角。

圖3 系統(tǒng)總計設(shè)計

2.2 硬件平臺設(shè)計

系統(tǒng)的硬件部分主要包括AMR核心處理器與MEMS微慣性傳感器穩(wěn)壓電路，工作電路和串口通訊電路等。

2.2.1 ARM核心處理器

根據(jù)系統(tǒng)的設(shè)計要求，選用ST公司STM32F446RC 作為系統(tǒng)的核心處理器。該ARM核心芯片為32位處理器，具有180 MHz的最高處理頻率可滿足姿態(tài)傾角數(shù)據(jù)處理要求；具有512 kB閃存和雙模四SPI接口、16位DMA與低功耗等特點[34]。

2.2.2 MEMS微慣性傳感器

姿態(tài)傾角測量系統(tǒng)的測量精度首先取決于所選取的慣性傳感器精度，然后才是傾角融合的處理。本文選取具有6自由度的ADIS公司16445作為慣性測量模塊，內(nèi)置一個三軸陀螺儀和一個三軸加速度計，均具備動態(tài)補償功能；集成溫度傳感器和SPI通訊[35]。其中陀螺儀的最大測量范圍為±250°/s，相應(yīng)分辨率為0.01°/s/LSB；加速度計測量范圍為±5 g，分辨率為0.25 mg/LSB；溫度計分辨率為0.073 86℃/LSB。程序設(shè)置傳感器的采樣頻率為100 Hz，能保證田間地形高低起伏；農(nóng)機具振動、晃動、高速作業(yè)等復(fù)雜工況下橫滾與俯仰傾角的準確測量。姿態(tài)傾角硬件平臺實物圖如圖4所示。

圖4 硬件平臺實物圖

2.3 軟件算法設(shè)計

農(nóng)機具作業(yè)時帶動姿態(tài)傾角測量系統(tǒng)在三維空間運動，可利用陀螺儀與加速度計解算農(nóng)機具實時姿態(tài)傾角。由MEMS微慣性傳感器原理所限，如前文1.2所述，準靜態(tài)情況下，通過三軸加速度計可以準確測量農(nóng)機具姿態(tài)傾角。但農(nóng)機具實際作業(yè)時難以保證勻速運動，向心加速度等外部加速度會影響姿態(tài)傾角解算。對于MEMS陀螺儀，除了固有零偏外還存在漂移特性，在動態(tài)、頻率較高情況下通過角速度積分可獲得較準確的傾角，但陀螺儀漂移的積分會隨時間累積產(chǎn)生積分誤差。因此實際應(yīng)用常將2種傳感器組合使用，通過濾波融合算法實現(xiàn)傾角的準確估計。常用的融合算法有互補濾波、梯度下降法與卡爾曼濾波算法[19,21]，其中基于卡爾曼濾波器的姿態(tài)融合算法應(yīng)用最為廣泛。

式中Δ為系統(tǒng)采樣間隔0.01 s。

農(nóng)機具姿態(tài)傾角測量系統(tǒng)具有動態(tài)時變特性，系統(tǒng)的狀態(tài)噪聲與測量噪聲均跟隨系統(tǒng)動態(tài)變化。為了提高姿態(tài)傾角融合精度，需根據(jù)系統(tǒng)運動特性自適應(yīng)調(diào)整系統(tǒng)狀態(tài)噪聲協(xié)方差矩陣與測量噪聲協(xié)方差矩陣。定義如下

MEMS微慣性傳感器的帶寬有限，系統(tǒng)狀態(tài)噪聲可隨系統(tǒng)旋轉(zhuǎn)運動大?。ㄊ剑?7））變化，故動態(tài)過程可將系統(tǒng)狀態(tài)噪聲與旋轉(zhuǎn)大小做近似線性處理如下[34]

式中為軸陀螺儀輸出，代表當前時刻。

對于本文系統(tǒng)，根據(jù)ADIS16445傳感器特性，有max=433°/s；2high=0.05；2low=0.01。

系統(tǒng)的測量噪聲具有時變特性，取決于系統(tǒng)的測量狀態(tài)，系統(tǒng)實際作業(yè)時產(chǎn)生的外部加速度會造成測量噪聲的增加。對于卡爾曼濾波模型，系統(tǒng)的初始化會對傾角融合精度產(chǎn)生影響。由于實際農(nóng)機具田間作業(yè)前均需經(jīng)過系統(tǒng)設(shè)計或啟動預(yù)熱階段（此時為準靜態(tài)），故可保證由三軸加速度計解算得到準確的姿態(tài)傾角初始值，模型初始化有效。此后可將系統(tǒng)測量噪聲與外部干擾加速度做近似線性處理如下[31,36]

3 試驗與結(jié)果分析

3.1 試驗設(shè)備及方法

為驗證農(nóng)機具姿態(tài)傾角測量系統(tǒng)的精確性與適用性，本文采用SGT320E三軸多功能轉(zhuǎn)臺、BD982雙天線定位測姿模塊與雷沃ZP9500型高地隙噴霧機等設(shè)備進行試驗驗證。三軸轉(zhuǎn)臺是測試角運動參數(shù)的標準設(shè)備, 通過設(shè)置三軸運動參數(shù)模擬多種運動狀態(tài)進而實現(xiàn)慣性傳感器標定與姿態(tài)測量系統(tǒng)模型驗證。BD982雙天線定位測姿模塊是由美國天寶公司生產(chǎn)的高精度定位測姿系統(tǒng)，姿態(tài)測量穩(wěn)定性與測量精度高，廣泛運用于各種工程測量領(lǐng)域，本文將其作為田間試驗姿態(tài)測量對照。雷沃ZP9500型是一種高地隙寬幅噴桿噴霧機，噴桿長11.5 m，田間作業(yè)時噴桿姿態(tài)傾角變化大，適宜作為農(nóng)機具姿態(tài)傾角測量系統(tǒng)田間試驗平臺。

3.2 三軸轉(zhuǎn)臺試驗與結(jié)果

SGT320E型三軸多功能轉(zhuǎn)臺由機械臺體、電控系統(tǒng)及連接電纜組成。轉(zhuǎn)臺臺體結(jié)構(gòu)采用U-O-O結(jié)構(gòu)，三軸均可連續(xù)無限旋轉(zhuǎn)，其速率分辨率為0.000 1°/s[37]。SGT320E型具有位置模式、速率模式和搖擺模式等3種轉(zhuǎn)臺運動模式，支持外同步觸發(fā)串口輸出，可實現(xiàn)轉(zhuǎn)臺數(shù)據(jù)與傳感器數(shù)據(jù)的同步記錄。試驗中分別采用三軸轉(zhuǎn)臺位置模式對ADIS16445三軸加速度計進行標定校驗；采用速率模式對ADIS16445三軸陀螺儀進行標定校驗；采用搖擺模式對農(nóng)機具姿態(tài)傾角測量系統(tǒng)的融合算法模型進行驗證并對其融合橫滾角與俯仰角的測量精度進行測試。如圖5所示，將農(nóng)機具姿態(tài)傾角測量模塊安裝在內(nèi)軸中心，保證ADIS16445的3個敏感軸分別與三軸轉(zhuǎn)臺的3個轉(zhuǎn)動軸平行。

圖5 三軸轉(zhuǎn)臺試驗現(xiàn)場圖

3.2.1 ADIS16445標定

參考文獻[37]所列舉的6位置加速度計標定方法，采用三軸多功能轉(zhuǎn)臺位置模式對ADIS16445的三軸加速度計進行標定。傳感器預(yù)熱完成后采集試驗數(shù)據(jù)，每個位置采集5 min，分別統(tǒng)計在6個不同位置下三軸加速度計輸出的均值，結(jié)果如表1所示。

表1 加速度計各敏感軸取向與加速度輸出

采用最小二乘法求解出ADIS16445三軸加速度計的標定系數(shù)矩陣為

由標定結(jié)果可知ADIS16445三軸加速度計的零偏值均在參考值0.075 mg（系數(shù)矩陣第4行為三軸加速度計零偏值）以內(nèi)，加速度計性能符合試驗要求。

參考文獻[24], MEMS陀螺儀誤差模型可表示為

式中為標度因素誤差；B為與重力加速度無關(guān)的零偏；為陀螺儀零平均值隨機零偏；為陀螺儀實際輸出值。采用三軸多功能轉(zhuǎn)臺速率模式對進行ADIS16445的三軸陀螺儀標定。傳感器預(yù)熱完成后采集試驗數(shù)據(jù)，設(shè)置轉(zhuǎn)臺以速率方式轉(zhuǎn)動，每次運行5 min，每個敏感軸分別采集在速率為?200至200°/s（三軸轉(zhuǎn)臺的外框最高轉(zhuǎn)動速率）。間隔為20°/s的ADIS16445陀螺儀數(shù)據(jù)，共計63組。采用數(shù)據(jù)擬合方法進行求解，如式（23）所示。

ADIS16445的三軸陀螺儀靜態(tài)偏移（B）均在參考值0.15 °/s范圍內(nèi)，陀螺儀性能符合試驗要求。

3.2.2 融合算法驗證

采用信號發(fā)生器產(chǎn)生幅值為5 V，頻率為10 Hz的方波同時觸發(fā)三軸轉(zhuǎn)臺與農(nóng)機具姿態(tài)傾角測量模塊，并分別設(shè)置轉(zhuǎn)臺內(nèi)軸與中軸以搖擺幅度為4°、搖擺頻率為1 Hz擺動，模塊預(yù)熱完成后由靜態(tài)零位置到搖擺運動模式模擬農(nóng)機具實際作業(yè)時產(chǎn)生的橫滾角與俯仰角，每組測試時長5 min，驗證融合算法動靜態(tài)測量精度，同時與普通陀螺積分方法進行對比。圖6a為內(nèi)軸搖擺產(chǎn)生橫滾角，準靜態(tài)測量誤差的情況；圖6b為中軸搖擺產(chǎn)生俯仰角，動態(tài)測量誤差情況。

注：搖擺幅度為4°，搖擺頻率為1 Hz。

由圖6可知，在農(nóng)機具實際作業(yè)時的傾角典型值（搖擺幅度為4°，頻率1 Hz）的三軸轉(zhuǎn)臺搖擺測試中，利用普通的積分方法計算傾角在準靜態(tài)時存在明顯累計誤差，陀螺儀漂移引起的誤差隨時間增長；動態(tài)誤差普遍大于1°。如表2所示，基于卡爾曼濾波的融合算法能有效抑制有陀螺儀零偏與漂移引起的累計誤差，傾角測量穩(wěn)定精準。靜態(tài)最大測量偏差為0.15°，動態(tài)最大測量偏差為0.5°。三軸轉(zhuǎn)臺試驗結(jié)果即驗證本文提出算法的優(yōu)越性與有效性。

表2 基于卡爾曼濾波的融合算法的三軸轉(zhuǎn)臺傾角測試結(jié)果

注：典型值為多次試驗統(tǒng)計最大偏差的平均值，下同。

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

3.3 田間試驗與結(jié)果

雷沃ZP9500型自走式高地隙噴桿噴霧機（簡稱高地隙噴霧機）是雷沃重工股份有限公司2016年推向市場的一款植保機械，四輪驅(qū)動、最小轉(zhuǎn)彎半徑3.2 m，最高離地間隙1.1 m，噴幅11.5 m，廣泛運用于水稻、小麥等種植全過程田間植保管理。

美國天寶公司生產(chǎn)的定位測姿模塊BD982支持高精度定位、姿態(tài)和航向輸出，穩(wěn)定性高，動態(tài)響應(yīng)快，當主從天線基線長度為10 m時其橫滾/俯仰角動態(tài)測量精度可達0.05°。本文試驗基線長度為1.4 m測量精度可達0.1°。廣泛運用于工程機械，汽車，農(nóng)業(yè)機械等領(lǐng)域。

田間試驗于2017年6月在華南農(nóng)業(yè)大學增城教學試驗基地進行，采用高地隙噴霧機平臺進行田間試驗，并以BD982作為農(nóng)機具傾角測量參照。如圖7所示，將農(nóng)機具姿態(tài)傾角測量模塊與BD982安裝于高地隙噴霧機頂部，農(nóng)機具姿態(tài)傾角測量模塊位于兩天線中心。

1.ZP9500型高地隙噴霧機 2. 農(nóng)機具姿態(tài)傾角測量系統(tǒng) 3. BD982雙天線定位測姿模塊

先進行橫滾角試驗，再將農(nóng)機具姿態(tài)傾角測量模塊水平旋轉(zhuǎn)90°進行俯仰角試驗（BD982雙天線系統(tǒng)僅有1維傾角輸出）。采用同步觸發(fā)方式，在預(yù)熱完成后開啟高地隙噴霧機采集試驗數(shù)據(jù)。試驗結(jié)果分別如圖8與圖9所示。

圖8 橫滾角融合算法田間對比驗證試驗

圖9 俯仰角融合算法田間對比驗證試驗

由圖8與圖9可知，帶自適應(yīng)的卡爾曼濾波算法能更好適應(yīng)田間復(fù)雜工況下的應(yīng)用，且具有更高的測量精度。田間試驗以BD982作為農(nóng)機具傾角測量參照，具體結(jié)果如表3所示。

表3 自適應(yīng)卡爾曼濾波田間傾角測試結(jié)果

由表3可知，本文研制的農(nóng)機具姿態(tài)傾角測量模塊動態(tài)測量平均誤差0.55°、最大誤差小于0.91°，可滿足農(nóng)機具精準作業(yè)要求。

4 結(jié)論與討論

1）本文設(shè)計了一種農(nóng)機具姿態(tài)傾角測量系統(tǒng)，采用歐拉角算法進行姿態(tài)解算、自適應(yīng)卡爾曼濾波算法融合加速度計和陀螺儀數(shù)據(jù)測量農(nóng)機具實時傾角。

2）在三軸多功能轉(zhuǎn)臺上對MEMS加速度計與陀螺儀進行了標定驗證，并測試傾角融合算法。測量靜態(tài)最大偏差為0.15°，動態(tài)最大偏差為0.5°。

3）以雷沃ZP9500型高地隙噴霧機為平臺、以BD982雙天線定位測姿模塊為參照進行田間試驗。試驗結(jié)果表明：帶自適應(yīng)的卡爾曼濾波算法能更好適應(yīng)田間復(fù)雜工況下的應(yīng)用，且具有更高的測量精度。動態(tài)測量平均誤差0.55°、最大誤差為0.91°，可滿足農(nóng)機具精準作業(yè)要求。

本文設(shè)計的農(nóng)機具姿態(tài)傾角測量系統(tǒng)仍需進一步進行農(nóng)機具實際作業(yè)傾角測量驗證，并根據(jù)實際情況進一步優(yōu)化融合算法。下一步研究還應(yīng)考慮加速度計與陀螺儀本身誤差模型，并嘗試與視覺、全球?qū)Ш叫l(wèi)星系統(tǒng)、激光雷達等傳感器結(jié)合提高農(nóng)機具姿態(tài)傾角測量精度。

[1] 中華人民共和國國務(wù)院，中共中央、國務(wù)院關(guān)于深入推進農(nóng)業(yè)供給側(cè)結(jié)構(gòu)性改革加快培育農(nóng)業(yè)農(nóng)村發(fā)展新動能的若干意見[Z]. 2017-02-06.

[2] 李楊. 寬幅農(nóng)業(yè)機械田間作業(yè)姿態(tài)監(jiān)測方法[D]. 長春：吉林大學，2016.

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

[3] 羅錫文，廖娟，胡煉，等. 提高農(nóng)業(yè)機械化水平促進農(nóng)業(yè)可持續(xù)發(fā)展[J]. 農(nóng)業(yè)工程學報，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] 羅錫文，廖娟，鄒湘軍，等. 信息技術(shù)提升農(nóng)業(yè)機械化水平[J]. 農(nóng)業(yè)工程學報，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的導(dǎo)航控制參數(shù)整定方法試驗[J]. 農(nóng)業(yè)工程學報，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慣性傳感器融合的水田激光平地機水平控制系統(tǒng)[J]. 農(nóng)業(yè)工程學報，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] 胡煉，林潮興，羅錫文，等. 農(nóng)機具自動調(diào)平控制系統(tǒng)設(shè)計與試驗[J]. 農(nóng)業(yè)工程學報，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]. 農(nóng)業(yè)工程學報，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]. 計算機應(yīng)用研究，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器件的無人機姿態(tài)測量系統(tǒng)設(shè)計與實現(xiàn)[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傳感器的兩軸姿態(tài)調(diào)整系統(tǒng)設(shè)計與試驗[J]. 農(nóng)業(yè)工程學報，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] 王曉燕，陳媛，陳兵旗，等. 免耕覆蓋地秸稈行茬導(dǎo)航路徑的圖像檢測[J]. 農(nóng)業(yè)機械學報，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] 胡靜濤，高雷，白曉平，等. 農(nóng)業(yè)機械自動導(dǎo)航技術(shù)研究進展[J]. 農(nóng)業(yè)工程學報，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慣性傳感器的姿態(tài)解算算法研究[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] 秦永元. 慣性導(dǎo)航[M]. 西安：科學出版社，2010：8－11.

[27] 徐濤，劉翠海，黃青斌. 水平定向鉆進隨鉆測量系統(tǒng)研究與設(shè)計[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] 張曉兵. 獨輪機器人姿態(tài)檢測信息融合算法的研究[D]. 哈爾濱：哈爾濱工業(yè)大學，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] 彭孝東，張鐵民，李繼宇，等. 三軸數(shù)字MEMS加速度計現(xiàn)場標定方法[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)

黃培奎，張智剛，羅錫文，劉兆朋，王 輝，林志健，高維煒. 農(nóng)機具姿態(tài)傾角測量系統(tǒng)設(shè)計與試驗[J]. 農(nóng)業(yè)工程學報，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）

黃培奎，博士生，主要從事農(nóng)業(yè)機械姿態(tài)檢測與導(dǎo)航控制。 Email：peikuihuang@stu.scau.edu.cn.

張智剛，副教授，博士，主要從事農(nóng)業(yè)機械自動導(dǎo)航技術(shù)、精細農(nóng)業(yè)。Email：zzg208@scau.edu.cn.