雙軸旋轉慣導系統轉軸傾角在線標定方法

陳 剛，劉建春，孫偉強，王俊柱，趙鳳昊

（天津航海儀器研究所，天津 300131）

雙軸旋轉慣導系統通過雙軸旋轉有效地調制了慣性元件的誤差[1-3]，相比捷聯慣性導航系統，大幅提高了長時間自主導航能力。雙軸旋轉慣導系統的機械編排與捷聯慣導系統的不同之處在于雙軸旋轉慣導系統的慣性解算得到的姿態進行解耦后才可以得到載體姿態[4]。轉軸傾角將會在雙軸旋轉慣導系統姿態解耦過程中，引入一個隨系統雙軸旋轉波動的姿態誤差。此姿態誤差在未補償轉軸傾角時，波動幅值在幾角分到十幾角分，因此標定補償系統轉軸傾角對保證姿態精度十分必要。劉沖研究了雙軸旋轉慣導系統轉軸傾角標定方法，其要求將系統置于實驗室內的靜基座上，然后通過觀察系統靜態下的姿態輸出隨雙軸旋轉的波動，標定出轉軸傾角的大小[5]。但是如果雙軸旋轉慣導系統在碼頭系泊時更換慣組，使用此方法難以標定出轉軸傾角。鑒于此，本文提出一種系泊狀態下轉軸傾角標定辦法。

1 轉軸傾角的誤差傳播特性

1.1 轉軸傾角的定義

首先定義本文數學推導過程中所涉及的坐標系。定義導航坐標系n 系為地理坐標系。由于旋轉機構兩個旋轉軸經過結構工裝標定后近似正交，定義旋轉機構坐標系r 為以旋轉機構內軸指向為y軸，外軸指向為z軸，構成的右手正交坐標系，并由定義可知r系會隨旋轉機構一起轉動。

記待補償系統計算導航坐標系為 n1系，慣組坐標系為p 系，載體坐標系為b 系，未補償轉軸傾角時由p系對兩個軸的轉角進行姿態解耦后得到的坐標系為b1系。記參考系統計算導航坐標系為 n1′系。設當兩個軸的轉角均為零時，b系與r系重合。

定義雙軸旋轉慣導系統的轉軸傾角為r系相對于p系的歐拉角。本文設雙軸旋轉慣導系統的轉軸傾角，圖1為轉軸傾角示意圖，圖中Ox′、rOyr′ 分別為坐標軸Oxr、Oyr在面Oxp yp的投影，Ozr′為坐標軸Ozr在面Oyp zp的投影。

圖1 轉軸傾角示意圖Fig.1 Schematic of the fixing error angles between axes and IMU

1.2 轉軸傾角引起姿態角波動的數學模型

待補償雙軸旋轉慣導系統的姿態誤差可分為兩部分，由轉軸傾角引起的姿態波動φ和由慣性元件誤差引起的姿態誤差角φn，其中，φ為 b1系相對于b系的姿態波動，對應的姿態誤差矩陣為系相對于n 系的姿態誤差，對應的姿態誤差矩陣為參考系統的姿態誤差為由慣性元件誤差引起的 n1′系相對于n 系的姿態誤差角φn′，其對應的姿態誤差矩陣為

雙軸旋轉慣導系統姿態矩陣的解算過程可分兩步。首先通過慣性元件的輸出，更新p 系相對于 n1系的姿態矩陣然后在的基礎上根據旋轉機構測角元件讀角及裝訂的轉軸傾角解算得到。若不補償轉軸傾角，則在第二步解算中將轉軸傾角設為這將會在解算結果中引入姿態波動φ，從而解算得到姿態矩陣

設雙軸旋轉過程中系統內框、外框分別相對初始位置依次繞其轉軸逆時針轉角為內、外轉軸上的測角元件輸出分別為α1、β1，測角噪聲分別為有：

并且設

使p 系依次繞其z軸和x軸轉動θz、θx角度得到p1系，其y軸與r系的y軸重合，從而為補償旋轉機構繞y軸的轉角α創造條件。在 p1系與r系y軸重合的基礎上，繼續繞其y軸轉動θy角度，得到 p2系，與r系完全重合，從而為補償旋轉機構繞z軸的轉角β創造條件。記根據以上分析，補償轉軸傾角時，姿態矩陣Cbn1的

計算過程可以表示為：

當

由式(2)(3)得φ對應的姿態誤差矩陣為：

由于θ各元素均為小量，從而θ可以看作從p系

式(4)轉換為四元數連乘的形式為[6]：轉動到r 系的等效旋轉矢量[7]。

