劉錫祥，楊 燕，黃永江，宋 清

（1. 微慣性儀表與先進導航技術(shù)教育部重點實驗室，南京 210096；2. 東南大學 儀器科學與工程學院，南京 210096）

未知緯度條件下基于重力視運動與小波去噪的SINS自對準方法

劉錫祥1,2，楊 燕1,2，黃永江1,2，宋 清1,2

（1. 微慣性儀表與先進導航技術(shù)教育部重點實驗室，南京 210096；2. 東南大學 儀器科學與工程學院，南京 210096）

基于慣性系的雙矢量定姿方法選擇慣性系中的兩個重力視運動向量作為不共線矢量，解決了傳統(tǒng)雙矢量定姿方法在晃動基座條件下易受載體角運動干擾而無法實現(xiàn)對準的問題，但該方法仍需要精確的地理緯度信息以參與對準計算。針對未知緯度條件下的SINS抗晃動自對準問題，提出了一種基于重力視運動的三矢量自對準方法。該方法將初始對準問題歸結(jié)為求解當前時刻導航系相對于初始時刻載體系的姿態(tài)矩陣問題，并利用矢量運算進行求解，仿真結(jié)果表明：加速度計隨機測量噪聲會映射為重力視運動隨機噪聲，降低對準精度；當加速度計隨機噪聲量級較大時，會帶來對準計算失敗。針對噪聲問題，引入Daubechies（db4）小波進行5層分解來實現(xiàn)對重力視運動的降噪，并選擇去噪后的重力視運動向量參與三矢量定姿解算，仿真結(jié)果表明：db4小波具有良好的去噪效果，基于小波去噪的三矢量自對準方法可以有效完成未知緯度條件下的SINS初始對準。

捷聯(lián)式慣性導航系統(tǒng)；初始對準；重力視運動；小波去噪；三矢量定姿

初始對準是捷聯(lián)式慣性導航系統(tǒng)(SINS)導航工作 的前提和基礎。一般而言，SINS的初始對準具體指：構(gòu)建數(shù)學平臺，并使其重合于預設的導航坐標系，同時獲取載體系相對于導航系的實時姿態(tài)矩陣[1-2]。經(jīng)過數(shù)十年的發(fā)展，在 SINS的初始對準方面有著許多成熟方法，如：基于雙矢量定姿的解析對準方法[3]，基于羅經(jīng)效應的羅經(jīng)對準方法[4]，基于載體運動約束的零速對準以及基于外部參考信息與最優(yōu)估計的傳遞對準等[5]。然而上述方法一般都需要精確的緯度信息加以輔助，如：解析對準中需要緯度信息分解地球自轉(zhuǎn)角速度，羅經(jīng)對準以及傳遞對準中需要緯度信息參與導航解算[6]，但是在諸如隧道、橋下、深海等特殊情況下，想要獲取精確的緯度信息卻并非一件容易的事。針對這種情況，尋找一種方法來實現(xiàn)未知緯度條件下的初始對準顯得十分必要。

近年來，為解決晃動條件下的初始對準問題，基于慣性系的初始對準方法受到了一定程度的關(guān)注。文獻[7-8]提出了搖擺基座上基于重力加速度信息的SINS粗對準方法，該方法將初始載體坐標系凝固為慣性坐標系，將姿態(tài)矩陣進行鏈式分解，通過觀察慣性系中的重力加速度漂移，具體實現(xiàn)了晃動條件下的初始對準，并通過積分運算平滑了加速度以提高對準精度。一般而言，當前基于慣性系的對準方法均選擇雙矢量定姿算法作為數(shù)學工具。該類對準方法在有效地隔離晃動干擾的同時，仍需要有精確的緯度信息加以輔助。

受到慣性系中各重力視運動向量間幾何關(guān)系的啟發(fā)，本文提出了一種基于重力視運動的三矢量法的SINS自對準方法，以解決未知緯度條件下的晃動基座對準問題。該方法是利用三個不共線的重力視運動向量求取重力視運動錐體底圓中心，并進一步通過矢量運算求出其當前的姿態(tài)矩陣。針對該方法對準精度易受到加速度計測量噪聲影響的問題，本文進一步引入db4小波對重力視運動向量進行預處理以去除噪聲。仿真結(jié)果表明，經(jīng)過小波去噪改進后的三矢量法的SINS自對準方法可以很好地實現(xiàn)未知緯度條件下的晃動基座對準問題。

