空地協(xié)同機(jī)器人目標(biāo)定位方法

黎榮華，汪 雙，張 華

西南科技大學(xué) 信息工程學(xué)院，四川 綿陽 621010

由無人機(jī)與地面無人車組成的空地協(xié)同機(jī)器人具有視野廣、載荷重等優(yōu)勢(shì)，被廣泛應(yīng)用于應(yīng)急救援、環(huán)境探測(cè)等領(lǐng)域[1]。目標(biāo)定位是無人機(jī)協(xié)同飛行、精準(zhǔn)降落等自主控制技術(shù)的基礎(chǔ)。為了節(jié)省空間以在地面機(jī)器人上搭載更多傳感器，無人機(jī)起降平臺(tái)的面積通常極為有限，這就對(duì)目標(biāo)定位的精度和實(shí)時(shí)性有較高要求。但由于無人機(jī)載重量有限，通常選擇搭載重量較輕但性能有限的嵌入式處理器進(jìn)行實(shí)時(shí)數(shù)據(jù)處理。在嵌入式處理器低性能的限制下，兼顧目標(biāo)定位的精度與實(shí)時(shí)性，是實(shí)現(xiàn)空地協(xié)同機(jī)器人中無人機(jī)自主控制的關(guān)鍵和難點(diǎn)。

視覺目標(biāo)跟蹤技術(shù)是無人機(jī)與地面無人車之間相對(duì)定位的關(guān)鍵。視覺目標(biāo)跟蹤方法主要分為三類：基于生成式模型的目標(biāo)跟蹤方法、相關(guān)濾波類目標(biāo)跟蹤方法和深度學(xué)習(xí)類目標(biāo)跟蹤方法。早期目標(biāo)跟蹤算法主要基于生成式模型，先建立目標(biāo)特征模型，再依據(jù)目標(biāo)模型匹配目標(biāo)，進(jìn)而實(shí)現(xiàn)目標(biāo)跟蹤。但由于缺乏目標(biāo)模型更新機(jī)制，目標(biāo)外觀模型變化時(shí)，生成式模型算法魯棒性不佳。Wang等[2]提出一種改進(jìn)Meanshift算法以適應(yīng)目標(biāo)尺度變化，但僅使用了顏色特征，導(dǎo)致目標(biāo)與背景顏色相似時(shí)跟蹤效果易受影響。Zhang等[3]提出了一種基于卡爾曼濾波和Camshift多特征融合算法的跟蹤算法，解決了目標(biāo)被短暫遮擋導(dǎo)致的跟蹤失敗問題，但不能實(shí)現(xiàn)長(zhǎng)期跟蹤。Palafox等[4]、孫雨婷等[5]提出基于霍夫變換的標(biāo)記識(shí)別方法實(shí)現(xiàn)了長(zhǎng)期目標(biāo)跟蹤，但算法幀率較低且跟蹤效果易受環(huán)境影響。陳菲雨等[6]在TLD框架下增加檢測(cè)自矯正環(huán)節(jié)，保證了長(zhǎng)時(shí)間跟蹤的準(zhǔn)確度，但由于實(shí)時(shí)性不夠高，只能通過將圖像無線傳輸回地面PC端進(jìn)行處理，從而易受無線網(wǎng)絡(luò)狀況影響。近年來，隨著相關(guān)濾波和深度學(xué)習(xí)方法被引入目標(biāo)跟蹤領(lǐng)域，目標(biāo)跟蹤的性能得到顯著提升。Henriques等[7]提出的核相關(guān)濾波（kernelized correlation filters，KCF）算法顯著提升了目標(biāo)跟蹤方法的實(shí)時(shí)性，但目標(biāo)尺度變化、被遮擋或短暫消失導(dǎo)致跟蹤漂移甚至失敗的問題仍未被解決。林淑彬等[8]在相關(guān)濾波的基礎(chǔ)上，利用歷史視圖信息，通過像差抑制響應(yīng)圖變換，解決在相機(jī)運(yùn)動(dòng)、尺度變化等無人機(jī)場(chǎng)景下跟蹤產(chǎn)生漂移的問題，然而在處理完全遮擋方面的性能相對(duì)較弱。Huang等[9]提出了一種抑制檢測(cè)過程中出現(xiàn)畸變的新方法，在KCF的基礎(chǔ)上，通過對(duì)檢測(cè)階段生成的響應(yīng)圖的變化率進(jìn)行限制，從而提高跟蹤目標(biāo)的準(zhǔn)確性，但仍未解決目標(biāo)被遮擋或消失導(dǎo)致跟蹤失敗的問題。嚴(yán)飛等[10]提出一種基于FPGA的自適應(yīng)跟蹤算法，具有較高的實(shí)時(shí)性和跟蹤精度，但跟蹤之前先要獲取第一幀的目標(biāo)位置，才能將目標(biāo)模板保存用于后續(xù)跟蹤，缺乏自主性。Karakostas等[11]、劉芳等[12]提升了跟蹤算法在目標(biāo)發(fā)生遮擋時(shí)的魯棒性，但都存在實(shí)時(shí)性較差的問題。鐘莎等[13]解決了運(yùn)動(dòng)模糊、目標(biāo)小、目標(biāo)抖動(dòng)等問題，但沿用的目標(biāo)檢測(cè)算法的損失函數(shù)，對(duì)于目標(biāo)的框選不是特別精確。YOLO（you only look once）系列（YOLO[14]、YOLOv2[15]、YOLOv3[16]、YOLOv4[17]）算法雖然在精度和速度上有了明顯提升，但在機(jī)載計(jì)算機(jī)上實(shí)時(shí)性仍不高。

