CN117808882A - 退化场景下基于多传感器融合的slam漂移检测与补偿方法 - Google Patents
退化场景下基于多传感器融合的slam漂移检测与补偿方法 Download PDFInfo
- Publication number
- CN117808882A CN117808882A CN202410225198.7A CN202410225198A CN117808882A CN 117808882 A CN117808882 A CN 117808882A CN 202410225198 A CN202410225198 A CN 202410225198A CN 117808882 A CN117808882 A CN 117808882A
- Authority
- CN
- China
- Prior art keywords
- pose
- current
- frame
- drift
- point cloud
- 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.)
- Granted
Links
- 238000001514 detection method Methods 0.000 title claims abstract description 58
- 238000000034 method Methods 0.000 title claims abstract description 49
- 230000004927 fusion Effects 0.000 title claims abstract description 27
- 230000015556 catabolic process Effects 0.000 title claims abstract description 20
- 238000006731 degradation reaction Methods 0.000 title claims abstract description 20
- 238000005457 optimization Methods 0.000 claims abstract description 36
- 238000012545 processing Methods 0.000 claims abstract description 11
- 239000011159 matrix material Substances 0.000 claims description 25
- 230000006870 function Effects 0.000 claims description 23
- 238000004422 calculation algorithm Methods 0.000 claims description 12
- 230000009466 transformation Effects 0.000 claims description 5
- 238000004364 calculation method Methods 0.000 claims description 4
- 230000003068 static effect Effects 0.000 claims description 4
- 238000004590 computer program Methods 0.000 claims description 3
- 238000010276 construction Methods 0.000 abstract description 7
- 238000010586 diagram Methods 0.000 description 13
- 238000013507 mapping Methods 0.000 description 9
- 230000000694 effects Effects 0.000 description 3
- 238000005516 engineering process Methods 0.000 description 3
- 238000011160 research Methods 0.000 description 3
- 230000006978 adaptation Effects 0.000 description 2
- 238000011161 development Methods 0.000 description 2
- 238000011156 evaluation Methods 0.000 description 2
- 239000000463 material Substances 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 230000035772 mutation Effects 0.000 description 2
- 238000012360 testing method Methods 0.000 description 2
- 238000012937 correction Methods 0.000 description 1
- 238000000354 decomposition reaction Methods 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 230000007613 environmental effect Effects 0.000 description 1
- 230000004807 localization Effects 0.000 description 1
- 238000012544 monitoring process Methods 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
- 238000012795 verification Methods 0.000 description 1
Classifications
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T7/00—Image analysis
- G06T7/70—Determining position or orientation of objects or cameras
- G06T7/73—Determining position or orientation of objects or cameras using feature-based methods
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06F—ELECTRIC DIGITAL DATA PROCESSING
- G06F18/00—Pattern recognition
- G06F18/20—Analysing
- G06F18/25—Fusion techniques
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T7/00—Image analysis
- G06T7/30—Determination of transform parameters for the alignment of images, i.e. image registration
- G06T7/33—Determination of transform parameters for the alignment of images, i.e. image registration using feature-based methods
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T2207/00—Indexing scheme for image analysis or image enhancement
- G06T2207/10—Image acquisition modality
- G06T2207/10028—Range image; Depth image; 3D point clouds
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T2207/00—Indexing scheme for image analysis or image enhancement
- G06T2207/10—Image acquisition modality
- G06T2207/10032—Satellite or aerial image; Remote sensing
- G06T2207/10044—Radar image
-
- 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
- Engineering & Computer Science (AREA)
- Theoretical Computer Science (AREA)
- Physics & Mathematics (AREA)
- Computer Vision & Pattern Recognition (AREA)
- General Physics & Mathematics (AREA)
- Data Mining & Analysis (AREA)
- Artificial Intelligence (AREA)
- Bioinformatics & Cheminformatics (AREA)
- Bioinformatics & Computational Biology (AREA)
- Evolutionary Biology (AREA)
- Evolutionary Computation (AREA)
- Life Sciences & Earth Sciences (AREA)
- General Engineering & Computer Science (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
本发明涉及一种退化场景下基于多传感器融合的SLAM漂移检测与补偿方法，包括：根据当前帧的预测位姿进行漂移检测；对当前帧进行正常状态检测，进一步确认当前帧是否处于正常状态；根据上述不同的检测结果，对当前帧分别进行相应的状态判断以及因子图优化处理，以获取当前帧的位姿优化结果；根据位姿优化结果进行当前雷达点云帧的位姿预测以及NDT点云配准处理。本发明还涉及一种相应的装置、处理器及其存储介质。采用了本发明的该退化场景下基于多传感器融合的SLAM漂移检测与补偿方法、装置、处理器及其存储介质，利用轮速里程计与雷达里程计之间的差值作为评判标准，有效减少了建图状态突变而导致图优化失效的问题，更适用于实际场景。
Description
技术领域
本发明涉及定位与建图技术领域，尤其涉及漂移检测与补偿领域，具体是指一种退化场景下基于多传感器融合的SLAM漂移检测与补偿方法。
背景技术
近年来，随着传感设备的不断更新迭代，定位与建图技术（SimultaneousLocalization and Mapping，SLAM）已经变得越来越成熟，成为了自动驾驶、室内机器人导航、安防监控、虚拟现实、游戏开发等应用领域中不可或缺的技术手段。然而，由于传感器误差、环境退化、传感器故障以及长时间运行等因素，SLAM***往往会出现或多或少的漂移现象，即随着时间的推移，定位和地图的误差会逐渐累积，更甚者会直接导致SLAM建图与定位的失败进而影响***的精度和稳定性。
漂移检测与补偿的研究旨在解决SLAM***中漂移问题，漂移检测的关键在于实时监测位姿的偏差以及点云地图的质量，目前通常使用关键帧之间的特征匹配结果或者点云配准误差、运动模型验证等手段，识别漂移的发生并及时采取补偿措施。漂移补偿则着重于修正漂移误差，通常使用闭环校正以及重启建图流程等方法，以提高SLAM***的鲁棒性和精度。然而现在大多的漂移检测与补偿方案，都使用了单一的漂移检测策略，比如只使用了点云配准误差判断当前建图状态是否正常，这样会导致漂移检测过于激进，将配准较差但是配准结果可用的状态鉴定为漂移，反而会影响整体的建图状态，并且目前的漂移补偿策略大多无法适用于极端情况，例如在某个漂移时刻，误差巨大导致整体轨迹丢失。
综上，目前的漂移检测与补偿算法大多都具有准确性低、鲁棒性与补偿效果较差等问题。
发明内容
本发明的目的是克服了上述现有技术的缺点，提供了一种具有较好的鲁棒性、准确性与适用性的退化场景下基于多传感器融合的SLAM漂移检测与补偿方法、装置、处理器及其计算机可读存储介质。
为了实现上述目的，本发明的退化场景下基于多传感器融合的SLAM漂移检测与补偿方法、装置、处理器及其计算机可读存储介质如下：
该退化场景下基于多传感器融合的SLAM漂移检测与补偿方法，其主要特点是，所述的方法包括以下步骤：
（1）根据当前帧的预测位姿进行漂移检测，以确认当前帧是否产生漂移；
（2）对当前帧进行正常状态检测，进一步确认当前帧是否处于正常状态；
（3）根据步骤（1）和（2）不同的检测结果，对当前帧分别进行相应的状态判断以及因子图优化处理，以获取当前帧的位姿优化结果；
（4）根据所述的位姿优化结果进行当前雷达点云帧的位姿预测以及NDT点云配准处理，以此完成漂移检测与补偿处理。
较佳地，所述的步骤（1）具体包括以下步骤：
（1.1）计算两帧点云之间的相对位姿与当前点云初值的乘积， 并将该乘积作为当前关键帧位姿的观测值；
（1.2）按照以下方式计算当前关键帧位姿观测值与上一关键帧之间的相对位姿：
；
其中，为上一关键帧的绝对位姿以及进行NDT配准的6×6维的hessian矩阵H，x、y、z、roll、pitch以及yaw分别代表两帧之间的x轴、y轴、z轴的相对偏移以及相对旋转的角度；
（1.3）将所述的hessian矩阵H按照以下方式进行SVD分解：
；
其中，U为左奇异向量矩阵，为右奇异向量矩阵，/>为对角线元素为H特征值的对角矩阵；
通过寻找对角线上的元素最大值/>与最小值/>，求解其两者之间的比值；
（1.4）设置静态变量标志位进行首帧判断，若当前帧为第一帧，则认为没有漂移，并将上述得到的x、y、z、roll、pitch、yaw以及ratio均存入相应的队列X、Y、Z、Roll、Pitch、Yaw以及Ratio中；否则，计算上述队列中所有数据的平均值、/>、/>、/>、/>、/>以及/>，并将新的数据分别存入相应的队列中；
（1.5）设置固定滑窗的阈值为n，当上述队列尺寸大于该阈值时，则删除相应队列最初存入的值；
（1.6）按照以下方式进行漂移检测：
；
若产生漂移则将漂移标志位设为1，即。
较佳地，所述的步骤（2）具体包括以下步骤：
（2.1）输入轮速里程计数据，包括当前车辆的线速度、角速度/>以及与当前帧相对应的时间戳信息/>，并将相应的轮速里程计数据按照时间先后顺序存入队列W中；
（2.2）将步骤（1.1）中输入的两帧点云的时间戳与/>，在所述的队列W中寻找在时间戳/>与/>之间的轮速里程计数据，并将该数据送入积分器计算/>时间段内的轮速里程计的位姿变换，具体如下：
；
；
其中，与/>分别为轮速里程计在/>时间段内变换的位置与姿态，且、/>，/>、/>、/>、/>、/>、/>分别代表了/>时间段内轮速里程计测量的x、y、z轴的相对偏移以及相对旋转的角度，/>与分别为t时刻轮速里程计线速度与角速度的测量值；
（2.3）按照以下方式对当前帧进行正常状态检测：
；
其中，、/>与/>分别为第一阈值、第二阈值以及第三阈值。
较佳地，所述的步骤（3）具体包括以下步骤：
（3.1）对当前帧进行状态判断，当为正常状态时，即则进入步骤（3.2），当为漂移状态时，即/>则进入步骤（3.3）；
（3.2）将相对位姿与/>、/>分别作为约束条件添加至因子图中作为因子图的between因子，并将相对位姿T与上一关键帧的绝对位姿相乘作为当前关键帧的观测值/>输入至因子图中进行优化，目标函数/>如下式：
；
其中，为先验因子误差函数，/>为雷达里程计的between因子误差函数，/>为轮速里程计的between因子误差函数，/>为各自因子的6×6协方差矩阵，其中/>，/>为上一时刻优化后的位姿，/>，/>为当前状态的观测值。
（3.3）将轮速里程计作为between因子添加至因子图中，并适当减小其协方差，因子图目标函数具体如下：
；
其中，，s为设置的比例参数；
（3.4）使用Levenberg-Marquardt非线性最小二乘优化算法求解步骤（3.2）和（3.3）中的目标函数，得到当前关键帧状态变量的估计值，即当前帧的位姿优化结果。
较佳地，所述的步骤（4）具体包括以下步骤：
（4.1）将所述的位姿优化结果作为IMU预积分器的先验值；
（4.2）在接收到新的雷达点云帧之前，将IMU数据持续添加至IMU预积分器中，并对点云帧的当前状态进行估计；
（4.3）当接收到新的雷达点云时，将步骤（4.2）输出的预测位姿作为当前雷达点云帧/>的位姿初值；
（4.4）以当前关键帧为圆心，在历史关键帧位姿队列中搜寻在半径范围r内的所有关键帧，并将这些关键帧进行点云叠加作为局部地图M；
（4.5）将所述的局部地图M与当前新的雷达点云帧作为NDT点云配准算法的输入，并进行NDT点云配准；
（4.6）将配准结果迭代输入至步骤（1）中，并开始新一轮的漂移检测与补偿。
该实现退化场景下基于多传感器融合的SLAM漂移检测与补偿的装置，其主要特点是，所述的装置包括：
处理器，被配置成执行计算机可执行指令；
存储器，存储一个或多个计算机可执行指令，所述的计算机可执行指令被所述的处理器执行时，实现上述所述的退化场景下基于多传感器融合的SLAM漂移检测与补偿方法的步骤。
该实现退化场景下基于多传感器融合的SLAM漂移检测与补偿的处理器，其主要特点是，所述的处理器被配置成执行计算机可执行指令，所述的计算机可执行指令被所述的处理器执行时，实现上述所述的退化场景下基于多传感器融合的SLAM漂移检测与补偿方法的步骤。
该计算机可读存储介质，其主要特点是，其上存储有计算机程序，所述的计算机程序可被处理器执行以实现上述所述的退化场景下基于多传感器融合的SLAM漂移检测与补偿方法的步骤。
采用了本发明的该退化场景下基于多传感器融合的SLAM漂移检测与补偿方法、装置、处理器及其计算机可读存储介质，利用NDT配准的Hessian矩阵以及当前帧间相对的估计位姿与滑窗内历史估计位姿的平均值作为当前是否产生漂移的评价标准，实现了更为鲁棒且温和的漂移检测效果。同时，为了准确判断当前建图状态是否由漂移恢复正常，本技术方案利用轮速里程计与雷达里程计之间的差值作为评判标准，有效减少了建图状态突变而导致图优化失效的问题，更适用于实际场景。由于利用轮速里程计与雷达里程计同时作为between因子，从而有效实现了对因子图的强约束，从而减少了漂移问题。此外，凭借鲁棒的漂移检测与正常状态检测算法，本技术方案提出在检测到漂移时可以只添加轮速里程计作为between因子，并相应减小其协方差，以给因子图足够的约束，从而在建图过程中出现漂移时实现了有效的漂移补偿，保证了建图的正常运行，提高了SLAM建图的鲁棒性以及环境适应能力。
综上所述，本技术方案在实际建图的过程中具有较好的鲁棒性、准确性与适用性。
附图说明
图1为本发明的退化场景下基于多传感器融合的SLAM漂移检测与补偿方法的流程图。
图2为在正常状态下进行因子图优化的示意图。
图3为在漂移状态下进行因子图优化的示意图。
具体实施方式
为了能够更清楚地描述本发明的技术内容，下面结合具体实施例来进行进一步的描述。
在详细说明根据本发明的实施例前，应该注意到的是，在下文中，术语“包括”、“包含”或任何其他变体旨在涵盖非排他性的包含，由此使得包括一系列要素的过程、方法、物品或者设备不仅包含这些要素，而且还包含没有明确列出的其他要素，或者为这种过程、方法、物品或者设备所固有的要素。
请参阅图1所示，该退化场景下基于多传感器融合的SLAM漂移检测与补偿方法，其中，所述的方法包括以下步骤：
（1）根据当前帧的预测位姿进行漂移检测，以确认当前帧是否产生漂移；
（2）对当前帧进行正常状态检测，进一步确认当前帧是否处于正常状态；
（3）根据步骤（1）和（2）不同的检测结果，对当前帧分别进行相应的状态判断以及因子图优化处理，以获取当前帧的位姿优化结果；
（4）根据所述的位姿优化结果进行当前雷达点云帧的位姿预测以及NDT点云配准处理，以此完成漂移检测与补偿处理。
作为本发明的优选实施方式，所述的步骤（1）具体包括以下步骤：
（1.1）计算两帧点云之间的相对位姿与当前点云初值的乘积， 并将该乘积作为当前关键帧位姿的观测值；
（1.2）按照以下方式计算当前关键帧位姿观测值与上一关键帧之间的相对位姿：
；
其中，为上一关键帧的绝对位姿以及进行NDT配准的6×6维的hessian矩阵H，x、y、z、roll、pitch以及yaw分别代表两帧之间的x轴、y轴、z轴的相对偏移以及相对旋转的角度；
（1.3）将所述的hessian矩阵H按照以下方式进行SVD分解：
；
其中，U为左奇异向量矩阵，为右奇异向量矩阵，/>为对角线元素为H特征值的对角矩阵；
通过寻找对角线上的元素最大值/>与最小值/>，求解其两者之间的比值；
（1.4）设置静态变量标志位进行首帧判断，若当前帧为第一帧，则认为没有漂移，并将上述得到的x、y、z、roll、pitch、yaw以及ratio均存入相应的队列X、Y、Z、Roll、Pitch、Yaw以及Ratio中；否则，计算上述队列中所有数据的平均值、/>、/>、/>、/>、/>以及/>，并将新的数据分别存入相应的队列中；
（1.5）设置固定滑窗的阈值为n，当上述队列尺寸大于该阈值时，则删除相应队列最初存入的值；
（1.6）按照以下方式进行漂移检测：
；
若产生漂移则将漂移标志位设为1，即。
作为本发明的优选实施方式，所述的步骤（2）具体包括以下步骤：
（2.1）输入轮速里程计数据，包括当前车辆的线速度、角速度/>以及与当前帧相对应的时间戳信息/>，并将相应的轮速里程计数据按照时间先后顺序存入队列W中；
（2.2）将步骤（1.1）中输入的两帧点云的时间戳与/>，在所述的队列W中寻找在时间戳/>与/>之间的轮速里程计数据，并将该数据送入积分器计算/>时间段内的轮速里程计的位姿变换，具体如下：
；
；
其中，与/>分别为轮速里程计在/>时间段内变换的位置与姿态，且、/>，/>、/>、/>、/>、/>、/>分别代表了/>时间段内轮速里程计测量的x、y、z轴的相对偏移以及相对旋转的角度，/>与分别为t时刻轮速里程计线速度与角速度的测量值；
（2.3）按照以下方式对当前帧进行正常状态检测：
；
其中，、/>与/>分别为第一阈值、第二阈值以及第三阈值。
作为本发明的优选实施方式，所述的步骤（3）具体包括以下步骤：
（3.1）对当前帧进行状态判断，当为正常状态时，即则进入步骤（3.2），当为漂移状态时，即/>则进入步骤（3.3）；
（3.2）将相对位姿T与、/>分别作为约束条件添加至因子图中作为因子图的between因子，并将相对位姿T与上一关键帧的绝对位姿相乘作为当前关键帧的观测值/>输入至因子图中进行优化，目标函数/>如下式：
；
其中，为先验因子误差函数，/>为雷达里程计的between因子误差函数，/>为轮速里程计的between因子误差函数，/>为各自因子的6×6协方差矩阵，其中/>，/>为上一时刻优化后的位姿，/>，为当前状态的观测值。
（3.3）将轮速里程计作为between因子添加至因子图中，并适当减小其协方差，因子图目标函数具体如下：
；
其中，，s为设置的比例参数；
（3.4）使用Levenberg-Marquardt非线性最小二乘优化算法求解步骤（3.2）和（3.3）中的目标函数，得到当前关键帧状态变量的估计值，即当前帧的位姿优化结果。
作为本发明的优选实施方式，所述的步骤（4）具体包括以下步骤：
（4.1）将所述的位姿优化结果作为IMU预积分器的先验值；
（4.2）在接收到新的雷达点云帧之前，将IMU数据持续添加至IMU预积分器中，并对点云帧的当前状态进行估计；
（4.3）当接收到新的雷达点云时，将步骤（4.2）输出的预测位姿作为当前雷达点云帧/>的位姿初值；
（4.4）以当前关键帧为圆心，在历史关键帧位姿队列中搜寻在半径范围r内的所有关键帧，并将这些关键帧进行点云叠加作为局部地图M；
（4.5）将所述的局部地图M与当前新的雷达点云帧作为NDT点云配准算法的输入，并进行NDT点云配准；
（4.6）将配准结果迭代输入至步骤（1）中，并开始新一轮的漂移检测与补偿。
在实际应用当中，本技术方案主要包括四个部分，分别是漂移检测、正常状态检测、因子图优化以及位姿预测与NDT配准。下面将详细介绍各个处理过程：
1.漂移检测：这一步用于确认当前帧是否产生漂移
a）计算两帧点云之间的相对位姿与当前点云初值的乘积作为当前关键帧位姿的观测值，接着计算当前关键帧位姿观测值与上一关键帧之间的相对位姿，其中/>为上一关键帧的绝对位姿，以及NDT配准的6×6维的hessian矩阵H，其中T由6维数据组成，如下：
；
其中x、y、z、roll、pitch以及yaw分别代表两帧之间的x、y、z轴相对偏移以及相对旋转的角度。
b）将hessian矩阵H进行SVD分解可得：
；
其中U为左奇异向量矩阵，为右奇异向量矩阵，/>为对角线元素为H特征值的对角矩阵。寻找/>对角线上的元素最大值/>与最小值/>，求解其两者的比值/>。
c）设置静态变量标志位进行首帧判断，若当前为第一帧，则认为没有漂移，并将上述x、y、z、roll、pitch、yaw以及ratio存入相应的队列X、Y、Z、Roll、Pitch、Yaw以及Ratio中。否则计算上述队列中所有数据的平均值、/>、/>、/>、/>、/>以及/>，并将新的数据分别存入队列。
d）设置固定滑窗大小，也就是设置阈值为n，当上述队列尺寸大于阈值时，删除队列最初存入的值。
e）进行漂移检测，如下式：
；
若产生漂移则将漂移标志位设为1，。
2.正常状态检测：这一步用于确认当前帧是否是正常状态
a）输入轮速里程计数据，包括当前车辆的线速度、角速度/>与其对应的时间戳信息/>。并将其按照时间先后顺序存入队列W中。
b）根据步骤1中输入的两帧点云的时间戳与/>，在队列W中寻找在/>与/>之间的轮速里程计数据，并将其送入积分器计算/>时间段内的轮速里程计的位姿变换，具体步骤如下：
；
；
其中与/>分别为轮速里程计在/>时间段内变换的位置与姿态，具体、/>，/>、/>、/>、/>、/>、/>分别代表了/>时间段内轮速里程计测量的x、y、z轴的相对偏移以及相对旋转的角度，/>与分别为t时刻轮速里程计线速度与角速度的测量值。
c）进行正常状态检测，如下式：
；
其中、/>与/>分别为所设阈值，本专利实验测试值可将其分别取值为0.5、0.5与0.05。
3.因子图优化
a）进行状态判断，当即当前帧状态为正常时：将T与/>、/>分别作为约束条件添加至因子图中作为因子图的between因子，并将T与上一关键帧的绝对位姿相乘作为当前关键帧的观测值/>输入至因子图中进行优化，目标函数/>如下式：
；
其中为先验因子误差函数、/>为雷达里程计的between因子误差函数、/>为轮速里程计的between因子误差函数。/>为各自因子的6×6协方差矩阵，其中/>，/>为上一时刻优化后的位姿，/>。因子图结构如图2所示。
b）当判定当前帧产生漂移，即时，不添加雷达里程计因子，只将轮速里程计作为between因子添加至因子图中，并且适当减小其协方差，具体因子图目标函数/>如下：
；
其中，s为设置的比例参数，实际测试可将其设置为100。因子图结构如图3所示。
c）因子图优化。使用Levenberg-Marquardt非线性最小二乘优化算法求解a与b步骤中的目标函数，得到当前关键帧状态变量的估计值，即当前帧的位姿优化结果。
4.位姿预测与NDT配准
a）将步骤3得到的位姿优化结果作为IMU预积分器的先验。
b）由于IMU传感器的频率较高，在接收到新的雷达点云帧之前，将IMU数据持续添加至IMU预积分器，并对当前状态进行估计。
c）当接收到新的雷达点云时，将步骤b输出的预测位姿作为当前雷达点云帧的位姿初值。
以当前关键帧为圆心在历史关键帧位姿队列中搜寻在半径范围r内的所有关键帧，并将这些关键帧点云叠加，作为局部地图M，并将局部地图M与当前新的雷达点云帧作为NDT点云配准算法的输入，并进行NDT点云配准，将配准结果迭代输入至步骤1，开始新一轮的漂移检测与补偿。
在实际应用当中，本技术方案使用雷达SLAM前端中用于点云配准的NDT算法所输出的hessian矩阵以及当前帧配准结果判断当前状态是否产生漂移，若产生漂移，则判定当前雷达里程计不可用，进而调整后端基于因子图进行位姿优化的策略，使用轮速里程计的相对位姿替换由点云配准输出的雷达里程计因子。同时还需要继续对建图状态进行判断，若当前状态已经恢复平稳，并未产生漂移，则将上述SLAM后端优化策略恢复为原始策略，即使用雷达里程计作为因子图优化的between因子。
SLAM漂移检测与补偿的研究对于提高自主导航***的鲁棒性、精度和长时间稳定运行具有重要意义。在无人车、智能仓储、智能家居等领域，准确的定位和地图信息对于实现智能化决策和规划至关重要。因此，该研究不仅可以推动SLAM技术的发展，还可以为智能机器人技术的进步和应用提供重要支持。
流程图中或在此以其他方式描述的任何过程或方法描述可以被理解为，表示包括一个或更多个用于实现特定逻辑功能或过程的步骤的可执行指令的代码的模块、片段或部分，并且本发明的优选实施方式的范围包括另外的实现，其中可以不按所示出或讨论的顺序，包括根据所涉及的功能按基本同时的方式或按相反的顺序，来执行功能，这应被本发明的实施例所属技术领域的技术人员所理解。
应当理解，本发明的各部分可以用硬件、软件、固件或它们的组合来实现。在上述实施方式中，多个步骤或方法可以用存储在存储器中且由合适的指令执行装置执行的软件或固件来实现。
本技术领域的普通技术人员可以理解实现上述实施例方法携带的全部或部分步骤是可以通过程序来指令相关的硬件完成的，程序可以存储于一种计算机可读存储介质中，该程序在执行时，包括方法实施例的步骤之一或其组合。
上述提到的存储介质可以是只读存储器，磁盘或光盘等。
在本说明书的描述中，参考术语“一实施例”、“一些实施例”、“示例”、“具体示例”、或“实施例”等的描述意指结合该实施例或示例描述的具体特征、结构、材料或者特点包含于本发明的至少一个实施例或示例中。在本说明书中，对上述术语的示意性表述不一定指的是相同的实施例或示例。而且，描述的具体特征、结构、材料或者特点可以在任何的一个或多个实施例或示例中以合适的方式结合。
尽管上面已经示出和描述了本发明的实施例，可以理解的是，上述实施例是示例性的，不能理解为对本发明的限制，本领域的普通技术人员在本发明的范围内可以对上述实施例进行变化、修改、替换和变型。
采用了本发明的该退化场景下基于多传感器融合的SLAM漂移检测与补偿方法、装置、处理器及其计算机可读存储介质，利用NDT配准的Hessian矩阵以及当前帧间相对的估计位姿与滑窗内历史估计位姿的平均值作为当前是否产生漂移的评价标准，实现了更为鲁棒且温和的漂移检测效果。同时，为了准确判断当前建图状态是否由漂移恢复正常，本技术方案利用轮速里程计与雷达里程计之间的差值作为评判标准，有效减少了建图状态突变而导致图优化失效的问题，更适用于实际场景。由于利用轮速里程计与雷达里程计同时作为between因子，从而有效实现了对因子图的强约束，从而减少了漂移问题。此外，凭借鲁棒的漂移检测与正常状态检测算法，本技术方案提出在检测到漂移时可以只添加轮速里程计作为between因子，并相应减小其协方差，以给因子图足够的约束，从而在建图过程中出现漂移时实现了有效的漂移补偿，保证了建图的正常运行，提高了SLAM建图的鲁棒性以及环境适应能力。
综上所述，本技术方案在实际建图的过程中具有较好的鲁棒性、准确性与适用性。
在此说明书中，本发明已参照其特定的实施例作了描述。但是，很显然仍可以作出各种修改和变换而不背离本发明的精神和范围。因此，说明书和附图应被认为是说明性的而非限制性的。
Claims (8)
1.一种退化场景下基于多传感器融合的SLAM漂移检测与补偿方法，其特征在于，所述的方法包括以下步骤：
（1）根据当前帧的预测位姿进行漂移检测，以确认当前帧是否产生漂移；
（2）对当前帧进行正常状态检测，进一步确认当前帧是否处于正常状态；
（3）根据步骤（1）和（2）不同的检测结果，对当前帧分别进行相应的状态判断以及因子图优化处理，以获取当前帧的位姿优化结果；
（4）根据所述的位姿优化结果进行当前雷达点云帧的位姿预测以及NDT点云配准处理，以此完成漂移检测与补偿处理。
2.根据权利要求1所述的退化场景下基于多传感器融合的SLAM漂移检测与补偿方法，其特征在于，所述的步骤（1）具体包括以下步骤：
（1.1）计算两帧点云之间的相对位姿与当前点云初值的乘积， 并将该乘积作为当前关键帧位姿的观测值；
（1.2）按照以下方式计算当前关键帧位姿观测值与上一关键帧之间的相对位姿：
；
其中，为上一关键帧的绝对位姿以及进行NDT配准的6×6维的hessian矩阵H，x、y、z，分别代表两帧之间的x轴、y轴、z轴的相对偏移，roll、pitch以及yaw表示相对旋转的角度；
（1.3）将所述的hessian矩阵H按照以下方式进行SVD分解：
；
其中，为左奇异向量矩阵，/>为右奇异向量矩阵，/>为对角线元素为H特征值的对角矩阵；
通过寻找对角线上的元素最大值/>与最小值/>，求解其两者之间的比值；
（1.4）设置静态变量标志位进行首帧判断，若当前帧为第一帧，则认为没有漂移，并将上述得到的x、y、z、roll、pitch、yaw以及ratio均存入相应的队列X、Y、Z、Roll、Pitch、Yaw以及Ratio中；否则，计算上述队列中所有数据的平均值、/>、/>、/>、/>、/>以及，并将新的数据分别存入相应的队列中；
（1.5）设置固定滑窗的阈值为n，当上述队列尺寸大于该阈值时，则删除相应队列最初存入的值；
（1.6）按照以下方式进行漂移检测：
；
若产生漂移则将漂移标志位设为1，即。
3.根据权利要求2所述的退化场景下基于多传感器融合的SLAM漂移检测与补偿方法，其特征在于，所述的步骤（2）具体包括以下步骤：
（2.1）输入轮速里程计数据，包括当前车辆的线速度、角速度/>以及与当前帧相对应的时间戳信息/>，并将相应的轮速里程计数据按照时间先后顺序存入队列W中；
（2.2）将步骤（1.1）中输入的两帧点云的时间戳与/>，在所述的队列W中寻找在时间戳/>与/>之间的轮速里程计数据，并将该数据送入积分器计算/>时间段内的轮速里程计的位姿变换，具体如下：
；
；
其中，与/>分别为轮速里程计在/>时间段内变换的位置与姿态，且、/>，/>、/>、/>，分别代表了/>时间段内轮速里程计测量的x、y、z轴的相对偏移，/>、/>、/>分别表示相对旋转的角度，/>与/>分别为/>时刻轮速里程计线速度与角速度的测量值；
（2.3）按照以下方式对当前帧进行正常状态检测：
；
其中，、/>与/>分别为第一阈值、第二阈值以及第三阈值。
4.根据权利要求3所述的退化场景下基于多传感器融合的SLAM漂移检测与补偿方法，其特征在于，所述的步骤（3）具体包括以下步骤：
（3.1）对当前帧进行状态判断，当为正常状态时，即则进入步骤（3.2），当为漂移状态时，即/>则进入步骤（3.3）；
（3.2）将相对位姿与/>、/>分别作为约束条件添加至因子图中作为因子图的between因子，并将相对位姿T与上一关键帧的绝对位姿相乘作为当前关键帧的观测值/>输入至因子图中进行优化，目标函数/>如下式：
；
其中，为先验因子误差函数，/>为雷达里程计的between因子误差函数，为轮速里程计的between因子误差函数，/>为各自因子的6×6协方差矩阵，其中，/>为上一时刻优化后的位姿，/>，/>为当前状态的观测值；
（3.3）将轮速里程计作为between因子添加至因子图中，并适当减小其协方差，因子图目标函数具体如下：
；
其中，，s为设置的比例参数；
（3.4）使用Levenberg-Marquardt非线性最小二乘优化算法求解步骤（3.2）和（3.3）中的目标函数，得到当前关键帧状态变量的估计值，即当前帧的位姿优化结果。
5.根据权利要求4所述的退化场景下基于多传感器融合的SLAM漂移检测与补偿方法，其特征在于，所述的步骤（4）具体包括以下步骤：
（4.1）将所述的位姿优化结果作为IMU预积分器的先验值；
（4.2）在接收到新的雷达点云帧之前，将IMU数据持续添加至IMU预积分器中，并对点云帧的当前状态进行估计；
（4.3）当接收到新的雷达点云时，将步骤（4.2）输出的预测位姿作为当前雷达点云帧/>的位姿初值；
（4.4）以当前关键帧为圆心，在历史关键帧位姿队列中搜寻在半径范围r内的所有关键帧，并将这些关键帧进行点云叠加作为局部地图M；
（4.5）将所述的局部地图M与当前新的雷达点云帧作为NDT点云配准算法的输入，并进行NDT点云配准；
（4.6）将配准结果迭代输入至步骤（1）中，并开始新一轮的漂移检测与补偿。
6.一种实现退化场景下基于多传感器融合的SLAM漂移检测与补偿的装置，其特征在于，所述的装置包括：
处理器，被配置成执行计算机可执行指令；
存储器，存储一个或多个计算机可执行指令，所述的计算机可执行指令被所述的处理器执行时，实现权利要求1～5中任一项所述的退化场景下基于多传感器融合的SLAM漂移检测与补偿方法的步骤。
7.一种实现退化场景下基于多传感器融合的SLAM漂移检测与补偿的处理器，其特征在于，所述的处理器被配置成执行计算机可执行指令，所述的计算机可执行指令被所述的处理器执行时，实现权利要求1～5中任一项所述的退化场景下基于多传感器融合的SLAM漂移检测与补偿方法的步骤。
8.一种计算机可读存储介质，其特征在于，其上存储有计算机程序，所述的计算机程序可被处理器执行以实现权利要求1～5中任一项所述的退化场景下基于多传感器融合的SLAM漂移检测与补偿方法的步骤。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202410225198.7A CN117808882B (zh) | 2024-02-29 | 2024-02-29 | 退化场景下基于多传感器融合的slam漂移检测与补偿方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202410225198.7A CN117808882B (zh) | 2024-02-29 | 2024-02-29 | 退化场景下基于多传感器融合的slam漂移检测与补偿方法 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN117808882A true CN117808882A (zh) | 2024-04-02 |
| CN117808882B CN117808882B (zh) | 2024-05-17 |
Family
ID=90428118
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202410225198.7A Active CN117808882B (zh) | 2024-02-29 | 2024-02-29 | 退化场景下基于多传感器融合的slam漂移检测与补偿方法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN117808882B (zh) |
Citations (10)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN109522832A (zh) * | 2018-11-06 | 2019-03-26 | 浙江工业大学 | 一种基于点云片段匹配约束和轨迹漂移优化的回环检测方法 |
| US20200309529A1 (en) * | 2019-03-29 | 2020-10-01 | Trimble Inc. | Slam assisted ins |
| US20210042958A1 (en) * | 2019-08-09 | 2021-02-11 | Facebook Technologies, Llc | Localization and mapping utilizing visual odometry |
| CN113516692A (zh) * | 2021-05-18 | 2021-10-19 | 上海汽车集团股份有限公司 | 一种多传感器融合的slam方法和装置 |
| CN114792338A (zh) * | 2022-01-10 | 2022-07-26 | 天津大学 | 基于先验三维激光雷达点云地图的视觉融合定位方法 |
| CN115272456A (zh) * | 2022-06-21 | 2022-11-01 | 东南大学 | 激光雷达与相机在线漂移检测方法、校正方法、装置及存储介质 |
| CN116878501A (zh) * | 2023-07-12 | 2023-10-13 | 北京理工大学 | 一种基于多传感器融合的高精度定位与建图***及方法 |
| CN116878542A (zh) * | 2023-07-26 | 2023-10-13 | 广西师范大学 | 一种抑制里程计高度漂移的激光slam方法 |
| CN117268408A (zh) * | 2023-08-02 | 2023-12-22 | 华能伊敏煤电有限责任公司 | 一种激光slam定位方法及*** |
| CN117541655A (zh) * | 2024-01-10 | 2024-02-09 | 上海几何伙伴智能驾驶有限公司 | 融合视觉语义消除radar建图z轴累积误差的方法 |
-
2024
- 2024-02-29 CN CN202410225198.7A patent/CN117808882B/zh active Active
Patent Citations (10)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN109522832A (zh) * | 2018-11-06 | 2019-03-26 | 浙江工业大学 | 一种基于点云片段匹配约束和轨迹漂移优化的回环检测方法 |
| US20200309529A1 (en) * | 2019-03-29 | 2020-10-01 | Trimble Inc. | Slam assisted ins |
| US20210042958A1 (en) * | 2019-08-09 | 2021-02-11 | Facebook Technologies, Llc | Localization and mapping utilizing visual odometry |
| CN113516692A (zh) * | 2021-05-18 | 2021-10-19 | 上海汽车集团股份有限公司 | 一种多传感器融合的slam方法和装置 |
| CN114792338A (zh) * | 2022-01-10 | 2022-07-26 | 天津大学 | 基于先验三维激光雷达点云地图的视觉融合定位方法 |
| CN115272456A (zh) * | 2022-06-21 | 2022-11-01 | 东南大学 | 激光雷达与相机在线漂移检测方法、校正方法、装置及存储介质 |
| CN116878501A (zh) * | 2023-07-12 | 2023-10-13 | 北京理工大学 | 一种基于多传感器融合的高精度定位与建图***及方法 |
| CN116878542A (zh) * | 2023-07-26 | 2023-10-13 | 广西师范大学 | 一种抑制里程计高度漂移的激光slam方法 |
| CN117268408A (zh) * | 2023-08-02 | 2023-12-22 | 华能伊敏煤电有限责任公司 | 一种激光slam定位方法及*** |
| CN117541655A (zh) * | 2024-01-10 | 2024-02-09 | 上海几何伙伴智能驾驶有限公司 | 融合视觉语义消除radar建图z轴累积误差的方法 |
Non-Patent Citations (3)
| Title |
|---|
| KINETIS60: "SLAM中的退化问题", Retrieved from the Internet <URL:https://blog.csdn.net/Curryfun/article/details/105838695> * |
| LI D ET AL: "The identification and compensation of static drift induced by external disturbances for LiDAR SLAM", IEEE, 13 April 2021 (2021-04-13), pages 58102 - 58115, XP011850548, DOI: 10.1109/ACCESS.2021.3072935 * |
| 聂晓玲: "基于激光雷达和IMU的三维高精度地图生成算法研究", 中国优秀硕士学位论文全文数据库-信息科技辑, 15 January 2023 (2023-01-15), pages 8 - 49 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN117808882B (zh) | 2024-05-17 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN110243358B (zh) | 多源融合的无人车室内外定位方法及*** | |
| JP5854655B2 (ja) | 無人機、特に自動操縦下でホバリング飛行を行うことができる無人機の水平速度を評価する方法 | |
| EP2519803B1 (en) | Technique for calibrating dead reckoning positioning data | |
| Fakharian et al. | Adaptive Kalman filtering based navigation: An IMU/GPS integration approach | |
| CN106814753B (zh) | 一种目标位置矫正方法、装置及*** | |
| Anjum et al. | Sensor data fusion using unscented kalman filter for accurate localization of mobile robots | |
| CN111915675B (zh) | 基于粒子漂移的粒子滤波点云定位方法及其装置和*** | |
| EP4345421A2 (en) | Method for calibrating sensor parameters based on autonomous driving, apparatus, storage medium, and vehicle | |
| CN113203429B (zh) | 一种陀螺仪温度漂移误差的在线估计及补偿方法 | |
| JP2017122960A (ja) | 車両位置推定装置 | |
| CN113538699A (zh) | 基于三维点云的定位方法、装置、设备及存储介质 | |
| CN113052855A (zh) | 一种基于视觉-imu-轮速计融合的语义slam方法 | |
| CN117739972B (zh) | 一种无全球卫星定位***的无人机进近阶段定位方法 | |
| CN117808882B (zh) | 退化场景下基于多传感器融合的slam漂移检测与补偿方法 | |
| CN112697153A (zh) | 自主移动设备的定位方法、电子设备及存储介质 | |
| CN115797490B (zh) | 基于激光视觉融合的建图方法及*** | |
| CN116338719A (zh) | 基于b样条函数的激光雷达-惯性-车辆融合定位方法 | |
| CN114001730B (zh) | 融合定位方法、装置、计算机设备和存储介质 | |
| CN115540854A (zh) | 一种基于uwb辅助的主动定位方法、设备和介质 | |
| CN115585805A (zh) | 车辆定位方法、定位装置、定位***以及车辆 | |
| CN113483762A (zh) | 一种位姿优化方法及设备 | |
| CN112611377A (zh) | 一种用于小车室外导航的状态预测方法、装置及存储介质 | |
| JPH0914962A (ja) | 建設車両の位置計測装置 | |
| CN114838726B (zh) | 一种基于多传感器数据融合的gps数据修正方法 | |
| WO2024048264A1 (ja) | 情報処理装置、および情報処理方法、並びにプログラム |
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 | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |