旋轉(zhuǎn)調(diào)制式慣導(dǎo)系統(tǒng)隔離載體運(yùn)動(dòng)算法

孫偉強(qiáng)，曹 東，戚嘉興，劉 偉，張靈妍

（1. 天津航海儀器研究所，天津 300131；2. 中國(guó)人民解放軍72433部隊(duì)自動(dòng)化工作站，濟(jì)南 250014）

旋轉(zhuǎn)調(diào)制式慣導(dǎo)系統(tǒng)隔離載體運(yùn)動(dòng)算法

孫偉強(qiáng)1，曹 東1，戚嘉興2，劉 偉1，張靈妍1

（1. 天津航海儀器研究所，天津 300131；2. 中國(guó)人民解放軍72433部隊(duì)自動(dòng)化工作站，濟(jì)南 250014）

旋轉(zhuǎn)調(diào)制式慣導(dǎo)系統(tǒng)中，轉(zhuǎn)位運(yùn)動(dòng)受載體運(yùn)動(dòng)的影響，誤差調(diào)制效果受影響。針對(duì)這一問(wèn)題，提出基于載體姿態(tài)角解耦的隔離載體運(yùn)動(dòng)算法。首先建立載體運(yùn)動(dòng)姿態(tài)角與旋轉(zhuǎn)機(jī)構(gòu)之間的數(shù)學(xué)關(guān)系，然后設(shè)計(jì)了隔離載體運(yùn)動(dòng)的力矩信號(hào)施加量的實(shí)時(shí)解耦算法，最終推導(dǎo)了隔離載體運(yùn)動(dòng)力矩信號(hào)的公式，完成了載體運(yùn)動(dòng)隔離算法的設(shè)計(jì)。數(shù)字仿真結(jié)果表明，通過(guò)該算法，可以降低載體運(yùn)動(dòng)對(duì)誤差調(diào)制規(guī)律的影響。跑車試驗(yàn)結(jié)果表明，通過(guò)隔離載體運(yùn)動(dòng)算法的應(yīng)用，轉(zhuǎn)向?qū)ο到y(tǒng)定位誤差影響由不隔離載體運(yùn)動(dòng)時(shí)約0.5 nm降低到了基本為0 nm。

旋轉(zhuǎn)調(diào)制；捷聯(lián)慣性導(dǎo)航系統(tǒng)；隔離載體運(yùn)動(dòng)；姿態(tài)角解耦算法

旋轉(zhuǎn)調(diào)制式慣導(dǎo)系統(tǒng)通過(guò)對(duì) IMU的旋轉(zhuǎn)來(lái)調(diào)制慣性元件的誤差[1-6]。IMU的角運(yùn)動(dòng)實(shí)際是系統(tǒng)轉(zhuǎn)位運(yùn)動(dòng)與載體運(yùn)動(dòng)兩種運(yùn)動(dòng)的復(fù)合，而真正起到誤差調(diào)制作用的是相對(duì)于地理坐標(biāo)系的系統(tǒng)轉(zhuǎn)位運(yùn)動(dòng)，如果不隔離載體運(yùn)動(dòng)，二者混在一起，將大大影響動(dòng)態(tài)條件下的誤差調(diào)制效果[7-10]。

針對(duì)這一問(wèn)題，本文提出基于載體姿態(tài)角解耦的載體運(yùn)動(dòng)隔離算法，研究載體姿態(tài)角變化與旋轉(zhuǎn)機(jī)構(gòu)之間的關(guān)系，設(shè)計(jì)載體運(yùn)動(dòng)姿態(tài)角的實(shí)時(shí)解耦算法，形成載體運(yùn)動(dòng)隔離算法，并進(jìn)行仿真和跑車試驗(yàn)。

1 姿態(tài)角解耦原理及算法

1.1 姿態(tài)角解耦原理

