專利名稱：一種農(nóng)業(yè)機(jī)械導(dǎo)航的組合定位方法
技術(shù)領(lǐng)域：
本發(fā)明涉及導(dǎo)航定位技術(shù)，特別涉及一種農(nóng)業(yè)機(jī)械導(dǎo)航的組合定位方法。
背景技術(shù)：
精細(xì)農(nóng)業(yè)包括定位處方農(nóng)作和農(nóng)情信息自動(dòng)采集，其中定位處方農(nóng)作要求農(nóng)業(yè)機(jī)械可以按照預(yù)先規(guī)劃好的路徑在田間行走，準(zhǔn)確到達(dá)目的地并完成既定作業(yè)任務(wù)。精確導(dǎo)航是實(shí)現(xiàn)農(nóng)業(yè)機(jī)械自主行走的關(guān)鍵技術(shù)之一，其定位精度直接影響農(nóng)業(yè)機(jī)械進(jìn)行路徑自動(dòng)跟蹤的質(zhì)量。因此，提高導(dǎo)航定位的精度，是改善農(nóng)業(yè)機(jī)械路徑跟蹤質(zhì)量的首要問題。
農(nóng)業(yè)機(jī)械導(dǎo)航定位的主要信息包括位置信息和航向角度信息。在以GPS為主的導(dǎo)航定位技術(shù)方面，已有研究成果主要采用RTK-DGPS和FOG實(shí)現(xiàn)農(nóng)業(yè)機(jī)械的精確定位，其成本較高。也有一些研究成果采用低價(jià)格低精度GPS融合高精度慣性傳感器，通過卡爾曼濾波方法實(shí)現(xiàn)農(nóng)業(yè)機(jī)械的精確定位；目前這類研究還較少，而且現(xiàn)有的研究忽視卡爾曼濾波的自適應(yīng)問題，難以避免濾波發(fā)散問題。

