一种机器人地图制作方法、装置和存储介质与流程

本发明涉及机器人环境感知技术，尤其涉及一种机器人地图制作方法、装置和存储介质。

背景技术：

近年来，随着各行业应用机器人的不断深入，制作可用于机器人定位和导航的环境地图越来越重要。现有方法即同步定位与地图创建(slam)方法，在未知场景下建图，由于受限于闭环的准确性，通常很难一次性完成大尺度下的全局地图制作并且保证全局地图的一致性；尤其是环境特征相似或较稀疏，环境中存在较多动态物体等复杂场景下，很容易因为闭环错误致使累积误差无法消除，从而导致建图过程失败。

技术实现要素：

为了克服现有技术的不足，本发明的目的之一在于提供一种机器人地图制作方法，其能解决现有技术受限于闭环的准确性，通常很难一次性完成大尺度下的全局地图制作；尤其是环境特征相似或较稀疏，环境中存在较多动态物体等复杂场景下，很容易因为闭环错误致使累积误差无法消除，从而导致建图过程失败的问题。

本发明的目的之二在于提供一种机器人地图制作装置，其能解决现有技术受限于闭环的准确性，通常很难一次性完成大尺度下的全局地图制作；尤其是环境特征相似或较稀疏，环境中存在较多动态物体等复杂场景下，很容易因为闭环错误致使累积误差无法消除，从而导致建图过程失败的问题。

本发明的目的之三在于提供一种机器人地图制作装置，其能解决现有技术受限于闭环的准确性，通常很难一次性完成大尺度下的全局地图制作；尤其是环境特征相似或较稀疏，环境中存在较多动态物体等复杂场景下，很容易因为闭环错误致使累积误差无法消除，从而导致建图过程失败的问题。

本发明的目的之四在于提供一种存储介质，存储有计算机程序，其能解决现有技术受限于闭环的准确性，通常很难一次性完成大尺度下的全局地图制作；尤其是环境特征相似或较稀疏，环境中存在较多动态物体等复杂场景下，很容易因为闭环错误致使累积误差无法消除，从而导致建图过程失败的问题。

本发明的目的之一采用以下技术方案实现：

一种机器人地图制作方法，包括以下步骤：

根据先验标记信息获取闭环位姿关系；

根据相邻帧位姿关系和闭环位姿关系求解最优化轨迹；

根据所述最优化轨迹将各帧观测信息拼接为全局地图。

进一步地，所述根据相邻帧位姿关系和闭环位姿关系求解最优化轨迹，具体为求解以下最优化问题：

其中，xi,xj,xp,xq表示待优化的机器人的位姿，(i,j∈c)表示i时刻与j时刻的位姿之间为相邻帧位姿关系，i时刻与j时刻的位姿的差值测量值为mj,i；(p,q∈d)表示p时刻与q时刻的位姿之间为闭环位姿关系，p时刻与q时刻的位姿的差值测量值为mq,p；e(xi,xj,mj,i)、e(xp,xq,mq,p)表示误差函数；

所述最优化轨迹包括优化后的各帧机器人的位姿。

进一步地，所述误差函数具体为：

其中，ωij、ωpq为测量对应的信息矩阵。

进一步地，闭环位姿关系中p时刻和q时刻位姿的差值测量值为：

其中x′p、x′q分别表示先验标记信息坐标系下p时刻和q时刻的机器人的位姿。

进一步地，相邻帧位姿关系中i时刻和j时刻位姿的差值测量值mj,i通过机器人里程计获取或通过迭代近邻算法匹配前后两帧的观测值计算。

进一步地，所述根据所述最优化轨迹将各帧观测信息拼接为全局地图，具体包括以下步骤：

将各帧观测信息根据对应的各帧优化后的位姿转换至全局坐标系，生成全局地图。

本发明的目的之二采用以下技术方案实现：

一种机器人地图制作装置，包括：

获取模块，用于根据先验标记信息获取闭环位姿关系；

求解模块，用于根据相邻帧位姿关系和闭环位姿关系求解最优化轨迹；

拼接模块，用于根据所述最优化轨迹将各帧观测信息拼接为全局地图。

进一步地，所述求解模块具体用于求解以下最优化问题：

