吳 雷，黃 斌，李旺靈，孫永榮

(南京航空航天大學(xué) 導(dǎo)航研究中心·南京·211106)

0 引 言

無(wú)人飛行器具有成本低、體積小、質(zhì)量小、機(jī)動(dòng)性強(qiáng)等獨(dú)到優(yōu)勢(shì)，因而在軍事、農(nóng)業(yè)和工業(yè)領(lǐng)域有著廣闊的應(yīng)用前景[1]。無(wú)人機(jī)在執(zhí)行任務(wù)時(shí)需要感知自身的位姿參數(shù)及周圍的環(huán)境信息。其常用的導(dǎo)航方式(如GPS和慣導(dǎo)技術(shù))因使用環(huán)境和成本等原因，難以滿足無(wú)人機(jī)在復(fù)雜環(huán)境下對(duì)高精度實(shí)時(shí)定位的需求?；谝曈X(jué)技術(shù)的位姿測(cè)量方法因結(jié)構(gòu)簡(jiǎn)單、非接觸、成本低、近距精度高等優(yōu)點(diǎn)，在無(wú)人機(jī)定位領(lǐng)域得到了廣泛的應(yīng)用[2-3]?，F(xiàn)有的基于機(jī)器視覺(jué)技術(shù)的無(wú)人機(jī)位姿測(cè)量方法，多通過(guò)對(duì)無(wú)人機(jī)本身進(jìn)行識(shí)別和定位而實(shí)現(xiàn)其功能，其精度與可靠性都不能令人滿意，而利用人工標(biāo)記點(diǎn)進(jìn)行識(shí)別的方法，存在著特征點(diǎn)布局及標(biāo)定等問(wèn)題。

為了滿足各種場(chǎng)景下的高精度、高速率的無(wú)人機(jī)位姿測(cè)量的要求，本文提出了一種以人工標(biāo)志信號(hào)燈為特征點(diǎn)的單目視覺(jué)測(cè)量方案[4]，利用由多場(chǎng)景確定的點(diǎn)位置測(cè)量方法[5]預(yù)先確定標(biāo)記點(diǎn)在無(wú)人機(jī)機(jī)體系下的三維空間位置，隨后用相機(jī)實(shí)時(shí)采集特征點(diǎn)的成像圖片，并提取它們的亞像素坐標(biāo)[6]，最后將點(diǎn)的二維和三維坐標(biāo)進(jìn)行匹配，利用EpnP算法[7]求解無(wú)人機(jī)的位置和姿態(tài)。

1 特征點(diǎn)空間位置求解

1.1 攝像機(jī)模型

常用的攝像機(jī)成像模型包括3種：小孔成像模型、正交投影模型、擬透視投影模型。其中，獲得應(yīng)用最多的是小孔成像模型，其也被稱為針孔相機(jī)模型。針孔成像模型涉及到圖1中4個(gè)坐標(biāo)系之間的轉(zhuǎn)換，現(xiàn)具體說(shuō)明如下：

(4)像素坐標(biāo)系：它的位置與像平面坐標(biāo)系一致，以像素為單位。不同于以上三者，其以物理長(zhǎng)度為單位。它的原點(diǎn)位于圖像的左上角，u軸與像平面坐標(biāo)的X軸平行，v軸與像平面坐標(biāo)系的Y軸平行,其坐標(biāo)系關(guān)系如圖1所示。

圖1 坐標(biāo)系關(guān)系Fig.1 Relationship of coordinate systems

通過(guò)對(duì)上述坐標(biāo)系進(jìn)行一系列的坐標(biāo)轉(zhuǎn)換，最終可以得到世界坐標(biāo)與像素坐標(biāo)之間的轉(zhuǎn)換關(guān)系，即針孔成像模型。

(1)

1.2 特征點(diǎn)三維位置的測(cè)量方法

在進(jìn)行無(wú)人機(jī)位姿測(cè)量時(shí)，首先需要獲得特征點(diǎn)在無(wú)人機(jī)機(jī)體系中的三維空間位置，而在實(shí)際應(yīng)用中，特征點(diǎn)的分布往往不規(guī)則，即使預(yù)先確定了位置，在安裝時(shí)也會(huì)出現(xiàn)誤差。如何精確地確定多個(gè)特征點(diǎn)的三維坐標(biāo)，是無(wú)人機(jī)位姿測(cè)量的基礎(chǔ)。本文采用了一種基于視覺(jué)技術(shù)的由多場(chǎng)景確定的空間點(diǎn)位置測(cè)量方法，其核心思想在于首先利用標(biāo)定模板標(biāo)定出各個(gè)場(chǎng)景的位姿參數(shù)，然后利用各個(gè)場(chǎng)景下的二維圖像坐標(biāo)與場(chǎng)景參數(shù)，求解特征點(diǎn)的三維空間位置。

