一种激光雷达、双目相机和惯导的在线联合标定方法

1.本发明涉及多传感器联合标定技术领域，尤其涉及一种激光雷达、双目相机和惯导的在线联合标定方法。


背景技术：

2.多传感器联合标定技术是机器人、无人驾驶等领域的重要难点。在当前多传感器联合标定算法中，大多数方法使用离线人工手动标定，需要标定板进行操作，或者只是两个传感器之间的融合，并不涉及三个及以上传感器的数据融合与标定。但在现实应用中，机器人或者无人驾驶汽车往往需要搭载多种传感器进行环境观测，并且这些传感器往往会随时间因为机械振动或者损伤产生偏移，所以研发一种多传感器的在线自动标定算法非常具有意义。


技术实现要素：

3.本发明的目的是提供一种激光雷达、双目相机和惯导的在线联合标定方法，不需要人为设置标定参照物，也不需要特定环境，提升了标定方法的便捷性和鲁棒性。
4.本发明为实现上述发明目的采用如下技术方案：
5.本发明提供了一种激光雷达、双目相机和惯导的在线联合标定方法，所述方法包括：
6.获取激光雷达数据、双目相机图像数据和惯导数据；
7.对双目相机图像数据、激光雷达数据进行实时直线特征提取，获得图像特征帧和激光雷达特征帧；
8.将单个图像特征帧与对应的相邻的惯导数据预积分结果相结合构造单目相机
‑
惯导数据对，将单个激光雷达特征帧与对应的相邻的惯导数据预积分结果相结合构造激光雷达
‑
惯导数据对；
9.根据单目相机
‑
惯导数据对和激光雷达
‑
惯导数据对信息，计算最优的单目相机与惯导以及激光雷达与惯导的外参；
10.以惯导数据的时间戳为基准，对齐激光雷达数据和双目相机图像数据的时间戳，构造激光雷达
‑
相机数据对，优化重投影误差并计算激光雷达和单目相机之间的外参；
11.将图像特征帧中的直线特征的坐标系统一到世界坐标系下，构造相机
‑
相机数据对并进行配准，计算两个单目相机之间的外参；
12.将单目相机
‑
惯导、激光雷达
‑
惯导、激光雷达
‑
单目相机和单目相机
‑
单目相机的外参进行全局优化，完成在线联合标定。
13.进一步地，获取激光雷达数据、双目相机图像数据和惯导数据，并对惯导数据进行预积分处理，初始化系统位姿。
14.进一步地，获取激光雷达数据、双目相机图像数据和惯导数据，并对惯导数据进行预积分处理，初始化系统位姿的方法包括：
15.通过ros实时获取激光雷达的点云数据、双目相机的图像数据和惯导的加速度、角速度数据；
16.对惯导的加速度和角速度进行预积分处理，计算得到系统的初始位姿，所述初始位姿包括速度、旋转和平移。
17.进一步地，对单帧图像帧和单帧激光雷达帧数据分别进行实时直线特征提取的方法包括：
18.利用改进的lsd算法提取图像帧的直线特征并过滤；
19.利用改进的ransac算法提取激光雷达帧的直线特征并过滤；
20.保存每一帧图像和点云数据的直线特征信息。
21.进一步地，计算最优的单目相机与惯导以及激光雷达与惯导的外参的方法包括：
22.将惯导预积分计算出的运动变换作用在当前时间戳的图像直线特征帧上，获得预测的下一帧图像直线特征帧，再利用lm非线性优化算法将预测的下一帧图像直线特征帧和真实的从图像原始数据提取到的下一帧图像直线特征帧作为算法输入，计算两者的重投影误差，得到相机
‑
惯导的外参；
23.将惯导预积分计算出的运动变换作用在当前时间戳的激光雷达直线特征帧上，获得预测的下一帧激光雷达直线特征帧，再利用lm+ndt非线性优化算法将预测的下一帧激光雷达直线特征帧和真实的从激光雷达原始数据提取到的下一帧激光雷达直线特征帧作为算法输入，计算两者的重投影误差，得到激光雷达
‑
惯导的外参。
24.进一步地，对齐激光雷达数据和双目相机图像数据的时间戳的方法包括：
25.以惯导数据的时间戳作为基准，通过timesynchronizer滤波器统一接收激光雷达和两个单目相机的数据；当所有数据都有相同的时间戳时，产生一个同步结果的回调函数，在回调函数里处理同步时间后的数据。
26.进一步地，计算激光雷达和单目相机之间的外参的方法包括：
27.对在时间戳上相邻的激光雷达
‑
相机数据对的直线特征进行pnp求解，计算出相机坐标系和雷达坐标系之间的外参。
28.进一步地，计算两个单目相机之间的外参的方法包括：
29.利用sift算法将两个图像帧中的直线特征进行匹配，并将图像二维坐标统一到世界坐标系下，计算两相机之间的外参。
30.进一步地，将单目相机
‑
惯导、激光雷达
‑
惯导、激光雷达
‑
单目相机和单目相机
‑
单目相机的外参进行全局优化，随着系统移动不断迭代更新，直至达到全局误差阈值的方法包括：
31.将单目相机
‑
惯导、激光雷达
‑
惯导、激光雷达
‑
单目相机和单目相机
‑
单目相机的外参代入对应数据对；
32.移动系统，随着数据更新，计算上一帧外参得到的当前帧特征匹配误差结果是否在可接受阈值范围内；
33.若达到全局误差阈值则接受上一帧外参，否则在新的误差结果上更新优化上一帧外参，以此不断迭代。
34.进一步地，所述系统包括通过支架固定的激光雷达、双目相机和惯导的硬件平台，所述激光雷达和双目相机的视野重合范围在80％以上。
35.本发明的有益效果如下：
36.本发明可以同时标定激光雷达、双目相机和惯导三种不同传感器的外参，为多模态数据融合技术提供了新型算法；
37.提出的三种传感器间的标定方法是在线全自动的，不需要人为设置标定参照物，也不需要特定环境，提升了标定方法的便捷性和鲁棒性。
附图说明
38.图1为根据本发明实施例提供的一种激光雷达、双目相机和惯导的在线联合标定方法的流程图；
39.图2为根据本发明实施例提供的一种激光雷达、双目相机和惯导的坐标系之间的变换关系图；
40.图3为根据本发明实施例提供的一种激光雷达、双目相机和惯导时间戳对齐后的数据示意图。
具体实施方式
41.下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
42.如图1所示，本发明的实施例1提供一种激光雷达、双目相机和惯导的在线联合标定方法，该方法包括以下步骤：
43.搭建激光雷达、双目相机和惯导的硬件平台；
44.实时获取激光雷达、双目相机和惯导的原始数据，并对惯导数据进行预积分处理，初始化系统位姿；以惯导数据的时间戳为基准，对齐激光雷达数据和图像数据的时间戳；对图像帧和激光雷达帧数据进行实时直线特征提取；将单个图像特征帧与图像帧间预积分的惯导数据以及单个激光雷达特征帧与激光雷达帧间预积分的惯导数据进行融合，构造相机
‑
惯导数据对和激光雷达
‑
惯导数据对；
45.惯导预积分结果提供运动补偿，以非线性优化方法计算最优的单目相机与惯导以及激光雷达与惯导的外参，使得相邻图像特征帧的重投影误差最小以及相邻激光雷达特征帧的点云配准误差最小；将激光雷达数据的直线特征投影到其相邻时间戳的图像二维平面上，构造激光雷达
‑
相机数据对，优化重投影误差并计算激光雷达和单目相机之间的外参；将两个单目相机的直线特征的坐标系统一到世界坐标系下，构造相机
‑
相机数据对并进行配准，计算两个单目相机之间的外参；
46.将相机
‑
惯导、激光雷达
‑
惯导、激光雷达
‑
相机和相机
‑
相机的外参进行全局优化，随着系统缓慢移动不断迭代更新，直至达到全局误差阈值。
47.所述搭建激光雷达、双目相机和惯导的硬件平台具体包括：使用金属架将激光雷达、双目相机和惯导固定，保证激光雷达和双目相机的视野重合范围达80％以上，以便于标定时可以较快地进行特征匹配。
48.所述实时获取激光雷达、双目相机和惯导的原始数据具体包括：在ros中创建激光
雷达数据帧消息并发布，其中记录了激光雷达数据帧的时间戳和点云xyz位置信息；创建双目相机数据帧消息并发布，其中记录了左右两个相机图像数据帧的时间戳和rbg像素信息；创建惯导数据帧消息并发布，其中记录了惯导采集到的加速度和角速度信息。
49.所述对惯导数据进行预积分处理，初始化系统位姿具体包括：利用ros订阅惯导数据帧发布的加速度a
b
和角速度w
b
信息，在考虑原始数据随时间发生的微小偏移b
a
、b
g
和高斯白噪声η
a
、η
g
，可以通过对某时间戳区间δt
ij
内的加速度和角速度进行积分运算，计算出相对于初始位置的旋转平移和速度量
[0050][0051][0052][0053]
其中，w1和b分别代表世界坐标系和惯导坐标系，g代表重力加速度，i和j时间戳区间δt
ij
的起始和终止位置，也可以用来表示相邻两关键数据帧的下标。通过预积分算法，就可以对系统位姿进行初始化，便于后期系统状态估计计算。
[0054]
所述以惯导数据的时间戳为基准，对齐激光雷达数据和图像数据的时间戳具体包括：考虑到图像帧的数据频率为30hz，激光雷达数据帧频率为60hz，惯导数据帧频率为200hz，在不考虑时间戳对齐问题的情况下，利用ros每接收两帧图像帧，就会同时接收到4帧激光雷达数据帧以及若干帧惯导数据帧，但由于三种传感器的硬件设备处理速度不同，导致采集的完整数据在时间戳上会产生一定偏差。算法将相邻两图像帧的时间戳作为公共时间戳区间的起始和终止，记录在此区间内的全部惯导数据的时间戳，再以这些惯导数据的时间戳为基准，搜索时间戳区间内的有效激光雷达数据帧，并将三种传感器在时间戳上的误差以常数补偿的形式进行时间戳对齐，效果如图3所示。
[0055]
所述对图像帧和激光雷达帧数据进行实时直线特征提取具体包括：为了提取直线特征，改进lsd算法对图像帧先进行高斯下采样，计算每个像素点的梯度值以及梯度方向，根据梯度值对所有像素点进行伪排序，建立状态列表，搜索梯度最大的像素点周围的方向在阈值内的像素，构造矩形，并根据矩形内的同向点密度进行截断，若误报数小于阈值，则接收该直线，由此将图像帧过滤成多个直线特征的集合l
cam
；改进ransac算法对激光雷达帧的三维点云进行过滤，若某个三维点及其周围小于阈值距离的三维点满足空间直线特征方程，则提取出来，构造点云直线特征，由此将激光雷达帧也过滤成多个直线特征的集合l
lidar
。由于激光雷达和相机的视野重叠率大于80％，所以大部分的直线特征都是可以用于配准计算的。
[0056]
所述将单个图像特征帧与图像帧间预积分的惯导数据以及单个激光雷达特征帧与激光雷达帧间预积分的惯导数据进行融合，构造相机
‑
惯导数据对和激光雷达
‑
惯导数据对，具体包括：利用ros将图像特征帧l
cam
和预积分后的惯导数据帧进行数据融合，构造相机
‑
惯导数据对cam_imu＝{t，x
c
，y
c
，r，p，v}，其中记录了对齐后的时间戳、图像帧里面直线的像素坐标和系统位姿信息；将激光雷达特征帧l
lidar
和预积分后的惯导数据帧进行数据融合，构造激光雷达
‑
惯导数据对lidar_imu＝{t，x
l
，y
l
，z
l
，r，p，v}，其中记录了对齐后的时间
戳、激光雷达帧里面直线的点云坐标和系统位姿信息。
[0057]
所述惯导预积分结果提供运动补偿，以非线性优化方法计算最优的单目相机与惯导以及激光雷达与惯导的外参，使得相邻图像特征帧的重投影误差最小以及相邻激光雷达特征帧的点云配准误差最小，具体包括：将两个相邻时间戳的图像特征帧l
cam
和l
′
cam
的坐标系统一到世界坐标系下，根据惯导预积分计算得到的旋转和平移得到运动补偿后的图像特征帧使用lm非线性优化算法找到合适的单目相机与惯导的外参，使得图像特征帧和l
′
cam
的重投影误差loss
cam_imu
最小；直接将惯导预积分结果作用在激光雷达特征帧l
lidar
上，得到运动补偿后的激光雷达特征帧使用lm非线性优化算法找到合适的激光雷达与惯导的外参，使得激光雷达特征帧和l
′
lidar
的配准误差loss
lidar_imu
最小。
[0058]
所述将激光雷达数据的直线特征投影到其相邻时间戳的图像二维平面上，构造激光雷达
‑
相机数据对lidar_cam＝{t，x
l
，y
l
，z
l
，x
c
，y
c
}，优化重投影误差并计算激光雷达和单目相机之间的外参，具体包括：激光雷达直线特征的三维点云[x
l
，y
l
，z
l
]在像平面[x
l
，y
l
]＝[x
l
/z
l
，y
l
/z
l
]，根据相机内参计算图像特征帧的相平面坐标[x
c
，y
c
]，使用lm非线性优化算法找到合适的激光雷达与相机的外参，使得图像特征帧的像平面和激光雷达特征帧的像平面的重投影误差loss
lidar_cam
最小。
[0059]
所述将两个单目相机的直线特征的坐标系统一到世界坐标系下，构造相机
‑
相机数据对cam1_cam2＝{t，x
c1
，y
c1
，x
c2
，y
c2
}并进行配准，计算两个单目相机之间的外参，具体包括：根据左右相机的内参和焦距，统一直线特征到世界坐标系下，使用lm非线性优化算法找到合适的相机与相机的外参，使得世界坐标系下的左右两个相机的图像特征帧坐标的配准误差loss
cam1_cam2
最小。
[0060]
所述将相机
‑
惯导、激光雷达
‑
惯导、激光雷达
‑
相机和相机
‑
相机的外参进行全局优化，随着系统缓慢移动不断迭代更新，直至达到全局误差阈值，具体包括：随着系统缓慢移动，会有新的传感器数据消息被ros发布，不断迭代计算得到新的外参误差：loss
′
cam_imu
、loss
′
lidar_imu
、loss
′
lidar_cam
、loss
′
cam1_cam2
，若达到局部和全局的误差阈值，则接收新的外参。
[0061]
利用ros发布三种传感器的数据消息，激光雷达数据消息包括时间戳和点云的xyz空间坐标，图像数据消息包括时间戳和像素点的rgb像素信息，惯导数据包括时间戳、加速度和角速度信息。这三个数据帧可以被实时订阅。
[0062]
考虑到图像帧的数据频率为30hz，激光雷达数据帧频率为60hz，惯导数据帧频率为200hz，所以以频率最低的图像帧作为基准，预积分处理相邻两图像帧的惯导数据，初步估计系统在此时间区间内的速度、旋转和平移。
[0063]
考虑到惯导数据帧频率为200hz最高，所以以惯导时间戳作为基准，计算步骤002中惯导预积分涉及到图像帧和激光雷达帧的时间戳偏差并将这三种传感器的相邻时间戳数据帧采取时间戳对齐处理。
[0064]
提取图像帧和激光雷达数据帧的直线特征，具体包括：利用改进的lsd算法提取图像帧的直线特征并过滤，利用改进的ransac算法提取激光雷达帧的直线特征并过滤，仅保存每一帧图像和点云数据的直线特征信息，删除其他无效数据，减小后期匹配的计算复杂度。
[0065]
将提取直线特征后的图像帧、激光雷达帧以及惯导数据帧两两构造新的数据对，具体包括：以某两个相邻图像帧的时间戳为起始结束，在理想情况下这两个图像帧时间戳之间会存在四帧激光雷达帧以及若干惯导数据帧，分别将两个相邻图像帧和惯导数据帧构造相机
‑
惯导数据对；将四个激光雷达帧和惯导数据帧构造激光雷达
‑
惯导数据对；将两个相邻图像帧和四个激光雷达帧构造相机
‑
激光雷达数据对；将左右两个相机的图像帧构造相机
‑
相机数据对。
[0066]
如图2所示，需要计算相机
‑
惯导、激光雷达
‑
惯导、相机
‑
激光雷达和相机
‑
相机的外参，分别为具体包括：对当前经过运动变换后的图像直线特征和下一帧的图像直线特征利用lm非线性优化算法计算其重投影误差，得到相机
‑
惯导的外参；对当前经过运动变换后的激光雷达直线特征和下一帧的激光雷达直线特征利用lm+ndt算法对点云进行配准，得到相机
‑
惯导的外参；对最相邻激光雷达
‑
相机数据对的直线特征进行pnp求解，计算出相机坐标系和雷达坐标系之间的外参。
[0067]
将相机
‑
惯导、激光雷达
‑
惯导、激光雷达
‑
相机和相机
‑
相机的外参进行全局优化，随着系统缓慢移动不断迭代更新，直至达到全局误差阈值，具体包括：将相机
‑
惯导、激光雷达
‑
惯导、激光雷达
‑
相机和相机
‑
相机的外参代入对应数据对，缓慢移动硬件系统，随着全部传感器的数据更新，计算上一帧外参得到的当前帧特征匹配误差结果是否在可接受阈值范围内，若达到全局误差阈值则接受上一帧外参，否则在新的误差结果上更新优化上一帧外参，以此不断迭代。
[0068]
以上所述仅是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明技术原理的前提下，还可以做出若干改进和变形，这些改进和变形也应视为本发明的保护范围。