四軸飛行器姿態解算算法設計與仿真

徐云川

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

【關鍵詞】姿態解算；STM32；慣性測量模塊；四元數

【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

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

1 姿態解算流程

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

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

2 姿態表示方法

2.1 歐拉角表示

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

3 姿態解算算法

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

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

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

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

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

4 結論

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

【參考文獻】

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

[2]劉浩蓬.植保四軸無人飛行器關鍵技術研究[D].武漢：華中農業大學，2015：29-31.

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

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

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

[6]趙建軍，陳濱，楊利斌.基于四元數的大地坐標轉換算法及實現[J].計算機工程與應用，2013，49（4）：202-205.

[責任編輯：湯靜]