本發(fā)明屬于慣性導(dǎo)航技術(shù)領(lǐng)域，特別涉及一種適用于農(nóng)業(yè)機(jī)械精準(zhǔn)作業(yè)的姿態(tài)傾角估計(jì)方法。

背景技術(shù)：

慣性導(dǎo)航技術(shù)是20世紀(jì)中期發(fā)展起來的完全自主式導(dǎo)航技術(shù)。通過慣性測量組件測量載體相對慣性空間的角速率與加速度信息，利用牛頓運(yùn)動定律自動推算載體瞬時速度與位置信息，具有不依賴外界信息、不向外界輻射能量、不受干擾、隱蔽性好的特點(diǎn)。能連續(xù)地提供載體的全部導(dǎo)航、制導(dǎo)參數(shù)(位置、線速度、角速度、姿態(tài)角)，廣泛運(yùn)用于航天、航空、航海領(lǐng)域，特別是軍事領(lǐng)域。而姿態(tài)傾角的準(zhǔn)確估量是實(shí)現(xiàn)運(yùn)動物體控制的關(guān)鍵參數(shù)。

姿態(tài)傾角的準(zhǔn)確估量對農(nóng)業(yè)機(jī)械精準(zhǔn)作業(yè)也尤為重要。譬如農(nóng)業(yè)機(jī)械導(dǎo)航作業(yè)時，由于田面的不平整，單純依賴gnss定位并不能保證拖拉機(jī)直線作業(yè)，只有借助車身實(shí)時姿態(tài)信息進(jìn)行投影校正，方能保證拖拉機(jī)按照規(guī)劃路徑準(zhǔn)確行走作業(yè)，具體說明書附圖1與圖2所示。又如平地機(jī)與噴霧機(jī)作業(yè)時，作業(yè)質(zhì)量的保證取決于平地鏟與噴杠的水平，這要求農(nóng)機(jī)具姿態(tài)檢測信息的實(shí)時反饋，以保證水平作業(yè)。準(zhǔn)確的姿態(tài)傾角檢測更是農(nóng)業(yè)機(jī)械與農(nóng)機(jī)具動力學(xué)建模、作業(yè)安全預(yù)警的研究的基礎(chǔ)。

農(nóng)業(yè)機(jī)械作業(yè)姿態(tài)傾角檢測多采用慣性傳感器、衛(wèi)星導(dǎo)航系統(tǒng)和圖像處理等方法獲取，還有一些采用多種傳感器融合技術(shù)進(jìn)行姿態(tài)監(jiān)測。其中慣性傳感器一般采用加速度計(jì)和陀螺儀、傾角傳感器、超聲波傳感器等對姿態(tài)進(jìn)行監(jiān)測。衛(wèi)星導(dǎo)航系統(tǒng)在農(nóng)業(yè)機(jī)械中的姿態(tài)傾角獲取方法主要是通過gps或北斗等衛(wèi)星導(dǎo)航系統(tǒng)結(jié)合其他慣性傳感器進(jìn)行融合測量。機(jī)器視覺的方法針對不同田間作物的特點(diǎn)，采取的處理算法也不同。常見的用于姿態(tài)傾角獲取的田間特征有作物行線、壟線、田間道路等。傳感器融合技術(shù)主要是將多個傳感器結(jié)合起來對姿態(tài)傾角進(jìn)行監(jiān)測。利用mems傳感器實(shí)對農(nóng)業(yè)機(jī)械作業(yè)姿態(tài)傾角測量是目前應(yīng)用最廣泛的方法，其經(jīng)濟(jì)性、穩(wěn)定性和適應(yīng)性指標(biāo)相比其他測量方法都具有突出優(yōu)勢。

然而目前農(nóng)業(yè)機(jī)械姿態(tài)傾角檢測傾向于直接將廣泛應(yīng)用于軍工、航天航空、船舶、汽車、機(jī)器人等各個領(lǐng)域的高精度姿態(tài)傳感器運(yùn)用過來。這既增加了農(nóng)業(yè)機(jī)械作業(yè)成本，同時由于缺乏對農(nóng)業(yè)機(jī)械本身特殊結(jié)構(gòu)以及農(nóng)田復(fù)雜作業(yè)環(huán)境的考慮，效果并不理想，產(chǎn)生許多實(shí)際應(yīng)用問題。

