CN117218350A - 一种基于固态雷达的slam实现方法及系统 - Google Patents
一种基于固态雷达的slam实现方法及系统 Download PDFInfo
- Publication number
- CN117218350A CN117218350A CN202311210728.2A CN202311210728A CN117218350A CN 117218350 A CN117218350 A CN 117218350A CN 202311210728 A CN202311210728 A CN 202311210728A CN 117218350 A CN117218350 A CN 117218350A
- Authority
- CN
- China
- Prior art keywords
- point
- frame
- key frame
- representing
- pose
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Pending
Links
- 238000000034 method Methods 0.000 title claims abstract description 80
- 238000005457 optimization Methods 0.000 claims abstract description 31
- 230000011218 segmentation Effects 0.000 claims abstract description 23
- 238000012216 screening Methods 0.000 claims abstract description 21
- 230000001133 acceleration Effects 0.000 claims abstract description 17
- 238000000605 extraction Methods 0.000 claims abstract description 16
- 239000007787 solid Substances 0.000 claims abstract description 14
- 238000004364 calculation method Methods 0.000 claims abstract description 8
- 238000005070 sampling Methods 0.000 claims abstract description 7
- 238000005259 measurement Methods 0.000 claims description 25
- 239000011159 matrix material Substances 0.000 claims description 20
- 238000007781 pre-processing Methods 0.000 claims description 20
- 230000008569 process Effects 0.000 claims description 17
- 238000004590 computer program Methods 0.000 claims description 11
- 230000003044 adaptive effect Effects 0.000 claims description 9
- 238000004422 calculation algorithm Methods 0.000 claims description 7
- 230000008859 change Effects 0.000 claims description 6
- 230000005484 gravity Effects 0.000 claims description 3
- 230000010354 integration Effects 0.000 claims description 3
- 230000000295 complement effect Effects 0.000 claims description 2
- 238000010276 construction Methods 0.000 abstract description 3
- 238000013507 mapping Methods 0.000 description 7
- 230000006870 function Effects 0.000 description 5
- 230000001788 irregular Effects 0.000 description 4
- 238000013459 approach Methods 0.000 description 3
- 238000012545 processing Methods 0.000 description 3
- 238000010586 diagram Methods 0.000 description 2
- 230000000694 effects Effects 0.000 description 2
- 238000011160 research Methods 0.000 description 2
- NAWXUBYGYWOOIX-SFHVURJKSA-N (2s)-2-[[4-[2-(2,4-diaminoquinazolin-6-yl)ethyl]benzoyl]amino]-4-methylidenepentanedioic acid Chemical compound C1=CC2=NC(N)=NC(N)=C2C=C1CCC1=CC=C(C(=O)N[C@@H](CC(=C)C(O)=O)C(O)=O)C=C1 NAWXUBYGYWOOIX-SFHVURJKSA-N 0.000 description 1
- 230000002159 abnormal effect Effects 0.000 description 1
- 230000005856 abnormality Effects 0.000 description 1
- 238000003491 array Methods 0.000 description 1
- 238000004891 communication Methods 0.000 description 1
- 238000007796 conventional method Methods 0.000 description 1
- 230000008878 coupling Effects 0.000 description 1
- 238000010168 coupling process Methods 0.000 description 1
- 238000005859 coupling reaction Methods 0.000 description 1
- 238000000354 decomposition reaction Methods 0.000 description 1
- 238000001514 detection method Methods 0.000 description 1
- 235000019800 disodium phosphate Nutrition 0.000 description 1
- 230000008030 elimination Effects 0.000 description 1
- 238000003379 elimination reaction Methods 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 230000004927 fusion Effects 0.000 description 1
- 230000004807 localization Effects 0.000 description 1
- 238000010606 normalization Methods 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
- 230000004044 response Effects 0.000 description 1
- 238000010187 selection method Methods 0.000 description 1
- 238000004088 simulation Methods 0.000 description 1
- 230000003068 static effect Effects 0.000 description 1
- 238000012795 verification Methods 0.000 description 1
- 230000000007 visual effect Effects 0.000 description 1
Classifications
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
- Y02T10/00—Road transport of goods or passengers
- Y02T10/10—Internal combustion engine [ICE] based vehicles
- Y02T10/40—Engine management systems
Landscapes
- Navigation (AREA)
Abstract
本发明公开了一种基于固态雷达的SLAM实现方法及系统，所述方法包括：利用固态雷达对目标环境进行采样得到点云数据以及基于IMU获取陀螺仪数据和加速度；对固态雷达每一帧扫描获取的点云数据进行分割筛选预处理；再进行特征提取得到平面特征以及边缘特征，进而利用平面特征以及边缘特征构建局部特征图，所述局部特征图用于更新全局地图，将未重合特征更新加入至全局地图；基于全局地图、局部特征图，采用相似点匹配特征点进行残差计算，确定当前帧的位姿；识别当前帧是否是关键帧，若是关键帧，利用IMU数据对所述关键帧的位姿进行二次优化。本发明技术方案通过引入关键帧以及融合IMU数据进而二次优化，有效提高建图精度。
Description
技术领域
本发明属于SLAM技术领域，具体涉及一种基于固态雷达的SLAM实现方法及系统。
背景技术
SLAM(Simultaneous Localization and Mapping)是一种用于无人系统或机器人导航的技术，可以同时实时地建立环境地图和机器人的位姿估计。它结合了感知、控制和计算机视觉等技术，通过传感器数据的处理和算法的计算，实现机器人在未知环境中的自主导航和定位。
现有研究大多数基于机械雷达，对于固态雷达的研究相对较少，而固态雷达不需要机械部件来扫描，因此具有高可靠性和长寿命，此外，还有一些比较突出的特点，比如高稳定性和快速响应，这使它在恶劣环境下也能实现高精度和高分辨率的探测。
然而，常规的雷达通过平行的基线进行扫描，但这种扫描方式并不适用于固态雷达。固态雷达的特殊结构需要采用不规则扫描方式来获取点云。现有研究中针对固态雷达特殊的扫描方式如下：
将雷达里程计和建图(LOAM)应用于扫描方式不规则的小FOV固态激光雷达(livoxmid-40和mid-100)，并在前端针对不规则的扫描方式设计点选取的方法；在后端迭代优化位姿从而达到类似的跟踪精度。但是该方法在不断迭代过程中会出现漂移的情况。
不同于中的沿着入射角和偏转角进行遍历以选择候选点，LiLi-OM提出了一种新的特征提取的方法，在预处理的过程中处理其不规则扫描模式，并提出紧耦合的方案，通过滑动窗口进行边缘化并直接融合惯性传感器IMU和LiDAR的测量值。
然而，上述方法在后端优化方面存在局限性，在面对具有挑战性的场景时缺乏准确性。比如，应用于固态雷达的LOAM算法当处理的原始数据达到一定量时，累积的误差将导致状态更新方程出现异常。这会出现信息丢失和漂移问题。LiLi-OM相比LOAM算法，在后端利用局部因子图优化，可以获得稳健的建图。但是该方法为了达到不错的实时性，使得特征过于稀疏。因此，该方法也存在建图精度低的问题。
发明内容
本发明的目的是提升基于固态雷达的SLAM建图精度，为此，提供一种基于固态雷达的SLAM实现方法及系统，本发明技术方案提供的所述方法提出关键帧概念，即借助IMU优化关键帧的雷达位姿，实现更加精准定位。
为此，本发明提供如下技术方案：
一种基于固态雷达的SLAM实现方法，包括以下步骤：
S1：利用固态雷达对目标环境进行采样得到点云数据以及基于惯性测量单元IMU获取陀螺仪数据和加速度；
S2：对所述固态雷达每一帧扫描获取的点云数据进行分割筛选预处理；
S3：对分割筛选预处理后的每一帧点云数据进行特征提取得到平面特征以及边缘特征，进而利用平面特征以及边缘特征构建局部特征图，所述局部特征图用于更新全局地图，将未重合特征更新加入所述全局地图；
S4：基于全局地图、局部特征图，采用相似点匹配特征点进行残差计算，确定当前帧的位姿；
S5：识别当前帧是否是关键帧，若是关键帧，利用所述惯性测量单元IMU的数据对所述关键帧的位姿进行二次优化得到更精准位姿。
进一步可选地，步骤S5中采用阈值法判断当前帧是否是关键帧，其中，当前帧相较于前一关键帧的点云变化率大于或等于判定阈值，所述当前帧为关键帧；否则，所述当前帧为常规帧；
其中，所述判定阈值为经验值或基于下述公式1或公式2确定的自适应阈值：
公式1：
其中，Ij为公式1确定的自适应阈值，Kp为参考帧匹配上一关键帧中变化的点云量，Ec，Er分别表示当前帧和参考帧中所有的点云量，Tc，Tr分别表示当前帧和参考帧与上一关键帧重叠部分的点云量；
公式2：
其中，Isa为基于公式2确定的自适应阈值，自定义系数λ,满足：Δdc,p为当前帧距离上一关键帧的距离。
关键帧最初是应用于视觉里程计，以达到实时精准追踪。而往往在有限的机载计算力下，关键帧的选择尤为重要。本发明技术方案引入关键帧，为了准确选择关键帧，设定了上述自适应阈值设定方法。其中，当3d雷达进行移动时，当前帧和上一关键帧会受到距离的影响，变得不再相似；以及较远距离会使重叠部分减少，这可能会导致跟踪丢失。为了更有效的选取关键帧，使得阈值更符合真实情况，本发明技术方案在自适应阈值上进一步改进，定义系数λ来更新阈值以及系数来细化阈值，进一步保证了关键帧选择的可靠性，提高系统定位精度和稳定性。
进一步可选地，步骤S4中依据下述优化方程确定当前帧的位姿，具体为：
式中，m,n分别为当前帧与全局地图匹配的边缘特征数量以及平面特征数量，为边缘特征点到最大特征值的拟合线的距离对应度量，/>为平面特征点到拟合平面的距离对应度量，/>为边缘特征点对应的点到线的残差，/>为平面特征点对应点到面的残差，/>表示当前帧的位姿。/>为扫描点在当前状态下世界坐标系的位姿，即将雷达坐标系下的扫描点转换到世界坐标系的结果，扫描点为边缘特征点或平面特征点。
进一步可选地，残差以及/>表示为：
其中，为扫描点在当前状态下世界坐标系的位姿，即将雷达坐标系下的扫描点转换到世界坐标系的结果，本残差/>公式/>中，扫描点/>为边缘特征点集合/>中的一点；在边缘特征点集合/>搜索点/>投影线上最近的5个特征点且所述5个特征点在同一直线上，/>表示这条直线上的第一个点，/>表示这条直线上的最后一个点，R(q)为旋转矩阵，t表示当前时刻；
本残差公式/>中，扫描点/>为平面特征点集合/>中的一点；在平面特征点集/>中搜索点/>投影面上最近的5个特征点且处于同一平面，/>表示在所述平面选取的三个点。
进一步可选地，所述方法还包括：步骤S5中利用所述惯性测量单元IMU的数据对所述关键帧的位姿进行二次优化的过程如下：
其中，二次优化函数表示为：
式中，表示滑动窗口边缘化测量的先验边缘残差项，/>和/>分别代表雷达和IMU的误差项，/>表示关键帧的位姿；η，κ分别表示设定的滑动窗口的起点、结束点，/>表示在滑动窗口内某一时刻的状态，k表示关键帧，即设定的所述滑动窗口用于限制关键帧数量，对滑动窗口内的关键帧进行计算，进而优化关键帧位姿。
本发明技术方案将IMU的误差项作为因子融入到因子图中，以此来约束关键帧之间的相对运动；将获得雷达和IMU的误差项通过最小化成本函数来优化雷达关键帧的位姿，以获得最大后验估计。
进一步可选地，所述先验边缘残差项、以及雷达和IMU的误差项表示如下：
其中，Γp和Hp是通过对进行舒尔补操作后获得的最新的边缘化参数，T表示矩阵转置符号，m,n分别为当前帧与全局地图匹配的边缘特征数量以及平面特征数量，/>为边缘特征点到最大特征值的拟合线的距离对应度量，i对应边缘特征点，/>为平面特征点到拟合平面的距离对应度量，j对应平面特征点；Pk+1，Pk分别表示关键词k+1，关键帧k的位姿，Rk表示关键帧k到关键帧k+1的旋转矩阵，/>表示旋转矩阵Rk的矩阵转置，Vk表示关键帧k对应的速度，ΔVk,k+1，Δtk,k+1分别表示关键帧k与关键帧k+1的速度差、时间差，q为四元数，/>qk+1，Δqk,k+1分别表示关键帧k、关键帧k+1对应的四元数以及四元数之差；/>分别为关键帧k与关键帧k+1对应的加速度偏置，/>分别为关键帧k与关键帧k+1对应的重力加速度偏置，gw为重力加速度。
表示关键帧k到k+1之间的陀螺仪和加速计读数构成的数组；表示四元数q的向量部分；/>表示预积分过程中对应的噪声协方差，/>为IMU误差项运算中的约束运算，其为是现有的技术；/>为自定义矩阵。
进一步可选地，所述分割筛选预处理包括剔除动态点、添加标签以及地面分割：
其中，删除动态点的过程为：对点云数据进行分类得到种子点，再对种子点集采用区域生长算法确定动态点并删除，4＜|z|＜6,p∈pSD，z表示在点p在z轴的值，单位：(m)，pSD表示种子点，用于区域增长；
添加标签的过程为：对点云数据进行分类得到背景点、非背景点集，即|z|≥4,p∈pBG，0.4＜|z|＜4,p∈pNBG式中，pBG表示背景点，pNBG表示非背景点；再依据下述公式判断背景点的前者遮挡关系：
其中，再已标签的背景点数组中检索背景点对应的距离d和夹角φ，若相同φ时，距离d值更小的点为被遮挡点，距离d值更大的点为遮挡点；
d表示点pf距原点的距离，pf(x)表示点pf在全局坐标系下x轴上的值，pf(y)表示点pf在y轴的值，φ表示水平方向上面的夹角，θf(x),f(y)为点pf与原点的夹角。
此外，本发明还提供一种基于上述方法的系统，包括：
数据采集单元，用于利用固态雷达对目标环境进行采样得到点云数据以及基于惯性测量单元IMU获取陀螺仪数据和加速度；
预处理单元，用于对所述固态雷达每一帧扫描获取的点云数据进行分割筛选预处理，所述分割筛选预处理至少包含剔除动态物体的点云数据；
特征提取单元，用于对分割筛选预处理后的每一帧点云数据进行特征提取得到平面特征以及边缘特征，进而利用平面特征以及边缘特征构建局部特征图，所述局部特征图用于更新全局地图，将未重合特征更新加入所述全局地图；
位姿优化单元，用于基于全局地图、局部特征图，采用相似点匹配特征点进行残差计算，确定当前帧的位姿；
关键帧位姿优化单元，用于识别当前帧是否是关键帧，若是关键帧，利用所述惯性测量单元IMU的数据对所述关键帧的位姿进行二次优化。
本发明还提供一种电子终端，至少包含：
一个或多个处理器；
以及存储了一个或多个计算机程序的存储介质；
其中，所述处理器调用所述计算机程序以实现：
一种基于固态雷达的SLAM实现方法的步骤。
本发明还提供一种计算机可读存储介质，存储了计算机程序，所述计算机程序被处理器调用以执行：
一种基于固态雷达的SLAM实现方法的步骤。
有益效果
与现有方法相比，本发明的优点有：
1.本发明技术方案采用迭代优化的方法，保证位姿间有效关联；并提出关键帧应用，借助IMU数据，融合IMU测量来优化关键帧的雷达位姿，实现更加精准定位，这能有效的解决漂移和信息丢失问题。
2.在实际应用中，本发明技术方案促使该系统在大幅度摆动和快速等条件下，都能保持很好的精度和建图效果，这使其在一些挑战的场景下也能稳定的工作。在位姿不断更新的过程中，前一个状态决定了下一个状态的准确性，尤其是借助高频的IMU，可以获得更为精准的位姿。
附图说明
图1是本发明提供的所述SLAM实现方法对应的系统架构示意图；
图2是本发明提供的基于迭代优化的紧耦合激光雷达惯性示意图。
具体实施方式
本发明提供的一种基于固态雷达的SLAM实现方法有效提高建图精度。其核心之一是引入关键帧并融合IMU数据对关键帧对应的雷达位姿进行二次优化；核心之二是引入边缘特征、平面特征对应的残差作为特征因子，在后端用迭代优化的方法，保证位姿间有效关联；核心之三是对原始点云数据进行预处理，即包含剔除动态物体以及针对遮挡问题，提供技术手段来区分遮挡点和被遮挡点，为有效解决遮挡问题奠定基础。本发明所述方法的目的在于获得雷达六个自由度运动的精准估计以及获得全局一致的地图。下面将结合实施例对本发明做进一步的说明。
实施例1：
本实施例提供的一种基于固态雷达的SLAM实现方法，包括以下步骤：
S1：利用固态雷达对目标环境进行采样得到点云数据以及基于惯性测量单元IMU获取陀螺仪数据和加速度。
本实施例选用固态雷达livoxAvia，其具备更大的垂直分辨率以及更加均匀的FoV覆盖范围。与传统的64线机械雷达相比，它的价格比同等性能的机械雷达要便宜的多。如图1所示，3D激光雷达对环境进行采样并以10Hz的频率流出点云，六轴IMU以更高的频率提供陀螺仪数据和加速度。
S2：对所述固态雷达每一帧扫描获取的点云数据进行分割筛选预处理。本实施例中分割筛选预处理的目的在于剔除动态物体的点云数据以及对点云进行标签分类，保留更加有效的点云数据，为后续建图奠定基础。其他可行的实施例中，并不约束于上述分割筛选预处理技术，即在满足地面分割的基础需求上，并不约束必须执行上述标签分类等预处理手段。
在激光雷达扫描过程中，动态物体可能在多个时刻出现在地图中，从而导致不稳定的建图。本实施例对一帧扫描的点云进行下采样并校正，然后再对校正的点云进行地面分割。在分割之前，需要对地面点进行评估选择再进行分类标记，其中，标签的作用也是为了更好的剔除其他的坏点(异常点)。
首先，针对动态物体，需要将动态物体对应的点云剔除。
设定3D点坐标为p＝[x,y,z]，以下面的条件对地面上的点进行分类：
式中，z表示在点p在z轴的值，pBG表示背景点，pNBG表示非背景点，pSD表示种子点，用于区域增长。从上可知，依据z轴坐标值划定各个点集，其中，种子点用于区域增长，筛选出动态点并删除。在“Egocentric ratio of pseudo occupancy-based dynamic objectremoval for static 3D point cloud map building”中，将每一帧点云与子图划分为栅格，计算描述子并筛选潜在的动态区域。本发明采用该方法，利用区域生长的算法，选取第一个种子点，在阈值范围内筛选与该种子性质相似的点合并到种子点所在的区域，然后像四周扩散；然后将种子点在垂直方向上的距离与阈值作比较，将超过阈值的点视为潜在动态点。由于本步骤为现有技术，因此对其不进行具体陈述。
在一些复杂的环境中会出现高大或者矮小的物体，地面上的点存在互相遮挡的问题，这可能造成了一部分特征点的丢失。为了解决这个问题，本发明按照上述公式给地面上的点定义为背景点、非背景点以及种子点。其中，考虑到在某些特定情况下，在一定高度上的背景存在互相遮挡问题，即背景点被前景遮挡还是前景点被背景遮挡，哪个应该先处理，存在前后关系，因此，本发明技术方案利用下述公式(2)来鉴别遮挡点和被遮挡点，一方面，遮挡点相较于被遮挡点更优先处理，即用于特征提取，应当理解，这些点都会存于数组中，在进行特征匹配的时候，一个有序的数组和一个无序的数组，必然是前者要的处理速度快些，进而可以提升检索点云的速度；二方面，通过标签表示遮挡关系，可以防止被遮挡点因为遮挡问题造成特征丢失，被标记后，可以纳入下一时刻特征点预选。
即再已标签的背景点数组中检索，若相同φ时，距离d值更小的点为遮挡点，距离d值更大的点为被遮挡点，其中，已标签的背景点数组为当前一帧的数据。d表示点pf距原点的距离，pf(x)表示该点在x坐标系上的值，pf(y)表示该点在y坐标系上的值，φ表示水平方向上面的夹角。
需要说明的是，前者预处理是本实施例的优选方案，其他可行的实施例中，并不局限于上述预处理方式，在不脱离本发明下述步骤的核心思路的基础上，采用其他预处理技术手段也是落入本发明的保护范围内。
S3：对分割筛选预处理后的每一帧点云数据进行特征提取得到平面特征以及边缘特征，进而利用平面特征以及边缘特征构建局部特征图，所述局部特征图用于更新全局地图，将未重合特征更新加入所述全局地图。
本实施例中采用“LOAM:Lidar Odometry and Mapping in Real-Time”的方式对预处理后的点云数据进行特征提取，即计算候选点的曲率值并排序，再依据设定的阈值来确定平面特征点和边缘特征点。由于特征提取的现有技术手段诸多，本发明并不约束提取边缘特征点以及平面特征点的具体实现手段。此外，局部特征图更新全局地图也是本领域的常规思路以及手段，对此不进行具体的陈述。
S4：基于全局地图、局部特征图，采用相似点匹配特征点进行残差计算，确定当前帧的位姿。由于固态雷达特性的扫描方式，所提取的特征无法在相邻两帧内始终匹配，因此提出一种迭代优化的方法来估计位姿。不同于常规的方法通过匹配来估计位姿，本文利用来自特征的残差进行约束优化。
定义一次扫描为W，在雷达帧ζ上提取对应的边缘特征点。若在雷达帧上有一个边缘特征点，先在局部特征图Mw中搜素符合对应关系的5个临近点(雷达帧点投影到局部地图上，搜索邻近的五个点)，然后计算其坐标均值和协方差矩阵，并执行特征分解；计算出最大的特征值并拟合成一条直线(该过程为现有技术，不进行详细陈述)。选取直线上两个点其点到线距离对应的度量关系如下
式中，表示在全局帧下的雷达位姿。R(q)表示关于q的旋转矩阵。表示拟合直线上的两点。/>为边缘特征点。t代表当前时刻。
对于平面特征点ps，在局部特征图中找寻附近5个平面特征点，其具体实现可以参考“Towards high-performance solid-state-lidar-inertial odometry andmapping,”，通过平面拟合求解超定性方程，然后将拟合的法向量进行归一化的处理，点到平面的度量可以表示为：
其中，表示拟合法向量的归一化处理，/>表示QR求解超定线方程中的变量。pw为平面特征点ps转换到全局地图的点，表示在全局帧下的雷达位姿。
设定位姿定义为为：
其中，表示当前帧的位置和方向，/>表示速度，/>表示四元数(现有的，与IMU数据相关)，b＝[ba,bg]表示来自IMU的陀螺仪和加速计的偏置。
如图2所示，对于迭代优化，利用来自边缘特征点和平面特征点的残差对各帧位姿进行优化。定义全局地图上一点 表示当前帧的边缘特征点的集合。在集合中搜索/>投影线上最近的5个特征点，然后计算这些特征点的均值和协方差矩阵。对应的点到线的残差计算为
其中，选取离点最近的5个点并保证在一条直线上。/>表示这条直线上的第一个点，/>表示这条直线上的最后一个点。/>表示当前点在全局坐标下的位姿，R(q)为旋转矩阵；/>为扫描点在当前状态下世界坐标系的位姿，即将雷达坐标系下的扫描点转换到世界坐标系的结果，本公式中，扫描点/>为边缘特征点集合/>中的一点。
与边缘点类似，表示当前帧平面点的集合。在集合中搜索/>投影面上最近的5个特征点，然后计算这些特征点的均值和协方差矩阵，对应的点到面的残差计算为：
其中，选取离点最近的5个点并保证在一个平面上。/>表示这个平面选取的三个点。/>表示扫描点在当前状态下世界坐标系的位姿，即将雷达坐标系下的扫描点转换到世界坐标系的结果，本公式中，扫描点/>为平面特征点集合/>中的一点。
为了实现轻量化，在扫描匹配的过程中使用帧到模型的方式，对当前的的位姿使用信域的方法来估计当前的状态并进行优化：
式中，m,n分别为当前帧与全局地图匹配的边缘特征数量以及平面特征数量，为边缘特征点到最大特征值的拟合线的距离对应度量，/>为平面特征点到拟合平面的距离对应度量，/>为边缘特征点对应的点到线的残差，/>为平面特征点对应点到面的残差，/>表示当前帧的位姿，其中，IMU的偏置b的初始可以预先给定。
需要说明的是，根据上述公式进行求解可以确定当前帧的雷达位姿。
S5：识别当前帧是否是关键帧，若是关键帧，利用所述惯性测量单元IMU的数据对所述关键帧的位姿进行二次优化。其中，步骤S5中采用阈值法判断当前帧是否是关键帧，其中，当前帧相较于前一关键帧的点云变化率大于或等于判定阈值，所述当前帧为关键帧；否则，所述当前帧为常规帧。其中，所述判定阈值为经验值或基于下述公式(9)或公式(10)确定的自适应阈值。而往往在有限的机载计算力下，关键帧的选择尤为重要。因此，本发明技术方案提出一种自适应关键帧的方法来选择合适的关键帧。本实施例优选公式(10)确定的自适应阈值，其他可行的实施例中，并不局限于此。
由于初始化没有上一关键帧的状态，因此假设当前帧与参考帧相似，然后利用跟踪变化的点云计算初始阈值。初始阈值定义如公式(9)：
其中，Kp为参考帧匹配上一关键帧中变化的点云量。Ec，Er分别表示当前帧和参考帧中所有的点云量。Tc，Tr分别表示当前帧和参考帧与上一关键帧重叠部分的点云量。
当3d雷达进行移动时，当前帧和上一关键帧会受到距离的影响，变得不再相似。另外，较远距离会使重叠部分减少，这可能会导致跟踪丢失。为了更有效的选取关键帧，使得阈值更符合真实情况，定义系数λ来更新阈值以及系数来细化阈值。自适应阈值定义如公式(10)所示：
其中，Δdc,p为当前帧距上一关键帧的距离。
需要说明的是，确定阈值后，通过当前帧与前一关键帧之间点云变化率是否大于或等于阈值来判别当前帧是否为关键帧，若是大于或等于阈值，当前帧视为关键帧；否则，视为常规帧。
选择有效关键帧可以提高系统的精度和稳定性。但在后端融合的过程中保持稀疏性很重要，因此本发明优选提出滑动窗口用以限制关键帧的数量。另一方面，固定大小的窗口意味着要丢弃一部分旧的关键帧。本文使用Schur消元过程对关键帧进行填入操作，该关键帧被边缘化后将不在使用。相应地计算新的先验，并将其添加到现有先验因子之上，以方便在下一个窗口使用该估计值。
本发明技术方案按照下述公式优化关键帧的位姿，其中，二次优化函数表示为：
式中，表示滑动窗口边缘化测量的先验边缘残差项，/>和/>分别代表雷达和IMU的误差项。/>表示关键帧的位姿。η，κ分别表示设定的滑动窗口的起点、结束点，/>表示在滑动窗口内某一时刻k的状态，设定的所述滑动窗口用于限制关键帧数量。
其中，先验边缘残差表达式如下：
式中，Γp和Hp是通过舒尔补操作后获得的最新的边缘化参数，T表示矩阵，由于参数Γp和Hp的计算时现有技术，因此不进行详细陈述。
雷达误差项来自特征信息的几何约束，当对准全局地图观测到的边缘和平面特征，其误差项表示为：
其中，和/>代表当前关键帧的边缘特征点到线、平面特征点到面的度量公式。
另一方面，本发明将IMU的误差项作为因子融入到因子图中，以此来约束关键帧之间的相对运动。IMU误差项被定义为：
其中，Γp和Hp是通过舒尔补操作后获得的最新的边缘化参数，T表示矩阵转置符号，m,n分别为当前帧与全局地图匹配的边缘特征数量以及平面特征数量，为边缘特征点到最大特征值的拟合线的距离对应度量，/>为平面特征点到拟合平面的距离对应度量；Pk+1，Pk分别表示关键词k+1，关键帧k的位姿，Rk表示关键帧k到关键帧k+1的旋转矩阵，Vk表示关键帧k对应的速度，ΔVk,k+1，Δtk,k+1分别表示关键帧k与关键帧k+1的速度差、时间差，q为四元数，/>qk+1，Δqk,k+1分别表示关键帧k、关键帧k+1对应的四元数以及四元数之差； 分别为关键帧k与关键帧k+1对应的加速度偏置，/>分别为关键帧k与关键帧k+1对应的重力加速度偏置，gw为重力加速度；
表示关键帧k到k+1之间的陀螺仪和加速计读数构成的数组；表示四元数q的向量部分；/>表示预积分过程中对应的噪声协方差，/>表示IMU误差项运算中的约束运算。
需要说明的式，非线性最小二乘问题可以用Ceres Slove来解决。另外，联合优化的状态值将作为IMU下一个状态的初值，这样可以避免IMU状态产生漂移。将IMU的状态通过线性插值的方法插入到雷达里程计中，这样可以解决运动模糊问题和减少雷达里程计的误差。
实施例2：
本发明实施例还提供一种基于上述方法的系统，包括：数据采集单元、预处理单元、特征提取单元、位姿优化单元以及关键帧位姿优化单元。
其中，数据采集单元用于利用固态雷达对目标环境进行采样得到点云数据以及基于惯性测量单元IMU获取陀螺仪数据和加速度；预处理单元用于对所述固态雷达每一帧扫描获取的点云数据进行分割筛选预处理，所述分割筛选预处理至少包含剔除动态物体的点云数据；特征提取单元用于对分割筛选预处理后的每一帧点云数据进行特征提取得到平面特征以及边缘特征，进而利用平面特征以及边缘特征构建局部特征图，所述局部特征图用于更新全局地图，将未重合特征更新加入所述全局地图；位姿优化单元用于基于全局地图、局部特征图，采用相似点匹配特征点进行残差计算，确定当前帧的位姿；关键帧位姿优化单元用于识别当前帧是否是关键帧，若是关键帧，利用所述惯性测量单元IMU的数据对所述关键帧的位姿进行二次优化。
应当理解，各个模块的实现过程可以参照前述方法的内容陈述，上述功能模块的划分仅仅是一种逻辑功能的划分，实际实现时可以有另外的划分方式，例如多个单元或组件可以结合或者可以集成到另一个系统，或一些特征可以忽略，或不执行。
实施例3：
本发明实施例还提供一种电子终端，至少包含：一个或多个处理器；以及以及存储了一个或多个计算机程序的存储介质；其中，所述处理器调用所述计算机程序以实现：一种基于固态雷达的SLAM实现方法的步骤。
其中，具体执行步骤S1-S5。
其中，各步骤的实现过程可以参照实施例1的详细步骤。存储器可能包含高速RAM存储器，也可能还包括非易失性除颤器，例如至少一个磁盘存储器。
如果存储器、处理器独立实现，则存储器、处理器和通信接口可以通过总线相互连接并完成相互间的通信。所述总线可以是工业标准体系结构总线，外部设备互联总线或扩展工业标准体系结构总线等。所述总线可以分为地址总线、数据总线、控制总线等。
可选的，在具体实现上，如果存储器、处理器集成在一块芯片上，则存储器、处理器可以通过内部接口完成相互之间的通信。
应当理解，在本发明实施例中，所称处理器可以是中央处理单元(CentralProcessing Unit，CPU)，该处理器还可以是其他通用处理器、数字信号处理器(DigitalSignal Processor，DSP)、专用集成电路(Application Specific Integrated Circuit，ASIC)、现成可编程门阵列(Field-Programmable Gate Array，FPGA)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件等。通用处理器可以是微处理器或者该处理器也可以是任何常规的处理器等。存储器可以包括只读存储器和随机存取存储器，并向处理器提供指令和数据。存储器的一部分还可以包括非易失性随机存取存储器。例如，存储器还可以存储设备类型的信息。
实施例4：
本发明实施例还提供一种计算机可读存储介质，存储了计算机程序，所述计算机程序被处理器调用以执行：一种基于固态雷达的SLAM实现方法的步骤。
其中，具体执行步骤S1-S5。
各个步骤的具体实现过程请参照前述方法的阐述。
所述可读存储介质为计算机可读存储介质，其可以是前述任一实施例所述的控制器的内部存储单元，例如控制器的硬盘或内存。所述可读存储介质也可以是所述控制器的外部存储设备，例如所述控制器上配备的插接式硬盘，智能存储卡(Smart Media Card，SMC)，安全数字(Secure Digital，SD)卡，闪存卡(Flash Card)等。进一步地，所述可读存储介质还可以既包括所述控制器的内部存储单元也包括外部存储设备。所述可读存储介质用于存储所述计算机程序以及所述控制器所需的其他程序和数据。所述可读存储介质还可以用于暂时地存储已经输出或者将要输出的数据。
基于这样的理解，本发明的技术方案本质上或者说对现有技术做出贡献的部分，或者该技术方案的全部或部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台计算机设备(可以是个人计算机，服务器，或者网络设备等)执行本发明各个实施例所述方法的全部或部分步骤。而前述的可读存储介质包括：U盘、移动硬盘、只读存储器(ROM，Read-Only Memory)、随机存取存储器(RAM，Random Access Memory)、磁碟或者光盘等各种可以存储程序代码的介质。
仿真验证:
综上，本发明技术方案提供的所述方法针对在挑战性的环境中，地面车辆能进行高精度的建图。其通过在前端预处理过程中考虑有效点的提取效率，对点云进行校正，过滤掉动态物体，并对点云进行标签分类，进一步减少无用点的数量。为了减少位姿的误差，通过全局地图检索构建特征k-d树，并产生特征因子来优化雷达的位姿。然后利用基于关键帧的滑动窗口融合IMU测量，使位姿得到二次优化。通过在两个平台上收集数据集对提出的方法进行了全面的评估。与LiHo和LiLi-OM相比，本发明技术方案Li-HFP可以实现更高精度的位姿估计和建图，未来的工作将在保证高精度的同时提高实时性。
需要强调的是，本发明所述的实例是说明性的，而不是限定性的，因此本发明不限于具体实施方式中所述的实例，凡是由本领域技术人员根据本发明的技术方案得出的其他实施方式，不脱离本发明宗旨和范围的，不论是修改还是替换，同样属于本发明的保护范围。
Claims (10)
1.一种基于固态雷达的SLAM实现方法，其特征在于：包括以下步骤：
S1：利用固态雷达对目标环境进行采样得到点云数据以及基于惯性测量单元IMU获取陀螺仪数据和加速度；
S2：对所述固态雷达每一帧扫描获取的点云数据进行分割筛选预处理；
S3：对分割筛选预处理后的每一帧点云数据进行特征提取得到平面特征以及边缘特征，进而利用平面特征以及边缘特征构建局部特征图，所述局部特征图用于更新全局地图，将未重合特征更新加入所述全局地图；
S4：基于全局地图、局部特征图，采用相似点匹配特征点进行残差计算，确定当前帧的位姿；
S5：识别当前帧是否是关键帧，若是关键帧，利用所述惯性测量单元IMU的数据对所述关键帧的位姿进行二次优化得到更精确位姿。
2.根据权利要求1所述的方法，其特征在于：步骤S5中采用阈值法判断当前帧是否是关键帧，其中，当前帧相较于前一关键帧的点云变化率大于或等于判定阈值，所述当前帧为关键帧；否则，所述当前帧为常规帧；
其中，所述判定阈值为经验值或基于下述公式1或公式2确定的自适应阈值：
公式1：
其中，Ij为公式1确定的自适应阈值，Kp为参考帧匹配上一关键帧中变化的点云量，Ec，Er分别表示当前帧和参考帧中所有的点云量，Tc，Tr分别表示当前帧和参考帧与上一关键帧重叠部分的点云量；
公式2：
其中，Isa为基于公式2确定的自适应阈值，自定义系数λ,满足：Δdc,p为当前帧距离上一关键帧的距离。
3.根据权利要求1所述的方法，其特征在于：步骤S4中依据下述优化方程确定当前帧的位姿，具体为：
式中，m,n分别为当前帧与全局地图匹配的边缘特征数量以及平面特征数量，为边缘特征点到最大特征值的拟合线的距离对应度量，/>为平面特征点到拟合平面的距离对应度量，/>为边缘特征点对应的点到线的残差，/>为平面特征点对应的点到面的残差，表示当前帧的位姿，/>为扫描点在当前状态下世界坐标系的位姿，即将雷达坐标系下的扫描点转换到世界坐标系的结果，扫描点为边缘特征点或平面特征点。
4.根据权利要求3所述的方法，其特征在于：残差以及/>表示为：
其中，为扫描点在当前状态下世界坐标系的位姿，即将雷达坐标系下的扫描点转换到世界坐标系的结果，本残差/>公式/>中，扫描点/>为边缘特征点集合中的一点；在边缘特征点集合/>搜索点/>投影线上最近的5个特征点且所述5个特征点在同一直线上，/>表示这条直线上的第一个点，/>表示这条直线上的最后一个点，R(q)为旋转矩阵，t表示当前时刻；
本残差公式/>中，扫描点/>为平面特征点集合/>中的一点；在平面特征点集/>中搜索点/>投影面上最近的5个特征点且处于同一平面，/>表示在所述平面选取的三个点。
5.根据权利要求1所述的方法，其特征在于：还包括：步骤S5中利用所述惯性测量单元IMU的数据对所述关键帧的位姿进行二次优化的过程如下：
其中，二次优化函数表示为：
式中，表示滑动窗口边缘化测量的先验边缘残差项，/>和/>分别代表雷达和IMU的误差项，/>表示关键帧的位姿；η，κ分别表示设定的滑动窗口的起点、结束点，表示在滑动窗口内某一时刻的状态，k表示关键帧，设定的所述滑动窗口用于限制关键帧数量。
6.根据权利要求5所述的方法，其特征在于：所述先验边缘残差项、以及雷达和IMU的误差项表示如下：
其中，Γp和Hp是通过舒尔补操作后获得的最新的边缘化参数，T表示矩阵转置符号，m,n分别为当前帧与全局地图匹配的边缘特征数量以及平面特征数量，为边缘特征点到最大特征值的拟合线的距离对应度量，/>为平面特征点到拟合平面的距离对应度量；Pk+1，Pk分别表示关键词k+1，关键帧k的位姿，Rk表示关键帧k到关键帧k+1的旋转矩阵，/>表示旋转矩阵Rk的矩阵转置，Vk表示关键帧k对应的速度，ΔVk,k+1，Δtk,k+1分别表示关键帧k与关键帧k+1的速度差、时间差，q为四元数，/>qk+1，Δqk,k+1分别表示关键帧k、关键帧k+1对应的四元数以及四元数之差；/>分别为关键帧k与关键帧k+1对应的加速度偏置，/>分别为关键帧k与关键帧k+1对应的重力加速度偏置，gw为重力加速度；/>表示关键帧k到k+1之间的陀螺仪和加速计读数构成的数组；/>表示四元数q的向量部分；表示预积分过程中对应的噪声协方差，/>为IMU的残差的约束运算；/>为自定义矩阵。
7.根据权利要求1所述的方法，其特征在于：所述分割筛选预处理包括剔除动态点、添加标签以及地面分割：
其中，删除动态点的过程为：对点云数据进行分类得到种子点，再对种子点集采用区域生长算法确定动态点并删除，4＜|z|＜6,p∈pSD，z表示在点p在z轴的值，单位：m，pSD表示种子点，用于区域增长；
添加标签的过程为：对点云数据进行分类得到背景点、非背景点集，即|z|≥4,p∈pBG，0.4＜|z|＜4,p∈pNBG式中，pBG表示背景点，pNBG表示非背景点；再依据下述公式判断背景点的前者遮挡关系：
其中，再已标签的背景点数组中检索背景点对应的距离d和夹角φ，若相同φ时，距离d值更小的点为被遮挡点，距离d值更大的点为遮挡点；
d表示点pf距原点的距离，pf(x)表示点pf在全局坐标系下x轴上的值，pf(y)表示点pf在y轴的值，φ表示水平方向上面的夹角，θf(x),f(y)为点pf与原点的夹角。
8.一种基于权利要求1-7任一项所述方法的系统，其特征在于：包括：
数据采集单元，用于利用固态雷达对目标环境进行采样得到点云数据以及基于惯性测量单元IMU获取陀螺仪数据和加速度；
预处理单元，用于对所述固态雷达每一帧扫描获取的点云数据进行分割筛选预处理，所述分割筛选预处理至少包含剔除动态物体的点云数据；
特征提取单元，用于对分割筛选预处理后的每一帧点云数据进行特征提取得到平面特征以及边缘特征，进而利用平面特征以及边缘特征构建局部特征图，所述局部特征图用于更新全局地图，将未重合特征更新加入所述全局地图；
位姿优化单元，用于基于全局地图、局部特征图，采用相似点匹配特征点进行残差计算，确定当前帧的位姿；
关键帧位姿优化单元，用于识别当前帧是否是关键帧，若是关键帧，利用所述惯性测量单元IMU的数据对所述关键帧的位姿进行二次优化。
9.一种电子终端，其特征在于：至少包含：
一个或多个处理器；
以及存储了一个或多个计算机程序的存储介质；
其中，所述处理器调用所述计算机程序以实现：
权利要求1-7任一项所述基于固态雷达的SLAM实现方法的步骤。
10.一种计算机可读存储介质，其特征在于：存储了计算机程序，所述计算机程序被处理器调用以执行：
权利要求1-7任一项所述基于固态雷达的SLAM实现方法的步骤。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202311210728.2A CN117218350A (zh) | 2023-09-19 | 2023-09-19 | 一种基于固态雷达的slam实现方法及系统 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202311210728.2A CN117218350A (zh) | 2023-09-19 | 2023-09-19 | 一种基于固态雷达的slam实现方法及系统 |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN117218350A true CN117218350A (zh) | 2023-12-12 |
Family
ID=89043988
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202311210728.2A Pending CN117218350A (zh) | 2023-09-19 | 2023-09-19 | 一种基于固态雷达的slam实现方法及系统 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN117218350A (zh) |
Cited By (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN117451033A (zh) * | 2023-12-21 | 2024-01-26 | 广东石油化工学院 | 一种同步定位与地图构建方法、装置、终端及介质 |
| CN117475092A (zh) * | 2023-12-27 | 2024-01-30 | 安徽蔚来智驾科技有限公司 | 位姿优化方法、设备、智能设备和介质 |
-
2023
- 2023-09-19 CN CN202311210728.2A patent/CN117218350A/zh active Pending
Cited By (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN117451033A (zh) * | 2023-12-21 | 2024-01-26 | 广东石油化工学院 | 一种同步定位与地图构建方法、装置、终端及介质 |
| CN117451033B (zh) * | 2023-12-21 | 2024-05-14 | 广东石油化工学院 | 一种同步定位与地图构建方法、装置、终端及介质 |
| CN117475092A (zh) * | 2023-12-27 | 2024-01-30 | 安徽蔚来智驾科技有限公司 | 位姿优化方法、设备、智能设备和介质 |
| CN117475092B (zh) * | 2023-12-27 | 2024-03-19 | 安徽蔚来智驾科技有限公司 | 位姿优化方法、设备、智能设备和介质 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN111929699B (zh) | 一种顾及动态障碍物的激光雷达惯导里程计与建图方法及系统 | |
| CN112734852B (zh) | 一种机器人建图方法、装置及计算设备 | |
| CN110988912B (zh) | 自动驾驶车辆的道路目标与距离检测方法、系统、装置 | |
| US10996062B2 (en) | Information processing device, data management device, data management system, method, and program | |
| CN109345588B (zh) | 一种基于Tag的六自由度姿态估计方法 | |
| CN117218350A (zh) | 一种基于固态雷达的slam实现方法及系统 | |
| CN112233177A (zh) | 一种无人机位姿估计方法及系统 | |
| CN114964212B (zh) | 面向未知空间探索的多机协同融合定位与建图方法 | |
| KR102490521B1 (ko) | 라이다 좌표계와 카메라 좌표계의 벡터 정합을 통한 자동 캘리브레이션 방법 | |
| CN115861352A (zh) | 单目视觉、imu和激光雷达的数据融合和边缘提取方法 | |
| CN117419719A (zh) | 一种融合imu的三维激光雷达定位与建图方法 | |
| CN117664124A (zh) | 基于ros的惯性导引与视觉信息融合的agv导航系统及方法 | |
| CN113495281B (zh) | 可移动平台的实时定位方法及装置 | |
| CN115952248A (zh) | 终端设备的位姿处理方法、装置、设备、介质及产品 | |
| KR102624644B1 (ko) | 벡터 맵을 이용한 이동체의 맵 매칭 위치 추정 방법 | |
| Ji et al. | Adaptive Denoising-Enhanced LiDAR Odometry for Degeneration Resilience in Diverse Terrains | |
| CN118149796B (zh) | 退化环境下的地图构建方法、装置、电子设备及存储介质 | |
| Subash | Automatic road extraction from satellite images using extended Kalman filtering and efficient particle filtering | |
| CN117611762B (zh) | 一种多层级地图构建方法、系统及电子设备 | |
| CN117433511B (zh) | 一种多传感器融合定位方法 | |
| Ravindranath et al. | 3D-3D Self-Calibration of Sensors Using Point Cloud Data | |
| Kotyuzanskiy et al. | Object recognition technology for autonomous unmanned control based on stereo data analysis and voxel representation of visible objects | |
| Luo et al. | A 3D LiDAR localization and mapping method incorporating inertial measurement unit | |
| Chu et al. | Removing past data of dynamic objects using static Velodyne LiDAR sensor | |
| CN117128970A (zh) | 一种多传感器融合的自动驾驶位姿估计方法 |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination |