一种多激光雷达和多相机联合标定方法及系统技术方案

一种多激光雷达和多相机联合标定方法及系统,该方法包括以下步骤：步骤S1、放置棋盘格靶标；步骤S2、提取棋盘格靶标角点；步骤S3、根据传感器类型选择不同传感器之间计算外参矩阵的方法，求解其中一个传感器和其他两个传感器的外参参数；步骤S4、计算基于多观测值的棋盘格靶标角点的坐标累积误差；步骤S5、移动棋盘格靶标，计算当前组观测值下棋盘格靶标角点的坐标累积误差；步骤S6、联合优化不同位置的棋盘格靶标角点的坐标累积误差，使得总的标定误差最小；同时，由于不同传感器组构成一个回环，回环的旋转矩阵应为单位阵，平移矩阵应为零矩阵。该方法可应用于多种场景，能够保证任意两个传感器之间的外参参数的精度。意两个传感器之间的外参参数的精度。意两个传感器之间的外参参数的精度。

【技术实现步骤摘要】
一种多激光雷达和多相机联合标定方法及系统


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

技术实现思路

[0004]鉴于上述问题和缺陷，本专利技术的目的在于提供一种多激光雷达和多相机联合标定方法，该方法能够实现对多激光雷达和多相机的标定；另外，该方法联合标定时加入回环约束，要求不同传感器组构成一个回环时，回环的旋转矩阵为单位阵，平移矩阵为零矩阵,从而保证任意两个传感器之间的外参参数的精度。
[0005]为实现上述目的，本专利技术采用如下技术方案：r/>[0006]一种多激光雷达和多相机联合标定方法，该方法包括以下步骤：
[0007]步骤S1、放置棋盘格靶标，通过调整棋盘格靶标角度及高度，使得棋盘格靶标可以完整出现在至少三个传感器视野中；
[0008]步骤S2、选择三个可同时观测到完整棋盘格靶标的传感器进行组合标定；启动传感器,提取棋盘格靶标角点；其中，三个传感器分别记为传感器Sa,传感器Sb以及传感器Sc；
[0009]步骤S3、对组合标定的三个传感器进行识别，根据传感器类型选择不同传感器之间计算外参矩阵的方法，以求解传感器Sb和其他两个传感器Sa、Sc的外参参数；
[0010]如传感器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、调整指定本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种多激光雷达和多相机联合标定方法，其特征在于，该方法包括以下步骤：步骤S1、放置棋盘格靶标，通过调整棋盘格靶标角度及高度，使得棋盘格靶标可以完整出现在至少三个传感器视野中；步骤S2、选择三个可同时观测到完整棋盘格靶标的传感器进行组合标定；启动传感器,提取棋盘格靶标角点；其中，三个传感器分别记为传感器Sa,传感器Sb以及传感器Sc；步骤S3、对组合标定的三个传感器进行识别，根据传感器类型选择不同传感器之间计算外参矩阵的方法，以求解传感器Sb和其他两个传感器Sa、Sc的外参参数；如传感器Sa为激光雷达传感器，传感器Sb为相机传感器，则非同源传感器之间的外参矩阵的计算公式如下：式中：w
i
为棋盘格靶标角点i的误差权重，λ为比例系数，数值大小等于点在相机传感器Sb构成的相机空间直角坐标系下的Z轴坐标的倒数，为激光雷达传感器Sa到相机传感器Sb的外参矩阵，为激光雷达传感器Sa扫描得到的棋盘格靶标角点在相机传感器Sb构成的像素平面坐标系下的坐标，X
ai
为棋盘格靶标角点在激光雷达传感器Sa构成的激光雷达空间直角坐标系下的坐标；X
bi
为棋盘格靶标角点在相机传感器Sb构成的像素平面坐标系下的坐标；为相机传感器Sb相应的内参矩阵；步骤S4、计算基于多观测值的棋盘格靶标角点的坐标累积误差根据步骤S3得到的外参参数，通过矩阵相乘，得到传感器Sa到传感器Sc的外参参数；根据获取的外参参数计算传感器Sa获取的棋盘格靶标角点,通过转换在传感器Sc构成的像素平面坐标系下或激光雷达空间直角坐标系下的坐标，计算通过转换获取的坐标与传感器Sc获取的相应的棋盘格靶标角点的欧式距离的加权平方和，从而得到由传感器Sa与传感器Sb之间的标定误差，以及传感器Sb与传感器Sc之间标定误差，引起的传感器Sa与传感器Sc之间标定误差；步骤S5、移动棋盘格靶标，按照步骤S1、步骤S2重新选择三个传感器进行组合标定，之后按照步骤S3、步骤S4计算当前组观测值下棋盘格靶标角点的坐标累积误差，直到完成所有传感器的标定；步骤S6、联合优化不同位置的棋盘格靶标角点的坐标累积误差，使得总的标定误差最小；同时，由于不同传感器组构成一个回环，回环的旋转矩阵应为单位阵，平移矩阵应为零矩阵。2.根据权利要求1所述的一种多激光雷达和多相机联合标定方法，其特征在于，当某一位置的棋盘格靶标可被三个以上的传感器同时观测到时，则不需要移动棋盘格靶标增加标定观测的位置，只需选择多个传感器组合进行联合标定，此时该位置的标定误差为多个传感器组合标定误差之和。3.根据权利要求1或2所述的一种多激光雷达和多相机联合标定方法，其特征在于，当所述传感器为激光雷达传感器时，步骤S2提取棋盘格靶标角点的具体步骤如下：步骤S2.1、同时启动所有激光雷达传感器，等到激光雷达传感器运行稳定后，获取各个激光雷达传感器得到的棋盘格靶标放置在指定位置的点云数据；
步骤S2.2、调整指定位置的棋盘格靶标的角度，再次启动所有激光雷达传感器，得到不同角度的棋盘格靶标的点云数据；步骤S2.3、分别提取步骤S2.1和步骤S2.2中不同激光雷达传感器获取的点云数据中棋盘格角点在激光雷达空间直角坐标系下的坐标，具体步骤如下：步骤S2.3.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
，δ根据激光雷达的角度分辨率计算；步骤S2.3.2、统计步骤S2.3.1得到的每个聚类中点云的个数，剔除点云个数小于阈值的聚类；步骤S2.3.3、根据特征向量之间的余弦相似度，合并不同扫描线分割的结果；步骤S2.3.4、根据步骤S2.3.3得到的点云聚类结果，检测点云聚类中是否存在棋盘格靶标；步骤S2.3.5、如果步骤S2.3.4判断得到表示棋盘格靶标的点云聚类，求棋盘格靶标上角点在激光雷达空间直角坐标系下的坐标。4.根据权利要求1或2所述的一种多激光雷达和多相机联合标定方法，其特征在于，当所述传感器为相机传感器时，步骤S2提取棋盘格靶标角点的具体步骤如下：步骤S2.1、同时启动所有相机传感器，等到相机传感器运行稳定后，获取各个相机传感器得到的棋盘格靶标放置在指定位置的图像；步骤S2.2、调整指定位置的棋盘格靶标的角度，再次启动所有相机传感器，得到不同角度的棋盘格靶标的图像；步骤S2.3、分别提取步骤S2.1和步骤S2.2中不同相机传感器获取的图像中棋盘格靶标角点在像素平面坐标系下的坐标，具体步骤如下：步骤S2.3.1、根据已知的畸变参数，对图像进行畸变校正，然后对畸变校正后的图像f(x，y)进行高斯平滑滤波处理，减少图像的噪声，得到平滑后的图像g(x，y)，数学公式如下：公式中：表示卷积操作，σ2表示函数的宽度参数,控制函数的径向作用范围；h(x，y)表示高斯平滑滤波核函数；(x，y)表示点在像素平面坐标系下的像素坐标，其中x的值表示点在像素平面坐标系u轴方向上的投影距离，y的值表示点在像素平面坐标系v轴方向上的投影距离，exp表示以自然常数e为底的指数函数；步骤S2.3.2、对步骤S2.3.1得到的平滑后的图像g(x，y)根据下述公式构建Hessian矩
阵：公式中：g
xx g
xy
及g
yy
分别是图像g(x，y)相对于x，y的二阶偏导数；步骤S2.3.3、分解步骤S2.3.2得到的Hessian矩阵，求取Hessian矩阵的特征值，数学公式如下：公式中：λ1，λ2表示Hessian矩阵两个特征值，D为计算Hessian矩阵特征值的中间临时变量；步骤S2.3.4、根据Hessian矩阵的性质，Hessian矩阵两个特征值λ1，λ2在数值上表示图像灰度的二阶导数的极大值和极小值，取形状算子S(x，y)＝λ1×
λ2，当S取极小值时的(x0，y0)坐标即为棋盘格靶标角点的坐标，数学公式如下：公式中：表示(x0，y0)坐标处的形状算子，min表示取最小值的操作，局部范围的最小值即为极小值；步骤S2.3.5、对步骤S2.3.4得到的每一个棋盘格靶标角点c遍历周围局部区域的像素，求每个像素的图像梯度向量与该像素到角点的向量的乘积的平方和；利用梯度下降优化算法优化棋盘格靶标角点的像素坐标使得得到的乘积的和最小；数学公式如下：公式中：c
′
为步骤S2.3.4得到的像素坐标，表示像素坐标c
′
的局部邻域，表示像素P属于像素坐标c
′
的局部邻域，为像素P的图像梯度向量g
P
的转置矩阵，argmin为求最小的操作，c为优化后的棋盘格靶标角点的像素坐标；步骤S2.3.6、步骤S2.3.5得到的棋盘格靶标角点坐标即为像素平面坐标系下的棋盘格靶标角点坐标。5.根据权利要求1或2所述的一种多激光雷达和多相机联合标定方法，其特征在于，当组合标定的传感器中传感器Sa与传感器Sb为激光雷达传感器时，步骤S3在求解传感器Sb与传感器Sa的外参参数时，采用梯度下降算法迭代优化传感器Sb与传感器Sa的外参矩阵；其中，激光雷达传感器Sa到激光雷达传感器Sb的外参矩阵的计算公式如下：式中，w
i
为棋盘格靶标角点i的误差权重，为激光雷达传感器Sa到激光雷达传感器Sb的外参矩阵，为由激光雷达传感器Sa扫描得到的棋盘格靶标角点在激光雷达传感器Sb构成的激光雷达空间直角坐标系的坐标，X
ai
为棋盘格靶标角点在激光雷达传感器Sa构成的激光雷达空间直角坐标系下的坐标；X
bi
为棋盘格靶标角点在激光雷达传感器Sb构成的激光雷达空间直角坐标系下的坐标。
6.根据权利要求1或2所述的一种多激光雷达和多相机联合标定方法，其特征在于，当组合标定的传感器中传感器Sa与传感器Sb为相机传感器时，步骤S3在求解传感器Sb与传感器Sa的外参参数时，采用梯度下降算法迭代优化传感器Sb与传感器Sa的外参矩阵；其中，相机传感器Sa以及Sb相应的内参矩阵已知，相机传感器Sa的尺度因子λ
a
可以根据棋盘格靶标正方形的尺寸以及相邻两个棋盘格靶标角点在相机传感器Sa构成的像素平面坐标系下的距离求得，相机传感器Sb的尺度因子λ
b
可以根据棋盘格靶标正方形的尺寸以及相邻两个棋盘格靶标角点在相机传感器Sb构成的像素平面坐标系下的距离求得；相机传感器Sb到相机传感器Sa的外参矩阵的计算公式如下：式中，为的逆矩阵，w
i
为棋盘格靶标角点i的误差权重，为相机传感器Sb到相机传感器Sa的外参矩阵；为相机传感器Sb拍摄得到的棋盘格靶标角点在相机传感器Sa构成的像素平面坐标系下的坐标，为相机传感器Sa获取的相应棋盘格靶角点...

【专利技术属性】
技术研发人员：陈禹行，刘立，
申请(专利权)人：北京易航远智科技有限公司，
类型：发明
国别省市：