本發(fā)明屬于多傳感器信息融合技術(shù)領(lǐng)域，尤其涉及一種基于秩濾波的農(nóng)業(yè)機(jī)器人組合導(dǎo)航信息融合方法。

背景技術(shù)：

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

技術(shù)實(shí)現(xiàn)要素：

本發(fā)明提出了一種基于秩濾波的農(nóng)業(yè)機(jī)器人組合導(dǎo)航信息融合方法，該方法的主要思路是在交互式多模型濾波框架下，采用秩濾波器替代傳統(tǒng)算法中的卡爾曼濾波器，突破傳統(tǒng)算法中系統(tǒng)噪聲必須為高斯分布的條件限制，增強(qiáng)各個(gè)子濾波器狀態(tài)估計(jì)的穩(wěn)定性，提高復(fù)雜環(huán)境下組合導(dǎo)航系統(tǒng)的濾波精度，增強(qiáng)農(nóng)業(yè)機(jī)器人自主導(dǎo)航定位的性能。

本發(fā)明采用交互式多模型濾波為算法結(jié)構(gòu)，將秩濾波器作為模型濾波步驟中的子濾波器進(jìn)行并行濾波，本發(fā)明具體包括如下步驟：

步驟1，根據(jù)農(nóng)業(yè)機(jī)器人所處環(huán)境，將土地平整度和衛(wèi)星信號(hào)強(qiáng)度分級(jí)，設(shè)定土地平整度因子和衛(wèi)星信號(hào)強(qiáng)度因子ξ，1≤ξ≤ξ_max，ξ∈Z，Z為整數(shù)集，ξ_max分別表示最大的土地不平整級(jí)別和最大的衛(wèi)星信號(hào)衰落級(jí)別；

步驟2，由慣性導(dǎo)航系統(tǒng)INS和全球衛(wèi)星導(dǎo)航系統(tǒng)GNSS所測(cè)量的數(shù)據(jù)，測(cè)量各級(jí)別土地平整度和衛(wèi)星信號(hào)衰落情形所對(duì)應(yīng)的量測(cè)噪聲ν，將量測(cè)噪聲ν及其協(xié)方差矩陣R作為可變參數(shù)建立組合導(dǎo)航系統(tǒng)模型集合M，設(shè)模型個(gè)數(shù)為r，各模型之間跳變的Markov轉(zhuǎn)移概率為π_ji，下標(biāo)i，j分別表示第i個(gè)模型和第j個(gè)模型；

步驟3，由k-1時(shí)刻的模型概率與先驗(yàn)的Markov轉(zhuǎn)移概率π_ji進(jìn)行交互，計(jì)算混合概率k≥1；

步驟4，對(duì)于第j個(gè)模型，j＝1,2,…，r，利用k-1時(shí)刻的狀態(tài)估計(jì)和協(xié)方差陣計(jì)算k時(shí)刻的狀態(tài)初值與協(xié)方差陣初值

步驟5，在組合導(dǎo)航系統(tǒng)獲得新的量測(cè)z^k之后，利用狀態(tài)初值與協(xié)方差陣初值采用秩濾波算法進(jìn)行濾波，得到k時(shí)刻的狀態(tài)估計(jì)和協(xié)方差陣

步驟6，利用混合概率殘差估值和殘差協(xié)方差計(jì)算當(dāng)前時(shí)刻的匹配模型對(duì)應(yīng)的殘差估值殘差協(xié)方差似然函數(shù)和模型概率

步驟7，利用各個(gè)模型濾波器k時(shí)刻的狀態(tài)估計(jì)協(xié)方差陣和模型概率計(jì)算導(dǎo)航參數(shù)的全局估計(jì)及估計(jì)誤差的協(xié)方差陣P_k|k；

步驟8，返回步驟3，重復(fù)步驟3至步驟7，直至接收到停止導(dǎo)航控制指令；

本發(fā)明步驟3中，通過如下公式計(jì)算混合概率

本發(fā)明步驟4中，通過如下公式計(jì)算k時(shí)刻的狀態(tài)初值與協(xié)方差陣初值

本發(fā)明步驟5包括如下步驟：

步驟5-1，利用k-1時(shí)刻的狀態(tài)估計(jì)一維標(biāo)準(zhǔn)分布概率p_β對(duì)應(yīng)的分位點(diǎn)和第β層采樣點(diǎn)的修正系數(shù)r_β，通過秩采樣機(jī)制求取采樣點(diǎn)集

