CN101699313B - 基于摄像机和三维激光雷达的外部参数标定方法及系统 - Google Patents

Abstract

本发明公开了一种基于摄像机和三维激光雷达的外部参数标定方法，通过根据三维激光雷达坐标系原点到不同位置靶标平面的垂线方向的测量误差的协方差，及垂线方向从三维激光雷达坐标系转换到摄像机坐标系的转换关系中测量误差的协方差，得到转换关系中测量误差的协方差中含有摄像机测量噪声的方差与三维激光雷达测量噪声的方差的平方和的算式；将得到的所有测量噪声的方差的平方和的倒数作为加权系数，标定出具有最大似然估计的旋转矩阵；本发明同时还公开了一种基于摄像机和三维激光雷达的外部参数标定系统；由于标定过程中考虑了测量误差对待标定旋转矩阵的影响，对旋转矩阵的标定结果中测量误差采用最大似然估计的算法，使标定结果更加准确。

Description

基于摄像机和三维激光雷达的外部参数标定方法及系统

技术领域

本发明涉及摄像机和三维激光雷达技术，尤指一种基于摄像机和三维激光雷达的外部参数标定方法及系统。

背景技术

目前，由于可见光摄像机的探测范围宽、信号丰富，在车辆环境感知的过程中得到了广泛的应用，但它易受外界因素影响，存在目标缺失、模糊等问题。近些年来，随着激光雷达的逐步发展，使用激光雷达测距已得到广泛应用，激光雷达具有测距原理简单，测量精度高、速度快，不受外界光照条件的影响等优点，其提供的距离信息和摄像机的色彩信息具有很强的互补性，因此融合两者的信息成为当今的研究热点。这种融合方式可以应用到很多场合，例如环境感知、障碍检测、机器人导航、三维重建和纹理映射等。

通常摄像机坐标系和激光雷达坐标系的转换关系，可以采用公式(1)表示：

m_c＝Rm_l+t    (1)

其中，m_c为摄像机坐标系的点，m_l为激光雷达坐标系的点，标定的目的就是：确定旋转矩阵R和平移向量t的值，将二者的坐标统一到同一个坐标系中。

近年来，在摄像机和激光雷达系统外部参数标定方面有很多研究成果，其中，QiLong Zhang较早研究了摄像机和二维激光雷达的标定方法，具体是：通过摄像机和二维激光雷达观测平面棋盘格时的不同“视图”，根据特征的约束条件可以求解出外部参数，建立起激光雷达每个测量值和摄像机中心到标定靶面的距离之间的几何约束关系，由于用到的激光雷达是二维的，因此可以假设z轴坐标为0，从而简化计算模型，采用非线性方法进一步优化R、t。

刘大学等特制了一块标定板，在黑色的平面上制作两个白色等腰直角三角形，顶点的位置精确已知，通过激光在黑色平面上不反射、在白色平面上反射的特点，实现雷达数据点和图像像素点的匹配，就可以实现R、t的标定。

在摄像机和三维激光雷达外部参数标定方面，D.Scaramuzza将三维激光雷达采集的距离图像进行变换，使场景自然边缘更清晰突出；再提取边缘线条与摄像机拍摄的相片中检测出来的边缘相匹配；最后，通过选择自然场景中，人工选择摄像机图像和三维激光雷达的距离图像的对应点，就可以实现外部参数的标定，这种标定的方法称作自标定。

Ranjith Unnikrishnan采用棋盘格作为靶标，进行标定摄像机以及和三维激光雷达的外部参数，并对结果进行全局优化，得到了很好的效果，但是这种方法没有考虑从不同靶标平面计算所得中间量的误差的影响。

上述所提到的各种方法都没有考虑到对标定结果中测量误差进行准确的估计，从而导致对外部参数旋转矩阵R的标定不够准确。

发明内容

有鉴于此，本发明的主要目的在于提供一种基于摄像机和三维激光雷达的外部参数标定方法及系统，使标定结果的准确性得到提高。

为达到上述目的，本发明的技术方案是这样实现的：

本发明提供了一种基于摄像机和三维激光雷达的外部参数标定方法，该方法包括：

摄像机拍摄不同位置棋盘格靶标的图像，建立靶标平面的特征点和对应图像平面上的点之间的关系，计算出摄像机的内参数矩阵，从而标定靶标相对于摄像机坐标系的外部参数旋转矩阵和平移向量；

将三维激光雷达数据进行处理，得到三维激光雷达坐标系下靶标平面方程的参数；

根据外部参数旋转矩阵和平移向量，得到摄像机坐标系原点到不同位置靶标平面的垂线方向和距离；

