一種四軸慣性穩(wěn)定平臺系統(tǒng)的二階動(dòng)態(tài)干擾力矩補(bǔ)償方法
【技術(shù)領(lǐng)域】
[0001] 本發(fā)明設(shè)及慣性穩(wěn)定平臺測量控制領(lǐng)域，特別設(shè)及一種四軸慣性穩(wěn)定平臺系統(tǒng)的 二階動(dòng)態(tài)干擾力矩補(bǔ)償方法，用于實(shí)現(xiàn)航空、航天領(lǐng)域的全姿態(tài)高精度導(dǎo)航。
【背景技術(shù)】
[0002] 由于S軸慣性平臺系統(tǒng)存在"框架鎖定"現(xiàn)象，難W滿足載體大機(jī)動(dòng)運(yùn)動(dòng)的要求， 因此，產(chǎn)生了四軸慣性平臺系統(tǒng)。四軸慣性平臺系統(tǒng)相對=軸慣性平臺系統(tǒng)，在臺體、內(nèi)框 架和外框架的基礎(chǔ)上增加了隨動(dòng)框架，隨動(dòng)框架處于平臺外框架和基座之間。
[0003] 在慣性穩(wěn)定平臺伺服系統(tǒng)工作時(shí)，系統(tǒng)的精度主要取決于巧螺儀，但在存在角運(yùn) 動(dòng)的動(dòng)態(tài)條件下，框架系統(tǒng)的動(dòng)態(tài)干擾力矩會(huì)造成平臺臺體的漂移。目前，假設(shè)二階動(dòng)態(tài)誤 差相對于巧螺儀漂移是個(gè)小量，因此在伺服系統(tǒng)中基本不考慮該項(xiàng)誤差。但是，隨著平臺系 統(tǒng)精度的提高，該項(xiàng)誤差逐漸顯著性逐漸提高。
[0004] 由于四軸平臺系統(tǒng)是由臺體、內(nèi)框架、外框架、隨動(dòng)框架等組成的復(fù)合體，在每個(gè) 框架都存在二階干擾力矩時(shí)，需要明確計(jì)算出四軸平臺合成的干擾力矩大小。同時(shí)，需要研 究如何減小此項(xiàng)干擾力矩。因此，需要給出四軸平臺系統(tǒng)的二階干擾力矩計(jì)算方法和補(bǔ)償 方法，W提高四軸慣性平臺系統(tǒng)在機(jī)動(dòng)飛行彈道中的使用精度。

【發(fā)明內(nèi)容】

