CN106228537A - 一种三维激光雷达与单目摄像机的联合标定方法 - Google Patents

Abstract

本发明涉及一种三维激光雷达与单目摄像机的联合标定方法，属于信息融合领域。本发明是为了在保证满足多传感器信息融合联合标定精度的要求下，提高标定效率。首先对单目摄像机进行标定，然后在每个位置下分别求解出靶标标定板在单目摄像机坐标系下的法向量以及靶标标定板到摄像机坐标系原点的距离，三维激光雷达坐标系下拟合平面的法向量以及三维激光雷达坐标系下原点到拟合平面的距离，再利用距离对应性原理，求解出联合标定的结果，最后再以联合标定的结果作为初值带入到优化目标函数中，获得最优的联合标定结果。本发明大大减少了人为的参与，减小了因复杂的人为操作而带来的随机误差，既简化了多传感器联合标定的流程又提高了标定的精度。

Description

一种三维激光雷达与单目摄像机的联合标定方法

技术领域

本发明涉及一种三维激光雷达与单目摄像机的联合标定方法，尤其涉及无人运动平台环境感知领域的多传感器数据融合技术，属于信息融合领域。

背景技术

对于无人运动平台环境中的感知***，使用多传感器数据融合技术，可以减小因环境的突变对***性能的影响，提高***的鲁棒性；利用多传感器信息融合，可以获得比单一传感器更高的分辨率并且能够提高***的可靠性和容错能力。而多种传感器之间信息融合的前提就是进行联合标定。视觉传感器虽然获得的信息丰富，但特别容易受外界天气、光照等因素的影响，缺少目标物体的三维数据信息；三维激光雷达虽然能够快速的获取空间目标物体的三维数据信息，但不能获得物体纹理、颜色等信息。因此，三维激光雷达和视觉传感器在性能上能够达到良好的互补效果，但是由于这两种传感器所获得的数据都是基于各自的坐标系，而数据融合需要将这两者所采集的数据表示在同一个坐标系下，因此需要通过对它们进行联合标定来确定这两个坐标系之间的转换关系。

近年来，国内外学术界对激光雷达和单目摄像机之间的联合标定做了大量的研究工作。美国华盛顿大学机器人研究室最先提出了激光雷达与单目摄像机的标定方法，陈远等人在此基础上对其进行了改进，并且相比之下提高了标定精度。anhu Li等人也提出了使用线特征标定激光雷达与摄像机外部参数的方法并且得到了比较满意的结果。FlorianSchweiger等人采用圆形标定靶标定摄像机与激光雷达的外部位置关系，得到激光雷达在摄像机坐标系下的姿态估计。付梦印等人采用立体棋盘格通过空间点匹配进行了摄像机与激光雷达的联合标定。项志宇等人利用传感器坐标原点到靶标板的距离对应性实现了联合标定。现有的三维激光雷达和单目摄像机联合标定的方案都比较复杂，而且部分方案中空间匹配特征点难以准确提取,导致误差较大。

发明内容

本发明的目的在于提出一种三维激光雷达与单目摄像机的联合标定方法，在满足联合标定精度的前提下，简化标定步骤，提高标定效率。

本发明的目的是通过下述技术方案实现的。

一种三维激光雷达与单目摄像机的联合标定方法，具体步骤如下：

步骤一：建立单目摄像机坐标系数学模型并进行摄像机内外参数标定；得到靶标标定板分别在摄像机坐标系下的法向量p_ci和靶标标定板到摄像机坐标系原点的距离d_ci；

所建立的单目摄像机坐标系数学模型是针孔近似模型，然后，采用张正友单目摄像机标定法进行相机内外参数的标定，得到摄像机的有效焦距f、图像主点坐标(u0,v0)、尺度因子fx和fy等内参数矩阵M，以及每个位置下的正交旋转矩阵R和平移向量T等外参数，最后，根据距离对应性原理得到靶标标定板分别在摄像机坐标系下的法向量p_ci和靶标标定板到摄像机坐标系原点的距离d_ci。

p_ci＝[R_3,i,1,R_3,i,2,R_3,i,3]^T (1)