姿態(tài)傾角更新解算是慣性導(dǎo)航系統(tǒng)的關(guān)鍵算法，傳統(tǒng)的姿態(tài)傾角更新算法有歐拉角法、方向余弦和四元素法。四元素法計(jì)算時需要進(jìn)行泰勒展開，通常采用忽略其高階項(xiàng)將非線性轉(zhuǎn)化成線性進(jìn)行姿態(tài)估算，存在誤差。方向余弦總共有9個參數(shù)，計(jì)算量大，不適宜工程應(yīng)用。歐拉角法是直接迭代歐拉角微分方程，計(jì)算最為簡便直接，適用于地面作業(yè)農(nóng)業(yè)機(jī)械(不存在俯仰角奇點(diǎn)情況)。mems陀螺儀存在固有零偏，長時間積分運(yùn)算存在累積誤差，不適合單獨(dú)使用。加速度計(jì)可以解算準(zhǔn)靜態(tài)峽物體歐拉姿態(tài)角，無積分運(yùn)算不存在累積誤差問題，但其解算的姿態(tài)角動態(tài)響應(yīng)差，無法在動態(tài)條件下應(yīng)用。目前國內(nèi)外已經(jīng)開發(fā)多種將兩類器件相結(jié)合的融合算法，例如互補(bǔ)濾波、卡爾曼濾波、梯度算法、模糊算法等，其中數(shù)卡爾曼濾波算法應(yīng)用最為廣泛。

但目前實(shí)際運(yùn)用時缺乏考慮傳感器本身特性，如陀螺儀與加速度計(jì)固有零偏、陀螺儀的隨機(jī)擾動等。同時還缺乏對不同工況下過程誤差協(xié)方差矩陣q與測量誤差協(xié)方差矩陣r的研究，適應(yīng)性有限，特別是對于作業(yè)工況與環(huán)境復(fù)雜的農(nóng)業(yè)機(jī)械。

技術(shù)實(shí)現(xiàn)要素：

針對農(nóng)業(yè)機(jī)械作業(yè)姿態(tài)傾角測量的需求以及慣性傳感器測量技術(shù)存在的問題，本發(fā)明提供了一種適用于農(nóng)業(yè)機(jī)械精準(zhǔn)作業(yè)的姿態(tài)傾角估計(jì)方法。其目的在與通過傳感器模型與算法模型的構(gòu)建，提供一種適用于農(nóng)業(yè)機(jī)械精準(zhǔn)作業(yè)需求的姿態(tài)傾角估計(jì)方法，并充分考慮其在農(nóng)業(yè)機(jī)械作業(yè)應(yīng)用的實(shí)際問題。

為了達(dá)到上述目的，本發(fā)明采用以下技術(shù)方案：

本發(fā)明的一種適用于農(nóng)業(yè)機(jī)械精準(zhǔn)作業(yè)的姿態(tài)傾角估計(jì)方法，包括下述步驟：

s1、對陀螺儀與加速度計(jì)固有零偏估算，準(zhǔn)靜態(tài)情況下在mems傳感器預(yù)熱完成后進(jìn)行統(tǒng)計(jì)平均估算，將傳感器原始輸出減去得到的固有零偏bax、bay、baz、bgx、bgy、bgz作為后續(xù)計(jì)算的輸入；

s2、采用二階ar模型估計(jì)x軸陀螺儀與y軸陀螺儀測量誤差φerr1、φerr2、θerr1、θerr2并作為卡爾曼濾波的狀態(tài)輸入量，并自適應(yīng)估算測量誤差協(xié)方差矩陣r；

s3、利用三軸加速度計(jì)的三維空間幾何模型估計(jì)姿態(tài)橫滾角φ、俯仰角θ的初始值，并作為卡爾曼濾波器的狀態(tài)輸入；

