專利名稱：一種農業機械導航分級定位的方法和系統的制作方法
技術領域：
本發明涉及導航技術領域，特別是涉及一種農業機械導航分級定位的方法和系統。

背景技術：
導航定位是農業機械導航中的首要問題，導航定位的精度直接影響到農業機械跟蹤預定義路徑的質量。農業機械導航定位的主要信息包括位置信息和航向角度信息、定位方法可分為絕對定位方法和相對定位方法。DGPS(Differential Global Positioning System，差分全球定位系統)作為典型的絕對定位系統，能夠全天候為接收機提供絕對位置、速度和方向等信息；但該系統需要接收四顆以上衛星才能決定車輛的位置，當受到外界干擾時，定位誤差將大大增加。機器視覺屬于相對定位方法，具有探測范圍寬、信號豐富完整的特點，在提供導航信息的同時還可獲得田間作物分布信息，其適應能力強，比較靈活，不需預先設定導航路線，非常適合行間作業的農業機械導航；但當地物標志信息不完整、外界光照條件變化時，都可能導致機器視覺導航定位系統的功能失效，并且它只能提供微觀尺度的導航定位數據，不適合進行大田內宏觀的路徑規劃及行走方向的引導。電子羅盤、加速度計、陀螺儀和里程計等都是廣泛使用的傳感器，短時間內具有很高的可信度；但由于隨機誤差和系統誤差的存在，測量信號將產生飄移，一段時間后將嚴重影響導航性能。
可見，單一導航定位方法都存在自身難以克服的缺點，因此在國內外的研究中，多采用組合導航定位方法實現農業機械的高精度定位。目前，常見的組合導航定位方法主要有基于光纖陀螺儀或電子羅盤的DR(Dead Reckoning，航位推算)技術、基于GIS(Geographicinformation system，地理信息系統)的地圖匹配技術、采用RTK-DGPS(Real Time Kinematic DGPS，實時動態DGPS)和FOG(Fiber-OpticsGyroscopes，光纖陀螺)實現精確定位的技術以及采用低價格低精度GPS(Global Positioning System，全球定位系統)融合高精度慣性傳感器，通過卡爾曼濾波方法實現農業機械精確定位的技術等等。
在實現本發明過程中，發明人發現現有技術中至少存在如下問題采用現有的組合導航定位技術時未充分考慮卡爾曼濾波自適應、難以避免濾波發散，從而造成農業機械導航定位的準確性較低，而且成本較高。


發明內容
本發明實施例要解決的問題是提供一種農業機械導航分級定位的方法和系統，以克服現有技術中農業機械導航定位的準確性較低的缺陷。
為達到上述目的，本發明實施例的技術方案提供一種農業機械導航分級定位的方法，所述方法包括以下步驟獲取第一定位參數，所述第一定位參數包括通過差分全球定位系統DGPS接收機獲取的農業機械的第一位置值，通過電子羅盤獲取的所述農業機械的第一航向角度值，通過對從加速度計得到的加速度值進行積分而獲取的所述農業機械的當前行進速度值；利用卡爾曼濾波器對所述第一位置值、第一航向角度值和當前行進速度值進行融合，獲取第二定位參數，所述第二定位參數包括所述農業機械的第二位置值和第二航向角度值；獲取第三定位參數，所述第三定位參數包括通過攝像裝置獲取的所述農業機械的第三位置值和第三航向角度值；利用多傳感器自適應加權融合算法將所述第二定位參數與所述第三定位參數進行融合，獲取目標定位參數，所述目標定位參數包括所述農業機械的目標位置值和目標航向角度值；根據所述目標定位參數對所述農業機械進行定位。
其中，在所述利用卡爾曼濾波器對第一位置值、第一航向角度值和當前行進速度值進行融合，獲取第二定位參數之前，還包括構建卡爾曼濾波器，所述構建卡爾曼濾波器的步驟具體包括建立基于常速度的卡爾曼濾波器的狀態方程和觀測方程；根據所述狀態方程和觀測方程，建立預測方程組和校正方程組；根據所述預測方程組和校正方程組構建卡爾曼濾波器。
其中，所述狀態方程為 其中，

為t時刻農業機械的運動狀態，Ф(t-1)為t-1時刻狀態轉移矩陣，

為t-1時刻農業機械的運動狀態； 所述觀測方程為 Z(t)＝H(t)X(t)+W(t) 其中，Z(t)為外部觀測向量，H(t)為外部觀測向量和狀態向量之間的測量矩陣，X(t)為t時刻農業機械的狀態向量，W(t)為高斯白噪聲序列。
其中，所述預測方程組為 其中，

