移动机器人组合感知系统及其配准方法改进

付梦印，杨毅，朱昊，王立平

(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.