專利名稱：基于二階插值濾波器的自主式水下潛器海上對準(zhǔn)方法
技術(shù)領(lǐng)域：
本發(fā)明涉及的是一種測量方法，特別是涉及一種捷聯(lián)慣性導(dǎo)航系統(tǒng)的初始對準(zhǔn)技術(shù)，尤其涉及一種自主式水下潛器光纖陀螺捷聯(lián)慣導(dǎo)系統(tǒng)脫離母船海上啟動時(shí)粗對準(zhǔn)方位誤差角比較大的對準(zhǔn)方法。
背景技術(shù)：
自主式水下潛器(Autonomous Underwater Vehicle，簡稱AUV)是80年代末90年代初在載人潛器和無人有纜遙控潛器(ROV)的技術(shù)基礎(chǔ)上迅速發(fā)展起米的一種新型水下運(yùn)載平臺。AUV在脫離母船后無法再用傳遞對準(zhǔn)等方法實(shí)現(xiàn)自身的初始對準(zhǔn)，因此尋找一種能快速對準(zhǔn)并保證一定精度的對準(zhǔn)方法成為AUV研究的一個(gè)難點(diǎn)。由于AUV自身體積小，重量輕，當(dāng)其脫離母船后受風(fēng)浪等外界環(huán)境的影響很大，因此AUV的粗對準(zhǔn)精度很低，而傳統(tǒng)的小失準(zhǔn)角AUV捷聯(lián)慣導(dǎo)系統(tǒng)(SINS)線性誤差模型只有在假設(shè)各種誤差源較小的條件下才成立。捷聯(lián)慣導(dǎo)系統(tǒng)初始對準(zhǔn)方法都是基于系統(tǒng)的誤差模型。大量文獻(xiàn)和工程實(shí)踐證明，對于AUV光纖陀螺捷聯(lián)慣導(dǎo)系統(tǒng)，當(dāng)方位誤差角為小角度時(shí)，對準(zhǔn)的誤差模型近似為線性，可以用卡爾曼濾波進(jìn)行狀態(tài)估計(jì)。當(dāng)方位誤差角為大角度時(shí)，對準(zhǔn)的誤差模型為非線性，在非線性濾波方面，比較廣泛使用的方法有兩個(gè)，一是以擴(kuò)展卡爾曼濾波(EKF)為代表的對非線性函數(shù)進(jìn)行線性化近似，利用離散的非線性方程圍繞濾波值