其中，n為狀態(tài)向量x的維數(shù)，ρ為采樣點(diǎn)層數(shù)，α，β分別表示第α個(gè)采樣點(diǎn)和第β層采樣點(diǎn)，l取值為[1,n]中的整數(shù)，表示矩陣平方根的第l列；

步驟5-2，將采樣點(diǎn)集進(jìn)行非線性傳播，得到非線性變換后點(diǎn)集

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

步驟5-3，進(jìn)一步預(yù)測(cè)k時(shí)刻的狀態(tài)估計(jì)和協(xié)方差陣

其中，為第α個(gè)采樣點(diǎn)對(duì)應(yīng)的協(xié)方差修正系數(shù)，取值為1，Q為過程噪聲方差矩陣，τ為協(xié)方差權(quán)重系數(shù)，通過如下公式計(jì)算：

步驟5-4，通過秩采樣機(jī)制求取采樣點(diǎn)集

步驟5-5，將采樣點(diǎn)集引入觀測(cè)方程并進(jìn)行非線性傳播，得到預(yù)測(cè)的量測(cè)點(diǎn)集

其中，h(·)為觀測(cè)方程非線性函數(shù)，ν^j為模型j對(duì)應(yīng)的量測(cè)噪聲向量；

步驟5-6，通過預(yù)測(cè)的量測(cè)點(diǎn)集計(jì)算量測(cè)估計(jì)

步驟5-7，通過如下公式計(jì)算新息協(xié)方差矩陣和互協(xié)方差陣

其中R^j為模型j對(duì)應(yīng)的量測(cè)噪聲方差矩陣；

步驟5-8，由新息協(xié)方差矩陣和互協(xié)方差陣計(jì)算增益

k時(shí)刻組合導(dǎo)航系統(tǒng)的量測(cè)值z(mì)_k到來之后，通過如下公式計(jì)算k時(shí)刻的狀態(tài)估計(jì)和協(xié)方差陣

本發(fā)明步驟6中，通過如下方法計(jì)算當(dāng)前時(shí)刻匹配模型對(duì)應(yīng)的殘差估值殘差協(xié)方差似然函數(shù)和模型概率

其中E[·]表示數(shù)學(xué)期望。

本發(fā)明步驟7中，通過如下公式計(jì)算導(dǎo)航參數(shù)的全局估計(jì)及其估計(jì)誤差的協(xié)方差陣P_k|k：

本發(fā)明步驟5-1和步驟5-4中，采用中位秩計(jì)算第β層采樣點(diǎn)對(duì)應(yīng)的一維標(biāo)準(zhǔn)分布概率p_β，計(jì)算表達(dá)式為p_β＝(β－0.3)/(2ρ+1.4)。

本發(fā)明針對(duì)現(xiàn)有技術(shù)的不足，將秩濾波器與交互式多模型算法相結(jié)合，解決非線性、非高斯系統(tǒng)中的狀態(tài)估計(jì)問題，提高了農(nóng)業(yè)機(jī)器人組合導(dǎo)航算法估計(jì)性能。本發(fā)明內(nèi)容同樣適用于慣性組合導(dǎo)航、目標(biāo)跟蹤與識(shí)別、圖像處理等其他多傳感器信息融合及多源數(shù)據(jù)處理應(yīng)用領(lǐng)域。

有益效果：本發(fā)明相對(duì)于現(xiàn)有技術(shù)的優(yōu)點(diǎn)為：

(1)本發(fā)明采用交互式多模型信息融合算法結(jié)構(gòu)，通過設(shè)計(jì)多個(gè)模型來逼近系統(tǒng)復(fù)雜的時(shí)變或非線性過程，能夠使在建模條件下分析得到的系統(tǒng)性能保持或接近最優(yōu)，相比于廣義偽貝葉斯算法，兼具了計(jì)算復(fù)雜度低和估計(jì)精度高的優(yōu)點(diǎn)。

(2)本發(fā)明將秩濾波器引入到交互式多模型信息融合算法中，秩濾波器通過秩統(tǒng)計(jì)量相關(guān)原理確定采樣點(diǎn)和權(quán)值，因此，它不受高斯分布條件限制，對(duì)于未知噪聲干擾具有更好的適應(yīng)性，從而使組合導(dǎo)航系統(tǒng)具有更好的穩(wěn)定性和抗干擾能力。

