，，

(浙江工業(yè)大學(xué) 信息工程學(xué)院，杭州 310023)

0 引言

四旋翼無人機具有自由度高，環(huán)境適應(yīng)能力強等優(yōu)點，在航空拍攝、交通巡邏、軍事偵察等領(lǐng)域有廣闊的應(yīng)用場景。無人機運動跟蹤的本質(zhì)是通過估計無人機與運動目標(biāo)之間的相對位姿來實現(xiàn)無人機對運動目標(biāo)的跟蹤。相對位姿的估計在航天器對接[1]、空中自主加油[2]、無人機編隊飛行[3]等領(lǐng)域應(yīng)用廣泛，相對于依靠單傳感器數(shù)據(jù)的相對位姿估計方法，基于多傳感器數(shù)據(jù)融合[4-5]的方法能夠提高位姿估計的準確性。隨著半導(dǎo)體技術(shù)的發(fā)展，傳感器的成本和體積大大減少，以及CPU處理性能的提升，使得融合多傳感器數(shù)據(jù)的相對位姿估計方法在小型旋翼無人機的運動跟蹤場景中應(yīng)用成為可能。探究更高效準確的多傳感數(shù)據(jù)融合的無人機跟蹤方法，是當(dāng)前的一個研究熱點。

基于單傳感器的相對位姿估計方法主要采用GPS、激光、視覺傳感器等。Shao Mingqin等人[6]研究了位置差分，偽距差分和相位差分3種常用的差分GPS方法，發(fā)現(xiàn)位置差分方法最容易實現(xiàn)但精度最低，偽距差分方法精度較高且限制較少，所以得到廣泛應(yīng)用，相位差分方法精度最高但由于限制較多而應(yīng)用有限?；诓罘諫PS的方法有一個明顯的缺點，當(dāng)在GPS信號弱或者缺失的地方，將會失效。Karl E.Wenzel等人[7]提出了基于視覺傳感器的相對位姿方法，跟隨無人機通過PnP方法計算得到與領(lǐng)航無人機的相對位姿，實現(xiàn)無人機的編隊飛行。文獻[8]中給出了另外一種相對位姿的估計方法POSIT，該方法可以在只有少數(shù)特征點的情況下，通過迭代求出物體姿態(tài)的近似解。但是基于圖像的方案仍面臨兩個主要問題：1)因相機運動而產(chǎn)生的圖像退化造成的運動目標(biāo)錯檢的情況，由于在目標(biāo)跟蹤中需要控制相機或者相機載體運動，因此獲取到的圖像也經(jīng)常會產(chǎn)生圖像退化的情況從而對運動目標(biāo)的提取和識別產(chǎn)生了影響；2)大量的圖像計算對處理器平臺的計算能力要求高，基于圖像的目標(biāo)跟蹤包含了大量圖像運算和圖像的運動補償，因此對于計算平臺的要求較高，普通的嵌入式平臺難以勝任。

融合多傳感器數(shù)據(jù)的相對位姿估計方法，通過在單傳感器方法的基礎(chǔ)上引入其他傳感器加以輔助以得到更精確的相對位姿估計。Zhu Yunfen等人[9]提出了基于SINS/GPS/IRST的無人機空中加油方法，該方法通過計算獲得加油機與無人機的相對慣導(dǎo)信息和相對GPS信息，同時通過無人機上紅外線傳感器得到無人機與加油機之間的距離，然后將三者數(shù)據(jù)進行融合獲取到無人機與加油機的相對位姿，實現(xiàn)無人機的空中加油。Jeremy Hardy等人[10]針對GPS信號部分地區(qū)無法使用且易受惡意劫持的情況，提出了一種基于視覺-IMU數(shù)據(jù)融合的無人機編隊飛行方法，該方法使用點對點的無線電傳輸無人機的IMU數(shù)據(jù)，計算得到無人機之間的相對位姿，然后通過固定在無人機上朝下的相機提出特征信息來計算無人機之間的相對位姿，最后通過卡爾曼濾波融合數(shù)據(jù)得到無人機之間的相對位姿關(guān)系。王龍等人[11]提出了緊耦合的視覺/INS相對位姿測量方法，并通過實驗證明緊耦合的方法相比于松耦合，有更高的實時性和測量精度。

本文結(jié)合相對位姿中多傳感器融合的方法，提出了基于EKF(Extended Kalman Filter，擴展卡爾曼濾波)的融合手機和無人機多傳感參數(shù)的運動跟蹤方法。

1 基于EKF的多傳感參數(shù)的融合方法

