煤矿井下移动机器人深度视觉自主导航研究.pdf

第 45 卷第 6 期煤 炭 学 报Vol. 45 No. 6 2020 年6 月JOURNAL OF CHINA COAL SOCIETYJun. 2020 移动阅读 马宏伟,王岩,杨林. 煤矿井下移动机器人深度视觉自主导航研究[J]. 煤炭学报,2020,4562193-2206. doi 10. 13225/ j. cnki. jccs. ZN20. 0214 MA Hongwei,WANG Yan,YANG Lin. Research on depth vision based mobile robot autonomous navigation in under- ground coal mine[J]. Journal of China Coal Society,2020,4562193-2206. doi10. 13225/ j. cnki. jccs. ZN20. 0214 煤矿井下移动机器人深度视觉自主导航研究 马宏伟1,2,王 岩1,2,杨 林1,2 1. 西安科技大学 机械工程学院,陕西 西安 710054; 2. 陕西省矿山机电装备智能监测重点实验室,陕西 西安 710054 摘 要煤矿井下移动机器人是煤矿机器人的主力军,煤矿井下移动机器人的自主导航是其研究的 难点和热点。 目前,煤矿井下移动机器人自主导航所必须的三维环境数据库尚未形成,尤其是制作 高分辨率、多信息融合的煤矿井下高精度地图还处于研究阶段。 为了有效解决煤矿井下移动机器 人自主导航问题,构建了基于深度相机的机器视觉系统,提出了一种基于深度视觉的导航方法,自 主导航过程分为地图创建与自主运行两个阶段。 在地图创建中① 对深度视觉数据进行特征提取 与匹配,利用 10 组煤矿井下真实视频截图,对比测试 5 种特征提取与匹配组合算法,结果表明 SURFSURFFLANN 与 GFTTBRIEFBF 算法能够在煤矿井下获得良好匹配结果;② 建立煤矿井 下移动机器人深度视觉定位与建图问题的捆集调整迭代最近点图模型Iterative Closest Points Bun- dle Adjustment,ICP BA;③ 通过图优化方式估计当前观测下的最优位姿与环境路标点坐标。 在实 验室场景中利用提出的 ICP-BA 图优化算法,建立了包含关键位姿与三维环境点的原始点云地图。 在自主运行阶段① 通过八叉树数据结构,将点云地图转化为移动机器人运动规划可使用的 Octo- map 导航地图,实验结果表明,Octomap 导航地图分辨率可调、系统资源占用低、索引效率高;② 使 用三维到二维映射的视觉图匹配 PNPPerspective N Points方法进行实时在线重定位;③ 基于图 搜索的 A∗A Star路径规划作为轨迹规划初值,自定义最小化能量损失泛函为最小化加加速度 的变化率Minimum-Snap求解 2 次规划问题,生成用于煤矿井下移动机器人运动执行的轨迹。 在 Matlab 开发环境中设计随机导航地图,生成时间分配、位置、速度、加速度、加加速度的最优轨迹规 划结果,验证了运动规划算法的正确性。 通过理论分析和实验验证,表明笔者提出的煤矿井下移动 机器人深度视觉自主导航方法的有效性。 关键词煤矿井下移动机器人;自主导航;深度视觉;地图创建;重定位;运动规划 中图分类号TP242;TD67 文献标志码A 文章编号0253-9993202006-2193-14 收稿日期2020-02-18 修回日期2020-05-09 责任编辑常 琛 基金项目国家自然科学基金面上资助项目50674075,51975468;陕西省科技统筹创新工程计划资助项目2013KTCL01-02 作者简介马宏伟1957,男,陕西兴平人,教授。 Tel029-85583159,E-mail287797063 qq． com 通讯作者王 岩1991,男,陕西西安人,博士研究生。 Tel029-85583159,E-mail1245994265 qq． com Research on depth vision based mobile robot autonomous navigation in underground coal mine MA Hongwei1,2,WANG Yan1,2,YANG Lin1,2 1. School of Mechanical Engineering,Xi’an University of Science and Technology,Xi’an 710054,China; 2. Shaanxi Key Laboratory for Intelligent Monito- ring of Mine Mechanical and Electrical Equipment,Xi’an 710054,China AbstractUnderground mobile robot is the main force of coal mine robot. Autonomous navigation is the difficulty and the hotspot task in research. Currently three-dimensional environment database which is necessary for au-tonomous 煤 炭 学 报 2020 年第 45 卷 navigation of mobile robots in coal mines has not been fully developed. In particularly,the production of high-resolu- tion,multi-ination fused,high-precision maps of underground coal mine is still under investigation. In order to solve the problem of autonomous navigation of mobile robot in underground coal mine,a machine vision system based on depth camera was built,and a navigation based on depth vision was proposed. The autonomous navigation process have two stages map creation and autonomous operation. In the stage of map creation,① depth vision data was used for feature extracting and matching. Five depth visual feature extraction and matching algorithms were com- pared and tested in ten groups underground coal mine images. Result shows that the algorithm SURFSURFFLANN and GFTTBRIEFBF have better perance. ② An Iterative Closest Points Bundle Adjustment model for depth vi- sion based localization and mapping problem of mobile robot in underground coal mine was established. ③ The optimal camera poses and landmarks under current observation were estimated by graph optimization. A laboratory scene origi- nal point cloud map containing key poses was established by using the proposed ICP-BA algorithm. In the stage of au- tonomous operation,① the point cloud map was transed into an octree data structure Octomap which can be used for mobile robot motion planning. Compared with the original point cloud map,Octomap had adjustable resolution,low system resource occupancy and high indexing efficiency. ② The PNP of 3 d to 2 d projecting was used for re- al-time online relocation. ③ On these basis,A∗ A Star path planning based on graph search was taken as the ini- tial value of trajectory planning,and the customized minimum-energy loss functional minimum-snap was used to solve the quadratic programming problem to generate the trajectory for motion controller. Random navigation map was designed in Matlab development environment,the optimal trajectory planning results of time allocations,positions,ve- locities,accelerations and jerks were generated,which verified the correctness of the proposed motion planning algo- rithm. Through the above theoretical analysis and experimental verification,the effectiveness of the proposed depth vi- sion autonomous navigation for underground coal mine mobile robot was proved. Key wordsunderground coal mine mobile robot;autonomous navigation;depth vision;map creation,relocation;motion planning 自主导航能力是煤矿井下移动机器人智能化程 度评价的重要指标,国家煤矿安监局制定的煤矿机 器人重点研发目录中,对 38 种煤矿机器人均提出 了相应的自主导航功能需求[1]。 随着计算机技术、 传感器技术、机器人技术等的快速发展,已有一些具 备自主导航能力的移动机器人相关研究成果及产品 应用[2]。 但针对煤矿井下这一特殊环境的移动机器 人自主导航问题讨论较少,仍存在诸多关键问题亟待 研究与解决。 煤矿井下移动机器人的自主导航就是要解决 井下 定 位 我 在 哪 [3]、 导 航 地 图 创 建 要 去 哪 [4]以及基于地图的运动规划怎么去[5]这 3 个关键技术问题,其中定位与地图创建存在相辅相 成关系,即高精度的定位需要基于精确的地图,创 建精确的地图需要基于高精度的定位,自主导航问 题中的定位与地图构建被描述为同步定位与建图 问题simultaneous localization and mapping [6]。 目 前移动机器人自主导航定位与建图问题中广泛使 用的是基于二维激光雷达的方法,通过扫描匹配生 成环境平面占据栅格地图,并基于该地图执行粒子 滤波算法实现二维实时定位[7]。 卡内基梅隆大学 领域机器人中心,在 20022006 年的地面移动式 土拨鼠groundhog机器人研发项目中,利用该方法 对 3 个废弃地下矿井实现了地图创建,同时采用 A ∗搜索策略实现了一定程度的自主探测功能,是首 个真实地下环境中实现半自主导航的项目[8]。 对 于无人机等非地面移动式机器人,上述方法无法满 足其三维空间下运动规划的要求。 三维激光雷达 可以高速精确的捕获环境中的距离信息,近年来随 着无人车领域的大力推广得到了大量应用,同时也 是煤矿井下三维地理信息系统GIS构建的主力传 感器[9]。 Clickmox 公司与 Inkonova 公司使用四旋 翼无人机平台,搭载三维激光雷达,可通过遥操作 无人机扫描矿井下巷道环境,并离线处理获得高精 度井下地图[10]。 但激光数据类型单一,可视化的三 维点云数据由于只具有距离信息与三维形貌,缺乏 环境纹理,因此通过三维激光雷达构建环境数据 后,处理时的环境信息标注与分类等工作还需耗费 大量的人力、算力。 面对煤矿井下移动机器人自主 导航问题,其任务执行过程中的临场性、实时性应 当处于更高的优先考虑级别。 移动机器人视觉导航是近年来的研究热点,内 4912 第 6 期马宏伟等煤矿井下移动机器人深度视觉自主导航研究 华达大学的自主机器人实验室,在 2019 年 DAR- PA SubT 地下挑战赛中,由无人机搭载单目相机,验 证了 Rovio 视觉定位算法,并使用局部加全局的运 动规划,完成了巡航任务[11]。 视觉传感器种类众 多,大致分为单目相机、多目相机、RGB-D 相机以 及新兴的事件相机event camera。 其中 RGB-D 相机又被称为深度相机,在获取视觉数据的同时使 用 TOFTime of Flight或者结构光原理获得环境中 的深度距离数据[12],因此省去了单双目相机的三角 化处理步骤,节约了计算量。 深度相机在地面开阔 环境下由于强光照、观测物距离较远,会出现精度 降低、数据失效等问题,而煤矿井下巷道环境较为 狭窄,光照较暗,Kinect v1 等结构光原理深度相机 甚至可以在完全黑暗的环境下使用。 同时,相对于 三维激光雷达,视觉的信息更加丰富,移动机器人 搭载视觉传感器任务执行后得到的结果更利于后 续决策的制定与实施。 综上所述,本文提出一种煤矿井下移动机器人深 度视觉自主导航方法。 1 深度视觉自主导航方法 该方法由机器人搭载 RGB-D 相机作为深度视 觉数据采集传感器,通过地图创建与自主运行 2 个阶 段实现自主导航任务。 地图创建阶段通过对深度视 觉数据的特征提取与匹配构建深度视觉数据关联 ICP 模型[13],利用图优化理论[14]构建与求解 ICP - BA 图模型的煤矿井下移动机器人深度视觉定位与建 图问题。 自主运行阶段使用 Octomap[15]作为机器人 导航地图,利用 PNP 原理[16]实时重定位。 在此基础 上,自主导航运动规划采用 A∗路径规划[17],Mini- mum-Snap[18]轨迹生成用于移动机器人的运动执行。 算法流程如图 1 所示。 图 1 煤矿井下移动机器人深度视觉自主导航流程 Fig． 1 Process of the depth vision based mobile robot autonomous navigation 2 深度视觉数据特征提取与匹配 传统 RGB 相机只能在一帧数据中获得一幅彩色 图像,而 RGB-D 相机在采集彩色图像的同时,可以 得到同尺寸的由 16 位整数记录的深度图。 图 2a 所示为本文所使用 Kinect v1 相机所采集的一帧深度 视觉数据,图 2b为彩色图,图 2c为深度图。 通 过深度图数据的读取,在彩色镜头标定后,可将单帧 RGB-D 深度视觉数据进行三维投影显示,如图 2d 所示。 图 2 深度视觉传感器介绍 Fig． 2 Introduction to depth vision sensor 深度视觉数据的图像处理是通过对采集到的视 觉图像进行相应特征计算并构建数据关联,常用特征 点 法 Feature-baseds [19] 与 直 接 法 Direct-s [20]。 直接法基于光照不变假设,面 对煤矿井下的特殊照明环境,特征点法通过选取合适 的特征描述子,可以忽略光照变化的影响。 另外,特 5912 煤 炭 学 报 2020 年第 45 卷 征点法还具有计算量更少、更易获得良好重定位效果 的优势。 因此,本文采用特征点法对深度视觉数据中 的连续两帧二维彩色图像进行图像处理。 世界坐标系是相机初始化成功后,与获取的第 1 帧图像固连的相机位姿定义的坐标系。 图像特征点 的提取与匹配就是找到三维世界坐标系下的环境特 征,以及在不同图像平面坐标系下的投影表达及其共 视关系。 如图 3 所示,世界坐标系下的立方体,在 k 时刻与 k1 时刻由相机捕获到图像,投影于其对应 图像平面。 图像平面中红点代表提取到的图像特征 点,蓝色连线代表两帧图像特征点的匹配关系。 图 3 图像特征点匹配模型 Fig． 3 Image feature point matching model 图像的特征点由提取到的关键点与计算出的描 述子两部分组成。 最直观的特征点类型是角点,角点 的关键点提取算法有 Harris,Fast,GFTT 等,这些基于 角点的关键点提取算法具有计算速度快等优势,但当 图像发生旋转、位移等变化时,角点的稳定性较差。 后续的研究者设计出具有更加稳定局部特征的特征 点类型,经典的方法有 SIFT,SURF,KAZE,Orb 等。 描述子是将提取到的关键点进行编码计算并存储为 一定的数据结构,广泛使用的描述子方法有 Freak, Brief,Brisk 等。 图像特征的匹配就是根据计算出的 描述子执行相似度计算,得分相近的描述子认为是同 一环境特征在不同图像下的投影点,常用的匹配方法 有 Brute-ForceBF,FLANN 等[21-23]。 图像特征点的提取与匹配方法众多,但对于不同 的环境特点、计算机算力、实时性需求等客观因素,需 要综合考量选择适宜的方案。 针对煤矿井下的环境 特点,本文设计如下实验截取煤矿井下拍摄的视频 中 10 组不同场景、相邻时刻图像图 4 中前两列,图 像分辨率为 960540,对 5 种图像特征提取与匹配 方法GFTTBRISKBF;GFTTBRIEFBF;ORB BRIEF BF; SIFT SIFT FLANN; SURF SURF FLANN进行了综合比较。 图 4 第 3 列展示的是本 文实验中 SURFSURFFLANN 匹配结果 为了更好的对比几种特征提取及匹配组合方法, 上述图像特征提取与匹配程序增加了汉明距离粗匹 配,与 Ransanc 剔除误匹配,同时未采用 GPU 加速及 多线程技术,开发环境为c,opencv3． 4． 6默认参 数,运行环境为Linux-64 系统,CPUI5-8 265U, RAM8 G,其结果见表 1。 表 1 对 10 组数据,5 种方法统计了匹配关键点 数量、耗时、准确度 3 个方面的评价结果。 其中红色、 蓝色、绿色标注数据,分别代表最多匹配关键点、最小 耗时、最高准确度,“”数据代表匹配失败。 结果可 见,SURFSURFFLANN 方法在 10 组数据中均为匹 配量最多,同时保持了较高的匹配准确度,但耗时严 重;SIFT SIFT FLANN 方法各项评价结果较差; ORBBRIEFBF 方法在耗时方面具有绝对优势,但 匹配成功率不足, 关键点提取数量与准确度较 差;GFTTBRISKBF 与 GFTTBRIEFBF 匹配关键 点数量与匹配准确度较为接近,但后者耗时远低于前 者。 综上 所 述, 在 不 考 虑 匹 配 耗 时 的 情 况 下, SURFSURFFLANN 与 GFTTBRIEFBF 方法综合 效果较优。 其中前者可以获得较多的稳定特征匹配 数据,有利于获得完整的导航地图,因此适用于煤矿 井下移动机器人深度视觉自主导航地图创建阶段中 离线使用,而后者耗时方面的优势,更适用于自主运 行阶段在线使用。 3 深度视觉图优化定位与建图 3． 1 深度视觉匹配误差 ICP 模型 视觉图像平面下的特征提取与匹配解决了二维 图像平面的数据关联问题,定义 k 时刻与 k1 时刻得 到的一组匹配深度视觉数据为 Fk [Uk,Vk,Dk];Fk1 [Uk1,Vk1,Dk1] 1 其中, U [u1,u2,u3,,un] V [v1,v2,v3,,vn] D [d1,d2,d3,,dn] 2 式中,F 为不同时刻的一组经过匹配的深度数据集 合;U,V 为图像平面坐标系下的匹配数据二维坐标 集;D 为对应坐标下的深度数据集;n 为一组匹配特 征点的个数。 对 k 时刻与 k1 时刻的 F 深度视觉匹配数据, 在针孔相机模型下进行二维到三维变换,可以得到匹 配特征点 Pk c与 P k1 c 在相机坐标下的三维表达 6912 第 6 期马宏伟等煤矿井下移动机器人深度视觉自主导航研究 图 4 图像特征匹配结果 Fig． 4 Results of the image feature extracting and matching Pk c [xk c,y k c,z k c];P k1 c [xk1 c ,yk1 c ,zk1 c ]3 xk c uk c - c xz k c/ fx yk c vk c - c yz k c/ fy zk c d k c/ s xk1 c uk1 c - c xz k1 c / fx yk1 c vk1 c - c yz k1 c / fy zk1 c d k1 c / s 4 其中,cx,cy,fx,fy为相机内参,本文使用 cam_calibra- tion 标定工具进行相机的内参标定;s 为自定义深度 缩放系数。 至此一组深度视觉匹配数据被转换为三 维到三维的 ICPIterative Closest Points模型,如图 5 所示,Ok c,O k1 c 为 k 时刻与 k1 时刻的相机坐标系原 点,其经过变换后的对应相机坐标系下的深度视觉三 维匹配数据为 Pk c,P k1 c 。 设 k 时刻相机坐标系到世界坐标系的旋转与平 移矩阵为 Rk wc,t k wc,可得 Pk w R k wcP k c t k wc Pk1 w R k1 wc Pk1 c t k1 wc 5 7912 煤 炭 学 报 2020 年第 45 卷 表 1 图像特征匹配结果统计 Table 1 Statistics of image feature matching results 关键点 描述子匹配参数 评估项组号 组 1组 2组 3组 4组 5组 6组 7组 8组 9组 10 数量231917177129174021 GFTTBRISKBF耗时/ ms299． 45343． 36352． 98346． 10342． 79362． 25362． 83352． 97353． 19 准确度0． 910． 660． 880． 820． 940． 62110． 66 数量1734237103222535769 GFTTBRIEFBF耗时/ ms37． 5640． 1639． 8040． 8847． 7045． 5941． 0447． 2748． 7545． 27 准确度0． 770． 910． 8610． 950． 720． 88110． 84 数量7367377731 ORBBRIEFBF耗时/ ms28． 5123． 5127． 8426． 3729． 2826． 5920． 56 准确度0． 710． 6610． 72110． 90 数量941572821830 SIFTSIFTFLANN耗时/ ms235． 34233． 17230． 85236． 68348． 00218． 41238． 47 准确度0． 660． 570． 8810． 8110． 83 数量1 2751762965339314462281 511499335 SURFSURFFLANN耗时/ ms223． 12164． 40193． 39209． 25364． 39238． 84205． 49301． 69193． 95195． 27 准确度0． 830． 700． 770． 640． 820． 770． 910． 970． 950． 84 图 5 深度视觉匹配误差 ICP 模型 Fig． 5 ICP-model of depth vision feature-matching error 其中,Pk w,P k1 w 为相机坐标系下深度视觉匹配数据 Pk c,P k1 c 在世界坐标系下的表达。 理想情况下世界坐 标系下的匹配深度视觉数据 Pk w,P k1 w 为同一路标点, 存在如下关系 Pk w P k1 w Pk c R k,k1 wc Pk1 c t k,k1 wc 6 由于数据噪声、误匹配等因素会产生一定的误 差,所以引入误差后的 Pk c模型如下所示,其中误差模 型 e 定义为 Pk c R k,k1 wc Pk1 c t k,k1 wc e e Pk c - Rk,k1 wc Pk1 c t k,k1 wc 7 其中,Rk,k1 wc ,tk,k1 wc 为世界坐标系下,k1 时刻相机坐 标系到 k 时刻相机坐标系的旋转与平移矩阵,即相机 位姿的变换矩阵,该误差模型 e 为两帧深度视觉数据 三维到三维的匹配误差 ICP 模型。 3． 2 深度视觉 ICP-BA 图模型构建与求解 煤矿井下移动机器人深度视觉自主导航地图构 建阶段的定位与建图问题就是求解世界坐标系下,连 续运动的相机位姿与环境中三维路标点坐标。 如图 6 所示,红色圆点为环境中的路标点表示待求解的地 图数据,蓝色相机图标代表不同时刻相机在世界坐标 系的位姿,连续两帧相机位姿之间存在旋转 R 与平 移 t 构成的变换矩阵。 图 6 世界坐标下相机位姿变换 Fig． 6 Camera pose translation in world frame 对于这一问题构建概率图模型如图 7 所示,其中 节点类型有相机位姿与环境路标点两种,边为不同相 机位姿下观测到不同路标节点的可视关系,同一相机 8912 第 6 期马宏伟等煤矿井下移动机器人深度视觉自主导航研究 位姿节点与多个路标节点相连表示当前位姿下相机 观测到多个环境路标信息,同一路标节点与多个位姿 节点相连表示关联的相机位姿状态下存在共视关系。 图 7 定位与建图问题图模型 Fig． 7 Graph of slam problem 按照机器人状态估计理论对深度视觉定位与建 图问题概率图模型做如下数学定义设 k 时刻状态为 sk,其中状态由 k 时刻的相机位姿与观测到的路标信 息组成。 sk [ck,lk] ck [xk c,y k c,z k c,qw,qx,qy,qz] lk [l k 1,l k 2,l k 3,,l k i] 8 式8为 k 时刻的状态定义,其中 c 为相机位姿 状态,由位置 xk c,y k c,z k c 与姿态四元数 qw,qx,qy,qz表 示,k 时刻观测到的路标点集 lk由 i 个路标点 li组 成。 定义整体定位与建图问题下的多个位姿与路标 状态集为 ξ,是由多次观测集 γ 得到,构建似然概率 为 PRγ| ξ ∏ n 1 prγk| ξk9 ξ ξ1 ξ2 ︙ ξm ,γ γ1 γ2 ︙ γn 10 由贝叶斯法则得到的后验概率为 PRξ | γ PRγ| ξPRξ PRγ 11 对于本文给出的煤矿井下深度视觉的概率图模 型,系统状态变量最优估计即最大后验估计 ξMAP,约 等于求解最小似然概率 PRγ| ξ的负对数 -∑ n k 1 ln prγk| ξk, 该过程表达如下 ξMAP argmax ξ PRξ | γ argmax ξ PRγ| ξ argmin ξ [ - ∑ n k 1 log prγk| ξk]12 假设观测条件概率分布服从如下高斯分布 prγk| ξ Νμk,113 则有 ξMAP argmin ξ ∑ n k 1 ‖γk - μ k‖2 14 其中,γi -μ i为 2． 1 节构建的深度视觉数据三维到三 维匹配误差 ICP 模型;‖‖2为二范数运算符。 由此 构建出煤矿井下移动机器人深度视觉自主导航地图 构建阶段定位与建图问题的 ICP-BA 模型,使用非线 性最小二乘形式的目标函数为 argmin ξ 1 2 ∑ n k 1 ‖Pk c - Rk,k1 wc Pk1 c t k,k1 wc ‖ 2 2 15 由于 ICP 误差模型是关于状态 s 的函数,式15 可改写为 Es Es argmin ξ 1 2 ∑ n k 1 ‖esk‖ 2 2 argmin 1 2 es Tes 16 将 k 时刻 ICP 误差函数 esk在 sk处一阶泰勒 展开有 esk Δs ≐ esk JksΔs17 式中,Jks为 k 时刻误差函数对 k 时刻状态量 sk的 雅克比矩阵。 将式17代入式16,可得整体目标函数的泰 勒近似展开式 EsΔs为 Es Δs ≐ 1 2 ∑ n k 1 ‖esk JksΔs‖ 2 2 1 2 es JΔs Tes JΔs 1 2 es Tes ΔsTJTes 1 2 ΔsTJTJΔs Es ΔsTJTes 1 2 ΔsTJTJΔs 18 式中,J 为不同时刻下的误差函数对其该时刻下状态 变量的雅克比矩阵 J ∂es ∂s ∂es1 ∂s ∂es2 ∂s ︙ ∂esn ∂s J1s J2s ︙ Jns 19 以图 7 为例,图中存在 4 个时刻下的相机观测到 了 7 个环境路标,该图模型下的雅克比矩阵与误差函 数矩阵为 9912 煤 炭 学 报 2020 年第 45 卷 J A11000B11000000 A120000B1200000 A1300000B130000 0A230000B230000 0A2400000B24000 00A3200B3200000 00A340000B34000 00A3500000B3500 00A36000000B360 000A44000B44000 000A4600000B460 000A47000000B47 12∗11 ,es ec1,l1 ec1,l2 ec1,l3 ec2,l3 ec2,l4 ec3,l2 ec3,l4 ec3,l5 ec3,l6 ec4,l4 ec4,l6 ec4,l7 12∗1 20 其中 Aij ∂eci,lj ∂ci Bij ∂eci,lj ∂li 21 对整体目标函数式18迭代寻找下降方向使得 目标函数最小,本文使用列文伯格-马夸尔特LM 算法对这一问题进行求解。 令 ∂Es ΔsTJTes 1 2 ΔsTJTJΔs ∂Δs 0 22 可得 JTJ αIΔslm- JTes,α ≥ 023 其中,α 为阻尼因子;I 为单位矩阵;Δslm为 LM 算法 下的状态迭代变化方向。 通过对式23中阻尼因子 α 选取适当初值与更新策略,迭代求解状态变化方向 Δslm,更新系统状态量 s 直到目标整体误差式18达 到既定稳定状态。 通过上述方式最小化构建的整体目标函数,并采 取滑动窗口策略,得到深度视觉定位与建图 ICP-BA 问题的解,即地图构建阶段相机的位姿估计与环境路 标点。 图 8a为实验室全景图,图 8b为在该场景下 使用上述方法得到位姿图与环境特征点集后,将关键 帧中点云数据拼接优化处理得到的结果,包含 104 个 关键位姿与 1 789 336 个三维环境点,其中关键帧选 取策略为内点数大于阈值,且系统跟踪线程空闲时插 入新的关键帧。 图 8 实验室点云地图 Fig． 8 Point-map of our laboratory 4 导航地图表达与重定位 4． 1 Octomap 地图结构 煤矿井下移动机器人深度视觉自主导航地图创 建阶段构建的点云地图为无序存储数据类型,无法直 接用于移动机器人的运动规划,同时大尺度环境下的 点云地图数据过大,其载入与读取异常耗时,不利于 地图数据的索引。 因此在煤矿井下移动机器人深度 视觉自主导航自主运行阶段需要对初始点云地图采 用一定数据结构重分配,以满足运动规划算法执行的 要求。 常见的移动机器人导航地图类型有 Voxel has- hing[24], TSDF TruncatedSignedDistanceFunc- tions [25], ESDF Euclidean Signed Distance Func- 0022 第 6 期马宏伟等煤矿井下移动机器人深度视觉自主导航研究 tions [26]等,本文的导航地图使用基于三维占据栅格 原理的 Octomap,其数据存储为八叉树结构。 如图 9 所示,一组三维空间下的 Octomap 类型地图将空间在 xyz 方向进行划分,每划分 1 次可得到 1 组 8 个叶子 节点,通过 n 次递归划分可以得到不同分辨率下的树 结构。 图 9 中灰色方框代表该节点被部分概率占据, 白色方框表示未被占据,最下层黑色方框表示完全占 据。 其中最下层表示的是最高分辨率下的叶子节点, 假设三维地图原始数据覆盖为8． 5 m5．