田間作業(yè)車輛外部加速度辨識與姿態(tài)測量系統(tǒng)研制

黃培奎，張智剛，羅錫文，劉兆朋， 王 輝，岳斌斌，高維煒

?

田間作業(yè)車輛外部加速度辨識與姿態(tài)測量系統(tǒng)研制

黃培奎，張智剛※，羅錫文，劉兆朋， 王 輝，岳斌斌，高維煒

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

復(fù)雜田間作業(yè)環(huán)境與精細作業(yè)效果要求農(nóng)機裝備具備實時精準感知農(nóng)機具姿態(tài)的能力，田間作業(yè)時普遍存在的車輛外部加速度對此帶來挑戰(zhàn)。為進一步提高農(nóng)機裝備作業(yè)質(zhì)量，該文以6軸微慣性傳感器為硬件傳感器，以方向余弦矩陣法進行姿態(tài)解算，基于一階外部加速度模型設(shè)計卡爾曼濾波融合算法，實現(xiàn)動態(tài)情況下田間作業(yè)車輛外部加速度辨識與姿態(tài)精準估計。分別采用Innova 2100型搖床與裝配有MTi300航向姿態(tài)參考模塊的高地隙噴霧機對系統(tǒng)進行試驗驗證。搖床試驗結(jié)果表明：在外部加速度小于10情況下，系統(tǒng)對外部加速度辨識誤差小于0.214 m/s2；田間作業(yè)高地隙噴霧機試驗結(jié)果表明，相比于MTi300，橫滾角最大誤差為0.23°，俯仰角最大誤差為0.39°。說明該文研制姿態(tài)測量系統(tǒng)可準確辨識外部加速度與測量姿態(tài)，研究結(jié)果可為滿足精細農(nóng)業(yè)作業(yè)要求的姿態(tài)測量系統(tǒng)研發(fā)提供依據(jù)。

農(nóng)業(yè)機械；傳感器；姿態(tài)估計；外部加速度；卡爾曼濾波；方向余弦矩陣

0 引 言

如何應(yīng)對由世界人口增長帶來的糧食安全問題引起人們廣泛關(guān)注[1-2]。聯(lián)合國2017發(fā)布的報告預(yù)測，2050年世界人口總數(shù)將達到98億[3]。精細農(nóng)業(yè)與智能農(nóng)機裝備是緩解當(dāng)前世界糧食安全緊張形勢的有效途徑。農(nóng)業(yè)生產(chǎn)的規(guī)模化與精細化，帶動智能農(nóng)機裝備的發(fā)展[4-6]。復(fù)雜的田間作業(yè)環(huán)境與精細作業(yè)效果，要求農(nóng)機裝備具備實時精準感知農(nóng)機具姿態(tài)的能力[7-9]。

農(nóng)機具姿態(tài)多采用慣性傳感器、衛(wèi)星導(dǎo)航系統(tǒng)和圖像處理等方法[4]。微慣性MEMS（micro-electro-mechanical system，微機電系統(tǒng)）傳感器具有低成本、低功耗、小尺寸、高精度等優(yōu)勢，成為目前應(yīng)用最廣泛的方法[10-12]。MEMS微慣性加速度計，在靜態(tài)或準靜態(tài)條件下能測量重力加速度，并作為測量剛體垂直參考。MEMS微慣性陀螺儀在動態(tài)情況下經(jīng)數(shù)值積分運算可獲得敏感軸姿態(tài)角[11]。然而田間作業(yè)情況下普遍存在的車輛外部加速度[13-16]對農(nóng)機具姿態(tài)實時精準測量帶來挑戰(zhàn)。

