一种基于SLAM建图的多传感器数据融合的算法.pdf

(19)国家知识产权局(12)发明专利申请(10)申请公布号CN115031752A(43)申请公布日2022.09.09(21)申请号202210638638.2G01C22/00(2006.01)(22)申请日2022.06.07G06K9/62(2022.01)(71)申请人潍坊学院地址261061山东省潍坊市东风东街5147号(72)发明人台流臣李文宇王文成吴萍尹贻宽(74)专利代理机构青岛鼎尖知识产权代理有限公司37318专利代理师宋涛(51)Int.Cl.G01C21/34(2006.01)G01S17/86(2020.01)G01S17/89(2020.01)G01C21/16(2006.01)权利要求书2页说明书8页附图4页(54)发明名称一种基于SLAM建图的多传感器数据融合的算法(57)摘要本发明公开了一种基于SLAM建图的多传感器数据融合的算法，具体涉及数据融合领域，包括如下步骤：步骤S1、激光雷达和深度相机接收IMU数据并对机器人进行初始位姿标定；步骤S2、激光雷达扫描周围环境获取激光数据，深度相机获取物体的深度信息生成三维点云图像；步骤S3、激光雷达利用UKF算法进行位姿的实时预测和估计，并辅助双目摄像头采集三维点云数据；步骤S4、激光数据粒子滤波构建的二维栅格地图且深度相机构建二维栅格地图；步骤S5、将激光雷达生成的二维栅格地图与深度相机形成的二维栅格地图通过贝叶斯估计滤波算法在决策级进一步融合，最终形成更加精准的全局二维栅格地图。CN115031752ACN115031752A权利要求书1/2页1.一种基于SLAM建图的多传感器数据融合的算法，其特征在于：包括如下步骤：步骤S1、激光雷达和深度相机接收IMU数据并对机器人进行初始位姿标定；步骤S2、激光雷达扫描周围环境获取激光数据，深度相机获取物体的深度信息生成三维点云图像；步骤S3、激光雷达利用UKF算法进行位姿的实时预测和估计，并依据激光雷达得到的位姿信息与双目摄像头的位置进行转换，辅助双目摄像头采集三维点云数据；步骤S4、激光数据粒子滤波构建的二维栅格地图；且深度相机获取的三维点云地图向下投影成二维信息，并通过Cartographer算法辅助构建二维栅格地图；步骤S5、将激光雷达生成的二维栅格地图与深度相机形成的二维栅格地图通过贝叶斯估计滤波算法在决策级进一步融合，最终形成更加精准的全局二维栅格地图。2.根据权利要求1所述的一种基于SLAM建图的多传感器数据融合的算法，其特征在于：在步骤S4中，对深度相机构建的二维栅格地图通过回环检测算法进行往复回环检测；具体的，将深度相机构建二维栅格地图中的位姿信息通过变换矩阵T将其变换到子图中，其公式如下：式中，当前雷达帧在子图坐标系下的位姿为Tξ，ξθ为角度信息，ξx、ξy为坐标信息。3.根据权利要求2所述的一种基于SLAM建图的多传感器数据融合的算法，其特征在于：将每一幅子图看作由多个位姿构成的概率网格地图，在位姿插入之前确定命中A和未命中B的点集，定义命中和未命中的比例为：对于每个网格命中概率更新公式为：‑1Mnew(x)＝clamp(Z(Z(Mold(x))·Z(PA)))未命中概率为：‑1Mnew(x)＝clamp(Z(Z(Mold(x))·Z(PB)))其中，clamp()将数字限制在一个范围[min,max]内，在这即进行归一化将数字限制在[0,1]之间，Mold(x)为上一次的概率预测值。4.根据权利要求1所述的一种基于SLAM建图的多传感器数据融合的算法，其特征在于：在步骤S3中，在激光数据中采集一组Sigma点用来估计激光雷达的位姿，其中为Sigma采样点经过非线性函数传递后对应的点，其具体公式如下：式中，Pyy和Pxy分别为预测更新后的互协方差和协方差。5.根据权利要求1所述的一种基于SLAM建图的多传感器数据融合的算法，其特征在于：2CN115031752A权利要求书2/2页在步骤S5中，对于决策级的融合，使用改进数据滤波融合，对每一个传感器的概率分布进行联合，形成联合的后验概率分布，之后将求取等到的联合分布对应的似然函数的最小值，作为多传感器融合后的值；通过已知向量Z，估计未知的n维状态向量X可以得到一种计算后验概率的方法如下：若K时刻的概率为p＝(xk)，且已获得K组的测量数据Zk＝{z1,…,zk}和先验分布如下：k‑1其中，p(zk∣xk)为基于激光雷达观测模型得到的似然函数；p(xk∣Z)为基于上一时刻的先验分布函数；p(Zk∣Zk‑1)为在上一时刻时对当前时刻的估计；对于融合多传感器的栅格地图，得到改进的融合公式：其中表示被占据的先验概率，表示传感器采集的占据概率，Po表示经过融合后的结果；将融合后得到的二维栅格地图和三维点云地图投影得到的二维地图进行融合，最终得到全局