為t時刻農業機械的運動狀態，Ф(t-1)為t-1時刻狀態轉移矩陣，

為t-1時刻農業機械的運動狀態，P(t|t)為t時刻濾波誤差方差陣，φ(t|t)為t時刻狀態轉移矩陣，P(t-1|t-1)為t-1時刻濾波誤差方差陣，φT(t|t)為t時刻狀態轉移矩陣的轉置矩陣，G(t-1)為t-1時刻過程噪聲分布矩陣，Q(t-1)為t-1時刻正定協方差矩陣，GT(t-1)為t-1時刻過程噪聲分布矩陣的轉置矩陣； 所述校正方程組為 其中，

為t時刻農業機械的運動狀態，

為t-1時刻農業機械的運動狀態，P(t|t)為t時刻濾波誤差方差陣，H(t)為t時刻外部觀測向量和狀態向量之間的測量矩陣，HT(t)為t時刻外部觀測向量和狀態向量之間的測量矩陣的轉置矩陣，R-1(t)為t時刻正定協方差矩陣的逆矩陣，Z(t)為t時刻外部觀測向量，P-1(t|t)為t時刻濾波誤差方差陣的逆矩陣，P-1(t|t-1)為t-1時刻濾波誤差方差陣。
其中，在所述卡爾曼濾波器中，利用DGPS接收機得到的第一位置值對所述狀態轉移矩陣進行實時更新。
其中，所述利用卡爾曼濾波器對第一位置值、第一航向角度值和當前行進速度值進行融合，具體包括利用所述第一位置值、第一航向角度值和當前行進速度值更新所述預測方程組的矩陣；通過所述預測方程組獲取下一時刻的狀態估計；利用所述校正方程組對所述下一時刻的狀態估計進行校正，獲取當前最優估計；根據所述當前最優估計，獲取當前最優估計的誤差協方差矩陣。
其中，在所述利用多傳感器自適應加權融合算法將第二定位參數與第三定位參數進行融合，獲取目標定位參數之前，還包括以下步驟判斷所述DGPS接收機和攝像裝置是否均能正常工作，如果所述DGPS接收機和攝像裝置都不能正常工作，則停止導航；如果所述DGPS接收機和攝像裝置中只有一個能正常工作，則判斷能正常工作的裝置得到的定位參數是否在誤差范圍內，如果是則采用單一傳感器進行導航，否則停止導航；如果所述DGPS接收機和攝像裝置都能正常工作，則判斷能正常工作的裝置得到的定位參數是否在誤差范圍內，如果所述DGPS接收機和攝像裝置得到的定位參數都不在誤差范圍內，則停止導航；如果所述DGPS接收機和攝像裝置得到的定位參數中只有一個在誤差范圍內，則采用單一傳感器進行導航；如果所述DGPS接收機和攝像裝置得到的定位參數都在誤差范圍內，則利用多傳感器自適應加權融合算法將所述DGPS接收機得到的第二定位參數和攝像裝置得到的第三定位參數進行融合。
其中，所述利用多傳感器自適應加權融合算法將第二定位參數與第三定位參數進行融合，具體包括 根據各測量設備獲取的定位參數的方差，獲取各測量設備的最優加權因子，所述最優加權因子由公式 (p＝1，2，…n) 獲取；其中，Wp為最優加權因子，σ2為各測量設備獲取的定位參數的方差； 根據公式 獲取融合后的定位參數；其中，

為融合后的定位參數，δq2為攝像裝置獲取的定位參數的方差，Xp為DGPS接收機得到的定位參數值，δp2為DGPS接收機獲取的定位參數的方差，Xq為攝像裝置得到的定位參數值。
本發明實施例的技術方案還提供了一種農業機械導航分級定位的系統，所述系統包括DGPS接收機，用于獲取農業機械的第一位置值；電子羅盤，用于獲取所述農業機械的第一航向角度值；加速度計，用于獲取所述農業機械的加速度值；攝像裝置，用于獲取所述農業機械的第三位置值和第三航向角度值；目標定位參數獲取裝置，用于對所述第一位置值、第一航向角度值和當前行進速度值進行融合，獲取第二定位參數，并將所述第二定位參數與所述第三定位參數進行融合，獲取目標定位參數；定位裝置，用于根據所述目標定位參數對所述農業機械進行定位。
其中，所述目標定位參數獲取裝置包括卡爾曼濾波器，用于對所述第一位置值、第一航向角度值和當前行進速度值進行融合；多傳感器自適應加權融合單元，用于將所述第二定位參數與所述第三定位參數進行融合。
上述技術方案僅是本發明的一個優選技術方案，具有如下優點本發明實施例通過對各測量設備獲取的定位參數行多次融合，可平滑DGPS定位數據，有效避免DGPS動態定位的異常結果、有效濾除測試噪聲、減小系統誤差，能夠形成連續、穩定、相對精確的農業機械的位置和航向角度信息，提高了農業機械導航定位的準確性。



