基于GPS 定位的旋翼無(wú)人機(jī)追蹤系統(tǒng)應(yīng)用研究

魯 軍，魏 峰，郝永平，徐九龍

（1.沈陽(yáng)理工大學(xué) 自動(dòng)化與電氣工程學(xué)院，沈陽(yáng) 110159；2.沈陽(yáng)理工大學(xué) 裝備工程學(xué)院，沈陽(yáng) 110159）

人工智能技術(shù)和智能控制技術(shù)發(fā)展迅速，牽引著無(wú)人系統(tǒng)自主控制理論和技術(shù)的發(fā)展，并使之逐漸成為自動(dòng)控制領(lǐng)域的研究熱點(diǎn)和難點(diǎn)[1]。隨著Pixhawk 開(kāi)源硬件熱度的上升和PX4 固件的更新，無(wú)人機(jī)（unmanned aerial vehicle，UAV）產(chǎn)業(yè)迎來(lái)快速發(fā)展的春天[2]。無(wú)人機(jī)技術(shù)的進(jìn)步，使無(wú)人機(jī)在軍事和民事領(lǐng)域都有廣闊的應(yīng)用前景[3]。能夠自主跟蹤機(jī)動(dòng)目標(biāo)的無(wú)人機(jī)在應(yīng)用中要求很高，許多研究致力于無(wú)人機(jī)的自動(dòng)跟蹤[4]。無(wú)人機(jī)目標(biāo)追蹤的研究，大多集中在圖像處理的視覺(jué)追蹤方案或控制規(guī)律上[5-6]。尋求識(shí)別更多目標(biāo)類型，準(zhǔn)確率更高的算法，以及遇到突發(fā)情況的解決方案，如環(huán)境因素（嚴(yán)重遮擋、光照變化、距離突變）等影響帶來(lái)的目標(biāo)丟失，以及目標(biāo)與自身因素（目標(biāo)的幾何變形、旋轉(zhuǎn)和無(wú)人機(jī)姿態(tài)變換等）造成的追蹤失敗[7]。對(duì)如何搭建無(wú)人機(jī)目標(biāo)追蹤硬件系統(tǒng)則研究較少。盡管仿真在一定程度上可以反映出算法的可行性，但將算法應(yīng)用到工程當(dāng)中所展現(xiàn)出的效果與仿真相比還存在一定的不同。

本文以Pixhawk 飛控和樹莓派4B 平臺(tái)為研究對(duì)象，通過(guò)將兩種平臺(tái)進(jìn)行硬件通信調(diào)試和目標(biāo)追蹤軟件開(kāi)發(fā)，使無(wú)人機(jī)實(shí)現(xiàn)目標(biāo)追蹤功能，可給研究人員搭建軟硬件系統(tǒng)提供參考。

1 系統(tǒng)硬件設(shè)計(jì)

無(wú)人機(jī)的自主飛行控制和航點(diǎn)計(jì)算均在機(jī)載電腦中完成[8]。將計(jì)算結(jié)果通過(guò)Mavlink 協(xié)議發(fā)送到飛控上，飛控運(yùn)行PX4 開(kāi)源代碼，連接GPS 獲取當(dāng)前的位置信息，實(shí)時(shí)與樹莓派通過(guò)串口通信，飛控內(nèi)部以u(píng)ORB 的通信方式傳遞消息，通過(guò)混控模塊將電信號(hào)輸出到電調(diào)、電機(jī)上，對(duì)無(wú)人機(jī)進(jìn)行位置和姿態(tài)控制。

1.1 Pixhawk4 飛控

Pixhawk4 高性能自動(dòng)駕駛儀是一款32 位ARM架構(gòu)的開(kāi)源飛控，其硬件和軟件都開(kāi)源，可根據(jù)需求自行設(shè)計(jì)軟硬件，適用于固定翼、多旋翼、直升機(jī)、無(wú)人車、無(wú)人船以及其他可移動(dòng)機(jī)器人平臺(tái)。Pixhawk4 的性能對(duì)于高校研究、業(yè)余玩家使用和公司產(chǎn)品研發(fā)都可以達(dá)到要求。飛控的主處理器為STM32F765，還有一片型號(hào)為STM32F100 的IO 處理器，合理分配運(yùn)算與IO 控制[9]。它還集成了BMI055（加速度計(jì)/陀螺儀）和ICM-20689（加速度計(jì)/陀螺儀），采用MS5611 氣壓計(jì)，IST8310 磁力計(jì)，通過(guò)程序?qū)⒍鄠鞲衅鞯臄?shù)據(jù)進(jìn)行融合，獲得更準(zhǔn)確的飛控狀態(tài)信息，保障飛行安全。將PWM（主輸出）接口與電調(diào)的信號(hào)接口連接，GPS 與SPI 接口連接，對(duì)于接收機(jī)接口根據(jù)發(fā)出的信號(hào)不同（SBUS/PPM）選擇對(duì)應(yīng)的接口，本系統(tǒng)采用SBUS 信號(hào)類型的接收機(jī)。4S1P（4 節(jié)電池串聯(lián)）型的鋰電池工作電壓為14.8 V～16.8 V，通過(guò)分電板降壓后，給飛控供電。系統(tǒng)硬件示意圖如圖1 所示。