d_ci＝R_3iT_i (2)

其中，R_3,i表示的是第i幅图片下靶标标定板在单目摄像机坐标系下的正交旋转矩阵R的第三列，T_i表示的是该幅图片下靶标标定板在单目摄像机坐标系下的平移向量；

步骤二：建立三维激光雷达坐标系数学模型并进行点云数据的平面拟合，

联合标定方法中所采用的三维激光雷达的坐标系的数学模型属于右手坐标系，定义激光雷达坐标系的原点位于激光雷达内部发射器的中心，三维激光雷达的X轴和Y轴分别平行于基座的两边，其Z轴位于旋转轴上，向上代表正方向，其中Y轴的正方向代表三维激光雷达内部发射器的正方向。

得到平面方程：

U_3,l,1x+U_3,l,2y+U_3,l,3z+D＝0 (3)

其中，U_3,l表示的是奇异值分解后得到的解析解中U的第三列,D为常数；

步骤三：根据步骤二所得的平面方程(3)以及点云数据的中心坐标得出三维激光雷达坐标系下拟合平面的法向量p_l,i以及三维激光雷达坐标系下原点到靶标平面板的距离d_l,i；

p_l,i＝[U_3,l,1 U_3,l,2 U_3,l,3]^T (4)

$d_{l,i}=\left|\right|-D\left|\right|=\left|\right|-U_{3,l,1}\stackrel{\‾}{x}-U_{3,l,2}\stackrel{\‾}{y}-U_{3,l,3}\stackrel{\‾}{z}\left|\right|---\left(5\right)$

步骤四：三维激光雷达与单目摄像机联合标定的结果求解；根据步骤一所得的表达式(1)和(2)以及步骤三所得的表达式(4)和(5)，以及距离对应性原理能够知道激光雷达坐标系到单目摄像机坐标系下的旋转矩阵R_LC和平移向量T_LC；

$\begin{array}{c}{R}_{LC}=\arg \max \underset{i=1}{\overset{n}{\mathrm{\Σ}}}\left({P_c}^T{R}_{LC}{P}_l\right)x=\arg \max tr\left({P_c}^T{R}_{LC}{P}_l\right)\\ =\arg \max tr\left(R_{LC}{P}_l{P_c}^T\right)\end{array}---\left(6\right)$

T_LC＝(P_cP_c ^T)^-1P_c(d_c-d_l) (7)

其中，公式(6)、(7)中P_c表示的是靶标标定板在单目摄像机坐标系下的法向量组成的矩阵，P_l表示的是靶标标定板在三维激光雷达坐标系下的法向量组成的矩阵，d_c表示的是单目摄像机坐标系原点到靶标标定板的距离组成的向量，d_l表示的是三维激光雷达坐标系原点到靶标标定板的距离组成的向量；

步骤五：三维激光雷达与单目摄像机联合标定的流程简化

靶标标定板均匀分布在单目摄像机视场内的不同位置且平面法向量均匀分布在八个象限内，以便在保证样本质量的情况下减少样本数量；所述靶标标定板摆放位置必须覆盖整个摄像机视野，并且每个位置摆放的靶标平面板的方向不能相同。

在每个位置下，分别先通过步骤一求解出单目摄像机的内参数矩阵和靶标标定板在单目摄像机坐标系下的法向量p_ci以及靶标标定板到摄像机坐标系原点的距离d_ci，再通过步骤三分别求解出三维激光雷达坐标系下拟合平面的法向量p_l以及三维激光雷达坐标系下原点到靶标平面板的距离d_l，然后将每个位置下两个坐标系的法向量分别组成如下矩阵表达式

P_c＝[p_c1 p_c2 ... p_cn] (8)

P_l＝[p_l1 p_l2 ... p_ln] (9)

将每个位置下的两个坐标系下原点到靶标标定板的距离分别组成如下向量表达式

d_c＝[d_c1 d_c2 ... d_cn]^T (10)

d_l＝[d_l1 d_l2 ... d_ln]^T (11)

最后，再将上述P_c、d_c、P_l、d_l代入到步骤四的公式(6)和(7)中，求解出联合标定的结果R_LC与T_LC；

