多旋翼無人機模型預測抗擾避障制導

蘭慶湘，陳 謀，雍可南

(南京航空航天大學 自動化學院, 江蘇 南京 211100)

多旋翼無人機因其機動性強、易于操作等優點廣泛應用于軍用和民用領域中，如偵察作戰、通信傳輸、航拍攝影和巡線檢修等。但隨著應用場景不斷擴大和更新，多旋翼無人機發生碰撞造成的墜機事故也不斷增加。因此，多旋翼無人機需要具備自主避障的功能，這是完成復雜任務的前提[1-4]。針對無人機、無人車等無人系統的避障問題，常見的避障方法有人工勢場法[5-7]、基于神經網絡和強化學習等的智能算法[8-10]、基于模型預測控制的方法[11-13]等。具體而言，人工勢場法通過設置目標的吸引力和障礙物的排斥力使得無人機在搜索路徑時沿著負梯度方向以尋找合適的避障路徑[5]。這一方法在實際應用中易于實現，但可能出現梯度為零的情況導致陷入局部極小值點。文獻[6]通過增加了轉向力和恢復力來避免陷入局部極小值點。文獻[7]基于自觸發控制與人工勢場法，采用了機器人操作系統(robot operating system, ROS)與運動捕捉相機實現了室內環境下的無人機避障。隨著人工智能的迅速發展，許多研究人員將神經網絡和強化學習等智能算法應用于無人機避障的研究中。文獻[8]基于深度學習在四旋翼上采用了能夠測量像素的亮度變化來輸出信號的事件相機，并實現了動態障礙物的避障。文獻[9]通過強化學習對碰撞概率進行估計，使得無人機在未知場景中保持低速，減少了碰撞可能。文獻[10]采用案例的推理算法實現了對已知類型障礙物的避障，對未知類型的障礙物采用改進的人工勢場法避障而后保留在案例庫中，并在實際移動機器人平臺上驗證了該算法的可行性。然而，存在外部干擾的情況下不依賴外部環境已知信息的無人機避障問題還需進一步研究。

模型預測控制(model predictive control, MPC)廣泛應用于無人機避障方面，國內外許多學者進行了基于MPC的無人機避障方法研究。文獻[11]針對無人車的避障問題，基于三維危險勢場進行路徑規劃，采用了多約束模型預測控制實現路徑跟蹤，仿真結果表明了該方法良好的避障與跟蹤性能。文獻[12]針對無人機的動態避障問題設計了軌跡分類方案，將動態障礙物的預測軌跡引入MPC的避障約束條件，采用室內運動捕捉系統獲取無人機和障礙物的數據，通過遠程計算機向小型四旋翼Crazyflie發送指令實現了室內的無人機避障。文獻[13]提出的軌跡生成框架將具有不確定性的障礙物未來軌跡表述為具有置信度的碰撞區域，在MPC中構造概率約束，仿真結果表明了該方法能夠比沒有預測的方法更好地實現避障。文獻[14]考慮了自主水下機器人傳感器探測距離較短的問題，將路徑規劃問題轉化為滾動時域優化問題，構造誤差動力學并采用MPC實現了路徑規劃與跟蹤控制一體化，并通過實際的水下航行器動力學模型驗證了所提算法的有效性。顯然，前述基于模型預測的方法能夠實現避障的目標。但是，多旋翼無人機在執行任務時容易受到風等外部干擾，因此對所設計的制導律需要具有一定的抗干擾能力。

干擾觀測器(disturbance observer, DOB)通過根據系統的實際輸入與輸出得到干擾估計值，將其作為補償從而消除干擾對系統性能的影響，這種方法可以和不同的制導策略結合，且易于實現，在許多領域被廣泛應用[15-17]。文獻[18]針對MPC的抗干擾問題將非線性干擾觀測器和MPC結合起來，構造了復合飛行控制器，通過仿真驗證了干擾條件下的算法效果。文獻[19]設計了不同的干擾觀測器對未知干擾和已知諧波頻率的干擾進行估計，與MPC結合實現了對輪式機器人的軌跡跟蹤，并通過仿真與實驗驗證了該方法的有效性。文獻[20]針對機電一體化系統的控制，將干擾觀測器的補償信號融入MPC解算最優軌跡中，通過實驗驗證了所提出的基于干擾觀測器的MPC比傳統的MPC更加有效。上述文獻通過干擾觀測器對MPC進行補償，或將干擾觀測器的估計值融入MPC的優化求解過程中，實現了較好的效果，但基于MPC的多旋翼無人機抗擾避障方法還需進一步研究。

