摘要： 針對(duì)四旋翼吊掛運(yùn)輸系統(tǒng)，本文引入了四繩吊掛運(yùn)輸方式，開展了四旋翼四繩吊掛運(yùn)輸系統(tǒng)的動(dòng)力學(xué)建模、軌跡規(guī)劃與控制研究。根據(jù)四旋翼和負(fù)載的相對(duì)位姿推導(dǎo)了繩索張力，建立了四旋翼四繩吊掛運(yùn)輸系統(tǒng)的動(dòng)力學(xué)模型。為平衡運(yùn)輸時(shí)間與負(fù)載擺動(dòng)抑制，引入時(shí)間和負(fù)載擺幅作為復(fù)合性能指標(biāo)，利用偽譜法將原本的最優(yōu)控制問題轉(zhuǎn)換為非線性規(guī)劃問題，求解了四旋翼四繩吊掛運(yùn)輸系統(tǒng)的最優(yōu)位置軌跡，設(shè)計(jì)了軌跡跟蹤算法，并通過數(shù)值仿真和實(shí)驗(yàn)驗(yàn)證了這一軌跡的合理性。

關(guān)鍵詞： 無人機(jī)吊掛運(yùn)輸； 非線性規(guī)劃； 擺動(dòng)抑制； 偽譜法

中圖分類號(hào)： V279； O322 文獻(xiàn)標(biāo)志碼： A 文章編號(hào)： 1004-4523（2024）10-1747-11

DOI：10.16385/j.cnki.issn.1004-4523.2024.10.013

引 言

無人機(jī)是通過無線遙控裝置或機(jī)載計(jì)算機(jī)內(nèi)置程序控制的無人駕駛飛行器。在過去的30多年里，無人機(jī)的普及度以前所未有的速度持續(xù)增長。目前有50多個(gè)國家正在開發(fā)超過1000種無人機(jī)型號(hào)，無人機(jī)已成為軍事和民用領(lǐng)域的重要組成部分，其作用不容忽視［1］。

無人機(jī)的回收技術(shù)有多種選擇，從空中懸停、傘降、起飛架滑行、攔阻網(wǎng)或天鉤懸停、氣墊懸停到垂直懸停［2?7］，都能夠滿足不同的需求。由于復(fù)雜的地形條件和多種規(guī)則的約束，垂直著陸的回收效率要遠(yuǎn)遠(yuǎn)高于其他回收方式。因此，無人機(jī)回收越來越趨向于垂直起降的設(shè)計(jì)，最常見的垂直起降型無人機(jī)為多旋翼無人機(jī)。

鑒于欠驅(qū)動(dòng)、強(qiáng)耦合和非線性等特點(diǎn)，大量學(xué)者關(guān)注了四旋翼運(yùn)輸系統(tǒng)［8］。四旋翼運(yùn)輸負(fù)載的方法有兩種。一種是裝配夾持器，但這種附加機(jī)構(gòu)會(huì)顯著提高四旋翼的附加慣性，從而減緩其姿態(tài)反應(yīng)的速度［9］。另一種方法是通過繩索連接四旋翼和負(fù)載，保持了四旋翼的敏捷性［10］。

本文針對(duì)第二種方法開展研究。采用四旋翼吊掛運(yùn)輸系統(tǒng)能夠有效避免負(fù)載外形與夾具的不匹配，減少負(fù)載對(duì)四旋翼轉(zhuǎn)動(dòng)慣量的影響，提升其靈活性，使其能夠在復(fù)雜的環(huán)境中迅速、有效地完成作業(yè)，而且無需考慮陸上交通的限制。

四旋翼吊掛運(yùn)輸系統(tǒng)按連接負(fù)載的繩索數(shù)量可分為四旋翼單繩吊掛運(yùn)輸系統(tǒng)和四旋翼多繩吊掛運(yùn)輸系統(tǒng)。四旋翼單繩吊掛運(yùn)輸系統(tǒng)是利用四旋翼位置運(yùn)動(dòng)和負(fù)載振蕩之間的自然耦合來實(shí)現(xiàn)對(duì)負(fù)載的擺動(dòng)抑制。文獻(xiàn)［11］研究了弱非線性條件下受控系統(tǒng)的穩(wěn)定性，并設(shè)計(jì)了一個(gè)實(shí)驗(yàn)系統(tǒng)，以便更好地控制繩系單體系統(tǒng)的面內(nèi)運(yùn)動(dòng)。文獻(xiàn)［12］提出了一種新的方法，將機(jī)器人的振動(dòng)控制轉(zhuǎn)換為模態(tài)空間，以便更好地控制柔性機(jī)器人的低階振動(dòng)，該方法通過規(guī)劃支桿的局部運(yùn)動(dòng)實(shí)現(xiàn)這一目標(biāo)。文獻(xiàn)［13］提出了一種非線性有限時(shí)間控制策略來解決四旋翼吊掛運(yùn)輸系統(tǒng)的運(yùn)動(dòng)控制問題，實(shí)現(xiàn)了存在擾動(dòng)情況下四旋翼吊掛運(yùn)輸系統(tǒng)的高性能運(yùn)動(dòng)控制。Guerrero?Sánchez等［14］提出了一種基于阻尼分配?無源控制策略，以穩(wěn)定四旋翼繩索吊掛負(fù)載的擺動(dòng)。Foehn等［15］將軌跡優(yōu)化問題轉(zhuǎn)化為一種具有互補(bǔ)約束的數(shù)學(xué)模型，從而實(shí)現(xiàn)了期望的軌跡規(guī)劃。