展開成泰勒級數(shù)，略去二階以上項(xiàng)，得到非線性系統(tǒng)的線性化模型，但是這種濾波方法引入了高階截?cái)嗾`差，估計(jì)精度比較低；另一個(gè)是基于近似概率密度分布的濾波估計(jì)方法，UKF就是其中的一種，UKF通過構(gòu)造一組采樣點(diǎn)來捕獲系統(tǒng)狀態(tài)的均值和方差，每個(gè)采樣點(diǎn)都經(jīng)過非線性函數(shù)的轉(zhuǎn)換，系統(tǒng)狀態(tài)預(yù)測的均值和方差由轉(zhuǎn)換后的采樣點(diǎn)給出，采樣點(diǎn)的多少決定了濾波時(shí)間，在相同的采樣頻率下，估計(jì)時(shí)間比較長。因此提高AUV光纖陀螺捷聯(lián)慣導(dǎo)系統(tǒng)在動基座大方位失準(zhǔn)角下的初始對準(zhǔn)精度，縮短初始對準(zhǔn)時(shí)間有著十分重要的意義。

發(fā)明內(nèi)容
本發(fā)明的目的在于提供一種可以達(dá)到較高適用精度的要求，對于AUV捷聯(lián)慣導(dǎo)系統(tǒng)脫離母船海上啟動、對準(zhǔn)精度高、對準(zhǔn)時(shí)間短、實(shí)現(xiàn)相對簡單的基于二階插值濾波器的自主式水下潛器海上對準(zhǔn)方法。
本發(fā)明的目的是這樣實(shí)現(xiàn)的 (1)光纖陀螺捷聯(lián)慣導(dǎo)系統(tǒng)預(yù)熱后采集光纖陀螺儀和石英撓性加速度計(jì)輸出的數(shù)據(jù)，根據(jù)加速度計(jì)的輸出與重力加速度的關(guān)系以及陀螺儀輸出與地球旋轉(zhuǎn)角速率的關(guān)系初步確定載體坐標(biāo)系到計(jì)算導(dǎo)航坐標(biāo)系的轉(zhuǎn)換矩陣Cbc； (2)建立光纖陀螺捷聯(lián)慣導(dǎo)系統(tǒng)在方位誤差角比較大(大于5°)時(shí)以速度誤差和三個(gè)誤差失準(zhǔn)角為狀態(tài)變量的非線性狀態(tài)方程和以速度誤差為觀測量的線性觀測方程； (3)將非線性的連續(xù)系統(tǒng)離散化，用二階插值(Second-order Divided Difference簡稱DD2)濾波器對離散的非線性系統(tǒng)進(jìn)行濾波，估計(jì)誤差失準(zhǔn)角； (4)利用步驟(3)估計(jì)得到的誤差失準(zhǔn)角對矩陣Cbc進(jìn)行誤差補(bǔ)償，得到精確的姿態(tài)矩陣，根據(jù)姿態(tài)矩陣得到載體姿態(tài)，完成初始對準(zhǔn)，進(jìn)入導(dǎo)航。
本發(fā)明還可以包括如下特征 1、所述建立光纖陀螺捷聯(lián)慣導(dǎo)系統(tǒng)在方位誤差角比較大時(shí)以速度誤差和三個(gè)誤差失準(zhǔn)角為狀態(tài)變量的非線性狀態(tài)方程和以速度誤差為觀測量的線性觀測方程包括 非線性狀態(tài)方程為 其中
B中其它元素為0； 其中，ΔVx、ΔVy是慣導(dǎo)系統(tǒng)計(jì)算速度與真實(shí)速度的誤差在地理系的投影；φx、φy、φz為三個(gè)誤差失準(zhǔn)角；

為加速度計(jì)算系統(tǒng)偏差在地理坐標(biāo)系的投影；εx、εy、εz為x、y、z軸陀螺漂移；Vxt、Vyt是載體在地理坐標(biāo)系x、y軸的真實(shí)速度；L是當(dāng)?shù)鼐暥龋籪′x、f′y為加速度計(jì)在x、y軸的讀數(shù)；ωie是地球旋轉(zhuǎn)角速率；

其中Re為赤道半徑，Rp為極軸半徑；e＝1/297；

為地理經(jīng)度； 線性觀測方程為z＝Cx+v 其中C＝[1 1 0 0 0 0 0 0 0 0]T， 將狀態(tài)方程和觀測方程離散化 xk+1＝f(xk，wk) yk＝g(xk，vk) 其中xk∈R10，是10×1的狀態(tài)向量；yk∈R2，是2×1的觀測向量；wk∈R10，是10×1的狀態(tài)噪聲向量；vk∈R2，是2×1的觀測噪聲向量；根據(jù)AUV捷聯(lián)慣導(dǎo)系統(tǒng)動機(jī)座大失準(zhǔn)角非線性誤差方程，假設(shè)系統(tǒng)的狀態(tài)噪聲和觀測噪聲是不相關(guān)Gaussian白噪聲，它們的均值和協(xié)方差為 E(wk)＝wk，E{(wk-w)(wj-w)}＝Qk E(vk)＝vk，E{(vk-v)(vj-v)}＝Rk

Sx，Sw和Sv分別是協(xié)方差預(yù)測

協(xié)方差Pk，狀態(tài)噪聲協(xié)方差均值Q和觀測噪聲協(xié)方差R的Cholesky分解， Pk＝SxSxT，Q＝SwSwT，R＝SvSvT。
2、用二階插值濾波器對離散的非線性系統(tǒng)進(jìn)行濾波，估計(jì)誤差失準(zhǔn)角的步驟如下 1)初始化狀態(tài)向量x的均值x0和協(xié)方差P0的Cholesky分解Sx(0)； 2)求輸出yk的一階差分

、Syv(1)和二階差分

(k)、Syv(2)(k)， 其中，

是

的第j列，Sv，j是Sv的第j列；步長設(shè)為 3)求輸出yk的計(jì)算值yk， 其中nx＝3，nw＝3； 4)求輸出yk的協(xié)方差的Cholesky分解Sy(k)， 5)求濾波增益Kk和Pxy(k)， Kk＝Pxy(k)(Sy(k)Sy(k)T)-1； 6)求狀態(tài)x的預(yù)測
7)求協(xié)方差預(yù)測

的Cholesky分解
8)計(jì)算狀態(tài)預(yù)測

的一階差分

Sxw(1)和二階差分

和Sxw(2)， 9)求xk+1， 其中，

是

的第p列，Sw，p是Sw的第p列； 10)求狀態(tài)xk+1的協(xié)方差Pk+1的Cholesky分解Sk+1， 11)把k增加到k+1從2)開始執(zhí)行。
本發(fā)明的方法具有如下優(yōu)點(diǎn)(1)克服了AUV脫離母船海上啟動時(shí)傳遞對準(zhǔn)和傳統(tǒng)的基于線性誤差模型的對準(zhǔn)方法不再適用的缺點(diǎn)，在不增加系統(tǒng)成本的條件下，可以達(dá)到較高適用精度的要求。(2)與基于泰勒展開式的EKF和基于近似概率密度分布的UKF不同，基于多維Stirling插值公式的二階插值濾波方法克服了EKF引入高階截?cái)嗾`差的缺點(diǎn)，也克服了UKF計(jì)算量大的缺點(diǎn)，對于AUV捷聯(lián)慣導(dǎo)系統(tǒng)脫離母船海上啟動，對準(zhǔn)精度高，對準(zhǔn)時(shí)間短，實(shí)現(xiàn)簡單。
對本發(fā)明的有益效果說明如下 為了驗(yàn)證本發(fā)明的實(shí)用性，進(jìn)行了Matlab仿真實(shí)驗(yàn)。
在中等海況下，AUV三軸搖擺幅值為6°、5°和2°； AUV速度8節(jié)； 陀螺的常值漂移為εbi＝0.01°/h，角隨機(jī)游走系數(shù)為