發(fā)明內(nèi)容
本發(fā)明的目的在于克服現(xiàn)有技術(shù)中存在的缺點(diǎn)，提供一種高精度的農(nóng)業(yè)機(jī)械導(dǎo)航的組合定位方法，將多個(gè)導(dǎo)航傳感器信息經(jīng)過融合處理后，得到農(nóng)業(yè)機(jī)械更為精確的當(dāng)前位姿信息。
本發(fā)明一種農(nóng)業(yè)機(jī)械導(dǎo)航的組合定位方法，通過下述步驟實(shí)現(xiàn)(1)首先采用偽距差分GPS傳感器測(cè)量得到農(nóng)業(yè)機(jī)械的初步定位位置值；(2)然后采用電子羅盤和微機(jī)械陀螺傳感器對(duì)農(nóng)業(yè)機(jī)械的航向角度進(jìn)行測(cè)量，并通過自適應(yīng)加權(quán)融合，推算得到最優(yōu)的航向角度值；(3)再采用自適應(yīng)卡爾曼濾波器對(duì)包括初步定位位置值和航向角度值的多源信息進(jìn)行再次融合，獲得農(nóng)業(yè)機(jī)械定位和航向角度信息的精確估計(jì)。
步驟2中，所述推算得到最優(yōu)的航向角度值的具體步驟如下首先電子羅盤傳感器對(duì)農(nóng)業(yè)機(jī)械的航向角度進(jìn)行測(cè)量，微機(jī)械陀螺傳感器對(duì)農(nóng)業(yè)機(jī)械的橫向角速率進(jìn)行測(cè)量，經(jīng)積分后轉(zhuǎn)換為航向角度測(cè)量值；然后采用基于協(xié)方差函數(shù)的加窗估計(jì)算法，在線估計(jì)電子羅盤和微機(jī)械陀螺的測(cè)量方差；再根據(jù)上述在線估計(jì)的測(cè)量方差，采用自適應(yīng)加權(quán)融合估計(jì)算法以自適應(yīng)的方式推算出兩個(gè)傳感器所對(duì)應(yīng)的最優(yōu)加權(quán)因子，最后推算得到最優(yōu)的航向角度值。
步驟2中，采用最優(yōu)估計(jì)值周期性地更新微機(jī)械陀螺傳感器橫向角速率的積分初值，以防止積分發(fā)散。
步驟3中，所述對(duì)多源信息進(jìn)行再次融合的具體步驟如下首先利用航位推算的基本原理建立常速度條件下的卡爾曼濾波器的狀態(tài)方程和觀測(cè)方程；然后基于新息的理論協(xié)方差陣概念，設(shè)計(jì)一種測(cè)量噪聲協(xié)方差矩陣R(k)的自適應(yīng)調(diào)節(jié)方法；最后，在卡爾曼濾波器狀態(tài)方程和觀測(cè)方程的基礎(chǔ)上，結(jié)合測(cè)量噪聲協(xié)方差矩陣R(k)的在線調(diào)節(jié)方法，建立預(yù)測(cè)方程組和校正方程組，構(gòu)建卡爾曼濾波器，實(shí)現(xiàn)農(nóng)業(yè)機(jī)械定位和航向角度信息的精確估計(jì)。
步驟3中，所述的常速度卡爾曼濾波器模型中，狀態(tài)空間和測(cè)量向量均為GPS接收機(jī)的位置數(shù)據(jù)(x，y)和速度數(shù)據(jù)v，航向角度估計(jì)數(shù)據(jù)對(duì)狀態(tài)轉(zhuǎn)移矩陣進(jìn)行實(shí)時(shí)更新。
步驟3中，所述R矩陣的自適應(yīng)調(diào)節(jié)中，主要采用新息的實(shí)際協(xié)方差陣Pr(k)與理論協(xié)方差矩陣Cr(k)進(jìn)行對(duì)比的方法，在線決策R矩陣的調(diào)節(jié)量。
所述偽距差分GPS傳感器是指差分全球定位系統(tǒng)(DGPS)接收機(jī)。所述電子羅盤是指磁航向傳感器電子羅盤。所述微機(jī)械陀螺傳感器是指基于MEMS技術(shù)的低成本微機(jī)械陀螺。
本發(fā)明與現(xiàn)有技術(shù)相比具有如下優(yōu)點(diǎn)和效果(1)本發(fā)明提出了農(nóng)業(yè)機(jī)械航向角度的自適應(yīng)加權(quán)融合估計(jì)算法，通過對(duì)電子羅盤和微機(jī)械陀螺信息的自適應(yīng)加權(quán)融合處理，可為導(dǎo)航控制提供更為精確和可靠的航向角度估計(jì)數(shù)據(jù)。
(2)本發(fā)明采用自適應(yīng)卡爾曼濾波器可抑制發(fā)散，平滑DGPS定位數(shù)據(jù)，有效避免DGPS動(dòng)態(tài)定位的異常結(jié)果，定位精度可控制在亞米級(jí)范圍內(nèi)。
(3)本發(fā)明的導(dǎo)航組合定位方式，將各個(gè)傳感器的測(cè)量數(shù)據(jù)進(jìn)行多次融合，既平滑又可以濾除測(cè)試噪聲，形成了連續(xù)、穩(wěn)定的農(nóng)業(yè)機(jī)械導(dǎo)航數(shù)據(jù)。


