基于補(bǔ)償函數(shù)觀測(cè)器的無人機(jī)吊掛飛行控制

劉肩山 唐毅 謝志明

摘 ?要：無人機(jī)吊掛飛行系統(tǒng)是多變量、欠驅(qū)動(dòng)、強(qiáng)耦合的非線性系統(tǒng)，且該系統(tǒng)存在模型偏差和未知擾動(dòng)，給無人機(jī)位置和吊掛飛行減擺控制帶來了困難。文章設(shè)計(jì)了一種基于補(bǔ)償函數(shù)觀測(cè)器（CFO）的非線性控制方法。首先，基于拉格朗日方程建立系統(tǒng)的動(dòng)力學(xué)模型。然后，設(shè)計(jì)了補(bǔ)償函數(shù)觀測(cè)器來估計(jì)未建模動(dòng)態(tài)和擾動(dòng)，并將估計(jì)值反饋給控制器以大幅減弱其給吊掛飛行減擺控制帶來的不利影響。仿真實(shí)驗(yàn)對(duì)比了線性自抗擾控制（LADRC）以及線性二次型調(diào)節(jié)器（LQR）兩種控制方法，結(jié)果表明設(shè)計(jì)的控制器抗擾性更好、魯棒性更強(qiáng)、控制精度更高。

關(guān)鍵詞：外界干擾；補(bǔ)償函數(shù)觀測(cè)器；障礙Lyapunov函數(shù)；反步控制器；減擺控制

中圖分類號(hào)：TP273.1 ? 文獻(xiàn)標(biāo)識(shí)碼：A ? 文章編號(hào)：2096-4706（2023）16-0062-05

Control of Unmanned Aerial Vehicle with a Slung-load Based on

Compensation Function Observer

LIU Jianshan， TANG Yi， XIE Zhiming

（Changsha Aeronautical Vocational and Technical College， Changsha ?410124， China）

Abstract： Unmanned Aerial Vehicle with a slung-load flight system is a multi-variable， underactuated and strong coupling nonlinear system， and the system has model bias and wind disturbance which increase the difficulty of UAV position and anti-swing control of slung-load flight. This paper designs a nonlinear control method based on CFO. Firstly， the system dynamic model is built by using Lagrangian mechanics equation. Then， a CFO is designed to estimate the unmodelled dynamics and disturbances， and the estimated values are feed back to the controller to greatly reduce its adverse effects on the slung-load flight anti-swing control. The simulation experiment compares the two control methods of LADRC and LQR， and results show that the designed controller has better disturbance immunity， stronger robustness and higher control precision.

Keywords： external disturbance; Compensation Function Observer; barrier Lyapunov function; backstepping controller; anti-swing control

0 ?引 ?言

四旋翼無人機(jī)吊掛飛行因其具有無須考慮吊掛負(fù)載外形影響、不受地理?xiàng)l件限制、效率高、成本低、操作靈活等優(yōu)點(diǎn)，在軍用和民用的各個(gè)領(lǐng)域得到應(yīng)用，如物資運(yùn)輸、搶險(xiǎn)救災(zāi)、水質(zhì)監(jiān)測(cè)取樣、雷區(qū)探測(cè)掃描等[1，2]。四旋翼吊掛無人機(jī)是一個(gè)多變量、非線性、欠驅(qū)動(dòng)、強(qiáng)耦合以及時(shí)變的系統(tǒng)，在實(shí)際飛行中，系統(tǒng)穩(wěn)定性會(huì)受到負(fù)載擺動(dòng)的影響，且存在系統(tǒng)參數(shù)變化、外界干擾、模型不精確等問題，使無人機(jī)吊掛飛行控制變得復(fù)雜。因此，為了擴(kuò)展其應(yīng)用范圍，設(shè)計(jì)具有高抗擾能力的控制器就顯得十分有必要，也成為近年來研究的熱點(diǎn)和難點(diǎn)。

