基于自適應積分反步法的四旋翼無人機抗干擾軌跡跟蹤控制

陳志明，劉龍武，劉 瑞，駱州淮

（南京航空航天大學 航天學院，南京 210016）

近年來，無人飛行器，因其便捷、低成本等優點已得到廣泛應用與發展，四旋翼作為無人飛行器的一種，具有體積小、垂直起降、操作簡單等優點，也成為航空領域的研究重點之一。

四旋翼無人機因其體積小、非線性、且為欠驅系統，對不確定干擾因素敏感，包括：風力擾動、空氣摩擦以及自身的不確定性參數。針對四旋翼無人機，研究人員已進行了許多相關研究并提出了一些魯棒性較好的控制系統，如干擾抑制控制[1]，模糊邏輯控制[2]、反步法控制[3,4]、LQR(linear quadratic regulator)控制[5,6]、滑?？刂芠7,8]等，但是，不確定外界干擾仍是四旋翼控制的關鍵點。如Linxing Xu，Hongjun Ma提出級聯有源干擾抑制控制作為無人機姿態控制分系統，此方案可一定程度上減少外界干擾帶來的影響，提高飛行穩定性，但應用于軌跡跟蹤控制時，對位置回路的干擾抑制較弱，無法保證軌跡跟蹤的準確性；Qi Lu，Beibei Ren[9]，Alberto Castillo[10]，Nigar Ahmed[11]，Ning Wang[12]等設計了干擾觀測器估計外部不確定干擾和系統的不確定性，此方案可有效減少外部干擾帶來的影響，但是，干擾感測器使得控制器結構復雜，且會帶來數據延遲；Tianpeng Huang[13]，Hakim Bouadi[14]，V.T. Hoang, M.D. Phung[15]等提出自適應滑?？刂破飨叨群妥藨B跟蹤誤差，Justin M.Selfridge[16]提出模型參考自適應控制應用于無人機姿態通道控制，Anastasia Razinkova[17]采用自適應補償方案減小外界干擾引起的無人機位置跟蹤誤差，雖然自適應補償因子可有效估計外界干擾項，減小控制誤差，但無人機指令跟蹤的速度與準確性仍是無人機控制的亟待解決的關鍵問題。此外，神經網絡也被廣泛應用于無人機的干擾補償，如：Hadi Razmi[18]提出基于自適應神經網絡的滑模高度控制器減小高度誤差；Jesús Enrique Sierra，Matilde Santos[19]提出自適應神經網絡估計器有效抑制風力和有效載荷干擾；Yanmin Chen，Yongling He 設計了分散的PID 神經網絡控制有效抵抗外界風力干擾，保證穩定性。雖然神經網絡結構的引入可實現更精確的不確定性干擾項的估計與補償，但神經網絡結構的復雜性必定會增加控制率計算量，加重無人機控制器負擔，實時性難以保證。因此，如何有效解決外界不確定干擾影響，并保證無人機的軌跡精確跟蹤仍是無人機控制的關鍵難題。

本文提出一種位姿雙回路自適應積分反步法實現無人機的軌跡精確跟蹤，其中自適應因子可有效補償外界不確定性干擾帶來的影響，同時，姿態回路控制器中加入了積分因子，可保證姿態回路指令跟蹤的準確性和穩定性。此方案可有效提高無人機的軌跡跟蹤能力，并保證無人機具有良好的魯棒性和穩定性。

1 動力學模型建立

本文基于Parrot Minidrone 建立四旋翼無人機動力學模型，如圖1 所示。四旋翼利用四個電機不同的組合實現自身的俯仰、橫滾、偏航和升降運動，如表1 所示。其中“+”表示電機的轉速增加，“-”表示電機轉速減小。假定四旋翼無人機為一個均勻對稱的剛體，由牛頓歐拉動力學算法可得到四旋翼的動力學模型如下[20]：

