旋轉式慣導系統光纖陀螺在線自標定方法

周元，鄧志紅，王博，付夢印

(北京理工大學 自動化學院，北京100081)

精確標定慣性器件的零偏、標度因數誤差、安裝誤差等并加以補償是提高慣導系統精度的關鍵一步.對這些參數除了在實驗室標定外，還可以在現場標定.現場標定是在慣性導航系統處于載體安裝狀態下進行的，相對于實驗室標定來說，現場標定環境更接近于系統的實際工作環境，針對性強，對于提高慣導系統的實際使用精度意義重大.但是在線標定無法利用轉臺等試驗設備，只能依靠重力場、地球自轉角速度、載體的運動及載體上其他輔助導航設備(如衛星接收機、里程計等)來完成.傳統的光纖陀螺在線標定方案是將其零偏列入初始對準狀態向量，采用Kalman濾波估計［1］，該方法通常選擇導航系統速度誤差作為外觀測量，在靜基座條件下可用系統實時解算的速度代替［2］.但以地速誤差為外觀測量的初始對準Kalman濾波的可觀測性低［1］，濾波估計的效果不理想.

近年來，誤差旋轉調制技術逐漸成為捷聯慣導領域的研究熱點.旋轉式慣導系統具有與平臺式系統類似的框架和轉軸，但功能不同，僅相當于在捷聯慣導系統外部加上轉動機構和測角裝置，通過使慣性測量單元(inertial measurement unit，IMU)旋轉來抑制慣性器件誤差的積累，成本較平臺式系統大幅降低［3-4］.旋轉式IMU可控制的角運動特性為光纖陀螺在線自主標定提供了新思路.利用IMU旋轉(包括連續旋轉和多位置轉停)可改善初始對準可觀測性從而提高對慣性器件誤差參數的估計精度［5-7］，但旋轉機構的誤差也將引入新的誤差源［8］.為實現光纖陀螺誤差的在線標定，本文根據旋轉式IMU的光纖陀螺輸出模型特點，提出了一種易于實現的基于遞推最小二乘(recursive least square，RLS)估計的光纖陀螺誤差參數高精度在線快速標定方案，并給出基于硬件系統的實現方法.仿真結果表明了所提出的方案的有效性，為該算法的工程化實現奠定了基礎.

1 旋轉式IMU光纖陀螺解耦RLS估計標定方案

1.1 光纖陀螺的在線標定數學模型

導航系統平臺(包括物理平臺和數學平臺)漂移主要是由陀螺的安裝誤差、標度因數誤差和漂移誤差等引起的［9］，其中安裝誤差、高次項誤差和噪聲等復雜因素一般難以估算，在線標定中可以忽略.慢變漂移分量會隨著工作過程中各種條件的改變發生緩慢變化.陀螺的逐次啟動漂移取決于陀螺啟動時的各種隨機因素，一旦啟動完成，改漂移便成為一個固定的常數［10］，標度因數誤差也可以用一個隨機常數來描述.因此在初始對準階段較短時間內需要進行標定的誤差項即為陀螺的逐次啟動漂移和陀螺的標度因數誤差，由此得到光纖陀螺在線標定數學模型:

式中:ωi和ωo分別為光纖陀螺的輸入角速度和輸出角速度，δK為標度因數誤差，ε為零偏，wg為隨機噪聲.

1.2 旋轉式IMU光纖陀螺解耦RLS估計數學模型

在旋轉式捷聯慣導系統中，從IMU坐標系到載體坐標系的方向余弦矩陣(稱為旋轉矩陣)數學模型由旋轉機構的結構和旋轉方案決定，對于內環轉軸與IMU坐標系的x軸重合，外環轉軸與載體坐標系z軸重合的雙軸旋轉式慣導系統，令內環軸、外環軸周期性連續旋轉，每旋轉一周改變轉向，則旋轉矩陣表達式如下:

