CN112729344A - 无需参照物的传感器外参标定方法 - Google Patents

Abstract

本申请公开了一种无需参照物的传感器外参标定方法，包括以下步骤：根据测量或计算每个传感器单独的运动轨迹，得出每个传感器每一时刻的位姿信息；利用手眼标定方程残差求和，构造相邻两个传感器的全局优化函数，进而计算出旋转参数初值；在得到旋转参数初值后，使用优化方式求解，得到旋转参数的最优解，完成平移外参标定；根据手眼标定方程，融合所有时刻、所有传感器的残差信息，构造优化目标函数，带入所求得的旋转外参标定，最终得到最优的平移参数以及尺度参数，完成平移外参标定。通过该方法能够在不同种类、不同数量传感器的情况下有效的一步完成对所有传感器外参的标定工作，适用于大规模、大批量标定的场景。

Description

无需参照物的传感器外参标定方法

技术领域

本申请涉及传感器技术领域，尤其涉及一种无需参照物的传感器外参标定方法。

背景技术

目前，为了处理复杂的外部环境，更好的完成决策，自动驾驶车辆和自主移动机器人往往采用多种、多个传感器协作来进行对外部环境的感知，如使用多个相机来感知周围的环境与色彩信息，使用多个雷达(或激光雷达)来预估自身与障碍物间的距离、使用IMU/GNSS/RTK等模块来精确计算自身所在的位置。

为了将多个传感器采集到的实时数据进行汇总、融合，必须事先知道各个传感器之间的坐标变换关系(即外参)，因此，在自动驾驶车辆和自主移动机器人投入使用前，必须进行精确的传感器外参标定。

目前外参标定方法主要有两类，分别是需要标定参照物的、基于特征的标定方法，和基于轨迹的标定方法。其中，

基于特征的标定方法需要首先制作一个特定尺寸、特定图案或形状的人工参照物，如棋盘格或二维码标定板、球形或立方体标定物块等，且需要特定的场地来使传感器能清晰地检测到人造参照物，这种方法费时费力，仅适用于实验室中简单的标定任务，不适用于大规模、大批量标定的场景，且无法应用在IMU/GNSS等某些特殊的传感器上。目前大多数相机、雷达厂商或整车方案提供商均提供此类标定方法。

基于轨迹的标定方法需要提前计算出每个传感器单独的运动轨迹，并使用机器人学中的手眼标定(Hand-eye Calibration)方法将多个传感器间的轨迹对齐，同时计算出不同轨迹间的变换关系。但目前此类方法尚未成熟，没有适配于多个传感器间的全局优化方法，对于缺乏尺度信息的相机传感器也无法得到很好的结果。

发明内容

本申请的目的在于克服上述现有技术的不足，提供一种无需参照物的传感器外参标定方法，是在标准的手眼标定流程上加入了多条轨迹间的全局约束，并把尺度信息纳入到待优化变量中进行全局优化。

本申请的技术方案提供一种无需参照物的传感器外参标定方法，1、一种无需参照物的传感器外参标定方法，包括传感器组，所述传感器组包括至少两个传感器，其特征在于，所述传感器外参标定方法包括以下步骤：

1)测算位姿信息：根据测量或计算每个所述传感器单独的运动轨迹，得出每个所述传感器两时刻间的位姿信息，包括每个所述传感器两时刻间的旋转信息和平移信息；

2)构造优化问题：利用手眼标定方程残差求和，构造两个所述传感器的全局优化函数

并求该全局优化函数的最小值，其中

是指在m时刻到m+1时刻之间传感器i的旋转信息，R_ij是指传感器 i和传感器j之间的旋转外参，N是指在一段时间内每个所述传感器得到的所述旋转信息的数量，n是指所述传感器组包括的传感器总个数；

3)标定旋转外参：计算相邻两个所述传感器之间的旋转参数初值，在得到所述旋转参数初值后，使用优化方式求解，得到所述旋转参数的最优解，进而完成旋转外参标定；