圖1是本發明實施例的一種農業機械導航分級定位的方法的流程圖； 圖2是本發明實施例的卡爾曼濾波器的工作原理圖； 圖3是本發明實施例的多傳感器自適應加權融合算法的原理示意圖； 圖4是本發明實施例的多傳感器自適應加權融合算法的流程圖； 圖5是本發明實施例的一種農業機械導航分級定位的系統的結構圖。

具體實施例方式 下面結合附圖和實施例，對本發明的具體實施方式
作進一步詳細描述。以下實施例用于說明本發明，但不用來限制本發明的范圍。
發明實施例的一種農業機械導航分級定位的方法的流程如圖1所示，所述方法包括以下步驟 步驟s101，獲取第一定位參數，所述第一定位參數包括通過差分全球定位系統DGPS接收機獲取的農業機械的第一位置值，通過電子羅盤獲取的所述農業機械的第一航向角度值，通過對從加速度計得到的加速度值進行積分而獲取的所述農業機械的當前行進速度值。
步驟s102，構建卡爾曼濾波器。首先建立基于常速度的卡爾曼濾波器的狀態方程和觀測方程，然后根據所述狀態方程和觀測方程，建立預測方程組和校正方程組，最后根據所述預測方程組和校正方程組構建卡爾曼濾波器。
步驟s103，利用卡爾曼濾波器對所述第一位置值、第一航向角度值和當前行進速度值進行融合，獲取第二定位參數，所述第二定位參數包括所述農業機械的第二位置值和第二航向角度值。其中，利用卡爾曼濾波器對第一位置值、第一航向角度值和當前行進速度值進行融合的步驟包括利用所述第一位置值、第一航向角度值和當前行進速度值更新所述預測方程組的矩陣；通過所述預測方程組獲取下一時刻的狀態估計；利用所述校正方程組對所述下一時刻的狀態估計進行校正，獲取當前最優估計；根據所述當前最優估計，獲取當前最優估計的誤差協方差矩陣。
步驟s104，獲取第三定位參數，所述第三定位參數包括通過攝像裝置獲取的所述農業機械的第三位置值和第三航向角度值。從每一時刻采集的圖像中提取導航線，得到導航線的特征參數，即導航線兩端點坐標值。導航線的特征參數建立在圖像坐標系下，通過坐標轉換關系，可以得到導航線特征參數在世界坐標系下的對應位置值。已知導航線方程的兩點坐標，根據公式θ＝atan[(x2-x1)/(y2-y1)]可求得導航線與Y軸夾角，即農機與導航線之間的航向角度值。
步驟s105，利用多傳感器自適應加權融合算法將所述第二定位參數與所述第三定位參數進行融合，獲取目標定位參數，所述目標定位參數包括所述農業機械的目標位置值和目標航向角度值。在獲取目標定位參數之前，還包括判斷所述DGPS接收機和攝像裝置是否均能正常工作，如果所述DGPS接收機和攝像裝置都不能正常工作，則停止導航；如果所述DGPS接收機和攝像裝置中只有一個能正常工作，則判斷能正常工作的裝置得到的定位參數是否在誤差范圍內，如果是則采用單一傳感器進行導航，否則停止導航；如果所述DGPS接收機和攝像裝置都能正常工作，則判斷能正常工作的裝置得到的定位參數是否在誤差范圍內，如果所述DGPS接收機和攝像裝置得到的定位參數都不在誤差范圍內，則停止導航；如果所述DGPS接收機和攝像裝置得到的定位參數中只有一個在誤差范圍內，則采用單一傳感器進行導航；如果所述DGPS接收機和攝像裝置得到的定位參數都在誤差范圍內，則利用多傳感器自適應加權融合算法將所述DGPS接收機得到的第二定位參數和攝像裝置得到的第三定位參數進行融合。利用多傳感器自適應加權融合算法將第二定位參數與第三定位參數進行融合的步驟包括根據各測量設備獲取的定位參數的方差，獲取各測量設備的最優加權因子，所述最優加權因子由公式 (p＝1，2，…n) 獲取；其中，Wp為最優加權因子，σ2為各測量設備獲取的定位參數的方差；根據公式 獲取融合后的定位參數；其中，

