本發(fā)明涉及農(nóng)業(yè)機(jī)械姿態(tài)測(cè)量領(lǐng)域，特別涉及一種基于雙天線GNSS接收機(jī)/陀螺儀信息融合的農(nóng)機(jī)橫滾角測(cè)試方法。

背景技術(shù)：

農(nóng)業(yè)機(jī)械自動(dòng)導(dǎo)航是精準(zhǔn)農(nóng)業(yè)技術(shù)體系中的一項(xiàng)核心關(guān)鍵技術(shù)，廣泛應(yīng)用于耕作、播種、施肥、噴藥、收獲等農(nóng)業(yè)生產(chǎn)過(guò)程。農(nóng)機(jī)的自動(dòng)導(dǎo)航控制主要是對(duì)農(nóng)機(jī)底部的橫向位置偏差進(jìn)行控制，即控制農(nóng)機(jī)跟蹤事先規(guī)劃好的作業(yè)路徑，使其與路徑之間的橫向位置偏差保持在一定的精度范圍之內(nèi)，從而滿足農(nóng)業(yè)生產(chǎn)的需要。在農(nóng)機(jī)自動(dòng)導(dǎo)航路徑跟蹤控制方法中，農(nóng)機(jī)底盤與規(guī)劃作業(yè)路徑之間的橫向位置偏差主要受GNSS接收機(jī)定位精度和傾斜校正中農(nóng)機(jī)橫滾角精度的影響。載波相位差分的GNSS接收機(jī)符合定位精度的要求，因此獲取農(nóng)機(jī)高精度的橫滾角信息是獲取精確橫向位置偏差的關(guān)鍵，而獲取精確的位置偏差是農(nóng)機(jī)控制穩(wěn)定和精確的關(guān)鍵。

傳統(tǒng)的高精度測(cè)姿是利用慣導(dǎo)來(lái)實(shí)現(xiàn)的，通過(guò)陀螺和加速度計(jì)感應(yīng)運(yùn)動(dòng)過(guò)程中的角速度以及線加速度，通過(guò)積分和推算方法獲得運(yùn)動(dòng)載體的姿態(tài)角。其設(shè)備復(fù)雜、價(jià)格昂貴、維修困難、長(zhǎng)期精度差。近年來(lái)由于GNSS載波相位測(cè)量技術(shù)迅猛發(fā)展，利用GNSS來(lái)測(cè)量運(yùn)動(dòng)載體的航向和姿態(tài)信息已經(jīng)成為導(dǎo)航領(lǐng)域的又一新的熱點(diǎn)。雙天線測(cè)姿技術(shù)是指在載體上合理安置兩個(gè)GNSS天線，通過(guò)天線間的載波相位差分，求解出兩天線的基線矢量，從而求解出載體的二維姿態(tài)信息。為了降低接收機(jī)硬件成本，一類采用時(shí)鐘同步技術(shù)的OEM雙天線GNSS接收機(jī)開始出現(xiàn)，并逐漸被運(yùn)用于高精度的姿態(tài)測(cè)量領(lǐng)域。2008年以后，一體雙天線接收機(jī)開始被廣泛關(guān)注和認(rèn)可，雙天線接收機(jī)開始大批量進(jìn)入市場(chǎng)。利用一體雙天線接收機(jī)測(cè)量運(yùn)動(dòng)載體的姿態(tài)信息，具有穩(wěn)定性好、成本低廉、精度高、實(shí)時(shí)性好、無(wú)需初始校正和無(wú)時(shí)間累積誤差等優(yōu)點(diǎn)，但存在傾斜角信息白噪聲較大的問(wèn)題。相對(duì)于雙天線GNSS而言，陀螺儀輸出數(shù)據(jù)平穩(wěn)，短時(shí)間穩(wěn)定性強(qiáng)。利用單軸陀螺儀角速度和雙天線傾斜角度信息融合的方法測(cè)試農(nóng)機(jī)橫滾角，對(duì)于農(nóng)機(jī)獲取精確、高可靠性的橫滾角具有十分重要的意義。

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

本發(fā)明的主要目的在于克服現(xiàn)有技術(shù)中雙天線GNSS接收機(jī)輸出的農(nóng)機(jī)橫滾角白噪聲大，無(wú)法滿足農(nóng)機(jī)傾斜校正的精度要求，提供一種基于雙天線GNSS接收機(jī)/陀螺儀信息融合的農(nóng)機(jī)橫滾角測(cè)試方法。

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

