姚洪斌，張 民，胡淦敏

(南京航空航天大學(xué)自動(dòng)化學(xué)院，南京 211100)

0 引言

近年來，無人機(jī)編隊(duì)飛行已經(jīng)成為控制領(lǐng)域的一個(gè)硏究熱點(diǎn)。無人機(jī)編隊(duì)飛行是指2架或者2架以上的無人機(jī)，接到任務(wù)指令后，可以根據(jù)當(dāng)時(shí)任務(wù)階段自主進(jìn)行隊(duì)形的集結(jié)、變化、保持、重構(gòu)等功能變化，同時(shí)也包括飛行任務(wù)的規(guī)劃和分配，整個(gè)無人機(jī)群在執(zhí)行任務(wù)過程中體現(xiàn)出高度協(xié)同性和一致性。但在實(shí)際工程應(yīng)用中發(fā)現(xiàn)，單架無人機(jī)會(huì)因?yàn)槔m(xù)航能力有限導(dǎo)致任務(wù)效率較低，嚴(yán)重制約了其應(yīng)用場(chǎng)景和性能優(yōu)勢(shì)。通過多無人機(jī)的相互協(xié)作，無人機(jī)群飛行可以有效地延伸無人機(jī)的任務(wù)執(zhí)行范圍，提升執(zhí)行任務(wù)的完成度和成功率，綜合性能與單機(jī)相比有了極大的提高，應(yīng)用范圍和任務(wù)場(chǎng)景也不斷拓展。國(guó)內(nèi)目前對(duì)無人機(jī)編隊(duì)飛行的研究基本上聚焦在協(xié)同控制方法的研究和實(shí)驗(yàn)室仿真階段。國(guó)外E.Frew等利用李雅普諾夫矢量場(chǎng)并基于視線角的制導(dǎo)策略，實(shí)現(xiàn)多無人機(jī)協(xié)同跟蹤目標(biāo)，最終使無人機(jī)穩(wěn)定收斂在極限環(huán)上做周期運(yùn)動(dòng)。國(guó)內(nèi)楊祖強(qiáng)等建立tau制導(dǎo)矢量場(chǎng)，設(shè)計(jì)了基于standoff的多機(jī)協(xié)同跟蹤算法，利用相位調(diào)整并結(jié)合人工勢(shì)場(chǎng)法避碰，以達(dá)到多機(jī)協(xié)同跟蹤目的。另外，固定翼飛機(jī)雖然具有高速巡航能力，但是對(duì)于起降跑道和平飛高度要求較高。多旋翼無人機(jī)具有垂直起降、空中懸停、低空低速飛行等獨(dú)特飛行技能，且具有很強(qiáng)的靈活性，被廣泛應(yīng)用于地圖航拍、農(nóng)業(yè)噴灑、偵查測(cè)繪等領(lǐng)域。

本文提出了一種基于PX4飛控的通過長(zhǎng)機(jī)領(lǐng)航的四旋翼無人機(jī)編隊(duì)系統(tǒng)，將基于距離變化率的協(xié)同定距跟蹤制導(dǎo)律成功應(yīng)用于多旋翼無人機(jī)平臺(tái)，設(shè)計(jì)的協(xié)同制導(dǎo)律公式簡(jiǎn)潔，所需要的傳感器參數(shù)信息大大減少，且整個(gè)系統(tǒng)的復(fù)雜度不隨飛行器數(shù)量的增加而增加，最終可以使多架無人機(jī)自主形成預(yù)設(shè)編隊(duì)并保持編隊(duì)，能夠?qū)Φ孛婺繕?biāo)進(jìn)行跟蹤，從而提高任務(wù)效率，最大化發(fā)揮多旋翼無人機(jī)的性能。同時(shí)，應(yīng)用機(jī)器人操作系統(tǒng)(Robot Operating System，ROS)搭建編隊(duì)軟件系統(tǒng)，實(shí)現(xiàn)了多無人機(jī)之間的信息交互，最后在Gazebo仿真環(huán)境中進(jìn)行測(cè)試，并分析了無人機(jī)群的飛行數(shù)據(jù)，從而驗(yàn)證了本系統(tǒng)的穩(wěn)定性和可行性。

1 問題描述

1.1 制導(dǎo)策略與任務(wù)需求

