CN101698303B - 一种三维激光和单目视觉间的自动标定方法 - Google Patents

Abstract

本发明属于机器人自主环境感知技术领域，涉及到三维激光和单目视觉间的自动标定方法。其特征在于通过设计的标定装置完成三维激光和单目视觉之间的自动联合标定。激光数据特征角点的提取过程分为检测与校正两阶段。检测阶段将原始三维激光数据与二值化后的测距数据矩阵进行映射关联与统计分析；校正阶段通过与由标定板中黑白格角点所构造的标准模板相匹配，提高了角点提取精度，进而采用迭代优化方法进行求解。本发明的效果益处是能够有效减少噪声和入射角对激光测距数据的影响，标定过程实现了激光-视觉数据特征角点提取与匹配的自动化和一体化，从而为多传感器信息融合奠定基础。

Description

一种三维激光和单目视觉间的自动标定方法

技术领域

本发明属于机器人自主环境感知技术领域，涉及到三维激光测距系统和视觉传感器之间的信息融合，特别涉及到三维激光和单目视觉间的自动标定方法。

背景技术

工作在复杂场景中的机器人系统依靠单一传感器无法有效完成自主环境感知与工作场景理解。不同类型传感器之间的信息融合是提高机器人环境理解与认知性能的必备手段，而多种传感器之间的标定又是其中的重要环节。以往的标定工作大多是针对视觉传感器与二维激光传感器之间的标定(Zhang Q L，Pless R.Extrinsic Calibrationof a Camera and Laser Range Finder IEEEInternational Conference on Intelligent Robotsand Systems(IROS)，2004)，主要是通过寻找二维激光数据的拐点信息或黑白格交错信息，完成视觉和激光数据点的匹配，但是存在的问题是依赖于扫描介质的特性，特别当激光测距距离较远或激光扫描数据点稀疏时，标定效果受到显著影响。通过实验对激光测距性能的结果分析得到激光测距性能会受到激光测距距离、被测物体介质表面特性以及扫描入射角等因素的影响(Ye C，Borenstein J.Characterization of a 2-D laserscanner formobile robot obstacle negotiation.Proceedings of the 2002IEEEInternational Conference on Roboticsand Automation.Washington DC，USA，2002：2512-2518；于金霞，蔡自兴，邹小兵，段琢华，移动机器人导航中激光雷达测距性能研究，传感技术学报2006，19(2))，如何针对这些影响因素进行标定装置设计和算法开发仍是需要探索和解决的问题。依赖于反射介质获得三维激光扫描数据的方法所获取的激光测距数据的稳定性无法保证，文献(刘佳，张爱武，马牧运，杨帆，王书民，刘晓萌.三维激光扫描测量系统中的摄像机标定.系统仿真学报.2008，20(z1))，以玻璃作为反射媒介，无法有效给出标定实验结果的验证分析，只能停留在仿真阶段。文献(Scaramuzza D，Harati A，and Siegwart R.Extrinsic Self Calibration of a Camera and a 3D Laser Range Finder fromNaturalScenes.Proceedings of the 2007)，依赖于人工手动点选匹配点进行三维激光和视觉间外参标定，由于人工点选的不准确，存在引入的人为误差无法进行量化控制与统计的问题，同时手动点选时间开销大。文献(Unnikrishnan R，Hebert M.FastExtrinsic Calibration of a Laser Rangefinder to a Camera.TechnicalReportCMU-RI-TR-05-09，Robotics Institute，Carnegie Mellon University，2005)在三维激光和视觉间的标定过程中激光视觉匹配对的提取较大依赖于三维激光传感器对黑白格标定装置的扫描效果，其局限性是仅在近距离和较小偏转角度条件下才能保证良好的扫描效果。

发明内容

本发明要解决的问题是提供了自动提取三维激光数据和单目视觉图像数据中特征匹配对的方法，实现三维激光和单目视觉之间的自动标定，解决了手动点选匹配对所引入的人为误差不可控和时间开销大问题，提高了匹配对提取精度。以设计的一体化标定装置为基础，减小了距离与偏转角对标定的影响，克服了标定只能限定在近距离范围内这一局限性。

本发明的技术方案如下：

