蔣鴻翔，丁祝順，王媛媛，盧小東

（北京航天控制儀器研究所，北京100039）

大型平臺的慣性穩(wěn)定與地理坐標系姿態(tài)跟蹤

蔣鴻翔，丁祝順，王媛媛，盧小東

（北京航天控制儀器研究所，北京100039）

介紹了一種大型兩軸穩(wěn)定平臺的慣性穩(wěn)定和地理系姿態(tài)跟蹤原理，建立了穩(wěn)定平臺俯仰框架和橫滾框架的運動學(xué)模型和動力學(xué)模型，進行了穩(wěn)定平臺橫滾通道的角速度回路和角位置回路設(shè)計，仿真分析了姿態(tài)角測量誤差作用下的穩(wěn)定平臺姿態(tài)跟蹤性能，與利用加速度計反饋實現(xiàn)調(diào)平的兩軸阻尼穩(wěn)定平臺進行對比，比對結(jié)果驗證了本文設(shè)計的控制系統(tǒng)在水平姿態(tài)跟蹤速度和抗干擾能力上的優(yōu)勢。平臺樣機進行了姿態(tài)跟蹤測試，結(jié)果驗證了結(jié)合定位定向系統(tǒng)（POS）的穩(wěn)定平臺慣性穩(wěn)定和姿態(tài)跟蹤控制方法可行。

大型穩(wěn)定平臺；運動學(xué)模型；動力學(xué)模型；慣性穩(wěn)定；水平姿態(tài)跟蹤

0 引言

隨著大型測量儀器在航空物探、航空遙感等領(lǐng)域的廣泛使用，大型慣性穩(wěn)定平臺的應(yīng)用也越來越廣泛。在這些系統(tǒng)中，慣性穩(wěn)定平臺的主要作用是：1）由于飛機在飛行過程中易受氣流或者發(fā)動機振動等因素影響，產(chǎn)生機體低頻角運動與高頻線振動，穩(wěn)定平臺可以有效隔離機體運動對載荷測量軸的干擾。2）由于飛機在飛行過程中載荷的水平姿態(tài)不為0以及航向始終變化，穩(wěn)定平臺可以實現(xiàn)在一段航線內(nèi)載荷測量軸對水平及特定航向的自動保持。

目前在國內(nèi)外航空遙感系統(tǒng)中，基于陀螺穩(wěn)定的中小型成像相機坐架已實現(xiàn)了商業(yè)化［1-2］，而在國外?？罩亓y量系統(tǒng)中，穩(wěn)定平臺的形式正由傳統(tǒng)的基于加速度計的阻尼式水平穩(wěn)定平臺，向利用基于組合導(dǎo)航原理的定位定向系統(tǒng)（POS）實現(xiàn)快速姿態(tài)跟蹤的新型穩(wěn)定平臺發(fā)展［3-5］。本文研究的大型慣性穩(wěn)定平臺應(yīng)用于航空重力測量，為兩軸慣性穩(wěn)定平臺。穩(wěn)定平臺自重大，所承載的重力傳感器質(zhì)量也大。驅(qū)動裝置采用可輸出100Nm以上峰值力矩的力矩電機。敏感元件采用POS，基于慣性/衛(wèi)星組合導(dǎo)航原理，由高性能導(dǎo)航計算機采集陀螺、加速度計和GPS信號，解算得到與其固連的重力傳感器在當(dāng)?shù)氐乩碜鴺讼迪碌母┭觥M滾和偏航姿態(tài)角數(shù)據(jù)，并與陀螺測量的慣性空間角速度數(shù)據(jù)一起，通過高速串行通信傳輸給穩(wěn)定平臺控制計算機，實現(xiàn)陀螺穩(wěn)定和水平姿態(tài)跟蹤控制。

本文將在慣性穩(wěn)定平臺控制建模、陀螺穩(wěn)定回路和姿態(tài)跟蹤回路的設(shè)計等方面，對大型慣性穩(wěn)定平臺動態(tài)環(huán)境下慣性穩(wěn)定及水平姿態(tài)跟蹤控制進行探討。

1 穩(wěn)定平臺控制模型

穩(wěn)定平臺的框架由內(nèi)至外分別為俯仰（與重力傳感器和POS固連）、橫滾、基座、底座框架結(jié)構(gòu)。穩(wěn)定平臺控制回路包括橫滾、俯仰2個通道。橫滾和俯仰框架運動范圍均小于±10°?；c底座通過阻尼減振器相連。穩(wěn)定平臺框架結(jié)構(gòu)與框架坐標系示意如圖1所示。

