四旋翼高速穩(wěn)定軌跡的自適應(yīng)控制方法

吳 穎，謝 云，李 森，李 力

（廣東工業(yè)大學(xué) 自動化學(xué)院，廣東 廣州 510000）

0 引言

四旋翼是由4 個電機和螺旋槳驅(qū)動的無人機。由于四旋翼具有較簡單的結(jié)構(gòu)，在科研、工業(yè)和農(nóng)業(yè)等方面[1?2]被廣泛應(yīng)用。但當(dāng)前四旋翼的控制研究主要在低速飛行控制方面，而較少有人關(guān)注四旋翼的高速（大于8 m/s）飛行控制研究。四旋翼的高速穩(wěn)定飛行對螺旋槳在高速飛行下的變形、四旋翼空氣動力學(xué)模型等研究具有相當(dāng)重要的意義[3]。具有系繩約束的無人機有許多應(yīng)用，如可以用于無人機的供電[4]、機器人遠(yuǎn)程控制的輔助[5]，還可以用于室內(nèi)定位[6]和無人機的定點位置控制[7]。與文獻[8]相似，本文利用系繩使四旋翼能夠在有限的空間做高速運動，但本文提出的控制方法是適用于質(zhì)量較大（大于1 kg）的無人機，且還提出了系繩拉力的動力學(xué)模型。

四旋翼飛行控制涉及四旋翼的位置控制和姿態(tài)控制，大部分論文研究四旋翼的姿態(tài)控制，都是用歐拉角表示四旋翼的姿態(tài)[9?10]。但用歐拉角表示四旋翼的姿態(tài)不僅會出現(xiàn)奇異性，而且沒辦法保證四旋翼平滑的姿態(tài)運動軌跡。而如果用旋轉(zhuǎn)矩陣表示四旋翼的姿態(tài)，則可以防止四旋翼的姿態(tài)出現(xiàn)奇異性，且可以對四旋翼的姿態(tài)進行軌跡規(guī)劃，使姿態(tài)運動軌跡更加平滑。而不同于文獻[11]利用so（3）的性質(zhì)對姿態(tài)誤差進行定義，本文是通過旋轉(zhuǎn)矩陣和羅德里格斯公式，將旋轉(zhuǎn)矩陣表示的四旋翼姿態(tài)誤差轉(zhuǎn)化為繞旋轉(zhuǎn)軸旋轉(zhuǎn)一定角度的變換。

四旋翼的位置控制采用基于四旋翼當(dāng)前位置設(shè)計自適應(yīng)控制器的方法，自適應(yīng)控制器在四旋翼的控制中有非常廣泛的應(yīng)用，既可以應(yīng)用于四旋翼的姿態(tài)控制[12]，也可以應(yīng)用于四旋翼在負(fù)載變化情況下的定高控制[13]、四旋翼的位置控制[14]和軌跡跟蹤[15]等。但這些研究都是在討論四旋翼低速飛行控制，而本文要討論的是四旋翼高速飛行控制，這要求四旋翼系統(tǒng)響應(yīng)有更短的調(diào)節(jié)時間和更好的穩(wěn)態(tài)性能。

而由于本文設(shè)計的控制器對控制模型精確度要求不高，魯棒性強，且空氣阻力比系繩拉力要小得多，故本文不考慮四旋翼的空氣動力學(xué)。所以本文在旋轉(zhuǎn)矩陣表示四旋翼姿態(tài)誤差的情況下設(shè)計姿態(tài)的比例控制算法和改進的位置自適應(yīng)控制器，可以根據(jù)四旋翼的當(dāng)前位置和期望軌跡調(diào)節(jié)期望速度，將姿態(tài)誤差控制在一定范圍內(nèi)。

1 系統(tǒng)模型

如圖1 所示，四旋翼的結(jié)構(gòu)是一個“+”型的結(jié)構(gòu)，由4 個電機和螺旋槳驅(qū)動。慣性坐標(biāo)軸和機體坐標(biāo)軸分別記作Oexeyeze和Obxbybzb。在建立四旋翼動力學(xué)模型前，假設(shè)繩子在四旋翼上的懸掛點、四旋翼的質(zhì)心、幾何中心和機體坐標(biāo)系在同一點上，慣性坐標(biāo)系則是相對于地面固定不變的。