圖1 系統(tǒng)硬件示意圖Fig.1 System hardware diagram

1.2 機(jī)載電腦

樹莓派4B 是一款基于ARM 架構(gòu)的微型電腦主板，將系統(tǒng)燒錄到SD 卡中運(yùn)行，開(kāi)機(jī)后可以看到圖形界面。主板共有4 個(gè)USB 接口，將USB2 口與攝像頭連接。USB 轉(zhuǎn)TTl 模塊的一端連接在樹莓派USB1 接口，另一端連接飛控的TELEM2 接口，再通過(guò)ROS 運(yùn)行腳本，實(shí)現(xiàn)兩者的通信。樹莓派運(yùn)行Ubuntu18.04 操作系統(tǒng)，需要通過(guò)命令下載相關(guān)的安裝包[10]，搭建程序的運(yùn)行環(huán)境，并連接顯示器才能正常工作，但作為機(jī)載電腦需被攜帶到天上，顯示器的重量與體積不方便攜帶，故將顯卡欺騙器插在HDMI 口上來(lái)解決這個(gè)問(wèn)題。當(dāng)筆記本電腦和樹莓派在同一網(wǎng)絡(luò)下，可通過(guò)NoMachine 軟件實(shí)現(xiàn)地面端控制天空端。筆記本電腦只控制launch 程序啟動(dòng)，無(wú)人機(jī)追蹤二維碼的過(guò)程全部由飛控和樹莓派計(jì)算，筆記本電腦不參與運(yùn)算。電池通過(guò)降壓器將電壓穩(wěn)定在5.2 V 左右即可保證樹莓派正常運(yùn)行。樹莓派與其它機(jī)載電腦相比價(jià)格低廉，處理器性能合適，但美中不足的是它沒(méi)有獨(dú)立圖形處理器（GPU），對(duì)于當(dāng)下大熱的深度學(xué)習(xí)程序運(yùn)行有一定阻礙，但識(shí)別二維碼并進(jìn)行追蹤，性能完全足夠。

2 系統(tǒng)軟件設(shè)計(jì)

2.1 程序總體設(shè)計(jì)

首先調(diào)用ARUCO 庫(kù)識(shí)別二維碼，若未檢測(cè)到二維碼，機(jī)載電腦讀取飛控的位置信息，更新當(dāng)前位置并保持懸停。反之，繼續(xù)判斷二維碼的ID 號(hào)是否正確，ARUCO 庫(kù)中多個(gè)二維碼都能被識(shí)別，但無(wú)人機(jī)只需要跟蹤特定的二維碼，故對(duì)不同的二維碼進(jìn)行標(biāo)號(hào)，當(dāng)多個(gè)二維碼同時(shí)出現(xiàn)在攝像頭前時(shí)，根據(jù)指定的ID 號(hào)追蹤二維碼。如果ID 號(hào)不正確，則無(wú)人機(jī)保持懸停。若檢測(cè)到正確的ID 號(hào)，機(jī)載電腦獲取無(wú)人機(jī)當(dāng)前位置信息，計(jì)算目標(biāo)的位置，兩個(gè)位置通過(guò)算法處理后，得出無(wú)人機(jī)的期望位置。無(wú)人機(jī)在追蹤目標(biāo)的過(guò)程中存在目標(biāo)丟失的現(xiàn)象，設(shè)置丟失閾值為10，在追蹤的過(guò)程中存在連續(xù)10次目標(biāo)丟失的現(xiàn)象，認(rèn)定目標(biāo)丟失。反之，認(rèn)定目標(biāo)存在，發(fā)送期望位置，完成追蹤。本文軟件系統(tǒng)的實(shí)時(shí)追蹤流程如圖2 所示。

圖2 實(shí)時(shí)追蹤流程Fig.2 Real-time tracking flow chart

2.2 ROS 節(jié)點(diǎn)程序設(shè)計(jì)

飛控與樹莓派相互傳輸信息需要通過(guò)相應(yīng)的節(jié)點(diǎn)來(lái)實(shí)現(xiàn)，使用C++編程語(yǔ)言編寫節(jié)點(diǎn)程序，既可保證程序響應(yīng)的速度快又可以調(diào)用OpenCV 庫(kù)進(jìn)行圖像處理識(shí)別二維碼。ROS 以節(jié)點(diǎn)對(duì)節(jié)點(diǎn)的方式編寫程序，這種方式高內(nèi)聚低耦合，單個(gè)節(jié)點(diǎn)的功能穩(wěn)定單一，各個(gè)節(jié)點(diǎn)之間聯(lián)系緊密，程序模塊的可重用性和可移植性高，程序更加穩(wěn)定可靠。

