徐云川

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

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

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

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

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

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

2.1 歐拉角表示

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

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

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

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

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

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

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

4 結(jié)論

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

【參考文獻】

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

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

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

[4]秦永元.慣性導航[M].北京：科學出版社，2006.

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

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

[責任編輯：湯靜]