由于吊掛負(fù)載并不能給系統(tǒng)增加控制輸入，四旋翼無人機(jī)吊掛系統(tǒng)是欠驅(qū)動(dòng)的，對(duì)該系統(tǒng)的建模主要使用歐拉—拉格朗日方程[3]，該方法基于能量平衡原理，不需要求解吊掛系繩上的作用力。文獻(xiàn)[4，5]在建模過程中考慮了更多的細(xì)節(jié)，如系繩的彈性、有無張力、以及負(fù)載空氣動(dòng)力學(xué)對(duì)無人機(jī)飛行控制的影響。Lee將無人機(jī)吊掛系統(tǒng)控制輸入分解為機(jī)體系的垂直分量和水平分量，在水平分量上采用反饋線性化控制[6]，不過該方法控制效果取決于系統(tǒng)模型是否精確。文獻(xiàn)[7]針對(duì)負(fù)載軌跡跟蹤控制，基于反步法設(shè)計(jì)了Lyapunov函數(shù)，證明了閉環(huán)系統(tǒng)的漸近穩(wěn)定性，實(shí)際的飛行實(shí)驗(yàn)也驗(yàn)證了無人機(jī)吊掛系統(tǒng)所有狀態(tài)的一致有界。文獻(xiàn)[8]采用分層滑?？刂品椒▉斫鉀Q控制變量耦合問題，并利用吊掛刀鋸擺角信息設(shè)計(jì)了無人機(jī)位置和負(fù)載擺角綜合控制器。

無人機(jī)吊掛系統(tǒng)需要考慮建模不精確、參數(shù)攝動(dòng)和外擾對(duì)飛行控制的影響，為提高系統(tǒng)的動(dòng)態(tài)性能和魯棒性，文獻(xiàn)[9]提出將被控對(duì)象存在的耦合、內(nèi)擾和外擾等直接視為總擾動(dòng)，設(shè)計(jì)擴(kuò)張觀測(cè)器（ESO）估計(jì)總擾動(dòng)。文獻(xiàn)[10]提出了補(bǔ)償函數(shù)觀測(cè)器，通過添加補(bǔ)償函數(shù)來抵消未知函數(shù)對(duì)估計(jì)精度的影響，能高精度估計(jì)未建模動(dòng)態(tài)和擾動(dòng)。

本文基于拉格朗日方程建立了四旋翼無人機(jī)吊掛系統(tǒng)動(dòng)力學(xué)模型，考慮到強(qiáng)耦合、空氣阻力、未建模動(dòng)態(tài)和風(fēng)擾等因素對(duì)飛行控制的影響，將無人機(jī)的位置和負(fù)載擺角綜合為一個(gè)控制向量，對(duì)被控變量進(jìn)行同頻率控制；設(shè)計(jì)CFO對(duì)擾動(dòng)和模型偏差進(jìn)行估計(jì)，并將估計(jì)值反饋給比例微分控制器，提高了系統(tǒng)的準(zhǔn)確性和穩(wěn)定性；最后，將本文設(shè)計(jì)的控制算法與基于LADRC和LQR的控制算法進(jìn)行了比較，結(jié)果表明本文設(shè)計(jì)的控制算法具有更好的控制效果。

1 ?系統(tǒng)模型分析

四旋翼無人機(jī)吊掛飛行系統(tǒng)的結(jié)構(gòu)簡(jiǎn)圖如圖1所示：圖1（a）中小球通過細(xì)繩系在無人機(jī)底部平板中心位置上；圖1（b）為其對(duì)應(yīng)的2維平面模型。

圖1（a）中，oi xi yi zi 表示慣性坐標(biāo)系，坐標(biāo)原點(diǎn)一般為無人機(jī)起飛點(diǎn)；ob xb yb zb表示無人機(jī)的機(jī)體坐標(biāo)系，坐標(biāo)原點(diǎn)位于無人機(jī)的質(zhì)心。圖1（b）中，f表示無人機(jī)的總升力，γ表示負(fù)載擺角，l表示細(xì)繩的長(zhǎng)度，θ表示無人機(jī)的俯仰角，mq和ml分別表示無人機(jī)和負(fù)載的質(zhì)量。根據(jù)實(shí)際飛行情況，可做如下合理假設(shè)：

1）將吊掛負(fù)載看作質(zhì)點(diǎn)，且吊掛點(diǎn)和無人機(jī)的質(zhì)心重合；

2）吊掛細(xì)繩質(zhì)量忽略不計(jì)，其長(zhǎng)度不會(huì)發(fā)生變化，且始終是張緊的；

3）吊掛負(fù)載始終在無人機(jī)的下方，即吊掛負(fù)載的擺角| γ | = π / 2。

基于拉格朗日方程可建立無人機(jī)吊掛飛行系統(tǒng)動(dòng)力學(xué)模型：

