利用偽觀測取代精密轉臺的原地旋轉調制尋北

牛小驥，王 強，李 由,2，唐 健

（1. 武漢大學 衛星導航定位技術研究中心，武漢 430079；2. 卡爾加里大學，加拿大 卡爾加里 T2N1N4）

利用偽觀測取代精密轉臺的原地旋轉調制尋北

牛小驥1，王 強1，李 由1,2，唐 健1

（1. 武漢大學 衛星導航定位技術研究中心，武漢 430079；2. 卡爾加里大學，加拿大 卡爾加里 T2N1N4）

針對傳統的多位置（如四位置）及旋轉調制尋北法所需精密轉位機構（如轉臺）帶來的體積、重量、功耗和成本問題，提出了一種不依賴于精密機械旋轉裝置的原地尋北方法。借助基于慣性的組合導航算法，并利用慣性測量單元原地旋轉（手動或自動）時位置和速度變化范圍有限這一假設作為偽觀測，實現與四位置法精度和效率相當的尋北。因為對偽觀測的使用不需精確已知每一時刻IMU相對于底座的角度，故可從本質上消除對精密角度或角速度測量裝置的依賴。大量（近100組）實測實驗表明，在采用0.5 (°)/h的光纖慣導時可在4 min內達到優于1°(1 sigma)的尋北精度。因此，可在尋北精度和時間相當的條件下，大大降低傳統尋北方法對系統的硬件要求，降低尋北成本。

尋北；卡爾曼濾波；旋轉調制技術；偽觀測信息；組合導航

陀螺尋北儀可以自主、快速、全天候地提供初始方位基準,在軍事和民用工程中顯示出日益廣闊的應用前景[1]。陀螺尋北儀的兩種經典尋北方法是四位置法和旋轉調制法[2]。四位置法是基于最小二乘的思路，通過對稱位置的測量不但能消除常值漂移的的影響，還能較好地降低標度因數對尋北結果的影響。但是，該方法需要精密的轉位機構控制慣性測量單元（IMU）在尋北時段內分別停在不同的精確已知的角位置上，因此造成系統的體積、重量、功耗和成本的增加[3]。相對于四位置法，旋轉調制尋北法憑借其對陀螺零偏的調制解調，理論上可以顯著縮短尋北時間，提高尋北精度。但是，旋轉調制尋北法的連續旋轉方案對系統硬件結構和性能，尤其是對轉臺的旋轉精度、轉速和轉動穩定性有很高的要求，即尋北精度很大程度上取決于轉臺精度[4-8]。為此，本文提出一種不依賴于精密機械旋轉裝置的原地旋轉調制尋北方法，利用慣導中的陀螺器件經過在線誤差補償后的輸出來推算旋轉調制過程中的精密轉角，然后使用基于偽觀測信息[9]的組合導航算法，實現與四位置法精度和效率相當的尋北方法，顯著降低了系統的體積、重量、功耗和成本。

1 基于偽觀測的旋轉調制尋北方案

1.1 算法概述

為了降低尋北對轉臺精度的依賴，本文使用IMU原地旋轉（手動或使用機器）時其位置和線速度變化范圍有限這兩種假設分別作為偽位置觀測和偽速度觀測，并使用偽觀測/INS組合導航卡爾曼濾波算法實現對航向角的估計。慣導的原地旋轉在組合導航算法中很大程度上抵消了陀螺零偏對航向估計的影響，從而提高了航向角的估計精度[10-13]。更重要的是，因為對偽觀測信息的使用不需要精確知道每一時刻的 IMU相對于底座的角度，本算法可從根本上消除對精密角度或角速度測量裝置的依賴。圖1給出了原地尋北算法的結構示意圖。

圖1 原地尋北算法結構示意圖Fig.1 Schematic of the in-situ north-seeking algorithm

如圖1所示，本文尋北算法主要包括幾個方面：慣導解算、IMU誤差建模、偽觀測/INS組合導航卡爾曼濾波，以及桿臂的處理。偽觀測的引入和其與 INS的組合導航卡爾曼濾波是本文的重點，也是取代精密轉位裝置為INS提供修正的關鍵。此外，本研究發現，桿臂的存在將影響尋北性能，并因此提出了存在桿臂情況下的偽觀測/INS組合方案。本節接下來將對尋北算法的各主要模塊分別進行描述。

1.2 慣導解算

