移動機器人組合感知系統(tǒng)及其配準方法改進

付夢印，楊毅，朱昊，王立平

(1.北京理工大學 自動化學院，北京100081;2.中國人民解放軍裝甲兵軍事代表局，北京100000)

0 引言

移動機器人在未知的復雜環(huán)境中行駛，通過單一種類的傳感器很難對環(huán)境進行合理的認知，將多種類傳感器得到的圖像與非圖像信息進行融合，成為機器人環(huán)境感知研究的一個重要發(fā)展方向。就移動機器人智能導航而言，可見光攝像機(簡稱攝像機)與激光測距雷達是最常用的圖像與非圖像傳感器。攝像機能夠快速獲取機器人行駛空間中物體的顏色與紋理，3D 激光雷達則更容易獲得機器人行駛空間中目標的距離與輪廓，對二者進行融合，構(gòu)建3D 組合感知系統(tǒng)，將會使移動機器人更全面、更準確地感知環(huán)境，提高其自主導航能力。

3D 激光雷達與攝像機的配準是構(gòu)建組合感知系統(tǒng)的基礎，其方法大致可分為基于平面的配準、基于輪廓線的配準以及基于特征點的配準?？▋?nèi)基.梅隆大學的Ranjith Unnikrishnan 與Martial Hebert提出了一種基于棋盤格平面的攝像機與激光雷達聯(lián)合標定配準方法，同時對攝像機與激光雷達進行標定，取得了不錯的效果［1］。文獻［2］通過激光雷達與攝像機對平面靶標不同位置進行檢測，根據(jù)距離對應性原理實現(xiàn)配準。文獻［3］提出了基于三角形標定板邊緣輪廓提取的聯(lián)合標定配準方法。文獻［4］通過增強物體自然邊緣的方式建立激光雷達與攝像機的位置對應關系，從而進行聯(lián)合標定。文獻［5］與文獻［6］基于立體視覺特征點標定原理，對自制的簡易鏤空棋盤格標定板進行掃描與拍攝，進而將3D 點云(即激光束在掃描過程中落在目標區(qū)的光點的集合)轉(zhuǎn)換為深度圖像，選定深度圖像與可見光圖像中對應的特征點來完成聯(lián)合標定，最后通過三維空間平面重構(gòu)進一步提高了此方法的精度?；谄矫娴呐錅史椒▽硕ò迤秸纫筝^高，需要的配準樣本量大;基于輪廓線的配準方法，其主要思想是通過關聯(lián)線段之間的幾何關系進行配準，但線段的準確提取難度較大;而基于特征點的配準方法，運算量相對較小，配準實驗簡便易行，但特征點的準確選取尤為關鍵。

本文在所構(gòu)建的組合感知系統(tǒng)上，對基于特征點的配準問題作了進一步研究。結(jié)合3D 激光雷達與攝像機工作特性，針對激光雷達混合像素及邊界點問題，設計了基于相關掃描序列濾波器(Associated Scanning Sequence Filter，ASSF)，很大程度上減少了混合像素對配準特征點提取的影響，提高了提取準確度，使3D 激光雷達與攝像機配準結(jié)果更為精確;另外，提出了基于結(jié)構(gòu)化特征的激光雷達坐標原點位置標定方法，減少了激光雷達的量測誤差?；谝陨涎芯拷Y(jié)果，使組合感知系統(tǒng)能夠準確地對移動機器人行駛環(huán)境進行感知。

1 組合感知系統(tǒng)與配準方法

1.1 組合感知系統(tǒng)

根據(jù)移動機器人自主導航的需要，本文設計了一種3D 激光雷達與攝像機組合感知系統(tǒng)，如圖1所示。將SICK LMS 200 二維激光測距儀與Amtec Powercube Wrist 兩自由度云臺組合安裝。通過云臺的俯仰與水平轉(zhuǎn)動，可以實現(xiàn)激光測距儀的三維掃描，實現(xiàn)3D 激光雷達的工作效果。云臺通過CAN總線與車載計算機相連，俯仰角度范圍-120°～120°，水平旋轉(zhuǎn)角度范圍0°～360°，旋轉(zhuǎn)角精度0.01°.由光學成像原理可知，當攝像機的光軸與3D 激光雷達的激光發(fā)射源同軸時，可見光圖像與激光點云的公共感知空間部分達到最大，因此，本文將攝像機設計安裝于激光雷達頂部，鏡頭豎直向上，通過云臺的俯仰運動，可將攝像機旋轉(zhuǎn)到激光雷達掃描坐標原點處，實現(xiàn)最大范圍區(qū)域的匹配(如圖2所示)。同時，為了保證測量姿態(tài)的準確性，在激光雷達的頂部設計安裝了姿態(tài)航向參考系統(tǒng)(AHRS)，用于補償由地形引起的系統(tǒng)姿態(tài)變化給測量帶來的誤差。激光掃描最大角度范圍-180°～180°，掃描數(shù)據(jù)通過422 高速串口以500 kbps 與計算機實現(xiàn)通訊。