附圖說明

下面結(jié)合附圖和具體實(shí)施方式對(duì)本發(fā)明做更進(jìn)一步的具體說明，本發(fā)明的上述或其他方面的優(yōu)點(diǎn)將會(huì)變得更加清楚。

圖1基于秩濾波的交互式多模型算法結(jié)構(gòu)圖；

圖2農(nóng)業(yè)機(jī)器人仿真運(yùn)行軌跡；

圖3俯仰角估計(jì)誤差曲線；

圖4橫搖角估計(jì)誤差曲線；

圖5航向角估計(jì)誤差曲線；

圖6東向速度估計(jì)誤差曲線；

圖7北向速度估計(jì)誤差曲線；

圖8緯度估計(jì)誤差曲線；

圖9經(jīng)度估計(jì)誤差曲線。

具體實(shí)施方式

下面結(jié)合附圖對(duì)發(fā)明的技術(shù)內(nèi)容進(jìn)行詳細(xì)說明：

本發(fā)明的基于秩濾波的農(nóng)業(yè)機(jī)器人組合導(dǎo)航信息融合方法，以交互式多模型算法為濾波框架，采用秩濾波器替代傳統(tǒng)算法中的卡爾曼濾波器，如圖1所示為基于秩濾波的交互式多模型算法結(jié)構(gòu)圖?，F(xiàn)以微電子機(jī)械(Micro-Electro-Mechanical System,MEMS)捷聯(lián)慣性導(dǎo)航系統(tǒng)和實(shí)時(shí)動(dòng)態(tài)(Real-time kinematic,RTK)全球定位系統(tǒng)RTK GPS構(gòu)成的農(nóng)業(yè)機(jī)器人組合導(dǎo)航系統(tǒng)為例來說明所提供的信息融合算法，具體步驟如下：

步驟1，根據(jù)農(nóng)業(yè)機(jī)器人所處環(huán)境，將土地平整度和衛(wèi)星信號(hào)強(qiáng)度分級(jí)，設(shè)定土地平整度因子和衛(wèi)星信號(hào)強(qiáng)度因子ξ(1≤ξ≤ξ_max，ξ∈Z),Z為整數(shù)集，ξ_max分別表示最大的土地不平整級(jí)別和衛(wèi)星信號(hào)衰落級(jí)別；

步驟2，以東北天坐標(biāo)系為導(dǎo)航坐標(biāo)系，右前上坐標(biāo)系為載體坐標(biāo)系，根據(jù)先驗(yàn)知識(shí)與農(nóng)業(yè)機(jī)器人所處環(huán)境(例如障礙物、土地平整度、遮擋、光照等)，建立MEMS捷聯(lián)慣性導(dǎo)航系統(tǒng)、RTK GPS組合導(dǎo)航系統(tǒng)模型：

其中，狀態(tài)向量X＝[φ_E φ_N φ_U δV_E δV_N δL δλ ε_bx ε_by ε_bz ▽_bx ▽_by ▽_bz]^T，φ_E、φ_N、φ_U為東向、北向和天向失準(zhǔn)角；δV_E、δV_N為東向、北向速度誤差；δL、δλ為緯度、經(jīng)度誤差；ε_bx、ε_by、ε_bz為陀螺的常值漂移；▽_bx、▽_by、▽_bz為加速度計(jì)常值漂移。以MEMS-INS和GPS測(cè)得的速度、位置之差作為觀測(cè)量，V_INS和V_GNSS為東北天坐標(biāo)系下MEMS-INS和GPS測(cè)得的速度，P_INS和P_GNSS為東北天坐標(biāo)系下MEMS-INS和GPS測(cè)得的位置；F和H分別為狀態(tài)轉(zhuǎn)移矩陣、觀測(cè)矩陣；ω為狀態(tài)噪聲向量，協(xié)方差為Q；為量測(cè)噪聲向量，考慮農(nóng)田復(fù)雜環(huán)境對(duì)傳感器測(cè)量精度的影響，建立以量測(cè)噪聲向量ν及其協(xié)方差矩陣R為變化參數(shù)的模型集合M，設(shè)模型個(gè)數(shù)為r，Markov轉(zhuǎn)移概率為π_ji，下標(biāo)i，j分別表示第i個(gè)模型和第j個(gè)模型。

步驟3，由k-1(k≥1)時(shí)刻的模型概率與先驗(yàn)的Markov轉(zhuǎn)移概率π_ji進(jìn)行交互，計(jì)算混合概率

