基于非線性模型預測的四旋翼無人機軌跡跟蹤控制

閆玉盼，饒 兵，劉硯菊，馮迎賓

(1.沈陽理工大學自動化與電氣工程學院，沈陽 110159；2.沈陽天眼智云智能技術研究院有限公司，沈陽 110179)

近年來，無人機因其敏捷性和實用性，被成功應用于災害搜索救援、警方執法和電力系統檢測等任務中[1-3]。 無人機精準的位置控制是順利完成上述工作任務的基礎，但由于四旋翼無人機具有非線性和強耦合等特點，為四旋翼無人機開發魯棒性強、穩定性高的控制器仍存在巨大挑戰。為此，研究者提出了多種控制方法，如PID、自適應控制、LQR 控制、滑?？刂?、反步控制等。 PID算法原理簡單，不需要建立精確的數學模型，但在雙閉環PID 控制中需要設計18 個參數，數目眾多，調參工作復雜[4]。 文獻[5]針對PID 參數整定問題，將模糊控制理論與PID 控制系統相結合，提出了模糊自適應PID 控制器，實現四旋翼無人機對PID 參數的自適應整定。 LQR 控制將四旋翼無人機的非線性模型線性化，設計閉環最優控制指標，實現位姿控制[6]。 反步控制的抗干擾能力較弱，穩定性差，并且對初始條件的要求較高[7]。 上述線性控制策略雖然在穩定的環境中取得良好的位姿控制效果，但是在實際應用中未充分考慮環境干擾和系統約束等問題，控制效果不佳。

隨著計算機技術的發展，無人機已經配有成熟的自動駕駛儀平臺，如PX4 或Pixhawk 等[8]，計算能力較強、處理器性能高，基于優化的模型預測控制(model predict control，MPC)算法被廣泛用于四旋翼無人機的控制系統[9-10]。 MPC 具有顯式處理系統復雜約束的能力，采用滾動優化、反饋矯正的策略，在線修正每個時刻的預測軌跡，具有較強的抗干擾能力[11]。 Chen 等[12]提出了串級形式的MPC 控制器，將MPC 分別用于位置控制和姿態控制中，可以處理每個控制量的約束范圍，保證四旋翼無人機穩定飛行。 周杰等[13]針對軌跡跟蹤中出現的超調問題，在MPC 的代價函數中引入了偏差變化率，改善了軌跡跟蹤性能。

以上MPC 控制方案都是基于線性化后的四旋翼無人機模型提出的，而實際的四旋翼無人機模型是非線性化的。 因此本文提出基于非線性模型的預測控制(nolinear model predict control，NMPC)方法，以四旋翼無人機非線性動力學模型作為控制器的預測模型，通過設計目標優化函數，將軌跡跟蹤控制問題轉化為非線性最優化在線求解問題，以實現軌跡跟蹤控制。

1 四旋翼無人機動力學模型

準確的四旋翼無人機動力學模型是設計控制器的基礎。 本文選用“X”型四旋翼無人機作為研究對象，假設四旋翼無人機為剛體且結構對稱，其重心在幾何中心處。 為描述四旋翼無人機的位置和姿態，定義地球坐標系OE-XEYEZE和機體坐標系OB-XBYBZB，如圖1 所示。

圖1 機體坐標系與地球坐標系Fig.1 Body coordinate system and earth coordinate system

四旋翼無人機的動力由四個螺旋槳高速旋轉產生[14]，可實現對四旋翼無人機姿態和位置的調整。 在機體坐標系下，UL為沿ZB軸的總拉力，τx、τy、τz為繞機體軸OEXE、OEYE、OEZE的轉動力矩，表示為

式中:?i(i＝1，2，3，4)為四個螺旋槳的轉速；l為螺旋槳中心到四旋翼無人機重心的距離；cT為推力系數；cM為反扭矩系數。

四旋翼無人機具有六個自由度的輸出，本文選用P＝(x，y，z)T和Θ＝(φ，θ，ψ)T分別表示四旋翼無人機在地球坐標系下的位置坐標和歐拉角，其中φ為滾轉角，θ為俯仰角，ψ為偏航角。

根據四旋翼無人機的非線性、強耦合的特點和控制器設計的需要，本文采用Euler-Lagrange 方程推導得到四旋翼無人機的非線性動力學模型[15]，如式(3)所示。