（1）

式中，q = [xq，zq，γ]T，表示系統(tǒng)的狀態(tài)向量，（xq，zq）表示無人機(jī)在慣性坐標(biāo)系中的二維平面位置。Mc（q），，G（q），Q分別表示系統(tǒng)的慣量矩陣，向心力矩陣，重力向量，廣義力向量。

Mc（q）的表達(dá)式為：

（2）

的表達(dá)式為：

（3）

G（q）的表達(dá)式為：

（4）

廣義力Q可表示為：

（5）

式中，U = [ux uz 0]T，表示升力向量，也是系統(tǒng)的控制輸入，，表示空氣阻力向量，dx，dz，dγ為空氣阻尼系數(shù)。

由式（2）可知，Mc（q）可逆，另外Mc（q），，G（q）均表示非定常矩陣，F(xiàn)d表示未知向量，可將式（1）寫成：

（6）

其中，Mc0表示Mc（q）-1的標(biāo)稱值，F(xiàn)u表示總擾

動(dòng)，其表達(dá)式為：

（7）

本文的研究目標(biāo)是保證無人機(jī)在慣性坐標(biāo)系xi和zi方向上運(yùn)動(dòng)到目標(biāo)位置，同時(shí)吊掛負(fù)載擺角收斂到0，可用下述數(shù)學(xué)語言描述：

，，（8）

式中，xqd和zqd表示無人機(jī)在慣性坐標(biāo)系下x方向和z方向的期望位置。

2 ?控制器設(shè)計(jì)

2.1 ?補(bǔ)償函數(shù)觀測(cè)器

CFO比ESO多使用了狀態(tài)向量的微分信息，且通過對(duì)狀態(tài)向量誤差及其微分進(jìn)行積分來補(bǔ)償未知模型函數(shù)項(xiàng)Fu的影響。CFO比ESO高兩個(gè)型別，為Ⅲ型系統(tǒng)，其精確度和收斂性都要優(yōu)于ESO。另外，CFO是指數(shù)收斂的，在Fu為三階無窮小函數(shù)向量的條件下，其擴(kuò)張狀態(tài)向量能以零穩(wěn)態(tài)誤差收斂到Fu。由式（6）可得系統(tǒng)的狀態(tài)方程：

（9）

其中，x1 = q，U表示控制輸入向量。由式（9）可得CFO表達(dá)式：

（10）

其中， 表示對(duì)q的估計(jì)值， 表示對(duì)總擾動(dòng)Fu的估計(jì)值， 表示誤差矩陣，λ表示濾波參數(shù)矩陣，L = [l1，l2]表示增益矩陣，l1，l2，λ均表示三階對(duì)角矩陣，其表達(dá)式如下：

，，（11）

式中，l1i，l2i，λi（i = x，z，γ）滿足如下特征方程：

s3 + l1i s2 + （ λi l1i + l2i ） s + λi l2i = 0 ? ? ? ?（12）

配置觀測(cè)器的極點(diǎn)為s1 = -wi，s2， 3 = -4wi，有l(wèi)1i = 9wi，l2i = 12wi2，λi = 4 / 3 wi，wi表示通道的帶寬。

2.2 ?控制器設(shè)計(jì)

通過CFO對(duì)系統(tǒng)的觀測(cè)，可以得到總擾動(dòng)估計(jì)值 ，結(jié)合總擾動(dòng)主動(dòng)補(bǔ)償和跟蹤誤差比例微分反饋的控制律設(shè)計(jì)如下：

（13）

其中，qd = [xqd，zqd，γqd]T表示無人機(jī)位置x、位置z和吊掛負(fù)載擺角γ的期望值組成的列向量，KP和Kd的表達(dá)式如下：

，（14）

式中，ωcx、ωcz和ωcγ分別表示控制器在位置x，位置z和擺角γ通道的帶寬。

3 ?仿真實(shí)驗(yàn)

