(19)国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 202210469587.5 (22)申请日 2022.04.28 (71)申请人 杭州电子科技大 学 地址 310018 浙江省杭州市下沙高教园区 (72)发明人 金建波　林志赟　王博　韩志敏　 (74)专利代理 机构 杭州君度专利代理事务所 (特殊普通 合伙) 33240 专利代理师 杨舟涛 (51)Int.Cl. G06T 7/73(2017.01) G06T 7/80(2017.01) (54)发明名称 一种基于视觉特征和3D激光的移动机器人 重定位方法及装置 (57)摘要 本发明公开了一种基于视觉特征和3D激光 的移动机器人重定位及装置的方法， 该方法首先 需要进行环境建图和Apriltag标签世界坐标自 动标定； 在移动机器人丢失定位时， 开启左、 右相 机检测标签功能， 在检测到标签后停止移动； 获 取移动机器人与标签之间的相对位姿， 并在一段 时间内做滤波处理， 保证位姿数据的稳定性； 根 据标定所得标签世界坐标恢复机器人所处世界 坐标， 实现初步重定位； 以该世界坐标为中心进 行定向范围搜索， 不断将扫描得到的3D点 云帧数 据与现存子图进行匹配， 实现精准重定位。 从而 恢复移动机器人利用多线激光雷达SLAM定位能 力， 保证移动机器人在实际运行过程能够以较小 的代价恢复定位能力。 权利要求书3页 说明书9页 附图3页 CN 114862953 A 2022.08.05 CN 114862953 A 1.一种基于视觉特征和3D激光的移动机器人重定位方法， 其特征在于， 该方法包括如 下步骤： 步骤1、 利用多线激光雷达和IMU传感器对实际场景进行3D层面的建图， 在地图构建的 过程中， 获取移动机器人相对于Aprilt ag标签的位姿， 根据移动机器人世界坐标转换得到 Apriltag标签世界坐标信息， 同时记录标签编号， 完成对Apri ltag标签的世界坐标的标定； 步骤2、 在移动机器人丢失自身定位时， 通过位于移动机器人上的左右相机不断获取移 动机器人左侧与右侧图像， 在任一相机的视野范围内检测到Apri ltag标签后即停止移动； 步骤3、 获取移动机器人相对Apri ltag标签的若干个位姿数据， 记为 集合S： 其中xi,yi,zi分别表示X,Y,Z轴方向上相对Apriltag标签的位移变换， φi分别表示 X,Z轴方向上相对Apri ltag标签的角度变换， n表示获取的位姿总数； 步骤4、 按照获取的Apriltag标签编号查找步骤1中标定得到的标签世界坐标， 记为 基于步骤3中得到的相对Apriltag标签的位姿数据， 进行移动机器人 的世界坐标推算， 实现移动机器人的初步重定位， 记移动机器人初步重定位结果为 步骤5、 将W2的坐标结合多线激光点云信息进 一步推算， 具体为： 首先对移动机器人上3D 激光的当前帧点云数据进 行体素滤波和噪声去除， 将当前移动机器人所处空间切分成固定 边长的立方格， 形成栅格空间， 依据3D激光扫描得到的点云数据中的点是否在立方格内将 立方格的中心值设为1和0， 立方格内其余点的值则通过相邻立方格的中心值来计算， 值的 大小表示障碍物的概率； 然后以坐标W2为中心， 以参数rs为半径并结合步骤4中W2相对于W1 的角度信息， 进行定向范围搜索； 对当前帧点云数据按照相等间隔的位移和角度进行3D层 面的变换， 并依据变换后的点在栅格空间中的坐标获取概率值， 计算概率值累加和， 以累加 和 为 最 大 值 时 对 应 的 间 隔 变 换 后的 位 姿 数 据 作 为 精 准 重 定 位 的 结 果 ， 即 2.根据权利要求1所述的一种基于视觉特征和3D激光的移动机器人重定位方法， 其特 征在于： 步骤3中， 将 记为pi， 将该集合去除异常值后采用均值滤波处理 得到均值 减少局部异常值对整体判定结果的影响。 3.根据权利要求1所述的一种基于视觉特征和3D激光的移动机器人重定位方法， 其特 征在于： 所述 步骤1中对Apri ltag标签的世界坐标的标定， 具体过程 为： 1‑1、 将移动机器人开始建图的点作为世界坐标系的原点， 在移动过程中， 将移动机器 人上的3D激光扫描得到的点云数据与之前扫描过程中已获得点云数据进 行匹配， 采用子图 集策略， 以N帧点云数据建立子图， 并以每N帧移动机器人位姿构 造节点， 在移动机器人不断 移动的过程中， 不断计算节点间的变换过程约束以及当前帧与相邻节点间的约束， 将约束 作为节点间的边， 并进行非线性优化， 将其 转化为图优化问题， 构建优化 函数如下： 权　利　要　求　书 1/3 页 2 CN 114862953 A 2上式中， 分别表示当前扫描帧具有约束关系的子图集 合和已获取的扫描帧的位姿集合， n1,n2分别表示与当前扫描帧具有约束关系的子图和在当 前扫描帧之前扫描帧的数量， Δij,Ωij分别表示扫描帧j相对子图i的相对位姿和协方差矩 阵， E(·)则表示关于 Ωij,Δij的非线性误差函数， 采用Levenberg ‑Marquardt算 法求 解最优子图位姿pm和最优扫描帧位姿ps， 结合移动机器人初始位姿转换为当前时刻下的移 动机器人世界坐标； 1‑2、 在构建地图的过程中， 左右相机不断获取 图像进行检测， 当检测到Apriltag标签 时， 用0和1区分左右相机记录的数据， 记录下此刻检测到的标签号和根据移动机器人世界 坐标转换 得到Apri ltag标签的世界坐标并保存以便后续使用。 4.根据权利要求1所述的一种基于视觉特征和3D激光的移动机器人重定位方法， 其特 征在于： 所述 步骤4中左右相机检测标签并获取相对位姿数据来进行初步重 定位， 具体为： 4‑1、 按照实际移动机器人移动过程中左右相机检测到标签与否分类， 情形可分为两 类， 一类是单侧， 另一类是双侧； 单侧是指只有左相机或者右相机检测到标签， 则进行步骤 4‑2， 双侧则是指左右相机均检测到标签， 则进行步骤4 ‑3。 4‑2、 在单侧情况下， 当相机范围内检测到多个标签时， 记检测到的标签编号为di,i＝ 1,...,m， 其中m表示检测标签数量； 根据标签编号查找对应标定所得标签世界坐标为 取此时检测所得移动机器人相对标签的位姿数据中的相对位移变 换信息 角度信息 和 得到移动机器人初步的世界坐标 为 角度 4‑3、 在双侧情况下， 记左侧检测所得标签编号为li,i＝1,...,ml， 其中ml表示左侧检测 标签数量， 右侧为rj,j＝1,...,mr， 其中mr表示右侧检测标签数量， 根据标签编号查找对应 标签世界坐标为 取此时检测所得移动机器 人 相 对 标 签 的 位 姿 数 据 中 的 相 对 位 移 变 换 信 息 角度信息 和 得到移动机器人初步的世界 坐标为 角度 5.根据权利要求4所述的一种基于视觉特征和3D激光的移动机器人重定位方法， 其特 征在于： 所述 步骤5结合多 线激光雷达点云数据匹配进行精准重 定位， 具体为： 5‑1、 以步骤4得到的移动机器人世界坐标 为球心， 参数rs为半径， 角度 和 建立球锥， 参数rs可根据实际环境进行自主设置； 在球锥范围内按照设定的若干个 等间隔 对3D激光当前帧扫描得到的点云数据进行3D空间上的变 换， 记变换 前的点云数据为C， 变换后的点云数据为C ′；权　利　要　求　书 2/3 页 3 CN 114862953 A 3