圖1 穩(wěn)定平臺框架坐標系圖Fig.1 Gimbals and coordinate diagram of the stabilization platform

其中，OXBYBZB為與基座框架固連的坐標系，OXRYRZR為與橫滾框架固連的坐標系，OXPYPZP為與俯仰框架固連的坐標系。

俯仰框架的角速度［ωPXωPYωPZ］與受控角運動［ωPXωRY］及基座角運動［ωBXωBYωBZ］的運動學(xué)關(guān)系如下：

其中，cP＝cosP，sP＝sinP，cR＝cosR，sR＝sinR。P為框架俯仰角，R為框架橫滾角。

由式（1）可知，這一兩軸穩(wěn)定平臺實現(xiàn)框架慣性穩(wěn)定的條件是：

由ωPY＝0可得：

結(jié)構(gòu)設(shè)計保證了穩(wěn)定平臺在俯仰框架角變化范圍內(nèi)不會出現(xiàn)cP為0的框架鎖定現(xiàn)象，因此式（3）可以滿足。俯仰框架的Z軸由于沒有運動自由度，其角速度直接與基座角速度相關(guān)。

忽略角速度的乘積項，則俯仰框架的動力學(xué)方程如下：

其中，JPX為俯仰框架繞XP軸的主慣性矩，TP為俯仰電機輸出轉(zhuǎn)矩，fP為俯仰軸上的摩擦力矩、質(zhì)量不平衡力矩等。

忽略角速度的乘積項和載體的角加速度，則橫滾框架的動力學(xué)方程如下：

其中，JRY為橫滾框架繞YR軸的主慣性矩，JPY和JPZ為俯仰框架繞YP、ZP軸的主慣性矩，TR為橫滾電機輸出轉(zhuǎn)矩，fR為橫滾軸上的摩擦力矩、質(zhì)量不平衡力矩等。

POS內(nèi)部包括捷聯(lián)慣性導(dǎo)航部分和GPS導(dǎo)航部分，捷聯(lián)慣導(dǎo)選擇本地水平指北坐標系（簡稱N系）為導(dǎo)航解算的數(shù)學(xué)平臺坐標系。OXNYNZN為本地地理坐標系，YN軸指向正北，XN軸指向正東，ZN軸由右手法則確定。俯仰框架坐標系（P系）也是POS基座坐標系，N系到P系的坐標轉(zhuǎn)換關(guān)系如圖2所示。

圖2 地理坐標系與POS基座坐標系關(guān)系圖Fig.2 Relationship diagram of POS base coordinate and local geographic coordinate

其中，航向角ψ為YP軸與YN軸的夾角，在水平面XNYNO上測量。俯仰角θ為YP軸與水平面的夾角，在垂直面Y1ZNO上測量。橫滾角γ為XP軸與水平面的夾角，在橫截面X2Z2O上測量。

由圖2可知，當(dāng)穩(wěn)定平臺控制框架轉(zhuǎn)動實現(xiàn)地理姿態(tài)跟蹤時，會使俯仰角θ和橫滾角γ趨近于0，此時俯仰框架用于安裝重力傳感器的安裝面XPYPO與水平面X1Y1O重合，重力傳感器的垂直測量軸平行于ZN軸即當(dāng)?shù)氐卮咕€。

與航空攝影測量中將差分GPS數(shù)據(jù)存儲和進行事后計算得到高精度位姿數(shù)據(jù)的POS處理方法不同的是，本文采用的是POS實時輸出的姿態(tài)數(shù)據(jù)。POS先進行捷聯(lián)導(dǎo)航計算，得到捷聯(lián)姿態(tài)矩陣、速度和位置信息。一旦得到更新的GPS信息，執(zhí)行Kalman狀態(tài)反饋計算，對捷聯(lián)解算的位置、速度和姿態(tài)角進行反饋修正。這一方法的姿態(tài)測量精度較低，但可實時輸出修正的姿態(tài)信息，適合于平臺姿態(tài)跟蹤控制。

已知從N系到P系之間的姿態(tài)矩陣為［6］：

由姿態(tài)矩陣可以計算出全部姿態(tài)角為：

其中，Cij代表其姿態(tài)矩陣內(nèi)部元素，i為行號，j為列號。

