CN111256689B

CN111256689B - 一种机器人定位方法、机器人和存储介质

Info

Publication number: CN111256689B
Application number: CN202010042753.4A
Authority: CN
Inventors: 胡仁志; 胡满; 杜楠枰
Original assignee: Beijing Zhihua Robot Technology Co ltd
Current assignee: Beijing Zhihua Robot Technology Co ltd
Priority date: 2020-01-15
Filing date: 2020-01-15
Publication date: 2022-01-21
Anticipated expiration: 2040-01-15
Also published as: CN111256689A

Abstract

本发明实施例涉及机器人定位方法、机器人和存储介质，所述方法包括：根据位移测量装置测量的第一测量值获得第一机器人位姿；根据惯性测量装置测量的第二测量值获得第二机器人位姿；根据第一机器人位姿和第二机器人位姿获得第三机器人位姿；获得摄像装置拍摄的包含标志码的图像，基于标志码的位姿和第三机器人位姿获得标志码的图像观测值；基于图优化算法根据标志码的图像观测值、获得机器人的估计位姿以及标志码的估计位姿。本发明实施例在机器人工作的环境中贴设易于识别的标志码，标志码作为确定的特征信息，无需特征匹配，提高定位精度。机器人中安装位移测量装置、惯性测量装置和摄像装置，采用多传感器测量数据融合，进一步提高定位精度。

Description

一种机器人定位方法、机器人和存储介质

技术领域

本发明实施例涉及智能机器人技术领域，特别涉及一种机器人定位方法、机器人和存储介质。

背景技术

无人叉车在工作过程中，需要定位和导航，目前，无人叉车的定位导航方式多采用基于视觉或激光的同步定位与建图(simultaneous localization and mapping，SLAM)导航。

基于视觉或激光的SLAM导航，是通过视觉传感器或激光导航传感器采集环境特征构建增量式地图，并通过扫描匹配的方式实现导航定位，为了实现准确定位需要环境空间中具有足够密集的环境特征点，否则会影响定位精度。

在实现本发明过程中，发明人发现上述方法至少存在如下问题：无人叉车多工作于仓库等较为空旷的场所，在仓库环境中，特征点较少，一般多为互相对称的立柱和墙面，且墙面缺少纹理特征，无人叉车采用基于视觉或激光的SLAM导航方法时，会因缺少足够的特征点导致定位精度较低。

发明内容

本发明实施例的目的是提供一种定位精度高的机器人定位方法、机器人和存储介质。

第一方面，本发明实施例提供了一种机器人定位方法，所述方法用于机器人，所述机器人包括位移测量装置、惯性测量装置和摄像装置，所述方法包括：

获得位移测量装置测量的第一测量值，基于所述第一测量值获得所述机器人的第一机器人位姿；

获得惯性测量装置测量的第二测量值，基于所述第二测量值获得所述机器人的第二机器人位姿；

根据所述第一机器人位姿和所述第二机器人位姿获得所述机器人的第三机器人位姿；

获得摄像装置拍摄的包含标志码的图像，基于所述标志码的位姿和所述第三机器人位姿获得所述标志码的图像观测值；

基于图优化算法根据所述标志码的图像观测值、获得所述机器人的估计位姿以及所述标志码的估计位姿。

在一些实施例中，所述方法还包括：

根据所述机器人的估计位姿和所述第一测量值更新所述机器人的第一机器人位姿。

在一些实施例中，所述方法还包括：

基于所述包含标志码的图像获得所述标志码的初始位姿。

在一些实施例中，所述基于图优化算法根据所述标志码的图像观测值、获得所述机器人的估计位姿以及所述标志码的估计位姿，包括：

获得标志码坐标系下至少一个角点的角点坐标；

基于所述标志码的图像观测值获得所述标志码至少一个角点的角点像素坐标；

基于图优化算法获得所述标志码的最优初始位姿；

获得所述摄像装置的最优初始位姿；

根据所述角点坐标、所述角点像素坐标、所述标志码的最优初始位姿和所述摄像装置的最优初始位姿、基于所述标志码的重投影误差最小关系获得所述机器人的估计位姿和所述标志码的估计位姿。

在一些实施例中，所述基于图优化算法获得所述标志码的最优初始位姿，包括：

建立第一图，所述第一图中节点为所述标志码的位姿在图像坐标系下的像素位姿；

