1、非線性四元數(shù)姿態(tài)控制摘要這篇文章解決了四旋翼非線性姿態(tài)控制的問題。一個基于單位四元數(shù)的全局姿態(tài)穩(wěn) 定控制器被提出，而且它的穩(wěn)定性和魯棒性都得到了證明。另外，一個優(yōu)先考慮關鍵的 滾轉和俯仰角度的控制再考慮偏航控制的方法被引入進來。最終，對這個被提出的非線 性控制器進行實驗，并論證了它的性能。.背景適用于小角度線性化的控制器已經(jīng)不適用于高機動性，高性能的要求。為了挖掘四 旋翼飛行器的所有潛能，全局角度控制器應該被采用。歐拉角產(chǎn)生的奇異點也應該避免， 因為從純數(shù)學的角度它限制了飛行器一些可能的動作，不適用于全天候角度控制。而四 元數(shù)和余弦矩陣可以做到這一點。這篇文章的內(nèi)容為：第二節(jié)提供了關于姿態(tài)表示

2、和四軸飛行器設置的背景材料。第三節(jié)提出了全局漸近穩(wěn)定控制了律。最后，所提出控制器的實驗結果見第四節(jié)，報告總結見第五節(jié)。.預備知識角度表示剛體的角度可以用從參考坐標系到機體坐標系的旋轉表示。 這些旋轉參數(shù)可以是歐 拉角（三個參數(shù)），單位四元數(shù)（四個參數(shù)）或旋轉矩陣（九個參數(shù)）。而，所有的三參 數(shù)表示法會產(chǎn)生奇異點，所有的k參數(shù)表示法會有（k-3）個約束。旋轉矩陣旋轉矩陣Rbi是一個從一個線性空間 山_ 3到另一個線性空間BLL3的線性變換， 同時保留長度和右手螺旋準則。由以上特性，可以直接得出Rbi亡SO(3)。設正交基M W I 和eB w b ,其中e1 =&,封)(1)向量，在坐標系I和B

3、下的坐標分別是力和七。那么，從向量I到向量七的線性變 換Rbi滿足下式:rB = RBIrI而Rbi是一個3 M3的矩陣BIBI BI BIRBI = (rx ryrz )矩陣Rbi的三個列向量是坐標系I的單位正交基在坐標系B中的表示。同理，該矩陣的三個行向量是是坐標系B的單位正交基在坐標系I中的表示。2.1.2歐拉角歐拉角基于以下事實：可以通過連續(xù)地應用三個基本旋轉矩陣來構造任何旋轉矩陣。然后可以通過三個基本旋轉的旋轉角度來參數(shù)化旋轉矩陣的九個元素。一個用于飛行器的非常普遍的旋轉順序是ZYX順序。如圖1所示圖1 ZYX順序的歐拉角.繞初始I系中z軸旋轉的角為偏航角中(I系T K系)。.繞K系

4、中y軸旋轉的角為俯仰角0 (K系TL系)。.繞L系中x軸旋轉的角為滾轉角* ( L系t B系)。因此Rbi可表小為 TOC o 1-5 h z Rbi =RblRlkRki(4)100IJ0-SCy Sr0|=0cs010即即0(5)0-Scs00CQ001一c四旃-se:= -c砰 + s4s0cg c 唧 + s 蘋覿s 爐e(6) s帆 + cjs-sjQg + 沖覦 c爐8 一其中c(.)和s(.)分別是cos(.)和sin(.)的縮寫。從旋轉矩陣到歐拉角的逆映射是仁二arctan2(R23, R33)(7)a a arcsin(R3)(8)中=arctan2(R2, R11)(9)其

5、中，Rj表示矩陣Rbi中的第i行，第j列的元素。2.1.3單位四元數(shù)一人、一一一E ，k，、八*，、每個旋轉行為都可以用由單包向重k和旋轉角u描述的繞一個固止軸的一次旋轉進行參數(shù)化表示。這被稱為特征軸旋轉，它體現(xiàn)了兩個方向之間的最短旋轉。在此基礎上，定義了一個單位四元數(shù)_ cOs 一1T ,q0 1 I2q=qq1q2q3 =1= 4,、(10)q1:3 電 )-ksin-12人四元數(shù)q的伴隨，范數(shù)和逆分別是0q1:3 J|q| = Jq2 +q； +q2 +q2兩個四元數(shù)p和q的乘法可定義為q p =Q(q) p其中q-牝-q31q1qO73q2q2q3q。7*-q2q1q。-Q(q)=注意

