黃成龍 周鈺峰 孫慧妮 孟愷 李恪堯

摘? 要：為解決傳統(tǒng)的小孔成像測(cè)距算法需要給出待測(cè)物體的形狀信息而較為煩瑣的問(wèn)題，提出一種基于大多數(shù)機(jī)器人攝像頭的高度不需要改變的情形，利用針孔相機(jī)模型，找出像素坐標(biāo)系和世界坐標(biāo)系的轉(zhuǎn)換關(guān)系，輸入至少三對(duì)現(xiàn)實(shí)世界和像素世界的點(diǎn)對(duì)后，便可以實(shí)現(xiàn)對(duì)地面和立于地面上的物體進(jìn)行測(cè)距的目的。該算法實(shí)現(xiàn)簡(jiǎn)單，且一定范圍內(nèi)測(cè)距精度較高。

關(guān)鍵詞：?jiǎn)文肯鄼C(jī);地面測(cè)距;移動(dòng)機(jī)器人;針孔相機(jī)模型

中圖分類號(hào)：TP391.4;TP242? ? 文獻(xiàn)標(biāo)識(shí)碼：A? ? ? 文章編號(hào)：2096-4706（2021）05-0118-04

Ground Distance Measurement Based on Monocular Camera

HUANG Chenglong，ZHOU Yufeng，SUN Huini，MENG Kai，LI Keyao

（School of Mechanical Electronic & Information Engineering，China University of Mining and Technology-Beijing，

Beijing? 100083，China）

Abstract：In order to solve the troublesome problem that the traditional pinhole imaging distance measurement algorithm needs to give the shape information of the object to be measured，a situation based on the height of most robot cameras does not need to be changed is proposed. The pinhole camera model is used to find out the transformation relationship between pixel coordinate system and world coordinate system. After inputting at least three pairs of point pairs in the real world and pixel world，it can be completed to the ground and the object on the ground distance measurement. The algorithm is simple to implement and has high accuracy in measuring distance in a certain range.

Keywords：monocular camera;ground distance measurement;mobile robot;pinhole camera model

0? 引? 言

測(cè)距對(duì)于移動(dòng)機(jī)器人來(lái)說(shuō)有著重要的意義，許多機(jī)器人重要技術(shù)，例如同步定位與建圖技術(shù)[1]，目標(biāo)跟蹤技術(shù)以及自動(dòng)駕駛技術(shù)等都需要進(jìn)行測(cè)距。目前的主流測(cè)距方法有激光雷達(dá)測(cè)距、毫米波雷達(dá)測(cè)距、視覺(jué)測(cè)距等。激光雷達(dá)雖然測(cè)距精度高，但是存在使用壽命以及價(jià)格昂貴的問(wèn)題;毫米波雷達(dá)則存在角度分辨率不高，以及精度受環(huán)境影響較大的問(wèn)題;而機(jī)器人視覺(jué)測(cè)距[2]因有靈活性大、成本效率高、適用范圍廣等優(yōu)勢(shì)而越來(lái)越受到關(guān)注。

目前主流的雙目相機(jī)測(cè)距的精度受光線、基線長(zhǎng)度等因素影響較大，且其匹配算法往往較為復(fù)雜。紅外相機(jī)測(cè)距無(wú)法對(duì)黑色物體、鏡面反射物體等進(jìn)行精確測(cè)距，且二者的造價(jià)往往相對(duì)昂貴。而單目視覺(jué)測(cè)距因造價(jià)低，部署方便，測(cè)距算法相對(duì)簡(jiǎn)單的優(yōu)勢(shì)而備受青睞。但是由于單個(gè)攝像頭丟失了深度信息，一些傳統(tǒng)單目測(cè)距算法需要給出待測(cè)物體的寬度或者高度才能進(jìn)行測(cè)距，這類方法不僅非常麻煩，誤差也較大。較為先進(jìn)的PNP測(cè)距算法[3]要想得到較高的準(zhǔn)確率往往需要較多的點(diǎn)對(duì)，且理論復(fù)雜。

本文基于針孔相機(jī)模型，從世界坐標(biāo)系到相機(jī)坐標(biāo)系轉(zhuǎn)化過(guò)程中，假設(shè)攝像頭光軸平行于地面，此時(shí)世界坐標(biāo)系到像素坐標(biāo)系的最終轉(zhuǎn)換將簡(jiǎn)化很多，即可實(shí)現(xiàn)定位和測(cè)距。

1? 算法設(shè)計(jì)

1.1? 相機(jī)模型

