[發(fā)明專利]一種水下組合導航系統(tǒng)行進間對準與姿態(tài)估計方法

在審

• 申請?zhí)枺?/td>: 202210415114.7
• 申請日: 2022-04-17
• 公開（公告）號: CN114777812A
• 公開（公告）日: 2022-07-22
• 發(fā)明（設計）人: 李鼎;于旭東;史文策;何泓洋;羅暉;吳苗;許江寧
• 申請（專利權）人: 中國人民解放軍國防科技大學
• 主分類號: G01C25/00
• 分類號: G01C25/00
• 代理公司: 湖南企企衛(wèi)知識產(chǎn)權代理有限公司 43257
• 代理人: 任合明

1.一種水下組合導航系統(tǒng)行進間對準與姿態(tài)估計方法，其特征在于，該方法分為以下步驟：

S1：將自主式水下航行器AUV的捷聯(lián)慣性導航系統(tǒng)SINS與多普勒測速儀DVL接入導航計算機，將AUV投放至任務區(qū)域；當AUV潛入水下，無法接收到GNSS信號時，通過傳統(tǒng)方法進行姿態(tài)粗對準；

S2：通過導航計算機接收到捷聯(lián)慣性導航系統(tǒng)中陀螺與加速度計輸出的角速度與加速度增量，利用導航計算機進行導航解算：

定義坐標系：選取“東—北—天”地理坐標系為導航坐標系，記為n系；選取“右—前—上”坐標系為載體坐標系，記為b系；地心慣性坐標系記為i系；地球坐標系記為e系；計算導航坐標系記為n′系；

S2.1執(zhí)行導航計算機計時器，初始化時刻k＝0；

S2.2計時器時間更新k＝k+1；

S2.3執(zhí)行慣性導航解算

通過陀螺輸出的采樣角增量和加速度計輸出的采樣比力速度增量解算得到k時刻的SINS姿態(tài)姿態(tài)矩陣速度和位置

S3：構建13維離散化的基于馬氏距離的VBRAKF濾波模型與觀測模型，具體步驟如下：

S3.1構建組合導航系統(tǒng)13維基于馬氏距離的VBRAKF的濾波模型：

S3.1.1執(zhí)行VBRAKF狀態(tài)一步預測更新為：

式(1)中，表示k-1時刻輸出的狀態(tài)向量X的后驗估計，表示狀態(tài)向量X從k-1時刻至k時刻的一步預測；X表示包含SINS姿態(tài)誤差速度誤差δv，位置誤差δP和陀螺漂移誤差ε^b、加速度零偏誤差的13維狀態(tài)向量，X的表達式為：

的初始值設置為0_1×13表示1×13維的零矩陣，上標T表示矩陣轉(zhuǎn)置；

式(2)中，位置誤差為δP＝[δL,δλ]，δL和δλ分別為SINS的緯度誤差與經(jīng)度誤差；速度誤差δv＝[δv_E,δv_N]，δv_E和δv_N分別為東向速度誤差和北向速度誤差；姿態(tài)失準角分別表示x軸、y軸和z軸方向的姿態(tài)失準角；陀螺漂移誤差為加速度計零偏誤差為

式(1)中，Φ_k|k-1為從k-1時刻到k時刻的狀態(tài)一步預測矩陣，計算公式為：

Φ_k|k-1≈I+F(k-1)T_s (3)

式(3)中，I為13維單位矩陣，T_s為離散化時間間隔，F(xiàn)(k-1)為k-1時刻的SINS系統(tǒng)模型矩陣；F(k-1)的表達式為：

其中0_i×j為i×j維的零矩陣，F(xiàn)_ij為子矩陣，各子矩陣的表達式為：

式(5)-式(14)中，R_e為地球半徑，v_E(k-1)為k-1時刻SINS的東向速度，v_N(k-1)為k-1時刻SINS的北向速度，L(k-1)為k-1時刻SINS的緯度，ω_ie為地球自轉(zhuǎn)角速度，f_U(k-1)表示k-1時刻加速度計輸出的比力在天向的投影，f_N(k-1)表示k-1時刻加速度計輸出的比力在北向的投影，f_E(k-1)表示k-1時刻加速度計輸出的比力在東向的投影，表示k-1時刻導航計算機解算得到的姿態(tài)矩陣，表示姿態(tài)矩陣的前兩行；

S3.1.2執(zhí)行VBRAKF狀態(tài)一步預測均方誤差矩陣更新：

