CN113589263B - 一种多个同源传感器联合标定方法及系统 - Google Patents

Abstract

一种多个同源传感器联合标定方法及系统,该方法包括以下步骤：步骤S1、放置棋盘格靶标，使棋盘格靶标在至少三个传感器中视野良好；步骤S2、提取棋盘格靶标角点并选择三个传感器进行组合标定；步骤S3、计算基于多观测值的棋盘格靶标角点的坐标累积误差；步骤S4、判断是否需要继续移动棋盘格靶标，增加观测值；每增加一组观测值，则需要按照步骤S1至S3计算当前组观测值下棋盘格靶标角点的坐标累积误差；步骤S5、联合优化不同位置的棋盘格靶标角点的坐标累积误差，使得总的标定误差最小；同时，由于不同传感器组构成一个回环，回环的旋转矩阵应为单位阵，平移矩阵应为零矩阵。该方法可应用于多种场景，能够保证任意两个传感器之间的外参参数的精度。

Description

一种多个同源传感器联合标定方法及系统

技术领域

本发明属于传感器标定技术领域，具体涉及一种多个同源传感器联合标定方法及系统。

现有技术

受传感器视场角和传感器分辨率的影响，单一的传感器很难精确感知周围环境的所有特征信息。为了实现对周围环境的精确感知和自身定位，现有无人驾驶车辆或智能机器人需要融合多传感器感知的信息，而多传感器标定是实现多传感器信息融合的必要步骤和先决条件。

目前，多激光雷达或多相机的标定主要集中在两个激光雷达或两个相机之间的标定，很少工作研究传感器数目大于2个的激光雷达或相机之间的标定。对于两个激光雷达之间的标定流程，主要是通过两个激光雷达同时拍摄一张平面棋盘格靶标，然后分别从两个激光雷达扫描得到的数据中提取棋盘格靶标的平面，通过匹配两个平面使得两个棋盘格靶标平面重合，从而得到两个激光雷达之间的外参参数。然而，这种标定方法由于无法保证所有的激光雷达可以同时观测到同一个棋盘格靶标；因此，不适用于多激光雷达的标定。对于两个相机之间的外参标定流程，主要是通过张正友标定法标定单个相机的内参矩阵及外参矩阵，通过级线约束求得两个相机之间的外参参数，但这种方法比较复杂，不适合进行多个相机之间的标定。

为了实现对多个相机的标定，中国专利CN 110458952 A公开了《一种基于三目视觉的标定方法》，该方法根据SIFT(尺度不变特征变换,Scale Invariant FeatureTransform)算子，采用汉明距离方法,两两匹配不同图像的对应控制点，然后采用光束平差参数优化法，优化两两相机之间的外部参数。但是该方法要求三个相机安装在同一水平线上，无法适用于自动驾驶车辆，因为对于自动驾驶车辆，相机安装位置不能保证这一条件总是成立。

中国专利CN 111553955 A公开了《一种多目相机三维系统及其标定方法》，该方法对每两组相机之间分别进行双目标定，得到相机组两两之间的外参参数，然后通过非线性最小二乘法优化共同区域的特征点的投影误差。但是该方法根据相机测得的特征点计算重投影误差时，被默认为不同特征点对标定误差具有相同的权重，容易出现因某些检测的特征点误差过大，造成得到的传感器之间的外参参数的误差过大。

中国专利CN 106228564 A公开了《一种多目相机外参两步联合在线标定的方法和系统》，该方法利用已知基准点的坐标，基于四元数的共线交会原理获取多目相机的初始外参，然后利用稀疏法光束平差，得到相机间的精确外参参数。但是该方法需要精确获取基准点的坐标，降低了该方法的适用性。

此外，上述多激光雷达或多相机之间相互标定方法要求天气良好、视野清楚，对标定场景要求较高。

综上可知，现有多相机或者多激光雷达的标定方法仍存在适用场景受限，适用性差，以及无法保证任意两个激光雷达或相机之间的外参参数精度的技术问题。

发明内容

鉴于上述问题和缺陷，本发明的目的在于提供一种多个同源传感器联合标定方法，该方法联合标定时加入回环约束，要求不同传感器组构成一个回环时，回环的旋转矩阵为单位阵，平移矩阵为零矩阵,从而保证任意两个传感器之间的外参参数的精度；另外，该方法通过引入不同特征点的误差权重，使其可以应用于多种场景，且通过设置不同的误差权重，保证整体优化时惩罚引入误差较大的特征点，避免因为某些检测的特征点误差过大，造成得到的传感器之间的外参参数的误差过大。

为实现上述目的，本发明采用如下技术方案：

一种多个同源传感器联合标定方法，该方法包括以下步骤：

步骤S1、放置至少一个棋盘格靶标，通过调整棋盘格靶标角度及高度，使得棋盘格靶标在至少三个传感器中视野良好；

步骤S2、提取棋盘格靶标角点并选择三个传感器进行组合标定，选择的三个传感器分别记为传感器Sa，传感器Sb以及传感器Sc；

步骤S3、计算基于多观测值的棋盘格靶标角点的坐标累积误差

步骤S3.1、分别对组合标定的三个传感器中任意两个传感器提取得到的棋盘格靶标角点进行匹配，保留两个传感器共同提取的棋盘格靶标角点；

步骤S3.2、根据步骤S3.1得到的棋盘格靶标角点坐标，分别求解传感器Sb和其他两个传感器Sa、Sc的外参参数；

步骤S3.3、根据步骤S3.2得到的外参参数，通过矩阵相乘，得到传感器Sa到传感器Sc的外参参数；根据获取的外参参数计算传感器Sa获取的棋盘格靶标角点，通过转换在传感器Sc构成的像素平面坐标系下或激光雷达空间直角坐标系下的坐标，计算通过转换获取的坐标与传感器Sc获取的相应的棋盘格靶标角点的欧式距离的加权平方和，从而得到由传感器Sa与传感器Sb之间的标定误差，以及传感器Sb与传感器Sc之间标定误差，引起的传感器Sa与传感器Sc之间标定误差；

步骤S4、判断是否需要继续移动棋盘格靶标，增加观测值；每增加一组观测值，则需要按照步骤S1至S3计算当前组观测值下棋盘格靶标角点的坐标累积误差；

步骤S5、联合优化不同位置的棋盘格靶标角点的坐标累积误差，使得总的标定误差最小；同时，由于不同传感器组构成一个回环，回环的旋转矩阵应为单位阵，平移矩阵应为零矩阵。

作为本发明的优选，步骤S2在选择组合标定的三个传感器时，选择标准如下：

a)优先选择提取棋盘格靶标角点数目最多的传感器；

b)如果从两个传感器扫描数据中提取的棋盘格靶标角点数目相同，优先选择重心距离棋盘格靶标平面更近的传感器；如果选择多个棋盘格靶标同时进行观测，比较传感器重心到不同棋盘格靶标平面的距离的平均值；

c)如果从两个传感器扫描数据中提取的棋盘格靶标角点数目相同，并且传感器重心距离棋盘格靶标平面相等，则选择测距精度或分辨率精度更高的传感器；

d)如果上述3个条件均无法选择唯一的传感器组合，则选择多个传感器组合分别进行联合标定，此时观测位置的标定误差为多个传感器组合的标定误差之和。

作为本发明的优选，步骤S4在判断是否需要继续移动棋盘格靶标时，判断标准如下：

a)根据传感器在车上的安装位置，以传感器为顶点，对于有共同观测视野的两两传感器之间用线相连作为边，构建无向图，以处于中心的传感器为中心顶点；

b)初始化步骤a)构建的无向图，令每个顶点的访问次数为0，每条边的访问次数为0；

c)当选取传感器中的任意三个传感器做组合标定时，无向图中传感器对应的顶点的访问次数加1，两两传感器构成的边的访问次数加1；

d)如果无向图中每个顶点的访问次数至少为1，并且中心顶点与其他任意顶点相连构成的边的访问次数至少为2，则无需继续移动棋盘格靶标；否则需要移动棋盘格靶标，增加观测值。

作为本发明的优选，步骤S5联合优化时，计算总的标定误差的计算公式如下：

E_total＝min(W₁E_p1+W₂E_p2+W₃E_p3+…+W_nE_pn)

式中，W_n为第n组传感器组合标定误差的权重，可以通过计算不同传感器组合的测距精度或分辨率设定，传感器的测距精度或分辨率越高，其所对应的标定误差权重越大；传感器的测距精度或分辨率越低，其所对应的标定误差权重越小；E_pn为第n组传感器组合时由传感器Sa与传感器Sb之间的标定误差，以及传感器Sb与传感器Sc之间标定误差，引起的传感器Sa与传感器Sc之间标定误差。

作为本发明的优选，当所述传感器为激光雷达时，步骤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判断得到表示棋盘格靶标的点云聚类，求棋盘格靶标上角点在激光雷达空间直角坐标系下的坐标。

作为本发明的优选，当所述传感器为激光雷达时，步骤S3.2在求解传感器Sb与其他两个传感器Sa、Sc的外参参数时，均采用梯度下降算法迭代优化传感器Sb与传感器Sa、Sc的外参矩阵；

其中，传感器Sa到传感器Sb的外参矩阵的计算公式如下：

式中，w_i为棋盘格靶标角点i的误差权重，为激光雷达传感器Sa到激光雷达传感器Sb的外参矩阵，为由激光雷达传感器Sa扫描得到的棋盘格靶标角点在激光雷达传感器Sb构成的激光雷达空间直角坐标系的坐标，X_ai为棋盘格靶标角点在激光雷达传感器Sa构成的激光雷达空间直角坐标系下的坐标；X_bi为棋盘格靶标角点在激光雷达传感器Sb构成的激光雷达空间直角坐标系下的坐标；

激光雷达传感器Sc到激光雷达传感器Sb的外参矩阵的计算公式如下：

式中，w_i为棋盘格靶标角点i的误差权重，为激光雷达传感器Sc到激光雷达传感器Sb的外参矩阵，为由激光雷达传感器Sc扫描得到的棋盘格靶标角点在激光雷达传感器Sb构成的激光雷达空间直角坐标系的坐标，X_bi为棋盘格靶标角点在激光雷达传感器Sb构成的激光雷达空间直角坐标系下的坐标；X_ci为棋盘格靶标角点在激光雷达传感器Sc构成的激光雷达空间直角坐标系下的坐标。

作为本发明的优选，当所述传感器为相机时，步骤S2提取棋盘格靶标角点的具体步骤如下：

步骤S2.1、同时启动所有相机传感器，等到相机传感器运行稳定后，获取各个相机传感器得到的棋盘格靶标放置在指定位置的图像；

步骤S2.2、调整指定位置的棋盘格靶标的角度，再次启动所有相机传感器，得到不同角度的棋盘格靶标的图像；

步骤S2.3、分别提取步骤S2.1和步骤S2.2中不同相机传感器获取的图像中棋盘格靶标角点在像素平面坐标系下的坐标，具体步骤如下：