在相機(jī)模型中，針孔相機(jī)[4]是相對(duì)簡(jiǎn)單而常用的模型。簡(jiǎn)單地說(shuō)就是把相機(jī)簡(jiǎn)化成小孔成像，可以想象，這種簡(jiǎn)化對(duì)于精度要求高的情況或者廣角相機(jī)是不適用的。針孔相機(jī)模型用到了透視變換的投影方法，即被投影物體處于一個(gè)四棱臺(tái)區(qū)域中，物體被投影到離相機(jī)較近的平面上，相機(jī)被抽象為一個(gè)點(diǎn)，而投影點(diǎn)是物體上的點(diǎn)和相機(jī)的連線與投影平面的交點(diǎn)。由于投影的路徑不再相互平行，因此會(huì)產(chǎn)生透視效果[4]。針孔相機(jī)模型存在四個(gè)坐標(biāo)系：世界坐標(biāo)系、相機(jī)坐標(biāo)系、圖像物理坐標(biāo)系和圖像像素坐標(biāo)系。此次算法假設(shè)世界坐標(biāo)系與攝像機(jī)坐標(biāo)系原點(diǎn)重合，且任一點(diǎn)用（X，Y，Z）表示，圖像像素坐標(biāo)系用（U，V）表示。

1.2? 攝像頭成像平面嚴(yán)格垂直地面的情況

小孔成像原理如圖1所示，H為攝像頭焦點(diǎn)，F(xiàn)為地面上的待測(cè)點(diǎn)，假設(shè)攝像頭光軸平行于地面，可以看出當(dāng)攝像頭的高度不變時(shí)，地面上所有的點(diǎn)到焦點(diǎn)的垂直距離Y不變。

在相機(jī)坐標(biāo)系下，建立針孔相機(jī)模型，以攝像頭中心為原點(diǎn)，以垂直地面豎直向上為Y軸建立右手直角坐標(biāo)系，對(duì)于地面上任一點(diǎn)（X，Y，Z），由三角形的相似性可得：

（1）

（2）

其中f為相機(jī)焦距，Δy為成像平面的對(duì)應(yīng)點(diǎn)到中心點(diǎn)的對(duì)應(yīng)距離。再由成像平面坐標(biāo)系到像素坐標(biāo)系（U，V），即經(jīng)過(guò)縮放以及平移，有：

（3）

（4）

其中，V0，U0為圖像的高度和寬度的一半。

從上述兩式可得如果相機(jī)高度不變，則對(duì)于平坦地面而言Y不變，便可以得到V和Z，以及U和X的一一對(duì)應(yīng)關(guān)系。

（5）

X=Zf（U-U0）·du? ? ? ? ? ? ? ? ? ? ? ? ? （6）

1.3? 攝像頭成像平面下邊沿平行于地面的情況

但是，通常很難保證攝像頭成像平面嚴(yán)格垂直地面，即V軸垂直地面，U軸平行于地面，大多數(shù)情況下只能做到其中一個(gè)條件，即保持相機(jī)底部V軸平行于地面，此時(shí)則需要對(duì)模型進(jìn)行修正。假設(shè)相機(jī)平面在相機(jī)坐標(biāo)系下繞X軸旋轉(zhuǎn)一定的角度θ，依然根據(jù)三角形相似原理，此時(shí)式（1）可調(diào)整為：

（7）

聯(lián)立式（1）和式（7）可解得可得：

（8）

相機(jī)的高度Y和攝像頭轉(zhuǎn)動(dòng)角度θ通常容易測(cè)得，但成像平面到像素平面的轉(zhuǎn)換比例dv和du較難測(cè)量，在實(shí)際使用中可以對(duì)上述公式進(jìn)行簡(jiǎn)化為反比例函數(shù)和線性函數(shù)。

（9）

X=k（U-U0）? ? ? ? ? ? ? ? ? ? ? ? ? ? （10）

只需要三組已知的Z和V的對(duì)應(yīng)點(diǎn)，以及1組已知的X和U的對(duì)應(yīng)點(diǎn)與攝像頭像素寬度，即可解出式（9）和式（10）對(duì)應(yīng)的函數(shù)參數(shù)a、b、c、k、U0。此時(shí)便確定了地面坐標(biāo)（X，Y，Z）。大多數(shù)情況下為提高精度也可以提供多個(gè)點(diǎn)進(jìn)行函數(shù)的擬合。

2? 實(shí)驗(yàn)設(shè)計(jì)及分析

2.1? 實(shí)驗(yàn)環(huán)境