步骤六：对步骤五所得的联合标定的结果R_LC和T_LC进行优化，得到最优解；

首先将标定结果R_LC、T_LC作为初值，带入目标函数表达式(12)中

$agrmin\underset{i=1}{\overset{m}{\Σ}}\frac{1}{mi}\underset{j=1}{\overset{mi}{\Σ}}\[p_{ci}^T(R_{LC}{x}_{il}^j+T_{LC})-d_{ci}\]---\left(12\right)$

公式(12)中：x_l,i为在第i幅距离图像中靶标平面板的3维点云矩阵， m＝m_i是第i幅距离图像的平面点数量；经过迭代过程，得到最优解R'_LC与T'_LC。

步骤五所述的靶标标定板摆放位置的数量至少为5个。

步骤二所述的平面拟合的方法采用的是最小二乘法来对三维激光雷达获得的靶标平面点云数据进行平面拟合的，其步骤主要包括：

1)首先准确提取出靶标平面板在三维激光雷达坐标系下的激光点云数据，将其放在一个文件夹下，计算出点云中心坐标然后在对整个平面板点云进行下一步处理；

2)将1)中所获得的点云数据与中心坐标值进行求差，然后再对其分别进行奇异值分解；

3)将2)中奇异值分解后得到的解析解，通过线性代数和几何学知识解算出平面点云拟合的平面方程:

U_3,l,1x+U_3,l,2y+U_3,l,3z+D＝0 (3)

有益效果

(1)与现有的多传感器数据融合技术中单目摄像机和激光雷达联合标定的方法相比，本发明的三维激光雷达与单目摄像机联合标定的方法，能够在保证满足联合标定精度的要求下，大大简化标定流程，极大地提高标定速度。

(2)所述的三维激光雷达与单目摄像机联合标定的方法，操作更加简便迅捷、可以大大降低因复杂的人工参与和繁琐的标定工具所带来的随机误差，极大地提高了标定效率。

(3)本发明中三维激光雷达与单目摄像机联合标定采用棋盘格作为靶标平面板，与其他利用空间特征点匹配算法相比，具有不受激光雷达混合像素影响和特征点提取误差较大的优点，可靠性较高，操作性较强。

附图说明

图1为单目摄像机坐标系O_cX_cY_cZ_c和图像像素坐标系uov以及图像物理坐标系xoy之间的关系；

图2为三维激光雷达坐标系O_lX_lY_lZ_l；

图3为三维激光雷达坐标系O_lX_lY_lZ_l、单目摄像机坐标系O_cX_cY_cZ_c和靶标标定板坐标系O_wX_wY_wZ_w之间的关系。

具体实施方式

下面结合附图和实施例对本发明进行详细描述。

一种三维激光雷达与单目摄像机的联合标定方法的主要步骤如下：

步骤一：建立单目摄像机坐标系数学模型并进行摄像机内外参数标定；

该方法中所采用的单目摄像机坐标系数学模型是针孔近似模型，如图1所示。首先，采用张正友单目摄像机标定法对相机的内参数进行标定，得到摄像机的有效焦距f、图像主点坐标(u0,v0)、尺度因子fx和fy等内参数矩阵M，以及每个位置下的正交旋转矩阵R和平移向量T等外参数。设空间某点的摄像机坐标为P(x_c,y_c,z_c)，其图像物理坐标为(x,y)，像素坐标为(u,v)，摄像机焦距为f，主点的像素坐标为(u₀,v₀)，dx,dy分别为x,y轴方向上单位像素间的物理距离，则由小孔成像原理可得图像像素坐标与摄像机三维坐标之间的关系为：

$\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]=\left[\begin{array}{ccc}\frac{f}{dx}& 0& u_0\\ 0& \frac{f}{dy}& v_0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}\frac{x_c}{z_c}\\ \frac{y_c}{z_c}\\ 1\end{array}\right]=M\left[\begin{array}{c}\frac{x_c}{z_C}\\ \frac{y_c}{z_c}\\ 1\end{array}\right]---\left(1\right)$