刻度因數(shù)誤差為δKgi＝10ppm； 加速度計(jì)的常值偏置為測量白噪聲為刻度因數(shù)誤差為δKai＝10ppm； 初始位置為經(jīng)度為126.6705°，緯度為45.7796°； 失準(zhǔn)角的初始條件φ(0)＝[1° 1° 10°]T； 插值濾波器初始值 x0＝
T Sx(0)＝diag{0.1 0.1 1° 1° 10° 10-4g 10-4g 0.01°/h 0.01°/h 0.01°/h} 在以上的實(shí)驗(yàn)條件下，仿真結(jié)果如下表所示 濾波方法估計(jì)時(shí)間失準(zhǔn)角φz估計(jì)誤差 EKF 293s12.4′ UKF 491s4.1′ DD2 353s4.9′

圖1本發(fā)明的結(jié)構(gòu)框圖； 圖2DD2濾波器流程圖； 圖3EKF方位失準(zhǔn)角φz估計(jì)誤差； 圖4UKF方位失準(zhǔn)角φz估計(jì)誤差； 圖5DD2濾波器方位失準(zhǔn)角φz估計(jì)誤差。
具體實(shí)施例方式 下面結(jié)合附圖舉例對本發(fā)明做更詳細(xì)地描述 本發(fā)明包括下列步驟 (1)光纖陀螺捷聯(lián)慣導(dǎo)系統(tǒng)預(yù)熱后采集光纖陀螺儀和石英撓性加速度計(jì)輸出的數(shù)據(jù)，根據(jù)加速度計(jì)的輸出與重力加速度的關(guān)系以及陀螺儀輸出與地球旋轉(zhuǎn)角速率的關(guān)系初步確定此時(shí)的姿態(tài)矩陣Cbc(此時(shí)水平誤差角為小角度，方位誤差角較大)。
(2)建立光纖陀螺捷聯(lián)慣導(dǎo)系統(tǒng)在方位誤差角比較大(大于5°)時(shí)以速度誤差和三個(gè)誤差失準(zhǔn)角為狀態(tài)變量的非線性狀態(tài)方程和以速度誤差為觀測量的線性觀測方程。
1)建立速度誤差方程 速度誤差方程為 (1) 其中L是當(dāng)?shù)鼐暥龋?