式(15)中，P_k-1|k-1稱為k-1時刻的狀態(tài)后驗估計均方誤差矩陣，P_k|k-1稱為從k-1時刻到k時刻的狀態(tài)一步預測均方誤差矩陣，利用式(15)，狀態(tài)后驗估計均方誤差矩陣P_k-1|k-1更新成為狀態(tài)一步預測均方誤差矩陣P_kk-1；

P_k-1|k-1初始值P_0|0為：

其中diag表示矩陣對角線元素，μg為加速度計零偏誤差單位，1μg＝10^-6g≈9.8×10^-6m/s²，13×13矩陣其他元素為0；

Q(k-1)稱為k-1時刻的系統(tǒng)噪聲矩陣，是一個13×13維的對角矩陣，由SINS陀螺和加速度計的隨機誤差決定；由于SINS誤差特性穩(wěn)定，因此系統(tǒng)噪聲矩陣是恒定的，即Q(k-1)＝Q(0)，初始值為：

S3.2將導航計算機SINS解算速度減去DVL輸出的觀測速度在計算導航系下的投影速度作為k時刻的觀測量Z(k)，構建VBRAKF觀測模型：

式(18)中，為k時刻DVL輸出的載體坐標系下的速度；

式(18)中第二至第六行是根據(jù)觀測量Z(k)構建觀測模型，為k時刻SINS真實的姿態(tài)矩陣，為k時刻的理想導航坐標系至計算導航坐標系間的姿態(tài)矩陣，即k時刻的姿態(tài)誤差矩陣的逆矩陣，表示k時刻的姿態(tài)誤差，×為叉乘符號，表示的反對稱矩陣，其表達式為：

式(18)中，vⁿ(k)為k時刻SINS真實的速度，δvⁿ(k)為k時刻SINS真實速度誤差；

Z(k)＝H(k)X(k)+V(k)為組合導航系統(tǒng)的觀測模型，H(k)為k時刻的觀測矩陣，其表達式為：

式(20)中，I_2×2為2×2的單位矩陣，0_2×2為2×2的零矩陣；實際式(20)計算過程中，用代替

V(k)為k時刻的觀測噪聲矩陣，由k時刻的東向速度觀測噪聲V_E(k)和北向速度觀測噪聲V_N(k)組成，其表達式為：

設R_E(k),R_N(k)分別為k時刻的東向、北向速度觀測噪聲方差，滿足如下條件：

E[·]表示求其期望值；觀測噪聲V(k)的協(xié)方差矩陣為：

觀測噪聲協(xié)方差矩陣R(0)的初始值為：

R(0)＝diag{(0.1m/s)²,(0.1m/s)²} (24)

S4：構建基于馬氏距離的VBRAKF魯棒自適應模塊，進行VBRAKF狀態(tài)估計更新：

S4.1將k時刻觀測噪聲協(xié)方差矩陣R(k)的先驗分布選擇為逆Wishart分布：

將觀測噪聲協(xié)方差矩陣R(k)的先驗分布選擇為逆Wishart分布，其表達式為：

p(R(k)|Z(1:(k-1)))＝IW(R(k)；u_k|k-1,U_k|k-1) (25)

式(25)中，p(R(k)|Z_1:k-1)為觀測噪聲協(xié)方差矩陣R(k)的先驗分布，Z(1:(k-1))為[1,k-1]時間段內(nèi)所有的觀測量，p(R(k)|Z(1:(k-1)))具體表示在[1,k-1]內(nèi)所有的觀測量先驗已知下，對k時刻觀測噪聲協(xié)方差矩陣R(k)的概率分布作估計；IW(R(k)；u_k|k-1,U_k|k-1)稱為R(k)的概率分布為逆Wishart分布，u_k|k-1稱為逆Wishart分布的自由度參數(shù)，U_k|k-1稱為逆Wishart分布的逆尺度矩陣；

S4.2更新逆Wishart分布的自由度參數(shù)與逆尺度矩陣

通過k-1時刻輸出的后驗估計u_k-1|k-1和U_k-1|k-1更新u_k|k-1和U_k|k-1：

U_k|k-1＝ρU_k-1|k-1 (26)

u_k|k-1＝ρ(u_k-1|k-1-m-1)+m+1 (27)

式(26)、(27)中ρ表示變分衰減因子；

其中，逆Wishart分布U_k-1|k-1的初值U_0|0：