多旋翼無人機在執行任務時，將通過機載傳感器感知外部環境，實時傳遞感知到的障礙物信息至制導與控制系統，并運行避障制導律獲取避障制導指令，通過控制系統對制導指令跟蹤實現障礙避讓，保證軌跡跟蹤過程的飛行安全。基于上述研究背景，本文針對多旋翼無人機避障與軌跡跟蹤問題，通過綜合設計干擾觀測器、輔助制導律和MPC算法，提出了一種抗擾避障制導律設計方法，并進行數值仿真與實物飛行驗證。

1 問題描述

鑒于多數情況下多旋翼無人機將搭載單線激光雷達，其可感知的障礙物將以二維平面為主，同時考慮到維數增加帶來的優化求解速度降低問題，因此只考慮二維平面，即多旋翼無人機水平方向的運動實現避障。水平方向下多旋翼無人機的狀態量(二維位置和偏航角)p=[x,y,ψ]T與輸入量(機體速度和偏航角速率)v=[u,v,r]T的關系可通過運動學方程獲得。考慮到實際飛行中存在的干擾，多旋翼無人機水平方向的運動方程[21]為：

(1)

式中，干擾為vid=[uid,vid,rid]T。

為了便于表述，令轉換矩陣為：

(2)

則式(1)可簡寫為：

(3)

備注1干擾vid并不與實際干擾具有一一對應關系，而是無人機系統在機體坐標系下的等效干擾，可能包括但不限于風干擾、傳感器漂移等。

考慮到實際多旋翼無人機的速度有限，在設計制導律時需要對輸入添加以下限制條件：

(4)

其中，vmax為一正常數。

鑒于是在跟蹤背景下研究多旋翼無人機的避障，定義參考軌跡pr=[xr(t),yr(t),ψr(t)]T由如下系統產生：

(5)

式中，vr=[ur,vr,rr]T為參考速度與偏航角速率。

定義第i個障礙物坐標為poi=[xoi,yoi]T(i=1,2,…,No),障礙區域的威脅半徑為roi。其中障礙物坐標位置可能與參考軌跡的距離小于障礙區域的威脅半徑，如圖1所示。

圖1 障礙物與參考軌跡示意Fig.1 Schematic diagram of obstacles and reference trajectory

目標為：針對多旋翼無人機水平方向運動模型式(3)，設計避障制導律，在滿足輸入飽和約束式(4)與避讓圖1所示障礙物的前提下，使得多旋翼無人機跟蹤由式(5)生成的參考軌跡。

為了方便制導律設計和穩定性分析，需對外部干擾進行假設，具體如下：

2 基于模型預測的避障制導律設計

本節首先設計干擾觀測器來估計未知干擾，然后設計輔助制導律作為MPC的約束參考量，最后給出基于模型預測的避障制導律設計方法并分析整個閉環系統的穩定性。

2.1 干擾觀測器設計

為了估計未知干擾，定義z為設計的觀測器內部狀態，構造如式(6)所示的干擾觀測器[22]。

(6)

(7)

定義干擾估計誤差為：

(8)

結合式(6)與假設1，對式(8)求導得：

=-S(p)R(ψ)evid

(9)

為了保證S(p)R(ψ)為正定矩陣，構造T(p)：

(10)

其中，kd1>0，kd2>0用于調節干擾觀測器增益。

由式(7)可得：

(11)

其中，η1和η2的取值為η1=-xsinψ+ycosψ，η2=-xcosψ-ysinψ。

從而得到增益矩陣S(p)R(ψ)：

(12)

2.2 輔助制導律設計

本小節通過設計輔助制導律來構造李雅普諾夫約束條件，以保證閉環系統在所設計的基于模型預測的抗擾避障制導律下的穩定性。

構造誤差：

(13)

多旋翼無人機與第i個障礙物的距離為：

(14)

參考文獻[23]，構造多旋翼無人機與第i個障礙物的勢能函數：

(15)

式中，參數R>0。

式(15)對x和y的偏導數分別為：

(16)

(17)

多旋翼無人機與障礙物的整體勢函數為：

(18)

式中，ko>0用于調節避障成本與狀態成本的權重。

構造如下變量：

(19)

(20)

(21)

其中，km1和km2為大于零的可調參數。

采用的制導律為：