根据三维激光雷达坐标系下靶标平面方程的参数，得到三维激光雷达坐标系原点到不同位置靶标平面的垂线方向；

根据三维激光雷达坐标系原点到不同位置靶标平面的垂线方向，从三维激光雷达坐标系转换到摄像机坐标系的转换关系中测量误差的协方差，并根据三维激光雷达坐标系原点到不同位置靶标平面的垂线方向的测量误差的协方差，得到转换关系中测量误差的协方差中含有摄像机测量噪声的方差与三维激光雷达测量噪声的方差的平方和的算式；

将得到的所有摄像机测量噪声的方差与三维激光雷达测量噪声的方差的平方和的倒数作为加权系数，标定出具有最大似然估计的旋转矩阵。

上述方案中，所述转换关系中的测量误差的协方差的计算，具体为：根据摄像机坐标系和三维激光雷达坐标系中的原点到不同位置靶标平面的垂线方向，得到垂线方向从三维激光雷达坐标系到摄像机坐标系的具有测量误差的转换关系，然后对测量误差进行协方差计算。

上述方案中，所述三维激光雷达坐标系原点到不同位置靶标平面的垂线方向的测量误差的协方差的计算，具体为：根据三维激光雷达坐标系原点到不同位置靶标平面的垂线方向，计算三维激光雷达坐标系原点到不同位置靶标平面的垂线方向的测量误差的协方差。

上述方案中，所述计算三维激光雷达坐标系下的靶标平面方程的参数，具体为：

通过三维激光雷达扫描到靶标时，落在靶标平面上的扫描点的坐标，进行三维激光雷达数据处理，得到三维激光雷达坐标系下靶标平面方程的参数。

上述方案中，所述标定靶标相对于摄像机坐标系的外部参数，具体为：通过建立靶标平面的特征点和对应图像平面上的点之间的关系，计算出摄像机的内参数矩阵，标定靶标相对于摄像机坐标系的外部参数。

上述方案中，所述得到具有最大似然估计的旋转矩阵后，该方法进一步包括：将求解出的旋转矩阵的值作为初始值，并根据摄像机坐标系中原点到不同位置靶标平面的距离，将靶标平面的点对应摄像机拍摄的图像平面的距离作为待最小化的目标函数，求解旋转矩阵的全局最优值。

本发明还提供了一种基于摄像机和三维激光雷达的外部参数标定系统，该系统包括：

第一外部参数标定模块，用于建立靶标平面的特征点和对应图像平面上的点之间的关系，计算出摄像机的内参数矩阵，提供靶标相对于摄像机坐标系的外部参数给第一垂线方向模块；

平面方程的参数模块，用于进行三维激光雷达数据处理，提供三维激光雷达坐标系下靶标平面方程的参数给第二垂线方向模块；

第一垂线方向模块，用于根据靶标相对于摄像机坐标系的外部参数，得到摄像机坐标系原点到不同位置靶标平面的垂线方向；

第二垂线方向模块，用于根据三维激光雷达坐标系下的靶标平面方程的参数，得到三维激光雷达坐标系原点到不同位置靶标平面的垂线方向；

第一距离模块，用于根据第一外部参数标定模块提供的外部参数，得到摄像机坐标系原点到不同位置靶标平面的距离；

第一计算模块，用于根据三维激光雷达坐标系中的原点到不同位置靶标平面的垂线方向的测量误差的协方差，以及各个垂线方向从三维激光雷达坐标系转换到摄像机坐标系的转换关系的测量误差的协方差，得到转换关系中测量误差的协方差中含有摄像机测量噪声的方差与三维激光雷达测量噪声的方差的平方和的算式；

旋转矩阵模块，用于将第一计算模块得到的所有摄像机测量噪声的方差与三维激光雷达测量噪声的方差的平方和的倒数，作为加权系数，标定出具有最大似然估计的旋转矩阵。

上述方案中，所述系统进一步包括：

第一协方差模块，用于根据三维激光雷达坐标系原点到不同位置靶标平面的垂线方向，计算三维激光雷达坐标系原点到不同位置靶标平面的垂线方向的测量误差的协方差；

第二协方差模块，计算转换关系模块提供的转换关系中测量误差的协方差；

转换关系模块，用于根据摄像机坐标系和三维激光雷达坐标系中的原点到不同位置靶标平面的垂线方向，计算垂线方向从三维激光雷达坐标系到摄像机坐标系的具有测量误差的转换关系；