步骤S2.3.1、根据已知的畸变参数，对图像进行畸变校正，然后对畸变校正后的图像f(x，y)进行高斯平滑滤波处理，减少图像的噪声，得到平滑后的图像g(x，y)，数学公式如下：

公式中：表示卷积操作，σ²表示函数的宽度参数，控制函数的径向作用范围；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矩阵的特征值，数学公式如下：

公式中：λ₁，λ₂表示Hessian矩阵两个特征值，D为计算Hessian矩阵特征值的中间临时变量；

步骤S2.3.4、根据Hessian矩阵的性质，Hessian矩阵两个特征值λ₁，λ₂在数值上表示图像灰度的二阶导数的极大值和极小值，取形状算子S(x，y)＝λ₁×λ₂，当S取极小值时的(x₀，y₀)坐标即为棋盘格靶标角点的坐标，数学公式如下：

公式中：表示(x₀，y₀)坐标处的形状算子，min表示取最小值的操作，局部范围的最小值即为极小值；

步骤S2.3.5、对步骤S2.3.4得到的每一个棋盘格靶标角点c遍历周围局部区域的像素，求每个像素的图像梯度向量与该像素到角点的向量的乘积的平方和；利用梯度下降优化算法优化棋盘格靶标角点的像素坐标使得得到的乘积的和最小；数学公式如下：

公式中：c′为步骤S2.3.4得到的像素坐标，表示像素坐标c′的局部邻域，表示像素P属于像素坐标c′的局部邻域，为像素P的图像梯度向量gP的转置矩阵，argmin为求最小的操作，c为优化后的棋盘格靶标角点的像素坐标；

步骤S2.3.6、步骤S2.3.5得到的棋盘格靶标角点坐标即为像素平面坐标系下的棋盘格靶标角点坐标。

作为本发明的优选，当所述传感器为相机时，步骤S3.2在求解传感器Sb与其他两个传感器Sa、Sc的外参参数时，均采用梯度下降算法迭代优化传感器Sb与传感器Sa、Sc的外参矩阵；其中，相机传感器Sa以及Sb相应的内参矩阵已知，相机传感器Sa的尺度因子λ_a可以根据棋盘格靶标正方形的尺寸以及相邻两个棋盘格靶标角点在相机传感器Sa构成的像素平面坐标系下的距离求得，相机传感器Sb的尺度因子λ_b可以根据棋盘格靶标正方形的尺寸以及相邻两个棋盘格靶标角点在相机传感器Sb构成的像素平面坐标系下的距离求得；

传感器Sb到传感器Sa的外参矩阵的计算公式如下：

式中，为的逆矩阵，w_i为棋盘格靶标角点i的误差权重，为相机传感器sb到相机传感器Sa的外参矩阵；为相机传感器Sb拍摄得到的棋盘格靶标角点在相机传感器Sa构成的像素平面坐标系下的坐标，为相机传感器Sa获取的相应棋盘格靶角点坐标，为相机传感器Sb获取的相应棋盘格靶角点坐标，λ为比例因子，等于λ_a与λ_b的比值；

同理，可计算传感器Sc到传感器Sb的外参矩阵。

作为本发明的进一步优选，步骤S2.3.5根据点云聚类计算棋盘格靶标的角点在激光雷达空间直角坐标系下的坐标时，具体步骤如下：

a)建立点云棋盘格坐标系及棋盘格靶标坐标系，将点云聚类的所有点投影到点云棋盘格坐标系XOY平面；此时，经处理得到的点云棋盘格坐标系XOY平面不能与棋盘格靶标坐标系重合；

b)根据点云聚类中每个点的灰度值，建立灰度值直方图；根据直方图最大的两个峰值R_L和R_H，求出用于判断某个点对应的颜色的阈值范围[τ_l，τ_h]；即如果一个点的灰度值大于上边界τ_h，该点表示白色；如果一个点的灰度值小于下边界τ_l，该点表示黑色，数学公式如下：

公式中：I_i为点云i的灰度值，C_i为该点表示的颜色，其中0表示白色，1表示黑色；

公式中：ε_g为一个大于2的常数，通常取4；

c)根据已知的棋盘格的类型、每个正方形的尺寸L_Grid以及棋盘格靶标左下角正方形的颜色C_(0，0)，计算每个正方形的颜色以及该正方形左下角及右上角在棋盘格靶标坐标系的坐标，数学公式如下：

公式中：i，j表示每个正方形的行列号，C_(i，j)表示行列号分别为i，j的正方形的颜色，N_C，N_R分别为每行/每列正方形的个数；

d)计算点云聚类中每个点的损失值；

e)根据Powell共轭方向集方法对步骤d)计算得到的点云聚类中所有点的损失值C_m进行优化，使得总的损失值C_m最小，可以得到优化后点云棋盘格坐标系到棋盘格靶标坐标系的转换矩阵

f)计算棋盘格靶标的角点在激光雷达空间直角坐标系下的坐标，数学公式如下：

公式中：为优化后点云棋盘格坐标系到棋盘格靶标坐标系的转换矩阵，可由步骤e)获得；为激光雷达空间直角坐标系转换到点云棋盘格坐标系的转换矩阵，旋转分量为平移分量为点云中心在激光雷达空间直角坐标系下的坐标，可由a)获得。

作为本发明的进一步优选，步骤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的曼哈顿距离；数学公式如下：

式中：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表示计算曼哈顿距离的函数；Δx₁，Δx₂，Δy₁，Δy₂表示点i到所在正方形四条边的距离；min表示取最小的输入值的操作。

本发明的另一目的在于一种多个同源传感器联合标定系统，该系统包括棋盘格靶标、至少四个同源传感器、数据处理模块、传感器组选择模块、特征点匹配模块、外参计算模块、累计误差计算模块、观测值判断模块、标定模块；其中，所述传感器采用激光雷达或相机，传感器与数据处理模块连接，用于将检测的数据发送给数据处理模块；

所述数据处理模块，用于对传感器发送的数据进行处理，计算棋盘格靶标角点在不同传感器构成的像素平面坐标系下或激光雷达空间直角坐标系下的坐标；

所述传感器组选择模块，用于根据数据处理模块发送的数据，选择对某一位置的棋盘格靶标进行组合标定的三个传感器，选择的三个传感器分别记为传感器Sa、传感器Sb以及传感器Sc；对传感器进行选择时，优先选择提取棋盘格靶标角点数目最多的传感器；如果从两个传感器扫描数据中提取的棋盘格靶标角点数目相同，优先选择重心距离棋盘格靶标平面更近的传感器；如果从两个传感器扫描数据中提取的棋盘格靶标角点数目相同，并且传感器重心距离棋盘格靶标平面相等，则选择测距精度或分辨率精度更高的传感器；

所述特征点匹配模块，用于根据传感器组选择模块的选择结果以及数据处理模块的计算结果，分别对组合标定的三个传感器中任意两个传感器提取得到的棋盘格靶标角点进行匹配，保留两个传感器共同提取的棋盘格靶标角点；

所述外参计算模块，用于根据特征点匹配模块的匹配结果，计算传感器Sb与其他两个传感器Sa、Sc的外参参数；

所述累计误差计算模块，用于根据外参计算模块计算的数据计算由传感器Sa与传感器Sb之间的标定误差，以及传感器Sb与传感器Sc之间标定误差，引起的传感器Sa与传感器Sc之间标定误差；

所述观测值判断模块，用于判断是否需要继续移动棋盘格靶标，增加观测值；每增加一组观测值，则需要计算当前组观测值下棋盘格靶标角点的坐标累积误差；

所述标定模块，联合优化不同位置的棋盘格靶标角点的坐标累积误差，使得总的标定误差最小；同时，由于不同传感器组构成一个回环，回环的旋转矩阵应为单位阵，平移矩阵应为零矩阵，以此计算外参参数。

本发明的优点和技术效果是：

1、本发明提供的标定方法不要求所有传感器都有共同视野，不要求传感器的安装位置以及数目，该方法可根据提取的棋盘格靶标特征点选择进行组合标定的传感器，之后根据多个位置不同的组合标定，进行外参参数的优化。

2、本发明在多激光雷达和多相机联合标定时加入回环约束，要求不同传感器组构成一个回环时，回环的旋转矩阵为单位阵，平移矩阵为零矩阵,从而保证任意两个传感器之间的外参参数的精度。

3、本发明通过引入不同特征点的误差权重，降低低分辨率传感器引起的标定误差的权重，使其可以适用于多种标定场景，不必要求标定时天气良好、视野清楚；如果天气良好、视野清楚，可以得到高精度的特征点，则所有特征点的误差权重均设为1；如果视野不佳，对相机检测特征点造成影响，则降低相机检测的特征点的误差权重；如果天气环境不好，则降低距离传感器较远的特征点的误差权重。

4、本发明通过设置不同的误差权重，保证整体优化时惩罚引入误差较大的特征点，避免因为某些检测的特征点误差过大，造成得到的传感器之间的外参参数的误差过大。

5、本发明在进行联合标定时，提出了根据传感器的位置，以传感器为顶点，两个传感器相连为边构建无向图，通过计算无向图中每个顶点以及每条边的访问次数，判断是否需要移动棋盘格靶标，增加观测值，该判断方法可以避免额外观测，减少工作量。

附图说明

图1是多激光雷达/多相机在车辆上的安装示意图之一；

图2是多激光雷达/多相机在车辆上的安装示意图之二；

图3是本发明标定方法的整体流程图；

图4是点云棋盘格坐标系示意图；

图5是点云棋盘格坐标系XOY平面与棋盘格靶标坐标系误差示意图；

图6是判断直方图峰值的示意图；

图7a是点在格子内时，根据点与棋盘格的位置关系计算曼哈顿距离的示意图；

图7b是点在棋盘格外部时，根据点与棋盘格的位置关系计算曼哈顿距离的示意图；

图8是图1中的传感器的无向图的拓扑结构；

图9是图1中的传感器S2与S5互换位置后的示意图；

图10是优化角点坐标的原理图；

图11是本发明标定系统的结构框图。

具体实施方式

下面将结合本申请实施例中的附图，对本申请实施例中的技术方案进行清楚、完整地描述；显然，所描述的实施例仅是本申请一部分实施例，而不是全部的实施例。基于本申请中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本申请保护的范围。

另外，在以下的实施方式中，很多细节描述是为了使得本申请能被更好的理解。然而，本领域技术人员可以毫不费力的认识到，其中部分特征在不同情况下是可以省略的，或者可以由其它方法所替代。在某些情况下，本申请相关的一些操作并没有在说明书中显示或者描述，这是为了避免本申请的核心部分被过多的描述所淹没，而对于本领域技术人员而言，详细描述这些相关操作并不是必要的，他们根据说明书中的描述以及本领域的一般技术知识即可完整了解相关操作。

需要说明的是，在不冲突的情况下，本公开中的实施方式及实施方式中的特征可以相互组合。下面将参考附图并结合实施方式来详细说明本公开的技术方案。