根据所述像素位姿、所述角点坐标和所述角点像素坐标、基于重投影误差最小关系获得所述标志码之间的最佳相对像素位姿；

建立第二图，所述第二图中各节点的连接边为所述最佳相对像素位姿，并为所述连接边设置权重；

分别以所述第二图中各个节点作为起点，获得所述起点的最小生成树；

确定各节点间最佳相对像素位姿加权之和最小的最小生成树对应的起点为初始节点；

基于佛洛伊德算法计算所述初始节点的像素位姿，对所述初始节点的像素位姿进行坐标转换，获得所述标志码的最优初始位姿。

根据所述像素位姿、所述角点坐标和所述角点像素坐标、基于所述标志码的重投影误差最小关系获得所述标志码之间的最佳相对像素位姿；

确定各节点间最佳相对像素位姿加权之和最小的最小生成树对应的起点为第一初始节点；

去除所述第二图中异常的边，获得第三图；

将所述第一初始节点作为起点和终点，获得至少一个包含多个节点的环路；

基于所述环路中节点间相对位姿的约束关系、以及所述节点间相对位姿和所述最佳相对像素位姿满足的加权平方差最小关系，计算节点间的相对位姿获得节点间的最优相对像素位姿；

分别以所述第三图中各个节点作为起点，获得所述起点的最小生成树；

确定各节点间最优相对像素位姿加权之和最小的最小生成树对应的起点为第二初始节点；

基于佛洛伊德算法计算所述第二初始节点的像素位姿，对所述第二初始节点的像素位姿进行坐标转换，获得所述标志码的最优初始位姿。

在一些实施例中，所述获得所述摄像装置的最优初始位姿，包括：

根据所述角点坐标、所述角点像素坐标和所述标志码的最优初始位姿、基于所述标志码的重投影误差最小关系获得所述摄像装置的最优初始位姿。

在一些实施例中，所述位移测量装置包括编码器，所述惯性测量装置包括陀螺仪。

第二方面，本发明实施例提供了一种机器人，所述机器人包括位移测量装置、惯性测量装置、摄像装置和控制器，所述控制器分别与所述位移测量装置、所述惯性测量装置和所述摄像装置通信连接；

所述控制器包括：

至少一个处理器；以及，

与所述至少一个处理器通信连接的存储器；其中，

所述存储器存储有可被所述至少一个处理器执行的指令，所述指令被所述至少一个处理器执行，以使所述至少一个处理器能够执行上述的方法。

第三方面，本发明实施例提供了一种非易失性计算机可读存储介质，其特征在于，所述计算机可读存储介质存储有计算机可执行指令，当所述计算机可执行指令被机器人执行时，使所述机器人执行上述的方法。

第四方面，本申请实施例还提供了一种计算机程序产品，所述计算机程序产品包括存储在非易失性计算机可读存储介质上的计算机程序，所述计算机程序包括程序指令，当所述程序指令被机器人执行时，使所述机器人执行上述的方法。

本发明实施例的机器人定位方法、机器人和存储介质，在机器人工作的环境中贴设易于识别的标志码，标志码作为确定的特征信息，无需特征匹配，提高定位精度。机器人中安装位移测量装置、惯性测量装置和摄像装置，采用多传感器测量数据融合，进一步提高定位精度。

附图说明

一个或多个实施例通过与之对应的附图中的图片进行示例性说明，这些示例性说明并不构成对实施例的限定，附图中具有相同参考数字标号的元件表示为类似的元件，除非有特别申明，附图中的图不构成比例限制。

图1是本发明实施例定位方法的应用场景示意图；

图2是本发明定位方法的一个实施例中标志码坐标系的示意图；

图3是本发明定位方法的一个实施例中机器人二维坐标系和世界二维坐标系的关系示意图；

图4是本发明定位方法的一个实施例的流程示意图；

图5是本发明机器人的一个实施例中控制器的硬件结构示意图。

具体实施方式

为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

图1示出了本发明实施例的机器人定位方法的应用场景，所述应用场景包括机器人10和标志码20，其中，机器人10为可移动机器人，例如扫地机器人、巡检机器人、无人采样机器人、无人叉车等等。标志码20可以为多个不同的标志码，贴设于机器人10的工作场所，用于帮助机器人10进行定位和导航，标志码20可以为任何易于识别的码，例如ArUco码，ArUco码固有的结构很容易从图像中提取，通过ArUco码的4个角点可以获得ArUco码的位姿，并且ArUco码的ID具有唯一性，提供了应用错误检测和矫正技术的可能性。

