坐式垂直起降無人機(jī)的一種姿態(tài)解算算法的設(shè)計(jì)

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

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

?

坐式垂直起降無人機(jī)的一種姿態(tài)解算算法的設(shè)計(jì)

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

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

摘要：固定翼模式水平飛行的坐式垂直起降無人機(jī)克服了傳統(tǒng)固定翼無人機(jī)起降條件要求高的缺點(diǎn)，繼承了其總體效率高的優(yōu)點(diǎn)，擁有巨大的發(fā)展?jié)摿头浅V闊的應(yīng)用前景。坐式垂直起降無人機(jī)在起降階段姿態(tài)變化范圍大，所使用的姿態(tài)傳感器在俯仰方向的角度變化超過90°時(shí)，根據(jù)四元數(shù)轉(zhuǎn)換出的歐拉角會(huì)出現(xiàn)奇異點(diǎn)，即萬向節(jié)死鎖。從解算算法出發(fā)，提出了一種更改旋轉(zhuǎn)順序的方法，避免奇異點(diǎn)出現(xiàn)。結(jié)果表明，此方法可以很好地應(yīng)用在垂直起降無人機(jī)的姿態(tài)解算上。

關(guān)鍵詞：飛行器控制；坐式垂直起降無人機(jī)；姿態(tài)測(cè)量；四元數(shù)；歐拉角；萬向節(jié)死鎖

E-mail：liudh@hebust.edu.cn

劉東輝,奚樂樂,牛孟然，等.坐式垂直起降無人機(jī)的一種姿態(tài)解算算法的設(shè)計(jì)[J].河北科技大學(xué)學(xué)報(bào),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.

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

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

但是，坐式垂直起降無人機(jī)在垂直-水平姿態(tài)轉(zhuǎn)換過程中，俯仰方向會(huì)發(fā)生達(dá)到甚至超過90°的變化，MPU-6050的其他2個(gè)軸的角度解算值會(huì)發(fā)生突變，使得坐式垂直起降無人機(jī)失去慣性基準(zhǔn)，很容易導(dǎo)致飛機(jī)失控墜毀。針對(duì)這個(gè)關(guān)鍵問題，本文提出一種合理的姿態(tài)角測(cè)量解算方式，提供穩(wěn)定、準(zhǔn)確的慣性基準(zhǔn)[7-8]。

1姿態(tài)解算問題

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

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

在傳統(tǒng)的姿態(tài)解算過程中，一般的四元數(shù)轉(zhuǎn)歐拉角[14-15]過程采用Z-Y-X的旋轉(zhuǎn)順序，設(shè)定四元數(shù)：

Q=w+xi+yj+zk，

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

(1)

按照Z-Y-X旋轉(zhuǎn)順序進(jìn)行旋轉(zhuǎn)的示意圖如圖2中的圖2 a)和圖2 b)所示。這種情況下，外圓環(huán)代表航向(ψ)，中圓環(huán)代表俯仰(θ)，內(nèi)圓環(huán)代表橫滾(φ)。Z軸為Y軸的父級(jí)，Y軸為X軸的父級(jí)，X軸為箭頭的父級(jí)，當(dāng)繞著Y軸旋轉(zhuǎn)90°或-90°時(shí)，Z軸和X軸將會(huì)出現(xiàn)上述的重合情況，便會(huì)發(fā)生萬向節(jié)死鎖。圖2中的圖2 a)為先繞Z軸旋轉(zhuǎn)45°的結(jié)果，圖2 b)為繞Y軸旋轉(zhuǎn)90°后的結(jié)果?？梢院苊黠@地看出，外圓環(huán)和內(nèi)圓環(huán)轉(zhuǎn)到了一個(gè)平面上，這個(gè)時(shí)候當(dāng)再次旋轉(zhuǎn)內(nèi)圓環(huán)或外圓環(huán)時(shí)，外圓環(huán)與內(nèi)圓環(huán)仍然在一個(gè)平面上，所以得到的結(jié)果是一樣的，這便是萬向節(jié)死鎖。

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

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

