(19)国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 202210321993.7 (22)申请日 2022.03.29 (71)申请人 南京工程学院 地址 211167 江苏省南京市江宁区弘景 大 道1号 (72)发明人 黄家才　汪涛　张铎　汤文俊　 唐安　高芳征　 (74)专利代理 机构 江苏圣典律师事务所 32 237 专利代理师 苏一帜 (51)Int.Cl. G06V 20/00(2022.01) G06V 10/26(2022.01) G06V 10/44(2022.01) G06V 10/762(2022.01) G06V 10/80(2022.01)G06K 9/62(2022.01) (54)发明名称 一种基于双目视觉里程计的足式机器人建 图与导航方法 (57)摘要 本发明实施例公开了一种基于双目视觉里 程计的足式机器人建图与导航方法， 涉及多足移 动机器人技术领域， 在脱离GPS的有效信号范围 后， 还能够实现足式机器人的自主导航。 本发明 包括： 通过双目相机采集图像帧； 提取各个时间 戳下的图像帧的特征点， 并建立相机坐标系， 之 后利用所提取的特征点和所述相机坐标系， 得到 里程计信息； 在所述足式机器人移动的过程中， 获取关键帧； 进一步建立子地图之后， 根据所述 子地图建立地图， 并在所述地图中规划行进路 线， 并按照所述规划行进路线进行导航。 通过所 设计的图像处理的方式， 避免足 式机器人运动过 程中起伏不定的状态所拍摄出的图像效果对计 算里程计产生影响。 权利要求书2页 说明书8页 附图7页 CN 115049910 A 2022.09.13 CN 115049910 A 1.一种基于双目视 觉里程计的足式机器人建图与导 航方法， 其特 征在于， 包括： S1、 通过双目相机采集图像帧； S2、 提取各个时间戳下的图像帧的特征点， 并建立相机坐标系， 之后利用所提取的特征 点和所述相机坐标系， 得到里程计信息； S3、 在所述足式机器人移动的过程中， 获取关键帧； S4、 通过扩展卡尔曼 滤波器对里程计信息进行 滤波估计， 得到里程计修 正数据； S5、 通过激光雷达获取环境感知数据， 所述环境感知数据包括： 所述足式机器人与障碍 物的距离数据； S6、 利用S4所获取的里程计修正数据、 S5所获取的环境感知数据和惯性测量单元采集 的数据， 建立用于底层运动控制的子地图， 其中， 由惯性测量单元采集的数据包括： 加速度 测量数据和角速度测量数据； S7、 根据所述子地图进行优化更新并得到地图。 2.根据权利要求1所述的方法， 其特征在于， 在所述足式机器人上安装有视觉里程计模 块、 底层运动模块和智能控制模块； 所述视觉里程计模块包括了安装在所述足式机器人上的双目相机， 所述双目相机包括 左目相机和右目相机； 所述底层运动模块包括了惯性测量单元(IMU)， 所述惯性测量单元用于获取所述足式 机器人的加速度数据和角速度数据； 所述足式机器人上还安装有激光雷达， 所述激光雷达 用于获取 所述足式机器人与障碍物的距离数据； 所采集的图像帧包括： 所述左目相机采集的左目灰度图像的图像帧和所述右目相机采 集的右目灰度图像的图像帧。 3.根据权利要求1或2所述的方法， 其特征在于， S2中， 所述利用所提取的图像帧的特征 点和所获取的三维坐标点， 得到里程计信息， 包括： 获取特征点在所述相机坐标系下不同时刻的三维坐标点， 并建立旋转平移变换矩阵 (RT， Rotate  Translati on Matrix)； 通过所述旋转平移变换矩阵得到所述双目相机在空间中的运动轨迹， 所述里程计信 息 包括了所述 运动轨迹的参数， 所述 运动轨迹的参数包括旋转方向和平 移距离。 4.根据权利要求1所述的方法， 其特征在于， 在S2中， 所述提取各个时间戳下的图像帧 的特征点， 包括： 对所提取的特征点进行特征匹配， 其中， 若两个特征点的描述子在 向量空间的距离相 近， 则合并为同一个特征点； 一个图像 帧的特征点的信息， 包括了关键点和描述子， 所述关 键点表示特征点在图像中的位置、 方向和尺度信息， 所述描述子表示特征点周围像素 的信 息。 5.根据权利要求4所述的方法， 其特征在于， 在S3中， 所述在所述足式机器人移动的过 程中， 获取关键帧， 包括： 在所述足式机器人移动的过程中， 获取各个时间段内拍摄的图像帧， 并存储为对应每 个时间段的图像帧集 合； 在每个图像帧集合中， 提取各个图像帧中的特征点的描述子信息， 并根据描述子信息 在空间向量中的距离， 筛 选各个图像帧集 合的关键帧。权　利　要　求　书 1/2 页 2 CN 115049910 A 26.根据权利要求4所述的方法， 其特征在于， 所述对所提取的特征点进行特征匹配， 包 括： 提取同一时间戳下的左目灰度图像和右目灰度图像的特 征点， 并进行 特征匹配； 在进行了特征匹配之后， 依次通过三角测量和PNP算法， 获取所述双目相机的视野内拍 摄到的物体在所述相机坐标系下的三维坐标点。 7.根据权利要求1或6所述的方法， 其特征在于， 在获取所述双目相机的视野内拍摄到 的各种物体在所述相机坐标系下的三维坐标点的过程中， 包括： 每次估计当前图像帧相对于上一图像帧的位姿Tcr， 则当前帧相对世界坐标系的坐标即 为： Tcw＝TcrTrw， 其中， 上一帧相对世界坐标系的坐标为Trw。 8.根据权利要求1所述的方法， 其特征在于， 在按照所述规划行进路线进行导航的过程 中， 还包括： 检测所述双目相机当前拍摄的图像帧与之前拍摄的关键帧是否有重合的部分， 若是则 判定所述多足机器人第二次路过 所述地图中的同一个空间点， 并重 定位所述多足机器人； 通过TF算法(TransForm)计算当前 所述多足机器人的位姿。 9.根据权利要求8所述的方法， 其特征在于， 所述对所述子地图进行优化更新并得到地 图， 包括： 所述多足机器人利用所述子地图开始进行移动， 并在移动过程中， 实时检测所述多足 机器人第二次路过 所述地图中的同一个空间点并记录， 作为 修订点； 在每一给 更新周期中， 获取至少一个修 订点后并记录在所述子地图中； 当所述子地图中的修订点停止增加后， 停止更新并将最后 一次更新的子地图作为所述 地图。 10.根据权利要求8所述的方法， 其特征在于， 所述判定所述多足机器人第二次路过所 述地图中的同一个空间点， 包括： 所述双目相机当前拍摄的图像帧与之前拍摄的关键帧， 存在至少10个相同的特征点， 则判定为同一个空间点。权　利　要　求　书 2/2 页 3 CN 115049910 A 3