基于Kalman濾波的室內(nèi)無人機(jī)實(shí)時(shí)定位算法研究

侯楊陽 華壯

摘? 要：文章針對小型無人機(jī)在室內(nèi)這一類環(huán)境的識別與跟蹤進(jìn)行了研究。利用標(biāo)志小球作為機(jī)身特征點(diǎn)，可以實(shí)現(xiàn)無人機(jī)的快速捕捉和實(shí)時(shí)跟蹤;在此基礎(chǔ)上加入Kalman濾波運(yùn)動估計(jì)，去除跟蹤過程中的噪聲干擾;最后計(jì)算出目標(biāo)三維信息，并利用Windows內(nèi)存映射技術(shù)將該數(shù)據(jù)傳輸給訪問進(jìn)程，實(shí)現(xiàn)了室內(nèi)小型無人機(jī)位置信息的實(shí)時(shí)捕捉和傳輸，為其自主飛行提供了可靠的數(shù)據(jù)依據(jù)。

關(guān)鍵詞：攝像機(jī)標(biāo)定;目標(biāo)識別與跟蹤;Kalman運(yùn)動估計(jì);內(nèi)存映射

中圖分類號：V279;TN98? ? ? ?文獻(xiàn)標(biāo)識碼：A 文章編號：2096-4706（2020）16-0013-04

Research on Real-time Positioning Algorithm of Indoor UAV Based on Kalman Filter

HOU Yangyang，HUA Zhuang

（School of Physics and Electronic Engineering，Jiangsu Second Normal University，Nanjing? 211200，China）

Abstract：Aiming at the indoor environment，the recognition and tracking of small drones are studied. Using the marker ball as the feature point of the fuselage，the UAV can be quickly captured and tracked in real time;on this basis，Kalman filter motion estimation is added to remove the noise interference in the tracking process;finally the three-dimensional information of the target is calculated and used the Windows memory mapping technology transmits the data to the access process，which realizes the real-time capture and transmission of the position information of the indoor small UAV，and provides a reliable data basis for its autonomous flight.

Keywords：camera calibration;target recognition and tracking;Kalman motion estimation;memory mapping

0? 引? 言

目前，無人機(jī)在戶外的應(yīng)用范圍已經(jīng)非常廣泛，而由于室內(nèi)環(huán)境復(fù)雜、障礙物多等原因，無人機(jī)在室內(nèi)的應(yīng)用范圍則相對比較局限，但也已經(jīng)有研究開始擴(kuò)展其可用性。利用無人機(jī)對室內(nèi)狀況的巡查和監(jiān)督中，無人機(jī)的定位和跟蹤尤為重要?，F(xiàn)有的無人機(jī)普遍采用的是慣性導(dǎo)航與GPS導(dǎo)航系統(tǒng)來確定自身的位置信息，這種方法不具備可視性。因此，本文基于實(shí)驗(yàn)室對雙目視覺與目標(biāo)識別方面的研究基礎(chǔ)，利用雙網(wǎng)絡(luò)攝像頭確認(rèn)與跟蹤無人機(jī)的空間信息，使得無人機(jī)的姿態(tài)信息一目了然。本文的研究內(nèi)容為室內(nèi)無人機(jī)姿態(tài)控制研究領(lǐng)域的工作者提供了另外一種發(fā)展思路。

1? 目標(biāo)識別

1.1? 攝像機(jī)標(biāo)定

相機(jī)平行放置的立體視覺系統(tǒng)最簡單[1]，也是最常用的方式，使雙目成像模型得到簡化，獲取物體的三維數(shù)據(jù)變得更加容易，簡化后的理想雙目視覺成像系統(tǒng)原理如圖1所示。

在理想的平行成像系統(tǒng)中，空間一點(diǎn)A（XW，YW，ZW）在左右圖像中坐標(biāo)分別為al（ul，vl）和ar（ur，vr），兩相同攝像機(jī)平行放置，焦距相等，均為f [2]。

根據(jù)該原理，本文搭建了平行的雙目視覺系統(tǒng)，其硬件主要包括兩個(gè)一體化高清網(wǎng)絡(luò)高速智能球形攝像機(jī)，兩根足夠長的網(wǎng)線，網(wǎng)絡(luò)服務(wù)器，一臺計(jì)算機(jī)和固定模擬云臺。硬件組成實(shí)物結(jié)構(gòu)如圖2所示。

攝像機(jī)標(biāo)定有多種方法可實(shí)現(xiàn)，其中，基于標(biāo)定物方法有諸多優(yōu)點(diǎn)：對攝像機(jī)模型沒有限制，適用于任意攝像機(jī)模型，有較高的標(biāo)定精度，操作簡單易實(shí)現(xiàn)[3]。因此，本文選用該方法進(jìn)行標(biāo)定，獲取兩攝像機(jī)的參數(shù)矩陣。

在空間中選取樣本點(diǎn)，計(jì)算得出左右攝像機(jī)的M矩陣：

1.2? 運(yùn)動目標(biāo)的檢測識別與跟蹤

