周東升，楊忠，廖祿偉，徐浩，許昌亮，張秋雁

1.南京航空航天大學(xué) 自動化學(xué)院,江蘇 南京 210016 2.貴州電網(wǎng)有限責(zé)任公司,貴州 貴陽 550002

隨著無人機應(yīng)用的不斷深化，無人機正逐漸成為一種空中作業(yè)平臺，除航拍外還用于完成諸如負載運輸[1]、空中抓取作業(yè)等任務(wù)[2-3]。無人機攜帶負載主要有夾持和吊掛[4-6]這2 種方式。負載運輸?shù)膽?yīng)用場景包括快遞運送、救援物資投放等。應(yīng)用場景的復(fù)雜性、負載類型的多樣性，給無人機的總體設(shè)計、機械結(jié)構(gòu)和控制策略帶來新的挑戰(zhàn)。

對于大重量負載，無人機需要提供更大的升力；對于大尺寸負載，則需要改進無人機的構(gòu)型以便運輸。因此，運輸大質(zhì)量大尺寸負載，無人機的尺寸與質(zhì)量也會隨之增大，并需配備大容量電池[7]。無人機質(zhì)量和尺寸的增加不僅會影響其靈活性[8]，而且會產(chǎn)生更高的制造和維護成本。

針對單架無人機運輸大質(zhì)量大尺寸負載的局限性[9-10]，有人考慮使用多架無人機配備高精度傳感器進行協(xié)同搬運，從而在不改變無人機尺度的前提下，有效提高其負載能力[11]。然而，多無人機在定位、控制和協(xié)作等方面存在較多問題，通信帶寬的限制是一項重要挑戰(zhàn)[12]。一般來講，多無人機之間的通信方式包括顯式通信和隱式通信2 類。顯式通信一般用于將控制信號或傳感器數(shù)據(jù)等信息直接傳遞給其他機器人[13]，隱式通信是指無人機與環(huán)境或其他無人機進行物理交互(如相互之間的交互力)或非物理交互(如視覺觀察)的作用，這種方式所需的信息由附著在無人機上的傳感器獲取。研究最多和最常用的方式是顯式通信方式，該方式易于分析，協(xié)作效率高，但在通信環(huán)境較差的情況下，可能會出現(xiàn)嚴(yán)重的問題。

協(xié)作搬運負載是多無人機協(xié)同作業(yè)應(yīng)用的一個重要方向，應(yīng)用發(fā)展前景良好，國外開展了大量研究。文獻[14]分析了使用一組無人機通過電纜來運輸大的有效載荷的問題，該結(jié)構(gòu)的選擇是為了在電纜張力影響的情況下保證有效載荷的靜態(tài)平衡。文獻[15]針對2 架無人機對大型物體的運輸，研究了基于被動力控制的無人機協(xié)同控制問題。然而，這些研究方法多采用軟繩或電纜進行運輸，負載的擺動容易引起無人機的不穩(wěn)定甚至失控。

為了解決上述多無人機系統(tǒng)通信以及負載擺動帶來的問題，本文設(shè)計了一種基于硬連接的隱式通信方式的力位混合控制方法。每架無人機與負載通過球形關(guān)節(jié)硬連接[16]，該球形關(guān)節(jié)具有3 個運動自由度并進行限位[17]，使得無人機的姿態(tài)動力學(xué)和負載的姿態(tài)動力學(xué)解耦，進而可以通過調(diào)整飛行器的姿態(tài)以及旋翼產(chǎn)生的推力大小來控制負載的運動。硬連接導(dǎo)致無人機與負載之間存在力的約束，為此采用導(dǎo)納控制技術(shù)[18]進行力的控制，其思想是通過感知外部的力來生成運動軌跡，調(diào)節(jié)無人機的運動，實現(xiàn)任意的力交互行為。本文設(shè)計的方法如下：1）給定主機期望的運動軌跡，主機和從機與負載硬連接，主機運動對負載產(chǎn)生力的作用，同時負載也對從機產(chǎn)生力的作用，從機通過力估計方法感知負載施加的作用力；2）通過導(dǎo)納控制技術(shù)改變從機的運動軌跡以適應(yīng)受到的負載的作用力；3）對導(dǎo)納控制器輸出的運動軌跡設(shè)計一個外部的控制回路，并通過線性二次型調(diào)節(jié)器[19]實現(xiàn)主從機位置姿態(tài)的穩(wěn)定控制，由于存在外部擾動力的作用，所以從機受到較小的力作用時導(dǎo)納控制器不啟動，僅通過位姿控制器實現(xiàn)位姿穩(wěn)定控制，只有受到較大的力作用時，導(dǎo)納控制器啟動，從機根據(jù)受到的力作用進行運動。