机器人10在为了完成用户的任务或者其他情况下，需要在未知环境中运动。在运动过程中为了实现自主定位和导航，需要构建增量式地图(例如包括多个标志码位姿的地图)，同时进行定位，即估计自身在地图中的位置。

其中，请参照图1，机器人10包括机器人主体11、摄像装置12、位移测量装置、惯性测量装置和控制器(位移测量装置、惯性测量装置和控制器图中未示出)。机器人主体11是机器人的主体结构，可以根据机器人10的实际需要，选用相应的形状结构及制造材质(如硬质塑料或者铝、铁等金属)。摄像装置12用于获得周围环境的图像，例如RGBD摄像机等。位移测量装置用于测量机器人10行进的位移，可以是任何合适的位移运动检测装置，例如编码器等。惯性测量装置用于测量机器人10的运动角度信息，可以是任何合适的角运动检测装置，例如陀螺仪等。

控制器13是内置于机器人主体11中的电子计算核心，用于执行逻辑运算步骤以实现机器人10的智能化控制。控制器13与摄像装置12、位移测量装置和惯性测量装置连接，用于根据摄像装置12采集的周围环境图像、以及位移测量装置和惯性测量装置的测量值，执行预设的算法进行自身定位和地图构图。

机器人10还包括行走机构，行走机构设置于机器人主体11上，是为机器人10提供移动能力的结构装置。该行走机构具体可以采用任何类型的移动装置实现，例如滚轮、履带式等。

本发明实施例的机器人定位方法尤其适用于工作于仓库等空旷场所的机器人(例如无人叉车)，在所述机器人定位方法用于上述场所时，可以避免上述场所因特征点少导致的定位精度低的问题，从而提高机器人定位精度，当然，本发明实施例的机器人定位方法也同样适用于工作于其他场合的机器人。

本发明实施例采用基于图优化算法的多传感器测量融合的定位方法，对机器人和标识码的位姿进行估计和更新，定位精度高。其基本思想思想在于预测+测量反馈，它由两部分组成，第一部分是状态预测方程，第二部分是观测方程。在预测阶段，要通过系统状态方程计算的预测值作为先验信息，之后在观测部分再重新更新这个信息。

本发明实施例涉及多个坐标系，包括世界坐标系、机器人坐标系、相机坐标系、图像坐标系、标志码坐标系和惯性测量坐标系(例如陀螺仪坐标系)。其中，标志码坐标系请参照图2，即以码的中心为圆点建立的坐标系。根据标志码的位姿可以获得标志码坐标系与世界坐标系的转换。

根据摄像装置的内参和外参，可以获得摄像装置的图像坐标系和相机坐标系的转换关系，以及世界坐标系到相机坐标系的转换。因为摄像装置和惯性测量装置均安装于机器人上，则分别可以获得相机坐标系和机器人坐标系的转换关系、以及惯性测量坐标系和机器人坐标系的转换关系。则，世界坐标系、机器人坐标系、相机坐标系、图像坐标系、标志码坐标系和惯性测量坐标系可以两两转换。图3示出了机器人二维坐标系(Xc，Yc)和世界二维坐标系(X，Y)的关系。

图4为本发明实施例提供的一种定位方法的流程示意图，所述方法可以由机器人执行(例如图1中的机器人10，具体的，在一些实施例中，所述方法由机器人10中的控制器执行)，如图4所示，所述方法包括：

101：获得位移测量装置测量的第一测量值，基于所述第一测量值获得所述机器人的第一机器人位姿。

102：获得惯性测量装置测量的第二测量值，基于所述第二测量值获得所述机器人的第二机器人位姿。

103：根据所述第一机器人位姿和所述第二机器人位姿获得所述机器人的第三机器人位姿。

104：获得摄像装置拍摄的包含标志码的图像，基于所述标志码的位姿和所述第三机器人位姿获得所述标志码的图像观测值。

其中，位移测量装置用于测量机器人10行进的位移，例如编码器等。惯性测量装置用于测量机器人10的运动角度信息，例如陀螺仪等，为方便论述，以下均以位移测量装置为编码器、惯性测量装置为陀螺仪为例说明。

