申 旺,鄒樹梁,鄧 騫

(1.南華大學(xué) 機(jī)械工程學(xué)院，湖南 衡陽(yáng) 421001； 2.南華大學(xué) 核設(shè)施應(yīng)急安全作業(yè)技術(shù)與裝備湖南省重點(diǎn)實(shí)驗(yàn)室 湖南 衡陽(yáng) 421001)

0 引 言

近年來(lái)，多旋翼無(wú)人機(jī)因?yàn)槠錂C(jī)動(dòng)性、可靠性等優(yōu)點(diǎn)成為研究熱點(diǎn)，而六旋翼無(wú)人機(jī)，相較于四旋翼無(wú)人機(jī)，具有更強(qiáng)的負(fù)載能力和更高的可靠性，而且由于其具有冗余的執(zhí)行機(jī)構(gòu)，可以在一個(gè)電機(jī)停止的情況時(shí)繼續(xù)工作。六旋翼無(wú)人機(jī)通過改變電機(jī)的轉(zhuǎn)速實(shí)現(xiàn)位置和姿態(tài)的控制，而六旋翼本身是一個(gè)欠驅(qū)動(dòng)系統(tǒng)，進(jìn)行對(duì)六旋翼的研究，特別是控制器的研究具有重大的意義。

國(guó)內(nèi)外有許多學(xué)者進(jìn)行了相關(guān)研究。徐會(huì)麗[1]采用改進(jìn)的PIDNN算法對(duì)六旋翼進(jìn)行控制，加快了響應(yīng)速度，緩解了超調(diào)現(xiàn)象，并且減少了調(diào)參次數(shù)；黃佳奇[2]設(shè)計(jì)了一種基于反步法的控制器，實(shí)驗(yàn)結(jié)果表明對(duì)無(wú)人機(jī)的位姿控制，所設(shè)計(jì)的控制器對(duì)定點(diǎn)懸停展現(xiàn)出良好的控制性能；王巍[3]利用滑?？刂品椒ㄑ芯苛怂男砗腿頍o(wú)人機(jī)的姿態(tài)調(diào)節(jié)和方位跟蹤的控制問題; 曾振華等[4]采用模糊自適應(yīng)速度PID控制系統(tǒng)實(shí)現(xiàn)多旋翼的自主精準(zhǔn)降落。

筆者基于牛頓-歐拉方程和牛頓第二定律，建立了六旋翼無(wú)人機(jī)的動(dòng)力學(xué)模型，在小角度的假設(shè)條件下，對(duì)動(dòng)力學(xué)模型進(jìn)行了線性化處理，設(shè)計(jì)雙環(huán)PID控制器，在MATLAB/Simulink中搭建模型并實(shí)現(xiàn)定點(diǎn)懸停的仿真，驗(yàn)證了模型的有效性，對(duì)后續(xù)控制系統(tǒng)的實(shí)驗(yàn)具有參考意義。

1 六旋翼無(wú)人機(jī)概述

1.1 六旋翼無(wú)人機(jī)結(jié)構(gòu)

無(wú)人機(jī)(Unmanned Aerial Vehicle,UAV),是利用無(wú)線電遙控設(shè)備和自備的程序控制裝置操縱的不載人飛機(jī)，六旋翼無(wú)人機(jī)則指旋翼數(shù)為6的無(wú)人機(jī)。根據(jù)旋翼的分布方式，分為I型，V型等，如圖1所示。

圖1 六旋翼無(wú)人結(jié)構(gòu)

六旋翼無(wú)人機(jī)的機(jī)體由機(jī)架和旋翼組成。機(jī)架承載六旋翼所有組件，六旋翼的安全性、可用性都和機(jī)架的布局密切相關(guān)。旋翼隨電機(jī)轉(zhuǎn)動(dòng)為無(wú)人機(jī)提供升力，相鄰兩旋翼的旋轉(zhuǎn)方向相反，反扭距可被有效抵消。與四旋翼無(wú)人機(jī)相比，六旋翼無(wú)人機(jī)具備冗余的執(zhí)行機(jī)構(gòu)，當(dāng)遇到較強(qiáng)外力干擾或者部分旋翼受擾時(shí)能表現(xiàn)出更好的穩(wěn)定性。

1.2 六旋翼飛行控制原理

以圖1(b)為例，說明四種基本運(yùn)動(dòng)的原理。