圖1 3D 激光雷達與攝像機組合感知系統(tǒng)Fig.1 Integrated perception system of 3Dlidar and camera

圖2 組合感知系統(tǒng)工作示意圖Fig.2 Schematic of integrated perception system performance

1.2 配準方法

3D 激光雷達與攝像機配準的過程其實質(zhì)就是尋找激光雷達空間與攝像機空間的交集，根據(jù)對應交集在兩個空間中幾何關系的相異特征計算空間轉(zhuǎn)換關系，所得到的轉(zhuǎn)換關系即是配準問題的解?；谔卣鼽c的配準方法介紹如下。由激光雷達的掃描方式可知掃描點云在激光雷達坐標系中的位置解算如下式

其中:云臺的俯仰角為φl;激光平面掃描角度為θl;激光打到物體上的光線長度為ρl.將點云數(shù)據(jù)按照XL軸向進行深度歸一化計算，可得到點云深度圖像。

通過平面圖像標定算法［7］，可以得到攝像機投影矩陣K，攝像機數(shù)學模型如式(2).該模型實現(xiàn)了從三維空間到二維空間的映射。

為了得到更多的特征匹配點，采用鏤空棋盤格作為標定模板。在激光雷達點云深度圖中提取m(m≥8)個特征點，同時在攝像機圖像中對應提取m(m≥8)個特征點［8］，特征像素點的齊次坐標為:Ui=［UiVi1］T，聯(lián)立激光雷達空間與攝像機空間中的m 組對應特征點可得到本質(zhì)矩陣E.對本質(zhì)矩陣進行SVD 分解E=UDVT，可解得兩空間的旋轉(zhuǎn)與平移關系，其中［xcyczc］T為攝像機坐標系內(nèi)對應激光點的位置。

進而可得激光點在圖像坐標系下的像素點坐標為［8］

其中:平移向量t 為帶尺度的平移向量;設尺度因子為kcl;不帶尺度的平移向量為t0.通過式(3)與式(4)，可得，

將選取的m 對激光點云與攝像機圖像中的特征點代入上式，可以得到尺度因子為kcl.

通過激光點的圖像坐標，索引相應的顏色值，得到3D 彩色激光點云

由AHRS 測得組合感知系統(tǒng)的偏航角為α，俯仰角為β，橫滾角為γ，［xelyelzel］T為激光雷達的地面絕對位置，則點云在大地坐標系下的坐標為

2 配準誤差分析與解決方法

在3D 激光雷達與攝像機的配準過程中，誤差主要由特征點提取產(chǎn)生。針對本配準方法，引起特征點提取誤差的原因可歸為以下情況:一是由于混合像素引起的特征點提取誤差，實驗發(fā)現(xiàn)，通過擬合平面的方法［6］，雖然一定程度上減少了混合像素的影響，但由于標定板與擬合平面的不完全匹配，會引入新的提取誤差;另一種是由于激光雷達坐標原點位置的不確定產(chǎn)生的點云解算誤差。針對以上問題，本文通過設計ASSF 濾波器以及標定激光雷達坐標原點位置來提高配準精度。進而，通過Nelder-Mead 方法對配準結(jié)果進一步優(yōu)化。

2.1 相關掃描序列濾波器

由3D 激光雷達的掃描方式可知，點云中相鄰激光點之間的距離信息具有相關性［9］，如圖3所示。根據(jù)混合像素及噪聲像素的方向不變性，在配準過程中，設計相關掃描序列濾波器，按掃描順序進行遞歸濾波，提取邊緣像素，確定特征點候選區(qū)域，削弱混合像素與噪聲像素對配準的影響。

