旋轉矢量在高動態全姿態飛行器運動方程中的應用

王紅輝，楊紹卿1，吳成富2，郝峰1，車曉濤1

（1.西安現代控制技術研究所，陜西西安710065；2.西北工業大學自動化學院，陜西西安710072）

旋轉矢量在高動態全姿態飛行器運動方程中的應用

王紅輝1，2，楊紹卿1，吳成富2，郝峰1，車曉濤1

（1.西安現代控制技術研究所，陜西西安710065；2.西北工業大學自動化學院，陜西西安710072）

基于旋轉矢量是表示角位置變化所對應的等效旋轉而不是角位置本身這一基本思想，同時借鑒旋轉矢量在捷聯慣性導航算法中的應用，建立了將旋轉矢量應用于高動態全姿態飛行器運動方程的數學框架。既克服了歐拉角法不適于全姿態解算的缺點，同時，相比于四元數法又提高了高動態角運動情況下姿態解算的效率。對旋轉矢量法、四元數法和歐拉角法在數值解算中的不可交換誤差進行了分析。據此，針對單通道具有高動態特性的軸對稱飛行器，建立了基于準彈體系的旋轉矢量法，提高了解算效率。基于某型滾轉導彈運動方程的數字仿真表明了旋轉矢量法在姿態解算中的有效性和廣泛性。

飛行器控制、導航技術；旋轉矢量；歐拉角；四元數；高動態；全姿態

0 引言

飛行器運動方程是表征飛行器運動規律的數學模型。對飛行器運動方程進行快速精確的數值解算，具有重要的理論和應用價值。其中，姿態解算是整個運動方程解算中十分重要的一環。

在飛行器的運動姿態解算中，常用的有歐拉角法和四元數法［1-2］。任意轉動都可通過坐標軸的連續3次旋轉來描述，這3個轉角就形成了一組歐拉角［3］。關于歐拉角的轉動次序，科學界尚未達成統一，對3次連續轉動的唯一約束為兩次相鄰轉動軸不能是同一個坐標軸。據此，歐拉角共有12種合法定義［4］。歐拉角在姿態描述中的優點是物理意義明確，但是在某些角度會出現運動學奇點。對不同的歐拉角轉動次序，出現的奇點會有所不同，因此，歐拉角不適于全姿態的解算。四元數是在19世紀中期由愛爾蘭數學家Hamilton引入數學中的，直到20世紀60年代才被應用于姿態解算中［5］。四元數在姿態解算中的優點是不會出現奇點，且四元數的運算律適于進行姿態解算。四元數的缺點是其4個參數的物理意義不明確，用于描述轉動的4個參數必須滿足歸一化條件［6-7］。但是，在姿態數值解算過程中，這種歸一化條件將會喪失，從而需要重新進行歸一化［8-9］。這說明四元數在姿態解算中存在明顯的歸一化誤差。同時，在剛體做有限轉動時，剛體的空間角位置與旋轉次序有關，即在姿態解算中存在旋轉的不可交換性誤差。龍格-庫塔法是飛行器運動微分方程比較常用的一種數值解法。四元數的龍格-庫塔法實際上相當于四元數畢卡算法的有限階近似，本文將對此予以分析。但四元數的畢卡算法僅為單子樣算法，數值解算的不可交換性誤差較大，尤其在高動態環境中，這種不可交換性誤差將更加嚴重［7］。為了提高四元數解算的精度，只能通過降低數值解算的步長來減小算法誤差。

除了歐拉角法和四元數算法外，可用于姿態解算的還有方向余弦矩陣法、羅德里格參數（或稱為吉布斯矢量）法［10-12］、等效旋轉矢量法［13-15］等。方向余弦矩陣可以根據兩個坐標系之間夾角的余弦定義，也可根據一個坐標的基矢量在另一個坐標的投影來定義［3，16］。因此，方向余弦矩陣的優點是物理意義明確，但其缺點是在求解時待定參數較多，并且存在正交歸一化誤差。羅德里格參數實質上是四元數在三維超平面上的投影，它去除了四元數的一個冗余度，因此存在和四元數同樣的誤差，同時在某些角度也會出現奇異性。等效旋轉矢量的概念由Bortz和Jordan于1971年提出［13，17-18］。Bortz根據方向余弦矩陣和旋轉矢量的解析關系，嚴格推導了旋轉矢量所滿足的微分方程，得出旋轉矢量的微分等于角速度加上非交換誤差項，從而將由旋轉的不可交換性引入的姿態誤差分離了出來。Bortz的工作為姿態解算的不可交換性誤差的分析和校正提供了嚴格的理論基礎。