本發(fā)明基于雙天線GNSS接收機(jī)/陀螺儀信息融合的農(nóng)機(jī)橫滾角測(cè)試方法，包括下述步驟：

1)為農(nóng)機(jī)建立車體坐標(biāo)系，車體坐標(biāo)系Oxy定義為以車體質(zhì)心位置為原點(diǎn)O，橫軸x指向車體前進(jìn)方向，縱軸y與橫軸x垂直指向車體前進(jìn)方向的左側(cè)；

2)雙天線固定支架為長(zhǎng)度不低于1.3m的矩形鋼管，安裝在車體質(zhì)心正上方的拖拉機(jī)駕駛室頂部，安裝原則為鋼管平行于車體坐標(biāo)系y軸，關(guān)于原點(diǎn)O對(duì)稱；

3)將GNSS主天線安裝在雙天線固定支架的右端，從天線安裝在固定支架的左端，主從天線關(guān)于車體坐標(biāo)系的x軸對(duì)稱；

4)陀螺儀安裝在農(nóng)機(jī)質(zhì)心正上方拖拉機(jī)座椅下方，旋轉(zhuǎn)軸指向車體坐標(biāo)系x軸方向；

5)設(shè)置雙天線GNSS接收機(jī)輸出NMEA-0183標(biāo)準(zhǔn)格式中$GPTRA數(shù)據(jù)包，截取$GPTRA數(shù)據(jù)包中的傾斜角ψ；利用陀螺儀獲取橫滾角速度ω；

6)采用卡爾曼濾波模型對(duì)步驟5)獲取的橫滾角度和橫滾角速度進(jìn)行融合處理，獲得農(nóng)機(jī)橫滾角度最優(yōu)估計(jì)值；

7)步驟6)中所述卡爾曼濾波器的模型中，狀態(tài)向量為農(nóng)業(yè)機(jī)械的橫滾角θ和陀螺儀的零偏b二維向量；測(cè)量量為雙天線GNSS接收機(jī)獲取的橫滾角ψ一維標(biāo)量；

8)系統(tǒng)過(guò)程噪聲協(xié)方差矩陣Q為用于表示狀態(tài)轉(zhuǎn)移方程的誤差大小，設(shè)定為常數(shù)矩陣，在仿真和試驗(yàn)過(guò)程中整定矩陣參數(shù)，測(cè)量噪聲方差R為雙天線GNSS接收機(jī)橫滾角測(cè)量值方差統(tǒng)計(jì)值。

作為優(yōu)選的技術(shù)方案，步驟5)中，所述雙天線GNSS接收機(jī)采用司南K528GNSS接收機(jī)，通過(guò)串口輸出NMEA-0183格式中的$GPTRA數(shù)據(jù)包，截取$GPTRA數(shù)據(jù)包中的航向角信息。

作為優(yōu)選的技術(shù)方案，步驟5)中，設(shè)置微型姿態(tài)參考系統(tǒng)AHRS通過(guò)串口輸出x軸MEMS陀螺儀標(biāo)定角速度數(shù)據(jù)，以獲取拖拉機(jī)的橫滾角速度。

作為優(yōu)選的技術(shù)方案，利用卡爾曼濾波器進(jìn)行信息融合的方法為：

首先建立卡爾曼濾波器的狀態(tài)方程和測(cè)量方程，橫滾角和橫滾角速度存在導(dǎo)數(shù)關(guān)系，系統(tǒng)傾斜真實(shí)角度θ用來(lái)做一個(gè)狀態(tài)向量，在該系統(tǒng)中，采用雙天線GNSS接收機(jī)橫滾角測(cè)量值估計(jì)出陀螺儀零偏b，以此偏差作為狀態(tài)向量得到相應(yīng)的狀態(tài)方程和觀測(cè)方程：

其次，通過(guò)系統(tǒng)過(guò)程噪聲協(xié)方差陣Q以及測(cè)量誤差的協(xié)方差矩陣R，對(duì)卡爾曼濾波器進(jìn)行校正，Q與R矩陣的形式如下：

R＝[r_double]

q_double和q_gyro分別是雙天線GNSS接收機(jī)和陀螺儀測(cè)量的協(xié)方差，其數(shù)值代表卡爾曼濾波器對(duì)其傳感器數(shù)據(jù)的信任程度，值越小表明信任程度越高；r_double是雙天線GNSS接收機(jī)橫滾角測(cè)量噪聲的方差統(tǒng)計(jì)值；

