四軸飛行器姿態(tài)解算算法設(shè)計(jì)與仿真

徐云川

【摘 要】為設(shè)計(jì)具備穩(wěn)定姿態(tài)控制的四軸飛行器，對(duì)四軸飛行器飛行姿態(tài)表示、數(shù)學(xué)模型、姿態(tài)解算算法進(jìn)行了研究。硬件上采用STM32作為主控芯片，以加速度計(jì)、陀螺儀為主體的慣性測(cè)量模塊作為姿態(tài)測(cè)量傳感器，測(cè)量出當(dāng)前飛機(jī)的三軸加速度與三軸角速度并傳送給主控芯片進(jìn)行四元數(shù)姿態(tài)解算，求解出當(dāng)前姿態(tài)角；軟件上對(duì)傳感器數(shù)據(jù)進(jìn)行融合，運(yùn)用MATLAB工具獲取實(shí)時(shí)姿態(tài)，實(shí)現(xiàn)對(duì)姿態(tài)的辨識(shí)和控制。實(shí)驗(yàn)表明，四元數(shù)姿態(tài)解算算法對(duì)飛行器具有良好的控制效果，能快速精準(zhǔn)響應(yīng)飛行狀態(tài)，滿足飛行器執(zhí)行特殊任務(wù)需要。

【關(guān)鍵詞】姿態(tài)解算；STM32；慣性測(cè)量模塊；四元數(shù)

【Abstract】In order to design a stabilized attitude control quadcopter，we research on attitude representation，mathematical model，attitude algorithm.In the side of hardware，use STM32 processor as master which execute quaternion attitude algorithm and IMU（Inertial Measurement Unit） which consist of accelerometer and gyroscope as attitude measurement sensor，the IMU measure current acceleration and angular velocity.In the side of software，using the MATLAB gets the real-time attitude from sensor. Experiments shows that，quaternion attitude algorithm can assurance the attitude of the UAV well，and it can meet the requirement of mission job.

【Key words】Attitude algorithm；STM32；Inertial Measurement Unit；Quaternion

四軸飛行器具有結(jié)構(gòu)新穎、飛行方式獨(dú)特、可垂直升降特點(diǎn)，近幾年軍用、民用均占據(jù)了越來越重要的地位。常見的四軸飛行器呈軸對(duì)稱結(jié)構(gòu)，主要由正交“十”字型機(jī)架、飛行控制板、傳感器組、電機(jī)和旋翼組成，電機(jī)和螺旋槳裝置是飛行器動(dòng)力裝置，改變電機(jī)的轉(zhuǎn)速、帶動(dòng)螺旋槳產(chǎn)生升力，為飛行器提供動(dòng)力；傳感器組包括由陀螺儀和加速度計(jì)組成的慣性測(cè)量模塊、磁力計(jì)、氣壓計(jì)、GPS模塊、超聲波模塊；飛行控制是飛行器運(yùn)動(dòng)的核心，飛行控制板實(shí)時(shí)收集傳感器組測(cè)得的數(shù)據(jù)，解算飛行姿態(tài)。

1 姿態(tài)解算流程

慣性測(cè)量模塊測(cè)量當(dāng)前飛機(jī)的三軸加速度和三軸角速度數(shù)據(jù)，轉(zhuǎn)化為四維向量，經(jīng)線性變換后輸出，利用四元數(shù)與歐拉角的關(guān)系，將當(dāng)前四元數(shù)轉(zhuǎn)換為歐拉角Pitch、Roll、Yaw即解算得到當(dāng)前飛機(jī)的姿態(tài)。如圖1是姿態(tài)解算流程圖。AD值是指MPU6050的陀螺儀和加速度值，三維陀螺儀值和三維加速度值，每個(gè)值為16位精度。AD值通過姿態(tài)解算算法得到飛行器當(dāng)前的姿態(tài)（姿態(tài)使用四元數(shù)表示），然后將四元數(shù)轉(zhuǎn)化為歐拉角，用于姿態(tài)控制算法（PID控制）中。

剛體姿態(tài)是描述剛體的剛體坐標(biāo)系與參考坐標(biāo)系的角位置關(guān)系，飛行器需實(shí)時(shí)獲取當(dāng)前姿態(tài)，根據(jù)需要操控動(dòng)作，如保持平衡、實(shí)現(xiàn)翻滾等。飛行器姿態(tài)解算就是將剛體坐標(biāo)系與地理坐標(biāo)系轉(zhuǎn)化，轉(zhuǎn)化有很多種表示方法，如矩陣表示、歐拉角表示、軸角表示和四元數(shù)表示。矩陣表示適合變換向量，歐拉角表示最直觀，軸角表示則適合幾何推導(dǎo)，而在組合旋轉(zhuǎn)方面，四元數(shù)表示最佳。因?yàn)樽藨B(tài)解算需要頻繁組合旋轉(zhuǎn)和用旋轉(zhuǎn)變換向量，所以采用四元數(shù)保存飛行器的姿態(tài)。在需要控制的時(shí)候，會(huì)將四元數(shù)轉(zhuǎn)化為歐拉角，然后輸入到姿態(tài)控制算法中。