提取的載體姿態(tài)角是載體在當(dāng)?shù)氐乩碜鴺?biāo)系中的三維角度。與IMU固連的臺(tái)體相對(duì)于導(dǎo)航坐標(biāo)系在不斷進(jìn)行轉(zhuǎn)位和停止運(yùn)動(dòng)，在停止時(shí)臺(tái)體相對(duì)于當(dāng)?shù)氐乩碜鴺?biāo)系保持靜止。某型慣性導(dǎo)航系統(tǒng)的框架配置示意圖如圖 1所示：最內(nèi)部的橢圓形表示慣性測(cè)量組件，稱之為臺(tái)體；與臺(tái)體相連的框架稱之為內(nèi)框，與內(nèi)框和外部相連的為外框；轉(zhuǎn)位機(jī)構(gòu)即電機(jī)的軸分別位于臺(tái)體軸、內(nèi)框軸和外框軸上。從圖 1中可以看出，由于轉(zhuǎn)位運(yùn)動(dòng)的存在，陀螺儀的輸入軸是在不斷變化的，使得陀螺儀輸入軸和裝在框架軸上的電機(jī)軸不平行，因而所需繞陀螺儀輸入軸的運(yùn)動(dòng)必需由繞三根框架軸的運(yùn)動(dòng)來(lái)保證。

圖1 某型慣性導(dǎo)航系統(tǒng)框架配置示意圖Fig.1 Frame configuration of an inertial navigation system

1.2 姿態(tài)角解耦算法

臺(tái)體坐標(biāo)系輸入軸的運(yùn)動(dòng)是已知的，求取繞三根架軸的運(yùn)動(dòng)與平臺(tái)式系統(tǒng)類似，通過(guò)以下推導(dǎo)獲取。設(shè)為與載體固連的坐標(biāo)系，為與外框架固連的坐標(biāo)系，為與內(nèi)框架固連的坐標(biāo)系，為與臺(tái)體固連的坐標(biāo)系。定義外框軸讀角為k，內(nèi)框軸讀角為ψ，臺(tái)體軸讀角為θ，則經(jīng)過(guò)以下規(guī)則變換可變換為繞軸正向旋轉(zhuǎn)k角得外框架坐標(biāo)系轉(zhuǎn)換矩陣記為繞Xo軸正向旋轉(zhuǎn)ψ角得內(nèi)框架坐標(biāo)系轉(zhuǎn)換矩陣記為繞YI軸旋轉(zhuǎn)θ角得臺(tái)體坐標(biāo)系轉(zhuǎn)換矩陣記為根據(jù)旋轉(zhuǎn)坐標(biāo)系之間的關(guān)系，有：

從而得到臺(tái)體坐標(biāo)系與載體坐標(biāo)系之間的關(guān)系為：

內(nèi)框坐標(biāo)系各軸角速度與外框坐標(biāo)系各軸角速度之間有如下關(guān)系：

從而有：

根據(jù)定義，三只電機(jī)分別在外框坐標(biāo)系的OZ、內(nèi)框坐標(biāo)系的OX軸以及臺(tái)體坐標(biāo)系的OY軸上，而需要求取的關(guān)系為：在已知臺(tái)體坐標(biāo)系各軸角速度的情況下，推導(dǎo)出其與三只電機(jī)角速度之間的轉(zhuǎn)換關(guān)系。考慮到則有：

三只電機(jī)構(gòu)成的坐標(biāo)系記為S系，為非正交坐標(biāo)系，則有：

對(duì)其求逆，即可得到臺(tái)體坐標(biāo)系到電機(jī)坐標(biāo)系之間的轉(zhuǎn)換矩陣為：

即：

式(2)的關(guān)系可以用圖2來(lái)表示。采用圖2所示的關(guān)系可以將臺(tái)體軸上陀螺儀的角速度轉(zhuǎn)換到三個(gè)軸的電機(jī)的角速度。

圖2 力矩信號(hào)解耦關(guān)系圖Fig.2 Decoupling relationship of the moment signals

2 隔離載體運(yùn)動(dòng)算法設(shè)計(jì)及仿真

2.1 隔離載體運(yùn)動(dòng)算法設(shè)計(jì)

根據(jù)以上研究，對(duì)旋轉(zhuǎn)調(diào)制式系統(tǒng)隔離載體運(yùn)動(dòng)算法進(jìn)行了設(shè)計(jì)分析，最終確定了旋轉(zhuǎn)調(diào)制式系統(tǒng)隔離載體運(yùn)動(dòng)算法。