為驗(yàn)證設(shè)計(jì)的控制器對(duì)無人機(jī)吊掛飛行系統(tǒng)的控制效果，在[10，20] s將如圖2所示的風(fēng)擾注入系統(tǒng)的x通道和z通道，在仿真的第30 s至35 s模擬了吊掛負(fù)載質(zhì)量持續(xù)減少一半的控制效果，并通過與LADRC和LQR控制方法進(jìn)行對(duì)比，以驗(yàn)證所提算法的有效性及優(yōu)越性。四旋翼無人機(jī)吊掛系統(tǒng)的參數(shù)為：mq = 1.008 2 kg，ml = 0.076 kg，l = 1.085 m，g = 9.81 m/s2。設(shè)定吊掛飛行系統(tǒng)的初始位置為：xq0 = 0 m、zq0 = 1.5 m，目標(biāo)位置為：xqd = -1.5 m、zqd = 3 m。

本文設(shè)計(jì)的CFO的相關(guān)參數(shù)為：wx = 6，wz = 6，wγ = 6，用于對(duì)比的ESO的相關(guān)參數(shù)為：ωox = 80，ωoz = 80，ωoγ = 500，ωox、ωoz和ωoγ分別表示ESO在位置x、位置z和擺角γ通道的帶寬。

為了對(duì)比CFO和ESO的收斂性和精度，本文設(shè)計(jì)的比例微分控制器和自抗擾控制器相關(guān)參數(shù)均為：ωcx = 1.5，ωcz = 1.5，ωcγ = 0.2，ωcx、ωcz和ωcγ分別表示控制器在位置x，位置z和擺角γ通道的帶寬。用于對(duì)比的LQR控制器參數(shù)是通過對(duì)系統(tǒng)在平衡點(diǎn)處進(jìn)行線性化處理后借助MATLAB中Simulink的Linear Analysis Tool求得。

圖3描述了CFO-PD、LADRC和LQR三種控制方法下無人機(jī)位置及負(fù)載擺角隨時(shí)間變化的情況。由圖3可知，LQR方法在無人機(jī)位置跟蹤方面和吊掛負(fù)載的減擺控制方面表現(xiàn)不佳，需要超過5 s的時(shí)間才能完成位置指令的零穩(wěn)態(tài)誤差跟蹤，并且在無人機(jī)飛往目標(biāo)位置過程中，負(fù)載擺角有明顯的振蕩。而CFO-PD和LADRC方法在位置跟蹤和負(fù)載減擺控制方面性能接近，在無人機(jī)飛往目標(biāo)點(diǎn)過程中，負(fù)載均無擺動(dòng)，且CFO-PD在位置z通道跟蹤快速性方面要優(yōu)于LADRC方法。當(dāng)飛行器受到風(fēng)擾時(shí)，LQR方法下的位置通道和擺角通道都出現(xiàn)了較大幅度的振蕩，抗干擾性能較差，其中吊掛負(fù)載的最大擺角甚至達(dá)到了13°，位置x通道的最大超調(diào)量為75%，位置z通道的最大超調(diào)量為80%；當(dāng)擾動(dòng)消失后，LQR需要4.5 s的調(diào)整時(shí)間以讓穩(wěn)態(tài)誤差消失。采用LADRC和CFO-PD控制器的系統(tǒng)在受到風(fēng)擾后，都表現(xiàn)出了良好的抗擾性能，無人機(jī)位置和擺角僅出現(xiàn)了很小幅度的波動(dòng)，且在擾動(dòng)消失后，很快就恢復(fù)到原來的狀態(tài)，但是從圖中可以看出，CFO-PD的魯棒性要明顯優(yōu)于LADRC，位置和吊掛負(fù)載擺角幾乎無任何波動(dòng)，這是因?yàn)镃FO是Ⅲ型系統(tǒng)，對(duì)擾動(dòng)的估計(jì)精度更高。另外，當(dāng)?shù)鯍熵?fù)載質(zhì)量減少時(shí)，可以發(fā)現(xiàn)LQR方法不能保證位置z的跟蹤，出現(xiàn)了0.4 m的穩(wěn)態(tài)誤差，而CFO-PD和LADRC方法幾乎不受影響。

圖4為三種控制方法下無人機(jī)的控制輸入隨時(shí)間變化情況，可以發(fā)現(xiàn)CFO-PD和LADRC方法均對(duì)控制指令在短時(shí)間做出了響應(yīng)，而LQR方法響應(yīng)較慢，而且控制輸入相比CFO-PD和LADRC小得多。在風(fēng)擾期間，CFO-PD和LADRC方法下的控制輸入均能很好地根據(jù)外部擾動(dòng)及時(shí)做出響應(yīng)，能跟上風(fēng)擾的變化，而LQR方法下的控制輸入則有明顯的滯后，且并不能快速跟上風(fēng)擾的變化。在吊掛負(fù)載質(zhì)量持續(xù)減少時(shí)，通過圖4（b）中局部放大區(qū)域可以看到，CFO-PD方法下的控制輸入響應(yīng)最為迅速，LADRC次之，LQR則最慢。