本文使用的標(biāo)定模板是棋盤格標(biāo)定板，將棋盤標(biāo)定板的橫軸、縱軸分別作為X軸、Y軸，Z軸垂直于棋盤平面，并以此建立棋盤坐標(biāo)系。由此，標(biāo)定板上每一個(gè)棋盤角點(diǎn)的三維坐標(biāo)已知，再由相機(jī)采集的場(chǎng)景圖片提取棋盤角點(diǎn)的圖像坐標(biāo)，通過(guò)坐標(biāo)之間的對(duì)應(yīng)關(guān)系可以解算出該場(chǎng)景的位姿參數(shù)。在此基礎(chǔ)上，再?gòu)拿總€(gè)場(chǎng)景的圖像中利用手動(dòng)標(biāo)記的方法給出待測(cè)量的特征點(diǎn)的圖像坐標(biāo)，結(jié)合對(duì)應(yīng)的場(chǎng)景位姿參數(shù)，根據(jù)相機(jī)模型便可以解算出特征點(diǎn)的三維空間位置，最后利用LM迭代算法使得重投影誤差實(shí)現(xiàn)最小化，從而提高位置求解的精度。如圖2所示，將棋盤格標(biāo)定板與無(wú)人機(jī)置于同一場(chǎng)景下，棋盤格用于求解當(dāng)前的場(chǎng)景信息，藍(lán)色的十字是由手動(dòng)選取的特征點(diǎn)的圖像位置。經(jīng)實(shí)驗(yàn)測(cè)量，手動(dòng)選點(diǎn)的方式可以滿足應(yīng)用精度需求。圖2(b)是由該方法獲得的特征點(diǎn)在棋盤坐標(biāo)系下的三維坐標(biāo)，通過(guò)對(duì)比可知，該方法的三維標(biāo)定誤差小于2mm。

(a) 特征點(diǎn)三維標(biāo)定場(chǎng)景圖(a) Scene of calibration

(b) 特征點(diǎn)三維標(biāo)定結(jié)果圖(b) Result of calibration圖2 特征點(diǎn)的三維標(biāo)定Fig.2 Three-dimensional calibration of the feature points

2 特征點(diǎn)圖像坐標(biāo)求解

2.1 特征點(diǎn)識(shí)別與跟蹤

本文采用多個(gè)紅色的LED燈作為標(biāo)記點(diǎn)。通電之后，LED燈會(huì)呈現(xiàn)出圓形的紅色光斑，且亮度較高，與環(huán)境及無(wú)人機(jī)本身的對(duì)比度較大，易于被檢測(cè)和識(shí)別。本文對(duì)于特征點(diǎn)的識(shí)別也正是基于顏色、亮度和形狀這3個(gè)特征。

在圖像處理中，顏色通常由3個(gè)獨(dú)立的屬性來(lái)描述，由此構(gòu)成顏色空間。按照構(gòu)成原理，顏色空間可分為2大類：基色顏色空間和色、亮分離顏色空間。前者的典型代表是RGB顏色空間，后者的典型代表是HSV空間。HSV空間將亮度從色彩中分離出來(lái)，從而可以更好地感知圖像中的顏色和亮度信息，繼而方便從圖像中提取感興趣的區(qū)域。工業(yè)相機(jī)采集的圖片一般為RGB格式，先將圖片從RGB空間轉(zhuǎn)換到HSV空間，紅色的范圍為0～10和156～180 兩個(gè)區(qū)間，然后以此為閾值對(duì)圖片進(jìn)行分割，提取出其中的紅色區(qū)域。在實(shí)際應(yīng)用中，可根據(jù)實(shí)際情況對(duì)參數(shù)進(jìn)行微調(diào)。