無人機(jī)協(xié)同跟蹤是無人機(jī)編隊(duì)在探測(cè)范圍內(nèi)發(fā)現(xiàn)地面目標(biāo)后，編隊(duì)成員之間可通過鏈路通信獲得相鄰無人機(jī)的狀態(tài)信息和地面目標(biāo)的信息，進(jìn)而對(duì)地面目標(biāo)進(jìn)行有效的協(xié)同跟蹤。無人機(jī)飛行控制系統(tǒng)由內(nèi)回路和外回路構(gòu)成。本文直接利用PX4開源飛控系統(tǒng)底層架構(gòu)作為內(nèi)回路控制無人機(jī)姿態(tài)，同時(shí)基于設(shè)計(jì)的編隊(duì)制導(dǎo)算法進(jìn)行位置控制和速度控制作為外回路的輸入，以響應(yīng)外回路給出的制導(dǎo)指令。目標(biāo)幾乎以恒定的速度在相當(dāng)平坦的地面上移動(dòng),并且在默認(rèn)條件下無人機(jī)的速度高于目標(biāo)速度。理想情況下，所有無人機(jī)在執(zhí)行跟蹤任務(wù)過程中，都會(huì)保持在同一水平高度上，所以這里僅考慮固定高度上的二維制導(dǎo)問題。制導(dǎo)策略如圖1所示。

圖1 制導(dǎo)策略示意圖Fig.1 Schematic diagram of guidance strategy

任務(wù)目標(biāo)可理解為地面目標(biāo)與無人機(jī)同時(shí)處于同一個(gè)二維平面上，多架無人機(jī)在跟蹤制導(dǎo)律作用下逐漸收斂到構(gòu)成以目標(biāo)為圓心、半徑為的圓編隊(duì)進(jìn)行穩(wěn)定跟蹤。在跟蹤編隊(duì)初步成形后，協(xié)同制導(dǎo)律開始作用，最終架無人機(jī)平均分布于軌跡圓上。

1.2 無人機(jī)建模

建立無人機(jī)模型的系統(tǒng)狀態(tài)空間，其描述形式為

(1)

=[,,,,,,,,,,,]

其中，為狀態(tài)向量；后面各項(xiàng)依次為滾轉(zhuǎn)角、俯仰角、偏航角、滾轉(zhuǎn)角速度、俯仰角速度、偏航角速度、機(jī)體坐標(biāo)系3個(gè)坐標(biāo)軸的空速分量、橫向位移、側(cè)向位移、高度；作為控制向量，且作為執(zhí)行機(jī)構(gòu)的操縱量，經(jīng)過底層飛控的控制器，最后都通過電機(jī)產(chǎn)生的力矩作為輸出。在制導(dǎo)問題研究中，將無人機(jī)狀態(tài)向量簡(jiǎn)化為=[,,,,,]，其運(yùn)動(dòng)模型為

(2)

(3)

2 編隊(duì)算法

當(dāng)跟蹤地面目標(biāo)時(shí)，無人機(jī)的飛行狀態(tài)分為順時(shí)針飛行和逆時(shí)針飛行。在本文中，為了便于分析，采用前者的飛行方式。本文使用如式(4)所示的無人機(jī)跟蹤地面目標(biāo)制導(dǎo)律

(4)

由圖2中的無人機(jī)間運(yùn)動(dòng)關(guān)系，得方程

(5)

(6)

其中，2架相鄰無人機(jī)與-1間距離用,-1(±在范圍內(nèi)，∈)表示，相鄰2架無人機(jī)軌跡所對(duì)圓心角用,-1表示，由幾何關(guān)系可得,-1=,-1=--1。

編隊(duì)的協(xié)同制導(dǎo)律形式為

圖2 相鄰無人機(jī)運(yùn)動(dòng)關(guān)系示意圖Fig.2 Schematic diagram of motion relationship between adjacent UAVs

=

(7)

控制結(jié)構(gòu)框圖如圖3所示，以跟隨無人機(jī)為例，跟隨無人機(jī)自身產(chǎn)生的控制指令經(jīng)過處理，在無人機(jī)編隊(duì)系統(tǒng)中輸入的分別是各個(gè)跟隨無人機(jī)實(shí)時(shí)采集到的位置信息和偏航角信息，經(jīng)過飛控底層得到的領(lǐng)航無人機(jī)信息作為編隊(duì)協(xié)同律的輸入，兩個(gè)控制律的輸出融合后共同發(fā)送給飛控，輸出為底層飛控姿態(tài)控制器計(jì)算的控制量。

圖3 編隊(duì)控制結(jié)構(gòu)框圖Fig.3 Block diagram of formation control structure

