室內(nèi)移動定位與測圖系統(tǒng)的高精度標(biāo)定

李帥鑫，李廣云，符京楊，李明磊，王 力

(1.信息工程大學(xué) 地理空間信息學(xué)院，鄭州 450000；2.信息工程大學(xué) 數(shù)據(jù)與目標(biāo)工程學(xué)院，鄭州 450000)

0 引言

室外環(huán)境下，基于慣性導(dǎo)航/衛(wèi)星導(dǎo)航組合的移動測量系統(tǒng)發(fā)展相對完善；而在室內(nèi)環(huán)境下，受障礙物、墻體的遮擋，衛(wèi)星導(dǎo)航系統(tǒng)等基于無線信號的導(dǎo)航定位手段無法正常工作[1]?；谝曈X的導(dǎo)航定位方法具有不依賴信號源、環(huán)境魯棒性強(qiáng)、地圖表達(dá)直觀等特點(diǎn)，因此更適合用于室內(nèi)定位與移動測圖。深度相機(jī)(red-green-blue-depth，RGBD)由1個彩色鏡頭、1個深度鏡頭以及1個測距儀組成，是一種能同時獲得環(huán)境影像與深度數(shù)據(jù)的傳感器。以Kinect為代表的RGBD相機(jī)具有價格低廉、數(shù)據(jù)獲取能力強(qiáng)等特點(diǎn)，近年來已成為計算機(jī)視覺、移動機(jī)器人、組合導(dǎo)航和室內(nèi)移動測量等領(lǐng)域的研究熱點(diǎn)[2]；但RGBD相機(jī)本身也存在深度測量精度不高、測距范圍有限、可視角較小等缺陷，文獻(xiàn)[3]對Kinect距離量測誤差進(jìn)行測試，指出其誤差最高可達(dá)1.5 cm：因此，研究克服RGBD相機(jī)測量缺陷的傳感器組合方案及其標(biāo)定方法具有重要意義。

基于飛行時間(time of flight，TOF)的RGBD相機(jī)誤差模型主要分為4類：第1類是圖像的暗噪聲和相機(jī)散射噪聲，可用濾波方法消除[4]；第2類是外部噪聲，包括目標(biāo)表面紋理的影響、多路徑效應(yīng)、溫度和濕度的影響等；第3類是設(shè)備自身的噪聲，如預(yù)熱時間、積分時間等[5]；第4類是設(shè)備系統(tǒng)誤差，包括設(shè)備各部件間的相對位姿和相機(jī)的主點(diǎn)偏移、焦距、畸變系數(shù)等內(nèi)參數(shù)。對組合系統(tǒng)的標(biāo)定主要是對第4類誤差模型建模。

對RGBD相機(jī)的標(biāo)定，文獻(xiàn)[6]提出兩步法(two-step independent，TSI)，即先利用干涉儀等高精度測距儀器對相機(jī)測距儀進(jìn)行標(biāo)定，再將其結(jié)果作為已知量解算內(nèi)參。文獻(xiàn)[7]提出一步法(one-step independent，OSI)，該方法利用光束法平差，將多視角影像、垂直影像及垂直距離量測值一并代入約束方程中聯(lián)合求解。文獻(xiàn)[8-9]在TSI和OSI的基礎(chǔ)上進(jìn)行改進(jìn)，提出改進(jìn)的兩步法(two-step dependent，TSD)，采用多角度和垂直方向的連續(xù)影像標(biāo)定相機(jī)內(nèi)參，并反算垂直方向的距離，得到測距儀的標(biāo)定參數(shù)。文獻(xiàn)[10]對這3種方法進(jìn)行了分析，指出OSI和TSD標(biāo)定精度差異不大，均優(yōu)于TSI；TSI存在焦距估計量偏小、深度估計量偏大的問題。文獻(xiàn)[11]提出采用紅外光源照射得到紅外影像，直接對深度相機(jī)進(jìn)行標(biāo)定。文獻(xiàn)[12]提出采用空間直線定長約束標(biāo)定深度相機(jī)。文獻(xiàn)[13]根據(jù)紅外影像與深度影像的配準(zhǔn)關(guān)系分別對彩色鏡頭和紅外鏡頭進(jìn)行標(biāo)定，并在標(biāo)準(zhǔn)長度基線場對深度量測值做進(jìn)一步標(biāo)定。