本发明旨在提供一种多个同源传感器联合标定方法，图1描述了本发明同源传感器联合标定时采用5个相机或激光雷达时，相机或激光雷达的安装位置以及棋盘格靶标摆放位置的示意图；其中，S1至S5代表相机或激光雷达，P1至P4代表棋盘格靶标；图2描述了本发明同源传感器联合标定时采用6个相机或激光雷达时，相机或激光雷达的安装位置以及棋盘格靶标摆放位置的示意图；其中，S1至S6代表相机或激光雷达，P1至P4代表棋盘格靶标；图3描述了本发明同源传感器联合标定方法的整体流程图。

实施例1所有传感器均为激光雷达传感器时的标定方法

如图1至图3所示，本发明提供的一种多个同源传感器联合标定方法，具体标定过程如下：

步骤S1、放置棋盘格靶标

将至少一个棋盘格靶标放置在距离大致在车前5-10m的P1位置，调整棋盘格靶标角度及高度，使得棋盘格靶标在至少三个激光雷达传感器中视野良好，可以完整出现在激光雷达传感器的扫描范围中；

其中，所述棋盘格靶标中心的高度与激光雷达传感器S1的安装高度(S1重心到地面的垂直距离)大致相等；棋盘格靶标每行每列包含的正方形的个数(N_C，N_R)以及每个正方形的尺寸L_Grid均是已知。

此步骤需要说明，如果选择放置多个棋盘格靶标，则需保证棋盘格靶标之间不能相互遮挡。另外，本发明对棋盘格靶标的放置顺序不做要求，可以先标定P2或者P3位置的棋盘格靶标，本实施例只举例最先标定放置在P1位置的棋盘格靶标这种情况。

步骤S2、提取棋盘格靶标角点并选择三个激光雷达传感器进行组合标定

步骤S2.1、同时启动所有激光雷达传感器，等到激光雷达传感器运行稳定后，获取各个激光雷达传感器得到的棋盘格靶标放置在P1位置的点云数据；

步骤S2.2、调整P1位置的棋盘格靶标的角度，再次启动所有激光雷达传感器，得到不同角度的棋盘格靶标的点云数据；

步骤S2.3、分别提取步骤S2.1和步骤S2.2中不同激光雷达传感器获取的点云数据中棋盘格角点在激光雷达空间直角坐标系下的坐标；其中，激光雷达空间直角坐标系是以激光雷达重心为原点，Y轴指向车前进的方向，Z轴垂直于车身，指向车顶，X轴与Y轴Z轴共同形成右手坐标系；激光雷达空间直角坐标系是局部坐标系，不同激光雷达可以构成不同激光雷达空间直角坐标系，同一激光雷达在不同位置构成不同的激光雷达空间直角坐标系；所述点云数据由多条扫描线组成，每一条扫描线由多个点组成；所述步骤S2.3的具体步骤如下：

步骤S2.3.1、对组成点云数据的每条扫描线，逐点计算相邻两个点的距离和角度，将一条扫描线分割成不同聚类，数学公式如下：

公式(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_i，δ根据激光雷达的角度分辨率计算。

步骤S2.3.2、统计步骤S2.3.1得到的每个聚类中点云的个数，剔除点云个数小于阈值的聚类；点云个数的阈值根据聚类中心到激光雷达重心的欧式距离及激光雷达的角度分辨率计算。

步骤S2.3.3、合并不同扫描线分割的结果；计算每个类别的特征向量，并根据特征向量之间的余弦相似度，合并余弦相似度高的扫描线。

步骤S2.3.4、根据步骤S2.3.3得到的点云聚类结果，检测点云聚类中是否存在棋盘格靶标；判断标准有3个，如下所示：

a)对每个点云聚类进行主成分分析(Principle Component Analysis，PCA)分解，得到每个聚类的特征向量和相应特征值(θ₁，θ₂，θ₃)，根据特征值(θ₁，θ₂，θ₃)计算各聚类区域的形状描述子；其中，所述形状描述子包括线性描述子、平面描述子和球状描述子三类。如果一个点云聚类表示棋盘格靶标，该聚类应该满足计算得到的平面描述子的值大于线性描述子和球状描述子，数学公式如下：

公式(2)：D_1D为线性描述子，D_2D为平面描述子，D_3D为球状描述子，max表示取最大值的操作；

b)统计每个点云聚类包含的点云的数目N_n，如果一个点云聚类表示棋盘格靶标，则该聚类包含的点的个数应该满足数学公式，如下所示：

公式(3)：N_thre表示激光雷达能扫描到的最多的点的个数，d_W，d_H分别是棋盘格靶标的宽和高，r为聚类中心到激光雷达重心的距离，Δh，Δv分别是激光雷达的水平和垂直分辨率，表示将输入值向下取整的操作。

c)对每个聚类进行KS(Kolmogorov-Smirnov)检验，检查聚类的点是否满足均匀分布，如果满足均匀分布，KS检验得到的D值应该小于阈值；其中，D值表示样本所属总体的分布与均匀分布之间的最大距离；阈值可以通过查找KS单样本检验临界值表获得；KS单样本检验临界值表是一张列举不同显著性水平及不同样本数量下的D的临界值的表格，根据聚类中的点云的个数以及提前设定的显著性水平，可以在KS单样本检验临界值表找到对应的最大的临界值，如果KS检验得到的D值大于该临界值，表明聚类不满足均匀分布；

如果某个点云聚类同时满足上述三个条件，则认为该点云聚类表示棋盘格靶标。如果不满足上述任意一个条件，则剔除该点云聚类，不进行后续操作。

步骤S2.3.5、如果步骤S2.3.4判断得到表示棋盘格靶标的点云聚类，求棋盘格靶标上角点(两个正方形相交的顶点)在激光雷达空间直角坐标系下的坐标，具体步骤如下：

a)以点云聚类的中心为原点，为X轴，为Y轴，为Z轴，旋转坐标轴，使得Z轴平行于点云法向量并指向激光雷达，X轴的指向与激光雷达空间直角坐标系下的X轴相同，以此得到的坐标系称为点云棋盘格坐标系(如图4所示)。之后以棋盘格靶标中心为原点，X轴平行棋盘格平面并且指向和点云棋盘格坐标系中X轴指向一致，Y轴平行棋盘格平面朝上，建立棋盘格靶标坐标系。

将点云聚类的所有点投影到点云棋盘格坐标系XOY平面，理想情况下投影后的点应该在棋盘格靶标范围内，但是由于激光雷达与棋盘格靶标之间不一定是垂直扫描，可能存在一定的角度，因而扫描得到的棋盘格点云可能存在坐标位置误差，使得计算得到的棋盘格点云特征向量不准确，因此经过步骤S2.3.5)中a)处理得到的点云棋盘格坐标系XOY平面不能与棋盘格靶标坐标系重合(如图5所示)。

b)根据点云聚类中每个点的灰度值，建立灰度值直方图；根据直方图最大的两个峰值R_L和R_H(如图6所示)，求出用于判断某个点对应的颜色的阈值范围[τ_l，τ_h]。即如果一个点的灰度值大于上边界τ_h，该点表示白色；如果一个点的灰度值小于下边界τ_l，该点表示黑色。数学公式如下：

公式(4)中：I_i为点云i的灰度值，C_i为该点表示的颜色，其中0表示白色，1表示黑色。

公式(5)中：ε_g为一个大于2的常数，通常取4；

c)根据已知的棋盘格的类型(即每行每列包含的正方形的个数)、每个正方形的尺寸L_Grid以及棋盘格靶标左下角正方形的颜色C_(0，0)，计算每个正方形的颜色以及该正方形左下角及右上角在棋盘格靶标坐标系的坐标，数学公式如下：

公式(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的正方形右上角在棋盘格靶标坐标系的坐标；

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的曼哈顿距离；数学公式如下：

公式(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表示计算曼哈顿距离的函数。Δx₁，Δx₂，Δy₁，Δy₂表示点i到所在正方形四条边的距离，min表示取最小的输入值的操作。

举例说明，点P的灰度值在[τ_l，τ_h]内，则点P对应的损失系数为0，即点P的损失值为0；否则判断点P是否在棋盘格内部，若点P在棋盘格内部，如图7(a)所示，因为点P距离顶点V4的曼哈顿距离最近，则点P的损失值为如果点P表示的颜色与顶点V4的颜色相同，则P点的损失值为0，否则为(Δx₁+Δy₁)；如点P在棋盘格外部，如图7(b)所示，因为点P距离棋盘格顶点G4的曼哈顿距离最近，则点P的损失值为(Δx₁+Δy₁)。

e)由步骤S2.3.5中b)可知棋盘格特征向量不准确，因此经过步骤S2.3.5中a)处理得到的点云棋盘格坐标系XOY平面不能与棋盘格靶标坐标系重合，根据公式(7)计算得到的C_m不等于0；根据Powell共轭方向集方法对公式(7)进行优化，使得计算得到总的损失值C_m最小，可以得到优化后点云棋盘格坐标系到棋盘格靶标坐标系的转换矩阵

f)计算棋盘格靶标的角点在激光雷达空间直角坐标系下的坐标，数学公式如下：

公式(8)中：为优化后点云棋盘格坐标系到棋盘格靶标坐标系的转换矩阵，可由步骤S2.3.5中e)获得；为激光雷达空间直角坐标系转换到点云棋盘格坐标系的转换矩阵，旋转分量为平移分量为点云中心在激光雷达空间直角坐标系下的坐标，可由步骤S2.3.5中a)获得。

步骤S2.4、根据步骤S2.3提取的不同激光雷达传感器扫描得到的棋盘格靶标角点，选择三个激光雷达传感器进行联合标定，根据提取的棋盘格靶标角点选择激光雷达传感器的标准如下：

a)优先选择提取棋盘格靶标角点数目最多的激光雷达传感器；

b)如果从两个激光雷达传感器扫描数据中提取的棋盘格靶标角点数目相同，优先选择重心距离棋盘格靶标平面更近的激光雷达传感器；如果选择多个棋盘格靶标同时进行观测，可以比较激光雷达传感器重心到不同棋盘格靶标平面的距离的平均值；

c)如果从两个激光雷达传感器扫描数据中提取的棋盘格靶标角点数目相同，并且激光雷达传感器重心距离棋盘格靶标平面相等，则选择测距精度更高的激光雷达传感器；

d)如果上述3个条件均无法选择唯一的激光雷达传感器组合，则选择多个激光雷达传感器组合分别进行联合标定；

本申请只选择三个激光雷达传感器进行联合标定的目的是计算某激光雷达传感器分别与其他两个激光雷达传感器标定产生的外参矩阵误差造成其他两个激光雷达传感器之间的外参矩阵的误差，并且使得计算便捷；根据上述原则，P1位置选取进行联合标定的激光雷达传感器为S1、S2与S3。

步骤S3、计算基于多观测值的棋盘格靶标角点的坐标累积误差

