石喜玲, 孫運(yùn)強(qiáng), 李 靜，余紅英

(1.中北大學(xué)電氣與控制工程學(xué)院，太原 030051; 2.中北大學(xué)信息與通信工程學(xué)院，太原 030051)

多旋翼無人機(jī)是一種便攜式動(dòng)力裝置，可以在危險(xiǎn)、惡劣的環(huán)境下工作，所以在學(xué)術(shù)研究、民用娛樂和軍事偵查有著重要的作用，已逐漸成為無人機(jī)(unmanned aerial vehicle, UVA)研究的前沿。四旋翼飛行器是可以垂直起降的非共軸式多旋翼飛行器，具有升降要求低、可懸停等特點(diǎn)[1]?？梢栽趶?fù)雜的空間起飛，還可以懸停在靜止或者移動(dòng)的目標(biāo)上，具有很好的機(jī)動(dòng)性。

四旋翼飛行器位姿控制可以通過調(diào)節(jié)四個(gè)螺旋槳的轉(zhuǎn)速，不需要尾翼，實(shí)現(xiàn)懸停、前飛、側(cè)飛和倒飛。但是，這些屬性是有代價(jià)的，四旋翼飛行器屬于高度欠驅(qū)動(dòng)非線性系統(tǒng)，因此控制器研究成為無人機(jī)設(shè)計(jì)師和研究人員關(guān)注更多的問題[2]。

針對(duì)這些問題，不同文獻(xiàn)中提出了各種方法來改變用于建模和設(shè)計(jì)四旋翼的控制系統(tǒng)。傳統(tǒng)的四旋翼結(jié)構(gòu)是每個(gè)旋翼安裝在離飛行器質(zhì)心等距離的角上，而所有旋翼的同步轉(zhuǎn)速是四旋翼建模和有效控制的關(guān)鍵[3]。Kim等[4]提出了高轉(zhuǎn)速電機(jī)陀螺效應(yīng)的建模方法，但這種陀螺效應(yīng)在四軸飛行器模型線性化過程中會(huì)消失。梁曉等[5]建立了四旋翼系繩運(yùn)輸系統(tǒng)軌跡規(guī)劃模型，通過分析系統(tǒng)在飛行中受到的約束情況，引入微分平滑理論，利用系統(tǒng)的微分平滑特性將系統(tǒng)狀態(tài)變量和控制輸入的約束統(tǒng)一映射到平滑輸出上，得到線性約束下的最優(yōu)化模型。無人機(jī)的最新研究已轉(zhuǎn)向帶有載荷設(shè)備的飛行任務(wù)(如拾取、移動(dòng)和放置操縱任務(wù))和既定軌跡飛行等，這些飛行任務(wù)的控制方法研究將需要一個(gè)更完整的無人機(jī)動(dòng)態(tài)模型。

基于經(jīng)典牛頓-歐拉角定律建立四旋翼六自由度剛體運(yùn)動(dòng)學(xué)模型，根據(jù)多旋翼的飛行環(huán)境等特點(diǎn)，針對(duì)滾轉(zhuǎn)角和俯仰角都非常小，對(duì)非線性模型進(jìn)行簡(jiǎn)化[6-7]，然后采用級(jí)聯(lián)雙閉環(huán)比例-積分-微分(proportional-integral-derivative,PID)控制法進(jìn)行位置和姿態(tài)控制器設(shè)計(jì)，搭建仿真模型驗(yàn)證并對(duì)控制參數(shù)整定調(diào)試，在飛控。

1 系統(tǒng)概述與建模

1.1 四旋翼控制剛體模型

四旋翼飛行器有4個(gè)旋翼且旋轉(zhuǎn)平面共面，4個(gè)獨(dú)立電機(jī)分別獨(dú)立驅(qū)動(dòng)4個(gè)頂點(diǎn)上的螺旋槳且以中心對(duì)稱，按轉(zhuǎn)動(dòng)方向不同，4個(gè)旋翼分為逆時(shí)針和順時(shí)針兩組[8]。由于兩組旋翼轉(zhuǎn)動(dòng)方向相反，所以旋翼的陀螺力矩和反扭矩可被有效抵消。另外通過差動(dòng)改變同組旋翼的轉(zhuǎn)速，可以在差動(dòng)力矩的作用下，實(shí)現(xiàn)滾轉(zhuǎn)和俯仰運(yùn)動(dòng)。偏航運(yùn)動(dòng)由兩組旋翼產(chǎn)生的反扭力矩之差實(shí)現(xiàn)，總升力為4個(gè)旋翼產(chǎn)生的升力之和[7]。四旋翼飛行器空間結(jié)構(gòu)如圖1所示。

圖1 四旋翼空間結(jié)構(gòu)和坐標(biāo)定義

建立四旋翼運(yùn)動(dòng)方程有必要確定旋翼機(jī)體的參考坐標(biāo)系。如圖 1所示，通常使用兩個(gè)參考坐標(biāo)系在空間坐標(biāo)中定義四旋翼，定義如下[6，9]。