[0005] 本發(fā)明的目的在于克服現(xiàn)有技術(shù)的不足，提供一種四軸慣性穩(wěn)定平臺系統(tǒng)的二階 干擾力矩補(bǔ)償方法，該方法首先精確計(jì)算得到四軸慣性穩(wěn)定平臺系統(tǒng)的合成二階干擾力 矩，然后計(jì)算得到進(jìn)行補(bǔ)償該干擾力矩的電機(jī)力矩，從而實(shí)現(xiàn)二階動(dòng)態(tài)干擾力矩的抑制，有 利于提高四軸慣性穩(wěn)定平臺系統(tǒng)的精度。
[0006] 本發(fā)明的上述目的通過W下技術(shù)方案實(shí)現(xiàn)：
[0007] -種四軸慣性穩(wěn)定平臺系統(tǒng)的二階動(dòng)態(tài)干擾力矩補(bǔ)償方法，用于計(jì)算四軸慣性穩(wěn) 定平臺系統(tǒng)的臺體合成二階動(dòng)態(tài)干擾力矩并進(jìn)行補(bǔ)償；所述四軸慣性穩(wěn)定平臺系統(tǒng)括基 座、隨動(dòng)框架、外框架、內(nèi)框架和臺體，對應(yīng)的本體坐標(biāo)系分別為基座本體坐標(biāo)系XAZi、隨 動(dòng)框架坐標(biāo)系XpsYpsZps、外框架本體坐標(biāo)系XpzYpzZpz、內(nèi)框架本體坐標(biāo)系XpiYpiZpi和臺體本體 坐標(biāo)系XpYpZp;所述五個(gè)坐標(biāo)系的原點(diǎn)重合，并且：臺體本體坐標(biāo)系的ZP軸與內(nèi)框架本體坐 標(biāo)系的Zp湘重合，外框架的本體坐標(biāo)系的YP2軸與內(nèi)框架本體坐標(biāo)系的YP湘重合，隨動(dòng)框 架本體坐標(biāo)系的Xp3軸與外框架本體坐標(biāo)系的XP2軸重合，基座本體坐標(biāo)系的X1軸與隨動(dòng)框 架本體坐標(biāo)系的Yp3軸重合；其中，基座與載體固連，在所述穩(wěn)定平臺系統(tǒng)在載體帶動(dòng)下發(fā) 生內(nèi)部相對轉(zhuǎn)動(dòng)時(shí)，基座繞隨動(dòng)框架本體坐標(biāo)系的Yp3軸轉(zhuǎn)動(dòng)，隨動(dòng)框架繞外框架本體坐標(biāo) 系的Xp2軸轉(zhuǎn)動(dòng)，外框架繞內(nèi)框架本體坐標(biāo)系的YP1軸轉(zhuǎn)動(dòng)，內(nèi)框架繞臺體本體坐標(biāo)系的ZP 軸轉(zhuǎn)動(dòng)；
[0008] 所述二階動(dòng)態(tài)干擾力矩補(bǔ)償方法包括如下步驟：
[0009] (1)、測量或計(jì)算得到四軸慣性穩(wěn)定平臺系統(tǒng)的轉(zhuǎn)動(dòng)慣量，包括：臺體相對于Xp軸、 Yp軸、Zp軸的轉(zhuǎn)動(dòng)慣量的《V、;內(nèi)框架相對于Xp湘、Ypi軸、Zp湘的轉(zhuǎn)動(dòng)慣量的 ^：*。,、韋。,、夕雁架相對于Xp2軸、Yp2軸、Zp2軸的轉(zhuǎn)動(dòng)慣量的疋隨 動(dòng)框架相對于Xp3軸、YP3軸、zP3軸的轉(zhuǎn)動(dòng)慣量的5
[0010] (2)、測量得到所述穩(wěn)定平臺系統(tǒng)內(nèi)部相對轉(zhuǎn)動(dòng)的角度和角速度，包括：隨動(dòng)框架 繞外框架本體坐標(biāo)系的Xp2軸轉(zhuǎn)動(dòng)的角度Pyk和角速度處.1，外框架繞內(nèi)框架本體坐標(biāo)系的 Ypl軸轉(zhuǎn)動(dòng)的角度Pyk和角速度A,，內(nèi)框架繞臺體本體坐標(biāo)系的Zp軸轉(zhuǎn)動(dòng)的角度P.k和角 速度么;
[0011] (3)、測量得到所述穩(wěn)定平臺系統(tǒng)的臺體本體相對慣性空間轉(zhuǎn)動(dòng)的角速度，包括： 臺體本體繞Xp軸轉(zhuǎn)動(dòng)的角速度臺體本體繞Yp軸轉(zhuǎn)動(dòng)的角速度0,,,;臺體本體繞Zp軸轉(zhuǎn) 動(dòng)的角速度
[0012] (4)、計(jì)算所述穩(wěn)定平臺系統(tǒng)的合成二階動(dòng)態(tài)干擾力矩，包括：合成到臺體Xp軸上 的二階動(dòng)態(tài)干擾力矩合成到臺體Yp軸上的二階動(dòng)態(tài)干擾力矩am,.P、合成到臺體Zp 軸上的二階動(dòng)態(tài)干擾力矩;合成到隨動(dòng)框架Yp3軸上的二階動(dòng)態(tài)干擾力矩；合成 到隨動(dòng)框架Zp3軸上的二階動(dòng)態(tài)干擾力矩;
[001引 巧）、根據(jù)步驟（4)計(jì)算得到的二階動(dòng)態(tài)干擾力矩，計(jì)算通過各框架軸端的電機(jī)進(jìn) 行實(shí)時(shí)補(bǔ)償時(shí)的電機(jī)力矩，包括：折合到隨動(dòng)框架坐標(biāo)系Yp3軸上的電機(jī)力矩AMdy,、折合 到外框架坐標(biāo)系的Xp2軸上的電機(jī)力矩AMDX、折合到內(nèi)框架坐標(biāo)系的Ypi軸上的電機(jī)力矩 AMdy、折合到臺體坐標(biāo)系Zp軸上的電機(jī)力矩AMDz;
[0014](6)、隨動(dòng)框架坐標(biāo)系Yp3軸上的電機(jī)、外框架坐標(biāo)系Xp2軸上的電機(jī)、內(nèi)框架坐標(biāo)系 的Ypi軸上的電機(jī)、臺體坐標(biāo)系Zp軸上的電機(jī)分別提供大小為AMdy,、AMd、、AMdy、AMd,的 電機(jī)力矩，對二階動(dòng)態(tài)干擾力矩進(jìn)行補(bǔ)償。
[0015] 本發(fā)明與現(xiàn)有技術(shù)相比具有W下優(yōu)點(diǎn)：
[0016] (1)、本發(fā)明利用四軸慣性穩(wěn)定平臺系統(tǒng)的轉(zhuǎn)動(dòng)慣量，穩(wěn)定平臺系統(tǒng)內(nèi)部相對轉(zhuǎn)動(dòng) 的角度，W及巧螺儀測量的臺體角速度和框架轉(zhuǎn)動(dòng)角速度，計(jì)算得到合成二階動(dòng)態(tài)干擾力 矩，計(jì)算結(jié)果準(zhǔn)確、適用性廣；
[0017] (2)、本發(fā)明利用計(jì)算的二階動(dòng)態(tài)干擾力矩精確值折算到電機(jī)力矩進(jìn)行實(shí)時(shí)補(bǔ)償 方式，相比現(xiàn)有平臺系統(tǒng)不進(jìn)行補(bǔ)償?shù)奶幚矸绞?，具有抑制誤差和提高精度的作用。
【附圖說明】
[0018]圖1為四軸慣性穩(wěn)定平臺系統(tǒng)中四個(gè)本體坐標(biāo)系之間的關(guān)系示意圖；
[0019] 圖2為采用本發(fā)明的四軸慣性平臺系統(tǒng)的二階動(dòng)態(tài)干擾力矩補(bǔ)償方法后，四軸慣 性平臺系統(tǒng)的伺服回路示意圖；
[0020] 圖3為本發(fā)明的四軸慣性穩(wěn)定平臺系統(tǒng)的二階動(dòng)態(tài)干擾力矩補(bǔ)償方法的流程圖。
【具體實(shí)施方式】
[0021] 下面結(jié)合附圖對本發(fā)明作進(jìn)一步詳細(xì)的描述：
[0022] 本發(fā)明提供的四軸慣性穩(wěn)定平臺系統(tǒng)的二階動(dòng)態(tài)干擾力矩補(bǔ)償方法，用于計(jì)算四 軸慣性穩(wěn)定平臺系統(tǒng)中臺體合成的二階動(dòng)態(tài)干擾力矩并進(jìn)行補(bǔ)償。其中，該四軸穩(wěn)定慣 性平臺系統(tǒng)包括基座、隨動(dòng)框架、外框架、內(nèi)框架和臺體，對應(yīng)的本體坐標(biāo)系分別為基座本 體坐標(biāo)系XAZi、隨動(dòng)框架坐標(biāo)系XpsYpsZps、外框架本體坐標(biāo)系XpzYpzZpz、內(nèi)框架本體坐標(biāo)系 XpiYpiZp郝臺體本體坐標(biāo)系XpYpZp。
[0023] 如圖1所示的五個(gè)坐標(biāo)系的關(guān)系示意圖，W上所述的五個(gè)坐標(biāo)系的原點(diǎn)重合，并 且存在如下相對約束關(guān)系：臺體本體坐標(biāo)系的Zp軸與內(nèi)框架本體坐標(biāo)系的ZP1軸重合，外框 架的本體坐標(biāo)系的Yp2軸與內(nèi)框架本體坐標(biāo)系的YP1軸重合，隨動(dòng)框架本體坐標(biāo)系的XP3軸與 外框架本體坐標(biāo)系的Xp2軸重合，基座本體坐標(biāo)系的X1軸與隨動(dòng)框架本體坐標(biāo)系的Y軸重 合。其中，基座與載體固連，在所述穩(wěn)定平臺系統(tǒng)在載體帶動(dòng)下發(fā)生內(nèi)部相對轉(zhuǎn)動(dòng)時(shí)：基座 繞隨動(dòng)框架本體坐標(biāo)系的Yp3軸轉(zhuǎn)動(dòng)且轉(zhuǎn)動(dòng)角度為Pyk,;隨動(dòng)框架繞外框架本體坐標(biāo)系的 Xp2軸轉(zhuǎn)動(dòng)且轉(zhuǎn)動(dòng)角度為Pyk;外框架繞內(nèi)框架本體坐標(biāo)系的Ypl軸轉(zhuǎn)動(dòng)且轉(zhuǎn)動(dòng)角度為Pyk， 內(nèi)框架繞臺體本體坐標(biāo)系的Zp軸轉(zhuǎn)動(dòng)且轉(zhuǎn)動(dòng)角度為P,k。
[0024] 如圖2所示的新型四軸平臺伺服回路，包含了合成到臺體Xp軸上的二階動(dòng)態(tài)干擾 力矩心、合成到臺體Yp軸上的二階動(dòng)態(tài)干擾力矩AMyp、合成到臺體2。軸上的二階動(dòng)態(tài) 干擾力矩AM,。；合成到隨動(dòng)框架Yp3軸上的二階動(dòng)態(tài)干擾力矩AMyk;合成到隨動(dòng)回路ZP3軸 上的二階動(dòng)態(tài)干擾力矩AW-。
[00巧]本發(fā)明的原理分析過程如下：
[0026]當(dāng)載體、隨動(dòng)框架、外框架與內(nèi)框架繞臺體軸Zp轉(zhuǎn)過P角時(shí)，有
[002引式（1)中?、,.' -P分別為臺體繞Xp、Yp、Zp軸的絕對角速度；
[0029] ^卸、?'v,、分別為載體、隨動(dòng)框架、外框架與內(nèi)框架一起繞Xpi、Ypi、Zpi軸的絕 對角速度；
[003引式中馬。分別為載體、隨動(dòng)框架與外框架一起繞Xp2、Yp2、Zp2軸的絕對 角速度； CN105115505A 說明書 4/19 頁
[003引當(dāng)載體、隨動(dòng)框架一起繞外環(huán)軸Xp2轉(zhuǎn)過Pxk角時(shí)，有
[0037] 式中@*,,，、麟,3、分別為載體與隨動(dòng)框架一起繞Xp3、Yp3、Zp3軸的絕對角速度。
[0043] 框架系統(tǒng)的動(dòng)力學(xué)方程可此沿臺體的S個(gè)軸Xp、Yp、Zp來列寫。每個(gè)臺體軸與其相 對應(yīng)的巧螺儀，可W看成是一個(gè)單軸平臺。采用歐拉法分別列寫出臺體、內(nèi)框架和外框架的 動(dòng)力學(xué)方程。
[0044] 臺體的動(dòng)力學(xué)方程：
[0046] 式中M化。為臺體軸力矩電機(jī)反饋力矩.Mg,、M。，、M。為巧螺儀對臺體的反 作用力矩；為臺體（包括巧螺儀殼體）對Xp、Yp、Zp軸的轉(zhuǎn)動(dòng)慣量;Mxp、Myp、 Mzp為臺體XP、Yp、Zp軸上的外力矩。
[0047] 內(nèi)框架的動(dòng)力學(xué)方程：
[0048] CN105115505A 說明書 5/19 頁
[0049] 式中心;,,、A/,p,、M_p,分別為內(nèi)框架軸上的外力矩，不包括電機(jī)力矩的反饋力 矩；^0，,為內(nèi)框架軸力矩電機(jī)反饋力矩；4。,:、為內(nèi)框架對Xpi、Ypi、Zp湘的轉(zhuǎn) 動(dòng)慣量。
[0050] 外框架的動(dòng)力學(xué)方程為：
[0052] 式中激、為外框架軸上的外力矩，不包括電機(jī)力矩的反饋力矩. 為外框架軸力矩電機(jī)反饋力矩:為J外框架對Xp2、Yp2、Zp2軸的轉(zhuǎn)動(dòng)慣 量。
[00巧]式中心X。，、My。;、Msp，分別為隨動(dòng)框架軸上的外力矩，不包括電機(jī)力矩的反饋 力矩；疋W;、也,3、疋>3:為隨動(dòng)框架對Xp3、Yp3、Zp3軸的轉(zhuǎn)動(dòng)慣量。％馬，為隨動(dòng)框架軸力矩 電機(jī)反饋力矩；由于基座隨著載體運(yùn)動(dòng)時(shí)對隨動(dòng)框架施加的力矩為
[0057]定義為折合到臺體軸Xp的轉(zhuǎn)動(dòng)慣量，為折合到臺體軸Yp的轉(zhuǎn)動(dòng)慣量J,y、Jyy、Ju、Jy.為框架系統(tǒng)的等效慣量積。則有四軸平臺的動(dòng)力學(xué)方程為： UIN丄UO丄丄OOUO A *八、丄<3/丄3 JA


[0085] 基于W上的理論分析，本發(fā)明的四軸慣性穩(wěn)定平臺系統(tǒng)的二階動(dòng)態(tài)干擾力矩計(jì)算 及其補(bǔ)償方法流程圖如圖3所示，具體實(shí)現(xiàn)步驟如下：
[0086] (1)、當(dāng)四軸慣性穩(wěn)定平臺系統(tǒng)的結(jié)構(gòu)確定后，通過有限元分析方法計(jì)算得到所 述平臺的轉(zhuǎn)動(dòng)慣量，或?qū)λ鏊妮S慣性穩(wěn)定平臺系統(tǒng)進(jìn)行測量得到轉(zhuǎn)動(dòng)慣量，具