其中，xi,xj,xp,xq表示待优化的机器人的位姿，(i,j∈c)表示i时刻与j时刻的位姿之间为相邻帧位姿关系，i时刻与j时刻的位姿的差值测量值为mj,i；(p,q∈d)表示p时刻与q时刻的位姿之间为闭环位姿关系，p时刻与q时刻的位姿的差值测量值为mq,p；e(xi,xj,mj,i)、e(xp,xq,mq,p)表示误差函数；

所述最优化轨迹包括优化后的各帧机器人的位姿。

进一步地，所述求解模块包括：

第一计算单元，用于计算闭环位姿关系中p时刻和q时刻位姿的差值测量值为：

其中x′p、x′q分别表示先验标记信息坐标系下p时刻和q时刻的机器人的位姿。

进一步地，所述求解模块还包括：

第二计算单元，用于通过机器人里程计获取相邻帧位姿关系中i时刻和j时刻位姿的差值测量值mj,i，或通过迭代近邻算法匹配前后两帧的观测值计算相邻帧位姿关系中i时刻和j时刻位姿的差值测量值mj,i。

本发明的目的之三采用以下技术方案实现：

机器人地图制作装置，包括存储器、处理器以及存储在存储器中并可在处理器上运行的计算机程序，处理器执行计算机程序时实现前述的机器人地图制作方法的步骤。

本发明的目的之四采用以下技术方案实现：

一种存储介质，所述介质存储有计算机程序，所述计算机程序被处理器执行时实现前述的机器人地图制作方法的步骤。

相比现有技术，本发明的有益效果在于：通过将可靠的位姿闭环约束引入轨迹优化过程，消除了机器人的累积误差，从而可以对任意尺度及大部分场景的环境建立全局一致的地图，也可以适应各类复杂环境，例如动态物体多、特征稀缺或单一等。

附图说明

图1为本发明实施例一的机器人地图制作方法的流程示意图；

图2为闭环位姿关系示意图；

图3为本发明实施例二的机器人地图制作装置的结构示意图；

图4为本发明实施例三的机器人地图制作装置的结构示意图。

具体实施方式

机器人地图制作，是指通过各类传感器采集环境信息，并通过技术手段将传感器数据有序融合；机器人地图主要用于机器人的定位和导航。

下面，结合附图以及具体实施方式，对本发明做进一步描述，需要说明的是，在不相冲突的前提下，以下描述的各实施例之间或各技术特征之间可以任意组合形成新的实施例。

机器人地图制作，是指通过各类传感器采集环境信息，并通过技术手段将传感器数据有序融合；地图主要用于机器人定位和导航。制作机器人地图的传感器为单目、双目或多目相机，深度相机，二维或三维激光传感器。

传感器可以设置在机器人上，获取各个时刻的观测帧，利用观测帧确定机器人的位姿(包括位置和朝向)，计算机器人的轨迹；本发明所指的机器人也可以是其他依赖地图导航定位的运动设备，例如智能玩具、可运动的智能家居、测绘车等。

实施例一

如图1为一种机器人地图制作方法，包括以下步骤：

步骤s110、根据先验标记信息获取闭环位姿关系。

机器人的位姿是指某个时刻机器人的位置和朝向，通常可以由平移分量ti和旋转分量ri表示，有：

闭环是指机器人在两个间距较远的时刻处于相同或近似的位姿。如图2所示的闭环位姿关系示意图，机器人从0时刻出发，带箭头的细直线代表机器人的朝向，粗直线表示机器人行进的轨迹，机器人的轨迹具体为某段时间内一系列位姿的集合；虚线圆代表0和n时刻的两个位姿的差值(包括位置和朝向)在阈值之内，n时刻的位姿和0时刻的位姿就形成闭环位姿关系。

进一步地，两个时刻，如n时刻和0时刻的两个位姿是否是闭环位姿关系可以由先验信息来验证。闭环位姿的约束在先验信息的基础上可以通过匹配视觉特征获得，例如根据二维码的位姿关系反算出机器人的位姿关系，检测到同一个二维码即表示机器人处于同一或相近似位姿，形成闭环；也可以通过人工操作得到，例如先后处于同一或相近似位姿时做相应的标记，从而保证了准确的闭环位姿关系，相近似位姿中的“相近似”指的是空间上的位姿近似。

步骤s120、根据相邻帧位姿关系和闭环位姿关系求解最优化轨迹。

机器人测得的轨迹与机器人实际走过的轨迹之间存在有累积误差，通过将闭环位姿关系引进到轨迹的优化中，可以减小累积误差，最优化轨迹更接近于机器人实际走过的轨迹。

