坐式垂直起降無人機的一種姿態解算算法的設計

劉東輝,奚樂樂,牛孟然,孫曉云，石　楠

(1.河北科技大學電氣工程學院， 河北石家莊　050018;2.石家莊鐵道大學電氣與電子工程學院，河北石家莊　050043)

?

坐式垂直起降無人機的一種姿態解算算法的設計

劉東輝1,奚樂樂1,牛孟然1,孫曉云2，石楠1

(1.河北科技大學電氣工程學院， 河北石家莊050018;2.石家莊鐵道大學電氣與電子工程學院，河北石家莊050043)

摘要：固定翼模式水平飛行的坐式垂直起降無人機克服了傳統固定翼無人機起降條件要求高的缺點，繼承了其總體效率高的優點，擁有巨大的發展潛力和非常廣闊的應用前景。坐式垂直起降無人機在起降階段姿態變化范圍大，所使用的姿態傳感器在俯仰方向的角度變化超過90°時，根據四元數轉換出的歐拉角會出現奇異點，即萬向節死鎖。從解算算法出發，提出了一種更改旋轉順序的方法，避免奇異點出現。結果表明，此方法可以很好地應用在垂直起降無人機的姿態解算上。

關鍵詞：飛行器控制；坐式垂直起降無人機；姿態測量；四元數；歐拉角；萬向節死鎖

E-mail：liudh@hebust.edu.cn

劉東輝,奚樂樂,牛孟然，等.坐式垂直起降無人機的一種姿態解算算法的設計[J].河北科技大學學報,2016,37(1):47-51.

LIU Donghui, XI Lele, NIU Mengran,et al.Design of attitude solution algorithm for tail-sitter VTOL UAV[J].Journal of Hebei University of Science and Technology,2016,37(1):47-51.

目前，傳統固定翼布局無人機的總體效率很高，但其起飛和降落過程對環境要求特別高，限制了其實際應用[1-2]。坐式垂直起降飛機是一種以垂直姿態，“坐”于地面的垂直起降飛行器，由于無人機不必考慮載員的姿勢，故而無人機非常適于采用此種垂直起降方式。它既保持高效率的傳統固定翼飛機構型，具有固定翼無人機飛行速度快、航程遠、反應靈活等特點，又具有直升機的垂直起降功能，降低起飛降落過程對環境的要求。隨著科技的發展，坐式垂直起降無人機受到了人們廣泛的關注，在軍事、民用等多方面都有非常廣闊的發展前景[3]。例如，垂直起降無人機可應用在高速巡航、應急運輸、冰面爆破、偵察、航拍航測等多種領域。

姿態測量系統及其算法是垂直起降無人機控制系統的核心部分。本文中坐式垂直起降無人機的姿態測量系統采用廉價且穩定性高的MPU-6050作為姿態傳感器，該傳感器具有6個自由度，整合了三軸加速度計和三軸陀螺儀。同時，MPU-6050內部集成了可控式的運動處理器DMP，并可通過IIC協議與第三方傳感器相連接，比如氣壓傳感器。結合高效的微處理器，大大增加系統的穩定性和精確性，同時保持了極低的硬件成本[4-6]。

但是，坐式垂直起降無人機在垂直-水平姿態轉換過程中，俯仰方向會發生達到甚至超過90°的變化，MPU-6050的其他2個軸的角度解算值會發生突變，使得坐式垂直起降無人機失去慣性基準，很容易導致飛機失控墜毀。針對這個關鍵問題，本文提出一種合理的姿態角測量解算方式，提供穩定、準確的慣性基準[7-8]。

1姿態解算問題

萬向節死鎖(Gimbal Lock)一般又被稱為萬向節鎖或者萬向鎖，是指當3個萬向節中的2個軸發生重合時，會失去一個自由度的現象。在飛行器姿態運動學方程中一般有四元數和歐拉角2種表示方式，也是最常用的2種姿態表示方法。用歐拉角表示的飛行器姿態運動學方程在大角度時會出現奇異點并進入所謂的“萬向節死鎖”狀態。而采用四元數來表示，則可以避免這個問題。因此，飛行器運動學方程常采用四元數來表示[9-11]。但是歐拉角表示姿態角比起四元數更加形象、直觀，具有明確的幾何意義，更易于被人理解。本文采用四元數的方法進行姿態解算，然后將四元數轉換為歐拉角。在轉換過程中，轉換結果和旋轉順序存在必然順序關系。在表示某一個姿態的過程中，四元數是唯一的，但是不同的旋轉順序會得到不同的歐拉角。所以針對不同的旋轉順序，四元數和歐拉角之間存在一對多的情況[12-13]。本系統采用的坐式垂直起降無人機和傳感器安裝示意圖如圖1中的圖1 a)和圖1 b)所示。

