自主水下航行器多普勒計程儀安裝誤差的校正方法
【專利摘要】本發(fā)明公開自主水下航行器多普勒計程儀安裝誤差的校正方法。1）數(shù)據(jù)采集：獲取AUV在水面時的東北向位置信息、深度信息、速度信息、姿態(tài)信息；2）濾波估計：根據(jù)帶誤差校正的DVL航位推算模型，取AUV三維位置信息及DVL三維安裝偏角為系統(tǒng)狀態(tài)向量，DVL測得速度信息及姿態(tài)傳感器測得姿態(tài)信息為系統(tǒng)輸入向量，取AUV三維位置信息為測量向量，建立離散時間系統(tǒng)狀態(tài)方程和測量方程；采用改進的平方根容積卡爾曼濾波進行濾波估計，得到DVL安裝偏角的估計值；3）誤差校正：根據(jù)安裝偏角的估計值得到DVL安裝誤差校正矩陣。本發(fā)明操作簡單，無需外部設(shè)備輔助，可有效估計出DVL的三維安裝偏差角，從而消除DVL安裝誤差對AUV導(dǎo)航精度的影響，具有很強的實用價值。
【專利說明】自主水下航行器多普勒計程儀安裝誤差的校正方法
【技術(shù)領(lǐng)域】
[0001]本發(fā)明涉及自主水下航行器(Autonomous Underwater Vehicle, AUV)導(dǎo)航儀器校準技術(shù)，屬于海洋工程領(lǐng)域。
【背景技術(shù)】
[0002]自主水下航行器是無纜連接的水下無人自主航行器平臺，在裝載合適的聲納、水文、化學(xué)等傳感器后可應(yīng)用于水下環(huán)境監(jiān)測、近海石油工程作業(yè)、水下搜索與測繪等領(lǐng)域。它具有控制靈活、航行面積廣闊、價格低廉等特點，是近年來海洋工程領(lǐng)域研究的熱點之
O
[0003]水下導(dǎo)航技術(shù)是實現(xiàn)水下航行器自主航行的關(guān)鍵，但是不同于傳統(tǒng)導(dǎo)航系統(tǒng)，水下導(dǎo)航技術(shù)具有海洋環(huán)境復(fù)雜、可獲取的外界信息少、系統(tǒng)功耗/體積制約等特點，具有極大的挑戰(zhàn)性。常用的水下導(dǎo)航傳感器有多普勒計程儀(Doppler Velocity Log, DVL)、電子羅盤、慣性導(dǎo)航系統(tǒng)、深度傳感器等，DVL利用安裝在AUV載體上的聲學(xué)換能器陣發(fā)射聲波，利用多普勒原理測量AUV對于水底或者水層/水面的相對速度，具有精度高的特點，是水下航行器導(dǎo)航系統(tǒng)的關(guān)鍵設(shè)備 。
[0004]DVL測得的速度信息是相對于自身定義的儀器坐標(biāo)系下的速度，在使用DVL測量的速度進行導(dǎo)航時，必須要經(jīng)過坐標(biāo)轉(zhuǎn)換，將其轉(zhuǎn)換到導(dǎo)航坐標(biāo)系下，這一過程通常需利用姿態(tài)傳感器測得的載體姿態(tài)信息。若DVL儀器坐標(biāo)系與載體坐標(biāo)系不存在安裝偏角，則坐標(biāo)轉(zhuǎn)換過程不會導(dǎo)致測量速 度的偏差。然而實際工程應(yīng)用中，DVL安裝誤差通常難以避免，其安裝偏角會導(dǎo)致速度轉(zhuǎn)換誤差，進而直接影響系統(tǒng)整體的導(dǎo)航精度，且隨著航行時間的增加會產(chǎn)生累計誤差，因此 有 必要對DVL的安裝誤差進行校正，以提高AUV自主導(dǎo)航精度?，F(xiàn)有的安裝誤差校正方法中平均速度校正法應(yīng)用比較廣泛，但是該方法忽略了橫滾和縱傾安裝偏角的影響，同時要求校正過程中，載體航行姿態(tài)保持不變并進行長距離直線航行。