式中:角標p表示IMU坐標系，其三坐標軸分別與IMU敏感軸重合;αj表示轉軸 j轉過的角度(j=x，z).

在靜基座下，旋轉式慣導系統光纖陀螺所敏感到的角速度為

式中:角標i、n、e分別表示地心慣性坐標系、地理坐標系(選為導航坐標系)、地球坐標系;L為初始對準時載體所在緯度位置，ωie為地球自轉角速率，ωrx和ωrz分別為轉軸x和轉軸z的旋轉角速度.

對于雙軸旋轉式慣導系統，可以仿照平臺式系統對其進行粗調平，使俯仰角、橫滾角近似為零，而方位角可以通過解析法近似求得，則姿態矩陣可以寫成以下形式:

根據式(1)可以得到旋轉式捷聯慣導系統靜基座下光纖陀螺的輸出模型為

式中:“～”表示該參數為光纖陀螺的測量值，下標x、y、z表示IMU坐標系的坐標軸方向;δKj為光纖陀螺標度因數誤差，ωipj為IMU相對慣性空間的角速度在IMU坐標系下的分量，εj和wgj分別表示陀螺的零偏和噪聲(j=x，y，z).

將式(2)～(4)代入式(5)，其中參數 φk、εj、δKj、ωie均為小量，將由它們相乘得到的二階、三階小量忽略，可得旋轉式捷聯慣導系統靜基座下光纖陀螺的輸出模型的簡化形式:

式中:L的值已知，ωrx、ωrz與 αx、αz可以分別從旋轉機構的角速度傳感器與角度傳感器實時測得，由此可以得到各陀螺零偏與標度因數誤差的線性組合的觀測量.式(6)的簡化近似略去了IMU陀螺組各通道之間的耦合，則每個光纖陀螺可視為一個獨立的通道，取其零偏和標度因數誤差作為狀態量，采集光纖陀螺輸出值進行最小二乘估計，可以實現光纖陀螺在線標定.各通道的觀測量分別為

為充分利用陀螺測量值，使用RLS估計以提高估計精度.根據旋轉IMU的光纖陀螺解耦輸出模型，各通道數據最小二乘估計解算流程相同，其遞推公式為

應用最小二乘標定時，取狀態向量X初值為零，Pj(0)和Rj分別按照光纖陀螺的標稱值取值，其中，Pj(0)為對角矩陣，它的對角元素為光纖陀螺的零偏和標度因數誤差的平方，Rj為光纖陀螺噪聲方差.

1.3 光纖陀螺誤差解耦RLS算法估計性能分析

對于在地球表面運動的車輛、艦船等載體，光纖陀螺捷聯式慣導系統初始對準的Kalman濾波狀態方程可寫為

式中:X為慣導系統初始對準狀態向量，WV為系統噪聲向量，系統狀態矩陣與系統噪聲系數矩陣中各子陣表達式如下:

式中:下標b表示載體坐標系，其坐標軸x、y、z分別指向載體右、前、上;R為地球半徑;c11，c12，…，c23為載體姿態矩陣的各元素;Vk表示載體速度，δVk表示其誤差;fk為地理坐標系下的比力分量(k=E，N，U);ωibj為載體相對慣性空間的角運動，Δj和waj分別為加速度計的零偏和噪聲(j=x，y，z).

初始對準中光纖陀螺標定可用解耦RLS估計替代，因此可以將狀態向量中陀螺零偏和標度因數誤差刪除，從而降低狀態方程階數.Kalman濾波器的運算量與其階數的三次方成正比.若系統狀態方程階數為n，觀測方程階數為m，則完成一次遞推計算需要完成4n3+(1+4m)n2+(2m2+2m)n+m3次乘除運算和4n3+(4m-2)n2-(2m+1)n+m3次加法運算［10］.原Kalman濾波狀態方程為14階，觀測方程為2階，而刪除光纖陀螺零偏和標度因數誤差狀態量以后，系統狀態方程為8階，觀測方程為2階，可知降階系統的Kalman濾波乘除法、加法運算量分別下降至原系統的21.12%和19.84%.

