四旋翼動力學建模及非線性PID軌跡跟蹤控制

石喜玲, 孫運強, 李 靜，余紅英

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

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

四旋翼飛行器位姿控制可以通過調節四個螺旋槳的轉速，不需要尾翼，實現懸停、前飛、側飛和倒飛。但是，這些屬性是有代價的，四旋翼飛行器屬于高度欠驅動非線性系統，因此控制器研究成為無人機設計師和研究人員關注更多的問題[2]。

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

基于經典牛頓-歐拉角定律建立四旋翼六自由度剛體運動學模型，根據多旋翼的飛行環境等特點，針對滾轉角和俯仰角都非常小，對非線性模型進行簡化[6-7]，然后采用級聯雙閉環比例-積分-微分(proportional-integral-derivative,PID)控制法進行位置和姿態控制器設計，搭建仿真模型驗證并對控制參數整定調試，在飛控。

1 系統概述與建模

1.1 四旋翼控制剛體模型

四旋翼飛行器有4個旋翼且旋轉平面共面，4個獨立電機分別獨立驅動4個頂點上的螺旋槳且以中心對稱，按轉動方向不同，4個旋翼分為逆時針和順時針兩組[8]。由于兩組旋翼轉動方向相反，所以旋翼的陀螺力矩和反扭矩可被有效抵消。另外通過差動改變同組旋翼的轉速，可以在差動力矩的作用下，實現滾轉和俯仰運動。偏航運動由兩組旋翼產生的反扭力矩之差實現，總升力為4個旋翼產生的升力之和[7]。四旋翼飛行器空間結構如圖1所示。

圖1 四旋翼空間結構和坐標定義

建立四旋翼運動方程有必要確定旋翼機體的參考坐標系。如圖 1所示，通常使用兩個參考坐標系在空間坐標中定義四旋翼，定義如下[6，9]。

機體(或運動物體)坐標系與四旋翼固連，四旋翼質心為坐標原點ob，三維坐標軸表示為xb、yb、zb。

重力指向zb軸方向。描述機體線性位置和角位置的坐標矢量表示為三軸平移速度[u,v,w]T和三軸轉動角速率[p,q,r]T。

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

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

(1)

四旋翼通常被描述為動態的高度非線性系統，具有 6個自由度(3個平移速度和 3個旋轉速度)，由 4個獨立的輸入即四個旋翼驅動，所以四旋翼動力學是一個典型的欠驅動控制系統的，具有參數不確定性[11]。

假設四旋翼飛行器是剛體，結構完全對稱，機體幾何中心和飛行器質心重合，忽略阻力系數，根據牛頓第二定律、動能定理和動量原理可以得到四旋翼飛行器的剛體運動模型和動力學模型:

(2)

(3)

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

1.2 動態模型線性化

通過上述原理推導出四旋翼的數學模型和運動方程本質上是非線性的，傳統上，時不變非線性模型的線性化是獲得更經典的控制器的必要條件[12]。

采用小擾動理論是目前最常用的一種線性化方法[13]。假設在室內對多旋翼的飛行調試中，俯仰角和滾轉角都非常小[8]，即

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

(4)

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

(5)

(6)

2 控制器設計

四旋翼控制器設計采用級聯雙閉環PID控制算法，外環是位置坐標控制，內環為姿態控制，位置控制包含姿態反解算以得到內環所需的期望姿態角。而四旋翼位置坐標改變由歐拉角變化引起，所以姿態控制是四旋翼飛行器控制的核心，其控制性能會極大地影響航行穩定性[14]。

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

設ep=p-pd, 則

(7)

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

(8)

期望姿態角為

(9)

由位置控制模塊產生的兩個姿態角滾轉角φd、俯仰角θd和系統輸入期望偏航角ψd作為姿態控制的輸入期望信號，算法如式(9)所示：

(10)

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

3 仿真分析

利用Simulink搭建了四旋翼系統控制剛體運動學模型，包含系統運動學模型和控制器兩大子系統。圖2為四旋翼級聯雙閉環PID控制器子系統，分為三個模塊，即位置控制模塊、姿態反解算模塊、姿態控制模塊。

四旋翼系統參數如表1所示。經過調試PID控制器參數選擇如表2所示。

圖2 位置和姿態的級聯控制器

表1 四旋翼系統參數

表2 控制器參數

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

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

(10)

如圖3、圖4仿真結果表明：四旋翼在起飛階段位置響應x方向超調為4.3%，y方向超調為7.3%，存在誤差范圍內的振蕩；達到相對穩定后，穩態誤差在x方向0.15%范圍內,y方向穩態誤差在1.4%范圍內；姿態角的最大值控制在0.1 rad之內，系統仿真驗證了 PID 控制的有效性。

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

圖3 定點懸停位置響應

圖4 定點懸停姿態響應

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

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

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

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

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

圖8 高度控制實時采樣數據

4 試驗

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

5 結論

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