本發(fā)明涉及協(xié)同導(dǎo)航算法領(lǐng)域，具體涉及一種針對未知量測噪聲下的具有自適應(yīng)的協(xié)同導(dǎo)航濾波方法。
背景技術(shù)：
自主水下航行器(Autonomous Underwater Vehicles,AUV)在民用還是軍事方面都有廣泛的應(yīng)用。隨著對海洋的探索越來越深入，使得AUV的使命也變得越來越復(fù)雜化和多樣化，如果僅僅依靠單個AUV去實現(xiàn)這些功能，不僅在成本上會提高很多，而且可靠性也會降低，而由多臺水下AUV組成的協(xié)作系統(tǒng)具有空間分布、功能分布、冗余等特點。然而不論單AUV還是多AUV系統(tǒng)以及其執(zhí)行何種任務(wù)，導(dǎo)航系統(tǒng)的優(yōu)劣直接影響系統(tǒng)的性能好壞。協(xié)同導(dǎo)航定位技術(shù)作為一種新的導(dǎo)航定位方法具有只對有限數(shù)目的AUV增加設(shè)備費從而來提高編隊的導(dǎo)航精度等優(yōu)點。由于水下環(huán)境和基于聲學(xué)定位的特殊性，協(xié)同導(dǎo)航濾波過程中所需要量測噪聲協(xié)方差陣具有時變性，傳統(tǒng)算法中是通過提前設(shè)置預(yù)值來進(jìn)行定位解算的，并未考慮到其它可能的變化，限制了AUV的適用范圍。本方法提出通過提前預(yù)設(shè)多組不同的量測噪聲協(xié)方差矩陣來提高協(xié)同導(dǎo)航濾波的自適應(yīng)性，擴大AUV的適用范圍。
技術(shù)實現(xiàn)要素：
本發(fā)明的目的在于提出一種在未知量測噪聲環(huán)境下時對協(xié)同編隊進(jìn)行定位能夠抑制噪聲未知下導(dǎo)致的從AUV定位精度下降問題的具有自適應(yīng)的協(xié)同導(dǎo)航濾波方法。
本發(fā)明的目的是這樣實現(xiàn)的
第一步：輸入交互過程
如圖1所示，交互式多模型算法是一個循環(huán)算法，通過輸入交互過程、模型濾波、模型概率更新以及輸出交互過程四個過程完成狀態(tài)估計。
在輸入交互過程中，首次通過預(yù)驗?zāi)Ｐ透怕诗@取混合狀態(tài)，下次循環(huán)則通過計算得到的模型概率替換。
協(xié)同導(dǎo)航從AUV交互混合狀態(tài)估計及協(xié)方差為：
式中為k-1時刻第j各濾波器的狀態(tài)估計；Pj(k-1)為對應(yīng)的協(xié)方差矩陣；P0i(k-1)為狀態(tài)估計所對應(yīng)的狀態(tài)誤差協(xié)方差矩陣。
首次濾波中，用初始模型預(yù)測概率代替模型條件轉(zhuǎn)移概率進(jìn)行運算，下個濾波周期中通過利用上一個周期各個濾波器的狀態(tài)估計和模型條件轉(zhuǎn)移概率進(jìn)行運算。
第二步：模型濾波
基于EKF的協(xié)同導(dǎo)航濾波器1工作如下：
系統(tǒng)狀態(tài)一步預(yù)測：其中表示從AUV在時刻k+1的一步估計狀態(tài)，f為從AUV運動學(xué)方程，為從AUV交互混合估計狀態(tài)1，uk量測輸入。
一步預(yù)測協(xié)方差矩陣為：
其中Fk為狀態(tài)轉(zhuǎn)移矩陣，Pk為上一時刻的的協(xié)方差矩陣，Gk為過程噪聲分布矩陣，Qk表示噪聲協(xié)方差矩陣。
量測量預(yù)測為：
其中h表示量測方程，分別為領(lǐng)航者AUV1，AUV2的狀態(tài)。
殘差信息和其協(xié)方差：
其中Zk+1為傳感器接收到的量測值，Hk+1為量測矩陣的雅可比矩陣，為濾波器1中的量測噪聲協(xié)方差矩陣。
濾波增益：
狀態(tài)估計及誤差協(xié)方差矩陣：
P1,k+1＝(I-Kk+1Hk+1)Pk+1,k，其中I為單位矩陣。
基于EKF的協(xié)同導(dǎo)航濾波器2中主要不同于濾波器1的是量測噪聲協(xié)方差矩陣選取了另外一組其他步驟相同，并得到濾波器2下殘差信息v2,k+1和其協(xié)方差S2,k+1，狀態(tài)估計及其誤差協(xié)方差矩陣P2,k+1。
第三步：模型概率更新
做為本算法的重要部分，通過獲取各個濾波器的殘差信息并且結(jié)合貝葉斯假設(shè)檢驗方法，得到各模型的似然函數(shù)，求法如下：
m＝3表示協(xié)同導(dǎo)航中量測向量的維數(shù)；vi(k)為協(xié)同導(dǎo)航模型在k時刻時通過卡爾曼濾波獲得的殘差向量；Si(k)為對應(yīng)殘差協(xié)方差矩陣。
結(jié)合上一時刻的模型概率以及模型先驗信息，得到k時刻的模型概率為
第四步：交互輸出
循環(huán)算法中最后一步，由前面各個基于不同模型濾波器的狀態(tài)估計值及其模型概率加權(quán)融合得到，結(jié)合各模型的從AUV聯(lián)合狀態(tài)估計和協(xié)方差矩陣P(k)計算如下：
至此完成一次輸出，進(jìn)入下一時刻的狀態(tài)估計。
本發(fā)明方法具有以下優(yōu)點：
相比于傳統(tǒng)的協(xié)同導(dǎo)航濾波方法，在噪聲未知的環(huán)境下條件下，本方能有效減小因為噪聲預(yù)設(shè)偏差過大導(dǎo)致的精度下降問題。本方法具有很好的模塊化特性，所以我們不僅可以增加模型數(shù)量，而且還可以自由的選取具有各種特性的濾波器，因此其具有很好的后期修正性。
附圖說明
圖1為本發(fā)明示意圖。
具體實施方式
下面結(jié)合附圖對本發(fā)明做進(jìn)一步描述。
本發(fā)明提供一種關(guān)于協(xié)同導(dǎo)航自適應(yīng)濾波方法。該方法通過事先構(gòu)建多組關(guān)于不同量測噪聲的狀態(tài)模型，并通過交互式多模型算法將各組先驗?zāi)Ｐ驼掀饋?，達(dá)到自適應(yīng)濾波的目的。該方法的特點在于在未知環(huán)境下通過應(yīng)用交互式多模型算法能夠有效抑制實際噪聲與預(yù)測噪聲不匹配時造成的導(dǎo)航精度過低的問題，同時該方法具有模塊化特點，能夠進(jìn)一步針對具體噪聲特點進(jìn)行濾波算法的優(yōu)化，適合應(yīng)用于協(xié)同導(dǎo)航中。
實施例：
具體操作如下所述：
下面結(jié)合圖1及實施例對本發(fā)明的內(nèi)容做進(jìn)一步詳細(xì)的闡述。
第一步：輸入交互過程
如圖1所示，交互式多模型算法是一個循環(huán)算法，通過輸入交互過程、模型濾波、模型概率更新以及輸出交互過程四個過程完成狀態(tài)估計。
在輸入交互過程中，首次通過預(yù)驗?zāi)Ｐ透怕诗@取混合狀態(tài)，下次循環(huán)則通過計算得到的模型概率替換。
協(xié)同導(dǎo)航從AUV交互混合狀態(tài)估計及協(xié)方差為：
式中為k-1時刻第j各濾波器的狀態(tài)估計；Pj(k-1)為對應(yīng)的協(xié)方差矩陣；P0i(k-1)為狀態(tài)估計所對應(yīng)的狀態(tài)誤差協(xié)方差矩陣。
首次濾波中，用初始模型預(yù)測概率代替模型條件轉(zhuǎn)移概率進(jìn)行運算，下個濾波周期中通過利用上一個周期各個濾波器的狀態(tài)估計和模型條件轉(zhuǎn)移概率進(jìn)行運算。
第二步：模型濾波
基于EKF的協(xié)同導(dǎo)航濾波器1工作如下：
系統(tǒng)狀態(tài)一步預(yù)測：其中表示從AUV在時刻k+1的一步估計狀態(tài)，f為從AUV運動學(xué)方程，為從AUV交互混合估計狀態(tài)1，uk量測輸入。
一步預(yù)測協(xié)方差矩陣為：
其中Fk為狀態(tài)轉(zhuǎn)移矩陣，Pk為上一時刻的的協(xié)方差矩陣，Gk為過程噪聲分布矩陣，Qk表示噪聲協(xié)方差矩陣。
量測量預(yù)測為：
其中h表示量測方程，分別為領(lǐng)航者AUV1，AUV2的狀態(tài)。
殘差信息和其協(xié)方差：
其中Zk+1為傳感器接收到的量測值，Hk+1為量測矩陣的雅可比矩陣，為濾波器1中的量測噪聲協(xié)方差矩陣。
濾波增益：
狀態(tài)估計及誤差協(xié)方差矩陣：
P1,k+1＝(I-Kk+1Hk+1)Pk+1,k，其中I為單位矩陣。
基于EKF的協(xié)同導(dǎo)航濾波器2中主要不同于濾波器1的是量測噪聲協(xié)方差矩陣選取了另外一組其他步驟相同，并得到濾波器2下殘差信息v2,k+1和其協(xié)方差S2,k+1，狀態(tài)估計及其誤差協(xié)方差矩陣P2,k+1
第三步：模型概率更新
做為本算法的重要部分，通過獲取各個濾波器的殘差信息并且結(jié)合貝葉斯假設(shè)檢驗方法，得到各模型的似然函數(shù)，求法如下：
m＝3表示協(xié)同導(dǎo)航中量測向量的維數(shù)；vi(k)為協(xié)同導(dǎo)航模型在k時刻時通過卡爾曼濾波獲得的殘差向量；Si(k)為對應(yīng)殘差協(xié)方差矩陣。
結(jié)合上一時刻的模型概率以及模型先驗信息，得到k時刻的模型概率為
第四步：交互輸出
循環(huán)算法中最后一步，由前面各個基于不同模型濾波器的狀態(tài)估計值及其模型概率加權(quán)融合得到，結(jié)合各模型的從AUV聯(lián)合狀態(tài)估計和協(xié)方差矩陣P(k)計算如下：
至此完成一次輸出，進(jìn)入下一時刻的狀態(tài)估計。
以上結(jié)合附圖對本發(fā)明的具體實施方式做了詳細(xì)說明，但這些說明不能被理解為限制了本發(fā)明的范圍，本發(fā)明的保護(hù)范圍由隨附的權(quán)利要求書限定，任何在本發(fā)明權(quán)利要求基礎(chǔ)上的改動都屬于本發(fā)明的保護(hù)范圍。