為融合后的定位參數，δq2為攝像裝置獲取的定位參數的方差，Xp為DGPS接收機得到的定位參數值，δp2為DGPS接收機獲取的定位參數的方差，Xq為攝像裝置得到的定位參數值。
步驟s106，根據所述目標定位參數對所述農業機械進行定位。
本實施例所采用的導航傳感器主要包括DGPS接收機、攝像裝置采用CCD(Charge Coupled Device，電荷耦合器件)攝像頭、電子羅盤和加速度計。利用DGPS接收機采集農業機械在WGS-84(WorldGeodetic System-84，1984年世界大地坐標系)下各位置點的坐標，作為初步定位信息；采用電子羅盤測量農業機械的航向角度信息；采用加速度計測其當前時刻的加速度，再通過積分得到當前時刻的速度值。將上述各傳感器組合安裝在農業機械上，利用數據采集軟件系統動態采集數據，設定采集頻率為1Hz。數據的采集、預處理和信息融合等均由軟件系統實現。
本發明的融合多傳感器信息的農業機械導航分級定位方法通過兩級融合，逐步對初步DGPS定位數據、航向角度數據、速度數據以及機器視覺獲得的定位數據進行融合，其具體實施步驟如下 1.采用DGPS接收機測量得到農業機械的初步定位信息 DGPS系統中，采用的是WGS-84坐標系，屬于地心坐標系，為使DGPS定位數據能用于導航控制系統，需要進行高斯投影變換。
地球橢球上任一點的位置，既可以表示為，又可以表示為。兩個坐標系之間的轉換公式為 式中，N為橢球的卯酉圈曲率半徑，a為地球橢球長半徑，b為短半徑，e為橢球的第一偏心率。
N＝a/W (2) W＝(1-e2sin2B)1/2(3) e2＝(a2-b2)/a2 (4) 2.采用卡爾曼濾波器對DGPS得到的農業機械的位置值和電子羅盤得到的航向角度值及通過對加速度計得到的初始值積分得到的當前速度值進行融合 (1)一般來說，農業機械的作業環境較為平整，工作時要求農業機械的行進速度較為緩慢并且恒定，除地頭轉彎不會有較大的轉向動作。因此，假設農業機械的行進速度恒定且轉向角加速度為0，則利用Kalman濾波器便可得到如下的狀態空間描述 X(t+1)＝Ф(t)X(t)+G(t)V(t)(5) 其中，X(t)為t時刻農業機械的狀態向量，且 X(t)＝(x(t)，y(t)，v(t))T (6) 其中，x(t)，y(t)為t時刻WGS-84坐標系下農業機械在高斯投影平面上的位置點坐標值，v(t)為t時刻農業機械的行進速度，Ф(t)為狀態轉移矩陣，G(t)為過程噪聲分布矩陣，V(t)為零均值高斯過程白噪聲向量，即 E[V(t)]＝0，E[V(u)VT(j)]＝Q(t)δkj (7) 設φ(t)為t時刻農業機械的航向角在高斯投影平面坐標下的角度分量，橫軸正向為0，逆時針為正，則狀態轉移矩陣定義為 可利用上一時刻農業機械的航向角度和前輪轉角的實際值對φ(t)矩陣進行實時更新。
(2)Kalman濾波器的基本觀測方程 Z(t)＝H(t)X(t)+W(t) (8) 利用基本觀測方程，將DGPS接收機得到的農業機械當前位置點坐標(x，y)，速度值v作為外部觀測量，則外部觀測向量Z(t)＝[x(t)y(t)v(t)]，外部觀測向量和狀態向量之間的測量矩陣為H(t)，是常數矩陣 W(t)是均值為0、方差為ri2的高斯白噪聲序列。測量的噪聲方差矩陣R(t)為 r1、r2、r3分別表示DGPS接收機、電子羅盤、加速度計的測量噪聲的標準差。
(3)Kalman濾波器的工作流程 在Kalman濾波器的狀態方程和觀測方程的基礎上，其遞歸差分方程組可用如下方程表示 P-1(t|t)＝P-1(t|t-1)+HT(t)R-1(t)H(t) (10) 式(9)，(10)為Kalman濾波器的校正方程組，由它們可以得到 為便于算法的實現，對式(11)，(12)做進一步推導，可以得到 其中， P(t|t)＝φ(t|t)P(t-1|t-1)φT(t|t)+G(t-1)Q(t-1)GT(t-1)(16) 式(15)、(16)為Kalman濾波器的預測方程組。
在上述各方程中，方程(9)為濾波方程，利用測量值Z(t)對預測的系統狀態估計進行更新。方程(10)為濾波誤差方差方程，利用計算出的Kalman濾波器增益對預測的濾波誤差方差陣進行更新。方程(15)為Kalman濾波器的狀態方程，代入t-1時刻的狀態預測值，可對t時刻農業機械的運動狀態進行預測。方程(16)為預報誤差方差方程，可以對濾波誤差方差陣進行預測。方程(14)說明農業機械的狀態估計信息融合等價于狀態估計與校正的融合結果之和。預測校正包括兩部分，其一是各個傳感器跟蹤與預測差值(跟蹤殘留誤差)的總和，每個差值由逆(預測)協方差加權；其二是整個系統預測與各個傳感器預測的差值(預測殘留誤差)的總和，由局部逆(預測)協方差加權。
通常Kalman濾波器的應用中只用一種殘留誤差——跟蹤殘留誤差來校正狀態估計的融合，但本發明采用兩種。跟蹤殘留誤差反映了整個系統中的不可預測信息所帶來的誤差，用來校正整個系統的狀態估計。但由于各傳感器的狀態估計與整個系統的狀態估計相互關聯，所以跟蹤殘留誤差不能完全反映不可預測信息所帶來的誤差。因而式(14)采用預測殘留誤差來校正，顯然比式(12)具有更明確的物理意義，且更容易實現。
(4)Kalman濾波器的工作原理 首先將初始狀態估計