等效旋轉矢量可由坐標系內的一個方向及坐標系在該方向上的旋轉角度定義，它表征不同時刻角位置變化所對應的一次性等效旋轉而不反映其中間過程。由于旋轉矢量對不可交換性誤差的分離，它在適于高動態角運動的捷聯慣性導航算法中應用廣泛［14，19-25］。旋轉矢量和歐拉角、羅德里格參數一樣，均為姿態的三維參數化表示，可表示任意姿態。Stuelpnagel從三維轉動群的角度表明姿態的三維參數化表示無法同時滿足全局性和非奇異性［26］。因此，Bortz方程在某些角度同樣呈現出奇異性。這影響了它在全姿態飛行器運動方程中的直接應用。目前，尚沒有公開的文獻對旋轉矢量在飛行器運動方程中的數值解算進行過詳細的研究。

基于旋轉矢量是表示角位置變化所對應的等效旋轉而不是角位置本身這一基本思想，同時借鑒旋轉矢量在捷聯慣性導航算法中的應用，本文建立了將旋轉矢量應用于高動態全姿態飛行器運動方程的數學框架。其基本思想是把姿態信息以不具有奇異性的四元數的形式表示，把姿態變化信息以旋轉矢量的形式表示。在每次數值解算之后，把姿態變化信息從旋轉矢量的表示中轉移到四元數的表示中。如此，旋轉矢量在每次數值解算更新之前始終為0，而更新之后始終是一個小量，從而遠離其運動學奇點。同時，由于旋轉矢量是姿態的無約束表示，在整個計算過程中，四元數能夠始終滿足歸一化條件。這樣，既克服了歐拉角和四元數算法的缺點，又保留了其優點。基于某型滾轉導彈運動方程的數字仿真驗證了該算法的有效性。對于非全姿態運動的情況，本文詳細分析了歐拉角法和旋轉矢量算法的不可交換性誤差，指出了在某些特定情況下歐拉角法比旋轉矢量算法解算精度偏高的原因。據此，針對軸對稱飛行器（如滾轉導彈），建立了在單通道高動態情況下應用旋轉矢量的數學框架，從而使得旋轉矢量法取得了和歐拉角法相當的解算精度。

1 坐標系定義和符號約定

為便于分析，本文運動方程的建立采用平板地球假設，即不考慮地球曲率的影響。

地面坐標系（g系）：地面坐標系與地球固連，坐標原點位于發射點，x軸與發射點和目標連線重合，指向目標為正；y軸沿地垂線，指天為正；z軸按右手定則來確定。

彈體坐標系（b系）：坐標原點位于導彈的質心，x軸與彈縱軸方向一致，指向頭部為正；對于軸對稱型導彈，其余兩軸與彈體固聯。對于面對稱型，y軸位于彈體縱向對稱面內與x軸垂直，指向上為正，z軸按右手定則來確定。

準彈體坐標系（q系）：坐標原點位于導彈的質心，x軸與彈縱軸方向一致，指向頭部為正；y軸在鉛垂面內，通過原點O垂直于x軸，指向上方為正；z軸按右手定則來確定。

在上述坐標系定義下，彈體姿態歐拉角（ψ，?，γ）采用yzx的轉動次序。單位四元數q記為q= （q0，q1，q2，q3）T，qv=（q1，q2，q3）T為矢量部分，q0為標量部分。q的共軛為q*=（q0，-q1，-q2，-q3）T，其逆為q-1=q*.四元數p和q的乘法滿足

或

對于任一矢量u，定義p=（0，u），則有

在上述定義下，歐拉角（ψ，?，γ）可根據姿態四元數q計算如下：

矢量u和v的叉乘可寫為

式中：（u×）為矢量u的斜對稱矩陣。