為加速度計(jì)算系統(tǒng)偏差在地理坐標(biāo)系的投影；ΔVx、ΔVy速度誤差在地理系的投影；f′x、f′y為加速度計(jì)在相應(yīng)坐標(biāo)軸的讀數(shù)；ωie是地球旋轉(zhuǎn)角速率。
2)建立誤差失準(zhǔn)角方程； 誤差失準(zhǔn)角為 φt＝[φx φy φz]T(2) 誤差失準(zhǔn)角的微分方程為 其中εx、εy、εz是陀螺漂移在地理系的投影；Vxt、Vty是載體在地理坐標(biāo)系的真實(shí)速度；ΔVx、ΔVy是慣導(dǎo)系統(tǒng)計(jì)算速度與真實(shí)速度的誤差；

(4)
其中Re為赤道半徑，Rp為極軸半徑； 3)建立光纖陀螺捷聯(lián)慣導(dǎo)系統(tǒng)的非線性狀態(tài)方程和觀測方程 z＝Cx+v 其中 z＝[Δvx Δvy]T(8)
B中其它元素為0。
C＝[1 1 0 0 0 0 0 0 0 0]T(11) 將狀態(tài)方程和觀測方程離散化 xk+1＝f(xk，wk) (13) yk＝g(xk，vk) 其中xk ∈R10，是10×1的狀態(tài)向量；yk ∈R2，是2×1的觀測向量；wk∈R10，是10×1的狀態(tài)噪聲向量；vk ∈R2，是2×1的觀測噪聲向量。根據(jù)AUV捷聯(lián)慣導(dǎo)系統(tǒng)動機(jī)座大失準(zhǔn)角非線性誤差方程，假設(shè)系統(tǒng)的狀態(tài)噪聲和觀測噪聲是不相關(guān)Gaussian白噪聲，它們的均值和協(xié)方差為 E(wk)＝wk，E{(wk-w)(wj-w)}＝Qk (14) E(vk)＝vk，E{(vk-v)(vj-v)}＝Rk (3)用DD2濾波器對AUV光纖陀螺捷聯(lián)慣導(dǎo)系統(tǒng)進(jìn)行濾波估計(jì)，估計(jì)誤差失準(zhǔn)角。
DD2濾波器估計(jì)誤差失準(zhǔn)角步驟如下 1)初始化狀態(tài)向量x的均值x0和協(xié)方差P0的Cholesky分解Sx(0)； 2)求輸出yk的一階差分

、Syv(1)和二階差分

(k)、Syv(2)(k)； (15) (16) 其中，

是

的第j列，Sv，j是Sv的第j列；步長設(shè)為 3)求輸出yk的計(jì)算值yk； (17) 其中nx＝3，nw＝3。
4)求輸出yk的協(xié)方差的Cholesky分解Sy(k)； 5)求濾波增益Kk和Pxy(k)； Kk＝Pxy(k)(Sy(k)Sy(k)T)-1(20) 6)求狀態(tài)x的預(yù)測
7)求協(xié)方差預(yù)測

的Cholesky分解
8)計(jì)算狀態(tài)預(yù)測

的一階差分

Sxw(1)和二階差分

和Ssw(2)； (23) (24) 9)求xk+1； (25) 其中，

是