如圖3所示，pi為激光雷達在相鄰角度的掃描點，li為pi與pi+1中ρ 值小的點向ρ 值大的點所作的垂線，Δρi與Δθi分別為相鄰激光點間距和相鄰點連線與li的夾角，如式(7)

其中:ρli、ρsi分別表示長、短光束;θrs為激光雷達掃描角分辨率。本文將混合像素與噪聲像素定義為壞點，以Δρi與Δθi為ASSF 的輸入，壞點判據(jù)為

圖3 相鄰激光點相關性Fig.3 Correlation between the close laser points

上式中，μi為對應的行或列相鄰激光點間存在壞點現(xiàn)象標志位(1 表示存在，0 表示不存在)。

定義:I 為激光點全集，Anr為正常點集合，Aed為邊緣點集合，Acp為壞點集合，Auc為待定點集合(待定壞點與邊界點)，Ag為好點集合。其中，Ag=AnrUAed，I=AgUAcpUAuc.

設p0∈Auc，共有n+1 個點，濾波過程為

1)若pi∈Ag，μi=1?pi∈Aed，pi+1∈Auc;

2)若pi∈Ag，μi=0?pi∈An，pi+1∈Ag;

3)若pi∈Auc，μi=1?pi∈Acp，pi+1∈Auc;

4)若pi∈Auc，μi=0?pi∈Aed，pi+1∈Ag;

5)若pi∈Acp，μi=1?pi∈Acp，pi+1∈Auc;

6)若pi∈Acp，μi=0??;

7)若i=n-1，則μi=1?pn∈Acp，μi=0?pn∈Aed.

至此，一個序列點集的濾波結(jié)束。對I 進行濾波，按點云行、列順序依次進行。由上述過程可知，此判定空間是完備的，經(jīng)過以上判定，可以較為準確搜索到混合像素以及邊緣像素，確定特征點候選區(qū)域。

2.2 激光雷達坐標原點位置誤差標定方法

由于激光雷達坐標原點的位置不便于直接測量，在實驗的過程中往往采用估計測量的方法確定其位置，或忽略其與云臺俯仰軸的位置、角度偏差。根據(jù)組合感知系統(tǒng)的安裝方式，本文設計了一種結(jié)構(gòu)環(huán)境中通過云臺水平旋轉(zhuǎn)掃描與俯仰旋轉(zhuǎn)掃描進行激光雷達坐標原點位置標定的方法。云臺與激光雷達安裝之間的俯仰與橫滾角度誤差可由AHRS 測出，在標定前已對其進行補償。由此可知，待標定參數(shù)為激光雷達坐標原點在云臺坐標系中的3D 坐標以及水平誤差角θhd.因為雷達坐標原點的高度坐標只在俯仰掃描時才會產(chǎn)生誤差，所以可首先對坐標原點位置進行水平標定。

如圖4(a)所示，OPT點為云臺轉(zhuǎn)軸的位置，OPTXPTYPT為云臺直角坐標系，將其置于水平位置。OL1為初始時刻激光雷達坐標原點的位置，OL1XL1YL1為初始時刻的激光雷達坐標系。OL1在云臺坐標系下的坐標為(x1，y1).云臺轉(zhuǎn)臂的長度為L1，激光雷達坐標原點距轉(zhuǎn)臂的垂直距離為L2.θhd為激光雷達與云臺轉(zhuǎn)臂之間的偏角誤差。當云臺轉(zhuǎn)臂水平旋轉(zhuǎn)θPL角時，雷達坐標原點位于OL2，坐標為(x2，y2)，OL2XL2YL2為此時建立的激光雷達坐標系。選擇任意結(jié)構(gòu)化特征明顯的環(huán)境，使激光雷達分別在位置OL1與位置OL2進行同位多次水平掃描，獲得環(huán)境點云數(shù)據(jù)。在兩組點云數(shù)據(jù)中尋找對應特征拐點，如圖中點pi、pj與pk.以點pj為例，設其在云臺坐標系下的坐標為(xpj，ypj)，(ρ1，θL1)與(ρ2，θL2)為點pj對于OL1與OL2的極坐標表示。根據(jù)云臺坐標系與激光坐標系的關系，可以得到下式

因此，

則式(10)可寫為

選取n(n ＞2)個特征點pi(i=1，2，…，n)，代入式(11)，考慮(sinθhd)2+(cos θhd)2=1 約束，通過對超定方程組進行非線性最小二乘，可得［L1 L2 sinθhdcos θhd］T的最優(yōu)估計值。