對傳感器安置參數(shù)的標(biāo)定主要指各傳感器相對位置和姿態(tài)參數(shù)的解算。對組合系統(tǒng)的標(biāo)定可在單相機(jī)內(nèi)參數(shù)已知的基礎(chǔ)上通過光束法平差完成[14]。即將各傳感器同一時刻獲取到的相同標(biāo)志在空間中位置不變作為約束，求解傳感器間相對位姿。文獻(xiàn)[15]詳細(xì)綜述了多傳感器組合系統(tǒng)的標(biāo)定方法，將其總結(jié)為兩步法和一步法。兩步法即先對傳感器單獨(dú)標(biāo)定，再帶入整體優(yōu)化方程中求解相對位姿；而一步法則將傳感器安置參數(shù)和內(nèi)參數(shù)一并加入優(yōu)化方程中，采用光束法平差聯(lián)合求解。文獻(xiàn)[16]提出利用物方空間坐標(biāo)已知的標(biāo)志點(diǎn)后方交會來進(jìn)行多相機(jī)相對位姿求解的方法。文獻(xiàn)[17]采用直接線性變換(direct linear transformation，DLT)解算激光掃描儀(terrestrial laser scanner，TLS)和相機(jī)的相對位姿。

本文針對RGBD相機(jī)/TLS組合的室內(nèi)移動測量系統(tǒng)傳感器參數(shù)的標(biāo)定問題展開研究，提出一種較為快速、高精度的系統(tǒng)性標(biāo)定方法。該方法分為RGBD相機(jī)標(biāo)定和傳感器安置參數(shù)標(biāo)定2部分；對RGBD相機(jī)的標(biāo)定采用附加空間定長約束的兩步法；對傳感器安置參數(shù)的標(biāo)定，提出基于重投影誤差最小約束的改進(jìn)直接線性變換法，以保證掃描儀在角點(diǎn)提取精度不高時標(biāo)定結(jié)果的魯棒性。通過分別求解各相機(jī)與掃描儀的安置參數(shù)，完成各傳感器間相對位置關(guān)系的標(biāo)定，最終實現(xiàn)組合系統(tǒng)獲取的影像和點(diǎn)云數(shù)據(jù)的整體映射。

1 標(biāo)定方法

1.1 方法概述

針孔相機(jī)模型如圖1所示。

圖1 針孔相機(jī)模型

圖1中：u、v為像素坐標(biāo)；p、q為圖像上2個點(diǎn)；O-ijk為相機(jī)坐標(biāo)；f為相機(jī)焦距；P、Q為相機(jī)坐標(biāo)系下2個物方點(diǎn)。組合系統(tǒng)內(nèi)各坐標(biāo)系及其變換關(guān)系如下：

(1)

(2)

(3)

式中f為鏡頭焦距。

(4)

4)組合系統(tǒng)中傳感器C1和C2間的剛體變換為

(5)

(6)

(7)

圖2 坐標(biāo)系位置關(guān)系

圖2中：Ow-XwYwZw為物方坐標(biāo)系；ORGB--XRGBYRGBZRGB為相機(jī)彩色鏡頭坐標(biāo)系；OIR-XIRYIRZIR為紅外鏡頭坐標(biāo)系；P為標(biāo)志點(diǎn)。組合系統(tǒng)各傳感器坐標(biāo)系位置關(guān)系如圖3所示。

圖3 組合系統(tǒng)傳感器位置關(guān)系

實現(xiàn)多深度相機(jī)組合定位與測圖系統(tǒng)的聯(lián)合標(biāo)定本質(zhì)上即統(tǒng)一系統(tǒng)中各傳感器坐標(biāo)系。標(biāo)定分為RGBD相機(jī)自標(biāo)定和傳感器安置參數(shù)標(biāo)定2個部分(如圖4所示)。