3 編隊(duì)系統(tǒng)設(shè)計(jì)

整體編隊(duì)實(shí)現(xiàn)采用圖4所示的分層結(jié)構(gòu)表示。該結(jié)構(gòu)總共4層，分別為進(jìn)行外部控制的應(yīng)用層、負(fù)責(zé)通信的通信層、融合數(shù)據(jù)的飛控層和各傳感器模型的接口層。

圖4 編隊(duì)系統(tǒng)層次結(jié)構(gòu)Fig.4 Hierarchy of formation system

應(yīng)用層通過規(guī)劃整體編隊(duì)隊(duì)形，使機(jī)群中的每個(gè)成員達(dá)到期望的位置和速度，并通過上文中的編隊(duì)算法計(jì)算出每架無人機(jī)在其機(jī)體坐標(biāo)系下的期望姿態(tài)。通信層可以利用系統(tǒng)程序設(shè)計(jì)基于開源ROS的分布式框架，為無人機(jī)提供通信服務(wù)。首先組建一個(gè)局域網(wǎng)，將應(yīng)用層計(jì)算出的期望速度、滾轉(zhuǎn)角、俯仰角等信息以及各無人機(jī)成員的自身狀態(tài)信息作為話題節(jié)點(diǎn)，在飛控層與應(yīng)用層之間傳遞。飛控層利用開源的PX4飛控負(fù)責(zé)無人機(jī)的信息融合，飛行控制棧是導(dǎo)航、制導(dǎo)、控制算法的集合，可以提供姿態(tài)解算、控制量分配、電機(jī)舵機(jī)控制等，使無人機(jī)在穩(wěn)定飛行的同時(shí)速度可以快速收斂到期望的值，保證無人機(jī)飛行的姿態(tài)穩(wěn)定且提供無人機(jī)速度和高度保持，從而控制無人機(jī)達(dá)到期望的跟蹤隊(duì)形。接口層主要是由無人機(jī)模型和各傳感器模型構(gòu)成。4個(gè)層次共同組成了無人機(jī)編隊(duì)控制系統(tǒng)。本文主要在通信層和應(yīng)用層搭建Gazebo仿真環(huán)境，利用ROS結(jié)合c++進(jìn)行程序設(shè)計(jì)。

GAZEBO是一款能夠提供高保真度的物理模擬的圖形界面(見圖5)，并且可以自定義增加場(chǎng)景，即使在復(fù)雜的環(huán)境下，仍然能夠準(zhǔn)確地模擬出機(jī)器人的行為，是一款對(duì)用戶和程序都非常友好的物理模型仿真軟件。本文系統(tǒng)使用這款軟件對(duì)現(xiàn)實(shí)場(chǎng)景進(jìn)行還原，并在其中進(jìn)行無人機(jī)編隊(duì)算法的驗(yàn)證。在GAZEBO仿真環(huán)境中，通過配置四旋翼無人機(jī)尺寸、質(zhì)量和相關(guān)傳感器、控制器等模型參數(shù)，得到基于PX4的iris四旋翼仿真模型，并使其與實(shí)際無人機(jī)基本保持一致，對(duì)其編號(hào)以模擬多架無人機(jī)在實(shí)際飛行環(huán)境中的效果。其次，修改仿真時(shí)運(yùn)行的 launch 文件，通過標(biāo)簽的方式添加無人機(jī)的屬性，改變無人機(jī)的個(gè)數(shù)、起始位置等，每個(gè)無人機(jī)都具有各自的用戶數(shù)據(jù)報(bào)協(xié)議(User Datagram Protocol，UDP)端口，并且與其mavros 包的UDP通信端口一一對(duì)應(yīng)，設(shè)置好后即可結(jié)合其他ROS包中的launch文件進(jìn)行編隊(duì)控制實(shí)驗(yàn)。啟動(dòng)后，可以在仿真環(huán)境中觀察到多架無人機(jī)形成隊(duì)形且保持隊(duì)形進(jìn)行預(yù)期活動(dòng)。

圖5 GAZEBO仿真界面Fig.5 GZEBO simulation interface