考慮到在實(shí)際應(yīng)用場(chǎng)景中可能出現(xiàn)其他紅色物體，進(jìn)而影響特征點(diǎn)的識(shí)別，因而需要加入更多的約束條件。由于LED燈的亮度較高，可以將其作為1項(xiàng)判斷指標(biāo)，對(duì)亮度通道進(jìn)行篩選。如果對(duì)亮度分量采用閾值分割的方法，閾值的選擇會(huì)對(duì)檢測(cè)效果影響較大，難以滿足亮度變化范圍較大的場(chǎng)景的需求。本文使用了一種調(diào)整相機(jī)曝光率的方法來(lái)對(duì)亮度特征進(jìn)行篩選，使得檢測(cè)效果對(duì)曝光率的選取不敏感。在實(shí)驗(yàn)中，發(fā)現(xiàn)了1個(gè)合適的曝光率，其可以兼容亮度大范圍變化的環(huán)境。圖3為相機(jī)在低曝光率下采集的特征點(diǎn)的圖片，特征點(diǎn)的亮度特征使其與背景實(shí)現(xiàn)了分割。進(jìn)一步地，本文以目標(biāo)的圓形形狀作為第3層篩選條件，從而提高了識(shí)別的可靠性，即先對(duì)上述提取出的區(qū)域進(jìn)行輪廓檢測(cè)，然后以區(qū)域的長(zhǎng)寬比、周長(zhǎng)面積比為指標(biāo)，剔除不滿足條件的區(qū)域。

上文通過(guò)特征點(diǎn)的顏色、亮度及形狀3個(gè)特征對(duì)目標(biāo)進(jìn)行識(shí)別，為了提高提取特征點(diǎn)的速度，本文在識(shí)別環(huán)節(jié)之后加入了跟蹤環(huán)節(jié)。由于無(wú)人機(jī)的運(yùn)動(dòng)是連續(xù)的，而且工業(yè)相機(jī)的幀率較高，因而在相鄰幀之間，無(wú)人機(jī)的運(yùn)動(dòng)幅度不大?；谶@一點(diǎn)，可以由上一幀檢測(cè)出的目標(biāo)位置預(yù)測(cè)下一幀的目標(biāo)位置，從而減小跟蹤的候選區(qū)域，縮短算法的處理時(shí)間。為了提高預(yù)測(cè)的可靠性，預(yù)測(cè)框要大于檢測(cè)框。當(dāng)開(kāi)始處理下一幀時(shí)，先要縮小預(yù)測(cè)框。本文采用的方法是在RGB空間做差。在幀與幀之間，目標(biāo)的RGB三通道像素值都可被視作固定不變，所以在跟蹤環(huán)節(jié)中，只需要將預(yù)測(cè)框中的三通道像素值與上一幀中目標(biāo)的三通道像素值做差，即可對(duì)目標(biāo)進(jìn)行分割，這樣可以實(shí)現(xiàn)預(yù)測(cè)框的最小化，最后在這個(gè)最小化的預(yù)測(cè)框中執(zhí)行上述檢測(cè)算法。跟蹤環(huán)節(jié)即充分利用上一幀的先驗(yàn)信息，將下一幀的檢測(cè)范圍最小化，使得目標(biāo)識(shí)別的速度大大提升。

圖3 相機(jī)在低曝光率下采集的特征點(diǎn)圖像Fig.3 Feature points image at low exposure rate

2.2 特征點(diǎn)坐標(biāo)提取

本文采用的LED標(biāo)記點(diǎn)在圖像中呈現(xiàn)為圓形小區(qū)域，其中心坐標(biāo)即可被視作特征點(diǎn)的二維圖像坐標(biāo)。如何提取目標(biāo)中心點(diǎn)，是視覺(jué)測(cè)量中的一個(gè)重要問(wèn)題。對(duì)此，研究人員已經(jīng)提出了多種方法，方法主要可以分為基于區(qū)域的方法和基于邊緣的方法2種。本文采用的是基于區(qū)域的重心法。通過(guò)求取區(qū)域的矩特征獲得其重心坐標(biāo)，并將重心坐標(biāo)作為圓形區(qū)域的中心。

矩是描述圖像特征的算子，矩技術(shù)被廣泛應(yīng)用于圖像檢索和識(shí)別、圖像匹配等領(lǐng)域。常見(jiàn)的矩描述子有幾何矩、正交矩等。使用幾何矩可以方便地求取區(qū)域的重心

(2)

圖4 特征點(diǎn)提取效果圖Fig.4 Effect drawing of feature points extracted

3 無(wú)人機(jī)位姿參數(shù)求解