1 基于重力視運動與矢量運算的SINS自對準算法

綜合公式(1)和公式(2)可知：只要獲取了慣性坐標系和當前導航坐標系之間的姿態(tài)矩陣（t ），通過鏈式法則，即可獲得姿態(tài)矩陣（t）完成初始對準。

1.1 慣性系中的重力視運動

視運動最初用來描述陀螺儀的定軸性。陀螺的視運動具體指：在隨地球自轉(zhuǎn)的導航系中觀察到的相對于慣性空間穩(wěn)定的自由陀螺儀的指向變化。不同于陀螺的視運動，本文定義重力視運動為：在慣性系中觀察到的隨地球自轉(zhuǎn)的導航系中重力加速度的指向變化。根據(jù)文獻[9]可知，慣性系的重力視運動可以描述為圖1所示的圓錐，錐頂位于地球球心，圓錐的中心軸與地球的自轉(zhuǎn)軸重合，錐體的底圓半徑由載體所在的緯度決定。

1.2 基于三個重力視運動向量的SINS自對準算法

圖1 慣性系中的重力視運動Fig.1 Apparent motion of gravity in inertial frame

本文分別采用“東北天ENU”與“右前上”為導航坐標系和載體坐標系。由慣性系中重力視運動的描述，可以將導航坐標系與重力視運動形成的錐體之間的幾何關(guān)系描述為圖2，其中點O表示地球球心，點Oc表示底面圓錐的圓心，其與載體所在位置緯度圈的圓心重合，并且位于地球的自轉(zhuǎn)軸上。在t時刻，導航系的原點為圓錐底圓圓周上的點On，向量與導航系的天向軸U重合，但與重力加速度在慣性系投影值gi（t）的方向相反，向量與地球自轉(zhuǎn)軸ωie重合，的叉乘積與導航系的東向軸E重合，U×E的叉乘積又與導航系的北向軸N重合。若已知導航系的各軸在慣性系的投影值，則可以通過下式求得矩陣 C（t）：

式中，E、N、U分別表示導航系各軸在慣性系的投影值。

圖2 基于重力視運動的對準機理Fig.2 Alignment mechanism based on gravitational apparent motion

基于上述分析，完成初始對準的關(guān)鍵在于獲取導航系各軸在慣性系中的投影值。因而，完成初始對準的關(guān)鍵步驟可以歸結(jié)為：1）在慣性系中構(gòu)建重力視運動向量；2）利用重力視運動向量求解導航系各軸在慣性系的投影值，并進一步求解Cnib0（t）。不考慮儀表安裝誤差，假設慣性測量組件坐標系與載體坐標系重合，則慣性系中的重力視運動向量可以用下式構(gòu)建：

為了獲得導航系各軸在慣性系中的投影值，需要獲取視運動錐體底圓圓心 Oc點的坐標（x0,y0,z0）。如圖3所示，假設三個不同時刻 tA、tB、tC的重力視運動向量分別為，各向量的計算值可理解為相應時刻的重力視運動向量在錐體底圓上的坐標值，分別為（x3,y3,z3）。根據(jù)解析幾何知識可求得向量的中垂線，兩條中垂線的交點即為圓錐底圓圓心 Oc，具體可以描述如下：

圖3 錐體底圓圓心的求取Fig.3 Calculation method for cone axis

AB的中垂線方程為：

3）求得圓心坐標（x0,y0,z0）聯(lián)立方程(8)(9)可以求得：

c

最后，向量N可以通過下式獲?。?/p>

進一步，可根據(jù)式(1)(2)(17)完成初始對準。該方法可獲取的理論最小對準誤差可表達如下：

1.3 仿真驗證

1.3.1 仿真條件設置

以艦船為例，為了方便分析，我們首先考慮艦船在靜基座情況下的對準情況，表1為仿真條件，表2為傳感器誤差參數(shù)。

在第一種條件下，艦船處于靜止狀態(tài)且儀表僅存在常值誤差而無隨機誤差，這種情況在現(xiàn)實中是不存在的，此處作為理論分析。