任意兩個坐標系的坐標變換可以看做是經過無中間過程的一次性等效旋轉形成，可表示為旋轉矢量的形式：

式中：ua為在任意坐標系a系下的旋轉瞬軸；φ為旋轉角度。旋轉矢量Φ對應的姿態變化四元數為

2 基于旋轉矢量法的姿態解算

2.1數學框架

旋轉矢量Φ的運動學方程，即Bortz方程［13-15］為

旋轉矢量雖然不具有全局非奇異性，卻滿足局域非奇異性。基于此，可以采用旋轉矢量的小量形式進行姿態解算，即把Φ看成是彈體坐標系從tk時刻到tk+1時刻角位置變化所對應的等效旋轉矢量，而不是看成g系到b系的等效旋轉矢量。這樣，把每次數值解算之后以旋轉矢量形式表示的姿態變化轉移到姿態四元數當中，從而使得旋轉矢量始終保持為小量。具體解算流程如下：

初值：已知初始姿態角（ψ0，?0，γ0），則初始姿態四元數可表示為

式中：ψ0=（0，ψ0，0）；?0=（0，0，?0）；γ0=（γ0，0，0）.旋轉矢量的初值為Φ0=（0，0，0）.

龍格-庫塔法解算：對于φ≠0，直接根據（7）式和（8）式計算姿態變化四元數和旋轉矢量。對于φ=0，無法直接代入（7）式和（8）式進行計算。這時，根據旋轉矢量的定義，（7）式取為

（8）式求極限可得

龍格-庫塔法每次解算之后得到新的旋轉矢量Φ，然后按（12）式計算姿態四元數：

式中：q+為更新后的姿態四元數；q-為更新前的姿態四元數。由更新后的姿態四元數按（4）式可計算姿態角。當一個完整積分步長的計算完畢并且姿態四元數更新之后，置Φ=Φ0.

Bortz方程和其他關于位置、速度等變量的運動學和動力學方程構成了一組描述飛行器運動的、完備的常微分方程組［1-2］。整個運動方程中基于旋轉矢量法的姿態計算流程如圖1所示。

圖1　基于旋轉矢量法的姿態計算流程圖Fig.1 Flow chart of attitude solution based on rotating vector

2.2旋轉矢量法和四元數法

姿態四元數的運動學方程為

該方程的解［6］為

式中：

在采用龍格-庫塔法進行數值解算時，旋轉矢量Φ相當于采用（16）式近似計算：

式中：Δθ為由角速度直接積分得到的角增量。這相當于Bortz方程只保留了第一項。此時，（14）式即為四元數的畢卡算法。

置Φ=Δθ，對（14）式以Δθ為變量進行級數展開，可得四元數的有限階近似算法。如對于4階近似算法，有

已知4階龍格-庫塔法的截斷誤差為O（h5），h為計算步長［27］。由于每一步長h對應的角增量為Δθ，因此4階龍格-庫塔法的截斷誤差相當于O（Δθ5），即4階龍格-庫塔法等價于（17）式。類似分析可得，各階龍格-庫塔法等價于四元數畢卡算法相應階次的近似算法。

由于四元數的畢卡算法只相當于旋轉矢量的單子樣算法［7］，對有限轉動引起的不可交換誤差的補償程度不夠，所以不適合高動態飛行器運動姿態的解算。

另外，在對（13）式采用龍格-庫塔法進行數值解算時，四元數會喪失其歸一化特性，在每次數值更新之后需重新進行歸一化。因此，基于四元數的姿態解算存在著歸一化誤差。由于旋轉矢量是姿態的無約束表示，在由（7）式和（12）式計算姿態四元數時，整個過程歸一化自動滿足，只存在計算機的舍入誤差。

2.3旋轉矢量法和歐拉角法

由（18）式可見，如果ωx?ωy，ωz≈0，即滾轉通道是高動態的，另外兩個通道是低動態的，那么由數值計算得到的Φy和Φz的不可交換誤差將大于Φx的不可交換誤差。但考慮到姿態角還需要按照（12）式進行計算，因此，基于旋轉矢量算法的數值解算產生的不可交換誤差對3個姿態角均有相當程度的影響。

另一方面，歐拉角的運動學方程為