4)标定平移外参：根据手眼标定方程，融合所有时刻、所有所述传感器的残差信息，构造优化目标函数，带入所求得的所述旋转参数，使用最小二乘问题的数值解法，得到最优的所述平移参数，最后完成平移外参标定。

在一些实施例中，将待优化的所有所述旋转参数表示为[S₁…S_n-1]^T，带入到上述步骤2)中可将全局优化函数改写为：

进一步的，上述步骤3)中计算相邻两个所述传感器之间的旋转参数初值按如下公式计算：

在一些实施例中，通过以下步骤可算出

的值，具体步骤如下：

①将所有的旋转测量值融合进一个大矩阵中：

其中

为矩阵的Kron积。

②对矩阵M进行奇异值分解：

③设V₁(m，n)为矩阵V₁第m行第n列的元素，按如下方法构造

④对矩阵

进行奇异值分解：

⑤按如下方法得到

在一些实施例中，上述步骤3)中在求解所述旋转参数[S₁…S_n-1]^T的最优解时，所采用的优化方式为高斯-牛顿法或列文伯格-马夸尔特法进行迭代求解。

在一些实施例中，所述传感器组中包含可通过里程计算得到所述平移信息的第一传感器，以及需要借助尺度参数与通过里程计算得到的平移变化幅度结合而得到所述平移信息的第二传感器，其中所述尺度参数通过上述步骤4)构造的所述优化目标函数可求得最优的尺度参数。

进一步的，所述传感器组包括至少一个相机、至少一个雷达和至少一个惯性导航模块，其中所述雷达和所述惯性导航模块属于所述第一传感器，所述相机属于所述第二传感器。

更进一步的，上述步骤4)中，融合所有时刻和所有所述传感器的残差信息而构造的所述优化目标函数如下：

并求该全局优化函数的最小值，

其中

是指在m时刻到m+1时刻之间所述第一传感器i的平移变换参数，

是指在m时刻到m+1时刻之间所述第二传感器j的平移变换参数， k_j是指所述第二传感器j的尺度参数，t_ij为所述传感器i和传感器j间的平移外参。

将待优化的所有所述平移参数表示为[p₁…p_n-1]^T，且所有R_ij均是已知的情况下，所述优化目标函数改写成：

||L·x-G||

并求该优化目标函数的最小值，

其中

X中包含了所述平移参数和所述尺度参数两部分的待标定数据，

和

为系数。

在一些实施例中，所述最小二乘问题的数值解法为QR分解法、稀疏矩阵方法或M-估计方法。

采用上述技术方案后，具有如下有益效果：

本申请在标准的手眼标定流程上引入了全局优化，在多于两个传感器的情况下表现更好，误差更小，不但能标定传感器间的旋转与平移参数，还可把传感器的尺度信息纳入到待优化变量中进行全局优化，最终可得到不同种类的传感器的外参标定，而且无需利用外部特定的人工参照物，能够在不同种类、不同数量传感器的情况下有效的一步完成对所有传感器外参的标定工作，适用于大规模、大批量标定的场景。

附图说明

图1是本申请多种、多个传感器之间的外参标定示意图；

图2是本申请一实施例中对传感器的各参数信息的符号定义示意图；

图3是本申请一实施例中两个传感器在两个时刻间的一个刚体闭环示意图。

具体实施方式

下面详细描述本发明的实施例，所述实施例的示例在附图中示出，其中自始至终相同或类似的标号表示相同或类似的元件或具有相同或类似功能的元件。下面通过参考附图描述的实施例是示例性的，仅用于解释本发明，而不能理解为对本发明的限制。