所述第一测量值即编码器测量值亦即编码器增量，根据编码器增量可以获得机器人位移，根据机器人初始位姿和机器人的位移可以获得机器人的位姿，即第一机器人位姿。机器人结构不同，编码器增量与机器人位移具有不同的关系，以机器人运动底盘采用三角形结构、前轮为驱动轮、后轮为辅助轮、前轮上安装有编码器和角度传感器、机器人坐标系的参考中心建立在后轮中心点上为例，机器人t时刻的位姿为：

其中，k为驱动轮的系数，Δe为编码器增量，δ为角度传感器测量值(即驱动轮与叉车坐标系Yc轴的夹角)，L为驱动轮到机器人坐标系原点的距离，ω_t为噪声。机器人的位移的增量Δs和角度增量Δθ中均含有噪声。式(1)即为机器人的运动模型，用于在预测阶段获得机器人的位姿。之后再在观测阶段对机器人和标志码的位姿进行更新。

在其中一些实施例中，采用陀螺仪和摄像装置融合的观测模型，为：

z_t,i＝h(ζ_t,m_i)+δ_t (2)

其中，Z_t,i为第i个标志码在t时刻在图像中的像素坐标，亦即第i个标志码在t时刻的图像观测值，m_i表示第i个标志码的位姿，h表示在t时刻时机器人坐标系下，摄像装置观测到的标志码的观测方程，δ_t为测量噪声，ζ_t为t时刻机器人的位姿。为了确保机器人定位的精度，该机器人位姿为融合了编码器和陀螺仪测量值的位姿，即根据编码器测量值获得第一机器人位姿，根据陀螺仪测量值获得第二机器人位姿，再结合第一机器人位姿和第二机器人位姿获得第三机器人位姿，ζ_t即为t时刻机器人的第三机器人位姿。

具体的，在其中一些实施例中，可以分别为第一机器人位姿和第二机器人位姿设置权重，第三机器人位姿为第一机器人位姿和第二机器人位姿的加权和。

第二机器人位姿可以通过陀螺仪的观测方程获得，机器人t时刻的位姿是陀螺仪在t-1时刻到t时刻之间多次测量值累加的结果，根据陀螺仪的观测方程，有：

z″_t＝h″(ζ_t)+δ″_t (3)

其中，h″表示陀螺仪的观测方程，ζ_t为t时刻的机器人位姿，z_t′为叉车t-1时刻到t时刻的角速度累加值，δ″_t为测量噪声。通过式(3)可以获得机器人t时刻的位姿，即第二机器人位姿。

基于标志码的位姿和第三机器人位姿获得标志码的图像观测值后，基于图优化算法和标志码的图像观测值，获得机器人的估计位姿和标志码的估计位姿。将机器人的估计位姿代入式(1)，可以进一步获得更新后的机器人位姿。

其中，随着时间的推移，利用运动方程(1)对机器人的位姿进行预测、以及利用观测方程(2)对机器人的位姿和标志码的位姿进行更新是一个持续的过程。m_i可以是t-1时刻获得的标志码的估计位姿，在未获得标志码估计位姿时，可以利用摄像装置拍摄的包含标志码的图像、基于三角测距方法获得机器人距离标志码的距离，再根据机器人位姿(可以是第三机器人位姿)获得标志码的初始位姿。具体的，对包含标志码的图像提取特征点(例如角点)，再利用三角测距方法可以测算出标志码的距离，该利用三角测距方法获得标志码距离的方法为现有技术，在此不再赘述。

105：基于图优化算法根据所述标志码的图像观测值、获得所述机器人的估计位姿以及所述标志码的估计位姿。

因为机器人坐标系和相机坐标系之间存在平移和旋转的固定关系，所以，对机器人位姿的优化即是对摄像装置位姿的优化。图优化的基本思想是用图模型来表达一个非线性最小二乘的优化问题，所以，首先是构建标志码的理论值和观测值的重投影误差的最小二乘关系，再通过该最小二乘关系获得摄像装置和标志码的估计位姿，从而根据摄像装置的估计位姿获得机器人的估计位姿。

具体的，在其中一个实施例中，构建的最小二乘关系如下：

其中，

其中，ζ_i,i＝1,...,M为标志码在世界坐标系下的位姿，ζ^j,j＝1,...,N为摄像装置在世界坐标系下的位姿，δ表示摄像装置参数，Ψ为重投影方程，Γ(*)为世界坐标系与图像坐标系的旋转平移变换矩阵，u_i,j′ ^t为标志码ζ_i,i＝1,...,M的四个角点的角点像素坐标，j′＝1,...,4表示四个角点，c_j′,j′＝1,...,4为标志码坐标系下四个角点的角点坐标，f^t表示t时刻获得的一帧图像，e_i ^t表示重投影误差。

