林笑亦,劉 軍,蘇 浩,鐘正虎

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

0 引言

在半實物仿真系統(tǒng)中,三軸仿真轉(zhuǎn)臺是最常用的硬件設(shè)備［1］,可以模擬飛行器在空間中滾轉(zhuǎn)、俯仰和偏航三個姿態(tài)的變化,從而復(fù)現(xiàn)飛行器運動時的各種姿態(tài)運動及動力學(xué)特性,最終實現(xiàn)飛行器整體設(shè)計的性能優(yōu)化。對于三軸仿真轉(zhuǎn)臺,每一個軸對應(yīng)一個姿態(tài)角,物理含義明確,因此應(yīng)用非常便利。但是,如果三軸轉(zhuǎn)臺在工作的過程中出現(xiàn)兩軸重合的情況,就會由三個自由度退化為兩個自由度,從而出現(xiàn)運動學(xué)奇異,無法模擬空間三個自由度的姿態(tài)運動。除此之外,當(dāng)三軸仿真轉(zhuǎn)臺在鄰近奇異位置運動時,其運動學(xué)傳遞特性會急劇變差,極大地增加了轉(zhuǎn)臺的研制難度。為了克服上述的運動學(xué)奇異性問題,四軸全姿態(tài)仿真轉(zhuǎn)臺的發(fā)展有著重要的意義［2］。

四軸轉(zhuǎn)臺是在三軸轉(zhuǎn)臺的基礎(chǔ)上增加一個冗余軸,利用冗余自由度機構(gòu)的特點［3-6］來規(guī)避三軸轉(zhuǎn)臺面臨的運動學(xué)奇異性問題,從而實現(xiàn)飛行器空間運動的全姿態(tài)全彈道仿真,對于大角度機動飛行器的研制具有極其重要的意義［7］。對于四軸轉(zhuǎn)臺,在初始狀態(tài)下有兩個運動軸重合,各軸的轉(zhuǎn)角與飛行器的姿態(tài)角沒有一一映射關(guān)系,因此對于某種確定的飛行狀態(tài),四軸轉(zhuǎn)臺有無數(shù)種框架角與之對應(yīng)。同時,由于四軸轉(zhuǎn)臺各軸轉(zhuǎn)角與姿態(tài)角的映射為非線性關(guān)系,進一步增加了框架角解算的難度。因此,如何求得最優(yōu)框架角成為四軸轉(zhuǎn)臺設(shè)計和研究的技術(shù)難點。目前,針對四軸轉(zhuǎn)臺控制策略的研究還相對較少。2010年,瑞士ACUTRONIC公司的Carter等［8］通過加權(quán)最小二乘法建立了四軸轉(zhuǎn)臺的框架角約束優(yōu)化條件。當(dāng)轉(zhuǎn)臺在遠離奇異位置的區(qū)域時,其按照三軸轉(zhuǎn)臺運行;當(dāng)轉(zhuǎn)臺臨近奇異位置時,通過自適應(yīng)調(diào)整基框架權(quán)值,使轉(zhuǎn)臺按照四軸轉(zhuǎn)臺運動,從而遠離奇異位置。同年,王曉晨等［9］通過梯度投影法求得了帶有指標優(yōu)化的框架角速度最優(yōu)解,并應(yīng)用動態(tài)控制實時計算了四軸轉(zhuǎn)臺最優(yōu)框架角。2015年,基于約束優(yōu)化理論,徐勤貝［10］通過Lagrange乘子法計算得到了四軸轉(zhuǎn)臺的最優(yōu)框架角。2017年,趙軍虎等［11］提出了一種四軸平臺隨動回路的方法,即當(dāng)外框架角等于±90°時,可以調(diào)整隨動框架旋轉(zhuǎn)90°以使得平臺規(guī)避不穩(wěn)定位置。

