(19)国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 202210990921.1 (22)申请日 2022.08.18 (71)申请人 南京工程学院 地址 211167 江苏省南京市江宁区弘景 大 道1号 (72)发明人 黄家才　汤文俊　高芳征　朱晓春　 汪涛　王徐寅　薛源　 (74)专利代理 机构 江苏圣典律师事务所 32 237 专利代理师 苏一帜 (51)Int.Cl. G01S 17/86(2020.01) G01S 17/89(2020.01) G01S 17/58(2006.01) G01C 21/00(2006.01) H04N 7/18(2006.01)H04N 13/271(2018.01) (54)发明名称 一种用于四足机器人的自主探索和轨迹监 测的方法 (57)摘要 本发明实施例公开了一种用 于四足机器人 的自主探索和轨迹监测的方法， 涉及移动机器人 技术领域， 能够提高摄像头与激光雷达的耦合处 理能力， 简化图像进行处理上的计算要求， 从而 节约硬件成本。 本发明包括： 通过激光雷达模块 扫描四足机器人周围的空间， 并获取激光数据， 同时， 深度相机拍摄的四足机器人周围的空间的 图像， 之后由嵌入式控制器将 激光数据与 深度相 机拍摄的图像进行同步； 嵌入式控制器通过SLA M 算法建立地图初始数据， 地图初始数据对应四足 机器人周围的空间； 嵌入式控制器中运行探索算 法， 并利用探索算法的输出结果确定四足机器人 周围的空间中的待探索的区域； 嵌入式控制器控 制四足机 器人本体向待探索的区域移动。 权利要求书3页 说明书9页 附图10页 CN 115356743 A 2022.11.18 CN 115356743 A 1.一种用于四足机器人的自主探索和轨迹监测的方法， 其特征在于， 所述方法用于一 种系统上， 所述系统中包括： 四足机器人本体(1)、 嵌入式控制器(2)、 激光雷达模块和深度 相机(5)； 所述方法包括： S1、 通过所述激光雷达模块扫描所述四足机器人周围的空间， 并获取激光数据， 同时， 深度相机(5)拍摄的所述四足机器人周围的空间的图像， 之后由嵌入式控制器(2)将所述激 光数据与深度相机(5)拍摄的图像进行同步； S2、 嵌入式控制器(2)接收所述激光数据， 并通过SLAM算法建立地图初始数据， 所述地 图初始数据对应所述四足机器人周围的空间； S3、 嵌入式控制器(2)中运行探索算法， 并利用探索算法的输出结果确定所述四足机器 人周围的空间中的待探索的区域； S4、 嵌入式控制器(2)控制四足机器人本体(1)向所述待探索的区域移动， 并在移动的 过程中同步更新所述地图初始数据， 直至获取对应所述四足机器人周围的空间的地图数 据。 2.根据权利要求1所述的方法， 其特征在于， 所述激光雷达模块包括了3D激光雷达(3) 和2D激光雷达(4)； 在四足机器人本体(1)上安装有嵌入式控制器(2)、 3D激光雷达(3)、 2D激光雷达(4)和 深度相机(5)； 3D激光雷达(3)通过以太网线连接 到嵌入式控制器(2)； 2D激光雷达(4)和深度相机(5)通过USB串行总线与嵌入式控制器(2)相连； 嵌入式控制 器(2)上安装了Ubuntu操作系统和机器人操作系统ROS(Robot  Operating   System)。 3.根据权利要求1或2所述的方法， 其特征在于， 所述嵌入式控制器(2)将所述激光数据 与深度相机(5)拍摄的图像进行同步， 包括： 嵌入式控制器(2)根据深度相机(5)拍摄的图像， 计算图像的像素点在不同的两帧之间 的位置移动情况； 之后， 进行非线性优化， 降低上述 位置移动情况的计算结果中的误差； 最后， 按照时间戳将深度相机(5)拍摄的图像与所述激光数据进行同步， 得到与所述激 光数据同步的图像数据。 4.根据权利要求3所述的方法， 其特征在于， 所述计算图像的像素点在不同的两帧之间 的位置移动情况， 包括： 计算像素点在时间 间隔Δt的两帧内的位置移动情况， 其中： (x,y)表示像素点的坐标， I表示像素点的亮度， Ix和Iy均为图像梯度,It为时间梯度， u ＝dx/dt和v＝dy/dt表示该像素点光流在水平和垂直方向上的速度分量， n表示在时间间隔权　利　要　求　书 1/3 页 2 CN 115356743 A 2Δt内关键帧的数量， dx表示像素点在x方向上的微分， dt表示像素点在y方向上的微分， Iiy表 示第i帧中y方向图像梯度积分项， Iix表示第i帧中x方向图像梯度积分项。 5.根据权利要求3所述的方法， 其特征在于， 在进行非线性优化的过程中， 采用的非线 性优化的目标函数为： 其中， 表示滑动窗口内的所有状态量， rp表示边缘化残差， Hp为图像信息矩阵舒尔补 变换后形成的新信息矩阵， 其中p 意为prior， rC是滑动窗口中特征点的视觉重 投影误差， ∑ ρ 表示曲面积分， cj表示相机第j帧拍摄的图像， 表示第l个特征在第j张图像中的像素通 过相机内参反向投影到单位球面上的三维坐标， 表示通过第l个特征在前一张图像中 的像素推算出的该特征在第 j张图像中的估计值， l表 示特征点编号为正整 数， j表示帧编号 为正整数， C表示camera相机 。 6.根据权利要求3所述的方法， 其特征在于， 所述按照时间戳将深度相机(5)拍摄的图 像与所述激光数据进行同步的过程中， 包括： 进行深度上的配准， 其中， 配准的目标函数为： Mopt＝argminM∑((M*si‑di)*ni)2    (3) 两个收敛点分别为源点si和目标点di， si和di分别在源面和目标面ni上， M是由旋转矩阵 rotation(R)和平 移矩阵shift(T)组成的4*4的3D刚体 变换矩阵。 7.根据权利 要求3所述的方法， 其特征在于， S1包括： 当四足机器人本体(1)处于室内空 间时， 通过2D 激光雷达(4)扫描室内空间， 并获取所述四足机器人周围的空间的二 维激光数 据； S2包括： 嵌入式控制器(2)接收所述二维激光数据， 并运行2D激光SLAM算法gmapping， 建立局部的占据栅格地图作为所述 地图初始数据； S3包括： 嵌入式控制器(2)中运行RRT(Rapid ‑exploration  Random Tree， 快速扩展随 机树)探索算法并对图像数据进行处理， 其中， 当节点生长到了所述图像数据中的未知区 域， 则将该节点判定为 边界点， 并进一 步将边界点标注在所述 地图初始数据上 形成边界； 所述待探索的区域包括 边界以外的区域。 8.根据权利要求7 所述的方法， 其特 征在于， 还 包括： 嵌入式控制器(2)加载并运行ROS导航栈(Navigation  Stack)中的核心功能包move_ base。 9.根据权利要求3所述的方法， 其特 征在于， 还 包括： S1包括： 当四足机器人本体(1)处于室外空间时， 通过3D激光雷达(3)扫描室外空间， 并 获取所述四足机器人周围的空间的三维激光数据； S2包括： 嵌入式控制器(2)接收所述三维激光数据， 并3D激光SLAM算法LIO_SAM(Lidar   Inertial Odometry Smoothing and Mapping)， 建立 点云地图作为所述 地图初始数据； S3包括： 嵌入式控制器(2)中运行用于图像处理的仿射变换模型和霍夫直线检测算法 模型， 并利用所述图像数据确定朝向所述待探索的区域的道路 并确定道路两边及道路中间权　利　要　求　书 2/3 页 3 CN 115356743 A 3