其中，

將式(6)代入式(5)中可以推導出：

式(7)與式(8)對應元素相等，可以得出：

將式(1)代入式(9)可以得到轉軸傾角引起的姿態波動的數學模型為：

其中，

1.3 待補償系統與參考系統的姿態差數學模型

本文以待補償系統與參考系統的姿態差作為觀測量標定待補償系統的轉軸傾角。設參考系統與待補償系統同剛性基座安裝，已補償轉軸傾角，并且兩套系統均已正確補償姿態零位。設由時間同步誤差引起的兩套系統之間姿態差為φts，對應的姿態矩陣為Cφts，參考系統的姿態零位為φ0，對應的姿態矩陣為Cφ0。

待補償系統輸出姿態為 b1系相對于 n1系的姿態，其對應的姿態矩陣可以表示為：

參考系統輸出姿態為b系相對于系的姿態，其對應的姿態矩陣可以表示為：

設兩套系統輸出姿態差為φb，其對應的姿態誤差矩陣可以表示為

由式(11)～(13)可得：

由于φb、φn、φn′、φts、φ均為小量，從而式(14)可以表示為：

展開并略去二階小量得到：

對φb進行微分得待補償系統與參考系統的姿態差的微分方程為

由式(16)可得，參考系統的姿態零位誤差在可以被看作小量的情況下，對轉軸傾角標定影響可被忽略。將式(16)展開可得：

2 轉軸傾角標定方案

2.1 FIR低通濾波器設計

式(17)中φts、φΔ均與載體搖擺的劇烈程度有關，并且φts受到載體搖擺的影響較大，若兩套系統之間的時間同步誤差為數毫秒，當海況較差時，φts波動幅值可達到數角分，相比φ為不可忽略的量。

若將系統姿態輸出中的載體搖擺分量濾掉，然后計算φb，可以消除φts對轉軸傾角標定的影響。有限長單位沖擊響應（FIR）濾波器具有線性相位的特點[8]，保證了濾波器對各頻率信號的延時是固定的，便于在標定轉軸傾角時補償濾波后姿態角與轉軸讀角之間的時間差。

首先對轉軸傾角引起的姿態波動進行頻域分析。圖2為轉軸傾角θ=[2.4′ 3.3′ 6.4′]T的雙軸旋轉慣導系統靜態時輸出姿態波動與某次系泊實驗艦船姿態波動的頻域圖。由圖2可以看出，轉軸傾角引起姿態波動的頻率主要集中在 0.05 Hz以下范圍。而船系泊時縱搖與橫搖搖擺的頻率較高，多在 0.06 Hz以上，艏搖頻率較低與轉軸傾角引起姿態波動的頻段相重合。相同波動幅值下，搖擺頻率越高與時間同步誤差相耦合引起的姿態誤差越大，對轉軸傾角標定影響越大。可以采用低通濾波器濾除待補償系統與參考系統系泊狀態下的高頻姿態波動，以減小其與時間同步誤差耦合對轉軸傾角標定的影響。

圖2 轉軸傾角引起的姿態波動頻域圖Fig.2 Frequency domain of the attitude fluctuation caused by the fixing error angles between axes and IMU

本文使用窗函數法設計FIR濾波器[9]。針對系統1Hz姿態輸出所設計的FIR低通濾波器，截止頻率為0.06 Hz，階數為400階，窗體為布萊克曼窗，其幅頻特性曲線如圖3所示。

設置待補償雙軸旋轉慣導系統轉軸傾角為θ=[2.4′ 3.3′ 6.4′]T，兩套系統時間同步誤差為5 ms，擺幅值與周期如表1所示，得到的濾波前和濾波后待補償系統相對參考系統姿態波動分別如圖4～6中虛線和實線所示。由圖4～6可以看出，濾波前計算得到的待補償系統與參考系統姿態差中包含載體搖擺與時間同步誤差的耦合誤差，而在濾波后此誤差被基本濾除。

圖3 FIR濾波器幅頻特性曲線Fig.3 Curves of FIR filter's amplitude-frequency

圖4 待補償系統相對參考系統的縱搖誤差波動Fig.4 Pitching error fluctuation of the compensated system relative to the reference system

圖5 待補償系統相對參考系統的橫搖誤差波動Fig.5 Rolling fluctuation of the compensated system relative to the reference system

圖6 待補償系統相對參考系統的航向誤差波動Fig.6 Heading error fluctuation of the compensated system relative to the reference system

表1 搖擺幅度和周期Tab.1 Amplitude and period of the swing

2.2 狀態方程建立

設狀態向量為