1 數(shù)學(xué)模型

1.1 運動學(xué)建模

雙無人機與負載之間的硬連接方式如圖1 所示，為雙無人機協(xié)同搬運系統(tǒng)。定義坐標(biāo)系I：為地球固連坐標(biāo)系 ；B：為與機身固連的機體坐標(biāo)系，i表示第i架無人機，原點與飛機質(zhì)心重合；P：為與負載重心固連的坐標(biāo)系。地球固連坐標(biāo)系、機體坐標(biāo)系和負載坐標(biāo)系都滿足右手定則。

圖1 雙無人機協(xié)同搬運坐標(biāo)系定義

圖2 為球形連接關(guān)節(jié)，外部球殼可以繞內(nèi)部球頭轉(zhuǎn)動。做出如下假設(shè)：1）每個飛行器的機體坐標(biāo)系Bi到負載參考坐標(biāo)系P的相對位置是已知的，用向量表示；2）表示飛行器機體系Bi到負載坐標(biāo)系P的旋轉(zhuǎn)矩陣。從而可以得到系統(tǒng)的運動學(xué)方程[20]為

圖2 球形關(guān)節(jié)

1.2 動力學(xué)建模

本文的雙無人機搬運系統(tǒng)的動力學(xué)模型通過牛頓-歐拉方程得到。為了方便后續(xù)控制方案的設(shè)計與研究，本文中系統(tǒng)的平動方程在地球固連坐標(biāo)系下描述，轉(zhuǎn)動方程在負載坐標(biāo)系下描述：

式中：Pω和分別為負載的加速度和角加速度；Pτmav為所有飛行器對負載施加的總力矩；Pτdrag為與空氣動力學(xué)相關(guān)的干擾力矩；為第i架無人機到負載的相對位置向量；PIsys為整個系統(tǒng)總的慣性，與負載慣性以及飛行器的質(zhì)量和位置有關(guān)。

2 控制策略

由于主機和從機與負載之間都通過球形關(guān)節(jié)進行連接且相互之間不存在顯示通信，考慮采用基于導(dǎo)納控制[21-23]的控制策略，即2 架飛行器攜帶負載懸停在同一高度，向主機發(fā)送目標(biāo)軌跡指令，從機通過感知負載傳遞的力進行跟隨運動，從機的控制策略如圖3 所示。

圖3 從機控制策略

2.1 力估計

主機接收指令并帶動負載一起運動，會導(dǎo)致負載對從機產(chǎn)生作用力，這個力采用卡爾曼濾波器算法[24-26]來估計，并確保濾波器是閉環(huán)穩(wěn)定的[18]。選取狀態(tài)量ξK=(ξ,F)，給定系統(tǒng)為

式中：ξK=(ξ,F)為選取的狀態(tài)變量，ξ˙K為其導(dǎo)數(shù)，ξ為從機三維位置、速度、歐拉角、角速度和角加速度信息，F(xiàn)為估計的力大小，u為輸入的力和力矩，AK、BK和CK為常數(shù)矩陣，WK和VK分別為過程噪聲和測量噪聲協(xié)方差矩陣。估計的狀態(tài)量為

建立飛行器模型，平動動力學(xué)表示為

式中：I p和Iv為從機的三維位置和速度向量，和分別是它們的導(dǎo)數(shù)，m為從機的質(zhì)量，RIB為從機機體系到地球固連坐標(biāo)系的旋轉(zhuǎn)矩陣，F(xiàn)為從機旋翼產(chǎn)生的總推力，fext是施加在無人機上的外力。