設儀表數(shù)據(jù)的采樣頻率為200 Hz，導航解算的更新周期為5 ms。根據(jù)艦船運動，通過逆向?qū)Ш浇馑憧煞囱莸玫絻x表的理想輸出值；在理想輸出上疊加上表2中的各誤差后，可模擬儀表的真實輸出；同時可利用艦船運動參數(shù)作為評價標準，以計算對準誤差。根據(jù)公式(18)以及表 2中的儀表誤差參數(shù)可以計算出縱搖、橫搖與航向的理論的最小對準誤差分別為0.0029°、-0.0029°、0.2228°。

表1 仿真條件Tab.1 Simulation conditions

表2 儀表誤差參數(shù)Tab.2 Sensor errors

1.3.2 仿真結(jié)果及分析

仿真持續(xù)600 s。系統(tǒng)選擇了0 s、2.5 s、5 s三個時刻的重力視運動向量以 5 s一個周期進行一次完整的三矢量對準。對準誤差如圖4所示，虛線代表的是第一種條件下的對準誤差，實線代表的是第二種條件下的對準誤差。對準誤差的統(tǒng)計結(jié)果如表3所示。

由圖4的對準誤差曲線可知：當慣性測量元件的測量值中不含有隨機誤差的時候，基于重力視運動的三矢量對準方法可以有效地完成未知緯度條件下的SINS初始對準，對準精度與理論精度值相當。但是當儀表的測量數(shù)據(jù)中存在隨機誤差時，航向?qū)薀o法完成，且水平（縱搖和橫搖）對準結(jié)果中含有較多的毛刺，與理論精度間存在較大的差異。

顯然兩種條件的區(qū)別僅在于儀表測量值中有無隨機噪聲。因陀螺儀的測量噪聲在積分過程中得到平滑，可以認為加速度計測量值中的隨機噪聲是導致第二種條件對準失敗的主要原因，該噪聲被投影為重力視運動向量中的隨機噪聲，從而導致導航系各軸在慣性系中投影值的計算精度下降。進一步針對搖擺基座條件下的仿真驗證將在解決噪聲干擾問題后進行。

表3 對準誤差統(tǒng)計結(jié)果Tab.3 Statistics of alignment error

圖4 對準誤差曲線Fig.4 Curves of alignment errors

2 基于小波去噪的SINS自對準改進方法

2.1 重力視運動向量的預處理

第1節(jié)分析表明，與慣性系中的雙矢量定姿算法類似[10-11]，基于三矢量定姿 SINS自對準方法的對準精度主要取決于加速度計的測量精度，尤其是加速度計測量值中隨機噪聲的影響。該噪聲帶來了兩個主要問題：1）慣性系中的重力視運動向量不能真實地反映重力漂移；2）選取的重力視運動向量可能存在共線問題。文獻[12-13]分析表明，要避免重力視運動向量間的共線，一是加大時間間隔，二是提高重力視運動的計算精度。但前者會帶來對準時間的冗長?；诖耍疚闹鉀Q重力視運動向量的計算精度問題。

近年來，針對噪聲問題涌現(xiàn)了許多成熟的數(shù)學方法，小波去噪是一類典型代表。小波變換相對于傳統(tǒng)的傅里葉變換具有對信號良好的時頻局部刻畫性能，它能夠在去除大部分噪聲的同時，保留信號的瞬態(tài)特征，本文選取了db4小波來對重力視運動向量進行去噪。

1）小波去噪原理

小波理論的迅速發(fā)展以及其良好的時頻特性，使其應用領域越來越廣泛，其中信號和圖像的降噪與壓縮是小波的重要應用之一。

在現(xiàn)實環(huán)境中，信號的傳輸與收集總是不可避免地會受到一些噪聲的影響。一個含噪的一維信號模型可以表示為如下形式：

