專利名稱：一種農(nóng)業(yè)機械導航分級定位的方法和系統(tǒng)的制作方法
技術領域：
本發(fā)明涉及導航技術領域，特別是涉及一種農(nóng)業(yè)機械導航分級定位的方法和系統(tǒng)。

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


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

為t時刻農(nóng)業(yè)機械的運動狀態(tài)，Ф(t-1)為t-1時刻狀態(tài)轉移矩陣，

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

為t時刻農(nóng)業(yè)機械的運動狀態(tài)，Ф(t-1)為t-1時刻狀態(tài)轉移矩陣，

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

為t時刻農(nóng)業(yè)機械的運動狀態(tài)，

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

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



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

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

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

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

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

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