6、，對應于旋轉矩陣I的四元數(shù)由下式給出-11qi向量r通過四元數(shù)q旋轉的計算公式如下p(r)=q p(r) q其中，rr是旋轉后的向量，p（.）是一個向量的四元數(shù)表示:o;-TGp從歐拉角到單位四元數(shù)的ZYX順序的映射為一與2（2屯2十射222即/2S/2C-/2C./2 C/2j2 s.72C/2SN2C./2 S/2J/2S.722c% 即/2 S/ S&2 Cg/22.1.4角度表示附錄旋轉矩陣和單位四元數(shù)都提供了角度的非奇異的特征表示。但是，需要說明的是， 單位四元數(shù)空間S3兩倍覆蓋了物理角度空間SO(3) o因此，單位四元數(shù)不是唯一的。實 際上，每對相反的單位四元數(shù) 土qwS3正對應相

7、同的姿態(tài)。對于空間 S3中的姿態(tài)控制任 務，這意味著必須設計一個控制器來穩(wěn)定一組斷開的平衡點。然而，單位四元數(shù)經(jīng)常被 選擇作為姿態(tài)控制的參數(shù)，因為他們是最小的全局非奇異參數(shù)。另外，單位四元數(shù)可以 讓我們輕松地理解幾何級別上發(fā)生的情況。2.2四旋翼飛行器卜面，我們考慮一個具有機體坐標系和慣性坐標系的四旋翼無人機，如圖2所示圖2機體坐標系B和慣性坐標系I下的四旋翼無人機這個四旋翼的輸入是四個電機的拉，輸出是位置，速度，加速度以及機體的姿態(tài)和角速 率。因為相比于輸入模型由更多的輸出，所以顯而易見，并不是所有的輸出能夠被獨立 地控制。在FMA中，使用一個級聯(lián)控制結構來控制四旋翼，它將位置控制作為最外環(huán)