根據(jù)文獻調(diào)查，部分學(xué)者已開展無人機等抗外部加速度干擾的算法研究，但適用于田間作業(yè)車輛外部加速度辨識與姿態(tài)估計的系統(tǒng)研究仍有待深入開展。Guo X等[17]利用滑模變結(jié)構(gòu)策略解決無人機作業(yè)過程中受非線性外部加速度影響問題，取得良好效果。Ligorio G[18]等基于微慣性傾角儀設(shè)計卡爾曼濾波算法實現(xiàn)人體運動姿態(tài)跟蹤，考慮了外部加速度的影響。Sabatini A M[19]等設(shè)計閾值切換算法補償外部加速度對可穿戴設(shè)備姿態(tài)測量所帶來的影響，對閾值選取有較高要求，不適用于復(fù)雜田間環(huán)境下的作業(yè)農(nóng)機。孟唐宇等[20]與Suh Y S等[21]均在微慣性傳感器和磁強計組合的基礎(chǔ)上，提出基于四元素和擴展卡爾曼濾波器的姿態(tài)解算與外部加速度同步估計算法，取得有益效果，但磁強計容易受外界電磁干擾，估算的外部加速度會受到一定影響，帶來誤差，不適用于田間作業(yè)車輛。Lee J K[22]等明確指出基于三軸加速度計與三軸陀螺儀對于物體姿態(tài)估計是完全足夠的。

本文采用6軸MEMS微慣性傳感器為硬件傳感器，以方向余弦矩陣法進行姿態(tài)解算，基于一階外部加速度計模型設(shè)計卡爾曼濾波融合算法，實現(xiàn)動態(tài)情況下田間作業(yè)車輛外部加速度辨識與姿態(tài)精準估計。并分別在Innova 2100型搖床與田間作業(yè)高地隙噴霧機上對系統(tǒng)進行試驗驗證，以期提出一種外部加速度辨識與姿態(tài)測量方法，解決作業(yè)機械姿態(tài)測量時受外部加速度影響嚴重的問題，并為滿足精細農(nóng)業(yè)作業(yè)要求的姿態(tài)測量系統(tǒng)研發(fā)提供依據(jù)。

1 基本原理

1.1 方向余弦空間姿態(tài)解算算法

式中11~33為方向余弦矩陣的元素，若運用常規(guī)的旋轉(zhuǎn)順序的歐拉角表示，旋轉(zhuǎn)矩陣等同于：

式中、和分別代表橫滾角、俯仰角與航向角，rad。

由式（2）可知旋轉(zhuǎn)矩陣的第三行元素只包含橫滾角與俯仰角，因此車體的橫滾與俯仰姿態(tài)可只借助方向余弦矩陣第三行元素進行計算：

取重力加速度為9.8 m/s2，則準靜態(tài)條件下三軸加速度計所測量的重力加速度矢量為：

因此，對于車體姿態(tài)角求解，即便考慮重力加速度矢量對加速度測量的影響，也只需方向余弦矩陣第三行元素[313233]即可。下文主要探討如何建立卡爾曼濾波算法實現(xiàn)[3132,33]的實時估計。

1.2 卡爾曼濾波融合算法

1.2.1 傳感器測量模型

對于MEMS陀螺儀（）與加速度計（），分別有以下傳感器測量模型[24]：

式中、y分別是三軸陀螺儀與三軸加速度計測量值，單位為rad/s和m/s2；為三軸角速率真實值，rad/s；為外部加速度，m/s2；為測量噪聲，各元素間是彼此獨立且均值為0的高斯白噪聲。MEMS傳感器的測量誤差來源包括常值零偏和移動隨機偏差，受復(fù)雜的工作環(huán)境影響，特別是溫度變化。但當(dāng)傳感器充分預(yù)熱后，測量誤差變化緩慢，影響極小[25-26]，故本文算法不考慮其影響。對外部加速度建立一階低通濾波測量模型[27]：

式中表示時間，為0到1之間的常值系數(shù)，取決于濾波器的截止頻率；為外部加速度測量模型的時變誤差。

1.2.2 卡爾曼濾波算法設(shè)計

參考文獻[28]建立田間作業(yè)車輛外部加速度辨識與姿態(tài)測量系統(tǒng)的卡爾曼濾波的狀態(tài)方程和觀測方程：

系統(tǒng)的狀態(tài)方程是基于捷聯(lián)慣性原理建立，因此對于旋轉(zhuǎn)矩陣隨時間的傳播方程可定義為：