其中，x，y，z，φ，θ，ψ分別表示無人機的3 個位置狀態量和3 個姿態角狀態量；p，q，r表示無人機滾轉、俯仰和偏航三通道的姿態角速度變化量；Ixx，Iyy，Izz為四旋翼的三軸轉動慣量；m為無人機質量；l表示機體中心與旋翼中心的距離；U1，U2，U3，U4分別為四旋翼虛擬控制變量；g為重力加速度；dx，dy，dz，dφ，dθ，dψ表示無人機6 通道的不確定性干擾變量。

圖1 四旋翼動力學模型Fig.1 The dynamic model of quadrotor

表1 四旋翼電機轉速與飛行動作關系Tab.1 The relationship of the four rotors and flying action

2 控制器設計

四旋翼無人機是一個多變量非線性欠驅動系統，包括4 個虛擬控制輸入變量和6 個狀態輸出變量。假定四旋翼為一個嚴格對稱系統，則高度控制與偏航控制可以作為獨立的控制通道，俯仰、滾轉和水平位置控制通道間相互耦合，4 個虛擬控制輸入變量U=U1+U2+U3+U4與6 個狀態輸出變量(x，y，z，φ，θ，ψ)的關系可表示為：

當U2，U3，U4已知時，姿態角和姿態角速度可由式(2)(3)反推得到，當U1，U2，U3，U4已知時，則x，y，z，可根據式(1)-(3)得到。因此，本文設計了一個基于自適應積分反步法的雙回路位姿控制系統，如圖2 所示。位置回路控制可保證四旋翼無人機快速響應跟蹤指令并準確跟蹤，姿態回路可解耦姿態與位置通道，同時保證四旋翼飛行的穩定性。自適應積分反步法可減少外部不確定性干擾給控制系統帶來的影響，提高四旋翼無人機控制系統的魯棒性。

圖2 基于自適應積分反步法的四旋翼雙回路控制系統Fig.2 The general block diagram of the dual-loop control system

圖2 中，Ux，Uy表示位置回路的虛擬控制輸入變量：

假定偏航角ψ已知，則期望滾轉角φd與期望俯仰角θ d可根據式(5)反推得到，

2.1 位置回路控制器設計

為減少外部不確性干擾因素對軌跡跟蹤控制的影響，本文在傳統反步法控制器的基礎上，增加了自適應因子，設計了一種自適應積分反步法以補償干擾因素帶來的誤差，提高飛行控制系統的魯棒性，保證四旋翼無人機具有良好的飛行效果。

根據式(5)和式(1)，四旋翼Z 通道的動力學微分方程可表示為：

定義Z 軸方向的飛行速度分量為w=，位置誤差為e z=z-zd，zd為四旋翼期望位置Z 軸方向的分量，則狀態變量z的李雅普諾夫函數可表示為：

狀態變量z的李雅普諾夫函數關于時間的微分為：

定義Z 軸方向速度分量w為αw，則式(9)可表示為：

其中，βw為真實值w與w估計值αw的差值。

設定

其中，cz為一個正常數，cz＞ 0，則式(10)可表示為：

根據李雅普諾夫穩定性理論可知，當βw=0時，式(13)滿足則Z通道控制系統穩定。然而，βw為狀態估計值與真實值之間的差值，不為零，則需進一步設計控制器。

定義

則式(14)關于時間的微分為：

由式(11)(12)可得：

進一步微分可得：

將式(17)帶入式(15)得：

進一步化簡為：

設定：

其中，cw為正常數，即cw＞ 0，則式(19)可表示為：

式(21)表明滿足李雅普諾夫穩定性理論。

由式(20)可反推得到w微分為：

根據式(7)和式(22)可計算得到Z 通道虛擬控制變量U1為

由于dz為未知量，設定dz估計值換dz，則式(23)可表示為：

進一步，設計一個新的李雅普諾夫函數求取自適應控制律，

其中，λz為自適應因子，則式(25)的關于時間的微分可表示為：

根據式(10)(20)(21)，式(27)可表示為

進一步化簡為：

式(30)表明滿足李雅普諾夫穩定性理論。