基于以上研究,本文提出了一種基于阻尼加權(quán)最小范數(shù)法的四軸轉(zhuǎn)臺框架角解算算法,在約束最小角速度的同時引入了權(quán)值分配和變阻尼因子,從而增強了四軸轉(zhuǎn)臺規(guī)避奇異的能力以及運動學(xué)逆解的奇異魯棒性,達到了實現(xiàn)全姿態(tài)仿真的目的。本文首先基于四元數(shù)法對四軸轉(zhuǎn)臺進行了正運動學(xué)分析,給出了四軸轉(zhuǎn)臺的運動學(xué)模型。其次,通過阻尼加權(quán)最小范數(shù)法構(gòu)建了四軸轉(zhuǎn)臺的最優(yōu)化條件并引入阻尼因子,得到了四軸框架角速度的奇異魯棒性解,通過閉環(huán)逆運動學(xué)算法得到了四個框架角,并進一步通過二次優(yōu)化提高了框架角解算的精度,確保了飛行器姿態(tài)的準確性。最后,通過仿真對算法進行了驗證。

1 四軸轉(zhuǎn)臺正運動學(xué)分析

1.1 飛行器姿態(tài)分析

飛行器在空間中的姿態(tài)可以通過地面坐標系與飛行器坐標系之間的轉(zhuǎn)換矩陣來表示?，F(xiàn)定義飛行器坐標系是按照Y-Z-X的順序旋轉(zhuǎn)得到,其旋轉(zhuǎn)過程如圖1所示。首先,飛行器繞慣性坐標系Oy軸旋轉(zhuǎn)ψ角,得到中間坐標系O-x′yz′;再繞中間坐標系的Oz′軸旋轉(zhuǎn)θ角,得到中間坐標系O-Xy′z′;最后繞OX軸旋轉(zhuǎn)γ角,得到飛行器坐標系O-XYZ。在此定義下,三次旋轉(zhuǎn)的單位四元數(shù)為

圖1 姿態(tài)角旋轉(zhuǎn)示意圖Fig.1 Schematic diagram of attitude angle rotation

通過飛行器的單位四元數(shù),可以進一步由式（1）計算得到其四元數(shù)矩陣

1.2 四軸轉(zhuǎn)臺姿態(tài)分析

四軸轉(zhuǎn)臺是在三軸轉(zhuǎn)臺的基礎(chǔ)上增加一個與中框架相同方向的基框軸,屬于冗余自由度機構(gòu)。在轉(zhuǎn)臺工作過程中,四軸轉(zhuǎn)臺利用冗余軸系的運動來避免運動奇異,從而保障轉(zhuǎn)臺能夠模擬飛行器在空間偏航、俯仰、滾轉(zhuǎn)三個方向上的全姿態(tài)運動。四軸轉(zhuǎn)臺分為立式和臥式兩種,本文研究的是臥式四軸轉(zhuǎn)臺,其結(jié)構(gòu)原理圖如圖2所示。由圖2可知,臥式四軸轉(zhuǎn)臺是在立式三軸轉(zhuǎn)臺的基礎(chǔ)上增加一個基框架,其四個框架角由外到內(nèi)依次為基框、外框、中框和內(nèi)框,旋轉(zhuǎn)角度分別為δ1、δ2、δ3和δ4,如圖3所示。

圖2 四軸轉(zhuǎn)臺結(jié)構(gòu)原理圖Fig.2 Structure schematic diagram of four-axis turntable

圖3 框架角旋轉(zhuǎn)示意圖Fig.3 Schematic diagram of frame angle rotation

對于臥式四軸轉(zhuǎn)臺,四個框架的單位四元數(shù)為

通過各個框架的單位四元數(shù),可以進一步由式（4）計算得到轉(zhuǎn)臺上負載的姿態(tài)四元數(shù)矩陣

式（5）中,q10、q11、q12、q13滿足

由于式（2）和式（5）轉(zhuǎn)動等效,可得

