專利名稱：集合卡爾曼濾波－粒子濾波相結(jié)合的auv組合導(dǎo)航方法
技術(shù)領(lǐng)域：
本發(fā)明涉及海洋工程領(lǐng)域，尤其是ー種自主水下航行器(Autonomous UnderwaterVehicle, AUV)組合導(dǎo)航方法。
背景技術(shù)：
自主水下航行器是目前海洋工程領(lǐng)域技術(shù)發(fā)展的熱點，在水下環(huán)境監(jiān)測、近海石油工程作業(yè)、水下捜索與測繪、以及水雷對抗和實時戰(zhàn)區(qū)警戒等軍事領(lǐng)域獲得越來越廣泛的應(yīng)用。導(dǎo)航技術(shù)是實現(xiàn)AUV實現(xiàn)自主航行的關(guān)鍵，由于AUV存在工作時間長、環(huán)境復(fù)雜、信息源少、隱蔽性要求高等工作特點，這給穩(wěn)定、精確的AUV自主導(dǎo)航帶來很大的困難和挑戰(zhàn)。近年來國際上對于AUV導(dǎo)航技術(shù)的研究一直非常活躍。水下導(dǎo)航技術(shù)通常需要在精度、運動范圍和自動化程度等之間進(jìn)行平衡，概括起來主要有聲學(xué)定位、地球物理導(dǎo)航及航位推算與慣性導(dǎo)航等多種類型的導(dǎo)航技木?，F(xiàn)有的AUV通常利用慣性導(dǎo)航系統(tǒng)(Inertial Navigation System, INS)采集的姿態(tài)角及加速度信息，三維電子羅盤(TCM)采集的姿態(tài)角信息以及多普勒計程儀(DopplerVelocity Log, DVL)采集的速度信息進(jìn)行組合導(dǎo)航，常用的數(shù)據(jù)融合方法包括卡爾曼濾波(Kalman Filter, KF)擴(kuò)展卡爾曼濾波(Extended Kalman Filter, EKF)、無味卡爾曼濾波(Unscented Kalman Filter, UKF)、粒子濾波(Particle Filter, PF)和集合卡爾曼濾波(Ensemble Kalman Filter, EnKF)等,其中擴(kuò)展卡爾曼濾波和無味卡爾曼濾波的共同問題是在非線性/非高斯性較強(qiáng)時收斂性急劇下降甚至發(fā)散。而粒子濾波利用序貫蒙特卡洛方法的，對于非線性/非高斯問題具有較好的性能，但是在粒子濾波中如果最新的觀測信息位于先驗概率分布的尾部或者似然函數(shù)相比先驗概率是峰化的，則會導(dǎo)致粒子選擇的盲目性，降低了估計精度。

發(fā)明內(nèi)容
本發(fā)明的目的是提供一種基于集合卡爾曼濾波一粒子濾波相結(jié)合的自主水下航行器組合導(dǎo)航方法，以提高AUV導(dǎo)航算法的精度。本發(fā)明解決其技術(shù)問題所采用的技術(shù)方案是ー種集合卡爾曼濾波一粒子濾波相結(jié)合的AUV組合導(dǎo)航方法，所述組合導(dǎo)航方法包括以下步驟I)數(shù)據(jù)采集獲取AUV在水面時的初始位置信息，采集AUV的速度和姿態(tài)角信息；2)濾波定位以步驟I)的初始位置信息、速度和姿態(tài)角信息定義狀態(tài)向量、觀測向量、狀態(tài)模型方程和觀測模型方程，濾波過程如下2. I)根據(jù)初始條件生成初始粒子和它相應(yīng)的權(quán)系數(shù)；2. 2)定義每個粒子背景集合= =，根據(jù)式(I)對背景集合進(jìn)行預(yù)測，
狀態(tài)模型方程Xk=HXH,wk)(I)其中Xk+ Xk分別表示k_l和k時4的狀態(tài)向量，Wk表示狀態(tài)過程噪聲，f ( )表示Xlri和Xk之間的非線性關(guān)系；計算背景集合的均值和協(xié)方差，并計算相應(yīng)的卡爾曼增益
權(quán)利要求
1.一種集合卡爾曼濾波一粒子濾波相結(jié)合的AUV組合導(dǎo)航方法，其特征在于所述組合導(dǎo)航方法包括以下步驟 1)數(shù)據(jù)采集獲取AUV在水面時的初始位置信息，采集AUV的速度和姿態(tài)角信息； 2)濾波定位以步驟I)的初始位置信息、速度和姿態(tài)角信息定義狀態(tài)向量、觀測向量、狀態(tài)模型方程和觀測模型方程，濾波過程如下. 2.I)根據(jù)初始條件生成初始粒子和它相應(yīng)的權(quán)系數(shù)； .2. 2)定義每個粒子背景集合Xp6=彳0/= 1,2，…，根據(jù)式(I)對背景集合進(jìn)行預(yù)測， 狀態(tài)模型方程
全文摘要
一種集合卡爾曼濾波－粒子濾波相結(jié)合的AUV組合導(dǎo)航方法，包括以下步驟1)數(shù)據(jù)采集利用全球定位系統(tǒng)獲取AUV在水面時的初始位置信息，利用多普勒計程儀、電子羅盤等導(dǎo)航傳感器采集AUV的速度和姿態(tài)角等信息；2)濾波定位利用基于集合卡爾曼濾波－粒子濾波相結(jié)合的濾波算法將傳感器采集到的導(dǎo)航信息進(jìn)行融合，估計得到AUV每一時刻的位置及姿態(tài)變化信息，實現(xiàn)對AUV的全局定位。本發(fā)明提供一種提高精度的集合卡爾曼濾波－粒子濾波相結(jié)合的AUV組合導(dǎo)航方法。
文檔編號G01C21/00GK102818567SQ20121028052
公開日2012年12月12日 申請日期2012年8月8日 優(yōu)先權(quán)日2012年8月8日
發(fā)明者李建龍, 溫國曦, 徐文 申請人:浙江大學(xué)