專利名稱：一種農業機械導航的組合定位方法
技術領域：
本發明涉及導航定位技術，特別涉及一種農業機械導航的組合定位方法。
背景技術：
精細農業包括定位處方農作和農情信息自動采集，其中定位處方農作要求農業機械可以按照預先規劃好的路徑在田間行走，準確到達目的地并完成既定作業任務。精確導航是實現農業機械自主行走的關鍵技術之一，其定位精度直接影響農業機械進行路徑自動跟蹤的質量。因此，提高導航定位的精度，是改善農業機械路徑跟蹤質量的首要問題。
農業機械導航定位的主要信息包括位置信息和航向角度信息。在以GPS為主的導航定位技術方面，已有研究成果主要采用RTK-DGPS和FOG實現農業機械的精確定位，其成本較高。也有一些研究成果采用低價格低精度GPS融合高精度慣性傳感器，通過卡爾曼濾波方法實現農業機械的精確定位；目前這類研究還較少，而且現有的研究忽視卡爾曼濾波的自適應問題，難以避免濾波發散問題。
發明內容
本發明的目的在于克服現有技術中存在的缺點，提供一種高精度的農業機械導航的組合定位方法，將多個導航傳感器信息經過融合處理后，得到農業機械更為精確的當前位姿信息。
本發明一種農業機械導航的組合定位方法，通過下述步驟實現(1)首先采用偽距差分GPS傳感器測量得到農業機械的初步定位位置值；(2)然后采用電子羅盤和微機械陀螺傳感器對農業機械的航向角度進行測量，并通過自適應加權融合，推算得到最優的航向角度值；(3)再采用自適應卡爾曼濾波器對包括初步定位位置值和航向角度值的多源信息進行再次融合，獲得農業機械定位和航向角度信息的精確估計。
步驟2中，所述推算得到最優的航向角度值的具體步驟如下首先電子羅盤傳感器對農業機械的航向角度進行測量，微機械陀螺傳感器對農業機械的橫向角速率進行測量，經積分后轉換為航向角度測量值；然后采用基于協方差函數的加窗估計算法，在線估計電子羅盤和微機械陀螺的測量方差；再根據上述在線估計的測量方差，采用自適應加權融合估計算法以自適應的方式推算出兩個傳感器所對應的最優加權因子，最后推算得到最優的航向角度值。
步驟2中，采用最優估計值周期性地更新微機械陀螺傳感器橫向角速率的積分初值，以防止積分發散。
步驟3中，所述對多源信息進行再次融合的具體步驟如下首先利用航位推算的基本原理建立常速度條件下的卡爾曼濾波器的狀態方程和觀測方程；然后基于新息的理論協方差陣概念，設計一種測量噪聲協方差矩陣R(k)的自適應調節方法；最后，在卡爾曼濾波器狀態方程和觀測方程的基礎上，結合測量噪聲協方差矩陣R(k)的在線調節方法，建立預測方程組和校正方程組，構建卡爾曼濾波器，實現農業機械定位和航向角度信息的精確估計。
步驟3中，所述的常速度卡爾曼濾波器模型中，狀態空間和測量向量均為GPS接收機的位置數據(x，y)和速度數據v，航向角度估計數據對狀態轉移矩陣進行實時更新。
步驟3中，所述R矩陣的自適應調節中，主要采用新息的實際協方差陣Pr(k)與理論協方差矩陣Cr(k)進行對比的方法，在線決策R矩陣的調節量。
所述偽距差分GPS傳感器是指差分全球定位系統(DGPS)接收機。所述電子羅盤是指磁航向傳感器電子羅盤。所述微機械陀螺傳感器是指基于MEMS技術的低成本微機械陀螺。
本發明與現有技術相比具有如下優點和效果(1)本發明提出了農業機械航向角度的自適應加權融合估計算法，通過對電子羅盤和微機械陀螺信息的自適應加權融合處理，可為導航控制提供更為精確和可靠的航向角度估計數據。
(2)本發明采用自適應卡爾曼濾波器可抑制發散，平滑DGPS定位數據，有效避免DGPS動態定位的異常結果，定位精度可控制在亞米級范圍內。
(3)本發明的導航組合定位方式，將各個傳感器的測量數據進行多次融合，既平滑又可以濾除測試噪聲，形成了連續、穩定的農業機械導航數據。
圖1為農業機械導航的組合定位系統組成示意圖。
圖2為卡爾曼濾波器的數據流向圖。
圖3為完全DGPS條件下Kalman濾波器直線跟蹤仿真結果。
圖4為由DGPS到GPS條件下Kalman濾波器直線跟蹤仿真結果。
圖5為完全DGPS條件下Kalman濾波器曲線跟蹤仿真結果。
圖6為由DGPS到GPS條件下Kalman濾波器曲線跟蹤仿真結果。
具體實施例方式
下面結合實施例對本發明做進一步詳細的描述，但本發明的實施方式不限于此。
本實施例采用的農業機械為久保田插秧機，其導航定位系統主要包括導航傳感器組合、便攜計算機和USB-RS232數據傳輸線等。導航傳感器組合包括偽距差分GPS、電子羅盤和微機械陀螺。偽距差分GPS可以采集插秧機在WGS-84坐標系下的定位坐標，作為初步定位信息；電子羅盤測量其航向角度信息(以正北方向為0，逆時針方向為正)；微機械陀螺測量其橫向角速率(逆時針方向為正)。便攜計算機實現數據采集、數據預處理和信息融合定位軟件系統的運行。便攜計算機與導航傳感器組合通過多根USB-RS232數據傳輸線建立連接。將上述多個傳感器組合安裝在插秧機后端的臺架上，然后用數據采集軟件系統進行動態數據采集，采集頻率是1Hz。數據采集、數據預處理和信息融合定位軟件系統采用Visual C++編程工具在Windows XP操作系統下開發完成。導航定位系統結構框圖如圖1。
所述插秧機導航定位系統所獲得的導航數據以偽距差分GPS定位數據為主，電子羅盤和微機械陀螺的航向角度數據為輔。為了獲得插秧機準確的位置及航向估計，本發明通過兩級融合的方法，逐次對原始航向角度數據和GPS定位數據進行融合。
插秧機的具體組合定位方法(1)首先采用偽距差分GPS傳感器測量得到插秧機的初步定位位置值；(2)然后采用電子羅盤和微機械陀螺傳感器對插秧機的航向角度進行測量，并通過自適應加權融合，推算得到最優的航向角度值，實現一級融合；(3)再采用自適應卡爾曼濾波器對包括初步定位位置值和航向角度值的多源信息進行二級融合，獲得插秧機定位和航向角度信息的精確估計。
具體步驟包括1、采用偽距差分GPS傳感器測量得到插秧機的初步定位位置值；GPS接收機輸出的經緯度坐標是WGS-84地心大地坐標。為了使DGPS定位數據能用于導航控制系統，需要將WGS-84地心大地坐標轉換為對應于WGS-84橢球的高斯平面坐標，即需進行高斯投影變換。
本發明采用如下公式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為投影帶中央子午線經度。卯酉圈曲率半徑N=a1-e2sin2B,]]>橢球第一偏心率e＝2-2，輔助變量t＝tanB，輔助變量η＝e′cosB，橢球第二偏心率e′=a2/b2-1,]]>a、b分別為參考橢球的長、短半徑，扁率＝(a-b)/a，X為赤道至緯度為B的平行圈的子午線弧長，其計算公式為X=c∫0B(1+e′2cos2B-2/3)dB]]>c為極曲率半徑。
本發明所用投影坐標系的主要參數為1)投影方式Gauss-Kruger2)中央經線114.000000(3度帶)3)水平偏移量500km4)地理坐標系GCS_WGS_19845)大地參照系D_WGS_19846)參考橢球體WGS_19847)橢球長軸6378137.0000008)橢球扁率0.0033528107高斯投影變換后的WGS-84橢球的高斯平面坐標可作為插秧機的初步定位位置值。
2、采用電子羅盤和微機械陀螺傳感器對插秧機的航向角度進行測量，并通過自適應加權融合，推算得到最優的航向角度值，實現一級融合；
設定電子羅盤和微機械陀螺的航向角度測量值分別為Xp和Xq，對應的觀測誤差分別為Vp和Vq，Xp和Xq的自協方差函數分別為Rpp和Rqq，互協方差函數為Rpq，第k次采樣時Rpp的時間域估計值為Rpp(k)，Rqq的時間域估計值為Rqq(k)，Rpq的時間域估計值為Rpq(k)。可通過如下步驟，完成一級融合1)前N次采樣時的Rpp、Rqq和Rpq的時間域估計值可由下述遞推公式獲得當k＜N時，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-μ)]]>當k＞N時，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-μ)]]>式中μ為采樣數據的均值，在采用上述遞推算式時，為了實時獲取μ，將后續加權融合算法中的航向角度一步預測值作為測量數據的μ。N為移動窗口長度。
2)由下式可以獲得電子羅盤和微機械陀螺的測量方差δp2、δq2δp2＝Rpp-Rpqδq2＝Rqq-Rpq3)融合后的航向角度數據 可由下式獲得X^=Xpδp2(1δp2+1δq2)+Xqδq2(1δp2+1δq2)=δq2Xp+δp2Xqδp2+δq2]]>3、采用自適應卡爾曼濾波器對包括初步定位位置值和航向角度值的多源信息進行二級融合，獲得插秧機定位和航向角度信息的精確估計。
首先依據航位推算原理建立自適應卡爾曼濾波器狀態轉移方程和觀測方程。
θk為插秧機方位角，即插秧機縱向方向與x軸正向之間的夾角；v為插秧機在縱向方向上的前進速度；(xk，yk)為插秧機在k時刻的坐標，(xk+1，yk+1)為插秧機在k+1時刻的坐標；T為采樣周期。采用下述遞推關系式推算插秧機的航位xk+1＝xk+Tvcosθkyk+1＝yk+Tvsinθk根據卡爾曼濾波器的基本狀態轉移方程X(k+1)＝φ(k)X(k)+W(k)，定義狀態空間向量X(k)＝[x(k) y(k) v(k)]，其中x(k)，y(k)為WGS-84坐標系下高斯投影平面坐標的東向和北向坐標，v(k)為插秧機縱向方向上的行進速度，θ(k)為插秧機縱向方向在高斯投影平面坐標下的角度分量，橫軸正向為0，逆時針為正。
狀態轉移矩陣定義為Φ(k)=10Tcos(θ(k-1))01Tsin(θ(k-1))001]]>采用上一時刻航向角度和轉向角度的測量值的一級融合結果，利用經坐標轉換后的航向角度估計值對Φ(k)矩陣進行實時更新。W(k)為離散時間白噪聲序列，系統過程的噪聲協方差矩陣為Q(k)。Q(k)設定為常數矩陣，在仿真和實驗過程中整定矩陣參數。
通過偽距差分GPS傳感器測量得到插秧機的位置(x，y)以及前進速度Vb。外部測量向量為Z(k)＝[x(k) y(k) v(k)]，測量的噪聲方差陣R為R(k)=r12r22r32]]>其中，r1、r2、r3分別表示GPS接收機x向、y向定位以及速度測量噪聲。然后采用線性離散Kalman濾波器的遞歸差分方程進行預測和校正預測方程組為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)在本發明中，使用在線自適應調節后的測量噪聲方差陣R，計算校正方程組的增益矩陣。具體自適應調節方法如下采用下述方程式來推算殘差或新息，即k時刻傳感器的實際觀測值與預測值之間的差值r(k)=z(k)-H(k)x^(k|k-1)]]>
上述殘差或新息的實際協方差公式為Pr(k)＝H(k)[Φ(k-1)P(k-1)ΦT(k-1)+Q(k-1)]HT(k)+R(k)上述殘差或新息的理論協方差公式為Cr(k)=1NΣi=i0kririT,i0=k-N+1]]>其中N為估計窗的大小。
如果上述推算出來的新息的實際協方差陣Pr(k)與理論協方差陣Cr(k)的計算結果近似相等，則說明當前測量噪聲協方差矩陣R(k)的估計是準確的。如果Pr(k)與Cr(k)的計算結果相差較大，則認為當前R(k)的估計結果需要根據其差別進行修正，若Pr(k)偏大，表明當前R(k)偏大，應適當減小，若Pr(k)偏小，表明當前R(k)偏小，應適當增大。
因此，采用下述公式對R(k)進行自適應調整R(k)=R(k)+λR(k)-TrL≤DOM(k)≤TrLR(k)-TrL≤DOM(k)≤TrLR(k)-λR(k)DOM(k)>TrL]]>上式中，λ是測量噪聲協方差矩陣R(k)的調節系數，TrL是Pr(k)與Cr(k)差別程度的界定常數。定義DOM(k)＝Tr(Pr(k)-Cr(k))，通過兩個協方差陣的差值矩陣的跡來表示其差別程度。
Kalman濾波器工作的數據流向圖如圖2所示。將初始狀態估計X和初始濾波誤差方差陣P估計引入Kalman濾波器的預測方程組中，啟動Kalman濾波。先由自適應加權融合算法對航向角度信息進行估計，并用估計結果更新預測方程組的矩陣，再由預測方程組對下一時刻狀態進行估計。預測過程結束，啟動Kalman濾波的校正過程，先對R矩陣進行自適應調節，然后引入校正方程組，解算增益矩陣，并根據當前測量結果(針對預測方程組對下一時刻狀態的估計結果)，對預測方程組的狀態估計進行更新，獲得當前最優估計，最后解算當前最優估計的誤差協方差矩陣，完成一次濾波過程。
測試例對本發明方位的定位效果進行仿真測試采用人工駕駛插秧機以恒定的前進速度在預定的各種路線上精確跟蹤行走。分別對其中的一條直線和一條圓曲線進行了多次數據采集。
選擇4組數據進行濾波器仿真。這4組數據中，兩組在曲線上，兩組在直線上。曲線上的兩組數據中，一組是完全DGPS定位的數據，一組是先有DGPS定位的數據，再有GPS定位的數據(DGPS差分信號中斷的情況下)。直線上的兩組數據亦是如此。
設濾波器的初始條件是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濾波器仿真程序讀取的第一行文本文件數據對狀態空間向量進行初始化。
對于直線跟蹤，得到如圖3，4所示的仿真結果。對于曲線跟蹤，得到如圖5，6所示的仿真結果。在完全DGPS條件下，從圖3、4可看出，濾波器對AB處原始DGPS定位的跳變起到了較好的平滑作用，定位精度有所改善；在由DGPS轉變為GPS定位的情況下，在圖5、6中的AB處，濾波器依靠航位推算定位，避免了DGPS的異常定位數據，但航位推算的累積誤差隨時間逐漸增大。這主要是由于GPS定位誤差比較大，濾波器的一致性判據始終不能滿足，長時間處于航位推算模式造成的。
橫向跟蹤誤差(XTE)參數指標是當前DGPS測量定位點到預定義路線的距離。仿真實驗以該參數指標評價Kalman濾波效果。從Kalman濾波仿真曲線和濾波前后橫向跟蹤誤差的統計結果(表1)可以看出，Kalman濾波器對定位結果具有一定的平滑和穩定作用，XTE誤差得到減小。無論直線跟蹤還是圓曲線跟蹤，在GPS差分信號丟失的情況下，可有效避免GPS動態定位的異常結果，最大XTE誤差和平均XTE誤差大大減小，提高了DGPS定位的質量。在完全DGPS定位情況下，該Kalman濾波器對DGPS定位精度可控制在亞米級范圍內，最大XTE誤差和平均XTE誤差在一定程度上得到減小。
表1濾波前后橫向跟蹤誤差(XTE)統計結果。
權利要求
1.一種農業機械導航的組合定位方法，其特征在于包括下述步驟(1)首先采用偽距差分GPS傳感器測量得到農業機械的初步定位位置值；(2)然后采用電子羅盤和微機械陀螺傳感器對農業機械的航向角度進行測量，并通過自適應加權融合，推算得到最優的航向角度值；(3)再采用自適應卡爾曼濾波器對包括初步定位位置值和航向角度值的多源信息進行再次融合，獲得農業機械定位和航向角度信息的精確估計。
2.根據權利要求1所述的農業機械導航的組合定位方法，其特征在于所述推算得到最優的航向角度值是首先電子羅盤傳感器對農業機械的航向角度進行測量，微機械陀螺傳感器對農業機械的橫向角速率進行測量，經積分后轉換為航向角度測量值；然后采用基于協方差函數的加窗估計算法，在線估計電子羅盤和微機械陀螺的測量方差；再根據上述在線估計的測量方差，采用自適應加權融合估計算法以自適應的方式推算出兩個傳感器所對應的最優加權因子，最后推算得到最優的航向角度值。
3.根據權利要求2所述的農業機械導航的組合定位方法，其特征在于用最優估計值周期性地更新微機械陀螺傳感器橫向角速率的積分初值。
4.根據權利要求1所述的農業機械導航的組合定位方法，其特征在于所述對多源信息進行再次融合的是首先利用航位推算的基本原理建立常速度條件下的卡爾曼濾波器的狀態方程和觀測方程；然后基于新息的理論協方差陣概念，設計一種測量噪聲協方差矩陣R(k)的自適應調節方法；最后，在卡爾曼濾波器狀態方程和觀測方程的基礎上，結合測量噪聲協方差矩陣R(k)的在線調節方法，建立預測方程組和校正方程組，構建卡爾曼濾波器，實現農業機械定位和航向角度信息的精確估計。
5.根據權利要求1或4所述的農業機械導航組合定位方法，其特征在于所述的常速度卡爾曼濾波器模型中，狀態空間和測量向量均為GPS接收機的位置數據(x，y)和速度數據v，航向角度估計數據對狀態轉移矩陣進行實時更新。
6.根據權利要求1或4所述的農業機械導航的組合定位方法，其特征在于所述R矩陣自適應調節方法，是采用新息的實際協方差陣Pr(k)與理論協方差矩陣Cr(k)進行對比的方法，在線決策R矩陣的調節量。
全文摘要
本發明公開了一種農業機械導航的組合定位方法。該方法首先采用偽距差分GPS傳感器測量得到農業機械的初步定位位置值；然后采用電子羅盤和微機械陀螺傳感器對農業機械的航向角度進行測量，并通過自適應加權融合，推算得到最優的航向角度值；再采用自適應卡爾曼濾波器對包括初步定位位置值和航向角度值的多源信息進行再次融合，獲得農業機械定位和航向角度信息的精確估計。本發明可為導航控制提供更為精確和可靠的航向角度估計數據，定位精度可控制在亞米級范圍內，而且可以形成連續、穩定的農業機械導航數據。
文檔編號G01S19/47GK101082493SQ20071002887
公開日2007年12月5日 申請日期2007年6月28日 優先權日2007年6月28日
發明者羅錫文, 張智剛, 周志艷, 趙祚喜 申請人:華南農業大學
專業數控銑床產品供應商
20年生產經驗，實力工廠，精工品質，質量有保障