由（19）式可看出，3個歐拉角和角速度各分量之間相互耦合。對于面對稱型飛行器來說，歐拉角法和旋轉矢量法精度相當。對于軸對稱飛行器（如滾轉導彈）來說，一般采用準彈體坐標系來描述飛行器的運動，此時，（19）式變為

式中：ωqx、ωqy、ωqz為彈體坐標系b相對于地面坐標系g的角速度在準彈體坐標系q下的投影，它直接由描述轉動動力學方程產生，不經過中間過程［1］。由（20）式可看出，對于滾轉通道具有高動態的軸對稱飛行器來說，滾轉角的解算對另外兩個姿態角解算沒有直接影響，它只以滾轉角速度的形式參與到描述轉動的動力學方程中［1］。偏航角ψ雖然受不可交換誤差的影響，但由于偏航和俯仰通道都是低動態的，因此解算精度也比較高。俯仰通道不存在不可交換項。這樣，就通過準彈體坐標系的建立，把低動態的俯仰/偏航通道和高動態的滾轉通道相對隔離開來。因此，基于準彈體系的歐拉角法的解算精度要比直接利用基于彈體系的旋轉矢量法解算精度高。

為適于對滾轉導彈這類飛行器進行姿態解算，同樣可以建立基于準彈體系的旋轉矢量算法。準彈體坐標系相對于地面坐標系只需要兩個歐拉角（ψ，?）即可描述。設Φ表示準彈體坐標系的角位置變化，則Bortz方程變為

（21）式和（22）式構成了滾轉通道具有高動態的軸對稱飛行器的姿態解算方程。

3 仿真驗證

基于某型滾轉導彈的運動方程對本文所提算法進行仿真驗證。基準彈道基于歐拉角法解算，步長為0.1 ms.不同的姿態角算法只要步長足夠小，都可以作為基準彈道的解算方法。數值計算方法采用4階龍格-庫塔法。下面對不同步長不同姿態角算法下的數值計算和基準彈道進行比較分析。

3.1旋轉矢量法和四元數法

旋轉矢量法和四元數法分別采用（8）式和（13）式進行計算，計算步長均為5 ms，時長為60 s.仿真結果如圖2～圖4所示。

由圖2～圖4可見，相比于旋轉矢量法，采用四元數法計算時，不可交換誤差和歸一化誤差影響十分明顯。由于滾轉通道動態比較大，因此，滾轉角解算誤差也較大。

3.2旋轉矢量法和準彈體系下的歐拉角法

旋轉矢量法和歐拉角法分別采用（8）式和（20）式進行計算。為對比明顯，計算步長取為10 ms，時長為60 s.仿真結果如圖5～圖7所示。

由圖5～圖7可見，同樣步長下，基于彈體系的旋轉矢量法比基于準彈體系的歐拉角法解算精度要低。同時，由于步長的增大，旋轉矢量法由于各通道耦合比較嚴重，低動態的俯仰和偏航通道由于受到滾轉通道的影響，出現了明顯的振蕩現象，并有發散的趨勢。

3.3準彈體系下的旋轉矢量法和歐拉角法

旋轉矢量法采用（21）式和（22）式，歐拉角法采用（20）式，計算步長取20 ms，時長60 s.仿真結果如圖8～圖10所示。

由圖8～圖10可看出，基于準彈體系的旋轉矢量算法和歐拉角法解算精度幾乎完全一樣。這說明了旋轉矢量算法應用的廣泛性。

圖2　偏航角誤差曲線Fig.2 Curves of yaw angle error

圖3　俯仰角誤差曲線Fig.3 Curves of pitch angle error

圖4　滾轉角誤差曲線Fig.4 Curves of roll angle error

圖5　偏航角誤差曲線Fig.5 Curves of yaw angle error

圖6　俯仰角誤差曲線Fig.6 Curves of pitch angle error

圖7　滾轉角誤差曲線Fig.7 Curves of roll angle error

圖8　偏航角誤差曲線Fig.8 Curves of yaw angle error

圖9　俯仰角誤差曲線Fig.9 Curves of pitch angle error

圖10　滾轉角誤差曲線Fig.10 Curves of roll angle error

4 結論