式中:為四旋翼無人機的位置加速度；為四旋翼無人機的姿態角加速度；為四旋翼無人機的姿態角速度；m為四旋翼無人機的質量；g為重力加速度；Ixx、Iyy、Izz分別為四旋翼無人機繞XE軸、YE軸、ZE軸的轉動慣量。

2 NMPC 控制器設計

2.1 基本原理

模型預測是一種基于在線優化的控制策略，在最優控制中具有約束系統變量和滾動優化的優點。 本文設計的NMPC 控制系統結構如圖2 所示，NMPC 控制器包括預測模型、滾動優化和反饋校正三部分。

圖2 NMPC 控制框圖Fig.2 NMPC control block diagram

首先設預測時域為N，控制時域為W。 在當前采樣時刻k，NMPC 控制器結合預測模型、目標函數和約束進行開環最優化求解，得到當前時刻的最優控制序列U(k)、U(k+1)、…、U(k+W-1)，取其第一個控制量U(k)作為四旋翼無人機的控制輸入，并將四旋翼無人機的輸出Y(k+1)傳到反饋校正模塊，完成閉環控制。 反饋校正模塊將預測模型輸出的預測值Xpre(k+1)和四旋翼無人機實際輸出的Y(k+1)進行反饋校正后輸出校正值Xc(k+1)。 在下一采樣時刻k+1，校正值Xc(k+1)和期望軌跡Xref(k+1)做差得出新的狀態量Xe(k+1)，在滾動優化中重新求解最優化問題，如此循環構成了完整的模型預測控制過程。

2.2 預測模型

系統的預測模型在預測控制中具有重要作用，預測模型可根據系統當前給定的狀態值和輸入值確定系統未來的狀態。 針對四旋翼無人機的四輸入、六輸出的非線性特點，結合式(3)，本文選擇采用離散狀態空間方程作為系統的預測模型。

