1.本技术涉及智能驾驶传感器标定领域，特别涉及一种面向激光雷达与相机在线漂移检测与外参校正的方法。
背景技术：2.智能驾驶汽车在行驶过程中，由于车身振动不可避免地导致所装备的激光雷达和相机的相对姿态发生漂移，相应地，两传感器之间的外参也会随之变化。这种情况下，先前的标定参数将无法对齐激光雷达坐标系与相机坐标系，造成自动驾驶汽车感知性能衰退甚至失效，严重威胁行驶安全。
3.面对车辆运行期间的传感器漂移问题，有研究根据标定参数准确时，激光雷达点云数据中的深度不连续点往往会投影到图像边缘上这一认知，通过搜索和判断外参是否使点云边缘投影和图像边缘灰度值的乘积最大，来确定传感器是否漂移以进行外参校正。然而，由于两个传感器之间存在视差，激光雷达点云中远处点构成的边缘往往与近处点的边缘相交叉，导致点云投影后出现边缘模糊。此外，相机图像中的大量无效边缘特征与点云深度不连续点无关，这可能导致特征对齐出错，算法向错误的方向优化。深度学习技术也被用来解决传感器在线标定问题，有研究通过训练具有不同漂移程度的五个神经网络模型，来迭代地优化校正外参，但是基于深度学习的方法性能严重依赖于训练数据，这意味着当车辆行驶环境或传感器参数发生变化时，这类方法因为泛化能力弱导致校正效果较差；此外，该类方法缺乏有效的传感器漂移判断机制，算法运行时每一帧都进行校正，无疑浪费了计算资源。
4.总之，现有技术存在的问题是：基于点云和图像边缘对齐的方法由于传感器视差以及大量无效图像边缘问题，对漂移的判断以及校正效果不佳；基于深度学习的技术，缺乏传感器漂移判断机制，并且依赖大量训练数据，泛化性能较差。且以上两类方法都是试图从不同模态数据中，提取共同环境特征，这是非常容易出错的；此外，以上两类方法在激光雷达线束分辨率较低的方向标定效果较差。
技术实现要素：5.本发明所要解决的技术问题是提供一种在线的传感器漂移检测方法、校正方法、装置及存储介质，用于解决智能驾驶汽车在行驶过程中的激光雷达和相机在线漂移检测和自动校正问题。
6.本发明首先提供一种激光雷达与相机在线漂移检测方法，其特征在于，包括：
7.点云与图像配准，包括：
8.在数据时域上搜索距离激光雷达点云数据帧最近的相机图像数据帧，完成激光雷达和相机的数据同步；
9.基于同步的数据，通过点云配准和图像配准估计前n帧到当前帧的激光雷达运动
和相机运动；
10.传感器漂移检测，包括：
11.基于估计得到的激光雷达运动和相机运动，求解对应帧的激光雷达和相机间的运动偏差；
12.设定阈值，当对应帧的激光雷达和相机间的运动偏差大于设定阈值时，检测当前帧是否发生传感器漂移。
13.本发明还提供一种激光雷达与相机在线漂移外参校正方法，其特征在于，包括：
14.基于激光雷达与相机在线漂移检测方法对传感器漂移检测；
15.对于检测结果为传感器漂移的，进行传感器外参的校正，对传感器外参进行校正的方法，包括：
16.估计带尺度的相机运动，将当前图像帧中的二维特征点与对应的前n帧点云中三维点相匹配，求解带尺度的相机运动；
17.根据带尺度的相机运动，得到前一帧和当前帧之间的运动矩阵并平均化；
18.平均化后将未漂移的前一帧点云转换到当前位置作为参考点云，通过与当前漂移的点云配准得到最终的校正外参。
19.本发明传感器漂移检测方法，利用点云配准和图像配准技术估计激光雷达和相机前后帧间的运动，通过两个模态运动大小的对比来判断是否漂移。本发明校正方法，根据传感器运动来在线校正外参漂移。本发明方法无需从不同模态中提取共同边缘特征，无需大量训练数据。
20.本发明还提供一种激光雷达与相机在线漂移检测与外参校正装置，其特征在于，包括：
21.相机，用于实时获取环境图像；
22.激光雷达，用于实时获取环境点云数据；
23.以及，
24.处理器，执行上述在线漂移检测与外参校正方法的步骤。
25.相对于现有技术，本发明具有以下有益效果：
26.1、特征提取更准确全面：本方法基于点云、图像配准原理，从相同模态中提取共同特征，特征提取精准；且所提取的特征更为全面，不限于边缘特征，特征更容易对齐，不会导致算法优化方向出错。
27.2、无需大量训练数据：不依赖大量训练数据，泛化性能好；
28.3、漂移检测和校正的鲁棒性好：对激光雷达分辨率大小不敏感，在各个方向的漂移检测和标定鲁棒性良好。
附图说明
29.图1是本技术一个实施例提供的一种激光雷达与相机在线漂移检测与外参校正方法的整体框架图；
30.图2是本技术一个实施例提供的没有发生传感器漂移时激光雷达和相机运动偏差典型的频率分布直方图和拟合的高斯分布曲线；
31.图3是本技术一个实施例提供的不同的全局系数α和帧数n下漂移检测正确率的统
计试验结果；
32.图4是本技术一个实施例提供的二维特征点ukf与三维点云点p
c,k-n
匹配的示意图；
33.图5是本技术一个实施例提供的在传感器无漂移、小漂移、大漂移下，与基于边缘对齐的方法和深度学习的方法的在线漂移检测和校正的试验结果图；
34.图6是本技术一个实施例提供的在传感器小漂移和大漂移下，与基于边缘对齐的方法和深度学习的方法的外参自动校正后的图像反投影点云图。
具体实施方式
35.为使本技术实施例的目的、技术方案和优点更加清楚，下面将结合附图对本技术实施方式作进一步地详细描述。
36.请参考图1，其示出了本技术一个实施例提供的激光雷达与相机在线漂移检测与外参校正方法的整体框架图，该方法应用于智能驾驶车辆行驶过程中。该激光雷达与相机在线漂移检测与外参校正方法，可以包括：
37.本实施例中，传感器漂移是指以相机坐标系为参考，激光雷达坐标系相对于相机坐标系的位置和姿态变化，称为传感器漂移。
38.步骤101，点云与图像配准：首先在数据时域上搜索距离激光雷达点云数据帧最近的相机图像数据帧，完成激光雷达和相机的数据同步，将同步后的数据作为输入数据；然后分别通过点云配准和图像配准估计前n帧到当前帧的激光雷达运动和相机运动。
39.步骤1011，激光雷达运动估计：首先对激光雷达点云进行预处理，包括去除地面点和较远处点、下采样等，以增强点云配准的鲁棒性和处理速度。然后，根据式(1)使用原始外参t
lc
将点云转换到相机坐标系中：
[0040][0041]
其中，和分别表示相机坐标系和激光雷达坐标系下的一个三维点，和分别是t
lc
的旋转分量和平移分量，可以表示为欧拉角的形式，即yaw、pitch和roll，[r
ψ
,r
θ
,r
φ
]
t
，
[0042]
最后，将前n帧点云和当前点云进行配准，以估计帧间的激光雷达运动其中，和分别是的旋转分量和平移分量，n＝1,2,...,n。由于已经将点云转换到相机坐标系中，所以此处的激光雷达运动在未发生传感器漂移的情况下也代表相机运动。
[0043]
步骤1012，相机运动估计：在运动估计之前，相机已经过内部标定以校正镜头畸变，相机内参为k。首先，利用图像特征提取算法提取和匹配前n帧图像i
k-n
,n＝1,2,...,n与当前图像ik间的特征点然后结合内参k求解帧间的本质矩阵最后将本质矩阵分解为前n帧图像i
k-n
与当前帧图像ik之间的变换矩阵其中，和分别是的旋转分量和平移分量。由于单目相机固有的尺度模糊性，平移分量
是归一化形式，没有尺度信息。
[0044]
步骤102，传感器漂移检测：在获取了一组激光雷达运动和相机运动之后，求解对应帧的激光雷达和相机运动偏差，当任一运动偏差大于设定阈值时，检测当前帧是否发生传感器漂移。
[0045]
步骤1021，传感器运动偏差求解：按照式(2)求解两传感器运动的旋转部分和的偏差
[0046][0047]
将偏差按照式(3)转化为欧拉角的形式，以清晰展示每个旋转方向上的偏差：
[0048][0049]
其中，e
ψ,k-n
、e
θ,k-n
和e
φ,k-n
分别表示yaw、pitch和roll方向上的运动偏差，r
ij
表示偏差中的元素。
[0050]
步骤1022，未漂移状态阈值确定：由于传感器测量噪声、同步误差和运动估计误差的存在，即使在没有发生传感器漂移的情况下，运动偏差e
ψ,k-n
、e
θ,k-n
和e
φ,k-n
也不为0。
[0051]
请参考图2，图2中示出了在没有发生传感器漂移时，激光雷达和相机运动偏差e
ψ,k-n
、e
θ,k-n
和e
φ,k-n
典型的频率分布直方图和拟合的高斯分布曲线。
[0052]
通过试验记录前n帧和当前帧之间的激光雷达和相机运动偏差e
ψ,k-n
、e
θ,k-n
和e
φ,k-n
，n＝1,2,...,n，统计其频率分布直方图，并按照式(4)拟合高斯分布函数：
[0053][0054]
其中μ
j,k-n
和σ
j,k-n
分别为k-n帧j方向上拟合的高斯分布函数的均值和方差。为了避免在没有发生传感器漂移时出现误判，偏差阈值设定为μ
j,k-n-3σ
j,k-n
和μ
j,k-n
+3σ
j,k-n
，j＝ψ,φ,θ，当存在式(5)时，认为当前帧没有发生传感器漂移，
[0055]
μ
j,k-n-3σ
j,k-n
≤e
j,k-n
≤μ
j,k-n
+3σ
j,k-n
ꢀꢀ
(5)
[0056]
本实施例中，试验平台为搭载激光雷达和相机的车辆平台，车辆以10km/h的低速在正常道路上缓慢行驶，同时车载电脑记录激光雷达和相机的感知信息，在传感器没有发生漂移的前提下运行步骤101，点云与图像配准以及步骤1021，传感器运动偏差求解，得到运动偏差数据，并生成每个旋转方向运动偏差的频率分布直方图，拟合高斯函数。
[0057]
步骤1023，漂移状态确定：如果任一欧拉角偏差e
j,k-n
超过阈值则当前帧可能已发生传感器漂移，相应旋转轴上发生漂移的置信度c
j,k-n
可通过以下公式描述：
[0058]
[0059]
考虑每一帧的最大欧拉角偏差以及三个欧拉角偏差的均方根，当式(7)成立时，认为当前帧发生了传感器漂移，
[0060][0061]
其中，α是全局系数。
[0062]
本实施例中，为了确定最佳超参数，即全局系数α和前n帧中n的大小，进行了一系列统计试验。搭载激光雷达和相机的车辆平台以10km/h的低速在正常道路上缓慢行驶，同时车载电脑记录激光雷达和相机的感知数据，作为输入数据。对输入的激光雷达点云数据每间隔大于等于n帧，即左乘一个随机偏移矩阵tr，如式(8)所示：
[0063]
pc＝trt
lc
p
l
ꢀꢀꢀ
(8)
[0064]
在不同的全局系数α和帧数n下，运行步骤101，点云与图像配准，步骤1021，传感器运动偏差求解，步骤1022，未漂移状态阈值确定，步骤1023，漂移状态确定。统计试验结果如图3所示，发现在n为7、全局系数α为0.945下，得到最大漂移状态判断精度99.63％。因此，选择上述两个数据作为步骤1023中的超参数。
[0065]
步骤103，外参自动校正：当检测到当前帧发生了传感器漂移时，首先估计带尺度的相机运动，将当前图像帧ik中的二维特征点与对应的前n帧点云p
k-n
中三维点p
c,k-n
相匹配，求解带尺度的相机运动结合步骤1011，激光雷达运动估计中得到的相机运动得到前一帧和当前帧之间的运动矩阵平均化后将点云p
k-1
转换到当前位置作为参考点云p
kr
，通过与当前漂移的点云pk配准得到最终的校正外参
[0066]
请参考图4，图4中示出了二维特征点与三维点云点p
c,k-n
匹配的示意图。
[0067]
步骤1031，带尺度的相机运动估计：首先，按照式(9)将未发生漂移的激光雷达点云p
k-n
投影到对应的图像i
k-n
上：
[0068][0069]
其中，是三维点p
c,k-n
在像素坐标系中的投影点，π(
·
)是投影函数。
[0070]
然后，在步骤1012，相机运动估计中已经提取了图像特征点按照式(10)根据图像特征点和三维投影点间的欧式距离搜索距图像特征点最近的三维投影点
[0071][0072]
其中，ta是范围阈值，在本实施例中，ta设置为2像素。若图像特征点附近存在多个三维投影点则选择深度最小的三维点p
c,k-n
对应的投影点将其与图像特征点匹配。
[0073]
在步骤1012，相机运动估计中，已经提取并匹配了前n帧图像i
k-n
和当前图像i
k-n
之间的特征点。那么由此可以建立前n帧点云p
k-n
中的三维点p
c,k-n
和当前图像中二维特征点的对应关系，使用pnp算法求解带尺度的相机运动
[0074]
步骤1032，转换矩阵求解：在步骤1011，激光雷达运动估计中，可以得到前n帧点云p
k-n
中每个相邻帧之间的激光雷达运动中每个相邻帧之间的激光雷达运动因为点云已经转换到相机坐标系，而且前n帧都没有出现传感器漂移，所以这里的也代表相机运动。据此进一步建立运动约束：
[0075][0076]
由此可以得到一系列前一帧和当前帧之间的运动矩阵矩阵通过式(12)求得平均化的运动矩阵
[0077][0078]
其中，是对应旋转分量的四元数，和分别是的旋转分量和平移分量。
[0079]
步骤1033，漂移点云校正：将前一帧没有漂移的点云p
k-1
通过运动矩阵转换到当前位置下作为参考点云p
kr
，然后将当前漂移的点云pk与参考点云p
kr
配准，得到最终的校正矩阵最后，激光雷达和相机之间的外参通过式(13)校正：
[0080][0081]
图5为本方法在传感器无漂移、小漂移、大漂移下，与基于边缘对齐的方法和深度学习的方法的在线漂移检测和校正的试验结果图，可以看出本方法总是能够识别正确的校准，而基于边缘对齐的方法存在一些错误的判断，基于深度学习的方法没有漂移判断机制，所以每帧都运行自动校正程序，但是其泛化能力不好，不能收敛到正确的外参。在之后的传感器小漂移和大漂移情况下，本方法在各个方向上都表现良好，能够有效的检测漂移并校正外参，完全胜过其他方法。
[0082]
图6为本方法在传感器小漂移和大漂移下，与基于边缘对齐的方法和深度学习的方法的外参自动校正后的图像反投影点云图，图中虚线框为对齐不准的地方。可以看出，当深度发生显著变化时，本方法的结果存在一些轻微的偏差，例如远处的绿化带投影到了车辆顶部。然而，与其他两种方法相比，本方法总体上具有更高的对齐精度和鲁棒性。
[0083]
综上所述，本技术实施例提供的一种激光雷达与相机在线漂移检测与外参校正方法，将同步后的激光雷达和相机数据作为输入数据，分别通过点云配准和图像配准估计前n
帧到当前帧的激光雷达运动和相机运动；求解激光雷达和相机运动偏差，当任一运动偏差大于设定阈值时，检测当前帧是否发生传感器漂移；当检测到当前帧发生了传感器漂移时，首先估计带尺度的相机运动，结合相机运动得到前一帧和当前帧间的运动矩阵平均化后将点云p
k-1
转换到当前位置作为参考点云p
kr
，通过与当前漂移的点云pk配准得到最终的校正外参所提出的激光雷达与相机在线漂移检测与外参校正方法基于点云、图像配准原理，从相同模态中提取共同特征，特征提取精准、全面且鲁棒，不依赖大量训练数据，泛化性能好，对激光雷达分辨率大小不敏感，在各个方向的漂移检测和标定鲁棒性良好。
[0084]
本实施例还提供了一种计算机可读存储介质，该计算机可读存储介质包括在用于存储信息(诸如计算机可读指令、数据结构、计算机程序模块或其他数据)的任何方法或技术中实施的易失性或非易失性、可移除或不可移除的介质。计算机可读存储介质包括但不限于ram(random access memory，随机存取存储器),rom(read-only memory，只读存储器)，eeprom(electrically erasable programmable read only memory，带电可擦可编程只读存储器)、闪存或其他存储器技术、cd-rom(compact disc read-only memory，光盘只读存储器)，数字多功能盘(dvd)或其他光盘存储、磁盒、磁带、磁盘存储或其他磁存储装置、或者可以用于存储期望的信息并且可以被计算机访问的任何其他的介质。
[0085]
本实施例中的计算机可读存储介质可用于存储一个或者多个计算机程序，其存储的一个或者多个计算机程序可被处理器执行，以实现上述实施例中激光雷达与相机在线漂移检测与外参校正方法步骤。
技术特征：1.一种激光雷达与相机在线漂移检测方法，其特征在于，包括：点云与图像配准，包括：在数据时域上搜索距离激光雷达点云数据帧最近的相机图像数据帧，完成激光雷达和相机的数据同步；基于同步的数据，通过点云配准和图像配准估计前n帧到当前帧的激光雷达运动和相机运动；传感器漂移检测，包括：基于估计得到的激光雷达运动和相机运动，求解对应帧的激光雷达和相机间的运动偏差；设定阈值，当对应帧的激光雷达和相机间的运动偏差大于设定阈值时，检测当前帧是否发生传感器漂移。2.根据权利要求1所述的激光雷达与相机在线漂移检测方法，其特征在于，通过点云配准和图像配准估计前n帧到当前帧的激光雷达运动和相机运动，包括：使用原始外参t
lc
将点云转换到相机坐标系中：其中，和分别表示相机坐标系和激光雷达坐标系下的一个三维点，和分别是t
lc
的旋转分量和平移分量，表示为欧拉角的形式，即yaw、pitch和roll，[r
ψ
,r
θ
,r
φ
]
t
，将前n帧点云和当前点云进行配准，以估计帧间的激光雷达运动其中，和分别是的旋转分量和平移分量，n＝1,2,...,n；提取和匹配前n帧图像i
k-n
与当前图像i
k
间的特征点然后结合相机内参k求解帧间的本质矩阵将本质矩阵分解为前n帧图像i
k-n
与当前帧图像i
k
之间的变换矩阵：其中，和分别是的旋转分量和平移分量。3.根据权利要求2所述的激光雷达与相机在线漂移检测方法，其特征在于，基于估计得到的激光雷达运动和相机运动，求解对应帧的激光雷达和相机间的运动偏差，包括：求解两传感器运动的旋转部分和的偏差的偏差将偏差转化为欧拉角的形式：e
ψ,k-n
＝atan2(r
21
,r
11
)
e
φ,k-n
＝atan2(r
32
,r
33
)其中，e
ψ,k-n
、e
θ,k-n
和e
φ,k-n
分别表示yaw、pitch和roll方向上的运动偏差，r
ij
表示偏差中的元素。4.根据权利要求3所述的激光雷达与相机在线漂移检测方法，其特征在于，设定阈值，当对应帧的激光雷达和相机间的运动偏差大于设定阈值时，检测当前帧是否发生传感器漂移，包括：根据前n帧和当前帧之间的激光雷达和相机运动偏差e
ψ,k-n
、e
θ,k-n
和e
φ,k-n
，n＝1,2,...,n，得到偏差阈值为μ
j,k-n-3σ
j,k-n
和μ
j,k-n
+3σ
j,k-n
，其中，μ
j,k-n
和σ
j,k-n
分别为k-n帧j方向上拟合的高斯分布函数的均值和方差，j＝ψ,φ,θ；拟合的高斯分布函数为：当μ
j,k-n-3σ
j,k-n
≤e
j,k-n
≤μ
j,k-n
+3σ
j,k-n
时，认为当前帧没有发生传感器漂移；如果任一欧拉角偏差e
j,k-n
超过阈值计算相应旋转轴上发生漂移的置信度c
j,k-n
：考虑每一帧的最大欧拉角偏差以及三个欧拉角偏差的均方根，当下式成立时，认为当前帧发生了传感器漂移，其中，α是全局系数。5.一种激光雷达与相机在线漂移外参校正方法，其特征在于，包括：基于权利要求1-4任一所述的激光雷达与相机在线漂移检测方法对传感器漂移检测；对于检测结果为传感器漂移的，进行传感器外参的校正。6.根据权利要求5所述的激光雷达与相机在线漂移外参校正方法，其特征在于，对传感器外参进行校正的方法，包括：估计带尺度的相机运动，将当前图像帧中的二维特征点与对应的前n帧点云中三维点相匹配，求解带尺度的相机运动；根据带尺度的相机运动，得到前一帧和当前帧之间的运动矩阵并平均化；平均化后将未漂移的前一帧点云转换到当前位置作为参考点云，通过与当前漂移的点云配准得到最终的校正外参。7.根据权利要求6所述的激光雷达与相机在线漂移外参校正方法，其特征在于，求解带尺度的相机运动的方法，包括：将未发生漂移的激光雷达点云p
k-n
投影到对应的图像i
k-n
上：
其中，是三维点p
c,k-n
在像素坐标系中的投影点，π(
·
)是投影函数；根据图像特征点和三维投影点间的欧式距离搜索距图像特征点最近的三维投影点投影点其中，t
a
是范围阈值；若图像特征点附近存在多个三维投影点则选择深度最小的三维点p
c,k-n
对应的投影点将其与图像特征点匹配；建立前n帧点云p
k-n
中的三维点p
c,k-n
和当前图像中二维特征点的对应关系，使用pnp算法按照下式求解带尺度的相机运动算法按照下式求解带尺度的相机运动根据带尺度的相机运动，得到前一帧和当前帧之间运动矩阵并平均化，包括：在激光雷达运动估计中，得到前n帧点云p
k-n
中每个相邻帧之间的激光雷达运动建立运动约束，得到一系列前一帧和当前帧之间的运动矩阵其中运动约束为：求得平均化的运动矩阵求得平均化的运动矩阵求得平均化的运动矩阵求得平均化的运动矩阵其中，是对应旋转分量的四元数，和分别是的旋转分量和平移分量；平均化后将未漂移的前一帧点云转换到当前位置作为参考点云，通过与当前漂移的点云配准得到最终的校正外参，包括：将前一帧没有漂移的点云p
k-1
通过运动矩阵转换到当前位置下作为参考点云将当前漂移的点云p
k
与参考点云配准，得到最终的校正矩阵根据最终的校正矩阵对激光雷达和相机之间的外参通过下式校正：
8.一种激光雷达与相机在线漂移检测与外参校正装置，其特征在于，包括：相机，用于实时获取环境图像；激光雷达，用于实时获取环境点云数据；以及，处理器，执行所述权利要求5-7任一所述激光雷达与相机在线外参校正方法的步骤。9.一种计算机可读存储介质，所述计算机可读存储介质存储有计算机程序，其特征在于，所述计算机程序被处理器执行时实现如权利要求5-7任一所述激光雷达与相机在线外参校正方法的步骤。
技术总结本发明公开了一种激光雷达与相机在线漂移检测方法、校正方法、装置及存储介质，在线漂移检测方法包括点云与图像配准和传感器漂移检测，点云与图像配准包括：在数据时域上搜索距离激光雷达点云数据帧最近的相机图像数据帧，完成激光雷达和相机的数据同步；基于同步的数据，通过点云配准和图像配准估计前N帧到当前帧的激光雷达运动和相机运动；传感器漂移检测包括：基于估计得到的激光雷达运动和相机运动，求解对应帧的激光雷达和相机间的运动偏差；当对应帧的激光雷达和相机间的运动偏差大于设定阈值时，检测当前帧是否发生传感器漂移。本发明方法特征提取精准、全面且鲁棒，泛化性能好，在各个方向的漂移检测和标定精度和鲁棒性良好。棒性良好。棒性良好。
技术研发人员：殷国栋 彭湃 徐利伟 庄伟超 祝小元 张宁 王金湘 耿可可 高强 张辉
受保护的技术使用者：东南大学
技术研发日：2022.06.21
技术公布日：2022/11/1