慣導解算主要思想：使用IMU量測的角速度和比力（或角增量和速度增量），結合IMU初始導航狀態（位置、速度和姿態），不斷預測IMU導航狀態。具體來講，首先使用陀螺測量的角速度向量進行姿態更新；然后，利用姿態信息將加速度計測量的比力向量從IMU坐標系（XYZ坐標系）轉換到導航坐標系（北東地坐標系）；之后，在導航坐標系內將重力向量加入比力構成加速度向量；之后，對加速度進行積分并結合上一時刻速度求得當前時刻的速度；在對速度進行積分并結合上一時刻位置求得當前時刻的位置。具體的慣導算法可參考文獻[14]。圖2為慣導算法示意圖，圖中 rn和 vn分別為IMU位置和速度，為IMU坐標系到導航坐標系的姿態轉換矩陣，為地球坐標系到導航坐標系的方向余弦矩陣， fb和 ω分別為比力和角速度向量在 IMU坐標系內的投影，ω和 ω分別為地球自轉角速度和由于 IMU在地球曲面運動造成的角速度，gn為本地重力向量， 符號 “×”表示兩個向量的叉乘， 符號“ω” 表示 “β”坐標系相對于“α” 坐標系的角速度在“χ”坐標系內的投影；“b”、“n”、“e”和 “i”依次代表IMU坐標系、導航坐標系、地球坐標系和慣性坐標系。

圖2 慣導算法示意圖Fig.2 Schematic of the inertial navigation algorithm

1.3 IMU誤差建模

僅考慮傳感器零偏、標度因數和噪聲的情況下，加速度計和陀螺輸出誤差模型為

式中：δ fb和 δω分別為比力和角速度誤差，ba和bg依次為加速度計和陀螺的零偏，δsa和δsg分別為加速度計和陀螺的標度因數誤差，符號“diag(·)”表示一個向量構成的對角陣，wa和wg分別為加速度計和陀螺測量噪聲。

1.4 偽觀測/INS卡爾曼濾波狀態方程

本文在導航坐標系下對慣性導航微分方程進行建模[15]：

式中：δrn、δvn和ψ分別為位置(緯度、經度、高程)誤差、速度誤差以及姿態角旋轉矢量誤差在n系中的投影；δgn是重力誤差在n系中的投影；δr˙n、δv˙n、ψ˙依次為各量的時間微分；fn為n系中比力向量。

零偏和標度因數誤差均建模為一階高斯馬爾科夫過程：

式中：τba、τbg、τsa及τsg分別代表各傳感器誤差的相關時間，wba、wbg、wsa及wsg為驅動白噪聲[16]。

使用上述慣性導航微分方程，卡爾曼濾波連續時間狀態方程可建模為

式中：x(t)為狀態參數陣，F(t)為系統狀態的轉移矩陣，w(t)為系統狀態的白噪聲過程，G(t)為系統狀態噪聲的驅動陣。其中狀態量為21維：

離散狀態方程為

式中：xk、xk-1分別為k和k-1時刻的狀態向量，Φk,k-1為離散后的狀態轉移矩陣，Γk-1為系統噪聲驅動陣，Wk-1為狀態的噪聲向量。忽略Φk,k-1中關于Δt=tk-tk-1的二次項，Φk,k-1可表示為如下形式：

式中：

其中，I為單位陣，0i×j為i×j的零矩陣，i＞0, j＞0。

1.5 偽觀測/INS卡爾曼濾波觀測方程

本文利用 IMU原地旋轉條件下的偽位置和偽速度約束代替GNSS位置和速度信息以完成尋北。具體的偽位置和偽速度模型為

式中：zr和zv分別為位置和速度量測向量，r?n和v?n為INS預測的位置和速度向量，n和n分別為偽位置和偽速度向量，n為常值，n=0，nr和nv分別為位置和速度量測噪聲。實際操作中IMU的位置和線速度變化在卡爾曼濾波量測噪聲陣（R）中體現。

1.6 包含桿臂的偽觀測/INS卡爾曼濾波觀測方程