本文選用橘黃色乒乓球來作為機(jī)身反射物，將乒乓球固定在小型無人機(jī)機(jī)身上作為機(jī)身特征，如圖3所示。

本系統(tǒng)需要識別的運(yùn)動實(shí)則為規(guī)則球狀目標(biāo)的運(yùn)動，可以有效利用該球的兩大特點(diǎn)：顏色和形狀特征。小球或者小型無人機(jī)所處的環(huán)境是未知的，可能復(fù)雜也可能簡單，因此根據(jù)目標(biāo)本身所具有的特征，將其從周圍環(huán)境中檢測出來，是算法需實(shí)現(xiàn)的第一步。

具體操作步驟為：

（1）圖像的采集處理，在Visual Studio 2013上實(shí)現(xiàn)兩攝像機(jī)的同步顯示。

（2）圖像預(yù)處理，消除原始圖像噪聲[4]。

（3）R通道顏色分離，將圖像二值化，并進(jìn)行形態(tài)學(xué)腐蝕濾波操作，消除毛刺，得到感興趣區(qū)域;

（4）查找輪廓，計(jì)算檢測到的所有輪廓的周長和面積，進(jìn)行輪廓剔除處理，最終得到小球的在二維圖像中的圓形輪廓[1]。

（5）由數(shù)學(xué)知識可知，給定一個(gè)圓，半徑為R，可以求得該圓形的面積為S，輪廓周長為C，有：S=πR2，C= 2πR，設(shè)定面積-周長系數(shù)為k，那么該圓的面積和周長滿足以下關(guān)系：

k==1

也就是說，給定以閉環(huán)區(qū)域，求得它的面積和周長，就能夠計(jì)算該區(qū)域的面積-周長系數(shù)k，對k可以大致做這樣的劃分：

利用輪廓剔除系數(shù)，可以有效過濾掉非目標(biāo)的連通區(qū)域，最終提取到目標(biāo)小球的輪廓，獲取左右視圖目標(biāo)中心點(diǎn)坐標(biāo)。

2? 基于Kalman的目標(biāo)運(yùn)動估計(jì)

2.1? Kalman濾波原理

卡爾曼濾波器（Kalman Filtering，KF）是針對動態(tài)系統(tǒng)的狀態(tài)序列進(jìn)行線性最小誤差估計(jì)的一種濾波算法，主要利用遞推原理來修正估計(jì)狀態(tài)，屬于常用的目標(biāo)狀態(tài)估計(jì)算法濾波器[5]。

單純利用攝像機(jī)進(jìn)行對目標(biāo)的跟蹤，很容易受環(huán)境的干擾，因此為了更加精準(zhǔn)地定位無人機(jī)目標(biāo)運(yùn)用卡爾曼濾波器，其可以根據(jù)上一幀小球的位置信息，結(jié)合當(dāng)前幀的觀測結(jié)果，實(shí)時(shí)準(zhǔn)確地預(yù)測和更新當(dāng)前幀的運(yùn)動情況。建立卡爾曼濾波器數(shù)學(xué)模型時(shí)，在很短的時(shí)間內(nèi)小球的運(yùn)動可以看成是線性直線運(yùn)動[3]。

具體實(shí)現(xiàn)步驟如下：

（1）計(jì)算運(yùn)動目標(biāo)的特征信息。通過對小球進(jìn)行預(yù)處理以及顏色輪廓特征檢測，得到左右攝像機(jī)小球質(zhì)心的二維坐標(biāo)。

（2）定義卡爾曼濾波器。

狀態(tài)向量：

X=[x? ?dx? ?y? ?dy]T

x、y分別是中心點(diǎn)在圖像坐標(biāo)系中沿x、y方向的位移，dx、dy則分別是沿x、y方向的速度。

狀態(tài)轉(zhuǎn)移矩陣：

觀測值則為當(dāng)前幀直接檢測到的小球的二維像素信息，橫縱坐標(biāo)分別為x、y，則觀測矩陣：

將第一幀檢測出的特征信息作為初始值，對卡爾曼濾波器進(jìn)行初始化，即對X0賦初值。

（3）用卡爾曼濾波器利用前一幀的信息和當(dāng)前幀的觀測值對當(dāng)前幀的目標(biāo)區(qū)域進(jìn)行預(yù)測和更新，并記錄更新后的當(dāng)前幀的目標(biāo)信息。

2.2? 實(shí)驗(yàn)驗(yàn)證與分析

2.2.1? 配置環(huán)境

（1）兩臺網(wǎng)絡(luò)化的球形攝像機(jī)作為圖像采集設(shè)備，圖像尺寸為1028×720，幀率為25幀/秒。

（2）使用一臺式電腦作為圖像處理計(jì)算機(jī)：CPU為Intel Core i5，主頻可達(dá)到3.20 GHz，內(nèi)存為8 GB。

（3）電腦為32位Windows 7操作系統(tǒng)，以Visual Studio為開發(fā)環(huán)境，使用C++語言實(shí)現(xiàn)程序編寫。

2.2.2? 實(shí)驗(yàn)驗(yàn)證

加入Kalman濾波器后，左右視圖跟蹤軌跡如圖4所示，其中曲線表示目標(biāo)的運(yùn)動軌跡。

