許江陰，趙宏強(qiáng),2，鄧 宇,2

(1.中南大學(xué) 高性能復(fù)雜制造國(guó)家重點(diǎn)實(shí)驗(yàn)室，長(zhǎng)沙 410083； 2.山河智能裝備股份有限公司，長(zhǎng)沙 410100)

四旋翼無(wú)人機(jī)可視化軌跡跟蹤仿真系統(tǒng)

許江陰1，趙宏強(qiáng)1,2，鄧 宇1,2

(1.中南大學(xué) 高性能復(fù)雜制造國(guó)家重點(diǎn)實(shí)驗(yàn)室，長(zhǎng)沙 410083； 2.山河智能裝備股份有限公司，長(zhǎng)沙 410100)

為了便于對(duì)四旋翼無(wú)人機(jī)控制算法進(jìn)行實(shí)驗(yàn)仿真和驗(yàn)證，聯(lián)合Solidworks和Matlab/SimMechanics工具箱設(shè)計(jì)了一種四旋翼無(wú)人機(jī)可視化軌跡跟蹤仿真系統(tǒng)；利用Solidworks搭建了四旋翼無(wú)人機(jī)三維實(shí)體模型，并通過(guò)Solidworks和Matlab轉(zhuǎn)換接口將該實(shí)體模型導(dǎo)入到Matlab/SimMechanics中；Matlab/SimMechanics提供了了三維可視化窗口，可以顯示無(wú)人機(jī)的實(shí)時(shí)仿真狀態(tài)；仿真平臺(tái)在Matlab/SimMechanics環(huán)境中實(shí)現(xiàn)，與Matlab/Simulink通信方便，可方便的將Simulink設(shè)計(jì)好的控制算法添加到仿真系統(tǒng)中，以進(jìn)行驗(yàn)證和參數(shù)整定，還具有姿態(tài)分析和數(shù)據(jù)分析等功能；軌跡跟蹤仿真結(jié)果表明，四旋翼無(wú)人機(jī)可視化軌跡跟蹤仿真系統(tǒng)直觀可視，準(zhǔn)確可靠，能較好地對(duì)控制算法進(jìn)行研究和測(cè)試，對(duì)四旋翼無(wú)人機(jī)以及控制算法的研究和開發(fā)具有重要價(jià)值。

四旋翼無(wú)人機(jī)；可視化仿真系統(tǒng)；實(shí)體建模；軌跡跟蹤

0 引言

隨著新型材料、微慣導(dǎo)、微機(jī)電及飛行控制等技術(shù)不斷發(fā)展，多旋翼無(wú)人機(jī)以其結(jié)構(gòu)簡(jiǎn)單、機(jī)動(dòng)性強(qiáng)、飛行方式獨(dú)特引起了廣泛關(guān)注，成為國(guó)際上研究的熱點(diǎn)之一[1]。本文所研究的四旋翼無(wú)人機(jī)是多旋翼無(wú)人機(jī)中的一種，具有體積小，重量輕，結(jié)構(gòu)緊湊對(duì)稱等優(yōu)點(diǎn)。四旋翼無(wú)人機(jī)是一個(gè)具有六個(gè)空間自由度和4個(gè)輸入的非線性欠驅(qū)動(dòng)多輸入多輸出系統(tǒng)，其控制算法的設(shè)計(jì)一直是國(guó)內(nèi)外的研究熱點(diǎn)[2-3]。許多控制算法都在四旋翼無(wú)人機(jī)上有所應(yīng)用，如PID控制[4-5]、滑?？刂芠6]、神經(jīng)網(wǎng)絡(luò)控制[7]、反步法[8-9]、模糊控制[10-11]等。對(duì)控制算法的仿真驗(yàn)證是四旋翼無(wú)人機(jī)試飛前最重要的一部分，目前很多研究都采用數(shù)值數(shù)據(jù)進(jìn)行仿真，無(wú)法直觀的表示無(wú)人機(jī)的實(shí)時(shí)位置和姿態(tài)，因此，設(shè)計(jì)一種可以在四旋翼無(wú)人機(jī)試飛前進(jìn)行可視化仿真，降低開發(fā)成本，提高控制算法開發(fā)效率[12-13]的三維可視化仿真系統(tǒng)引起了研究者的廣泛興趣。殷強(qiáng)等人[12]結(jié)合FlightGear模擬飛行軟件和Mtalab/Simulink建立四旋翼無(wú)人機(jī)可視化系統(tǒng)。周德新等人[13]通過(guò)C++語(yǔ)言設(shè)計(jì)了一個(gè)用于驗(yàn)證飛行控制系統(tǒng)的全數(shù)字仿真平臺(tái)，可視化顯示窗口采用了民航飛機(jī)中的FPD(Primary Flight Display，主飛行顯示器)界面進(jìn)行顯示。