由于測量噪聲的存在，實際運用中三軸角速率真實值不能直接測量得到。但由陀螺儀測量模型可知：

結(jié)合式（7）可知系統(tǒng)的狀態(tài)轉(zhuǎn)移矩陣和狀態(tài)噪聲定義為：

故系統(tǒng)的狀態(tài)噪聲協(xié)方差矩陣可定義為：

式中為期望計算。借助式（11），可進一步細化為：

系統(tǒng)的觀測方程加速度模型建立時，考慮了外部加速度的補償問題。外部加速度的估計誤差可定義為：

式中右上角減號表示先驗估計，為卡爾曼濾波器的預(yù)估值。結(jié)合式（4）~（6），式（14）可進一步細化為：

式中左上角加號表示后驗估計，為卡爾曼濾波器的修正值。

由式（15）可知，左邊式子已基于外部加速度模型消除了動態(tài)情況下外部加速度對加速度計的測量影響，因此對應(yīng)系統(tǒng)的觀測方程有：

系統(tǒng)的觀測噪聲協(xié)方差矩陣可定義為：

卡爾曼濾波算法的具體執(zhí)行步驟為：

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

2）計算先驗誤差協(xié)方差矩陣

3）計算卡爾曼濾波增益

4）計算后驗狀態(tài)更新

5）計算后驗誤差協(xié)方差矩陣

值得注意的是，完成步驟4）計算后需對測量矢量進行歸一化處理。得到最終測量矩陣估計值后再計算當(dāng)前時刻外部加速度值：

算法流程如圖1所示。

圖1 卡爾曼濾波算法流程圖

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

2.1 硬件平臺

硬件系統(tǒng)主要由核心處理器與6軸微慣性組成的田間車輛外部加速度辨識與姿態(tài)測量系統(tǒng)。其中，核心處理器選用ST公司的STM32F446RC ARM芯片。該芯片處理器為32位，最高處理頻率180 MHz，可滿足田間作業(yè)車輛姿態(tài)測量與外部加速度辨識要求。其內(nèi)部包含512kB閃存、雙模四SPI接口和16位DMA，具有低功耗等特點[29]。

6軸微慣性傳感器選用ADIS公司的16445型號微慣性測量模塊。該模塊包含三軸陀螺儀和三軸加速度計，均具備動態(tài)補償功能；集成溫度傳感器和 SPI 通訊[30]。陀螺儀的測量范圍為±250 °/s，分辨率為0.01 °/s /LSB；加速度計測量范圍為±5，分辨率為0.25 mg/LSB。設(shè)置傳感器的采樣頻率為100Hz，保證田間地形高低起伏、農(nóng)機具振動、晃動、高速作業(yè)等復(fù)雜工況下姿態(tài)角的準確測量與外部加速度辨識。系統(tǒng)實物如圖2所示。

圖2 外部加速度辨識與姿態(tài)測量系統(tǒng)實物圖

2.2 軟件算法

田間作業(yè)車輛帶動測量系統(tǒng)在三維空間運動，如前文1.2所述，可借助MEMS三軸陀螺儀與三軸加速度計融合算法實現(xiàn)姿態(tài)角實時解算與外部加速度實時辨識。常見的多傳感器融合算法包括互補濾波、梯度下降和卡爾曼濾波算法等[31]，其中卡爾曼濾波算法應(yīng)用最為廣泛。

萊蕪山水生態(tài)資源豐富，發(fā)展鄉(xiāng)村旅游是促進鄉(xiāng)村振興、實現(xiàn)精準脫貧的重要途徑，楊桂釗委員就對此非常關(guān)注：“請問下一步如何通過發(fā)展鄉(xiāng)村旅游促進鄉(xiāng)村振興，實現(xiàn)精準脫貧呢？”