由式（7）可知,四個框架角δ1、δ2、δ3、δ4和三個姿態(tài)角ψ、θ、γ之間并不是一一對應(yīng)的關(guān)系,在給定三個姿態(tài)角的情況下有無數(shù)組框架角與之對應(yīng)。接下來,將通過阻尼加權(quán)最小范數(shù)法和二次優(yōu)化得到最優(yōu)解。

2 四軸轉(zhuǎn)臺逆運動學(xué)分析

2.1 阻尼加權(quán)最小范數(shù)法

根據(jù)式（7）,將等式對時間求導(dǎo)可以得到

式（9）中,J=Jx+Jδ。

為了計算四軸轉(zhuǎn)臺的最優(yōu)框架角速度,基于加權(quán)最小范數(shù)法［13-14］構(gòu)建了式（8）的約束優(yōu)化條件

式（10）中,W為加權(quán)矩陣,定義如下

式（11）中,w1、w2、w3和w4分別為基框架、外框架、中框架和內(nèi)框架的權(quán)重因子,具體定義后文會詳述（2.3節(jié)）。

對于式（10）所示的約束優(yōu)化條件,基于加權(quán)最小范數(shù)法重新定義加權(quán)矩陣和加權(quán)框架角速度

因此,對應(yīng)的式（9）可以表示為

其對應(yīng)的解為文獻［15］中提出的針對冗余自由度機構(gòu)的加權(quán)最小范數(shù)解

這里的通解與傳統(tǒng)最小范數(shù)法的通解不同,主要是針對多解問題的最小范數(shù)解。

對式（16）中矩陣JW-1JT進行奇異值分解,可以求得框架角速度的范數(shù)為

式（17）中,［a1a2a3a4］T=UT,U由J=UΣVT分解求得,σi為矩陣JW-1JT的奇異值。由式（17）可知,矩陣奇異值越小,框架角速度范數(shù)越大,而當(dāng)奇異值接近于零時,框架角速度范數(shù)為無窮大,影響四軸轉(zhuǎn)臺的穩(wěn)定性。因此,本文中進一步引入了阻尼因子λ,得到了加權(quán)最小范數(shù)法的奇異魯棒性解

在引入阻尼因子后,可以得到框架角速度的范數(shù)解為

由式（19）可知,在引入阻尼因子后,可以有效地避免奇異值過小時框架角速度過大的情況。阻尼因子的設(shè)置采用Chiaverini［16］提出的連續(xù)可調(diào)的阻尼因子設(shè)置方法,如式（20）所示。若所求得的σmin小于所設(shè)定的臨界值,引入阻尼因子可以提升逆解的魯棒性。

式（20）中,σmin為Jacobi矩陣奇異值的最小數(shù),σ0為最小奇異值臨界值,λmax為最大阻尼因子。對于最大阻尼因子λmax以及最小奇異值臨界值σ0的選取原則,如果取值過大,雖然可以有效地克服奇異,但是卻引入了較大的跟蹤誤差;相反,如果取值過小,奇異則得不到有效地解決,系統(tǒng)運動平滑性不好。因此,在算法應(yīng)用的過程中通過反復(fù)調(diào)整得到兩者最優(yōu)值,從而使框架角能平滑過渡,不會出現(xiàn)太大的瞬時框架角速度。

2.2 基于閉環(huán)逆運動學(xué)的奇異魯棒性逆解解算

由于最小范數(shù)法屬于開環(huán)算法,每一次計算的過程中都會產(chǎn)生誤差,誤差的不斷累積會導(dǎo)致求解精度過低。因此,本文進一步引入閉環(huán)求解,通過迭代的方式使結(jié)果達到求解精度。

式（16）經(jīng)過離散后,得到

式（21）中,Δδi=δi+1-δi,δi為第i次迭代中當(dāng)前時刻的框架角位置,δi+1為下一次迭代中的框架角位置,ei=x（k+1）-x。其中,x（k+1）為下一時刻的期望姿態(tài)角位置,x為當(dāng)前時刻的姿態(tài)角位置。