機(jī)體(或運(yùn)動(dòng)物體)坐標(biāo)系與四旋翼固連，四旋翼質(zhì)心為坐標(biāo)原點(diǎn)ob，三維坐標(biāo)軸表示為xb、yb、zb。

重力指向zb軸方向。描述機(jī)體線性位置和角位置的坐標(biāo)矢量表示為三軸平移速度[u,v,w]T和三軸轉(zhuǎn)動(dòng)角速率[p,q,r]T。

慣性(或地球)坐標(biāo)系用于表示機(jī)體相對(duì)于地面的運(yùn)動(dòng)狀態(tài)，以四旋翼起飛位置或地心作為坐標(biāo)原點(diǎn)o，三維坐標(biāo)軸表示為x、y、z。其中，z軸指向地心方向，垂直地面向下。在此坐標(biāo)系內(nèi),描述四旋翼線性和角度定位的是位置矢量[x,y,z]T和姿態(tài)歐拉角為[φ,θ,ψ]T，其中φ為滾轉(zhuǎn)角，θ為俯仰角，ψ為偏航角。

在機(jī)體和慣性坐標(biāo)系中四個(gè)矢量組合表示了四軸飛行器的12種狀態(tài)(6個(gè)姿態(tài)和 6個(gè)位置和線速度狀態(tài))[10]，假定慣性坐標(biāo)系按照zyx順序旋轉(zhuǎn)到機(jī)體坐標(biāo)系，則其旋轉(zhuǎn)矩陣R如式(1)所示：

(1)

四旋翼通常被描述為動(dòng)態(tài)的高度非線性系統(tǒng)，具有 6個(gè)自由度(3個(gè)平移速度和 3個(gè)旋轉(zhuǎn)速度)，由 4個(gè)獨(dú)立的輸入即四個(gè)旋翼驅(qū)動(dòng)，所以四旋翼動(dòng)力學(xué)是一個(gè)典型的欠驅(qū)動(dòng)控制系統(tǒng)的，具有參數(shù)不確定性[11]。

假設(shè)四旋翼飛行器是剛體，結(jié)構(gòu)完全對(duì)稱，機(jī)體幾何中心和飛行器質(zhì)心重合，忽略阻力系數(shù)，根據(jù)牛頓第二定律、動(dòng)能定理和動(dòng)量原理可以得到四旋翼飛行器的剛體運(yùn)動(dòng)模型和動(dòng)力學(xué)模型:

(2)

(3)

式中：m為四旋翼機(jī)體的質(zhì)量；g為重力加速度；(x,y,z)為在慣性坐標(biāo)系下四旋翼空間的位置；(φ,θ,ψ)為在其坐標(biāo)系下四旋翼飛行器的姿態(tài)，分別代表滾轉(zhuǎn)角、俯仰角和偏航角；Ui(i=1,2,3,4)為旋翼在機(jī)體上產(chǎn)生的總升力和三軸力矩；Ix、Iy、Iz為旋翼和電機(jī)分別繞x軸、y軸和z軸旋轉(zhuǎn)時(shí)的轉(zhuǎn)動(dòng)慣量；l為飛行器機(jī)體質(zhì)心與其旋翼旋轉(zhuǎn)軸二者之間的距離。

1.2 動(dòng)態(tài)模型線性化

通過上述原理推導(dǎo)出四旋翼的數(shù)學(xué)模型和運(yùn)動(dòng)方程本質(zhì)上是非線性的，傳統(tǒng)上，時(shí)不變非線性模型的線性化是獲得更經(jīng)典的控制器的必要條件[12]。

采用小擾動(dòng)理論是目前最常用的一種線性化方法[13]。假設(shè)在室內(nèi)對(duì)多旋翼的飛行調(diào)試中，俯仰角和滾轉(zhuǎn)角都非常小[8]，即

sinφ≈φ,cosφ≈1,sinθ≈θ,cosθ≈1,

(4)

上述原始系統(tǒng)模型[式(2)、式(3)]可以解耦為位置通道式(4)和姿態(tài)線性模型式(5)：

(5)

(6)

2 控制器設(shè)計(jì)

四旋翼控制器設(shè)計(jì)采用級(jí)聯(lián)雙閉環(huán)PID控制算法，外環(huán)是位置坐標(biāo)控制，內(nèi)環(huán)為姿態(tài)控制，位置控制包含姿態(tài)反解算以得到內(nèi)環(huán)所需的期望姿態(tài)角。而四旋翼位置坐標(biāo)改變由歐拉角變化引起，所以姿態(tài)控制是四旋翼飛行器控制的核心，其控制性能會(huì)極大地影響航行穩(wěn)定性[14]。

假設(shè)四旋翼期望位置為pd=[xd,yd,zd]T，實(shí)際位置測(cè)量值為p=[x,y,z]T，位置PID控制器用來確保當(dāng)t→∞ 時(shí)，‖p(t)-pd(t)‖→0，控制算法如下。

