基于四元數的四軸無人機姿態的估計和控制

冀亮　錢正洪　白茹

摘 要： 介紹一種只需少量計算就可實現四軸無人機姿態的估計和控制的新穎方法。整個算法采用四元數表示，四元數運算基于慣性測量單元（IMU）的加速度和陀螺儀傳感器。姿態估計使用四元數代替三角函數，允許用加速度數據分析推導和優化梯度下降算法來減少由陀螺儀傳感器產生的誤差。姿態控制將當前姿態與目標姿態的姿態誤差反饋到PD控制器來實現想要的目標姿態。姿態估計中用到的梯度下降算法通過四元數對應的歐拉角的均方誤差驗證，靜態均方誤差小于0.945°。實際的飛行測試可以實現一個穩定的飛行，同時驗證了姿態控制算法。

關鍵詞： 四軸無人機； 四元數； 姿態估計； 姿態控制

中圖分類號： TN967?34 文獻標識碼： A 文章編號： 1004?373X（2015）11?0112?05

Attitude estimation and control of quaternion based quad?axis UAV

JI Liang， QIAN Zheng?hong， BAI Ru

（Center for Integrated Spintronic Devices， Hangzhou Dianzi University， Hangzhou 310018， China）

Abstract： A novel method with few calculations is presented to realize estimation and control of quad?axis UAV. The whole algorithm is expressed with quaternion， and the quaternion calculation is based on the acceleration of inertial measurement units （IMU） and gyroscope sensor. The quaternion is applied in attitude estimation instead of trigonometric function. Gradient descent algorithm is analyzed， deduced and optimized with acceleration data to reduce the error generated by gyroscope sensor. The error of current attitude and target attitude is fed back to PD controller with attitude control， to realize the expectant target attitude. The gradient descent algorithm applied in attitude estimation is verified with RMS error which is obtained from Euler angle corresponded with quaternion. The static RMS error is less than 0.945°. The stable flying is realized by actual flight test， meanwhile the attitude control algorithm is verified.

Keywords： quad?axis UAV； quaternion； attitude estimation； attitude control

0 引 言

四軸飛行器是眾多無人機中的典型代表，它通過四個電機作為引擎來實現空中飛行。四軸飛行器類似于傳統的直升機，可以垂直起飛和降落，但是它還具有其他的眾多優勢，如機械結構簡單、機動性強、成本低廉。因此，四軸飛行器受到業余愛好者和學術研究領域越來越多的關注。

由于其體積小、重量輕、可以攜帶一定的載荷以及具有自動導航的能力，四軸飛行器可以解決一些特定的任務，如交通監視、電影錄制以及到人類無法到達或者很難到達的地方去實施營救。

系統采用了四元數[1]的方法，相比于歐拉角，四元數不僅可以減小大量的浮點數運算，而且可以避免奇異性問題，以便實現全姿態工作能力。

四軸飛行器的姿態估計使用的是梯度下降算法[2?3]，相比于卡爾曼算法，梯度下降算法具有計算量小和效率高的優勢。四軸飛行器的姿態控制是通過改變四個電機的相應速度來改變其推力和扭矩的。為了實現更好的飛行效果，在調試過程中需要不斷調整PID[4?5]的三個參數，直到飛行器可以穩定飛行。

1 結構和飛行原理

1.1 結構

四軸飛行器的實物如圖1所示，四軸飛行器的結構模型如圖2所示。四個電機分別固定在垂直對角線的四個頂點上，分成前后和左右兩組。兩組電機的旋轉方向相反，而每組的兩個電機的旋轉方向相同，這樣是為了抵消反扭力矩。四軸飛行器具有固定的螺距，因此僅僅通過改變四個電機的轉速就可以實現各種復雜的動作。

1.2 飛行原理[6]

四軸飛行器的幾種基本飛行動作包括：懸停、垂直起降、俯仰運動、橫滾運動和偏航運動。下面對飛行器的幾種基本飛行動作和飛行原理做簡要的分析：

懸停：當四軸飛行器的四個電機的轉速相等且四個電機的升力總和等于其自身的重力時，四軸飛行器處于懸停狀態。

垂直起降：在懸停的基礎上，如果四個電機的升力總和大于或小于其自身重力，那么四軸飛行器將實現垂直起降。