式中， s（k）為含噪信號， f（k）為有用信號， e（k）為噪聲信號。這里我們考慮 e（k）為一個1級高斯白噪聲，通常表現(xiàn)為高頻信號，而在實際的工程應用中， f（k）通常表現(xiàn)為低頻信號或者是一些比較平穩(wěn)的信號。因此可以將小波去噪的原理描述如下：首先對原始信號進行小波分解，由于噪聲信號大多包含在具有較高頻率的細節(jié)中，因而可以通過門限閾值等形式對分解的小波進行處理，然后對處理后的信號進行重構(gòu)即可達到去噪的目的[14-17]。對信號的降噪實質(zhì)上就是抑制原信號中的無用部分恢復其中的有用部分。

2）小波降噪的步驟和方法

一般而言，一維信號的降噪過程可以分為以下三個步驟：

① 分解過程：選定一種小波，對信號進行N層小波(小波包)分解；

② 作用閾值過程：對分解得到的各層系數(shù)選擇一個閾值，并對細節(jié)系數(shù)作用軟閾值處理；

③ 重建過程：降噪處理后的系數(shù)通過小波(小波包)重建恢復原始信號。

在小波去噪的三個過程中，最重要的一步是閾值的選擇以及如何進行閾值量化，本文中選擇了分層的軟閾值方法對原始信號進行降噪處理，其基本原理是根據(jù)方差最小的原則通過對系數(shù)的無偏似然估計來確定閾值。采用分層閾值的處理方法雖然會遺失原始信號的部分性能，但是去噪后的信號要比全局閾值的結(jié)果光滑許多，并且能夠最大限度地反映原信號本身的性質(zhì)。

3）仿真驗證

本文采用了db4小波，分解層次設置為5層，對重力矢量投影值進行去噪。仿真過程持續(xù)600 s，仿真條件是在表1的第二種條件下進行的，圖5中實線代表的是去噪前的慣性系各軸的重力視運動向量值，虛線是db4小波進行5層分解，通過分層閾值的方式得到的去噪后的重力視運動向量值。從圖5可以看出，小波去噪可以有效地去除儀表測量值中的隨機噪聲，較好地保留了有用信號。

圖5 慣性系中重力視運動投影值Fig.5 Projections of gravitational apparent motion in inertial frame

3 基于小波去噪的SINS自對準改進方法驗證

3.1 仿真條件設置

為了驗證基于小波去噪改進對準方案的有效性，進行晃動基座條件下的仿真驗證。設有表4所示的兩種運動情況，其中條件二同表1中條件二，儀表的常值誤差與隨機誤差如表2所示。在表4中的第三種條件下，艦船以進行晃動，A、f、 η0與 θ0分別表示搖擺幅值、頻率、初始相位及搖擺中心。搖擺參數(shù)如表5所示。

表4 仿真條件Tab.4 Simulation conditions

表5 搖擺參數(shù)Tab.5 Swing parameters

3.2 仿真結(jié)果及分析

圖 6是改進方法在第二種條件下的對準誤差曲線，在對準過程中，選擇10 s、150 s、250 s三個時刻的重力視運動向量參與對準計算，對準在250 s結(jié)束，250 s之后進行純慣性導航解算，解算時長為300 s。圖7是改進方法在第三種條件下的對準誤差曲線，在對準過程中，選擇0 s、400 s、900 s三個時刻的重力視運動向量參與對準計算，對準在900 s結(jié)束，900 s之后進行純慣性導航解算，解算時長為300 s。以對準結(jié)束后的捷聯(lián)解算誤差作為統(tǒng)計數(shù)據(jù)以評價對準結(jié)果，統(tǒng)計結(jié)果如表6所示。

由于純慣性導航時間有限，僅為300 s，其解算誤差、儀表誤差的累積量有限，解算誤差主要體現(xiàn)為初始對準誤差。分析圖6、圖7中誤差曲線以及表6中的統(tǒng)計結(jié)果，可以認為小波去噪有效地剔除了加速度計測量值中的隨機噪聲，基于小波去噪與三矢量的自對準算法可以有效完成未知緯度條件的 SINS初始對準，且對準精度接近于理論極限精度。

表6 改進方法的對準誤差統(tǒng)計結(jié)果Tab.6 Statistics of alignment errors based on improved method

圖6 改進方法在第二種對準條件下的對準誤差曲線Fig.6 Curves of alignment errors based on the improved method under the second condition

圖7 改進方法在第三種對準條件下的對準誤差曲線Fig.7 Curves of alignment errors based on the improved method under the third condition