本文開發(fā)了一種基于Solidworks和Matlab/SimMechanics的四旋翼無(wú)人機(jī)可視化軌跡跟蹤仿真系統(tǒng)，首先，通過(guò)物理建模的方式對(duì)四旋翼無(wú)人機(jī)進(jìn)行可視化建模，然后在Simulink里搭建軌跡跟蹤控制模塊,對(duì)四旋翼無(wú)人機(jī)PID控制算法進(jìn)行可視化軌跡跟蹤仿真分析。該仿真系統(tǒng)可以在四旋翼無(wú)人機(jī)進(jìn)行試飛之前，模擬實(shí)際飛行狀態(tài)，即在可視化實(shí)驗(yàn)環(huán)境條件下對(duì)飛控算法的可靠性進(jìn)行驗(yàn)證和參數(shù)調(diào)整，對(duì)四旋翼無(wú)人機(jī)的實(shí)際飛行狀態(tài)進(jìn)行評(píng)估，提高調(diào)試效率，減少實(shí)際試飛中由于控制算法錯(cuò)誤帶來(lái)的不必要損失，保證實(shí)際飛行試驗(yàn)的安全。

1 四旋翼無(wú)人機(jī)的運(yùn)動(dòng)學(xué)分析

四旋翼無(wú)人機(jī)通過(guò)控制4個(gè)電機(jī)的轉(zhuǎn)速來(lái)改變螺旋槳的速度，從而控制四旋翼無(wú)人機(jī)的飛行姿態(tài)和位置。四旋翼無(wú)人機(jī)有十字型和X型兩種結(jié)構(gòu)，本文研究的是X型結(jié)構(gòu)的四旋翼無(wú)人機(jī)，機(jī)頭方向?yàn)?號(hào)和3號(hào)電機(jī)夾角平分線的正前方，其中 1號(hào)和2號(hào)電機(jī)逆時(shí)針轉(zhuǎn)動(dòng)，3號(hào)和4號(hào)電機(jī)順時(shí)針轉(zhuǎn)動(dòng)，規(guī)定順時(shí)針?lè)较驗(yàn)檎?，如圖1所示。對(duì)四旋翼無(wú)人機(jī)進(jìn)行運(yùn)動(dòng)學(xué)分析，首先對(duì)四旋翼無(wú)人機(jī)做出如下假設(shè)：

(1)四旋翼無(wú)人機(jī)為完全對(duì)稱的剛體，忽略其結(jié)構(gòu)和彈性變形；

(2)機(jī)體坐標(biāo)系與四旋翼無(wú)人機(jī)的質(zhì)心重合；

(3)假設(shè)地面坐標(biāo)系為慣性坐標(biāo)系。

圖1 X型四旋翼無(wú)人機(jī)結(jié)構(gòu)圖

在機(jī)體坐標(biāo)系里，單個(gè)電機(jī)驅(qū)動(dòng)旋翼所產(chǎn)生的升力為Fi=kbwi2，力矩為Ti=kdwi2，其中i=(1,2,3,4)，kb表示升力系數(shù)，kd為轉(zhuǎn)矩系數(shù)，wi為第i個(gè)電機(jī)的角速度。定義歐拉角η=[φθψ]T，φ為滾轉(zhuǎn)角，θ為俯仰角，ψ為偏航角,無(wú)人機(jī)角速度Ω=[p,q,r]，忽略空氣阻力，體坐標(biāo)系下四旋翼無(wú)人機(jī)受到的推力Fb：