1）基于旋轉矢量是表示角位置變化所對應的等效旋轉而不是角位置本身這一基本思想，同時借鑒旋轉矢量在捷聯慣性導航中的應用，本文建立了將旋轉矢量應用于高動態全姿態飛行器運動方程的數學框架。既克服了歐拉角法不適于全姿態解算的缺點，同時，相比于四元數法又提高了高動態角運動情況下進行姿態解算的效率。

2）分析了不可交換誤差和歸一化誤差對四元數法的影響，表明了旋轉矢量算法的優越性。

3）比較分析了不可交換誤差對旋轉矢量法和歐拉角法在姿態解算中的影響。指出了在某些特定情況下歐拉角法解算精度優于旋轉矢量法的原因，并據此建立了在單通道高動態情況下應用旋轉矢量法的數學框架，從而使得旋轉矢量法在這種情況下取得了和歐拉角法相當的解算精度。

4）數值仿真結果表明，本文提出的旋轉矢量法不僅特別適用于高動態全姿態飛行器運動方程的解算，而且具有應用的廣泛性。

（References）

［1］ 錢杏芳，林瑞雄，趙亞男.導彈飛行力學［M］.北京：北京理工大學出版社，2008. QIAN Xing-fang，LIN Rui-xiong，ZHAO Ya-nan.Flight mechanics of missiles［M］.Beijing：Beijing Institute of Technology Press，2008.（in Chinese）

［2］ 方振平，陳萬春，張曙光.航空飛行器飛行動力學［M］.北京：北京航空航天大學出版社，2012. FANG Zhen-ping，CHEN Wan-chun，ZHANG Shu-guang.Flight dynamics of aero vehicles［M］.Beijing：Beihang University Press，2012.（in Chinese）

［3］ Diebel J.Reprsenting attitude：Euler angles，unit quaternions，and rotationvectors［D］.Standford：Standford University，2006.

［4］ Greenwood D T.Principles of dynamics［M］.Upper Saddle River，New Jersey：Prentice Hall，1988.

［5］ 武元新.對偶四元數導航算法與非線性高斯濾波研究［D］.長沙：國防科學技術大學，2005. WU Yuan-xin.Research on dual quaternion navigation algorithm andnonlinear Gaussian filtering［D］.Changsha：National University of Defense Technology，2005.（in Chinese）

［6］ 高鐘毓.慣性導航系統技術［M］.北京：清華大學出版社，2012. GAO Zhong-yu.Inertial navigation system technology［M］.Beijing：Tsinghua University Press，2012.（in Chinese）

［7］ 秦永元.慣性導航［M］.北京：科學出版社，2010. QIN Yong-yuan.Inertial navigation［M］.Beijing：Science Press，2010.（in Chinese）

［8］ Deutschmann J，Markley F L，Bar-Itzhack I Y.Quaternion normalization in spacecraft attitude determination［C］∥Proceedings of the AIAA Guidance，Navigation，and Control Conference.Washington DC，US：AIAA，1991：908-916.

［9］ Bar-Itzhack I Y，Deutschmann J，Markley F L.Quaternion normalization in spacecraft attitude determination［C］∥Flight Mechanics and Estimation Theory Symposium.Hilton Head Island：AIAA，1992：441-454.

［10］ Schaub H，Junkins J L.Stereographic orientation parameters for attitude dynamics：a generalization of the Rodrigues parameters ［J］.Journal of the Astronautical Sciences，1996，44（1）：1-19.

［11］ Idan M.Estimation of Rodrigues parameters from vector observations［J］.IEEE Transactions on Aerospace and Electronic Systems，1996，32（2）：578-586.

［12］ Crassidis J L，Markley F L.Attitude estimation using modified Rodrigues parameters［C］∥Proceedings of Flight Mechanics/Estimation Theory Symposium.Greenblt，MD：NASA-Goddard Space Flight Center，1996：71-83.

［13］ Bortz J E.A new mathematical formulation for strapdown inertial navigation［J］.IEEE Transactions on Aerospace and Electronic Systems，1971，AES-7（1）：61-66.

［14］ Savage P G.Strapdown inertial navigation integration algorithm design part 1：attitude algorithms［J］.Journal of Guidance，Control，and Dynamics，1998，21（1）：19-28.

