(19)国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 202210805209.X (22)申请日 2022.07.08 (71)申请人 同济大学 地址 200092 上海市杨 浦区四平路1239号 (72)发明人 刘成菊　陈启军　侯哲　石文博　 (74)专利代理 机构 上海科盛知识产权代理有限 公司 312 25 专利代理师 杨宏泰 (51)Int.Cl. G06T 17/05(2011.01) G06T 7/73(2017.01) G06T 7/80(2017.01) (54)发明名称 一种面向单调无纹理大场景的多传感器融 合SLAM方法 (57)摘要 本发明涉及一种面向单调无纹理大场景的 多传感器融合SLAM 方法， 包括以下步骤： S1、 获取 3D激光雷达、 IMU传感器、 深度相机和轮式编码器 的信息； S2、 将深度相机采集到的每一帧图像通 过图像金字塔提取ORB特征点， 并实现特征点之 间的匹配， 根据匹配结果获取深度相机的位姿估 计值； S3、 监听IMU与轮式编码器的信息进行位姿 融合， 获取更新后的位姿矩阵和协方差矩阵； S4、 将IMU与轮式编码器融合后的位姿与 深度相机的 位姿估计值进一步融合； S5、 通过将3D激光雷达 扫描到的点云进行分类匹配， 得到去畸变点云； S6、 对去畸变点云进行进一步分类匹配， 并构建 全局地图。 与现有技术相比， 本发明具有提高系 统鲁棒性、 提高运 算速度、 部署简单等优点。 权利要求书3页 说明书6页 附图2页 CN 115272596 A 2022.11.01 CN 115272596 A 1.一种面向单调无纹理大场景的多传感器融合SLAM方法， 用以减少在单调无纹理大场 景下雷达定位产生的漂移， 其特 征在于， 包括以下步骤： S1、 获取3D激光雷达、 IMU传感器、 深度相机和轮式编码器的信息； S2、 将深度相机采集到的每一帧图像通过图像金字塔提取ORB 特征点， 并实现特征点之 间的匹配， 根据匹配结果获取深度相机的位姿估计值； S3、 监听IMU与轮式编码器的信息， 通过扩展卡尔曼滤波进行位姿融合， 获取更新后的 位姿矩阵和协方差矩阵； S4、 将IMU与轮式编码器 融合后的位姿与深度相机的位姿估计值进一步融合， 并根据视 觉里程计提取的ORB特 征点数量确定融合的动态权 重； S5、 通过将3D激光雷达扫描到的点云进行分类匹配， 再通过最小二乘计算出雷达当前 位姿并得到去畸变点云； S6、 对去畸变点云进行进一 步分类匹配， 得到准确的位姿， 并构建全局地图。 2.根据权利要求1所述的一种面向单调无纹理大场景的多传感器融合SLAM方法， 其特 征在于， 所述的步骤S1具体为： 根据3D激光雷达的实际参数和应用要求获得激光点云并进行滤波， 将深度相机的像素 信息和深度信息分为两个通道进行提取。 3.根据权利要求1所述的一种面向单调无纹理大场景的多传感器融合SLAM方法， 其特 征在于， 所述的步骤S2具体为： 对于ORB特征点中的FAST角点， 首先采用图像金字塔进行角点的提取以保证尺度特性， 并在提取过程中保证特征点在每一层金字塔上均匀分布以提高系统的鲁棒性和稳定性， 然 后通过灰度质心法获取角点方向， 计算特征点的BRIEF描述子并通过Hamming距离进行匹 配， 最后采用光束平差法， 构建相机位姿关于重投影误差的最小二乘问题， 使用列文伯格 ‑ 马尔夸特法迭代求 解相机位姿。 4.根据权利要求3所述的一种面向单调无纹理大场景的多传感器融合SLAM方法， 其特 征在于， 采用图像金字塔提取角点的计算式为： 其中， Nm为金字塔第m层所需提取的特征点数量， N为所需提取特征点的总数， s为金字塔 相邻层之间的缩放因子， M为金字塔总层数。 5.根据权利要求3所述的一种面向单调无纹理大场景的多传感器融合SLAM方法， 其特 征在于， 为克服无 纹理环境中难以提取到特征点的影响， 提出指标δ 以衡量视觉里程计的准 确度， 则指标δ 的表达式为： 其中， n_featurepo ints为特征点提取的最大 数量， n为提取到的特 征点数量。 6.根据权利要求1所述的一种面向单调无纹理大场景的多传感器融合SLAM方法， 其特 征在于， 所述的步骤S3具体为：权　利　要　求　书 1/3 页 2 CN 115272596 A 2首先启动ROS节点并监听轮式编码器和IMU发布的信息， 构建扩展卡尔曼滤波器， 进行 状态更新， 以获取 的轮式编码器信息作为观测 量和协方差矩阵并进行状态更新， 并发送更 新后的状态变量和协方差矩阵； 然后获取IMU信息作为观测量和协方差矩阵并根据轮式编 码器发送和状态变量和协方差矩阵进 行状态更新； 最后发布更新后的状态变量和协方差矩 阵作为融合后的IMU里程计信息 。 7.根据权利要求1所述的一种面向单调无纹理大场景的多传感器融合SLAM方法， 其特 征在于， 所述的步骤S4中， 在将视觉里程计与机器人自身运动里程计融合时， 根据ORB特征 点数量确定融合的动态权 重， 则有： 当视觉里程计提取到的特 征点数量 n＞80时： 当视觉里程计提取到的特 征点数量 n＜30时： 当视觉里程计提取到的特 征点数量3 0＜n＜80时： 其中， t为融合后的位置向量， q为融合后的姿态四元数， tcamera为相机的位置向量， qcamera为相机的姿态四元数， timu为IMU里程计的位置向量， qimu为里程计的姿态四元数， 进而 根据动态权 重输出融合后的位姿。 8.根据权利要求1所述的一种面向单调无纹理大场景的多传感器融合SLAM方法， 其特 征在于， 所述的步骤S5中， 通过计算每个特征点的平面光滑度， 将其分类为平面点和边缘 点， 通过计算点到线距离匹配边缘点， 通过计算点到面距离匹配平面点， 得到雷达 关于距离 的最小二乘函数， 进 行迭代优化， 得到雷达位姿， 并通过将点云数据重投影到初始时刻消除 雷达运动产生的点云畸变。 9.根据权利要求8所述的一种面向单调无纹理大场景的多传感器融合SLAM方法， 其特 征在于， 所述的平面 光滑度c的表达式为： 其中， 雷达坐标系被设定为L， Lk表示第k个周期时的雷达观测坐标系， XL (k,i)为第k个周 期扫描到的第i个点云， S为当前点所在的集 合。 10.根据权利要求1所述的一种面向单调无纹理大场景的多传感器融合SLAM方法， 其特 征在于， 所述的步骤S 6中， 特征点的进一步分类匹配过程与步骤S5相同， 通过在地图上存储 点云到一个立方体空间中， 提取出与当前点云相交的特征点， 计算特征点周围集合的协方 差矩阵， 并计算协方差矩阵的特征值和特征向量， 根据特征值和特征向量判断点为平面点权　利　要　求　书 2/3 页 3 CN 115272596 A 3