(1)

Fb通過(guò)轉(zhuǎn)換矩陣R轉(zhuǎn)換到慣性坐標(biāo)系下無(wú)人機(jī)受力Fg:

Fg=RFb-mgEz

(2)

R為從體坐標(biāo)系到慣性坐標(biāo)系的轉(zhuǎn)換矩陣：

R=

(3)

在慣性坐標(biāo)系下，由牛頓第二定律F=ma，結(jié)合(1)、(2)、(3)式可得線性位移方程：

(4)

(5)

(6)

當(dāng)姿態(tài)角變化較小時(shí)認(rèn)為式(6)可以簡(jiǎn)化為:

(7)

由(5)和(7)式可得:

(8)

取:

(9)

其中:l為電機(jī)中心到機(jī)體坐標(biāo)系的距離:

則:

Mx=u2,My=u3,Mz=u4

(10)

由式(4)、(8)、(9)、(10)可得四旋翼無(wú)人機(jī)的運(yùn)動(dòng)學(xué)方程:

(11)

2 Solidworks和Matlab可視化聯(lián)合仿真系統(tǒng)設(shè)計(jì)

2.1 搭建四旋翼無(wú)人機(jī)可視化仿真模型

Solidworks是Windows環(huán)境下的三維實(shí)體設(shè)計(jì)軟件，Solidworks工作界面簡(jiǎn)潔直觀，可以隨時(shí)修改特征，來(lái)改變零部件的形狀和尺寸，并且在裝配的環(huán)境里，可以方便地設(shè)計(jì)和修改零部件，具有強(qiáng)大的實(shí)體建模功能。同時(shí)，Matlab下的SimMechanics功能工具箱里提供了大量模塊，使用這些模塊可以方便的建立機(jī)構(gòu)運(yùn)動(dòng)的模型，不僅可以進(jìn)行機(jī)械系統(tǒng)的單獨(dú)分析，還可以與Simulink設(shè)計(jì)的控制器及其它動(dòng)態(tài)系統(tǒng)相連進(jìn)行綜合仿真以及控制策略的研究[14-16],但SimMechanics在三維建模方面功能有限，因此，本文根據(jù)現(xiàn)有的四旋翼無(wú)人機(jī)的形狀和尺寸，先用Solidworks對(duì)四旋翼無(wú)人機(jī)進(jìn)行三維實(shí)體建模，然后再導(dǎo)入到Matlab下的SimMechanics中，并與設(shè)計(jì)的控制器進(jìn)行連接。因?yàn)樗男頍o(wú)人機(jī)是通過(guò)4個(gè)電機(jī)來(lái)控制無(wú)人機(jī)的運(yùn)動(dòng)，所以為了使生成的SimMechanics模型更加簡(jiǎn)潔，將四旋翼無(wú)人機(jī)分為機(jī)體和4個(gè)電機(jī)-螺旋槳五部分，這就需要將四旋翼無(wú)人機(jī)在Solidworks中按照這五部分進(jìn)行裝配。

Solidworks導(dǎo)入到Matlab的步驟：

建立Solidworks和Matlab接口，在Solidworks安裝SimMechanicsLink插件，然后用Solidworks打開建立的四旋翼無(wú)人機(jī)模型.SLDASM，如圖2，輸出為二代SimMechanics的.xml文件。

圖2 四旋翼無(wú)人機(jī)Solidworks模型

打開Matlab，輸入smimport(‘.xml文件名’)，將生成四旋翼無(wú)人機(jī)的SimMechanics模型，如圖3，其可視化模型如圖4，從圖中可以看出整個(gè)機(jī)構(gòu)由地坐標(biāo)系、機(jī)體和4個(gè)電機(jī)-螺旋槳模塊組成，其中機(jī)體與電機(jī)-螺旋槳通過(guò)單自由度模塊連接。