优化模块，用于将旋转矩阵模块标定的旋转矩阵的值作为初始值，并根据摄像机坐标系中原点到不同位置靶标平面的距离，将靶标平面的点对应摄像机拍摄的图像平面的距离作为待最小化的目标函数，求解旋转矩阵的全局最优值。

本发明提供的基于摄像机和三维激光雷达的外部参数标定方法及系统，通过对三维激光雷达坐标系原点到不同位置靶标平面垂线方向的误差的协方差的计算，以及垂线方向从三维激光雷达坐标系转换到摄像机坐标系的转换关系中测量误差的协方差的计算，将转换关系中测量误差的协方差转换为含有摄像机测量噪声的方差与三维激光雷达测量噪声的方差的平方和的算式，将所有摄像机测量噪声的方差与三维激光雷达测量噪声的方差的平方和的倒数作为加权系数，标定出具有最大似然估计的旋转矩阵。由于本发明在标定过程中考虑了测量误差对待标定旋转矩阵的影响，且误差的计算采用最大似然估计的方法，从而使得标定旋转矩阵的结果更加准确，提高了外部参数标定结果的准确性。

附图说明

图1为本发明实现基于摄像机和三维激光雷达的外部参数标定方法的流程示意图；

图2为本发明中摄像机坐标系、三维激光雷达坐标系和靶标平面的位置关系图；

图3为本发明实现基于摄像机和三维激光雷达的外部参数标定系统的结构示意图。

具体实施方式

本发明的基本思想是：根据三维激光雷达坐标系原点到不同位置靶标平面垂线方向的测量误差的协方差，以及垂线方向从三维激光雷达坐标系转换到摄像机坐标系的转换关系中测量误差的协方差，得到转换关系中测量误差的协方差中含有摄像机测量噪声的方差与三维激光雷达测量噪声的方差的平方和的算式，将得到的所有摄像机测量噪声的方差与三维激光雷达测量噪声的方差的平方和的倒数作为加权系数，标定出具有最大似然估计的旋转矩阵。

下面结合附图及具体实施例对本发明再做进一步的详细说明。

本发明实现基于摄像机和三维激光雷达的外部参数标定方法，如图1所示，包括以下步骤：

步骤101：摄像机拍摄不同位置棋盘格靶标的图像，建立靶标平面的特征点和对应图像平面上的点之间的关系，计算出摄像机的内参数矩阵A，从而标定靶标相对于摄像机坐标系的外部参数旋转矩阵R_c和平移向量t_c。

根据张正友等人提出的基于平面靶标的摄像机标定方法，要求摄像机在两个以上不同的方位拍摄同一个平面靶标，摄像机和平面靶标都可以自由移动，无需知道运动参数。在标定过程中，假设摄像机的内部参数不变，在不同角度拍摄靶标时，只有外部参数改变，通过摄像机拍摄不同位置棋盘格靶标的图像，提取棋盘格靶标上方格的角点作为特征点，建立靶标点和对应图像点之间的关系，从而计算出摄像机的内参数和靶标相对于摄像机坐标系的外部参数。

具体的，通过以平面棋盘格作为靶标，摄像机拍摄靶标在不同位置的图像，提取棋盘格靶标上方格的角点作为靶标平面的特征点，建立靶标平面的特征点和对应图像平面上的点之间的关系，计算出摄像机的内参数，从而标定靶标相对于摄像机坐标系的外部参数R_c和t_c。标定过程如下所述：

首先，求解单应矩阵H：

设靶标平面的特征点 $\tilde{M}=\left[\begin{array}{ccc}x& y& 1\end{array}\right],$ 相应的图像平面上的点 $\tilde{M}=\left[\begin{array}{ccc}u& v& 1\end{array}\right],$ 二者的映射关系如公式(2)所示：

$s\tilde{m}=A\left[\begin{array}{ccc}{r}_{c1}& r_{c2}& t_c\end{array}\right]\tilde{M}---\left(2\right)$

公式(2)中，s是尺度因子，r_c1、r_c2分别为靶标平面的特征点从靶标平面坐标系变换到图像坐标系的旋转矩阵R_c的前两列，t_c是相应的平移向量；A为摄像机内参数， $A=\begin{array}{}\end{array}\left[\begin{array}{ccc}{\mathrm{\α}}_x& \mathrm{\γ}& u_0\\ 0& {\mathrm{\α}}_y& v_0\\ 0& 0& 1\end{array}\right],$ 其中，α_x、α_y分别是图像坐标系中u轴和v轴的尺度因子，u₀、v₀是光学中心，γ是u轴和v轴不垂直因子，很多情况下，令γ＝0。