如圖1所示，無人機上裝有視覺傳感器和慣性傳感器，運動目標(biāo)(本文以人體運動為例)持有具備IMU的移動設(shè)備，手機通過Wi-Fi將IMU數(shù)據(jù)發(fā)送給無人機，然后無人機將3個傳感器的數(shù)據(jù)作為輸入進行數(shù)據(jù)融合，最后實現(xiàn)無人機對運動目標(biāo)的運動跟蹤。IMU有采樣頻率快的特點，能夠在快速劇烈運動的情況下得到運動狀態(tài)，但是由于陀螺儀和加速度計本身存在測量誤差，在長時間的計算過程中會產(chǎn)生漂移的情況，融合視覺傳感器數(shù)據(jù)能夠有效的抑制漂移；同時在視覺傳感器出現(xiàn)遮擋或者錯檢情況時，可暫時依靠IMU數(shù)據(jù)進行跟蹤，從而提高系統(tǒng)的魯棒性。本文通過將視覺傳感器獲取到的運動目標(biāo)特征點坐標(biāo)直接與IMU獲取到的相位位姿進行融合，簡化相對位姿求解過程，實現(xiàn)多傳感器融合的無人機實時運動跟蹤。

圖1 無人機運動跟蹤系統(tǒng)示意圖

EKF方法通常包括狀態(tài)估計和測量更新兩個部分，本小節(jié)將詳細介紹系統(tǒng)的狀態(tài)估計部分?；谝曈X傳感器數(shù)據(jù)的狀態(tài)估計計算量大，且相對于IMU的更新頻率太低，所以使用IMU來進行狀態(tài)估計更符合系統(tǒng)要求。

同時由于多傳感器的存在，需要先將所有數(shù)據(jù)轉(zhuǎn)換到同一坐標(biāo)系，再進行數(shù)據(jù)的融合。本文坐標(biāo)系定義如圖2所示，世界坐標(biāo)系為G，無人機IMU坐標(biāo)系Iu，無人機相機坐標(biāo)系C以及運動目標(biāo)IMU坐標(biāo)系It。

圖2 系統(tǒng)坐標(biāo)系定義

1.1 IMU狀態(tài)模型

IMU由三軸陀螺儀和三軸加速度計組成，陀螺儀可以獲取到IMU自身的旋轉(zhuǎn)角速度，加速度計則可以獲取到自身的線性加速度，由于測量誤差的存在，給出IMU的測量模型[12]：

ωm=Iω+bg+ng

(1)

(2)

已知IMU的測量模型，可以得到IMU的狀態(tài)向量：

(3)

(4)

(5)

其中：|Iω(t)×|表示反對稱矩陣，可由式(6)得到：

(6)

在無人機運動跟蹤過程中，需要時刻估計無人機與運動目標(biāo)的相對位姿，由式(1)、(2)可以得到角速度和線加速度的估計(不考慮測量高斯白噪聲n)，分別由式(7)、(8)給出：

(7)

(8)

再根據(jù)式(7)、(8)進行離散化(雅各比矩陣)可以得到[k,k+1]時刻的狀態(tài)估計值：

(9)

1.2 IMU誤差狀態(tài)模型

(10)

?δq

(11)

(12)

在確定系統(tǒng)誤差狀態(tài)向量后，根據(jù)IMU運動連續(xù)時間狀態(tài)公式(4)和IMU狀態(tài)估計公式(9)得出IMU誤差狀態(tài)連續(xù)時間轉(zhuǎn)移矩陣：

(13)

(14)

將誤差轉(zhuǎn)移方程離散化，可以得到Fd和Gd，用于求IMU估計過程中的協(xié)方差，設(shè)協(xié)方差P的初始值為零，P的更新方程如式(15)所示：

Pk + 1=Fd·Pk·FdT+Gd·Q·GdT

(15)

式(15)中，Q為噪聲矩陣，如式(16)所示：

(16)

(17)

(18)

(19)

2 視覺測量更新

在獲取到無人機和運動物體的狀態(tài)估計后，需要通過觀測量對狀態(tài)估計進行更新。本文將視覺傳感器的輸入作為觀測量，通過特征點檢測算法檢測運動物體，建立測量更新方程來對IMU狀態(tài)估計進行更新，最后將修正后的狀態(tài)發(fā)送給無人機控制系統(tǒng)。本文選擇將檢測到的運動目標(biāo)圖像坐標(biāo)直接作為EKF的輸入，避免需要大量計算負載的PnP算法求解，提高系統(tǒng)的實時性。

圖像數(shù)據(jù)的處理使用ORB算法[14]，ORB算法采用運算速度非?？斓腇AST角點檢測子，并針對FAST特征不具備方向的問題，加入了FAST特征的方向信息；特征點描述部分則是在BRIEF的基礎(chǔ)上改進了其對圖像噪聲敏感和不具備旋轉(zhuǎn)不比性的特點；雖然在特征點的描述細致程度上不如SIFT[15]和SURF[16]算法，但是在計算速度上，ORB性能明顯優(yōu)于兩者，針對嵌入式平臺計算能力有限和本文對實時性的要求，本文選擇ORB算法提取特征點。