為了測(cè)試在最差的情況下算法的精度，本實(shí)驗(yàn)使用三個(gè)點(diǎn)對(duì)擬合式（9）和一個(gè)點(diǎn)對(duì)擬合式（10）。實(shí)驗(yàn)設(shè)置攝像頭中心離地高度Y約為16 cm，圖片的寬高為640×480，給出了位于攝像頭正前方即Z軸30 cm，40 cm，50 cm距離處的三個(gè)（V，Z）點(diǎn)對(duì)，以及位于X軸50 cm，X軸12 cm處的一個(gè)點(diǎn)對(duì)（U，X）。計(jì)算出對(duì)應(yīng)的參數(shù)為c=-6.37，a=15 679.34，b=-23.18，k=0.001 169 6。測(cè)量方式為固定攝像頭不動(dòng)，移動(dòng)一個(gè)顏色鮮明的物體，手動(dòng)在圖片上找出對(duì)應(yīng)的點(diǎn)，并記錄下真實(shí)軸距離、預(yù)測(cè)軸距離以及對(duì)應(yīng)的像素坐標(biāo)。

2.2? 實(shí)驗(yàn)設(shè)計(jì)

2.2.1? Z軸測(cè)量結(jié)果

此次實(shí)驗(yàn)測(cè)試當(dāng)X=0時(shí)，Z以步長(zhǎng)為10 cm，由30增至200 cm時(shí)算法對(duì)各點(diǎn)預(yù)測(cè)的Z值。對(duì)Z軸的測(cè)量結(jié)果如表1所示。

由圖2可以看到Z軸誤差隨距離的增大而增大，符合Z和V成反比例的關(guān)系模型，且最大誤差為0.03。但是由于隨著距離增大，每減小一個(gè)像素增加的距離也越大，可以從圖3中看出Z值越大，像素坐標(biāo)點(diǎn)也越發(fā)密集，如圖2所示?？傮w而言，算法在Z軸2米內(nèi)的測(cè)距精度都達(dá)到了可以接受的水平（偏差<10%～15%）。

2.2.2? X軸測(cè)量結(jié)果

此次實(shí)驗(yàn)測(cè)試當(dāng)Z=0時(shí)，世界坐標(biāo)系下X軸由-40 cm，以10 cm為步長(zhǎng)，增至40 cm處算法得到的預(yù)測(cè)X軸距離，對(duì)X軸的測(cè)量結(jié)果如表2所示。

如圖4所示，對(duì)于X軸，由于使用的相機(jī)較為廉價(jià)會(huì)導(dǎo)致較大程度的畫(huà)面畸變，雖然可以通過(guò)軟件進(jìn)行校正，但準(zhǔn)確來(lái)說(shuō)這些情況都屬于無(wú)法完全消除的偏差，只能盡量減少這種誤差，故在圖片兩邊的Z值偏小導(dǎo)致誤差較大。但可以看出未進(jìn)行校正的情況下圖片的最大相對(duì)誤差也不超過(guò)10%，在對(duì)精度要求不是非常嚴(yán)格的情況下也可以使用。

3? 算法用處及改進(jìn)

3.1? 算法應(yīng)用場(chǎng)景

此算法可以用于二維建圖，由于這個(gè)算法的本質(zhì)是對(duì)處于同一平面上的各點(diǎn)的坐標(biāo)確定，故如果可以通過(guò)語(yǔ)義識(shí)別從圖像中找出地面的輪廓即可對(duì)地面上各個(gè)點(diǎn)進(jìn)行定位，而不需要額外的激光雷達(dá)、毫米波雷達(dá)等測(cè)距工具[5]。

對(duì)于立于地面上的物體，只需要找出物體的下邊緣輪廓，即找出物體在地面上的點(diǎn)的坐標(biāo)便可以實(shí)現(xiàn)對(duì)物體的測(cè)距，而不需要知道現(xiàn)實(shí)世界下物體輪廓的具體大小。

3.2? 算法改進(jìn)