俯仰和橫滾運動：在懸停的基礎上，如果前面電機的轉速增加（或減小），并且后面電機的轉速相應地減小（或增加）來保持升力和重力的平衡，四軸飛行器可以實現俯仰運動。橫滾運動與俯仰運動的原理一樣。

偏航運動：在懸停的基礎上，如果一組對角線電機的轉速增加，而另一組對角線電機的轉速相應地減小來保持升力與重力的平衡，四軸飛行器可實現偏航運動。

2 四元數

2.1 定義

四元數法是利用一種超復數的乘法來等效反映姿態變換的方法。四元數的定義如式（1）所示：

[q=qw+qx?i+qy?j+qz?k=qwqxqyqzT] （1）

用于表示旋轉的四元數都歸一化為單位化的四元數，即：

[q2=q2w+q2x+q2y+q2z=1] （2）

通過旋轉軸和繞該旋轉軸旋轉的角度可以構造一個四元數，如圖3所示。

[qw=cosθ2] （3）

[qx=sinθ2?cosβx] （4）

[qy=sinθ2?cosβy] （5）

[qz=sinθ2?cosβz] （6）

其中： [cos（βx）cos（βy）cos（βz）T]為旋轉軸；[cosβx，][cosβy，][cosβz]分別為[x，][y，][z]軸的方向余弦；[θ]為繞該旋轉軸所旋轉的角度。

其中，操作符[?]代表四元數乘法，四元數乘法將在下面的姿態估計推導過程中起到很重要的作用。

2.3 四元數的逆

[q-1=q?=qw-qx-qy-qzT] （8）

式中：-1表示逆；*表示共軛；T表示轉置。

3 姿態估計

3.1 來自陀螺儀的姿態估算

三軸陀螺儀固定在載體上，可以測量機體坐標系下[x，][y，][z]軸的角速度量，分別用[ωx，][ωy，][ωz]表示。四元數的變化率[7]可以用角速度量來表示：

[q=qwqxqyqz=0-ωx2-ωy2-ωz2ωx20ωz2-ωy2ωy2-ωz20ωx2ωz2ωy2-ωx20?qwqxqyqz] （9）

在已知第[n]次的姿態估算值的情況下，可以通過四元數的變化率，得出只有陀螺儀的情況下估算出第[n+1]次的姿態：

[qω，n+1=qest，n+qw，n?Δt] （10）

式中：下標est表示最終估算的結果；下標[ω]表示僅通過陀螺儀傳感器估算的結果；[Δt]表示姿態解算周期。

將式（9）代入式（10），可以獲得式（11）：

[qω，n+1=1-Δθx2-Δθy2-Δθz2Δθx21Δθz2-Δθy2Δθy2-Δθz21Δθx2Δθz2-Δθy2-Δθx21?qest，n，wqest，n，xqest，n，yqest，n，z] （11）

[Δθ=ω?Δt] （12）

通過比較式（7）和式（11），可以簡化來自陀螺儀的姿態更新方程：

[qω，n+1=qest，n?Δq] （13）

[Δq=1Δθx2Δθy2Δθz2] （14）

3.2 來自加速度計的姿態糾正

在地理坐標系下的重力可表示為：

[Ed=001T] （15）

在機體坐標系下，加速度計測量的重力可以表示為：

[Ss=axayazT] （16）

式中，上標E表示地理坐標系；上標S表示機體坐標系。認為加速度計僅僅測量重力或者由于非慣性力產生的額外加速度可以忽略不計。由地理坐標系到機體坐標系的轉換矩陣為：

[T=1-2q2y-2q2z2qxqy+2qwqz2qxqz-2qwqy2qxqy-2qwqz1-2q2x-2q2z2qyqz+2qwqx2qxqz+2qwqy2qyqz-2qwqx1-2q2x-2q2y] （17）

那么通過加速度計表示的姿態誤差函數為：

[fg（q）=T?Ed-Ss] （18）

即：

[fg（q）=2qxqz-2qwqy-ax2qyqz+2qwqx-ay1-2q2x-2q2y-az] （19）

如果姿態四元數沒有誤差的話，式（19）的結果應該為0，正是由于存在誤差，所以才通過該目標誤差函數來求解誤差最小值的姿態四元數。由于式（19）的結果有正有負，為了使其一直保持為正，可以進行式（20）的處理：

[f（q）=fg（q）?fg（q）T] （20）