接下來對激光雷達坐標原點位置進行垂直高度標定，標定過程如圖4(b)所示。其中，ρxi為激光雷達光束長度在對應掃描面上的距離深度值分量，標定時為了減少誤差影響，取均值。L2由水平標定得到，φl為掃描面俯仰角。h 即為所求激光雷達坐標原點的垂直高度。進行n 次相異俯仰角掃描，可得h 估計值。

2.3 配準結(jié)果優(yōu)化

將激光點云到對應圖像平面的像素距離作為優(yōu)化目標函數(shù)，以上述方法綜合求得的配準結(jié)果為初始值，對R 與t 進行優(yōu)化。設=［xliylizli］T表示第i 個激光特征點在激光雷達坐標系的坐標，優(yōu)化目標函數(shù)為

圖4 激光雷達坐標原點位置標定Fig.4 Lidar origin calibration

圖5 3D 激光雷達特征點提取Fig.5 Feature points extraction from point cloud

基于Nelder-Mead 非線性優(yōu)化方法求得使目標函數(shù)達到最小值的R 與t.

3 實驗結(jié)果及分析

以自制的鏤空棋盤格為標志板，通過組合感知系統(tǒng)對其進行俯仰掃描與拍攝，俯仰角范圍為-25°～25°，橫掃角范圍-50°～50°.在激光雷達掃描角分辨率為0.25°的情況下，設置閾值Δθth與Δρth分別為86°和50 mm，通過ASSF 濾波器濾波，得到鏤空棋盤格的邊界點集，濾波結(jié)果如圖5所示。由圖可知，點云在標定棋盤格交匯處不是一點，而是一個區(qū)域。標定時可手動選取特征點候選區(qū)，特征點的xlf，ylf，zlf坐標值分別由行列邊界點的x 均值、列邊界點的y 均值以及行邊界點的z 均值確定。由此既可以消除混合像素的干擾，減少激光雷達點云分布稀疏的影響，又可以避免擬合平面帶來的誤差。

利用室內(nèi)結(jié)構(gòu)環(huán)境，對激光雷達坐標原點在云臺坐標系的三維坐標與偏角進行了標定。如圖6所示先對L1、L2與θhd進行標定。通過旋轉(zhuǎn)云臺，分別在云臺水平角20°、-10°、-40°以及-70°位置對每個結(jié)構(gòu)目標進行20 次掃描，通過擬合直線的交點得到標定點位置，由式(11)得到L1、L2與θhd測量值(如表1所示)。

圖6 激光雷達坐標原點位置水平標定實驗Fig.6 Level calibration experiments of lidar origin

表1 激光雷達水平標定結(jié)果Tab.1 Level calibration data of lidar origin mm

在水平標定的基礎上，對激光雷達坐標原點位置進行垂直標定。對處理過的玻璃板進行正面掃描，俯仰角范圍-5°～25°，角度分辨率0.25°.為減少誤差，對每個俯仰角度進行20 次掃描，根據(jù)式(12)得標定結(jié)果為h=-16.7 mm，由此可知，激光雷達坐標原點處于云臺俯仰軸下方。通過云臺旋轉(zhuǎn)構(gòu)建部分室內(nèi)環(huán)境3D 點云圖(俯視圖)，可以對比出雷達坐標原點標定前后的差別，由圖7可以驗證雷達坐標原點標定方法的有效性。

圖7 激光雷達坐標原點標定驗證實驗Fig.7 Validation of lidar origin calibration

根據(jù)標定出的雷達坐標原點參數(shù)，對激光特征點在空間中的位置進行校正，確定統(tǒng)一的激光雷達掃描原點，校正后與校正前的特征點分布如圖8所示。

圖8 激光特征點校正圖Fig.8 Calibration of lidar feature points

通過本文前面提到的配準方法可得到激光雷達空間與攝像機空間的轉(zhuǎn)換關系，旋轉(zhuǎn)矩陣R 與平移向量t 如表2所示，與基于擬合平面的方法進行比較，精度得到提高。

表2 激光雷達與攝像機空間轉(zhuǎn)換關系Tab.2 Conversion between lidar and camera