步驟4，對(duì)于第j＝1,2,…，r個(gè)模型，利用k-1時(shí)刻的狀態(tài)估計(jì)和協(xié)方差陣計(jì)算k時(shí)刻的狀態(tài)初值與協(xié)方差陣初值

步驟5，在組合導(dǎo)航系統(tǒng)獲得新的量測(cè)z^k之后，利用步驟4中計(jì)算得到的狀態(tài)初值與協(xié)方差陣初值采用秩濾波算法進(jìn)行濾波，得到k時(shí)刻的狀態(tài)估計(jì)協(xié)方差陣殘差估值以及殘差協(xié)方差具體流程為：

步驟5-1，以α，β為下標(biāo)分別表示第α個(gè)采樣點(diǎn)和第β層采樣點(diǎn)，利用k-1時(shí)刻的狀態(tài)估計(jì)一維標(biāo)準(zhǔn)分布概率p_β對(duì)應(yīng)的分位點(diǎn)和第β層采樣點(diǎn)的修正系數(shù)r_β，通過秩采樣機(jī)制求取采樣點(diǎn)集

其中，n為狀態(tài)向量x的維數(shù)，ρ為采樣點(diǎn)層數(shù)，l取值為[1,n]中的整數(shù)，表示矩陣平方根的第l列，第β層采樣點(diǎn)對(duì)應(yīng)的概率p_β采用中位秩計(jì)算得到，計(jì)算表達(dá)式為p_β＝(β－0.3)/(2ρ+1.4)。

步驟5-2，將采樣點(diǎn)集進(jìn)行非線性傳播，得到非線性變換后點(diǎn)集

其中，f(·)為狀態(tài)方程非線性函數(shù)，ω為過程噪聲向量。

步驟5-3，進(jìn)一步預(yù)測(cè)狀態(tài)估計(jì)和協(xié)方差陣

其中，為第α個(gè)采樣點(diǎn)對(duì)應(yīng)的協(xié)方差修正系數(shù)，取值為1；Q為過程噪聲方差矩陣；τ為協(xié)方差權(quán)重系數(shù)，其計(jì)算公式為：

步驟5-4，通過秩采樣機(jī)制求取采樣點(diǎn)集

其中，第β層采樣點(diǎn)對(duì)應(yīng)的概率p_β采用中位秩計(jì)算得到，計(jì)算表達(dá)式為p_β＝(β－0.3)/(2ρ+1.4)。

步驟5-5，將采樣點(diǎn)集引入觀測(cè)方程并進(jìn)行非線性傳播，得到預(yù)測(cè)的量測(cè)點(diǎn)集

其中，h(·)為觀測(cè)方程非線性函數(shù)，ν^j為模型j對(duì)應(yīng)的量測(cè)噪聲向量。

步驟5-6，通過預(yù)測(cè)的量測(cè)點(diǎn)集計(jì)算量測(cè)估計(jì)

步驟5-7，計(jì)算新息協(xié)方差矩陣和互協(xié)方差陣

其中R^j為模型j對(duì)應(yīng)的的量測(cè)噪聲方差矩陣。

步驟5-8，由新息協(xié)方差矩陣和互協(xié)方差陣計(jì)算增益

k時(shí)刻組合導(dǎo)航系統(tǒng)的量測(cè)值z(mì)_k到來之后，計(jì)算k時(shí)刻的狀態(tài)估計(jì)和協(xié)方差陣具體計(jì)算公式為：

步驟6，通過如下方法計(jì)算當(dāng)前時(shí)刻匹配模型對(duì)應(yīng)的殘差估值殘差協(xié)方差似然函數(shù)和模型概率：

步驟7，利用步驟5中各模型濾波器所得到的狀態(tài)估計(jì)協(xié)方差陣和步驟6中獲取的模型概率計(jì)算融合輸出結(jié)果：

步驟8，返回步驟3，重復(fù)步驟3至步驟7，直至接收到停止導(dǎo)航控制指令。

實(shí)施例

為驗(yàn)證本發(fā)明的可行性，在Matlab下進(jìn)行了仿真，濾波初始值及仿真參數(shù)設(shè)置如下：

陀螺隨機(jī)常值漂移為1°/h，白噪聲隨機(jī)漂移為0.1°/h；

加速度計(jì)偏置誤差為1mg，白噪聲隨機(jī)漂移為0.1mg；