web_cam 節(jié)點(diǎn)讀取攝像頭的視頻信息，轉(zhuǎn)換為image_raw 話題并發(fā)布。aruco 節(jié)點(diǎn)訂閱圖像話題，經(jīng)過(guò)計(jì)算得出二維碼的編號(hào)，布爾型目標(biāo)檢測(cè)判斷值，目標(biāo)姿態(tài)（四元數(shù)）和目標(biāo)坐標(biāo)，并封裝為aruco_det話題，發(fā)布到目標(biāo)追蹤節(jié)點(diǎn)。

object_tracking 節(jié)點(diǎn)計(jì)算無(wú)人機(jī)的期望位置并做出決策，發(fā)送給底層控制模塊，此節(jié)點(diǎn)訂閱了無(wú)人機(jī)的位置信息和二維碼的位置信息。由式（1）～式（3）分別求出在機(jī)體系下目標(biāo)位置xyz 三軸坐標(biāo)。

視網(wǎng)膜黃斑部是人眼視覺(jué)最敏感的部位，它位于視網(wǎng)膜血管顳上及顳下支之間的6mm范圍的區(qū)域，是眼底的后極部（約為視野中央6°），是維護(hù)中央視力的最關(guān)鍵部位，一旦這里發(fā)生病變，往往對(duì)患者的中心視力造成嚴(yán)重影響。

此公式的目的是將相機(jī)系下目標(biāo)的位置坐標(biāo)轉(zhuǎn)換為機(jī)體系下坐標(biāo)。body_pos［］數(shù)組存儲(chǔ)的是機(jī)體系下被追蹤目標(biāo)的位置，前三位分別代表機(jī)體坐標(biāo)系下的xyz 三軸的點(diǎn)。機(jī)體坐標(biāo)系以無(wú)人機(jī)的質(zhì)心為原點(diǎn)，前方為x 軸的正方向，左方為y 軸正方向，上方為z 軸正方向。aruco［］數(shù)組存放相機(jī)坐標(biāo)系中目標(biāo)的坐標(biāo)。相機(jī)坐標(biāo)系下，右方為x 軸正方向，下方為y 軸正方向，前方為z 軸正方向，原點(diǎn)為相機(jī)聚焦中心。cam［］數(shù)組存放的是在機(jī)體坐標(biāo)系下，攝像頭與飛控的相對(duì)位置，因?yàn)橐詳z像頭為原點(diǎn)去計(jì)算目標(biāo)的相對(duì)位置，所以需要對(duì)無(wú)人機(jī)到目標(biāo)的位置進(jìn)行補(bǔ)償，若無(wú)人機(jī)質(zhì)心與攝像頭之間的距離較小cam［］參數(shù)可以設(shè)置為0。坐標(biāo)系屬性（機(jī)體/相機(jī)）如表1 所示。

表1 機(jī)體坐標(biāo)系/相機(jī)坐標(biāo)系屬性表Tab.1 Coordinate system attribute table of body/camera

得到機(jī)體系下目標(biāo)的坐標(biāo)后，再次將坐標(biāo)轉(zhuǎn)化到與無(wú)人機(jī)固連的ENU（東北天）坐標(biāo)系下，即body_enu［］原點(diǎn)位于無(wú)人機(jī)質(zhì)心，yaw=0°為x 軸正方向，yaw=90°為y 軸正方向，上方為z 軸的正方向。

通過(guò)式（4）～式（6）求出ENU 坐標(biāo)系下，被追蹤目標(biāo)的坐標(biāo)。enu_pos［］數(shù)組的前三位分別是ENU坐標(biāo)系下XYZ 三軸坐標(biāo)。ENU 坐標(biāo)系原點(diǎn)為無(wú)人機(jī)起飛時(shí)的位置，正東方為x 軸正方向，正北方為y軸正方向，豎直向上為z 軸正方向。drone_pos［］代表ENU 坐標(biāo)系下無(wú)人機(jī)當(dāng)前的位置。坐標(biāo)系屬性（ENU/機(jī)體固練ENU）如表2 所示。

表2 ENU 坐標(biāo)系/與機(jī)體固連ENU 坐標(biāo)系屬性表Tab.2 Coordinate system attribute table of ENU/ENU of body

由式（7）～式（9）得出無(wú)人機(jī)的期望XYZ 三軸坐標(biāo)。