其中，c_j′已知，标志码的图像观测值为标志码在图像中的像素坐标，通过提取图像中的角点特征点，可以获得标志码四个角点的角点像素坐标u_i,j′ ^t，将c_j′和u_i,j′ ^t代入式(5)，求解f(x)最小时获得摄像装置位姿ζ^j和标志码位姿ζ_i。然后根据摄像装置和机器人的坐标变换关系，获得优化的机器人位姿。

其中，在一些实施例中，可以采用列文伯格-马夸尔特算法(Levenberg-Marquardt，LM)求解，LM算法为一种局部搜索算法，需要一个良好的摄像装置和标志码的初始位姿来避免陷入局部极小，因此，需要先确定摄像装置和标志码的最优初始位姿。获得摄像装置和标志码的最优初始位姿后，代入式(5)中，获得摄像装置和标志码的估计位姿。

在其中一个实施例中，获得标志码的最优初始位姿包括以下步骤：

1051：建立第一图，所述第一图中节点为所述标志码的位姿在图像坐标系下的像素位姿。

1052：根据所述像素位姿、所述角点坐标和所述角点像素坐标、基于所述标志码的重投影误差最小关系获得所述标志码之间的最佳相对像素位姿。

首先，建立Pose quiver(即第一图)，第一图中节点为标志码的位姿在图像坐标系下的像素位姿，边为标志码与标志码之间的相对位姿(亦即一个标志码到另一个标志码的坐标变换)，当一幅图像中可以一次观测到两个或两个以上标志码时，就计算标志码与标志码之间的相对位姿，并为第一图添加一条边和两个节点。由于噪声、相机移动、低分辨率等原因，标志码与标志码之间每条边的质量有差异性。我们的目标是确定标志码之间的最佳相对姿态

为下一步建立Pose Graph做准备。其中，标志码的位姿可以是t-1时刻获得的标志码的估计位姿，在未获得标志码估计位姿时，也可以是利用三角测距方法获得的标志码的初始位姿。

因此，在序列图像中，建立标志码之间相对位姿的重投影误差，并将误差求和，构建最小二乘问题，找到两标志码之间的最佳相对位姿：

其中，X_j,i为图像序列中连接节点i和节点j的边的集合。F_i,j为同时包含节点i和节点j的图像序列。k＝1,...,4表示标志码4个角点。γ_j ^t为t时刻对应的图像中，标志码的位姿在图像坐标系下的像素位姿，γ_j,i ^t为t时刻对应的图像中节点i到节点j的相对位姿。

通过使式(6)最小可以求解节点间的相对位姿γ_j,i ^t即最佳相对位姿

1053：建立第二图，所述第二图中各节点的连接边为所述最佳相对像素位姿，并为所述连接边设置权重。

1054：分别以所述第二图中各个节点作为起点，获得所述起点的最小生成树。

1055：确定各节点间最佳相对像素位姿加权之和最小的最小生成树对应的起点为第一初始节点。

接着，建立Pose Graph(即第二图)，第二图中边为最佳相对位姿

为每条边设置权重

e_j,i＝e_i,j。然后，利用Pose Graph获得标志码的最优初始位姿。

从第二图中选择一个节点a(γ_a＝I_4×4)作为起点，获得该起点的最小生成树，然后再以剩下的各个节点为起点，分别建立最小生成树。其中，如果节点a到节点i的路径为a,b,...,h,i,则节点i的位姿为：

使所有边的重投影误差的和为最小生成树的代价，使最小生成树代价最小的节点为第一初始节点，即确定各节点间最佳相对像素位姿加权之和最小的最小生成树对应的起点为第一初始节点。在另一些实施例中，也可以不为边设置权重，则，各节点间最佳相对像素位姿之和最小的最小生成树对应的起点为第一初始节点。

在其中一些实施例中，可以基于佛洛伊德算法计算第一初始节点的像素位姿，可以将此处获得的第一初始节点的像素位姿进行坐标变换，并将坐标变换获得的位姿作为标志码的最优初始位姿。在另一些实施例中，也可以进一步利用步骤156-161对第二图进行优化，并利用优化后的图获得标志码的最优初始位姿。