則Z 通道的虛擬控制變量可表示為：

借鑒Z 通道設計方法，X 通道和Y 通道的虛擬控制變量可表示為：

其中，cu，cv，cx，cy，λx，λy為正常數；βu，βv分別表示X 軸速度分量u和Y 軸速度分量v的真實值與估計值的差值；分別為X 軸位置分量期望值和Y 軸位置分量期望值；ex，ey分別為X 軸位置分量x誤差和Y 軸位置分量y誤差。

2.2 姿態回路控制器設計

姿態回路與速度回路相互影響，穩定的姿態回路指令跟蹤可保證位置回路的良好的軌跡跟蹤效果，與位置回路相似，姿態回路同樣采用自適應反步法實現姿態指令的準確跟蹤，同時，在設計的自適應反步法的基礎上，增加了積分項，減小姿態指令跟蹤誤差，提高四旋翼無人機飛行穩定性。

定義偏航角ψ跟蹤誤差為eψ=ψ-ψd，ψd為設定的期望偏航角，定義積分項為則關于偏航角ψ的李雅普諾夫函數可表示為：

其中，λψ為正常數，即λψ＞ 0。

式(33)關于時間的微分為：

定義偏航角ψ跟蹤誤差關于時間的導數，估計值為αψ，則式(34)可表示為：

其中，

定義

其中，cψ為正常數，即cψ＞ 0。式(35)可簡化為：

定義：

根據式(36)(37)可得：

進一步化簡可得：

根據式(39)-(41)可得：

定義:

其中，cr為正常數，即cr＞ 0，則式(42)可表示為：

采用同樣的設計方法設計滾轉和俯仰通道得虛擬控制輸入變量為：

其中，cp，cq，cφ，cθ，λφ，λθ，均為正常數；φd和θd分別為期望滾轉角和期望俯仰角；eφ，eφ分別為滾轉角和俯仰角跟蹤誤差；βφ和βθ為

3 仿真實驗與分析

為驗證本文設計的自適應積分反步法抗干擾能力，這里選取已建立的四旋翼動力學模型為研究對象，并分別在不同頻率和不同強度隨機干擾影響下測試無人機指令跟蹤精度和跟蹤誤差修正能力。其中四旋翼動力學模型的關鍵參數如表2 所示。

表2 四旋翼動力學模型參數及初始化參數Tab.2 The model parameters and initialization parameters

其中，x0，y0，z0為四旋翼初始位置狀態量；u0，v0，w0為四旋翼初始速度變量；φ0，θ0，ψ0為四旋翼初始姿態角；p0，q0，r0為初始姿態角速度。

設定四旋翼位置回路控制參數為：cx= 7，cy= 7，cz=6.6，cu=1.38，cv=1.38，cw= 1.4，λx= 4.6，λy= 4.6，λz= 4.6；姿態回路自適應積分反步法控制參數為：λφ= 0.1，λθ= 0.1，λψ=0.05，cφ= 3，cθ=3，cψ=5，cp=17，cq=17，cr=30，。

仿真1：設定四旋翼受強度為6 m/s2，頻率為1 Hz 的隨機干擾影響，驗證自適應積分反步法雙回路控制系統在外界干擾下準確跟蹤控制指令的能力。仿真結果如圖3-7 所示。

圖3 四旋翼隨機干擾變化曲線Fig.3 The disturbance of quadrotor

圖4 四旋翼指令跟蹤軌跡曲線Fig.4 The path of trajectory tracking

圖5 X 通道指令跟蹤及誤差曲線Fig.5 The responses of trajectory tracking of X channel

圖6 Y 通道指令跟蹤及誤差曲線Fig.6 The responses of trajectory tracking of Y channel

圖7 Z 通道指令跟蹤及誤差曲線Fig.7 The responses of trajectory tracking of Z channel