(22)

(23)

其中，kl為可調參數。

針對上述設計的輔助制導律，為便于后文的結論分析，歸納其特征于定理1。

定理1考慮式(3)所示的具有外部有界干擾的多旋翼無人機運動學方程，采用式(22)所示的制導律，當選取參數KM=diag(km1,km2,km3)，KN=diag(kn1,kn2,kn3)滿足式(24)～(25)，則可以使閉環系統的誤差最終一致有界，且同時實現避障與跟蹤的目標。

0

(24)

(25)

證明：考慮如下所示的李雅普諾夫函數：

V=V1+V2+Vo

(26)

對式(26)求導，得到V的導數為：

(27)

考慮式(3)、式(8)、式(9)、式(13)、式(21)、式(22)，則式(27)可以寫成如下形式：

koζTR(ψ)va1+koζTR(ψ)evid

(28)

(29)

將式(23)代入式(29)抵消掉后三項，從而得到：

(31)

2.3 抗擾避障制導律設計

為了進一步改善避障與跟蹤效果，基于MPC結合上述輔助制導律進行設計。

給定MPC的預測周期為T，預測步長為N，采樣周期為δ，則有T=Nδ。

對于每一個預測時刻，成本函數為：

(32)

tk時刻預測的N個步長的成本函數累加得到總成本函數：

(33)

根據式(27)得到的李雅普諾夫函數導數，添加如式(34)所示的穩定性約束條件[24]。

(34)

采用式(33)的成本函數，添加式(4)和式(34)所示的約束條件，MPC優化問題為：

(35)

基于模型預測的抗擾避障制導算法的主要步驟如下所示。

步驟1：選取合適的采樣間隔δ、多旋翼無人機狀態初值以及參數設置，給出參考狀態與參考輸入、障礙物坐標與半徑，初始仿真步數k=0，初始仿真時間tk=0，選取最大仿真時間tmax；

步驟3：求解優化問題(35)得到制導指令v(tk)；

步驟4：求解得到的tk時刻的解v(tk)作為當前仿真步長，即[tk,tk+δ]時間內的輸入，根據式(3)更新多旋翼無人機的狀態值；

步驟5：tk=tk+δ，k=k+1，重復步驟2～5至tk≥tmax。

針對上述算法中設計的避障制導律，可以得到如下定理：

定理2考慮式(3)所示的具有外部有界干擾的多旋翼無人機運動學方程，采用優化問題式(35)求解得到的制導律，當初始時刻t0有解時，則對于任意時刻MPC優化問題能夠保證有解，在所設計的制導律下多旋翼無人機能夠實現避障與軌跡跟蹤。

(36)

故式(22)所得到的va有界。

(37)

由va有界可知式(37)所示的輸入序列符合約束式(4)。因此由數學歸納法可知當t0時刻存在解時，能夠保證優化問題式(35)的遞歸可行性。又由式(34)可知對于任意tk時刻

(38)

即在優化問題式(35)求解得到的制導律下能夠實現軌跡跟蹤與避障。

備注2避障與跟蹤的效果可以通過相關參數進行調節。當多旋翼無人機系統狀態與輸入的成本權重Q和R選取過大時，會導致多旋翼無人機過多偏向于參考軌跡，進而導致避障失敗；反之，當避障成本權重ko選取過大時，會使多旋翼無人機更加遠離障礙物，導致跟蹤效果變差，甚至任務失敗。因此，用戶在進行權值設置時，需要結合實際情況進行適當取舍。另外，當多旋翼無人機系統的速度限幅區間較大時，多旋翼無人機會采取更加劇烈的機動過程實現避障。然而，受制于姿態回路的響應，過于劇烈的機動過程會導致多旋翼無人機的估計產生較大的波動，甚至控制失效；另一方面，當限制較小時，避障過程將耗費更久時間，且完成避障后對參考軌跡的跟蹤也更慢。因此，在實際飛行測試中需要多次測試選取合適的參數取值。采樣周期δ與實際飛行測試中程序的運行周期有關，因此選擇與飛控程序運行周期相近的值。預測步長N較小則會使避障與跟蹤效果變差，但N增大會增加每個周期的解算時間，過大可能會導致無法在指定的周期完成實時解算，因此需要在能保證實時性的前提下選擇較大的步長。

3 實驗結果

3.1 數值仿真

為驗證所提出的避障方法的有效性，基于文獻[25]提出的開源求解器Casadi編寫C++程序進行數值仿真與實際飛行驗證。