s4、建立包含6個狀態(tài)向量與2個觀測向量的系統(tǒng)卡爾曼濾波狀態(tài)方程和測量方程，并借助三軸加速度計(jì)空間幾何模型自適應(yīng)調(diào)整過程誤差協(xié)方差矩陣q，精準(zhǔn)估算姿態(tài)傾角信息。

作為優(yōu)選的技術(shù)方案，步驟s1之前，還包括硬件平臺的搭建步驟，所述硬件平臺是基于adis16445慣性傳感器與stm32f446核心處理器搭建而成，具有穩(wěn)定可靠3軸加速度計(jì)、3軸陀螺儀與溫度信息輸出，輸出頻率為100hz。

作為優(yōu)選的技術(shù)方案，adis16445慣性傳感器在上電后至農(nóng)業(yè)機(jī)械開始作業(yè)前的準(zhǔn)靜態(tài)過程中，監(jiān)控傳感器輸出溫度，預(yù)熱完成后便開始存儲傳感器原始數(shù)據(jù)，利用統(tǒng)計(jì)平均方法估算出三軸陀螺儀和三軸加速度計(jì)的固有偏零。

作為優(yōu)選的技術(shù)方案，步驟s2中還包括下述步驟：

搭建x軸與y軸陀螺儀的時間序列自回歸滑動平均誤差模型:

其中n與m為陀螺儀誤差模型階數(shù)，ai與bj為陀螺儀誤差模型系數(shù)，ε為符合正態(tài)分布的隨機(jī)白噪聲殘差，k為本次計(jì)算的時序；

陀螺儀誤差模型根據(jù)aic、bic法則定型后可解算出φerr1、φerr2、θerr1、θerr24個狀態(tài)向量。

作為優(yōu)選的技術(shù)方案，在步驟s3中，

初始姿態(tài)傳感器為準(zhǔn)靜態(tài)，利用三軸加速度計(jì)的空間幾何模型估算姿態(tài)傾角原始量：

橫滾角初始值

俯仰角初始值

其中ax、ay、az分別為三軸加速度計(jì)的測量值，由此可估算2個初始狀態(tài)向量。

作為優(yōu)選的技術(shù)方案，步驟s4具體為：

建立姿態(tài)參考系統(tǒng)的外部拓展卡爾曼濾波狀態(tài)方程與測量方程：

其中xk和zk分別表示姿態(tài)參考系統(tǒng)的狀態(tài)向量與觀測向量，uk-1為系統(tǒng)輸入，ωk-1和υk分別為狀態(tài)方程的過程誤差與測量方程的測量噪聲，k表示本次計(jì)算的時序其中：

系統(tǒng)的狀態(tài)向量為

系統(tǒng)方程為

系統(tǒng)狀態(tài)矩陣為

系統(tǒng)輸入矩陣為

系統(tǒng)觀測向量為

系統(tǒng)輸出矩陣

作為優(yōu)選的技術(shù)方案，狀態(tài)方程的過程誤差ωk-1與觀測方程的測量誤差υk均假設(shè)為零均值且符合正態(tài)分布的高斯白噪聲，對應(yīng)著系統(tǒng)的過程誤差協(xié)方差矩陣qk和測量誤差協(xié)方差矩陣rk，其中：

ωk～n(0,qk)，υk～n(0,rk)且cov[ωk,υk]＝0。

作為優(yōu)選的技術(shù)方案，測量誤差協(xié)方差矩陣rk由x軸陀螺儀與y軸陀螺儀的2階ar誤差模型決定，過程誤差協(xié)方差矩陣qk由三軸加速度計(jì)的空間幾何模型決定，其中：

e表示陀螺儀測量誤差殘差計(jì)算；

表示系統(tǒng)過程誤差協(xié)方差計(jì)算。

作為優(yōu)選的技術(shù)方案，過程誤差協(xié)方差由三軸加速度計(jì)的空間幾何模型決定，其中：

與為系統(tǒng)過程誤差協(xié)方差的最大值與最小值，對應(yīng)著不同的運(yùn)用工況；t為置信系數(shù)，取值范圍為0到1；α為系統(tǒng)過程誤差協(xié)方差系數(shù)，有三軸加速度計(jì)空間幾何模型決定；

過程誤差協(xié)方差系數(shù)α為：