ROS在此無人機(jī)編隊(duì)系統(tǒng)中，可作為無人機(jī)起飛、切換模式等命令的接收者，也可以作為經(jīng)過內(nèi)部相關(guān)程序包計(jì)算后的數(shù)據(jù)處理者以及制導(dǎo)信息的發(fā)布者。通過微型空中飛行器鏈路通信協(xié)議(Micro Air Vehicle Link，MAVLink)和其可擴(kuò)展通信節(jié)點(diǎn)mavros，在編隊(duì)中的無人機(jī)會(huì)向服務(wù)器發(fā)布自己的相關(guān)狀態(tài)信息，同時(shí)，若其想要接收相鄰無人機(jī)的相關(guān)狀態(tài)，可以通過訂閱話題的方式進(jìn)行獲取，與其建立通信關(guān)系，即可達(dá)成目的。內(nèi)部 ROS 接收來自發(fā)布節(jié)點(diǎn)以及各個(gè)傳感器的信息，計(jì)算無人機(jī)的制導(dǎo)信息，并將此信息傳遞給飛控，進(jìn)而控制無人機(jī)飛行姿態(tài)，其中涉及的訂閱/發(fā)布的ROS話題如表1所示。同時(shí)，設(shè)計(jì)了能夠觀察各機(jī)詳細(xì)數(shù)據(jù)和進(jìn)行指令切換的小型地面站，如圖6所示。

表1 訂閱/發(fā)布的ROS話題

圖6 小型地面站Fig.6 Small ground station

4 飛行仿真驗(yàn)證

通過仿真實(shí)驗(yàn)，無人機(jī)的跟蹤軌跡如圖7所示。

圖7 無人機(jī)協(xié)同跟蹤目標(biāo)軌跡Fig.7 UAV cooperative tracking target trajectory

4架無人機(jī)最終能通過外部指令切換飛行模式在各自位置起飛，每架無人機(jī)起飛的水平間隔為8m，起飛后先平飛，之后搜索到目標(biāo)后進(jìn)入跟蹤階段，在5m的高度對(duì)目標(biāo)進(jìn)行跟蹤，跟蹤過程中4架無人機(jī)在跟蹤圓軌跡中保持均勻分布，飛行過程仿真曲線如圖8所示。

(a) 無人機(jī)對(duì)目標(biāo)距離

(b) 無人機(jī)高度變化

(c) 滾轉(zhuǎn)角變化曲線

(d) 俯仰角變化曲線

(e) 滾轉(zhuǎn)角速率變化

(f) 俯仰角速率變化

(g) 無人機(jī)速度變化

(h) 無人機(jī)之間距離變化

觀察過程仿真曲線結(jié)果可知：無人機(jī)能夠根據(jù)制導(dǎo)律給出的指令很好地跟蹤地面移動(dòng)目標(biāo)，對(duì)目標(biāo)距離穩(wěn)定在10m，無人機(jī)在跟蹤過程中在2.5～2.7m/s的速度范圍內(nèi)變化，高度偏差都在±0.1m以內(nèi)，跟蹤過程保證了4架無人機(jī)之間的距離保持在15m左右；同時(shí)，姿態(tài)也能根據(jù)指令及時(shí)調(diào)整，俯仰角和滾轉(zhuǎn)角都能達(dá)到良好的控制效果，驗(yàn)證了本文無人機(jī)編隊(duì)系統(tǒng)可以有效且穩(wěn)定地跟蹤目標(biāo)。

5 結(jié)論

本文采用開源的PX4 四旋翼無人機(jī)飛控，圍繞編隊(duì)協(xié)同跟蹤問題開展工作，主要結(jié)論為：

1)利用領(lǐng)航者-跟隨者編隊(duì)方式與ROS分布式模型的優(yōu)點(diǎn)，設(shè)計(jì)了編隊(duì)協(xié)同控制系統(tǒng)，ROS系統(tǒng)將控制底層與上層應(yīng)用相互分離，隱藏各自細(xì)節(jié)，具有良好的互操作性。

2)將基于距離變化率的制導(dǎo)律結(jié)合PX4飛控的位置和速度控制，在GAZEBO環(huán)境中搭建多架四旋翼實(shí)際模型，對(duì)編隊(duì)控制系統(tǒng)的效果及功能進(jìn)行編隊(duì)仿真驗(yàn)證。

3)仿真結(jié)果表明，本文提出的編隊(duì)跟蹤策略和算法可以有效地跟蹤地面目標(biāo)，具有工程應(yīng)用價(jià)值。

4)同時(shí)本系統(tǒng)也具有良好的拓展性，在后續(xù)實(shí)驗(yàn)中可以通過對(duì)無人機(jī)和機(jī)載設(shè)備選型，進(jìn)而對(duì)本文提出的編隊(duì)算法進(jìn)行實(shí)物驗(yàn)證。