基于四元數(shù)坐標(biāo)變換法的火炮身管姿態(tài)伺服控制研究

韓崇偉，李 偉，陳騰飛，楊 剛，張志鵬

(西北機(jī)電工程研究所，陜西 咸陽(yáng) 712099)

?

基于四元數(shù)坐標(biāo)變換法的火炮身管姿態(tài)伺服控制研究

韓崇偉，李 偉，陳騰飛，楊 剛，張志鵬

(西北機(jī)電工程研究所，陜西 咸陽(yáng) 712099)

某履帶式自行火炮隨動(dòng)系統(tǒng)采用捷聯(lián)慣性導(dǎo)航裝置來(lái)檢測(cè)火炮身管姿態(tài)，實(shí)現(xiàn)射角射向的高精度控制。為解決火炮隨動(dòng)系統(tǒng)高低向伺服和方位向伺服存在的控制耦合問(wèn)題，采用四元數(shù)法，將捷聯(lián)慣導(dǎo)測(cè)量值與射角射向主令諸元從地理坐標(biāo)系下轉(zhuǎn)換到車體坐標(biāo)系下，分別解出方位向和高低向調(diào)轉(zhuǎn)誤差，通過(guò)位置控制器和電機(jī)驅(qū)動(dòng)系統(tǒng)完成火炮身管姿態(tài)閉環(huán)控制。在車體初始姿態(tài)分別為水平和傾斜6°條件下進(jìn)行仿真和試驗(yàn)。結(jié)果表明：采用四元數(shù)坐標(biāo)變換法的火炮身管姿態(tài)伺服控制系統(tǒng)具有實(shí)現(xiàn)簡(jiǎn)單、調(diào)炮平穩(wěn)、瞄準(zhǔn)精確和超調(diào)小等技術(shù)特點(diǎn)，滿足系統(tǒng)設(shè)計(jì)要求。

兵器科學(xué)與技術(shù)；自行火炮；捷聯(lián)慣導(dǎo)；四元數(shù)；伺服系統(tǒng)

火炮身管姿態(tài)伺服控制系統(tǒng)包括方位向伺服和高低向伺服系統(tǒng)，采用捷聯(lián)慣性導(dǎo)航裝置(SINS)來(lái)直接檢測(cè)火炮身管在地理坐標(biāo)系下的姿態(tài)角(包括方位角、俯仰角和橫滾角)，分別與SINS輸出的方位角和俯仰角構(gòu)成位置閉環(huán)系統(tǒng)。根據(jù)射角、射向求解誤差，實(shí)現(xiàn)方位向伺服和高低向(俯仰向)伺服，可使火炮身管繞炮塔方向軸作回轉(zhuǎn)運(yùn)動(dòng)、繞俯仰(耳)軸作俯仰轉(zhuǎn)動(dòng)。與采用火炮車體坐標(biāo)系下測(cè)角反饋的傳統(tǒng)伺服系統(tǒng)相比，實(shí)現(xiàn)身管姿態(tài)伺服的難點(diǎn)在于：慣導(dǎo)的安裝方式造成方位向伺服和高低向伺服的控制不再獨(dú)立而發(fā)生耦合，且都受車體姿態(tài)影響；車體為履帶式底盤，采用油氣懸掛，火炮身管在調(diào)轉(zhuǎn)過(guò)程中的不平衡力矩易引起車體姿態(tài)變化，對(duì)姿態(tài)伺服系統(tǒng)形成干擾。因此，建立包含車體姿態(tài)擾動(dòng)的身管姿態(tài)伺服控制模型十分必要。

文獻(xiàn)[1-4]對(duì)火炮身管指向控制進(jìn)行了研究和探討，但未涉及伺服系統(tǒng)及車體姿態(tài)的干擾問(wèn)題。文獻(xiàn)[5]根據(jù)全閉環(huán)伺服操瞄控制的SINS測(cè)量反饋和操瞄主令諸元直接求差，將操瞄控制模型等效變換至平動(dòng)坐標(biāo)系后，耦合干擾、未知干擾等可看作是各伺服分系統(tǒng)的總干擾，采用自抗擾控制(ADRC)理論分別設(shè)計(jì)伺服分系統(tǒng)的控制器，對(duì)系統(tǒng)的不確定擾動(dòng)進(jìn)行實(shí)時(shí)估計(jì)，并實(shí)施補(bǔ)償，從而解決操瞄系統(tǒng)伺服控制的耦合干擾對(duì)瞄準(zhǔn)性能的影響問(wèn)題，取得了良好的效果。但是ADRC的控制參數(shù)非常多，設(shè)計(jì)能夠滿足使用要求的控制器難度較大。

