基于包絡切換的四旋翼吊掛無人機避障軌跡優(yōu)化研究*

康晶杰 黃政 文浩

（南京航空航天大學機械結構力學及控制國家重點實驗室，南京210016）

引言

四旋翼無人機具有可垂直起降、機動性強、易維護等特點，近年來在民用、軍用以及警用領域獲得了極大的發(fā)展.通過四旋翼無人機吊掛載荷飛行，可以在其他運輸方式難以到達的地方，完成物資運送和人員救援等任務.四旋翼吊掛無人機飛行技術已在救災、滅火以及危險區(qū)域援助等方面呈現出突出優(yōu)勢，成為近年的研究熱點［1，2］.開展四旋翼吊掛無人機軌跡優(yōu)化研究，解決其在復雜環(huán)境下的應用問題，對推動四旋翼吊掛無人機技術發(fā)展具有重要意義.

相對于無吊掛系統(tǒng)，四旋翼吊掛無人機的動力學、軌跡優(yōu)化與控制問題無疑具有更大難度.一方面，吊掛與無人機本體間的動力學耦合使系統(tǒng)復雜度大增.特別是在吊掛無人機升降和飛行過程中，可能因系繩的松弛∕拉緊切換會產生動力學描述切換，因而難以建立各個飛行階段的統(tǒng)一動力學模型.另一方面，由于缺乏直接作用于吊掛體的作動力，僅能通過動力學耦合間接施加控制，無疑增大了吊掛無人機的軌跡優(yōu)化和控制難度.

四旋翼吊掛無人機動力學建模是開展軌跡優(yōu)化及控制研究的基礎.近年來，學者們針對四旋翼吊掛無人機系統(tǒng)發(fā)展了多種動力學模型.例如，Zhou等［3］假設吊掛無人機上的系繩始終處在繃緊狀態(tài)，建立了吊掛無人機的三維動力學模型.Sreenath等［4］考慮系繩拉緊和不拉緊兩種情況，將吊掛無人機系統(tǒng)建模為混合微分平坦系統(tǒng).Goodarzi等［5］摒棄了系繩是無質量且始終張緊的假設，將其建模為由球形節(jié)點串聯的一系列連桿.Foehn等［6］采用三個關節(jié)坐標來描述有效載荷運動，其中兩個為旋轉關節(jié)，而另一個是柱形關節(jié).該研究將系繩拉緊和放松狀態(tài)建模為一個互補約束，從而實現了兩種狀態(tài)的統(tǒng)一建模.

軌跡優(yōu)化是實現吊掛無人機自主飛行的重要環(huán)節(jié)，是指在滿足特定約束條件下，確定從起始狀態(tài)到目的狀態(tài)的轉移軌跡以及相應的控制輸入，以優(yōu)化任務性能指標.例如，Foehn等［6］將吊掛四旋翼無人機軌跡優(yōu)化問題表述為具有互補約束的數學規(guī)劃問題，通過序列二次規(guī)劃（Sequential Quadratic Programming，簡稱 SQP）求解了最優(yōu)軌跡 .Tartaglione等［7］將吊掛無人機系統(tǒng)避障問題視為兩個圓形的不干涉問題，并基于模型預測控制（Model Predictive Control，簡稱MPC）方法計算了最優(yōu)軌跡.Son等［8］僅考慮吊掛物避障問題，計算出了連續(xù)避障多個圓形障礙物的無碰撞軌跡，并完成了相應的實驗.值得注意的是，現有吊掛無人機避障運動研究中，通常將吊掛無人機全系統(tǒng)包絡圓半徑設為定值，忽略了吊掛載荷擺幅度變化導致的包絡圓切換的問題.然而，當系繩長于無人機本體包絡圓半徑時，吊掛載荷可能越出無人機本體包絡圓的范圍，造成包絡圓切換.若將系統(tǒng)整體包絡圓半徑固定設置為系繩全長，則過于保守［6］；而若僅考慮無人機本體包絡圓半徑，則不能用于吊掛物擺出本體包絡圓的情況，存在碰撞風險［7］.此外，為了簡化分析，現有吊掛無人機軌跡優(yōu)化研究中，通常將障礙區(qū)域近似為圓形，其避障軌跡優(yōu)化方法缺乏普適性，實際工程意義欠佳［6-8］.