参照下面的描述和附图，将清楚本发明的实施例的这些和其他方面。在这些描述和附图中，具体公开了本发明的实施例中的一些特定实施方式，来表示实施本发明的实施例的原理的一些方式，但是应当理解，本发明的实施例的范围不受此限制。相反，本发明的实施例包括落入所附加权利要求书的精神和内涵范围内的所有变化、修改和等同物。

本申请所述一种无需参照物的传感器外参标定方法，包括传感器组，该传感器组分为两种：可通过里程计算得到所述平移信息的第一传感器，以及需要借助尺度参数与通过里程计算得到的平移变化幅度结合而得到所述平移信息的第二传感器，该传感器组包括至少一个相机、至少一个雷达和至少一个导航模块，其中所述雷达和所述惯性导航模块属于所述第一传感器，所述相机属于所述第二传感器。如图1所示，该实施例图中所示的传感器共有5个，其中图中标示的传感器1为惯性导航模块，图中标示的传感器2和传感器3为雷达，图中标示的传感器4和传感器5为相机。其中惯性导航模块可以为IMU/GNSS/RTK 等模块，雷达可以为激光雷达。IMU/GNSS/RTK是惯导模块中包含的子模块，不同型号的惯导模块会包含不同的子模块，比如有的惯导模块只有IMU子模块，有的惯导模块既有IMU子模块又有GNSS子模块，有的惯导模块既有IMU子模块又有RTK子模块。而对于一个装有多个传感器的自主移动设备而言，每两个传感器间都有一个旋转参数和一个平移参数，如图1中所示的，旋转参数用R标示，平移参数用t标示。

在进行标定方案前，需要事先测量或计算出每个传感器单独的运动轨迹(包括每一时刻的旋转参数R和平移参数t两部分)。对于IMU/GNSS传感器，可以通过简单的滤波、积分操作即可直接得到其运动轨迹；对于相机和雷达，目前有多种开源的里程计算法可以使用，如适用于相机的ORB-SLAM算法、COLMAP算法，适用于雷达的LOAM算法、GH-ICP算法等。由于相机内部结构的因素，单目相机无法仅通过里程计算法得到每一时刻精确的平移参数t的数值，仅能得到t 的变化幅度，即相机真实的平移参数t为通过里程计算法得到的t的某个倍数 (即t_real＝k·t_measure)，这里的k即使尺度信息是一个待定系数，可以在接下来的标定流程中求解。

在得到每个传感器每一时刻的位姿信息后，按图2的方式进行符号定义：假定传感器的总数量为n，其中前v个传感器无法得到平移的尺度信息，后n-v 个传感器可以得到平移的尺度信息，

或

为两时刻间传感器i的变换参数(可以通过测量或计算得到)，我们假定在一段时间内，每个传感器都可以得到N组这样的变换参数对。

进一步地，定义传感器i和传感器j之间的旋转外参为R_ij，传感器i和传感器j之间的平移外参为t_ij，为了简化表示，使用S_i和p_i来代表相邻两传感器间的外参，即：

S₁＝R₁₂，S₂＝R₂₃，…，S_n-1＝R_n-1，n

p₁＝t₁₂，p₂＝t₂₃，…，p_n-1＝t_n-1，n

根据机器人学中的刚体变换累加原理，任意R_ij和t_ij均可由S_i和p_i表示：

R_ij＝S_iS_i+1S_i+2…S_j-1

t_ij＝S_iS_i+1S_i+2…S_j-2p_j-1

+S_iS_i+1S_i+2…S_j-3p_j-2

+…

+S_iS_i+1p_i+2

+S_ip_i+1

+p_i

可见，只要计算出所有的S_i和p_i，便可得到所有传感器间的外参R_ij和t_ij。

所以，在标定算法中，将待优化的所述旋转参数表示为[S₁…S_n-1]^T，将待优化的所述平移参数表示为[p₁…p_n-1]^T，将待优化的所述尺度参数表示为 [k₁…k_v]^T，其中S_i为3自由度的三维旋转矩阵，p_i为三维列向量，k_i为标量。

