基于單目攝像頭的地面測距方法

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

摘? 要：為解決傳統的小孔成像測距算法需要給出待測物體的形狀信息而較為煩瑣的問題，提出一種基于大多數機器人攝像頭的高度不需要改變的情形，利用針孔相機模型，找出像素坐標系和世界坐標系的轉換關系，輸入至少三對現實世界和像素世界的點對后，便可以實現對地面和立于地面上的物體進行測距的目的。該算法實現簡單，且一定范圍內測距精度較高。

關鍵詞：單目相機;地面測距;移動機器人;針孔相機模型

中圖分類號：TP391.4;TP242? ? 文獻標識碼：A? ? ? 文章編號：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? 引? 言

測距對于移動機器人來說有著重要的意義，許多機器人重要技術，例如同步定位與建圖技術[1]，目標跟蹤技術以及自動駕駛技術等都需要進行測距。目前的主流測距方法有激光雷達測距、毫米波雷達測距、視覺測距等。激光雷達雖然測距精度高，但是存在使用壽命以及價格昂貴的問題;毫米波雷達則存在角度分辨率不高，以及精度受環境影響較大的問題;而機器人視覺測距[2]因有靈活性大、成本效率高、適用范圍廣等優勢而越來越受到關注。

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

本文基于針孔相機模型，從世界坐標系到相機坐標系轉化過程中，假設攝像頭光軸平行于地面，此時世界坐標系到像素坐標系的最終轉換將簡化很多，即可實現定位和測距。

1? 算法設計

1.1? 相機模型

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

1.2? 攝像頭成像平面嚴格垂直地面的情況

小孔成像原理如圖1所示，H為攝像頭焦點，F為地面上的待測點，假設攝像頭光軸平行于地面，可以看出當攝像頭的高度不變時，地面上所有的點到焦點的垂直距離Y不變。

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

（1）

（2）

其中f為相機焦距，Δy為成像平面的對應點到中心點的對應距離。再由成像平面坐標系到像素坐標系（U，V），即經過縮放以及平移，有：

（3）

（4）

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

從上述兩式可得如果相機高度不變，則對于平坦地面而言Y不變，便可以得到V和Z，以及U和X的一一對應關系。

（5）

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

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

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

（7）

聯立式（1）和式（7）可解得可得：

（8）

相機的高度Y和攝像頭轉動角度θ通常容易測得，但成像平面到像素平面的轉換比例dv和du較難測量，在實際使用中可以對上述公式進行簡化為反比例函數和線性函數。

（9）

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

只需要三組已知的Z和V的對應點，以及1組已知的X和U的對應點與攝像頭像素寬度，即可解出式（9）和式（10）對應的函數參數a、b、c、k、U0。此時便確定了地面坐標（X，Y，Z）。大多數情況下為提高精度也可以提供多個點進行函數的擬合。

2? 實驗設計及分析

2.1? 實驗環境

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

2.2? 實驗設計

2.2.1? Z軸測量結果

此次實驗測試當X=0時，Z以步長為10 cm，由30增至200 cm時算法對各點預測的Z值。對Z軸的測量結果如表1所示。

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

2.2.2? X軸測量結果

此次實驗測試當Z=0時，世界坐標系下X軸由-40 cm，以10 cm為步長，增至40 cm處算法得到的預測X軸距離，對X軸的測量結果如表2所示。

如圖4所示，對于X軸，由于使用的相機較為廉價會導致較大程度的畫面畸變，雖然可以通過軟件進行校正，但準確來說這些情況都屬于無法完全消除的偏差，只能盡量減少這種誤差，故在圖片兩邊的Z值偏小導致誤差較大。但可以看出未進行校正的情況下圖片的最大相對誤差也不超過10%，在對精度要求不是非常嚴格的情況下也可以使用。

3? 算法用處及改進

3.1? 算法應用場景

此算法可以用于二維建圖，由于這個算法的本質是對處于同一平面上的各點的坐標確定，故如果可以通過語義識別從圖像中找出地面的輪廓即可對地面上各個點進行定位，而不需要額外的激光雷達、毫米波雷達等測距工具[5]。

對于立于地面上的物體，只需要找出物體的下邊緣輪廓，即找出物體在地面上的點的坐標便可以實現對物體的測距，而不需要知道現實世界下物體輪廓的具體大小。

3.2? 算法改進

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

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

4? 結? 論

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

參考文獻：

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

[2] 李宏偉，陳冰，張賀磊.計算機視覺測距技術研究 [J].電視技術，2020，44（8）：60-61+71.

[3] 李占旗，高繼東，彭凱，等.基于PnP算法的前方車輛測距方法 [J].國外電子測量技術，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前端視覺里程計的優化算法 [J].哈爾濱理工大學學報，2020，25（6）：157-164.

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

作者簡介：黃成龍（2000—），男，漢族，江西贛州人，本科在讀，研究方向：嵌入式移動機器人。