定義系統的狀態量為X(k) ＝[x1，x2，x3，x4，x5，x6，x7，x8，x9，x10，x11，x12]T，其中x1＝x、x2＝y、；系統的輸入量為U(k) ＝[u1，u2，u3，u4]T，其中；系統的輸出量為Y(k) ＝[x，y，z，φ，θ，φ，。 則四旋翼無人機的非線性離散狀態空間模型為

式中f(X(k)，U(k))為非線性離散映射函數，表達式如式(5)所示。

式中:Sα為sinα，Cα為cosα，α∈{x4，x5，x6}，參數ai(i＝1，2，…，6)表達式如式(6)所示。

2.3 滾動優化

為保證四旋翼無人機快速、穩定地跟蹤期望軌跡，本文采用二次型函數計算預測狀態與期望狀態的距離誤差，以誤差最小化作為代價函數，即通過計算Xpre-Xref和U-Uref的誤差最小來求解最優控制量U，其中Xref為參考軌跡、Uref為四旋翼無人機懸停所需的控制量，計算表達式為

式中?ir(i＝1，2，3，4)為四旋翼無人機懸停時四個螺旋槳的轉速，且。

預測時域[k，k+N]內，定義目標函數為

式中:J為目標函數；Q和H分別為狀態權重矩陣和輸入權重矩陣，Q∈R12×12，H∈R4×4；‖·‖2A表示歐幾里得范數，A∈{Q，H}；Xpre(k)為k時刻的預測狀態；Xref(k)為k時刻的期望軌跡；Uref(k)為k時刻的期望控制輸入。

式(8)的第一項是通過最小化目標函數J求得局部最優解，即未來W個時刻的控制量U(k)、…、U(k+W-1)使得預測時域[k，k+N]內的預測值Xpre(k)、…、Xpre(k+N-1)盡可能接近期望值Xref(k)、…、Xref(k+N-1)；其第二項是對控制量的約束函數，使U-Uref最小，即U(k)的值接近Uref，可有效防止控制量發生突變，甚至四旋翼失控。 另一方面，考慮到四旋翼無人機螺旋槳轉速受電壓限制，設定控制輸入的幅值約束為

式中分別為U的最小控制輸入矩陣和最大控制輸入矩陣。

結合離散系統動力學方程和系統約束，NMPC 算法轉化為有限時域內的非線性規劃問題(nonlinear programming，NLP)[16]。 當前時刻為為k時，NLP 表示如下。

式中:Xpre(k) ＝X(k)表示在k時刻時，取預測值Xpre(k)等于當前狀態值X(k)；Xpre(k+i)表示預測時域[k，k+N]內第k+i時刻預測狀態；δ表示采樣時間。

本文使用多重打靶法(multiple shooting method，MSM)[17]求解NLP 問題，將得出最優控制序列的第一個元素U1作為四旋翼無人機的控制輸入U(k)。 將獲得的四旋翼無人機狀態用于下一時刻k+1 的優化問題，上述過程循環執行，直至控制過程完成。

2.4 反饋校正

在實際的四旋翼無人機動態系統控制中，存在干擾和模型不精確的問題，預測模型輸出的預測值與實際值存在偏差，因此僅基于預測模型和最優控制求解的控制量不能保證四旋翼無人機緊密跟蹤期望軌跡，若不及時利用實際信息進行反饋校正，由式(4)給出的預測值會偏離實際值。 為此，本文設計實時反饋進行校正，將當前時刻的實際測量值X(k)與預測值Xpre(k)做差，引入誤差ek，即

誤差反映了預測模型的不確定干擾造成的輸出值不準確現象，因此可用誤差預測未來的輸出誤差，以修正預測值。 選取反饋系數矩陣K＝[0.12，0.12，0.05，0.1，0.1，0.1，0，0，0，0，0，0]T，通過加權方法預測未來的誤差，可得到校正后的預測輸出

在k+1 時刻，校正后的預測輸出Xc(k+1)通過移位變成初始預測值，如此循環反復在線進行。

3 仿真實驗

為驗證本文設計的NMPC 算法的有效性，并考慮四旋翼無人機的飛行成本和安全性，首先在Matlab 中搭建仿真平臺對本文算法進行仿真驗證，四旋翼無人機的模型參數如表1 所示。

表1 四旋翼無人機模型參數Table 1 Parameters of quadrotor model

為驗證本文設計的NMPC 方法的優越性，在仿真條件和四旋翼無人機參數一致的情況下，開展對比實驗，對比的方法主要包括PID、串級MPC[12]、改進MPC[13]控制方法。 設計兩組仿真實驗，第一組仿真為跟蹤圓形參考軌跡，第二組仿真為跟蹤正方形參考軌跡。

3.1 圓形期望軌跡

在四旋翼無人機非線性軌跡跟蹤控制中，NMPC 控制器的參數選取如表2 所示。 四旋翼無人機初始狀態為。 設圓形參考軌跡為

表2 NMPC 控制器參數Table 2 NMPC controller parameters

式中:xd、yd、zd為四旋翼無人機在地球坐標系OE-XEYEZE中的期望位置，m；t表示時間，t∈[0，20]，s。

本節仿真部分使用x軸、y軸、z軸分別表示OE-XEYEZE中的OEXE軸、OEYE軸、OEZE軸。

圖3 為4 種控制方法的三維圓形軌跡跟蹤效果。 在初始時刻，NMPC 和改進MPC 控制方案的軌跡跟蹤性能較好，超調量??；PID 和串級MPC明顯偏離圓形參考軌跡；在14 s 時，在x軸方向加入幅值為0.5的階躍信號模擬陣風干擾，如圖3中箭頭所示，NMPC 受到擾動后，輕微偏離軌跡，受擾動影響較小，抗干擾能力強；PID 偏離軌跡，且不能快速恢復跟蹤；串級MPC 受擾動最大，存在安全問題；改進MPC 出現明顯偏移誤差，抗干擾能力差。

圖3 圓形軌跡跟蹤效果對比圖Fig.3 Comparison diagram of tracking effect of circular trajectory

圖4 為位置跟蹤效果曲線，圖5 為位置跟蹤誤差曲線。

圖4 圓形軌跡跟蹤效果Fig.4 Tracking effect of circular trajectory

圖5 圓形軌跡跟蹤誤差Fig.5 Tracking errors of circular trajectory

分析圖4 ～5如下。

1)跟蹤性能

NMPC 控制器能夠精確地跟蹤期望圓形軌跡，幾乎實現無誤差跟蹤，且超調量??；PID 控制器在z軸上出現較大超調量，由圖4(b)局部放大圖可見，在曲線的拐點處，PID 控制器存在跟蹤誤差；串級MPC 的超調量小，卻一直存在跟蹤誤差；改進NMPC 無超調量，誤差收斂速度較慢。

2)抗干擾性能

NMPC 和串級MPC 控制器的抗干擾性能最好，受到擾動后可以快速收斂誤差；PID 和改進MPC 控制器的抗擾性較差，在干擾情況下無法保證快速收斂。