SimMechanics模型可以與Simulink的控制系統(tǒng)模型方便的結(jié)合，SimMechanics提供了驅(qū)動(dòng)模塊和傳感器模塊與Simulink的其他模塊進(jìn)行信號(hào)傳遞，在同一個(gè)環(huán)境中對(duì)控制器和受控對(duì)象建模，進(jìn)行仿真分析。由于四旋翼無(wú)人機(jī)具有6個(gè)空間自由度，所以在地坐標(biāo)系與機(jī)體、4個(gè)電機(jī)-螺旋槳之間添加六自由度模塊，并在六自由度模塊上添加位置和角度傳感器，修改模塊里的重力方向使之與仿真圖中的z軸方向一致，對(duì)模型中的4個(gè)單自由度模塊添加所需的驅(qū)動(dòng)力矩模塊以及由力矩產(chǎn)生的轉(zhuǎn)速傳感器模塊，并將其傳遞給對(duì)應(yīng)的螺旋槳，帶動(dòng)螺旋槳轉(zhuǎn)動(dòng)，此時(shí)Matlab里的四旋翼的無(wú)人機(jī)SimMechanics模型已搭建好，如圖5。

圖3 四旋翼無(wú)人機(jī)SimMechanics模型

圖4 四旋翼無(wú)人機(jī)SimMechanics可視化模型

2.2 設(shè)計(jì)PID控制模塊

因?yàn)镻ID控制具有結(jié)構(gòu)簡(jiǎn)單、調(diào)整方便、控制可靠、穩(wěn)定性好等優(yōu)點(diǎn)，在工業(yè)控制上具有廣泛的應(yīng)用，所以本文采用PID控制算法控制四旋翼無(wú)人機(jī)的軌跡跟蹤。PID控制由比例、積分、微分3個(gè)單元組成，PID控制規(guī)律就是對(duì)偏差e(t)進(jìn)行比例、積分、微分變換的控制規(guī)律，表示為：

圖5 已添加驅(qū)動(dòng)的四旋翼無(wú)人機(jī)SimMechanics模型

(12)

其中為比例控制項(xiàng)，為比例系數(shù)；為積分控制項(xiàng)，為積分時(shí)間常數(shù)；為微分控制項(xiàng)，為微分時(shí)間常數(shù)。

本文采用內(nèi)外環(huán)PID控制算法控制四旋翼無(wú)人機(jī)的飛行姿態(tài)，內(nèi)環(huán)采用PID控制無(wú)人機(jī)的姿態(tài)角，外環(huán)采用PD控制無(wú)人機(jī)的位置。系統(tǒng)輸入量為目標(biāo)的期望位置x1,y1,z1，期望偏航角ψ1，由位置控制器計(jì)算出期望的滾轉(zhuǎn)角φ1，俯仰角θ1，根據(jù)期望的姿態(tài)角和高度由姿態(tài)控制器和運(yùn)動(dòng)學(xué)逆解計(jì)算出四旋翼無(wú)人機(jī)4個(gè)電機(jī)所需要的轉(zhuǎn)矩控制量，輸入到四旋翼無(wú)人機(jī)模型中，接下來(lái)在Matlab的Simulink模塊里搭建四旋翼無(wú)人機(jī)的軌跡跟蹤控制模塊并在可視化仿真系統(tǒng)里對(duì)其進(jìn)行驗(yàn)證，四旋翼無(wú)人機(jī)軌跡跟蹤系統(tǒng)的Simulink仿真模型，如圖6。

圖6 四旋翼無(wú)人機(jī)軌跡跟蹤控制Simulink框圖

3 四旋翼無(wú)人機(jī)軌跡跟蹤系統(tǒng)仿真分析

為驗(yàn)證四旋翼無(wú)人機(jī)可視化軌跡跟蹤系統(tǒng)的可靠性，對(duì)所設(shè)計(jì)的PID軌跡跟蹤控制算法進(jìn)行仿真，四旋翼無(wú)人機(jī)的初始位置為：x=0,y=0,z=0,期望的輸入軌跡為：x1=cos(t),y1=sin(t),z1=1,ψ1=0，輸出為四旋翼無(wú)人機(jī)的實(shí)際軌跡為x,y,z,ψ,仿真結(jié)果如圖7～11，圖7為四旋翼無(wú)人機(jī)的可視化運(yùn)動(dòng)仿真圖，其中圖(a)為四旋翼無(wú)人機(jī)的側(cè)視圖，圖(b)為四旋翼無(wú)人機(jī)在2s時(shí)的俯視圖，圖(c)為四旋翼無(wú)人機(jī)在4s時(shí)的俯視圖，圖(d)為四旋翼無(wú)人機(jī)在6s時(shí)的俯視圖。