其中矩阵M为单目摄像机的内参数矩阵。

然后，针对具体的第i幅图像，计算出摄像机坐标系中靶标标定板的法向量p_c,i以及靶标标定板到摄像机坐标系原点的距离d_ci，具体算法如下：

(1)假设R_3i代表指定的摄像机内参数下外部旋转矩阵R_3i的第三列，则根据线性代数和几何学的知识可知，R_3i代表的就是该幅图片下，靶标标定板在单目摄像机坐标系下的法向量方向，并将其记为p_c,i；

p_ci＝[R_3,i,1,R_3,i,2,R_3,i,3]^T (2)

(2)根据(1)得到的R_3i和步骤一得到的标定后的外部平移向量T_i，就可以得到靶标标定板平面到单目摄像机坐标系原点的距离，并将该距离记为d_ci；

d_ci＝R_3iT_i (3)

步骤二：建立三维激光雷达坐标系数学模型并进行点云数据的平面拟合；

联合标定方法中所采用的三维激光雷达的坐标系的数学模型属于右手坐标系，定义激光雷达坐标系的原点位于激光雷达内部发射器的中心，三维激光雷达的X轴和Y轴分别平行于基座的两边，其Z轴位于旋转轴上，向上代表正方向，其中Y轴的正方向代表三维激光雷达内部发射器的正方向，如图2所示。

所述平面拟合的方法采用的是最小二乘法来对三维激光雷达获得的靶标平面点云数据进行平面拟合的，其步骤主要包括：

(1)首先准确提取出靶标平面板在三维激光雷达坐标系下的激光点云数据，然后对这些点云数据进行噪声处理，以处理掉飞点等带有随机性的误差数据，最后将剩下的比较可靠、真实的点云数据保存下来，并计算出这些数据的中心点坐标值然后再对整个平面板点云进行下一步处理。

(2)将(1)中所获得的处理后的点云数据与中心坐标值进行求差，然后在对其进行奇异值分解；设点云数据为(x_i,y_i,z_i),则

$C_{li}=(x_{li},y_{li},z_{li})-(\stackrel{\‾}{x},\stackrel{\‾}{y},\stackrel{\‾}{z})---\left(4\right)$

[U_l,i,S_l,i,V_l,i]＝C_li ^T×C_li (5)

(3)将(2)中奇异值分解后得到的解析解，通过线性代数和几何学知识解算出平面点云拟合的平面方程为：

U_3,l,1x+U_3,l,2y+U_3,l,3z+D＝0 (6)

其中，U_3,l表示的是奇异值分解后得到的解析解中U的第三列,D为常数；

步骤三：根据步骤二所得的平面方程(6)以及点云数据的中心坐标计算出第i个位置下三维激光雷达坐标系下拟合平面的法向量p_l,i以及三维激光雷达坐标系下原点到靶标平面板的距离d_l,i；

根据步骤二和线性代数知识可知，奇异值分解后得到的解析解中的第三列代表的就是拟合平面在三维激光雷达坐标系下的法向量P_l,i，将点云平面的中心点坐标带入到平面拟合的平面方程中，就可以求出三维激光雷达坐标系下原点到靶标平面板的距离d_l,i；

p_l,i＝[U_3,l,1 U_3,l,2 U_3,l,3]^T (7)

$d_{l,i}=\left|\right|-D\left|\right|=\left|\right|-U_{3,l,1}\stackrel{\‾}{x}-U_{3,l,2}\stackrel{\‾}{y}-U_{3,l,3}\stackrel{\‾}{z}\left|\right|---\left(8\right)$

步骤四：三维激光雷达与单目摄像机联合标定的结果求解；

联合标定中各坐标系关系如图3所示，其中O_wX_wY_wZ_w为世界坐标系，令世界坐标系与靶标标定板坐标系重合，原点定义在靶标标定板的一个顶点，X轴、Y轴分别平行于靶标标定板的两边，Z轴垂直于靶标标定板平面,O_cX_cY_cZ_c为单目摄像机坐标系，O_lX_lY_lZ_l为三维激光雷达坐标系，T为三维激光雷达坐标系与单目摄像机坐标系之间的平移向量，在两个坐标系下靶标标定板的法向量分别为p_c和p_l，两坐标系原点到标定板平面的距离分别为d_c和d_l，具体算法如下:

