專利名稱：一種用于水下滑翔器的高精度組合導(dǎo)航定位方法
一種用于水下滑翔器的高精度組合導(dǎo)航定位方法技術(shù)領(lǐng)域
本發(fā)明屬于導(dǎo)航技術(shù)領(lǐng)域，涉及水下滑翔器的導(dǎo)航定位，為一種用于水下滑翔器 的高精度組合導(dǎo)航定位方法，該方法具有高可靠性、高精度、高穩(wěn)定性及實(shí)時(shí)性等特點(diǎn)，可 實(shí)現(xiàn)水下滑翔器的準(zhǔn)確定位和自主導(dǎo)航。
背景技術(shù)：
對(duì)于水下滑翔器的定位與導(dǎo)航，為了達(dá)到低功耗、低成本、長航時(shí)的目的，要求導(dǎo) 航傳感器的數(shù)量盡量少，基于微型機(jī)電系統(tǒng)MEMS的慣性測(cè)量單元IMU以其體積小、重量輕、 低功耗等優(yōu)點(diǎn)，加之其不受外界干擾，能在短期內(nèi)提供較高精度，被作為水下滑翔器的首選 導(dǎo)航元件。然而基于MEMS的MU本身存在一些固有器件誤差，加上熱噪聲、電子干擾等，會(huì) 造成非靜態(tài)的隨機(jī)誤差漂移和MEMS器件的標(biāo)度因子呈較嚴(yán)重的非線性。在運(yùn)行期間，這 些非線性誤差會(huì)增長，基于MEMS的慣性導(dǎo)航系統(tǒng)INS的性能也會(huì)隨之下降，在沒有額外的 校正時(shí)位置誤差會(huì)累積增長。對(duì)于大多數(shù)水面導(dǎo)航，全球定位系統(tǒng)GPS已經(jīng)是很成熟且可 靠的技術(shù)，但在水下，GPS可能遇到失鎖、信號(hào)阻斷、多徑效應(yīng)問題。所以航位推算算法將對(duì) INS的誤差校正起到至關(guān)重要的作用。
盡管卡爾曼濾波KF通過去除INS誤差中線性部分來補(bǔ)償系統(tǒng)輸出，但KF是適用 于由高斯白噪聲所產(chǎn)生的線性誤差狀態(tài)模型，對(duì)基于MEMS的INS的非線性動(dòng)態(tài)模型，它的 效果并不佳。擴(kuò)展卡爾曼濾波EKF通過去掉高次項(xiàng)強(qiáng)制將非線性模型近似成線性化模型。 但這種線性化在高維非線性模型中將會(huì)導(dǎo)致濾波器的發(fā)散，效果較差。無跡卡爾曼濾波UKF 從原理上克服了 EKF的缺陷，在非線性系統(tǒng)中比EKF要優(yōu)很多，但是UKF在計(jì)算非線性變換 的隨機(jī)變量統(tǒng)計(jì)性時(shí)會(huì)有較大計(jì)算量。
因水下滑翔器導(dǎo)航與定位要求低功耗、長航時(shí)、高精度、小體積等，所以制約了器 件的選擇，而目前已有的算法均有不足。如何結(jié)合水下滑翔器導(dǎo)航的背景，在數(shù)據(jù)處理、算 法優(yōu)化、濾波方式等方面進(jìn)行算法設(shè)計(jì)以達(dá)到高精度、高實(shí)時(shí)性、長航時(shí)、高穩(wěn)定性等，在國 內(nèi)外還沒有太多報(bào)道。發(fā)明內(nèi)容
本發(fā)明要解決的問題是水下滑翔器導(dǎo)航與定位要求低功耗、長航時(shí)、高精度、小 體積等，所以制約了器件的選擇，而目前已有的算法均有不足，需要一種用于水下滑翔器的 導(dǎo)航定位方法，克服傳統(tǒng)組合導(dǎo)航系統(tǒng)由于誤差隨時(shí)間累積而不能高精度長航時(shí)實(shí)時(shí)導(dǎo)航 與定位的欠缺，在數(shù)據(jù)處理、算法優(yōu)化、濾波方式等方面達(dá)到高精度、高實(shí)時(shí)性、長航時(shí)和高 穩(wěn)定性。
本發(fā)明的技術(shù)方案為一種用于水下滑翔器的高精度組合導(dǎo)航定位方法，以慣性 測(cè)量單元IMU為導(dǎo)航原件，包括以下步驟
I)將慣性測(cè)量單元IMU的輸出數(shù)據(jù)輸入無限脈沖響應(yīng)IIR低通濾波器，去除掉粗 大噪聲，完成數(shù)據(jù)的粗處理；
2)對(duì)粗處理后的數(shù)據(jù)進(jìn)行細(xì)處理，所述細(xì)處理為計(jì)算粗處理后得到的數(shù)據(jù)的高斯 分布均值和方差；3)采用基于龍格一庫塔法Runge-Kutta的航位推算DR,推算每個(gè)更新周期內(nèi)滑翔 器的位置、速度和姿態(tài)；4)設(shè)置兩級(jí)濾波系統(tǒng)，第一級(jí)濾波為自適應(yīng)卡爾曼濾波AKF，第二級(jí)濾波是無跡卡 爾曼濾波UKF，自適應(yīng)卡爾曼濾波AKF通過自適應(yīng)模糊控制調(diào)節(jié)濾波增益，通過自適應(yīng)強(qiáng)跟 蹤的擴(kuò)展卡爾曼濾波EKF預(yù)測(cè)均方誤差，所述濾波增益與預(yù)測(cè)均方誤差共同調(diào)節(jié)得到估計(jì) 均方誤差，將所述估計(jì)均方誤差代入無跡卡爾曼濾波UKF，得到兩級(jí)濾波系統(tǒng)，兩級(jí)濾波系 統(tǒng)對(duì)輸入數(shù)據(jù)進(jìn)行UKF濾波；5)將步驟2)得到的高斯分布均值和方差，以及步驟3)得到的推算數(shù)據(jù)融合進(jìn)行 UKF濾波，由UKF估出的誤差反饋回慣性測(cè)量單元IMU，校正慣性測(cè)量單元IMU的累積誤差， 維持慣性測(cè)量單元MU的精度，輸出準(zhǔn)確的位置、速度和姿態(tài)信息，實(shí)現(xiàn)穩(wěn)定的導(dǎo)航定位。步驟3)的基于龍格一庫塔法Runge-Kutta的航位推算DR具體為設(shè)航位推算 的采樣周期為T，在采樣周期內(nèi)進(jìn)行m次速度、位置和姿態(tài)的信息更新，每次更新的周期為
權(quán)利要求
1.一種用于水下滑翔器的高精度組合導(dǎo)航定位方法，其特征是以慣性測(cè)量單元IMU為導(dǎo)航原件，包括以下步驟 1)將慣性測(cè)量單元IMU的輸出數(shù)據(jù)輸入無限脈沖響應(yīng)IIR低通濾波器，去除掉粗大噪聲，完成數(shù)據(jù)的粗處理； 2)對(duì)粗處理后的數(shù)據(jù)進(jìn)行細(xì)處理，所述細(xì)處理為計(jì)算粗處理后得到的數(shù)據(jù)的高斯分布均值和方差； 3)采用基于龍格一庫塔法Runge-Kutta的航位推算DR，推算每個(gè)更新周期內(nèi)滑翔器的位置、速度和姿態(tài)； 4)設(shè)置兩級(jí)濾波系統(tǒng)，第一級(jí)濾波為自適應(yīng)卡爾曼濾波AKF，第二級(jí)濾波是無跡卡爾曼濾波UKF，自適應(yīng)卡爾曼濾波AKF通過自適應(yīng)模糊控制調(diào)節(jié)濾波增益，通過自適應(yīng)強(qiáng)跟蹤的擴(kuò)展卡爾曼濾波EKF預(yù)測(cè)均方誤差，所述濾波增益與預(yù)測(cè)均方誤差共同調(diào)節(jié)得到估計(jì)均方誤差，將所述估計(jì)均方誤差代入無跡卡爾曼濾波UKF，得到兩級(jí)濾波系統(tǒng)，兩級(jí)濾波系統(tǒng)對(duì)輸入數(shù)據(jù)進(jìn)行UKF濾波； 5)將步驟2)得到的高斯分布均值和方差，以及步驟3)得到的推算數(shù)據(jù)融合進(jìn)行UKF濾波，由UKF估出的誤差反饋回慣性測(cè)量單元IMU，校正慣性測(cè)量單元IMU的累積誤差，維持慣性測(cè)量單元IMU的精度，輸出準(zhǔn)確的位置、速度和姿態(tài)信息，實(shí)現(xiàn)穩(wěn)定的導(dǎo)航定位。
2.根據(jù)權(quán)利要求1所述的一種用于水下滑翔器的高精度組合導(dǎo)航定位方法，其特征是步驟3)的基于龍格一庫塔法Runge-Kutta的航位推算DR具體為設(shè)航位推算的采樣周期為T，在采樣周期內(nèi)進(jìn)行m次速度、位置和姿態(tài)的信息更新，每次更新的周期為λ =工，采樣 m前速度為％，在采樣周期內(nèi)每次更新的速度為J=I, 2，....，m，采樣后速度為Vm，則在時(shí)間t內(nèi)更新的速度由式(b)遞推
3.根據(jù)權(quán)利要求1所述的一種用于水下滑翔器的高精度組合導(dǎo)航定位方法，其特征是步驟4)中第一級(jí)濾波自適應(yīng)卡爾曼濾波AKF具體為 a)卡爾曼方程中濾波增益方程為&(札4,+A)-S式中Pk/H為k時(shí)刻的理論預(yù)測(cè)均方誤差，Hk為k時(shí)刻的理論量測(cè)矩陣，Rk為k時(shí)刻的理論量測(cè)噪聲的方差矩陣；自適應(yīng)卡爾曼濾波AKF采用模糊自適應(yīng)算法調(diào)節(jié)濾波增益Kk，記.V,: = HkPk^Hl + Rk，C1c=HHt +R[，式中V' k/H為k時(shí)刻的實(shí)際預(yù)測(cè)均方誤差，H' k*k時(shí)刻的實(shí)際量測(cè)矩陣，Ri kSk時(shí)刻的實(shí)際量測(cè)噪聲的方差矩陣，Sk表示k時(shí)刻更新序列的理論協(xié)方差，Ck表不k時(shí)刻實(shí)際協(xié)方差，Sk = HlcPkfk^Hl +Rk ;記deItak=Ck-Sk, deltak為模糊邏輯控制器的輸入，Rk作為輸出，如果delta,不變則Rk維持不變；如果delta,大于零則Rk減??；如果delta,小于零則Rk增大，通過自適應(yīng)調(diào)節(jié)Rk，自適應(yīng)調(diào)節(jié)濾波增益 b)自適應(yīng)卡爾曼濾波AKF通過自適應(yīng)強(qiáng)跟蹤擴(kuò)展卡爾曼濾波EKF來調(diào)節(jié)理論預(yù)測(cè)均方誤差Ρρη，設(shè)自適應(yīng)強(qiáng)跟蹤EKF的狀態(tài)估計(jì)為JT4 s :hl+Kk(Zk -H(Xk^l)-Fk),式中足Α—, =Φ .—為狀態(tài)一步預(yù)測(cè)，Φ,,η為k-1到k時(shí)刻的系統(tǒng)一步轉(zhuǎn)移矩陣，U k-Ι時(shí)刻的狀態(tài)預(yù)測(cè)，Kk為濾波增益，Zk為k時(shí)刻的量測(cè)向量，h( ·)為觀測(cè)函數(shù)，rk為量測(cè)方程中帶時(shí)變均值和協(xié)方差的高斯白噪聲序列的均值；預(yù)測(cè)均方誤差為 i =4—Au,+β—,，式中Qlri為量測(cè)方程中帶時(shí)變均值和協(xié)方差的高斯白噪聲序列的協(xié)方差，為k-Ι時(shí)刻的自適應(yīng)漸消因子，采用時(shí)變的自適應(yīng)漸消因子對(duì)過去的數(shù)據(jù)進(jìn)行漸消，以減弱陳舊數(shù)據(jù)對(duì)當(dāng)前濾波值的影響，通過實(shí)時(shí)調(diào)整狀態(tài)預(yù)測(cè)誤差的協(xié)方差矩陣以及相應(yīng)的增益矩陣來實(shí)現(xiàn)減弱影響這一目標(biāo)，自適應(yīng)漸消因子由下式確定[i Λ),a- < i其中 λ Q，k=max{l，tr[Nk]/tr[Mk]},
4.根據(jù)權(quán)利要求1所述的一種用于水下滑翔器的高精度組合導(dǎo)航定位方法，其特征是步驟4)中第二級(jí)濾波無跡卡爾曼濾波UKF具體為考慮驅(qū)動(dòng)噪聲對(duì)系統(tǒng)的影響，在UKF中進(jìn)行狀態(tài)擴(kuò)維，加入量測(cè)噪聲Rk、系統(tǒng)噪聲Qk及自適應(yīng)卡爾曼濾波AKF得到的估計(jì)均方誤差Pk/k進(jìn)行狀態(tài)擴(kuò)維，組成新的協(xié)方差矩陣，進(jìn)行擴(kuò)維后的狀態(tài)變量和協(xié)方差矩陣為 狀態(tài)變量K =[4 wkT Vl ]Γ式中Xk表示k時(shí)刻的系統(tǒng)狀態(tài)向量，Wk表示k時(shí)刻的系統(tǒng)噪聲，Vk為k時(shí)刻的量測(cè)噪聲; 協(xié)方差矩陣
全文摘要
本發(fā)明公布了一種用于水下滑翔器的高精度組合導(dǎo)航定位方法，慣性測(cè)量單元IMU的輸出經(jīng)過粗處理和細(xì)處理后得到較高精度的數(shù)據(jù)，融合基于Runge-Kutta法(RK4)的航位推算得到的數(shù)據(jù)，再經(jīng)自適應(yīng)卡爾曼AKF和無跡卡爾曼UKF二級(jí)濾波，濾波后反饋回IMU來校正IMU的累積誤差，最終輸出較準(zhǔn)的位置速度姿態(tài)等信息。本發(fā)明用于水下滑翔器的自主導(dǎo)航，具有高精度、實(shí)時(shí)性、穩(wěn)定性等優(yōu)點(diǎn)，使得整體導(dǎo)航系統(tǒng)能長航時(shí)、高精度、低功耗穩(wěn)定運(yùn)行，能迅速準(zhǔn)確地得到水下航行器當(dāng)前位姿信息，同時(shí)為其提供航跡和位置參數(shù)。
文檔編號(hào)G01C21/16GK103033186SQ201210585170
公開日2013年4月10日 申請(qǐng)日期2012年12月30日 優(yōu)先權(quán)日2012年12月30日
發(fā)明者陳熙源, 黃浩乾, 劉虎, 周智愷, 唐建, 方琳 申請(qǐng)人:東南大學(xué)