本文基于帶有互補約束的吊掛四旋翼無人機三維動力學模型，考慮非圓避障區(qū)域以及吊掛載荷擺動導致的避撞包絡圓切換問題，開展避障軌跡優(yōu)化研究.文中基于R函數建立了不同包絡圓下的統(tǒng)一避障約束，使用碰撞檢測算法計算包絡圓與障礙物的距離，并通過非線性最優(yōu)控制方法實現吊掛無人機避障軌跡優(yōu)化.最終，將開環(huán)最優(yōu)控制問題離散為非線性規(guī)劃問題，通過數值求解得到了吊掛無人機的最優(yōu)運動軌跡.

1 吊掛無人機動力學模型

吊掛四旋翼無人機系統(tǒng)簡圖如圖1所示.建模過程中采用了3套坐標系，即世界坐標系owxwywzw、機體坐標系ob-xbybzb和吊掛物坐標系osxsyszs.圖中吊掛無人機單個旋翼所提供的升力為Fi，提供的繞旋轉軸的反作用力矩為Mi，i=1，2，3，4分別與四個旋翼對應 .建模時近似認為Mi=kFi，其中k為常值比例系數.無人機旋翼產生的繞體軸系各軸的力矩分別為τxb，τyb和τzb.此外，本文以mQ與mq分別代表四旋翼無人機和吊掛物的質量，I=[Ixx，Iyy，Izz]T為四旋翼無人機相對于相對連體坐標系軸的轉動慣量.吊掛無人機系統(tǒng)的整體動力學方程為［6］：

圖1 吊掛無人機系統(tǒng)結構簡圖Fig.1 A schematic diagram of the quadrotor with slung payload

其中，通過互補約束式（2）-式（4）對拉緊-松弛系繩進行統(tǒng)一描述，q=[xQ，yQ，zQ，ψ，θ，φ，αL，βL，ρ]T表示系統(tǒng)的狀態(tài)向量，pQ=[xQ，yQ，zQ]T表示四旋翼無人機在慣性系下的位置坐標，η=[ψ，θ，φ]T表示用于無人機本體姿態(tài)描述的Z-Y-X形式歐拉角.此外，αL和βL分別表示系繩繞坐標系os-xsyszs中xs軸和ys軸的轉動的角度.式中，l0和ρ分別表示系繩原長和當前長度；Jλ(q)和λ分別表示約束方程映射在廣義坐標下的雅可比矩陣和系繩張力；B(q)和U分別表示無人機的輸入矩陣以及力和力矩輸入向量，其中 U=[F，τψ，τθ，τφ]的四個分量分別是四個旋翼推力的合力大小以及對應于歐拉角ψ，θ和φ的廣義力矩.為簡化分析，考慮歐拉角為較小值，將歐拉角廣義力矩τψ，τθ和τφ分別近似為體軸力矩τzb，τyb和τxb，導出近似表達式U=Eu，其中

2 吊掛無人機軌跡優(yōu)化

本節(jié)將建立四旋翼吊掛無人機軌跡優(yōu)化問題的非線性最優(yōu)控制模型，基于R函數描述不同包絡圓下的統(tǒng)一避障約束［9］，使用碰撞檢測算法計算包絡圓與障礙物的距離［10，11］.并通過Legendre-Gauss-Radau（LGR）偽譜法將非線性最優(yōu)控制問題離散，以便通過非線性規(guī)劃方法求解［12］.

2.1 軌跡優(yōu)化模型

為便于建立軌跡優(yōu)化模型，將式（1）改寫為：

引入狀態(tài)向量 x=[q，q?]T，將方程 6 轉化化為狀態(tài)空間形式吊掛無人機的軌跡優(yōu)化問題可表述為如下非線性最優(yōu)控制問題

其中，J(x，u，t)性能指標函數；Θ為不等式路徑約束；Φ為系統(tǒng)初始時刻和末端時刻約束，對應于給定的t0時刻的初始狀態(tài)

和tf時刻的末端狀態(tài)

考慮到飛行中的能源消耗問題，希望在滿足其他約束條件的情況下，使吊掛無人機在優(yōu)化軌跡中的能源消耗較少.進一步考慮狀態(tài)誤差抑制需求，選取最優(yōu)性能指標［13-15］為

其中Q'和R'分別是狀態(tài)和對角加權系數矩陣.

2.2 路徑約束

為了計算出安全有效且滿足吊掛無人機自身物理限制的軌跡，需要對其模型施加適當的約束限制.主要考慮的約束如下：

