一種四軸慣性穩定平臺系統的伺服回路解耦方法
【技術領域】
[0001] 本發明涉及慣性測量技術領域，特別涉及一種四軸慣性穩定平臺系統的伺服回路 解耦方法，主要用于航空、航天領域的全姿態高精度導航。
【背景技術】
[0002] 由于三軸慣性平臺系統存在"框架鎖定"現象，難以滿足載體大機動運動的要求， 因此，產生了四軸慣性平臺系統。四軸慣性平臺系統相對三軸慣性平臺系統，在臺體、內框 架和外框架的基礎上增加了隨動框架，隨動框架處于平臺外框架和基座之間。
[0003] 傳統的解決方法如下：隨動回路信號來自于內框架角，采用正割分解器進行增益 補償。但該方法的缺點是在外框架角為90°時，存在奇異值。參見文獻"四軸平臺伺服系 統建模研究，中國慣性技術學報Vol. 10, No. 5, 2002年10月"，以及"四軸平臺隨動系統的模 型分析與設計，導航與控制，2014年第4期"。
[0004]目前，解決該問題的辦法主要為：在外框架角為90°時采用框架鎖定，一旦通過 該角度值則又恢復成原隨動方案。參見文獻"四軸平臺外框架角±90°時運動特性仿真分 析，導航與控制，2009年第2期"。但該方法的缺點是，在外框架角一直保持90°時，則四軸 平臺退化為三軸平臺，仍然存在"框架鎖定"現象，不能實現載體運動的全姿態功能。
[0005] 總之，上述方法存在著以條件判斷來克服奇異值的缺點，為此，需要研究一種無奇 異值、不受軌跡限制的解耦方法。
[0006] 下面介紹目前現有技術的研究情況：
[0007] 首先，四軸慣性穩定平臺系統的五個本體坐標系的定義如圖1所示，從中可以看 出各本體坐標系之間關系。在圖1中，設々a為內框架相對臺體的相對角速度，為外框 架相對內框架的相對角速度，我^為為基座（箭體）相對外框架的相對角速度，Λ,?.為基座 (箭體）相對隨動框架的相對角速度。
[0008] 設4、&為臺體（包括陀螺儀殼體）對Xp、Yp、Zp軸的轉動慣量； 氣、氣,為內框架對Χρ1、Υρ1、Ζρ1軸的轉動慣量；眾2、畋 2、眾2為外框架對χρ2、 12、2|52軸的轉動慣量；々.，、《/：^、義￥為隨動框架對\ 3、13、2|53軸的轉動慣量。定義/1 為折合到臺體軸Xp的轉動慣量，為折合到臺體軸Yp的轉動慣量；J xy、Jyx、Jxz、Jyz為框架 系統的等效慣量積。其中：
[0009] CN 105180936 A 說明書 2/7 頁
[0015] 設Mzp為臺體軸干擾力矩，麗％為臺體軸力矩電機反饋力矩；M 1h為內框架軸干 擾力矩，￥%為內框架軸力矩電機反饋力矩；1:%為外框架軸干擾力矩，^^_為外框架軸 力矩電機反饋力矩；a^1為隨動框架軸上的外力矩，jfef^3為隨動框架軸力矩電機反饋力 矩；則四軸慣性平臺系統的各軸端力矩作用到臺體三軸的合成力矩如下：
CN 105180936 A 說明書 3/7 頁
[0018] 6V 分別為臺體繞xP、yP、z p軸的絕對角速度，可通過正交安裝于臺體的 陀螺儀測量得到；則四軸慣性平臺系統的臺體動力學方程為
(9)
[0020] 可以看出，在3個陀螺儀角速率氣信息已知時，有4個控制執行環節 Μ?;、MAi、MA2、，伺服回路不完全可控。因此，目前的解決措施是增加隨動控制回 路，使內框架角Pyk近似為0°，此時有動力學方程
[0022] 結合臺體三軸動力學方程后，四軸慣性平臺系統的動力學方程為：
[0027] 此時，空間解耦矩陣在β yk近似為0時，可簡化為
[0029] 如圖2所示的現有解耦伺服回路原理框圖，現有技術在在原三個穩定回路的坐標 分解器基礎上，增加正割分解器sec β xk。即把《V = /),,、Secpxk作為隨動回路的一個環節。 但也看出，在βχ1?趨于±90°時，存在奇異值，secP xk趨于無窮大。

【發明內容】