梯度下降算法是眾多最優化算法中在實現和計算方面最簡單的一種算法。式（21）是梯度下降算法的第[k]次迭代；[?f]是目標誤差函數的梯度，描述了目標誤差函數上升最快的方向，相反，負梯度描述了目標誤差函數下降最快的方向；[λk]是梯度下降算法第[k]次迭代的最優步長[3]；[H]是海森（Hesse）矩陣[3，8]，在這里是目標誤差函數對四元數的二次偏導構成的[4×4]矩陣。

[qk+1=qk-λk??fk] （21）

[?f=?f?qw?f?qx?f?qy?f?qzT] （22）

[λk=（?fk）T??fk（?fk）T?Hk??fk] （23）

[H=?2f?qw2?2f?qw?qx?2f?qw?qy?2f?qw?qz?2f?qx?qw?2f?qx2?2f?qx?qy?2f?qx?qz?2f?qy?qw?2f?qy?qx?2f?qy2?2f?qy?qz?2f?qz?qw?2f?qz?qx?2f?qz?qy?2f?qz2] （24）

傳統上梯度下降算法需要多次迭代才能尋求最優解，但是在姿態解算中，每個解算周期只需進行一次迭代就可滿足要求。因此，通過陀螺儀更新，加速度進行誤差糾正后的姿態更新方程變為：

[qest，n+1=qest，n?Δq-λn??f] （25）

4 姿態控制[9]

4.1 計算當前姿態與目標姿態的姿態誤差

通過前面的姿態估計已經確定當前姿態，現在應該確定目標姿態。姿態四元數可以用歐拉角來表示[10]：

[q=qwqxqyqz=cos?2cosθ2cosφ2+sin?2sinθ2sinφ2sin?2cosθ2cosφ2-cos?2sinθ2sinφ2cos?2sinθ2cosφ2+sin?2cosθ2sinφ2cos?2cosθ2sinφ2-sin?2sinθ2cosφ2] （26）

式中：[?，][θ，][φ]分別代表橫滾角，俯仰角和偏航角。

假定系統上電后的初始姿態為：

[b=qwqxqyqzT] （27）

結合式（26），令俯仰角[θ，]橫滾角[?]為0，得到初始目標姿態為：

[m=qw00qzT] （28）

來自手持遙控器的運動增量為[n，]則可以確定最終的目標姿態為：

[t=m?n] （29）

目前已知道當前姿態[a]和目標姿態[t，]令當前姿態與目標姿態的姿態誤差為[Δr，]則：

[a?Δr=t] （30）

兩邊同時乘以[a]的逆，可得姿態誤差：

[Δr=a-1?t] （31）

4.2 用姿態誤差得出需要的力矩

姿態誤差為一四元數，結合式（3）～式（6），可將姿態誤差表示為：

[Δr=ΔrwΔrxΔryΔrz=cosθ2vx?sinθ2vy?sinθ2vz?sinθ2] （32）

結合圖3，可知姿態誤差的旋轉軸為[v=（vxvyvz），]是一個單位向量，并且可以用來表示力矩的方向。當[θ]足夠小時，[sinθ2]與[θ]成正比，因此可以用來計算力矩強度，令：

[hn=ΔrxΔryΔrzTn] （33）

如果用PD控制，便可以得到力矩[mon：]

[mon=KP?hn+KD?（hn-hn-1）] （34）

式中：[KP]為PID中的比例系數；[KD]為PID中的微分系數。這里未采用積分是因為積分雖然可以消除靜態誤差，但是會產生一定延時，對于實時控制的四軸飛行器來說很不利，所以在這里未采用PID中的積分項。

4.3 使用力矩來控制油門

四軸飛行器有“X”型和“+”型模型，本文采用“+”模型，其結構如圖2所示。四軸飛行器的輸出油門是基礎值和增量值兩個值的和?；A值用來控制整體升力，各個電機相等；增量值用來實現穩定、加速等動作，相當于對基礎值的微調，各個電機各不相同，且它們的和為0。力矩僅取決于增量值。

如果需要[x]軸的力矩（不妨取前飛），則油門增量值應為：0、2號電機不變，1號電機負，3號電機正，油門需要變化的方向（direction）為：[dx=0-101T，]引入[x]軸的力矩（moment）修正系數（靈敏度）[MOx，]則需要增加[x]軸力矩[Δmox]時，油門（throttle）增量為：