為簡化控制模型，將POS輸出姿態(tài)環(huán)節(jié)用陀螺角速度的直接積分進行模擬，最終穩(wěn)定平臺的兩通道總控制模型如圖3所示。

圖3 穩(wěn)定平臺控制模型Fig.3 Total control model of the stabilization platform

其中，T1和T2為3×3矩陣。GPS（s）和GRS（s）為俯仰和橫滾穩(wěn)定回路控制器，GPA（s）和GRA（s）為俯仰和橫滾姿態(tài)跟蹤回路控制器。rP和rR為俯仰和橫滾跟蹤回路的輸入姿態(tài)角指令，為實現(xiàn)水平姿態(tài)保持，令其為0。Kg和KP分別為角速度和姿態(tài)角環(huán)節(jié)的比例因子。

2 控制回路設(shè)計與仿真

穩(wěn)定平臺的控制回路采用內(nèi)環(huán)為速度環(huán)、外環(huán)為位置環(huán)的雙回路控制方案。在典型雙回路控制系統(tǒng)中，角速度回路的主要作用是提供角速度阻尼以抑制干擾力矩，同時快速響應(yīng)角位置回路的輸出。而角位置回路的主要作用是改變系統(tǒng)的角位置，使其精確地隨動輸入的角位置指令。穩(wěn)定平臺由于采用POS陀螺的角速度作為慣性空間角速度反饋信息，采用POS導(dǎo)航解算姿態(tài)作為角位置反饋信息，因此可以實現(xiàn)穩(wěn)定平臺在慣性空間內(nèi)的速率穩(wěn)定和水平姿態(tài)跟蹤。

由于穩(wěn)定平臺2個通道的控制相互獨立，以橫滾通道控制回路設(shè)計為例，其原理框圖如圖4所示。

圖4 橫滾通道控制回路原理框圖Fig.4 Principle control loop diagram of roll channel

其中，K1為PWM驅(qū)動及電機環(huán)節(jié)的比例因子，τ1為驅(qū)動環(huán)節(jié)時間常數(shù)，τ2為POS速率采樣和姿態(tài)解算環(huán)節(jié)時間常數(shù)。J為繞橫滾軸的轉(zhuǎn)動慣量。ns為陀螺速率信號干擾，nA為POS姿態(tài)信號干擾。由于穩(wěn)定平臺自身結(jié)構(gòu)特點，cP基本等于1，可以不需要在回路中補償由于俯仰角變化造成的角速度回路增益變化。

角速度回路控制器GS（s）和角位置回路控制器GA（s）均采用比例-積分式。角速度回路的控制帶寬主要受慣性穩(wěn)定平臺的結(jié)構(gòu)諧振頻率的限制，將開環(huán)剪切頻率設(shè)計為6Hz～8Hz，能夠克服典型的頻帶在1Hz以下的載體干擾角運動。角位置回路的開環(huán)剪切頻率需設(shè)計為角速度回路的開環(huán)剪切頻率的1/10甚至更小，本文設(shè)置為0．3Hz～0．4Hz。由于POS更新角速度和姿態(tài)角信息的頻率為100Hz，對2條控制回路相位裕度減小的影響均可忽略。

由圖4可知控制回路中的干擾項包括：

1）平臺軸端干擾力矩fR；

2）載體角運動ωBX和ωBZ；

3）角速度誤差ns；

4）姿態(tài)角誤差nA。

在fR作用下的橫滾角γ為：

其中，分母的第2項為角速度回路的開環(huán)傳遞函數(shù)，第3項為角速度的開環(huán)傳遞函數(shù)乘以角位置回路的開環(huán)傳遞函數(shù)。一般來說，干擾力矩的頻率會高于角位置回路的剪切頻率，此頻率處的角位置回路開環(huán)傳遞函數(shù)幅值小于0dB，造成第3項的值小于第2項。由此可知，干擾力矩主要由角速度回路進行抑制，角位置回路也起了一定的抑制效果。分子表明穩(wěn)定平臺的轉(zhuǎn)動慣量也會起到抑制干擾力矩的作用。

令ωB＝sP·cP·ωBZ+sP·sR·ωBX，則在ωB作用下的橫滾角γ為：

由式（9）可知，2條回路對載體干擾角運動的隔離與對干擾力矩的隔離相似，角速度回路在抑制載體擾動角運動時起主要作用。角速度誤差ns與橫滾角γ的關(guān)系也如式（9）所示。