步骤S3.1、由于从不同的激光雷达传感器扫描数据中提取的棋盘格靶标角点的数目可能不同，即从激光雷达传感器S1扫描得到的棋盘格靶标角点，可能在从激光雷达传感器S2扫描数据中提取的棋盘格靶标角点中找不到对应点。因此采用正态分布变换(NormalDistribution Transform，NDT)算法分别对激光雷达传感器S1与S2、激光雷达传感器S1与S3、激光雷达传感器S2与S3提取得到的棋盘格靶标角点进行匹配，只保留两个激光雷达传感器共同提取的棋盘格靶标角点。

步骤S3.2、根据步骤S3.1得到的激光雷达传感器S1与S2，以及激光雷达传感器S2与S3共同提取的棋盘格靶标角点在不同激光雷达空间直角坐标系下的坐标，分别求解激光雷达传感器S2到激光雷达传感器S1的外参矩阵激光雷达传感器S3到激光雷达传感器S2的外参矩阵

以求解激光雷达传感器S1到S2的外参为例：

将棋盘格靶标角点在激光雷达传感器S1构成的激光雷达空间直角坐标系下的坐标X1通过外参矩阵转换到激光雷达传感器S2构成的激光雷达空间直角坐标系中，获取由激光雷达传感器S1扫描得到的棋盘格靶标角点在激光雷达传感器S2构成的激光雷达空间直角坐标系的坐标燃后计算与激光雷达传感器S2获取的相应棋盘格靶角点坐标X₂的欧式距离的加权平方和Eerr；通过梯度下降算法迭代优化激光雷达传感器S1到激光雷达传感器S2的外参矩阵使得误差最小。为的逆矩阵。梯度下降算法迭代的终止条件是迭代的次数大于给定的相应阈值或者梯度下降的幅度小于相应的阈值。数学公式如下：

公式(9)中：w_i为棋盘格靶标角点i的误差权重，w_i有多种求解方法，公式(9)选取棋盘格靶标角点到激光雷达传感器S1重心的距离的倒数作为权重。

根据相同的原理，求解激光雷达传感器S3到激光雷达传感器S2的外参矩阵方法原理如下：

将棋盘格靶标角点在激光雷达传感器S3构成的激光雷达空间直角坐标系下的坐标X₃通过外参矩阵转换到激光雷达传感器S2构成的激光雷达空间直角坐标系下，得到由激光雷达传感器S3扫描得到的棋盘格靶标角点在激光雷达传感器S2构成的激光雷达空间直角坐标系下的坐标燃后计算与激光雷达传感器S2获取的相应标定盘角点X₂的欧式距离的加权平方和E_err；通过梯度下降法迭代优化外参矩阵使得误差Eerr最小，梯度下降算法迭代的终止条件是迭代的次数大于给定的相应阈值或者梯度下降的幅度小于相应的阈值。数学公式如下：

公式(10)中：w_i为棋盘格靶标角点i的误差权重，公式(10)选取棋盘格靶标角点到传感器S2重心的距离的倒数作为权重。

步骤S3.3、根据步骤S3.2得到的激光雷达传感器S1到激光雷达传感器S2的外参矩阵以及激光雷达传感器S2到激光雷达传感器S3的外参矩阵通过矩阵相乘，得到激光雷达传感器S1到激光雷达传感器S3的外参矩阵将棋盘格靶标角点在激光雷达传感器S1构成的激光雷达空间直角坐标系下的坐标X₁通过求得的外参矩阵转换到激光雷达传感器S3构成的激光雷达空间直角坐标系中，得到激光雷达传感器S1获取的棋盘格靶标角点通过转换在激光雷达传感器S3构成的激光雷达空间直角坐标系下的坐标计算与激光雷达传感器S3获取的相应的棋盘格靶标角点X₃的欧式距离的加权平方和E_p1，数学公式为：

公式(11)中：E_p1表示激光雷达传感器S1与激光雷达传感器S2之间的标定误差以及激光雷达传感器S2与激光雷达传感器S3之间标定误差，引起的激光雷达传感器S1与激光雷达传感器S3之间标定误差，w_i表示P1位置的棋盘格靶标角点的误差权重，公式(11)选取棋盘格靶标角点到激光雷达传感器S1重心的距离的倒数；为矩阵的逆矩阵。

步骤S4、判断是否需要继续移动棋盘格靶标，增加观测值：

判断是否需要继续移动棋盘格靶标的判断标准如下：

a)根据激光雷达传感器S1-S5在车上的安装位置，以激光雷达传感器顶点，两两激光雷达传感器相连作为边，构建无向图(如图8所示)。由于车体的遮挡，激光雷达传感器S3与S5，以及S2与S4没有共同观测视野，因此S2与S4，S5与S3之间没有连线；S1为中心顶点；

b)初始化a)构建的无向图，令每个顶点的访问次数为0，每条边的访问次数为0；

c)当选取激光雷达传感器S1-S5中的任意三个激光雷达传感器做组合标定时，无向图中激光雷达传感器对应的顶点的访问次数加1，两两激光雷达传感器构成的边的访问次数加1；

d)如果无向图中每个顶点的访问次数至少为1，并且中心顶点与其他任意顶点相连构成的边的访问次数至少为2，则无需继续移动棋盘格靶标；否则需要移动棋盘格靶标，重复步骤S1-S3，增加观测值，直到满足d)条件。

步骤S4.1、根据判断标准，仅在P1位置观测不满足d)条件，需要移动棋盘格靶标，增加观测值；将至少一个棋盘格靶标放置在P2位置，大致距离车身为2m-5m，调整棋盘格靶标的高度和角度，确保棋盘格靶标在至少三个激光雷达传感器的共同视野范围内，重复上述步骤S2.1S2.4，选取激光雷达传感器S1、S2、S5进行组合标定；根据步骤S3，获取激光雷达传感器S1到激光雷达传感器S5的外参矩阵激光雷达传感器S5到激光雷达传感器S2的外参矩阵然后根据矩阵相乘，获得激光雷达传感器S1到激光雷达传感器S2的外参矩阵根据步骤S3.3中的原理，将P2位置的棋盘格靶标角点在激光雷达传感器S1构成的激光雷达空间直角坐标系下的坐标X’₁，通过求得的外参矩阵转换到激光雷达传感器S2构成的激光雷达空间直角坐标系下，得到激光雷达传感器S1获取的P2位置的棋盘格靶标角点通过转换在激光雷达传感器S2构成的激光雷达空间直角坐标系下的坐标计算与激光雷达传感器S2获取的相应的棋盘格靶标角点X’₂的欧式距离的加权平方和E_p2，从而得到由激光雷达传感器S1与激光雷达传感器S5之间的标定误差以及激光雷达传感器S5与激光雷达传感器S2之间标定误差，引起的激光雷达传感器S1与激光雷达传感器S2之间标定误差。数学公式为：

公式(12)中：表示P2位置的棋盘格靶标角点i在激光雷达传感器S1构成的激光雷达空间直角坐标系下的坐标，表示P2位置的棋盘格靶标角点i在激光雷达传感器S2构成的激光雷达空间直角坐标系下的坐标；w_i表示P2位置的棋盘格靶标角点的误差权重，本公式选取棋盘格靶标角点到激光雷达传感器S1重心的距离的倒数作为误差权重。

步骤S4.2、根据判断标准，仅在P1、P2位置观测不满足d)条件，需要移动棋盘格靶标，增加观测值；将至少一个棋盘格靶标放置在P3位置，大致距离车身为2m-5m，调整棋盘格靶标的高度和角度，确保棋盘格靶标在至少三个激光雷达传感器的共同视野范围内，重复上述步骤S2.1-S2.4，选取激光雷达传感器S1、S4、S5进行组合标定；根据步骤S3，获取激光雷达传感器S1到激光雷达传感器S4的外参矩阵激光雷达传感器S4到激光雷达传感器S5的外参矩阵然后根据矩阵相乘，获得激光雷达传感器S1到激光雷达传感器S5的外参矩阵根据步骤S3.3中的原理，将P3位置的棋盘格靶标角点在激光雷达传感器S1构成的激光雷达空间直角坐标系下的坐标X”₁，通过求得的外参矩阵转换到激光雷达传感器S5构成的激光雷达空间直角坐标系下，得到激光雷达传感器S1获取的P3位置的棋盘格靶标角点通过转换在激光雷达传感器S5构成的激光雷达空间直角坐标系下的坐标计算与激光雷达传感器S5获取的相应的棋盘格靶标角点的欧式距离的加权平方和E_p3，从而得到由激光雷达传感器s1与激光雷达传感器S4之间的标定误差，以及激光雷达传感器S4与激光雷达传感器S5之间标定误差，引起的激光雷达传感器S1与激光雷达传感器S5之间标定误差，数学公式为：

公式(13)中：表示P3位置的棋盘格靶标角点i在激光雷达传感器S1构成的激光雷达空间直角坐标系下的坐标，表示P3位置的棋盘格靶标角点i在激光雷达传感器S5构成的激光雷达空间直角坐标系下的坐标；w_i表示P3位置的棋盘格靶标角点的误差权重，本公式选取棋盘格靶标角点到激光雷达传感器S1重心的距离的倒数作为误差权重。

步骤S4.3、根据判断标准，仅在P1、P2、P3位置观测不满足d)条件，需要移动棋盘格靶标，增加观测值；将至少一个棋盘格靶标放置在P4位置，大致距离车身为2m-5m，调整棋盘格靶标的高度和角度，确保棋盘格靶标在至少三个激光雷达传感器的共同视野范围内。重复上述步骤S2.1-S2.4，选取激光雷达传感器S1、S3、S4进行组合标定；根据步骤S3，获取激光雷达传感器S1到激光雷达传感器S3的外参矩阵激光雷达传感器S3到激光雷达传感器S4的外参矩阵然后根据矩阵相乘，获得激光雷达传感器S1到激光雷达传感器S4的外参矩阵根据步骤S3.3中的原理，将P4位置的棋盘格靶标角点在激光雷达传感器S1构成的激光雷达空间直角坐标系下的坐标X″′₁，通过求得的外参矩阵转换到激光雷达传感器S4构成的激光雷达空间直角坐标系坐标系中，得到激光雷达传感器S1获取的P4位置的棋盘格靶标角点通过转换在激光雷达传感器S4构成的激光雷达空间直角坐标系下的坐标计算与激光雷达传感器S4获取的相应的棋盘格靶标角点X″′₄的欧式距离的加权平方和E_p4，以此计算由激光雷达传感器s1与激光雷达传感器S3之间的标定误差，以及激光雷达传感器S3与激光雷达传感器S4之间标定误差，引起的激光雷达传感器S1与激光雷达传感器S4之间标定误差，数学公式为：

公式(14)中：表示P4位置的棋盘格靶标角点i在激光雷达传感器S1构成的激光雷达空间直角坐标系下的坐标，表示P4位置的棋盘格靶标角点i在激光雷达传感器S4构成的激光雷达空间直角坐标系下的坐标；w_i表示P4位置的棋盘格靶标角点的误差权重，本公式选取棋盘格靶标角点到激光雷达传感器S1重心的距离的倒数作为误差权重。