轉(zhuǎn)動動力學(xué)可以近似表示為

在上述模型基礎(chǔ)上，卡爾曼濾波的狀態(tài)變量選擇為

由于外力和外力矩的動力學(xué)是未知的，所以假設(shè)它們僅由零均值白噪聲驅(qū)動。測量向量考慮了白噪聲的影響，定義為

濾波器的輸入向量由姿態(tài)和總推力給出[27]：

式中?cmd、θcmd和ψcmd分別為輸入的滾轉(zhuǎn)角、俯仰角和偏航角指令。

2.2 導(dǎo)納控制

導(dǎo)納控制提供了一種飛行器與外界的物理交互方式，即一個位置控制的機械系統(tǒng)，它可以通過感知外部的力來生成一個運動軌跡，調(diào)節(jié)飛行器的運動，實現(xiàn)任意的交互行為。因此，從機可以看成一個被動運動的質(zhì)點。這種力和運動間的動態(tài)關(guān)系可以表示為

導(dǎo)納控制器可以表示為狀態(tài)空間方程形式，選取狀態(tài)量ξad=(IΛr,Ivr)和輸入量uad=可以得到：

2.3 位姿控制器設(shè)計

線性二次型調(diào)節(jié)器（LQR）其控制對象為線性系統(tǒng)，而性能指標(biāo)為控制輸入和對象狀態(tài)組成的二次型函數(shù)，最優(yōu)解具有統(tǒng)一的表達式，容易求得閉環(huán)形式的最優(yōu)控制，且易于工程實現(xiàn)[19]。對于單個無人機本身則分別設(shè)計位置控制器和姿態(tài)控制器。

假設(shè)位置控制的狀態(tài)空間模型為

狀態(tài)量、輸入量和輸出量分別為

類似的，姿態(tài)控制的狀態(tài)空間模型為

其中狀態(tài)量XΘ(t)、輸入量BUM(t)和輸出量YΘ(t)分別表示為

定義LQR 的性能指標(biāo)為

式中Q、R是對狀態(tài)變量和輸入量的加權(quán)矩陣。

求解二次型最優(yōu)控制即找到狀態(tài)反饋控制律U(t),使得性能指標(biāo)J取得極小值，可采用Ricatti 方程[28]：

可以得到

經(jīng)過分析試驗選取使系統(tǒng)性能良好的位置和姿態(tài)控制器的Q、R分別為

3 仿真驗證

為了驗證本文提出方法的有效性，設(shè)計了基于Matlab/Simulink 環(huán)境下搭建雙無人機協(xié)同搬運的仿真，系統(tǒng)仿真參數(shù)如下：單架飛行器質(zhì)量（mi）為2.12 kg，負載質(zhì)量（mp）為1.06 kg，x軸/y軸轉(zhuǎn)動慣量（Ixx/Iyy）為10.52 kg·m2，z軸轉(zhuǎn)動慣量（Izz）為0.91kg·m2，重力加速度（g）為9.81m/s2，升力系數(shù)和阻力系數(shù)（kf/km）分別為3.07×10-5和5.33×10-7。

3.1 單架無人機位姿穩(wěn)定控制仿真

對于主機和從機分別進行位姿穩(wěn)定控制實驗，驗證LQR 控制器對于單架飛行器的控制效果，且由文獻[21]可知，單獨設(shè)計導(dǎo)納控制器不會影響從機內(nèi)部回路的整體運動學(xué)。首先測試位置姿態(tài)控制器的性能，無人機初始位置前10 s垂直起飛至目標(biāo)點進而到達期望位置點同時設(shè)定姿態(tài)期望為飛行軌跡如圖4所示，圖5為同步的姿態(tài)曲線。

圖4 位置控制的飛行軌跡

圖5 位置控制的姿態(tài)曲線

由圖4 和圖5 可見，采用LQR 控制器的無人機能夠迅速平穩(wěn)地飛向目標(biāo)點并懸停在目標(biāo)點；與此同時，飛行器能夠保持姿態(tài)在很小的范圍內(nèi)變化。所以LQR 控制器的穩(wěn)定控制效果較為理想。