在空地協(xié)同應(yīng)用場(chǎng)景下，為了兼顧目標(biāo)定位的精度和實(shí)時(shí)性，解決無人機(jī)對(duì)地面無人車的實(shí)時(shí)精準(zhǔn)定位問題，提出一種新型定位標(biāo)記及標(biāo)記檢測(cè)方法，將定位標(biāo)記部署于地面無人車上，通過檢測(cè)標(biāo)記位置和方向，進(jìn)而實(shí)現(xiàn)對(duì)地面無人車的識(shí)別與定位。該方法根據(jù)標(biāo)記簡(jiǎn)單易識(shí)別的特點(diǎn)，基于標(biāo)記顏色和輪廓幾何特征，結(jié)合顏色分割和輪廓提取對(duì)圖像進(jìn)行預(yù)處理，縮小目標(biāo)識(shí)別范圍，并依據(jù)標(biāo)記幾何性質(zhì)，快速識(shí)別目標(biāo)，從而保證方法具有較高的實(shí)時(shí)性。同時(shí)，該方法舍棄通用目標(biāo)跟蹤方法的模型更新機(jī)制，犧牲抗遮擋性能和通用性，而避免因模型更新機(jī)制引入跟蹤漂移，以此獲取更高的定位精度，從而更適用于空地協(xié)同無人機(jī)自主控制場(chǎng)景。

1 標(biāo)記設(shè)計(jì)與檢測(cè)

首先，結(jié)合雙邊濾波、顏色分割和輪廓提取等預(yù)處理方法，提取圖形中輪廓特征；然后，利用目標(biāo)檢測(cè)算法從提取的輪廓特征中識(shí)別目標(biāo)輪廓，并計(jì)算目標(biāo)在圖像平面中像素坐標(biāo)和方向；最后，基于針孔相機(jī)成像模型，估計(jì)目標(biāo)在無人機(jī)機(jī)體坐標(biāo)系下的位置和姿態(tài)，流程如圖1所示。

圖1 方法流程圖Fig.1 Flow chart of method

1.1 標(biāo)記設(shè)計(jì)

為滿足無人機(jī)協(xié)同飛行目標(biāo)定位需要，定位標(biāo)記應(yīng)滿足以下要求：（1）標(biāo)記能夠提供位置和偏航角信息；（2）標(biāo)記顏色特征明顯，以便和背景信息進(jìn)行區(qū)分；（3）簡(jiǎn)單易識(shí)別，以保證機(jī)載計(jì)算機(jī)能夠?qū)崟r(shí)性處理。