圖1 四旋翼模型

1.1 旋轉(zhuǎn)矩陣

文中用旋轉(zhuǎn)矩陣來表示四旋翼的姿態(tài)，假設(shè)xeb，yeb，zeb分別表示機體坐標(biāo)系的x,y,z軸在慣性坐標(biāo)系下的坐標(biāo)（兩坐標(biāo)系的原點重合），則機體坐標(biāo)系到慣性坐標(biāo)系下的旋轉(zhuǎn)矩陣為：

若點q在機體坐標(biāo)系下的坐標(biāo)記為qb，則點q在慣性坐標(biāo)系下的坐標(biāo)為：

1.2 運動學(xué)與動力學(xué)模型

四旋翼是一個有6 個自由度的剛體，但其控制輸入只有4 個電機的轉(zhuǎn)速，所以四旋翼是一個欠驅(qū)動的系統(tǒng)。四旋翼的運動學(xué)與動力學(xué)模型如下：

式中：x∈R3表示四旋翼在慣性坐標(biāo)系下的位置；v∈R3表示慣性坐標(biāo)系下的速度；R∈R3×3表示從機體坐標(biāo)系到慣性坐標(biāo)系的旋轉(zhuǎn)矩陣；ωb∈R3表示機體坐標(biāo)系下的角速度；m∈R 表示四旋翼的質(zhì)量；J∈R3×3表示四旋翼的慣性矩陣；f∈R 表示4 個螺旋槳產(chǎn)生的總推力；τ∈R3表示螺旋槳產(chǎn)生機體坐標(biāo)系下的推力矩；g是表示重力加速度；Ft∈R3是系繩產(chǎn)生的拉力；e3=[0 0 1]T?！倪\算的定義為，若ω=[ω1ω2ω3]T，則：

1.3 推力和轉(zhuǎn)動力矩模型

文中忽略電機的動力學(xué)模型，假設(shè)四旋翼的螺旋槳產(chǎn)生的推力與電機的轉(zhuǎn)速平方成正比。因此，系統(tǒng)的控制輸入就是電機的轉(zhuǎn)速，因此，螺旋槳產(chǎn)生的總推力和機體坐標(biāo)系下的動力矩為：

式中：cT和cM都是常數(shù)；d是四旋翼的臂長。

1.4 系繩拉力模型

假設(shè)系繩是拉緊的，沒有重量且沒有彈性，不可伸縮的，因此系繩對四旋翼提供了位置限制。系繩拉力的模型如下：

式中：ft是指拉力的長度；pt是指拉力的方向。通過牛頓第二定理和向心加速度的公式推導(dǎo)可得pt和ft：

式中系繩的長度lt=|x|。

2 軌跡跟蹤

在設(shè)計四旋翼的控制器之前，假設(shè)控制器以高頻率讀取四旋翼狀態(tài)的測量值，包括四旋翼的位置、速度、姿態(tài)和角速度，并且假設(shè)四旋翼狀態(tài)測量值的誤差足夠小，可以忽略不計。因此，本文目標(biāo)是設(shè)計一個控制器，讓四旋翼跟隨給定的軌跡飛行。給定一個期望的圓軌跡xd和四旋翼的偏航角ψd，控制器結(jié)構(gòu)如圖2 所示。

圖2 軌跡跟蹤控制器結(jié)構(gòu)

四旋翼位置控制器的輸出是四旋翼的期望機體坐標(biāo)系z軸方向b3d和四旋翼的期望推力，四旋翼的姿態(tài)控制器根據(jù)位置控制器的輸出，即四旋翼的期望機體坐標(biāo)系z軸方向b3d和期望的偏航角ψd，計算輸出四旋翼的期望控制力矩。四旋翼是一個欠驅(qū)動系統(tǒng)，四旋翼的位置控制依賴于姿態(tài)控制器的性能，當(dāng)姿態(tài)控制器控制性能較差，姿態(tài)誤差較大時，位置控制器的性能也會變差，位置誤差也會變大。只有當(dāng)姿態(tài)誤差趨近于0 時，位置誤差才有可能趨近于0。

2.1 姿態(tài)控制