3.2 雙機協(xié)同位姿追蹤仿真

當(dāng)雙機進行協(xié)同搬運任務(wù)時，通過仿真結(jié)果驗證控制策略的有效性以及控制器的控制性能。

考慮主機以及負載運動對從機的影響，并根據(jù)選取的連接關(guān)節(jié)，假定負載對從機的力作用于從機的質(zhì)心。若沒有外力作用，則前述控制方案簡化為底層的位置姿態(tài)控制器，但是由于系統(tǒng)內(nèi)外的干擾，會導(dǎo)致估計的力不為零，所以對于導(dǎo)納控制器設(shè)計一個控制“開關(guān)”，只有當(dāng)估計的力高于某一閾值時，導(dǎo)納控制器啟動，反之，對于較小的力，導(dǎo)納控制器不啟動。

3.2.1 從機受瞬時負載力的仿真

選取從機剛度矩陣K=0，當(dāng)從機x軸方向受到瞬時負載力時，導(dǎo)納控制器輸出響應(yīng)以及從機估計的力響應(yīng)如圖6 所示，圖7 和圖8 為相應(yīng)主機和從機姿態(tài)響應(yīng)。

圖6 從機x 軸響應(yīng)

圖7 主機姿態(tài)曲線

圖8 從機姿態(tài)曲線

從機的運動依賴于外部受到的力，由于濾波器進行力估計需要收斂時間，如圖6，估計的力和實際的力之間存在延遲。且由于擾動力的存在，在導(dǎo)納控制器對力進行響應(yīng)之前，飛行器已經(jīng)開始運動。如圖7 和圖8 所示，當(dāng)從機受到瞬時力，由于硬連接產(chǎn)生的相互作用，主機同時會受到負載的拉力，主機和從機姿態(tài)角都會出現(xiàn)一定程度的波動，但10 s 后主機和從機的姿態(tài)都恢復(fù)穩(wěn)定，可以看出LQR 控制器能夠保證主從機姿態(tài)的穩(wěn)定。

3.2.2 主從機連續(xù)軌跡追蹤仿真

對雙機協(xié)同搬運負載進行連續(xù)時間軌跡仿真，給定主機如式（1）的連續(xù)軌跡期望，主機和從機的三維軌跡曲線，即螺旋線軌跡如圖9 所示。圖10 和圖11 為追蹤螺旋線軌跡時相應(yīng)主機和從機姿態(tài)響應(yīng)曲線。

圖9 主機和從機螺旋線跟蹤軌跡曲線

圖10 主機姿態(tài)曲線

圖11 從機姿態(tài)曲線

結(jié)果表明，對于連續(xù)的螺旋線軌跡，從機能夠保證位置的穩(wěn)定跟蹤。主機和從機與負載剛性連接，在三維空間能夠保持相對位置不變。圖10 和圖11 表明主從機在跟蹤螺旋線軌跡過程中，主機姿態(tài)角雖然會出現(xiàn)波動，但能夠快速保持穩(wěn)定，從機由于被動力驅(qū)動，姿態(tài)角會出現(xiàn)持續(xù)的較小幅度的變化?？傮w來說，本文設(shè)計的控制方法，能夠?qū)崿F(xiàn)雙機穩(wěn)定地協(xié)同搬運負載的任務(wù)。

4 結(jié)論

本文對雙無人機協(xié)作搬運系統(tǒng)控制方案進行了研究。該方案在位置姿態(tài)控制器基礎(chǔ)之上采用卡爾曼濾波器估計施加在從機上的外力，并通過導(dǎo)納控制器根據(jù)受到的外力生成運動軌跡。實驗結(jié)果可以證明本文提出方案的有效性。在今后研究中，可以進一步開發(fā)力位混合控制策略，深入考慮分析風(fēng)擾等外部干擾產(chǎn)生的問題，加強系統(tǒng)魯棒性，進而提升該系統(tǒng)執(zhí)行困難任務(wù)的能力。