圖6 為姿態角響應曲線。 NMPC 和改進MPC控制器的姿態角響應曲線平滑，擾動情況下無較大振蕩，最大誤差僅有0.2 rad；PID 在0 ～1.3 s 內和14 s 后均出現大幅度振蕩；串級MPC 控制器的姿態角響應曲線出現較大誤差，飛行穩定性能差。

圖6 圓形軌跡姿態角響應曲線Fig.6 Attitude angle response curve of circular trajectory

圖7 為四旋翼無人機的控制量仿真圖。NMPC、PID 和改進MPC 控制器對幅值有約束能力，數值相近；串級MPC 的控制量變化較大，不適合應用于實際飛行。

圖7 圓形軌跡控制量Fig.7 Control quantity of circular trajectory

3.2 正方形期望軌跡

為驗證控制器的魯棒性，開展正方形軌跡跟蹤控制實驗。 四旋翼無人機初始狀態和NMPC控制參數同上，設正方形的參考軌跡方程為

正方形軌跡跟蹤的三維效果如圖8 所示，NMPC、串級MPC 和改進MPC 控制器的軌跡跟蹤曲線平穩且光滑，其中串級MPC 的軌跡一直存在跟蹤誤差；PID 在起飛階段出現較大超調，存在安全性問題。 在14 s 時，x軸方向加入幅值為0.5的階躍信號模擬陣風干擾，如圖6 中所示，PID 和NMPC 的位置發生小范圍偏移；串級MPC 和改進MPC 出現較大跟蹤誤差，改進MPC 的軌跡2 s 后重新恢復了軌跡跟蹤，但串級MPC 控制直至仿真結束后仍未修正跟蹤誤差。

圖8 正方形軌跡跟蹤效果對比圖Fig.8 Comparison diagram of tracking effect of square trajectory

正方軌跡效果和跟蹤誤差如圖9 ～10 所示。

圖9 正方形軌跡跟蹤效果Fig.9 Tracking effect of square trajectory

圖10 正方形軌跡跟蹤誤差Fig.10 Tracking errors of square trajectory

分析圖9 ～10 如下。

1)軌跡跟蹤性能:NMPC 控制器的軌跡超調量小，可實現無誤差跟蹤；PID 的軌跡在拐點處存在0.2 m 左右的跟蹤誤差；串級MPC 控制器的軌跡在x軸和y軸上誤差最大，且一直未修正；改進MPC 消除了超調量，跟蹤性能較好。

2)抗干擾性能:由圖9 局部放大圖可見，在1 s 出現擾動后，NMPC 和串級MPC 均可在2.1 s內可修正誤差，具有一定的抗擾性；PID 受擾動影響，出現振蕩，經4 s 后誤差收斂到0，抗干擾能力較差；改進MPC 控制器的抗干擾性能較強，但誤差收斂速度較慢。

圖11 為姿態角響應曲線。 NMPC 的滾轉角和俯仰角的響應曲線平滑，無波動，且穩定后誤差保持在0.1 rad 內；PID 的滾轉角和俯仰角響應曲線在0 ～2.5 s 內均出現1 rad 左右的振蕩，且在14 s左右俯仰角受擾動影響后再次振蕩，PID 在姿態角控制方面穩定性較差；串級MPC 的滾轉角和偏航角在0 ～3 s 內出現大幅度振蕩，俯仰角存在波動，飛行狀態不穩定；改進MPC 的姿態角響應比較穩定。

圖11 正方形軌跡姿態角響應曲線Fig.11 Attitude angle response curve of square trajectory

圖12 為四旋翼無人機的控制量仿真圖。NMPC 和改進MPC 的控制量數值幅度較小，能量消耗較少；PID 和串級MPC 在0 ～2.6 s 內控制量數值變化幅度大，不符合工程實際。

圖12 正方形軌跡控制量Fig.12 Control quantity of square trajectory

4 結論

本文設計了基于四旋翼無人機非線性動力學的NMPC 控制器，旨在利用非線性預測模型和NMPC 處理系統約束的能力，將軌跡跟蹤問題轉化為最優化問題，以完成精準的軌跡跟蹤任務。在仿真實驗中，將NMPC 與PID、串級MPC 和改進MPC 控制方法進行對比，結果表明，NMPC 控制器提高了控制系統的穩定性和跟蹤性能，且具有較強的抗干擾能力。