圖4 標(biāo)定流程

具體步驟如下：

1)RGBD相機(jī)標(biāo)定。對Kinect 2.0 RGBD相機(jī)采用張正友標(biāo)定法[17]對彩色和深度相機(jī)分別進(jìn)行標(biāo)定，得到各相機(jī)的內(nèi)方位元素和畸變系數(shù)初值。之后根據(jù)定長直線約束條件對深度相機(jī)內(nèi)參進(jìn)行優(yōu)化。最后采用光束法平差求解2個鏡頭間的外參并對鏡頭內(nèi)參進(jìn)行整體優(yōu)化。

2)傳感器安置參數(shù)標(biāo)定。從激光掃描儀得到的點(diǎn)云中提取棋盤格角點(diǎn)，建立掃描儀坐標(biāo)系與Kinect坐標(biāo)系的相對位置關(guān)系，并采用直接線性變換求解，得到傳感器安置參數(shù)初值。之后根據(jù)重投影誤差最小的約束條件進(jìn)行整體優(yōu)化。

1.2 RGBD相機(jī)標(biāo)定方法

文獻(xiàn)[18]利用棋盤格標(biāo)定板與相片的單應(yīng)性變換關(guān)系對相機(jī)進(jìn)行標(biāo)定，較傳統(tǒng)標(biāo)定方法具有使用靈活、操作簡便等特點(diǎn)，且標(biāo)定精度優(yōu)于自標(biāo)定方法；因此本文將張正友標(biāo)定法的結(jié)果作為初值，采用空間直線定長約束進(jìn)一步優(yōu)化。

1.2.1 基于空間定長直線約束的深度鏡頭標(biāo)定方法

由式(1)～式(7)可得

(8)

(9)

式中：i為示棋盤格特征角點(diǎn)編號；j為拍攝的圖像編號；k為相機(jī)編號；sijk為尺度因子，其值為相機(jī)k拍攝第j幅圖像時特征點(diǎn)i所對應(yīng)深度值的倒數(shù)。實際計算中由于切向畸變遠(yuǎn)小于徑向畸變，因此可直接忽略切向畸變的影響，即

(10)

由式(10)得像素坐標(biāo)與像方坐標(biāo)的變換關(guān)系為

(11)

(12)

根據(jù)該約束條件，RGBD相機(jī)標(biāo)定時目標(biāo)函數(shù)I可寫為

(13)

將式(13)寫為誤差方程形式

v=CΔ-L

(14)

根據(jù)最小二乘原則，可求解非線性誤差方程

Δ=(CTPC)-1CTPL

(15)

式中：Δ為相機(jī)內(nèi)參改正數(shù)；C為非線性方程在初值位置的雅克比矩陣；L為I的估值；P為權(quán)陣。由于量測精度與距離有關(guān)，則可根據(jù)直線2個端點(diǎn)深度值平均數(shù)的倒數(shù)定權(quán)，具體形式可表示為：

1.2.2 彩色鏡頭內(nèi)參估計與相對位置估計

將深度鏡頭內(nèi)方位元素作為已知量，反算棋盤格角點(diǎn)在相機(jī)坐標(biāo)系下的坐標(biāo)，并代入彩色圖像共線條件方程中，求解彩色內(nèi)參和鏡頭間相對位置[19]，最后根據(jù)重投影誤差最小的約束條件采用光束法平差對解算結(jié)果進(jìn)行整體優(yōu)化[20-21]。

根據(jù)文獻(xiàn)[15]所述，相機(jī)相對位置估計的準(zhǔn)確性受控制點(diǎn)空間布局的影響；因此為保證精度，在采集圖像時應(yīng)盡量保證棋盤格位于圖像中部2/3的區(qū)域，避免整個標(biāo)定板都位于圖像的邊角位置。

1.3 傳感器安置參數(shù)標(biāo)定方法

