基于PX4飛控的多旋翼無人機編隊跟蹤系統(tǒng)設計

姚洪斌，張 民，胡淦敏

(南京航空航天大學自動化學院，南京 211100)

0 引言

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

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

1 問題描述

1.1 制導策略與任務需求

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

圖1 制導策略示意圖Fig.1 Schematic diagram of guidance strategy

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

1.2 無人機建模

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

(1)

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

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

(2)

(3)

2 編隊算法

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

(4)

由圖2中的無人機間運動關系，得方程

(5)

(6)

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

編隊的協(xié)同制導律形式為

圖2 相鄰無人機運動關系示意圖Fig.2 Schematic diagram of motion relationship between adjacent UAVs

=

(7)

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

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

3 編隊系統(tǒng)設計

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

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

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

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

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

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

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

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

4 飛行仿真驗證

通過仿真實驗，無人機的跟蹤軌跡如圖7所示。

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

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

(a) 無人機對目標距離

(b) 無人機高度變化

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

(d) 俯仰角變化曲線

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

(f) 俯仰角速率變化

(g) 無人機速度變化

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

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

5 結(jié)論

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

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

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

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

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