和初始濾波誤差方差陣P(t-1)估計引入Kalman濾波器的預測方程組進行Kalman濾波。采用DGPS、電子羅盤及加速度計采集到的數據更新預測方程組的矩陣，再由預測方程組對下一時刻狀態進行估計。預測過程結束后再利用Kalman濾波器進行校正，將R矩陣引入校正方程組，根據當前測量結果(針對預測方程組對下一時刻狀態的估計結果)，對預測方程組的狀態估計進行更新，獲得當前最優估計，最后解算當前最優估計的誤差協方差矩陣，完成一次濾波。Kalman濾波器的工作原理圖如圖2所示。
3.采用多傳感器自適應加權融合算法將機器視覺提供的位置值和航向角度值與初次融合得到相對精確的農業機械的位置和航向角度信息進行再次融合 不同傳感器有各自不同的加權因子，多傳感器自適應加權融合算法的中心思想是在總均方誤差最小這一最優條件下，根據各個傳感器所得到的測量值以自適應的方式尋找各個傳感器所對應的最優加權因子，使融合后的值達到最優。
多傳感器自適應加權融合算法的原理示意圖如圖3所示，具體實現步驟如下 (1)最優加權因子的求取 設n個傳感器的方差分別為σ12，σ22，…，σn2，所需估計的真值為X，各傳感器的測量值分別為X1，X2，…，Xn，各測量值相互獨立，并且是X的無偏估計；各傳感器的加權因子分別為W1，W2，…，Wn，融合后的

和加權因子滿足 總均方誤差為 因為X1，X2，…Xn相互獨立，并且為X的無偏估計，所以
故σ2可以寫成 總均方誤差σ2是關于各加權因子的多元二次函數，因此，σ2必然存在最小值。該最小值的求取是加權因子滿足公式(18)約束條件的多元函數極值的求取。根據多元函數求極值理論，可求出總均方誤差最小時對應的加權因子為 此時對應的最小均方誤差為 (2)測量方差的加窗估計算法 設DGPS接收機和機器視覺傳感器的測量值分別為Xp和Xq，對應的觀測誤差分別為Vp和Vq，其中Vp和Vq互不相關，且均值為0。Xp和Xq的自協方差函數分別為Rpp和Rqq，互協方差函數為Rpq，第k次采樣時Rpp的時間域估計值為Rpp(k)，Rqq的時間域估計值為Rqq(k)，Rpq的時間域估計值為Rpq(k)，則由前N次采樣的Rpp，Rqq和Rpq的時間域估計值可以得到如下遞推公式 隨著系統長時間運行，系統參數會發生緩慢變化，噪聲特征也會發生某種程度的緩變，為了在保證估計精度的前提下及時跟蹤噪聲的變化提出加窗方差估計方法 當k＜N時， 當k≥N時， 式中μ為采樣數據的平均值，采用上述遞推算式時，為實時獲取μ，將得到的加權融合后的導航參數做為測量數據的μ。N為移動窗口長度。則DGPS接收機和機器視覺傳感器的方差δp2和δq2分別為 δp2＝Rpp-Rpq(33) δq2＝Rqq-Rpq(34) 由公式(17)和(22)可得到融合后的導航參數