1056：去除所述第二图中异常的边，获得第三图。

其中，可以通过计算边权重的标准差和均值，去除Pose Graph中异常的边，得到进一步优化的Pose Graph子图(即第三图)。

1057：将所述第一初始节点作为起点和终点，获得至少一个包含多个节点的环路。

1058：基于所述环路中节点间相对位姿的约束关系、以及所述节点间相对位姿和所述最佳相对像素位姿满足的加权平方差最小关系，计算节点间的相对位姿获得节点间的最优相对像素位姿。

在第三图中，以第一初始节点作为起点和终点，建立若干包含多个节点的循环路径c＝(1,...,n)，由于以同一个节点作为环路的起点和终点，该循环路径c中的节点满足约束条件：

为了获得最优相对像素位姿，将最优相对像素位姿和最佳相对像素位姿建立加权平方差最小关系：

其中，权重w_k,k+1是相对位姿的置信度，并且满足：

还同时满足式(7)，利用拉格朗日乘子法，可以获得标志码之间的最优相对像素位姿

1059：分别以所述第三图中各个节点作为起点，获得所述起点的最小生成树。

1060：确定各节点间最优相对像素位姿加权之和最小的最小生成树对应的起点为第二初始节点。

1061：基于佛洛伊德算法计算所述第二初始节点的像素位姿，对所述第二初始节点的像素位姿进行坐标转换，获得所述标志码的最优初始位姿。

将最优相对像素位姿

作为第三图中的边，从第三图中选择一个节点a(γ_a＝I_4×4)作为起点，获得该起点的最小生成树，然后再以剩下的各个节点为起点，分别建立最小生成树。其中，如果节点a到节点i的路径为a,b,...,h,i,则节点i的位姿为：

使所有边的重投影误差的和为最小生成树的代价，使最小生成树代价最小的节点为第二初始节点，即确定各节点间最优相对像素位姿加权之和最小的最小生成树对应的起点为第二初始节点。

在其中一些实施例中，可以基于佛洛伊德算法计算第二初始节点的像素位姿，并对第二初始节点的像素位姿进行坐标变换，作为标志码的最优初始位姿。

在图像中，由于图像的模糊性问题同一个标志码的位姿估计有两个值，因为可以从两个角度得到相同的投影(即标志码在图像中的像素位姿)，

摄像装置的位姿为：

其中，γ_i ^t为t时刻标志码在图像中的像素位姿，

为标志码的最优初始位姿，则在t时刻求摄像装置位姿的问题就变成求t时刻标志码在图像中的像素位姿的问题。求标志码在图像坐标系中的位姿问题需要解决模糊性问题，建立重投影误差最小化问题：

通过求解式(10)可以获得正确的标志码在图像坐标系中的像素位姿γ_i ^t，则通过式(9)可以获得摄像装置t时刻的位姿，即摄像装置的最优初始位姿。

将摄像装置和标志码的最优初始位姿分别代入式(5)中，求解获得摄像装置和标志码的估计位姿。

需要说明的是，为简化算式，便于理解，式(2)中用符号h表示摄像装置的观测方程,式(3)中用符号h″表示陀螺仪观测方程，式(5)中用符号Ψ表示重投影方程、用符号Γ(*)表示世界坐标系与图像坐标系的旋转平移变换矩阵，式(6)和式(10)中用符号Ψ表示重投影方程。该摄像装置的观测方程、陀螺仪观测方程以及重投影方程均为现有技术，可以通过查询现有技术获得，因此在此不再赘述。而且，各坐标系之间的转换关系亦为现有技术，在此亦不予赘述。

图5是本发明机器人的一个实施例中控制器31的硬件结构示意图，如图5所示，控制器31包括：

一个或多个处理器31a以及存储器31b，图5中以一个处理器31a为例。

处理器31a和存储器31b可以通过总线或者其他方式连接，图5中以通过总线连接为例。

存储器31b作为一种非易失性计算机可读存储介质，可用于存储非易失性软件程序、非易失性计算机可执行程序以及模块，如本申请实施例中的定位方法对应的程序指令/模块。处理器31a通过运行存储在存储器31b中的非易失性软件程序、指令以及模块，从而执行控制器的各种功能应用以及数据处理，即实现上述方法实施例的定位方法。