姿態(tài)角誤差nA與橫滾角γ的關(guān)系為：

其中，HS（s）為角速度回路從角速度輸入到角速度輸出的閉環(huán)傳遞函數(shù)，對于角位置回路這一環(huán)節(jié)可以被簡化成HS（s）＝1/Kg。

根據(jù)角位置回路從姿態(tài)角指令輸入（姿態(tài)角誤差輸入）到橫滾角輸出的閉環(huán)傳遞函數(shù)，知這一姿態(tài)跟蹤回路的帶寬約為0．4Hz，只要姿態(tài)角誤差為低頻周期信號，且頻率低于0．4Hz，就會引起相應(yīng)的橫滾角波動。但是如果姿態(tài)角誤差僅為瞬時或短時誤差（比如載體大機動造成POS在1s～2s內(nèi)的姿態(tài)解算誤差），當(dāng)這一誤差消失后，穩(wěn)定平臺會控制橫滾角快速趨于0。

傳統(tǒng)上采用加速度計進行調(diào)平控制的兩軸穩(wěn)定平臺［7］，為了使加速度計測得的比力只反映臺體的傾斜，不受載體運動加速度的影響，必須將加速度計調(diào)平回路的帶寬盡量降低。由于帶寬低，因此一旦回路中有力矩或角速度的瞬時擾動造成的姿態(tài)變化，或者是載體瞬時加速度引起的穩(wěn)定平臺水平姿態(tài)變化，穩(wěn)定平臺水平姿態(tài)的回調(diào)時間將會很長。

在仿真中模擬了本文姿態(tài)跟蹤穩(wěn)定平臺和采用加速度計調(diào)平控制的兩軸穩(wěn)定平臺（帶寬為0．01Hz）。令水平姿態(tài)跟蹤過程中都因載體加速度而造成了姿態(tài)測量誤差（脈沖幅值為0．05°，脈沖寬度為20s），姿態(tài)誤差以及由此造成的平臺橫滾角控制波形如圖5所示。

圖5 姿態(tài)測量誤差引起橫滾角控制誤差圖Fig.5 Roll attitude control error caused by attitude measurement error

由仿真結(jié)果可以看出，由于本文研究的姿態(tài)跟蹤穩(wěn)定平臺采用了POS，姿態(tài)跟蹤回路的帶寬較高，這使得姿態(tài)跟蹤回路能快速且高精度地響應(yīng)POS輸出的地理系水平姿態(tài)，實現(xiàn)水平姿態(tài)保持。

3 平臺樣機姿態(tài)跟蹤測試

穩(wěn)定平臺樣機由俯仰框架、橫滾框架、基座框架、底座、POS、軸端的測角元件和電機以及控制電路組成如圖6所示。模擬重力傳感器工裝與俯仰框架固連，作為被穩(wěn)定的臺體?；蚣芡ㄟ^減振器與底座框架連接，底座框架再固定于載體上。

為實測平臺的姿態(tài)跟蹤精度，將一臺高精度慣組安裝在臺體上表面中心位置，以測量出平臺臺體在姿態(tài)跟蹤回路控制下的實時橫滾、俯仰角，其姿態(tài)測量精度為0．001°。

圖6 穩(wěn)定平臺實物圖Fig.6 Physical object picture of the stabilization platform

平臺靜態(tài)條件下，用突然加載配重的方法給橫滾控制回路施以約1Nm的常值不平衡力矩，穩(wěn)定平臺橫滾姿態(tài)控制結(jié)果和高精度慣組輸出的橫滾姿態(tài)準確值如圖7所示。

由實測數(shù)據(jù)可知，由于POS陀螺儀自身零偏不穩(wěn)定、初始對準誤差、載體振動引起的陀螺標度因數(shù)變化等因素，加之POS根據(jù)刷新頻率為1Hz的GPS信號進行姿態(tài)反饋修正計算的綜合結(jié)果，造成POS輸出姿態(tài)總是呈現(xiàn)出周期約1s的鋸齒波形式，但在1s其變化范圍僅為±0．0015°，相比于干擾力矩來說對姿態(tài)跟蹤精度影響極小。

總體來說，POS輸出姿態(tài)與作為姿態(tài)基準的高精度慣組輸出姿態(tài)變化趨勢吻合，說明了本文提出的基于POS的穩(wěn)定平臺姿態(tài)跟蹤控制方法是可行的。