最后，采用線性離散卡爾曼濾波器的遞歸差分方程進(jìn)行狀態(tài)向量預(yù)測(cè)和測(cè)量向量校正：

預(yù)測(cè)方程組為：

式中，是k時(shí)刻的預(yù)測(cè)結(jié)果，是k-1時(shí)刻的最優(yōu)結(jié)果，是對(duì)應(yīng)的協(xié)方差，A^T表示A的轉(zhuǎn)置矩陣，Q是系統(tǒng)的過(guò)程協(xié)方差；

校正方程組為：

K_k為時(shí)刻k時(shí)的卡爾曼增益矩陣，是時(shí)刻k的最優(yōu)化估算值。

作為優(yōu)選的技術(shù)方案，為了使卡爾曼濾波器不斷的運(yùn)行下去直到找到最優(yōu)的角度值，需更新k狀態(tài)下的協(xié)方差P_k，I為單位陣，當(dāng)系統(tǒng)進(jìn)入k+1狀態(tài)時(shí),P_k就是式子(2)的P_k-1，(3)、(4)、(5)式為卡爾曼濾波器狀態(tài)更新方程，計(jì)算完時(shí)間更新方程和測(cè)量更新方程后，再次重復(fù)上一次計(jì)算得到的后驗(yàn)估計(jì)，作為下一次計(jì)算的先驗(yàn)估計(jì)，這樣，周而復(fù)始、循環(huán)反復(fù)地運(yùn)算下去直至找到最優(yōu)的結(jié)果。

作為優(yōu)選的技術(shù)方案，步驟7)中，所述卡爾曼濾波模型中，狀態(tài)轉(zhuǎn)移方程是基于角度、角速度的計(jì)算關(guān)系推算原理構(gòu)建的，ω為橫滾角速度控制輸入，由陀螺儀獲得的加速度值實(shí)時(shí)更新。

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

1、本發(fā)明采用卡爾曼濾波方法對(duì)來(lái)自雙天線GNSS接收機(jī)和陀螺儀的信號(hào)進(jìn)行融合，解決由于雙天線GNSS接收機(jī)橫滾角測(cè)量噪聲大不易直接用于農(nóng)機(jī)姿態(tài)校正的問(wèn)題。

2本發(fā)明雙天線GNSS接收機(jī)/陀螺儀信息融合的農(nóng)機(jī)橫滾角測(cè)試方法可形成更連續(xù)和穩(wěn)定的農(nóng)業(yè)機(jī)械橫滾角數(shù)據(jù)，使得農(nóng)機(jī)導(dǎo)航的控制更加穩(wěn)定準(zhǔn)確。

3本發(fā)明雙天線GNSS接收機(jī)/陀螺儀信息融合的農(nóng)機(jī)橫滾角測(cè)試方法相比于加速度計(jì)與陀螺儀數(shù)據(jù)融合，雙天線GNSS橫滾角測(cè)試方法比加速度計(jì)橫滾角測(cè)試方法動(dòng)態(tài)性能好。

4本發(fā)明雙天線GNSS接收機(jī)/陀螺儀信息融合的農(nóng)機(jī)橫滾角測(cè)試方法可以代替昂貴的高精度姿態(tài)傳感器測(cè)量農(nóng)機(jī)橫滾角，節(jié)省農(nóng)機(jī)自動(dòng)導(dǎo)航系統(tǒng)成本。

附圖說(shuō)明

圖1為本發(fā)明實(shí)施例農(nóng)機(jī)橫滾角測(cè)試方法流程圖；

圖2為本發(fā)明實(shí)施例設(shè)備安裝結(jié)構(gòu)示意圖；

圖3為X軸陀螺儀原始數(shù)據(jù)累積橫滾角；

圖4是陀螺儀零偏最優(yōu)估計(jì)值；

圖5是濾波前后橫滾角對(duì)比圖。

具體實(shí)施方式

下面結(jié)合實(shí)施例及附圖對(duì)本發(fā)明作進(jìn)一步詳細(xì)的描述，但本發(fā)明的實(shí)施方式不限于此。

實(shí)施例