與只有單繩的四旋翼吊掛運(yùn)輸系統(tǒng)相比，四旋翼多繩吊掛運(yùn)輸系統(tǒng)中負(fù)載擺動(dòng)與四旋翼姿態(tài)運(yùn)動(dòng)也存在耦合，因此負(fù)載擺動(dòng)可以得到更有效的抑制。文獻(xiàn)［16］研究了一種新型四旋翼航空運(yùn)輸系統(tǒng)，討論了繩索不同狀態(tài)下的穩(wěn)定條件，以顯示四根繩索系統(tǒng)的優(yōu)點(diǎn)，并在李群SO（3）上設(shè)計(jì)了一種分層自適應(yīng)控制器以完成四旋翼負(fù)載運(yùn)輸。

四旋翼吊掛運(yùn)輸系統(tǒng)的負(fù)載擺動(dòng)幅值依賴于期望的運(yùn)輸路徑，本文將通過路徑規(guī)劃來實(shí)現(xiàn)負(fù)載擺動(dòng)抑制。路徑規(guī)劃旨在尋求一條特定目標(biāo)函數(shù)的最佳路線，以便實(shí)現(xiàn)從起點(diǎn)到目的地的運(yùn)動(dòng)［17］?；跀?shù)學(xué)模型的算法可以分為線性規(guī)劃法（Linear Programming，LP）和非線性規(guī)劃法（Nonlinear Programming，NLP）。

Fiaz等［18］提出了一種混合組合方法，將度量時(shí)態(tài)邏輯規(guī)范轉(zhuǎn)化為線性約束，并使用混合整數(shù)線性規(guī)劃求解器求解所需路徑的相關(guān)最優(yōu)控制問題。Tang等［19］提出了一種基于混合整數(shù)二次規(guī)劃的方法，通過優(yōu)化運(yùn)動(dòng)軌跡，實(shí)現(xiàn)了在已知障礙環(huán)境中對(duì)四旋翼吊掛負(fù)載運(yùn)輸?shù)目刂啤８鶕?jù)文獻(xiàn)［18?19］的研究結(jié)果，線性規(guī)劃算法具有結(jié)構(gòu)緊湊、運(yùn)行效率極高、操作便捷等特性，然而，由于計(jì)算能力受到局部變量的影響，其處理能力受到一定的限制。文獻(xiàn)［20］描述了一種基于非線性模型預(yù)測控制和閉環(huán)局部軌跡規(guī)劃的在線方法，實(shí)現(xiàn)了動(dòng)態(tài)環(huán)境下無人機(jī)吊掛負(fù)載系統(tǒng)的避障控制。Liang等［21］將原來的時(shí)間最優(yōu)問題轉(zhuǎn)化為非線性規(guī)劃問題，使用非線性規(guī)劃技術(shù)對(duì)四旋翼運(yùn)輸系統(tǒng)進(jìn)行動(dòng)力學(xué)分析和時(shí)間優(yōu)化運(yùn)動(dòng)規(guī)劃。

綜上所述的文獻(xiàn)研究都是圍繞四旋翼單繩吊掛運(yùn)輸系統(tǒng)的，目前針對(duì)四旋翼四繩吊掛運(yùn)輸系統(tǒng)的研究相對(duì)較少，本文在文獻(xiàn)［21］的基礎(chǔ)上研究了四旋翼四繩吊掛運(yùn)輸問題，引入時(shí)間和負(fù)載擺幅作為復(fù)合性能指標(biāo)，求解了最優(yōu)軌跡。首先，給出繩索張力約束下的四旋翼和負(fù)載的動(dòng)力學(xué)方程；其次，通過偽譜法將四旋翼四繩吊掛運(yùn)輸系統(tǒng)的運(yùn)動(dòng)軌跡規(guī)劃問題轉(zhuǎn)換為一類非線性規(guī)劃問題，求解得到最優(yōu)軌跡；最后，通過數(shù)值仿真與飛行實(shí)驗(yàn)驗(yàn)證最優(yōu)軌跡的合理性。本文針對(duì)四旋翼四繩吊掛運(yùn)輸系統(tǒng)，建立了高精度動(dòng)力學(xué)模型，給出了最優(yōu)路徑規(guī)劃方法，四旋翼四繩吊掛運(yùn)輸系統(tǒng)通過此方法準(zhǔn)確地運(yùn)輸負(fù)載到達(dá)了目標(biāo)點(diǎn)。

1 飛行控制建模

圖1展示了一種四旋翼四繩吊掛運(yùn)輸系統(tǒng)。坐標(biāo)系為固定在地面上的慣性坐標(biāo)系，坐標(biāo)系為固定在四旋翼機(jī)身上的體坐標(biāo)系。

1.1 動(dòng)力學(xué)模型

