康晶杰 黃政 文浩

（南京航空航天大學(xué)機(jī)械結(jié)構(gòu)力學(xué)及控制國(guó)家重點(diǎn)實(shí)驗(yàn)室，南京210016）

引言

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

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

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

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

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

1 吊掛無(wú)人機(jī)動(dòng)力學(xué)模型

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

圖1 吊掛無(wú)人機(jī)系統(tǒng)結(jié)構(gòu)簡(jiǎn)圖Fig.1 A schematic diagram of the quadrotor with slung payload

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

2 吊掛無(wú)人機(jī)軌跡優(yōu)化

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

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

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

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

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

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

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

其中Q'和R'分別是狀態(tài)和對(duì)角加權(quán)系數(shù)矩陣.

2.2 路徑約束

為了計(jì)算出安全有效且滿足吊掛無(wú)人機(jī)自身物理限制的軌跡，需要對(duì)其模型施加適當(dāng)?shù)募s束限制.主要考慮的約束如下：

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

（2）推力約束.考慮到吊掛無(wú)人機(jī)的硬件限制，各個(gè)電機(jī)驅(qū)動(dòng)螺旋槳產(chǎn)生的推力Fi應(yīng)滿足

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

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

（4）避障約束.為了使吊掛無(wú)人機(jī)能安全地實(shí)現(xiàn)障礙物繞飛.可以考慮使用吊掛無(wú)人機(jī)系統(tǒng)包絡(luò)圓來(lái)有效簡(jiǎn)化系統(tǒng)的外形描述，如圖2所示.考慮掛載荷擺動(dòng)導(dǎo)致的避撞包絡(luò)圓切換問(wèn)題：a）當(dāng)載荷位置超出四旋翼無(wú)人機(jī)本體包絡(luò)圓范圍時(shí)，取系統(tǒng)整體包絡(luò)圓半徑為r1；b）當(dāng)載荷運(yùn)動(dòng)沒(méi)有超出四旋翼無(wú)人機(jī)本體包絡(luò)圓范圍時(shí)，取包絡(luò)圓半徑為r2.

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

根據(jù)吊掛無(wú)人機(jī)的系統(tǒng)模型可得

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

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

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

2.3 LGR偽譜算法

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

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

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

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

式中撇號(hào)表示對(duì)變換時(shí)間τ的導(dǎo)數(shù).由此可將式（8）中的狀態(tài)方程離散為

式（8）中的端點(diǎn)和路徑約束經(jīng)過(guò)偽譜法離散得到

此外可采用Gauss積分公式對(duì)式（11）對(duì)應(yīng)的目標(biāo)函數(shù)進(jìn)行離散近似.基于上述方法，將吊掛無(wú)人機(jī)軌跡優(yōu)化問(wèn)題轉(zhuǎn)化成了標(biāo)準(zhǔn)的非線性規(guī)劃（Nonlinear Programming，簡(jiǎn)稱(chēng)NLP）問(wèn)題［12］.

3 數(shù)值仿真結(jié)果

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

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

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

表3 路徑約束參數(shù)Table3 Path constraint parameter

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

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

圖3 吊掛無(wú)人機(jī)運(yùn)動(dòng)軌跡圖Fig.3 Trajectory diagram of the UAV with slung load

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

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

圖6 吊掛無(wú)人機(jī)控制輸入力隨時(shí)間歷程Fig.6 The UAV with slung load controls the input force over time

4 總結(jié)

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