本文設(shè)計(jì)的紅色雙圓形定位標(biāo)記如圖2所示。標(biāo)記為紅色，易與背景信息進(jìn)行區(qū)分。標(biāo)記采用易識(shí)別的圓形圖案設(shè)計(jì)，能夠保證實(shí)時(shí)性處理。將標(biāo)記中大圓圓心作為標(biāo)記位置，兩圓圓心連線作為偏航角參考線，可以提供位置和偏航角信息。與現(xiàn)有H型、矩形和三角形標(biāo)記使用角點(diǎn)特征相比，此標(biāo)記使用圓形輪廓特征，識(shí)別計(jì)算量小，在機(jī)載計(jì)算機(jī)上處理能達(dá)到更好的實(shí)時(shí)性。

圖2 定位標(biāo)記Fig.2 Positioning mark

1.2 標(biāo)記檢測(cè)

1.2.1 預(yù)處理

無人機(jī)圖像中背景信息通常較為復(fù)雜，若直接對(duì)圖像進(jìn)行邊緣檢測(cè)，會(huì)提取到過多與目標(biāo)無關(guān)的輪廓特征，而對(duì)目標(biāo)輪廓識(shí)別產(chǎn)生干擾，且增加識(shí)別運(yùn)算量。利用定位標(biāo)記顏色特征較為明顯的特點(diǎn)，在Suzuki-Abe輪廓提取算法前引入顏色分割，只對(duì)圖像中與標(biāo)記顏色相同的區(qū)域進(jìn)行輪廓提取，可以減少提取到的輪廓特征數(shù)量，進(jìn)而縮小識(shí)別范圍，提高識(shí)別成功率和實(shí)時(shí)性。

相機(jī)大多直接輸出RGB圖像，而RGB顏色模型受光照影響較大，不利于顏色分割?？紤]到HSV顏色模型受光照影響較小，故先將圖像從RGB顏色模型轉(zhuǎn)換成HSV顏色模型再進(jìn)行顏色分割，可以減小光照對(duì)顏色分割的影響。改進(jìn)前后輪廓提取效果如圖3所示?？梢钥闯觯腩伾指詈筇崛〉降妮喞獢?shù)量明顯減少，有利于減少誤識(shí)別和提高實(shí)時(shí)性。

圖3 輪廓提取效果對(duì)比Fig.3 Comparison of contours extraction effect

1.2.2 輪廓識(shí)別與定位

Hough變換常被用于圓形輪廓識(shí)別，但經(jīng)過實(shí)際測(cè)試，在無人機(jī)處于傾斜姿態(tài)時(shí)，圓形標(biāo)記在圖像中會(huì)形變?yōu)闄E圓形，導(dǎo)致識(shí)別出的圓心位置不準(zhǔn)確。針對(duì)此問題，提出一種圓形輪廓檢測(cè)方法，先利用圓的幾何性質(zhì)識(shí)別圓形輪廓，再結(jié)合輪廓重心求解法計(jì)算圓心。

（1）圓形輪廓識(shí)別

由于圓具有周長(zhǎng)的平方L2與面積S的比值為4π這一幾何性質(zhì)，故通過計(jì)算輪廓周長(zhǎng)平方和面積的比值，就可以判斷其形狀是否為圓，運(yùn)算量相較于Hough變換更小，從而可以達(dá)到更高的實(shí)時(shí)性。

（2）圓心求解

由于圓形輪廓重心就是圓心，故利用輪廓幾何矩重心求解法得到圓心像素坐標(biāo)，計(jì)算公式如下：

式中，f(x,y)為圖像在像素坐標(biāo)(x,y)處的灰度值，m10、m01為輪廓一階矩，m00為輪廓零階矩。

兩種方法檢測(cè)效果如圖4所示。結(jié)果表明，在無人機(jī)姿態(tài)變化引起標(biāo)記在圖像中發(fā)生形變的情況下，利用幾何矩計(jì)算目標(biāo)重心，可以獲取更高的定位精度。同時(shí)，統(tǒng)計(jì)兩種方法對(duì)一百幀圖像的檢測(cè)耗時(shí)，得到所提方法每幀圖像平均檢測(cè)耗時(shí)約為0.5 ms，優(yōu)于Hough變換的5 ms。

圖4 檢測(cè)效果對(duì)比Fig.4 Comparison of detection effect

將標(biāo)記大圓圓心像素坐標(biāo)作為目標(biāo)位置，向量P1P2與U軸正向夾角θ′作為目標(biāo)方向，示意圖如圖5所示。