其中，圖3 為軌跡跟蹤過程中受到的隨機干擾曲線變化圖；圖4 為四旋翼無人機軌跡三維變化曲線；圖5、圖6 和圖7 中的上圖分別為X 通道、Y 通道和Z 通道的軌跡跟蹤曲線；圖5、圖6 和圖7 中的下圖為對應的軌跡跟蹤誤差曲線。圖4-7 中紅色線條為參考軌跡，藍色為基于傳統反步法的指令跟蹤曲線，綠色為基于自適應積分反步法的指令跟蹤曲線。

由圖4 以及圖5、圖6 和圖7 中的上圖可看出，藍色曲線存在多處震蕩偏離參考軌跡的現象，特別是圖7 上圖中的Z 通道軌跡變化曲線震蕩嚴重偏離了紅色曲線，而綠色曲線變化較為平滑，無劇烈震蕩現象，且更加接近于參考軌跡。圖5、圖6 和圖7 中的下圖反映了X、Y、Z 通道對應的軌跡跟蹤誤差曲線，可明顯看出，綠色曲線振幅較小，ex、ey、ez在整個仿真周期中最大峰值僅為-0.30 m、0.39 m、-0.14 m，而藍色曲線最大峰值可達到-0.65 m、-0.69 m、1.64 m。表明在低頻率、高強度干擾影響下，基于自適應積分反步法控制器能更好地估計并補償干擾誤差，表現效果更好。

仿真2：設定四旋翼受強度為62m/s ，頻率為5 Hz 的隨機干擾影響，驗證自適應積分反步法雙回路控制系統在外界干擾下準確跟蹤控制指令的能力。仿真結果如圖8-12 所示。

圖8 四旋翼隨機干擾變化曲線Fig.8 The disturbance of quadrotor

圖9 四旋翼指令跟蹤軌跡變化曲線Fig.9 The path of trajectory tracking

圖10 X 通道指令跟蹤及誤差曲線Fig.10 The responses of trajectory tracking of X channel

其中，圖8 為軌跡跟蹤過程中受到的隨機干擾曲線變化圖；圖9 為四旋翼軌跡三維變化曲線；圖10、圖11 和圖12 中的上圖分別為X 通道、Y 通道和Z 通道的軌跡跟蹤曲線；圖10、圖11 和圖12 中的下圖為對應的軌跡跟蹤誤差曲線。圖9-12 中紅色線條為參考軌跡，藍色為基于傳統反步法的指令跟蹤曲線，綠色為基于自適應積分反步法的指令跟蹤曲線。

圖11 Y 通道指令跟蹤及誤差曲線Fig.11 The responses of trajectory tracking of Y channel

圖12 Z 通道指令跟蹤及誤差曲線Fig.12 The responses of trajectory tracking of Z channel

由圖9 以及圖10-12 中的上圖可看出，藍色曲線存在多處震蕩偏離參考軌跡的現象，特別是圖12 的上圖中的Z 通道軌跡變化曲線震蕩嚴重偏離了紅色曲線，且通過對比圖5-7 中的上圖可知，四旋翼在同強度高頻干擾信號下跟蹤表現更好，實際飛行軌跡更加靠近參考軌跡，但無人機軌跡震蕩加劇，特別是在基于傳統反步法控制器下，無人機震蕩頻率更加頻繁，無人機穩定性能表現更差。而圖9 以及圖10-12 的上圖中綠色曲線變化較為平滑，無劇烈震蕩現象，且更加接近于參考軌跡。圖10-12 中的下圖反映了X、Y、Z 通道對應的軌跡跟蹤誤差曲線，可明顯看出，除初始階段綠色曲線出現較大峰外，后期軌跡跟蹤過程中基于自適應積分反步法的四旋翼軌跡跟蹤誤差振幅明顯較小，而藍色曲線在整個飛行過程中均劇烈震蕩，且存在后期跟蹤過程中震蕩加劇現象，進一步表明四旋翼在高頻率、高強度干擾影響下，基于自適應積分反步法控制器能夠更快響應干擾，并自適應調整補償值，使得后期軌跡跟蹤效果明顯優于基于傳統反步法的軌跡跟蹤效果。