(1) 俯仰運(yùn)動(dòng)：規(guī)定機(jī)體抬頭為正，機(jī)體低頭為負(fù)。當(dāng)無(wú)人機(jī)接收到抬頭指令時(shí)，1號(hào)、6號(hào)電機(jī)轉(zhuǎn)速增大，3號(hào)、4號(hào)電機(jī)轉(zhuǎn)速減小，產(chǎn)生使機(jī)體抬頭的力矩；收到低頭指令時(shí)，3號(hào)、4號(hào)電機(jī)轉(zhuǎn)速增大，1號(hào)、6號(hào)電機(jī)轉(zhuǎn)速減小，產(chǎn)生使機(jī)體低頭的力矩。

(2) 滾轉(zhuǎn)運(yùn)動(dòng)：規(guī)定向右滾轉(zhuǎn)為正，向左滾轉(zhuǎn)為負(fù)。當(dāng)無(wú)人機(jī)接收到右滾轉(zhuǎn)指令時(shí)，4號(hào)、5號(hào)、6號(hào)電機(jī)轉(zhuǎn)速增大，同時(shí)1號(hào)、2號(hào)、3號(hào)電機(jī)轉(zhuǎn)速減小，產(chǎn)生使機(jī)體右滾轉(zhuǎn)的力矩；收到左滾轉(zhuǎn)指令時(shí)，1號(hào)、2號(hào)、3號(hào)電機(jī)轉(zhuǎn)速增大，4號(hào)、5號(hào)、6號(hào)電機(jī)轉(zhuǎn)速減小，產(chǎn)生使機(jī)體向左滾轉(zhuǎn)的力矩。

(3) 航運(yùn)動(dòng)：規(guī)定向右偏航為正，向左偏航為負(fù)。當(dāng)無(wú)人機(jī)接收到右偏航指令時(shí)，1號(hào)、3號(hào)、5號(hào)電機(jī)轉(zhuǎn)速增大，2號(hào)、4號(hào)、6號(hào)電機(jī)轉(zhuǎn)速減小，產(chǎn)生使機(jī)體右偏航的力矩；當(dāng)接收到左偏航指令時(shí)，2號(hào)、4號(hào)、6號(hào)電機(jī)轉(zhuǎn)速增大，1號(hào)、3號(hào)、5號(hào)電機(jī)轉(zhuǎn)速減小，產(chǎn)生使機(jī)體左偏航的力矩。

(4) 升降運(yùn)動(dòng)：規(guī)定上升為正，下降為負(fù)。當(dāng)無(wú)人機(jī)接收到上升指令時(shí)，所有電機(jī)轉(zhuǎn)速增大，當(dāng)升力大于重力時(shí)，無(wú)人機(jī)上升；當(dāng)無(wú)人機(jī)接收到下降指令時(shí)，所有電機(jī)轉(zhuǎn)速減小，升力小于重力，無(wú)人機(jī)下降。

2 六旋翼動(dòng)力學(xué)建模

2.1 建模假設(shè)條件

(1) 六旋翼是剛體；

(2) 六旋翼的質(zhì)量和轉(zhuǎn)動(dòng)慣量不變；

(3) 六旋翼的幾何中心和重心一致；

(4) 六旋翼只受重力和旋翼拉力，其中旋翼拉力沿zb軸向下，重力沿ze方向；

(5) 奇數(shù)標(biāo)號(hào)的旋翼槳葉逆時(shí)針轉(zhuǎn)動(dòng)，偶數(shù)符號(hào)的旋翼槳葉順時(shí)針轉(zhuǎn)動(dòng)[5]。

2.2 旋轉(zhuǎn)矩陣的形成

地球坐標(biāo)系e按照zyx順序旋轉(zhuǎn)到地球坐標(biāo)系b，3次旋轉(zhuǎn)分別對(duì)應(yīng)一個(gè)基本旋轉(zhuǎn)矩陣：

第1次為地球坐標(biāo)系繞z軸旋轉(zhuǎn)ψ，得b1坐標(biāo)系，對(duì)應(yīng)基本旋轉(zhuǎn)矩陣為：

(1)

第2次為b1坐標(biāo)系繞其y軸旋轉(zhuǎn)θ，得b2坐標(biāo)系，對(duì)應(yīng)基本旋轉(zhuǎn)矩陣為：

(2)

第3次為b2坐標(biāo)系繞其x軸旋轉(zhuǎn)φ，得到b坐標(biāo)系，對(duì)應(yīng)基本旋轉(zhuǎn)矩陣為：

(3)

(4)

2.3 運(yùn)動(dòng)學(xué)模型

(5)

式中：ev為六旋翼在地球系下的速度矢量；Θ為姿態(tài)角；bw為機(jī)體角速度。

2.4 位置動(dòng)力學(xué)模型

設(shè)旋翼轉(zhuǎn)速為Ωi(i=1,2,…6),產(chǎn)生的拉力為Ti，其關(guān)系如下[9]：