筆者根據(jù)火炮身管姿態(tài)伺服控制中方位向回轉(zhuǎn)運(yùn)動(dòng)和高低向運(yùn)動(dòng)的特點(diǎn)，采用四元數(shù)法完成了其從地理坐標(biāo)系到車體坐標(biāo)系的轉(zhuǎn)換，解出了火炮身管的方位向調(diào)轉(zhuǎn)角和高低向調(diào)轉(zhuǎn)角，通過(guò)位置控制器和電機(jī)驅(qū)動(dòng)系統(tǒng)實(shí)現(xiàn)了身管姿態(tài)伺服控制。

1 坐標(biāo)系定義

SINS平行于火炮身管且固定于火炮耳軸上，隨火炮身管轉(zhuǎn)動(dòng)，炮塔可繞方位向回轉(zhuǎn)軸左右轉(zhuǎn)動(dòng)，火炮身管在炮塔上可繞俯仰軸作上下起落運(yùn)動(dòng)，其簡(jiǎn)化圖如圖1所示。圖中所涉及的坐標(biāo)系包括地理坐標(biāo)系、SINS機(jī)體坐標(biāo)系、火炮身管俯仰部分坐標(biāo)系、炮塔坐標(biāo)系和車體坐標(biāo)系。

地理坐標(biāo)系Onxnynzn即SINS導(dǎo)航坐標(biāo)系，將原點(diǎn)On定為SINS所處位置點(diǎn)，Onyn軸平行于水平面指向正北，Onxn軸平行于水平面指正東，Onzn軸垂直水平面向上。

SINS機(jī)體坐標(biāo)系Obxbybzb的原點(diǎn)Ob為SINS慣性組件的中心， Obyb為SINS機(jī)體的縱軸，Obxb為SINS機(jī)體的橫軸，Obzb垂直于Obyb， 組成的平面向上。

火炮身管俯仰部分坐標(biāo)系Opxpypzp的原點(diǎn)Op為火炮俯仰轉(zhuǎn)動(dòng)(耳)軸與火炮身管俯仰平面的交點(diǎn)，Opxp軸與火炮身管軸線重合指向彈丸射向，Opyp軸與耳軸重合指向炮塔右側(cè)，Opzp軸垂直于Opxp與Opyp構(gòu)成的平面指向上。理想狀態(tài)下，SINS機(jī)體坐標(biāo)系Obxbybzb的橫軸與耳軸平行,SINS機(jī)體坐標(biāo)系Obxbybzb的縱軸與Opyp平行。火炮身管俯仰部分繞Opxp軸即耳軸旋轉(zhuǎn)為εb角,稱為高低角。

炮塔坐標(biāo)系Ohxhyhzh的原點(diǎn)Oh為炮塔方位回轉(zhuǎn)軸與方位回轉(zhuǎn)平面的交點(diǎn)，Ohyh為火炮身管軸線向炮塔方向回轉(zhuǎn)平面的投影，指向彈丸射向，Ohzh軸沿炮塔方向回轉(zhuǎn)軸指向上，與Obzb平行，Ohxh軸垂直于Ohyh與Ohzh構(gòu)成的平面向右。炮塔繞Ohyh軸轉(zhuǎn)動(dòng)的角度為βb角,稱為方位角。

(1)

式中，ψ、θ和γ為SINS檢測(cè)的姿態(tài)角,分別為方位角、俯仰角和橫滾角。

各坐標(biāo)系原點(diǎn)不重合，若需要可平移至重合，車體姿態(tài)也可以類似確定。

2 身管姿態(tài)伺服系統(tǒng)控制模型

在當(dāng)前地理坐標(biāo)系下的身管姿態(tài)和目標(biāo)位置已知、車體姿態(tài)未知的情況下，利用四元數(shù)法進(jìn)行坐標(biāo)變換。根據(jù)身管指向的主令諸元(包括射角、射向)和當(dāng)前的SINS輸出的方位角和俯仰角，求解車體坐標(biāo)系下的高低和方向誤差量α、β，該過(guò)程為逆向計(jì)算法。

火炮身管在運(yùn)動(dòng)過(guò)程中，考慮不平衡力矩影響車體姿態(tài)變化，采用Langrange法建立履帶式自行火炮車體及其懸掛、火炮俯仰部分等動(dòng)力學(xué)模型，受篇幅限制，直接給出某履帶式自行火炮身管姿態(tài)伺服系統(tǒng)控制原理，如圖2所示。