8、 控制(圖3)。這個控制方式的內(nèi)外環(huán)時間尺度是分離的，只要內(nèi)環(huán)明顯快于外環(huán)，它就 是有效的。在這篇報告中，我們只考慮角度控制環(huán)。假設機上控制回路和四旋翼的動態(tài) 特性非?？煲灾劣诒豢叵到y(tǒng)可以被看做一個以期望角速率 A作為直接輸入的剛體。那么四旋翼動態(tài)模型可以由下式表示1IJ:3O 1q V-5 21 、, 2(S(q1：3)+qI p(20)其中S（q：3）表示qi：3的斜矩陣S( qi:3) -一。q3.一q2-q30qiq2q10(21)FeedforwardJIDesiredTrajectoryPositionControllerOnboard ControllerAttitude Con

9、troLler以匕血duadrocaptcrState feedback (position, velocity： acceleration, attitude, angular body rate)圖3級聯(lián)控制結構框圖證明:由于載體的運動，四元數(shù) q是變量，即*q1,q2,q3是時間的函數(shù)。這里設剛體繞瞬時轉軸n轉過日角，其角速度為(22)設這個機體坐標系（b系）和慣性坐標系（n系）之間的變換四元數(shù)的三角形式為e e 4q = cos sin n22(23)對公式（22）進行求導，得dq1 , - d-1-d -4二-sin cos ndt22 dt22dt這里假設旋轉軸未變化，即 如=0,

10、則 dtsin-電 2 dt(24)(25)(26) TOC o 1-5 h z dq1 ch1cbiq =二-sin -cosndt22dt22dt又因為q為單位四元數(shù)，所以n n=_i,則有dq1cd1c d4=-sin -cosn dt22dt22 dtdi f f*n cos sin - ndt22 1 n二 2-nb至此，公式(20)前半部分得證，后半部分將q和p展開相乘即可得到3.姿態(tài)控制設計首先第一步，位置環(huán)產(chǎn)生的期望加速度和期望偏航角必須轉換成期望的姿態(tài)qcmd。之后的目標就是設計一個能夠將四旋翼穩(wěn)定在任意期望的物理姿態(tài)角的反饋法則。因為在空間SO(3)中的任意物理角度對應空間

11、S3中的兩個對映四元數(shù)，可以通過將q穩(wěn)定在 士qcmd上實現(xiàn)穩(wěn)定性。如果忽視這個因素，基于四元數(shù)的控制器可能會導致不良現(xiàn)象比 如，unwinding,剛體不必要的全旋轉(a full rotation) 0為了解決unwinding的問題，控 制器必須滿足cmd(q) ”cmd(-q)(27)顯而易見，因為q和-q表示相同的物理角度，控制器的輸出也應該相同。這個問題 可以通過改變q0 0時q的符號解決。但是改變了符號會導致控制器不連續(xù)。有趣的是， 這不是一個弊端，因為可以證明所有的連續(xù)狀態(tài)反饋控制法則至多是幾乎全局穩(wěn)定?？刂品▌t考慮下面的控制法則：21， qe,0 - 0cmd(q) = sg

12、n(qe，0)qe,1：3，sgn(qe，0)(28)-1，qe,0 : 0其中E是一階系統(tǒng)時間常數(shù)S,qe =q,，qcmd為衡量誤差，表示從q到q0md的旋轉。然后，土qcmd是一個全局漸進穩(wěn)定平衡點證明：不失一般性，設置qcmd =qI。定義一個自主的混合裝置，H =(Z,Q, f ,Init,Dom, E,G,R),其中Z=z1,z2,兩個離散狀態(tài)，對映空間S3的上下兩個半球。Q=S3u14,連續(xù)狀態(tài)q,位于三位球面上，表示目前的角度。向量場1,、-q p -1cmd(z,q) , if z = 4f(z,q)=、.(29)1-q p -1cmd(z,q) , if z = z22其中

13、 TOC o 1-5 h z 224qe,i：3 二 一 qi：3,if z = zimd(z,q)= 。(30)22 qe,1:3 =-q：3, if z = z2LTT上述公式分別表示四元數(shù)在上半球 z1或下半球z2時，q隨時間的變化率。域 Dom(z1) = Dom z2 = S3。邊界E =(42),(22,乙),表示從4到z2的可能性，反之亦然。保護函數(shù) G(4,z2)S3 q0 o,G(z21z1) q = S3 q0 0 Vq,(z,q) w Dom(z)q，*V(z,q) =(z,q) f (z,q) W0 Vq,(z,q) w Dom(z)。二 q此外，V(z,q)0 Vz=

14、 Z , Vqw Dom(z)qJ而且 V(z,q ) = 0 Vz= Z 0 我們注意 到當z改變狀態(tài)時V(z,q)并不跳變，因此V(z,q)是嚴格遞減的，這意味著q是一個 全局漸進穩(wěn)定平衡點。備注1.偏差qe表示從當前角度q到期望角度qcmd的旋轉。通過取qe的符號，可以 保證旋轉角總是小于或等于180,。這就意味著控制器總是以最小旋轉角旋轉剛體。 但是注意，一個最小的旋轉角度通常并不意味著一個時間最優(yōu)的旋轉操作。如果旋 轉角速度被限制這是顯而易見的。例如，|Qj EQ, i wx,y,z andfor i # j。備注2.控制法則（28）關于qcmd =qi的形象解釋如圖4所示。圖4:上

15、述控制法則將角度穩(wěn)定在 %=1或%=1。但是，在q0=0處有一個間斷點如果不考慮qe,。的符號，控制器將只能是幾乎全局漸進穩(wěn)定，因為 q = -1是一個不穩(wěn)定平衡點。備注3.由定義知，何=1, VqwS3,因此，辰：M1。則最大可能輸出被限制為 15/_2。這對于飽和的系統(tǒng)尤其具有優(yōu)勢。+ q； +q； Iqoq1一qq2-qoq3備注4.不失一般T假設qcmd =q 。將控制法則（28）代入狀態(tài)方程（20）中(32)(33)忽略平衡點q的小偏差（q。比1,勺：3 1_ 1）,式（32）可簡化為飛。10 1一qq1q2一qq2-qoq31這是一個無耦合的時間常數(shù)為 七的一階系統(tǒng)。備注，七表示四

16、元數(shù)誤差衰減到 1/e的時間，而不是歐拉角誤差衰減到 1/e的時間。3.1.1魯棒性盡管所提出的控制器實現(xiàn)了任何所需姿態(tài)的全局漸近穩(wěn)定性,但可以證明控制器對于任意小的測量噪聲不穩(wěn)健。通過參考文獻8的嚴密證明，可以構建一個噪聲信號qnoise,使得當q任意地開始任意接近不連續(xù)性時，能始終保持接近不連續(xù)性。令當前角度表示為僅)1 cos 一I12)ik sin 1 2 I2人(34)定義qnoise為qnoise (q)losgI 2 Jksin(酗)-I 2力(35)其中k是角度q的本征軸，而且-P：(q) =|-p0,0，0,f 0 0( Hf n ot -nf n 之a(chǎn) 2冗with 0 0

17、, Vm(M) = 0,然后，當a|wB0時，Vm (q) = 2qq。1q0 qi:：3qi:3(39) =|q0|kTksin；sini工I 2 J 0因此，從任何在卜-可W P范圍內(nèi)開始的角度，系統(tǒng)不會收斂到期望角度。？ ？ ？在文獻8中，提出了一個帶有亢奮記憶狀態(tài)的不連續(xù)控制器能夠避免這種有害的平 衡。這種亢奮記憶狀態(tài)定義了一個旋轉方向，他可以使機體沿著最短過渡的方向旋轉。 如果|na|EB。，亢奮記憶狀態(tài)沒有獲得更新旋轉狀態(tài)仍然保持目前狀態(tài)，則會留下一 個任意小的噪聲信號都有可能使控制器不穩(wěn)定的區(qū)域。在實踐中，如果控制法則用在一 個離散時間控制器上，亢奮記憶狀態(tài)是不需要的。由于離散時

18、間控制器的輸出在兩次更 新之間是不變的，旋轉方向在這個時間段也是不變的，因此離散時間控制器的行為就像 帶滯后的連續(xù)時間控制器。期望姿態(tài)考慮圖2所示的帶有機體坐標系B的四旋翼飛行器，可以很明顯地看出，四旋翼僅 僅在能方向有加速度。因此，期望加速度 Icmd必須轉化為目標定位qcmd, Z軸才能與期 望加速度方向對齊。與每個指向應用一樣，圍繞指向方向的旋轉是無關緊要的，比如， 關于推力方向段的旋轉對四旋翼的平移行為沒有影響。因此，把控制任務分為兩部分是 合理的：簡化的姿態(tài)控制：只有推力的關鍵指向方向是受控制的。偏航角不被直接控制，但 是始終選擇qcmd都不會引起圍繞偏航軸的旋轉完整的姿態(tài)控制：推力

19、變量的指向和偏航角都是受控制的。選擇 qcmd所以對應的z軸和既取是一致的，而且中小cmd簡化的姿態(tài)控制給定任意期望加速度Imd ,期望推力和方向都能計算得到(40)(41)1MB _ Iacmd0mdcmdCOllcmd = IScmd11簡化的偏差四元數(shù)qe,red（圖5）是將四旋翼從當前姿態(tài)旋轉到期望姿態(tài)，公式如下qe,red 值Yicos 一I 12H fa ) k sin I 22)f 0()cos I 一2sin - 12 MleB BIe Ie -Ieze cmd, zzB Ie(42)(43)(44)其中IeB是當前姿態(tài)q對應的z軸，口是當前推力方向和期望推力方向的夾角:=ar