圖7 四旋翼無(wú)人機(jī)可視化運(yùn)動(dòng)仿真圖

圖8為X軸方向的軌跡跟蹤曲線，由圖可知在t=0時(shí)刻，在X軸上的目標(biāo)坐標(biāo)與實(shí)際坐標(biāo)相差1 m，經(jīng)過(guò)2s后基本能的跟蹤X軸上的軌跡變化，圖9為Y軸方向的軌跡跟蹤曲線，由圖可知在起飛穩(wěn)定后也能基本的跟蹤上Y軸的軌跡變化，圖10為高度方向的跟蹤曲線，由圖可知在1 s左右高度有一個(gè)超調(diào)量，3 s左右恢復(fù)到目標(biāo)位置，跟蹤誤差基本為零。圖11為偏航角跟蹤曲線，由圖可知?jiǎng)傞_始偏航角有波動(dòng)，在3 s左右恢復(fù)正常。整個(gè)仿真過(guò)程在仿真系統(tǒng)里直觀可視，效果逼真，數(shù)據(jù)處理清晰明確。

圖8 X軸方向的軌跡跟蹤曲線

圖9 Y軸方向的軌跡跟蹤曲線

圖10 高度方向的跟蹤曲線

4 結(jié)論

本文基于Solidworks強(qiáng)大的三維建模功能以及Solidworks和Matlab/SimMechanics的轉(zhuǎn)換接口，快速建立四旋翼無(wú)人機(jī)的SimMechanics可視化模型，結(jié)合Simulink里設(shè)計(jì)的軌跡跟蹤控制器，搭建四旋翼無(wú)人機(jī)可視化軌跡跟蹤仿真系統(tǒng)。四旋翼無(wú)人機(jī)可視化軌跡跟蹤仿真系統(tǒng)具備實(shí)體機(jī)控制算法仿真及分析功能，可以直觀地顯示四旋翼無(wú)人機(jī)的實(shí)時(shí)飛行狀態(tài)，使用方便，結(jié)果逼真。四旋翼無(wú)人機(jī)可視化軌跡跟蹤仿真系統(tǒng)能

圖11 偏航角跟蹤曲線

較好的對(duì)控制算法進(jìn)行可視化動(dòng)態(tài)仿真與驗(yàn)證，提高四旋翼無(wú)人機(jī)控制算法的研究效率，降低開發(fā)成本。

[1] 蘇哲宇.一種微小型四旋翼飛行器控制系統(tǒng)的設(shè)計(jì)與開發(fā)[D].沈陽(yáng)：東北大學(xué),2012.

[2] 楊明志,王 敏.四旋翼微型飛行器控制系統(tǒng)設(shè)計(jì)[J].計(jì)算機(jī)測(cè)量與控制, 2008, 16(4):485-487.

[3] 王 璐.四旋翼無(wú)人飛行器控制技術(shù)研究[D].哈爾濱：哈爾濱工程大學(xué),2012.

[4]SalihAL,MoghavvemiM,MohamedHAF,etal.ModellingandPIDcontrollerdesignforaquadrotorunmannedairvehicle[A].IEEEInternationalConferenceonAutomationQualityandTestingRobotics[C].IEEE, 2010:1-5.

[5] 周紅坤.微型旋翼無(wú)人機(jī)航跡規(guī)劃與航跡跟蹤控制系統(tǒng)設(shè)計(jì)[D].成都：電子科技大學(xué), 2015.

[6] 賈燕波,楊 峰,秦緒菲.基于滑?？刂频乃男盹w行器控制算法研究[J].山東師范大學(xué)學(xué)報(bào)(自然科學(xué)版),2014(4).