2.1 測量方程

(20)

(21)

(22)

CPtar=ICR·GIR(GPtar-GPuav) +ICR·IPC

(23)

(24)

其中：Hf為測量雅克比矩陣，由h(x)求導(dǎo)獲得，見式(25):

(25)

2.2 狀態(tài)更新

在確定測量更新模型之后，可以得到完整EKF系統(tǒng)方程：

(26)

可根據(jù)EKF迭代得到無人機和運動目標(biāo)的誤差狀態(tài)估計值，然后用于校正IMU狀態(tài)估計。

3 實驗分析

本文中多傳感器的時間戳同步是數(shù)據(jù)融合的關(guān)鍵，無人機與移動設(shè)備上的IMU采樣頻率存在差異，無人機IMU的采樣頻率可以達到200 Hz或更高，而移動設(shè)備上的IMU受限于硬件性能，采樣頻率普遍在50～100 Hz，且兩者無法做到絕對的時間戳同步，本文針對這個問題，采用ROS[18](Robot Operating System，機器人操作系統(tǒng))的消息同步機制，將無人機與移動設(shè)備的IMU數(shù)據(jù)進行相對時間同步，然后將獲取到無人機和手機的狀態(tài)估計。本文中相機的采樣頻率為30 Hz，圖像分辨率為752×480，通過使用文獻[19]的方法，在相機采樣時刻，選取距當(dāng)前時刻最近的IMU狀態(tài)估計，進行測量更新，校正狀態(tài)估計。

作為實驗所需條件之一，本文編寫了一個Android APP用于采集手機中IMU的數(shù)據(jù)，并將數(shù)據(jù)轉(zhuǎn)化為ROS消息格式，然后通過Wi-Fi傳輸給無人機，實驗中使用的手機是三星Galaxy S7 edge，該機使用的IMU傳感器為LSM6DS3，同時無人機將使用LP-RESEARCH公司的LPMS-USBAL傳感器，實驗平臺搭建如圖3所示。

圖3 無人機運動跟蹤實驗平臺搭建

獲取到需要的傳感器參數(shù)后，本文針對應(yīng)用場景制作了兩類數(shù)據(jù)集，一類模擬無人機懸停，運動物體運動的場景；另一類則模擬無人機跟隨運動物體運動的場景。實驗中本文方法將通過與基于圖像的POSIT方法進行比較，驗證本文方法的準確性。

圖4 無人機懸停場景下的相對位置

圖5 無人機懸停場景下的相對位置誤差

其中圖4給出了無人機懸停場景下無人機與運動目標(biāo)的相對位置，其中虛線部分為運動目標(biāo)走過的真實路線，實線部分和點劃線則分別是本文方法計算得到的相對位置和POSIT方法計算得到的相對位置，圖5則表示無人機懸停場景下本文方法和POSIT方法計算的相對位置誤差；無人機跟隨運動場景下無人機與運動目標(biāo)保持同一速度和方向做直線運動6 m，同時保持無人機與運動物體之間的距離不變，圖6代表無人機運動跟蹤場景下的產(chǎn)生相對位置誤差，同樣實線代表本文方法，點劃線為POSIT方法。兩種場景下的相對位置誤差如表1所示，從表中可以得出，不論無人機懸?；蛘吒檲鼍跋?，本文方法得到的相對位置誤差都要優(yōu)于POSIT方法。無人機懸停情況下，本文方法得到的相對位置誤差為0.67 m，跟蹤場景下為0.273 m。

圖6 無人機跟蹤場景下的相對位置誤差

表1 相對位置誤差

4 結(jié)論

本文提出融合手機和無人機多傳感器參數(shù)的無人機運動目標(biāo)跟蹤方法用于解決基于單目視覺傳感器運動跟蹤中存在的問題。通過設(shè)計Android APP將手機中的IMU數(shù)據(jù)通過Wi-Fi傳輸給無人機，使得無人機可以通過使用手機和無人機的IMU數(shù)據(jù)用于系統(tǒng)狀態(tài)估計，然后通過視覺傳感器獲取的運動目標(biāo)圖像坐標(biāo)作為擴展卡爾曼濾波的測量更新修正IMU狀態(tài)估計產(chǎn)生的誤差。最后通過實測數(shù)據(jù)集模擬無人機懸停和無人機跟蹤場景，證明本文設(shè)計的方法在無人機的運動目標(biāo)跟蹤中能夠達到0.67m的定位精度，相對于單目視覺的方法有更高的精度。為了驗證該方法在無人機上的實際效果，未來的工作需要在無人機上運行本方法，并進行調(diào)試，同時將針對以人體為運動目標(biāo)的場景擴展到其他應(yīng)用場景。