令H＝A[r_c1  r_c2  t_c]，则公式(2)可以简写成公式(3)：

$s\tilde{m}=H\tilde{M}---\left(3\right)$

令 $f={\left[\begin{array}{ccc}{h}_1^T& h_2^T& h_3^T\end{array}\right]}^T,$ 其中，

表示H矩阵的三个列向量的转置，图像平面上的点和靶标平面的特征点的映射关系可以写成公式(4)的形式：

$\left[\begin{array}{ccc}{\tilde{M}}^T& 0^T& -u{\tilde{M}}^T\\ 0^T& {\tilde{M}}^T& -v{\tilde{M}}^T\end{array}\right]f=0---\left(4\right)$

根据拍摄的每幅图像上的n个点，采用最小二乘法可以求解出每幅图像所对应的矩阵H_i，i＝1，2，...。

其次，根据得到的所有图像的H矩阵，求解中间向量b：

令B＝A^-TA^-1，则：

$B=A^{-T}{A}^{-1}=\left[\begin{array}{ccc}{B}_{11}& B_{12}& B_{13}\\ B_{21}& B_{22}& B_{23}\\ B_{31}& B_{32}& B_{33}\end{array}\right]$

将上述矩阵写成向量形式，如公式(5)所示：

b＝[B₁₁  B₁₂  B₂₂  B₁₃  B₂₃  B₃₃]^T    (5)

那么，就有公式(6)所示关系式存在：

$h_i^T{\mathrm{Bh}}_j=v_{\mathrm{ij}}^{T}b---\left(6\right)$

其中，h_i＝[h_i1  h_i2  h_i3]^T，h_j＝[h_j1  h_j2  h_j3]^T，i和j均表示第几幅图像，i＝1，2，…；j＝1，2，…；

v_ij＝[h_i1h_j1  h_i1h_j2+h_i2h_j1  h_i2h_j2  h_i3h_j1+h_i1h_j2  h_i3h_j2+h_i2h_j3  h_i3h_j3]^T

将所有v_ij组成矩阵V，V是2n×6的矩阵，其中，n＝1，2，…；存在公式(7)所示关系式：

Vb＝0    (7)

通过公式(7)可以求解出b。

最后，根据b可以分解出摄像机的内参数矩阵A，并根据A^-1求解R_c，t_c：

根据b分解出摄像机的内参数具体为：将b利用Cholesky矩阵分解算法求解出A^-1，再求逆得到A；

标定每幅图像的外部参数t_c为：

r_c1＝λA^-1h₁

r_c2＝λA^-1h₂

r_c3＝r_c1×r_c2

t_c＝λA^-1h₃    (8)

其中λ＝1/||A^-1h₁||＝1/||A^-1h₂||，所以标定的外部参数中的旋转矩阵为：

R_c＝[r_c1  r_c2  r_c3]    (9)

步骤102：将三维激光雷达数据进行处理，得到三维激光雷达坐标系下靶标平面方程的参数：a、b、c、d；

具体的，三维激光雷达的坐标系如下：以三维激光雷达的俯仰旋转轴中心为原点，z轴垂直于地面向上，x轴水平向右，y轴指向三维激光雷达前方，x、y、z三轴满足右手坐标系法则。当三维激光雷达扫描到靶标时，落在靶标平面上的扫描点坐标为(x_li，y_li，z_li)；

三维激光雷达对此扫描点的读数为(ρ，α，θ)，其中ρ为距离值，α和θ分别为水平和俯仰扫描角，则该扫描点的空间坐标为：

x_li＝ρcosα，y_li＝ρsinαcosθ，z_li＝ρsinαsinθ

靶标的平面方程可以用公式(10)表示：

ax+by+cz+d＝0，且a²+b²+c²+d²＝1    (10)

将扫描点坐标代入靶标的平面方程，采用最小二乘法可以求出在三维激光雷达坐标系下靶标平面方程的参数a、b、c、d。

步骤103：根据步骤101中得到的R_c和t_c，得到摄像机坐标系原点到不同位置靶标平面的垂线方向和距离；

如图2所示，以第i个靶标平面为例，摄像机坐标系的原点O_c到第i个靶标平面的距离为λ_c，i，垂线方向为γ_c，i；三维激光雷达坐标系原点O_l到第i个靶标平面的距离为λ_l，i，垂线方向为γ_l，i；

根据步骤101中得到的R_c和t_c，摄像机坐标系原点到第i个靶标平面的垂线方向和距离为：

${\mathrm{\γ}}_{c,i}=R_{c,i}\left[\begin{array}{c}0\\ 0\\ -1\end{array}\right]=-R_{3,c,i}---\left(11\right)$