由于RGBD相機(jī)測量視角較小，且對于距離較遠(yuǎn)的區(qū)域量測精度較低，為保證測圖的效率和精度，采用由2臺Kinect2.0相機(jī)和1臺三維TLS的組合定位與測圖系統(tǒng)方案。相機(jī)分別拍攝2個方向以保證足夠大的探測范圍，TLS用于較遠(yuǎn)目標(biāo)深度的探測。TLS擁有360°視角，因此可將各傳感器坐標(biāo)統(tǒng)一到TLS坐標(biāo)系下。

直接線性變換是根據(jù)共線條件方程的線性變換解算外方位元素的一種方法[17]，其特點(diǎn)是不需要變換初值，更適合計算大旋角外方位元素。需要指出的是該方法要求至少6對以上不共面的控制點(diǎn)，因此標(biāo)定標(biāo)志的設(shè)計需要特別注意；另外，直接線性變換依賴于空間點(diǎn)的精度，若個別物方控制點(diǎn)存在偏差，該方法的標(biāo)定精度將大大下降：因此本文提出一種改進(jìn)的直接線性變換方法，將直接線性變換結(jié)果作為初值代入光束法平差模型中做整體優(yōu)化。

1.3.1 直接線性變換法

1.3.2 基于重投影誤差最小的優(yōu)化方法

(16)

(17)

(18)

(19)

-b=HΔx*

(20)

將X=X+Δx*作為初值迭代計算，直到結(jié)果收斂。需要說明的是：求位姿矩陣的雅克比矩陣實際上就是在6維變換空間求導(dǎo)；但由于旋轉(zhuǎn)矩陣對加法不封閉，不能構(gòu)成線性空間，因此求導(dǎo)時需要利用李群SO(3)和對應(yīng)的李代數(shù)SE(3)[23]。

2 實驗與結(jié)果分析

2.1 實驗方案

實驗對一個由2臺Kinect 2.0 RGBD相機(jī)和1臺Faro Focus3D 120激光掃描儀組成的組合定位與測圖系統(tǒng)進(jìn)行標(biāo)定(如圖5(a)所示)；標(biāo)定板采用尺寸為6個×8個網(wǎng)格的棋盤格板，網(wǎng)格邊長90 mm。Kinect的原始彩色、深度、紅外影像數(shù)據(jù)通過Kinect和OpenCV庫獲取得到(如圖5(b)和圖5(c)所示)，標(biāo)定系統(tǒng)采用C++編程實現(xiàn)。

數(shù)據(jù)采集時，每幀數(shù)據(jù)包含彩色、深度和紅外影像，分別提取彩色、紅外影像棋盤格角點(diǎn)。選擇對角線上的4個影像點(diǎn)交叉互連，在一幅圖像上構(gòu)成2個定長約束。第j次拍攝的點(diǎn)n在彩色、紅外影像上的像素坐標(biāo)分別為(unjc，vnjc)和(unji，vnji)，對應(yīng)深度值(dnj，dnj)。相機(jī)內(nèi)參數(shù)和傳感器安置參數(shù)的精度通過反算得到的棋盤格角點(diǎn)像點(diǎn)坐標(biāo)與量測的像點(diǎn)坐標(biāo)的偏差來評定。

圖5 實驗設(shè)備和角點(diǎn)提取

2.2 RGBD相機(jī)標(biāo)定

實驗分別對2個Kinect 2.0 RGBD相機(jī)進(jìn)行標(biāo)定，共采集到50組有效影像數(shù)據(jù)，每組數(shù)據(jù)中包含紅外影像、深度影像和彩色影像，且保證所選定長線段的端點(diǎn)處有深度值。RGBD相機(jī)標(biāo)定結(jié)果及精度如表1～4所示，其中旋轉(zhuǎn)外參數(shù)用羅德里格斯旋轉(zhuǎn)向量的3個分量r1、r2、r3表示，平移外參數(shù)由tx、ty、tz表示。

表1 左側(cè)Kinect內(nèi)參數(shù)及精度

表2 左側(cè)Kinect 2個鏡頭外參數(shù) mm

表3 右側(cè)Kinect內(nèi)參數(shù)及精度

表4 右側(cè)Kinect 2個鏡頭外參數(shù) mm