本文采用卡爾曼濾波算法融合三軸陀螺儀與三軸加速度計信息，實現(xiàn)田間作業(yè)車輛外部加速度與姿態(tài)角的實時估計。考慮動態(tài)情況下車輛外部加速度影響，建立一階外部加速度模型，利用方向余弦矩陣法進行姿態(tài)解算，具體卡爾曼濾波算法如下：

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

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

為驗證系統(tǒng)測量的精確性與系統(tǒng)的適用性，借助Innova 2100型搖床、MTi 300航向與姿態(tài)參考模塊和ZP 9500型自走式高地隙噴霧機進行試驗驗證。

美國New Brunswick公司生產(chǎn)的Innova 2100型搖床（簡稱搖床）能提供25~500 r/min的向心旋轉(zhuǎn)運動，控制精度為±1 r/min，具有操作方便、性能穩(wěn)定等特點。根據(jù)向心加速度定義=4π22，本文將外部加速度辨識與姿態(tài)測量最小系統(tǒng)固定在搖床上距離旋轉(zhuǎn)中軸為的位置，通過設(shè)定不同旋轉(zhuǎn)速度，測量系統(tǒng)對外部加速度辨識的準確度。

ZP 9500 型自走式高地隙噴桿噴霧機（簡稱高地隙噴霧機）是雷沃重工股份有限公司2016年推向市場的一款植保機械，四輪驅(qū)動、最小轉(zhuǎn)彎半徑3.2 m，最高離地間隙1.1m，噴幅11.5 m，廣泛運用于水稻、小麥等種植全過程田間植保管理。荷蘭Xsens公司生產(chǎn)的MTi 300型航向與姿態(tài)參考系統(tǒng)能在動態(tài)情況下提供0.3°的姿態(tài)角測量精度，廣泛運用與無人機、軍工和航運等領(lǐng)域。本文將其作為田間作業(yè)車輛外部加速度辨識與姿態(tài)測量系統(tǒng)的姿態(tài)角測量參考，安裝在田間作業(yè)高地隙噴霧機。

3.2 搖床試驗與結(jié)果分析

由于實際農(nóng)業(yè)車輛在田間作業(yè)時，外部加速度測量沒有統(tǒng)一的參照標準，故本文基于搖床進行外部加速度辨識試驗，試驗于2018年5月在美國普渡大學(xué)土木工程樓恒溫實驗室進行。如圖3所示，將系統(tǒng)固定在距離搖床旋轉(zhuǎn)中軸0.1m位置。硬件系統(tǒng)充分預(yù)熱后分別設(shè)置搖床以60、120、180、240和300 r/min轉(zhuǎn)速旋轉(zhuǎn)，對比系統(tǒng)對軸外部加速度的辨識與實際計算的搖床向心加速度，驗證對車輛外部加速度辨識的準確性。

圖3 Innova 2100型搖床試驗

以旋轉(zhuǎn)速率為60 r/min試驗結(jié)果為例具體說明。當(dāng)搖床旋轉(zhuǎn)速度為60 r/min時，為1，向心加速度的理論計算值為3.948 m/s2，即圖4中水平線所示值。由圖4可知，此時系統(tǒng)能準備辨識搖床提供的外部加速度，測量最大誤差小于0.01m/s2。

圖4 搖床旋轉(zhuǎn)速度為60 r·min-1的試驗結(jié)果

根據(jù)實踐經(jīng)驗，農(nóng)業(yè)作業(yè)機械轉(zhuǎn)彎時受外部加速度影響最為顯著，此時速度一般不超過1.5m/s、轉(zhuǎn)彎半徑一般不小于2 m，否則容易發(fā)生翻車事故。故根據(jù)向心力公式可知，農(nóng)業(yè)作業(yè)機械受水平方向外部加速度影響幅值一般不超過1。由此可知，當(dāng)搖床旋轉(zhuǎn)速度為300 r/min時（向心加速度的計算值為98.685 m/s2）為常規(guī)重力加速度的10倍，即便在復(fù)雜水田條件下，也達到作業(yè)農(nóng)機具的極限值。故本文試驗最大旋轉(zhuǎn)速度設(shè)置為300 r/min，具體試驗結(jié)果如表1所示。