由于偽觀測是基于 IMU旋轉過程中位置和線速度變化范圍有限這一假設，因此針對實際使用過程中IMU的位置和速度有可能變化這一現象，本文也給出了相應的解決方案。桿臂包含兩部分：系統性部分和隨機性部分。系統性部分為常值，如將IMU安裝在轉臺上進行尋北時，桿臂為IMU測量中心到轉臺面旋轉中心的距離；隨機性部分多不為常值，如在不使用轉臺的情況下（如手動旋轉IMU），桿臂為IMU測量中心至IMU在慣性坐標系中的旋轉中心的距離。本文推導了包含系統性桿臂誤差的偽觀測量測方程，并將隨機性桿臂誤差在卡爾曼濾波量測噪聲陣(R)中體現。

RM和RN為子午圈半徑和卯酉圈半徑，B表示緯度，h表示高程。對式(10)求導可得：

利用式(10)，得偽位置估計為

偽位置觀測量可以建模為

式中：er表示位置誤差。則由(12)(13)可得如下松組合位置觀測模型：

對式(17)做擾動分析，得到偽速度估計：

又偽速度測量可以寫成：

故根據式(15)(16)，松組合KF速度觀測模型為

“偽位置+偽速度”觀測模型為

1.7 卡爾曼濾波器參數設置

卡爾曼濾波主要參數包括初始狀態協方差矩陣P0、狀態噪聲陣Q和量測噪聲陣R。初始協方差陣P0為對角陣，其值在程序的配置文件中設置，分別對應初始位置、速度和姿態標準差信息、陀螺零偏標準差、加速度計零偏標準差、陀螺標度因數標準差、加速度計標度因數標準差信息。

系統噪聲方差陣Q為對角陣，其值根據傳感器的角度隨機游走、速度隨機游走、零偏標準差、標度因數標準差信息計算。

量測噪聲方差陣R的值根據實際情況下IMU測量中心可能的位置和速度變化進行設置。若采用機械裝置控制旋轉，則量測噪聲較??；若采用如手動旋轉等方式，則量測噪聲較大。

1.8 卡爾曼濾波預測與更新

預測：

根據上述狀態方程和量測方程，通過卡爾曼濾波算法，可估算出航向角，即尋北角度。

2 實驗和分析

為了論證本方法尋北的精度，本文使用轉臺控制IMU旋轉，并計算四位置尋北結果作為對比參考，而在尋北計算中不使用任何轉臺提供的角位置或角速度信息。實測實驗在武漢大學轉臺實驗室進行，實驗室坐標為緯度φ=30.51667°，經度λ=114.3556°。使用的光纖捷聯慣導器件指標如下：陀螺零偏穩定性為0.5 (°)/h，角度隨機游走為0.05 (°)/√h；三軸加速度計零偏穩定性為 100 μg，速度隨機游走為 0.1 ms/√t。

依據文獻[17]中陀螺角度隨機游走(ARW)對慣導系統初始對準影響的相關公式，可計算出在實驗室位置4 min內ARW對尋北的影響為 0.8563°，陀螺ARW和零偏對三軸單位置尋北精度總的影響為2.3709°。

下面將對四位置法和本文的原地尋北法的實測進行分析：

四位置法尋北基于最小二乘思路，其精度主要受陀螺ARW的影響；原地尋北基于卡爾曼濾波的思路，水平面內常值漂移在旋轉過程中被調制，通過積分解調消除，尋北精度也主要受ARW的影響。因此該原地尋北方法在理論上有望達到與四位置法相當的尋北精度。

本次實驗四位置尋北法計算航向角采用的是多位置尋北的最小二乘原理[1]。通過統計96個樣本的尋北結果，得到其均方根（RMS）為 0.8604°。接下來將會對原地尋北法的實測進行分析。

原地尋北實驗數據采集過程描述：先在初始角位置靜態20 s，然后進行總時長達3 min 20 s的連續正反交替旋轉（交替時長是靈活選擇的），最后轉回到初始位置靜態20 s，尋北總時長4 min。

桿臂效應是影響尋北精度的重要因素，也是工程應用中無法回避的問題。桿臂設置不正確會造成桿臂效應，桿臂效應增大會造成位置誤差增大，如此偽觀測/INS組合導航算法不能對陀螺零偏、航向角等參數進行很好的估計。圖3為不同桿臂條件下，陀螺零偏的估計曲線，該圖表明對于高精度尋北必須考慮桿臂的影響。正因為如此，本研究提出了包含桿臂信息的偽觀測/INS卡爾曼濾波量測方程。