(6)

式中：Kt為拉力系數(shù)。

(7)

(8)

由牛頓第二定律，對(duì)六旋翼進(jìn)行受力分析，在速度較小的情況下忽略阻力，可得:

mev·=eT-G

(9)

式中：G為重力。展開可得:

(10)

整理得:

(11)

2.5 姿態(tài)動(dòng)力學(xué)模型

在機(jī)體坐標(biāo)系下建立六旋翼姿態(tài)動(dòng)力學(xué)方程如下:

Ibw·=-bw×(Ibw)+τ+Ga

(12)

由文獻(xiàn)[7]可知：

(13)

其中，如圖2所示:l1=l2=l3=l4=l5=l6=l為機(jī)體中心到電機(jī)的距離，αi(i=1,2…5,6)表示機(jī)臂與xb軸正方向之間的夾角，Km為反扭距系數(shù)，則反扭距大小表示為：

圖2 六旋翼機(jī)架布局參數(shù)

(14)

采用小角度假設(shè)，W≈I3，可以得到以下關(guān)系：

(15)

將式(13)～(15)代入式(12)展開可得：

(16)

2.6 模型線性化

六旋翼的動(dòng)力學(xué)模型已經(jīng)建立，為了方便控制器的設(shè)計(jì)，對(duì)非線性模型進(jìn)行簡(jiǎn)化和線性化。因此忽略式(13)中的-bw×(Ibw)+Ga，得到如下簡(jiǎn)化模型：

(17)

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

經(jīng)典PID控制器不需要特定的模型參數(shù)，易于實(shí)現(xiàn),是目前使用最多的控制算法[8]。PID控制器由比例單元P、積分單元I、微分單元D組成，分別對(duì)應(yīng)目前誤差，過去累計(jì)誤差和未來(lái)誤差。

六旋翼控制器由外環(huán)的位置控制和內(nèi)環(huán)的姿態(tài)控制組成。位置控制利用期望的位置解算得到期望的滾轉(zhuǎn)角、期望的俯仰角和期望的拉力。姿態(tài)控制利用期望的姿態(tài)角解算出期望的滾轉(zhuǎn)力矩、俯仰力矩和偏航力矩。

3.1 位置控制

位置控制原理是由期望的位置解算得到期望的滾轉(zhuǎn)角、俯仰角和期望的拉力[10]。

假設(shè)期望位置為pd=[xd,yd,zd]T測(cè)量值為p=[x,y,z]T，則位置誤差可以表示為ex=x-xd,ey=y-yd,ez=z-zd，控制器如下:

(18)

3.2 姿態(tài)控制

姿態(tài)控制利用期望的俯仰角θd、滾轉(zhuǎn)角φd、偏航角ψd解算出期望的俯仰力矩、滾轉(zhuǎn)力矩、偏航力矩，而期望的姿態(tài)角由位置控制模塊可以得到。

(19)

4 仿真分析

在MATLAB/Simulink環(huán)境中建立仿真模型如圖3，包含位置控制模塊，姿態(tài)控制模塊六旋翼動(dòng)力學(xué)模塊。六旋翼參數(shù)如表1所列。

表1 六旋翼無(wú)人機(jī)參數(shù)

圖3 六旋翼控制仿真模型

給定目標(biāo)位置(1，2，3)，初始位置為(0，0，0)，仿真結(jié)果如圖4～6。

圖4 x方向位置變化

圖6 z方向位置變化

根據(jù)仿真結(jié)果可知，六旋翼從起飛到指定懸停位置用時(shí)10 s左右，響應(yīng)速度符合預(yù)期。x方向的超調(diào)量為7.5%，y方向超調(diào)量為6.4%，z方向超調(diào)量為3.7%，均在可接受范圍內(nèi)；穩(wěn)態(tài)誤差約為0，表示能懸停在指定位置上。仿真驗(yàn)證了PID控制的快速性及精確性。

5 結(jié) 語(yǔ)

建立了六旋翼無(wú)人機(jī)的動(dòng)力學(xué)模型，并且對(duì)非線性模型進(jìn)行了線性化處理，為了驗(yàn)證模型的正確性，設(shè)計(jì)了位置PID控制器和姿態(tài)PID控制器。在MATLAB/Simulink中的仿真結(jié)果，實(shí)現(xiàn)了對(duì)六旋翼無(wú)人機(jī)的定點(diǎn)懸停控制，驗(yàn)證了動(dòng)力學(xué)模型的正確性和PID控制器的有效性，對(duì)后續(xù)控制系統(tǒng)的實(shí)驗(yàn)具有參考意義。