2 姿態(tài)表示方法

2.1 歐拉角表示

歐拉角是用來描述剛體在三維歐幾里得空間的取向。對(duì)于在三維空間里的一個(gè)參考系，任何坐標(biāo)系的取向，都可以用三個(gè)歐拉角表示，即滾轉(zhuǎn)角（Roll，？準(zhǔn)）、俯仰角（Pitch，θ）、偏航角（Yaw，ψ）。根據(jù)歐拉旋轉(zhuǎn)定力，通過三次旋轉(zhuǎn)即可使剛體坐標(biāo)旋轉(zhuǎn)到與地理坐標(biāo)系重合或者飛行器平衡姿態(tài)的位置，三次坐標(biāo)變換的旋轉(zhuǎn)矩陣的乘積即為歐拉角姿態(tài)矩陣。圖2表示剛體三次坐標(biāo)旋轉(zhuǎn)得到歐拉角，圖中OXYZ是參考坐標(biāo)系，OXYZ是剛體坐標(biāo)系。

3 姿態(tài)解算算法

姿態(tài)解算的目的是獲取飛行器實(shí)時(shí)準(zhǔn)確的姿態(tài)角度，慣性測(cè)量模塊包含了加速度計(jì)和陀螺儀兩種測(cè)量姿態(tài)角度的傳感器[2]。加速度計(jì)靜態(tài)性能良好，在飛行器靜止?fàn)顟B(tài)下，能準(zhǔn)確測(cè)量當(dāng)前機(jī)體相對(duì)地理坐標(biāo)系的姿態(tài)角度；陀螺儀有良好的動(dòng)態(tài)性能，通過積分獲取機(jī)體姿態(tài)角度相對(duì)初始角度的變化量，缺點(diǎn)是偏差會(huì)隨著積分時(shí)間無限增大。飛行器要實(shí)現(xiàn)平衡可控飛行就需要從慣性測(cè)量模塊獲得具有噪聲小、誤差小、動(dòng)態(tài)性能好的姿態(tài)信號(hào)，如前所述，飛行器姿態(tài)信號(hào)指其姿態(tài)角。加速度計(jì)輸出三軸重力分量信號(hào)，陀螺儀輸出繞三軸旋轉(zhuǎn)的角速度信號(hào)。飛行器在飛行過程中，電機(jī)的高速旋轉(zhuǎn)引起的機(jī)體震動(dòng)會(huì)給陀螺儀、加速度計(jì)的輸出信號(hào)帶來噪聲影響，實(shí)際的頻譜分析圖（如圖3所示）也恰恰印證了電機(jī)震動(dòng)帶來噪聲影響，大量噪聲的引入會(huì)使控制系統(tǒng)對(duì)飛行器的姿態(tài)解算會(huì)出現(xiàn)很大的誤差，為減小誤差對(duì)控制系統(tǒng)的影響，需要對(duì)加速度計(jì)、陀螺儀測(cè)量數(shù)據(jù)進(jìn)行融合濾波處理得到更加有效的飛行器姿態(tài)信號(hào)，彌補(bǔ)各自缺陷。

為獲得比較精確的飛行姿態(tài)角度，對(duì)加速度計(jì)采集到的數(shù)據(jù)進(jìn)行低通濾波去除高頻噪聲，對(duì)陀螺儀采集到的數(shù)據(jù)進(jìn)行高通濾波消除積分漂移的影響，為保證姿態(tài)數(shù)據(jù)準(zhǔn)確，需要對(duì)加速度計(jì)和陀螺儀設(shè)定量程和精度如表1所示。

