基于阻尼最小范數法的四軸轉臺逆運動學算法分析

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

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

0 引言

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

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

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

1 四軸轉臺正運動學分析

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

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

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

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

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

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

圖2 四軸轉臺結構原理圖Fig.2 Structure schematic diagram of four-axis turntable

圖3 框架角旋轉示意圖Fig.3 Schematic diagram of frame angle rotation

對于臥式四軸轉臺,四個框架的單位四元數為

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

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

由于式（2）和式（5）轉動等效,可得

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

2 四軸轉臺逆運動學分析

2.1 阻尼加權最小范數法

根據式（7）,將等式對時間求導可以得到

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

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

式（10）中,W為加權矩陣,定義如下

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

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

因此,對應的式（9）可以表示為

其對應的解為文獻［15］中提出的針對冗余自由度機構的加權最小范數解

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

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

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

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

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

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

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

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

式（16）經過離散后,得到

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

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

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

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

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

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

具體的迭代步驟如下:

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

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

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

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

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

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

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

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

2.3 權重因子優(yōu)化設計

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

定義外框架權重

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

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

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

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

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

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

中框架的權重因子w3的變化曲線如圖6所示?？梢钥闯?當中框架接近±90°時,中框架權重因子w3隨之增大,趨近于無窮,通過優(yōu)化權重因子將中框架角約束到了（-90°,90°）的范圍內。

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

根據式（28）,定義基框架權重為

式（30）中,參數K根據實際情況確定。

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

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

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

3 仿真分析與實驗驗證

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

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

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

根據旋轉順序,可得彈體的角速度

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

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

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

三軸轉臺的角速度曲線如圖9所示。

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

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

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

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

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

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

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

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

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

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

圖14 四軸轉臺實物圖Fig.14 Physical drawing of four-axis turntable

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

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

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

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

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

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

4 結論

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