四旋翼無人機可視化軌跡跟蹤仿真系統

許江陰，趙宏強,2，鄧 宇,2

(1.中南大學 高性能復雜制造國家重點實驗室，長沙 410083； 2.山河智能裝備股份有限公司，長沙 410100)

四旋翼無人機可視化軌跡跟蹤仿真系統

許江陰1，趙宏強1,2，鄧 宇1,2

(1.中南大學 高性能復雜制造國家重點實驗室，長沙 410083； 2.山河智能裝備股份有限公司，長沙 410100)

為了便于對四旋翼無人機控制算法進行實驗仿真和驗證，聯合Solidworks和Matlab/SimMechanics工具箱設計了一種四旋翼無人機可視化軌跡跟蹤仿真系統；利用Solidworks搭建了四旋翼無人機三維實體模型，并通過Solidworks和Matlab轉換接口將該實體模型導入到Matlab/SimMechanics中；Matlab/SimMechanics提供了了三維可視化窗口，可以顯示無人機的實時仿真狀態；仿真平臺在Matlab/SimMechanics環境中實現，與Matlab/Simulink通信方便，可方便的將Simulink設計好的控制算法添加到仿真系統中，以進行驗證和參數整定，還具有姿態分析和數據分析等功能；軌跡跟蹤仿真結果表明，四旋翼無人機可視化軌跡跟蹤仿真系統直觀可視，準確可靠，能較好地對控制算法進行研究和測試，對四旋翼無人機以及控制算法的研究和開發具有重要價值。

四旋翼無人機；可視化仿真系統；實體建模；軌跡跟蹤

0 引言

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

本文開發了一種基于Solidworks和Matlab/SimMechanics的四旋翼無人機可視化軌跡跟蹤仿真系統，首先，通過物理建模的方式對四旋翼無人機進行可視化建模，然后在Simulink里搭建軌跡跟蹤控制模塊,對四旋翼無人機PID控制算法進行可視化軌跡跟蹤仿真分析。該仿真系統可以在四旋翼無人機進行試飛之前，模擬實際飛行狀態，即在可視化實驗環境條件下對飛控算法的可靠性進行驗證和參數調整，對四旋翼無人機的實際飛行狀態進行評估，提高調試效率，減少實際試飛中由于控制算法錯誤帶來的不必要損失，保證實際飛行試驗的安全。

1 四旋翼無人機的運動學分析

四旋翼無人機通過控制4個電機的轉速來改變螺旋槳的速度，從而控制四旋翼無人機的飛行姿態和位置。四旋翼無人機有十字型和X型兩種結構，本文研究的是X型結構的四旋翼無人機，機頭方向為1號和3號電機夾角平分線的正前方，其中 1號和2號電機逆時針轉動，3號和4號電機順時針轉動，規定順時針方向為正，如圖1所示。對四旋翼無人機進行運動學分析，首先對四旋翼無人機做出如下假設：

(1)四旋翼無人機為完全對稱的剛體，忽略其結構和彈性變形；

(2)機體坐標系與四旋翼無人機的質心重合；

(3)假設地面坐標系為慣性坐標系。

圖1 X型四旋翼無人機結構圖

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

(1)

Fb通過轉換矩陣R轉換到慣性坐標系下無人機受力Fg:

Fg=RFb-mgEz

(2)

R為從體坐標系到慣性坐標系的轉換矩陣：

R=

(3)

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

(4)

(5)

(6)

當姿態角變化較小時認為式(6)可以簡化為:

(7)

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

(8)

取:

(9)

其中:l為電機中心到機體坐標系的距離:

則:

Mx=u2,My=u3,Mz=u4

(10)

由式(4)、(8)、(9)、(10)可得四旋翼無人機的運動學方程:

(11)

2 Solidworks和Matlab可視化聯合仿真系統設計

2.1 搭建四旋翼無人機可視化仿真模型

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

Solidworks導入到Matlab的步驟：

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

圖2 四旋翼無人機Solidworks模型

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

SimMechanics模型可以與Simulink的控制系統模型方便的結合，SimMechanics提供了驅動模塊和傳感器模塊與Simulink的其他模塊進行信號傳遞，在同一個環境中對控制器和受控對象建模，進行仿真分析。由于四旋翼無人機具有6個空間自由度，所以在地坐標系與機體、4個電機-螺旋槳之間添加六自由度模塊，并在六自由度模塊上添加位置和角度傳感器，修改模塊里的重力方向使之與仿真圖中的z軸方向一致，對模型中的4個單自由度模塊添加所需的驅動力矩模塊以及由力矩產生的轉速傳感器模塊，并將其傳遞給對應的螺旋槳，帶動螺旋槳轉動，此時Matlab里的四旋翼的無人機SimMechanics模型已搭建好，如圖5。

圖3 四旋翼無人機SimMechanics模型

圖4 四旋翼無人機SimMechanics可視化模型

2.2 設計PID控制模塊