在本文的配準標定工作基礎上，組合感知系統(tǒng)通過式(4)～式(6)完成了帶顏色的3D 點云環(huán)境感知與構(gòu)建實驗，進一步驗證了組合感知系統(tǒng)與其配準方法的有效性。實驗部分結(jié)果如圖9、圖10所示，其中，(a)為實驗場景原圖，(b)為激光雷達與攝像機配準后的3D 彩色點云俯視圖，(c)為3D 彩色點云正視圖。圖10是組合感知系統(tǒng)在4個角度所拍攝的場景圖，基于本文的雷達坐標原點標定結(jié)果，實現(xiàn)了點云與圖像的準確配準與拼接。

圖9 組合感知系統(tǒng)室外環(huán)境點云重建Fig.9 Outdoor environment reconstruction with integrated perception system

圖10 組合感知系統(tǒng)室內(nèi)環(huán)境點云重建Fig.10 Indoor environment reconstruction with integrated perception system

4 結(jié)論

本文設計了一種組合感知系統(tǒng)，通過云臺俯仰旋轉(zhuǎn)使3D 激光雷達與攝像機實現(xiàn)了對環(huán)境的同軸感知，增大了共同感知的空間范圍。針對激光雷達與攝像機的空間配準問題進行改進，設計了相關掃描序列濾波器(ASSF)，根據(jù)混合像素與噪聲像素的分布特性，按照點云序列進行遞歸濾波，減少了配準過程中混合像素對特征點提取的影響，使3D 激光雷達與攝像機配準結(jié)果更為精確;并且，設計了基于結(jié)構(gòu)化特征的激光雷達坐標原點位置標定方法，減少了由激光雷達坐標原點位置不確定造成的量測解算誤差。組合感知系統(tǒng)環(huán)境配準與重構(gòu)的實驗結(jié)果表明，本文方法能夠有效地提高配準精度，使組合感知系統(tǒng)真正應用于移動機器人導航。

References)

［1］ Ranjith Unnikrishnan，Martial Hebert.Fast extrinsic calibration of a laser rangefinder to a camera［D］.Pittsburgh:Carnegie Mellon University，2005.

［2］ 項志宇，鄭路.攝像機與3D 激光雷達聯(lián)合標定的新方法［J］.浙江大學學報:工學版，2009，43(8):1401-1405.XIANG Zhi-yu，ZHENG Lu.Novel joint calibration method of camera and 3D laser range finder［J］.Journal of Zhejiang University:Engineering Science，2009，43(8):1401-1405.(in Chinese)

［3］ Ganhua Li，Yunhui Liu，Li Dong，et al.An algorithm for extrinsic parameters calibration of a camera and a laser range finder using line features［C］∥Proceedings of the 2007 IEEE/RSJ International Conference on Intelligent Robots and Systems，San Diego:IEEE，2007:3854-3859.

［4］ Davide Scaramuzza，Ahad Harati，Roland Siegwart.Extrinsic self calibration of a camera and a 3D laser range finder from natural scenes［C］∥Proceedings of the 2007 IEEE/RSJ International Conference on Intelligent Robots and Systems，San Diego:IEEE，2007:4164-4169.

［5］ 付夢印，劉明陽.視覺傳感器與激光測距雷達空間對準方法［J］.紅外與激光工程，2009，38(1):74-78.FU Meng-yin，LIU Ming-yang.Space alignment of vision sensor and LMS［J］.Infrared and Laser Engineering，2009，38(1):74-78.(in Chinese)

［6］ 鄧志紅，劉明陽，付夢印.一種改進的視覺傳感器與激光測距雷達特征匹配點提取算法［J］.光學技術，2010，36(1):43-47.DENG Zhi-hong，LIU Ming-yang，F(xiàn)U Meng-yin.A improved method of matching point extracting for vision sensor and LMS space alignment［J］.Optical Technique，2010，36(1):43-47.(in Chinese)

［7］ Zhang Zheng-you.Flexible camera calibration by viewing a plane from unknown orientations［C］∥Proceedings of International Conference on Computer Vision，Corfu，1999:666-673.

［8］ Richard I.Hartley.In defense of the eight-point algorithm［J］.IEEE Transactions on Pattern Analysis and Machine Intelligence，1997，19(6):580-593.

［9］ Cang Ye，Johann Borenstein.Characterization of a 2-D laser scanner for mobile robot obstacle negotiation［C］∥Proceedings of the 2002 IEEE International Conference on Robotics and Automation.Washington DC:IEEE，2002:2512-2518.