為了充分平衡計算時間和計算精度,在算法中的計算誤差設(shè)置為1×10-3°。為了進一步提升解算精度,本文在每次迭代完成后引入了二次優(yōu)化。由于每次迭代都以前一時刻的框架角為初始值,因此二次優(yōu)化還可以提升初始值的準確性,從而提升每次迭代收斂的快速性。

式（22）中,p10、p11、p12、p13滿足

進而可以求得二次優(yōu)化后除基框架角外的其他框架角

綜上所述,基于阻尼最小范數(shù)法的閉環(huán)逆運動學(xué)原理框圖如圖4所示。

圖4 基于閉環(huán)阻尼最小范數(shù)法的運動學(xué)逆解框圖Fig.4 Block diagram of inverse kinematics solution based on closed-loop damping least-norm method

具體的迭代步驟如下:

1）初始化各個參數(shù),首先初始化框架角δ1（0）、δ2（0）、δ3（0）、δ4（0）以及參數(shù)λmax、σ0、ε;

2）計算下一時刻目標姿態(tài)角位置x（k+1）和當(dāng)前時刻姿態(tài)角位置x的差值ei;

3）計算Jacobi矩陣Jx和Jδ,同時計算矩陣J=Jδ;

4）計算矩陣J的奇異值最小值σmin,比較σmin和最小奇異值臨界值σ0,從而確定阻尼因子的大小;

5）根據(jù)Δδi=W-1（JiW-1+λ2I）-1ei計算每一個迭代步中的框架角增量Δδi,令δi+1=δi+Δδi得到下一個迭代步中的框架角;

6）通過運動學(xué)正解計算當(dāng)前時刻的姿態(tài)角位置x;

7）根據(jù)期望姿態(tài)角位置計算誤差ei,判斷誤差ei是否達到求解精度,若ei＞ε,則返回步驟3繼續(xù),若ei≤ε,得到滿足條件的框架角δ^=δi;

8）應(yīng)用二次計算模塊對δ^進行優(yōu)化,得到下一時刻的框架角δ（k+1）,返回步驟2進行下一次計算,直至完成計算周期T,迭代結(jié)束。

2.3 權(quán)重因子優(yōu)化設(shè)計

根據(jù)四軸轉(zhuǎn)臺的框架結(jié)構(gòu),由文獻［7］可知,當(dāng)四軸轉(zhuǎn)臺的兩兩軸重合,即δ2=0°且δ3=±90°或δ2=-180°且δ3=±90°時,會產(chǎn)生四軸轉(zhuǎn)臺自身運動學(xué)奇異,由此引入優(yōu)化函數(shù)H1和H2

定義外框架權(quán)重

式（27）中,參數(shù)A1、A2、B1、B2根據(jù)實際情況確定。

外框架的權(quán)重因子w2在特定條件下的變化曲線如圖5所示。外框架的權(quán)重因子由δ2、δ3確定以規(guī)避四軸轉(zhuǎn)臺自身的奇異性,當(dāng)δ2、δ3同時滿足奇異條件時,外框架的權(quán)重因子w2增大,從而達到規(guī)避四軸轉(zhuǎn)臺自身奇異位置的目的。

圖5 外框架權(quán)重因子變化曲線Fig.5 Change curve of outer frame weight factor

由式（16）可知,對于任意的三個目標姿態(tài)角都可以由四個框架角表示。在解算中框架角度時,為了避免反三角函數(shù)的多解問題,引入了框架角優(yōu)化指標函數(shù)

式（28）中,δ3min≤δ3≤δ3max。

通過優(yōu)化指標函數(shù),對中框架的權(quán)重因子w3進行定義［17］

中框架的權(quán)重因子w3的變化曲線如圖6所示。可以看出,當(dāng)中框架接近±90°時,中框架權(quán)重因子w3隨之增大,趨近于無窮,通過優(yōu)化權(quán)重因子將中框架角約束到了（-90°,90°）的范圍內(nèi)。