${\mathrm{\λ}}_{c,i}={\mathrm{\γ}}_{c,i}^T\·t_{c,i}---(12-1)$

R_3，c，i是旋转矩阵R_c，i的第三列，摄像机坐标系原点到不同位置靶标平面的垂线方向可以写成如下形式：

Г_c＝[γ_c，1  γ_c，2 … γ_c，n]    (12-2)

其中，Г_c为3×n矩阵，这里n为靶标平面放置次数，即“图像-距离”对数。

步骤104：根据步骤102中得到的a、b、c、d，得到三维激光雷达坐标系原点到不同位置靶标平面的垂线方向；

根据步骤102中得到的a、b、c、d，以第i个靶标平面为例，三维激光雷达坐标系原点到第i个靶标平面的垂线方向通过下式计算：

${\mathrm{\γ}}_{l,i}=\frac{1}{\sqrt{a^2+b^2+c^2}}{\left[\begin{array}{ccc}a,& b,& c\end{array}\right]}^T---\left(13\right)$

${\mathrm{\λ}}_{l,i}=\frac{\left|d_i\right|}{\sqrt{a_i^2+b_i^2+c_i^2+d_i^2}}---\left(14\right)$

三维激光雷达坐标系原点到不同位置靶标平面的垂线方向和距离可以写成如下形式：

Г_l＝[γ_l，1  γ_l，2…γ_l，n]     (15)

Λ_l＝[λ_l，1  λ_l，2…λ_l，n]^T    (16)

其中，Г_l为3×n矩阵，n为“距离-图像”对数。

步骤105：计算三维激光雷达坐标系原点到不同位置靶标平面的垂线方向的测量误差的协方差；

具体的，以第i个靶标平面为例，从三维激光雷达数据中计算所得的γ_l，i也不是靶标平面垂线方向的真值，有误差Δγ_l，i，即

(γ_l，i)_true＝γ_l，i+Δγ_l，i    (17)

Δγ_l，i是测量误差，是符合高斯分布的随机变量，位于和(γ_l，i)_true相切的平面内，则协方差为：

$E\left[{\mathrm{\Δ\γ}}_{l,i}{{\mathrm{\Δ\γ}}_{l,i}}^T\right]={\mathrm{\σ}}_{l,i}^2[I-{\left({\mathrm{\γ}}_{l,i}\right)}_{\mathrm{true}}{\left({\mathrm{\γ}}_{l,i}\right)}_{\mathrm{true}}^T]---\left(18\right)$

其中，

为三维激光雷达测量噪声的方差。

步骤106：计算各个垂线方向从三维激光雷达坐标系变换到摄像机坐标系的包含摄像机测量误差的转换关系；

具体的，根据摄像机坐标系和三维激光雷达坐标系中，原点到各个不同位置靶标平面的垂线方向，以第i个靶标平面为例，将三维激光雷达坐标系中的垂线方向转换到摄像机坐标系中，可以用下式表示：

(γ_c，i)_true＝A(γ_l，i)_true    (19)

AA^T＝I                         (20)

由于摄像机在图像采集、处理过程中存在各种各样的误差，因此实际测量结果可以用下式表示：

γ_c，i＝A(γ_l，i)_true+Δγ_c，i    (21)

其中，i＝1，2，3，...，γ_c，i是根据摄像机实际拍摄图像所计算的结果，Δγ_c，i是测量误差，是符合高斯分布的随机变量，位于和(γ_c，i)_true相切的平面内。

步骤107：对转换关系中的测量误差进行协方差计算，得到转换关系中的测量误差的协方差中含有摄像机测量噪声的方差与三维激光雷达测量噪声的方差的平方和的算式；

具体的，对公式(21)中的随机变量Δγ_c，i的协方差进行计算，得到：

$E\left[{\mathrm{\Δ\γ}}_{c,i}{{\mathrm{\Δ\γ}}_{c,i}}^T\right]={\mathrm{\σ}}_{c,i}^2[I-{\left({\mathrm{\γ}}_{c,i}\right)}_{\mathrm{true}}{\left({\mathrm{\γ}}_{c,i}\right)}_{\mathrm{true}}^T]---\left(22\right)$

其中，

为摄像机测量噪声的方差。

根据公式(17)和公式(21)，可得：

γ_c，i＝A(γ_l，i+Δγ_l，i)+Δγ_c，i    (23)

γ_c，i＝Aγ_l，i+AΔγ_l，i+Δγ_c，i     (24)

根据公式(18)和公式(22)，对上式(24)中包含随机变量的各项进行协方差计算，不难得到：