對于使用參數辨識法進行初始對準的捷聯慣導系統，如果已知陀螺的等效東向漂移，則可以提高求方位失準角初值的精度［11］.利用最小二乘法標定的陀螺誤差參數求得陀螺東向等效漂移，可以改善參數辨識法對準精度.

2 光纖陀螺誤差參數的解耦RLS估計仿真研究

通過Matlab軟件仿真實驗，比較解耦RLS估計和捷聯慣導初始對準Kalman濾波在光纖陀螺在線標定方面的性能.仿真條件設定如下:靜基座初始對準過程中，載體所在緯度為30°，載體的航向角為30.06°，為 30°，粗調平后俯仰角、橫滾角均為0.06°，光纖陀螺的零偏為0.01°/h，標度因數誤差為10×10-6，陀螺隨機噪聲為標準差為0.005(°)/h的高斯白噪聲.加速度計的零偏為10-4g，噪聲標準差為0.5×10-4g.慣性器件的采樣頻率為100 Hz，其中旋轉式慣導系統的轉軸結構與式(2)系統相同，其雙軸均以5(°)/s的角速度旋轉.分別進行900 s的最小二乘估計和Kalman濾波，光纖陀螺零偏和標度因數誤差的估計誤差如圖1、2所示.

圖1 2種方法陀螺零偏的估計誤差比較Fig.1 The comparison of estimation errors of the two methods about gyro biases

圖2 2種方法陀螺標度因數誤差的估計誤差比較Fig.2 The comparison of estimation errors of the two methods about scale factor errors

在仿真實驗后期(t=450～900 s)，2種方法的估計誤差絕對值的均值如表1.

根據仿真結果可知:相對捷聯慣導初始對準Kalman濾波，旋轉IMU光纖陀螺解耦RLS估計的2種陀螺誤差參數的平均標定誤差分別下降至52.41%和46.68%，證明采用解耦近似模型的RLS估計可以更加快速、準確地對光纖陀螺進行在線標定，從而為導航解算提供更加準確的光纖陀螺誤差軟件補償參數.

對于使用參數辨識法進行初始對準的捷聯慣導系統，求方位失準角初值的方法［11］為

式中，誤差項εE/(ωiecos L)可用標定的陀螺誤差參數進行補償，即估算陀螺東向等效漂移:

如果陀螺零偏未經標定，εE將全部成為計算近似誤差，則按照仿真實驗的參數設置與仿真結果，可求得經補償后由陀螺漂移引起的方位失準角初值參數辨識誤差降低至原來的47.99%.

表1 2種方法的估計誤差絕對值的均值Table 1 Comparison of estimation errors of the two methods

3 自標定算法硬件實現方法研究

光纖陀螺誤差參數標定可以用于導航系統的自主校正，也可用于慣性穩定平臺的初始對準與標定，或對光纖陀螺進行元件級的快速標定，因此可以設計相對獨立的專用數字信號處理嵌入式系統以滿足不同的應用需求.解耦RLS法的矩陣運算階數低，因此具有易于用數字系統實現的優勢.利用運算硬件模塊的分時復用技術［12］，可以節省硬件資源，體現算法的工程應用優勢.

3.1 RLS估計的程序編排

為實現算法，需將其整理成與離散化的程序編排形式.令式(8)中，各矩陣的元素如下:

代入式(12)的遞推計算公式，可以得到陀螺誤差參數估計值和P矩陣的遞推計算公式:

其中:

按上述解算程序迭代計算，即可實現對旋轉式慣導系統IMU的光纖陀螺誤差參數的RLS估計.

3.2 基于FPGA的運算電路實現