本實(shí)施例采用農(nóng)業(yè)機(jī)械為雷沃M904-D輪式拖拉機(jī)，采用的設(shè)備或裝置主要包括：司南K528雙天線GNSS接收機(jī)、雙天線固定支架和微型姿態(tài)參考系統(tǒng)AHRS。如圖2所示為本發(fā)明實(shí)施例的設(shè)備安裝結(jié)構(gòu)示意圖，為輪式拖拉機(jī)1建立車體坐標(biāo)系，車體坐標(biāo)系Oxy定義為以車體質(zhì)心位置為原點(diǎn)O，橫軸x指向車體前進(jìn)方向，縱軸y與橫軸垂直指向車體前進(jìn)方向的左側(cè)。雙天線固定支架2為長(zhǎng)度不低于1.3m的矩形鋼管，安裝在車體質(zhì)心正上方的拖拉機(jī)駕駛室頂部，安裝原則為鋼管平行于車體坐標(biāo)系y軸，關(guān)于原點(diǎn)O對(duì)稱。GNSS主天線3安裝在雙天線固定支架的右端(車體前進(jìn)方向?yàn)榍?，從天線4安裝在固定支架的左端，主從天線關(guān)于車體坐標(biāo)系的x軸對(duì)稱。設(shè)置雙天線接收機(jī)輸出NMEA-0183標(biāo)準(zhǔn)格式中$GPTRA數(shù)據(jù)包。截取$GPTRA數(shù)據(jù)包中的傾斜角ψ。截取微型姿態(tài)參考系統(tǒng)AHRS獲取的橫滾角速度ω。采用卡爾曼濾波器對(duì)獲取的橫滾角度ψ和橫滾角速度ω進(jìn)行融合處理，獲得農(nóng)機(jī)橫滾角度精確估計(jì)值。所述卡爾曼濾波器的模型中，狀態(tài)向量為農(nóng)業(yè)機(jī)械的橫滾角θ和陀螺儀的零偏b；狀態(tài)轉(zhuǎn)移方程是基于角度、角速度的計(jì)算關(guān)系推算原理構(gòu)建的。測(cè)量值為雙天線接收機(jī)獲取的橫滾角ψ。測(cè)量矩陣是基于預(yù)測(cè)狀態(tài)向量與觀測(cè)值之間關(guān)系構(gòu)建的。

如圖1所示，本實(shí)施例雙天線GNSS接收機(jī)/陀螺儀信息融合的農(nóng)機(jī)橫滾角測(cè)試方法，具體處理步驟和公式推導(dǎo)如下：

1、接收機(jī)數(shù)據(jù)采集：設(shè)置司南K528GNSS接收機(jī)通過(guò)串口以10Hz的輸出頻率輸出NMEA-0183格式中的$GPTRA數(shù)據(jù)包，利用XCOM V2.0串口助手接收$GPTRA數(shù)據(jù)，打開串口助手時(shí)間戳功能，即在每包$GPTRA數(shù)據(jù)后面加上獲取該包數(shù)據(jù)的時(shí)間信息，數(shù)據(jù)采集完成后保存至txt文本文檔。

2、姿態(tài)傳感器數(shù)據(jù)采集：設(shè)置AHRS通過(guò)串口以20Hz的輸出頻率輸出X軸MEMS陀螺儀角速度標(biāo)定數(shù)據(jù)，利用XCOM V2.0串口助手接收AHRS輸出數(shù)據(jù)，打開串口助手時(shí)間戳功能，即在每包數(shù)據(jù)后面加上獲取該包數(shù)據(jù)的時(shí)間信息。姿態(tài)傳感器和接收機(jī)的數(shù)據(jù)采集同時(shí)進(jìn)行，數(shù)據(jù)采集完成后保存至txt文本文檔。

3、根據(jù)雙天GNSS接收機(jī)和AHRS數(shù)據(jù)包獲取時(shí)間信息，利用Microsoft excel工作表的數(shù)據(jù)篩選功能截取出雙天線GNSS接收機(jī)k時(shí)刻的橫滾角ψ_k和陀螺儀對(duì)應(yīng)時(shí)刻的橫滾角速度ω_k。

4、根據(jù)步驟3獲取的k時(shí)刻的橫滾角ψ_k和橫滾角速度ω_k，卡爾曼濾波器數(shù)據(jù)融合過(guò)程：

