地图构建方法及机器人控制系统与流程

本发明涉及计算机领域，尤其涉及一种地图构建方法及机器人控制系统。
背景技术：
：在移动机器人领域，slam(simultaneouslocalizationandmapping是一个非常基础而关键的问题，是机器人学领域最热门的研究方向之一，在过去的近30年中得到了广泛的关注与研究。slam研究领域的学者们提出了大量的slam方法，包括各种各样的传感器、优化技术和地图描述。获得机器人工作空间的地图是实现定位、避障、导航以及自主性任务的关键问题，尤其当移动机器人工作在复杂、动态环境中，仅仅使用在板传感器快速地生成并维护工作空间的3维地图是非常重要的。激光扫描仪能够以高频率提供精确的环境的几何测量，许多先进的slam系统结合各种改进的icp(iterativeclosestpoint)算法，计算观测之间的相对运动。在移动机器人上配备机械臂可以通过2维激光扫描仪实现精确的3维定位，结合imu能够在移动机器人平台上实现地图构建，通过两个激光扫描仪和icp算法能够以毫米级的精度估计机器人位姿。但是，激光扫描仪存在笨重、昂贵和获取信息量少的问题，此外，icp的缺陷包括依赖于良好的初始化估计以防止局部最小值和缺乏整体匹配质量的衡量。激光扫描仪存在的主要不足是价格高、耗能大和体积重量大，如何通过更易于实现的方式实现地图构建，是当前需要解决的技术问题。技术实现要素：本发明提供一种地图构建方法及机器人控制系统，能够更易于实现地图构建。本发明实施例采用如下技术方案：一种地图构建方法，包括：提取当前帧的特征点；获取参考帧的特征点，根据所述当前帧的特征点及所述参考帧的特征点，得出当前位姿；通过帧间配准获取的位姿估计存在累积误差，将局部地图中的关键帧的特征点投影到所述当前帧；当所述前帧符合新增关键帧的条件时，向所述局部地图插入关键帧。可选的，所述提取当前帧的特征点包括：对所述当前帧采用orb算法检测特征点并计算特征点的方向，通过随机选取特征点邻域内的像素点对进行比较获得所述特征点二进制描述符；所述根据所述当前帧的特征点及所述参考帧的特征点，得出当前位姿包括：根据运动模型预估所述参考帧的特征点在所述当前帧的区域内利用所述二进制描述符搜索匹配点，并通过方向一致性校验优化初始对应点，通过帧间匹配计算出所述当前位姿。可选的，还包括：所述局部地图构建过程中检测冗余的关键帧并删除。可选的，还包括：根据所述局部地图优化所述关键帧的位姿，确定所述关键帧作为候选回环关键帧；计算新插入的关键帧与闭环关键帧之间的相似变换，确定闭环检测和闭环关键帧；根据确定的闭环关键帧，在互可见图中插入新的边以连接闭环，融合重复的地图点。可选的，所述根据所述局部地图优化所述关键帧的位姿，确定所述关键帧作为候选回环关键帧包括：根据局部地图优化所述关键帧ki的位姿，检测新插入的所述关键帧ki与其它关键帧是否构成回路闭合；基于视觉词袋模型计算所述关键帧ki和互可见图中相邻的关键帧之间的相似性，记录最低得分smin，使用同样的方法计算所述关键帧ki与地图中其它关键帧之间的相似性，忽略所有得分低于smin的关键帧，在某个关键帧的互可见图中连续检测出3个候选回环关键帧，即确定该关键帧作为候选回环关键帧；所述计算新插入的关键帧与闭环关键帧之间的相似变换，确定闭环检测和闭环关键帧，包括：检测出候选闭环关键帧后，计算新插入的关键帧与闭环关键帧之间的相似变换，所述相似变换即回环中的累积误差，计算当前关键帧和候选回环关键帧地图点之间的对应关系，得到一组3d到3d的对应点，采用ransac迭代计算相似变换，达到拥有设定数量的内点，则确定闭环检测和闭环关键帧；所述根据确定的闭环关键帧，在互可见图中插入新的边以连接闭环，融合重复的地图点，包括：根据确定的闭环关键帧，在互可见图中插入新的边以连接闭环，融合重复的地图点，更正当前关键帧的位姿，进而将更正传递到闭合环路中的所有关键帧以及地图点，所有的关键帧都将产生新的边以将重新优化后的关键帧连接成回环，消除回环中的累积误差。可选的，还包括：根据所述关键帧，生成点云地图；根据所述点云地图构建3d环境的八叉树地图。可选的，所述根据所述关键帧，生成点云地图包括：确定空间位置[x,y,z]和像素坐标[u,v,d]之间的对应关系：d＝z·s其中，fx、fy、cx和cy是摄像机内部参数，通过校准摄像机校准获得，s表示深度缩放因子，d表示图像深度。那么，可以推导出像素在空间中的位置，采用矩阵的表达方式：其中，c为摄像机内参矩阵，r和t分别表示摄像机的旋转和位移。可选的，所述根据所述点云地图构建3d环境的八叉树地图，包括：八叉树中的叶子，用概率的形式表示是否被占据，对于叶子节点n，在1,2,…t时刻的观测数据为z1,z2,...zt，则该节点记录的信息：其中，p(n)一般情况下假设为0.5，此处引入logit变换：两边取logit变换，代入p(n)＝0.5，化简后得：l(n|z1:t)＝l(n|z1:t-1)+l(n|zt)对于任意空间中任意一点，每次获得新的观测能够直接加到原来的上面，因此更新方式灵活，父节点可以根据子节点的数值计算其占据概率，主要有取平均值和取最大值两种方式，因此基于八叉树地图库能够在生成点云图的过程中，实时的构建3d环境的八叉树地图。一种机器人控制系统，包括：传感器、计算机、控制板、机器人；所述传感器获取距离参数；所述控制板用于控制所述机器人；所述计算机用于执行：提取当前帧的特征点；获取参考帧的特征点，根据所述当前帧的特征点及所述参考帧的特征点，得出当前位姿；通过帧间配准获取的位姿估计存在累积误差，将局部地图中的关键帧的特征点投影到所述当前帧；当所述前帧符合新增关键帧的条件时，向所述局部地图插入关键帧。可选的，所述计算机还用于执行：所述局部地图构建过程中检测冗余的关键帧并删除；所述计算机还用于执行：根据所述局部地图优化所述关键帧的位姿，确定所述关键帧作为候选回环关键帧；计算新插入的关键帧与闭环关键帧之间的相似变换，确定闭环检测和闭环关键帧；根据确定的闭环关键帧，在互可见图中插入新的边以连接闭环，融合重复的地图点；所述计算机还用于执行：根据所述关键帧，生成点云地图；根据所述点云地图构建3d环境的八叉树地图。本发明实施例提供的地图构建方法及机器人控制系统，提取当前帧的特征点；获取参考帧的特征点，根据当前帧的特征点及参考帧的特征点，得出当前位姿；通过帧间配准获取的位姿估计存在累积误差，将局部地图中的关键帧的特征点投影到当前帧；当前帧符合新增关键帧的条件时，向局部地图插入关键帧。从而能够更易于实现地图构建。附图说明此处的附图被并入说明书中并构成本说明书的一部分，示出了符合本发明的实施例，并与说明书一起解释本发明的原理。图1为本发明实施例提供的一种地图构建方法的流程图。图2为本发明实施例提供的一种机器人控制系统的结构示意图。图3为本发明实施例提供的一种机器人控制系统的中传感器成像示意图。图4为本发明实施例提供的一种机器人控制系统的控制板示意图。具体实施方式下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。实施例1本发明实施例提供一种地图构建方法，如图1所示，该方法包括：11、提取当前帧的特征点；12、获取参考帧的特征点，根据所述当前帧的特征点及所述参考帧的特征点，得出当前位姿；13、通过帧间配准获取的位姿估计存在累积误差，将局部地图中的关键帧的特征点投影到所述当前帧；14、当所述前帧符合新增关键帧的条件时，向所述局部地图插入关键帧。可选的，所述提取当前帧的特征点包括：对所述当前帧采用orb(orientedfastandrotatedbrief)算法检测特征点并计算特征点的方向，通过随机选取特征点邻域内的像素点对进行比较获得所述特征点二进制描述符。具体的，对当前帧采用orb算法检测特征点并计算特征点的方向，方向信息在关键点特征描述符的匹配中非常有用，通过随机选取特征点邻域内的像素点对进行比较获得该特征点二进制描述符。视觉特征点的提取和描述基于opencv库实现，其中每一帧提取1000个特征点，尺度因子为1.2，尺度分层数为8。所述根据所述当前帧的特征点及所述参考帧的特征点，得出当前位姿包括：根据运动模型预估所述参考帧的特征点在所述当前帧的区域内利用所述二进制描述符搜索匹配点，并通过方向一致性校验优化初始对应点，通过帧间匹配计算出所述当前位姿。具体的，当前帧的特征点与参考帧的特征点存在对应关系，根据运动模型预估参考帧的特征点在当前帧的区域，在该区域内利用上一步骤计算的二进制描述符搜索匹配点，并通过方向一致性校验优化初始对应点。至此，获得一系列3d到2d的对应关系，在ransac方案中解决pnp(perspective-n-point)问题，通过帧间匹配计算出摄像机的当前位姿。可选的，所述通过帧间配准获取的位姿估计存在累积误差，将局部地图中的关键帧的特征点投影到所述当前帧包括：通过帧间配准获取的位姿估计存在累积误差，将局部地图中的关键帧的特征点投影到当前帧，构造优化特征估计位姿的残差。优化后得到的特征点的位置，比之前通过摄像机位姿估计的更精确，由此可优化地图中的特征点位置和摄像机的位姿。可选的，还包括：所述局部地图构建过程中检测冗余的关键帧并删除。具体的，精确的局部地图有利于优化帧间匹配的累积误差，当前帧符合新增关键帧的条件，则向局部地图插入关键帧并采用视觉词袋模型进行描述。检查新插入的关键帧中的地图点在相关的关键帧中的重投影误差，忽略不能满足极线约束的错误匹配。剔除不符合要求的地图点以提高算法的鲁棒性，减少存在误差的地图点对位姿估计的优化造成不利影响。采用局部捆集调整(bundleadjustment，ba)优化新的局部地图，包括新插入的关键帧及其相关联的关键帧，以及这些关键帧中的全部地图点。由于ba的复杂度是关键帧数量的三次方，局部地图构建过程中检测冗余的关键帧并删除。在相同的场景下，关键帧的数量不会随着摄像机的重复运动而无限制的增加。关键帧的增加仅在摄像机进入新的场景时而产生，有利于长时间的构图，删除冗余关键帧能够有效提升算法的实时性又能保持鲁棒性。可选的，还包括：根据所述局部地图优化所述关键帧的位姿，确定所述关键帧作为候选回环关键帧；计算新插入的关键帧与闭环关键帧之间的相似变换，确定闭环检测和闭环关键帧；根据确定的闭环关键帧，在互可见图中插入新的边以连接闭环，融合重复的地图点。可选的，所述根据所述局部地图优化所述关键帧的位姿，确定所述关键帧作为候选回环关键帧包括：根据局部地图优化所述关键帧ki的位姿，检测新插入的所述关键帧ki与其它关键帧是否构成回路闭合；基于视觉词袋模型计算所述关键帧ki和互可见图中相邻的关键帧之间的相似性，记录最低得分smin，使用同样的方法计算所述关键帧ki与地图中其它关键帧之间的相似性，忽略所有得分低于smin的关键帧，在某个关键帧的互可见图中连续检测出3个候选回环关键帧，即确定该关键帧作为候选回环关键帧。具体的，根据局部地图优化关键帧ki的位姿，尤其是大尺度范围中，仍存在误差累积问题。检测新插入的关键帧ki与其它关键帧是否构成回路闭合，二者位姿偏差即为累积的误差，进而优化闭合回路中的所有关键帧。首先，基于视觉词袋模型计算ki和互可见图中相邻的关键帧之间的相似性，记录最低得分smin。然后，使用同样的方法计算ki与地图中其它关键帧之间的相似性，忽略所有得分低于smin的关键帧。最后，在某个关键帧的互可见图中连续检测出3个候选回环关键帧，即确定该关键帧作为候选回环关键帧。所述计算新插入的关键帧与闭环关键帧之间的相似变换，确定闭环检测和闭环关键帧，包括：检测出候选闭环关键帧后，计算新插入的关键帧与闭环关键帧之间的相似变换，所述相似变换即回环中的累积误差，计算当前关键帧和候选回环关键帧地图点之间的对应关系，得到一组3d到3d的对应点，采用ransac迭代计算相似变换，达到拥有设定数量的内点，则确定闭环检测和闭环关键帧。具体的，检测出候选闭环关键帧后，计算新插入的关键帧与闭环关键帧之间的相似变换，该相似变换即回环中的累积误差。计算当前关键帧和候选回环关键帧地图点之间的对应关系，得到一组3d到3d的对应点，采用ransac迭代计算相似变换。如果拥有足够的内点，那么就能确定闭环检测和闭环关键帧。所述根据确定的闭环关键帧，在互可见图中插入新的边以连接闭环，融合重复的地图点，包括：根据确定的闭环关键帧，在互可见图中插入新的边以连接闭环，融合重复的地图点，更正当前关键帧的位姿，进而将更正传递到闭合环路中的所有关键帧以及地图点，所有的关键帧都将产生新的边以将重新优化后的关键帧连接成回环，消除回环中的累积误差。具体的，根据确定的闭环关键帧，在互可见图中插入新的边以连接闭环，融合重复的地图点。更正当前关键帧的位姿，进而将更正传递到闭合环路中的所有关键帧以及地图点。所有的关键帧都将产生新的边以将重新优化后的关键帧连接成回环，消除了回环中的累积误差。可选的，还包括：根据所述关键帧，生成点云地图；根据所述点云地图构建3d环境的八叉树地图。可选的，所述根据所述关键帧，生成点云地图包括：确定空间位置[x,y,z]和像素坐标[u,v,d]之间的对应关系：d＝z·s其中，fx、fy、cx和cy是摄像机内部参数，通过校准摄像机校准获得，s表示深度缩放因子，d表示图像深度。那么，可以推导出像素在空间中的位置，采用矩阵的表达方式：其中，c为摄像机内参矩阵，r和t分别表示摄像机的旋转和位移。具体的，上述稀疏地图主要为提升定位的精度和鲁棒性服务，而不适合于避障等应用，因此在高精度和高鲁棒性的关键帧的基础之上，采用点云图和八叉树地图技术，构建适合机器人导航等应用的3d网格地图。首先，3d环境可由一系列点描述：x＝{x1,x2,……,xn}，其中xi＝[r,g,b,x,y,z]，分别表示该点的颜色和空间位置共6个元素。对于rgb-d传感器而言，颜色信息存储于彩色图像，绝对尺度的空间位置可以通过针孔摄像机模型和深度图像的数据推算获得。如图2所示的针孔摄像机模型，根据几何关系可得空间位置[x,y,z]和像素坐标[u,v,d]之间的对应关系：d＝z·s其中，fx、fy、cx和cy是摄像机内部参数，通过校准摄像机校准获得，s表示深度缩放因子，d表示图像深度。那么，可以推导出像素在空间中的位置，采用矩阵的表达方式：其中，c为摄像机内参矩阵，r和t分别表示摄像机的旋转和位移。可选的，所述根据所述点云地图构建3d环境的八叉树地图，包括：八叉树地图能够描述任意形式的3d环境，能够以概率的形式更新，具有更新方式灵活、无需先验知识和地图形式非常紧凑等优点。八叉树中的叶子，用概率的形式表示是否被占据，对于叶子节点n，在1,2,…t时刻的观测数据为z1,z2,...zt，则该节点记录的信息：其中，p(n)一般情况下假设为0.5，此处引入logit变换：两边取logit变换，代入p(n)＝0.5，化简后得：l(n|z1:t)＝l(n|z1:t-1)+l(n|zt)由上式可知，对于任意空间中任意一点，每次获得新的观测能够直接加到原来的上面，因此更新方式灵活。父节点可以根据子节点的数值计算其占据概率，主要有取平均值和取最大值两种方式，因此基于八叉树地图库能够在生成点云图的过程中，实时的构建3d环境的八叉树地图。本发明实施例提供的地图构建方法，提取当前帧的特征点；获取参考帧的特征点，根据当前帧的特征点及参考帧的特征点，得出当前位姿；通过帧间配准获取的位姿估计存在累积误差，将局部地图中的关键帧的特征点投影到当前帧；当前帧符合新增关键帧的条件时，向局部地图插入关键帧。从而能够更易于实现地图构建。实施例2本发明实施例提供一种机器人控制系统，该机器人控制系统可以应用上述实施例1提供的方法。如图3所示，该系统包括：传感器21、计算机22、控制板23、机器人24；所述传感器21获取距离参数；所述控制板23用于控制所述机器人；所述计算机22用于执行：提取当前帧的特征点；获取参考帧的特征点，根据所述当前帧的特征点及所述参考帧的特征点，得出当前位姿；通过帧间配准获取的位姿估计存在累积误差，将局部地图中的关键帧的特征点投影到所述当前帧；当所述前帧符合新增关键帧的条件时，向所述局部地图插入关键帧。可选的，所述计算机22还用于执行：所述局部地图构建过程中检测冗余的关键帧并删除；所述计算机还22用于执行：根据所述局部地图优化所述关键帧的位姿，确定所述关键帧作为候选回环关键帧；计算新插入的关键帧与闭环关键帧之间的相似变换，确定闭环检测和闭环关键帧；根据确定的闭环关键帧，在互可见图中插入新的边以连接闭环，融合重复的地图点；所述计算机22还用于执行：根据所述关键帧，生成点云地图；根据所述点云地图构建3d环境的八叉树地图。本发明实施例的计算机22的详细功能可以参照上述实施例1，此处不赘述。在一个实施例中，该机器人控制系统可以采用如下平台硬件设计：传感器21采用硬件kinect2.0、计算机22采用微型计算机(例如i7处理器的微型计算机。)、控制板23采用pixhawk、机器人23为移动机器人，移动机器人平台基座为axialax10。可选的，移动机器人平台的基座为ax-10攀爬车的底座，具备多功能爬行能力和灵活的速度控制特性，由ae-2esc和20t电机提供动力，as-3金属齿轮伺服系统安全且精确的控制。移动机器人平台底座见相关参数如下表：规格数值轴距(mm)307长度(mm)467.4宽度(mm)295.3高度(mm)238离地间隙(mm)76.2重量(kg)1.9可选的，微软的kinect2.0传感器提供了一个基于在板的tof距离传感器。kinect2.0由多个传感器组成，包括rgb数字摄像机、麦克风和3dtof距离摄像机传感器，能够提供深度图像，相关参数如下表所示：规格数值rgb摄像机像素精度1920×1080tof摄像机像素精度512×424帧速率30fpstof传感器深度距离(m)0.5-4.5传感器尺寸(cm)25×6.5×6.5可选的，主控计算机是移动机器人平台视觉定位与地图构建算法以及自主控制的核心，它主要处理视觉传感器的数据并对整个平台进行控制，要求处理能力强和运行速度快。龙浩微动研发的高性能、低功耗、低辐射、全功能单板计算机，其硬件参数和处理运行速度能够满足移动机器人实时运行的要求，且能完成定位、地图构建、障碍物检测和路径规划等高级任务的计算需求，同时具有足够的存储空间来记录数据。下表为本文主控计算机的配置硬件参数，主控计算机通过usb3.0接口与kinect2.0连接，通过usb2.0接口与pixhawk建立连接，在板主控计算机搭载ubuntu14.04lts操作系统、indigo版的机器人操作系统(ros)和opencv(2.4.11)图像处理库。可选的，pixhawk控制板控制部分是执行命令和驱动控制的枢纽，主控计算机接收kinect2.0传感器数据，通过vslam算法构建环境的3d地图同时估计机器人平台的当前位姿估计。pixhawk控制板是一个高性能的自动驾驶模块，兼容固定翼飞行器、多旋翼飞行器、直升机、车辆和船舶等移动机器人平台。pixhawk控制板是一款开源的微控制平台，装配了具有强处理能力的stm32f427cortexm4168mhz微处理器，ram为256kb，flash存储为2mb。除了主要的cpu外，还有一个stm32f103处理器，避免主处理器失效时机器人平台的失控。为了拓展自主能力，pixhawk控制板提供了多个接口来连接外部传感器，如图4所示。pixhawk主要参数如下本发明实施例提供的机器人控制系统，提取当前帧的特征点；获取参考帧的特征点，根据当前帧的特征点及参考帧的特征点，得出当前位姿；通过帧间配准获取的位姿估计存在累积误差，将局部地图中的关键帧的特征点投影到当前帧；当前帧符合新增关键帧的条件时，向局部地图插入关键帧。从而能够更易于实现地图构建。以上，仅为本发明的具体实施方式，但本发明的保护范围并不局限，任何熟悉本
技术领域：
的技术人员在本发明揭露的技术范围内，可轻易想到变化或替换，都应涵盖在本发明的保护范围之内。因此，本发明的保护范围应以权利要求的保护范围为准。当前第1页12