三维激光雷达坐标系通过乘以一个旋转矩阵R_LC后，其原点到靶标平面的法向方向，理论上应该与单目摄像机坐标系原点到靶标平面的法向方向平行，即可以转化为寻找使转换后的二者法向量方向之间角度偏差最小化的目标函数。该目标函数满足相应角度余弦之和最大，因此可以利用该理论解得R_LC，即：

$\begin{array}{c}{R}_{LC}=\arg \max \underset{i=1}{\overset{n}{\mathrm{\Σ}}}\left({P_c}^T{R}_{LC}{P}_l\right)x=\arg \max tr\left({P_c}^T{R}_{LC}{P}_l\right)\\ =\arg \max tr\left(R_{LC}{P}_l{P_c}^T\right)\end{array}---\left(9\right)$

其解析解为R_LC'＝VU^T,其中USV^T＝P_lP_c ^T为矩阵奇异值分解，R_LC为同时满足R_LC ^TR_LC＝I和detR_LC＝1的正交矩阵。

根据二者之间的坐标关系可以知道，单目摄像机坐标系原点与激光雷达坐标系原点到靶标标定板平面的距离差为

P_ciT_LC＝||d_ci-d_li|| (10)

取距离目标函数

$min\underset{i=1}{\overset{n}{\Σ}}{\[{P_{ci}}^T{T}_{LC}-\left(\right||d_{ci}-d_{li}|\left|\right)\]}^2---\left(11\right)$

利用最小二乘法解得公式(11)中的解析解得

T_LC＝(P_cP_c ^T)^-1P_c(d_c-d_l) (12)

其中，公式(9)、(10)、(11)、(12)中P_c表示的是靶标标定板在单目摄像机坐标系下的法向量组成的矩阵P_c＝[p_c1 p_c2 ... p_cn]，P_l表示的是靶标标定板在三维激光雷达坐标系下的法向量组成的矩阵P_l＝[p_l1 p_l2 ... p_ln]，d_c表示的是单目摄像机坐标系原点到靶标标定板的距离组成的向量d_c＝[d_c1 d_c2 ... d_cn]^T，d_l表示的是三维激光雷达坐标系原点到靶标标定板的距离组成的向量d_l＝[d_l1 d_l2 ... d_ln]^T，T_LC表示的是三维激光雷达坐标系与单目摄像机坐标系之间的平移向量，与图3所示中的R相同；

步骤五：三维激光雷达与单目摄像机联合标定的流程简化

靶标标定板均匀分布在单目摄像机视场内且平面法向量均匀分布在八个象限内，以便在保证样本质量的情况下减少样本数量，所述靶标标定板摆放位置的数量至少为5个，这5个位置必须覆盖整个摄像机视野，并且每个位置摆放的靶标平面板的方向不能相同。

根据数学理论的空间几何和矩阵知识可以知道，3个不共线的点成等边三角形分布时，平面拟合的抗噪效果最好，因此当三个平面位置的法向量成等夹角分布时，所求的解析解精度最高。但是，在实际的标定试验中，标定板成3个等夹角位置摆放操作时，特别容易引入随机误差，因此可以选择摆放5个标定位置，使标定板均匀地分布在单目摄像机视场内且平面法向量均匀分布在八个象限内，这5个位置必须覆盖整个摄像机视野，并且每个位置摆放的靶标平面板的方向不能相同，这样就可以仅仅通过使用一个标定板，均匀摆放在5个不同的位置。在每个位置下，分别先通过步骤一求解出单目摄像机的内参数矩阵和靶标标定板在单目摄像机坐标系下的法向量p_ci以及靶标标定板到摄像机坐标系原点的距离d_ci，通过步骤三分别求解出三维激光雷达坐标系下拟合平面的法向量p_li以及三维激光雷达坐标系下原点到靶标平面板的距离d_li，按照这个流程，总共计算出上述5个位置下的相关数据，依次得到靶标标定板在单目摄像机坐标系下的法向量矩阵P_c以及靶标标定板到摄像机坐标系原点的距离向量d_c和三维激光雷达坐标系下拟合平面的法向量矩阵P_l以及三维激光雷达坐标系下原点到靶标平面板的距离向量d_l；

