本发明专利技术涉及一种退化场景下基于多传感器融合的SLAM漂移检测与补偿方法，包括：根据当前帧的预测位姿进行漂移检测；对当前帧进行正常状态检测，进一步确认当前帧是否处于正常状态；根据上述不同的检测结果，对当前帧分别进行相应的状态判断以及因子图优化处理，以获取当前帧的位姿优化结果；根据位姿优化结果进行当前雷达点云帧的位姿预测以及NDT点云配准处理。本发明专利技术还涉及一种相应的装置、处理器及其存储介质。采用了本发明专利技术的该退化场景下基于多传感器融合的SLAM漂移检测与补偿方法、装置、处理器及其存储介质，利用轮速里程计与雷达里程计之间的差值作为评判标准，有效减少了建图状态突变而导致图优化失效的问题，更适用于实际场景。
【技术实现步骤摘要】
本专利技术涉及定位与建图，尤其涉及漂移检测与补偿领域，具体是指一种退化场景下基于多传感器融合的slam漂移检测与补偿方法。
技术介绍
1、近年来，随着传感设备的不断更新迭代，定位与建图技术（simultaneouslocalization and mapping，slam）已经变得越来越成熟，成为了自动驾驶、室内机器人导航、安防监控、虚拟现实、游戏开发等应用领域中不可或缺的技术手段。然而，由于传感器误差、环境退化、传感器故障以及长时间运行等因素，slam系统往往会出现或多或少的漂移现象，即随着时间的推移，定位和地图的误差会逐渐累积，更甚者会直接导致slam建图与定位的失败进而影响系统的精度和稳定性。
2、漂移检测与补偿的研究旨在解决slam系统中漂移问题，漂移检测的关键在于实时监测位姿的偏差以及点云地图的质量，目前通常使用关键帧之间的特征匹配结果或者点云配准误差、运动模型验证等手段，识别漂移的发生并及时采取补偿措施。漂移补偿则着重于修正漂移误差，通常使用闭环校正以及重启建图流程等方法，以提高slam系统的鲁棒性和精度。然而现在大多的漂移检测与补偿方案，都使用了单一的漂移检测策略，比如只使用了点云配准误差判断当前建图状态是否正常，这样会导致漂移检测过于激进，将配准较差但是配准结果可用的状态鉴定为漂移，反而会影响整体的建图状态，并且目前的漂移补偿策略大多无法适用于极端情况，例如在某个漂移时刻，误差巨大导致整体轨迹丢失。
3、综上，目前的漂移检测与补偿算法大多都具有准确性低、鲁棒性与补偿效果较差等问题。
技术实现思路
1、本专利技术的目的是克服了上述现有技术的缺点，提供了一种具有较好的鲁棒性、准确性与适用性的退化场景下基于多传感器融合的slam漂移检测与补偿方法、装置、处理器及其计算机可读存储介质。
2、为了实现上述目的，本专利技术的退化场景下基于多传感器融合的slam漂移检测与补偿方法、装置、处理器及其计算机可读存储介质如下：
3、该退化场景下基于多传感器融合的slam漂移检测与补偿方法，其主要特点是，所述的方法包括以下步骤：
4、（1）根据当前帧的预测位姿进行漂移检测，以确认当前帧是否产生漂移；
5、（2）对当前帧进行正常状态检测，进一步确认当前帧是否处于正常状态；
6、（3）根据步骤（1）和（2）不同的检测结果，对当前帧分别进行相应的状态判断以及因子图优化处理，以获取当前帧的位姿优化结果；
7、（4）根据所述的位姿优化结果进行当前雷达点云帧的位姿预测以及ndt点云配准处理，以此完成漂移检测与补偿处理。
8、较佳地，所述的步骤（1）具体包括以下步骤：
9、（1.1）计算两帧点云之间的相对位姿与当前点云初值的乘积， 并将该乘积作为当前关键帧位姿的观测值；
10、（1.2）按照以下方式计算当前关键帧位姿观测值与上一关键帧之间的相对位姿：
11、；
12、其中，为上一关键帧的绝对位姿以及进行ndt配准的6×6维的hessian矩阵h，x、y、z、roll、pitch以及yaw分别代表两帧之间的x轴、y轴、z轴的相对偏移以及相对旋转的角度；
13、（1.3）将所述的hessian矩阵h按照以下方式进行svd分解：
14、；
15、其中，u为左奇异向量矩阵，为右奇异向量矩阵，为对角线元素为h特征值的对角矩阵；
16、通过寻找对角线上的元素最大值与最小值，求解其两者之间的比值；
17、（1.4）设置静态变量标志位进行首帧判断，若当前帧为第一帧，则认为没有漂移，并将上述得到的x、y、z、roll、pitch、yaw以及ratio均存入相应的队列x、y、z、roll、pitch、yaw以及ratio中；否则，计算上述队列中所有数据的平均值、、、、、以及，并将新的数据分别存入相应的队列中；
18、（1.5）设置固定滑窗的阈值为n，当上述队列尺寸大于该阈值时，则删除相应队列最初存入的值；
19、（1.6）按照以下方式进行漂移检测：
20、；
21、若产生漂移则将漂移标志位设为1，即。
22、较佳地，所述的步骤（2）具体包括以下步骤：
23、（2.1）输入轮速里程计数据，包括当前车辆的线速度、角速度以及与当前帧相对应的时间戳信息，并将相应的轮速里程计数据按照时间先后顺序存入队列w中；
24、（2.2）将步骤（1.1）中输入的两帧点云的时间戳与，在所述的队列w中寻找在时间戳与之间的轮速里程计数据，并将该数据送入积分器计算时间段内的轮速里程计的位姿变换，具体如下：
25、；
26、；
27、其中，与分别为轮速里程计在时间段内变换的位置与姿态，且、，、、、、、分别代表了时间段内轮速里程计测量的x、y、z轴的相对偏移以及相对旋转的角度，与分别为t时刻轮速里程计线速度与角速度的测量值；
28、（2.3）按照以下方式对当前帧进行正常状态检测：
29、；
30、其中，、与分别为第一阈值、第二阈值以及第三阈值。
31、较佳地，所述的步骤（3）具体包括以下步骤：
32、（3.1）对当前帧进行状态判断，当为正常状态时，即则进入步骤（3.2），当为漂移状态时，即则进入步骤（3.3）；
33、（3.2）将相对位姿与、分别作为约束条件添加至因子图中作为因子图的between因子，并将相对位姿t与上一关键帧的绝对位姿相乘作为当前关键帧的观测值输入至因子图中进行优化，目标函数如下式：
34、；
35、其中，为先验因子误差函数，为雷达里程计的between因子误差函数，为轮速里程计的between因子误差函数，为各自因子的6×6协方差矩阵，其中，为上一时刻优化后的位姿，，为当前状态的观测值。
36、（3.3）将轮速里程计作为between因子添加至因子图中，并适当减小其协方差，因子图目标函数具体如下：
37、；
38、其中，，s为设置的比例参数；
39、（3.4）使用levenberg-marquardt非线性最小二乘优化算法求解步骤（3.2）和（3.3）中的目标函数，得到当前关键帧状态变量的估计值，即当前帧的位姿优化结果。
40、较佳地，所述的步骤（4）具体包括以下步骤：
41、（4.1）将所述的位姿优化结果作为imu预积分器的先验值；
42、（4.2）在接收到新的雷达点云帧之前，将imu数据持续添加至imu预积分器中，并对点云帧的当前状态进行估计；
43、（4.3）当接收到新的雷达点云时，将步骤（4.2）输出的预测位姿作为当前雷达点云帧的位姿初值；
44、（4.4）以当前关键帧为圆心，在历史关键帧位姿队列中搜寻在半径范围r内的所有关键帧，并将本文档来自技高网
...
【技术保护点】
1.一种退化场景下基于多传感器融合的SLAM漂移检测与补偿方法，其特征在于，所述的方法包括以下步骤：
2.根据权利要求1所述的退化场景下基于多传感器融合的SLAM漂移检测与补偿方法，其特征在于，所述的步骤（1）具体包括以下步骤：
3.根据权利要求2所述的退化场景下基于多传感器融合的SLAM漂移检测与补偿方法，其特征在于，所述的步骤（2）具体包括以下步骤：
4.根据权利要求3所述的退化场景下基于多传感器融合的SLAM漂移检测与补偿方法，其特征在于，所述的步骤（3）具体包括以下步骤：
5.根据权利要求4所述的退化场景下基于多传感器融合的SLAM漂移检测与补偿方法，其特征在于，所述的步骤（4）具体包括以下步骤：
6.一种实现退化场景下基于多传感器融合的SLAM漂移检测与补偿的装置，其特征在于，所述的装置包括：
7.一种实现退化场景下基于多传感器融合的SLAM漂移检测与补偿的处理器，其特征在于，所述的处理器被配置成执行计算机可执行指令，所述的计算机可执行指令被所述的处理器执行时，实现权利要求1～5中任一项所述的退化场景下基于多传感器融合的SLAM漂移检测与补偿方法的步骤。
8.一种计算机可读存储介质，其特征在于，其上存储有计算机程序，所述的计算机程序可被处理器执行以实现权利要求1～5中任一项所述的退化场景下基于多传感器融合的SLAM漂移检测与补偿方法的步骤。
...
【技术特征摘要】
1.一种退化场景下基于多传感器融合的slam漂移检测与补偿方法，其特征在于，所述的方法包括以下步骤：
2.根据权利要求1所述的退化场景下基于多传感器融合的slam漂移检测与补偿方法，其特征在于，所述的步骤（1）具体包括以下步骤：
3.根据权利要求2所述的退化场景下基于多传感器融合的slam漂移检测与补偿方法，其特征在于，所述的步骤（2）具体包括以下步骤：
4.根据权利要求3所述的退化场景下基于多传感器融合的slam漂移检测与补偿方法，其特征在于，所述的步骤（3）具体包括以下步骤：
5.根据权利要求4所述的退化场景下基于多传感器融合的slam漂移检测与补偿方法，其特征在于，...
【专利技术属性】
技术研发人员：李琛玮，赵映重，曾啟华，陆新飞，薛旦，史颂华，
申请(专利权)人：上海几何伙伴智能驾驶有限公司，
类型：发明
国别省市：
还没有人留言评论。发表了对其他浏览者有用的留言会获得科技券。