g為當(dāng)?shù)刂亓铀俣龋?/p>

當(dāng)處于準(zhǔn)靜態(tài)時，有

本發(fā)明與現(xiàn)有技術(shù)相比，具有如下優(yōu)點(diǎn)和有益效果：

1、本發(fā)明采用mems慣性傳感器，集成度高，性能穩(wěn)定，成本低；

2、本發(fā)明考慮了陀螺儀與加速度計(jì)固有零偏的影響，采用統(tǒng)計(jì)平均方法進(jìn)行估算，減小原始傳感器輸出誤差；

3、本發(fā)明考慮了陀螺儀隨機(jī)誤差的影響，建立陀螺儀2階ar誤差模型，提高陀螺儀測量的準(zhǔn)確性；

4、本發(fā)明考慮了實(shí)際不同工況下過程誤差與測量誤差的變化，基于加速度計(jì)與陀螺儀模型自適應(yīng)協(xié)方差矩陣q與r，提高系統(tǒng)適用性；

5、本發(fā)明搭建系統(tǒng)的具有6個狀態(tài)向量和2個觀測向量的外部拓展卡爾曼濾波模型進(jìn)行姿態(tài)角融合，提高姿態(tài)解算精度；

綜上所述，本發(fā)明提供一種適用于農(nóng)業(yè)機(jī)械精準(zhǔn)作業(yè)的姿態(tài)傾角估計(jì)方法，采用mems慣性傳感器；考慮陀螺儀與加速度計(jì)固有零偏的影響，準(zhǔn)靜態(tài)時利用統(tǒng)計(jì)平均算法進(jìn)行估算濾除；建立陀螺儀的2階ar誤差模型估算陀螺儀的隨機(jī)零偏，并結(jié)合三軸加速度計(jì)空間幾何模型自適應(yīng)調(diào)整系統(tǒng)協(xié)方差矩陣q和r；搭建具有6個狀態(tài)向量與2個觀測向量的外部拓展卡爾曼濾波模型進(jìn)行姿態(tài)傾角融合，保證了農(nóng)業(yè)機(jī)械精準(zhǔn)作業(yè)的姿態(tài)傾角測量精度與穩(wěn)定性。

附圖說明

圖1為地形起伏時橫滾傾角對農(nóng)業(yè)機(jī)械田間精準(zhǔn)作業(yè)影響的示意圖；

圖2為地形起伏時俯仰傾角對農(nóng)業(yè)機(jī)械田間精準(zhǔn)作業(yè)影響的示意圖；

圖3為本發(fā)明采用的技術(shù)路線圖；

圖4姿態(tài)傾角測量系統(tǒng)坐標(biāo)系示意圖。

具體實(shí)施方式

為了使本發(fā)明的目的、技術(shù)方案及優(yōu)點(diǎn)更加清楚明白，以下結(jié)合附圖及實(shí)例，對本發(fā)明進(jìn)行進(jìn)一步詳細(xì)說明。此處所描述的具體實(shí)例僅僅用以解釋本發(fā)明，并不用于限定本發(fā)明。此外，下面所描述的本發(fā)明各個實(shí)施方式中所涉及到的技術(shù)特征只要彼此之間未構(gòu)成沖突就可以互相結(jié)合。