【發(fā)明內(nèi)容】

[0005]本發(fā)明的目的在于針對現(xiàn)有技術(shù)的不足，提供自主水下航行器多普勒計程儀安裝誤差的校正方法。
[0006]自主水下航行器多普勒計程儀安裝誤差的校正方法，步驟如下:
[0007]I)數(shù)據(jù)采集:利用全球定位系統(tǒng)獲取AUV在水面時的東北向位置信息，深度計獲取AUV的深度信息，利用DVL采集AUV的速度信息，利用姿態(tài)傳感器獲取姿態(tài)信息；
[0008]2)濾波估計:根據(jù)帶誤差校正的DVL航位推算模型，取AUV三維位置信息及DVL三維安裝偏角為系統(tǒng)狀態(tài)向量，DVL測得速度信息及姿態(tài)傳感器測得姿態(tài)信息為系統(tǒng)輸入向量，取AUV三維位置信息為測量向量，建立離散時間系統(tǒng)狀態(tài)方程和測量方程；采用改進的平方根容積卡爾曼濾波進行濾波估計，得到DVL安裝偏角的估計值；
[0009]3)誤差校正:根據(jù)安裝偏角的估計值得到DVL安裝誤差校正矩陣，用于AUV導(dǎo)航系統(tǒng)校準，提高AUV導(dǎo)航精度。[0010]步驟2)中，所述的帶誤差校正的DVL航位推算模型推導(dǎo)如下:
[0011]根據(jù)載體坐標(biāo)系與DVL儀器坐標(biāo)系的轉(zhuǎn)換關(guān)系，存在安裝偏差時，DVL測量速度的校正公式可表示為:
[0012]
[0013]其中，Vdy <]T為DVL測量得到的三維速度，Vby Vz6]T為載
體速度，<(π)為安裝誤差校正矩陣，即DVL儀器坐標(biāo)系向載體坐標(biāo)系轉(zhuǎn)換的旋轉(zhuǎn)矩陣，Π = [α β y]τ為儀器坐標(biāo)系相對于載體坐標(biāo)系的橫滾、縱傾和航偏三維偏角，當(dāng)DVL和姿態(tài)傳感器安裝固定后，三維偏角為固定值，安裝誤差校正矩陣K (Π)為:
[0014]
cos;/ cos β - sin / cos a + cos y sin β?ηα sin ysin a + cos ycos β sin aR (Tl) = sin/cos/i cos/cos or + sin/sin /isin a - cos;/ sin a + sin y sin β cos a {^2 )
-sin βcos/7sin ^cos/7 cos a
[0015]根據(jù)三維姿態(tài)信息，將載體移動速度投影到導(dǎo)航坐標(biāo)系，在前一刻推算得到的位置信息上進行累加得到下一時刻的推算位置信息，得到帶誤差校正的DVL航位推算模型數(shù)學(xué)表不為:
[0016]Yn = Rim-^b(3)
[0017]Pw(k) = p,!(k -\) + R；； (?(k -\))-R1J- v"(k-\).At(4)
[0018]其中，pn(k) = [xyz]T為k時刻推算得到的三維位置信息，Δ t為航位推算時間間隔，Θ = [φ θ ψ]τ為姿態(tài)傳感器測得的載體橫滾、縱傾和航偏角信息，&Β(Θ)為載體坐標(biāo)系
向?qū)Ш阶鴺?biāo)系轉(zhuǎn)換的旋轉(zhuǎn)矩陣，表示為:
[0019]
cosi//cos (9 -sin ψ cos ^ + cosi// sin ^sin ^si η i/ s i η φ + cos y/ cos ?9 s i η ^
= sin cos (9 cos ψ cos ^ + siη ψ sim9 sin φ- cos ψ sin ^ + sin i// sin Θ cos φ OJ
-sin Θ cos ^sin ^cos/9 cos ^
[0020]步驟2)中，所述的離散時間系統(tǒng)狀態(tài)方程為:
[0021 ] xk=f (Xh，Uh, Ii^1)(6)
[0022]其中XH=IiXyzaβ Y；Hk-l時刻系統(tǒng)狀態(tài)向量，Uw =[多 θ ψ vdx vdy vf]
Sk-1時刻系統(tǒng)輸入向量，IV1 Sk-1時刻系統(tǒng)噪聲向量，為零均值不相關(guān)高斯白噪聲，協(xié)方差矩陣為Qlri，對應(yīng)的系統(tǒng)狀態(tài)變量誤差方程為:
[0023][x y ^ RK?)-R1'-yd(7)
[0024][? β fj =[0 O Of(8)
[0025]選取AUV三維位置信息為測量向量，則離散化的系統(tǒng)測量方程為:
【權(quán)利要求】
1.一種自主水下航行器多普勒計程儀安裝誤差的校正方法，其特征在于，步驟如下: 1)數(shù)據(jù)采集:利用全球定位系統(tǒng)獲取AUV在水面時的東北向位置信息，深度計獲取AUV的深度信息，利用DVL采集AUV的速度信息，利用姿態(tài)傳感器獲取姿態(tài)信息； 2)濾波估計:根據(jù)帶誤差校正的DVL航位推算模型，取AUV三維位置信息及DVL三維安裝偏角為系統(tǒng)狀態(tài)向量，DVL測得速度信息及姿態(tài)傳感器測得姿態(tài)信息為系統(tǒng)輸入向量，取AUV三維位置信息為測量向量，建立離散時間系統(tǒng)狀態(tài)方程和測量方程；采用改進的平方根容積卡爾曼濾波進行濾波估計，得到DVL安裝偏角的估計值； 3)誤差校正:根據(jù)安裝偏角的估計值得到DVL安裝誤差校正矩陣，用于AUV導(dǎo)航系統(tǒng)校準，提高AUV導(dǎo)航精度。
2.根據(jù)權(quán)利要求1所述的校正方法，其特征在于，步驟2)中，所述的帶誤差校正的DVL航位推算模型推導(dǎo)如下: 根據(jù)載體坐標(biāo)系與DVL儀器坐標(biāo)系的轉(zhuǎn)換關(guān)系，存在安裝偏差時，DVL測量速度的校正公式可表示為:
3.根據(jù)權(quán)利要求1所述的校正方法，其特征在于，步驟2)中，所述的離散時間系統(tǒng)狀態(tài)方程為:
xk-f (xk-!, uk_1? nk—J(6 )其中 Xh= [X y Z α β YiHk-1 時刻系統(tǒng)狀態(tài)向量，Uw =[0 θ ψ Vix v'l, vd_ ]TSk-1時刻系統(tǒng)輸入向量，Iv1 Sk-1時刻系統(tǒng)噪聲向量，為零均值不相關(guān)高斯白噪聲，協(xié)方差矩陣為Qlri，對應(yīng)的系統(tǒng)狀態(tài)變量誤差方程為:
4.根據(jù)權(quán)利要求1所述的校正方法，其特征在于，步驟2)中，所述的改進的平方根容積卡爾曼濾波流程具體步驟如下: 1)設(shè)定初始參數(shù) 設(shè)定初始狀態(tài)值Xtl,初始協(xié)方差矩陣P。； 2)時間更新 首先要計算基本的容積點和對應(yīng)的權(quán)值，使用三階容積原則獲得基本容積點和對應(yīng)權(quán)值為
5.根據(jù)權(quán)利要求1所述的校正方法，其特征在于，步驟I)中，所述的DVL采集到的所述速度信息包括前進速度信息、橫向速度信息、垂直速度信息中的一種或多種。
6.根據(jù)權(quán)利要求1所述的校正方法，其特征在于，步驟I)中，所述的姿態(tài)信息包括航偏角信息、橫滾角信息、縱傾角信息中的一種或幾種。
7.根據(jù)權(quán)利要求1所述的校正方法，其特征在于，步驟I)中，所述的東北向位置信息來自于經(jīng)度信息和緯度信息。
【文檔編號】G01C25/00GK103697910SQ201310689696
【公開日】2014年4月2日 申請日期:2013年12月14日 優(yōu)先權(quán)日:2013年12月14日
【發(fā)明者】李建龍, 孫晨, 徐文, 徐元欣 申請人:浙江大學(xué)