本文將所運(yùn)輸?shù)呢?fù)載近似為一個(gè)質(zhì)點(diǎn)，因此，四旋翼四繩吊掛運(yùn)輸系統(tǒng)中無人機(jī)和負(fù)載的運(yùn)動(dòng)學(xué)模型可以分別表示為：

（1）

（2）

（3）

式中 和分別為四旋翼質(zhì)心和負(fù)載在慣性系中的位置矢量；和分別為四旋翼和負(fù)載在慣性系中的速度矢量；表示滾轉(zhuǎn)角、俯仰角和偏航角構(gòu)成的向量；為四旋翼角速度矢量；為角速度對(duì)應(yīng)Xb，Yb和Zb軸轉(zhuǎn)換為姿態(tài)角的變換矩陣，具體表達(dá)式如下：

（4）

四旋翼四繩吊掛運(yùn)輸系統(tǒng)中無人機(jī)和負(fù)載的動(dòng)力學(xué)模型在慣性坐標(biāo)系下可以分別表示為：

（5）

（6）

（7）

式中 和分別為四旋翼和負(fù)載在慣性坐標(biāo)系中的加速度矢量；為四根繩索在慣性坐標(biāo)系下的張力之和；和分別為四旋翼和負(fù)載的重力，其中，g為重力加速度；m1和m2分別為四旋翼和負(fù)載的質(zhì)量；為四旋翼轉(zhuǎn)動(dòng)慣量；為升力對(duì)四旋翼質(zhì)心產(chǎn)生的力矩；為四根繩索對(duì)四旋翼質(zhì)心產(chǎn)生的力矩之和；FT表示四旋翼在慣性坐標(biāo)系下的升力，其具體表達(dá)式為：

（8）

式中 為四旋翼在體坐標(biāo)系下的升力；R為體坐標(biāo)系轉(zhuǎn)換為慣性坐標(biāo)系的旋轉(zhuǎn)矩陣。

1.2 繩索的張力、阻力和力矩

第l根繩索的方向矢量記為。繩索的方向矢量及其導(dǎo)數(shù)可由三角形的矢量關(guān)系獲得，關(guān)系式如下式所示：

（9）

（10）

式中 表示第l根繩索在機(jī)體坐標(biāo)系下連接點(diǎn)的位置矢量。第l根繩索的長度和變化率分別如下式所示：

（11）

（12）

繩索有張力的條件是被拉伸，為此可以引入函數(shù)，其定義為：

當(dāng)繩索緊繃時(shí)：

（13）

當(dāng)繩索松弛時(shí)：

（14）

第l根繩索的張力和四根繩索的合力可以表示為：

（15）

式中 bl表示第l根繩索的初始長度；表示繩索的彈性系數(shù)。本文阻力假設(shè)為零。

第l根繩索的張力力矩τl和四根繩索的合力矩τ可以表示為：

（16）

式中 Tl表示四旋翼在機(jī)體坐標(biāo)系下第l根繩索的張力，其具體表達(dá)式為：

（17）

式中 R1為慣性坐標(biāo)系轉(zhuǎn)換為體坐標(biāo)系的旋轉(zhuǎn)矩陣。

2 最優(yōu)軌跡求解

2.1 偽譜法

本文的飛行器軌跡優(yōu)化是一個(gè)非線性，且受到繩長、推力、力矩、角度、速度等約束的最優(yōu)控制問題。通過直接優(yōu)化性能指標(biāo)，軌跡優(yōu)化的數(shù)值方法可以分為間接法和直接法，以滿足不同的需求。間接法通過Lagrange算子法［22］和變分法［23］推導(dǎo)最優(yōu)控制一階必要條件，將軌跡優(yōu)化問題轉(zhuǎn)換為Hamiltonian多點(diǎn)邊值問題［24］。直接法將優(yōu)化軌跡問題通過變量離散化離散為非線性規(guī)劃問題。因?yàn)殚g接法在處理小規(guī)模問題時(shí)與直接法求解的優(yōu)化結(jié)果相差無幾，卻需要更長的計(jì)算時(shí)間和成本，同時(shí)間接法受初始條件的影響較大，而直接法通常受初始條件的影響不大［25］，因此本文采用直接法來解決軌跡優(yōu)化問題，以達(dá)到更好的效果。

根據(jù)配點(diǎn)法，直接法可以分為歐拉法、Runge?Kutta法、Hermit?Simpson法以及偽譜法［26?29］，它們各自具有獨(dú)特的優(yōu)勢。鑒于偽譜法具有出色的計(jì)算性能，且在航空航天領(lǐng)域中已成為一種有力的解決方案，因此本文將采用它來實(shí)現(xiàn)軌跡優(yōu)化的目標(biāo)。偽譜法有Legendre，Radau以及Gauss等多種方法［30?32］，它們都能夠提供準(zhǔn)確的信息。Radau偽譜法是一種有效的NLP算法，它可以通過協(xié)態(tài)映射定理和NLP算法的最優(yōu)解來確定初始點(diǎn)的控制變量，因此被廣泛用于解決軌跡優(yōu)化問題。Radau偽譜法在網(wǎng)格優(yōu)化時(shí)采用的3種方法分別為h方法、p方法和hp方法［33?35］。本文采用hp自適應(yīng)Radau偽譜法，它將h方法和p方法的優(yōu)勢結(jié)合以求解軌跡優(yōu)化問題，從而獲得更好的效果。本文選取的軟件是采用hp自適應(yīng)Radau偽譜法的MATLAB最優(yōu)控制軟件GPOPS?Ⅱ ［36］。

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