P_c＝[p_c1 p_c2 p_c3 p_c4 p_c5] (13)

P_l＝[p_l1 p_l2 p_l3 p_l4 p_l5] (14)

将这5个位置下的两个坐标系下原点到靶标标定板的距离分别组成如下向量表达式

d_c＝[d_c1 d_c2 d_c3 d_c4 d_c5]^T (15)

d_l＝[d_l1 d_l2 d_l3 d_l4 d_l5]^T (16)

最后，再将上述P_c、d_c、P_l、d_l代入到步骤四的公式(9)和(12)中，求解出联合标定的结果R_LC与T_LC；

这样仅仅通过将一个靶标标定板均匀摆放在5个不同方向的不同位置，就能实现满足精度要求的联合标定，大大简化了联合标定的流程。

步骤六：对步骤五所得的联合标定的结果R_LC和T_LC进行优化，得到最优解；

首先将标定结果R_LC、T_LC作为初值，带入目标函数表达式(17)中

$agrmin\underset{i=1}{\overset{m}{\Σ}}\frac{1}{mi}\underset{j=1}{\overset{mi}{\Σ}}\[p_{ci}^T(R_{LC}{x}_{il}^j+T_{LC})-d_{ci}\]---\left(17\right)$

公式(12)中：x_l,i为在第i幅距离图像中靶标平面板的3维点云矩阵， m＝m_i是第i幅距离图像的平面点数量；经过迭代过程，得到最优解R'_LC与T'_LC。

综上所述，本发明的目的是为了在保证满足多传感器信息融合联合标定精度的要求下，简化标定流程，提高标定效率，具有很强的操作性和可靠性。

Claims (5)

1.一种三维激光雷达与单目摄像机的联合标定方法，其特征在于：具体步骤如下：

步骤一：建立单目摄像机坐标系数学模型并进行摄像机内外参数标定；得到靶标标定板分别在摄像机坐标系下的法向量p_ci和靶标标定板到摄像机坐标系原点的距离d_ci；

p_ci＝[R_3,i,1,R_3,i,2,R_3,i,3]^T (1)

d_ci＝R_3iT_i (2)

其中，R_3,i表示的是第i幅图片下靶标标定板在单目摄像机坐标系下的正交旋转矩阵R的第三列，T_i表示的是该幅图片下靶标标定板在单目摄像机坐标系下的平移向量；

步骤二：建立三维激光雷达坐标系数学模型并进行点云数据的平面拟合，得到平面方程：

U_3,l,1x+U_3,l,2y+U_3,l,3z+D＝0 (3)

其中，U_3,l表示的是奇异值分解后得到的解析解中U的第三列,D为常数；

步骤三：根据步骤二所得的平面方程(3)以及点云数据的中心坐标得出三维激光雷达坐标系下拟合平面的法向量p_l,i以及三维激光雷达坐标系下原点到靶标平面板的距离d_l,i；

p_l,i＝[U_3,l,1 U_3,l,2 U_3,l,3]^T (4)

步骤四：三维激光雷达与单目摄像机联合标定的结果求解；根据步骤一所得的表达式(1)和(2)以及步骤三所得的表达式(4)和(5)，以及距离对应性原理能够知道激光雷达坐标系到单目摄像机坐标系下的旋转矩阵R_LC和平移 向量T_LC；

T_LC＝(P_cP_c ^T)^-1P_c(d_c-d_l) (7)

其中，公式(6)、(7)中P_c表示的是靶标标定板在单目摄像机坐标系下的法向量组成的矩阵，P_l表示的是靶标标定板在三维激光雷达坐标系下的法向量组成的矩阵，d_c表示的是单目摄像机坐标系原点到靶标标定板的距离组成的向量，d_l表示的是三维激光雷达坐标系原点到靶标标定板的距离组成的向量；