（1）互補約束.上文中提到將系繩拉緊和松弛狀態(tài)表達為一個互補約束，其含義為：當繩子繃緊時l0-ρ=0，且 λ≥0；繩子松弛時 l0-ρ≥0，且λ =0.該約束對應式（2）-式（4）.

（2）推力約束.考慮到吊掛無人機的硬件限制，各個電機驅動螺旋槳產生的推力Fi應滿足

式中Fmin和Fmax分別表示推力下、上限.

（3）角度約束.考慮到吊掛無人機在控制飛行時的安全問題，給四旋翼無人機的姿態(tài)角和系繩的擺動角施加如下約束

（4）避障約束.為了使吊掛無人機能安全地實現障礙物繞飛.可以考慮使用吊掛無人機系統(tǒng)包絡圓來有效簡化系統(tǒng)的外形描述，如圖2所示.考慮掛載荷擺動導致的避撞包絡圓切換問題：a）當載荷位置超出四旋翼無人機本體包絡圓范圍時，取系統(tǒng)整體包絡圓半徑為r1；b）當載荷運動沒有超出四旋翼無人機本體包絡圓范圍時，取包絡圓半徑為r2.

圖2 吊掛無人機系統(tǒng)包絡圓最大半徑Fig.2 Maximum radius of enveloping circle of hanging uav system

根據吊掛無人機的系統(tǒng)模型可得

其中，rp為四旋翼無人機螺旋槳半徑，L為吊掛無人機系統(tǒng)中四旋翼無人機的機臂長度.數值計算過程中發(fā)現，若直接根據包絡圓切換條件和式（14）計算整體包絡圓直徑，數值計算耗時很長.為此，基于R函數，對包絡圓直徑做近似計算，以提高計算效率.R函數是指僅由實變量的符號決定自身符號的實值函數［9］，例如可以用來求最小值或最大值：

其中，在碰撞檢測中，將吊掛無人機系統(tǒng)抽象成半徑為rmax的包絡圓OQ.考慮對中心在坐標原點o的長為a，寬為b且順時針旋轉s度的矩形障礙物進行避障.同時，由于無人機定位精度差異，可以增加安全距離r'，使系統(tǒng)安全地避開障礙物.

為移除圓形障礙物假設，處理更一般性的障礙物 .采 用 GJK（Gilbert-Johnson-Keerthi）和 EPA（Expanding Polytope Algorithm）算法，進行吊掛無人機包絡圓與障礙區(qū)域的碰撞檢測和距離計算［10］.由于GJK算法只適用于檢測兩凸體間的距離，以及是否發(fā)生碰撞，無法解決優(yōu)化迭代過程中兩凸體相互重疊和穿刺的情況.為此，引入EPA算法對兩凸體碰撞后相互重疊部分的穿透深度和穿透向量進行計算，從而保證定義的名義距離能夠在實數域內連續(xù)，以保證優(yōu)化算法迭代順利進行.

2.3 LGR偽譜算法

針對上述優(yōu)化問題的求解，目前應用較廣泛的是LGR偽譜算法，其優(yōu)點在于可以處理任務約束和非線性因素，并對給定性能指標進行優(yōu)化.針對于吊掛無人機機動最優(yōu)控制模型，使用全局多項式對狀態(tài)變量、控制變量和乘子變量進行插值近似，在一系列LGR配置點施加動力學方程以及其他路徑約束，從而將最優(yōu)控制問題轉化為非線性規(guī)劃問題.由于LGR配置點位于半開區(qū)間τ∈[-1，1)中，首先需要對變換時間τ和連續(xù)飛行時間t∈[t0，tf]進行仿射變換

在轉換后的區(qū)間τ∈[-1，1)內分布著N個LGR配置點(τ1，τ2，L，τN)，其中

并且引入未配置的點τN+1=1來對應機動飛行的終止時間tf.值得注意的是，為了提高數值求解效率和精度，實際計算中常通過hp自適應算法等策略對所使用的配置數目和分布進行自適應調整［12］.使用Lagrange內插多項式對狀態(tài)向量、控制向量和乘子變量進行插值近似

式中Xi，Ui和Λi分別為狀態(tài)向量和控制向量在配置點處的離散近似值.對式（23）中的第一個式子進行求導運算，可得τk處狀態(tài)變量導數近似為：

式中撇號表示對變換時間τ的導數.由此可將式（8）中的狀態(tài)方程離散為

式（8）中的端點和路徑約束經過偽譜法離散得到