如圖3所示，本實(shí)施例適用于農(nóng)業(yè)機(jī)械精準(zhǔn)作業(yè)的姿態(tài)傾角估計(jì)方法：包括對陀螺儀與加速度計(jì)固有零偏估算和結(jié)合陀螺儀ar誤差模型的自適應(yīng)卡爾曼濾波姿態(tài)融合算法。其中陀螺儀與加速度計(jì)固有零偏估計(jì)算法是準(zhǔn)靜態(tài)情況下在mems(micro-electromechanicalsystems)傳感器預(yù)熱完成后進(jìn)行統(tǒng)計(jì)平均估算，將傳感器原始輸出減去得到的固有零偏bax、bay、baz、bgx、bgy、bgz作為后續(xù)計(jì)算的輸入。采用二階ar(autoregression)模型估計(jì)x軸陀螺儀與y軸陀螺儀測量誤差φerr1、φerr2、θerr1、θerr2并作為卡爾曼濾波的狀態(tài)輸入量并自適應(yīng)估算測量誤差協(xié)方差矩陣r。利用三軸加速度計(jì)的三維空間幾何模型估計(jì)姿態(tài)橫滾角φ、俯仰角θ的初始值，并作為卡爾曼濾波器的狀態(tài)輸入。建立包含6個狀態(tài)向量與2個觀測向量的系統(tǒng)卡爾曼濾波狀態(tài)方程和測量方程，并借助三軸加速度計(jì)空間幾何模型自適應(yīng)調(diào)整過程誤差協(xié)方差矩陣q。本發(fā)明方法考慮陀螺儀與加速度計(jì)的固有零偏，可避免姿態(tài)傾角估算時引入系統(tǒng)誤差；建立陀螺儀的二階ar誤差模型可避免隨機(jī)陀螺儀隨機(jī)誤差對姿態(tài)估計(jì)造成影響；而建立基于q、r協(xié)方差矩陣的自適應(yīng)卡爾曼濾波系統(tǒng)方程可以保證在不同狀態(tài)下的姿態(tài)估計(jì)精度。

首先是硬件平臺的搭建，本發(fā)明的硬件平臺是基于adis16445慣性傳感器與stm32f446核心處理器搭建而成，具有穩(wěn)定可靠3軸加速度計(jì)、3軸陀螺儀與溫度信息輸出，輸出頻率為100hz。由于本發(fā)明重點(diǎn)在于姿態(tài)傾角融合算法介紹，故詳細(xì)的硬件平臺信息不在此展開介紹。

建立姿態(tài)傾角測量系統(tǒng)坐標(biāo)系，如圖4所示：繞坐標(biāo)系x軸轉(zhuǎn)動為橫滾角φ，繞坐標(biāo)系y軸轉(zhuǎn)動為俯仰角θ。φ和θ均屬于歐拉轉(zhuǎn)動角，基于歐拉角的物理特性，再平臺式慣性導(dǎo)航系統(tǒng)中，歐拉角可以通過坐標(biāo)系的3個框架的角傳感器測量。

姿態(tài)傾角測量對應(yīng)的框架角速率分別為和由歐拉角隨時間的傳遞關(guān)系可建立歐拉角形式的陀螺儀運(yùn)動方程：

并由此推導(dǎo)出姿態(tài)測量模型

陀螺儀與加速度計(jì)固有零偏估計(jì)：硬件系統(tǒng)一上電程序便開始運(yùn)行，adis16445以100hz輸出傳感器原始數(shù)據(jù)，從預(yù)熱到農(nóng)業(yè)機(jī)械開始作業(yè)前姿態(tài)傾角測量系統(tǒng)處于準(zhǔn)靜態(tài)。

程序初始化后開始監(jiān)控adis16445輸出的溫度信息，adis16445的溫度測量精度為0.07386攝氏度。程序設(shè)立長度為500的堆棧存儲溫度信息，實(shí)時更新比較堆棧中溫度最大值與最小值的差值。若差值小于0.3，由經(jīng)驗(yàn)可判斷姿態(tài)傾角測量模塊已達(dá)到熱平衡(即工作溫度)。

此時程序開始打開陀螺儀與加速度計(jì)存儲堆棧，長度500。由統(tǒng)計(jì)平均算法可估算出，三軸加速度計(jì)于三軸陀螺儀在準(zhǔn)靜態(tài)的固有零偏bax、bay、baz、bgx、bgy、bgz。

搭建x軸與y軸陀螺儀的時間序列自回歸滑動平均誤差模型:其中n與m為陀螺儀誤差模型階數(shù)，ai與bj為陀螺儀誤差模型系數(shù)，ε為符合正態(tài)分布的隨機(jī)白噪聲殘差，k為本次計(jì)算的時序。根據(jù)記錄的準(zhǔn)靜態(tài)時x軸與y軸陀螺儀數(shù)據(jù),離線多次基于matlab進(jìn)行陀螺儀的時間序列自回歸滑動平均誤差模型判定,運(yùn)用最通用的aic(akaikeinformationcriterion)與bic(bayesianinformationcriterion)準(zhǔn)則判定，結(jié)果為2階ar模型最為適宜。故實(shí)際工作時可在程序初始化后實(shí)時解算得到φerr1、φerr2、θerr1、θerr24個狀態(tài)向量，其模型為：