大部分四旋翼的姿態(tài)控制器的姿態(tài)表示都是用歐拉角，但其實相比旋轉(zhuǎn)矩陣和四元數(shù)表示，用歐拉角表示姿態(tài)會出現(xiàn)奇異性問題，而且歐拉角是一個很不直觀的方法，即使已知四旋翼當(dāng)前姿態(tài)的歐拉角，也很難知道四旋翼機頭的具體朝向。四旋翼姿態(tài)的歐拉角與旋轉(zhuǎn)矩陣之間的變換關(guān)系如下：

由變換關(guān)系可以看出，即使對姿態(tài)歐拉角進行軌跡規(guī)劃，四旋翼的姿態(tài)也不會平滑的變換。所以本文采用旋轉(zhuǎn)矩陣表示四旋翼的姿態(tài)，但旋轉(zhuǎn)矩陣有一個缺點是有9 個變量，所以采用羅德里格斯公式，把旋轉(zhuǎn)矩陣轉(zhuǎn)化為一個一一對應(yīng)的旋轉(zhuǎn)軸ω∈R3和一個旋轉(zhuǎn)角度θ∈[0,2π)，且由于該變換是一一對應(yīng)的，因此可以避免四旋翼姿態(tài)的奇異性問題。四旋翼期望姿態(tài)的旋轉(zhuǎn)矩陣Rd可由ψd和b3d推導(dǎo)得出。而Rd的第3 列向量是期望姿態(tài)的機體坐標(biāo)系的z軸在慣性坐標(biāo)系下的坐標(biāo)，即期望機體坐標(biāo)系z軸方向b3d，因此可得：

由給定的期望偏航角ψd可得：

由于在本文的研究中，機體坐標(biāo)軸的z軸方向在慣性坐標(biāo)軸z軸的分量肯定與慣性坐標(biāo)軸z軸的正方向同向，由四旋翼的動力學(xué)模型可假定cosφd和cosθd均大于等于0，則可求出cosφd，cosθd，sinφd，sinθd，即可求出Rd。求出Rd之后，要利用期望姿態(tài)的旋轉(zhuǎn)矩陣和四旋翼當(dāng)前姿態(tài)的旋轉(zhuǎn)矩陣Rc求出四旋翼機體坐標(biāo)系下的期望角速度，所以誤差旋轉(zhuǎn)矩陣應(yīng)取Re=(Rc)-1Rd。已知羅德里格斯公式的表達式如下：

則根據(jù)式（7）可得：

而且，如果R=I，則θ=0°，ω可取任意值。

因此，可以記姿態(tài)誤差旋轉(zhuǎn)矩陣為：

對誤差旋轉(zhuǎn)矩陣?yán)檬剑?）和∨運算轉(zhuǎn)化成一個表示四旋翼姿態(tài)誤差的向量eR∈R3，有：

式中∨為∧運算的逆運算。接下來定義四旋翼機體角速度的誤差為：

式中ω為當(dāng)前機體角速度。

因此，四旋翼姿態(tài)控制器的輸出為：

將式（9）代入式（11），可得四旋翼姿態(tài)控制器輸出：

2.2 位置控制

四旋翼的位置控制器依賴于四旋翼的姿態(tài)控制器，所以當(dāng)四旋翼在高速飛行時，對姿態(tài)控制器的跟蹤誤差要求很高，如果四旋翼的姿態(tài)誤差太大時，四旋翼的位置跟蹤誤差就會發(fā)散而無法收斂。為了解決這個問題，本文設(shè)計一個位置控制器，能夠使四旋翼在保持高速飛行的同時逐步調(diào)節(jié)姿態(tài)誤差。位置控制器能夠以四旋翼當(dāng)前位置和期望軌跡去計算四旋翼的期望速度，然后通過比例控制算法控制四旋翼跟隨期望軌跡。

由于本文四旋翼是進行高速圓周運動，所以若給定四旋翼的期望軌跡：

則四旋翼在當(dāng)前時刻的期望速度為：

式中x為當(dāng)前時刻四旋翼的位置。

定義四旋翼的速度誤差為：

式中v為當(dāng)前時刻四旋翼的速度。

四旋翼位置控制器輸出四旋翼期望推力和fd以及四旋翼期望機體坐標(biāo)系z軸方向b3d分別為：

3 仿真數(shù)據(jù)與分析