圖1　坐式垂直起降無人機和傳感器安裝示意圖Fig.1　VTOL UAV and sensor installation schematic diagram

在傳統的姿態解算過程中，一般的四元數轉歐拉角[14-15]過程采用Z-Y-X的旋轉順序，設定四元數：

Q=w+xi+yj+zk，

且繞X軸旋轉的角度為φ，繞Y軸旋轉的角度為θ，繞Z軸旋轉的角度為ψ。四元數轉歐拉角的轉換公式如式(1)所示。

(1)

按照Z-Y-X旋轉順序進行旋轉的示意圖如圖2中的圖2 a)和圖2 b)所示。這種情況下，外圓環代表航向(ψ)，中圓環代表俯仰(θ)，內圓環代表橫滾(φ)。Z軸為Y軸的父級，Y軸為X軸的父級，X軸為箭頭的父級，當繞著Y軸旋轉90°或-90°時，Z軸和X軸將會出現上述的重合情況，便會發生萬向節死鎖。圖2中的圖2 a)為先繞Z軸旋轉45°的結果，圖2 b)為繞Y軸旋轉90°后的結果。可以很明顯地看出，外圓環和內圓環轉到了一個平面上，這個時候當再次旋轉內圓環或外圓環時，外圓環與內圓環仍然在一個平面上，所以得到的結果是一樣的，這便是萬向節死鎖。

根據萬向節死鎖的特性，一般繞著中間軸旋轉90°時會出現奇異現象，所以可以根據系統的要求，改變四元數轉換為歐拉角時的旋轉順序，從而避免垂直起降飛機在俯仰變化范圍超過90°過程中出現奇異點。所以，在坐式垂直起降無人機上的姿態解算不能采用傳統的Z-Y-X旋轉順序。針對這種情況，可以采用變換旋轉順序的方法克服Y軸在90°時候的奇異點，以達到坐式垂直起降無人機姿態解算的正確性。例如采用Z-X-Y順序進行四元數到歐拉角的轉換。

圖3中的圖3a)和圖3b)為按照Z-X-Y旋轉順序進行旋轉的結果，Z為X的父級，X為Y的父級，Y為箭頭的父級。其中圖3a)為沿著Y軸旋轉90°的結果，圖3b)為沿著X軸旋轉60°結果。因坐式垂直起降無人機正常情況下，沿X軸(橫滾)方向幾乎不會出現90°的旋轉，這樣便克服了萬向節死鎖現象的發生。按照Z-X-Y旋轉順序進行旋轉時的四元數轉歐拉角公式如式(2)所示：

(2)

圖2　按Z-Y-X順序旋轉示意圖Fig.2　Rotation diagram in Z-Y-X order

圖3　按Z-X-Y順序旋轉示意圖Fig.3　Rotation diagram in Z-X-Y order

2實驗研究

本測試系統通過讀取MPU-6050傳感器的原始數據[16-17]，得到三軸陀螺儀數據和三軸加速度數據，并通過互補濾波算法進行數據的融合，得到四元數，然后根據旋轉順序來選擇四元數轉歐拉角公式，得到最終的歐拉角。在比較過程中，本系統采用STM32系列微處理器[18]進行數據融合，并將更新的四元數通過Z-Y-X和Z-X-Y2種轉換公式進行四元數到歐拉角的轉換，并通過串口將2種旋轉順序輸出的角度φ，θ，ψ同時打印出來，通過Matlab軟件進行數據處理，得到數據融合的曲線。按Z-Y-X順序和按Z-X-Y順序旋轉生成的歐拉角數據圖如圖4和圖5所示。

在圖4中，橫軸為時間軸，縱軸為各姿態軸的角度。數據采集過程的初期，飛機處于水平飛行姿態，飛機俯仰角(θ)保持基本穩定，滾轉角(φ)和方向角(ψ)能夠在一定范圍內被測量。但當俯仰角達到90°時，滾轉角和方向角都出現了劇烈的變化，并出現異常值；隨著俯仰角在90°左右變化，滾轉角和方向角反復在±180°范圍內劇烈震蕩，完全失去參考價值。

圖4　按Z-Y-X順序旋轉數據圖Fig.4　Rotation data graph in Z-Y-X order

圖5　按Z-X-Y順序旋轉數據圖Fig.5　Rotation data graph in Z-X-Y order

在圖5中，橫軸為時間軸，縱軸為各姿態軸的角度。數據采集過程的初期，飛機處于水平飛行姿態，飛機俯仰角(φ)保持基本穩定，滾轉角(θ)和方向角(ψ)能夠在一定范圍內被測量。當飛機俯仰角逐漸變大并達到90°時，滾轉角和方向角都能在一定范圍內被測量，并沒有劇烈震蕩的奇異點出現，在垂直起降無人機的姿態解算系統中可以被采用。