表1 Innova 2100 型搖床試驗結(jié)果

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

為進一步驗證系統(tǒng)在實際田間的作業(yè)姿態(tài)測量效果，于2018年8月在華南農(nóng)業(yè)大學(xué)增城教學(xué)試驗基地進行實際田間試驗驗證。采用高地隙噴霧機平臺進行田間試驗，并以MTi300航向姿態(tài)參考模塊為姿態(tài)測量參照，對比文獻[4]提出的卡爾曼濾波算法，驗證本文帶外部加速度補償?shù)淖藨B(tài)融合算法在農(nóng)業(yè)機械作業(yè)存在外部加速度影響的有效性。如圖5所示，系統(tǒng)與MTi300航向姿態(tài)參考模塊水平安裝于高地隙噴霧機前端噴桿中心，保證2模塊傳感器坐標系相互平行。

采用信號發(fā)生器觸發(fā)（5 V，10 Hz）方波進行同步，保證采集姿態(tài)在時間軸上的對應(yīng)。在預(yù)熱完全后進行試驗數(shù)據(jù)采集，試驗結(jié)果如圖6所示。

圖5 田間試驗圖

圖6 田間試驗結(jié)果

由圖6可知，基于一階外部加速度模型的卡爾曼濾波姿態(tài)融合算法能很好適應(yīng)復(fù)雜田間作業(yè)工況下的應(yīng)用。相比于普通的卡爾曼濾波融合算法，在外部加速度存在情況下具有較高的測量精度（平均精度提高0.2°）與跟隨性。以MTi300航向姿態(tài)測量模塊為測量參照，具體測量結(jié)果如表2所示。

由表2可知，本文研制系統(tǒng)在實際田間作業(yè)車輛上，相比于MTi300航向姿態(tài)測量模塊，橫滾角誤差小于0.23°，俯仰角誤差小于0.39°，滿足農(nóng)業(yè)機械精準作業(yè)要求。

表2 田間試驗結(jié)果

4 結(jié) 論

1）針對動態(tài)情況下田間作業(yè)車輛普遍存在的外部加速度，影響了農(nóng)機具姿態(tài)的精準測量問題，本文設(shè)計了一種田間作業(yè)車輛外部加速度辨識于姿態(tài)測量的最小系統(tǒng)。采用方向余弦矩陣法進行姿態(tài)解算，基于一階外部加速度模型設(shè)計卡爾曼濾波融合算法，并基于6軸微慣性傳感器設(shè)計硬件系統(tǒng)。

2）采用Innova 2100 型搖床，以不同旋轉(zhuǎn)速率對外部加速度辨識與姿態(tài)測量系統(tǒng)進行試驗。試驗結(jié)果表明，在在外部加速度不超過10的動態(tài)情況下，系統(tǒng)的外部加速度的標識誤差小于0.214 m/s2，滿足實際復(fù)雜農(nóng)田情況下作業(yè)車輛的使用。

3）以雷沃ZP 9500型高地隙噴霧機為田間試驗平臺，采用MTi 300航向姿態(tài)測量模塊為姿態(tài)參照進行田間姿態(tài)測量對比驗證實驗。田間試驗結(jié)果表明：基于一階外部加速度模型的卡爾曼濾波融合算法能，很好適應(yīng)復(fù)雜田間作業(yè)工況下的應(yīng)用，具有較高的測量精度。橫滾角誤差不超過0.23°，俯仰角誤差不超過0.39°，滿足農(nóng)業(yè)機械精準作業(yè)要求。

本文設(shè)計的田間作業(yè)車輛外部加速度辨識與姿態(tài)測量系統(tǒng)仍需進行更大范圍的田間農(nóng)機具實際作業(yè)驗證，并根據(jù)實際情況進一步優(yōu)化融合算法。下一步研究還應(yīng)進一步考慮田間作業(yè)車輛普遍存在的側(cè)滑問題，并嘗試與視覺、全球?qū)Ш叫l(wèi)星系統(tǒng)、激光雷達等傳感器結(jié)合，進一步提高農(nóng)機具姿態(tài)測量精度。