步骤S4.4、根据判断标准，在P1、P2、P3、P4位置观测满足d)条件，不需要继续移动棋盘格靶标。

步骤S5、联合优化不同位置的棋盘格靶标角点的坐标累积误差

联合优化E_p1，E_p2，E_p3，E_p4，使得总的标定误差E_total最小，数学公式如下：

公式(15)中：分别是中的旋转分量和平移分量，是的逆矩阵；分别是中的旋转分量和平移分量，分别是中的旋转分量和平移分量；分别是中的旋转分量和平移分量；分别是中的旋转分量和平移分量；W₁，W₂，W₃，W₄为不同传感器组合标定误差的权重，可以通过计算不同传感器组合的测距精度或者分辨率设定。设定规则如下：传感器的测距精度或分辨率越高，其所对应的标定误差权重越大；传感器的测距精度或分辨率越低，其所对应的标定误差权重越小。

步骤S5可以通过使用matlab中的fmincon函数迭代求解，迭代的终止条件可以是迭代次数超过阈值或者总的标定误差E_total收敛。

需要说明：由于步骤2中挑选激光雷达传感器进行组合标定不考虑激光雷达传感器的位置，因此当图1中传感器S1～S5的安装位置发生变化时，本方法仍然成立。当S2与S5位置调换时(如图9所示)，P1位置进行观测时，步骤S2.4选择的三个激光雷达传感器为S1、S3、S5，此时E_p1表示为激光雷达传感器S1与激光雷达传感器S5之间的标定误差以及激光雷达传感器S5与激光雷达传感器S3之间标定误差，引起的激光雷达传感器S1与激光雷达传感器S3之间标定误差；P3位置进行观测时，步骤S2.4选择的三个激光雷达传感器为S1、S2、S4，E_p3表示为激光雷达传感器S1与激光雷达传感器S2之间的标定误差以及激光雷达传感器S2与激光雷达传感器S4之间标定误差，引起的激光雷达传感器S1与激光雷达传感器S4之间标定误差；根据步骤S3.2激光雷达传感器之间的标定误差公式，重新计算E_p1，E_p3，并且根据回环约束，重新计算约束条件。公式如下：

公式(16)中：分别是中的旋转分量和平移分量，是的逆矩阵；分别是中的旋转分量和平移分量，即为激光雷达传感器S3到激光雷达传感器S5的外参矩阵，可以通过标定激光雷达传感器S3与激光雷达传感器S5获得，标定方法与步骤S3.2中标定激光雷达传感器S2与激光雷达传感器S3相同，分别是中的旋转分量和平移分量；为的逆矩阵；分别是中的旋转分量和平移分量，即为激光雷达传感器S2到激光雷达传感器S4的外参矩阵，可以通过标定激光雷达传感器S2与激光雷达传感器S4获得，标定方法与步骤S3.2中标定激光雷达传感器S2与激光雷达传感器S3相同；分别是中的旋转分量和平移分量；W₁，W₂，W₃，W₄为不同激光雷达传感器组合标定误差的权重，可以通过计算不同激光雷达传感器组合的测距精度设定。

另外，本发明针对安装的激光雷达传感器的个数大于5的情况(如图2所示)，可以根据步骤S4.4中的判断条件，适当增加标定观测的位置。例如，一辆无人驾驶车辆在图1中所示的5个传感器基础上又增加第6个激光雷达传感器S6，S1～S5的安装位置同图1图中所示相同，S6安装在S2与S5一侧，当S1、S2、S5、S6四个传感器在观测P2位置的棋盘格靶标时满足步骤S2.4中的d)条件，可以组成多个传感器组合，即激光雷达传感器组合S1，S2，S6以及激光雷达传感器组合S1，S5，S6，对选择的多个传感器组合分别进行标定时，则不需要增加标定观测的位置，此时P2位置的标定误差为激光雷达传感器S1，S2，S6组成的标定误差与激光雷达传感器S1，S5，S6组成的标定误差之和。否则需要移动棋盘格靶标增加观测值，直到满足步骤S4.4中的观测终止条件。

为使本领域技术人员清楚理解上述计算过程，下面针对S1、S2、S5、S6四个激光雷达传感器可以同时构成激光雷达传感器组合S1，S2，S6以及激光雷达传感器组合S1，S5，S6分别进行标定这种情况给出数学方程，公式如下：

公式(17)中：E_p2为激光雷达传感器S1与激光雷达传感器S6之间的标定误差以及激光雷达传感器S6与激光雷达传感器S2之间标定误差，引起的激光雷达传感器S1与激光雷达传感器S2之间标定误差；E′_p2为激光雷达传感器S1与激光雷达传感器S5之间的标定误差以及激光雷达传感器S6与激光雷达传感器S5之间标定误差，引起的激光雷达传感器S1与激光雷达传感器S6之间标定误差；W₁为激光雷达传感器组合S1、S2、S3标定误差的权重，W₂为激光雷达传感器组合S1、S2、S6标定误差的权重，W′₂为激光雷达传感器组合S1、S5、S6标定误差的权重，W₃为激光雷达传感器组合S1、S4、S5标定误差的权重，W₄为激光雷达传感器组合S1、S3、S4标定误差的权重；W₁～W₄可以通过计算不同激光雷达传感器组合的测距精度或分辨率设定；分别是激光雷达传感器S6与激光雷达传感器S5之间外参矩阵中的旋转分量和平移分量，可以通过标定激光雷达传感器S6与激光雷达传感器S5获得，方法与步骤S3中激光雷达传感器S2与激光雷达传感器S3之间的标定方法相同； 分别是激光雷达传感器S6与激光雷达传感器S1之间外参矩阵中的旋转分量和平移分量，是的逆矩阵；可以通过标定激光雷达传感器S1与激光雷达传感器S6获得，标定的方法与步骤S3中传感器S1与传感器S2之间的标定方法相同。

实施例2所有传感器均为相机传感器时的标定方法

如图1至图3所示，本发明提供的一种多个同源传感器联合标定方法，具体标定过程如下：

步骤S1、放置棋盘格靶标

将至少一个棋盘格靶标放置在距离车前大致在510m的P1位置，调整棋盘格靶标角度及高度，使得棋盘格靶标在至少三个相机传感器中视野良好，可以完整出现在相机传感器的扫描范围中；

其中，所述棋盘格靶标中心的高度与相机传感器S1的安装高度(相机传感器S1重心到地面的垂直距离)大致相等；棋盘格靶标每行每列包含的正方形的个数(N_C，N_R)以及每个正方形的尺寸L_Grid均是已知；每个相机传感器的内参参数、畸变参数均为已知。

此步骤需要说明，如果放置多个棋盘格靶标，则需保证棋盘格靶标之间不能相互遮挡。另外，本发明对棋盘格靶标的放置顺序不做要求，可以先标定P2或者P3位置的棋盘格靶标，本实施例只举例最先标定放置在P1位置的棋盘格靶标这种情况。

步骤S2、提取棋盘格靶标角点并选择三个相机传感器进行组合标定

步骤S2.1、同时启动所有相机传感器，等到相机传感器运行稳定后，获取各个相机传感器得到的棋盘格靶标放置在P1位置的图像；

步骤S2.2、调整P1位置的棋盘格靶标的角度，再次启动所有相机传感器，得到不同角度的棋盘格靶标的图像；

步骤S2.3、分别提取步骤S2.1和步骤S2.2中不同相机传感器获取的图像中棋盘格靶标角点在像素平面坐标系下的坐标，像素平面坐标系是以相片左上角为坐标系原点，横轴为u，指向右，对应相机空间直角坐标系中的X轴，纵轴为v，指向下，对应相机直角空间坐标系中的Y轴，像素平面坐标系是局部坐标系，不同相机拍摄的不同照片可以构成不同的像素平面坐标系，同一相机拍摄的不同照片可以构成不同的像素平面坐标系；相机空间直角坐标系是以相机重心为坐标原点，Y轴垂直与车辆，指向车底；X轴垂直于车辆行进的方向，指向右；Z轴指向车辆前进的方向；相机空间直角坐标系是局部坐标系，不同相机构成不同的相机空间直角坐标系，同一相机在不同位置构成不同的相机空间直角坐标系；具体步骤如下：

步骤S2.3.1、根据已知的畸变参数，对图像进行畸变校正，然后对畸变校正后的图像f(x，y)进行高斯平滑滤波处理，减少图像的噪声，得到平滑后的图像g(x，y)，数学公式如下：

公式(18)中：表示卷积操作，σ²表示函数的宽度参数，控制函数的径向作用范围；h(x，y)表示高斯平滑滤波核函数；(x，y)表示点在像素平面坐标系下的像素坐标，其中x的值表示点在像素平面坐标系u轴方向上的投影距离，y的值表示点在像素平面坐标系v轴方向上的投影距离，exp表示以自然常数e为底的指数函数。

步骤S2.3.2)对步骤S2.3.1得到的平滑后的图像g(x，y)根据数学公式(19)构建Hessian矩阵：

公式(19)中：g_xx g_xy及g_yy分别是图像g(x，y)相对于x，y的二阶偏导数。

步骤S2.3.3、分解步骤S2.3.2得到的Hessian矩阵，求取Hessian矩阵的特征值，数学公式如下：

公式(20)中：λ₁，λ₂表示Hessian矩阵两个特征值，D为计算Hessian矩阵特征值的中间临时变量。

步骤S2.3.4、根据Hessian矩阵的性质，Hessian矩阵两个特征值λx，λ₂在数值上表示图像灰度的二阶导数的极大值和极小值，取形状算子S(x，y)＝λ₁×λ₂，当S取极小值时的(x₀，y₀)坐标即为棋盘格靶标角点的坐标，数学公式如下：

公式(21)中：表示(x₀，y₀)坐标处的形状算子，min表示取最小值的操作，局部范围的最小值即为极小值。

步骤S2.3.5、由于图像中的像素灰度值呈连续变化，因此步骤S2.3.4得到的像素坐标不一定是棋盘格靶标角点的像素坐标，为了得到真实的棋盘格靶标角点位置，需要对像素坐标进一步优化，原理如下，如图10所示，棋盘格靶标角点局部区域内的一个像素P的图像梯度向量gP与像素到棋盘格靶标角点的向量的乘积为0。因此对步骤S2.3.4得到的每一个棋盘格靶标角点c遍历周围局部区域的像素，求每个像素的图像梯度向量与该像素到角点的向量的乘积的平方和；利用梯度下降优化算法优化棋盘格靶标角点的像素坐标使得得到的乘积的和最小；梯度下降优化算法终止的条件是迭代的次数超过相应的阈值或者梯度下降的阈值小于相应的阈值；数学公式如下：