值得注意的是，式(23)中分母可能存在為零的時刻，即Excosψ+Eysinψ=0或-Exsinψ+Eycosψ=0。這種情況下表示誤差在機體系下的投影量(障礙物不在區域內)或誤差與勢能的疊加在機體系下的投影量(障礙物在檢測區域內)為零。為了避免使多旋翼無人機陷入這一局部極值點，應添加常量使多旋翼無人機跳出該點繼續前進。考慮到實際中取等號的情況較嚴苛，當|Excosψ+Eysinψ|≤時令ua1取值為ua1=時令va1取值為

期望軌跡為：

(39)

期望速度ur，vr與偏航角速率rr由式(40)給出。

(40)

干擾量設為：

(41)

考慮到多旋翼無人機實際的飛行速度的大小與變化律，式(41)所設置的干擾量的變化速度遠慢于飛機速度變化，所以可以認為滿足假設1。

選取仿真時長tmax=100 s，障礙物坐標為[3.93,10]T和[1.93,40]T，障礙物的威脅半徑ro1=ro2=1.5 m。選取干擾觀測器參數kd1=kd2=1，輔助制導律參數KM=diag(1,1,1)，KN=diag(2,2,2)，kl=1，避障成本參數ko=1。MPC采樣間隔δ=0.02 s，預測步長N=10。考慮到實際飛行中的不確定性，所設置的成本權重參數R中關于速度的兩項較小，即不一定要嚴格按照期望的速度飛行。因此，權重矩陣Q=diag(5,5,10)，R=diag(0.5,0.5,10)。

為了盡可能在飛行過程中確保飛行的安全，偏航角速率盡可能變動較小，多旋翼無人機的速度也應限制在合理的安全范圍內。因此，將速度與偏航角速度約束設為：

(42)

使用上述參數進行仿真，多旋翼無人機的初始狀態為p0=[0,0,0]T，多旋翼無人機初始設為懸停狀態，干擾觀測器的內部狀態初值設為z=[2,2,2]T。

為了體現方法在抗擾避障過程中的效果，本節給出常規MPC算法的仿真，并與本文所提出的方法進行比較，仿真結果如圖2～5所示。從圖2可以看出，受外部擾動的影響，常規MPC算法在規避障礙物后未能及時追蹤參考軌跡，而本文所提出的抗擾避障制導律能夠保證多旋翼無人機有效地避開障礙物，并在外部擾動下對期望軌跡進行較好的跟蹤。圖3為多旋翼無人機的二維坐標和偏航角的具體信息。相較于MPC算法，本文所提出的方法能夠在接近障礙物時及時調整多旋翼無人機的狀態進行規避，并在完成規避后及時跟蹤參考軌跡。由于仿真中添加了外部干擾，且在跟蹤與避障過程中需要結合整個系統的成本值對偏航角進行調整以獲取最優的成本值，所以偏航角并不會保持固定的期望值。圖4為兩種方法求解的線速度與偏航角速度指令，受干擾的影響，MPC算法在規避障礙物后未能及時跟蹤參考軌跡，使得跟蹤誤差增大，相較于本文的方法出現了較多的飽和，使得輸入成本較高。由于在仿真中需要進行避障與減小干擾對多旋翼無人機的影響，而期望狀態的計算中不包括這兩者，所以實際求解的線速度與偏航角速度指令具有一定的偏差。從圖5可以看出，隨著仿真的進行，干擾觀測器的估計誤差逐漸減小，能夠對干擾進行較好的估計。從上述仿真結果可以看出，相較于MPC算法，本文所提出的方法能夠減小干擾對多旋翼無人機的影響，達到更好的避障與軌跡跟蹤效果。

圖2 避障仿真效果Fig.2 Obstacle avoidance simulation renderings

(a) x

(b) y

(c) ψ圖3 多旋翼無人機仿真狀態Fig.3 Simulation states of multi-rotor UAV

(a) u

(b) v

(c) r圖4 MPC求解制導指令與參考值Fig.4 MPC solves guidance command and reference values

(a) uid

(b) vid

(c) rid圖5 干擾值與估計值Fig.5 Actual and estimated disturbance values

3.2 實物驗證

基于大疆的M600 Pro型號無人機進行實際多旋翼無人機上的算法測試與驗證。考慮到多旋翼無人機自身的硬件性能有限，難以完成優化問題的實時求解，機載計算機上基于DJI所提供的開源ROS開發框架[26]編寫基于ROS和C++的程序以實現所需的制導算法。

