买专利卖专利找龙图腾，真高效！ 查专利查商标用IPTOP,全免费！专利年费监控用IP管家,真方便！

龙图腾网获悉山东科技大学;青岛卓信通智能科技有限公司申请的专利一种动态场景下基于全局信息的多源紧耦合SLAM方法获国家发明授权专利权，本发明授权专利权由国家知识产权局授予，授权公告号为：CN120160614B 。

龙图腾网通过国家知识产权局官网在2025-08-26发布的发明授权授权公告中获悉：该发明授权的专利申请号/专利号为：202510609372.2，技术领域涉及：G01C21/00；该发明授权一种动态场景下基于全局信息的多源紧耦合SLAM方法是由王海霞;陈鹏宇;卢晓;梁慧斌;张华宇;张治国;孙巧巧;刘芮东;王倩设计研发完成，并于2025-05-13向国家知识产权局提交的专利申请。

本一种动态场景下基于全局信息的多源紧耦合SLAM方法在说明书摘要公布了：本发明公开了一种动态场景下基于全局信息的多源紧耦合SLAM方法，属于移动机器人领域。本发明通过多源传感器数据融合与算法优化实现高效定位与建图。首先，结合双目相机、单线激光雷达、IMU及里程计数据，利用点云滤波、动态目标检测和运动畸变校正技术，消除无效点云以及数据畸变，并融合相机与激光雷达观测数据；其次，通过无迹卡尔曼滤波UKF融合IMU与里程计数据，输出高精度初始位姿估计；最后，改进Gmapping算法，采用子图融合与广度优先搜索策略优化栅格地图更新模块，增强SLAM模块环境特征保留能力。在动态、复杂场景下该方法具有更好地鲁棒性，构建地图更加符合真实环境，同时能有效减少地图中伪障碍物特征。