5、首先建立卡爾曼濾波器的狀態(tài)方程和測(cè)量方程。橫滾角和橫滾角速度存在導(dǎo)數(shù)關(guān)系，系統(tǒng)傾斜真實(shí)角度θ用來(lái)做狀態(tài)向量一個(gè)分量。在該系統(tǒng)中，采用雙天線GNSS接收機(jī)測(cè)量得到的橫滾角估計(jì)出陀螺儀零偏b，以陀螺儀零偏作為狀態(tài)向量的另一個(gè)分量，得到相應(yīng)的狀態(tài)方程和觀測(cè)方程：

6、系統(tǒng)過(guò)程噪聲協(xié)方差陣Q以及測(cè)量誤差的協(xié)方差矩陣R，對(duì)卡爾曼濾波器進(jìn)行校正。Q與R矩陣的形式如下：

R＝[r_double]

7、q_double和q_gyro分別是雙天線GNSS接收機(jī)和陀螺儀測(cè)量的協(xié)方差，其數(shù)值代表卡爾曼濾波器對(duì)其傳感器數(shù)據(jù)的信任程度，值越小表明信任程度越高。r_double是雙天線GNSS接收機(jī)橫滾角測(cè)量噪聲的方差統(tǒng)計(jì)值。

8、綜上所述，采用線性離散卡爾曼濾波器的遞歸差分方程進(jìn)行狀態(tài)向量預(yù)測(cè)和測(cè)量向量校正：

預(yù)測(cè)方程組為：

式中,是k時(shí)刻的預(yù)測(cè)結(jié)果，是k-1時(shí)刻的最優(yōu)結(jié)果，是對(duì)應(yīng)的協(xié)方差。A^T表示A的轉(zhuǎn)置矩陣，Q是系統(tǒng)的過(guò)程協(xié)方差。式子(1)、(2)即對(duì)系統(tǒng)的狀態(tài)更新。

校正方程組為：

K_k為時(shí)刻k時(shí)的卡爾曼增益矩陣，是時(shí)刻k的最優(yōu)化估算值。為了使卡爾曼濾波器不斷的運(yùn)行下去直到找到最優(yōu)的角度值，我們還要更新k狀態(tài)下的協(xié)方差P_k。I為單位陣。當(dāng)系統(tǒng)進(jìn)入k+1狀態(tài)時(shí),P_k就是式子(2)的P_k-1，(3)、(4)、(5)式為卡爾曼濾波器狀態(tài)更新方程。計(jì)算完時(shí)間更新方程和測(cè)量更新方程后，再次重復(fù)上一次計(jì)算得到的后驗(yàn)估計(jì)，作為下一次計(jì)算的先驗(yàn)估計(jì)，這樣，周而復(fù)始、循環(huán)反復(fù)地運(yùn)算下去直至找到最優(yōu)的結(jié)果。

對(duì)上述信息融合方法進(jìn)行仿真測(cè)試，設(shè)定卡爾曼濾波的初始化條件是：仿真數(shù)據(jù)來(lái)自自主研發(fā)的雷沃M904-D拖拉機(jī)雙天線GNSS導(dǎo)航控制系統(tǒng)，設(shè)定卡爾曼濾波器的初始條件是：

圖3是x軸陀螺儀原始數(shù)據(jù)累積橫滾角，可以看出誤差隨時(shí)間累積越來(lái)越大。圖4是本信息融合方法對(duì)陀螺儀零偏的最優(yōu)估計(jì)值，可以看出零偏估計(jì)值迅速收斂穩(wěn)定在某個(gè)值附近。圖5是濾波前后橫滾角對(duì)比圖，線1是雙天線GNSS接收機(jī)輸出的原始橫滾角，線2是雙天線GNSS接收機(jī)/陀螺儀信息融合后的農(nóng)機(jī)橫滾角，線3是陀螺儀經(jīng)過(guò)零偏校正后累積的橫滾角。從圖5中可看出，雙天線GNSS接收機(jī)輸出的橫滾角趨勢(shì)正確，但白噪聲較大。陀螺儀累積的橫滾角數(shù)據(jù)平穩(wěn)，短時(shí)間穩(wěn)定性強(qiáng)，但長(zhǎng)期精度差。采用卡爾曼濾波方法對(duì)雙天線GNSS接收機(jī)/陀螺儀獲取的信息進(jìn)行融合處理，可形成更連續(xù)、穩(wěn)定、準(zhǔn)確的農(nóng)機(jī)橫滾角。

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