的第p列，Sw，p是Sw的第p列； 10)求狀態(tài)xk+1的協(xié)方差Pk+1的Cholesky分解Sk+1； 11)把k增加到k+1從2)開始執(zhí)行。
(4)利用步驟(3)估計(jì)得到的失準(zhǔn)角對姿態(tài)矩陣Cbt進(jìn)行誤差補(bǔ)償，得到精確的姿態(tài)矩陣根據(jù)姿態(tài)矩陣得到載體姿態(tài)，完成初始對準(zhǔn)，進(jìn)入導(dǎo)航。
經(jīng)過插值濾波器濾波，估算出三個(gè)誤差失準(zhǔn)角φx、φy、φz，對矩陣Cbt進(jìn)行補(bǔ)償 其中 根據(jù)Cbt得到載體姿態(tài)，即縱搖角、橫搖角和航向角的主值如下


由上述三個(gè)主值判斷真值的公式如下 θ＝θ主(30)

至此，初始對準(zhǔn)完成，可以進(jìn)入導(dǎo)航狀態(tài)。
權(quán)利要求
1、一種基于二階插值濾波器的自主式水下潛器海上對準(zhǔn)方法，其特征是
(1)光纖陀螺捷聯(lián)慣導(dǎo)系統(tǒng)預(yù)熱后采集光纖陀螺儀和石英撓性加速度計(jì)輸出的數(shù)據(jù)，根據(jù)加速度計(jì)的輸出與重力加速度的關(guān)系以及陀螺儀輸出與地球旋轉(zhuǎn)角速率的關(guān)系初步確定載體坐標(biāo)系到計(jì)算導(dǎo)航坐標(biāo)系的轉(zhuǎn)換矩陣Cbc；
(2)建立光纖陀螺捷聯(lián)慣導(dǎo)系統(tǒng)在方位誤差角比較大、即大于5°時(shí)以速度誤差和三個(gè)誤差失準(zhǔn)角為狀態(tài)變量的非線性狀態(tài)方程和以速度誤差為觀測量的線性觀測方程；
(3)將非線性的連續(xù)系統(tǒng)離散化，用二階插值濾波器對離散的非線性系統(tǒng)進(jìn)行濾波，估計(jì)誤差失準(zhǔn)角；
(4)利用步驟(3)估計(jì)得到的誤差失準(zhǔn)角對矩陣Cbc進(jìn)行誤差補(bǔ)償，得到精確的姿態(tài)矩陣，根據(jù)姿態(tài)矩陣得到載體姿態(tài)，完成初始對準(zhǔn)，進(jìn)入導(dǎo)航。
2、根據(jù)權(quán)利要求1所述的基于二階插值濾波器的自主式水下潛器海上對準(zhǔn)方法，其特征是所述建立光纖陀螺捷聯(lián)慣導(dǎo)系統(tǒng)在方位誤差角比較大時(shí)以速度誤差和三個(gè)誤差失準(zhǔn)角為狀態(tài)變量的非線性狀態(tài)方程和以速度誤差為觀測量的線性觀測方程包括
非線性狀態(tài)方程為
其中
，中其它元素為0；
其中，ΔVx、ΔVy是慣導(dǎo)系統(tǒng)計(jì)算速度與真實(shí)速度的誤差在地理系的投影；φx、φy、φz為三個(gè)誤差失準(zhǔn)角；
為加速度計(jì)算系統(tǒng)偏差在地理坐標(biāo)系的投影；εx、εy、εz為x、y、z軸陀螺漂移；Vxt、Vyt是載體在地理坐標(biāo)系x、y軸的真實(shí)速度；L是當(dāng)?shù)鼐暥?；f′x、f′y為加速度計(jì)在x、y軸的讀數(shù)；ωie是地球旋轉(zhuǎn)角速率；
其中Re為赤道半徑，Rp為極軸半徑；e＝1/297；
為地理經(jīng)度；
線性觀測方程為z＝Cx+v
其中C＝[1 1 0 0 0 0 0 0 0 0]T，
將狀態(tài)方程和觀測方程離散化
xk+1＝f(xk，wk)
yk＝g(xk，vk)
其中xk∈R10，是10×1的狀態(tài)向量；yk∈R2，是2×1的觀測向量；wk∈R10，是10×1的狀態(tài)噪聲向量；vk∈R2，是2×1的觀測噪聲向量；根據(jù)AUV捷聯(lián)慣導(dǎo)系統(tǒng)動機(jī)座大失準(zhǔn)角非線性誤差方程，假設(shè)系統(tǒng)的狀態(tài)噪聲和觀測噪聲是不相關(guān)Gaussian自噪聲，它們的均值和協(xié)方差為
E(wk)＝wk，E{(wk-w)(wj-w)}＝Qk
E(vk)＝vk，E{(vk-v)(vj-v)}＝Rk
Sx，Sw和Sv分別是協(xié)方差預(yù)測
協(xié)方差Pk，狀態(tài)噪聲協(xié)方差均值Q和觀測噪聲協(xié)方差R的Cholesky分解，
Pk＝SxSxT，Q＝SwSwT，R＝SvSvT。
3、根據(jù)權(quán)利要求1或2所述的基于二階插值濾波器的自主式水下潛器海上對準(zhǔn)方法，其特征是用二階插值濾波器對離散的非線性系統(tǒng)進(jìn)行濾波，估計(jì)誤差失準(zhǔn)角的步驟如下
1)初始化狀態(tài)向量x的均值x0和協(xié)方差P0的Cholesky分解Sx(0)；
2)求輸出yk的一階差分
Syv(1)和二階差分
Syv(2)(k)，
其中，
是
的第j列，Sv，j是Sv的第j列；步長設(shè)為[7]
3)求輸出yk的計(jì)算值yk，
其中nx＝3，nw＝3；
4)求輸出yk的協(xié)方差的Cholesky分解Sy(k)，
5)求濾波增益Kk和Pxy(k)，
Kk＝Pxy(k)(Sy(k)Sy(k)T)-1；
6)求狀態(tài)x的預(yù)測
7)求協(xié)方差預(yù)測
的Cholesky分解
(k)，
8)計(jì)算狀態(tài)預(yù)測
的一階差分
和二階差分
9)求xk+1，
其中，
是
的第p列，Sw，p是Sw的第p列；
10)求狀態(tài)xk+1的協(xié)方差Pk+1的Cholesky分解Sk+1，
11)把k增加到k+1從2)開始執(zhí)行。
全文摘要
本發(fā)明提供的是一種基于二階插值濾波器的自主式水下潛器海上對準(zhǔn)方法。(1)采集光纖陀螺儀和石英撓性加速度計(jì)輸出的數(shù)據(jù)，初步確定載體坐標(biāo)系到計(jì)算導(dǎo)航坐標(biāo)系的轉(zhuǎn)換矩陣Cbc；(2)建立光纖陀螺捷聯(lián)慣導(dǎo)系統(tǒng)在方位誤差角比較大時(shí)以速度誤差和三個(gè)誤差失準(zhǔn)角為狀態(tài)變量的非線性狀態(tài)方程和以速度誤差為觀測量的線性觀測方程；(3)將非線性的連續(xù)系統(tǒng)離散化，用二階插值濾波器對離散的非線性系統(tǒng)進(jìn)行濾波，估計(jì)誤差失準(zhǔn)角；(4)利用步驟(3)估計(jì)得到的誤差失準(zhǔn)角對姿態(tài)矩陣Cbc進(jìn)行誤差補(bǔ)償，得到精確的姿態(tài)矩陣，根據(jù)姿態(tài)矩陣得到載體姿態(tài)，完成初始對準(zhǔn)，進(jìn)入導(dǎo)航。本發(fā)明可以達(dá)到較高適用精度的要求，對準(zhǔn)精度高，對準(zhǔn)時(shí)間短，實(shí)現(xiàn)簡單。
文檔編號G01C25/00GK101639365SQ20091007256
公開日2010年2月3日 申請日期2009年7月22日 優(yōu)先權(quán)日2009年7月22日
發(fā)明者偉 高, 趙桂玲, 鑫 張, 博 徐, 奔粵陽, 陳世同, 強(qiáng) 于, 高洪濤, 磊 吳, 偉 王 申請人:哈爾濱工程大學(xué)