易于在結構化的硬件數字系統中實現是本文算法的特色之一.通過算法的解耦簡化，最小二乘運算矩陣階數降至二階，提高了設計結構化邏輯電路的可行性.由于各陀螺通道最小二乘法的I/O數據格式與解算程序相同，因此可用同一運算電路分時處理三通道數據，從而節省2/3的邏輯資源.Xilinx公司的System Generator是一種結構化、模塊化的FPGA開發工具，本文基于此軟件工具實現基于解耦RLS的標定算法，系統的解算數據流程如圖3.系統的各主要功能模塊的作用和實現方法如下:

1)時序控制模塊.在每個解算周期的不同時期，時序控制模塊依次向數據預處理模塊、數據選擇模塊、讀寫控制模塊提供使能信號.

2)輸入接口模塊.分別采集陀螺儀、轉軸角速度傳感器、角位置傳感器輸出值，同時從導航計算機讀取裝訂信息(載體緯度、粗對準航向角).其中陀螺輸出值和角速度并行讀取，而L、αx、αz在每個解算周期開始串行讀取.

圖3 RLS估計陀螺在線標定系統解算數據流圖Fig.3 The computation data flow of RLS estimation based online gyro calibration system

3)數據預處理模塊.每個解算周期的前100個時鐘周期為數據預處理階段，按式(7)計算各通道外觀測量和觀測矩陣:

對復雜的正余弦IP核進行分時復用，串行輸入L、αx、αz并用2 個時分多路開關 IP 核分別對正、余弦輸出進行串并轉換，可節省3/4的正余弦IP核邏輯資源.

4)數據選擇模塊.提供實現分時復用的多路開關［13］的控制信號:用3個多路開關分別接收預處理模塊輸出數據，進行并串轉換，以節省邏輯資源;用1個多路開關接收RLS分子模塊輸出的X、P元素數據.

5)讀寫控制模塊與存儲器組模塊.控制存儲器組模塊用6個RAM存儲6個X、P矩陣元素數據以實現遞推解算，與數據選擇模塊同步讀取相應通道的x1、x2、p11、p12、p21、p22，并將除法器運算結果分別寫入相應RAM的相應通道地址.在x1和x2的RAM的輸出端設置外部接口，在相應的時序即可讀取光纖陀螺誤差參數估計值.

6)RLS解算模塊組.由RLS分子、分母模塊、除法器模塊組成.通過多路開關將各分子參數串行輸入除法器與公共的分母相除，可以節省5/6的除法器邏輯資源，得到實時的狀態量估計值與P.

在System Generator開發環境下實現設計并進行仿真驗證，仿真參數設置與上文仿真條件相同，而其中遞推解算周期按 FPGA系統設定為0.1 ms，經過0.5 s的RLS估計(解算5 000次)，得到的光纖陀螺零偏誤差參數估計值分別為0.006 249(°)/h，0.006 353(°)/h，0.009 314(°)/h，標度因數誤差參 數 估 計 值 分 別 為9.065 5× 10-6，9.047 2×10-6，9.047 9×10-6.實驗結果表明基于FPGA實現的解耦RLS估計數字信號處理系統可以實現對旋轉IMU中光纖陀螺的零偏與標度因數誤差的在線標定.

4 結論

本文通過對旋轉IMU中光纖陀螺輸出模型的解耦簡化，建立其在線自主標定數學模型，設計了光纖陀螺零偏和標度因數誤差RLS估計算法及標定方案.仿真實驗表明，相對于捷聯慣導靜基座Kalman濾波對準方法，可提高光纖陀螺誤差參數的估計精度.

利用標定參數可改善初始對準性能，對于Kalman濾波初始對準.利用標定參數進行模型降階，可使Kalman濾波運算量降低約80%.對于參數辨識法初始對準，陀螺誤差的標定值可使由陀螺漂移引起的方位失準角初值的近似誤差降低50%以上.

