室內移動定位與測圖系統的高精度標定

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

(1.信息工程大學 地理空間信息學院，鄭州 450000；2.信息工程大學 數據與目標工程學院，鄭州 450000)

0 引言

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

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

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

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

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

1 標定方法

1.1 方法概述

針孔相機模型如圖1所示。

圖1 針孔相機模型

圖1中：u、v為像素坐標；p、q為圖像上2個點；O-ijk為相機坐標；f為相機焦距；P、Q為相機坐標系下2個物方點。組合系統內各坐標系及其變換關系如下：

(1)

(2)

(3)

式中f為鏡頭焦距。

(4)

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

(5)

(6)

(7)

圖2 坐標系位置關系

圖2中：Ow-XwYwZw為物方坐標系；ORGB--XRGBYRGBZRGB為相機彩色鏡頭坐標系；OIR-XIRYIRZIR為紅外鏡頭坐標系；P為標志點。組合系統各傳感器坐標系位置關系如圖3所示。

圖3 組合系統傳感器位置關系

實現多深度相機組合定位與測圖系統的聯合標定本質上即統一系統中各傳感器坐標系。標定分為RGBD相機自標定和傳感器安置參數標定2個部分(如圖4所示)。

圖4 標定流程

具體步驟如下：

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

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

1.2 RGBD相機標定方法

文獻[18]利用棋盤格標定板與相片的單應性變換關系對相機進行標定，較傳統標定方法具有使用靈活、操作簡便等特點，且標定精度優于自標定方法；因此本文將張正友標定法的結果作為初值，采用空間直線定長約束進一步優化。

1.2.1 基于空間定長直線約束的深度鏡頭標定方法

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

(8)

(9)

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

(10)

由式(10)得像素坐標與像方坐標的變換關系為

(11)

(12)

根據該約束條件，RGBD相機標定時目標函數I可寫為

(13)

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

v=CΔ-L

(14)

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

Δ=(CTPC)-1CTPL

(15)

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

1.2.2 彩色鏡頭內參估計與相對位置估計

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

根據文獻[15]所述，相機相對位置估計的準確性受控制點空間布局的影響；因此為保證精度，在采集圖像時應盡量保證棋盤格位于圖像中部2/3的區域，避免整個標定板都位于圖像的邊角位置。

1.3 傳感器安置參數標定方法

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

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

1.3.1 直接線性變換法

1.3.2 基于重投影誤差最小的優化方法

(16)

(17)

(18)

(19)

-b=HΔx*

(20)

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

2 實驗與結果分析

2.1 實驗方案

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

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

圖5 實驗設備和角點提取

2.2 RGBD相機標定

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

表1 左側Kinect內參數及精度

表2 左側Kinect 2個鏡頭外參數 mm

表3 右側Kinect內參數及精度

表4 右側Kinect 2個鏡頭外參數 mm

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

2.3 組合系統安置參數標定

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

圖6 多標定板角點提取

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

表5 組合系統傳感器安置參數均值

圖7 特征點平均精度

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

圖8 點云角點提取誤差

3 結束語

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

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