陀螺儀能感受到載體相對(duì)于慣性空間的運(yùn)動(dòng)。將陀螺儀的輸出經(jīng)過(guò)解調(diào)、校正、調(diào)制及功率放大后，反饋輸入給框架軸上的力矩電機(jī)，產(chǎn)生恢復(fù)力矩，直接或通過(guò)減速器以平衡作用于載體運(yùn)動(dòng)的干擾，保持框架角方位相對(duì)于慣性空間保持不變，給框架電機(jī)施加力矩的公式如式(3)所示，由于框架角方位相對(duì)于慣性空間保持不變，這就是慣性穩(wěn)定平臺(tái)的工作原理。

艦船慣性導(dǎo)航系統(tǒng)通常采用半解析式系統(tǒng)的方案，相對(duì)于慣性平臺(tái)式系統(tǒng)的工作原理，半解析式系統(tǒng)的臺(tái)體需要跟蹤當(dāng)?shù)氐乩碜鴺?biāo)系，規(guī)定當(dāng)?shù)氐乩碜鴺?biāo)系為東、北、天坐標(biāo)系，當(dāng)?shù)氐乩硐档慕撬俾释ㄟ^(guò)姿態(tài)轉(zhuǎn)換矩陣可以投影到臺(tái)體坐標(biāo)系上，姿態(tài)轉(zhuǎn)換矩陣由系統(tǒng)的捷聯(lián)解算提供，則當(dāng)系統(tǒng)穩(wěn)定在當(dāng)?shù)氐乩碜鴺?biāo)系下時(shí)，力矩電機(jī)的施加力矩如式(4)所示：

誤差調(diào)制式系統(tǒng)中的轉(zhuǎn)位方案是相對(duì)于導(dǎo)航坐標(biāo)系進(jìn)行設(shè)計(jì)編排的，陀螺儀和加速度計(jì)只有相對(duì)于導(dǎo)航坐標(biāo)系按轉(zhuǎn)位方案進(jìn)行旋轉(zhuǎn)，才能夠達(dá)到誤差調(diào)制的效果。在誤差調(diào)制式慣導(dǎo)系統(tǒng)中，采用了當(dāng)?shù)氐乩碜鴺?biāo)系作為導(dǎo)航坐標(biāo)系，由于陀螺儀與加速度計(jì)與臺(tái)體坐標(biāo)系固連，因此陀螺儀與加速度計(jì)的轉(zhuǎn)位規(guī)律即臺(tái)體的轉(zhuǎn)動(dòng)角速率，記為wa，則誤差調(diào)制式系統(tǒng)的隔離載體運(yùn)動(dòng)算法如式(5)所示：

2.2 隔離載體運(yùn)動(dòng)算法仿真試驗(yàn)

上面對(duì)基于姿態(tài)角解耦的隔離載體運(yùn)動(dòng)算法的原理及方法給出了細(xì)致的闡述說(shuō)明，本節(jié)針對(duì)提出的隔離載體運(yùn)動(dòng)算法進(jìn)行仿真驗(yàn)證試驗(yàn)。仿真試驗(yàn)?zāi)Ｐ椭邪ㄝd體運(yùn)動(dòng)環(huán)境的模擬、誤差調(diào)制式系統(tǒng)陀螺儀與加速度計(jì)數(shù)據(jù)生成的模型、捷聯(lián)解算以及姿態(tài)角提取模型、隔離載體運(yùn)動(dòng)算法模型，仿真框圖如圖3所示。

圖3 隔離載體運(yùn)動(dòng)算法仿真框圖Fig.3 Simulation frame of carrier movement isolation algorithm

圖4～6依次為無(wú)搖擺旋轉(zhuǎn)調(diào)制系統(tǒng)z軸陀螺儀輸出曲線、有搖擺無(wú)隔離旋轉(zhuǎn)調(diào)制系統(tǒng)z軸陀螺儀輸出曲線和有搖擺有隔離旋轉(zhuǎn)調(diào)制系統(tǒng)z軸陀螺儀輸出曲線。通過(guò)對(duì)比可以看出，通過(guò)引入隔離載體運(yùn)動(dòng)算法，外界運(yùn)動(dòng)對(duì)陀螺儀輸出影響基本隔離掉了，保證了旋轉(zhuǎn)調(diào)制規(guī)律。