將式（1）～（3）中變量組成一組狀態(tài)向量x=［x1 y1 z1 x2 y2 z2 vx1 vy1 vz1 vx2 vy2 vz2 ? θ ψ wx wy wz］T18×1，可以把方程（1）～（3），（5）～（7）轉(zhuǎn)化為下式中的狀態(tài)空間形式：

（18）

式中 和分別為關(guān)于狀態(tài)向量x的向量和矩陣；U=［06×1 u1 u1 u1 06×1 u2 u3 u4］18×1；；h（y）18×1為關(guān)于狀態(tài)向量y的矩陣；為重力和力矩的組合矩陣。

2.3 優(yōu)化性能指標(biāo)

本文的性能指標(biāo)是四旋翼四繩吊掛運(yùn)輸系統(tǒng)的負(fù)載擺幅。因此，性能指標(biāo)函數(shù)定義如下：

（19）

式中 tf為終端時(shí)間；x20，y20分別為負(fù)載在Xe，Ye方向上的初始坐標(biāo)。

2.4 路徑約束

考慮到工程中無人機(jī)的驅(qū)動(dòng)能力有限，本文限制了無人機(jī)的推力和力矩以及姿態(tài)角。主要考慮的約束如下：

（1）推力u1和力矩u2，u3，u4約束如下：

（20）

（2）角度?，θ，ψ的約束如下：

（21）

（22）

（23）

2.5 Radau偽譜法求解

Radau偽譜法的工作原理是將系統(tǒng)問題的狀態(tài)變量、控制變量和離散約束條件通過在一系列Legendre?Guass?Radau（LGR）點(diǎn)上構(gòu)建Lagrange插值多項(xiàng)式轉(zhuǎn)換為非線性規(guī)劃問題，然后利用IPOPT求解器對(duì)這些非線性問題進(jìn)行求解得到最優(yōu)方案。由于LGR配置點(diǎn)位于半開區(qū)間中，首先需要把τ轉(zhuǎn)換為四旋翼飛行的連續(xù)時(shí)間，轉(zhuǎn)換公式如下：

（24）

式（18）～（23）相應(yīng)地轉(zhuǎn)換為：

微分狀態(tài)方程：

（25）

優(yōu)化性能指標(biāo)：

（26）

路徑約束：

（27）

（28）

（29）

（30）

然后對(duì)上述公式進(jìn)行近似，標(biāo)準(zhǔn)Legendre? Gauss?Radau（LGR）點(diǎn)定義為多項(xiàng)式的根，其中為N階Legendre多項(xiàng)式：

（31）

（1）狀態(tài)向量的近似

記優(yōu)化軌跡上的N個(gè)標(biāo)準(zhǔn)LGR點(diǎn)分別為，同時(shí)增加節(jié)點(diǎn)為插值節(jié)點(diǎn)，則狀態(tài)向量可由Lagrange全局插值多項(xiàng)式近似：

（32）

（33）

式中 表示狀態(tài)變量x在插值節(jié)點(diǎn)處的取值；表示一個(gè)Lagrange插值基函數(shù)，它具有正交性，并且具有如下的分布特征：

（34）

（2）控制變量的近似

（35）

式中 。

（3）微分狀態(tài)方程約束的近似

（36）

在標(biāo)準(zhǔn)LGR點(diǎn)處，式（38）可以整理為：

（37）

式中 Dji為N×（N+1）維Radau偽譜微分矩陣：

（38）

式中 。

將式（39）代入微分狀態(tài)方程（26）的左端，即可在配點(diǎn)處將微分狀態(tài)方程轉(zhuǎn)換為代數(shù)方程：

（39）

（4）性能指標(biāo)的近似

根據(jù)Gauss?Radau積分法則，需要在標(biāo)準(zhǔn)LGR點(diǎn)上進(jìn)行積分，以計(jì)算出性能指標(biāo)式（27）右側(cè)的積分項(xiàng)：

（40）

式中 wi為標(biāo)準(zhǔn)LGR點(diǎn)τi的積分權(quán)重系數(shù)：

（41）

通過將式（40）等式右端替換為式（26），可以獲得一個(gè)近似的性能指標(biāo)表示方法：

（42）

（5）路徑約束條件離散

（43）

（44）

（45）

（46）

通過以上變換，連續(xù)最優(yōu)控制問題就轉(zhuǎn)換成非線性規(guī)劃問題，通過求解器尋找參數(shù)，，在滿足參數(shù)化的代數(shù)方程（35），（39）以及約束條件（43）～（46）的前提下，使得性能指標(biāo)（42）最小。

3 跟蹤優(yōu)化軌跡