[0030] 本發明的目的在于克服現有技術的不足，提供一種四軸慣性穩定平臺系統的伺服 回路解耦方法，該方法可以在平臺系統相對轉動角度為任意值的情況下，實現無奇異值的 伺服回路解耦計算，從而提高了載體無軌跡約束條件下的全姿態適應能力。
[0031] 本發明的上述目的通過以下技術方案實現：
[0032] -種四軸慣性穩定平臺系統的伺服回路解耦方法，基于四軸慣性穩定平臺系統實 現，所述穩定平臺系統包括基座、隨動框架、外框架、內框架和臺體，對應的本體坐標系分別 為基座本體坐標系X1Y1Z1、隨動框架坐標系Xp3Y p3Zp3、外框架本體坐標系Xp2Yp2Zp2、內框架本 體坐標系XplYplZpl和臺體本體坐標系X PYPZP;所述五個坐標系的原點重合，并且：臺體本體 坐標系的Zp軸與內框架本體坐標系的Z ρ1軸重合，外框架的本體坐標系的Y ρ2軸與內框架本 體坐標系的Ypl軸重合，隨動框架本體坐標系的X ρ3軸與外框架本體坐標系的X ρ2軸重合，基 座本體坐標系的X1軸與隨動框架本體坐標系的Y軸重合；其中，基座與載體固連，在所述穩 定平臺系統在載體帶動下發生內部相對轉動時，基座繞隨動框架本體坐標系的Yp3軸轉動， 隨動框架繞外框架本體坐標系的Xp2軸轉動，外框架繞內框架本體坐標系的Y ρ1軸轉動，內框 架繞臺體本體坐標系的Zp軸轉動；
[0033] 所述四軸慣性平臺系統伺服回路解耦方法實現步驟如下：
[0034] (1)、根據臺體上安裝的陀螺儀輸出的角速度，得到臺體在Xp軸、Y p軸和Z p軸上的 角速度分量伙、M、氣^
[0035] (2)、測量得到四軸慣性穩定平臺系統內部相對轉動的角度和角速度，包括：隨動 框架繞外框架本體坐標系的Xp2軸轉動的角度β xk，外框架繞內框架本體坐標系的Ypl軸轉 動的角度Pyk和角速度戶,?.，內框架繞臺體本體坐標系的Zp軸轉動的角度β zk和角速度 ；
[0036] (3)、計算臺體、內框架、外框架和隨動框架的轉動角速度，具體計算公式如下：
[0041] 其中，ωζ為臺體Zp軸的合成轉動角速度；ω ￥為內框架Ypl軸的合成轉動角速度； ?.為外框架XlJtt的合成轉動角速度；ω yk(為隨動框架Yp3軸的合成轉動角速度。
[0042] 上述的四軸慣性穩定平臺系統的伺服回路解耦方法，在步驟（2)中，通過如下方 法測量得到四軸慣性穩定平臺系統內部相對轉動角度和角速度：
[0043] 在外框架的Xp2軸上安裝角度傳感器，測量得到隨動框架繞外框架本體坐標系的 Xp2軸轉動的角度β xk;在內框架的Ypl軸上安裝角度傳感器，測量得到外框架繞內框架本體 坐標系的Ypl軸轉動的角度β yk和角速度片/:在臺體Zp軸上安裝傳感器測量內框架繞臺體 本體坐標系的Zp軸轉動的角度β zk和角速度^
[0044] 上述的四軸慣性穩定平臺系統的伺服回路解耦方法，在步驟（2)中，轉動角度 U yk、βζ1?的取值范圍為〇~360°。
[0045] 本發明與現有技術相比具有以下優點：
[0046] (1)、本發明給出的解耦計算公式，可以在系統內部相對轉動角0~360°范圍內 實現無奇異值計算，克服了現有技術在外框架角βΛ= ±90°、內框架角β yk= ±90°時 的奇異值問題；相比現有的解耦方法更準確、適用性更廣；
[0047] (2)、本發明在解親計算中，在原相對角速度基礎上進行正余弦計算，不存在增益 放大的問題，避免了現有解耦方法中增益趨于無窮大的問題。
【附圖說明】
[0048] 圖1為四軸慣性穩定平臺系統中四個本體坐標系之間的關系示意圖；
[0049] 圖2為現有技術中采用的解耦方案中動調陀螺四軸慣性穩定平臺伺服回路原理 框圖；
[0050] 圖3為本發明的四軸慣性平臺系統伺服回路解耦方法；
[0051] 圖4為本發明采用的解耦方案中動調陀螺四軸慣性穩定平臺伺服回路原理框圖。
【具體實施方式】
[0052] 下面結合附圖和具體實施例對本發明作進一步詳細的描述：
[0053] 本發明提供的一種四軸慣性穩定平臺系統的伺服回路解耦方法，基于四軸慣性穩 定平臺系統實現。該四軸穩定平臺系統包括基座、隨動框架、外框架、內框架和臺體，對應 的本體坐標系分別為基座本體坐標系X1Y1Z1、隨動框架坐標系X p3Yp3Zp3、外