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

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

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

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

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

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

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

1.1 Pixhawk4 飛控

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

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

1.2 機載電腦

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

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

2.1 程序總體設(shè)計

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

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

2.2 ROS 節(jié)點程序設(shè)計

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

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

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

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

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

表1 機體坐標系/相機坐標系屬性表Tab.1 Coordinate system attribute table of body/camera

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

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

表2 ENU 坐標系/與機體固連ENU 坐標系屬性表Tab.2 Coordinate system attribute table of ENU/ENU of body

由式（7）～式（9）得出無人機的期望XYZ 三軸坐標。

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

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

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

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

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

3 追蹤測試

3.1 識別測試

圖4 二維碼識別圖Fig.4 QR code identification diagram

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

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

3.2 無人機追蹤測試

測試流程：

（1）通過遙控器將無人機升至一定高度并懸停；

（2）筆記本電腦通過NoMachine 軟件遠程控制機載電腦啟動launch 文件，確保機載電腦不斷向無人機發(fā)送位置信息；

（3）通過遙控器將無人機切換到offboard 模式；

（4）無人機首次識別二維碼發(fā)生偏航，對坐標系進行校正；

（5）無人機再次識別二維碼開始追蹤。

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

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

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

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

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

無人機實際飛行軌跡如圖7 所示。

圖7 實際軌跡圖Fig.7 Actual trajectory

4 結(jié)語

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