通過實(shí)驗(yàn)數(shù)據(jù)采集，運(yùn)動軌跡點(diǎn)的計(jì)算值和測量值之間的比較如表1所示。

實(shí)驗(yàn)結(jié)果表明，該系統(tǒng)測量誤差在20 mm以內(nèi)，較其他測量方法有較高的測量精度;系統(tǒng)處理時(shí)間控制在50 ms以內(nèi)，即平均每秒處理20幀，滿足系統(tǒng)實(shí)時(shí)性的要求;加入Kalman濾波器后，去除了環(huán)境干擾造成的毛刺現(xiàn)象，運(yùn)動軌跡較為平滑。

3? 基于內(nèi)存映射的數(shù)據(jù)傳輸與共享

通過上述內(nèi)容可以得到目標(biāo)的實(shí)時(shí)位置信息，這些數(shù)據(jù)需要被實(shí)時(shí)快速地傳輸給無人機(jī)飛行控制系統(tǒng)，即圖像處理進(jìn)程和飛行控制進(jìn)程間需要數(shù)據(jù)的傳輸通道，使得無人機(jī)可以根據(jù)反饋信息及時(shí)調(diào)整自身位置情況[1]。針對這一需求，對同一PC中不同進(jìn)程間的數(shù)據(jù)傳輸方法做了具體分析，最后采用了內(nèi)存映射文件，利用該技術(shù)來實(shí)現(xiàn)不同進(jìn)程間數(shù)據(jù)的實(shí)時(shí)傳輸與共享[6]。實(shí)驗(yàn)表明，該方法在進(jìn)程數(shù)據(jù)實(shí)時(shí)傳輸上取得了較為理想的效果，實(shí)驗(yàn)結(jié)果如表2所示。

可以看到目標(biāo)識別與跟蹤進(jìn)程中得到的數(shù)據(jù)被實(shí)時(shí)地傳遞給另一訪問進(jìn)程，實(shí)驗(yàn)效果較為理想。同樣，在無人機(jī)的上位機(jī)飛行控制系統(tǒng)中也可以實(shí)現(xiàn)對該數(shù)據(jù)的實(shí)時(shí)訪問與讀取，這為整個(gè)無人機(jī)飛行控制系統(tǒng)實(shí)現(xiàn)自主飛行提供了可靠數(shù)據(jù)，打下了堅(jiān)實(shí)的基礎(chǔ)。

4? 結(jié)? 論

針對室內(nèi)環(huán)境區(qū)域，本文實(shí)現(xiàn)了基于Kalman濾波的無人機(jī)實(shí)時(shí)定位方法。首先，利用雙目視覺原理搭建了實(shí)驗(yàn)系統(tǒng)平臺，并對兩攝像機(jī)進(jìn)行了參數(shù)標(biāo)定;利用機(jī)身輔助特征——黃色小球，實(shí)現(xiàn)對目標(biāo)的準(zhǔn)確識別;為了更好地追蹤目標(biāo)，加入了Kalman濾波運(yùn)動估計(jì)，利用遞推原理建立幀間關(guān)系，實(shí)現(xiàn)跟蹤的可靠性;最后將得到的目標(biāo)三維數(shù)據(jù)通過內(nèi)存映射的方法實(shí)時(shí)傳輸給訪問進(jìn)程。整個(gè)方法有較強(qiáng)的實(shí)時(shí)性，且獲取的三維信息有較高的精度，具有一定的應(yīng)用價(jià)值。

參考文獻(xiàn)：

[1] SHIN D，TJAHJADI T. Clique descriptor of affine invariant regions for robust wide baseline image matching [J]. Pattern Recognition，2010，43（10）：3261-3272.

[2] MARINAS J，SALGADO L，ARR?SPIDE J，et al. Traffic Sign Detection and Tracking Using Robust 3D Analysis [C]//Proceedings of the 2012 Third International Conference on Emerging Security Technologies. Washington：IEEE，2012：78-81.

[3] GUPTA S G，GHONGE M M，JAWANDHIYA P M. Review of Unmanned Aircraft System（UAS） [J]. International Journal of Advanced Research in Computer Engineering & Technology （IJARCET），2013，2（4）：1646-1658.

[4] 于清華，黃開宏，盧惠民，等.基于雙目視覺的足球機(jī)器人對三維空間目標(biāo)的運(yùn)動估計(jì)與攔截 [C]//西安：中國自動化學(xué)會控制理論專業(yè)委員會.第三十二屆中國控制會議論文集（D卷），2013：5943-5948.

[5] 趙廣輝，卓松，徐曉龍.基于卡爾曼濾波的多目標(biāo)跟蹤方法 [J].計(jì)算機(jī)科學(xué)，2018，45（8）：253-257+276.

[6] RICHTER J，NASARRE C.Windows核心編程：第5版 [M].葛子昂，周靖，廖敏，譯.北京：清華大學(xué)出版社，2008.

作者簡介：侯楊陽（1989—），女，漢族，山東菏澤人，助理實(shí)驗(yàn)員，碩士，研究方向：圖像處理算法分析與研究。