一种三维激光和单目视觉间的自动标定方法，其特征在于：利用激光扫描镂空黑白格标定板时所产生的测距突变特性，通过原始三维激光数据与二值化后的测距数据矩阵间的映射关联，基于测距数据的分布模式检测激光数据特征角点；所述的激光数据特征角点是标定装置中黑白格相交所构成的角点；

首先在黑白格标定板每个方格的正中按同一尺寸制作镂空圆孔，根据激光数据在圆孔边界所发生的测距突变，并结合由界定的区域数据所定义的平面拟合公式

，其中a＝[a₁，a₂，a₃]^T为平面法向量，q_i＝[x，y，z]^T为区域坐标点，计算得到二值化测距数据矩阵并统计其直方图，并将检测到的直方图峰值点列的中值点设置为圆孔中心点；

然后利用检测到的圆孔中心点，按照四邻域关系计算与黑白格角点相对应的激光数据特征角点，并使用四元数法与由标定板中黑白格角点所构造的标准模板相匹配，校正初始激光数据特征角点的误差，从而与视觉中提取的黑白格角点形成匹配对，并根据迭代优化算法确定三维激光与单目视觉间的标定关系。

本发明的效果与益处是能够有效的减小了噪声、距离、入射角等对三维激光和单目视觉传感器间标定的影响，克服了标定只能限定在近距离范围内这一局限性。标定过程实现了激光-视觉数据匹配对提取与匹配的自动化和一体化，无手动点选匹配对所引入的人为误差。标定所使用的装置设计简单合理，并且便于携带与应用，可完成在野外、废墟等复杂环境下三维激光与单目视觉间的快速准确标定，从而保证利用三维彩色激光测距点云对被测场景的直观渲染，以及相关虚拟现实技术的实现，而且对基于多传感器信息融合的智能移动机器人技术的应用奠定了良好基础。

附图说明

图1是三维激光测距系统与单目视觉标定装置的示意图。

图2是从三维激光点云数据到二维图像数据所建立的多级映射关系图。

图3(a)是某一角度和距离下的标定装置图，(b)是与图(a)相对应的激光点云数据。

图4(a)是标定装置区域划分与角点提取的模拟图，(b)是大角度偏转或区域偏移情况下区域划分与角点提取的模拟图。

图5是3×4的标准模板图。

图6(a)是某一距离和角度下的标定装置图，(b)是对(a)中视觉图像的特征角点提取结果图。

图7是大连理工大学管理学院正门入口场景图。

图8是在图7场景中采集三维激光数据点云化后的效果图。

图9是根据标定结果利用图7对图8进行染色的得到的效果图。

图10是三维激光测距系统与单目视觉环境数据采集设备图。

具体实施方式

以下结合技术方案和附图详细叙述本发明的具体实施方式。

实施例

1.激光传感器特性分析及标定装置的设计

不同介质对激光传感器所发射的激光束具有不同的反射率，其中有光泽物体的反射率要高于无光泽物体，同样不同入射角对测距的影响也非常显著，但目标物体表面颜色对测距影响不大。针对上述激光特性，开发了三维激光与视觉一体化标定装置(如图1所示)，装置的材料为有光泽相纸，以保证对激光具有较高反射率。在相纸表面喷绘出4×5的正方形黑白格，每个黑白格的边长为25cm。在每个黑白格正中心进行镂空，镂空形状为半径为5cm的圆形。经过实验验证，选择圆形作为镂空形状是为了保证算法的鲁棒性和避免激光边缘效应所产生的影响。

2.标定所需激光数据特征角点的提取

标定中需要提取的激光数据特征角点是标定装置中黑白格相交所构成的角点，该角点是利用镂空圆孔的中心点坐标的统计平均与校正所获得的。角点提取分为检测和校正两个环节，其中检测环节利用激光测距数据的统计特性进行粗提取，该方法可减少激光测距噪声点的影响。校正阶段与由标定板中黑白格角点所构造的标准模板进行匹配，提高了角点检测的精度，并可减小激光入射角对测距的影响。

三维激光测距角点检测算法步骤如下：

(1)确定标定装置的粗略位置范围。选定标定装置所在大致位置，根据激光测距点云数据与激光扫描顺序索引值之间的映射关系(如图2所示)，筛选出该区域的数据(如图3(a)(b)所示)。

(2)根据范围确定平面。根据区域内的数据点拟合平面，即

$a=\arg \underset{a}{\min }\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{\left|\right|a^T{q}_i\left|\right|}^2---\left(1\right)$