20、ccos IezB - IeLhz最終，期望姿態(tài)可由下式得出qcmd,red q qe, red注：qe,red的最后一個元素總是為零，因為k 1 6 ,這表明Ccmd,z=0。由于歐拉角的結構，這并不意味著偏航角 中是恒定的。3圖5 qe,red的幾何表示完整的姿態(tài)控制一個期望偏航角中,和推力方向&，：可以完整的表示姿態(tài)。俯仰和滾轉角，“md和%d,可以通過將eL,z映射到中間坐標系K和L上得到。首先，將期望推力方向向量言，工在K坐標系中表示，即沿著初始Z軸旋轉mdKecmd ,z二 RKI lecmd,zCOS# cmd )sin(中 cmd )0Sin(- cmd)cos( cmd )0

21、011(45)(46)(Kecmd ,z,3 J期望俯仰角8cmd可以通過把方向向量e，z投影到34平面進行重構（圖6）： am,、，使用公式（17）進行限制。首先，計算旋轉軸 Blezlecmd ,z Ie(54)然后，定義傾斜旋轉為cJ%iit cos (55)(56)12)Ik sin i%t-I 2人坐落在孔徑為21 max的椎體中的新的目標推力方向是BB片P lenew cmd,z 一 qtiltP lecmd ,zqtilt值得一提的是，這并不能阻止四軸飛行器翻轉過來。雖然現(xiàn)在可以到達的角度收到 了限制，飛行器還是可能通過一些外部的動力旋轉到一些極端的角度。然后，恢復到命 令姿態(tài)的

22、最短角度旋轉仍然可以涉及到執(zhí)行翻轉。啟發(fā)式算法*前面提到的控制法則（28）總是通過盡可能短的旋轉將無人機穩(wěn)定在期望的方向。 這是一個非常令人滿意的結果，但是，它假設角速度A可以被直接地，無窮快地控制，這在實踐中是不可能的。想象飛行器有幾乎180一偏航誤差的情況。此時，飛行器無論繞k軸旋轉還是繞-k軸旋轉并沒有大的區(qū)別。但是，如果向很大，很明顯沿著和Cz相 同的方向旋轉比沿著相反的方向加速要快得多。旋轉時間越長越快與否的問題，并不是 微不足道的？ ？。實際上，角速度 8的上升時間依賴于多個參數(shù)，比如，推力大小和角 速度的值，但是不存在解決這個問題的代數(shù)表達式。為了保持小的計算量，這里簡單 的啟發(fā)式的方法。之0,顯然什么都不用改這里，只引入偏航角誤差的啟發(fā)式算法，但是，偏航角和滾轉角使用它的工作原理 是一樣的。假設思四旋翼的角速度為 Q o qmix的旋轉軸是:;0勤1或:二0-1丁。如