［15］ Shuster M.The kinematic equation for the rotation vector［J］. IEEE Transactions on Aerospace and Electronic Systems，1993，29（1）：263-267.

［16］ 楊紹卿.火箭外彈道偏差與修正理論［M］.北京：國防工業出版社，2011. YANG Shao-qing.The trajectory error and correction theory of rockets［M］.Beijing：National Defense Industry Press，2011. （in Chinese）

［17］ Jordan J W.An accurate strapdown direction cosine algorithm ［M］.Washington DC，US：National Aeronautics and Space Administration，1969.

［18］ Bortz J E.A new concept in strapdown inertial navigation［M］. Washington DC，US：National Aeronautics and Space Administration，1970.

［19］ Miller R B.A new strapdown attitude algorithm［J］.Journal of Guidance，Control，and Dynamics，1983，6（4）：287-291.

［20］ Lee J G，Mark J G，Tazartes D A，et al.Extension of strapdown attitude algorithm for high-frequency base motion［J］.Journal of Guidance，Control，and Dynamics，1990，13（4）：738-743.

［21］ Jiang Y F，Lin Y P.Improved strapdown coning algorithms［J］. IEEE Transactions on Aerospace and Electronic Systems，1992，28（2）：484-490.

［22］ Musoff H，Murphy J H.Study of strapdown navigation attitude algorithms［J］.Journal of Guidance，Control，and Dynamics，1995，18（2）：287-290.

［23］ Savage P G.Strapdown inertial navigation integration algorithm design part 2：velocity and position algorithms［J］.Journal of Guidance，Control，and Dynamics，1998，21（2）：208-221.

［24］ Wang Z，Yin X，Li P，et al.Application of rotation vector algorithm for SINS attitude updating［C］∥Proceedings of the 12th IEEE International Conference on Signal Processing.Hangzhou，China：IEEE，2014：2390-2393.

［25］ Wang Z，Chen X，Zeng Q.Comparison of strapdown inertial navigation algorithm based on rotation vector and dual quaternion ［J］.Chinese Journal of Aeronautics，2013，26（2）：442-448.

［26］ Stuelpnagel J.On the parametrization of the three-dimensional rotation group［J］.SIAM Review，1964，6（4）：422-430.

［27］ 李慶揚，王能超，易大義.數值分析［M］.北京：清華大學出版社，2008. LI Qing-yang，WANG Neng-chao，YI Da-yi.Numerical analysis ［M］.Beijing：Tsinghua University Press，2008.（in Chinese）

Application of Rotating Vector in Equations of Motion for All-attitude Aircrafts

WANG Hong-hui1，2，YANG Shao-qing1，WU Cheng-fu2，HAO Feng1，CHE Xiao-tao1
（1.Xi’an Modern Control Technology Research Institute，Xi’an 710065，Shaanxi，China；2.School of Automation，Northwestern Polytechnical University，Xi’an 710072，Shaanxi，China）

A mathematical framework to apply a rotation vector in the equations of motion for all-attitude aircrafts is established based on the basic idea of that the rotation vector is to express an equivalent rotation corresponding to the change in angular position rather than the angular position.This idea is stimulated by the rotating vector in the strapdown attitude algorithm.The rotation vector method is available for all-attitude solution while the Euler angle method is not.Meanwhile，compared with the quaternion method，the rotation vector method can improve the efficiency of attitude solution.The non-commutative errors in the rotation vector method，the quaternion method and the Euler angle method are analyzed in detail. A rotation vector method based on the quasi-body frame is developed to improve the efficiency of attitude solution for axial-symmetry aircrafts of which a single channel has high dynamic characteristics.The digital simulations based on the equations of motion for some rolling missile show the effectiveness and generality of the rotation vector method.

control and navigation technoogy of aerocraft；rotation vector；Euler angle；quaternion；high-dynamic；all-attitude

TJ765.1

A

1000-1093（2016）03-0439-08

10.3969/j.issn.1000-1093.2016.03.008

2015-07-02

總裝備部預先研究項目（1020702）；陜西省博士后科研項目（2015年）

王紅輝（1985—），男，博士后。E-mail：wanggh06@outlook.com；楊紹卿（1941—），男，中國工程院院士。E-mail：bq203rjc@163.com