因為PID控制具有結構簡單、調整方便、控制可靠、穩定性好等優點，在工業控制上具有廣泛的應用，所以本文采用PID控制算法控制四旋翼無人機的軌跡跟蹤。PID控制由比例、積分、微分3個單元組成，PID控制規律就是對偏差e(t)進行比例、積分、微分變換的控制規律，表示為：

圖5 已添加驅動的四旋翼無人機SimMechanics模型

(12)

其中為比例控制項，為比例系數；為積分控制項，為積分時間常數；為微分控制項，為微分時間常數。

本文采用內外環PID控制算法控制四旋翼無人機的飛行姿態，內環采用PID控制無人機的姿態角，外環采用PD控制無人機的位置。系統輸入量為目標的期望位置x1,y1,z1，期望偏航角ψ1，由位置控制器計算出期望的滾轉角φ1，俯仰角θ1，根據期望的姿態角和高度由姿態控制器和運動學逆解計算出四旋翼無人機4個電機所需要的轉矩控制量，輸入到四旋翼無人機模型中，接下來在Matlab的Simulink模塊里搭建四旋翼無人機的軌跡跟蹤控制模塊并在可視化仿真系統里對其進行驗證，四旋翼無人機軌跡跟蹤系統的Simulink仿真模型，如圖6。

圖6 四旋翼無人機軌跡跟蹤控制Simulink框圖

3 四旋翼無人機軌跡跟蹤系統仿真分析

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

圖7 四旋翼無人機可視化運動仿真圖

圖8為X軸方向的軌跡跟蹤曲線，由圖可知在t=0時刻，在X軸上的目標坐標與實際坐標相差1 m，經過2s后基本能的跟蹤X軸上的軌跡變化，圖9為Y軸方向的軌跡跟蹤曲線，由圖可知在起飛穩定后也能基本的跟蹤上Y軸的軌跡變化，圖10為高度方向的跟蹤曲線，由圖可知在1 s左右高度有一個超調量，3 s左右恢復到目標位置，跟蹤誤差基本為零。圖11為偏航角跟蹤曲線，由圖可知剛開始偏航角有波動，在3 s左右恢復正常。整個仿真過程在仿真系統里直觀可視，效果逼真，數據處理清晰明確。

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

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

圖10 高度方向的跟蹤曲線

4 結論

本文基于Solidworks強大的三維建模功能以及Solidworks和Matlab/SimMechanics的轉換接口，快速建立四旋翼無人機的SimMechanics可視化模型，結合Simulink里設計的軌跡跟蹤控制器，搭建四旋翼無人機可視化軌跡跟蹤仿真系統。四旋翼無人機可視化軌跡跟蹤仿真系統具備實體機控制算法仿真及分析功能，可以直觀地顯示四旋翼無人機的實時飛行狀態，使用方便，結果逼真。四旋翼無人機可視化軌跡跟蹤仿真系統能

圖11 偏航角跟蹤曲線

較好的對控制算法進行可視化動態仿真與驗證，提高四旋翼無人機控制算法的研究效率，降低開發成本。

[1] 蘇哲宇.一種微小型四旋翼飛行器控制系統的設計與開發[D].沈陽：東北大學,2012.

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

[3] 王 璐.四旋翼無人飛行器控制技術研究[D].哈爾濱：哈爾濱工程大學,2012.

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

[5] 周紅坤.微型旋翼無人機航跡規劃與航跡跟蹤控制系統設計[D].成都：電子科技大學, 2015.

[6] 賈燕波,楊 峰,秦緒菲.基于滑模控制的四旋翼飛行器控制算法研究[J].山東師范大學學報(自然科學版),2014(4).

[7] 黃曉雷,錢正祥,趙 博,等.神經網絡直接逆控制在無人機飛控系統中的應用研究[J]. 計算機測量與控制, 2008, 16(10):1448-1450.

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

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

[10] 段鳳陽,陳 鵬,郝 爽.無人機姿態控制器設計及仿真[J].計算機測量與控制, 2015, 23(5):1571-1574.

[11] 楊恩泉,高金源.無人機機動軌跡跟蹤系統設計[J]. 飛行力學, 2007, 25(2):30-33.

[12] 殷 強,鮮 斌,張 垚,等.四旋翼無人機可視化仿真系統研究[A].中國自動化學會控制理論專業委員會C卷[C]. 2011.

[13] 周德新,馬騰達.四旋翼無人機飛控系統仿真平臺研究[J]. 計算機測量與控制, 2014, 22(2):424-426.

[14] 楊樹川,邵金龍,楊術明,等.基于Solidworks&SimMechanics的機構運動分析與仿真[J]. 河北工業科技, 2011, 28(4):221-225.

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

[16] 姚太克.一類三自由度并聯機構的特性研究與優化設計[D].合肥：中國科學技術大學, 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。

國家科技支撐計劃(2014BAD06B07-1); 湖南省軍民融合專項。

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

1671-4598(2017)03-0130-04

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

TP391.9

A