目標(biāo)物體的位姿測(cè)量即是求解攝像機(jī)坐標(biāo)系與物體空間坐標(biāo)系的轉(zhuǎn)換關(guān)系，一般可分為2種情況，一種是物體空間坐標(biāo)系的原點(diǎn)和方向固定，當(dāng)物體運(yùn)動(dòng)時(shí)，求解物體在固定的空間坐標(biāo)系下的位姿參數(shù)；另一種是空間坐標(biāo)系隨物體一起運(yùn)動(dòng)，此時(shí)物體位姿解算即為求解攝像機(jī)坐標(biāo)系與物體空間坐標(biāo)系的位姿關(guān)系。針對(duì)無(wú)人機(jī)的位姿測(cè)量，一般采用的是第2種形式，即將物體空間坐標(biāo)系與無(wú)人機(jī)機(jī)體坐標(biāo)系固連，測(cè)量無(wú)人機(jī)相對(duì)于攝像機(jī)的平移和旋轉(zhuǎn)關(guān)系。針對(duì)平移關(guān)系的描述，通常采用3×1的平移向量，而針對(duì)旋轉(zhuǎn)關(guān)系，通常采用3×3的旋轉(zhuǎn)矩陣來(lái)描述。

n點(diǎn)透視問(wèn)題簡(jiǎn)稱為PNP(Perspective-N-Points)問(wèn)題，主要研究在給定相機(jī)內(nèi)參數(shù)和一系列2D-3D對(duì)應(yīng)點(diǎn)時(shí)，如何求解目標(biāo)物體的位置和姿態(tài)信息。當(dāng)特征點(diǎn)的個(gè)數(shù)和布局不同時(shí)，PNP問(wèn)題的求解算法和解算精度都不同。經(jīng)典的PNP算法包括P3P、DLS、NPL、EpnP等，其中EpnP算法的使用最為廣泛，當(dāng)前針對(duì)PNP問(wèn)題的研究也大都建立在EpnP的基礎(chǔ)上。本文使用的位姿參數(shù)求解算法也是EpnP算法。

EpnP算法用非迭代方式求解PNP問(wèn)題，它的計(jì)算復(fù)雜度與特征點(diǎn)的個(gè)數(shù)n線性相關(guān)，所以該算法的速度超過(guò)了大多數(shù)算法，其精度也只比部分迭代算法略低，而遠(yuǎn)高于其他非迭代算法。EpnP的核心思想在于將n個(gè)3D點(diǎn)表示為4個(gè)虛擬控制點(diǎn)的加權(quán)和。如圖5所示，空間中任意3D點(diǎn)的坐標(biāo)均可以用4個(gè)不共面的3D點(diǎn)坐標(biāo)的權(quán)重表示。PNP問(wèn)題可以簡(jiǎn)化為估計(jì)這些控制點(diǎn)在相機(jī)參考系下的坐標(biāo)。這些坐標(biāo)可以表示為1個(gè)12×12的矩陣的特征向量的加權(quán)和，通過(guò)求解少量二次方程，算出權(quán)重系數(shù)，即可求解控制點(diǎn)的坐標(biāo)。

圖5 EpnP算法的原理示意圖Fig.5 Principle of EpnP Algorithm

在求解目標(biāo)位姿參數(shù)時(shí)，EpnP算法需要相機(jī)內(nèi)參數(shù)及3個(gè)以上的2D-3D特征點(diǎn)對(duì)，其中相機(jī)的內(nèi)參數(shù)使用張正友相機(jī)標(biāo)定法求得，特征點(diǎn)的三維世界坐標(biāo)可通過(guò)第1節(jié)的多場(chǎng)景下點(diǎn)的標(biāo)定方法求解，特征點(diǎn)的二維圖像坐標(biāo)可由第2節(jié)的圖像檢測(cè)與跟蹤算法獲得，然后將二維坐標(biāo)與三維坐標(biāo)進(jìn)行匹配，以確定它們之間的對(duì)應(yīng)關(guān)系，最后基于EpnP算法精確而快速地測(cè)出目標(biāo)無(wú)人機(jī)的位置、姿態(tài)信息。

4 實(shí)驗(yàn)結(jié)果與分析