式中，bh、bp分別為伺服被控對(duì)象的方向、高低控制增益。位置控制器均采用PI控制器。

3 四元數(shù)坐標(biāo)變換法的控制誤差求取

身管初始姿態(tài)時(shí)，已知SINS的姿態(tài)為ψ0、θ0、γ0，身管指向目標(biāo)的主令諸元為ψ1、θ1，該過(guò)程在地理坐標(biāo)系下用四元數(shù)可以表述為[8]

Q=Qγ1?Qψ?Qθ?Q-γ0

(2)

身管在方向伺服系統(tǒng)和高低伺服系統(tǒng)的帶動(dòng)下在車體坐標(biāo)下分別調(diào)轉(zhuǎn)α、β至目標(biāo)指向，用四元數(shù)描述的最簡(jiǎn)過(guò)程為

Q=Qα?Qβ

(3)

根據(jù)坐標(biāo)系的定義，火炮身管指向的調(diào)轉(zhuǎn)運(yùn)動(dòng)為同一個(gè)旋轉(zhuǎn)運(yùn)動(dòng)的不同坐標(biāo)系下的數(shù)學(xué)描述，故式(2)、(3)旋轉(zhuǎn)四元數(shù)等效，從而建立非線性方程組，化簡(jiǎn)如下：

(4)

跟等效旋轉(zhuǎn)矢量Q的系數(shù)有關(guān)，限于篇幅，此處略。

由于非線性方程組(4)的個(gè)數(shù)多于未知數(shù)的個(gè)數(shù)，在此采用最小二乘解的廣義逆法求解出PI位置器的控制誤差量α、β，整個(gè)調(diào)炮過(guò)程車體皆保持水平情況下，很容易驗(yàn)證β=Δψ,α=Δθ，否則是不相等的。同時(shí)發(fā)現(xiàn)，采用四元數(shù)的坐標(biāo)變換法算法略微復(fù)雜，現(xiàn)有的嵌入式計(jì)算機(jī)技術(shù)容易實(shí)現(xiàn)，但沒(méi)有需要設(shè)計(jì)的參數(shù)，其運(yùn)算的參數(shù)比如高低軸g的參數(shù)都根據(jù)慣導(dǎo)的參數(shù)實(shí)時(shí)精確確定，而方向軸f是根據(jù)不同時(shí)刻高低軸g叉乘得到。而對(duì)于整個(gè)火炮身管全閉環(huán)指向的控制的伺服系統(tǒng)PI位置控制器參數(shù)可根據(jù)常規(guī)方法并結(jié)合anti-windup控制方法設(shè)計(jì)。

方向軸(方向回轉(zhuǎn)平面法向)和高低軸(火炮耳軸)在地理坐標(biāo)系下的矢量分別為f=b1i+b2j+b3k，g=a1i+a2j+a3k。當(dāng)SINS安裝在身管后，其位置與耳軸的相對(duì)位置就固定，根據(jù)坐標(biāo)系的定義，此時(shí)就可先標(biāo)定在SINS橫軸與耳軸矢量在俯仰部分坐標(biāo)系下的關(guān)系。那么第l步g在火炮俯仰部分坐標(biāo)系下的實(shí)時(shí)矢量為gp(l)=a1pi+a2pj+a3pk，其中a1p=a3p=0，a2p=1，那么其中實(shí)時(shí)高低軸矢量g(l)為

(5)

方向矢量的通用求解是根據(jù)高低矢量，即

(6)

式中，L為間隔的計(jì)算步距，根據(jù)求解的Δα動(dòng)態(tài)確定，以適應(yīng)車體姿態(tài)的緩慢變化，反應(yīng)車體姿態(tài)的f變化也不快。由于SINS的姿態(tài)輸出值含有較小不確定值，若Δα為小角度時(shí)，f的求解將不準(zhǔn)確。由于f的變化比較緩慢，利用求解的Δα=|α0-αi|來(lái)確定L是完全可行的。α0、α1分別為改變確定f后的第1次求解值和最后1次求解值。

4 仿真與驗(yàn)證