圖5 標(biāo)記位置和方向Fig.5 Location and direction of mark

求解大圓圓心像素坐標(biāo)得到目標(biāo)位置P1(u,v)，同理可得小圓圓心位置P2(u′,v′)，最后由兩圓圓心幾何關(guān)系即可得到目標(biāo)方向θ′。

1.2.3 位姿估計(jì)

位姿估計(jì)是根據(jù)檢測(cè)到像素坐標(biāo)系下目標(biāo)的像素坐標(biāo)P1(u,v)和方向角θ′，估計(jì)機(jī)體坐標(biāo)系下目標(biāo)點(diǎn)三維坐標(biāo)P(x,y,z)和偏航角θ。假設(shè)固定安裝相機(jī)時(shí)已將相機(jī)坐標(biāo)系與無人機(jī)機(jī)體坐標(biāo)系對(duì)齊，則近似將相機(jī)坐標(biāo)系作為機(jī)體坐標(biāo)系。根據(jù)針孔相機(jī)成像模型，相機(jī)坐標(biāo)系下目標(biāo)三維坐標(biāo)與像素坐標(biāo)系下目標(biāo)像素坐標(biāo)間關(guān)系如下：

式中，K為相機(jī)內(nèi)參矩陣，由相機(jī)標(biāo)定得到，P為相機(jī)坐標(biāo)系下目標(biāo)三維位置矢量，z為目標(biāo)深度信息，由雙目相機(jī)直接給出。求解此矩陣方程即可得到機(jī)體坐標(biāo)系下目標(biāo)三維坐標(biāo)。同時(shí)，由成像模型幾何關(guān)系可知，目標(biāo)機(jī)體坐標(biāo)系下偏航角θ與像素坐標(biāo)系下方向角θ′相等。

2 實(shí)驗(yàn)與分析

為驗(yàn)證所提方法有效性，搭建了如圖6所示空地協(xié)同機(jī)器人實(shí)驗(yàn)平臺(tái)和動(dòng)作捕捉系統(tǒng)實(shí)驗(yàn)場(chǎng)地。動(dòng)作捕捉系統(tǒng)能實(shí)時(shí)定位場(chǎng)地中物體，定位精度可達(dá)到亞毫米級(jí)，可近似作為理論真值，用于分析本文方法定位精度。

圖6 實(shí)驗(yàn)平臺(tái)和場(chǎng)地Fig.6 Testsite and platform

本文主要從方法魯棒性、實(shí)時(shí)性、精度和有效性進(jìn)行驗(yàn)證，實(shí)驗(yàn)硬件及環(huán)境配置如表1所示。

表1 硬件及環(huán)境配置Table 1 Hardware and environment configuration

2.1 主觀實(shí)驗(yàn)

為驗(yàn)證所提方法魯棒性，根據(jù)無人機(jī)實(shí)際飛行特點(diǎn)，在高度變化、姿態(tài)變化、光照變化、背景復(fù)雜和圖像模糊五種場(chǎng)景下進(jìn)行目標(biāo)檢測(cè)實(shí)驗(yàn)，目標(biāo)檢測(cè)效果如圖7所示。

圖7 目標(biāo)檢測(cè)效果Fig.7 Target detection effect

實(shí)驗(yàn)結(jié)果顯示，所提方法在無人機(jī)常見飛行場(chǎng)景下魯棒性較好。

2.2 量化實(shí)驗(yàn)

為準(zhǔn)確分析所提方法定位精度，將均方根誤差（root mean square error，RMSE）作為精度評(píng)價(jià)指標(biāo)，并選取核相關(guān)濾波算法與本文方法進(jìn)行對(duì)比實(shí)驗(yàn)。

式中，x為估計(jì)值，x*為對(duì)應(yīng)真值，n為數(shù)據(jù)量。實(shí)驗(yàn)中無人機(jī)保持懸停，高度為1.5 m，控制地面無人車按一定軌跡移動(dòng)，兩種方法定位數(shù)據(jù)如圖8所示。由于目標(biāo)z軸位置由雙目相機(jī)直接給出，故只對(duì)算法x軸、y軸和偏航角定位數(shù)據(jù)進(jìn)行分析。