进一步地，步骤s120根据相邻帧位姿关系和闭环位姿关系求解最优化轨迹，具体为求解以下最优化问题：

其中，xi,xj,xp,xq表示待优化的机器人的位姿，其初值可通过里程计获得；(i,j∈c)表示i时刻与j时刻的位姿之间为相邻帧位姿关系，i时刻与j时刻的位姿的差值测量值为mj,i；(p,q∈d)表示p时刻与q时刻的位姿之间为闭环位姿关系，p时刻与q时刻的位姿的差值测量值为mq,p；e(xi,xj,mj,i)、e(xp,xq,mq,p)表示误差函数。

所述最优化轨迹包括优化后的各帧机器人的位姿。

进一步地，在本实施例中，误差函数具体为：

其中，ωij、ωpq为测量对应的信息矩阵，信息矩阵为协方差矩阵的逆矩阵，用于度量差值测量值的可靠性，根据现有技术可以实现，不再赘述。

基于相邻帧的运动关系及准确的闭环位姿关系，利用非线性优化方法求解机器人轨迹的最大后验估计或最大似然估计，可获得机器人的精确的运动轨迹。由于利用了闭环位姿关系的闭环约束，减小了累积误差，求得的轨迹更接近于机器人实际走过的轨迹，为地图拼接提供了更好的基础。

进一步地，闭环位姿关系中p时刻和q时刻两个时刻位姿的差值测量值mq,p，可以根据先验标记信息(例如二维码)得到的位姿计算得到：

其中x′p、x′q分别表示先验标记信息坐标系下p时刻和q时刻的机器人的位姿。

另外，所述差值测量值mq,p还可以根据p时刻与q时刻的观测值通过现有的迭代近邻匹配计算获得，其初值根据里程计信息确定。

进一步地，相邻帧位姿关系中i时刻和j时刻两个时刻位姿的差值测量值mj,i可以通过机器人里程计获取或通过迭代近邻算法匹配i时刻和j时刻位姿的观测值计算，均属于现有技术，不再赘述。

步骤s130、根据所述最优化轨迹将各帧观测信息拼接为全局地图。

运动设备拍摄的各帧观测信息表示的地图信息之间是离散的，无法准确拼接。如两帧观测信息之间有重复的地方，只有知道拍摄两帧观测信息时运动设备分别所处的位姿，才可以将两帧观测信息统一到同一坐标系下，准确的将两帧观测信息拼接。而最优化轨迹可以保证位姿更准确，从而得到的全局地图也更准确。

进一步地，步骤s130根据所述最优化轨迹将各帧观测信息拼接为全局地图，具体包括以下步骤：

将各帧观测信息根据对应的各帧优化后的位姿转换至全局坐标系，拼接生成全局地图。

基于准确的轨迹信息，将运动设备，如机器人，测绘车等获取的各帧的特征或点云转换至同一坐标系下，从而生成全局地图。

制作机器人地图的传感器为单目、双目或多目相机，深度相机，二维或三维激光传感器；机器人观测信息可以包括稠密的障碍物点云地图，例如激光点云地图，也可以包括稀疏的环境特征地图，例如视觉orb特征等。观测信息的获取与处理均属于现有技术，不再赘述。

进一步地，将各帧观测信息根据对应的各帧优化后的位姿转换至全局坐标系，生成全局地图，具体为根据以下算式实现：

其中，ri和ti分别为i时刻位姿的旋转分量和平移分量；k＝0,…,k为i时刻观测到的观测信息，例如激光点、orb等环境特征在机器人当前坐标系下的位置。经过将各帧观测信息统一到全局坐标系下，就完成了地图的拼接。

进一步地，地图拼接过程中可以引入现有的光线追踪等方法，剔除杂点及动态障碍物点，只留下静态或规则障碍物的地图。具体为根据位姿和观测计算概率栅格地图，将概率值低于阈值的特征点剔除。

本发明通过将可靠的位姿闭环约束引入轨迹优化过程，消除了机器人的累积误差，从而可以对任意尺度及大部分场景的环境建立全局一致的地图，也可以适应各类复杂环境，例如动态物体多、特征稀缺或单一等。

实施例二

如图3所示的机器人地图制作装置，包括：

获取模块110，用于根据先验标记信息获取闭环位姿关系；