[1] 錢永蘭，毛留喜，周廣勝. 全球主要糧食作物產(chǎn)量變化及其氣象災(zāi)害風(fēng)險評估[J]. 農(nóng)業(yè)工程學(xué)報，2016，32(1)： 226－235. Qian Yonglan, Mao Liuxi, Zhou Guangsheng. Changes in global main crop yields and its meteorological risk assessment[J]. Transactions of the Chinese Society of Agricultural Engineering (Transactions of the CSAE), 2016, 32(1): 226－235. (in Chinese with English abstract)

[2] Bai G, Ge Y, Hussain W, et al. A multi-sensor system for high throughput field phenotyping in soybean and wheat breeding[J]. Computers & Electronics in Agriculture, 2016, 128(10): 181－192.

[3] United Nations Department of Economic and Social Affairs, Population Division (2017). World Population Prospects: The 2017 Revision, Key Findings and Advance Tables[Z]. http://www.un.org/en/development/desa/population/ ESA/P/WP/248.[2018-06-18]

[4] 黃培奎，張智剛，羅錫文，等. 農(nóng)機具姿態(tài)傾角測量系統(tǒng)設(shè)計與試驗[J]. 農(nóng)業(yè)工程學(xué)報，2017，33(22)：9－16. Huang Peikui, Zhang Zhigang, Luo Xiwen, et al. 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)

[5] 羅錫文，廖娟，胡煉，等. 提高農(nóng)業(yè)機械化水平促進農(nóng)業(yè)可持續(xù)發(fā)展[J]. 農(nóng)業(yè)工程學(xué)報，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)

[6] 羅錫文，廖娟，鄒湘軍，等. 信息技術(shù)提升農(nóng)業(yè)機械化水平[J]. 農(nóng)業(yè)工程學(xué)報，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)

[7] 黃培奎，趙祚喜，靳俊棟，等. 基于AutoTrac的導(dǎo)航控制參數(shù)整定方法試驗[J]. 農(nóng)業(yè)工程學(xué)報，2015，31(增刊2)：100－106. Huang Peikui, Zhao Zuoxi, Jin Jundong, et al. Navigation control parameter tuning method based on AutoTrac[J]. Transactions of the Chinese Society of Agricultural Engineering (Transactions of the CSAE), 2015, 31(Supp.2): 100－106. (in Chinese with English abstract)

[8] 胡煉，林潮興，羅錫文，等. 農(nóng)機具自動調(diào)平控制系統(tǒng)設(shè)計與試驗[J]. 農(nóng)業(yè)工程學(xué)報，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] 馬超，鄭永軍，譚彧，等. 基于MEMS傳感器的兩軸姿態(tài)調(diào)整系統(tǒng)設(shè)計與試驗[J]. 農(nóng)業(yè)工程學(xué)報，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)

[10] 胡靜濤，高雷，白曉平，等. 農(nóng)業(yè)機械自動導(dǎo)航技術(shù)研究進展[J]. 農(nóng)業(yè)工程學(xué)報，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)

[11] Ahmed H, Tahir M. Accurate attitude estimation of a moving land vehicle using low-cost MEMS IMU sensors[J]. IEEE Transactions on Intelligent Transportation Systems, 2017, 18(7): 1723－1739.

[12] Guo Z, Cheng F, Li B, et al. Research development of silicon MEMS gyroscopes: A review[J]. Microsystem Technologies, 2015, 21(10): 2053－2066.

[13] 華南農(nóng)業(yè)大學(xué). MEMS慣性傳感器的車身傾角測量方法: 106197376 A [P]. 2018-08-07.

[14] 楊偉偉. 水田激光平地機控制系統(tǒng)優(yōu)化設(shè)計[D].廣州：華南農(nóng)業(yè)大學(xué)，2016. Yang Weiwei. Optimal Design for the Control System for Paddy Field Laser Leveler [D]. Guangzhou: South China Agricultural University, 2016. (in Chinese with English abstract)