3結語

通過調整四元數轉換歐拉角的旋轉順序，可以非常有效地解決出現奇異點的情況，避免萬向節死鎖現象的發生。實驗證明，在四元數轉歐拉角的過程中，選擇Z-Y-X旋轉順序在俯仰方向旋轉出現90°或者-90°時，便會發生奇異現象，其他2個軸的輸出角度會發生突變，無法得到正確的姿態信息[19-20]。而在相同情況下，Z-X-Y旋轉順序下輸出的角度不會出現奇異現象，所以選擇合理的旋轉順序，可以有效避免坐式垂直起降無人機在俯仰方向變化90°時給偏轉和橫滾方向造成的角度突變的發生，解決了坐式垂直起降無人機姿態解算的一個問題。

參考文獻/References:

[1]秦博,王蕾.無人機發展綜述[J].飛航導彈,2002(8):4-10.

QIN Bo, WANG Lei. The review about the UAV[J].Aerodynamic Missile Journal,2002(8):4-10.

[2]王冠林，武哲.垂直起降無人機總體方案分析及控制策略綜合研究[J].飛機設計,2006(3):25-30.

WANG Guanlin,WU Zhe.Configurations and control strategy for VTOL[J].Aircraft Design,2006(3):25-30.

[3]賴水清,嚴峰,徐珂.傾轉旋翼機過渡飛行階段控制律設計研究[J]. 直升機技術, 2009(3):52-55.

LAI Shuiqing, YAN Feng, XU Ke. Design and research of control law for tilt rotor in transition flight phase[J]. Helicopter Technique,2009(3):52-55.

[4]馮智勇,曾瀚,張力,等.基于陀螺儀及加速度計信號融合的姿態角度測量[J]. 西南師范大學學報(自然科學版), 2011,36(4):137-141.

FENG Zhiyong, ZENG Han, ZHANG Li, et al. Angle measurement based on gyroscope and accelerometer signal fusion[J]. Journal of Southwest China Normal University(Natural Science Edition),2011,36(4):137-141.

[5]劉東輝,劉浩,孫曉云,等.裝甲車輛模擬射擊中身管角度采集裝置設計[J].河北科技大學學報,2014,35(4):376-383.

LIU Donghui, LIU Hao, SUN Xiaoyun, et al. Design of barrel angle acquisition device in the simulated shooting of armored vehicles[J]. Journal of Hebei University of Science and Technology,2014,35(4):376-383.

[6]王書巖. 利用STM32片內A/D實現的同步電機數據采集控制器[J].河北工業科技，2014,31(2):177-180.

WANG Shuyan. Realization of synchronous motor data acquisition controller based on multi-channel A/D of STM32 chip[J]. Hebei Journal of Industrial Science and Technology, 2014,31(2):177-180.

[7]張帆,曹喜濱,鄒經湘.一種新的全角度四元數與歐拉角的轉換算法[J].南京理工大學學報(自然科學版),2002,26(4):376-380.

ZHANG Fan, CAO Xibin, ZOU Jingxiang. A New large-scale transformation algorithm of quaternion to euler angle[J]. Journal of Nanjing University of Science and Technology,2002,26(4):376-380.

[8]王海峰,王成良.解決歐拉方程奇異性的方法探討[J]. 飛行力學, 2006,24(3):94-96.

WANG Haifeng,WANG Chengliang. A new solution for singularity of euler equations[J]. Flight Dynamics,2006,24(3):94-96.

[9]熊志斌,劉喜斌.克服歐拉方程奇異性的方法研究[J]. 岳陽師范學院學報(自然科學版), 2000,13(4):60-62.

XIONG Zhibin,LIU Xibin. Research on methods for overcome the singularity of the Euler equation[J]. Journal of Yueyang Normal University,2000,13(4):60-62.

[10]許毛躍,張登成,李嘉林.四元數在歐拉方程中的應用研究[J].飛行力學,2002,20(1):67-70.

XU Maoyue, ZHANG Dengcheng, LI Jialin. Study of the application of quaternion to euler equation[J].Flight Dynamics,2002,20(1):67-70.

[11]郭訪社,于云峰,劉書盼,等.捷聯慣導系統姿態更新旋轉矢量算法的優化[J]. 計算機工程與科學, 2008,30(9):69-72.

GUO Fangshe,YU Yunfeng,LIU Shupan, et al.Optimization of the rotation vector algorithm for strap down inertial navigation system attitude updating[J]. Computer Engineering & Science,2008,30(9):69-72.