$({\mathrm{A\Δ\γ}}_{l,i}+{\mathrm{\Δ\γ}}_{c,i}){({\mathrm{A\Δ\γ}}_{l,i}+{\mathrm{\Δ\γ}}_{c,i})}^T]$

$=\mathrm{AE}\left[{\mathrm{\Δ\γ}}_{l,i}{{\mathrm{\Δ\γ}}_{l,i}}^T\right]A^T+E\left[{\mathrm{\Δ\γ}}_{c,i}{{\mathrm{\Δ\γ}}_{c,i}}^T\right]$

$={\mathrm{A\σ}}_{l,i}^2[I-{\left({\mathrm{\γ}}_{l,i}\right)}_{\mathrm{true}}{\left({\mathrm{\γ}}_{l,i}\right)}_{\mathrm{true}}^T]A^T+{\mathrm{\σ}}_{c,i}^2[I-{\left({\mathrm{\γ}}_{c,i}\right)}_{\mathrm{true}}{\left({\mathrm{\γ}}_{c,i}\right)}_{\mathrm{true}}^T]$

$={\mathrm{\σ}}_{l,i}^2[I-{\left(A{\mathrm{\γ}}_{l,i}\right)}_{\mathrm{true}}{\left({\mathrm{\γ}}_{l,i}A\right)}_{\mathrm{true}}^T]+{\mathrm{\σ}}_{c,i}^2[I-{\left({\mathrm{\γ}}_{c,i}\right)}_{\mathrm{true}}{\left({\mathrm{\γ}}_{c,i}\right)}_{\mathrm{true}}^T]$

$=({\mathrm{\σ}}_{l,i}^2+{\mathrm{\σ}}_{c,i}^2)[I-{\left({\mathrm{\γ}}_{c,i}\right)}_{\mathrm{true}}{\left({\mathrm{\γ}}_{c,i}\right)}_{\mathrm{true}}^T]---\left(25\right)$

步骤108：将得到的所有摄像机测量噪声的方差与三维激光雷达测量噪声的方差的平方和的倒数作为加权系数，标定出具有最大似然估计的旋转矩阵；

具体的，参考Shuster证明的如果加权系数a_i选为

则形如

的问题等价于最大似然估计问题。

则上式(25)中，令

则标定具有最大似然估计的旋转矩阵R的过程可以写成下面公式(26)：

$R=\arg \underset{R}{\max }\underset{i}{\mathrm{\Σ}}{a}_i{\mathrm{\γ}}_{c,i}^T\left({\mathrm{R\γ}}_{l,i}\right)=\underset{R}{\max }\mathrm{trace}\left(\mathrm{\Σ}{\mathrm{\Γ}}_c^{T}R{\mathrm{\Γ}}_l\right)=\underset{R}{\max }\mathrm{trace}\left(R{\mathrm{\Γ}}_l\mathrm{\Σ}{\mathrm{\Γ}}_c^T\right)---\left(26\right)$

其中∑为对角矩阵，对角元素分别为加权系数a_i，i＝1，2，3…，Г_l可由公式(15)得到。

求解R可以采用OPP(Orthogonal Procrustes Problem)问题的解法。不难得出：

R＝V′U′^T    (27)

其中V′和U′是

进行奇异值分解的结果，即

可选的，所述方法进一步包括步骤109：将标定的旋转矩阵作为初始值，并根据摄像机坐标系中原点到不同位置靶标平面的距离，将靶标平面的点对应摄像机拍摄的图像平面的距离作为待最小化的目标函数，通过迭代求解全局最优值；

具体的，通过上述步骤101至步骤108，求解出的旋转矩阵R的值，可以将这个值作为初始值，并根据摄像机坐标系中原点到不同位置靶标平面的距离，将靶标平面的点对应摄像机拍摄的图像平面的距离作为待最小化的目标函数，通过迭代求解全局最优值。

设x_l，i为三维激光雷达坐标系的第i幅图像中靶标平面上的点的坐标矩阵：

$x_{l,i}=\left[\begin{array}{cccc}{x}_{l,i}^{\left(0\right)}& x_{l,i}^{\left(1\right)}& \·\·\·& x_{l,i}^{\left(m\right)}\end{array}\right]---\left(28\right)$

其中m是点的数量，全局优化的目标函数可以写成如下形式：