此外可采用Gauss積分公式對式（11）對應的目標函數進行離散近似.基于上述方法，將吊掛無人機軌跡優(yōu)化問題轉化成了標準的非線性規(guī)劃（Nonlinear Programming，簡稱NLP）問題［12］.

3 數值仿真結果

本節(jié)通過數值仿真算例對前述軌跡優(yōu)化方法進行考核驗證.表1給出了仿真采用的吊掛無人機系統(tǒng)物理參數，表2為吊掛無人機的初始狀態(tài)和路徑規(guī)劃的參數，表3為施加在路徑上的路徑約束參數.設定飛行時間tf=4s，吊掛無人機系統(tǒng)由pw，0飛行至pw，1，給定系數矩陣分別為

表1 吊掛無人機系統(tǒng)的物理參數Table1 Physical parameters of hanging drone system

表2 吊掛無人機初始狀態(tài)和末端狀態(tài)的位置和速度參數Table2 Position and speed parameters of the initial state and end state of the hanging drone

表3 路徑約束參數Table3 Path constraint parameter

數值仿真中，采用通用偽譜最優(yōu)控制軟件GPOPS（GeneralPseudospectralOptimalControl Software）對最優(yōu)問題進行離散，并選用hp自適應算法對配置點進行自動調整，以提高數值求解效率和精度.使用SNOPT求解器（Sparse Nonlinear Optimizer）中的SQP算法求解離散后的NLP問題［16］. 此 外 ，通 過 引 入 Flexible-Collision-Library（FCL）碰撞檢測庫中的GJK-EPA模塊，實現吊掛無人機包絡圓和障礙物之間的距離檢測及計算［17］.該仿真程序運行計算機的中央處理器為AMD R5 3550H，其主頻為2.1GHz；內存容量為16G；仿真軟件為Matlab R2019a.仿真算例的結果顯示經過了hp自適應算法迭代，其總耗時約為262秒.

LGR偽譜法求得的最優(yōu)軌跡優(yōu)化結果如圖3-圖6所示.其中，圖3為吊掛無人機避障飛行運動軌跡在x-y平面的投影.圖4給出了吊掛無人機位移、姿態(tài)角和系繩擺動角隨時間歷程，圖5為相應的速度響應曲線.從圖3可以看出，吊掛無人機成功避開了矩形障礙物，未發(fā)生任何碰撞，并且在系統(tǒng)通過矩形障礙物第二個頂點時，吊掛無人機的包絡圓發(fā)生了切換.從圖4（a-c）可以看出，吊掛無人機的位移、姿態(tài)角和系繩擺動角最終都到達了期望位置，證明吊掛無人機系統(tǒng)成功完成了避障飛行.此外，圖5（a）和（b）表明要求的速度和角速度的終端約束得到了滿足.圖6表明所給定的推力約束式12得到了滿足.受限于計算規(guī)模和硬件性能，難以通過LGR偽譜法實時求解出最優(yōu)軌跡.因此，擬在后續(xù)研究中以求得的最優(yōu)軌跡為參考，通過軌跡跟蹤方法實現實時控制和實驗驗證.

圖3 吊掛無人機運動軌跡圖Fig.3 Trajectory diagram of the UAV with slung load

圖4 吊掛無人機位移（a）、姿態(tài)角（b）和系繩擺角（c）隨時間歷程Fig.4The displacement（a），attitude angle（b）and tethered pendulum angle（c）of the UAV with slung load over time

圖5 吊掛無人機平移速度（a）與角速度（b）隨時間歷程Fig.5 The translation velocity（a）and angular velocity（b）of the UAV with slung load over time

圖6 吊掛無人機控制輸入力隨時間歷程Fig.6 The UAV with slung load controls the input force over time

4 總結

本文基于帶有互補約束的三維吊掛無人機模型，研究了軌跡優(yōu)化和控制問題.通過互補約束對拉緊-松弛系繩進行統(tǒng)一描述，建立了吊掛四旋翼無人機系統(tǒng)的整體動力學模型，為吊掛無人機軌跡優(yōu)化提供了基礎.采用R函數在軌跡優(yōu)化過程中實時判斷吊掛無人機系統(tǒng)最小包絡圓半徑，并使用GJK-EPA算法來檢測包絡圓與障礙物之間的名義距離，通過非線性最優(yōu)控制方法實現吊掛無人機避障軌跡優(yōu)化.利用LGR偽譜法，將開環(huán)最優(yōu)控制問題離散為非線性規(guī)劃問題，通過數值求解得到了吊掛無人機的最優(yōu)運動軌跡.