一种多激光雷达和多相机联合标定方法及系统与流程

1.本发明属于传感器标定技术领域，具体涉及一种多激光雷达和多相机联合标定方法及系统。
现有技术
2.激光雷达和相机的耦合需要对传感器进行精确的联合标定，保证不同传感器对同一物体的感知数据在同一坐标系中可以融合。
3.目前激光雷达和相机标定主要是对单激光雷达和单相机进行标定，很少工作研究多激光雷达和多相机的标定。单激光雷达和单相机标定主要通过激光雷达和相机同时拍摄一张平面棋盘格靶标,然后根据激光雷达和相机之间的相对位置关系作为几何约束,从而确定激光雷达和相机之间的外参参数。但是这种标定不适用于多激光雷达与多相机的标定，因为不能保证所有传感器可以同时观测到同一个棋盘格靶标。有学者分别标定激光雷达和不同相机，从而得到激光雷达和不同相机的外参参数，但是这种方法只能保证激光雷达和相机之间外参参数的精度，如果根据激光雷达和不同相机之间的外参来计算不同相机之间的外参参数，则会引起较大误差。此外，上述激光雷达和相机之间相互标定方法要求天气良好、视野清楚，对标定场景要求较高。


技术实现要素：

4.鉴于上述问题和缺陷，本发明的目的在于提供一种多激光雷达和多相机联合标定方法，该方法能够实现对多激光雷达和多相机的标定；另外，该方法联合标定时加入回环约束，要求不同传感器组构成一个回环时，回环的旋转矩阵为单位阵，平移矩阵为零矩阵,从而保证任意两个传感器之间的外参参数的精度。
5.为实现上述目的，本发明采用如下技术方案：
6.一种多激光雷达和多相机联合标定方法，该方法包括以下步骤：
7.步骤s1、放置棋盘格靶标，通过调整棋盘格靶标角度及高度，使得棋盘格靶标可以完整出现在至少三个传感器视野中；
8.步骤s2、选择三个可同时观测到完整棋盘格靶标的传感器进行组合标定；启动传感器,提取棋盘格靶标角点；其中，三个传感器分别记为传感器sa,传感器sb以及传感器sc；
9.步骤s3、对组合标定的三个传感器进行识别，根据传感器类型选择不同传感器之间计算外参矩阵的方法，以求解传感器sb和其他两个传感器sa、sc的外参参数；
10.如传感器sa为激光雷达传感器，传感器sb为相机传感器，则非同源传感器之间的外参矩阵的计算公式如下：
[0011][0012]
式中：w
i
为棋盘格靶标角点i的误差权重，λ为比例系数，数值大小等于点在相机传感器sb构成的相机空间直角坐标系下的z轴坐标的倒数，为激光雷达传感器sa到相机传
感器sb的外参矩阵，为激光雷达传感器sa扫描得到的棋盘格靶标角点在相机传感器sb构成的像素平面坐标系下的坐标，x
ai
为棋盘格靶标角点在激光雷达传感器sa构成的激光雷达空间直角坐标系下的坐标；x
bi
为棋盘格靶标角点在相机传感器sb构成的像素平面坐标系下的坐标；为相机传感器sb相应的内参矩阵；
[0013]
步骤s4、计算基于多观测值的棋盘格靶标角点的坐标累积误差
[0014]
根据步骤s3得到的外参参数，通过矩阵相乘，得到传感器sa到传感器sc的外参参数；根据获取的外参参数计算传感器sa获取的棋盘格靶标角点,通过转换在传感器sc构成的像素平面坐标系下或激光雷达空间直角坐标系下的坐标，计算通过转换获取的坐标与传感器sc获取的相应的棋盘格靶标角点的欧式距离的加权平方和，从而得到由传感器sa与传感器sb之间的标定误差，以及传感器sb与传感器sc之间标定误差，引起的传感器sa与传感器sc之间标定误差；
[0015]
步骤s5、移动棋盘格靶标，按照步骤s1、步骤s2重新选择三个传感器进行组合标定，之后按照步骤s3、步骤s4计算当前组观测值下棋盘格靶标角点的坐标累积误差，直到完成所有传感器的标定；
[0016]
步骤s6、联合优化不同位置的棋盘格靶标角点的坐标累积误差，使得总的标定误差最小；同时，由于不同传感器组构成一个回环，回环的旋转矩阵应为单位阵，平移矩阵应为零矩阵。
[0017]
作为本发明的优选，当某一位置的棋盘格靶标可被三个以上的传感器同时观测到时，则不需要移动棋盘格靶标增加标定观测的位置，只需选择多个传感器组合进行联合标定，此时该位置的标定误差为多个传感器组合标定误差之和。
[0018]
作为本发明的优选，当所述传感器为激光雷达传感器时，步骤s2提取棋盘格靶标角点的具体步骤如下：
[0019]
步骤s2.1、同时启动所有激光雷达传感器，等到激光雷达传感器运行稳定后，获取各个激光雷达传感器得到的棋盘格靶标放置在指定位置的点云数据；
[0020]
步骤s2.2、调整指定位置的棋盘格靶标的角度，再次启动所有激光雷达传感器，得到不同角度的棋盘格靶标的点云数据；
[0021]
步骤s2.3、分别提取步骤s2.1和步骤s2.2中不同激光雷达传感器获取的点云数据中棋盘格角点在激光雷达空间直角坐标系下的坐标，具体步骤如下：
[0022]
步骤s2.3.1、对组成点云数据的每条扫描线，逐点计算相邻两个点的距离和角度，将一条扫描线分割成不同聚类，数学公式如下：
[0023][0024]
公式中：i为点云的序号，[x
i
‑1，y
i
‑1，z
i
‑1]，[x
i
，y
i
，z
i
],[x
i+1
，y
i+1
，z
i+1
]分别是激光点i
‑
1，i，i+1在激光雷达空间直角坐标系下的坐标，d
i
是激光点i与激光点i
‑
1在激光雷达空间直角坐标系下的欧式距离，d
t
表示激光点i与激光点i
‑
1在激光雷达空间直角坐标系下的欧式距离的阈值，δ表示激光点i与激光点i
‑
1构成的向量与激光点i与激光点i+1构成的向量之间余弦值的阈值；d
t
，δ根据激光雷达的角度分辨率计算；
[0025]
步骤s2.3.2、统计步骤s2.3.1得到的每个聚类中点云的个数，剔除点云个数小于阈值的聚类；
[0026]
步骤s2.3.3、根据特征向量之间的余弦相似度，合并不同扫描线分割的结果；
[0027]
步骤s2.3.4、根据步骤s2.3.3得到的点云聚类结果，检测点云聚类中是否存在棋盘格靶标；
[0028]
步骤s2.3.5、如果步骤s2.3.4判断得到表示棋盘格靶标的点云聚类，求棋盘格靶标上角点在激光雷达空间直角坐标系下的坐标。
[0029]
作为本发明的优选，当所述传感器为相机传感器时，步骤s2提取棋盘格靶标角点的具体步骤如下：
[0030]
步骤s2.1、同时启动所有相机传感器，等到相机传感器运行稳定后，获取各个相机传感器得到的棋盘格靶标放置在指定位置的图像；
[0031]
步骤s2.2、调整指定位置的棋盘格靶标的角度，再次启动所有相机传感器，得到不同角度的棋盘格靶标的图像；
[0032]
步骤s2.3、分别提取步骤s2.1和步骤s2.2中不同相机传感器获取的图像中棋盘格靶标角点在像素平面坐标系下的坐标，具体步骤如下：
[0033]
步骤s2.3.1、根据已知的畸变参数，对图像进行畸变校正，然后对畸变校正后的图像f(x，y)进行高斯平滑滤波处理，减少图像的噪声，得到平滑后的图像g(x，y)，数学公式如下：
[0034][0035]
公式中：表示卷积操作，σ2表示函数的宽度参数,控制函数的径向作用范围；h(x，y)表示高斯平滑滤波核函数；(x，y)表示点在像素平面坐标系下的像素坐标，其中x的值表示点在像素平面坐标系u轴方向上的投影距离，y的值表示点在像素平面坐标系v轴方向上的投影距离，exp表示以自然常数e为底的指数函数；
[0036]
步骤s2.3.2、对步骤s2.3.1得到的平滑后的图像g(x，y)根据下述公式构建hessian矩阵：
[0037][0038]
公式中：g
xx
g
xy
及g
yy
分别是图像g(x，y)相对于x,y的二阶偏导数；
[0039]
步骤s2.3.3、分解步骤s2.3.2得到的hessian矩阵，求取hessian矩阵的特征值，数学公式如下：
[0040][0041]
公式中：λ1，λ2表示hessian矩阵两个特征值，d为计算hessian矩阵特征值的中间临时变量；
[0042]
步骤s2.3.4、根据hessian矩阵的性质，hessian矩阵两个特征值λ1，λ2在数值上表
示图像灰度的二阶导数的极大值和极小值，取形状算子s(x,y)＝λ1×
λ2,当s取极小值时的(x0，y0)坐标即为棋盘格靶标角点的坐标，数学公式如下：
[0043][0044]
公式中：表示(x0，y0)坐标处的形状算子，min表示取最小值的操作，局部范围的最小值即为极小值；
[0045]
步骤s2.3.5、对步骤s2.3.4得到的每一个棋盘格靶标角点c遍历周围局部区域的像素，求每个像素的图像梯度向量与该像素到角点的向量的乘积的平方和；利用梯度下降优化算法优化棋盘格靶标角点的像素坐标使得得到的乘积的和最小；数学公式如下：
[0046][0047]
公式中：c
′
为步骤s2.3.4得到的像素坐标，表示像素坐标c
′
的局部邻域，表示像素p属于像素坐标c
′
的局部邻域，为像素p的图像梯度向量g
p
的转置矩阵，argmin为求最小的操作，c为优化后的棋盘格靶标角点的像素坐标；
[0048]
步骤s2.3.6、步骤s2.3.5得到的棋盘格靶标角点坐标即为像素平面坐标系下的棋盘格靶标角点坐标。
[0049]
作为本发明的优选，当组合标定的传感器中传感器sa与传感器sb为激光雷达传感器时，步骤s3在求解传感器sb与传感器sa的外参参数时，采用梯度下降算法迭代优化传感器sb与传感器sa的外参矩阵；
[0050]
其中，激光雷达传感器sa到激光雷达传感器sb的外参矩阵的计算公式如下：
[0051][0052]
式中，w
i
为棋盘格靶标角点i的误差权重，为激光雷达传感器sa到激光雷达传感器sb的外参矩阵，为由激光雷达传感器sa扫描得到的棋盘格靶标角点在激光雷达传感器sb构成的激光雷达空间直角坐标系的坐标，x
ai
为棋盘格靶标角点在激光雷达传感器sa构成的激光雷达空间直角坐标系下的坐标；x
bi
为棋盘格靶标角点在激光雷达传感器sb构成的激光雷达空间直角坐标系下的坐标。
[0053]
作为本发明的优选，当组合标定的传感器中传感器sa与传感器sb为相机传感器时，步骤s3在求解传感器sb与传感器sa的外参参数时，采用梯度下降算法迭代优化传感器sb与传感器sa的外参矩阵；其中，相机传感器sa以及sb相应的内参矩阵已知，相机传感器sa的尺度因子λ
a
可以根据棋盘格靶标正方形的尺寸以及相邻两个棋盘格靶标角点在相机传感器sa构成的像素平面坐标系下的距离求得,相机传感器sb的尺度因子λ
b
可以根据棋盘格靶标正方形的尺寸以及相邻两个棋盘格靶标角点在相机传感器sb构成的像素平面坐标系下的距离求得；
[0054]
相机传感器sb到相机传感器sa的外参矩阵的计算公式如下：
[0055][0056]
式中，为的逆矩阵，w
i
为棋盘格靶标角点i的误差权重，为相机传感器sb到相机传感器sa的外参矩阵；为相机传感器sb拍摄得到的棋盘格靶标角点在相机传
感器sa构成的像素平面坐标系下的坐标，为相机传感器sa获取的相应棋盘格靶角点坐标，为相机传感器sb获取的相应棋盘格靶角点坐标，λ为比例因子，等于λ
a
与λ
b
的比值。
[0057]
作为本发明的优选，步骤s6联合优化时，计算总的标定误差的计算公式如下：
[0058]
e
total
＝min(w1e
p1
+w2e
p2
+w3e
p3
+
…
+w
n
e
pn
)
[0059]
式中，w
n
为第n组传感器组合标定误差的权重，可以通过计算不同传感器组合的测距精度或分辨率设定，传感器的测距精度或分辨率越高，其所对应的标定误差权重越大；传感器的测距精度或分辨率越低，其所对应的标定误差权重越小；e
pn
为第n组传感器组合时由传感器sa与传感器sb之间的标定误差，以及传感器sb与传感器sc之间标定误差，引起的传感器sa与传感器sc之间标定误差。
[0060]
作为本发明的进一步优选，步骤s2.3.5根据点云聚类计算棋盘格靶标的角点在激光雷达空间直角坐标系下的坐标时，具体步骤如下：
[0061]
a)建立点云棋盘格坐标系及棋盘格靶标坐标系，将点云聚类的所有点投影到点云棋盘格坐标系xoy平面；此时，经处理得到的点云棋盘格坐标系xoy平面不能与棋盘格靶标坐标系重合；
[0062]
b)根据点云聚类中每个点的灰度值，建立灰度值直方图；根据直方图最大的两个峰值r
l
和r
h
，求出用于判断某个点对应的颜色的阈值范围[τ
l
，τ
h
]；即如果一个点的灰度值大于上边界τ
h
，该点表示白色；如果一个点的灰度值小于下边界τ
l
，该点表示黑色，数学公式如下：
[0063][0064]
公式中：i
i
为点云i的灰度值，c
i
为该点表示的颜色，其中0表示白色，1表示黑色；
[0065][0066]
公式中：ε
g
为一个大于2的常数，通常取4；
[0067]
c)根据已知的棋盘格的类型、每个正方形的尺寸l
grid
以及棋盘格靶标左下角正方形的颜色c
(0，0)
，计算每个正方形的颜色以及该正方形左下角及右上角在棋盘格靶标坐标系的坐标，数学公式如下：
[0068][0069]
公式中：i，j表示每个正方形的行列号，c
(i，j)
表示行列号分别为i，j的正方形的颜色，n
c
，n
r
分别为每行/每列正方形的个数；
[0070]
d)计算点云聚类中每个点的损失值；
[0071]
e)根据powell共轭方向集方法对步骤d)计算得到的点云聚类中所有点的损失值cm进行优化，使得总的损失值c
m
最小，可以得到优化后点云棋盘格坐标系到棋盘格靶标坐
标系的转换矩阵
[0072]
f)计算棋盘格靶标的角点在激光雷达空间直角坐标系下的坐标，数学公式如下：
[0073][0074]
公式中：为优化后点云棋盘格坐标系到棋盘格靶标坐标系的转换矩阵，可由步骤e)获得；为激光雷达空间直角坐标系转换到点云棋盘格坐标系的转换矩阵，旋转分量为平移分量为点云中心在激光雷达空间直角坐标系下的坐标，可由a)获得。
[0075]
作为本发明的进一步优选，步骤d)计算点云聚类中每个点的损失值的规则如下：
[0076]
首先判断点i的灰度值i
i
是否在[τ
l
，τ
h
]内，如果在[τ
l
，τ
h
]之内，点i的损失值为0；然后判断点i在棋盘格靶标坐标系的坐标是否落在棋盘格靶标坐标系下的棋盘格范围中，如果点i在棋盘格范围外，计算点i到棋盘格四个顶点g最小的曼哈顿距离，记为点i的损失值；如果点i在棋盘格范围内，找到距离点i的曼哈顿距离最小的棋盘格角点v
i
，并判断点i的颜色c
i
与棋盘格角点v
i
的颜色是否一致；如果一致，则点i的损失值为0；如果不一致，点i的损失值为点i到棋盘格角点v
i
的曼哈顿距离；数学公式如下：
[0077][0078]
式中：c
m
表示点云聚类中所有点的损失值，f
g
为点i的损失系数函数，如果点i的灰度值在阈值范围[τ
l
，τ
h
]内，则损失系数f
g
为0，否则损失系数f
g
为1，c
i
表示点i的颜色，表示点i所在正方形的四个顶点中，距离点i曼哈顿距离最近的顶点的颜色；f
in
为判断点i是否在棋盘格范围内的函数；g表示棋盘格的四个顶点，v
i
表示点i所在正方形的顶点；表示点i在棋盘格靶标坐标系下的坐标；f
d
表示计算曼哈顿距离的函数；δx1，δx2，δy1，δy2表示点i到所在正方形四条边的距离；min表示取最小的输入值的操作。
[0079]
本发明的另一目的在于一种多激光雷达和多相机联合标定系统，该系统包括棋盘格靶标、至少四个传感器、第一数据处理模块、第二数据处理模块、传感器组选择模块、传感器识别模块、第一外参计算模块、第二外参计算模块、第三外参计算模块、累计误差计算模块、标定模块；其中，所述传感器包括激光雷达传感器和相机传感器，激光雷达传感器与第一数据处理模块连接，用于将检测的数据发送给第一数据处理模块；相机传感器与第二数据处理模块连接，用于将检测的数据发送给第二数据处理模块；
[0080]
所述第一数据处理模块，用于对激光雷达传感器发送的数据进行处理，计算棋盘格靶标角点在不同激光雷达传感器构成的激光雷达空间直角坐标系下的坐标；
[0081]
所述第二数据处理模块，用于对相机传感器发送的数据进行处理，计算棋盘格靶
标角点在不同相机传感器构成的像素平面坐标系下的坐标；
[0082]
所述传感器组选择模块，用于根据第一数据处理模块、第二数据处理模块发送的数据，选择对某一位置的棋盘格靶标进行组合标定的三个传感器,选择的三个传感器分别记为传感器sa、传感器sb以及传感器sc；对传感器进行选择时，选择棋盘格靶标完整的出现在传感器视野中的传感器；
[0083]
所述传感器识别模块，用于对组合标定的三个传感器的类型进行识别，根据传感器类型选择不同传感器之间计算外参矩阵的方法，当传感器sa与传感器sb均为激光雷达传感器时，则利用第一外参计算模块计算；当传感器sa与传感器sb均为相机传感器时，则利用第二外参计算模块计算，当传感器sa为激光雷达传感器，传感器sb为相机传感器时，则利用第三外参计算模块计算；
[0084]
所述第一外参计算模块，用于计算激光雷达传感器sa与激光雷达传感器sb之间的外参参数，计算公式如下：
[0085][0086]
式中，w
i
为棋盘格靶标角点i的误差权重，为激光雷达传感器sa到激光雷达传感器sb的外参矩阵，为由激光雷达传感器sa扫描得到的棋盘格靶标角点在激光雷达传感器sb构成的激光雷达空间直角坐标系的坐标，x
ai
为棋盘格靶标角点在激光雷达传感器sa构成的激光雷达空间直角坐标系下的坐标；x
bi
为棋盘格靶标角点在激光雷达传感器sb构成的激光雷达空间直角坐标系下的坐标；
[0087]
所述第二外参计算模块，用于计算相机传感器sb与相机传感器sa之间的外参参数；计算公式如下：
[0088][0089]
式中，为的逆矩阵，w
i
为棋盘格靶标角点i的误差权重，为相机传感器sb到相机传感器sa的外参矩阵；为相机传感器sb拍摄得到的棋盘格靶标角点在相机传感器sa构成的像素平面坐标系下的坐标，为相机传感器sa获取的相应棋盘格靶角点坐标，为相机传感器sb获取的相应棋盘格靶角点坐标，λ为比例因子，等于λ
a
与λ
b
的比值；相机传感器sa的尺度因子λ
a
可以根据棋盘格靶标正方形的尺寸以及相邻两个棋盘格靶标角点在相机传感器sa构成的像素平面坐标系下的距离求得；相机传感器sb的尺度因子λ
b
可以根据棋盘格靶标正方形的尺寸以及相邻两个棋盘格靶标角点在相机传感器sb构成的像素平面坐标系下的距离求得；
[0090]
所述第三外参计算模块，用于计算激光雷达传感器sa与相机传感器sb之间的外参参数，计算公式如下：
[0091][0092]
式中：w
i
为棋盘格靶标角点i的误差权重，λ为比例系数，数值大小等于点在相机传感器sb构成的相机空间直角坐标系下的z轴坐标的倒数，为激光雷达传感器sa到相机传感器sb的外参矩阵，为激光雷达传感器sa扫描得到的棋盘格靶标角点在相机传感器sb
构成的像素平面坐标系下的坐标，x
ai
为棋盘格靶标角点在激光雷达传感器sa构成的激光雷达空间直角坐标系下的坐标；x
bi
为棋盘格靶标角点在相机传感器sb构成的像素平面坐标系下的坐标；为相机传感器sb相应的内参矩阵；
[0093]
所述累计误差计算模块，用于根据第一外参计算模块、第二外参计算模块、第三外参计算模块计算的数据计算由传感器sa与传感器sb之间的标定误差，以及传感器sb与传感器sc之间标定误差，引起的传感器sa与传感器sc之间标定误差；
[0094]
所述标定模块，用于联合优化不同位置的棋盘格靶标角点的坐标累积误差，使得总的标定误差最小；同时，由于不同传感器组构成一个回环，回环的旋转矩阵应为单位阵，平移矩阵应为零矩阵，以此计算外参参数。
[0095]
本发明的优点和技术效果是：
[0096]
1、本发明提供的标定方法不要求所有传感器都有共同视野，不要求传感器的安装位置以及数目，该方法可根据棋盘格靶标是否完整出现在传感器视野中，选择进行组合标定的传感器，之后根据多个位置不同的组合标定，进行外参参数的优化。
[0097]
2、本发明在多激光雷达和多相机联合标定时加入回环约束，要求不同传感器组构成一个回环时，回环的旋转矩阵为单位阵，平移矩阵为零矩阵,从而保证任意两个传感器之间的外参参数的精度。
[0098]
3、本发明通过引入不同特征点的误差权重，降低低分辨率传感器引起的标定误差的权重，使其可以适用于多种标定场景，不必要求标定时天气良好、视野清楚；如果天气良好、视野清楚，可以得到高精度的特征点，则所有特征点的误差权重均设为1；如果视野不佳，对相机检测特征点造成影响，则降低相机检测的特征点的误差权重；如果天气环境不好，则降低距离传感器较远的特征点的误差权重。
[0099]
4、本发明通过设置不同的误差权重，保证整体优化时惩罚引入误差较大的特征点，避免因为某些检测的特征点误差过大，造成得到的传感器之间的外参参数的误差过大。
附图说明
[0100]
图1是多激光雷达和多相机在车辆上的安装示意图之一；
[0101]
图2是多激光雷达和多相机在车辆上的安装示意图之二；
[0102]
图3是本发明标定方法的整体流程图；
[0103]
图4是点云棋盘格坐标系示意图；
[0104]
图5是点云棋盘格坐标系xoy平面与棋盘格靶标坐标系误差示意图；
[0105]
图6是判断直方图峰值的示意图；
[0106]
图7a是点在格子内时，根据点与棋盘格的位置关系计算曼哈顿距离的示意图；
[0107]
图7b是点在棋盘格外部时，根据点与棋盘格的位置关系计算曼哈顿距离的示意图；
[0108]
图8是图1中的传感器s2与s5互换位置后的示意图；
[0109]
图9是优化角点坐标的原理图；
[0110]
图10是本发明标定系统的结构框图。
具体实施方式
[0111]
下面将结合本技术实施例中的附图，对本技术实施例中的技术方案进行清楚、完整地描述；显然，所描述的实施例仅是本技术一部分实施例，而不是全部的实施例。基于本技术中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本技术保护的范围。
[0112]
另外，在以下的实施方式中，很多细节描述是为了使得本技术能被更好的理解。然而，本领域技术人员可以毫不费力的认识到，其中部分特征在不同情况下是可以省略的，或者可以由其它方法所替代。在某些情况下，本技术相关的一些操作并没有在说明书中显示或者描述，这是为了避免本技术的核心部分被过多的描述所淹没，而对于本领域技术人员而言，详细描述这些相关操作并不是必要的，他们根据说明书中的描述以及本领域的一般技术知识即可完整了解相关操作。
[0113]
需要说明的是，在不冲突的情况下，本公开中的实施方式及实施方式中的特征可以相互组合。此外，术语“第一”、“第二”等仅用于区分描述，而不能理解为指示或暗示相对重要性。下面将参考附图并结合实施方式来详细说明本公开的技术方案。
[0114]
本发明旨在提供一种多激光雷达和多相机联合标定方法，图1描述了本发明多激光雷达和多相机联合标定时采用2个相机传感器和3个激光雷达传感器时，相机传感器和激光雷达传感器的安装位置以及棋盘格靶标摆放位置的示意图；其中，s1、s4、s5代表激光雷达传感器，s2、s3代表相机传感器(实施例中所有相机的内参参数矩阵、畸变参数均为已知)，p1至p4代表棋盘格靶标；图2描述了本发明多激光雷达和多相机联合标定时采用6个相机传感器或激光雷达传感器时，相机传感器或激光雷达传感器的安装位置以及棋盘格靶标摆放位置的示意图；图3描述了本发明多激光雷达和多相机联合标定方法的整体流程图。
[0115]
实施例1一种多激光雷达和多相机联合标定方法
[0116]
如图1至图3所示，本发明提供的一种多激光雷达和多相机联合标定方法，具体标定过程如下：
[0117]
步骤s1、放置棋盘格靶标
[0118]
将棋盘格靶标放置在s1、s2、s3三个传感器共同视野范围内，距离车前大致在5
‑
10m的p1位置，调整棋盘格靶标角度及高度，使得棋盘格靶标在传感器中视野良好，棋盘格靶标可以完整出现在传感器视野中；
[0119]
其中，所述棋盘格靶标中心的高度与激光雷达传感器s1的安装高度(s1重心到地面的垂直距离)大致相等；棋盘格靶标每行每列包含的正方形的个数(n
c
，n
r
)以及每个正方形的尺寸l
grid
均是已知。
[0120]
本发明对棋盘格靶标的放置顺序不做要求，可以先标定p2或者p3位置的棋盘格靶标，本实施例只举例最先标定放置在p1位置的棋盘格靶标这种情况。
[0121]
步骤s2、提取棋盘格靶标角点
[0122]
步骤s2.1、同时启动传感器s1、s2、s3，等到传感器运行稳定后，获取各个传感器得到的棋盘格靶标放置在p1位置的图像以及点云数据；
[0123]
步骤s2.2、调整p1位置的棋盘格靶标的角度，再次启动传感器s1、s2、s3，得到不同角度的棋盘格靶标的图像和点云数据；
[0124]
步骤s2.3、提取步骤s2.1和步骤s2.2中激光雷达传感器s1获取的点云数据中棋盘
格角点在激光雷达空间直角坐标系下的坐标；其中，激光雷达空间直角坐标系是以激光雷达重心为原点，y轴指向车前进的方向，z轴垂直于车身，指向车顶，x轴与y轴z轴共同形成右手坐标系；激光雷达空间直角坐标系是局部坐标系，不同激光雷达可以构成不同激光雷达空间直角坐标系，同一激光雷达在不同位置构成不同的激光雷达空间直角坐标系；所述点云数据由多条扫描线组成，每一条扫描线由多个点组成；所述步骤s2.3的具体步骤如下：
[0125]
步骤s2.3.1、对组成点云数据的每条扫描线，逐点计算相邻两个点的距离和角度，将一条扫描线分割成不同聚类，数学公式如下：
[0126][0127]
公式(1)中：i为点云的序号，[x
i
‑1，y
i
‑1，z
i
‑1]，[x
i
，y
i
，z
i
],[x
i+1
，y
i+1
，z
i+1
]分别是激光点i
‑
1，i，i+1在激光雷达空间直角坐标系下的坐标，d
i
是激光点i与激光点i
‑
1在激光雷达空间直角坐标系下的欧式距离，d
t
表示激光点i与激光点i
‑
1在激光雷达空间直角坐标系下的欧式距离的阈值，δ表示激光点i与激光点i
‑
1构成的向量与激光点i与激光点i+1构成的向量之间余弦值的阈值；d
t
，δ根据激光雷达的角度分辨率计算。
[0128]
步骤s2.3.2、统计步骤s2.3.1得到的每个聚类中点云的个数，剔除点云个数小于阈值的聚类；点云个数的阈值根据聚类中心到激光雷达重心的欧式距离及激光雷达的角度分辨率计算。
[0129]
步骤s2.3.3、合并不同扫描线分割的结果；计算每个类别的特征向量，并根据特征向量之间的余弦相似度，合并余弦相似度高的扫描线。
[0130]
步骤s2.3.4、根据步骤s2.3.3得到的点云聚类结果，检测点云聚类中是否存在棋盘格靶标；判断标准有3个，如下所示：
[0131]
a)对每个点云聚类进行主成分分析(principle component analysis,pca)分解，得到每个聚类的特征向量和相应特征值(θ1，θ2，θ3),根据特征值(θ1，θ2，θ3)计算各聚类区域的形状描述子；其中，所述形状描述子包括线性描述子、平面描述子和球状描述子三类。如果一个点云聚类表示棋盘格靶标，该聚类应该满足计算得到的平面描述子的值大于线性描述子和球状描述子，数学公式如下：
[0132][0133]
公式(2)：d
1d
为线性描述子，d
2d
为平面描述子，d
3d
为球状描述子，max表示取最大值的操作；
[0134]
b)统计每个点云聚类包含的点云的数目nn，如果一个点云聚类表示棋盘格靶标，则该聚类包含的点的个数应该满足数学公式，如下所示：
[0135]
[0136]
公式(3)：n
tre
表示激光雷达能扫描到的最多的点的个数,d
w
，d
h
分别是棋盘格靶标的宽和高，r为聚类中心到激光雷达重心的距离，δh，δv分别是激光雷达的水平和垂直分辨率，表示将输入值向下取整的操作。
[0137]
c)对每个聚类进行ks(kolmogorov
–
smirnov)检验，检查聚类的点是否满足均匀分布，如果满足均匀分布，ks检验得到的d值应该小于阈值；其中，d值表示样本所属总体的分布与均匀分布之间的最大距离；阈值可以通过查找ks单样本检验临界值表获得；ks单样本检验临界值表是一张列举不同显著性水平及不同样本数量下的d的临界值的表格，根据聚类中的点云的个数以及提前设定的显著性水平，可以在ks单样本检验临界值表找到对应的最大的临界值，如果ks检验得到的d值大于该临界值，表明聚类不满足均匀分布；
[0138]
如果某个点云聚类同时满足上述三个条件，则认为该点云聚类表示棋盘格靶标。如果不满足上述任意一个条件，则剔除该点云聚类，不进行后续操作。
[0139]
步骤s2.3.5、如果步骤s2.3.4判断得到表示棋盘格靶标的点云聚类，求棋盘格靶标上角点(两个正方形相交的顶点)在激光雷达空间直角坐标系下的坐标，具体步骤如下：
[0140]
a)以点云聚类的中心为原点，为x轴，为y轴，为z轴，旋转坐标轴，使得z轴平行于点云法向量并指向激光雷达，x轴的指向与激光雷达空间直角坐标系下的x轴相同，以此得到的坐标系称为点云棋盘格坐标系(如图4所示)。之后以棋盘格靶标中心为原点，x轴平行棋盘格平面并且指向和点云棋盘格坐标系中x轴指向一致，y轴平行棋盘格平面朝上，建立棋盘格靶标坐标系。
[0141]
将点云聚类的所有点投影到点云棋盘格坐标系xoy平面，理想情况下投影后的点应该在棋盘格靶标范围内，但是由于激光雷达与棋盘格靶标之间不一定是垂直扫描，可能存在一定的角度，因而扫描得到的棋盘格点云可能存在坐标位置误差，使得计算得到的棋盘格点云特征向量不准确，因此经过步骤s2.3.5)中a)处理得到的点云棋盘格坐标系xoy平面不能与棋盘格靶标坐标系重合(如图5所示)。
[0142]
b)根据点云聚类中每个点的灰度值，建立灰度值直方图；根据直方图最大的两个峰值r
l
和r
h
(如图6所示)，求出用于判断某个点对应的颜色的阈值范围[τ
l
，τ
h
]。即如果一个点的灰度值大于上边界τ
h
，该点表示白色；如果一个点的灰度值小于下边界τ
l
，该点表示黑色。数学公式如下：
[0143][0144]
公式(4)中：i
i
为点云i的灰度值，c
i
为该点表示的颜色，其中0表示白色，1表示黑色。
[0145][0146]
公式(5)中：ε
g
为一个大于2的常数，通常取4；
[0147]
c)根据已知的棋盘格的类型(即每行每列包含的正方形的个数)、每个正方形的尺寸l
grid
以及棋盘格靶标左下角正方形的颜色c
(0，0)
，计算每个正方形的颜色以及该正方形左下角及右上角在棋盘格靶标坐标系的坐标，数学公式如下：
[0148][0149]
公式(6)中：i，j表示每个正方形的行列号，c
(i，j)
表示行列号分别为i，j的正方形的颜色，n
c
，n
r
分别为每行/每列正方形的个数，(x
i
，y
i
)表示行列号分别为i，j的正方形左下角在棋盘格靶标坐标系的坐标,(x
i+1
，y
i+1
)表示行列号分别为i，j的正方形右上角在棋盘格靶标坐标系的坐标；
[0150]
d)计算点云聚类中每个点的损失值，规则如下：首先判断点i的灰度值i
i
是否在[τ
l
，τ
h
]内，如果在[τ
l
，τ
h
]之内，点i的损失值为0；然后判断点i在棋盘格靶标坐标系的坐标是否落在棋盘格靶标坐标系下的棋盘格范围中，如果点i在棋盘格范围外，计算点i到棋盘格四个顶点g最小的曼哈顿距离，记为点i的损失值；如果点i在棋盘格范围内，找到距离点i的曼哈顿距离最小的棋盘格角点v
i
，并判断点i的颜色c
i
与棋盘格角点v
i
的颜色是否一致；如果一致，则点i的损失值为0；如果不一致，点i的损失值为点i到棋盘格角点v
i
的曼哈顿距离；数学公式如下：
[0151][0152]
公式(7)中：c
m
表示点云聚类中所有点的损失值，f
g
为点i的损失系数函数，如果点i的灰度值在阈值范围[τ
l
，τ
h
]内，则损失系数f
g
为0，否则损失系数f
g
为1，c
i
表示点i的颜色，表示点i所在正方形的四个顶点中，距离点i曼哈顿距离最近的顶点的颜色，f
in
为判断点i是否在棋盘格范围内的函数，g表示棋盘格的四个顶点，v
i
表示点i所在正方形的顶点，表示点i在棋盘格靶标坐标系下的坐标，f
d
表示计算曼哈顿距离的函数。δx1，δx2，δy1，δy2表示点i到所在正方形四条边的距离，min表示取最小的输入值的操作。
[0153]
举例说明，点p的灰度值在[τ
l
，τ
h
]内，则点p对应的损失系数为0，即点p的损失值为0；否则判断点p是否在棋盘格内部，若点p在棋盘格内部，如图7(a)所示，因为点p距离顶点v4的曼哈顿距离最近，则点p的损失值为如果点p表示的颜色与顶点v4的颜色相同，则p点的损失值为0,否则为(δx1+δy1)；如点p在棋盘格外部，如图7(b)所示，因为点p距离棋盘格顶点g4的曼哈顿距离最近，则点p的损失值为(δx1+δy1)。
[0154]
e)由步骤s2.3.5中b)可知棋盘格特征向量不准确，因此经过步骤s2.3.5中a)处理得到的点云棋盘格坐标系xoy平面不能与棋盘格靶标坐标系重合，根据公式(7)计算得到的c
m
不等于0；根据powell共轭方向集方法对公式(7)进行优化，使得计算得
到总的损失值c
m
最小，可以得到优化后点云棋盘格坐标系到棋盘格靶标坐标系的转换矩阵
[0155]
f)计算棋盘格靶标的角点在激光雷达空间直角坐标系下的坐标，数学公式如下：
[0156][0157]
公式(8)中：为优化后点云棋盘格坐标系到棋盘格靶标坐标系的转换矩阵，可由步骤s2.3.5中e)获得；为激光雷达空间直角坐标系转换到点云棋盘格坐标系的转换矩阵，旋转分量为平移分量为点云中心在激光雷达空间直角坐标系下的坐标，可由步骤s2.3.5中a)获得。
[0158]
步骤s2.4、分别提取步骤s2.1和步骤s2.2中相机传感器s2和s3获取的图像中棋盘格靶标角点在像素平面坐标系下的坐标，像素平面坐标系是以相片左上角为坐标系原点，横轴为u，指向右，对应相机空间直角坐标系中的x轴，纵轴为v，指向下，对应相机直角空间坐标系中的y轴，像素平面坐标系是局部坐标系，不同相机拍摄的不同照片可以构成不同的像素平面坐标系，同一相机拍摄的不同照片可以构成不同的像素平面坐标系；相机空间直角坐标系是以相机重心为坐标原点，y轴垂直与车辆，指向车底；x轴垂直于车辆行进的方向，指向右；z轴指向车辆前进的方向；相机空间直角坐标系是局部坐标系，不同相机构成不同的相机空间直角坐标系，同一相机在不同位置构成不同的相机空间直角坐标系；具体步骤如下：
[0159]
步骤s2.4.1、根据已知的畸变参数，对图像进行畸变校正，然后对畸变校正后的图像f(x，y)进行高斯平滑滤波处理，减少图像的噪声，得到平滑后的图像g(x，y)，数学公式如下：
[0160][0161]
公式(18)中：表示卷积操作，σ2表示函数的宽度参数,控制函数的径向作用范围；h(x，y)表示高斯平滑滤波核函数；(x，y)表示点在像素平面坐标系下的像素坐标，其中x的值表示点在像素平面坐标系u轴方向上的投影距离，y的值表示点在像素平面坐标系v轴方向上的投影距离，exp表示以自然常数e为底的指数函数。
[0162]
步骤s2.4.2)对步骤s2.4.1得到的平滑后的图像g(x，y)根据数学公式(19)构建hessian矩阵：
[0163][0164]
公式(19)中：g
xx
g
xy
及g
yy
分别是图像g(x，y)相对于x,y的二阶偏导数。
[0165]
步骤s2.4.3、分解步骤s2.4.2得到的hessian矩阵，求取hessian矩阵的特征值，数学公式如下：
[0166][0167]
公式(20)中：λ1，λ2表示hessian矩阵两个特征值，d为计算hessian矩阵特征值的中间临时变量。
[0168]
步骤s2.4.4、根据hessian矩阵的性质，hessian矩阵两个特征值λ1，λ2在数值上表示图像灰度的二阶导数的极大值和极小值，取形状算子s(x,y)＝λ1×
λ2,当s取极小值时的(x0，y0)坐标即为棋盘格靶标角点的坐标，数学公式如下：
[0169][0170]
公式(21)中：表示(x0，y0)坐标处的形状算子，min表示取最小值的操作，局部范围的最小值即为极小值。
[0171]
步骤s2.4.5、由于图像中的像素灰度值呈连续变化，因此步骤s2.4.4得到的像素坐标不一定是棋盘格靶标角点的像素坐标，为了得到真实的棋盘格靶标角点位置，需要对像素坐标进一步优化，原理如下，如图9所示，棋盘格靶标角点局部区域内的一个像素p的图像梯度向量gp与像素到棋盘格靶标角点的向量的乘积为0。因此对步骤s2.4.4得到的每一个棋盘格靶标角点c遍历周围局部区域的像素，求每个像素的图像梯度向量与该像素到角点的向量的乘积的平方和；利用梯度下降优化算法优化棋盘格靶标角点的像素坐标使得得到的乘积的和最小；梯度下降优化算法终止的条件是迭代的次数超过相应的阈值或者梯度下降的阈值小于相应的阈值；数学公式如下：
[0172][0173]
公式(22)中：c
′
为步骤s2.3.4得到的像素坐标，表示像素坐标c
′
的局部邻域，表示像素p属于像素坐标c
′
的局部邻域，为像素p的图像梯度向量g
p
的转置矩阵，argmin为求最小的操作，c为优化后的棋盘格靶标角点的像素坐标。
[0174]
步骤s2.4.6、步骤s2.4.5得到的棋盘格靶标角点坐标即为像素平面坐标系下的棋盘格靶标角点坐标。
[0175]
步骤s3、根据传感器类型选择不同传感器之间计算外参矩阵的方法；
[0176]
多激光雷达传感器和多相机传感器联合标定，根据传感器不同的类型，可以分成激光雷达传感器与激光雷达传感器标定，相机传感器与相机传感器标定，以及激光雷达传感器与相机传感器标定三种情况；
[0177]
(1)以激光雷达传感器s1与相机传感器s2为例，激光雷达传感器与相机传感器的标定原理如下：
[0178]
将棋盘格靶标角点在激光雷达传感器s1构成的激光雷达空间直角坐标系下的坐标x1通过外参矩阵转换到相机传感器s2构成的相机空间直角坐标系中，并进行坐标归一化，即将其在相机空间直角坐标系下的坐标分别除以其在相机空间直角坐标系下的z轴坐标，使得其在相机空间直角坐标系下z轴方向的坐标为1，然后通过小孔成像模型，利用相机传感器s2已知内参参数最终将棋盘格靶标角点投影到相机传感器s2构成的像素平面
坐标系中，获取由激光雷达传感器s1扫描得到的棋盘格靶标角点在相机传感器s2构成的像素平面坐标系下的坐标然后计算与相机传感器s2获取的相应棋盘格靶角点坐标x2的欧式距离的加权平方和e
err
；通过梯度下降算法迭代优化激光雷达传感器s1到相机传感器s2的外参矩阵使得误差最小，从而完成激光雷达传感器s1到相机传感器s2的标定，得到激光雷达传感器s1到相机传感器s2的外参矩阵数学公式如下：
[0179][0180][0181]
公式(14)中：w
i
为棋盘格靶标角点i的误差权重，λ为比例系数，数值大小等于点在相机传感器s2构成的相机空间直角坐标系下的z轴坐标的倒数，w
i
有多种求解方法，公式(14)采用棋盘格靶标角点到某一传感器的距离的倒数，即棋盘格靶标角点到激光雷达传感器s1重心的距离的倒数作为权重。
[0182]
(2)以相机传感器s2与相机传感器s3为例，相机传感器与相机传感器的标定原理如下：
[0183]
将棋盘格靶标角点在相机传感器s3构成的像素平面坐标系下的坐标x3通过相机传感器s3的内参矩阵的逆矩阵以及尺度因子γ3转换到相机传感器s3构成的相机空间直角坐标系下，尺度因子γ3可以根据正方形的尺寸l
grid
以及相邻两个棋盘格靶标角点在相机传感器s3构成的像素平面坐标系下距离求得，然后通过相机传感器s3到相机传感器s2的外参矩阵转换到相机传感器s2构成的相机空间直角坐标系下，最后通过相机传感器s2的内参矩阵以及尺度因子γ2转换到相机s2构成的像素平面坐标系下，尺度因子γ2可以根据正方形的尺寸l
grid
以及相邻两个棋盘格靶标角点在相机传感器s2构成的像素平面坐标系下距离求得，得到由相机传感器s3拍摄的棋盘格靶标角点在相机传感器s2构成的像素平面坐标系下的坐标然后计算与相机传感器s2获取的相应标定盘角点x2的欧式距离的加权平方和e
err
；通过梯度下降法迭代优化外参矩阵使得误差e
err
最小，从而完成相机传感器s2到相机传感器s3的标定，得到相机传感器s2到相机传感器s3的外参矩阵梯度下降算法迭代的终止条件是迭代的次数大于给定的相应阈值或者梯度下降的幅度小于相应的阈值，数学公式如下：
[0184][0185][0186]
公式(15)中:为相机传感器s3的内参矩阵的逆矩阵，以及均为已知，γ为比例因子，等于γ2与γ3的比值。w
i
为棋盘格靶标角点i的误差权重，公式(15)选取棋盘格靶标角点到相机传感器s2重心的距离的倒数作为权重。
[0187]
(3)以激光雷达传感器s1与激光雷达传感器s4为例，激光雷达传感器与激光雷达传感器的标定原理如下：
[0188]
将棋盘格靶标角点在激光雷达传感器s1构成的激光雷达空间直角坐标系下的坐标x1通过外参矩阵转换到激光雷达传感器s4构成的激光雷达空间直角坐标系中，获取由
激光雷达传感器s1扫描得到的棋盘格靶标角点在激光雷达传感器s4构成的激光雷达空间直角坐标系的坐标然后计算与激光雷达传感器s4获取的相应棋盘格靶角点坐标x4的欧式距离的加权平方和e
err
；通过梯度下降算法迭代优化激光雷达传感器s1到激光雷达传感器s4的外参矩阵使得误差最小，从而完成激光雷达传感器s1到激光雷达传感器s4的标定，得到激光雷达传感器s1到激光雷达传感器s4的外参矩阵数学公式如下：
[0189][0190]
公式(16)中：w
i
为棋盘格靶标角点i的误差权重，w
i
有多种求解方法，公式(16)选取棋盘格靶标角点到激光雷达传感器s1重心的距离的倒数作为权重。
[0191]
步骤s3.1、根据步骤s2.3及步骤s2.4得到的棋盘格靶标角点在激光雷达空间直角坐标系及像素平面坐标系下的坐标，以及激光雷达传感器与相机传感器的标定原理，求解激光雷达传感器s1到相机传感器s2的外参矩阵
[0192]
步骤s3.2、根据步骤s2.3及步骤s2.4得到的棋盘格靶标角点在不同像素平面坐标系下的坐标，以及相机传感器与相机传感器的标定原理，求解传感器传感器s2到传感器传感器s3的外参矩阵
[0193]
步骤s4、计算基于多观测值的棋盘格靶标角点的坐标累积误差
[0194]
根据步骤s3得到的激光雷达传感器s1到相机传感器s2的外参矩阵以及相机传感器s2到相机传感器s3的外参矩阵通过矩阵相乘，得到激光雷达传感器s1到相机传感器s3的外参矩阵将棋盘格靶标角点在激光雷达传感器s1激光雷达空间直角坐标系下的坐标x1通过求得的外参矩阵转换到相机传感器s3构成的相机空间直角坐标系下，并完成坐标归一化，然后通过相机传感器s3的内参矩阵转换到相机传感器s3像素平面坐标系中,得到激光雷达传感器s1获取的棋盘格靶标角点通过转换在相机传感器s3像素平面坐标系下的坐标计算与相机传感器s3获取的相应的棋盘格靶标角点x3的欧式距离的加权平方和e
p1
，数学公式为：
[0195][0196][0197]
公式(17)中:e
p1
表示激光雷达传感器s1与相机传感器s2之间的标定误差以及相机传感器s2与相机传感器s3之间标定误差，引起的激光雷达传感器s1与相机传感器s3之间标定误差，w
i
表示p1位置的棋盘格靶标角点的误差权重，根据说明选取棋盘格靶标角点到激光雷达传感器s1重心的距离的倒数，λ为比例系数，数值大小等于点在相机传感器s3构成的相机空间直角坐标系下的z轴坐标的倒数；
[0198]
步骤s5、移动棋盘格靶标，计算不同位置棋盘格靶标角点的坐标累积误差
[0199]
步骤s5.1、将棋盘格靶标放置在p2位置，到车身的距离大致为2～5m，调整棋盘格靶标的角度及高度，确保棋盘格靶标在传感器s1、s2、s5的共同视野范围内,棋盘格靶标可
以完整出现在传感器视野中；重复上述步骤s2,提取不同传感器获取的数据中的棋盘格靶标的角点坐标,根据步骤s3中不同传感器之间标定的原理,分别获取激光雷达传感器s1到激光雷达传感器s5的外参矩阵激光雷达传感器s5到相机传感器s2的外参矩阵然后根据矩阵相乘，获得激光雷达传感器s1到相机传感器s2的外参矩阵根据步骤s4中的原理,将p2位置的棋盘格靶标角点在激光雷达传感器s1构成的激光雷达空间直角坐标系下的坐标x
’1通过求得的外参矩阵转换到相机传感器s2构成的相机空间直角坐标系下，并完成坐标归一化，然后通过相机传感器s2的内参矩阵转换到传感器传感器s2像素平面坐标系下,得到激光雷达传感器s1获取的p2位置的棋盘格靶标角点通过转换在相机传感器s2构成的像素平面坐标系下的坐标计算与相机传感器s2获取的相应的棋盘格靶标角点x
’2的欧式距离的加权平方和e
p2
，从而得到由激光雷达传感器s1与激光雷达传感器s5之间的标定误差以及激光雷达传感器s5与相机传感器s2之间标定误差，引起的激光雷达传感器s1与相机传感器s2之间标定误差，数学公式为：
[0200][0201][0202]
公式(18)中:表示p2位置的棋盘格靶标角点在激光雷达传感器s1构成的激光雷达空间直角坐标系下的坐标，表示p2位置的棋盘格靶标角点在相机传感器s2构成的像素平面坐标系下的坐标，w
i
表示p2位置的棋盘格靶标角点的误差权重，公式(18)选取棋盘格靶标角点到激光雷达传感器s1重心的距离的倒数作为误差权重，λ为比例系数，数值大小等于点在相机传感器s2构成的相机空间直角坐标系下的z轴坐标的倒数。
[0203]
步骤s5.2、将棋盘格靶标放置在p3位置，距离车身大致为2m
‑
5m，调整棋盘格靶标的高度和角度，确保棋盘格靶标在传感器s1、s4、s5的共同视野范围内,棋盘格靶标可以完整出现在传感器视野中；重复上述步骤s2，提取不同传感器获取的数据中的棋盘格靶标的角点坐标,根据步骤s3中不同传感器之间标定的原理,分别获取激光雷达传感器s1到激光雷达传感器s4的外参矩阵激光雷达传感器s4到激光雷达传感器s5的外参矩阵然后根据矩阵相乘，获得激光雷达传感器s1到激光雷达传感器s5的外参矩阵将p3位置的棋盘格靶标角点在激光雷达传感器s1构成的激光雷达空间直角坐标系下的坐标x”1
通过求得的外参矩阵转换到激光雷达传感器s5构成的激光雷达空间直角坐标系下,得到激光雷达传感器s1获取的p3位置的棋盘格靶标角点通过转换在激光雷达传感器s5构成的激光雷达空间直角坐标系下的坐标计算与激光雷达传感器s5获取的相应的棋盘格靶标角点的欧式距离的加权平方和e
p3
，从而得到由激光雷达传感器s1与激光雷达传感器s4之间的标定误差以及激光雷达传感器s4与激光雷达传感器s5之间标定误差，引起的激光雷达传感器s1与激光雷达传感器s5之间标定误差，数学公式为：
[0204][0205][0206]
公式(19)中:表示p3位置的棋盘格靶标角点i在激光雷达传感器s1构成的激光雷达空间直角坐标系下的坐标，表示p3位置的棋盘格靶标角点i在激光雷达传感器s5构成的激光雷达空间坐标系下的坐标,w
i
表示p3位置的棋盘格靶标角点的误差权重，公式(19)选取棋盘格靶标角点到激光雷达传感器s1重心的距离的倒数。
[0207]
步骤s5.3、将棋盘格靶标放置在p4位置,距离车身大致为2m
‑
5m，调整棋盘格靶标的高度和角度，确保棋盘格靶标在传感器s1、s3、s4的共同视野范围内，棋盘格靶标可以完整出现在传感器视野中；重复上述步骤s2,提取不同传感器获取的数据中的棋盘格靶标的角点坐标,根据步骤s3中不同传感器之间标定的原理,分别获取激光雷达传感器s1到相机传感器s3的外参矩阵相机传感器s3到激光雷达传感器s4的外参矩阵然后根据矩阵相乘，获得激光雷达传感器s1到激光雷达传感器s4的外参矩阵将p4位置的棋盘格靶标角点在激光雷达传感器s1构成的激光雷达空间直角坐标系下的坐标x
″′1通过求得的外参矩阵转换到激光雷达传感器s4构成的激光雷达空间直角坐标系下,得到激光雷达传感器s1获取的p4位置的棋盘格靶标角点通过转换在传感器s4构成的激光雷达空间直角坐标系下的坐标计算与传感器s4获取的相应的棋盘格靶标角点x
″′4的欧式距离的加权平方和e
p4
，以此计算由激光雷达传感器s1与相机传感器s3之间的标定误差以及相机传感器s3与激光雷达传感器s4之间标定误差，引起的激光雷达传感器s1与激光雷达传感器s4之间标定误差，则数学公式为：
[0208][0209][0210]
公式(20)中:表示p4位置的棋盘格靶标角点i在激光雷达传感器s1构成的激光雷达空间直角坐标系下的坐标，表示p4位置的棋盘格靶标角点i在激光雷达传感器s4构成的激光雷达空间直角坐标系下的坐标，w
i
表示p4位置的棋盘格靶标角点的误差权重，公式(20)选取棋盘格靶标角点到激光雷达传感器s1重心的距离的倒数。
[0211]
步骤s6、联合优化不同位置的棋盘格靶标角点的坐标累积误差
[0212]
联合优化e
p1
，e
p2
,e
p3
,e
p4
，使得总的标定误差e
total
最小，数学公式如下：
[0213][0214]
公式(21)中:分别是中的旋转分量和平移分量，是的逆矩阵；
分别是中的旋转分量和平移分量，分别是中的旋转分量和平移分量；分别是中的旋转分量和平移分量；分别是中的旋转分量和平移分量；w1，w2，w3，w4为不同传感器组合标定误差的权重，可以通过计算不同传感器组合的测距精度或分辨率设定。设定规则如下：传感器的测距精度或分辨率越高，其所对应的标定误差权重越大；传感器的测距精度或分辨率越低，其所对应的标定误差权重越小。
[0215]
步骤s6可以通过使用matlab中的fmincon函数迭代求解，迭代的终止条件可以是迭代次数超过阈值或者总的标定误差e
total
收敛。
[0216]
需要说明：由于步骤s3传感器进行组合标定时不考虑传感器的位置，因此当图1中传感器s1～s5的安装位置发生变化时，本方法仍然成立。例如:s2与s5位置调换(如图8所示)，步骤s4的联合求解公式仍然成立，此时e
p1
表示为传感器s1与传感器s5之间的标定误差以及传感器s5与传感器s3之间标定误差，引起的传感器s1与传感器s3之间标定误差；e
p3
表示为传感器s1与传感器s2之间的标定误差以及传感器s2与传感器s4之间标定误差，引起的传感器s1与传感器s4之间标定误差；根据步骤s3不同传感器之间的标定误差公式，重新计算e
p1
，e
p3
，并且根据回环约束，重新计算约束条件。公式如下：
[0217][0218]
公式(22)中分别是中的旋转分量和平移分量,是的逆矩阵；分别是中的旋转分量和平移分量,即为传感器s3到传感器s5的外参矩阵，可以通过标定传感器s3与传感器s5获得，标定方法与步骤s3中标定传感器s1与传感器s2相同，分别是中的旋转分量和平移分量；为的逆矩阵；分别是中的旋转分量和平移分量,即为传感器s2到传感器s4的外参矩阵，可以通过标定传感器s2与传感器s4获得，标定方法与步骤s3中标定传感器s1与传感器s2相同；分别是中的旋转分量和平移分量；w1，w2，w3，w4为不同传感器组合标定误差的权重，可以通过计算不同传感器组合的测距精度设定。
[0219]
另外，本发明针对安装的传感器的个数大于5的情况(如图2所示)，可以根据两两传感器之间的视野情况，适当增加标定观测的位置。例如，一辆无人驾驶车辆在图1中所示的5个传感器基础上又增加第6个相机传感器s6,s1～s5的安装位置同图1图中所示相同，s6安装在s2与s5一侧，如果s1、s2、s5、s6四个传感器可以同时观测到p2位置的棋盘格靶标，则不需要增加标定观测的位置，p2位置的标定误差为s1,s2，s6组成的标定误差与s1,s5，s6组成的标定误差之和。如果s1、s2、s5、s6四个传感器不能同时观测到p2位置的棋盘格靶标，则需要新增观测位置p5,保证s1,s2，s6可以同时观测到p2位置的棋盘格靶标，s1,s5，s6可以同时观测到p5位置的棋盘格靶标。
[0220]
为使本领域技术人员清楚理解上述计算过程，下面针对s1、s2、s5、s6四个传感器可以同时观测到p2位置的棋盘格靶标这种情况，给出数学方程，如下所示。其他增加传感器的情况可根据实际情况，修改方程。
[0221][0222]
公式(23)中e
p2
为传感器s1与传感器s6之间的标定误差以及传感器s6与传感器s2之间标定误差，引起的传感器s1与传感器s2之间标定误差；e
′
p2
为传感器s1与传感器s5之间的标定误差以及传感器s6与传感器s5之间标定误差，引起的传感器s1与传感器s6之间标定误差,w1为传感器组合s1、s2、s3标定误差的权重，w2为传感器组合s1、s2、s6标定误差的权重，w
′2为传感器组合s1、s5、s6标定误差的权重，w3为传感器组合s1、s4、s5标定误差的权重，w4为传感器组合s1、s3、s4标定误差的权重；w1～w4可以通过计算不同传感器组合的测距精度或分辨率设定；分别是传感器s6与传感器s5之间外参矩阵中的旋转分量和平移分量，可以通过标定传感器s6与传感器s5获得，方法与步骤s3)中传感器s1与传感器s2之间的标定方法相同；分别是传感器s6与传感器s1之间外参矩阵中的旋转分量和平移分量，是的逆矩阵,可以通过标定传感器s1与传感器s6获得，标定的方法与步骤s3中传感器s1与传感器s2之间的标定方法相同。
[0223]
需要说明，本发明对传感器的安装个数和位置没有要求，本技术提供的上述传感器的安装形式并不用于限定本发明的保护范围，本发明还可以在车的其他位置增加传感器，如增加其他传感器，可根据传感器的安装位置及实际情况相应修改上述方程。
[0224]
实施例2一种多激光雷达和多相机联合标定系统
[0225]
如图1、图10所示，本发明提供的一种多激光雷达和多相机联合标定系统，包括棋盘格靶标、至少四个同源传感器、第一数据处理模块1、第二数据处理模块2、传感器组选择模块3、传感器识别模块4、第一外参计算模块5、第二外参计算模块6、第三外参计算模块7、累计误差计算模块8、标定模块9；其中，所述棋盘格靶标放置在车附近某一位置，棋盘格靶标是由多个大小相等的黑白相间的正方形组成的靶标；所述传感器安装在车上，标定系统对传感器的安装位置以及传感器的类型不做要求，传感器包括激光雷达传感器和相机传感器，激光雷达传感器与第一数据处理模块1连接，用于将检测的数据发送给第一数据处理模块1；相机传感器与第二数据处理模块2连接，用于将检测的数据发送给第二数据处理模块2；
[0226]
所述数据处理模块1，用于对激光雷达传感器发送的数据进行处理，计算棋盘格靶标角点在不同激光雷达传感器构成的激光雷达空间直角坐标系下的坐标；
[0227]
所述第二数据处理模块2，用于对相机传感器发送的数据进行处理，计算棋盘格靶标角点在不同相机传感器构成的像素平面坐标系下的坐标；
[0228]
所述传感器组选择模块3，用于根据第一数据处理模块1、第二数据处理模块2发送的数据，选择对某一位置的棋盘格靶标进行组合标定的三个传感器，选择的三个传感器分别记为传感器sa,传感器sb以及传感器sc；对传感器进行选择时，选择棋盘格靶标完整的出现在传感器视野中的传感器；
[0229]
所述传感器识别模块4，用于对组合标定的三个传感器的类型进行识别，根据传感器类型选择不同传感器之间计算外参矩阵的方法，当传感器sa与传感器sb均为激光雷达传
感器时，则利用第一外参计算模块5计算；当传感器sa与传感器sb均为相机传感器时，则利用第二外参计算模块6计算，当传感器sa为激光雷达传感器，传感器sb为相机传感器时，则利用第三外参计算模块7计算；
[0230]
所述第一外参计算模块5，用于计算激光雷达传感器sa与激光雷达传感器sb之间的外参参数，计算公式如下：
[0231][0232]
式中，w
i
为棋盘格靶标角点i的误差权重，为激光雷达传感器sa到激光雷达传感器sb的外参矩阵，为由激光雷达传感器sa扫描得到的棋盘格靶标角点在激光雷达传感器sb构成的激光雷达空间直角坐标系的坐标，x
ai
为棋盘格靶标角点在激光雷达传感器sa构成的激光雷达空间直角坐标系下的坐标；x
bi
为棋盘格靶标角点在激光雷达传感器sb构成的激光雷达空间直角坐标系下的坐标；
[0233]
所述第二外参计算模块6，用于计算相机传感器sb与相机传感器sa之间的外参参数；计算公式如下：
[0234][0235]
式中，为的逆矩阵，w
i
为棋盘格靶标角点i的误差权重，为相机传感器sb到相机传感器sa的外参矩阵；为相机传感器sb拍摄得到的棋盘格靶标角点在相机传感器sa构成的像素平面坐标系下的坐标，为相机传感器sa获取的相应棋盘格靶角点坐标，为相机传感器sb获取的相应棋盘格靶角点坐标，λ为比例因子，等于λ
a
与λ
b
的比值；相机传感器sa的尺度因子λ
a
可以根据棋盘格靶标正方形的尺寸以及相邻两个棋盘格靶标角点在相机传感器sa构成的像素平面坐标系下的距离求得；相机传感器sb的尺度因子λ
b
可以根据棋盘格靶标正方形的尺寸以及相邻两个棋盘格靶标角点在相机传感器sb构成的像素平面坐标系下的距离求得；
[0236]
所述第三外参计算模块7，用于计算激光雷达传感器sa与相机传感器sb之间的外参参数，计算公式如下：
[0237][0238]
式中：w
i
为棋盘格靶标角点i的误差权重，λ为比例系数，数值大小等于点在相机传感器sb构成的相机空间直角坐标系下的z轴坐标的倒数，为激光雷达传感器sa到相机传感器sb的外参矩阵，为激光雷达传感器sa扫描得到的棋盘格靶标角点在相机传感器sb构成的像素平面坐标系下的坐标，x
ai
为棋盘格靶标角点在激光雷达传感器sa构成的激光雷达空间直角坐标系下的坐标；x
bi
为棋盘格靶标角点在相机传感器sb构成的像素平面坐标系下的坐标；为相机传感器sb相应的内参矩阵；
[0239]
所述累计误差计算模块8，用于根据第一外参计算模块5、第二外参计算模块6、第三外参计算模块7计算的数据，计算由传感器sa与传感器sb之间的标定误差，以及传感器sb与传感器sc之间标定误差，引起的传感器sa与传感器sc之间标定误差；
[0240]
所述标定模块9，用于联合优化不同位置的棋盘格靶标角点的坐标累积误差，使得
总的标定误差最小；同时，由于不同传感器组构成一个回环，回环的旋转矩阵应为单位阵，平移矩阵应为零矩阵，以此计算外参参数。
[0241]
本领域技术人员可以理解，上述实施方式中各种方法/模块的全部或部分功能可以通过硬件的方式实现，也可以通过计算机程序的方式实现。当上述实施方式中全部或部分功能通过计算机程序的方式实现时，该程序可以存储于一计算机可读存储介质中，存储介质可以包括：只读存储器、随机存储器、磁盘、光盘、硬盘等，通过计算机执行该程序以实现上述功能。例如，将程序存储在设备的存储器中，当通过处理器执行存储器中程序，即可实现上述全部或部分功能。
[0242]
另外，当上述实施方式中全部或部分功能通过计算机程序的方式实现时，该程序也可以存储在服务器、另一计算机、磁盘、光盘、闪存盘或移动硬盘等存储介质中，通过下载或复制保存到本地设备的存储器中，或对本地设备的系统进行版本更新，当通过处理器执行存储器中的程序时，即可实现上述实施方式中全部或部分功能。
[0243]
以上应用了具体个例对本发明进行阐述，只是用于帮助理解本发明，并不用以限制本发明。对于本发明所属技术领域的技术人员，依据本发明的思想，还可以做出若干简单推演、变形或替换。