為了驗(yàn)證GPOPS求得的Xe，Ye，Ze軸方向的最優(yōu)軌跡的可行性，采用了外環(huán)PD控制＋內(nèi)環(huán)PID控制的控制系統(tǒng)來跟蹤四旋翼的軌跡。系統(tǒng)的外環(huán)位置控制器根據(jù)最優(yōu)軌跡確定期望的俯仰角和偏航角，并將二者和控制力輸入到內(nèi)環(huán)姿態(tài)控制器中。通過這種方式可以調(diào)整四旋翼的姿態(tài)，從而驗(yàn)證最優(yōu)軌跡的可行性。本文控制系統(tǒng)結(jié)構(gòu)如圖2所示。

3.1 外環(huán)位置控制器

根據(jù)前面建立的四旋翼四繩吊掛運(yùn)輸系統(tǒng)的模型，在去除負(fù)載和繩子的作用下，外環(huán)四旋翼位置模型可以由下式描述：

（47）

（48）

（49）

設(shè)分別為四旋翼在Xe，Ye，Ze軸上的期望位置，分別為的PD虛擬位置控制輸入。如下式所示：

（50）

（51）

（52）

式中 分別為四旋翼位置的比例系數(shù)；分別為四旋翼位置的微分系數(shù)。

因?yàn)樵谒男鞵ID控制中的期望控制量是，所以設(shè)計(jì)推力的期望控制量，即外環(huán)控制器如下式所示：

（53）

3.2 內(nèi)環(huán)姿態(tài)控制器

內(nèi)環(huán)姿態(tài)控制器的輸入是期望滾轉(zhuǎn)角?d和期望俯仰角θd，根據(jù)式（49），（50），（52），（53）推導(dǎo)獲得，推導(dǎo)結(jié)果如下式所示：

（54）

（55）

同理，根據(jù)推導(dǎo)外環(huán)四旋翼位置模型的方式，推導(dǎo)得內(nèi)環(huán)姿態(tài)模型如下式所示：

（56）

（57）

（58）

設(shè)分別為四旋翼在Xb，Yb，Zb軸上的期望姿態(tài)角，建立的PID虛擬姿態(tài)控制輸入如下式所示：

（59）

（60）

（61）

式中 分別為四旋翼姿態(tài)角的比例系數(shù)；分別為四旋翼姿態(tài)角的積分系數(shù)；分別為四旋翼姿態(tài)角的微分系數(shù)。

考慮到轉(zhuǎn)動(dòng)慣量很小，為了簡化計(jì)算和提高計(jì)算效率，忽略了姿態(tài)角一階導(dǎo)項(xiàng)對(duì)姿態(tài)角動(dòng)力學(xué)模型的影響。因此，在時(shí)姿態(tài)角一階導(dǎo)項(xiàng)不存在，推導(dǎo)式（58）～（63）分別得到力矩的期望控制量，設(shè)計(jì)內(nèi)環(huán)控制器如下式所示：

（62）

（63）

（64）

4 數(shù)值仿真

4.1 軌跡優(yōu)化仿真結(jié)果

通過數(shù)值仿真，本節(jié)對(duì)前述軌跡優(yōu)化方法進(jìn)行了詳細(xì)的驗(yàn)證和評(píng)估。表1為四旋翼四繩吊掛運(yùn)輸系統(tǒng)的物理參數(shù)。

為了不影響實(shí)驗(yàn)無人機(jī)的攝像機(jī)正常工作，四根繩子的連接點(diǎn)A1，A2，A3，A4位置分布如圖3所示，其中，d2，d3分布如圖3所示。

設(shè)變量下標(biāo)加“0”為初始狀態(tài)值，變量下標(biāo)加“f”為末端狀態(tài)值，四旋翼和負(fù)載變量的初、末狀態(tài)值如表2所示，四旋翼的旋翼控制力和控制力矩以及姿態(tài)角的約束范圍如表3所示。

在數(shù)值仿真中，利用GPOPS對(duì)優(yōu)化問題進(jìn)行離散化處理。本文四旋翼四繩吊掛運(yùn)輸系統(tǒng)的最優(yōu)位置軌跡如圖4所示，從圖中可以看出四旋翼運(yùn)輸負(fù)載到達(dá)期望位置的時(shí)間為10 s。為更好地觀察負(fù)載的擺動(dòng)，建立了一個(gè)誤差擺動(dòng)的變量：

（65）

求出的擺動(dòng)幅值如圖5所示。其最大值為0.01279 m，平均值為0.000347 m。本文通過設(shè)置控制器跟蹤軌跡和實(shí)驗(yàn)驗(yàn)證來證明此軌跡的合理性。

4.2 軌跡跟蹤結(jié)果

采用先調(diào)節(jié)內(nèi)環(huán)后調(diào)節(jié)外環(huán)的方法調(diào)節(jié)系統(tǒng)各個(gè)PID控制器的參數(shù)。經(jīng)過精心調(diào)試，表4顯示了最終的成果。

四旋翼四繩吊掛運(yùn)輸系統(tǒng)運(yùn)輸負(fù)載到達(dá)期望位置的軌跡跟蹤如圖6～9所示。

由圖6～8可知，PID控制跟蹤軌跡在10 s內(nèi)到達(dá)目標(biāo)位置，Xe，Ye，Ze方向的最終跟蹤誤差分別小于0.143，0.036，0.026 m。