綜上，本文提出的CFO-BLF反步控制器能使無人機(jī)吊掛飛行系統(tǒng)快速、高精度飛往目標(biāo)位置，且在飛行過程中能極大抑制吊掛負(fù)載的擺動(dòng)，同時(shí)在對(duì)抗外部風(fēng)擾和內(nèi)擾方面具有極強(qiáng)的魯棒性。

4 ?結(jié) ?論

針對(duì)風(fēng)擾和系統(tǒng)內(nèi)擾下的無人機(jī)吊掛飛行系統(tǒng)減擺控制問題，本文通過對(duì)系統(tǒng)動(dòng)力學(xué)模型分析得到系統(tǒng)狀態(tài)方程，并設(shè)計(jì)了基于補(bǔ)償函數(shù)觀測(cè)器的非線性控制器。補(bǔ)償函數(shù)觀測(cè)器可以有效估計(jì)內(nèi)擾和外界干擾，并將這些總擾動(dòng)精確地補(bǔ)償給PD控制器，抑制總擾動(dòng)對(duì)系統(tǒng)帶來的不利影響。仿真實(shí)驗(yàn)表明，本文設(shè)計(jì)的非線性控制器實(shí)現(xiàn)了無人機(jī)吊掛飛行系統(tǒng)的精確位置控制和吊掛負(fù)載的減擺控制，具有比LADRC和LQR控制器更快的跟蹤速度、更強(qiáng)的魯棒性和更高的跟蹤精度。由于條件和時(shí)間有限，本文是從仿真的角度進(jìn)行研究，下一步的工作將在實(shí)際系統(tǒng)中進(jìn)行試驗(yàn)驗(yàn)證。

參考文獻(xiàn)：

[1] 齊俊桐，平原.無人機(jī)吊掛飛行控制技術(shù)綜述 [J].無人系統(tǒng)技術(shù)，2018，1（1）：83-90.

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

[3] DE ANGELIS E L，GIULIETTI F，PIPELEERS G. Two-time-scale control of a multirotor aircraft for suspended load transportation [J].Aerospace Science and Technology，2019，84：193-203.

[4] SHIRANI B，NAJAFI M，IZADI I. Cooperative load transportation using multiple UAVs [J].Aerospace Science & Technology，2019，84（Jan）：158-169.

[5] KOTARU P，WU G，SREENATH K. Dynamics and control of a quadrotor with a payload suspended through an elastic cable [C]//2017 American Control Conference （ACC）.Windsor：IEEE，2017：1-4.

[6] LEE T. Geometric control of quadrotor UAVs transporting a cable-suspended rigid body [J].IEEE Transactions on Control Systems Technology，2017，26（1）：255-264.

[7] YU G，CABECINHAS D，CUNHA R，et al. Nonlinear backstepping control of a quadrotor-slung load system [J].IEEE/ASME Transactions on Mechatronics，2019，24（5）：2304-2315.

[8] 常樂.吊掛刀鋸樹障清理空中機(jī)器人控制技術(shù)研究 [D].南京：南京航空航天大學(xué)，2020.

[9] 高志強(qiáng).自抗擾控制思想探究 [J].控制理論與應(yīng)用，2013，30（12）：1498-1510.

[10] 齊國(guó)元，李闊，王琨.基于補(bǔ)償函數(shù)觀測(cè)器的四旋翼無人機(jī)姿態(tài)受限控制 [J].吉林大學(xué)學(xué)報(bào)：工學(xué)版，2023，53（3）：853-862.

作者簡(jiǎn)介：劉肩山（1989—），男，漢族，江西九江人，講師，碩士研究生，研究方向：無人機(jī)非線性控制；唐毅（1979—），男，漢族，湖南長(zhǎng)沙人，講師，碩士研究生，研究方向：無人機(jī)飛控開發(fā)；謝志明（1981—），男，漢族，湖南株洲人，講師，碩士研究生，研究方向：無人機(jī)應(yīng)用與系統(tǒng)開發(fā)。