[12]余楊,張洪鉞.高精度捷聯姿態算法設計[J].中國慣性技術學報,2008,16(1):39-43.

YU Yang, ZHANG Hongyue. Design for an accurate strap down attitude algorithm[J].Journal of Chinese Inertial Technology,2008,16(1):39-43.

[13]劉俊峰.三維轉動的四元數表述[J].大學物理,2004,23(4):39-43.

LIU Junfeng. The quaternion describe for three-dimensional [J].College Physics,2004,23(4):39-43.

[14]梁延德,程敏,何福本,等.基于互補濾波器的四旋翼飛行器姿態解算[J].傳感器與微系統,2011,30(11):56-61.

LIANG Yande, CHENG Min, HE Fuben, et al. Attitude estimation of a quad- rotor aircraft based on complementary filter[J]. Transducer and Microsystem Technologies,2011,30(11):56-61.

[15]喬相偉,周衛東,吉宇人.基于四元數粒子濾波的飛行器姿態估計算法研究[J].兵工學報,2012,33(9):1070-1074.

QIAO Xiangwei, ZHOU Weidong, JI Yuren. Study on aerial vehicle attitude estimation based on quaternion particle filter algorithm[J]. Acta Armamentarii,2012,33(9):1070-1074.

[16]劉東輝,趙新偉,李天寶,等. 無位置傳感器無刷直流電機數控調速器設計[J].河北科技大學學報,2013,34(4):302-307.

LIU Donghui, ZHAO Xinwei,LI Tianbao, et al. Design of digital controller for sensorless brushless DC motor speed regulation[J].Journal of Hebei University of Science and Technology,2013,34(4):302-307.

[17]劉忠,梁曉庚,曹秉剛,等. 基于四元數的空間全方位算法研究 [J].西安交通大學學報,2006,40(5):618-620.

LIU Zhong, LIANG Xiaogeng, CAO Binggang, et al. Space omni directional algorithm study based on quaterion[J]. Journal of Xi'an Jiaotong University, 2006,40(5):618-620.

[18]王書巖.利用Cortex-M3實現同步電動機失步保護的裝置[J].河北工業科技,2012,29(6):426-428.

WANG Shuyan. Using Cortex-M3 to realize synchronous motor out-of-step protection device[J]. Hebei Journal of Industrial Science and Technology, 2012,29(6):426-428.

[19]李連仲,王小虎,蔡述江.捷聯慣性導航、制導系統中方向余弦矩陣的遞推算法 [J].宇航學報,2006,27(3):349-353.

LI Lianzhong, WANG Xiaohu, CAI Shujiang. A new recursion figure used in the matrix of the direction cosine in the strap down intertial navigation and guideance system[J].Journal of Astronautics,2006,27(3):349-353.

[20]陳萬春,肖業倫.矢陣與四元數的關系及其在飛行力學中的應用[J].宇航學報,1997,18(1):23-30.

CHEN Wanchun, XIAO Yelun. Vectrixized quaterion and its application in flight dynamics[J].Journal of Astronautics, 1997,18(1):23-30.

Design of attitude solution algorithm for tail-sitter VTOL UAV

LIU Donghui1, XI Lele1, NIU Mengran1, SUN Xiaoyun2， SHI Nan1

(1.School of Electrical Engineering, Hebei University of Science and Technology, Shijiazhuang, Hebei 050018, China; 2.School of Electrical and Electronic Engineering, Shijiazhuang Tiedao University, Shijiazhuang, Hebei 050043, China)

Abstract:The tail-sitter Vertical Takeoff and Landing (VTOL) Unmanned Aerial Vehicle(UAV), flying in a fixed-wing model, overcomes many shortcomings of traditional fixed-wing UAVs, and inherits the advantage of high overall efficiency, which means it has great development potential and very broad application prospects. The attitude of tail-sitter VTOL UAV shows a wide change range in its takeoff and landing stages, and when the attitude sensor changes more than 90 degrees in pitch direction, the Euler angles converted by the Quaternions will have singular points, which means gimbal deadlock appears. From the solution algorithm, this paper provides a method of changing the order of rotation to avoid the appearance of singular points. The results show that this method can be well applied to the attitude solution of the VTOL UAV.

Keywords:aircraft control; tail-sitter VTOL UAV; attitude measuring; quaternion; Euler angle; gimbal deadlock

作者簡介：劉東輝(1971—)，男，山西晉城人，教授，博士，主要從事電器信息檢測與信息處理技術方面的研究。

基金項目：國家自然科學基金(51274144)

收稿日期：2015-07-16；修回日期：2015-11-03；責任編輯：李穆

中圖分類號：V249.1

文獻標志碼：A

doi:10.7535/hbkd.2016yx01008

文章編號：1008-1542(2016)01-0047-05