由圖9可知，PID控制跟蹤軌跡在10 s后最大擺動(dòng)誤差小于0.058 m，因此仿真結(jié)果表明，控制系統(tǒng)實(shí)現(xiàn)了四旋翼對(duì)最優(yōu)軌跡的跟蹤。

5 飛行實(shí)驗(yàn)

為了驗(yàn)證本文所得到的優(yōu)化軌跡，開展了室內(nèi)四旋翼無人機(jī)吊掛運(yùn)輸系統(tǒng)飛行實(shí)驗(yàn)，該實(shí)驗(yàn)平臺(tái)主要由三部分組成：一是四旋翼無人機(jī)吊掛運(yùn)輸系統(tǒng)，包括TELLO四旋翼和吊掛負(fù)載，其中為了實(shí)現(xiàn)運(yùn)動(dòng)捕捉，四旋翼上設(shè)置了3個(gè)標(biāo)記點(diǎn)，負(fù)載上設(shè)置了1個(gè)標(biāo)記點(diǎn)來跟蹤軌跡，如圖10所示；二是運(yùn)動(dòng)捕捉系統(tǒng)，由8個(gè)型號(hào)為OptiTrack PrimeX13的動(dòng)作捕捉相機(jī)組成，測量四旋翼飛行器和負(fù)載的位置，并通過TCP/IP通信協(xié)議傳輸?shù)降孛婀ぷ髡?，如圖11所示；三是地面工作站，用來連接TELLO四旋翼和接收來自運(yùn)動(dòng)捕捉系統(tǒng)的位置信息。表5為四旋翼四繩吊掛運(yùn)輸系統(tǒng)實(shí)驗(yàn)時(shí)所用的參數(shù)。

通過MATLAB發(fā)送指令到TELLO確定初始位置，令初始位置為［0； 0； 0］，使TELLO沿著最優(yōu)軌跡向目標(biāo)點(diǎn)［1； 1 ；0］飛行。飛行軌跡如圖12～15所示。

從圖12～14中可以看出，TELLO飛行軌跡與GPOPS飛行軌跡在Xe，Ye，Ze方向，10 s后的跟蹤誤差分別小于0.14，0.171和0.136 m。

由圖15可知，負(fù)載飛行擺動(dòng)曲線在10 s后最大擺動(dòng)誤差小于0.039 m。因?yàn)樵陲w行實(shí)驗(yàn)中，四旋翼達(dá)到期望位置的擺幅較小，所以得出了最優(yōu)軌跡被跟蹤的結(jié)論，這證明了求解的最優(yōu)軌跡是有效且合理的。

6 結(jié) 論

本文建立了四旋翼四繩吊掛運(yùn)輸系統(tǒng)的三維模型，有效地改善了四旋翼飛行的軌跡。用偽譜法將連續(xù)時(shí)間最優(yōu)飛行軌跡問題離散為非線性規(guī)劃問題，通過GPOPS求解得到了四旋翼四繩吊掛運(yùn)輸系統(tǒng)的最優(yōu)飛行軌跡，最后進(jìn)行了軌跡跟蹤仿真和飛行實(shí)驗(yàn)。通過仿真和實(shí)驗(yàn)可以發(fā)現(xiàn)最優(yōu)飛行軌跡是可行且合理的。

參考文獻(xiàn)：

［1］Mohammadhasani A. Intelligent control of a quadrotor with suspended load［D］. Edomonton，Alberta： University of Alberta，2022.

［2］席慶彪，張波，田小雄，等. 基于風(fēng)場估測的無人機(jī)傘降定點(diǎn)回收研究［J］. 西北工業(yè)大學(xué)學(xué)報(bào)，2004，22（4）： 452-456.

Xi Qingbiao，Zhang Bo，Tian Xiaoxiong，et al. On navigation control of UAV in recovery stage to ensure reliable parachute landing on assigned spot with wind disturbance estimation considered［J］. Journal of Northwestern Polytechnical University，2004，22（4）： 452-456.

［3］郭耀江. 無人機(jī)著陸技術(shù)研究［J］. 現(xiàn)代導(dǎo)航，2013，4（3）： 195-197.

Guo Yaojiang . Research on unmanned aerial vehicle （UAV） landing technique［J］. Modern Navigation，2013，4（3）： 195-197.

［4］陳晨，周洲. 無人機(jī)滑跑起飛過程及其數(shù)學(xué)模型研究［J］. 科學(xué)技術(shù)與工程，2007，7（13）： 3198-3201.

Chen Chen，Zhou Zhou. Process and mathematic model study of a UAV during taxing and takeoff［J］. Science Technology and Engineering，2007，7（13）： 3198-3201.

［5］李悅，裴錦華. 無人機(jī)攔阻網(wǎng)回收動(dòng)力學(xué)建模與仿真［J］. 航天返回與遙感，2012，33（1）： 1-7.

Li Yue，Pei Jinhua. Dynamic modeling and simulation of arresting net recovery system for UAV［J］. Spacecraft Recovery & Remote Sensing，2012，33（1）： 1-7.

［6］張忠偉. 無人機(jī)回收氣囊減震性能的計(jì)算分析［J］.液壓與氣動(dòng)，2004，28（2）： 28-31.