[7] 黃曉雷,錢正祥,趙 博,等.神經(jīng)網(wǎng)絡(luò)直接逆控制在無(wú)人機(jī)飛控系統(tǒng)中的應(yīng)用研究[J]. 計(jì)算機(jī)測(cè)量與控制, 2008, 16(10):1448-1450.

[8]HuangM,XianB,DiaoC,etal.Adaptivetrackingcontrolofunderactuatedquadrotorunmannedaerialvehiclesviabackstepping[C].IEEE, 2010:2076-2081.

[9]SteinbergM.NonlinearAdaptiveFlightControlwithaBacksteppingDesignApproach[J]. 1998.

[10] 段鳳陽(yáng),陳 鵬,郝 爽.無(wú)人機(jī)姿態(tài)控制器設(shè)計(jì)及仿真[J].計(jì)算機(jī)測(cè)量與控制, 2015, 23(5):1571-1574.

[11] 楊恩泉,高金源.無(wú)人機(jī)機(jī)動(dòng)軌跡跟蹤系統(tǒng)設(shè)計(jì)[J]. 飛行力學(xué), 2007, 25(2):30-33.

[12] 殷 強(qiáng),鮮 斌,張 垚,等.四旋翼無(wú)人機(jī)可視化仿真系統(tǒng)研究[A].中國(guó)自動(dòng)化學(xué)會(huì)控制理論專業(yè)委員會(huì)C卷[C]. 2011.

[13] 周德新,馬騰達(dá).四旋翼無(wú)人機(jī)飛控系統(tǒng)仿真平臺(tái)研究[J]. 計(jì)算機(jī)測(cè)量與控制, 2014, 22(2):424-426.

[14] 楊樹川,邵金龍,楊術(shù)明,等.基于Solidworks&SimMechanics的機(jī)構(gòu)運(yùn)動(dòng)分析與仿真[J]. 河北工業(yè)科技, 2011, 28(4):221-225.

[15] 陳愛(ài)波,寧淑榮,陳五一.基于MATLAB與SolidWorks的六自由度機(jī)器人聯(lián)合建模仿真[J].機(jī)電技術(shù),2014(6):57-60.

[16] 姚太克.一類三自由度并聯(lián)機(jī)構(gòu)的特性研究與優(yōu)化設(shè)計(jì)[D].合肥：中國(guó)科學(xué)技術(shù)大學(xué), 2013.

Visual Trajectory Tracking Simulation System for Quadrotor UAV

Xu Jiangyin1,Zhao Hongqiang1,2, Deng Yu1,2

(1.Central South University, Changsha 410083, China;2.Sunward Intelligent Equipment CO., Ltd., Changsha 410100, China)

In order to facilitate the simulation and verification of the quadrotor UAV control algorithm，combination of Solidworks and Matlab/SimMechanics toolbox designed a quadrotor UAV visual trajectory tracking simulation system. Using Solidworks built a quadrotor UAV three-dimensional entity model and imported the model into Matlab/SimMechanics through Solidworks and Matlab conversion interface. Matlab / SimMechanics provides a three-dimensional visualization window, and can display the status of real-time simulation of UAV. Simulation platform is realized in Matlab/SimMechanics environment, and easy to communicate with Matlab/Simulink, and can added control algorithm designed by Simulink to simulation system easily for verification and parameter tuning, also has posture analysis and data analysis and other functions. The result of trajectory tracking simulation show that quadrotor UAV visual trajectory tracking simulation system is intuitive and visual, accurate and reliable, and can research and test of control algorithm. It has important value for the research and development of the quadrotor UAV and control algorithm.

quadrotor UAV; visual simulation system；entity modeling; trajectory tracking

2016-06-30；

2016-11-02。

國(guó)家科技支撐計(jì)劃(2014BAD06B07-1); 湖南省軍民融合專項(xiàng)。

許江陰(1990-)，女，河南商丘人，碩士研究生，主要從事控制理論與工程方向的研究。

1671-4598(2017)03-0130-04

10.16526/j.cnki.11-4762/tp.2017.03.036

TP391.9

A