由表可以看出，標(biāo)定精度在影像坐標(biāo)系的2個方向均可達(dá)到0.2個像素左右，符合精度要求，在安置參數(shù)標(biāo)定時可作為已知量帶入解算。

2.3 組合系統(tǒng)安置參數(shù)標(biāo)定

實驗采用Faro Focus3D 120 TLS和Kinect組合平臺采集數(shù)據(jù)。為保證直接線性變換解算初值的精度，采用3個×4個網(wǎng)格的4個棋盤格標(biāo)定板，并使各標(biāo)定板互不平行。棋盤板的布設(shè)盡量使角點(diǎn)均勻分布于影像中。采用多棋盤格板生長算法[24]提取彩色影像中的棋盤格角點(diǎn)，利用點(diǎn)云角點(diǎn)提取方法獲得點(diǎn)云中的棋盤格角點(diǎn)，角點(diǎn)提取結(jié)果如圖6所示。為了消除傳感器偶然誤差，實驗共進(jìn)行10次，取平均結(jié)果為最終標(biāo)定結(jié)果。

圖6 多標(biāo)定板角點(diǎn)提取

實驗中，安置參數(shù)的精度由內(nèi)符合精度評價，即計算觀測像點(diǎn)與估計像點(diǎn)間的坐標(biāo)差。傳感器安置參數(shù)解算結(jié)果均值如表5所示，平均精度如圖7所示。其中：φ、θ、ψ分別為橫滾角、俯仰角、偏航角；tx、ty、tz為平移分量。

表5 組合系統(tǒng)傳感器安置參數(shù)均值

圖7 特征點(diǎn)平均精度

圖7中深色線表示DLT結(jié)果的精度，淺色線表示以DLT結(jié)果作為初值進(jìn)行優(yōu)化后的精度。由圖中可以看出：DLT解算結(jié)果在圖像2側(cè)點(diǎn)位精度較差，最大可達(dá)5個像素，整體起伏較大；優(yōu)化后精度顯著提升，絕大部分點(diǎn)位精度可達(dá)1個像素左右，最大不超過2個像素，整體起伏也較為平穩(wěn)。起伏較大說明存在偏差較大的點(diǎn)，如點(diǎn)云對應(yīng)右側(cè)影像的2、43、47，這些點(diǎn)受到棋盤格角點(diǎn)提取偏差的影響(如圖8所示)而導(dǎo)致重投影誤差很大。優(yōu)化后這些點(diǎn)位精度提升，可見該算法對角點(diǎn)提取精度具有一定魯棒性。Kinect相機(jī)單個像素的物理尺寸約為0.008 mm，因此安置參數(shù)標(biāo)定結(jié)果的精度完全滿足后期影像間的拼接與建圖要求。

圖8 點(diǎn)云角點(diǎn)提取誤差

3 結(jié)束語

本文提出了一套針對多RGBD相機(jī)組合定位與測圖系統(tǒng)安置參數(shù)的較為快速、高精度的標(biāo)定方案。整體處理流程包括RGBD相機(jī)內(nèi)參數(shù)及桿臂值標(biāo)定和組合系統(tǒng)傳感器安置參數(shù)標(biāo)定2部分。其中RGBD相機(jī)的標(biāo)定采用基于定長直線約束的標(biāo)定方法；針對安置參數(shù)的標(biāo)定提出優(yōu)化的直接線性變換方法。實驗結(jié)果表明該標(biāo)定方案精度可達(dá)1個像素左右，符合測圖時影像拼接的需求。

本標(biāo)定方案的意義在于：1)提出對掃描儀特征點(diǎn)提取精度不高時較為魯棒的安置參數(shù)標(biāo)定方法；2)針對本文的移動測量平臺提出了一套完整的高精度標(biāo)定方案，可快速完成傳感器自身及其安置參數(shù)的高精度標(biāo)定；3)將傳統(tǒng)直接線性變換法的結(jié)果作為整體優(yōu)化的初值，采用光束法平差進(jìn)一步提升標(biāo)定結(jié)果的精度，對于其他組合設(shè)備安置參數(shù)的標(biāo)定同樣具有參考意義。