采集得到的加速度計(jì)（ax，ay，az）和陀螺儀（gx，gy，yz）需通過四元數(shù)解算姿態(tài)數(shù)據(jù)。運(yùn)用傳感器數(shù)據(jù)融合思想，設(shè)計(jì)實(shí)時(shí)更新姿態(tài)四元數(shù)算法，具體計(jì)算方式是，初始化四元數(shù)向量，從傳感器獲得加速度和角速度，將加速度值規(guī)范化，用四元數(shù)求取重力分量，采用互補(bǔ)濾波方式，計(jì)算重力分量與加速度計(jì)測(cè)量誤差，用所得誤差修正陀螺儀，用修正后的陀螺儀值（gx，gy，gz）對(duì)四元數(shù)進(jìn)行更新，由新的四元數(shù)和下一個(gè)周期采集得到的加速度計(jì)和陀螺儀的原始數(shù)據(jù)重復(fù)計(jì)算，循環(huán)獲得陀螺儀的值，更新四元數(shù)，將更新后的四元數(shù)規(guī)范化，應(yīng)用式（4）轉(zhuǎn)化為歐拉角，得到飛行姿態(tài)。

四元數(shù)解算方法最終可以得到飛行器運(yùn)動(dòng)過程中的滾轉(zhuǎn)角（Roll，？準(zhǔn)）、俯仰角（Pitch，θ）、偏航角（Yaw，ψ），準(zhǔn)確的姿態(tài)解算為四軸飛行器姿態(tài)的精準(zhǔn)可知提供必要的條件。飛控板執(zhí)行姿態(tài)解算程序，將采集到的原始加速度數(shù)據(jù)、陀螺儀數(shù)據(jù)等通過MATLAB串口采集到上位機(jī)，并實(shí)時(shí)通過波形顯示數(shù)據(jù)，MATLAB實(shí)時(shí)姿態(tài)顯示界面如圖4所示，界面左側(cè)是串口設(shè)置參數(shù)以及MPU6050原始數(shù)據(jù)，界面中間是加速度計(jì)和陀螺儀解算結(jié)果，界面右側(cè)是解算得到的姿態(tài)角和姿態(tài)圖。

在姿態(tài)圖中，設(shè)定OXYZ是地理坐標(biāo)系，OABC是機(jī)體坐標(biāo)系，姿態(tài)角一旦有偏移可以立馬反映到姿態(tài)圖上。從MATLAB實(shí)時(shí)姿態(tài)顯示數(shù)據(jù)曲線可以看到，通過一系列的濾波處理，可以實(shí)現(xiàn)對(duì)加速度計(jì)、陀螺儀原始數(shù)據(jù)的去噪聲處理，同時(shí)通過四元數(shù)姿態(tài)解算可以得到用于飛行器姿態(tài)控制的有效姿態(tài)數(shù)據(jù)，通過姿態(tài)融合方法有效消除了加速度計(jì)的高頻噪聲和陀螺儀的積分漂移的影響。

4 結(jié)論

本文詳細(xì)分析了四軸飛行器的飛行運(yùn)動(dòng)原理、飛行姿態(tài)的歐拉角和四元數(shù)姿態(tài)表示，建立了相應(yīng)的數(shù)學(xué)模型，設(shè)計(jì)并制作了飛行控制系統(tǒng)硬件電路，包括電源供電電路、用于姿態(tài)檢測(cè)的多傳感器電路等，著重研究和分析了慣性測(cè)量模塊MPU6050中加速度計(jì)和陀螺儀信號(hào)噪聲，設(shè)計(jì)了一種姿態(tài)解算算法，并運(yùn)用MATLAB工具輔助設(shè)計(jì)了互補(bǔ)濾波傳感器融合飛行姿態(tài)實(shí)時(shí)顯示系統(tǒng)。

【參考文獻(xiàn)】

[1]何瑜.四軸飛行器控制系統(tǒng)設(shè)計(jì)及其姿態(tài)解算和控制算法研究[D].成都：電子科技大學(xué)，2015：14-16.

[2]劉浩蓬.植保四軸無人飛行器關(guān)鍵技術(shù)研究[D].武漢：華中農(nóng)業(yè)大學(xué)，2015：29-31.

[3]冀亮，錢正洪，白茹.基于四元數(shù)的四軸無人機(jī)姿態(tài)的估計(jì)和控制[J].現(xiàn)代電子技術(shù)，2015，11（38）：112-116.

[4]秦永元.慣性導(dǎo)航[M].北京：科學(xué)出版社，2006.

[5]劉峰，呂強(qiáng)，王國勝，等.四軸飛行器姿態(tài)控制系統(tǒng)設(shè)計(jì)[J].計(jì)算機(jī)測(cè)量與控制，2011，19（3）：583-585.

[6]趙建軍，陳濱，楊利斌.基于四元數(shù)的大地坐標(biāo)轉(zhuǎn)換算法及實(shí)現(xiàn)[J].計(jì)算機(jī)工程與應(yīng)用，2013，49（4）：202-205.

[責(zé)任編輯：湯靜]