仿真3：設定四旋翼受強度為12m/s ，頻率為1 Hz 的隨機干擾影響，驗證自適應積分反步法雙回路控制系統在外界干擾下準確跟蹤控制指令的能力。仿真結果如圖13-17 所示。

圖13 四旋翼隨機干擾變化曲線Fig.13 The disturbance of quadrotor

圖14 四旋翼指令跟蹤軌跡曲線Fig.14 The path of trajectory tracking

其中，圖13 為軌跡跟蹤過程中受到的隨機干擾曲線變化圖；圖14 為四旋翼軌跡三維變化曲線；圖15-17 中的上圖分別為X 通道、Y 通道和Z 通道的軌跡跟蹤曲線；圖15-17 中的下圖為對應的軌跡跟蹤誤差曲線。圖13-17 中紅色線條為參考軌跡，藍色為基于傳統反步法的指令跟蹤曲線，綠色為基于自適應積分反步法的指令跟蹤曲線。

圖15 X 通道指令跟蹤及誤差曲線Fig.15 The responses of trajectory tracking of X channel

圖16 Y 通道指令跟蹤及誤差曲線Fig.16 The responses of trajectory tracking of Y channel

由圖14 以及圖15-17 中的上圖可看出，在低頻、低強度隨機干擾影響下，基于自適應反步法與基于傳統反步法的軌跡跟蹤均表現良好，進一步說明該類型干擾對無人機實際飛行造成的影響可忽略不計，但圖中小圖仍可看出，綠色曲線較藍色曲線更加接近于參考軌跡。由圖15-17 的下圖可知，雖基于自適應積分反步法的軌跡跟蹤誤差在初始階段誤差較大，但第一個峰值過后跟蹤誤差明顯減小，且能夠穩定保持在較小的范圍內震蕩；而基于傳統反步法的軌跡跟蹤誤差在全程均表現出大幅度震蕩現象。分析表明自適應積分反步法在低強度、低頻率干擾下同樣表現出更加優秀的跟蹤效果。

圖17 Z 通道指令跟蹤及誤差曲線Fig.17 The responses of trajectory tracking of Z channel

通過對比仿真1 與仿真2 測試效果可知，同強度下，低頻干擾對無人機飛行影響更大；對比仿真1 與仿真3 測試效果可知，同頻率下，高強度干擾對無人機更容易造成無人機失穩現象。在低頻高強度干擾下，自適應反步法軌跡跟蹤精度分別為0.30 m、0.39 m、0.14 m，傳統反步法跟蹤精度分別為0.65 m、0.69 m、1.64 m，跟蹤精度提高4.2 倍。且在軌跡跟蹤后期，自適應積分反步法跟蹤誤差進一步減小，三通道分別表現為0.1125 m、0.1109 m、0.1182 m，穩定性提高13 倍。進一步表明本文設計的自適應積分反步法抗干擾控制器在各類干擾環境影響下具有更好的穩定性。

4 結 論

本文提出了一種基于自適應積分反步法的無人機軌跡跟蹤控制策略，有效解決了四旋翼無人機因外界干擾及自身不確定性因素造成的軌跡跟蹤誤差大，難以控制的問題，提高了無人機的軌跡跟蹤精度與速度，并保證了無人機良好的魯棒性和穩定性。其中，在位置回路中加入了自適應因子補償不確定性干擾誤差，提高軌跡跟蹤的精度，在速度回路中分別加入了自適應因子和指令跟蹤誤差積分項，在弱化誤差影響的同時保證了無人機良好的穩定性。最后通過不同強度、不同頻率下抗干擾測試，證明了自適應積分反步法抗干擾控制器的有效性與可行性。

本文提出的自適應積分反步法雖已在Simulink 環境下仿真驗證了其具有更好的抗干擾性能，但由于精力有限，未完成實物飛行測試，后續研究將從無人機實際飛行抗干擾測試方面進一步深入研究，論證本文提出的自適應積分反步法的工程適用性。