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

有效

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

1.一種基于秩濾波的農業機器人組合導航信息融合方法，其特征在于，包括如下步驟：

步驟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，利用混合概率殘差估值和殘差協方差計算當前時刻的匹配模型對應的殘差估值殘差協方差似然函數和模型概率

步驟7，利用各個模型濾波器k時刻的狀態估計協方差陣和模型概率計算導航參數的全局估計及其估計誤差的協方差陣P_k|k；

步驟8，返回步驟3，重復步驟3至步驟7，直至接收到停止導航控制指令；

步驟3中，通過如下公式計算混合概率

步驟4中，通過如下公式計算k時刻的狀態初值與協方差陣初值

步驟5包括如下步驟：

步驟5-1，利用k-1時刻的狀態估計一維標準分布概率p_β對應的分位點λ_pβ和第β層采樣點的修正系數r_β，通過秩采樣機制求取采樣點集

其中，n為狀態向量x的維數，ρ為采樣點層數，α，β分別表示第α個采樣點和第β層采樣點，l取值為[1,n]中的整數，表示矩陣平方根的第l列；

步驟5-2，將采樣點集進行非線性傳播，得到非線性變換后點集

其中，f(·)為狀態方程非線性函數，ω為過程噪聲向量；

步驟5-3，進一步預測k時刻的狀態估計和協方差陣

其中，為第α個采樣點對應的協方差修正系數，取值為1，Q為過程噪聲方差矩陣，τ為協方差權重系數，通過如下公式計算：

步驟5-4，通過秩采樣機制求取采樣點集

步驟5-5，將采樣點集引入觀測方程并進行非線性傳播，得到預測的量測點集

其中，h(·)為觀測方程非線性函數，ν^j為模型j對應的量測噪聲向量；

步驟5-6，通過預測的量測點集計算量測估計

步驟5-7，通過如下公式計算新息協方差矩陣和互協方差陣

其中R^j為模型j對應的量測噪聲方差矩陣；

步驟5-8，由新息協方差矩陣和互協方差陣計算增益

k時刻組合導航系統的量測值z_k到來之后，通過如下公式計算k時刻的狀態估計和協方差陣

步驟6中，通過如下方法計算當前時刻匹配模型對應的殘差估值殘差協方差似然函數和模型概率

其中E[·]表示數學期望；

步驟7中，通過如下公式計算導航參數的全局估計及其估計誤差的協方差陣P_k|k：

步驟5-1和步驟5-4中，采用中位秩計算第β層采樣點對應的一維標準分布概率p_β，計算表達式為p_β＝(β－0.3)/(2ρ+1.4)。