圖3 不同桿臂下原地尋北陀螺零偏估計曲線（左圖：正確桿臂 右圖：錯誤桿臂）Fig.3 Estimation of gyro bias by the in-situ north-seeking method with different lever arms (Left: correct lever arm Right: wrong lever arm)

圖4為一次實驗初始航向角給定 -10°（初始航向角真值為0°）時的航向角誤差收斂曲線圖。尋北1 min后，解算結果趨于穩定，該圖說明了本文所提出的原地尋北方法的可行性。

圖4 單次實驗原地尋北航向角誤差輸出曲線Fig.4 An example of heading error convergence plot by the in-situ north-seeking

圖5為多個樣本原地尋北航向角誤差輸出曲線，呈現了方位角解算過程中誤差的變化趨勢，可見尋北時間大于2 min后，所有樣本結果趨于穩定。該圖也說明了原地尋北的結果與初始角位置的選取無關。

圖5 原地尋北航向角誤差輸出曲線Fig.5 Heading error convergence plots by the in-situ north-seeking

圖6為原地尋北每次實驗結果航向角誤差分布圖。96個樣本解算結果中，除了一個粗差，其余 95樣本的解算結果均在白噪聲影響的3倍中誤差內。仔細觀察圖6結果會發現：尋北結果偏差普遍偏負，這應該是初始航向角統一設定為比真值小 10°（即初始航向誤差為-10°）引起的；多次尋北結果之間沒有相關性，與白噪聲對尋北結果的影響特征相符。

根據上述96個樣本，統計出的RMS為0.8633°。統計結果表明該原地尋北方法能達到優于 0.9°(4 min)的尋北精度。

圖6 原地尋北每次實驗結果航向角誤差分布圖Fig.6 Heading errors of the in-situ north-seeking in multiple experiments

3 結 論

本文借助組合導航算法和載體原地旋轉條件下的偽位置和偽速度觀測，提出了一種不依賴于精密機械旋轉裝置的原地旋轉調制尋北方法。此外，本文發現了桿臂，即IMU測量中心到偽觀測中心（IMU相對慣性坐標系的旋轉中心）的向量對尋北精度的影響，并推導了包含桿臂的偽觀測/INS量測方程。

通過近 100次實驗結果驗證了其正確性和可行性。實驗結果統計表明，中低等級光纖慣導（0.5 (°)/h的零偏穩定性）在 4 min內的尋北精度能夠達到0.8633°(RMS)，其精度和效率與四位置法（0.8604° (RMS)）相當。桿臂對比實驗表明，該原地旋轉調制尋北方法需要考慮偽觀測的桿臂的影響。

由于不需要精密轉臺，而只需要一定程度的旋轉運動，該方法與傳統多位置法和旋轉調制法對比，具有獨特的體積、重量、功耗和成本優勢。該方法特別適用于載體本身就有連續旋轉運動的應用場景，例如做連續方位掃描的雷達、旋轉火箭彈等。

（References）：

[1] 段苛苛. 基于光纖陀螺的尋北系統關鍵技術研究[D].北京交通大學博士學位論文, 2014-07. Du ke-ke. Study on the key technology of north seeking system based on fiber optic gyroscope[D]. Beijing: Beijing Jiaotong University, 2014-07.

[2] 王海明, 李四海. 一種緯度未知的全姿態光纖陀螺尋北方法[J]. 中國慣性技術學報, 2014, 22(1): 63-66. Wang Hai-ming, Li Si-hai. Seeking north method for fiber optic gyroscope north-seeker at all-attitude and unknown latitude[J]. Journal of Chinese Inertial Technology, 2014, 22(1): 63-66.

[3] Duan K K, Li D H, Pei R Z. Study on the error of fourposition north seeking system based on the single axis fiber optic gyroscope[J]. Advanced Materials Research, 2012, 503: 1158-1163.

[4] Ben Y, Chai Y, Gao W, et al. Analysis of error for a rotating strap-down inertial navigation system with fibro gyro[J]. Journal of Marine Science and Application, 2010, 9(4): 419-424.

[5] Lv P, Liu J, Lai J, et al. Decrease in accuracy of a rotational SINS caused by its rotary table’s errors[J]. Int. J. Adv. Robot. Syst, 2014, 11: 1-10.

[6] Sun W, Xu A G, Che L N, et al. Accuracy improvement of SINS based on IMU rotational motion[J]. Aerospace and Electronic Systems Magazine, 2012, 27(8): 4-10.