為 此時對應的最小均方誤差為 (3)多傳感器自適應加權融合算法的實現 首先判斷DGPS接收機和機器視覺傳感器是否均能正常工作 1)如都不能正常工作則停止導航； 2)如只有一個正常工作，則判斷其得到的定位參數是否在誤差范圍內，如果是則選擇單一傳感器導航方法，如果否則停止導航； 3)如果二者都能正常工作，仍需判斷其定位數據是否在誤差范圍內如兩個定位數據均不在誤差范圍內則停止導航；如只有一個定位數據值在誤差范圍內則采用單一傳感器導航方式；如兩定位數據均在誤差范圍內，利用自適應加權融合估計的方法進行信息融合以提高定位的精度。
判斷機器視覺傳感器正常工作的標準是軟件能清楚地采集圖像，并對圖像進行處理；判斷DGPS接收機正常工作的標準是其能正常接收衛星信號，并保證能進行差分。利用設定閾值的方法判斷定位數據是否滿足誤差范圍，閾值的選擇和農業機械的前輪間距有關。本實施例中農業機械的前輪間距為0.78m，因此設定誤差范圍為[-0.5m，0.5m]。
多傳感器信息融合自適應加權融合算法不要求知道傳感器測量數據的任何先驗知識，只需傳感器所提供的測量數據即可以自適應的方式，尋找各自所對應的最優加權因子，計算出融合后的數據，并可保證融合后總均方誤差最小，使融合后的估計值達到最優，其算法流程圖如圖4所示。
4.三種定位方法的定位效果分析 為驗證本發明的融合多傳感器信息的農業機械導航分級定位方法的穩定性，通過實驗對定位效果進行測試，實驗過程如下 (1)選取一條東西方向的直線作為預定義路徑，首先采用GPS4700接收機在預定義直線軌跡的兩端點A，B處各進行30分鐘靜態定位，并對測量數據進行平均，取得兩端點的精確定位結果，A點為(444729.280301，4429977.863371)，B點為(444759.627519，4429977.651654)，再利用解析幾何方法獲得該預定義路徑在高斯投影平面坐標系中的幾何描述； (2)由于GPS4700的定位頻率為1Hz，機器視覺傳感器的定位頻率約為10Hz，因此設定兩傳感器信息的融合頻率為1Hz。實時紀錄融合后的定位信息，將三種定位方法得到的定位信息進行比較，并通過統計學方法對數據進行分析； (3)農業機械和預定義路徑之間的位置偏差XTE是定位和導航精度的主要評價參數，獲取準確的XTE數據是評價系統精度的基礎。在獲取XTE參數的基礎上，對XTE進行統計分析。選取XTE的平均值|XXTE|，最大值|XXTE|max和標準差σXTE三項數據作為定位精度的評價指標。
實驗結果分析 首先對實驗數據進行定性分析，然后通過統計學方法對實驗數據進行定量分析。分析結果如表1所示。
表1 從表1可看出融合后的統計量平均值，最大值和標準差均為最小。而且，融合算法去除了機器視覺定位中的跳變點，消除了可能出現誤差的數據，提高了導航定位的穩定性和精度。同時，融合后的定位數據的波動最小，穩定性最好。
本發明實施例的一種農業機械導航分級定位的系統如圖5所示，該系統包括DGPS接收機51、電子羅盤52、加速度計53、攝像裝置54、目標定位參數獲取裝置55和定位裝置56，其中目標定位參數獲取裝置55分別與DGPS接收機51、電子羅盤52、加速度計53、攝像裝置54、定位裝置56連接。
DGPS接收機51用于獲取農業機械的第一位置值；電子羅盤52用于獲取所述農業機械的第一航向角度值；加速度計53用于獲取所述農業機械的加速度值；攝像裝置54用于獲取所述農業機械的第三位置值和第三航向角度值；目標定位參數獲取裝置55用于對所述第一位置值、第一航向角度值和當前行進速度值進行融合，獲取第二定位參數，并將所述第二定位參數與所述第三定位參數進行融合，獲取目標定位參數；定位裝置56用于根據所述目標定位參數對所述農業機械進行定位。
目標定位參數獲取裝置55包括卡爾曼濾波器551和多傳感器自適應加權融合單元552，其中卡爾曼濾波器551分別與DGPS接收機51、電子羅盤52、加速度計53連接，多傳感器自適應加權融合單元552分別與攝像裝置54和卡爾曼濾波器551連接。
卡爾曼濾波器551用于對所述第一位置值、第一航向角度值和當前行進速度值進行融合；多傳感器自適應加權融合單元552用于將所述第二定位參數與所述第三定位參數進行融合。
本發明采用卡爾曼濾波器融合DGPS得到的農業機械的當前位置值和電子羅盤得到的航向角度值及通過對加速度計得到的初始值積分得到的速度值，可平滑DGPS定位數據，有效避免DGPS動態定位的異常結果。另外，本發明提出采用多傳感器自適應加權融合算法將機器視覺提供的位置值和航向角度值與初次融合得到相對精確的農業機械的位置和航向角度信息進行再次融合，獲得最優的農業機械位置值和航向角度值。進一步，本發明的農業機械導航分級定位方法，將各個傳感器的測量數據進行多次融合，可以有效濾除測試噪聲、減小系統誤差，能夠形成連續、穩定、相對精確的農業機械的位置和航向角度信息 以上所述僅是本發明的優選實施方式，應當指出，對于本技術領域的普通技術人員來說，在不脫離本發明技術原理的前提下，還可以做出若干改進和潤飾，這些改進和潤飾也應視為本發明的保護范圍。
權利要求
1、 一種農業機械導航分級定位的方法，其特征在于，所述方法包括以下步驟
獲取第一定位參數，所述第一定位參數包括通過差分全球定位系統DGPS接收機獲取的農業機械的第一位置值，通過電子羅盤獲取的所述農業機械的第一航向角度值，通過對從加速度計得到的加速度值進行積分而獲取的所述農業機械的當前行進速度值；
利用卡爾曼濾波器對所述第一位置值、第一航向角度值和當前行進速度值進行融合，獲取第二定位參數，所述第二定位參數包括所述農業機械的第二位置值和第二航向角度值；
獲取第三定位參數，所述第三定位參數包括通過攝像裝置獲取的所述農業機械的第三位置值和第三航向角度值；
利用多傳感器自適應加權融合算法將所述第二定位參數與所述第三定位參數進行融合，獲取目標定位參數，所述目標定位參數包括所述農業機械的目標位置值和目標航向角度值；
根據所述目標定位參數對所述農業機械進行定位。
2、 如權利要求1所述農業機械導航分級定位的方法，其特征在于，在所述利用卡爾曼濾波器對第一位置值、第一航向角度值和當前行進速度值進行融合，獲取第二定位參數之前，還包括構建卡爾曼濾波器，所述構建卡爾曼濾波器的步驟具體包括
建立基于常速度的卡爾曼濾波器的狀態方程和觀測方程；
根據所述狀態方程和觀測方程，建立預測方程組和校正方程組；
根據所述預測方程組和校正方程組構建卡爾曼濾波器。
3、 如權利要求2所述農業機械導航分級定位的方法，其特征在于，所述狀態方程為
其中，
為t時刻農業機械的運動狀態，Φ(t-1)為t-1時刻狀態轉移矩陣，
為t-1時刻農業機械的運動狀態；
所述觀測方程為
Z(t)＝H(t)X(t)+W(t)
其中，Z(t)為外部觀測向量，H(t)為外部觀測向量和狀態向量之間的測量矩陣，X(t)為t時刻農業機械的狀態向量，W(t)為高斯白噪聲序列。
4、 如權利要求2所述農業機械導航分級定位的方法，其特征在于，所述預測方程組為
其中，
為t時刻農業機械的運動狀態，Φ(t-1)為t-1時刻狀態轉移矩陣，
為t-1時刻農業機械的運動狀態，P(t|t)為t時刻濾波誤差方差陣，φ(t|t)為t時刻狀態轉移矩陣，P(t-1|t-1)為t-1時刻濾波誤差方差陣，φT(t|t)為t時刻狀態轉移矩陣的轉置矩陣，G(t-1)為t-1時刻過程噪聲分布矩陣，Q(t-1)為t-1時刻正定協方差矩陣，GT(t-1)為t-1時刻過程噪聲分布矩陣的轉置矩陣；
所述校正方程組為
其中，
為t時刻農業機械的運動狀態，
為t-1時刻農業機械的運動狀態，P(t|t)為t時刻濾波誤差方差陣，H(t)為t時刻外部觀測向量和狀態向量之間的測量矩陣，HT(t)為t時刻外部觀測向量和狀態向量之間的測量矩陣的轉置矩陣，R-1(t)為t時刻正定協方差矩陣的逆矩陣，Z(t)為t時刻外部觀測向量，P-1(t|t)為t時刻濾波誤差方差陣的逆矩陣，P-1(t|t-1)為t-1時刻濾波誤差方差陣。
5、 如權利要求2至4任一項所述農業機械導航分級定位的方法，其特征在于，在所述卡爾曼濾波器中，利用DGPS接收機得到的第一位置值對所述狀態轉移矩陣進行實時更新。
6、 如權利要求2至4任一項所述農業機械導航分級定位的方法，其特征在于，所述利用卡爾曼濾波器對第一位置值、第一航向角度值和當前行進速度值進行融合，具體包括
利用所述第一位置值、第一航向角度值和當前行進速度值更新所述預測方程組的矩陣；
通過所述預測方程組獲取下一時刻的狀態估計；
利用所述校正方程組對所述下一時刻的狀態估計進行校正，獲取當前最優估計；
根據所述當前最優估計，獲取當前最優估計的誤差協方差矩陣。
7、 如權利要求1所述農業機械導航分級定位的方法，其特征在于，在所述利用多傳感器自適應加權融合算法將第二定位參數與第三定位參數進行融合，獲取目標定位參數之前，還包括以下步驟
判斷所述DGPS接收機和攝像裝置是否均能正常工作，
如果所述DGPS接收機和攝像裝置都不能正常工作，則停止導航；
如果所述DGPS接收機和攝像裝置中只有一個能正常工作，則判斷能正常工作的裝置得到的定位參數是否在誤差范圍內，如果是則采用單一傳感器進行導航，否則停止導航；
如果所述DGPS接收機和攝像裝置都能正常工作，則判斷能正常工作的裝置得到的定位參數是否在誤差范圍內，
如果所述DGPS接收機和攝像裝置得到的定位參數都不在誤差范圍內，則停止導航；
如果所述DGPS接收機和攝像裝置得到的定位參數中只有一個在誤差范圍內，則采用單一傳感器進行導航；
如果所述DGPS接收機和攝像裝置得到的定位參數都在誤差范圍內，則利用多傳感器自適應加權融合算法將所述DGPS接收機得到的第二定位參數和攝像裝置得到的第三定位參數進行融合。
8、 如權利要求1所述消息的讀寫方法，其特征在于，所述利用多傳感器自適應加權融合算法將第二定位參數與第三定位參數進行融合，具體包括
根據各測量設備獲取的定位參數的方差，獲取各測量設備的最優加權因子，所述最優加權因子由公式
(p＝1，2，…n)
獲取；其中，Wp為最優加權因子，σ2為各測量設備獲取的定位參數的方差；
根據公式
獲取融合后的定位參數；其中，
為融合后的定位參數，δq2為攝像裝置獲取的定位參數的方差，Xp為DGPS接收機得到的定位參數值，δp2為DGPS接收機獲取的定位參數的方差，Xq為攝像裝置得到的定位參數值。
9、 一種農業機械導航分級定位的系統，其特征在于，所述系統包括
DGPS接收機，用于獲取農業機械的第一位置值；
電子羅盤，用于獲取所述農業機械的第一航向角度值；
加速度計，用于獲取所述農業機械的加速度值；
攝像裝置，用于獲取所述農業機械的第三位置值和第三航向角度值；
目標定位參數獲取裝置，用于對所述第一位置值、第一航向角度值和當前行進速度值進行融合，獲取第二定位參數，并將所述第二定位參數與所述第三定位參數進行融合，獲取目標定位參數；
定位裝置，用于根據所述目標定位參數對所述農業機械進行定位。
10、 如權利要求9所述農業機械導航分級定位的系統，其特征在于，所述目標定位參數獲取裝置包括
卡爾曼濾波器，用于對所述第一位置值、第一航向角度值和當前行進速度值進行融合；
多傳感器自適應加權融合單元，用于將所述第二定位參數與所述第三定位參數進行融合。
全文摘要
本發明公開了一種農業機械導航分級定位的方法，包括獲取第一定位參數；利用卡爾曼濾波器對第一定位參數進行融合，獲取第二定位參數；獲取第三定位參數；利用多傳感器自適應加權融合算法將第二定位參數與第三定位參數進行融合，獲取目標定位參數；根據目標定位參數對農業機械進行定位。本發明還公開了一種農業機械導航分級定位的系統。本發明通過對各測量設備獲取的定位參數行多次融合，可平滑DGPS定位數據，有效避免DGPS動態定位的異常結果、有效濾除測試噪聲、減小系統誤差，能夠形成連續、穩定、精確的農業機械的位置和航向角度信息，提高了農業機械導航定位的準確性。
文檔編號G01S5/14GK101285686SQ20081011359
公開日2008年10月15日 申請日期2008年5月29日 優先權日2008年5月29日
發明者剛 劉, 孟祥健, 萬曉君 申請人:中國農業大學