U_0|0＝(u_0|0-m-1)R(0) (28)

初值u_0|0＝10；

S4.3構建基于馬氏距離的魯棒化模塊，計算k時刻的新息e(k)的馬氏距離，并計算膨脹因子ω(k)；新息表示觀測量Z(k)與狀態(tài)一步預測量之間的差，膨脹因子ω(k)用來衡量觀測量Z(k)被污染的程度：

S4.3.1計算新息e(k)馬氏距離：

式(29)中M(k)為k時刻新息e(k)的馬氏距離，馬氏距離公式中間部分利用的是上一時刻估計得到的近似為k時刻的觀測噪聲矩陣R(k)；

S4.3.2通過S4.3.1得到的馬氏距離判斷觀測量Z(k)是否為野值，計算膨脹因子ω(k)以衡量觀測量Z(k)被污染的程度；

具體判斷方法為：

選擇統(tǒng)計門限函數(shù)對于觀測量Z(k)，若S4.3.1計算的馬氏距離滿足M(k)²≤η²，則Z(k)被標記為正常觀測量，此時膨脹因子為ω(k)＝1；

如果馬氏距離滿足M(k)²＞η²，則Z(k)被標記為野值；此時，膨脹因子ω(k)＞1，通過牛頓迭代法求解膨脹因子ω(k)，公式為：

式(30)中，上標(j)表示牛頓迭代次數(shù)，當前時刻觀測噪聲協(xié)方差陣R(k)利用上一時刻估計值近似，即膨脹因子迭代的初值ω(k)⁽⁰⁾＝1；每次迭代后的均方誤差矩陣：

S4.4構建基于馬氏距離的變分貝葉斯魯棒自適應模塊，通過變分貝葉斯方法進行固定點迭代，估計觀測噪聲協(xié)方差矩陣及狀態(tài)向量：

利用式(1)、(15)、(26)、(27)得到的P_k|k-1，U_k|k-1，u_k|k-1初始化固定點迭代：

中上標(0)表示當前固定點迭代次數(shù)為0，第i次固定點迭代近似的參數(shù)表示形式為

變分貝葉斯固定點迭代估計的步驟為：

S4.4.1計算第i次迭代變分貝葉斯估計均方誤差矩陣B(k)⁽ⁱ⁾，公式為：

S4.4.2計算第i+1次迭代逆尺度矩陣近似估計觀測噪聲協(xié)方差矩陣

S4.4.3計算第i+1次迭代的濾波增益K(k)⁽ⁱ⁺¹⁾

S4.4.4計算第i+1次迭代近似的狀態(tài)后驗估計以及狀態(tài)后驗估計均方誤差陣

S4.4.5循環(huán)執(zhí)行S4.4.1-S4.4.4，當i＝N-1時，結(jié)束迭代；其中，N代表迭代次數(shù)；輸出k時刻迭代計算完成的狀態(tài)向量狀態(tài)后驗估計均方誤差矩陣觀測噪聲協(xié)方差矩陣逆尺度矩陣

輸出的狀態(tài)向量是通過VBRAKF得到的k時刻SINS解算導航信息包含的誤差，包括k時刻AUV的姿態(tài)誤差位置誤差δP(k)，速度誤差δv(k)，陀螺漂移誤差ε^b(k)和加速度計零偏誤差

S5：構建導航誤差反饋補償回路，得到k時刻AUV的姿態(tài)、位置、速度在任意時刻的修正值，并進入k+1時刻循環(huán)執(zhí)行S2.2-S4.2，直到導航結(jié)束：

S5.1將S3.3.6得到的濾波估計誤差值反饋補償給S2得到導航解算結(jié)果：

S5.1.1將k時刻姿態(tài)誤差轉(zhuǎn)換為姿態(tài)誤差矩陣

其中，簡記三角函數(shù)由S4.4得到；

S5.1.2將S5.1.1得到的姿態(tài)誤差矩陣S4.3得到的速度誤差δv(k)與位置誤差δP(k)反饋修正給S2.3得到的SINS解算姿態(tài)位置與速度具體方法如下：

式(39)中，表示的轉(zhuǎn)置矩陣；通過式(39)-(41),即得到了k時刻導航修正值，包括高精度的姿態(tài)速度和位置

S5.2進入下一時刻，循環(huán)執(zhí)行S2.2-S5.2，得到k+1時刻的高精度姿態(tài)速度和位置直到導航結(jié)束。