$\arg \underset{R,t}{\min }\underset{i=1}{\overset{n}{\mathrm{\Σ}}}\frac{1}{m\left(i\right)}\underset{j=1}{\overset{m\left(i\right)}{\mathrm{\Σ}}}{({\mathrm{\γ}}_{c,i}^T({\mathrm{Rx}}_{l,i}^{\left(j\right)}+t)-{\mathrm{\λ}}_{c,i})}^2---\left(29\right)$

其中

是3×1向量，m＝m(i)是点的数量，t为三维激光雷达坐标系转换到摄像机坐标系的平移向量。

通过迭代三维激光雷达坐标系的每幅图像中靶标平面上的点的坐标，求解出优化后的R。

基于上述方法，本发明还提出基于摄像机和三维激光雷达的外部参数标定系统，如图3所示，该系统包括：第一计算模块31、旋转矩阵模块32；其中，

第一计算模块31，用于根据三维激光雷达坐标系中的原点到不同位置靶标平面的垂线方向的测量误差的协方差，以及各个垂线方向从三维激光雷达坐标系转换到摄像机坐标系的转换关系的测量误差的协方差，得到转换关系中测量误差的协方差中含有摄像机测量噪声的方差与三维激光雷达测量噪声的方差的平方和的算式；

旋转矩阵模块32，用于将第一计算模块31得到的所有摄像机测量噪声的方差与三维激光雷达测量噪声的方差的平方和的倒数，作为加权系数，标定出具有最大似然估计的旋转矩阵；

所述外部参数标定系统进一步包括第一协方差模块33、第二协方差模块34、转换关系模块35、第一垂线方向模块36、第二垂线方向模块37；其中，

第一协方差模块33，用于根据三维激光雷达坐标系原点到不同位置靶标平面的垂线方向，计算三维激光雷达坐标系原点到不同位置靶标平面的垂线方向的测量误差的协方差，并提供给第一计算模块31；

第二协方差模块34，用于计算转换关系模块35提供的转换关系中测量误差的协方差，并提供给第一计算模块31；

转换关系模块35，用于根据摄像机坐标系和三维激光雷达坐标系中的原点到不同位置靶标平面的垂线方向，计算垂线方向从三维激光雷达坐标系到摄像机坐标系的具有测量误差的转换关系；

第一垂线方向模块36，用于根据标定的靶标相对于摄像机坐标系的外部参数R_c和t_c，得到摄像机坐标系原点到不同位置靶标平面的垂线方向；

第二垂线方向模块37，用于根据三维激光雷达坐标系下的靶标平面方程的参数，得到三维激光雷达坐标系原点到不同位置靶标平面的垂线方向；

进一步的，该系统还包括第一外部参数标定模块38，用于建立靶标平面的特征点和对应图像平面上的点之间的关系，计算出摄像机的内参数矩阵，从而提供靶标相对于摄像机坐标系的外部参数R_c和t_c给第一垂线方向模块36；

进一步的，该系统还包括第一距离模块39，用于根据第一外部参数标定模块38提供的外部参数R_c和t_c，得到摄像机坐标系原点到不同位置靶标平面的距离；

进一步的，该系统还包括平面方程的参数模块40，用于进行三维激光雷达数据处理，提供三维激光雷达坐标系下靶标平面方程的参数a、b、c、d给第二垂线方向模块37；

进一步的，该系统还包括优化模块41，用于将旋转矩阵模块32标定的旋转矩阵的值作为初始值，并根据第一距离模块39得到的摄像机坐标系中原点到不同位置靶标平面的距离，将靶标平面的点对应摄像机拍摄的图像平面的距离作为待最小化的目标函数，通过迭代求解全局最优值。

以上所述，仅为本发明的较佳实施例而已，并非用于限定本发明的保护范围，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。

Claims (8)

1.一种基于摄像机和三维激光雷达的外部参数标定方法，其特征在于，该方法包括：

摄像机拍摄不同位置棋盘格靶标的图像，建立靶标平面的特征点和对应图像平面上的点之间的关系，计算出摄像机的内参数矩阵，从而标定靶标相对于摄像机坐标系的外部参数旋转矩阵和平移向量；

将三维激光雷达数据进行处理，得到三维激光雷达坐标系下靶标平面方程的参数；

根据外部参数旋转矩阵和平移向量，得到摄像机坐标系原点到不同位置靶标平面的垂线方向和距离；

根据三维激光雷达坐标系下靶标平面方程的参数，得到三维激光雷达坐标系原点到不同位置靶标平面的垂线方向；

根据三维激光雷达坐标系原点到不同位置靶标平面的垂线方向，从三维激光雷达坐标系转换到摄像机坐标系的转换关系中测量误差的协方差，并根据三维激光雷达坐标系原点到不同位置靶标平面的垂线方向的测量误差的协方差，得到转换关系中测量误差的协方差中含有摄像机测量噪声的方差与三维激光雷达测量噪声的方差的平方和的算式；

