一种固态激光雷达-相机紧耦合位姿估计方法技术

本发明专利技术提出一种固态激光雷达

【技术实现步骤摘要】
一种固态激光雷达
‑
相机紧耦合位姿估计方法


[0001]本专利技术涉及固态激光雷达位姿估计，属于机器人状态估计领域。在三维重建、VR看房、SLAM、自动驾驶、机器人等领域具有广阔的应用前景。

技术介绍

[0002]激光雷达分两类，传统机械式激光雷达(代表公司：Velodyne)和新兴的固态激光雷达(代表公司：Livox)。机械式激光雷达价格昂贵(32线激光雷达需要几十万元)，需要机械旋转结构旋转激光发射器，扫描出的点云呈现环状。而固态激光雷达相对便宜(Livox Horizon仅需8000元)，通过内部光学棱镜旋转激光发射器获得非重复扫描的点云，在进行积分后可以获得覆盖视场的稠密点云。本专利技术聚焦固态激光雷达的位姿估计技术，它属于激光雷达位姿估计的一个子类。此外本专利技术为固态激光雷达
‑
相机紧耦合位姿估计，即紧密地利用相机图像信息和激光雷达点云信息进行位姿估计，与分别估计相机位姿和激光雷达位姿再融合的松耦合形式有较大区别。
[0003]激光雷达位姿估计技术是一种对激光雷达的位置和姿态进行估计的技术，也可以理解成激光雷达点云配准技术或激光雷达建图技术，在各行各业有着许多重要的应用。比如自动驾驶车辆预先构建区域的高精地图(点云地图)，然后利用车载激光雷达的单帧点云数据与高精地图进行匹配，进而得到车辆的位姿(参见文献：Egger P,Borges P V K,Catt G,et al.PoseMap:Lifelong,Multi
‑
Environment 3D LiDAR Localization[C]//2018IEEE/RSJ International Conference on Intelligent Robots and Systems(IROS).IEEE,2018.)。再比如对移动激光雷达进行时序点云数据帧间关联，从而对场景进行建图(参见文献：Zhang J,Singh S.LOAM:Lidar odometry and mapping in real
‑
time[C]//Robotics:Science and Systems.2014,2(9):1
‑
9.)，该技术在遥感测绘、三维重建、数字孪生等领域中均有重要作用。
[0004]激光雷达位姿估计有三种常用方法：1.基于ICP(Iterative Closest Point)的方法，ICP方法是一种交替进行求最近点对、剔除距离较大点对和根据匹配点对找最优刚体变换的算法，属于EM(Expectation Maximum)优化算法范畴。研究人员在经典的点到点的ICP(参见文献：Besl P J,McKay N D.Method for registration of 3
‑
D shapes[C]//Sensor fusion IV:control paradigms and data structures.Spie,1992,1611:586
‑
606.)算法基础上发展了点到面的ICP算法(参见文献：Low K L.Linear least
‑
squares optimization for point
‑
to
‑
plane icp surface registration[J].Chapel Hill,University of North Carolina,2004,4(10):1
‑
3.)，面到面的ICP(参见文献：Segal A,Haehnel D,Thrun S.Generalized
‑
icp[C]//Robotics:science and systems.2009,2(4):435.)算法和基于体素的面到面ICP(参见文献：Koide K,Yokozuka M,Oishi S,et al.Voxelized gicp for fast and accurate3d point cloud registration[C]//2021IEEE International Conference on Robotics and Automation(ICRA).IEEE,2021:11054
‑
11059.)算法。2.基于NDT(Normal Distributions Transform)的方法(参见文献：
Biber P,Straβer W.The normal distributions transform:A new approach to laser scan matching[C]//Proceedings 2003IEEE/RSJ International Conference on Intelligent Robots and Systems(IROS 2003)(Cat.No.03CH37453).IEEE,2003,3:2743
‑
2748.)，具体步骤为首先将参考点云划分为指定大小的体素，然后对每个体素内拟合一个高斯分布(计算高斯分布均值和协方差矩阵)。接着对目标点云位姿进行非线性最小二乘优化，使经过刚体变换后的目标点云落入参考点云分布的概率密度之和最大。3.基于LOAM(Lidar Odometry and Mapping)的方法，该方法是基于激光雷达的SLAM系统较常采用的方法，具体步骤是对扫描产生的点云线束计算拉普拉斯算子，通过计算结果大小来把点云分成平面点和边缘点两类。接着使用面到面的ICP和点到线的ICP进行位姿优化。后续LOAM系统进一步发展引入了回环检测，比如Lego
‑
LOAM(参见文献：Shan T,Englot B.Lego
‑
loam:Lightweight and ground
‑
optimized lidar odometry and mapping on variable terrain[C]//2018IEEE/RSJ International Conference on Intelligent Robots and Systems(IROS).IEEE,2018:4758
‑
4765.)；针对新型的固态激光雷达特点进行改进，比如Livox
‑
LOAM(参见文献：Lin J,Zhang F.Loam livox:A fast,robust,high
‑
precision LiDAR odometry and mapping package for LiDARs of small FoV[C]//2020IEEE International Conference on Robotics and Automation(ICRA).IEEE,2020:3126
‑
3131.)。
[0005]三种方法的详细对比如下表所示，其中ICP、ND本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种固态激光雷达
‑
相机紧耦合位姿估计方法，其特征在于，具体步骤如下：步骤一：对相机相邻两帧图像进行尺度不变特征变换SIFT特征点检测和描述子提取，然后进行特征点匹配，获得图像匹配二维特征点；步骤二：将相邻两帧固态激光雷达的非重复扫描点云投影到对应图像帧上，得到深度图；再通过深度图补全算法得到稠密深度图，进而图像的二维匹配点通过深度图变成三维匹配点；步骤三：对三维匹配点进行基于刚体变换的随机抽样一致RANSAC算法，得到相邻相机位姿；通过相机和固态激光雷达构成的位姿约束计算相邻固态激光雷达位姿，即粗略估计位姿；步骤四：对固态激光雷达点云计算曲率，并根据曲率划分平面点和边缘点分别构建点到面的迭代最近点算法ICP和点到线的ICP激光雷达因子；步骤五：对图像提取2D配准点构建基于对极几何的相机因子；步骤六：融合激光雷达优化因子和相机优化因子，并进行因子图优化，最终得到精细的固态激光雷达位姿估计；输出：配准好的固态激光雷达点云，固态激光雷达位姿。2.根据权利要求1所述的一种固态激光雷达
‑
相机紧耦合位姿估计方法，其特征在于：所述步骤一具体如下：1.1：对相邻图像进行SIFT特征点检测并提取描述子；1.2：使用蛮力匹配器进行粗匹配，用基于单应性矩阵的模型剔除粗匹配中的离群点，进而得到最终图像相邻帧二维配准点。3.根据权利要求1所述的一种固态激光雷达
‑
相机紧耦合位姿估计方法，其特征在于：所述步骤二具体如下：2.1：将点云投影至可见光图像上，将点云沿着相机z轴方向的深度信息赋值给最近邻像素，即获得深度图像，此时深度图像还存在许多未定义点；2.2：采用基于形态学运算的深度图补全算法，对深度图进行补全，交替使用形态学闭操作、膨胀操作、中值滤波与高斯滤波操作；在深度图补全之后，整张深度图均有深度定义，进而通过反向投影把所有二维匹配点均可变成三维匹配点；具体步骤为：设图像匹配像素的齐次坐标为x＝[u，v，1],则它对应的在相机坐标系下的三维坐标求解方式见式(1)；其中K
rgb
，D
rgb
分别为可见光相机内参和畸变系数，已经提前标定好，是已知值；其中代表在相机归一化平面上根据畸变系数D
rgb
对像素p进行去畸变操作，z是可见光图像像素[u，v]处的深度；由于之前已经获得了补全深度图I
depth
，各个像素的深度在其上均有定义z＝I
depth
(u，v)，因此根据式(1)所有二维匹配点均可变成三维匹配点。4.根据权利要求1所述的一种固态激光雷达
‑
相机紧耦合位姿估计方法，其特征在于：所述步骤三具体如下：3.1：使用基于刚体变换的随机一致性采样算法RANSAC剔除3D匹配点对中的误匹配点，得到相邻相机的粗略估计位姿；步骤二输出N对三维匹配点集合S
M
＝{(s
k
，d
k
)}
k＝1，2，..，N
，其中(s
k
，d
k
)代表上一帧图像第k个对应点s
k
与当前帧图像第k个对应点d
k
构成的点对；从S
M
中
随机从中采样T轮，每一轮使用均匀分布随机采样m个点对；然后用m个点对计算刚体变换[R
i
|t
i
]；其中R
i
代表第i轮采样计算的旋转矩阵，t
i
代表第i轮采样计算的平移向量；利用该刚体变换对所有s
k
做变换，得到变换后的点s
′
k
＝R
i
×
s
k
+t
i
，若s
′
k
与d
k
的距离小于距离阈值∈
d
，则被判定为内点；统计每一轮采样得到刚体变换的内点数目，进而输出内点数目最多的刚体变换作为最优刚体变换，即相机帧间相对位姿粗略估计3.2：基于刚体变换的RANSAC算法将输出相机帧间相对位姿粗略估计根据激光雷达与相机位姿约束关系，激光雷达...

【专利技术属性】
技术研发人员：白相志，汪虹宇，
申请(专利权)人：北京航空航天大学，
类型：发明
国别省市：