步骤五：三维激光雷达与单目摄像机联合标定的流程简化

靶标标定板均匀分布在单目摄像机视场内的不同位置且平面法向量均匀分布在八个象限内，以便在保证样本质量的情况下减少样本数量；所述靶标标定板摆放位置必须覆盖整个摄像机视野，并且每个位置摆放的靶标平面板的方向不能相同。

在每个位置下，分别先通过步骤一求解出单目摄像机的内参数矩阵和靶标标定板在单目摄像机坐标系下的法向量p_ci以及靶标标定板到摄像机坐标系原点的距离d_ci，再通过步骤三分别求解出三维激光雷达坐标系下拟合平面的法向量p_l以及三维激光雷达坐标系下原点到靶标平面板的距离d_l，然后将每个位置下两个坐标系的法向量分别组成如下矩阵表达式

P_c＝[p_c1 p_c2 ... p_cn] (8)

P_l＝[p_l1 p_l2 ... p_ln] (9)

将每个位置下的两个坐标系下原点到靶标标定板的距离分别组成如下向量表达式

d_c＝[d_c1 d_c2 ... d_cn]^T (10)

d_l＝[d_l1 d_l2 ... d_ln]^T (11)

最后，再将上述P_c、d_c、P_l、d_l代入到步骤四的公式(6)和(7)中，求解出联合标定的结果R_LC与T_LC；

步骤六：对步骤五所得的联合标定的结果R_LC和T_LC进行优化，得到最优解；

首先将标定结果R_LC、T_LC作为初值，带入目标函数表达式(12)中

公式(12)中：x_l,i为在第i幅距离图像中靶标平面板的3维点云矩阵，m＝m_i是第i幅距离图像的平面点数量；经过迭代过程，得到最优解R'_LC与T'_LC。

2.如权利要求1所述的一种三维激光雷达与单目摄像机的联合标定方法，其特征在于：步骤五所述的靶标标定板摆放位置的数量至少为5个。

3.如权利要求1所述的一种三维激光雷达与单目摄像机的联合标定方法，其特征在于：步骤一所述的得到靶标标定板分别在摄像机坐标系下的法向量p_ci和靶标标定板到摄像机坐标系原点的距离d_ci的方法为：所建立的单目摄像机坐标系数学模型是针孔近似模型，然后，采用张正友单目摄像机标定法进行相机内外参数的标定，得到摄像机的有效焦距f、图像主点坐标(u0,v0)、尺度因子fx和fy等内参数矩阵M，以及每个位置下的正交旋转矩阵R和平移向量T等外参数，最后，根据距离对应性原理得到靶标标定板分别在摄像机坐标系下的法向量p_ci和靶标标定板到摄像机坐标系原点的距离d_ci。

4.如权利要求1所述的一种三维激光雷达与单目摄像机的联合标定方法，其特征在于：步骤二所述的三维激光雷达坐标系数学模型的建立方法为：联合 标定方法中所采用的三维激光雷达的坐标系的数学模型属于右手坐标系，定义激光雷达坐标系的原点位于激光雷达内部发射器的中心，三维激光雷达的X轴和Y轴分别平行于基座的两边，其Z轴位于旋转轴上，向上代表正方向，其中Y轴的正方向代表三维激光雷达内部发射器的正方向。

5.如权利要求1所述的一种三维激光雷达与单目摄像机的联合标定方法，其特征在于：步骤二所述的平面拟合的方法采用的是最小二乘法来对三维激光雷达获得的靶标平面点云数据进行平面拟合的，其步骤主要包括：

1)首先准确提取出靶标平面板在三维激光雷达坐标系下的激光点云数据，将其放在一个文件夹下，计算出点云中心坐标然后在对整个平面板点云进行下一步处理；

2)将1)中所获得的点云数据与中心坐标值进行求差，然后再对其分别进行奇异值分解；

3)将2)中奇异值分解后得到的解析解，通过线性代数和几何学知识解算出平面点云拟合的平面方程:

U_3,l,1x+U_3,l,2y+U_3,l,3z+D＝0 (3)。