圖6 中框架權(quán)重因子變化曲線Fig.6 Change curve of middle frame weight factor

根據(jù)式（28）,定義基框架權(quán)重為

式（30）中,參數(shù)K根據(jù)實際情況確定。

基框架的權(quán)重因子w1隨中框架角的變化曲線如圖7所示。四軸轉(zhuǎn)臺在運動的過程中,當(dāng)中框架趨于奇異位置時,需要基框架權(quán)重值減小,運動速度增大,從而避免奇異;當(dāng)中框架與奇異位置較遠時,則需要增大權(quán)重值,減小運動速度,以達到降低電機負荷、提升轉(zhuǎn)臺動態(tài)性能的目的。

圖7 基框架權(quán)重因子變化曲線Fig.7 Change curve of base frame weight factor

在四軸轉(zhuǎn)臺四個框架中,內(nèi)框架的動態(tài)性能最好,為了保證內(nèi)框架在轉(zhuǎn)動范圍內(nèi)保持高速運動,設(shè)置內(nèi)框架的權(quán)重因子w4=1。

3 仿真分析與實驗驗證

由Matlab計算得到目標姿態(tài)角輸入下的框架角如圖8所示。

由圖8可知,飛行器的軌跡中存在俯仰角和偏航角都≥90°的情況,接下來分別用立式三軸轉(zhuǎn)臺和臥式三軸轉(zhuǎn)臺模擬彈道曲線。

圖8 目標姿態(tài)角曲線Fig.8 Curves of target attitude angle

根據(jù)旋轉(zhuǎn)順序,可得彈體的角速度

對立式三軸轉(zhuǎn)臺,其外框為偏航ψl,中框為俯仰θl,內(nèi)框為滾轉(zhuǎn)γl。旋轉(zhuǎn)順序為先偏航、再俯仰、最后滾轉(zhuǎn),姿態(tài)微分方程為

對臥式三軸轉(zhuǎn)臺,其外框為俯仰θw,中框為偏航ψw,內(nèi)框為滾轉(zhuǎn)γw。旋轉(zhuǎn)順序為先俯仰、再偏航、最后滾轉(zhuǎn),姿態(tài)微分方程為

由式（31）可知,在θl=±90°時,cosθl=0,此時外框和內(nèi)框重合,導(dǎo)致立式三軸轉(zhuǎn)臺丟失一個自由度,產(chǎn)生奇異性,無法模擬負載三個自由度的姿態(tài)運動。同理,由式（32）可知,在ψw=±90°時,cosψw=0,此時外框和內(nèi)框重合,導(dǎo)致臥式三軸轉(zhuǎn)臺丟失一個自由度,產(chǎn)生奇異性,無法模擬負載三個自由度的姿態(tài)運動。

三軸轉(zhuǎn)臺的角速度曲線如圖9所示。

由圖9可知,無論是立式三軸轉(zhuǎn)臺還是臥式三軸轉(zhuǎn)臺,在模擬圖8給定的彈道曲線的過程中會出現(xiàn)奇異,角速度發(fā)生突變從而導(dǎo)致角加速度過大,超過了轉(zhuǎn)臺的驅(qū)動能力,導(dǎo)致仿真斷開。

圖9 三軸轉(zhuǎn)臺框架角速度曲線Fig.9 Frame angular velocity curves of three-axis turntable

利用本文的運動學(xué)反解算法可以得到四軸轉(zhuǎn)臺的四個框架角位置變化曲線,如圖10所示。其角速度和角加速度曲線如圖11、圖12所示。

圖10 四軸轉(zhuǎn)臺框架角位置變化曲線Fig.10 Frame angle position change curves of four-axis turntable

圖12 四軸轉(zhuǎn)臺框架角加速度曲線Fig.12 Frame angular acceleration curves of four-axis turntable