Zhang Zhongwei. Analysis for airbag shock absorber served as UAV landing shock absorber［J］. Chinese Hydraulics & Pneumatics，2004，28（2）： 28-31.

［7］王科雷，周洲，馬悅文，等. 垂直起降固定翼無人機(jī)技術(shù)發(fā)展及趨勢分析［J］.航空工程進(jìn)展，2022，13（5）：1-13.

Wang Kelei，Zhou Zhou，Ma Yuewen，et al. Development and trend analysis of vertical takeoff and landing fixed wing UAV［J］. Advances in Aeronautical Science and Engineering，2022，13（5）： 1-13.

［8］梁曉，胡欲立. 四旋翼吊掛運(yùn)輸系統(tǒng)動(dòng)態(tài)反饋線性化軌跡控制［J］.自動(dòng)化學(xué)報(bào)，2020 ，46（9）： 1993-2002.

Liang Xiao，Hu Yuli. Trajectory control of quadrotor with cable-suspended load via dynamic feedback linearization［J］. Acta Automatica Sinica，2020，46（9）： 1993-2002.

［9］Vandanipour M，Khodabandeh M. Adaptive fractional order sliding mode control for a quadrotor with a varying load［J］. Aerospace Science and Technology，2019，86： 737-747.

［10］Sadr S，Moosavian S A A，Zarafshan P. Dynamics modeling and control of a quadrotor with swing load［J］. Journal of Robotics，2014，2014： 265897.

［11］金棟平，丁鋒. 繩系單體系統(tǒng)面內(nèi)運(yùn)動(dòng)的定位控制［J］. 振動(dòng)工程學(xué)報(bào)，2008，21（1）： 1-6.

Jin Dongping，Ding Feng. Position control of in-plane motion of the tethered-payload system［J］. Journal of Vibration Engineering，2008，21（1）： 1-6.

［12］邊宇樞，高志慧，贠超.基于結(jié)構(gòu)特征的柔性機(jī)器人動(dòng)態(tài)性能優(yōu)化研究［J］.振動(dòng)工程學(xué)報(bào)，2009，22（1）：36-40.

Bian Yushu，Gao Zhihui，Yun Chao. Dynamic performance optimization for flexible manipulators based on their structure features［J］. Journal of Vibration Engineering，2009，22（1）： 36-40.

［13］Guerrero-Sánchez M E，Lozano R，Castillo P，et al. Nonlinear control strategies for a UAV carrying a load with swing attenuation［J］. Applied Mathematical Modelling，2021，91： 709-722.

［14］Guerrero-Sánchez M E，Mercado-Ravell D A，Lozano R，et al. Swing-attenuation for a quadrotor transporting a cable-suspended payload［J］. ISA Transactions，2017，68： 433-449.

［15］Foehn P，F(xiàn)alanga D，Kuppuswamy N，et al. Fast trajectory optimization for agile quadrotor maneuvers with a cable-suspended payload［C］//Robotics： Science and Systems . Cambridge，MA，United States： 2017：30-40.

［16］Chen T，Shan J J. A novel cable-suspended quadrotor transportation system： from theory to experiment［J］. Aerospace Science and Technology，2020，104： 105974.

［17］張廣林，胡小梅，柴劍飛，等. 路徑規(guī)劃算法及其應(yīng)用綜述［J］.現(xiàn)代機(jī)械，2011（5）： 85-90.

Zhang Guanglin，Hu Xiaomei，Chai Jianfei，et al. Summary of path planning algorithm and its application［J］. Modern Mechanical，2011（5）： 85-90.

［18］Fiaz U A，Baras J S. Fast，composable rescue mission planning for UAVs using metric temporal logic［J］. IFAC- Papers Online，2020，53（2）： 15404-15411.

［19］Tang S，Kumar V. Mixed integer quadratic program trajectory generation for a quadrotor with a cable-suspended payload［C］//2015 IEEE International Conference on Robotics and Automation （ICRA）. Seattle，Washington，USA，2015.

［20］Potdar N D，de Croon G C H E，Alonso-Mora J，et al. Online trajectory planning and control of a MAV payload system in dynamic environments［J］. Autonomous Robots，2020，44（6）： 1065-1089.

［21］Liang X，F(xiàn)ang Y C，Sun N，et al. Dynamics analysis and time-optimal motion planning for unmanned quadrotor transportation systems［J］. Mechatronics，2018，50： 16-29.

［22］黃建峰. Lagrange展開定理與組合反演論［D］. 蘇州：蘇州大學(xué)，2017.

Huang Jianfeng. Treatise on the Lagrange expansion theorem and combinatorial inversions［D］. Suzhou： Soochow University，2017.

［23］寇海江，袁惠群. 旋轉(zhuǎn)大變形板振動(dòng)高階非線性效應(yīng)的變分法研究［J］. 振動(dòng)工程學(xué)報(bào)，2015，28（1）：44-51.

Kou Haijiang，Yuan Huiqun. High-order nonlinear vibration analysis of the rotating large deflection plate based on the variational principle［J］. Journal of Vibration Engineering，2015，28（1）： 44-51.