本发明授权一种动态场景下基于全局信息的多源紧耦合SLAM方法在权利要求书中公布了：1.一种动态场景下基于全局信息的多源紧耦合SLAM方法，其特征在于，包括以下步骤： 步骤1：传感器数据融合处理； 结合双目相机、激光雷达、IMU及里程计数据，利用点云滤波、动态目标检测和运动畸变校正技术，消除无效点云以及数据畸变，并融合双目相机与激光雷达的观测数据； 步骤2：位姿估计； 通过时空配准将IMU数据统一到里程计坐标系下，然后使用无迹卡尔曼滤波融合里程计与IMU，输出初始位姿估计数据； 步骤3：SLAM建图； 改进Gmapping算法，采用子图融合与广度优先搜索策略优化栅格地图更新模块； 步骤1中，具体包括以下步骤： 步骤1.1：对双目相机输出的3D点云进行无效点云滤除，包括地面、墙面及动态障碍物在内的无效点云，并通过动态投影生成2D点云； 步骤1.2：对激光雷达的2D点云进行运动畸变校正；基于里程计和IMU数据计算点云中每个点的平移和旋转偏移量，生成校正后的激光雷达点云； 步骤1.3：将双目相机的2D点云与激光雷达的2D点云进行时空配准，融合后转换为激光数据格式，得到融合激光观测数据； 步骤3中，具体包括以下步骤： 步骤3.1：将融合激光观测数据与初始位姿估计数据输入改进的Gmapping算法中，通过粒子滤波器确定最终位姿； 步骤3.2：采用子图融合与广度优先搜索策略更新栅格地图，生成环境特征准确的全局地图； 步骤3.2中，具体包括以下步骤： 步骤3.2.1：为每一帧融合激光数据生成子栅格地图，并根据占据概率规则更新子图； 步骤3.2.2：将各子图与全局地图进行融合，对相同栅格采用加权概率叠加规则； 步骤3.2.3：采用广度优先搜索对全局地图中的封闭自由区域填充为未知状态，消除伪障碍物特征； 步骤1.1中，无效点云滤除包括以下步骤： 步骤1.1.1：通过体素滤波对双目相机输出的3D点云进行降采样，并通过直通滤波滤除激光雷达扫描平面以上的点云以及地面点云，得到点云集合P； 步骤1.1.2：随机选择点云集合P＝{pi,i＝1,2…,k}中三个点p1、p2、p3，计算三点所在平面的方程：Ax+By+Cz+D＝0； 步骤1.1.3：依次计算点pixi,yi,zi到平面的距离di： 将di小于设定阈值dmax的点视为平面内点，如果内点数量满足预设的最小内点数，则使用内点继续拟合；如果内点数量不足，则取消原始内点标注，重新进行随机采样拟合，直至迭代结束，将内点从点云集合P中删除，实现无效点云滤除； 步骤1.1.4：通过统计滤波滤除剩余点云集合中的离散点，从双目相机输出的3D点云中提取出相对独立的障碍物3D点云信息； 步骤1.1.5：基于yolo目标检测算法识别动态障碍物，结合双目视差测距计算目标空间距离，删除目标识别框内的点云； 步骤1.1.6：将处理完后的点云实时投影至相机坐标系的水平坐标面，即获得障碍物2D点云； 步骤1.1.5中，具体包括以下步骤： 步骤1.1.5.1：对特定移动目标进行图像数据集采集与模型训练； 对特定的移动目标预先进行图像数据集采集，然后在配置好的yolo环境下训练模型，以便在双目相机上通过yolo目标检测算法进行特定移动目标的识别； 步骤1.1.5.2：根据双目视差测距原理计算动态目标的空间距离Z； 根据yolo目标检测算法得到目标在图像坐标系下的目标识别框的中心像素坐标xp,yp和像素尺寸size_xp、size_yp；根据yolo目标检测算法识别的移动目标在双目相机上的成像视差，计算空间距离Z：具体如下： 移动目标中心点P在双目相机的成像点分别为PL、PR，两个成像点PL和PR之间的距离为： 其中，b为双目相机的基线，f为焦距，L是双目相机光心到成像面的距离的两倍，xL、xR分别表示点P在左、右摄像头成像平面上的成像位置相对于成像平面中心的水平偏移量； 根据相似三角形理论，计算出P的空间距离Z： 视差xL-xR通过双目图像匹配获取； 步骤1.1.5.3：将目标识别框的像素坐标转换为相机坐标系下的三维坐标，并删除框内点云； 首先根据像素分辨率size_p将目标识别框的中心像素坐标和像素尺寸转换成图像坐标系下的实际坐标xtru,ytru和实际尺寸size_xtru、size_ytru 然后分别计算出目标识别框的四个点顶点pi在图像坐标系下的真实坐标： 假设目标识别框在图像坐标系下的坐标为pxtru-i,ytru-i，在相机坐标系下的坐标为PXc-i,Yc-i,Zc-i； 根据相似关系，计算目标识别框的四个顶点pi在相机坐标系下的坐标Xc-i,Yc-i,Zc-i： 其中，f为焦距，Zc-i为双目视差测距提供的目标空间距离Z值，将点云中点坐标x、y在相机坐标系下目标识别框范围内，Z值小于Zc-i的点删除，去除特定移动目标点云； 步骤1.2中，通过里程计和IMU数据对激光雷达点云进行畸变校正，包括以下步骤： 步骤1.2.1：基于激光雷达采集周期内里程计与IMU的位姿变化，计算每个激光点的平移偏移量； 假设激光雷达发射第一个激光点的时刻为ts，雷达坐标系为fs，发射最后一个激光点的时刻为te，则里程计和IMU的数据采集范围应为ts-1至te+1，确保能够完整计算出激光雷达一个采集周期内的平移和旋转；假设ts-1时刻的里程计位移和旋转矩阵为Tstart，te+1时刻的位移和旋转矩阵为Tend，则从ts-1至te+1轮速计坐标变换矩阵T计算公式为： 其中，R和trs即分别为在ts至te的一个周期内发生的旋转和位移对应的矩阵； 假设一个采集周期内某一个激光点对应时刻为tp，这时的雷达坐标系为fp，激光点在fp坐标系下的坐标为p，trs-x和trs-y分别为一个采集周期内X方向和Y方向的总偏移量，则p相对于第一个激光点在X和Y方向的偏移量posX、posY如下： 步骤1.2.2：根据IMU角速度积分，计算绕Z轴的旋转偏移量； 假设发射第一个采集点时激光雷达的角度为θs，采集周期内第k帧IMU数据相对于第一帧IMU数据对应时刻转动的角度值为θk，ωτ是时刻t角速度，则θk的计算公式为： 设θfront、θback分别为tp前一时刻和后一时刻的IMU数据相对第一帧IMU数据的旋转角度，则p相对于第一个数据点绕Z轴的旋转偏移量rotZ为： 步骤1.2.3：通过坐标变换矩阵Tcur对每个激光点进行校正； 假设第一个采集点的变换矩阵为Ts，校正后的点为pcorr，根据每个点相对于第一个点的平移和旋转偏移量，确定当前点p的坐标系fp到第一个采集点的坐标系fs之间的坐标变换矩阵Tcur，计算出点p校正后的坐标pcorr为： pcorr＝Ts -1×Tcur×p； 激光雷达每完成一次数据采集，分别对点云中的每一个点执行步骤1.2.1、步骤1.2.2操作，确定每一个点相对于第一个点的平移和旋转偏移量，然后根据步骤1.2.3所述公式进行坐标变换后，即得到运动畸变校正后的激光雷达点云； 步骤1.3中，包括以下步骤： 步骤1.3.1：通过Autoware标定工具获取双目相机与激光雷达的外参矩阵R,T，将双目相机2D点云转换至激光雷达坐标系；具体如下： 在已知相机内参条件下，通过Autoware中的标定工具，联合标定双目相机与激光雷达，获取二者坐标系外参矩阵，设R为旋转矩阵，T为平移矩阵，pc为经过步骤1.1处理后的双目相机2D点云坐标，pcl为双目相机2D点云在激光雷达坐标系下的坐标，根据下式进行双目相机2D点云与激光雷达2D点云的空间配准，得到双目相机2D点云在激光雷达坐标系下的坐标； 步骤1.3.2：将时间戳对齐的双目相机2D点云与激光雷达2D点云进行并集融合，并通过开源工具转换为激光数据格式；具体如下： 假设进行畸变校正后的激光雷达2D点云集为A，通过空间配准后的激光雷达坐标系下的双目相机2D点云集合为B，将时间戳对齐的A和B通过并集操作进行融合，即得到融合点云数据；然后将融合点云数据转换为激光数据格式。

如需购买、转让、实施、许可或投资类似专利技术，可联系本专利的申请人或专利权人山东科技大学;青岛卓信通智能科技有限公司，其通讯地址为：266590 山东省青岛市黄岛区前湾港路579号；或者联系龙图腾网官方客服，联系龙图腾网可拨打电话0551-65771310或微信搜索“龙图腾网”。