[7] Gao W, Zhang Y, Wang J. Research on initial alignment and self-calibration of rotary strapdown inertial navigation systems[J]. Sensors, 2015, 15(2): 3154-3171.

[8] Yuan B, Liao D, Han S. Error compensation of an optical gyro INS by multi-axis rotation[J]. Measurement Science and Technology, 2012, 23(2): 025102.

[9] Li Y, Niu X, Zhang Q, et al. An in situ hand calibration method using a pseudo-observation scheme for low-end inertial measurement units[J]. Measurement Science and Technology, 2012, 23(10): 105104.

[10] Hong S, Lee M H, Chun H H, et al. Observability of error states in GPS/INS integration[J]. IEEE Transactions on Vehicular Technology, 2005, 54(2): 731-743.

[11] Li Y, Chen Q, Niu X, et al. Simulation analysis for the influences of vehicle maneuvers to the attitude estimations of GNSS/INS navigation systems[C]//China Satellite Navigation Conference 2012 Proceedings. Springer Berlin Heidelberg, 2012: 679-694.

[12] Li Y, Li Y, Rizos C, et al. Observability analysis of SINS/GPS during in-motion alignment using singular value decomposition[C]//Advanced Materials Research. 2012, 433: 5918-5923.

[13] Wu Y, Zhang H, Wu M, et al. Observability of strapdown INS alignment: A global perspective[J]. IEEE Transactions on Aerospace and Electronic Systems, 2012, 48(1): 78-102.

[14] Noureldin A, Karamat T B, Georgy J. Fundamentals of INS, GPS and their integration[M]. Springer Science & Business Media, 2012: 1-312.

[15] Shin E H. Estimation techniques for low-cost inertial navigation[J]. UCGE report, 2005, 20219.

[16] Niu X, Zhang Q, Gong L, et al. Development and evaluation of GNSS/INS data processing software for position and orientation systems[J]. Survey Review, 2015, 47(341): 87-98.

[17] 朱奎寶, 張春熹, 宋凝芳. 光纖陀螺角度隨機游走對慣導系統影響[J]. 壓電與聲光, 2007, 29(3): 292-294. Zhu Kui-bao, Zhang Chun-xi, Song Ning-fang. Effection of angle random walk of fiber optic gyro(FOG) on INS[J]. Piezoelectrics & Acoustooptics, 2007, 29(3): 292-294.

North-seeking based on in-situ rotation-modulation using pseudo-observation scheme without precise turntable

NIU Xiao-ji1, WANG Qiang1, LI You1,2, TANG Jian1
(1. GNSS Center, Wuhan University, Wuhan 430079, China; 2. University of Calgary, Calgary T2N1N4, Canada)

Traditional multi-position (e.g., four-position) and rotation-modulation based north-seeking methods require a sophisticated indexing mechanism (e.g., a precise turntable), which results in high volume, weight, power consumption and costs. In this paper, an in-situ north-seeking method based on rotation-modulation is put forward, which does not rely on any precise mechanical rotation device. With the aid of an integrated inertial navigation algorithm, the proposed north-seeking method can realize such a north-seeking as its accuracy and efficiency are equivalent to those by the four-position method, in which it adopts such a pseudo-observation as the in-situ rotation of IMU is supposed to have limited position and speed variation. Since the pseudoobservation do not require precise IMU angle relative to the base during the whole north-seeking process, the precision angle and angular velocity measurement devices can be essentially eliminated. A large amount of tests (almost 100 groups) indicate that the accuracy can achieve ＜1° within 4 min when using an IMU equipped with fiber optic gyros that have a bias stability of less than 0.5 (°)/h, and the hardware requirement and costs can be significantly reduced compared with that by traditional north-seeking approaches.

north-seeking; Kalman filter; rotation-modulation; pseudo-observation; integrated navigation

U666.1

：A

2015-09-28；

：2015-11-28

中央高校基本科研業務費專項資金資助項目（2042014KF0258）；國家自然科學基金資助項目（41304004，41174028）

牛小驥（1973—），男，教授，博士生導師，從事慣性導航和GNSS/INS組合導航技術研究。E-mail: xjniu@whu.edu.cn

聯 系 人：唐健（1981—），男，博士，講師，從事室內定位與測圖研究。E-mail: tangjian@whu.edu.cn

1005-6734(2015)06-0707-07

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