一种用于自动驾驶车辆的多传感器标定方法及装置与流程

1.本发明涉及自动驾驶技术领域，具体而言，涉及一种用于自动驾驶车辆的多传感器标定方法及装置。


背景技术：

2.在自动驾驶的多传感器方案中，如何方便而准确的标定传感器的内参，以及传感器之间的外参一直是一个富有挑战性的问题。
3.已知的多传感器标定方法，通常为需要手动操作的方法。例如，对自动驾驶车辆的多相机进行标定时，需要人工手持标定板使得被标定相机拍摄各个视角下的标定板。然而，这类操作往往需要富有经验的技术人员来操作，准确性较差。因此，为了提高多传感器标定的准确性，亟需一种多传感器标定方法。


技术实现要素：

4.本发明提供了一种用于自动驾驶车辆的多传感器标定方法及装置，以提高多传感器标定的准确性。具体的技术方案如下。
5.第一方面，本发明实施例提供一种用于自动驾驶车辆的多传感器标定方法，所述自动驾驶车辆安装有多个相机，所述方法包括：
6.获取各所述相机采集的相机图像，以及各所述相机图像对应的采集时刻；根据各所述相机图像的采集时刻，对各所述相机图像进行同步，得到各图像集；每个图像集中包含每个所述相机所采集的一张相机图像，每个图像集中任两张相机图像的采集时刻之差小于预设时间阈值；
7.提取各所述相机图像中的特征点，并根据所提取的特征点对每两张相机图像进行特征匹配，得到各图像匹配对；
8.根据各图像匹配对进行场景重建，得到各所述相机图像的位姿，以及各所述相机图像中所包括特征点对应的世界系的3d点坐标；
9.确定各所述相机中的主相机，根据各图像集中所述主相机所采集相机图像的位姿，计算各图像集对应的主相机坐标系；针对每个其他相机，根据各图像集中该相机所采集相机图像的位姿，以及各图像集对应的主相机坐标系，计算该相机在各主相机坐标系中的恢复位姿，并根据各恢复位姿计算该相机相对所述主相机的初始外参；
10.根据所述主相机的坐标系、各其他相机相对所述主相机的初始外参以及各所述特征点对应的世界系的3d点坐标，对预设优化函数进行优化，得到各所述相机的内参以及各所述其他相机相对于所述主相机的外参。
11.可选的，所述根据各图像集中所述主相机所采集主相机图像的位姿，计算各图像集对应的主相机坐标系；针对每个其他相机，根据各图像集中该相机所采集相机图像的位姿，以及各图像集对应的主相机坐标系，计算该相机在各主相机坐标系中的恢复位姿，并根据各恢复位姿计算该相机相对所述主相机的初始外参的步骤包括：
12.针对每个图像集，确定该图像集对应的主相机坐标系为该图像集中所述主相机所采集相机图像的位姿；
13.针对每个其他相机，确定该相机在任一主相机坐标系中的恢复位姿为，该主相机坐标系对应图像集中该相机所采集相机图像的位姿，与该主相机坐标系的逆的乘积；确定该相机相对所述主相机的初始外参为该相机在各主相机坐标系中的恢复位姿的平均值。
14.可选的，所述根据所述主相机的坐标系、各其他相机相对所述主相机的初始外参以及各所述特征点对应的世界系的3d点坐标，对预设优化函数进行优化，得到各所述相机的内参以及各所述其他相机相对于所述主相机的外参的步骤包括：
15.根据以下公式对各相机的内参和各相机相对所述主相机的外参进行优化：
[0016][0017]
表示j时刻主相机h的坐标系，表示相机i相对于主相机h的外参，θi表示相机i的内参，i表示所有相机集合，j表示所有采集时刻集合，pw为世界系的3d点坐标，u为与3d点匹配的相机图像中的像素点坐标，是由相机内参θi决定的相机的投影模型。
[0018]
可选的，所述方法还包括：
[0019]
根据各所述相机图像，各所述相机的内参，以及各所述其他相机相对于所述主相机的外参，计算各相邻主相机图像之间的相对位姿；
[0020]
针对每帧所述主相机图像，确定从该主相机图像的采集时刻到上一帧主相机图像的采集时刻之间的时间为积分时间；
[0021]
获取轮速编码器输出的脉冲，根据输入的车轮半径依次对所述脉冲进行各所述积分时间的积分得到所述无人驾驶车辆的各帧间位姿；
[0022]
根据各相邻主相机图像之间的相对位姿，以及所述各帧间位姿，估计得到所述多个相机组成的多相机模组相对于轮速的外参初值；所述外参初值包括：俯仰角、滚转角、偏航角、尺度、横向平移以及纵向平移；
[0023]
对所述车轮半径进行优化，得到优化车轮半径；
[0024]
根据所述优化车轮半径依次对所述脉冲进行各所述积分时间的积分得到所述无人驾驶车辆车轮的各帧间优化位姿；根据各相邻主相机图像之间的相对位姿，以及所述各帧间优化位姿，计算得到所述多相机模组相对于轮速的外参。
[0025]
可选的，所述根据各相邻主相机图像之间的相对位姿，以及所述各帧间位姿，估计得到所述多个相机组成的多相机模组相对于轮速的外参初值的步骤包括：
[0026]
在每个所述帧间位姿中识别出旋转分量，根据所述旋转分量通过欧拉角分解得到该帧间位姿对应的偏航角，并根据所述偏航角计算角速度；
[0027]
将角速度大于预设阈值的帧间位姿作为目标帧间位姿，以及与该帧间位姿对应时刻的相对位姿作为目标相对位姿；
[0028]
求解俯仰角和滚转角，并基于所述目标帧间位姿和目标相对位姿，通过奇异值分解svd得到相机系到轮速系外参的初步结果将各所述目标相对位姿左乘作为更新后的目标相对位姿，再次进行svd分解得到相机系到轮速系外参，直到外参结果变化值小
于预设变化阈值时，得到偏航角、尺度、横向平移以及纵向平移。
[0029]
可选的，所述求解俯仰角和滚转角的步骤包括：
[0030]
获取在各所述主相机图像采集时刻范围内的激光雷达lidar点云，并对所述lidar点云进行平面拟合，得到lidar系中的地平面法向量v
l
，获取lidar系和相机系的外参，根据所述lidar系和相机系的外参计算所述v
l
在相机系中的地平面法向量vc，计算vc与轮速系中地平面法向量vo的角度，得到俯仰角和滚转角；或
[0031]
将各主相机图像相对第一帧主相机图像的位姿进行平面拟合，得到在相机系中的地平面法向量vc，计算vc与轮速系中地平面法向量vo的角度，得到俯仰角和滚转角；或
[0032]
在所述各相邻主相机图像之间的相对位姿和各帧间位姿中，识别出旋转分量，基于识别出的旋转分量构建包含俯仰角和滚转角的关系式，并对所述关系式进行svd分解得到俯仰角和滚转角。
[0033]
可选的，所述对所述车轮半径进行优化，得到优化车轮半径的步骤包括：
[0034]
通过以下公式，对车轮半径r进行优化：
[0035][0036]
表示所述多相机模组相对于轮速的外参，表示的旋转分量，表示的平移分量，表示各帧间位姿矩阵的平移分量，表示各相对位姿矩阵的平移分量，s表示尺度，i表示单位矩阵。
[0037]
可选的，所述方法还包括：
[0038]
针对每个所述其他相机，将该相机相对于所述主相机的外参，与所述多相机模组相对于轮速的外参的逆矩阵的乘积，确定为该相机在轮速系中的外参。
[0039]
第二方面，本发明实施例提供一种用于自动驾驶车辆的多传感器标定装置，所述自动驾驶车辆安装有多个相机，所述装置包括：
[0040]
相机图像获取模块，用于获取各所述相机采集的相机图像，以及各所述相机图像对应的采集时刻；根据各所述相机图像的采集时刻，对各所述相机图像进行同步，得到各图像集；每个图像集中包含每个所述相机所采集的一张相机图像，每个图像集中任两张相机图像的采集时刻之差小于预设时间阈值；
[0041]
相机图像匹配模块，用于提取各所述相机图像中的特征点，并根据所提取的特征点对每两张相机图像进行特征匹配，得到各图像匹配对；
[0042]
场景重建模块，用于根据各图像匹配对进行场景重建，得到各所述相机图像的位姿，以及各所述相机图像中所包括特征点对应的世界系的3d点坐标；
[0043]
初始外参确定模块，用于确定各所述相机中的主相机，根据各图像集中所述主相机所采集相机图像的位姿，计算各图像集对应的主相机坐标系；针对每个其他相机，根据各图像集中该相机所采集相机图像的位姿，以及各图像集对应的主相机坐标系，计算该相机在各主相机坐标系中的恢复位姿，并根据各恢复位姿计算该相机相对所述主相机的初始外参；
[0044]
外参优化模块，用于根据所述主相机的坐标系、各其他相机相对所述主相机的初始外参以及各所述特征点对应的世界系的3d点坐标，对预设优化函数进行优化，得到各所
述相机的内参以及各所述其他相机相对于所述主相机的外参。
[0045]
可选的，所述初始外参确定模块，具体用于：
[0046]
针对每个图像集，确定该图像集对应的主相机坐标系为该图像集中所述主相机所采集相机图像的位姿；
[0047]
针对每个其他相机，确定该相机在任一主相机坐标系中的恢复位姿为，该主相机坐标系对应图像集中该相机所采集相机图像的位姿，与该主相机坐标系的逆的乘积；确定该相机相对所述主相机的初始外参为该相机在各主相机坐标系中的恢复位姿的平均值。
[0048]
可选的，所述外参优化模块，具体用于：
[0049]
根据以下公式对各相机的内参和各相机相对所述主相机的外参进行优化：
[0050][0051]
表示j时刻主相机h的坐标系，表示相机i相对于主相机h的外参，θi表示相机i的内参，i表示所有相机集合，j表示所有采集时刻集合，pw为世界系的3d点坐标，u为与3d点匹配的相机图像中的像素点坐标，是由相机内参θi决定的相机的投影模型。
[0052]
可选的，所述装置还包括：
[0053]
相对位姿计算模块，用于根据各所述相机图像，各所述相机的内参，以及各所述其他相机相对于所述主相机的外参，计算各相邻主相机图像之间的相对位姿；
[0054]
积分时间确定模块，用于针对每帧所述主相机图像，确定从该主相机图像的采集时刻到上一帧主相机图像的采集时刻之间的时间为积分时间；
[0055]
帧间位姿计算模块，用于获取轮速编码器输出的脉冲，根据输入的车轮半径依次对所述脉冲进行各所述积分时间的积分得到所述无人驾驶车辆的各帧间位姿；
[0056]
外参初值估计模块，用于根据各相邻主相机图像之间的相对位姿，以及所述各帧间位姿，估计得到所述多个相机组成的多相机模组相对于轮速的外参初值；所述外参初值包括：俯仰角、滚转角、偏航角、尺度、横向平移以及纵向平移；
[0057]
车轮半径优化模块，用于对所述车轮半径进行优化，得到优化车轮半径；
[0058]
轮速外参计算模块，用于根据所述优化车轮半径依次对所述脉冲进行各所述积分时间的积分得到所述无人驾驶车辆的各帧间优化位姿；根据各相邻主相机图像之间的相对位姿，以及所述各帧间优化位姿，计算得到所述多相机模组相对于轮速的外参。
[0059]
可选的，所述外参初值估计模块包括：
[0060]
角速度计算子模块，用于在每个所述帧间位姿中识别出旋转分量，根据所述旋转分量通过欧拉角分解得到该帧间位姿对应的偏航角，并根据所述偏航角计算角速度；
[0061]
目标位姿确定子模块，用于将角速度大于预设阈值的帧间位姿作为目标帧间位姿，以及与该帧间位姿对应时刻的相对位姿作为目标相对位姿；
[0062]
外参估计子模块，用于求解俯仰角和滚转角，并基于所述目标帧间位姿和目标相对位姿，通过奇异值分解svd得到相机系到轮速系外参的初步结果将各所述目标相对位姿左乘作为更新后的目标相对位姿，再次进行svd分解得到相机系到轮速系外参，
直到外参结果变化值小于预设变化阈值时，得到偏航角、尺度、横向平移以及纵向平移。
[0063]
可选的，所述外参估计子模块，具体用于：
[0064]
获取在各所述主相机图像采集时刻范围内的激光雷达lidar点云，并对所述lidar点云进行平面拟合，得到lidar系中的地平面法向量v
l
，获取lidar系和相机系的外参，根据所述lidar系和相机系的外参计算所述v
l
在相机系中的地平面法向量vc，计算vc与轮速系中地平面法向量vo的角度，得到俯仰角和滚转角；或
[0065]
将各主相机图像相对第一帧主相机图像的位姿进行平面拟合，得到在相机系中的地平面法向量vc，计算vc与轮速系中地平面法向量vo的角度，得到俯仰角和滚转角；或
[0066]
在所述各相邻主相机图像之间的相对位姿和各帧间位姿中，识别出旋转分量，基于识别出的旋转分量构建包含俯仰角和滚转角的关系式，并对所述关系式进行svd分解得到俯仰角和滚转角。
[0067]
可选的，所述车轮半径优化模块，具体用于：
[0068]
通过以下公式，对车轮半径r进行优化：
[0069][0070]
表示所述多相机模组相对于轮速的外参，表示的旋转分量，表示的平移分量，表示各帧间位姿矩阵的平移分量，表示各相对位姿矩阵的平移分量，s表示尺度，i表示单位矩阵。
[0071]
可选的，所述装置还包括：
[0072]
坐标转换模块，用于针对每个所述其他相机，将该相机相对于所述主相机的外参，与所述多相机模组相对于轮速的外参的逆矩阵的乘积，确定为该相机在轮速系中的外参。
[0073]
由上述内容可知，本发明实施例提供的一种用于自动驾驶车辆的多传感器标定方法及装置，能够根据各相机采集的相机图像进行场景重建，得到各相机图像的位姿以及各相机图像中所包括特征点对应的3d点坐标，场景重建结果即可反映各相机之间的位置关系，从而可以根据场景重建结果计算得到各相机的内参，以及各相机相对主相机的外参，整个标定方法不需要人工参与，不需要任何外在物体的辅助，结果客观准确，从而能够提高相机标定的准确性。并且，本实施例中的相机标定过程不需要建设标定场等固定的场地，从而可以减少资金投入，节省相机标定的成本。另外，与建设标定场来进行相机标定相比，本发明实施例的方案可以在自动驾驶车辆行驶至任意区域时进行相机标定，从而能够提高相机标定的便捷性。当然，实施本发明的任一产品或方法并不一定需要同时达到以上所述的所有优点。
[0074]
本发明实施例的创新点包括：
[0075]
1、能够根据各相机采集的相机图像进行场景重建，得到各相机图像的位姿以及各相机图像中所包括特征点对应的3d点坐标，场景重建结果即可反映各相机之间的位置关系，从而可以根据场景重建结果计算得到各相机的内参，以及各相机相对主相机的外参，整个标定方法不需要人工参与，不需要任何外在物体的辅助，结果客观准确，从而能够提高相机标定的准确性。并且，本实施例中的相机标定过程不需要建设标定场等固定的场地，从而可以减少资金投入，节省相机标定的成本。另外，与建设标定场来进行相机标定相比，本发
明实施例的方案可以在自动驾驶车辆行驶至任意区域时进行相机标定，从而能够提高相机标定的便捷性。
[0076]
2、对多相机模组进行标定后，还可以基于标定得到的相机内参和外参，对轮速进行标定，得到多相机模组相对于轮速的外参，从而在自动驾驶车辆行驶过程中，可以对各传感器采集的数据进行准确的处理，进而保证自动驾驶车辆行驶的安全性。
[0077]
3、在帧间位姿中，角速度较大的表明激励充足，在轮速标定过程中，选取角速度较大的帧间位姿，以及与所选取的帧间位姿对应时刻的相对位姿来计算多相机模组相对于轮速的外参，能够提高外参标定的准确性。
[0078]
4、在进行轮速标定时，人工测量的车轮半径会有误差，从而影响帧间位姿的准确性，因此，在得到外参初值后对车轮半径进行优化，并根据优化后的车轮半径计算得到多相机模组相对于轮速的外参，能够提高外参计算的准确性。
[0079]
5、对相机进行标定时，计算得到的相机外参均以主相机为参考系，而在无人驾驶车辆系统中，我们更需要以车体系，也就是轮速系为参照系，因此，可以将各相机在主相机参考系中的外参，转换至轮速系中，可以在后续对各传感器采集的数据进行处理时，提高数据处理的准确性。
附图说明
[0080]
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单介绍。显而易见地，下面描述中的附图仅仅是本发明的一些实施例。对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
[0081]
图1为本发明实施例提供的用于自动驾驶车辆的多传感器标定方法的一种流程示意图；
[0082]
图2为本发明实施例提供的用于自动驾驶车辆的多传感器标定方法的另一种流程示意图；
[0083]
图3为本发明实施例提供的用于自动驾驶车辆的多传感器标定装置的一种结构示意图。
具体实施方式
[0084]
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整的描述。显然，所描述的实施例仅仅是本发明的一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有付出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
[0085]
需要说明的是，本发明实施例及附图中的术语“包括”和“具有”以及它们的任何变形，意图在于覆盖不排他的包含。例如包含的一系列步骤或单元的过程、方法、系统、产品或设备没有限定于已列出的步骤或单元，而是可选地还包括没有列出的步骤或单元，或可选地还包括对于这些过程、方法、产品或设备固有的其他步骤或单元。
[0086]
本发明实施例公开了一种用于自动驾驶车辆的多传感器标定方法及装置，能够提高多传感器标定的准确性。下面对本发明实施例进行详细说明。
[0087]
图1为本发明实施例提供的用于自动驾驶车辆的多传感器标定方法的一种流程示意图。该方法应用于自动驾驶车辆。该方法具体包括以下步骤。
[0088]
s110：获取各相机采集的相机图像，以及各相机图像对应的采集时刻；根据各相机图像的采集时刻，对各相机图像进行同步，得到各图像集；每个图像集中包含每个相机所采集的一张相机图像，每个图像集中任两张相机图像的采集时刻之差小于预设时间阈值。
[0089]
在本发明实施例中，对多相机模组进行标定时，数据输入为各相机采集的相机图像，其中，各相机图像标注有对应的时间戳，也就是各相机图像的采集时刻。数据输出即为各相机的内参以及相机之间的外参。
[0090]
在本发明实施例中，可以预先对各相机进行同步，并且可以设定各相机均具有相同的图像采集周期，以保证各相机均在相同的时刻进行图像采集。然而，在实际应用中，由于各相机时钟晶振等区别，各相机采集相机图像的时刻不完全相同。并且，在对相机进行标定时，需要对同一时刻各相机采集的相机图像进行对比，才能确定出各相机之间的位置关系，进而标定得到各相机的内参，和相机之间的外参。
[0091]
因此，在获取到各相机图像后，可以对各相机图像进行同步，得到各图像集。也就是说，可以确定出属于同一时刻采集的相机图像，以准确的进行相机标定。
[0092]
例如，当自动驾驶车辆中安装有相机a、b、c、d、e，分别获取到相机a采集的相机图像1、2、3、4、5、6；相机b采集的相机图像7、8、9、10、11、12；相机c采集的相机图像13、14、15、16、17、18；相机d采集的相机图像19、20、21、22、23、24；相机e采集的相机图像25、26、27、28、29、30时，对各相机图像进行同步后，可以分别得到以下图像集：
[0093]
一、{1、7、13、19、25}
[0094]
二、{2、8、14、20、26}
[0095]
三、{3、9、15、21、27}
[0096]
四、{4、10、16、22、28}
[0097]
五、{5、11、17、23、29}
[0098]
六、{6、12、18、24、30}。
[0099]
s120：提取各相机图像中的特征点，并根据所提取的特征点对每两张相机图像进行特征匹配，得到各图像匹配对。
[0100]
例如，可以采用sift(scale-invariant feature transform，尺度不变特征转换)算法来提取各相机图像中的特征点。对每两张相机图像进行特征匹配时，可以针对每一个图像匹配对，计算对极几何，估计基础矩阵(fundamental matrix)或者本质矩阵(essential matrix)，并通过随机抽样一致算法(random sample consensus，ransac)优化改善匹配对。
[0101]
s130：根据各图像匹配对进行场景重建，得到各相机图像的位姿，以及各相机图像中所包括特征点对应的世界系的3d点坐标。
[0102]
例如，可以首先初始化选择两帧相机图像进行第一次光束平差法(bundle adjustment，ba)，然后循环添加新的相机图像进行新的ba优化，最后直到所有的相机图像都被添加到ba优化中，此时就得到了所有相机图像的位姿和场景的视觉点云地图，也即各相机图像中所包括特征点对应的世界系的3d点坐标。
[0103]
s140：确定各相机中的主相机，根据各图像集中主相机所采集相机图像的位姿，计
算各图像集对应的主相机坐标系；针对每个其他相机，根据各图像集中该相机所采集相机图像的位姿，以及各图像集对应的主相机坐标系，计算该相机在各主相机坐标系中的恢复位姿，并根据各恢复位姿计算该相机相对主相机的初始外参。
[0104]
在一种实现方式中，可以将任一相机确定为主相机。确定主相机后，可以针对每个图像集，确定该图像集对应的主相机坐标系为该图像集中主相机所采集相机图像的位姿；针对每个其他相机，确定该相机在任一主相机坐标系中的恢复位姿为，该主相机坐标系对应图像集中该相机所采集相机图像的位姿，与该主相机坐标系的逆的乘积。
[0105]
例如，当任一图像集中第i个相机所采集相机图像通过定位器获得位姿是第0个相机是主相机，则主相机的坐标系为：第i个相机在该主相机坐标系中的恢复位姿
[0106]
确定各图像集对应的主相机坐标系和各其他相机在主相机坐标系的恢复位姿后，可以针对每个其他相机，确定该相机相对主相机的初始外参为该相机在各主相机坐标系中的恢复位姿的平均值。通过多个图像集来计算各其他相机相对主相机的外参估计，能够提高计算结果的准确性。
[0107]
s150：根据主相机的坐标系、各其他相机相对主相机的初始外参以及各特征点对应的世界系的3d点坐标，对预设优化函数进行优化，得到各相机的内参以及各其他相机相对于主相机的外参。
[0108]
在一种实现方式中，可以根据以下公式对各相机的内参和各相机相对主相机的外参进行优化：
[0109][0110]
表示j时刻主相机h的坐标系，表示相机i相对于主相机h的外参，θi表示相机i的内参，i表示所有相机集合，j表示所有采集时刻集合，pw为世界系的3d点坐标，u为与3d点匹配的相机图像中的像素点坐标，是由相机内参θi决定的相机的投影模型。
[0111]
通过上述公式进行优化时，所优化的是所有相机的内参、各相机相对主相机的外参、主相机在j时刻的坐标系以及各3d点坐标pw。
[0112]
在实际应用中，可以在自动驾驶车辆行驶过程中不断对各相机内参和各相机相对主相机的外参进行优化。具体的，自动驾驶车辆行驶过程中，各相机不断的进行相机图像采集，通过不断的根据各相机采集的相机图像对各相机内参，以及各相机相对主相机的外参进行优化。当每个相机和主相机同时被恢复位姿的对数达到阈值时，数据采集结束，根据所采集到的数据进行优化。
[0113]
由上述内容可知，本实施例可以根据各相机采集的相机图像进行场景重建，得到各相机图像的位姿以及各相机图像中所包括特征点对应的3d点坐标，场景重建结果即可反映各相机之间的位置关系，从而可以根据场景重建结果计算得到各相机的内参，以及各相机相对主相机的外参，整个标定方法不需要人工参与，不需要任何外在物体的辅助，结果客
观准确，从而能够提高相机标定的准确性。并且，本实施例中的相机标定过程不需要建设标定场等固定的场地，从而可以减少资金投入，节省相机标定的成本。另外，与建设标定场来进行相机标定相比，本发明实施例的方案可以在自动驾驶车辆行驶至任意区域时进行相机标定，从而能够提高相机标定的便捷性。
[0114]
作为本发明实施例的一种实施方式，对相机进行标定后，还可以基于标定得到的相机内参和各相机相对主相机的外参对轮速进行标定。
[0115]
轮速标定的输入数据包括里程计输出的相对位姿以及轮速脉冲输出的帧间估计。其中，里程计输入为多相机图像，里程计得到各相机图像相对于第一帧相机图像的位姿，从而计算得到各帧之间的相对位姿。轮速编码器输出脉冲数，通过积分得到轮子的帧间位姿。
[0116]
具体的，如图2所示，轮速标定的过程包括以下步骤：
[0117]
s210：根据各相机图像，各相机的内参，以及各其他相机相对于主相机的外参，计算各相邻主相机图像之间的相对位姿。
[0118]
对轮速进行标定时，可以将整个多相机模组看作一个刚体，估计整个多相机模组在不同时刻的位姿。
[0119]
具体的，可以将各相机图像，各相机的内参，以及各其他相机相对于主相机的外参输入里程计，里程计得到各相机图像相对于第一帧相机图像的位姿，从而计算得到各相邻主相机图像之间的相对位姿。
[0120]
s220：针对每帧主相机图像，确定从该主相机图像的采集时刻到上一帧主相机图像的采集时刻之间的时间为积分时间。
[0121]
例如，当任一帧主相机图像的采集时刻为2020.3.20.12:31，上一帧主相机图像的采集时刻为2020.3.20.12:33时，即可确定根据该主相机图像确定的积分时间为2020.3.20.12:31-2020.3.20.12:33。
[0122]
对每帧主相机图像均进行上述计算后，即可得到n-1个积分时间，n为主相机图像的总数量。例如，当存在5帧主相机图像0、1、2、3、4时，得到的积分时间即为0-1、1-2、2-3、3-4，一共4个积分时间。
[0123]
s230：获取轮速编码器输出的脉冲，根据输入的车轮半径依次对脉冲进行各积分时间的积分得到无人驾驶车辆的各帧间位姿。
[0124]
其中，上述车轮半径可以为人工测量得到的出厂车轮半径。
[0125]
s240：根据各相邻主相机图像之间的相对位姿，以及各帧间位姿，估计得到多个相机组成的多相机模组相对于轮速的外参初值；外参初值包括：俯仰角、滚转角、偏航角、尺度、横向平移以及纵向平移。
[0126]
在本发明实施例中，可以定义x轴方向为车横向，车体绕x轴的旋转为俯仰角pitch；y轴方向为车纵向，绕y轴的旋转为滚转角roll；z轴为高度，绕z轴的旋转为偏航角yaw。
[0127]
在本发明实施例中，可以基于手眼标定算法对轮速进行标定。手眼标定算法是一种将两段轨迹对齐，并求解出二者转换关系的算法。在轮速标定中，这两段轨迹是相机轨迹和轮速轨迹，相机轨迹由里程计输出，为各时刻的位姿，轮速轨迹由脉冲积分获得，通过计算各帧位姿之间的差值，得到我们需要的相对位姿。
[0128]
在一种实现方式中，可以在每个帧间位姿中识别出旋转分量，根据旋转分量通过
欧拉角分解得到该帧间位姿对应的偏航角，并根据偏航角计算角速度；将角速度大于预设阈值的帧间位姿作为目标帧间位姿，以及与该帧间位姿对应时刻的相对位姿作为目标相对位姿；求解俯仰角和滚转角，并基于目标帧间位姿和目标相对位姿，通过奇异值分解svd得到相机系到轮速系外参的初步结果将各目标相对位姿左乘作为更新后的目标相对位姿，再次进行svd分解得到相机系到轮速系外参，直到外参结果变化值小于预设变化阈值时，得到偏航角、尺度、横向平移以及纵向平移。
[0129]
例如，为待估计的外参，即多相机模组相对于轮速的转换，为轮子帧间位姿，为里程计相对位姿，三者满足通过两段轨迹，能够得到很多组多组为了构建健康的求解问题，取出的旋转部分，通过欧拉角分解得到这段时间轮子的偏航角yaw，除以时间间隔，得到角速度，角速度大的认为激励充足，用来求解外参，至此筛选出多组求解外参，至此筛选出多组为了保证标定效果，在标定过程中，要求载体要尽可能多转弯以保证足够的运动激励。由于轮子对高度的不可观测性，标定时载体要避免上下坡。
[0130]
求解俯仰角和滚转角时，可以采用不同的算法计算。在一种实现方式中，可以获取在各主相机图像采集时刻范围内的激光雷达lidar点云，并对lidar点云进行平面拟合，得到lidar系中的地平面法向量v
l
，获取lidar系和相机系的外参，根据lidar系和相机系的外参计算v
l
在相机系中的地平面法向量vc，计算vc与轮速系中地平面法向量vo的角度，得到俯仰角和滚转角。
[0131]
在另一种实现方式中，可以将各主相机图像相对第一帧主相机图像的位姿进行平面拟合，得到在相机系中的地平面法向量计算vc，计算vc与轮速系中地平面法向量vo的角度，得到俯仰角和滚转角。
[0132]
在另一种实现方式中，可以在各相邻主相机图像之间的相对位姿和各帧间位姿中，识别出旋转分量，基于识别出的旋转分量构建包含俯仰角和滚转角的关系式，并对关系式进行svd分解得到俯仰角和滚转角。其中，上述关系式可以为：
[0133]
s250：对车轮半径进行优化，得到优化车轮半径。
[0134]
在求解外参初值的过程中，误差有一部分来源于车轮半径，因此在进行线性求解后，会构建一个非线性优化问题，优化变量为多相机模组相对于轮速的外参以及车轮半径。
[0135]
例如，可以通过以下公式，对车轮半径r进行优化：
[0136][0137]
表示多相机模组相对于轮速的外参，表示的旋转分量，表示的平移分量，表示各帧间位姿矩阵的平移分量，表示各相对位姿矩阵的平移分量，s表示尺度，i表示单位矩阵。
[0138]
s260：根据优化车轮半径依次对脉冲进行各积分时间的积分得到无人驾驶车辆到各帧间优化位姿；根据各相邻主相机图像之间的相对位姿，以及各帧间优化位姿，计算得到多相机模组相对于轮速的外参。
[0139]
对车轮半径进行优化后，重新对轮速脉冲积分，重复上述步骤即可计算得到多相机模组相对于轮速的外参。
[0140]
本实施例中，对多相机模组进行标定后，还可以基于标定得到的相机内参和外参，对轮速进行标定，得到多相机模组相对于轮速的外参，从而在自动驾驶车辆行驶过程中，可以对各传感器采集的数据进行准确的处理，进而保证自动驾驶车辆行驶的安全性。在帧间位姿中，角速度较大的表明激励充足，在轮速标定过程中，选取角速度较大的帧间位姿，以及与所选取的帧间位姿对应时刻的相对位姿来计算多相机模组相对于轮速的外参，能够提高外参标定的准确性。在进行轮速标定时，人工测量的车轮半径会有误差，从而影响帧间位姿的准确性，因此，在得到外参初值后对车轮半径进行优化，并根据优化后的车轮半径计算得到多相机模组相对于轮速的外参，能够提高外参计算的准确性。
[0141]
作为本发明实施例的一种实施方式，对相机和轮速均完成标定后，还可以针对每个其他相机，将该相机相对于主相机的外参，与多相机模组相对于轮速的外参的逆矩阵的乘积，确定为该相机在轮速系中的外参。
[0142]
对相机进行标定时，计算得到的相机外参均以主相机为参考系，而在无人驾驶车辆系统中，我们更需要以车体系，也就是轮速系为参照系，因此，可以将各相机在主相机参考系中的外参，转换至轮速系中，可以在后续对各传感器采集的数据进行处理时，提高数据处理的准确性。
[0143]
具体的，可以根据各相机的内参以及各其他相机相对于主相机的外参，对各相机图像进行特征点的检测与匹配，得到各相机图像的目标位姿；针对每个相机采集的目标相机图像，根据各目标相机图像的采集时刻和目标位姿，确定各目标相机图像与前一帧目标相机图像的距离和角度间隔，并在任一目标相机图像与前一帧目标相机图像的距离大于预设距离阈值，且角度间隔大于预设角度阈值时，将各相机在该目标相机图像采集时刻所采集的相机图像均确定为关键帧；针对各关键帧，确定该关键帧中梯度值大于预设梯度阈值的梯度点，并根据该关键帧的目标位姿对该关键帧中各梯度点进行深度估计得到该关键帧对应的局部三维点信息；根据各关键帧的采集时刻，以及各关键帧对应的局部三维点信息，统计得到各采集时刻对应的三维点集合；获取lidar点云数据，并在lidar点云数据中识别时间戳与各关键帧的采集时间对应的关键lidar点云数据；根据lidar测量模型，将关键lidar点云数据投影至二维空间，并进行双线性插值，得到各lidar深度图；根据各采集时刻对应的三维点集合与各lidar深度图，构建关于由多个相机组成的多相机模组到lidar的外参的非线性最小二乘优化函数，并对优化函数进行优化，得到多相机模组到lidar的外参。
[0144]
对各相机图像进行处理时，例如可以将各相机图像输入到一个多相机视觉里程计系统中，此系统通过视觉特征点的检测与匹配，计算出每一帧相机图像的位姿，并选取所有相机图像中距离与角度间隔足够大的帧作为关键帧。之后将关键帧的位姿与相机图像输入到一个视觉实时稠密重建系统中，此系统选取图像中有梯度的点进行深度估计，输出每一帧的视觉三维点信息，第i帧视觉三维点的集合可以称为{vi}。
[0145]
对于lidar数据，处理方式是可以通过时间戳信息找到时间上对应了以上视觉关
键帧的关键lidar数据，同时lidar的测量模型π
l
(
·
):r3→
r2是已知的，此映射可以将三维空间的点云数据投影至二维空间上，再经过双线性插值，最终得到lidar深度图，第i帧lidar深度图可以称为{ii}。
[0146]
在完成以上对相机图像和lidar数据的各自预处理后，确定了每帧对应的视觉与lidar数据：{vi}与{ii}，而要标定的参数是多相机模组到lidar的外参通过对{vi}与{ii}进行基于距离信息的关联，构建一个关于外参的非线性最小二乘问题，即可优化得到多相机模组到lidar的外参
[0147]
其中，可以根据以下公式对多相机模组到lidar的外参进行优化：
[0148][0149]
π
l
(
·
)表示lidar测量模型，ii(x)表示i时刻对应的lidar深度图在二维坐标x(u,v)的取值，pc表示相机系下的三维点坐标，vi表示i时刻对应的三维点集合，‖pc‖表示三维点pc(x,y,z)的模长。
[0150]
对多相机模组进行标定后，还可以基于标定得到的相机内参和外参，对lidar进行标定，得到多相机模组到lidar的外参，从而在自动驾驶车辆行驶过程中，可以对各传感器采集的数据进行准确的处理，进而保证自动驾驶车辆行驶的安全性。
[0151]
如图3所示，其示出了本发明实施例提供一种用于自动驾驶车辆的多传感器标定装置，所述自动驾驶车辆安装有多个相机，所述装置包括：
[0152]
相机图像获取模块310，用于获取各所述相机采集的相机图像，以及各所述相机图像对应的采集时刻；根据各所述相机图像的采集时刻，对各所述相机图像进行同步，得到各图像集；每个图像集中包含每个所述相机所采集的一张相机图像，每个图像集中任两张相机图像的采集时刻之差小于预设时间阈值；
[0153]
相机图像匹配模块320，用于提取各所述相机图像中的特征点，并根据所提取的特征点对每两张相机图像进行特征匹配，得到各图像匹配对；
[0154]
场景重建模块330，用于根据各图像匹配对进行场景重建，得到各所述相机图像的位姿，以及各所述相机图像中所包括特征点对应的世界系的3d点坐标；
[0155]
初始外参确定模块340，用于确定各所述相机中的主相机，根据各图像集中所述主相机所采集相机图像的位姿，计算各图像集对应的主相机坐标系；针对每个其他相机，根据各图像集中该相机所采集相机图像的位姿，以及各图像集对应的主相机坐标系，计算该相机在各主相机坐标系中的恢复位姿，并根据各恢复位姿计算该相机相对所述主相机的初始外参；
[0156]
外参优化模块350，用于根据所述主相机的坐标系、各其他相机相对所述主相机的初始外参以及各所述特征点对应的世界系的3d点坐标，对预设优化函数进行优化，得到各所述相机的内参以及各所述其他相机相对于所述主相机的外参。
[0157]
可选的，所述初始外参确定模块340，具体用于：
[0158]
针对每个图像集，确定该图像集对应的主相机坐标系为该图像集中所述主相机所采集相机图像的位姿；
[0159]
针对每个其他相机，确定该相机在任一主相机坐标系中的恢复位姿为，该主相机坐标系对应图像集中该相机所采集相机图像的位姿，与该主相机坐标系的逆的乘积；确定该相机相对所述主相机的初始外参为该相机在各主相机坐标系中的恢复位姿的平均值。
[0160]
可选的，所述外参优化模块350，具体用于：
[0161]
根据以下公式对各相机的内参和各相机相对所述主相机的外参进行优化：
[0162][0163]
表示j时刻主相机h的坐标系，表示相机i相对于主相机h的外参，θi表示相机i的内参，i表示所有相机集合，j表示所有采集时刻集合，pw为世界系的3d点坐标，u为与3d点匹配的相机图像中的像素点坐标，是由相机内参θi决定的相机的投影模型。
[0164]
可选的，所述装置还包括：
[0165]
相对位姿计算模块，用于根据各所述相机图像，各所述相机的内参，以及各所述其他相机相对于所述主相机的外参，计算各相邻主相机图像之间的相对位姿；
[0166]
积分时间确定模块，用于针对每帧所述主相机图像，确定从该主相机图像的采集时刻到上一帧主相机图像的采集时刻之间的时间为积分时间；
[0167]
帧间位姿计算模块，用于获取轮速编码器输出的脉冲，根据输入的车轮半径依次对所述脉冲进行各所述积分时间的积分得到所述无人驾驶车辆的各帧间位姿；
[0168]
外参初值估计模块，用于根据各相邻主相机图像之间的相对位姿，以及所述各帧间位姿，估计得到所述多个相机组成的多相机模组相对于轮速的外参初值；所述外参初值包括：俯仰角、滚转角、偏航角、尺度、横向平移以及纵向平移；
[0169]
车轮半径优化模块，用于对所述车轮半径进行优化，得到优化车轮半径；
[0170]
轮速外参计算模块，用于根据所述优化车轮半径依次对所述脉冲进行各所述积分时间的积分得到所述无人驾驶车辆的各帧间优化位姿；根据各相邻主相机图像之间的相对位姿，以及所述各帧间优化位姿，计算得到所述多相机模组相对于轮速的外参。
[0171]
可选的，所述外参初值估计模块包括：
[0172]
角速度计算子模块，用于在每个所述帧间位姿中识别出旋转分量，根据所述旋转分量通过欧拉角分解得到该帧间位姿对应的偏航角，并根据所述偏航角计算角速度；
[0173]
目标位姿确定子模块，用于将角速度大于预设阈值的帧间位姿作为目标帧间位姿，以及与该帧间位姿对应时刻的相对位姿作为目标相对位姿；
[0174]
外参估计子模块，用于求解俯仰角和滚转角，并基于所述目标帧间位姿和目标相对位姿，通过奇异值分解svd得到相机系到轮速系外参的初步结果将各所述目标相对位姿左乘作为更新后的目标相对位姿，再次进行svd分解得到相机系到轮速系外参，直到外参结果变化值小于预设变化阈值时，得到偏航角、尺度、横向平移以及纵向平移。
[0175]
可选的，所述外参估计子模块，具体用于：
[0176]
获取在各所述主相机图像采集时刻范围内的激光雷达lidar点云，并对所述lidar点云进行平面拟合，得到lidar系中的地平面法向量v
l
，获取lidar系和相机系的外参，根据
所述lidar系和相机系的外参计算所述v
l
在相机系中的地平面法向量vc，计算vc与轮速系中地平面法向量vo的角度，得到俯仰角和滚转角；或
[0177]
将各主相机图像相对第一帧主相机图像的位姿进行平面拟合，得到在相机系中的地平面法向量vc，计算vc与轮速系中地平面法向量vo的角度，得到俯仰角和滚转角；或
[0178]
在所述各相邻主相机图像之间的相对位姿和各帧间位姿中，识别出旋转分量，基于识别出的旋转分量构建包含俯仰角和滚转角的关系式，并对所述关系式进行svd分解得到俯仰角和滚转角。
[0179]
可选的，所述车轮半径优化模块，具体用于：
[0180]
通过以下公式，对车轮半径r进行优化：
[0181][0182]
表示所述多相机模组相对于轮速的外参，表示的旋转分量，表示的平移分量，表示各帧间位姿矩阵的平移分量，表示各相对位姿矩阵的平移分量，s表示尺度，i表示单位矩阵。
[0183]
可选的，所述装置还包括：
[0184]
坐标转换模块，用于针对每个所述其他相机，将该相机相对于所述主相机的外参，与所述多相机模组相对于轮速的外参的逆矩阵的乘积，确定为该相机在轮速系中的外参。
[0185]
本发明实施例提供的一种用于自动驾驶车辆的多传感器标定装置，能够根据各相机采集的相机图像进行场景重建，得到各相机图像的位姿以及各相机图像中所包括特征点对应的3d点坐标，场景重建结果即可反映各相机之间的位置关系，从而可以根据场景重建结果计算得到各相机的内参，以及各相机相对主相机的外参，整个标定方法不需要人工参与，不需要任何外在物体的辅助，结果客观准确，从而能够提高相机标定的准确性。并且，本实施例中的相机标定过程不需要建设标定场等固定的场地，从而可以减少资金投入，节省相机标定的成本。另外，与建设标定场来进行相机标定相比，本发明实施例的方案可以在自动驾驶车辆行驶至任意区域时进行相机标定，从而能够提高相机标定的便捷性。
[0186]
上述装置实施例与方法实施例相对应，与该方法实施例具有同样的技术效果，具体说明参见方法实施例。装置实施例是基于方法实施例得到的，具体的说明可以参见方法实施例部分，此处不再赘述。
[0187]
本领域普通技术人员可以理解：附图只是一个实施例的示意图，附图中的模块或流程并不一定是实施本发明所必须的。
[0188]
本领域普通技术人员可以理解：实施例中的装置中的模块可以按照实施例描述分布于实施例的装置中，也可以进行相应变化位于不同于本实施例的一个或多个装置中。上述实施例的模块可以合并为一个模块，也可以进一步拆分成多个子模块。
[0189]
最后应说明的是：以上实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明实施例技术方案的精神和范围。