MEMS捷聯(lián)慣性導(dǎo)航系統(tǒng)初始水平姿態(tài)角誤差為1°，航向角誤差為3°；

初始速度為0m/s，初始速度誤差為0m/s；

初始位置為東經(jīng)118°，北緯32°，高度100m，初始位置誤差為0m；

RTK GPS位置測(cè)量精度為0.1米；

MEMS捷聯(lián)慣性導(dǎo)航系統(tǒng)采樣周期為1ms，RTK GPS采樣周期為0.1s；

最大的土地不平整級(jí)別最大衛(wèi)星信號(hào)衰落級(jí)別ξ_max＝1，則建立的模型集合M中模型個(gè)數(shù)r為3，Markov轉(zhuǎn)移概率為

仿真時(shí)間為3600s，假設(shè)農(nóng)業(yè)機(jī)器人在農(nóng)田中模擬“割草機(jī)”運(yùn)動(dòng)，軌跡如圖2所示。在同等條件下，導(dǎo)航計(jì)算機(jī)根據(jù)傳感器數(shù)據(jù)和系統(tǒng)模型，分別采用交互式多模型擴(kuò)展卡爾曼濾波(IMM-EKF)、交互式多模型無跡卡爾曼濾波(IMM-UKF)和本發(fā)明提出的交互式多模型秩濾波(IMM-RKF)算法進(jìn)行濾波估計(jì)，對(duì)比三種算法在組合導(dǎo)航系統(tǒng)中的估計(jì)結(jié)果。圖3為俯仰角估計(jì)誤差曲線，圖4為橫搖角估計(jì)誤差曲線圖，圖5為航向角估計(jì)誤差曲線，可以看出，三種算法所得到的水平姿態(tài)角誤差均能收斂到很小的值并且收斂速度較快，航向角誤差的修正效果不是很明顯，但也能看出逐漸收斂的趨勢(shì)，收斂速度較慢。從航向角誤差的收斂過程可以看出，本發(fā)明提出的IMM-RKF算法估計(jì)效果優(yōu)于其它兩種算法；圖6、圖7分別為東向、北向速度估計(jì)誤差曲線，由于實(shí)時(shí)動(dòng)態(tài)GPS提供的速度信息對(duì)SINS的速度誤差進(jìn)行修正，采用IMM-RKF算法速度誤差基本保持在0.2m/s以內(nèi)，明顯優(yōu)于其它兩種算法；圖8、圖9分別為緯度、經(jīng)度估計(jì)誤差曲線，可以看出IMM-UKF與IMM-RKF算法位置誤差較為接近，IMM-EKF算法誤差較大。進(jìn)一步的，將三種算法得到的位置估計(jì)誤差進(jìn)行數(shù)值比較，采用IMM-EKF算法得到的經(jīng)度估計(jì)誤差均方差為0.051m，最大達(dá)到0.095m；緯度估計(jì)誤差均方差為0.045m，最大達(dá)到0.102m。采用IMM-UKF算法得到的經(jīng)度估計(jì)誤差為0.042m，最大為0.049m；緯度估計(jì)誤差均方差為0.008m，最大為0.061m。采用IMM-RKF算法得到的經(jīng)度估計(jì)誤差為0.027m，最大為0.032m；緯度估計(jì)誤差均方差為0.005m，最大為0.039m。從數(shù)據(jù)對(duì)比結(jié)果可以看出，本發(fā)明提出的基于秩濾波的農(nóng)業(yè)機(jī)器人組合導(dǎo)航信息融合方法性能優(yōu)于現(xiàn)有的其它兩種交互式多模型信息融合算。

本發(fā)明提供了一種基于秩濾波的農(nóng)業(yè)機(jī)器人組合導(dǎo)航信息融合方法，具體實(shí)現(xiàn)該技術(shù)方案的方法和途徑很多，以上所述僅是本發(fā)明的優(yōu)選實(shí)施方式，應(yīng)當(dāng)指出，對(duì)于本技術(shù)領(lǐng)域的普通技術(shù)人員來說，在不脫離本發(fā)明原理的前提下，還可以做出若干改進(jìn)和潤飾，這些改進(jìn)和潤飾也應(yīng)視為本發(fā)明的保護(hù)范圍。本實(shí)施例中未明確的各組成部分均可用現(xiàn)有技術(shù)加以實(shí)現(xiàn)。