采用Simulink 對四旋翼進行仿真，四旋翼飛行器的參數(shù)如下：

根據(jù)建立的四旋翼模型，測試傳統(tǒng)PID 控制器和本文提出的自適應(yīng)調(diào)節(jié)控制器在不同規(guī)劃軌跡下的性能，取四旋翼的姿態(tài)控制器參數(shù)為kR=[11 11 11]T和kω=[4.8 4.8 9.8]T，若四旋翼的初始位置x0，初始姿態(tài)R0為：

四旋翼低速飛行的期望軌跡xd，ψd分別為：

四旋翼高速飛行的期望軌跡xd，ψd分別為：

若四旋翼的位置控制器為傳統(tǒng)PID 控制器，且PID控制器的參數(shù)如表1 所示。

表1 PID 控制器的參數(shù)

在控制器期望軌跡輸入為低速飛行軌跡的情況下，四旋翼的控制響應(yīng)如圖3 所示。

圖3 PID 控制響應(yīng)

四旋翼的姿態(tài)誤差超調(diào)量較大，且姿態(tài)誤差響應(yīng)震蕩較大，達到穩(wěn)定需要不少于5 s，而且系統(tǒng)線速度的調(diào)節(jié)時間也比較長，穩(wěn)態(tài)性能也比較差。而傳統(tǒng)PID 控制器在輸入高速飛行軌跡的情況下，四旋翼的姿態(tài)誤差和位置誤差都會發(fā)散，無法跟隨期望軌跡飛行。

而在給定參數(shù)kv=[0.6 0.6 3]T的情況下，由圖4、圖5 所示，在低速飛行下，四旋翼的姿態(tài)誤差能在1 s 內(nèi)迅速減小趨近于0，且在2 s 內(nèi)速度誤差能夠逐漸收斂趨近于0，且沿圓軌跡飛行時最大切向加速度的變化也比較平滑，四旋翼線速度的穩(wěn)態(tài)性能也比PID 控制器要好。

圖4 自適應(yīng)控制低速飛行響應(yīng)

圖5 自適應(yīng)控制高速飛行響應(yīng)

四旋翼能夠?qū)崿F(xiàn)穩(wěn)定加速至期望速度，且能在2 s內(nèi)實現(xiàn)穩(wěn)定地跟隨期望軌跡。而在高速飛行情況下，四旋翼的姿態(tài)誤差也能在1 s 內(nèi)迅速減小并逐漸趨近于0，而且最大速度能夠達到10.5 m/s，沿圓軌跡飛行的最大切向加速度也能達到4 m/s2，能夠穩(wěn)定加速至期望速度，從而實現(xiàn)穩(wěn)定跟隨期望軌跡。

因此，相比于傳統(tǒng)PID 控制器，本控制器在高速飛行情況下，能夠穩(wěn)定地控制四旋翼的速度達到高速，且在低速飛行的情況下也比PID 控制器的控制效果更好。

4 結(jié)論

本文提出一種能夠控制四旋翼高速穩(wěn)定飛行的控制方法，該方法分別提出了四旋翼姿態(tài)和位置的控制器，在姿態(tài)控制方面，用旋轉(zhuǎn)矩陣表示四旋翼的姿態(tài)，通過羅德里格斯公式與四旋翼的姿態(tài)動力學(xué)模型對四旋翼的姿態(tài)誤差進行了定義，設(shè)計了比例控制算法控制四旋翼的姿態(tài)。在位置控制方面，利用四旋翼的狀態(tài)和期望軌跡得到期望速度，在誤差較大時減小速度。利用系繩使四旋翼能夠在有限空間高速飛行，同時提出四旋翼飛行時受到的拉力動力學(xué)模型。通過仿真數(shù)據(jù)分析得到以下結(jié)論：本文提出的姿態(tài)控制器能使四旋翼的姿態(tài)誤差迅速減小并趨于穩(wěn)定。相比于傳統(tǒng)PID 控制器，位置控制器在高低速情況下都能使四旋翼穩(wěn)定跟隨期望軌跡速度，最高能夠達到10.5 m/s?；诒究刂破?，四旋翼還可以實現(xiàn)各種復(fù)雜的控制策略，并且可以對四旋翼的姿態(tài)進行規(guī)劃，這也將是下一步的研究方向。