4 結(jié)束語

本文提出并實現(xiàn)了一種未知緯度條件下，利用重力視運動實現(xiàn)SINS自對準的方法。該方法選擇了慣性坐標系下三個不共線的重力視運動向量，通過矢量運算求取當前時刻導航系相對于初始時刻載體系的姿態(tài)矩陣，并進一步利用矩陣鏈式乘法完成初始對準。仿真結(jié)果表明，該方法的對準精度易受到重力視運動向量中的隨機噪聲的干擾，針對這一問題，本文選用了db4小波通過分層閾值的方法實現(xiàn)對重力視運動向量的預處理，將預處理過后的重力視運動向量運用于三矢量法的SINS自對準中，實現(xiàn)了對三矢量自對準算法的改進。仿真結(jié)果表明：db4小波可以有效地從重力視運動中去除高頻噪聲，利用消噪后的重力視運動向量參與對準計算，可以有效地完成未知緯度情況下的SINS的抗晃動自對準。

（References）：

[1] Ali J, Fang Jiancheng. Alignment of strapdown inertial navigation system: A literature survey spanned over the last 14 years[D]. School of instrumentation Science and Optoelectronics Engineering, Beijing University of Aeronautics and Astronautics, Beijing, 2004: 100083.

[2] 張燕, 陳星陽. 導彈姿態(tài)矩陣更新算法推導和仿真驗證[J]. 四川兵工學報, 2013, 34(8): 26-29. Zhang Yan, Chen Xing-yang. Derivation and simulation validating for attitude matrix updating algorithms for missiles[J]. Sichuan Ordnance Journal, 2013, 34(8): 26-29.

[3] 張劍慧, 秦永元, 龍瑞. 捷聯(lián)慣導系統(tǒng)雙矢量定姿方法研究[J]. 計算機測量與控制, 2010(11): 2634-2637. Zhang Jian-hui, Qin Yong-yuan, Long Rui. Studying on algorithm of double-vector attitude determination in SINS[J]. Computer Measurement & Control, 2010(11): 2634-2637.

[4] 經(jīng)張俊, 程向紅, 王宇. 捷聯(lián)羅經(jīng)的動基座自對準技術(shù)[J]. 中國慣性技術(shù)學報, 2009, 17(4): 408-412. Jing Zhang-jun, Cheng Xiang-hong, Wang Yu. Technology of strapdown gyrocompass self-alignment on moving bases[J]. Journal of Chinese Inertial Technology, 2009, 17(4): 408-412.

[5] 楊功流, 王麗芬, 袁二凱, 等. 大方位失準角下艦載機快速傳遞對準技術(shù)[J]. 中國慣性技術(shù)學報, 2014, 22(1): 45-50. Yang Gong-liu, Wang Li-fen, Yuan Er-kai, et al. Rapid transfer alignment of lager misalignment angle for carrier aircrafts [J]. Journal of Chinese Inertial Technology, 2014, 22(1):45-50.

[6] Silva F O, Hemerly E M, Filho W C L. Influence of latitude in coarse self-alignment of strapdown inertial navigation systems[C]//2014 IEEE/ION Position, Location and Navigation Symposium. 2014: 1219-1226.

[7] 秦永元, 嚴恭敏, 顧冬晴, 等. 搖擺基座上基于信息的捷聯(lián)慣導粗對準研究[J]. 西北工業(yè)大學學報, 2006, 23(5): 681-684. Qin Yong-yuan, Yan Gong-min, Gu Dong-qing, et al. A clever way of SINS coarse alignment despite rocking ship [J]. Journal of NorthWestern Polytechniacal University, 2006, 23(5): 681-684.

[8] 趙長山, 秦永元, 白亮. 基于雙矢量定姿的搖擺基座粗對準算法分析與實驗[J]. 中國慣性技術(shù)學報, 2009, 17(4): 436-440. Zhao Chang-shan, Qin Yong-yuan, Bai Liang. Analysis and test of swaying-based analytic coarse alignment[J]. Journal of Chinese Inertial Technology, 2009, 17(4): 436-440.