本文算法具有易于硬件系統實現的優勢，根據各陀螺通道解算程序相同的特點，利用存儲器和時分多路開關實現RLS解算模塊的分時復用，相關邏輯資源可以節省2/3.在外觀測量初始化模塊和RLS解算模塊中分別對三角函數和除法器IP核分時復用，實現正余弦運算和除法運算的邏輯資源可分別節省3/4和5/6.本文基于FPGA實現了解耦RLS估計算法，為其在導航系統中的實現奠定了基礎.

［1］GAO Wei，BEN Yueyan，ZHANG Xin，et al.Rapid fine strap down INS alignment method under marine mooring condition［J］.IEEE Transactions on Aerospace and Electronic Systems，2011，47(4):2887-2896.

［2］何昆鵬，許德新，吳簡彤，等.船用捷聯慣性導航系統在系泊狀態下快速初始對準與標定［J］.哈爾濱工程大學學報，2008，29(9):944-950.HE Kunpeng，XUDexin，WU Jiantong，et al.Initial rapid alignment/calibration of amarine strap down inertial navigation system inmoorage［J］.Journal of Harbin Engineering University，2008，29(9):944-950.

［3］SUN Feng，SUN Wei，GAO Wei，et al.Research on the technology of rotational motion for FOG strapdown inertial navigation system［C］//Proceedings of IEEE International Conference on Mechatronics and Automation.Changchun，China，2009:4913-4918.

［4］BEN Yueyang，WU Xiao，CHAIYongli，et al.Research on error modulating of sin S based on sin gle-axis rotation［C］//Proceedings of International Conference on Complex Medical Engineering.Harbin，China，2011:293-297.

［5］WU Yuanxin，ZHANG Hongliang，WU Meiping，et al.Observability of strapdown INS alignment:a global perspective［J］.IEEE Transactions on Aerospace and Electronic Systems，2012，48(1):78-102.

［6］PECHT E，MINTCHEV M P.Observability analysis for INS alignment in horizontal drilling［J］.IEEE Transactions on Instrumentation and Measurement，2007，56(5):1935-1945.

［7］PECHT E，MINTCHEV M P.Modeling of Observability during in-drilling alignment for horizontal directional drilling［J］.IEEE Transactions on Instrumentation and Measurement，2007，56(5):1946-1954.

［8］LAI Jizhou，LV Pin，LIU Jianye，et al.Analysis of coning motion caused by turntable＇s vibration in rotation inertial navigation system［C］//Proceedings of IEEE/ION Position Location and Navigation Symposium.Nanjing，China，2012:869-876.

［9］陳世同，孫楓，李緒友.集成光學相位調制器相位漂移補償方法研究［J］.哈爾濱工程大學學報，2008，29(1):45-49.CHEN Shitong，SUN Feng，LI Xuyou.Compensation for phase drift in an integrated optical phase modulator［J］.Joumal of Harbin Engineering University，2008，29(1):45-49.

［10］張國良，曾靜.組合導航原理與技術［M］.西安:西安交通大學出版社，2008:150-152.ZHANG Guoliang，ZHENG Jing.Principles and technology of integration navigation［M］.Xi’an:Xi’an Jiaotong University Press，2008:150-152.

［11］秦永元.慣性導航［M］.北京:科學出版社，2006:363-373.QIN Yongyuan.Inertial navigation［M］.Beijing:Science Press，2006:363-373.

［12］RUAN Aiwu，LIAO，Yongbo，LI Ping，et al.An ALU-based universal architecture for FIR filters［C］//Proceedings of 2009 International Conference on Communications，Circuits and Systems，Milpitas，CA，2009:1070-1073.

［13］BENLACHTAR Y，WATTSPM，BOUZIANER，et al.Real-time digital signal processing for the generation of optical orthogonal frequency-division-multiplexed signals［J］.IEEE Journal of Selected Topics in Quantum Electronics，2010，16(5):1235-1244.