由式(16)得到系統的狀態方程為：

其中，

由于在使用FIR濾波器將載體姿態輸出中包含的載體搖擺分量完全濾去的理想情況下，為常量。因此有相比為小量，亦可作噪聲處理。

其中，

2.3 量測方程建立

設系統量測量Z為參考系統與待補償系統輸出姿態經過同一FIR濾波器濾波后的歐拉角誤差，其對應的姿態矩陣通過式(12)計算得到。可得量測方程為：

將式(18)(20)離散化，再采用Kalman濾波方法進行估計，即可獲得轉軸傾角的最優估計，實現轉軸傾角的標定[10]。

2.4 轉軸傾角標定流程

由以上推導分析得到的雙軸旋轉慣導系統系泊狀態下轉軸傾角標定流程如圖7所示。

圖7 轉軸傾角標定流程示意圖Fig.7 Schematic of calibration process for the fixing error angles between axes and IMU during digital

3 仿真驗證

3.1 數字仿真驗證

數字仿真中設置待補償雙軸旋轉慣導系統誤差如表2所示，時間同步誤差為5 ms，搖擺參數如表1所示。

表2 數字仿真誤差項設置Tab.2 Error settings for digital simulation

卡爾曼濾波器初值設置如下：

仿真結果如圖8所示。卡爾曼濾波器收斂時，所估計轉軸傾角為，估計誤差為

由圖8可以看出，θx、θy在濾波開始后即迅速收斂，θz在濾波開始一段時間后才開始收斂。其原因是轉軸傾角的收斂順序和轉位次序有關。在第一個轉位次序中，內框轉角α1=0 °，內框轉動角速度α1=0(°)/s，根據式(19)可得：

從而矩陣F第三列元素為零，第一個轉位次序所激發的姿態波動與θz無關，從而θz在濾波開始時沒有收斂趨勢。

當進行第二個轉位次序，內框轉動外框停止時，有=0 (°)/s，由式(19)可得：

從而第二個轉位次序中，矩陣F第三列元素不為零，θz可被觀測，迅速收斂。

圖8 數字仿真轉軸傾角卡爾曼濾波估計值Fig.8 Kalman filter's output of the fixing error angles between axes and IMU during digital simulation

3.2 半實物仿真驗證

本文采用安裝在搖擺臺剛性基座上的兩套雙軸激光慣導系統進行半實物仿真實驗，分別命名這兩套系統為1#系統、2#系統，其中1#系統已補償轉軸傾角，作為參考系統，2#系統未補償轉軸傾角，作為待補償系統。兩套系統精對準結束后正確補償姿態零位。實際在艦船系泊狀態下進行轉軸傾角標定時，船舶搖擺姿態的頻譜比較復雜，經FIR濾波后待補償系統與參考系統的姿態輸出中仍存在未被濾掉的載體搖擺姿態。

為充分考慮實際艦船系泊情況，進行3次試驗，設置搖擺臺的搖擺幅度和頻率如表3所示，分別測試本方法在靜止狀態、低頻搖擺與高頻搖擺下的轉軸傾角標定效果。

如圖9～11所示為3次實驗的轉軸傾角標定結果，3次實驗轉軸傾角分別收斂于比較實驗1和2的實驗結果可以得出載體高頻搖擺對轉軸傾角標定幾乎無影響。

表3 半實物仿真搖擺臺設置Tab.3 Swing table setting in semi-physical simulation

圖9 2#系統θx標定結果Fig.9 Calibration result of the 2# system's θx

圖10 2#系統θy標定結果Fig.10 Calibration result of the 2# system's θy

圖11 2#系統θz標定結果Fig.11 Calibration result of the 2# system's θz

比較實驗1和3的實驗結果可以得出低頻搖擺對標定精度影響較小，分析其原因是低頻搖擺頻率較低、幅值較小，與兩套系統之間時間同步誤差耦合引起的角度誤差相比轉軸傾角引起的姿態波動為小量，并且波動規律不完全相同，因而對轉軸傾角標定的收斂過程中的波動影響較大，而對收斂精度影響較小。

由3次實驗的實驗結果可以得出，本方法在靜態與系泊狀態下轉軸傾角標定精度相當。

4 結 論

本文建立了轉軸傾角引起姿態波動的數學模型，推導了狀態方程與量測方程，并設計了FIR低通濾波器消除參考系統與待補償系統間時間同步誤差的影響，進而實現了系泊狀態下雙軸旋轉慣導系統轉軸傾角的標定。

本文所提出的方法能夠實現轉軸傾角的自動標定，對提高系統的可維護性具有重要意義。