[15] Widodo R B, Wada C. Attitude estimation using kalman filtering: external acceleration compensation considerations[J]. Journal of Sensors, 2016 (10): 1－24.

[16] Suh Y S, Park S K, Kang H J, et al. Attitude estimation adaptively compensating external acceleration[J]. JSME International Journal Series C Mechanical Systems, Machine Elements and Manufacturing. 2006; 49(1): 172－179.

[17] Guo X, Wang J, Liao F, et al. Distributed adaptive sliding mode control strategy for vehicle-following systems With nonlinear acceleration uncertainties[J]. IEEE Transactions on Vehicular Technology, 2017, 66(2): 981－991.

[18] Ligorio G, Sabatini A M. A novel kalman filter for human motion tracking with an inertial-based dynamic inclinometer[J]. IEEE Trans Biomed Eng, 2015, 62(8): 2033－2043.

[19] Sabatini A M. Estimating three-dimensional orientation of human body parts by inertial/magnetic sensing[J]. Sensors, 2011, 11(2): 1489－1525.

[20] 孟唐宇，浦劍濤，方建軍，等. 姿態(tài)解算與外力加速度同步估計算法[J]. 計算機應(yīng)用，2016，36(5)：1469－1474.Meng Tangyu, Pu Jiantao, Fang Jianjun, et al. Synchronization estimation algorithm for attitude algorithm and external force acceleration[J]. Transactions of Journal of Computer Applications, 2016, 36(5): 1469－1474. (in Chinese with English abstract)

[21] Suh Y S. Orientation estimation using a quaternion-based indirect kalman filter with adaptive estimation of external acceleration[J]. IEEE Transactions on Instrumentation & Measurement, 2010, 59(12): 3296－3305.

[22] Lee J K, Park E J, Robinovitch S N. Estimation of attitude and external acceleration using inertial sensor measurement during various dynamic conditions[J]. IEEE Trans Instrum Meas, 2012, 61(8): 2262－2273.

[23] Wu D. Strapdown INS/GPS integrated navigation using geometric algebra[J]. Advances in Applied Clifford Algebras, 2013, 23(3): 767－785.

[24] Fourati H, Belkhiat D E. Multisensor attitude estimation: Hundamental concepts and applications[M]. New York： CRC Press, 2016.

[25] 錢華明，夏全喜，闕興濤，等. 基于Kalman濾波的MEMS陀螺儀濾波算法[J]. 哈爾濱工程大學(xué)學(xué)報，2010，31(9)：1217－1221. Qian Huaming，Xia Quanxi，Que Xingtao, et al. Algorithm for a MEMS gyroscope based on kalman filter[J]. Transactions of Journal of Harbin Engineering University, 2010, 31(9): 1217－1221. (in Chinese with English abstract)

[26] Rados?aw Cechowicz. Bias drift estimation for MEMS gyroscope used in inertial navigation[J]. Acta Mechanica Et Automatica, 2017, 11(2): 104－110.

[27] He P, Cardou P, Desbiens A, et al. Estimating the orientation of a rigid body moving in space using inertial sensors[J]. Multibody System Dynamics, 2015, 35(1): 63－89.

[28] Welch G. An introduction to the kalman filter[J]. Course Notes of Acm Siggraph, 1995, 8(7): 127－132.

[29] STMicroelectronics, STM32F446RC[EB/OL]. http://www.st.com/content/st_com/zh/products/microcontrollers/stm32-32-bit-arm-cortex-mcus/stm32f4-series/stm32f446/stm32f446rc.html. [2018-08-05]

[30] Analog Devices, ADIS16445_cn[EB/OL]. https://www.analog. com/en/products/adis16445.html# product-overview. (2018- 08-05)[2018- 12-14].

[31] 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.

Development of external acceleration identification and attitude estimation system of field working vehicle

Huang Peikui, Zhang Zhigang※, Luo Xiwen, Liu Zhaopeng, Wang Hui, Yue Binbin, Gao Weiwei

(1.510642,; 2510642,)