公式(22)中：c′为步骤S2.3.4得到的像素坐标，表示像素坐标c′的局部邻域，p∈表示像素P属于像素坐标c′的局部邻域，为像素P的图像梯度向量gP的转置矩阵，argmin为求最小的操作，c为优化后的棋盘格靶标角点的像素坐标。

步骤S2.3.6、步骤S2.3.5得到的棋盘格靶标角点坐标即为像素平面坐标系下的棋盘格靶标角点坐标。

步骤S2.4、根据步骤S2.3提取的不同相机传感器拍摄得到的棋盘格靶标角点，选择三个相机传感器进行联合标定，根据提取的棋盘格靶标角点选择相机传感器的标准如下：

a)优先选择提取棋盘格靶标角点数目最多的相机传感器；

b)如果从两个相机传感器拍摄得到的图像中提取的棋盘格靶标角点数目相同，优先选择重心距离棋盘格靶标平面更近的相机传感器；如果选择多个棋盘格靶标同时进行观测，可以比较相机重心到不同棋盘格靶标平面的距离的平均值；

c)如果从两个相机传感器拍摄得到的图像中提取的棋盘格靶标角点数目相同，并且相机重心距离棋盘格靶标平面相等，则选择分辨率精度更高的相机。

d)如果上述3个条件均无法选择唯一的相机传感器组合，则选择多个相机传感器组合分别进行联合标定；

本发明只选择三个相机传感器进行联合标定的目的是计算某一相机分别与其他两个相机标定产生的外参矩阵误差造成其他两个相机传感器之间的外参矩阵的误差，并且使得计算便捷；根据上述原则，P1位置选取进行联合标定的相机传感器为S1、S2与S3。

步骤S3、计算基于多观测值的棋盘格靶标角点的坐标累积误差

步骤S3.1、由于从不同的相机拍摄的图像中提取的棋盘格靶标角点的数目可能不同，即从相机传感器S1拍摄的图像中提取的棋盘格靶标角点，可能在从相机传感器S2拍摄的图像中提取的棋盘格靶标角点中找不到对应点。因此采用尺度不变特征变换(ScaleInvariant Feature Transform，SIFT)算法分别对相机传感器S1与S2、相机传感器S1与S3、相机传感器S2与S3提取得到的棋盘格靶标角点进行匹配，只保留两个相机传感器共同提取的棋盘格靶标角点。

步骤S3.2、根据步骤S3.1得到的相机传感器S1与S2，以及相机传感器S2与S3共同提取的棋盘格靶标角点在不同像素平面坐标系下的坐标，分别求解相机传感器S2到相机传感器S1的外参矩阵相机传感器S3到相机传感器S2的外参矩阵

求解相机传感器S2到相机传感器S1的外参矩阵的主要流程如下：

将棋盘格靶标角点在相机传感器S2构成的像素平面坐标系下的坐标X₂通过相机传感器S2的内参矩阵的逆矩阵以及尺度因子λ₂转换到相机传感器S2构成的相机空间直角坐标系下，尺度因子λ₂可以根据正方形的尺寸L_crid以及相邻两个棋盘格靶标角点在相机传感器S2构成的像素平面坐标系下的距离求得，然后通过相机传感器S2到相机传感器S1的外参矩阵转换到相机传感器S1构成的相机空间直角坐标系下，最后通过相机传感器S1的内参矩阵以及尺度因子λ₁转换到相机传感器S1构成的像素平面坐标系下，尺度因子λ₁可以根据正方形的尺寸L_Grid以及相邻两个棋盘格靶标角点在相机传感器S1构成的像素平面坐标系下的距离求得，获取由相机传感器S2拍摄得到的棋盘格靶标角点在相机传感器S1构成的像素平面坐标系下的坐标然后计算与相机传感器S1获取的相应棋盘格靶角点坐标X₁的欧式距离的加权平方和Eerr；通过梯度下降算法迭代优化相机传感器S2到相机传感器S1的外参矩阵使得误差最小；梯度下降算法迭代的终止条件是迭代的次数大于给定的相应阈值或者梯度下降的幅度小于相应的阈值；数学公式如下：

公式(23)中：为的逆矩阵，及均为已知，w_i为棋盘格靶标角点i的误差权重，w_i采用棋盘格靶标角点到相机传感器S1重心的距离的倒数作为误差权重，λ为比例因子，等于λ₁与λ₂的比值。

同理可以求得相机传感器S3到相机传感器S2的外参矩阵

步骤S3.3、根据步骤S3.2得到的相机传感器S1到相机传感器S2的外参矩阵以及相机传感器S2到相机传感器S3的外参矩阵通过矩阵相乘，得到相机传感器S1到相机传感器S3的外参矩阵将棋盘格靶标角点在相机传感器S1构成的像素平面坐标系下的坐标X₁通过相机传感器S1的内参矩阵的逆矩阵以及尺度因子λ₁转换到相机传感器S1构成的相机空间直角坐标系下，然后根据求得的外参矩阵转换到相机传感器S3构成的相机空间直角坐标系中，通过相机传感器S3的内参矩阵以及尺度因子λ₃，得到相机传感器S1获取的棋盘格靶标角点通过转换在相机传感器S3构成的像素平面坐标系下的坐标尺度因子λ₃可以根据正方形的尺寸L_crid以及相邻两个棋盘格靶标角点在相机传感器S3构成的像素平面坐标系下距离求得，计算与相机传感器S3获取的相应的棋盘格靶标角点X₃的欧式距离的加权平方和E_p1。数学公式为：

公式(24)中：E_p1表示相机传感器S1与相机传感器S2之间的标定误差以及相机传感器S2与相机传感器S3之间标定误差，引起的相机传感器S1与相机传感器S3之间标定误差；表示相机传感器S3的内参矩阵，表示相机传感器S1的内参矩阵的逆矩阵；w_i表示P1位置的棋盘格靶标角点的误差权重，本公式采用棋盘格靶标角点到相机传感器S1重心的距离的倒数作为误差权重；为矩阵的逆矩阵；为矩阵的逆矩阵；γ为比例因子，等于λ₁与λ₃的比值。

步骤S4、判断是否需要继续移动棋盘格靶标，增加观测值

判断是否需要继续移动棋盘格靶标的判断标准如下：

a)根据相机传感器S1-S5在车上的安装位置，以相机传感器为顶点，两两相机传感器相连作为边，构建无向图(如图8所示)，由于车体的遮挡，相机传感器S3与S5，以及相机传感器S2与S4没有共同观测视野，因此S2与S4，S5与S3之间没有连线；S1记为中心顶点；

b)初始化a)构建的无向图，令每个顶点的访问次数为0，每条边的访问次数为0；

c)当选取相机传感器S1-S5中的任意三个相机传感器做组合标定时，无向图中相机传感器对应的顶点的访问次数加1，两两相机传感器构成的边的访问次数加1；

d)如果无向图中每个顶点的访问次数至少为1，并且中心顶点与其他任意顶点相连构成的边的访问次数至少为2，则无需继续移动棋盘格靶标；否则需要移动棋盘格靶标，增加观测值。

步骤S4.1、根据判断标准，仅在P1位置观测不满足d)条件，需要移动棋盘格靶标，增加观测值；将至少一个棋盘格靶标放置在距离车身2-5m的P2位置，调整棋盘格靶标角度及高度，使得棋盘格靶标在至少3个相机传感器中视野良好，可以完整出现在相机传感器的拍摄范围中，重复上述步骤S2.1-S2.4，选取相机传感器S1、S2、S5进行组合标定；根据步骤3，获取相机传感器S1到相机传感器S5的外参矩阵相机传感器S5到相机传感器S2的外参矩阵然后根据矩阵相乘，获得相机传感器S1到相机传感器S2的外参矩阵根据步骤S3.3中的原理，将P2位置的棋盘格靶标角点在相机传感器S1构成的像素平面坐标系下的坐标X’₁，通过相机传感器S1的内参矩阵的逆矩阵以及尺度因子λ′₁转换到相机传感器S1构成的相机空间直角坐标系下，然后根据求得的外参矩阵转换到相机传感器S2构成的相机空间直角坐标系中，通过相机传感器S2的内参矩阵以及尺度因子λ′₂，得到相机传感器S1获取的P2位置的棋盘格靶标角点通过转换在相机传感器S2构成的像素平面坐标系下的坐标计算与相机传感器S2获取的相应的棋盘格靶标角点X’₂的欧式距离的加权平方和E_p2，从而得到由相机传感器S1与相机传感器S5之间的标定误差以及相机传感器S5与相机传感器S2之间标定误差，引起的相机传感器S1与相机传感器S2之间标定误差。数学公式为：

公式(25)中：表示P2位置的棋盘格靶标角点在相机传感器S1构成的像素平面坐标系下的坐标，表示P2位置的棋盘格靶标角点在相机传感器S2构成的像素平面坐标系下的坐标，γ′为比例因子，等于λ′₁与λ′₂的比值，尺度因子λ′₁可以根据正方形的尺寸L_Grid以及相邻两个棋盘格靶标角点在相机传感器S1构成的像素平面坐标系下的距离求得，尺度因子λ′₂可以根据正方形的尺寸L_Grid以及相邻两个棋盘格靶标角点在相机传感器S2构成的像素平面坐标系下的距离求得；w_i表示P2位置的棋盘格靶标角点的误差权重，本公式采用棋盘格靶标角点到传感器S1重心的距离的倒数作为误差权重。

步骤S4.2、根据判断标准，仅在P1、P2位置观测不满足d)条件，需要移动棋盘格靶标，增加观测值；将至少一个棋盘格靶标放置在距离车身2-5m的P3位置，调整棋盘格靶标角度及高度，使得棋盘格靶标在至少三个相机传感器中视野良好，可以完整出现在相机传感器的拍摄范围中，重复上述步骤S2.1-S2.4，选取相机传感器S1、S4、S5进行组合标定；根据步骤S3，获取相机传感器S1到相机传感器S4的外参矩阵相机传感器S4到相机传感器S5的外参矩阵然后根据矩阵相乘，获得相机传感器S1到相机传感器S5的外参矩阵根据步骤S3.3中的原理，将P3位置的棋盘格靶标角点在相机传感器S1构成的像素平面坐标系下的坐标X”₁，通过相机传感器S1的内参矩阵的逆矩阵以及尺度因子λ″₁转换到相机传感器S1构成的相机空间直角坐标系下，然后通过求得的外参矩阵转换到相机传感器S5构成的相机空间直角坐标系中，通过相机传感器S5已知的内参矩阵以及尺度因子λ″₅，得到得到相机传感器S1获取的P3位置的棋盘格靶标角点通过转换在相机传感器S5构成的像素平面坐标系下的坐标计算与相机传感器S5获取的相应的棋盘格靶标角点的欧式距离的加权平方和E_p3，从而得到由相机传感器S1与相机传感器S4之间的标定误差，以及相机传感器S4与相机传感器S5之间标定误差，引起的相机传感器S1与相机传感器S5之间标定误差，数学公式为：

公式(26)中：表示P3位置的棋盘格靶标角点i在相机传感器S1构成的像素平