uav_pos［］為ENU 坐標(biāo)系下無(wú)人機(jī)的期望位置。track_distance［］代表ENU 坐標(biāo)系下無(wú)人機(jī)與目標(biāo)保持的距離。將需要的坐標(biāo)轉(zhuǎn)換到同一坐標(biāo)系后，求解無(wú)人機(jī)的期望位置時(shí)，只要知道目標(biāo)位置和希望保持的距離，無(wú)需無(wú)人機(jī)當(dāng)前位置，使求解無(wú)人機(jī)期望位置更準(zhǔn)確、容易。

px4_sender 節(jié)點(diǎn)通過(guò)二次包裝mavros 話題實(shí)現(xiàn)對(duì)無(wú)人機(jī)位置、速度加速度的控制使其到達(dá)目標(biāo)點(diǎn)。pos_estimator 節(jié)點(diǎn)訂閱無(wú)人機(jī)的位置信息，若無(wú)人機(jī)使用GPS 以外的定位方式如激光雷達(dá)、t265 視覺(jué)定位，則此節(jié)點(diǎn)將數(shù)據(jù)進(jìn)行融合獲得更準(zhǔn)確的位置信息。此處mavros 節(jié)點(diǎn)代表飛控自身的數(shù)據(jù)，通過(guò)各節(jié)點(diǎn)信息傳遞使其實(shí)現(xiàn)追蹤二維碼的功能。ROS 節(jié)點(diǎn)表如表3 所示。

表3 ROS 節(jié)點(diǎn)表Tab.3 ROS node table

ROS 節(jié)點(diǎn)圖如圖3 所示。

圖3 ROS 節(jié)點(diǎn)圖Fig.3 ROS node diagram

3 追蹤測(cè)試

3.1 識(shí)別測(cè)試

圖4 二維碼識(shí)別圖Fig.4 QR code identification diagram

確認(rèn)識(shí)別目標(biāo)后，使用rostopic list 指令查看已發(fā)布話題，使用rostopic echo/promethues/object_decetion/aruco_det 指令打印出識(shí)別節(jié)點(diǎn)發(fā)布的話題消息。如圖5 所示成功顯示二維碼編號(hào)，識(shí)別狀態(tài)，目標(biāo)位置等消息。

圖5 話題消息圖Fig.5 Topic message diagram

3.2 無(wú)人機(jī)追蹤測(cè)試

測(cè)試流程：

（1）通過(guò)遙控器將無(wú)人機(jī)升至一定高度并懸停；

（2）筆記本電腦通過(guò)NoMachine 軟件遠(yuǎn)程控制機(jī)載電腦啟動(dòng)launch 文件，確保機(jī)載電腦不斷向無(wú)人機(jī)發(fā)送位置信息；

（3）通過(guò)遙控器將無(wú)人機(jī)切換到offboard 模式；

（4）無(wú)人機(jī)首次識(shí)別二維碼發(fā)生偏航，對(duì)坐標(biāo)系進(jìn)行校正；

（5）無(wú)人機(jī)再次識(shí)別二維碼開(kāi)始追蹤。

3.3 數(shù)據(jù)分析

測(cè)試數(shù)據(jù)如圖6 所示，可從曲線中可得出如下結(jié)論：

（1）由圖6（a～c）可知，機(jī)載計(jì)算機(jī)發(fā)出的期望位置與無(wú)人機(jī)實(shí)際位置的曲線趨勢(shì)基本一致，說(shuō)明無(wú)人機(jī)接收到期望位置后，做出反應(yīng)向目標(biāo)點(diǎn)飛去。

（2）由圖6（d）可知，根據(jù)3 軸的運(yùn)動(dòng)范圍和誤差可知，運(yùn)動(dòng)范圍越大誤差越大。x 軸方向的運(yùn)動(dòng)范圍：-（1～5）m，誤差大約為+0.5 m；y 軸方向的運(yùn)動(dòng)范圍：（2～-4）m，誤差大約為（0.1～-0.5）m；z 軸方向的運(yùn)動(dòng)范圍：（0.5～-1）m，誤差大約為±0.15 m。

圖6 測(cè)試數(shù)據(jù)圖Fig.6 Test data diagram

無(wú)人機(jī)實(shí)際飛行軌跡如圖7 所示。

圖7 實(shí)際軌跡圖Fig.7 Actual trajectory

4 結(jié)語(yǔ)

經(jīng)過(guò)飛行測(cè)試及實(shí)驗(yàn)結(jié)果分析，本文搭建的目標(biāo)追蹤系統(tǒng)可行，能使無(wú)人機(jī)自主追蹤二維碼，且軟硬件開(kāi)源容易獲取，可根據(jù)具體需求二次開(kāi)發(fā)更改算法與決策，對(duì)驗(yàn)證無(wú)人機(jī)目標(biāo)追蹤程序有一定的應(yīng)用價(jià)值。