設(shè)ep=p-pd, 則

(7)

式(7)中：kp、ki、kd分別為PID算法的比例、積分、微分增益系數(shù)。

(8)

期望姿態(tài)角為

(9)

由位置控制模塊產(chǎn)生的兩個(gè)姿態(tài)角滾轉(zhuǎn)角φd、俯仰角θd和系統(tǒng)輸入期望偏航角ψd作為姿態(tài)控制的輸入期望信號(hào)，算法如式(9)所示：

(10)

式(10)中：kpφ,pθ,pψ、kiφ,iθ,iψ、kdφ,dθ,dψ分別為比例、積分、微分增益系數(shù)。

3 仿真分析

利用Simulink搭建了四旋翼系統(tǒng)控制剛體運(yùn)動(dòng)學(xué)模型，包含系統(tǒng)運(yùn)動(dòng)學(xué)模型和控制器兩大子系統(tǒng)。圖2為四旋翼級(jí)聯(lián)雙閉環(huán)PID控制器子系統(tǒng)，分為三個(gè)模塊，即位置控制模塊、姿態(tài)反解算模塊、姿態(tài)控制模塊。

四旋翼系統(tǒng)參數(shù)如表1所示。經(jīng)過調(diào)試PID控制器參數(shù)選擇如表2所示。

圖2 位置和姿態(tài)的級(jí)聯(lián)控制器

表1 四旋翼系統(tǒng)參數(shù)

表2 控制器參數(shù)

假定四旋翼飛行器的起飛位置初始值為(0,0,0)，期望定點(diǎn)懸停坐標(biāo)為

pd=[xd,yd,zd]T=(20,20,-5)

(10)

如圖3、圖4仿真結(jié)果表明：四旋翼在起飛階段位置響應(yīng)x方向超調(diào)為4.3%，y方向超調(diào)為7.3%，存在誤差范圍內(nèi)的振蕩；達(dá)到相對(duì)穩(wěn)定后，穩(wěn)態(tài)誤差在x方向0.15%范圍內(nèi),y方向穩(wěn)態(tài)誤差在1.4%范圍內(nèi)；姿態(tài)角的最大值控制在0.1 rad之內(nèi)，系統(tǒng)仿真驗(yàn)證了 PID 控制的有效性。

假定四旋翼已被命令在x方向沿正弦路徑以1 m振幅，0.1頻率運(yùn)動(dòng)，即xd=f(x)=sin(0.1x)。

圖3 定點(diǎn)懸停位置響應(yīng)

圖4 定點(diǎn)懸停姿態(tài)響應(yīng)

圖5 二維平面正弦軌跡跟蹤[sin(0.1x)]

圖6 三維正弦軌跡跟蹤[sin(0.1x)]

圖5、圖6分別從二維平面和三維空間兩個(gè)角度展示了四旋翼正弦軌跡跟蹤的結(jié)果，可以看出跟蹤誤差很小。

但隨著頻率增加當(dāng)xd=f(x)=sin(0.3x)時(shí)，跟蹤結(jié)果如圖7所示，可以看出系統(tǒng)超調(diào)量明顯增加，這證明了當(dāng)前控制算法對(duì)小角度、低速度軌跡的限制。

圖7 二維平面正弦軌跡跟蹤[sin(0.3x)]

圖8 高度控制實(shí)時(shí)采樣數(shù)據(jù)

4 試驗(yàn)

通過在MATLAB環(huán)境下建立四旋翼飛行器的非線性模型，采用PID級(jí)聯(lián)控制方法對(duì)給定軌跡的四旋翼進(jìn)行跟蹤，多次調(diào)試控制增益得到最優(yōu)增益組合。利用自主搭建的基于stm32四旋翼飛行器對(duì)該級(jí)聯(lián)PID控制算法進(jìn)行驗(yàn)證將數(shù)據(jù)回傳到上位機(jī)進(jìn)行分析。圖8所示為四旋翼高度調(diào)節(jié)實(shí)時(shí)數(shù)據(jù)波形圖，目標(biāo)高度為1 000 mm，z在降落起飛時(shí)存在相對(duì)較大的誤差，這是由于飛行器機(jī)體的振動(dòng)及干擾誤差造成。四旋翼達(dá)到目標(biāo)高度平穩(wěn)懸停后，誤差控制在1%范圍內(nèi)。從而可以證明級(jí)聯(lián)PID控制方法在定高懸停飛行中的有效性。

5 結(jié)論

研究結(jié)果表明對(duì)于小速度、小角度假設(shè)條件，能很好地實(shí)現(xiàn)對(duì)四旋翼飛行器既定軌跡的跟蹤控制。然而對(duì)于更高的飛行要求是不夠的，在今后的工作中，將考慮大角度、大速度軌跡的跟蹤，需要對(duì)控制算法進(jìn)一步改進(jìn)。