圖4 無(wú)搖擺陀螺輸出曲線Fig.4 Gyro output without swing

圖 5 有搖擺未隔離時(shí)陀螺輸出曲線Fig.5 Gyro output with swing and without isolating

圖6 有搖擺采取隔離陀螺輸出曲線Fig.6 Gyro output with swing and isolating

3 試驗(yàn)驗(yàn)證

采用隔離載體運(yùn)動(dòng)算法，在某型旋轉(zhuǎn)調(diào)制式慣性導(dǎo)航系統(tǒng)中進(jìn)行了跑車試驗(yàn)進(jìn)行驗(yàn)證，試驗(yàn)采用同一套旋轉(zhuǎn)式系統(tǒng)，試驗(yàn)車輛也為同一輛。試驗(yàn)過(guò)程為：對(duì)準(zhǔn)完成后開始進(jìn)行導(dǎo)航，導(dǎo)航過(guò)程中載體有運(yùn)動(dòng)變化，結(jié)果如圖7和圖8所示。圖中分別給出了未采用隔離方案和采用隔離方案的定位結(jié)果與航向變化的對(duì)比，從中可以看出：未采用隔離運(yùn)動(dòng)方案時(shí)，載體轉(zhuǎn)向機(jī)動(dòng)約對(duì)定位結(jié)果有0.5 nm的影響；采用隔離運(yùn)動(dòng)方案后，載體機(jī)動(dòng)對(duì)系統(tǒng)定位結(jié)果基本無(wú)影響。隔離載體運(yùn)動(dòng)方案大大提高了系統(tǒng)的動(dòng)態(tài)適應(yīng)性能。

圖7 未隔離定位誤差與航向曲線Fig.7 Positioning error and heading without isolating

圖 8 隔離載體運(yùn)動(dòng)定位誤差與航向曲線Fig.8 Positioning error and heading with isolating

4 結(jié) 論

通過(guò)數(shù)字仿真和跑車試驗(yàn)驗(yàn)證，說(shuō)明所設(shè)計(jì)的基于姿態(tài)角解耦的載體運(yùn)動(dòng)隔離算法可以實(shí)現(xiàn)旋轉(zhuǎn)調(diào)制式慣導(dǎo)系統(tǒng)中對(duì)載體運(yùn)動(dòng)的隔離，保證誤差調(diào)制規(guī)律的實(shí)現(xiàn)，提高系統(tǒng)定位精度。

（References）：

[1]Yin H L, Yang G L, Song N F, et al. Error modulation scheme analyzing for dual-axis rotating fiber-optic gyro inertial navigation system[J]. Sensor Letters, 2012, 10(7):1359-1363.

[2]Song N F, Cai Q Z, Yang G L, et al. Analysis and calibration of the mounting errors between inertial measurement unit and turntable in dual-axis rotational inertial navigation system[J]. Measurement Science and Technology, 2013,24(11): 115002-1-115002-10.

[3]Tucker T, Levison E. The AN/WSN-7B marine gyrocompass navigator[C]//Proc. of the National Technical Meeting of the Institute of Navigation. 2000: 348-357.

[4]Chang Guo-bin, Xu Jiang-ning, Li An, et al. Error analysis and simulation of the dual-axis rotation-dwell autocompensation strapdown inertial navigation system[C]//International Conference on Measuring Technology and Mechatronics Automation. 2010.

[5]Levinson E, Majure R. Accuracy enhancement techniques applied to the marine ring laser inertial navigator (MARLIN)[J]. Journal of The Institute of Navigation, 1987, 34(1):64-85.

[6]Ishibashi S, Tsukioka S, Yoshida H. Accuracy improvement of an inertial navigation system brought about by the rotational motion[J]. Oceans, 2007, 25(10): 1-5.

[7]唐江河, 李文耀, 詹雙豪, 等. 一種外環(huán)水平結(jié)構(gòu)雙軸光纖慣導(dǎo)系統(tǒng)旋轉(zhuǎn)方案設(shè)計(jì)方法[J]. 導(dǎo)航定位與授時(shí),2016, 4(3): 1-8.Tang Jiang-he, Li Wen-yao, Zhan Shuang-hao, et al.Rotation scheme design method for dual-axis ins with horizontal outer-axis structure[J]. Navigation Positioning and Timing, 2016, 4(3): 1-8.