由于任意两传感器间的外参R_ij和t_ij是不会随着时间变化的，而传感器位姿

或

则是时刻在变化着。因此，可以在两个时刻间找到一个刚体变换闭环，如图3所示。结合图3，对于任意两个传感器、任意两个时刻，下面的两条手眼标定方程是恒成立的：

R_AR_X＝R_XR_B

R_At_X+t_A＝R_xt_B+t_X

接下来即可按如下方式进行外参标定：

1、标定旋转参数

1.1、全局优化函数的构造

我们将每两个传感器、每两个时刻间的手眼标定方程残差求和，构造如下的全局优化函数，并求此函数的最小值，即构建如下的优化问题：

根据前文所述的符号定义，我们可以将上述优化问题改写为：

其中待优化的变量[S₁…S_n-1]^T即为所需求得的旋转参数。

1.2、计算旋转参数初值

先只考虑相邻两传感器间的旋转变换关系，按如下方式计算旋转参数的初值：

此方程为标准的手眼标定方程，故无需使用数值优化方法即可算出

的值，计算方法可分为以下5步：

①将所有的旋转测量值融合进一个大矩阵中：

其中

为矩阵的Kron积。

②对矩阵M进行奇异值分解：

③设V₁(m，n)为矩阵V₁第m行第n列的元素，按如下方法构造

④对矩阵

进行奇异值分解：

⑤按如下方法得到

1.3、使用优化方法求解旋转参数

在得到旋转参数的初值

后，我们可以使用高斯-牛顿法 (G-N法)或列文伯格-马夸尔特法(L-M法)进行迭代求解，并得到旋转参数 [S₁…S_n-1]^T的最优解，进而得到旋转外参标定。

2、标定平移参数和尺度参数

根据手眼标定方程，融合所有时刻、所有传感器的残差信息，可以构造如下的优化目标函数：

并求此函数的最小值，即构建如下的优化问题

由于已经在第一步中将所有的旋转参数标定完成，因此可以认为此时所有的R_ij均是已知的，在所有R_ij均已知的情况下，t_ij是可以由[p₁…p_n-1]^T线性表示的，即：

t_ij＝R_i，j-1p_j-1+R_i，j-2p_j-2+…+R_i，i+1p_i+1+p_i

因此待优化目标函数可以写为：

||L·X-G||

并求该优化目标函数的最小值，即求：min||L·X-G||

其中

包含了平移参数和尺度参数两部分的待标定数据，

和

为系数。这是一个最小二乘问题，我们可以使用QR分解法、稀疏矩阵方法或M-估计方法来得到最优的平移参数和尺度参数，最后得到相应的传感器的平移外参标定。

至此，旋转参数、平移参数、尺度参数三部分的未知量均已标定完毕。

本申请实施例的无需参照物的传感器外参标定方法，是在标准的手眼标定流程上引入了全局优化，故在多于两个传感器的情况下表现更好，误差更小，不但能标定传感器间的旋转与平移参数，还可把传感器的尺度信息纳入到待优化变量中进行全局优化，最终可得到不同种类的传感器的外参标定，而且无需利用外部特定的人工参照物，能够在不同种类、不同数量传感器的情况下有效的一步完成对所有传感器外参的标定工作，适用于大规模、大批量标定的场景。

以上所述的仅是本申请的原理和较佳的实施例。应当指出，对于本领域的普通技术人员来说，在本申请原理的基础上，还可以做出若干其它变型，也应视为本申请的保护范围。

Claims (10)

1.一种无需参照物的传感器外参标定方法，包括传感器组，所述传感器组包括至少两个传感器，其特征在于，所述传感器外参标定方法包括以下步骤：

1)测算位姿信息：根据测量或计算每个所述传感器单独的运动轨迹，得出每个所述传感器两时刻间的位姿信息，包括每个所述传感器两时刻间的旋转信息和平移信息；