圖1為農(nóng)業(yè)機(jī)械導(dǎo)航的組合定位系統(tǒng)組成示意圖。
圖2為卡爾曼濾波器的數(shù)據(jù)流向圖。
圖3為完全DGPS條件下Kalman濾波器直線跟蹤仿真結(jié)果。
圖4為由DGPS到GPS條件下Kalman濾波器直線跟蹤仿真結(jié)果。
圖5為完全DGPS條件下Kalman濾波器曲線跟蹤仿真結(jié)果。
圖6為由DGPS到GPS條件下Kalman濾波器曲線跟蹤仿真結(jié)果。
具體實(shí)施例方式
下面結(jié)合實(shí)施例對(duì)本發(fā)明做進(jìn)一步詳細(xì)的描述，但本發(fā)明的實(shí)施方式不限于此。
本實(shí)施例采用的農(nóng)業(yè)機(jī)械為久保田插秧機(jī)，其導(dǎo)航定位系統(tǒng)主要包括導(dǎo)航傳感器組合、便攜計(jì)算機(jī)和USB-RS232數(shù)據(jù)傳輸線等。導(dǎo)航傳感器組合包括偽距差分GPS、電子羅盤和微機(jī)械陀螺。偽距差分GPS可以采集插秧機(jī)在WGS-84坐標(biāo)系下的定位坐標(biāo)，作為初步定位信息；電子羅盤測(cè)量其航向角度信息(以正北方向?yàn)?，逆時(shí)針方向?yàn)檎?；微機(jī)械陀螺測(cè)量其橫向角速率(逆時(shí)針方向?yàn)檎?。便攜計(jì)算機(jī)實(shí)現(xiàn)數(shù)據(jù)采集、數(shù)據(jù)預(yù)處理和信息融合定位軟件系統(tǒng)的運(yùn)行。便攜計(jì)算機(jī)與導(dǎo)航傳感器組合通過多根USB-RS232數(shù)據(jù)傳輸線建立連接。將上述多個(gè)傳感器組合安裝在插秧機(jī)后端的臺(tái)架上，然后用數(shù)據(jù)采集軟件系統(tǒng)進(jìn)行動(dòng)態(tài)數(shù)據(jù)采集，采集頻率是1Hz。數(shù)據(jù)采集、數(shù)據(jù)預(yù)處理和信息融合定位軟件系統(tǒng)采用Visual C++編程工具在Windows XP操作系統(tǒng)下開發(fā)完成。導(dǎo)航定位系統(tǒng)結(jié)構(gòu)框圖如圖1。
所述插秧機(jī)導(dǎo)航定位系統(tǒng)所獲得的導(dǎo)航數(shù)據(jù)以偽距差分GPS定位數(shù)據(jù)為主，電子羅盤和微機(jī)械陀螺的航向角度數(shù)據(jù)為輔。為了獲得插秧機(jī)準(zhǔn)確的位置及航向估計(jì)，本發(fā)明通過兩級(jí)融合的方法，逐次對(duì)原始航向角度數(shù)據(jù)和GPS定位數(shù)據(jù)進(jìn)行融合。
插秧機(jī)的具體組合定位方法(1)首先采用偽距差分GPS傳感器測(cè)量得到插秧機(jī)的初步定位位置值；(2)然后采用電子羅盤和微機(jī)械陀螺傳感器對(duì)插秧機(jī)的航向角度進(jìn)行測(cè)量，并通過自適應(yīng)加權(quán)融合，推算得到最優(yōu)的航向角度值，實(shí)現(xiàn)一級(jí)融合；(3)再采用自適應(yīng)卡爾曼濾波器對(duì)包括初步定位位置值和航向角度值的多源信息進(jìn)行二級(jí)融合，獲得插秧機(jī)定位和航向角度信息的精確估計(jì)。
具體步驟包括1、采用偽距差分GPS傳感器測(cè)量得到插秧機(jī)的初步定位位置值；GPS接收機(jī)輸出的經(jīng)緯度坐標(biāo)是WGS-84地心大地坐標(biāo)。為了使DGPS定位數(shù)據(jù)能用于導(dǎo)航控制系統(tǒng)，需要將WGS-84地心大地坐標(biāo)轉(zhuǎn)換為對(duì)應(yīng)于WGS-84橢球的高斯平面坐標(biāo)，即需進(jìn)行高斯投影變換。
本發(fā)明采用如下公式x=X+l22NsinBcosB+l424NsinBcos3B(5-t2+9η2+4η4)+l6720NsinBcos5B(61-]]>58t2+t4)]]>y=lNcosB+l36Ncos3B(1-t2+η2)+l5120Ncos5B(5-18t2+t4+14η2-58η2t2)]]>+500000]]>式中，l＝L-L0，L0為投影帶中央子午線經(jīng)度。卯酉圈曲率半徑N=a1-e2sin2B,]]>橢球第一偏心率e＝2-2，輔助變量t＝tanB，輔助變量η＝e′cosB，橢球第二偏心率e′=a2/b2-1,]]>a、b分別為參考橢球的長(zhǎng)、短半徑，扁率＝(a-b)/a，X為赤道至緯度為B的平行圈的子午線弧長(zhǎng)，其計(jì)算公式為X=c∫0B(1+e′2cos2B-2/3)dB]]>c為極曲率半徑。
本發(fā)明所用投影坐標(biāo)系的主要參數(shù)為1)投影方式Gauss-Kruger2)中央經(jīng)線114.000000(3度帶)3)水平偏移量500km4)地理坐標(biāo)系GCS_WGS_19845)大地參照系D_WGS_19846)參考橢球體WGS_19847)橢球長(zhǎng)軸6378137.0000008)橢球扁率0.0033528107高斯投影變換后的WGS-84橢球的高斯平面坐標(biāo)可作為插秧機(jī)的初步定位位置值。
2、采用電子羅盤和微機(jī)械陀螺傳感器對(duì)插秧機(jī)的航向角度進(jìn)行測(cè)量，并通過自適應(yīng)加權(quán)融合，推算得到最優(yōu)的航向角度值，實(shí)現(xiàn)一級(jí)融合；
設(shè)定電子羅盤和微機(jī)械陀螺的航向角度測(cè)量值分別為Xp和Xq，對(duì)應(yīng)的觀測(cè)誤差分別為Vp和Vq，Xp和Xq的自協(xié)方差函數(shù)分別為Rpp和Rqq，互協(xié)方差函數(shù)為Rpq，第k次采樣時(shí)Rpp的時(shí)間域估計(jì)值為Rpp(k)，Rqq的時(shí)間域估計(jì)值為Rqq(k)，Rpq的時(shí)間域估計(jì)值為Rpq(k)。可通過如下步驟，完成一級(jí)融合1)前N次采樣時(shí)的Rpp、Rqq和Rpq的時(shí)間域估計(jì)值可由下述遞推公式獲得當(dāng)k＜N時(shí)，Rpp(k)=k-1kRpp(k-1)+1k(Xp-μ)(Xp-μ)]]>Rqq(k)=k-1kRqq(k-1)+1k(Xq-μ)(Xq-μ)]]>Rpq(k)=Rqp(k)=k-1kRpq(k-1)+1k(Xp-μ)(Xq-μ)]]>當(dāng)k＞N時(shí)，Rpp(k)=N-1NRpp(k-1)+1N(Xp-μ)(Xp-μ)]]>Rqq(k)=N-1NRqq(k-1)+1N(Xq-μ)(Xq-μ)]]>Rpq(k)=Rqp(k)=N-1NRpq(k-1)+1N(Xp-μ)(Xq-μ)]]>式中μ為采樣數(shù)據(jù)的均值，在采用上述遞推算式時(shí)，為了實(shí)時(shí)獲取μ，將后續(xù)加權(quán)融合算法中的航向角度一步預(yù)測(cè)值作為測(cè)量數(shù)據(jù)的μ。N為移動(dòng)窗口長(zhǎng)度。
2)由下式可以獲得電子羅盤和微機(jī)械陀螺的測(cè)量方差δp2、δq2δp2＝Rpp-Rpqδq2＝Rqq-Rpq3)融合后的航向角度數(shù)據(jù) 可由下式獲得X^=Xpδp2(1δp2+1δq2)+Xqδq2(1δp2+1δq2)=δq2Xp+δp2Xqδp2+δq2]]>3、采用自適應(yīng)卡爾曼濾波器對(duì)包括初步定位位置值和航向角度值的多源信息進(jìn)行二級(jí)融合，獲得插秧機(jī)定位和航向角度信息的精確估計(jì)。
首先依據(jù)航位推算原理建立自適應(yīng)卡爾曼濾波器狀態(tài)轉(zhuǎn)移方程和觀測(cè)方程。
θk為插秧機(jī)方位角，即插秧機(jī)縱向方向與x軸正向之間的夾角；v為插秧機(jī)在縱向方向上的前進(jìn)速度；(xk，yk)為插秧機(jī)在k時(shí)刻的坐標(biāo)，(xk+1，yk+1)為插秧機(jī)在k+1時(shí)刻的坐標(biāo)；T為采樣周期。采用下述遞推關(guān)系式推算插秧機(jī)的航位xk+1＝xk+Tvcosθkyk+1＝y(tǒng)k+Tvsinθk根據(jù)卡爾曼濾波器的基本狀態(tài)轉(zhuǎn)移方程X(k+1)＝φ(k)X(k)+W(k)，定義狀態(tài)空間向量X(k)＝[x(k) y(k) v(k)]，其中x(k)，y(k)為WGS-84坐標(biāo)系下高斯投影平面坐標(biāo)的東向和北向坐標(biāo)，v(k)為插秧機(jī)縱向方向上的行進(jìn)速度，θ(k)為插秧機(jī)縱向方向在高斯投影平面坐標(biāo)下的角度分量，橫軸正向?yàn)?，逆時(shí)針為正。
狀態(tài)轉(zhuǎn)移矩陣定義為Φ(k)=10Tcos(θ(k-1))01Tsin(θ(k-1))001]]>采用上一時(shí)刻航向角度和轉(zhuǎn)向角度的測(cè)量值的一級(jí)融合結(jié)果，利用經(jīng)坐標(biāo)轉(zhuǎn)換后的航向角度估計(jì)值對(duì)Φ(k)矩陣進(jìn)行實(shí)時(shí)更新。W(k)為離散時(shí)間白噪聲序列，系統(tǒng)過程的噪聲協(xié)方差矩陣為Q(k)。Q(k)設(shè)定為常數(shù)矩陣，在仿真和實(shí)驗(yàn)過程中整定矩陣參數(shù)。
通過偽距差分GPS傳感器測(cè)量得到插秧機(jī)的位置(x，y)以及前進(jìn)速度Vb。外部測(cè)量向量為Z(k)＝[x(k) y(k) v(k)]，測(cè)量的噪聲方差陣R為R(k)=r12r22r32]]>其中，r1、r2、r3分別表示GPS接收機(jī)x向、y向定位以及速度測(cè)量噪聲。然后采用線性離散Kalman濾波器的遞歸差分方程進(jìn)行預(yù)測(cè)和校正預(yù)測(cè)方程組為x^(k|k-1)=Φ(k-1)x^(k-1)]]>P(k|k-1)＝Φ(k-1)P(k-1)Φ(k-1)T+Q(k-1)校正方程組為Kk＝P(k|k-1)HT(HP(k|k-1)HT+R(k))-1x^(k)=x^(k|k-1)+Kk(Z(k)-Hx^(k|k-1))]]>P(k)＝(I-KkH)P(k|k-1)在本發(fā)明中，使用在線自適應(yīng)調(diào)節(jié)后的測(cè)量噪聲方差陣R，計(jì)算校正方程組的增益矩陣。具體自適應(yīng)調(diào)節(jié)方法如下采用下述方程式來推算殘差或新息，即k時(shí)刻傳感器的實(shí)際觀測(cè)值與預(yù)測(cè)值之間的差值r(k)=z(k)-H(k)x^(k|k-1)]]>
上述殘差或新息的實(shí)際協(xié)方差公式為Pr(k)＝H(k)[Φ(k-1)P(k-1)ΦT(k-1)+Q(k-1)]HT(k)+R(k)上述殘差或新息的理論協(xié)方差公式為Cr(k)=1NΣi=i0kririT,i0=k-N+1]]>其中N為估計(jì)窗的大小。
如果上述推算出來的新息的實(shí)際協(xié)方差陣Pr(k)與理論協(xié)方差陣Cr(k)的計(jì)算結(jié)果近似相等，則說明當(dāng)前測(cè)量噪聲協(xié)方差矩陣R(k)的估計(jì)是準(zhǔn)確的。如果Pr(k)與Cr(k)的計(jì)算結(jié)果相差較大，則認(rèn)為當(dāng)前R(k)的估計(jì)結(jié)果需要根據(jù)其差別進(jìn)行修正，若Pr(k)偏大，表明當(dāng)前R(k)偏大，應(yīng)適當(dāng)減小，若Pr(k)偏小，表明當(dāng)前R(k)偏小，應(yīng)適當(dāng)增大。
因此，采用下述公式對(duì)R(k)進(jìn)行自適應(yīng)調(diào)整R(k)=R(k)+λR(k)-TrL≤DOM(k)≤TrLR(k)-TrL≤DOM(k)≤TrLR(k)-λR(k)DOM(k)>TrL]]>上式中，λ是測(cè)量噪聲協(xié)方差矩陣R(k)的調(diào)節(jié)系數(shù)，TrL是Pr(k)與Cr(k)差別程度的界定常數(shù)。定義DOM(k)＝Tr(Pr(k)-Cr(k))，通過兩個(gè)協(xié)方差陣的差值矩陣的跡來表示其差別程度。
Kalman濾波器工作的數(shù)據(jù)流向圖如圖2所示。將初始狀態(tài)估計(jì)X和初始濾波誤差方差陣P估計(jì)引入Kalman濾波器的預(yù)測(cè)方程組中，啟動(dòng)Kalman濾波。先由自適應(yīng)加權(quán)融合算法對(duì)航向角度信息進(jìn)行估計(jì)，并用估計(jì)結(jié)果更新預(yù)測(cè)方程組的矩陣，再由預(yù)測(cè)方程組對(duì)下一時(shí)刻狀態(tài)進(jìn)行估計(jì)。預(yù)測(cè)過程結(jié)束，啟動(dòng)Kalman濾波的校正過程，先對(duì)R矩陣進(jìn)行自適應(yīng)調(diào)節(jié)，然后引入校正方程組，解算增益矩陣，并根據(jù)當(dāng)前測(cè)量結(jié)果(針對(duì)預(yù)測(cè)方程組對(duì)下一時(shí)刻狀態(tài)的估計(jì)結(jié)果)，對(duì)預(yù)測(cè)方程組的狀態(tài)估計(jì)進(jìn)行更新，獲得當(dāng)前最優(yōu)估計(jì)，最后解算當(dāng)前最優(yōu)估計(jì)的誤差協(xié)方差矩陣，完成一次濾波過程。
測(cè)試?yán)龑?duì)本發(fā)明方位的定位效果進(jìn)行仿真測(cè)試采用人工駕駛插秧機(jī)以恒定的前進(jìn)速度在預(yù)定的各種路線上精確跟蹤行走。分別對(duì)其中的一條直線和一條圓曲線進(jìn)行了多次數(shù)據(jù)采集。
選擇4組數(shù)據(jù)進(jìn)行濾波器仿真。這4組數(shù)據(jù)中，兩組在曲線上，兩組在直線上。曲線上的兩組數(shù)據(jù)中，一組是完全DGPS定位的數(shù)據(jù)，一組是先有DGPS定位的數(shù)據(jù)，再有GPS定位的數(shù)據(jù)(DGPS差分信號(hào)中斷的情況下)。直線上的兩組數(shù)據(jù)亦是如此。
設(shè)濾波器的初始條件是P(0)=4.04.00.5]]>R(0)=0.80.80.01]]>Q(0)=0.030.030.002]]>N＝15，TrL＝1.1，λ＝0.1，RESIDUAL＝0.8，利用Kalman濾波器仿真程序讀取的第一行文本文件數(shù)據(jù)對(duì)狀態(tài)空間向量進(jìn)行初始化。
對(duì)于直線跟蹤，得到如圖3，4所示的仿真結(jié)果。對(duì)于曲線跟蹤，得到如圖5，6所示的仿真結(jié)果。在完全DGPS條件下，從圖3、4可看出，濾波器對(duì)AB處原始DGPS定位的跳變起到了較好的平滑作用，定位精度有所改善；在由DGPS轉(zhuǎn)變?yōu)镚PS定位的情況下，在圖5、6中的AB處，濾波器依靠航位推算定位，避免了DGPS的異常定位數(shù)據(jù)，但航位推算的累積誤差隨時(shí)間逐漸增大。這主要是由于GPS定位誤差比較大，濾波器的一致性判據(jù)始終不能滿足，長(zhǎng)時(shí)間處于航位推算模式造成的。
橫向跟蹤誤差(XTE)參數(shù)指標(biāo)是當(dāng)前DGPS測(cè)量定位點(diǎn)到預(yù)定義路線的距離。仿真實(shí)驗(yàn)以該參數(shù)指標(biāo)評(píng)價(jià)Kalman濾波效果。從Kalman濾波仿真曲線和濾波前后橫向跟蹤誤差的統(tǒng)計(jì)結(jié)果(表1)可以看出，Kalman濾波器對(duì)定位結(jié)果具有一定的平滑和穩(wěn)定作用，XTE誤差得到減小。無論直線跟蹤還是圓曲線跟蹤，在GPS差分信號(hào)丟失的情況下，可有效避免GPS動(dòng)態(tài)定位的異常結(jié)果，最大XTE誤差和平均XTE誤差大大減小，提高了DGPS定位的質(zhì)量。在完全DGPS定位情況下，該Kalman濾波器對(duì)DGPS定位精度可控制在亞米級(jí)范圍內(nèi)，最大XTE誤差和平均XTE誤差在一定程度上得到減小。
表1濾波前后橫向跟蹤誤差(XTE)統(tǒng)計(jì)結(jié)果。