[8]常國(guó)賓, 許江寧, 李安, 等. 載體運(yùn)動(dòng)對(duì)雙軸連續(xù)旋轉(zhuǎn)調(diào)制式慣導(dǎo)方案誤差的影響[J]. 中國(guó)慣性技術(shù)學(xué)報(bào),2011, 19(2): 175-179.Chang Guo-bin, Xu Jiang-ning, Li An et al. Influence of body dynamics on the error of INS scheme with dual-axis continuous autocompensation technique[J]. Journal of Chinese Inertial Technology, 2011, 19(2): 175-179.

[9]張倫東, 練軍想, 胡小平. 載體角運(yùn)動(dòng)對(duì)旋轉(zhuǎn)式慣導(dǎo)系統(tǒng)旋轉(zhuǎn)調(diào)制效果的影響[J]. 國(guó)防科技大學(xué)學(xué)報(bào), 2011,33(4): 152-156.Zhang Lun-dong, Lian Jun-xiang, Hu Xiao-ping. The effect of vehicle angle motion on rotation modulation technology for rotating INS[J]. Journal of National University of Defense Technology, 2011, 19(2): 175-179.

[10]覃方君, 李安, 徐江寧. 載體角運(yùn)動(dòng)對(duì)旋轉(zhuǎn)調(diào)制慣導(dǎo)系統(tǒng)誤差影響分析[J]. 武漢大學(xué)學(xué)報(bào)(信息科學(xué)版), 2012,37(7): 831-833.Qin Fang-jun, Li An, Xu Jiang-ning. Analysis of errors of rotating modulation ins effected by angular motion of vehicle[J]. Geomatics and Information Science of Wuhan University, 2012, 37(7): 831-833.

[11]王國(guó)臣, 袁保倫, 饒谷音, 等. 激光陀螺雙軸旋轉(zhuǎn)導(dǎo)航系統(tǒng)高精度轉(zhuǎn)動(dòng)控制的設(shè)計(jì)與實(shí)現(xiàn)[J]. 中國(guó)慣性技術(shù)學(xué)報(bào), 2015, 23(4): 438-441.Wang Guo-chen, Yuan Bao-lun, Rao Gu-yin, et al. Highprecision rotating scheme for laser gyroscope dual-axis rotating inertial navigation system[J]. Journal of Chinese Inertial Technology, 2015, 23(4): 438-441.

Carrier movement isolation algorithm for rotary-modulation inertial navigation system

SUN Wei-qiang1, CAO Dong1, QI Jia-xing2, LIU Wei1, ZHANG Ling-yan1
(1. Tianjin Navigation Instrument Research Institute, Tianjin 300131, China;2. Automation Station in Unit 72433 of PLA, Jinan 250031, China)

In rotary-modulation INS (inertial navigation system), the rotary modulation effect is affected by the carrier movement. An algorithm for isolating the carrier movement was brought up to solve this problem.First, the mathematical relationship between the carrier movement attitude angles and the executing components was established. Then the real-time decoupling algorithm of the moment signals for isolating the movement was designed. At last, the formula for isolating the moment signals of the carrier movement was derived, and the design of the carrier movement isolation algorithm was finished. Numerical simulations verify that this algorithm could reduce the influence of the carrier movement on the rotary modulation. The vehicle running experiment shows that, with this algorithm, the positioning error caused by the turning is reduced to almost 0 nm from 0.5 nm.

rotary modulation; strapdown inertial navigation system; carrier movement isolation; attitude decoupling algorithm

1005-6734(2017)04-0432-04

10.13695/j.cnki.12-1222/o4.2017.04.002

U661.1

A

2017-04-27；

2017-07-27

裝備預(yù)先研究項(xiàng)目（3020107010202）

孫偉強(qiáng)（1984— ），男，工學(xué)碩士，高工，從事捷聯(lián)式慣性導(dǎo)航系統(tǒng)研究。E-mail: sunwq1984@163.com