其中a＝[a₁，a₂，a₃]^T为平面法向量，q_i＝[x，y，z]^T为区域坐标点。

为了提高计算效率，可根据装置所在范围的四个界限点直接确定平面方程。

(3)激光测距数据二值化。由扫描序列索引值关系确定的区域包含激光穿过镂空圆孔所扫描到的标定装置后侧的测距数据，依据到所拟合平面的距离值d来对区域数据进行二值化处理。

$I_i=\begin{array}{c}\left\{\begin{array}{cc}255& d_i<D\\ 0& d_i>=D\end{array}\right.---\left(2\right)\end{array}$

其中I_i为区域内i点二值化后的取值，D为经验距离阈值，D＝15cm。二值化后，镂空圆孔区域的数据为0，其他区域数据为255，从而可以进一步借鉴图像处理方法进行角点提取。

(4)提取角点。如图4(a)所示的标定装置模拟简化图，4×5的黑白格中，圆形表示镂空区域，中心的红色十字表示镂空中心，黄色的圆点表示黑白格的角点。蓝色虚线将平面进行均匀划分。利用3)中二值化后所得数据，首先按列统计0和255的点数分布直方图，并根据峰值点确定镂空中心的行坐标，同理也可按行统计得到列坐标，具体见如下公式：

$\left\{\begin{array}{c}i=\mathrm{median}\left(\arg \underset{i}{\max }\underset{j}{\overset{\mathrm{bin}}{\mathrm{\&Sigma;}}}1(I_{\mathrm{ij}}=0)\right)\\ j=\mathrm{median}\left(\arg \underset{j}{\max }\underset{i}{\overset{\mathrm{bin}}{\mathrm{\&Sigma;}}}1(I_{\mathrm{ij}}=0)\right)\end{array}\right.---\left(3\right)$

其中i，j分别表示行列下标，函数1(·)为0-1函数，如果括号内条件为真则值为1，否则为0。median()为中值选择函数，I_ij表示二值化取值。经过图像到激光数据的映射(如图2所示)，从而确定了4×5的镂空中心坐标，并可进一步根据四邻域关系确定3×4的黑白格角点坐标。

上述算法完成了对角点的初步检测。该检测算法的优点在于：角点检测算法鲁棒性好。正如图4(a)所示，选择圆形作为镂空形状既减少了噪声的影响(其他形状不利于镂空中心提取)，同时其几何特性保证了镂空中心提取的正确性。此外，如果标定装置的倾斜角度过大，或区域的范围选择不理想，这些因素的影响可能会产生如图4(b)所示的不合理间隔区域划分。在该种情况下，上述算法同样能正确的完成镂空中心的提取。此外上述算法支持角点提取顺序的可控选择，从而易于实现与图像中所提取角点的按次序一一对应。

由于受到激光入射角及噪声的影响，检测到的角点无法保证严格位于同一平面上，同时相同间隔内的角点也无法保证严格在同一直线上。因此需要下面给出的算法对检测角点进行进一步优化处理，从而提高精度。

3.三维激光测距角点的校正算法

由检测算法可知，所得到的角点具有一定的误差，需要进一步校正以提高精度。校正算法借鉴最近点迭代(ICP)思想，利用检测出来的激光角点作为初始值与已知模板进行匹配，使得检测点与标准模板的误差总和最小，具体见公式：

$\left[\begin{array}{cc}{R}_{3\&times;3}& T_{3\&times;1}\end{array}\right]=\underset{R_{3\&times;3},T_{3\&times;1}}{\arg \min }\underset{i=1}{\overset{n}{\mathrm{\&Sigma;}}}{||{\stackrel{\&OverBar;}{I}}_i-\left[\begin{array}{cc}{R}_{3\&times;3}& T_{3\&times;1}\end{array}\right]I_i||}^2---\left(4\right)$

其中R_3×3，T_3×1分别为旋转和平移矩阵，

为检测点的齐次坐标，I_i为模型点的齐次坐标。图5所示为构造的标定模板模型，每个方格边长为25cm，起始位置处于X-Y平面。采用四元数法求解三维映射变换。

利用四元数法求三维映射变换的算法如下：

(1)求两个三维点集的重心坐标：

p_i∈P，i ≤ N，

p′_i∈P′，i≤N。

(2)求两数据点集的协方差矩阵： ${\mathrm{\&Lambda;}}_{3\&times;3}=\frac{1}{N}\underset{i=1}{\overset{N}{\mathrm{\&Sigma;}}}(p_i-\stackrel{\&OverBar;}{p}){(p_i^{\&prime;}-{\stackrel{\&OverBar;}{p}}^{\&prime;})}^T.$

(3)令M_3×3＝Λ-Λ^T，设K_3×1＝[M₂₃，M₃₁，M₁₂]^T，构造矩阵如下

$H_{4\&times;4}=\left[\begin{array}{cc}\mathrm{tr}\left(\mathrm{\&Lambda;}\right)& K^T\\ K& \mathrm{\&Lambda;}+{\mathrm{\&Delta;}}^T-\mathrm{tr}\left(\mathrm{\&Lambda;}\right)I_{3\&times;3}\end{array}\right]---\left(5\right)$

(4)求H矩阵最大特征值对应的特征向量，即为旋转变换的四元数表达。

利用与矩阵的转换关系得到旋转变换矩阵R_3×3，进一步根据两点集间重心偏移计算平移变换向量

4.三维激光数据到二维视觉数据间变换关系的迭代计算

选定图像中与3×4激光测距角点相对应的区域，由于镂空孔后环境的影响使得视觉特征角点的提取受到干扰，所以首先对该区域采用自适应阈值二值化处理，然后利用OpenCV中黑白格角点检测函数提取角点，提取的角点如图6所示。

根据已经获得的激光数据特征角点和视觉数据特征角点匹配对，采用迭代优化方法来求解三维空间到二维空间的射影变换，此处采用高斯牛顿迭代法进行外参数优化求取。令

图像二维齐次坐标向量，m＝[X，Y，Z，1]是三维空间测量齐次坐标向量，

也即求m使得：

$m=\arg \underset{m}{\min }\underset{i=1}{\overset{n}{\mathrm{\&Sigma;}}}{\left|\right|{\stackrel{\&OverBar;}{m}}_i-m_i\left(\stackrel{\&RightArrow;}{p}\right)\left|\right|}^2---\left(6\right)$

其中f_x，f_y分别为x，y方向的焦距，(u_x，u_y)是主点相对于图像中心的偏移向量。此处f_x，f_y，u_x，u_y已知，是通过传统的内参标定方法获得的。r₁，r₂，r₃分别为旋转矩阵的列间量，t_x，t_y，t_z分别为平移向量分量。

由高斯牛顿迭代法，给定初始解令

在

附近有：

$m_i({\stackrel{\&RightArrow;}{p}}^k+{\mathrm{\&Delta;}}^k)=m_i\left({\stackrel{\&RightArrow;}{p}}^k\right)+J_{{\stackrel{\&RightArrow;}{p}}^k}{\mathrm{\&Delta;}}^k---\left(7\right)$

其中Jacobian矩阵

寻找下一个迭代点

使得：

$\underset{i=1}{\overset{n}{\mathrm{\&Sigma;}}}{\left|\right|{\stackrel{\&OverBar;}{m}}_i-m\left({\stackrel{\&RightArrow;}{p}}^{k+1}\right)\left|\right|}^2=\underset{{\mathrm{\&Delta;}}^k}{\min }\underset{i=1}{\overset{n}{\mathrm{\&Sigma;}}}\left|\right|{\stackrel{\&OverBar;}{m}}_i-m_i\left({\stackrel{\&RightArrow;}{p}}^k\right)-J_{{\stackrel{\&RightArrow;}{p}}^k}{\mathrm{\&Delta;}}^k\left|\right|=\underset{{\mathrm{\&Delta;}}^k}{\min }\underset{i=1}{\overset{n}{\mathrm{\&Sigma;}}}\left|\right|{\stackrel{\&RightArrow;}{e}}_i^k-J_{{\stackrel{\&RightArrow;}{p}}^k}{\mathrm{\&Delta;}}^k\left|\right|$

令

得正规化方程：

${\mathrm{\&Delta;}}^k={\left({J^T}_{{\stackrel{\&RightArrow;}{p}}^k}{J}_{{\stackrel{\&RightArrow;}{p}}^k}\right)}^{-1}{J^T}_{{\stackrel{\&RightArrow;}{p}}^k}{\stackrel{\&RightArrow;}{e}}_i^k---\left(8\right)$

代入前式的高斯牛顿法的迭代格式：

${\stackrel{\&RightArrow;}{p}}^{k+1}={\stackrel{\&RightArrow;}{p}}^k+{\left({J^T}_{{\stackrel{\&RightArrow;}{p}}^k}{J}_{{\stackrel{\&RightArrow;}{p}}^k}\right)}^{-1}{J^T}_{{\stackrel{\&RightArrow;}{p}}^k}{\stackrel{\&RightArrow;}{e}}^k---\left(9\right)$

利用该迭代格式求取满足公式(6)的变换参数，得到标定结果。图8所示为三维激光测距数据点云化后所显示的室外场景，图9直观的给出经过标定后，利用图7的场景视觉图像对激光点云数据进行染色的效果图，从而检验了所提标定算法的正确性和有效性。

为了验证本方法的有效性，利用如图10所构建的设备来进行标定方法的验证。采用SICK LMS 291型激光传感器，其平面扫描角度为0-180度，纵向范围动态可调，分为8米、16米、32米(距离分辨率为1毫米)、80米(距离分辨率为1厘米)四个扫描范围，此处采用32米扫描范围。云台电机具有可选旋转角度分辨率，包括：0.192°、0.288°、0.480°和0.960°。此处选取0.192°，起始角度为30°，扫描范围为30°-130°。利用电机带动激光传感器获得场景的三维激光测距数据。标定装置的尺寸采用4×5的黑白格，每个黑白格的边长为25cm，在每个黑白格中心进行镂空，选取圆形作为镂空形状，半径为5cm，如图1所示。单目摄像头采用FlyCapture单目摄像头，分辨率为1024×768，经过标定内部参数为：

f_x＝894.43884277    f_y＝893.06408691

u_x＝504.93334961    u_y＝368.08633423

其中f_x，f_y分别为x，y方向的焦距，(u_x，u_y)是主点相对于图像中心的偏移向量。

连续扫描三幅不同角度和不同位置的场景，提取三维激光测距数据和单目视觉的匹配点。基于该标定装置，采用的角点提取算法所提取的角点匹配对稳定，受距离远近和倾斜角度变化的影响较小。标定装置与采集设备之间距离的合适范围为2m-10m之间，标定装置倾角的合适范围为-70°-+70°之间。

对3幅场景共36个角点采用高斯牛顿迭代法求解所得到的迭代误差表如下：

得到的三维激光测距系统与单目摄像机传感器的外参数为：

◆旋转矩阵：

$R=\left[\begin{array}{ccc}0.02729959& 0.99962574& 1.75325444e-003\\ -0.12945299& 5.27446438e-003& -0.99157155\\ -0.99120969& 0.02684253& 0.12954853\end{array}\right]$

◆平移矩阵：

T＝[5.38621889e-003  0.44423267  -0.15120429]

由上表可以看出经过4次迭代后，误差为0.2876个像素。从定性的角度分析，利用场景图像对三维激光测距数据点进行染色形成的彩色三维激光点云效果图，可以对标定效果进行直观验证。附图9给出了室外场景下的三维激光点云染色效果图。

Claims (1)

1.一种三维激光和单目视觉间的自动标定方法，其特征在于：利用激光扫描镂空黑白格标定板时所产生的测距突变特性，通过原始三维激光数据与二值化后的测距数据矩阵间的映射关联，基于测距数据的分布模式检测激光数据特征角点；所述的激光数据特征角点是标定装置中黑白格相交所构成的角点；

首先在黑白格标定板每个方格的正中按同一尺寸制作镂空圆孔，根据激光数据在圆孔边界所发生的测距突变，并结合由界定的区域数据所定义的平面拟合公式

，其中a＝[a₁，a₂，a₃]^T为平面法向量，q_i＝[x，y，z]^T为区域坐标点，计算得到二值化测距数据矩阵并统计其直方图，并将检测到的直方图峰值点列的中值点设置为圆孔中心点；

然后利用检测到的圆孔中心点，按照四邻域关系计算与黑白格角点相对应的激光数据特征角点，并使用四元数法与由标定板中黑白格角点所构造的标准模板相匹配，校正初始激光数据特征角点的误差，从而与视觉中提取的黑白格角点形成匹配对，并根据迭代优化算法确定三维激光与单目视觉间的标定关系。

Images