面坐标系下的坐标，表示P3位置的棋盘格靶标角点i在相机传感器S5构成的像素

平面坐标系下的坐标；γ″为比例因子，等于λ″₁与λ″₅的比值，尺度因子λ″₁可以根据正方形的尺寸L_Grid以及相邻两个棋盘格靶标角点在相机传感器S1构成的像素平面坐标系下的距离求得，尺度因子λ″₅可以根据正方形的尺寸L_Grid以及相邻两个棋盘格靶标角点在相机传感器S5构成的像素平面坐标系下的距离求得；w_i表示P3位置的棋盘格靶标角点的误差权重，本公式采用棋盘格靶标角点到传感器S1重心的距离的倒数作为误差权重。

步骤S4.3、根据判断标准，仅在P1、P2、P3位置观测不满足d)条件，需要移动棋盘格靶标，增加观测值；将至少一个棋盘格靶标放置在距离车身2-5m的P4位置，调整棋盘格靶标角度及高度，使得棋盘格靶标在至少三个相机传感器中视野良好，可以完整出现在相机传感器的拍摄范围中，重复上述步骤S2.1-S2.4，选取相机传感器S1、S3、S4进行组合标定；根据步骤S3，获取相机传感器S1到相机传感器S3的外参矩阵相机传感器S3到相机传感器S4的外参矩阵然后根据矩阵相乘，获得相机传感器S1到相机传感器S4的外参矩阵根据步骤S3.3中的原理，将P4位置的棋盘格靶标角点在相机传感器S1构成的像素平面坐标系下的坐标X″′₁，通过相机传感器S1的内参矩阵的逆矩阵以及尺度因子λ″′₁转换到相机传感器S1构成的相机空间直角坐标系下，然后通过求得的外参矩阵转换到相机传感器S4构成的相机空间直角坐标系下，根据相机传感器S4的内参矩阵以及尺度因子λ″′₄，得到相机传感器S1获取的P4位置的棋盘格靶标角点通过转换在相机传感器S4构成的像素平面坐标系下的坐标计算与相机传感器S4获取的相应的棋盘格靶标角点X″′₄的欧式距离的加权平方和E_p4，以此计算由相机传感器S1与相机传感器S3之间的标定误差，以及相机传感器S3与相机传感器S4之间标定误差，引起的相机传感器S1与相机传感器S4之间标定误差，数学公式为：

公式(27)中：表示P4位置的棋盘格靶标角点i在相机传感器S1构成的像素平面坐标系下的坐标，表示P4位置的棋盘格靶标角点i在相机传感器S4构成的像素平面坐标系下的坐标；γ″′为比例因子，等于λ″′₁与λ″′₄的比值，尺度因子λ″′₁可以根据正方形的尺寸L_Grid以及相邻两个棋盘格靶标角点在相机传感器S1构成的像素平面坐标系下的距离求得，尺度因子λ″′₄可以根据正方形的尺寸L_Grid以及相邻两个棋盘格靶标角点在相机传感器S4构成的像素平面坐标系下的距离求得；w_i表示P4位置的棋盘格靶标角点的误差权重，本公式采用棋盘格靶标角点到相机传感器S1重心的距离的倒数作为误差权重。

步骤S4.4、根据判断标准，在P1、P2、P3、P4位置观测满足d)条件，不需要继续移动棋盘格靶标。

步骤S5、联合优化不同位置的棋盘格靶标角点的坐标累积误差

联合优化E_p1，E_p2，E_p3，E_p4，使得总的标定误差E_total最小，数学公式如下：

公式(28)中：分别是中的旋转分量和平移分量，是的逆矩阵；分别是中的旋转分量和平移分量，分别是中的旋转分量和平移分量；分别是中的旋转分量和平移分量；分别是中的旋转分量和平移分量；W₁，W₂，W₃，W₄为不同相机传感器组合标定误差的权重，可以通过计算不同相机传感器组合的测距精度或分辨率设定。设定规则如下：相机传感器的测距精度或分辨率越高，其所对应的标定误差权重越大；相机传感器的测距精度或分辨率越低，其所对应的标定误差权重越小。

步骤5可以通过使用matlab中的fmincon函数迭代求解，迭代的终止条件可以是迭代次数超过阈值或者总的标定误差E_total收敛。

需要说明，本发明对传感器的安装个数和位置没有要求，本申请提供的上述传感器的安装形式并不用于限定本发明的保护范围，本发明还可以在车的其他位置增加传感器，如增加其他传感器，可根据传感器的安装位置及实际情况相应修改上述方程。

实施例3多个同源传感器联合标定系统

如图1、图11所示，本发明提供的一种多个同源传感器联合标定系统，包括棋盘格靶标、至少四个同源传感器、数据处理模块1、传感器组选择模块2、特征点匹配模块3、外参计算模块4、累计误差计算模块8、观测值判断模块6、标定模块7；其中，所述棋盘格靶标放置在车附近某一位置，棋盘格靶标是由多个大小相等的黑白相间的正方形组成的靶标；所述传感器安装在车上，标定系统对传感器的安装位置以及传感器的类型不做要求，即传感器可以全为激光雷达或相机，传感器与数据处理模块连接，用于将检测的数据发送给数据处理模块1；

所述数据处理模块1，用于对传感器发送的数据进行处理，计算棋盘格靶标角点在不同传感器构成的像素平面坐标系下或激光雷达空间直角坐标系下的坐标；

所述传感器组选择模块2，用于根据数据处理模块1发送的数据，选择对某一位置的棋盘格靶标进行组合标定的三个传感器，选择的三个传感器分别记为传感器Sa，传感器Sb以及传感器Sc；对传感器进行选择时，优先选择提取棋盘格靶标角点数目最多的传感器；如果从两个传感器扫描数据中提取的棋盘格靶标角点数目相同，优先选择重心距离棋盘格靶标平面更近的传感器；如果从两个传感器扫描数据中提取的棋盘格靶标角点数目相同，并且传感器重心距离棋盘格靶标平面相等，则选择测距精度或分辨率精度更高的传感器；

所述特征点匹配模块3，用于根据传感器组选择模块1的选择结果以及数据处理模块1的计算结果，分别对组合标定的三个传感器中任意两个传感器提取得到的棋盘格靶标角点进行匹配，保留两个传感器共同提取的棋盘格靶标角点；

所述外参计算模块4，用于根据特征点匹配模块3的匹配结果，计算传感器Sb与其他两个传感器Sa、Sc的外参参数；

所述累计误差计算模块5，用于根据外参计算模块4计算的数据计算由传感器Sa与传感器Sb之间的标定误差，以及传感器Sb与传感器Sc之间标定误差，引起的传感器Sa与传感器Sc之间标定误差；

所述观测值判断模块6，用于判断是否需要继续移动棋盘格靶标，增加观测值；每增加一组观测值，则需要计算当前组观测值下棋盘格靶标角点的坐标累积误差；

所述标定模块7，联合优化不同位置的棋盘格靶标角点的坐标累积误差，使得总的标定误差最小；同时，由于不同传感器组构成一个回环，回环的旋转矩阵应为单位阵，平移矩阵应为零矩阵，以此计算外参参数。

本领域技术人员可以理解，上述实施方式中各种方法/模块的全部或部分功能可以通过硬件的方式实现，也可以通过计算机程序的方式实现。当上述实施方式中全部或部分功能通过计算机程序的方式实现时，该程序可以存储于一计算机可读存储介质中，存储介质可以包括：只读存储器、随机存储器、磁盘、光盘、硬盘等，通过计算机执行该程序以实现上述功能。例如，将程序存储在设备的存储器中，当通过处理器执行存储器中程序，即可实现上述全部或部分功能。

另外，当上述实施方式中全部或部分功能通过计算机程序的方式实现时，该程序也可以存储在服务器、另一计算机、磁盘、光盘、闪存盘或移动硬盘等存储介质中，通过下载或复制保存到本地设备的存储器中，或对本地设备的系统进行版本更新，当通过处理器执行存储器中的程序时，即可实现上述实施方式中全部或部分功能。

以上应用了具体个例对本发明进行阐述，只是用于帮助理解本发明，并不用以限制本发明。对于本发明所属技术领域的技术人员，依据本发明的思想，还可以做出若干简单推演、变形或替换。

Claims (10)

1.一种多个同源传感器联合标定方法，其特征在于，该方法包括以下步骤：

步骤S1、放置至少一个棋盘格靶标，通过调整棋盘格靶标角度及高度，使得棋盘格靶标在至少三个传感器中视野良好；

步骤S2、提取棋盘格靶标角点并选择三个传感器进行组合标定，选择的三个传感器分别记为传感器Sa，传感器Sb以及传感器Sc；

步骤S3、计算基于多观测值的棋盘格靶标角点的坐标累积误差

步骤S3.1、分别对组合标定的三个传感器中任意两个传感器提取得到的棋盘格靶标角点进行匹配，保留两个传感器共同提取的棋盘格靶标角点；

步骤S3.2、根据步骤S3.1得到的棋盘格靶标角点坐标，分别求解传感器Sb和其他两个传感器Sa、Sc的外参参数；

步骤S3.3、根据步骤S3.2得到的外参参数，通过矩阵相乘，得到传感器Sa到传感器Sc的外参参数；根据获取的外参参数计算传感器Sa获取的棋盘格靶标角点，通过转换在传感器Sc构成的像素平面坐标系下或激光雷达空间直角坐标系下的坐标，计算通过转换获取的坐标与传感器Sc获取的相应的棋盘格靶标角点的欧式距离的加权平方和，从而得到由传感器Sa与传感器Sb之间的标定误差，以及传感器Sb与传感器Sc之间标定误差，引起的传感器Sa与传感器Sc之间标定误差；

步骤S4、判断是否需要继续移动棋盘格靶标，增加观测值；每增加一组观测值，则需要按照步骤S1至S3计算当前组观测值下棋盘格靶标角点的坐标累积误差；

步骤S5、联合优化不同位置的棋盘格靶标角点的坐标累积误差，使得总的标定误差最小；同时，由于不同传感器组构成一个回环，回环的旋转矩阵应为单位阵，平移矩阵应为零矩阵。

2.根据权利要求1所述的联合标定方法，其特征在于，步骤S2在选择组合标定的三个传感器时，选择标准如下：

a)优先选择提取棋盘格靶标角点数目最多的传感器；

b)如果从两个传感器扫描数据中提取的棋盘格靶标角点数目相同，优先选择重心距离棋盘格靶标平面更近的传感器；如果选择多个棋盘格靶标同时进行观测，比较传感器重心到不同棋盘格靶标平面的距离的平均值；

c)如果从两个传感器扫描数据中提取的棋盘格靶标角点数目相同，并且传感器重心距离棋盘格靶标平面相等，则选择测距精度或分辨率精度更高的传感器；

d)如果上述3个条件均无法选择唯一的传感器组合，则选择多个传感器组合分别进行联合标定，此时观测位置的标定误差为多个传感器组合的标定误差之和。

