[發明專利]一種基于秩濾波的農業機器人組合導航信息融合方法

有效

• 申請號: 201611059641.X
• 申請日: 2016-11-25
• 公開（公告）號: CN106772524B
• 公開（公告）日: 2019-10-18
• 發明（設計）人: 王磊;李雙喜;張玉山;王岳
• 申請（專利權）人: 安徽科技學院
• 主分類號: G01S19/49
• 分類號: G01S19/49
• 代理公司: 南京蘇高專利商標事務所(普通合伙) 32204
• 代理人: 肖明芳

本發明公開了一種基于秩濾波的農業機器人組合導航信息融合方法，采用交互式多模型濾波為算法結構，通過上一個時刻各個模型濾波器的輸出結果計算當前時刻濾波的初值，將秩濾波器作為模型濾波步驟中的子濾波器進行并行濾波，利用模型濾波中得到的殘差、殘差協方差對模型概率進行更新，對各個秩濾波器估值進行加權融合從而得到交互輸出結果。由導航計算機根據組合導航系統的濾波模型及算法流程，完成組合導航的數據處理及解算工作。本發明能夠提高復雜環境下組合導航系統的濾波精度，使得農業機器人具有適應范圍廣和抗干擾能力強的優點。

技術領域

本發明屬于多傳感器信息融合技術領域，尤其涉及一種基于秩濾波的農業機器人組合導航信息融合方法。

背景技術

農業機器人工作在復雜的開放式非結構化農田環境中(障礙物、土地松軟程度不一高低不平、遮擋、光照變化等)，它需要通過自身攜帶的傳感器感知周圍的環境，對感知到的信息進行處理，獲取自身的位置和姿態信息，然后實時進行路徑規劃和導航控制，完成各項作業任務。因此，導航技術是實現農業機器人自動化作業的關鍵。Kalman濾波器一般適用于系統為線性、噪聲統計特性服從高斯分布并完全已知的情況；傳統的擴展卡爾曼濾波(extended Kalman filter，EKF)及其改進方法只能處理弱非線性系統及高斯噪聲條件下的狀態估計問題。為此，人們提出了一系列以貝葉斯濾波理論為框架、基于數值積分近似的非線性狀態估計方法，其中具有代表性的方法包括中心差分卡爾曼濾波、無跡卡爾曼濾波、求積分卡爾曼濾波、容積卡爾曼濾波等。它們的共同特點是均假設系統中噪聲服從高斯分布，即為高斯濾波器。秩濾波器(rank Kalman filter,RKF)具有與高斯濾波器相同的濾波結構，它通過秩統計量原理確定采樣點和權值，因此還適用于具有非高斯噪聲的系統。交互式多模型(Interacting Multiple Model,IMM)算法以廣義偽貝葉斯算法為基礎，利用Markov切換系數實現各個模型之間的交互，兼具計算復雜度低和估計精度高的特點。

發明內容

本發明提出了一種基于秩濾波的農業機器人組合導航信息融合方法，該方法的主要思路是在交互式多模型濾波框架下，采用秩濾波器替代傳統算法中的卡爾曼濾波器，突破傳統算法中系統噪聲必須為高斯分布的條件限制，增強各個子濾波器狀態估計的穩定性，提高復雜環境下組合導航系統的濾波精度，增強農業機器人自主導航定位的性能。

本發明采用交互式多模型濾波為算法結構，將秩濾波器作為模型濾波步驟中的子濾波器進行并行濾波，本發明具體包括如下步驟：

步驟1，根據農業機器人所處環境，將土地平整度和衛星信號強度分級，設定土地平整度因子和衛星信號強度因子ξ，1≤ξ≤ξ_max，ξ∈Z，Z為整數集，ξ_max分別表示最大的土地不平整級別和最大的衛星信號衰落級別；

步驟2，由慣性導航系統INS和全球衛星導航系統GNSS所測量的數據，測量各級別土地平整度和衛星信號衰落情形所對應的量測噪聲ν，將量測噪聲ν及其協方差矩陣R作為可變參數建立組合導航系統模型集合M，設模型個數為r，各模型之間跳變的Markov轉移概率為π_ji，下標i，j分別表示第i個模型和第j個模型；

步驟3，由k-1時刻的模型概率與先驗的Markov轉移概率π_ji進行交互，計算混合概率k≥1；

步驟4，對于第j個模型，j＝1,2,…，r，利用k-1時刻的狀態估計和協方差陣計算k時刻的狀態初值與協方差陣初值

步驟5，在組合導航系統獲得新的量測z^k之后，利用狀態初值與協方差陣初值采用秩濾波算法進行濾波，得到k時刻的狀態估計和協方差陣

步驟6，利用混合概率殘差估值和殘差協方差計算當前時刻的匹配模型對應的殘差估值殘差協方差似然函數和模型概率