多傳感器數據融合的四軸飛行器姿態角解算
2023-10-08 12:20:52唐震宇張云
自動化與信息工程 2023年1期
唐震宇 張云
本文引用格式：唐震宇，張云.多傳感器數據融合的四軸飛行器姿態角解算[J].自動化與信息工程，2023，44（1）：33-38.
TANG Zhenyu， ZHANG Yun.?Multi-sensor data fusion for attitude angle calculation of four axis aircraft[J]. Automation & Information Engineering， 2023，44（1）：33-38.
摘要：針對四軸飛行器的姿態角解算方法存在周期長、精度低的問題，提出擴展卡爾曼濾波方法。首先，分析陀螺儀、加速度計、Mahony互補濾波3種姿態角的解算方法；然后，對擴展卡爾曼濾波方法進行公式推導，指出通過迭代可改變卡爾曼增益大小，提高解算精度；最后，通過實驗得出，與Mahony互補濾波相比，擴展卡爾曼濾波更可靠。利用擴展卡爾曼濾波方法進行姿態角解算，可提高四軸飛行器的控制精度和穩定性，滿足高精度工作場所的需求，具有一定的實踐意義。
關鍵詞：擴展卡爾曼濾波；Mahony互補濾波；四軸飛行器；姿態角解算；數據融合
中圖分類號：TP249???????????文獻標志碼：A????????????文章編號：1674-2605（2023）01-0006-06
DOI：10.3969/j.issn.1674-2605.2023.01.006
Multi-sensor Data Fusion for Attitude Angle Calculation of ??????????????Four Axis Aircraft
TANG Zhenyu??ZHANG Yun
（Shandong Jiaotong University Rail Transit College，?Jinan 250000，?China）
Abstract： Aiming at the problem of long period and low accuracy of attitude angle calculation method for four axis aircraft， an extended Kalman filter method is proposed. First of all， the three attitude angle calculation methods of gyroscope， accelerometer and Mahony complementary filter are analyzed; Then， the formula of extended Kalman filtering method is deduced， and it is pointed out that the Kalman gain can be changed by iteration to improve the solution accuracy; Finally， the experiment shows that the extended Kalman filter is more reliable than Mahony complementary filter. The extended Kalman filter method is used to solve the attitude angle， which can improve the control accuracy and stability of the four axis aircraft， and meet the requirements of high-precision workplaces. It has certain practical significance.
Keywords：?extended Kalman filter; Mahony complementary filtering; four axis aircraft; attitude angle calculation; data fusion
0 ?引言
四軸飛行器具有操控靈活方便，執行效率高等特點，廣泛應用于生產生活的多個領域，如電網巡檢、礦山航攝、農業灌溉、森林消防、航拍攝像等[1]。四軸飛行器主要的設計難點在于平衡控制，其核心為讀取姿態角。
傳統的姿態角解算方法主要有2種：1） 利用MPU6050芯片的DMP庫讀取姿態角，該方法雖然由硬件完成，可減輕處理器負擔，但姿態角的輸出頻率
僅有200 Hz[2]，無法滿足復雜環境的工作需求；2） 采用Mahony互補濾波進行姿態解算[3]，將通過陀螺儀得到的角速度數據和加速度計得到的重力加速度數據進行融合，得到較為可靠的姿態數據。但互補濾波主要通過PI運算進行誤差補償，需要在不同的條件下分別進行參數調整，較為繁瑣。
在Mahony互補濾波的基礎上提出擴展卡爾曼濾波的數據融合方法，即將陀螺儀和加速度計得到的數據進行融合，并隨著運算迭代自動調節陀螺儀和加速
度計的原始數據在姿態角解算中的占比[4-5]，從而提高解算后姿態角的精度，以滿足飛行器在不同環境中工作的需要。
本文利用擴展卡爾曼濾波方法進行姿態角解算。首先，將黃金五條公式于后驗估計點展開，并通過雅可比矩陣進行線性化[6]；然后，將陀螺儀和加速度計得到的原始數據進行融合并不斷迭代，得出較為準確的四元數；最后，將四元數轉化為姿態角。
1 ?姿態解算算法推導
四軸飛行器通過雙環PID運算進行控制。PID控制器的輸出對應四旋翼4個電機的占空比，通過不斷迭代運算，實時更新占空比，實現四軸飛行器的平穩控制[7]。PID控制器的輸入為期望姿態角。在姿態角讀取過程中，為修正非線性誤差以及彌補傳感器的設計缺陷，需將加速度計與陀螺儀讀取到的數據進行融合，得到準確的當前姿態角。
1.1 ?姿態角的描述
在姿態角解算過程中，需用到歐拉角表示的旋轉矩陣、四元數表示的旋轉矩陣以及四元數和歐拉角之間的轉化公式。
1.1.1 ?歐拉角
歐拉角是用來確定定點轉動剛體位置的一組獨立參考參量，包括橫滾角、俯仰角、偏航角，物理示意圖如圖1所示。
歐拉角運算過程較復雜，可能出現萬向鎖鎖死的問題，為此引入四元數概念。
1.1.2 ?四元數
四元數在代數上是復數的擴展，類似于復數是實數的擴展[8]，基本方程為
四元數形如a+bi+cj+dk，其中a、b、c、d均為實數，且a2+b2+c2+d2 =?1。
通過四元數進行歐拉角求解，可以減少芯片運算負擔，提高運算速度。
1.1.3 ?歐拉角表示的旋轉矩陣
歐拉角的物理含義為機體坐標系從一個空間狀態轉移到另一個空間狀態所需的數據運算[9]，其矩陣表達形式為
1.1.4 ?四元數的獲取
四元數的計算公式為
式中：ωx、ωy、ωz分別為陀螺儀得到的x、y、z軸角速度，qx為四元數。
1.1.5??四元數表示的旋轉矩陣
四元數表示的旋轉矩陣（公式（4））與公式（2）具有相同功能，可將復雜的三角運算簡化。
1.1.6 ?四元數轉化為歐拉角
令公式（2）和公式（4）位于相同行列的矩陣元素對應相等，聯立方程求解，可得到當前狀態下的四元數，將其代入公式（5），可得到當前狀態下的姿態角。
1.2 ?姿態角解算
1.2.1 ?基于陀螺儀的姿態角解算方法
陀螺儀得到的原始數據為x、y、z軸的角速度ωx、ωy、ωz，姿態角變化率和角速度的關系為
當機體處于水平靜止狀態時，θ、φ為0，公式（6）可簡化為
由公式（7）可以看出：當機體處于水平靜止狀態時，姿態角變化率與角速度之間為簡單的積分關系；當機體處于非水平靜止狀態時，姿態角變化率與角速度之間為非線性關系。
1.2.2??基于加速度計的姿態角解算方法
基于加速度計的姿態角解算方法的思路為：將地球坐標系的重力加速度分解到機體坐標系，即通過對公式（2）轉置，得到從地球坐標系轉化到機體坐標系的轉化矩陣，并通過重力方向矩陣運算，得到x、y、z軸上的重力加速度，如公式（8）所示。
以求解某一時刻的姿態角θ為例，將加速度計得到的原始數據代入公式（8），進一步轉化為公式（9），通過求解反三角函數計算俯仰角。
需注意，飛行器機頭的指向無論如何改變，都不能改變重力在x、y、z軸上的分量，因此無法通過此方法求解偏航角。
1.2.3??基于Mahony互補濾波的姿態角解算方法
若僅使用陀螺儀得到的角速度求解姿態角，需通過積分的方式。由于陀螺儀自身存在著誤差，且該誤差在積分過程中會累計，并隨著時間的延長被放大。因此，將加速度計和陀螺儀得到的數據進行融合，可減小非線性誤差。
通過加速度計修正累計誤差。誤差判斷圖如圖2所示，其中g為加速度計得到的實際重力加速度，v為四元數推導的重力加速度。令向量g和v進行叉乘運算，得到向量ρ的模值，可反映當前推導的重力加速度與實際重力加速度的誤差大小。
向量g可通過公式（8）求得，則誤差ei的計算公式為
誤差ei可反映陀螺儀數據的可靠程度。若誤差ei為0，說明陀螺儀得到的數據與真實值相同，可靠性較高；若誤差ei較大，說明陀螺儀得到的數據可靠性較低。因此，為權衡陀螺儀與加速度計在姿態角解算中的權重，采用誤差積分的方式，如公式（12）所示。
式中：ei（k）為當前累計的誤差值，e（k）為當前誤差值。
通過公式（12）計算誤差ei，并通過PI控制器補償，可得到較為精確的角速度：
1.2.4??基于擴展卡爾曼濾波的姿態角解算方法
在姿態角解算的過程中，根據陀螺儀得到的估計值、加速度計得到的測量值與真實值的誤差并非一成不變的，會因外界因素及器件自身的影響在一定范圍內波動[10]。利用Mahony互補濾波進行數據融合時，這2個數據的占比無法改變，而采用擴展卡爾曼濾波方法，這2個數據的占比可隨著系統的迭代更新隨時改變，從而提高姿態角的解算精度。
1.2.4.1??解算模型分析
數據融合過程建立的系統是非線性系統，而卡爾曼濾波主要應用于線性系統，因此需要通過雅可比矩陣、泰勒公式將系統線性化，從而得到擴展卡爾曼濾波，其對應的黃金五條公式也隨之變化為
1.2.4.2 ?擴展卡爾曼濾波的應用
在數學模型的推導過程中，由于采用了近似估計方法，因而其模型中含有過程噪聲。在數據測量的過程中，受外界不可控因素的影響，導致測量結果與真實值之間存在偏差，因而測量模型含有測量噪聲[11]。上述兩種噪聲在實際工程中無法測得。因此，擴展卡爾曼濾波的主要思路為：在一個不準確的數學模型得到的估計值（陀螺儀計算的四元數），以及一個不準確的測量結果（加速度計測得的重力加速度）的基礎上，通過擴展卡爾曼濾波方法對姿態角的最優值進行估計。以俯仰角為例，擴展卡爾曼濾波姿態估計主要分為下面5個步驟。
1） 計算先驗估計值讀取陀螺儀的數據ω，并代入公式（3），可求當前的先驗四元數值。
式中：為上一次計算得到的四元數矩陣的后驗估計值，Ak為狀態轉移矩陣，計算公式為
2） 計算先驗估計的協方差矩陣
將狀態轉移矩陣Ak以及系統噪聲協方差矩陣Q代入公式（15），可得到先驗估計的協方差矩陣。
3） 計算卡爾曼增益Kk
在機體坐標系下加速度計的表示方式如公式（22）所示。
因為加速度計是在地球坐標系下讀取重力加速度的，所以需要利用轉移矩陣將重力加速度從地球坐標系轉移到機體坐標系。為公式（4）的轉置。由公式（22）可以看出，加速度計由3個函數和4個未知數組成，其對應的雅可比矩陣為
將以上求得的結果代入公式（16），便可求得當前的卡爾曼增益Kk。
4） 計算后驗估計（最終輸出值）
將四元數的先驗估計、卡爾曼增益Kk、陀螺儀得到的角速度旋轉矩陣、在機體坐標系下加速度計的表達式、當前加速度計的測量值zk代入公式（17），可得到當前四元數的后驗估計值。
5） 更新協方差矩陣
將卡爾曼增益Kk、雅可比矩陣入公式（18），得到協方差矩陣的后驗估計，將其重新進入第一步進行迭代。
通過以上5步多次迭代，可得到更加精確的四元數。將該四元數代入公式（5），即可解算當前姿態角。
2 ?實驗驗證
本實驗利用STM32單片機讀取MPU6050的原始數據，并通過串口將當前陀螺儀的角速度數據與加速度計的加速度數據實時發送至上位機。
首先，利用MATLAB將基于陀螺儀、加速度計、Mahony互補濾波、擴展卡爾曼濾波的4種姿態角解算方法以程序的形式實現；然后，將原始數據輸入到上述4種姿態角的解算方法；最后，通過曲線圖觀察各解算方法的優劣。
2.1 ?傳統的解算方法
在對MPU6050施加外部抖動的情況下，通過Mahony互補濾波、陀螺儀、加速度計3種解算方法得到的俯仰角數據圖譜如圖3所示。
由圖3可知：通過Mahony互補濾波方法得到的數據較為準確，能夠實時顯示姿態角的變化；僅通過陀螺儀解算得到的姿態角，在初始時刻較為準確，但隨著時間的推移，其誤差不斷積分，顯現明顯的漂移現象；僅通過加速度計解算得到的姿態角因外部磁場的影響，噪聲明顯、數據可靠性差。
2.2 ?擴展卡爾曼濾波解算方法
在對MPU6050施加外部抖動的前提下，通過加速度計、陀螺儀、Mahony互補濾波、擴展卡爾曼濾波4種解算方法得到的俯仰角數據圖譜如圖4所示。
由圖4可以看出，通過擴展卡爾曼濾波方法得到的數據相比Mahony互補濾波，噪聲更小，擬合的曲線更平滑，解算效果更好。
3??結束語
本文通過公式推導，詳細介紹陀螺儀、加速度計、Mahony互補濾波3種基本姿態角的解算方法。在此基礎上，對擴展卡爾曼濾波的解算方法進行公式推導。通過實驗將加速度計、陀螺儀、Mahony互補濾波、擴展卡爾曼濾波4種姿態角解算方法的解算結果以圖譜的形式進行比對分析，發現各解算方法的變化規律符合理論推導。實驗結果表明，通過引入卡爾曼增益，可提高角度解算精度，相比其他解算方法更可靠，可滿足高精度工作環境的需求。
參考文獻
[1] 張成，趙玫，楊洪勇，等.野外航拍用四軸飛行器的設計[J].魯東大學學報（自然科學版），2018，34（4）：378-384.
[2] 李臣龍，強俊.基于STM32和MPU6050姿態解算的研究與實現[J].佳木斯大學學報（自然科學版），2017，35（2）：295-298;?316.
[3] 陳國定，周鵬豪，胡朕豪，等.基于MPU6050的四軸硬件姿態解算研究[J].機電工程，2018，35（1）：95-100.
[4] 伏家杰，周翟和，尹輝，等.基于四元數二階互補濾波的四旋翼姿態解算[J].機械制造與自動化，2019，48（2）：169-172;176.
[5] 李慶黨，張義龍.基于卡爾曼濾波和互補濾波的改進型姿態解算方法[J].青島科技大學學報（自然科學版），2019，40（6）： 85-89;98.
[6] 龍子洋，項鵬，隋國榮.基于擴展卡爾曼濾波的球形機器人姿態解算[J].軟件工程，2022，25（7）：47-50.
[7] 孫春虎，方愿捷，王靜，等.一種新型四軸飛行器姿態角控制方法與仿真[J].巢湖學院學報，2019，21（6）：127-132.
[8] 張捍衛，喻錚錚，雷偉偉.四元數的基本概念與向量旋轉的歐拉公式[J].大地測量與地球動力學，2020，40（5）：502-506.
[9] 程烺，俞家勇，馬龍稱，等.單位四元數、羅德里格轉換模型與歐拉角的映射關系[J].北京測繪，2020，34（1）：44-50.
[10] 段敏，趙凌，周瑩.基于擴展卡爾曼濾波的四旋翼無人機姿態估計方法[J].現代信息科技，2022，6（4）：7-11.
[11] 溫彬彬，溫鵬智，胡雪花.EKF在機器人目標跟蹤中的應用[J].電腦知識與技術，2021，17（36）：116-118.
作者簡介：
唐震宇，男，2000年生，本科，主要研究方向：自動化。E-mail：?1543344869@qq.com
張云（通信作者），1977年生，男，博士，副教授，主要研究方向：嵌入式系統。E-mail：?zhangyun0204@126.com