自行火炮駐停時(shí)車體方位角為ψb=80°，火炮在車體系下的初始角度分別為：方位角βb=0°，高低角εb=0°，方位主令ψref=2 500mrad，高低主令θref=1 000mrad。高低伺服和方位伺服的驅(qū)動(dòng)傳遞函數(shù)分別為：bh=337，bp=523，ah=25，ap=40。飽和幅值為10V，死區(qū)絕對(duì)值為0.008V。方向隨動(dòng)P參數(shù)為0.5，I參數(shù)為0.001；高低隨動(dòng)P參數(shù)為0.7，I參數(shù)為0.001。采用四元數(shù)坐標(biāo)變換法求取控制誤差量后，火炮身管姿態(tài)伺服控制分別在車體初始姿態(tài)水平(θb=0°，φb=0°)和車體初始姿態(tài)傾斜6°(θb=6°，φb=6°)的情況下進(jìn)行仿真，得到結(jié)果如圖3～6所示。

圖3、4分別為車體初始姿態(tài)水平及姿態(tài)傾斜6°條件下調(diào)炮時(shí)車體姿態(tài)變化曲線，表明車體姿態(tài)隨著伺服系統(tǒng)調(diào)轉(zhuǎn)控制結(jié)束也趨于穩(wěn)定至定值，伺服系統(tǒng)調(diào)轉(zhuǎn)啟動(dòng)、制動(dòng)加速度對(duì)車體姿態(tài)而言是較大的擾動(dòng)。同時(shí)，車體姿態(tài)變化也是瞄準(zhǔn)系統(tǒng)的擾動(dòng)，由于車體懸掛系統(tǒng)的彈性變形導(dǎo)致車體姿態(tài)變化，放大了對(duì)操瞄控制系統(tǒng)的擾動(dòng)。

圖5、6分別為火炮身管姿態(tài)控制應(yīng)用四元數(shù)坐標(biāo)變化法后，在車體初始姿態(tài)水平及姿態(tài)傾斜6°條件下火炮身管在大地坐標(biāo)下的調(diào)炮角度曲線。在車體初始姿態(tài)為水平情況下，高低伺服和方向伺服系統(tǒng)到位調(diào)整時(shí)間較短。而在車體初始傾斜6°條件下，方向伺服調(diào)整時(shí)間略長(zhǎng)，有約10mrad的超調(diào)，但滿足以0.1mrad誤差作為到位評(píng)定標(biāo)準(zhǔn)下，其調(diào)炮時(shí)間滿足系統(tǒng)要求，且調(diào)炮過(guò)程中未引起車體姿態(tài)振蕩，車體姿態(tài)平穩(wěn)變化趨于穩(wěn)定。雖然采用四元數(shù)的坐標(biāo)變化法，算法略微復(fù)雜，但對(duì)控制器的設(shè)計(jì)要求降低了。

圖7、8分別為車體坐標(biāo)系下的火炮身管指向在車體初始姿態(tài)水平及車體初始姿態(tài)傾斜6°條件下α、β的調(diào)轉(zhuǎn)角度，與圖5、6比較可知，β≠Δψ,α≠Δθ，即使在車體姿態(tài)初始姿態(tài)為水平的情況下，因?yàn)榛鹋谏砉茉谡{(diào)轉(zhuǎn)過(guò)程中的其不平衡力矩使車體姿態(tài)緩慢歪斜，不再水平，故通常情況下β≠Δψ,α≠Δθ。

5 結(jié)論

考慮到火炮身管姿態(tài)伺服控制系統(tǒng)中的各伺服分系統(tǒng)車體坐標(biāo)系下存在速度耦合干擾，筆者采用四元數(shù)坐標(biāo)變換法，將捷聯(lián)慣導(dǎo)測(cè)量值與主令諸元從地理坐標(biāo)系下轉(zhuǎn)換到車體坐標(biāo)下，進(jìn)而求解火炮身管方位向和高低向調(diào)轉(zhuǎn)角。這個(gè)過(guò)程無(wú)需其他參數(shù)，且對(duì)控制器的設(shè)計(jì)要求不高。仿真研究結(jié)果表明：采用四元數(shù)坐標(biāo)變換法的火炮身管伺服系統(tǒng)能在有限時(shí)間內(nèi)實(shí)現(xiàn)平穩(wěn)、精確瞄準(zhǔn)，超調(diào)小，滿足系統(tǒng)設(shè)計(jì)要求。該模型的建立和控制方法的應(yīng)用為此類系統(tǒng)的研究提供了另一種途徑。

References)

