基于自適應魯棒濾波的SINS/DVL動基座初始對準方法

肖 強，文篤石，段明磊，楊 童，張成林

(1. 云南公路聯網收費管理有限公司，云南 昆明 650100; 2. 西安郵電大學計算機學院，陜西 西安 710000)

0 引 言

隨著慣性器件的發展，捷聯慣導系統（strapdown inertial navigation system, SINS）在民用和軍用等方面的應用越來越廣泛[1-2]。捷聯慣導和多普勒測速儀（Doppler velocimeter, DVL）組合的導航系統在水下設備中應用成熟。水下的自主導航對初始姿態的精度要求非常高，如何獲得一個高精度的初始姿態是一個熱點課題。

文獻[3]提出了在靜基座下的解析對準方法，通過加速度計和陀螺儀的量測值和雙矢量定姿方法[4]獲得姿態。為了解決在晃動基座上的對準問題，文獻[5]提出了坐標系凝固的思路，將姿態陣分解為三個矩陣，其中兩個由慣性凝固的坐標系確定，另一個是初始時刻的姿態陣，并通過雙矢量定姿方法確定姿態，但是該方法對量測信息的利用率不高。文獻[6]提出了基于最優化的對準方法，將對準問題轉化為連續姿態確定問題，通過構造包含所有測量信息的對準矩陣獲得當前姿態。但這些方法都是基于晃動基座的，為了能夠在對準時保持載體的高機動性，需要研究基于動基座的對準方法。文獻[7]提出了交錯矢量的方法抑制儀表的累積誤差，文獻[8]推導了嚴格速度、位置積分公式解決儀表不同頻采樣的問題。文獻[9-10]基于積分公式，推導了位置軌跡積分公式和滑動窗位置軌跡積分公式，提高了SINS/GNSS對準的魯棒性。這些基于SINS/GNSS的對準方法在水下會因衛星信號受限而無法使用。基于嚴格積分公式，文獻[11]推導了SINS/DVL的積分公式，并研究了精對準過程。文獻[12]基于逆向導航原理，提高了對準的速度。但是這些方法都未考慮DVL信息異常值干擾的問題。文獻[13]提出了一種抑制DVL異常值噪聲的魯棒對準方法，利用模值匹配技術檢測異常噪聲、構造魯棒濾波權值，通過魯棒四元數濾波器確定姿態，提高了對準的魯棒性。文獻[14]提出了一種基于參數估計的方法實現DVL輔助行進間對準，通過自適應噪聲削弱了DVL噪聲的影響，提高了對準的穩定性，但對DVL異常值檢測不足，魯棒性不強。文獻[15]基于矢量重構技術，通過重構觀測矢量隔離了DVL異常干擾，提高了對準精度。但是魯棒濾波器對DVL的量測噪聲敏感，無法動態跟蹤噪聲，從而降低了對準的精度。

為解決文獻[15]方法中的問題，本文提出基于自適應噪聲的魯棒濾波技術動態跟蹤DVL量測噪聲，通過自適應量測噪聲削弱傳統魯棒濾波對噪聲的敏感度，優化參數估計過程，實現提高矢量重構精度的目的，進而提高粗對準的精度。

1 坐標系定義

1)i系是以地球為中心的慣性系，x軸在赤道平面內且指向春分點，z軸指向地球自轉方向，三軸構成右手坐標系；

2)e系是以地球為中心的地球坐標系，x軸在赤道平面內且指向中央子午線，z軸指向地球自轉方向，三軸構成右手坐標系；

3)n系是導航坐標系，x、y、z分別指向的東、北、天方向；

4)b系是載體坐標系，x、y、z分別指向載體的右、前、上方向；

5)n0系是慣性凝固導航系，與對準初始時刻的n系一致，并相對于i系無轉動；

6)b0系是慣性凝固載體系，與對準初始時刻的b系重合，并與慣性系之間無轉動。

2 DVL輔助嚴格積分公式

3 基于自適應魯棒濾波的矢量重構方法

首先研究了觀測矢量的誤差模型，然后分析參考矢量的解析式構成，確定了觀測矢量的重構模型，接著提出了自適應魯棒濾波方法估計參數，提高了參數估計的精度，最后給出了粗對準的算法流程。

3.1 觀測矢量誤差模型

3.2 觀測矢量重構模型

3.3 自適應魯棒濾波

本文方法較文獻[15]的方法的優勢就在于自適應噪聲的構造。通過重構模型計算觀測矢量，并根據計算的觀測矢量和測量的觀測矢量構造自適應量測噪聲，提高濾波的魯棒性。

3.4 算法原理與總結

本文方法設計了自適應量測噪聲的魯棒濾波器估計矢量重構所需的參數，解決了傳統方法在參數估計方面的不足：對量測信息的噪聲敏感，無法動態跟蹤量測噪聲，從而降低了參數估計的精度，進而影響到粗對準的精度和速度。流程如表1所示。