[Δthx=MOx?dx?Δmox] （35）

即：

[Δthx=0-MOx?Δmox0MOx?ΔmoxT] （36）

[y，][z]軸同理可得：

[dy=-1010T] （37）

[dz=1-11-1T] （38）

[Δthy=MOy?dy?Δmoy] （39）

[Δthz=MOz?dz?Δmoz] （40）

力矩修正系數用于平衡各軸的響應靈敏度。[x，][y]軸的力矩由螺旋槳的升力直接提供，響應夠靈敏，[MOx]和[MOy]相等，取較小的值；而[z]軸的力矩需要靠螺旋槳的反轉力矩來提供，相同油門增量，能提供的力矩較小，所以[MOz]應該取較大的值，為了方便也可以都設為1，但最好根據實際情況進行調整。

把各軸分量疊加起來就是任意軸的情況，其中力矩為[mo=ΔmoxΔmoyΔmozT，]則油門增量值為：

[Δth=Δthx+Δthy+Δthz] （41）

即：

[Δth=0 -MOy?Δmoy -MOz?Δmoz-MOx?Δmox +0 +MOz?Δmoz0 +MOy?Δmoy -MOz?ΔmozMOx?Δmox +0 +MOz?Δmoz] （42）

最后把增量值和基礎值疊加起來，就完成一個控制周期了。假定油門基礎量（Base）為base，四個電機的油門量用motor0，motor1，motor2，motor3表示，則四個電機的油門量為：

[motor0=base-MOy?Δmoy-MOz?Δmozmotor1=base-MOx?Δmox+MOz?Δmozmotor2=base+MOy?Δmoy-MOz?Δmozmotor3=base+MOx?Δmox+MOz?Δmoz] （43）

5 實驗與結果

5.1 姿態估計的實驗

姿態估計可以用如圖4的平臺進行驗證。

5.2 PID調試平臺

為四軸飛行器制作了一個PID調試平臺，如圖6所示。

結合圖2，將0號和2號電機固定在平臺的固定軸上，來調節[x]軸的PID參數。需要不斷調節PID參數，直到四軸飛行器可以穩定的水平飛行。同樣方法獲得[y]軸與[z]軸的PID參數。

5.3 飛行測試

四軸飛行器在空中可以實現穩定的飛行，飛行效果如圖7所示。

6 結 語

本文介紹了一種基于四元數的姿態估計和姿態控制算法。姿態估計采用梯度下降算法，通過姿態的靜態均方誤差驗證了姿態估計的可行性。姿態控制算法采用的是PID算法，通過飛行測試，可以驗證姿態控制算法的可行性。

參考文獻

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

[2] MADQWICK S O， HARRISON A J， VAIDVANATHAN A. Estimation of IMU and MARG orientation using a gradient descent algorithm [C]// 2011 IEEE International Conference on Rehabilitation Robotics. [S.l.]： IEEE， 2011： 1?7.

[3] 佚名.無約束最優化方法[EB/OL].[2012?07?21].http：//www.cnblogs.com/zhangchaoyang/articles/2600491.html.

[4] 佚名.PID控制[EB/OL].[2015?03?11].http：//baike.baidu.com/view/262316.htm？fr=aladdin.

[5] 佚名.PID控制理論[EB/OL].[2010?12?20].http：//wenku.baidu.com/link？url=6k8WZ81gQMog7kOHu?PGQ5QDHFSmWKZ3UwIhM_

cgXgTMeo1m7VcReguP0K5Iplzn7riTEAP1MSX3MvrhelO_82HXDSmADJ0_c4TC9gIO4m.

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

[7] 鄧正隆.慣性技術[M].哈爾濱：哈爾濱工業大學出版社，2006.

[8] 林亞坤，錢正洪，白茹，等.基于GMR傳感器的姿態檢測系統設計[J].儀表技術與傳感器，2014（8）：53?55.

[9] 佚名.四軸：完善中的控制算法[EB/OL].[2012?09?15].http：//blog.sina.com.cn/s/blog_81f1e26801019sg4.html.

[10] 佚名.四元數與歐拉角之間的轉換[EB/OL].[2010?11?21]. http：//www.cnblogs.com/wqj1212/archive/2010/11/21/1883033.html.