存储器31b可以包括存储程序区和存储数据区，其中，存储程序区可存储操作系统、至少一个功能所需要的应用程序；存储数据区可存储根据控制器的使用所创建的数据等。此外，存储器31b可以包括高速随机存取存储器，还可以包括非易失性存储器，例如至少一个磁盘存储器件、闪存器件、或其他非易失性固态存储器件。在一些实施例中，存储器31b可选包括相对于处理器31a远程设置的存储器，这些远程存储器可以通过网络连接至控制器。上述网络的实例包括但不限于互联网、企业内部网、局域网、移动通信网及其组合。

所述一个或者多个模块存储在所述存储器31b中，当被所述一个或者多个处理器31a执行时，执行上述任意方法实施例中的定位方法，例如，执行以上描述的图4中的方法步骤101至步骤105。

上述产品可执行本申请实施例所提供的方法，具备执行方法相应的功能模块和有益效果。未在本实施例中详尽描述的技术细节，可参见本申请实施例所提供的方法。

本申请实施例提供了一种非易失性计算机可读存储介质，所述计算机可读存储介质存储有计算机可执行指令，该计算机可执行指令被一个或多个处理器执行，例如图5中的一个处理器31a，可使得上述一个或多个处理器可执行上述任意方法实施例中的定位方法，执行以上描述的图4中的方法步骤101至步骤105。

以上所描述的装置实施例仅仅是示意性的，其中所述作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部模块来实现本实施例方案的目的。

通过以上的实施例的描述，本领域普通技术人员可以清楚地了解到各实施例可借助软件加通用硬件平台的方式来实现，当然也可以通过硬件。本领域普通技术人员可以理解实现上述实施例方法中的全部或部分流程是可以通过计算机程序来指令相关的硬件来完成，所述的程序可存储于一计算机可读取存储介质中，该程序在执行时，可包括如上述各方法的实施例的流程。其中，所述的存储介质可为磁碟、光盘、只读存储记忆体(Read-OnlyMemory,ROM)或随机存储记忆体(RandomAccessMemory,RAM)等。

最后应说明的是：以上实施例仅用以说明本发明的技术方案，而非对其限制；在本发明的思路下，以上实施例或者不同实施例中的技术特征之间也可以进行组合，步骤可以以任意顺序实现，并存在如上所述的本发明的不同方面的许多其它变化，为了简明，它们没有在细节中提供；尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的范围。

Claims

1.一种机器人定位方法，所述方法用于机器人，所述机器人包括位移测量装置、惯性测量装置和摄像装置，其特征在于，所述方法包括：

获得标志码坐标系下至少一个角点的角点坐标；

基于图优化算法获得所述标志码的最优初始位姿；

获得所述摄像装置的最优初始位姿；

根据所述角点坐标、所述角点像素坐标、所述标志码的最优初始位姿和所述摄像装置的最优初始位姿、基于所述标志码的重投影误差最小关系获得所述机器人的估计位姿和所述标志码的估计位姿；

其中，所述基于图优化算法获得所述标志码的最优初始位姿，包括：

基于佛洛伊德算法计算所述初始节点的像素位姿，对所述初始节点的像素位姿进行坐标转换，获得所述标志码的最优初始位姿；

或者，所述基于图优化算法获得所述标志码的最优初始位姿，包括：

去除所述第二图中异常的边，获得第三图；

2.根据权利要求1所述的方法，其特征在于，所述方法还包括：

3.根据权利要求1或2所述的方法，其特征在于，所述方法还包括：

基于所述包含标志码的图像获得所述标志码的初始位姿。

4.根据权利要求1所述的方法，其特征在于，所述获得所述摄像装置的最优初始位姿，包括：

5.根据权利要求1所述的方法，其特征在于，所述位移测量装置包括编码器，所述惯性测量装置包括陀螺仪。

6.一种机器人，其特征在于，所述机器人包括位移测量装置、惯性测量装置、摄像装置和控制器，所述控制器分别与所述位移测量装置、所述惯性测量装置和所述摄像装置通信连接；

所述控制器包括：

至少一个处理器；以及，

与所述至少一个处理器通信连接的存储器；其中，

所述存储器存储有可被所述至少一个处理器执行的指令，所述指令被所述至少一个处理器执行，以使所述至少一个处理器能够执行权利要求1-5任一项所述的方法。

7.一种非易失性计算机可读存储介质，其特征在于，所述计算机可读存储介质存储有计算机可执行指令，当所述计算机可执行指令被机器人执行时，使所述机器人执行如权利要求1-5任一项所述的方法。