[9] Gaiffe T. From R&D brassboards to navigation grade FOG-based INS: the experience of Photonetics/Ixsea [C]//Optical Fiber Sensors Conference Technical Digest. IEEE, 2002: 1-4.

[10] Yan Gong-min, Weng Jun, Bai Liang, et al. Initial inmovement alignment and position determination based on inertial reference frame[J]. Systems Engineering and Electronics, 2011, 33(3): 618-621.

[11] Liu Xixiang, Xu Xiaosu, Zhao Yu, et al. An initial alignment method for strapdown gyrocompass based on gravitational apparent motion in inertial frame[J]. Measurement, 2014, 55: 593-604.

[12] Liu Xixiang, Liu Xianjun, Song Qing, et al. A novel self-alignment method for SINS based on three vectors of gravitational apparent motion in inertial frame[J]. Measurement, 2015, 62: 47-62.

[13] Liu Yiting, Xu Xiaosu, Liu Xixiang, et al. A self- alignment algorithm for SINS based on gravitational apparent motion and sensor data denoising[J]. Sensors, 2015, 15(5): 9827-9853.

[14] 胡禮勇, 李釗, 李建軍, 等. 基于小波的慣導系統(tǒng)初始對準方法研究[J]. 無線電工程, 2011, 41(4): 31-33. Hu Li-yong, Li Zhao, Li Jian-jun, et al. Initial coarse alignment technique based on wavelet for inertial navigation system[J]. Journal of Wireless Engineering, 2011, 41(4): 31-33.

[15] 趙春蓮, 翁浚, 嚴恭敏, 等. 小波域閾值濾波在基于慣性系對準中的應用[J]. 測控技術(shù), 2014, 33(2): 32-35. Zhao Chun-lian, Weng Jun, Yan Gong-min, et al. Application of wavelet domain threshold filtering in alignment based on inertial frame[J]. Journal of Measurement and Control Technology, 2014, 33(2): 32-35.

[16] Ilyas M, Yang Y, Zhang R. SINS initial alignment using wavelet de-noising method for aircraft navigation[C]// 2012 10th World Congress on Intelligent Control and Automation. IEEE, 2012: 3921-3926.

[17] Huang B, Sun P J, Wang X M. The denoising method based on wavelet transform in signal dynamic detection [C]//Advanced Materials Research. 2014, 889: 766-769.

Self-alignment algorithm without latitude for SINS based on gravitational apparent motion and wavelet denoising

LIU Xi-xiang1,2, YANG Yan1,2, HUANG Yong-jiang1,2, SONG Qing1,2
(1. Key Laboratory of Micro-inertial Instrument and Advanced Navigation Technology, Ministry of Education, Southeast University, Nanjing 210096, China; 2. School of Instrument Science & Engineering, Southeast University, Nanjing 210096, China)

Double-vector attitude determination algorithm in inertial frame takes two gravitational apparent motion vectors as non-collinear vectors. Although this method solve the traditional algorithm’s problem that the information is susceptible to angular motion disturbance on swinging base, it still needs accurate latitude information to participate in alignment calculation. Aiming to fulfill the alignment for strapdown inertial navigation system without aided latitude information, a self-alignment method with three gravitational apparent motion vectors is designed. In this method, the alignment problem is attributed to solving the attitude matrix between current navigation frame and initial body frame and is solved with vector operation. Simulation results indicate that those random noises in the accelerator will be projected in gravitation apparent motion vectors and decrease the alignment accuracy, and even cause alignment failure when with large noise. For denoising, the daubechies (db4) wavelet is introduced to decompose gravitational apparent motions with 5 layers, and three denoised apparent motion vectors are selected to participate in the alignment. Simulation results indicate that the db4 owns excellent denoising effects and the alignment method with three apparent motion vectors and db4 in inertial frame can fulfill the alignment without aided latitude information.

strapdown inertial navigation system; initial alignment; gravitational apparent motion; wavelet denosing; tri-vector attitude determination

U666.1

：A

2016-02-04；

：2016-05-20

自然科學基金（61273056）

劉錫祥（1976—），男，博士，教授，研究方向為慣性導航與組合導航技術(shù)。E-mail: scliuseu@163.com

1005-6734(2016)03-0306-08

10.13695/j.cnki.12-1222/o3.2016.03.006