在實(shí)驗(yàn)中，以六旋翼無(wú)人機(jī)的一側(cè)安裝的6個(gè)紅色的LED燈作為標(biāo)記點(diǎn)，其空間布局如圖2(b)所示。采用Point Grey公司的CMLN系列工業(yè)相機(jī)，對(duì)上文提出的算法進(jìn)行實(shí)驗(yàn)測(cè)試，如圖6所示。本文從穩(wěn)定性、速度和精度3個(gè)方面測(cè)試了算法性能，算法的穩(wěn)定性用正確識(shí)別的幀數(shù)占總幀數(shù)的比率來(lái)衡量。在實(shí)驗(yàn)中，正確識(shí)別率達(dá)到了98%以上。針對(duì)單幀圖片的處理時(shí)間，可以作為衡量算法速度的指標(biāo)。本文算法對(duì)單幀圖片的處理時(shí)間可分為2類，檢測(cè)狀態(tài)下的平均處理時(shí)間為5ms,跟蹤狀態(tài)下的平均處理時(shí)間為2ms，說(shuō)明該算法的實(shí)時(shí)性較好。

圖6 安裝LED燈的旋翼機(jī)實(shí)驗(yàn)裝置圖Fig.6 Experimental device with LED lights in UAV

位姿解算算法的另一項(xiàng)重要指標(biāo)是其解算精度。高精度的位姿參數(shù)是無(wú)人機(jī)完成復(fù)雜任務(wù)的基礎(chǔ)，因而在實(shí)驗(yàn)中，需重點(diǎn)對(duì)位姿解算誤差進(jìn)行測(cè)試和分析。本文從位置解算誤差和姿態(tài)解算誤差2個(gè)方面分別進(jìn)行了討論。針對(duì)位置誤差的測(cè)量，本文采用的實(shí)驗(yàn)裝置是高精度的三軸移動(dòng)平臺(tái)。在實(shí)驗(yàn)中，將相機(jī)固定在平臺(tái)的移動(dòng)支架上，而將無(wú)人機(jī)保持靜止，通過(guò)電機(jī)精確地控制相機(jī)在橫向、縱向、前向3個(gè)方向上的移動(dòng)，對(duì)比相機(jī)移動(dòng)距離與視覺(jué)算法解算的距離變化值，即可得到算法的解算誤差。對(duì)于姿態(tài)誤差的測(cè)量，本文采用的是三軸轉(zhuǎn)臺(tái)。在實(shí)驗(yàn)中，將旋翼機(jī)安裝在轉(zhuǎn)臺(tái)上，保持相機(jī)靜止，控制轉(zhuǎn)臺(tái)，改變機(jī)體的航向、俯仰、橫滾三個(gè)角度，并將其與解算的變化值進(jìn)行對(duì)比，以測(cè)量姿態(tài)解算的誤差。

如圖7所示，本文算法在空間坐標(biāo)系的三個(gè)軸向上的位置解算誤差均小于2%，且在深度方向上的誤差相比橫向和縱向的誤差均略大；本文算法在機(jī)體坐標(biāo)系上的姿態(tài)解算誤差約為8%。大多數(shù)應(yīng)用場(chǎng)合，對(duì)于位置信息的準(zhǔn)確度需求較高，而對(duì)姿態(tài)的精度需求略低，因而本文算法可以滿足多數(shù)場(chǎng)合對(duì)無(wú)人機(jī)定位的需求。

圖7 位姿解算誤差圖Fig.7 Error of position solution

5 結(jié) 論

本文提出了一種基于特征點(diǎn)的無(wú)人機(jī)位姿單目視覺(jué)測(cè)量方法，采用多場(chǎng)景下點(diǎn)的三維位置標(biāo)定方法解決了特征點(diǎn)的三維位置求解問(wèn)題。通過(guò)應(yīng)用該方法，特征點(diǎn)標(biāo)記不需要精確地安裝至特定位置，而可以在安裝之后進(jìn)行標(biāo)定求解，大大地降低了特征點(diǎn)安裝的難度；利用特征標(biāo)記的亮度、顏色和形狀特征提取特征點(diǎn)成像區(qū)域，用跟蹤算法提高特征點(diǎn)提取速度，通過(guò)重心法獲得特征點(diǎn)的二維亞像素坐標(biāo)，最后用EpnP算法求解目標(biāo)的位置和姿態(tài)參數(shù)。實(shí)驗(yàn)結(jié)果表明，該方法的穩(wěn)定性和運(yùn)行速度較高，位置解算誤差小于2%，姿態(tài)解算誤差約為8%，可以滿足一般場(chǎng)景對(duì)無(wú)人機(jī)定位精度的需求。