基于四元數(shù)的四軸無人機姿態(tài)的估計和控制

冀亮　錢正洪　白茹

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

關(guān)鍵詞： 四軸無人機； 四元數(shù)； 姿態(tài)估計； 姿態(tài)控制

中圖分類號： 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 引 言

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

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

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

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

1 結(jié)構(gòu)和飛行原理

1.1 結(jié)構(gòu)

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

1.2 飛行原理[6]

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

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

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

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

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

2 四元數(shù)

2.1 定義

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

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

用于表示旋轉(zhuǎn)的四元數(shù)都歸一化為單位化的四元數(shù)，即：

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

通過旋轉(zhuǎn)軸和繞該旋轉(zhuǎn)軸旋轉(zhuǎn)的角度可以構(gòu)造一個四元數(shù)，如圖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]為旋轉(zhuǎn)軸；[cosβx，][cosβy，][cosβz]分別為[x，][y，][z]軸的方向余弦；[θ]為繞該旋轉(zhuǎn)軸所旋轉(zhuǎn)的角度。

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

2.3 四元數(shù)的逆

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

式中：-1表示逆；*表示共軛；T表示轉(zhuǎn)置。

3 姿態(tài)估計

3.1 來自陀螺儀的姿態(tài)估算

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

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

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

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

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

將式（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），可以簡化來自陀螺儀的姿態(tài)更新方程：

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

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

3.2 來自加速度計的姿態(tài)糾正

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

[Ed=001T] （15）

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

[Ss=axayazT] （16）

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

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

那么通過加速度計表示的姿態(tài)誤差函數(shù)為：

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

即：

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

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

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

梯度下降算法是眾多最優(yōu)化算法中在實現(xiàn)和計算方面最簡單的一種算法。式（21）是梯度下降算法的第[k]次迭代；[?f]是目標誤差函數(shù)的梯度，描述了目標誤差函數(shù)上升最快的方向，相反，負梯度描述了目標誤差函數(shù)下降最快的方向；[λk]是梯度下降算法第[k]次迭代的最優(yōu)步長[3]；[H]是海森（Hesse）矩陣[3，8]，在這里是目標誤差函數(shù)對四元數(shù)的二次偏導構(gòu)成的[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）

傳統(tǒng)上梯度下降算法需要多次迭代才能尋求最優(yōu)解，但是在姿態(tài)解算中，每個解算周期只需進行一次迭代就可滿足要求。因此，通過陀螺儀更新，加速度進行誤差糾正后的姿態(tài)更新方程變?yōu)椋?/p>

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

4 姿態(tài)控制[9]

4.1 計算當前姿態(tài)與目標姿態(tài)的姿態(tài)誤差

通過前面的姿態(tài)估計已經(jīng)確定當前姿態(tài)，現(xiàn)在應該確定目標姿態(tài)。姿態(tài)四元數(shù)可以用歐拉角來表示[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）

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

假定系統(tǒng)上電后的初始姿態(tài)為：

[b=qwqxqyqzT] （27）

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

[m=qw00qzT] （28）

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

[t=m?n] （29）

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

[a?Δr=t] （30）

兩邊同時乘以[a]的逆，可得姿態(tài)誤差：

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

4.2 用姿態(tài)誤差得出需要的力矩

姿態(tài)誤差為一四元數(shù)，結(jié)合式（3）～式（6），可將姿態(tài)誤差表示為：

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

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

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

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

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

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

4.3 使用力矩來控制油門

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

如果需要[x]軸的力矩（不妨取前飛），則油門增量值應為：0、2號電機不變，1號電機負，3號電機正，油門需要變化的方向（direction）為：[dx=0-101T，]引入[x]軸的力矩（moment）修正系數(shù)（靈敏度）[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）

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

把各軸分量疊加起來就是任意軸的情況，其中力矩為[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 實驗與結(jié)果

5.1 姿態(tài)估計的實驗

姿態(tài)估計可以用如圖4的平臺進行驗證。

5.2 PID調(diào)試平臺

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

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

5.3 飛行測試

四軸飛行器在空中可以實現(xiàn)穩(wěn)定的飛行，飛行效果如圖7所示。

6 結(jié) 語

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

參考文獻

[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] 佚名.無約束最優(yōu)化方法[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] 劉峰，呂強，王國勝，等.四軸飛行器姿態(tài)控制系統(tǒng)設計[J].計算機測量與控制，2011，19（3）：583?585.

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

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

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

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