The development of precision agriculture, intelligent agricultural machinery and equipment are an effective way to alleviate the current tense situation in world food security. The complex field environment and meticulous work effects require the agricultural machinery to have the ability to perceive the attitude of the agricultural machinery accurately in real time. For example, the precision navigation control and the leveling control of agricultural implements are all dependent on the accurate measurement of attitude. What’s more, the attitude of agricultural implements is one of the key parameters of agricultural mechanics modeling and agricultural implements safety warning learning. However, the external acceleration of the vehicle, which is generally present under dynamic operation conditions, poses a challenge. In order to further improve the precision operation of agricultural machinery and equipment, the paper developed a minimal hardware system for external acceleration identification and attitude estimation used for field working vehicles, and verified by the experiments taken place on the Innova 2100 shaker and the ZP9500 high level sprayer in the field. Modern micro-electromechanical systems (MEMS) technologies provide the moderate-cost and miniaturized solutions for the development of attitude reference system. By using of highly-integrated inertial measurement units (IMUs) ADIS16445 provided by ADI company and micro ARM processor STM32F446 provided by ST company, the hardware platform was built. ADIS16445 ISensor? included 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 direction cosine matrix algorithm. Based on the gyroscope and accelerometer measurement model, an one order external acceleration measurement model was proposed, and a Kalman filter fusion algorithm with 4 state vectors was established. Since the bias of gyroscopes and accelerometers was stable after hardware preheated, the impact of bias of MEMS inertial sensors in the fusion algorithm was not considered. Innova 2100 shaker is a standard equipment of rotary motion, by setting different rotation speed, different centripetal accelerations can be achieved to verify the external acceleration identification. Innova 2100 shaker test results showed that the measure error was less than 0.214 m/s2under the external acceleration lower than 10. Field experiments were conducted on Innova 2100 shaker and the ZP9500 high level sprayer provided by LOVOL company with the assistance of attitude and position reference system (AHRS) MTi300 provided by Xsens company. The MTi300 AHRS provide high precision attitude and heading output with high stability and fast dynamic response with dynamic measurement accuracy of 0.3 °, which make it widely used in navigation implements, automobiles, agricultural implements and other fields. Experiment results from high level sprayer showed that compared to the MTi300 AHRS, the average measurement error of roll angle was 0.069 ° and the maximal measurement error was 0.23 ° respectively. The average measurement error of pitch angle was 0.078 ° and the maximal measurement error was 0.39 ° respectively. Test results verified that the proposed Kalman filter algorithm was accuracy and stable, which can improving the quality of agricultural implements operations and have more applicability.

agricultural machinery; sensors; attitude estimation; external acceleration; kalman filtering; directional cosine matrix

黃培奎，張智剛，羅錫文，劉兆朋，王 輝，岳斌斌，高維煒. 田間作業(yè)車輛外部加速度辨識與姿態(tài)測量系統(tǒng)研制[J]. 農(nóng)業(yè)工程學(xué)報，2019，35(3)：9－15. doi：10.11975/j.issn.1002-6819.2019.03.002 http://www.tcsae.org

Huang Peikui, Zhang Zhigang, Luo Xiwen, Liu Zhaopeng, Wang Hui, Yue Binbin, Gao Weiwei. Development of external acceleration identification and attitude estimation system of field working vehicle[J]. Transactions of the Chinese Society of Agricultural Engineering (Transactions of the CSAE), 2019, 35(3): 9－15. (in Chinese with English abstract) doi：10.11975/j.issn.1002-6819.2019.03.002 http://www.tcsae.org

2018-06-24

2019-01-12

“十三五”國家重點研發(fā)計劃項目（2017YFD0700400- 2017YFD0700404）；廣東省科技計劃項目（2016B020205003）

黃培奎，博士，主要從事智能農(nóng)機裝備姿態(tài)檢測研究。 Email：peikuihuang@stu.scau.edu.cn

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

10.11975/j.issn.1002-6819.2019.03.002

S220.5；TP391

A

1002-6819(2019)-03-0009-07