4 結(jié)論

本文通過建立大型穩(wěn)定平臺俯仰框架和橫滾框架的運動學(xué)模型和動力學(xué)模型，并以橫滾通道為例，分析了角速度回路和角位置回路的控制設(shè)計，將為大型穩(wěn)定平臺工程樣機控制系統(tǒng)的設(shè)計與實現(xiàn)提供理論依據(jù)。

通過仿真模擬了姿態(tài)測量誤差作用下的穩(wěn)定平臺橫滾角變化情況，與采用加速度計調(diào)平控制的兩軸穩(wěn)定平臺水平姿態(tài)跟蹤效果進行了對比，證實了本文研究的大型穩(wěn)定平臺在姿態(tài)跟蹤方面的優(yōu)勢。

進行了平臺樣機姿態(tài)跟蹤測試，驗證了本文提出的結(jié)合POS的穩(wěn)定平臺慣性穩(wěn)定和姿態(tài)跟蹤控制方法的可行性。

［1］LH System Gmbh．Leica PAV80 gyro-stabilized sensor mount user manual［R］．2008．

［2］LH System Gmbh．Leica PAV30 gyro-stabilized camera mount technical reference manual［R］．2003．

［3］朱如意，丁祝順，夏剛．航空重力儀/梯度儀慣性穩(wěn)定平臺研制現(xiàn)狀［J］．導(dǎo)航與控制，2011，10（2）：69-74．ZHU Ru-yi，DING Zhu-shun，XIA Gang．Present state of airbornegravityandgradiometerinertialstabilized platforms［J］．Navigation and Control，2011，10（2）：69-74．

［4］張昌達．幾種新型的航空重力測量系統(tǒng)和航空重力梯度測量系統(tǒng)［J］．物探與化探，2005，29（6）：471-476．ZHANG Chang-da．Several new types of airborne gravimetric systems and airborne gravity gradiometric systems［J］．Geophysical and Geochemical Exploration，2005，29（6）：471-476．［5］張永明，盛君，張貴賓．航空重力測量技術(shù)的現(xiàn)狀及應(yīng)用［J］．勘探地球物理進展，2006，29（2）：94-97．ZHANG Yong-ming，SHENG Jun，ZHANG Gui-bin．Status and applications of airborne gravimetry［J］．Progress in Exploration Geophysics，2006，29（2）：94-97．

［6］高鐘毓．慣性導(dǎo)航系統(tǒng)技術(shù)［M］．北京：清華大學(xué)出版社，2012． GAO Zhong-yu．Inertial navigation system technology［M］．Beijing：Tsinghua University Press，2012．

［7］Bruton A M，Hammada Y，F(xiàn)erguson S，et al．A comparison of inertial platform，damped 2-axis platform and strap-down airborne gravimetry［C］．International Symposium on Kinematic System in Geodesy，Geomatics and Navigation，Banff，Alberta，2001：542-550．

Inertial Stabilization and Attitude Tracking in the Geographic Coordinate System of the Large Platform

JIANG Hong-xiang，DING Zhu-shun，WANG Yuan-yuan，LU Xiao-dong
（Beijing Institute of Aerospace Control Devices，Beijing 100039）

The principles of inertial stabilizing and geographic coordinate system attitude tracking for a kind of large two-axis stabilization platform are described．The kinematics models and dynamics models of the pitch and roll gimbals of the platform are built．The angle rate control loop and angle position control loop of roll control channel are designed．The simulations of attitude tracking under the condition of the attitude measurement error are done，compared with that of a kind of twoaxis damped stabilization platform utilized the accelerators feedback for horizontal attitude leveling．The result demonstrated that the designed control system in this paper has the advantages in quickly attitude tracking and anti-disturbance capabilities．The attitude tracking test of the stabilized platform prototype was done．Feasibility of the inertial stabilizing and attitude tracking control methods for the stabilizing platform combined with position and orientation system（POS）are demonstrated．

large stabilized platform；kinematics model；dynamics model；inertial stabilizing；horizontal attitude tracking

TP273

A

1674-5558（2017）01-01288

10．3969/j．issn．1674-5558．2017．03．003

蔣鴻翔，男，博士，高級工程師，研究方向為慣性穩(wěn)定平臺控制技術(shù)。

2016-06-30