表1 本文對準方法的流程總結

4 仿真實驗與分析

簡記表23種方法。

表2 對準算法和測試條件

圖1 運動姿態和軌跡信息

圖2 DVL輸出值

圖3是3種方法的觀測矢量重構的結果。從圖中可以看到，DVLR1和DVLAR基本重合，說明在DVLR1方法中的魯棒濾波估計參數時，當量測噪聲參數R準確時，參數估計得到的重構矢量和DVLAR的自適應濾波一致，而DVLR2因其R小于實際儀器誤差，所以在估計參數時出現了不足，導致重構的觀測矢量出現如圖3的誤差，其中x軸出現了漂移誤差，y軸在 0 ～100s出現一個向下的誤差波動。

圖3 觀測矢量重構

圖4是3種方法的濾波器量測噪聲參數。DVLR1和DVLR2是傳統的濾波方法，量測噪聲是一個定值，而DVLAR方法是一個動態變化的自適應噪聲。隨著對準的進行，DVLAR的自適應噪聲從初始的較大值逐漸收斂至一個自適應值，也正是這個自適應噪聲參數使得DVLAR方法可以動態跟蹤DVL的噪聲，削弱了傳統方法[15]對儀表噪聲的敏感度，提高了參數估計的準確性，使得觀測矢量重構精度更高，對準的魯棒性更強，對準精度更高。

圖4 濾波器量測噪聲

圖5是三種方法的對準誤差對比圖。圖5(a)中俯仰角對準，DVLR1和DVLAR在100 s內達到收斂精度并達到穩定，DVLR2用了300 s左右才達到和另兩種方法相當的精度，對準速度慢了兩倍。文獻[15]的傳統方法因其對DVL噪聲敏感，使得DVLR2在R與DVL實際噪聲不匹配時表現了和DVLR1方法不同的性能。對準600 s，DVLR1的誤差 為 - 8.886×10-4°，DVLR2誤差 為 - 6.245×10-3°，DVLAR為 - 6.085×10-4°，DVLAR比DVLR2準確度提高了90%。

圖5 對準誤差

從圖5(b)橫滾角誤差中可以看到，DVLR2方法進行到120 s才達到收斂，而DVLR1和本文方法DVLAR在30 s左右就達到收斂精度，對準速度提高了75%。并且隨著對準過程進行到400 s，DVLR2方法開始出現一個向下的誤差漂移，精度下降。600 s對準結束時刻，DVLR1誤差為-9.298×10-4°，DVLR2誤 差 為 - 9.251×10-2°，DVLAR為-1.301×10-3°，DVLAR較DVLR2誤差降低了98%。

圖5(c)航向角對準中，DVLR1和DVLAR都在不到200 s處達到收斂，但是DVLR2方法用了近500 s的時間才達到和DVLR1和DVLAR方法相當的精度，DVLAR方法對準速度較DVLR2提高了60%。600 s對準DVLR1誤差為 - 1.969×10-2°，DVLR2為0 . 1507°，DVLAR為- 3.138×10-2°，DVLAR相比于DVLR2準確度提高了79%。

DVLAR方法因自適應噪聲的魯棒濾波技術估計參數重構矢量，增強了對DVL測量噪聲的動態跟蹤能力，弱化了文獻[15]傳統方法對DVL噪聲的敏感度，提高了粗對準的速度和精度。

從表3可以看出，在對DVL的噪聲設置不準確時，傳統方法DVLR2的均值和標準差誤差都明顯大于DVLAR，并且設置準確的DVLR1和DVLAR的誤差相當。從誤差的降低率可以看到，DVLAR方法要顯著優于DVLR方法[15]。DVLAR方法得益于動態跟蹤DVL的測量噪聲，使得參數估計更準確，重構的觀測矢量更接近真實值，從而粗對準的速度更快，精度更高。

表3 航向角誤差統計

基于自適應噪聲的本文方法也更適合實際使用，在組合導航中可能存在DVL多路徑效應和環境惡劣等意外情況，同時傳統方法需要根據測速儀器件設置測量噪聲，而本文方法無需該參數設置，使用簡單，采用算法進行自適應噪聲更新，動態跟蹤測量噪聲，魯棒性更強，抗干擾能力顯著優于傳統方法。

5 結束語

針對傳統的基于矢量重構的動基座魯棒對準方法對DVL測量噪聲敏感的問題，本文研究一種基于自適應噪聲魯棒濾波技術的動基座魯棒粗對準方法。通過對魯棒濾波估計觀測矢量重構參數的過程構造自適應噪聲，實現動態跟蹤DVL測量噪聲，削弱傳統魯棒濾波技術對DVL測量噪聲的敏感度，改善矢量重構的估計精度，提高動基座粗對準的速度和精度。仿真實驗結果表明，本文動基座魯棒對準方法較傳統方法具有更好的魯棒性，對準更穩定，精度更高，速度更快。