權(quán)利要求
1.一種農(nóng)業(yè)機(jī)械導(dǎo)航的組合定位方法，其特征在于包括下述步驟(1)首先采用偽距差分GPS傳感器測(cè)量得到農(nóng)業(yè)機(jī)械的初步定位位置值；(2)然后采用電子羅盤和微機(jī)械陀螺傳感器對(duì)農(nóng)業(yè)機(jī)械的航向角度進(jìn)行測(cè)量，并通過自適應(yīng)加權(quán)融合，推算得到最優(yōu)的航向角度值；(3)再采用自適應(yīng)卡爾曼濾波器對(duì)包括初步定位位置值和航向角度值的多源信息進(jìn)行再次融合，獲得農(nóng)業(yè)機(jī)械定位和航向角度信息的精確估計(jì)。
2.根據(jù)權(quán)利要求1所述的農(nóng)業(yè)機(jī)械導(dǎo)航的組合定位方法，其特征在于所述推算得到最優(yōu)的航向角度值是首先電子羅盤傳感器對(duì)農(nóng)業(yè)機(jī)械的航向角度進(jìn)行測(cè)量，微機(jī)械陀螺傳感器對(duì)農(nóng)業(yè)機(jī)械的橫向角速率進(jìn)行測(cè)量，經(jīng)積分后轉(zhuǎn)換為航向角度測(cè)量值；然后采用基于協(xié)方差函數(shù)的加窗估計(jì)算法，在線估計(jì)電子羅盤和微機(jī)械陀螺的測(cè)量方差；再根據(jù)上述在線估計(jì)的測(cè)量方差，采用自適應(yīng)加權(quán)融合估計(jì)算法以自適應(yīng)的方式推算出兩個(gè)傳感器所對(duì)應(yīng)的最優(yōu)加權(quán)因子，最后推算得到最優(yōu)的航向角度值。
3.根據(jù)權(quán)利要求2所述的農(nóng)業(yè)機(jī)械導(dǎo)航的組合定位方法，其特征在于用最優(yōu)估計(jì)值周期性地更新微機(jī)械陀螺傳感器橫向角速率的積分初值。
4.根據(jù)權(quán)利要求1所述的農(nóng)業(yè)機(jī)械導(dǎo)航的組合定位方法，其特征在于所述對(duì)多源信息進(jìn)行再次融合的是首先利用航位推算的基本原理建立常速度條件下的卡爾曼濾波器的狀態(tài)方程和觀測(cè)方程；然后基于新息的理論協(xié)方差陣概念，設(shè)計(jì)一種測(cè)量噪聲協(xié)方差矩陣R(k)的自適應(yīng)調(diào)節(jié)方法；最后，在卡爾曼濾波器狀態(tài)方程和觀測(cè)方程的基礎(chǔ)上，結(jié)合測(cè)量噪聲協(xié)方差矩陣R(k)的在線調(diào)節(jié)方法，建立預(yù)測(cè)方程組和校正方程組，構(gòu)建卡爾曼濾波器，實(shí)現(xiàn)農(nóng)業(yè)機(jī)械定位和航向角度信息的精確估計(jì)。
5.根據(jù)權(quán)利要求1或4所述的農(nóng)業(yè)機(jī)械導(dǎo)航組合定位方法，其特征在于所述的常速度卡爾曼濾波器模型中，狀態(tài)空間和測(cè)量向量均為GPS接收機(jī)的位置數(shù)據(jù)(x，y)和速度數(shù)據(jù)v，航向角度估計(jì)數(shù)據(jù)對(duì)狀態(tài)轉(zhuǎn)移矩陣進(jìn)行實(shí)時(shí)更新。
6.根據(jù)權(quán)利要求1或4所述的農(nóng)業(yè)機(jī)械導(dǎo)航的組合定位方法，其特征在于所述R矩陣自適應(yīng)調(diào)節(jié)方法，是采用新息的實(shí)際協(xié)方差陣Pr(k)與理論協(xié)方差矩陣Cr(k)進(jìn)行對(duì)比的方法，在線決策R矩陣的調(diào)節(jié)量。
全文摘要
本發(fā)明公開了一種農(nóng)業(yè)機(jī)械導(dǎo)航的組合定位方法。該方法首先采用偽距差分GPS傳感器測(cè)量得到農(nóng)業(yè)機(jī)械的初步定位位置值；然后采用電子羅盤和微機(jī)械陀螺傳感器對(duì)農(nóng)業(yè)機(jī)械的航向角度進(jìn)行測(cè)量，并通過自適應(yīng)加權(quán)融合，推算得到最優(yōu)的航向角度值；再采用自適應(yīng)卡爾曼濾波器對(duì)包括初步定位位置值和航向角度值的多源信息進(jìn)行再次融合，獲得農(nóng)業(yè)機(jī)械定位和航向角度信息的精確估計(jì)。本發(fā)明可為導(dǎo)航控制提供更為精確和可靠的航向角度估計(jì)數(shù)據(jù)，定位精度可控制在亞米級(jí)范圍內(nèi)，而且可以形成連續(xù)、穩(wěn)定的農(nóng)業(yè)機(jī)械導(dǎo)航數(shù)據(jù)。
文檔編號(hào)G01S19/47GK101082493SQ20071002887
公開日2007年12月5日 申請(qǐng)日期2007年6月28日 優(yōu)先權(quán)日2007年6月28日
發(fā)明者羅錫文, 張智剛, 周志艷, 趙祚喜 申請(qǐng)人:華南農(nóng)業(yè)大學(xué)