2)构造优化问题：利用手眼标定方程残差求和，构造两个所述传感器的全局优化函数

并求该全局优化函数的最小值，其中

是指在m时刻到m+1时刻之间传感器i的旋转信息，R_ij是指传感器i和传感器j之间的旋转外参，N是指在一段时间内每个所述传感器得到的所述旋转信息的数量，n是指所述传感器组包括的传感器总个数；

3)标定旋转外参：计算相邻两个所述传感器之间的旋转参数初值，在得到所述旋转参数初值后，使用优化方式求解，得到所述旋转参数的最优解，进而完成旋转外参标定；

4)标定平移外参：根据手眼标定方程，融合所有时刻、所有所述传感器的残差信息，构造优化目标函数，带入所求得的所述旋转参数，使用最小二乘问题的数值解法，得到最优的所述平移参数，最后完成平移外参标定。

2.根据权利要求1所述的无需参照物的传感器外参标定方法，其特征在于，将待优化的所有所述旋转参数表示为[S₁ … S_n-1]^T，带入到上述步骤2)中可将全局优化函数改写为：

3.根据权利要求2所述的无需参照物的传感器外参标定方法，其特征在于，上述步骤3)中计算相邻两个所述传感器之间的旋转参数初值按如下公式计算：

4.根据权利要求3所述的无需参照物的传感器外参标定方法，其特征在于，通过以下步骤可算出

的值，具体步骤如下：

①将所有的旋转测量值融合进一个大矩阵中：

其中

为矩阵的Kron积。

②对矩阵M进行奇异值分解：

M＝U₁S₁V₁ ^T

③设V₁(m，n)为矩阵V₁第例行第n列的元素，按如下方法构造V₁ ^*：

④对矩阵V₁ ^*进行奇异值分解：

⑤按如下方法得到

5.根据权利要求4所述的无需参照物的传感器外参标定方法，其特征在于，上述步骤3)中在求解所述旋转参数[S₁ … S_n-1]^T的最优解时，所采用的优化方式为高斯-牛顿法或列文伯格-马夸尔特法进行迭代求解。

6.根据权利要求1所述的无需参照物的传感器外参标定方法，其特征在于，所述传感器组中包含可通过里程计算得到所述平移信息的第一传感器，以及需要借助尺度参数与通过里程计算得到的平移变化幅度结合而得到所述平移信息的第二传感器，其中所述尺度参数通过上述步骤4)构造的所述优化目标函数可求得最优的尺度参数。

7.根据权利要求6所述的无需参照物的传感器外参标定方法，其特征在于，所述传感器组包括至少一个相机、至少一个雷达和至少一个惯性导航模块，其中所述雷达和所述惯性导航模块属于所述第一传感器，所述相机属于所述第二传感器。

8.根据权利要求6所述的无需参照物的传感器外参标定方法，其特征在于，上述步骤4)中，融合所有时刻和所有所述传感器的残差信息而构造的所述优化目标函数如下：

并求该全局优化函数的最小值，

其中

是指在m时刻到m+1时刻之间所述第一传感器i的平移变换参数，

是指在m时刻到m+1时刻之间所述第二传感器j的平移变换参数，k_j是指所述第二传感器j的尺度参数，t_ij为所述传感器i和传感器j间的平移外参。

9.根据权利要求8所述的无需参照物的传感器外参标定方法，其特征在于，将待优化的所有所述平移参数表示为[p₁ … p_n-1]^T，且所有R_ij均是已知的情况下，所述优化目标函数改写成：

||L·X-G||

并求该优化目标函数的最小值，

其中

X中包含了所述平移参数和所述尺度参数两部分的待标定数据，

和

为系数。

10.根据权利要求1所述的无需参照物的传感器外参标定方法，其特征在于，所述最小二乘问题的数值解法为QR分解法、稀疏矩阵方法或M-估计方法。

Images