求解模块120，用于根据相邻帧位姿关系和闭环位姿关系求解最优化轨迹；

拼接模块130，用于根据所述最优化轨迹将各帧观测信息拼接为全局地图。

进一步地，求解模块120具体用于求解以下最优化问题：

其中，xi,xj,xp,xq表示待优化的机器人的位姿，(i,j∈c)表示i时刻与j时刻的位姿之间为相邻帧位姿关系，i时刻与j时刻的位姿的差值测量值为mj,i；(p,q∈d)表示p时刻与q时刻的位姿之间为闭环位姿关系，p时刻与q时刻的位姿的差值测量值为mq,p；e(xi,xj,mj,i)、e(xp,xq,mq,p)表示误差函数；

所述最优化轨迹包括优化后的各帧机器人的位姿。

进一步地，求解模块120包括：

第一计算单元，用于计算闭环位姿关系中p时刻和q时刻位姿的差值测量值为：

其中x′p、x′q分别表示先验标记信息坐标系下p时刻和q时刻的机器人的位姿。

进一步地，求解模块120还包括：

第二计算单元，用于通过机器人里程计获取相邻帧位姿关系中i时刻和j时刻位姿的差值测量值mj,i，或通过迭代近邻算法匹配前后两帧的观测值计算相邻帧位姿关系中i时刻和j时刻位姿的差值测量值mj,i。

进一步地，拼接模块130具体用于将各帧观测信息根据对应的各帧优化后的位姿转换至全局坐标系，生成全局地图。

本实施例中的装置与前述实施例中的方法是基于同一发明构思下的两个方面，在前面已经对方法实施过程作了详细的描述，所以本领域技术人员可根据前述描述清楚地了解本实施中的系统的结构及实施过程，为了说明书的简洁，在此就不再赘述。

为了描述的方便，描述以上装置时以功能分为各种模块分别描述。当然，在实施本发明时可以把各模块的功能在同一个或多个软件和/或硬件中实现。

通过以上的实施方式的描述可知，本领域的技术人员可以清楚地了解到本发明可借助软件加必需的通用硬件平台的方式来实现。基于这样的理解，本发明的技术方案本质上或者说对现有技术做出贡献的部分可以以软件产品的形式体现出来，该计算机软件产品可以存储在存储介质中，如rom/ram、磁碟、光盘等，包括若干指令用以使得一台计算机设备(可以是个人计算机，服务器，或者网络设备等)执行本发明各个实施例或者实施例的某些部分所述的方法，如：

一种存储介质，所述存储介质存储有计算机程序，所述计算机程序被处理器执行时实现前述机器人地图制作方法的步骤。

描述的装置实施例仅仅是示意性的，其中所述作为分离部件说明的模块或单元可以是或者也可以不是物理上分开的，作为模块或单元示意的部件可以是或者也可以不是物理模块，既可以位于一个地方，或者也可以分布到多个网络模块上。可以根据实际的需要选择其中的部分或者全部单元来实现本实施例方案的目的。本领域普通技术人员在不付出创造性劳动的情况下，即可以理解并实施。

本发明可用于众多通用或专用的计算系统环境或配置中。例如：个人计算机、服务器计算机、手持设备或便携式设备、平板型设备、多处理器系统、基于微处理器的系统、机顶盒、可编程的消费电子设备、网络pc、小型计算机、大型计算机、包括以上任何系统或设备的分布式计算环境等等，如实施例四。

实施例三

如图4所示机器人地图制作装置，包括存储器200、处理器300以及存储在存储器200中并可在处理器300上运行的计算机程序，处理器300执行计算机程序时实现上述机器人地图制作方法的步骤。

本实施例中的装置与前述实施例中的方法是基于同一发明构思下的两个方面，在前面已经对方法实施过程作了详细的描述，所以本领域技术人员可根据前述描述清楚地了解本实施中的系统的结构及实施过程，为了说明书的简洁，在此就不再赘述。

本发明实施例提供的机器人地图制作装置，可以通过将可靠的位姿闭环约束引入轨迹优化过程，消除了机器人的累积误差，从而可以对任意尺度及大部分场景的环境建立全局一致的地图，也可以适应各类复杂环境，例如动态物体多、特征稀缺或单一等。

上述实施方式仅为本发明的优选实施方式，不能以此来限定本发明保护的范围，本领域的技术人员在本发明的基础上所做的任何非实质性的变化及替换均属于本发明所要求保护的范围。