由圖10～圖12可知,四個框架角變化平滑,角速度和角加速度都在轉(zhuǎn)臺運行范圍內(nèi),即使在飛行器俯仰角大于90°時也不會出現(xiàn)突變,充分說明了所提出算法的合理性。

為了驗證算法的精度,進一步由框架角正運動學(xué)計算得到了飛行的姿態(tài)角,并分析了輸入與輸出姿態(tài)軌跡之間的誤差,如圖13所示。

由圖13可知,理論計算上,滾轉(zhuǎn)角誤差絕對值的最大值為1.1×10-9°,偏航角誤差絕對值的最大值為4.9×10-9°,俯仰角誤差絕對值的最大值為7.1×10-11°,充分說明了所提出方法的準確性。

圖13 四軸轉(zhuǎn)臺的解算誤差曲線Fig.13 Calculation error curves of four-axis turntable

最后,通過本單位研制的四軸轉(zhuǎn)臺對四軸轉(zhuǎn)臺自身的四個框架進行驗證,四軸轉(zhuǎn)臺實物如圖14所示。

圖14 四軸轉(zhuǎn)臺實物圖Fig.14 Physical drawing of four-axis turntable

輸入四軸轉(zhuǎn)臺的框架運動軌跡,并進一步通過運動學(xué)正解運算,得到姿態(tài)角的跟蹤曲線如圖15所示。

由圖15可知,姿態(tài)角的指令曲線和實際運行曲線（反饋曲線）之間存在一定的偏差,這跟轉(zhuǎn)臺控制器設(shè)計、機械結(jié)構(gòu)摩擦以及測角元件精度等有關(guān)。進一步輸出姿態(tài)角的幅值跟蹤誤差,如圖16所示。

圖15 四軸轉(zhuǎn)臺目標姿態(tài)角跟蹤曲線Fig.15 Target attitude angle tracking curves of four-axis turntable

圖16 四軸轉(zhuǎn)臺目標姿態(tài)角幅值跟蹤誤差Fig.16 Target attitude angle amplitude tracking error curves of four-axis turntable

由圖16可知,滾轉(zhuǎn)角誤差絕對值的最大值為2.2×10-3°,偏航角誤差絕對值的最大值為2.2×10-3°,俯仰角誤差絕對值的最大值為1.9×10-3°,滿足轉(zhuǎn)臺的技術(shù)指標。

綜上所述,通過Matlab仿真驗證和實驗可知,本文所提出的方法能夠高精度地解決四軸轉(zhuǎn)臺運動學(xué)逆解中的多解問題,而且可以確保四個框架角變化平穩(wěn),不會出現(xiàn)突變,在誤差允許范圍內(nèi),滿足飛行器全姿態(tài)仿真的要求［18］。

4 結(jié)論

本文基于阻尼加權(quán)最小范數(shù)法提出了四軸轉(zhuǎn)臺的閉環(huán)逆運動學(xué)求解方法,可以實現(xiàn)四軸轉(zhuǎn)臺框架角的高精度解算。在所提出的方法中,結(jié)合轉(zhuǎn)臺實際工況引入了權(quán)重因子和阻尼因子,避免了Jacobi矩陣接近奇異時對算法的影響,充分確保了運算過程的穩(wěn)定性。由閉環(huán)逆運動學(xué)計算得到阻尼最小范數(shù)法的奇異魯棒性逆解后,進一步引入了二次優(yōu)化,不僅降低了運算時間,而且確保了算法的準確性。最后,通過Matlab仿真驗證和實驗驗證了算法的正確性。由仿真結(jié)果可知,本文所提出的方法可以平滑地將姿態(tài)角高精度地轉(zhuǎn)化為框架角,確保了四軸轉(zhuǎn)臺工作過程中的連續(xù)性,為大機動飛行器的全姿態(tài)仿真奠定了基礎(chǔ)。