将得到的所有摄像机测量噪声的方差与三维激光雷达测量噪声的方差的平方和的倒数作为加权系数，标定出具有最大似然估计的旋转矩阵。

2.根据权利要求1所述的方法，其特征在于，所述转换关系中的测量误差的协方差的计算，具体为：根据摄像机坐标系和三维激光雷达坐标系中的原点到不同位置靶标平面的垂线方向，得到垂线方向从三维激光雷达坐标系到摄像机坐标系的具有测量误差的转换关系，然后对测量误差进行协方差计算。

3.根据权利要求1或2所述的方法，其特征在于，所述三维激光雷达坐标系原点到不同位置靶标平面的垂线方向的测量误差的协方差的计算，具体为：根据三维激光雷达坐标系原点到不同位置靶标平面的垂线方向，计算三维激光雷达坐标系原点到不同位置靶标平面的垂线方向的测量误差的协方差。

4.根据权利要求2所述的方法，其特征在于，所述计算三维激光雷达坐标系下的靶标平面方程的参数，具体为：

通过三维激光雷达扫描到靶标时，落在靶标平面上的扫描点的坐标，进行三维激光雷达数据处理，得到三维激光雷达坐标系下靶标平面方程的参数。

5.根据权利要求3所述的方法，其特征在于，所述标定靶标相对于摄像机坐标系的外部参数，具体为：通过建立靶标平面的特征点和对应图像平面上的点之间的关系，计算出摄像机的内参数矩阵，标定靶标相对于摄像机坐标系的外部参数。

6.根据权利要求1所述的方法，其特征在于，所述得到具有最大似然估计的旋转矩阵后，该方法进一步包括：将求解出的旋转矩阵的值作为初始值，并根据摄像机坐标系中原点到不同位置靶标平面的距离，将靶标平面的点对应摄像机拍摄的图像平面的距离作为待最小化的目标函数，求解旋转矩阵的全局最优值。

7.一种基于摄像机和三维激光雷达的外部参数标定系统，其特征在于，该系统包括：

第一外部参数标定模块，用于建立靶标平面的特征点和对应图像平面上的点之间的关系，计算出摄像机的内参数矩阵，提供靶标相对于摄像机坐标系的外部参数给第一垂线方向模块；

平面方程的参数模块，用于进行三维激光雷达数据处理，提供三维激光雷达坐标系下靶标平面方程的参数给第二垂线方向模块；

第一垂线方向模块，用于根据靶标相对于摄像机坐标系的外部参数，得到摄像机坐标系原点到不同位置靶标平面的垂线方向；

第二垂线方向模块，用于根据三维激光雷达坐标系下的靶标平面方程的参数，得到三维激光雷达坐标系原点到不同位置靶标平面的垂线方向；

第一距离模块，用于根据第一外部参数标定模块提供的外部参数，得到摄像机坐标系原点到不同位置靶标平面的距离；

第一计算模块，用于根据三维激光雷达坐标系中的原点到不同位置靶标平面的垂线方向的测量误差的协方差，以及各个垂线方向从三维激光雷达坐标系转换到摄像机坐标系的转换关系的测量误差的协方差，得到转换关系中测量误差的协方差中含有摄像机测量噪声的方差与三维激光雷达测量噪声的方差的平方和的算式；

旋转矩阵模块，用于将第一计算模块得到的所有摄像机测量噪声的方差与三维激光雷达测量噪声的方差的平方和的倒数，作为加权系数，标定出具有最大似然估计的旋转矩阵。

8.根据权利要求7所述的系统，其特征在于，所述系统进一步包括：

第一协方差模块，用于根据三维激光雷达坐标系原点到不同位置靶标平面的垂线方向，计算三维激光雷达坐标系原点到不同位置靶标平面的垂线方向的测量误差的协方差；

第二协方差模块，计算转换关系模块提供的转换关系中测量误差的协方差；

转换关系模块，用于根据摄像机坐标系和三维激光雷达坐标系中的原点到不同位置靶标平面的垂线方向，计算垂线方向从三维激光雷达坐标系到摄像机坐标系的具有测量误差的转换关系；

优化模块，用于将旋转矩阵模块标定的旋转矩阵的值作为初始值，并根据摄像机坐标系中原点到不同位置靶标平面的距离，将靶标平面的点对应摄像机拍摄的图像平面的距离作为待最小化的目标函数，求解旋转矩阵的全局最优值。

Images