(19)国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 202210906761.8 (22)申请日 2022.07.29 (71)申请人 浙江工业大 学 地址 310014 浙江省杭州市拱 墅区潮王路 18号 (72)发明人 欧县华　董翔宇　吴鸿志　游仲南　 陈强　何熊熊　 (74)专利代理 机构 浙江千克知识产权代理有限 公司 33246 专利代理师 冷红梅 (51)Int.Cl. G06T 19/00(2011.01) G06T 17/05(2011.01) G06T 7/33(2017.01) G06V 10/26(2022.01)G06V 10/764(2022.01) G06V 10/80(2022.01) (54)发明名称 一种用于医院场景的轮式机器人语义SLAM 方法 (57)摘要 一种用于医院场景的轮式机器人语义SLAM 方法， 涉及机器人定位技术领域。 本发明通过 RGB‑D相机采集医院环境的RGB图像特征和Depth 深度信息， 利用医院场景中的固有语义特征构建 地图并实现机器人的实时定位， 如护士站、 导向 标识、 楼牌等。 具体分为SLAM模式和仅定位模式 两个模式， 仅定位模式为基于SLA M模式建立的医 院语义地图的轻量级定位模式。 利用RGB ‑D相机 和惯性测量单元(IMU)融合， 构建局部视觉语义 地图和提高仅定位模式下的定位精度。 本发明具 有较高精度的局部地图定位和位姿 估计。 权利要求书3页 说明书6页 附图1页 CN 115249300 A 2022.10.28 CN 115249300 A 1.一种用于医院场景的轮式机器人语义SLAM方法， 其特征在于： 所述语义SLAM方法包 括以下步骤： 步骤(1)RGB ‑D相机输入图像数据， 通过图像分割网络进行RGB图像的像素级语义分割， 提取图像 语义特征； 步骤(2)利用步骤(1)提取的医 院环境像素级语义特征融合Depth图像直接生成地图点 云， 并利用惯性测量单 元(IMU)转换点云坐标， 构建局部语义 地图； 步骤(3)基于NDT算法不断匹配两个局部地图， 得到两个局部地 图的相对位姿， 通过特 殊欧式群SE(3)矫 正当前局部地图的位姿， 并和候选闭合局部地图融合实现闭环； 步骤(4)将闭合后的地图进行全局BA优化， 构建全局语义 地图； 仅定位模式为一个轻量级的局部地图跟踪模式， 即不执行回环检测和全局优化的算 法， 仅进行局部地图的匹配以实现移动机器人的定位； 步骤(5)与SLAM模式的局部地图构建相似， 构建局部语义 地图； 步骤(6)将步骤(5)构建的局部语义地图与基于SLAM模式构建的医院环境全局语义地 图进行匹配来估计当前机器人的位姿。 2.根据权利要求1所述的用于医院场景的轮 式机器人语义SLAM方法， 其特征在于： 所述 步骤(1)的过程如下： 通过SegNet像素级语义分割， 将相机传入的RGB图像分割成不同类别， 该网络通过医 院 拍摄的图像进行专门训练， 将像素划分为指路标识、 护士站、 药房、 病房、 电梯、 楼道和 墙壁 等具有医院特 征的像素类别。 3.根据权利要求1或2所述的用于医院场景的轮式机器人语义SLAM方法， 其特征在于： 所述步骤(2)的过程如下： 通过如下公式将步骤(1)分割RGB图像得到的像素级语义分割图像与Depth图像融合， 直接生成三维空间的地图点云： 其中， 是点云坐标系， 是图像坐标系， fx， fy是相机内参， D为深度值； 生成的三维空间地图点利用机器人的惯性测量单元IMU进行坐标转换， 从相机坐标系 转移到世界坐标系下， 其中， [R0 t0]是惯性测量单元(IMU)的位姿， 是这些点在世界坐标系下的坐标，权　利　要　求　书 1/3 页 2 CN 115249300 A 2是这些点在相机坐标系下的空间坐标， 这些点聚集成一个局部地图。 4.根据权利要求3所述的用于医院场景的轮 式机器人语义SLAM方法， 其特征在于： 所述 步骤(3)的过程如下： 利用NDT点云配准算法不断匹配局部点云， 直到匹配到与当前地图点位姿相似的点云； 并利用特殊欧式群SE(3)矫正当前局部地图的位姿， 和候选闭合局部地图融合实现环路闭 合； 把局部地图点云分割成一个个网格， 计算每个网格中点的数量， 满足一定的阈值， 则并 根据均值和方差将各部分中的点云 分布拟合 为正态分布， 均值和方差的计算方法为： 其中， 表示局部点云的均值向量， 表示三维列向量， n表示落在每个网格内的点的数 量， i表示 n个点中的第i个点， 每一个网格中的概 率密度函数为： 其中， p(x)表示每一个 网格中的概率密度 函数， ∑表示点云分布的方差， D/2表示x服从 D/2元正态分布； 通过以上方法计算局部地图点云中点的响应概率， 并以此作为目标函数， 求解出使该 目标函数 取最大值的位姿变换关系。 待求的优化目标函数为： 其中： x′＝Rx+t                           (7) 式中， x’代表点云中的点的坐标， R表示旋转矩阵， t 表示平移向量； 得到两片点云的位姿变换关系后通过特殊欧式群SE(3)矫正当前局部地 图的位姿， 这 样就能使闭环的两片点云对齐实现闭环。 5.根据权利要求4所述的用于医院场景的轮 式机器人语义SLAM方法， 其特征在于： 所述 步骤(4)的过程如下： 利用全局BA来优化全局地图， 在上述步骤得到的闭合地图中， 节点是每个局部地图对 应的相机位姿， 它包含三个轴旋转r＝[rx ry rz]T和平移t＝[tx ty tz]T。 位姿图优化 可以表 示为以下代价 函数： 其中， χ*＝[r0,t0,...,rt,tt]T， 即所有局部地图的位姿， 是局部地图t和t+1之间的 相对位姿， L是闭环对的集合， 是闭环帧i和j之间的相对位姿， 函数f( ·)计算两个局 部 地图之间的相对位姿； 通过L‑M法进行优化， 在经过位姿优化后， 我们通过更新的位姿将局部地图堆叠在一 起， 形成了一个全局语义 地图。权　利　要　求　书 2/3 页 3 CN 115249300 A 3