［24］黃國強(qiáng)，陸宇平，南英. 飛行器軌跡優(yōu)化數(shù)值算法綜述［J］.中國科學(xué)： 技術(shù)科學(xué)，2012，42（9）： 1016-1036.

Huang Guoqiang，Lu Yuping，Nan Ying. A survey of numerical algorithms for trajectory optimization of flight vehicles［J］. Scientia Sinica（Technologica），2012，42（9）：1016-1036.

［25］陳聰，關(guān)成啟，史宏亮.飛行器軌跡優(yōu)化的直接數(shù)值解法綜述［J］. 戰(zhàn)術(shù)導(dǎo)彈控制技術(shù)，2009（2）： 33-40.

Chen Cong ，Guan Chengqi，Shi Hongliang. Survey of numerical methods for direct aircraft trajectory optimization［J］. Control Technology of Tactical Missile ，2009（2）： 33-40.

［26］Betts J T. Practical Methods for Optimal Control and Estimation Using Nonlinear Programming，Second Edition［M］. US： Society for Industrial and Applied Mathematics，2009： 132-134.

［27］邢譽(yù)峰，馮偉. 李級(jí)數(shù)法與Runge-Kutta法［J］. 振動(dòng)工程學(xué)報(bào)，2007，20（5）： 519-522.

Xing Yufeng，F(xiàn)eng Wei. Lie series algorithms and Runge-Kutta algorithms［J］. Journal of Vibration Engineering，2007，20（5）： 519-522.

［28］Hargraves C R，Paris S W. Direct trajectory optimization using nonlinear programming and collocation［J］. Journal of Guidance，Control，and Dynamics，1987，10（4）： 338-342.

［29］唐怡. 偽譜法中非線性規(guī)劃問題求解方法的應(yīng)用研究［D］. 綿陽：西南科技大學(xué)，2020.

Tang Yi. Application research of nonlinear programming problem solving method in pseudospectral method［D］. Mianyang： Southwest University of Science and Technology，2020.

［30］文浩，金棟平，胡海巖. 繩系衛(wèi)星收放控制地面實(shí)驗(yàn)研究［J］.振動(dòng)工程學(xué)報(bào)，2010，23（1）： 7-11.

Wen Hao，Jin Dongping，Hu Haiyan. Ground-based experimental study on deployment and retrieval control of tethered satellite［J］. Journal of Vibration Engineering，2010，23（1）： 7-11.

［31］Fahroo F，Ross I M. Pseudospectral methods for infinite-horizon nonlinear optimal control problems［J］. Journal of Guidance，Control，and Dynamics，2008，31（4）： 927-936.

［32］張洪倩. 基于高斯偽譜法的彈道優(yōu)化設(shè)計(jì)與實(shí)現(xiàn)［D］.南京：南京理工大學(xué)，2014.

Zhang Hongqian. Trajectory optimization design and realization based on Gaussian pseudospectral method［D］. Nanjing： Nanjing University of Science and Technology，2014.

［33］Jain S，Tsiotras P. Trajectory optimization using multiresolution techniques［J］. Journal of Guidance，Control，and Dynamics，2008，31（5）： 1424-1436.

［34］Zhao Y M，Tsiotras P. Mesh refinement using density function for solving optimal control problems［C］. AIAA Infotech @ Aerospace Conference. Seattle，Washington，USA： 2009.

［35］邱文杰，孟秀云. 基于hp自適應(yīng)偽譜法的飛行器多階段軌跡優(yōu)化［J］.北京理工大學(xué)學(xué)報(bào)，2017，37（4）：412-417.

Qiu Wenjie，Meng Xiuyun. Multi-phase trajectory optimization of vehicle based on hp-adaptive pseudospectral method［J］. Transactions of Beijing Institute of Technology，2017，37（4）： 412-417.

［36］Rao A V，Benson D，Darby C L，et al. User’s manual for GPOPS version 5.0： a MATLAB software for solving multiple-phase optimal control problems using hp-adaptive pseudospectral methods［M］. Grainesville，F(xiàn)L： University of Florida，2011.

Dynamics and control of UAV transportation system with a four-cable-suspended payload

MO Lan，WANG Yan-kai，WEI Ming-hong，CHEN Ti

（State Key Laboratory of Mechanics and Control of Aerospace Structures，Nanjing University of Aeronautics and Astronautics，Nanjing 210016，China）

Abstract： This paper considers a quadrotor transportation system with a four-cable-suspended payload. The relative position between quadrotor and payload is introduced and used to derive the tension of cables and describe the transportation system. A cost function inspired by payload and time is built to equipoise rapid UAV positioning and payload swing elimination. Then，the pseudo-spectral method is applied to transform the optimal control problem into a nonlinear programming problem and solve the optimal trajectory. A quadrotor transportation system’s trajectory tracking is facilitated by a PID controller. The optimal trajectory is validated through the presentation of both simulation and experimental results at last.

Key words： UAV transportation with a cable-suspended payload；nonlinear programming；swing suppression； the pseudo-spectral method

作者簡介： 莫 蘭（1998―），女，碩士研究生。E-mail： molsz2101026@nuaa.edu.cn。

通訊作者： 陳 提（1989―），男，博士，教授。E-mail： chenti@nuaa.edu.cn。