在實際飛行測試中采用本文所提出的抗擾避障制導律，結合無人機飛控所提供的控制算法完成避障。將編寫的MPC程序移植在機載計算機上，采用相關ROS服務改變MPC的輸入輸出接口，并添加實物飛行測試中所必需的相關代碼。通過自行編寫的MPC程序在機載計算機上實時求解制導指令而后采用串口發送至底層飛控，從而實現多旋翼無人機避障與軌跡跟蹤。障礙物由相機獲得，實驗過程中默認已知。

備注2中，障礙物的信息包括：其在慣性坐標系下的坐標及對應的危險半徑。需要說明的是，該種障礙物信息描述方式具有通用性，當機載激光雷達或深度相機檢測到障礙物后，可依據扇面信息將障礙物描述為上述形式，具體可參考文獻[27]。

實際飛行驗證的算法步驟如下所示。

步驟1：開啟相關ROS服務以建立起飛降落請求、獲取狀態信息與向無人機發送制導指令的接口，完成各參數的初始化，正常運行后自動起飛至MPC程序的起點，按照設定的頻率進行求解；

步驟2：獲取當前時刻無人機的位置、速度以及與障礙物的距離等信息，更新干擾估計值，將獲取的實時數據作為MPC的輸入進行求解，得到當前時刻的制導指令；

步驟3：將求解的制導指令發送至無人機底層飛控，使無人機繼續沿期望軌跡飛行或避開障礙物；

步驟4：根據ROS話題消息記錄當前時刻的無人機數據以便后續分析；

步驟5：完成當前時刻的計算，更新時間tk=tk+δ，k=k+1后再從步驟2開始計算，直至tk≥tmax。

實際飛行驗證中采用與仿真一致的參數設定，實驗結果如圖6～9所示。

圖6 避障實際效果Fig.6 Obstacle avoidance actual renderings

(a) x

(b) y

(c) ψ圖7 多旋翼無人機實驗狀態Fig.7 Experimental states of multi-rotor UAV

圖6和圖7表明了本文所提出的避障制導律的實際測試效果，在遠離障礙物時能夠對期望軌跡進行跟蹤，在接近障礙物時調整狀態實現避障，并在避障完成后繼續跟蹤期望軌跡。相較于圖2和圖3其性能有所下降，這主要是仿真與實際飛行測試的差異所導致的。從圖8中可以看出，MPC解算得到的制導指令發送至底層飛控后需要一定的時間才能響應，因此會對實際飛行的效果產生一定的影響。由于除了人為設定的干擾量，實際環境中必然還存在其他未知的干擾以及傳感器在測量中存在的噪聲，因此圖9所示的干擾估計誤差存在一定的波動。從實際飛行測試的結果中可以看出，本文所提出的抗擾避障制導律能夠應用于實際多旋翼無人機上的軌跡跟蹤與避障。

(a) u

(b) v

(c) r圖8 無人機對算法指令的響應效果Fig.8 Response effect of UAV to algorithm command

(a) euid

(b) evid

(c) erid圖9 干擾估計誤差Fig.9 Disturbance estimation error

3.3 仿真與實驗結果差異分析

從上述仿真和實驗結果中可以看出，實驗結果比仿真結果差，這是實際飛行中的實驗設備、實驗環境等因素導致的，主要有以下幾點：

1)實際飛行中傳感器的噪聲對獲取的多旋翼無人機狀態數據存在影響；

2)多旋翼無人機的實際速度和角速度由底層的姿態回路對MPC解算的制導指令進行響應來實現，其響應特性影響著實際飛行的結果；

3)電機也具有內部動態，無法很好跟蹤姿態控制器生成的參考指令，必然會對實際飛行效果產生影響。

針對上述可能產生影響的情況，可以通過調整多旋翼無人機飛行環境和自身的飛控參數等來改善實際飛行的效果。

4 結論

針對干擾下的多旋翼無人機的避障問題，本文引入干擾觀測器與輔助制導律，基于MPC方法設計抗擾避障制導律。數值仿真和實物飛行驗證結果表明，所提出的抗擾避障制導律能夠有效地規避飛行途徑中的障礙物，并具有一定的抗擾能力，能夠應用于小型多旋翼無人機在實際環境中實現干擾下的實時軌跡跟蹤與避障。