移動機器人組合感知系統及其配準方法改進

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

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

0 引言

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

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

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

1 組合感知系統與配準方法

1.1 組合感知系統

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

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

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

1.2 配準方法

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

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

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

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

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

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

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

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

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

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

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

2.1 相關掃描序列濾波器

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

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

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

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

上式中，μi為對應的行或列相鄰激光點間存在壞點現象標志位(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.

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

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

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

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

因此，

則式(10)可寫為

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

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

2.3 配準結果優化

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

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

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

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

3 實驗結果及分析

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

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

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

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

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

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

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

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

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

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

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

圖9 組合感知系統室外環境點云重建Fig.9 Outdoor environment reconstruction with integrated perception system

圖10 組合感知系統室內環境點云重建Fig.10 Indoor environment reconstruction with integrated perception system

4 結論

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

References)

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

［2］ 項志宇，鄭路.攝像機與3D 激光雷達聯合標定的新方法［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，FU 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.