如果要提高算法的精度，首先最簡(jiǎn)單的方法便是給出更多的世界坐標(biāo)系與像素坐標(biāo)系的點(diǎn)對(duì)來(lái)對(duì)函數(shù)進(jìn)行擬合，擬合度越高測(cè)距精度顯然也就越高。通常情況下，對(duì)于提供的點(diǎn)對(duì)中間的點(diǎn)測(cè)距精度較高，所以最好給出的點(diǎn)對(duì)是從可視范圍內(nèi)地面上最近的點(diǎn)到最遠(yuǎn)距離的較多分段上的點(diǎn)對(duì)坐標(biāo)，而不是集中于某一部分上的幾個(gè)點(diǎn)對(duì)。然后就是對(duì)圖片進(jìn)行去畸變預(yù)處理，以減少同一X軸距離下圖像兩邊與中間的點(diǎn)的距離差，此時(shí)可以提升左右兩邊的測(cè)距精度。也可以提高相機(jī)的視野寬度和分辨率，以降低一個(gè)像素所代表的距離，這樣可以顯著提升對(duì)遠(yuǎn)距離的點(diǎn)的測(cè)距精度，同時(shí)也可以提高目標(biāo)識(shí)別的精度來(lái)更準(zhǔn)確地確定物體位置。另外，如果想要提高測(cè)距的距離，那么需要提高攝像頭的高度，但此時(shí)一個(gè)像素所間隔的現(xiàn)實(shí)距離也會(huì)增加，故同時(shí)最好可以提升攝像頭的分辨率，以及可以盡可能地向下傾斜攝像頭，以使可視范圍內(nèi)的最遠(yuǎn)地面落在圖片上靠上的位置。

此算法雖然不要求攝像頭光軸平行于地面，但是要求攝像頭下邊緣平行于地面，即不可以繞Z軸有旋轉(zhuǎn)角，否則像素坐標(biāo)系下的V值相同的點(diǎn)在世界坐標(biāo)下的X軸距離將不同。如果使用PNP求解法[6]，找出兩個(gè)坐標(biāo)下的旋轉(zhuǎn)和平移矩陣，則攝像頭繞Z軸也可以有一定的旋轉(zhuǎn)。

4? 結(jié)? 論

本文基于針孔相機(jī)模型提出了一種在攝像頭高度不變的情況下，不需要提前知道物體的具體高度和寬度的，并且相對(duì)簡(jiǎn)單的單目測(cè)距方法，且測(cè)距距離較近時(shí)表現(xiàn)較好。此算法可用于二維的同步定位和建圖，以及目標(biāo)跟蹤等技術(shù)。本次實(shí)驗(yàn)實(shí)現(xiàn)了攝像頭高度在一般移動(dòng)機(jī)器人高度（16厘米）時(shí)，可對(duì)Z軸最遠(yuǎn)200厘米處的點(diǎn)進(jìn)行測(cè)距，對(duì)Z軸測(cè)距時(shí)最大相對(duì)誤差不超5%，離Z軸越遠(yuǎn)誤差越大，且在Z軸120厘米處對(duì)X軸測(cè)距時(shí)最大相對(duì)誤差不超10%。若要提高測(cè)距精度，可以提供更多的點(diǎn)對(duì)、提高攝像頭的分辨率以及對(duì)圖片進(jìn)行去畸變處理。若要提高最遠(yuǎn)測(cè)距距離，可以增加攝像頭的高度以及適當(dāng)將攝像頭向下傾斜。算法允許攝像頭沿X、Y軸任意旋轉(zhuǎn)，但不允許繞Z軸旋轉(zhuǎn)，這點(diǎn)可以通過(guò)找出像素坐標(biāo)系到世界坐標(biāo)系的變換矩陣而繼續(xù)改進(jìn)。

參考文獻(xiàn)：

[1] 董伯麟，柴旭.基于IMU/視覺(jué)融合的導(dǎo)航定位算法研究 [J].壓電與聲光，2020，42（5）：724-728.

[2] 李宏偉，陳冰，張賀磊.計(jì)算機(jī)視覺(jué)測(cè)距技術(shù)研究 [J].電視技術(shù)，2020，44（8）：60-61+71.

[3] 李占旗，高繼東，彭凱，等.基于PnP算法的前方車(chē)輛測(cè)距方法 [J].國(guó)外電子測(cè)量技術(shù)，2020，39（12）：104-108.

[4] JUAREZSALAZAR R，ZHENG J，DIAZRAMIREZ V H. Distorted pinhole camera modeling and calibration [J].Applied optics，2020，59（36）：11310-11318.

[5] 董靜薇，蔣燚銘，韓知雨.RGB-DSLAM前端視覺(jué)里程計(jì)的優(yōu)化算法 [J].哈爾濱理工大學(xué)學(xué)報(bào)，2020，25（6）：157-164.

[6] 王平，周雪峰，安愛(ài)民，等.一種魯棒且線性的PnP問(wèn)題求解方法[J].儀器儀表學(xué)報(bào)，2020，41（9）：271-280.

作者簡(jiǎn)介：黃成龍（2000—），男，漢族，江西贛州人，本科在讀，研究方向：嵌入式移動(dòng)機(jī)器人。