圖8 定位數(shù)據(jù)對(duì)比Fig.8 Comparison of location data

實(shí)驗(yàn)結(jié)果表明，在核相關(guān)濾波方法初始化目標(biāo)時(shí)，無人機(jī)的輕微晃動(dòng)會(huì)導(dǎo)致難以手動(dòng)準(zhǔn)確框選目標(biāo)，從而在初始幀引入誤差。而且，核相關(guān)濾波方法定位精度會(huì)隨時(shí)間延長(zhǎng)而下降。經(jīng)過計(jì)算，本文方法x軸和y軸位置數(shù)據(jù)均方根誤差分別是3.9 mm和3.6 mm，優(yōu)于核相關(guān)濾波方法的13.3 mm和14.3 mm。由于核相關(guān)濾波方法不能識(shí)別目標(biāo)方向，因此僅將本文方法偏航角數(shù)據(jù)與真值對(duì)比，均方根誤差為1.5°。

為了分析所提方法實(shí)時(shí)性，將處理每幀圖像的平均耗時(shí)tˉ作為衡量方法實(shí)時(shí)性的指標(biāo)。

實(shí)驗(yàn)統(tǒng)計(jì)了一段時(shí)間內(nèi)兩種方法處理每幀圖像的耗時(shí)，隨機(jī)選取其中30幀結(jié)果如圖9所示。經(jīng)過計(jì)算，本文方法平均耗時(shí)約為12 ms，優(yōu)于核相關(guān)濾波方法的56 ms。

圖9 耗時(shí)對(duì)比Fig.9 Comparison of time comsuming

2.3 自主跟蹤飛行實(shí)驗(yàn)

為進(jìn)一步驗(yàn)證在無人機(jī)自主跟蹤地面無人車場(chǎng)景下方法的有效性，與無人機(jī)控制相結(jié)合，基于搭建的空地機(jī)器人實(shí)驗(yàn)平臺(tái)和動(dòng)作捕捉系統(tǒng)實(shí)驗(yàn)場(chǎng)地，進(jìn)行了無人機(jī)跟蹤飛行實(shí)驗(yàn)。由于室內(nèi)實(shí)驗(yàn)場(chǎng)地高度限制，為保證機(jī)器人安全，故設(shè)置無人機(jī)與無人車相對(duì)高度1.5 m。利用動(dòng)作捕捉系統(tǒng)記錄下無人機(jī)與無人車的二維平面軌跡，如圖10所示。對(duì)比無人機(jī)與無人車軌跡可知，無人機(jī)能夠穩(wěn)定跟蹤地面無人車飛行。因此，本文所提相對(duì)定位方法能夠有效應(yīng)用于無人機(jī)自主控制中對(duì)地面無人車的識(shí)別定位。

圖10 平面軌跡對(duì)比Fig.10 Comparison of plane trajectory

3 結(jié)語

針對(duì)空地協(xié)同機(jī)器人中無人機(jī)對(duì)地面無人車的實(shí)時(shí)精準(zhǔn)定位問題，提出了一種新型定位標(biāo)記及標(biāo)記檢測(cè)方法。該方法首先根據(jù)標(biāo)記顏色特征，在輪廓提取前引入顏色分割，排除背景干擾以減少誤識(shí)別，并縮小識(shí)別范圍以保證較高的實(shí)時(shí)性；然后，提出一種基于輪廓幾何特征和幾何矩理論的圓形輪廓快速識(shí)別定位方法，兼顧了目標(biāo)定位的精度和實(shí)時(shí)性。實(shí)驗(yàn)結(jié)果表明，所提方法滿足無人機(jī)對(duì)地面無人車的實(shí)時(shí)精準(zhǔn)定位要求，為實(shí)際工程應(yīng)用提供了有力的理論依據(jù)和數(shù)據(jù)支撐，具備顯著的工程意義。雖然本文方法在定位精度和實(shí)時(shí)性上有一定的示范效果，但后續(xù)還會(huì)針對(duì)目標(biāo)發(fā)生遮擋導(dǎo)致識(shí)別定位失敗問題進(jìn)行深入研究，以期達(dá)到更為魯棒的識(shí)別定位效果。