(2)

圖2　按Z-Y-X順序旋轉(zhuǎn)示意圖Fig.2　Rotation diagram in Z-Y-X order

圖3　按Z-X-Y順序旋轉(zhuǎn)示意圖Fig.3　Rotation diagram in Z-X-Y order

2實(shí)驗(yàn)研究

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

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

圖4　按Z-Y-X順序旋轉(zhuǎn)數(shù)據(jù)圖Fig.4　Rotation data graph in Z-Y-X order

圖5　按Z-X-Y順序旋轉(zhuǎn)數(shù)據(jù)圖Fig.5　Rotation data graph in Z-X-Y order

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

3結(jié)語

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

參考文獻(xiàn)/References:

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

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

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

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

[3]賴水清,嚴(yán)峰,徐珂.傾轉(zhuǎn)旋翼機(jī)過渡飛行階段控制律設(shè)計(jì)研究[J]. 直升機(jī)技術(shù), 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ì)信號(hào)融合的姿態(tài)角度測(cè)量[J]. 西南師范大學(xué)學(xué)報(bào)(自然科學(xué)版), 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]劉東輝,劉浩,孫曉云,等.裝甲車輛模擬射擊中身管角度采集裝置設(shè)計(jì)[J].河北科技大學(xué)學(xué)報(bào),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片內(nèi)A/D實(shí)現(xiàn)的同步電機(jī)數(shù)據(jù)采集控制器[J].河北工業(yè)科技，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īng)湘.一種新的全角度四元數(shù)與歐拉角的轉(zhuǎn)換算法[J].南京理工大學(xué)學(xué)報(bào)(自然科學(xué)版),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]. 飛行力學(xué), 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]. 岳陽師范學(xué)院學(xué)報(bào)(自然科學(xué)版), 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]許毛躍,張登成,李嘉林.四元數(shù)在歐拉方程中的應(yīng)用研究[J].飛行力學(xué),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]郭訪社,于云峰,劉書盼,等.捷聯(lián)慣導(dǎo)系統(tǒng)姿態(tài)更新旋轉(zhuǎn)矢量算法的優(yōu)化[J]. 計(jì)算機(jī)工程與科學(xué), 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]余楊,張洪鉞.高精度捷聯(lián)姿態(tài)算法設(shè)計(jì)[J].中國(guó)慣性技術(shù)學(xué)報(bào),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]劉俊峰.三維轉(zhuǎn)動(dòng)的四元數(shù)表述[J].大學(xué)物理,2004,23(4):39-43.

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

[14]梁延德,程敏,何福本,等.基于互補(bǔ)濾波器的四旋翼飛行器姿態(tài)解算[J].傳感器與微系統(tǒng),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]喬相偉,周衛(wèi)東,吉宇人.基于四元數(shù)粒子濾波的飛行器姿態(tài)估計(jì)算法研究[J].兵工學(xué)報(bào),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ī)數(shù)控調(diào)速器設(shè)計(jì)[J].河北科技大學(xué)學(xué)報(bào),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]劉忠,梁曉庚,曹秉剛,等. 基于四元數(shù)的空間全方位算法研究 [J].西安交通大學(xué)學(xué)報(bào),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實(shí)現(xiàn)同步電動(dòng)機(jī)失步保護(hù)的裝置[J].河北工業(yè)科技,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]李連仲,王小虎,蔡述江.捷聯(lián)慣性導(dǎo)航、制導(dǎo)系統(tǒng)中方向余弦矩陣的遞推算法 [J].宇航學(xué)報(bào),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]陳萬春,肖業(yè)倫.矢陣與四元數(shù)的關(guān)系及其在飛行力學(xué)中的應(yīng)用[J].宇航學(xué)報(bào),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

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

基金項(xiàng)目：國(guó)家自然科學(xué)基金(51274144)

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

中圖分類號(hào)：V249.1

文獻(xiàn)標(biāo)志碼：A

doi:10.7535/hbkd.2016yx01008

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