3.根据权利要求1所述的联合标定方法，其特征在于，步骤S4在判断是否需要继续移动棋盘格靶标时，判断标准如下：

a)根据传感器在车上的安装位置，以传感器为顶点，对于有共同观测视野的两两传感器之间用线相连作为边，构建无向图，以处于中心的传感器为中心顶点；

b)初始化步骤a)构建的无向图，令每个顶点的访问次数为0，每条边的访问次数为0；

c)当选取传感器中的任意三个传感器做组合标定时，无向图中传感器对应的顶点的访问次数加1，两两传感器构成的边的访问次数加1；

d)如果无向图中每个顶点的访问次数至少为1，并且中心顶点与其他任意顶点相连构成的边的访问次数至少为2，则无需继续移动棋盘格靶标；否则需要移动棋盘格靶标，增加观测值。

4.根据权利要求1所述的联合标定方法，其特征在于，步骤S5联合优化时，计算总的标定误差的计算公式如下：

E_total＝min(W₁E_p1+W₂E_p2+W₃E_p3+…+W_nE_pn)

式中，W_n为第n组传感器组合标定误差的权重，可以通过计算不同传感器组合的测距精度或分辨率设定，传感器的测距精度或分辨率越高，其所对应的标定误差权重越大；传感器的测距精度或分辨率越低，其所对应的标定误差权重越小；E_pn为第n组传感器组合时由传感器Sa与传感器Sb之间的标定误差，以及传感器Sb与传感器Sc之间标定误差，引起的传感器Sa与传感器Sc之间标定误差。

5.根据权利要求1至4任意一项所述的联合标定方法，其特征在于，当所述传感器为激光雷达时，步骤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判断得到表示棋盘格靶标的点云聚类，求棋盘格靶标上角点在激光雷达空间直角坐标系下的坐标。

6.根据权利要求1至4任意一项所述的联合标定方法，其特征在于，当所述传感器为激光雷达时，步骤S3.2在求解传感器Sb与其他两个传感器Sa、Sc的外参参数时，均采用梯度下降算法迭代优化传感器Sb与传感器Sa、Sc的外参矩阵；

其中，传感器Sa到传感器Sb的外参矩阵的计算公式如下：

式中，w_i为棋盘格靶标角点i的误差权重，为激光雷达传感器Sa到激光雷达传感器Sb的外参矩阵，为由激光雷达传感器Sa扫描得到的棋盘格靶标角点在激光雷达传感器sb构成的激光雷达空间直角坐标系的坐标，X_ai为棋盘格靶标角点在激光雷达传感器Sa构成的激光雷达空间直角坐标系下的坐标；X_bi为棋盘格靶标角点在激光雷达传感器Sb构成的激光雷达空间直角坐标系下的坐标；

激光雷达传感器Sc到激光雷达传感器Sb的外参矩阵的计算公式如下：

式中，w_i为棋盘格靶标角点i的误差权重，为激光雷达传感器Sc到激光雷达传感器Sb的外参矩阵，为由激光雷达传感器Sc扫描得到的棋盘格靶标角点在激光雷达传感器Sb构成的激光雷达空间直角坐标系的坐标，X_bi为棋盘格靶标角点在激光雷达传感器Sb构成的激光雷达空间直角坐标系下的坐标；X_ci为棋盘格靶标角点在激光雷达传感器Sc构成的激光雷达空间直角坐标系下的坐标。

7.根据权利要求1至4任意一项所述的联合标定方法，其特征在于，当所述传感器为相机时，步骤S2提取棋盘格靶标角点的具体步骤如下：

步骤S2.1、同时启动所有相机传感器，等到相机传感器运行稳定后，获取各个相机传感器得到的棋盘格靶标放置在指定位置的图像；

步骤S2.2、调整指定位置的棋盘格靶标的角度，再次启动所有相机传感器，得到不同角度的棋盘格靶标的图像；

步骤S2.3、分别提取步骤S2.1和步骤S2.2中不同相机传感器获取的图像中棋盘格靶标角点在像素平面坐标系下的坐标，具体步骤如下：

步骤S2.3.1、根据已知的畸变参数，对图像进行畸变校正，然后对畸变校正后的图像f(x，y)进行高斯平滑滤波处理，减少图像的噪声，得到平滑后的图像g(x，y)，数学公式如下：

公式中：表示卷积操作，σ²表示函数的宽度参数，控制函数的径向作用范围；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矩阵的特征值，数学公式如下：

公式中：λ₁，λ₂表示Hessian矩阵两个特征值，D为计算Hessian矩阵特征值的中间临时变量；

步骤S2.3.4、根据Hessian矩阵的性质，Hessian矩阵两个特征值λ₁，λ₂在数值上表示图像灰度的二阶导数的极大值和极小值，取形状算子S(x，y)＝λ₁×λ₂，当S取极小值时的(x₀，y₀)坐标即为棋盘格靶标角点的坐标，数学公式如下：

公式中：表示(x₀，y₀)坐标处的形状算子，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得到的棋盘格靶标角点坐标即为像素平面坐标系下的棋盘格靶标角点坐标。

8.根据权利要求1至4任意一项所述的联合标定方法，其特征在于，当所述传感器为相机时，步骤S3.2在求解传感器Sb与其他两个传感器Sa、Sc的外参参数时，均采用梯度下降算法迭代优化传感器Sb与传感器传感器Sa、Sc的外参矩阵；其中，相机传感器Sa以及Sb相应的内参矩阵已知，相机传感器Sa的尺度因子λ_a可以根据棋盘格靶标正方形的尺寸以及相邻两个棋盘格靶标角点在相机传感器Sa构成的像素平面坐标系下的距离求得，相机传感器Sb的尺度因子λ_b可以根据棋盘格靶标正方形的尺寸以及相邻两个棋盘格靶标角点在相机传感器Sb构成的像素平面坐标系下的距离求得；

传感器Sb到传感器Sa的外参矩阵的计算公式如下：

式中，为的逆矩阵，w_i为棋盘格靶标角点i的误差权重，为相机传感器Sb到相机传感器Sa的外参矩阵；为相机传感器Sb拍摄得到的棋盘格靶标角点在相机传感器Sa构成的像素平面坐标系下的坐标，为相机传感器Sa获取的相应棋盘格靶角点坐标，为相机传感器Sb获取的相应棋盘格靶角点坐标，λ为比例因子，等于λ_a与λ_b的比值；

同理，计算传感器Sc到传感器Sb的外参矩阵。

9.根据权利要求5所述的联合标定方法，其特征在于，步骤S2.3.5根据点云聚类计算棋盘格靶标的角点在激光雷达空间直角坐标系下的坐标时，具体步骤如下：

a)建立点云棋盘格坐标系及棋盘格靶标坐标系，将点云聚类的所有点投影到点云棋盘格坐标系XOY平面；此时，经处理得到的点云棋盘格坐标系XOY平面不能与棋盘格靶标坐标系重合；

b)根据点云聚类中每个点的灰度值，建立灰度值直方图；根据直方图最大的两个峰值R_L和R_H，求出用于判断某个点对应的颜色的阈值范围[τ_l，τ_h]；即如果一个点的灰度值大于上边界τ_h，该点表示白色；如果一个点的灰度值小于下边界τ_l，该点表示黑色，数学公式如下：

公式中：I_i为点云i的灰度值，C_i为该点表示的颜色，其中0表示白色，1表示黑色；

公式中：ε_g为一个大于2的常数，通常取4；

c)根据已知的棋盘格的类型、每个正方形的尺寸L_Grid以及棋盘格靶标左下角正方形的颜色C_(0，0)，计算每个正方形的颜色以及该正方形左下角及右上角在棋盘格靶标坐标系的坐标，数学公式如下：

公式中：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的正方形右上角在棋盘格靶标坐标系的坐标；

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的曼哈顿距离；数学公式如下：

式中：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表示计算曼哈顿距离的函数；Δx₁，Δx₂，Δy₁，Δy₂表示点i到所在正方形四条边的距离；min表示取最小的输入值的操作；

e)根据Powell共轭方向集方法对步骤d)计算得到的点云聚类中所有点的损失值C_m进行优化，使得总的损失值C_m最小，可以得到优化后点云棋盘格坐标系到棋盘格靶标坐标系的转换矩阵

f)计算棋盘格靶标的角点在激光雷达空间直角坐标系下的坐标，数学公式如下：

公式中：为优化后点云棋盘格坐标系到棋盘格靶标坐标系的转换矩阵，可由步骤e)获得；为激光雷达空间直角坐标系转换到点云棋盘格坐标系的转换矩阵，旋转分量为平移分量为点云中心在激光雷达空间直角坐标系下的坐标，可由a)获得。

10.一种多个同源传感器联合标定系统，其特征在于，该系统包括棋盘格靶标、至少四个同源传感器、数据处理模块、传感器组选择模块、特征点匹配模块、外参计算模块、累计误差计算模块、观测值判断模块、标定模块；其中，所述传感器采用激光雷达或相机，传感器与数据处理模块连接，用于将检测的数据发送给数据处理模块；

所述数据处理模块，用于对传感器发送的数据进行处理，计算棋盘格靶标角点在不同传感器构成的像素平面坐标系下或激光雷达空间直角坐标系下的坐标；

所述传感器组选择模块，用于根据数据处理模块发送的数据，选择对某一位置的棋盘格靶标进行组合标定的三个传感器，选择的三个传感器分别记为传感器Sa、传感器Sb以及传感器Sc；对传感器进行选择时，优先选择提取棋盘格靶标角点数目最多的传感器；如果从两个传感器扫描数据中提取的棋盘格靶标角点数目相同，优先选择重心距离棋盘格靶标平面更近的传感器；如果从两个传感器扫描数据中提取的棋盘格靶标角点数目相同，并且传感器重心距离棋盘格靶标平面相等，则选择测距精度或分辨率精度更高的传感器；

所述特征点匹配模块，用于根据传感器组选择模块的选择结果以及数据处理模块的计算结果，分别对组合标定的三个传感器中任意两个传感器提取得到的棋盘格靶标角点进行匹配，保留两个传感器共同提取的棋盘格靶标角点；

所述外参计算模块，用于根据特征点匹配模块的匹配结果，计算传感器Sb与其他两个传感器Sa、Sc的外参参数；

所述累计误差计算模块，用于根据外参计算模块计算的数据计算由传感器Sa与传感器Sb之间的标定误差，以及传感器Sb与传感器Sc之间标定误差，引起的传感器Sa与传感器Sc之间标定误差；

所述观测值判断模块，用于判断是否需要继续移动棋盘格靶标，增加观测值；每增加一组观测值，则需要计算当前组观测值下棋盘格靶标角点的坐标累积误差；

所述标定模块，联合优化不同位置的棋盘格靶标角点的坐标累积误差，使得总的标定误差最小；同时，由于不同传感器组构成一个回环，回环的旋转矩阵应为单位阵，平移矩阵应为零矩阵，以此计算外参参数。