[1]嚴(yán)德斌，胡朝根，陳紅軍．基于激光陀螺捷聯(lián)慣導(dǎo)技術(shù)的火炮身管指向系統(tǒng)設(shè)計(jì)[J]．艦船電子工程，2011，31(9)：57-59. YAN Debin，HU Chaogen，CHEN Hongjun．Design of gun turntable system based on ring laser gyro strap-on inertial navigation system[J]．Ship Electronic Engineering， 2011，31(9)：57-59.(in Chinese)

[2]胡田，付慶紅，佟俐．剛體姿態(tài)控制在火炮自動(dòng)操瞄中的應(yīng)用[J]．火力與指揮控制，2009，34(12)：148-150． HU Tian，F(xiàn)U Qinghong，TONG Li．Application of rigid body attitude control in gun’s automatic aiming[J]．Fire Control & Command Control，2009，34(12)：148-150.(in Chinese)

[3]GU Y L，LOH R N K，COLEMAB N,et al．Control of wea-pon pointing systems base on robotic formulation[C]∥ Proceeding of the 1992 American Control Conference. USA：IEEE, 1992:413-419.

[4]田建輝，錢林方，徐亞棟，等．無(wú)人炮塔火力線跟蹤神經(jīng)滑模控制[J]．兵工學(xué)報(bào)，2011，32(6)：641-645.

TIAN Jianhui，QIAN Linfang，XU Yadong，et al．Neural sliding mode control for tracking of axis of firepower of unmanned turret[J]．Acta Armamentarii, 2011，32(6)：641- 645.(in Chinese)

[5]李偉，楊剛，陳騰飛，等．某全閉環(huán)操瞄系統(tǒng)的火炮身管指向控制研究[J]．兵工學(xué)報(bào)，2015，36(9)：1811- 1818． LI Wei，YANG Gang，CHEN Tengfei，et al．Reasearch on gun pointing control of a full closed-loop aimming system[J]．Acta Armamentarii, 2015，36(9)：1811-1818.(in Chinese)

[6]喻凡．車輛動(dòng)力學(xué)及其控制[M]．北京：機(jī)械工業(yè)出版社, 2013：174-182. YU Fan．Armored vehicle dynamics and control[M]．Beijing：China Machine Press, 2013:174-182.(in Chinese)

[7]IKENAGA S,LEWIS F L,CAMPOS J,et al．Active suspension control of ground vehicle based on a full-vehicle model[C]∥Proceedings of 2000 American Control Confe-rence．USA：IEEE，2000，6: 4019-4024．

[8]湯勇剛，楊建文，李宗良. 基于四元數(shù)法的火炮隨動(dòng)系統(tǒng)解耦控制[J].火力與指揮控制學(xué)報(bào)，2002，27(5) :75- 78． TANG Yonggang，YANG Jianwen，LI Zongliang．Decoup-ling control of fire control servo system based on quaternion method[J]．Fire Control & Command Control，2002，27(5): 75-78．(in Chinese)

Gun Tube Position Servo Control Based on Quaternion Coordinate Transformation Method

HAN Chongwei, LI Wei, CHEN Tengfei, YANG Gang, ZHANG Zhipeng

(Northwest Institute of Mechanical & Electrical Engineering, Xianyang 712099, Shaanxi, China)

Strapdown Inertial Navigation System (SINS) is used to measure the pointing of gun barrel for highly accurate aiming with tracked self-propelled artillery. However, it causes the control coupling of pitching servo and azimuth servo and the aiming property to be easily affected by vehicle posture. According to the full closed-loop aiming system model under vehicle coordinates, the servo control model under quasi geodetic coordinates including the disturbance of mismatched uncertainty is obtained. Active Disturbance Rejection Controllers (ADRC) is designed for each axial servo system, with the coupling and modeling error considered to be external disturbance. ADRC can be used to observe the external disturbance and compensate for the nonlinear state error controller to eliminate the overshoot caused by it. Simulation of servo aiming systems with the initial posture of vehicle being horizontal and inclination degree being 6 degrees is carried out respectively. The results show that compared with the adaptive sliding mode variable structure control, the servo system using ADRC strategy obtains more steady and more accurate targeting results, and gets smoother controller output, but suppresses the unmatched uncertain interface effectively, which satisfies the design requirements of the system.

ordnance science and technology;self-propelled artillery;SINS;quaternion;servo system

10.19323/j.issn.1673-6524.2016.04.003

2015-12-17

韓崇偉(1963—)，男，研究員級(jí)高級(jí)工程師，主要從事火炮伺服系統(tǒng)技術(shù)研究。E-mail：hangreat202@163.com

TJ818,TP273

A

1673-6524(2016)04-0010-05