準(zhǔn)靜態(tài)情況下，利用三軸加速度計(jì)的空間幾何模型估算姿態(tài)傾角原始量：其中橫滾角初始值俯仰角初始值ax、ay、az分別為三軸加速度計(jì)的測量值。實(shí)際工作時可在陀螺儀與加速度計(jì)固有零偏估算完成后實(shí)時解算得到φ、θ2個初始狀態(tài)向量；

建立姿態(tài)傾角參考系統(tǒng)的外部拓展卡爾曼濾波狀態(tài)方程與測量方程：其中xk和zk分別表示姿態(tài)傾角參考系統(tǒng)的狀態(tài)向量與觀測向量，uk-1為系統(tǒng)輸入，ωk-1和υk分別為狀態(tài)方程的過程誤差與測量方程的測量噪聲，k表示本次計(jì)算的時序，a、b、h分別為系統(tǒng)狀態(tài)矩陣、輸入矩陣與輸出矩陣，其中：

系統(tǒng)的狀態(tài)向量為

系統(tǒng)方程為

系統(tǒng)狀態(tài)矩陣為

系統(tǒng)輸入矩陣為

系統(tǒng)觀測向量為

系統(tǒng)輸出矩陣

系統(tǒng)狀態(tài)方程的過程誤差ωk-1與觀測方程的測量誤差υk均假設(shè)為零均值且符合正態(tài)分布的高斯白噪聲，對應(yīng)著系統(tǒng)的過程誤差協(xié)方差矩陣qk和測量誤差協(xié)方差矩陣rk，其中：

ωk～n(0,qk)，υk～n(0,rk)且cov[ωk,υk]＝0；

過程誤差協(xié)方差矩陣qk和測量誤差協(xié)方差矩陣rk自適應(yīng)：測量誤差協(xié)方差矩陣rk由x軸陀螺儀與y軸陀螺儀的2階ar誤差模型決定，過程誤差協(xié)方差矩陣qk由三軸加速度計(jì)的空間幾何模型決定，其中：

e表示陀螺儀測殘差計(jì)算，為不同工況下自適應(yīng)實(shí)時更新；

其中表示系統(tǒng)過程誤差協(xié)方差計(jì)算；

系統(tǒng)過程誤差協(xié)方差由三軸加速度計(jì)的空間幾何模型決定，其中：

與為系統(tǒng)過程誤差協(xié)方差的最大值與最小值，本發(fā)明中的經(jīng)驗(yàn)值為0.00038與0.000112；t為置信系數(shù)，取值范圍為0到1，本發(fā)明中的經(jīng)驗(yàn)值為0.9；α為系統(tǒng)過程誤差協(xié)方差系數(shù)，有三軸加速度計(jì)空間幾何模型決定；

本發(fā)明中系統(tǒng)過程誤差協(xié)方差系數(shù)α為：g為當(dāng)?shù)刂亓铀俣龋?/p>

當(dāng)處于準(zhǔn)靜態(tài)時有

不同工況下q自適應(yīng)實(shí)時更新。

可以看出本發(fā)明充分考慮了mems慣性陀螺儀與加速計(jì)特性，首先對準(zhǔn)靜態(tài)下固有零偏進(jìn)行估算剔除，又建立了陀螺儀的2階ar模型排除了隨機(jī)擾動的影響。建立自適應(yīng)外部卡爾曼濾波模型，提高了不同工況下的解算精度與系統(tǒng)測量精度。

上述實(shí)施例為本發(fā)明較佳的實(shí)施方式，但本發(fā)明的實(shí)施方式并不受上述實(shí)施例的限制，其他的任何未背離本發(fā)明的精神實(shí)質(zhì)與原理下所作的改變、修飾、替代、組合、簡化，均應(yīng)為等效的置換方式，都包含在本發(fā)明的保護(hù)范圍之內(nèi)。