CN112097768A

CN112097768A - 机器人位姿的确定方法、装置、机器人及存储介质

Info

Publication number: CN112097768A
Application number: CN202011287700.5A
Authority: CN
Inventors: 赖有仿; 刘志超; 黄明强
Original assignee: Ubtech Robotics Corp
Current assignee: Ubtech Robotics Corp
Priority date: 2020-11-17
Filing date: 2020-11-17
Publication date: 2020-12-18
Anticipated expiration: 2040-11-17
Also published as: CN112097768B; WO2022105024A1

Abstract

本申请公开了一种机器人位姿的确定方法，包括：获取机器人的相机采集到的二维码图像，以及所述机器人的惯性传感器采集到的传感数据，并确定所述机器人在预设时长内的里程信息，其中，所述传感数据包括加速度和角速度；根据从所述二维码图像中识别得到的二维码信息和姿态估计函数，确定所述相机的第一位姿，并根据所述传感数据确定所述惯性传感器的第二位姿；基于所述第一位姿和所述第二位姿进行紧耦合优化处理，得到第三位姿；将所述第三位姿和所述里程信息进行融合处理，得到所述机器人的位姿。本申请还公开了一种机器人位姿的确定装置、机器人以及计算机可读存储介质。本申请提高了在复杂场景中定位机器人位姿的准确率。

Description

机器人位姿的确定方法、装置、机器人及存储介质

技术领域

本申请涉及机器人领域，尤其涉及一种机器人位姿的确定方法、机器人位姿的确定装置、机器人以及计算机可读存储介质。

背景技术

在机器人自主执行任务时，获取到准确的、机器人自身的运动姿态，是机器人自主执行任务的完成质量的关键。但目前采用视觉定位建图的移动机器人在复杂场景（比如舞台展台等场所）下容易受到灯光变化、人流等影响，容易导致定位丢失或定位不准确。

因此，目前机器人的定位方式一般难以适应光线复杂、多动态物等复杂场景，导致在复杂场景下定位机器人位姿的准确率低下。

上述内容仅用于辅助理解本申请的技术方案，并不代表承认上述内容是现有技术。

发明内容

本申请的主要目的在于提供一种机器人位姿的确定方法、机器人位姿的确定装置、机器人以及计算机可读存储介质，旨在解决如何提高在复杂场景中定位机器人位姿的准确率的问题。

为实现上述目的，本申请提供一种机器人位姿的确定方法，包括以下步骤：

获取机器人的相机采集到的二维码图像，以及所述机器人的惯性传感器采集到的传感数据，并确定所述机器人在预设时长内的里程信息，其中，所述传感数据包括加速度和角速度；

根据从所述二维码图像中识别得到的二维码信息和姿态估计函数，确定所述相机的第一位姿，并根据所述传感数据确定所述惯性传感器的第二位姿；

基于所述第一位姿和所述第二位姿进行紧耦合优化处理，得到第三位姿；

将所述第三位姿和所述里程信息进行融合处理，得到所述机器人的位姿。

可选的，所述基于所述第一位姿和所述第二位姿进行紧耦合优化处理，得到第三位姿的步骤包括：

根据所述二维码图像和所述传感数据获取边缘化的先验信息；

基于所述边缘化的先验信息、所述第一位姿和所述第二位姿进行紧耦合优化处理，得到第三位姿。

可选的，所述将所述第三位姿和所述里程信息进行融合处理，得到所述机器人的位姿的步骤还包括：

利用扩展卡尔曼滤波算法对所述第三位姿和所述里程信息进行融合处理，得到所述机器人的位姿。

可选的，所述利用扩展卡尔曼滤波算法对所述第三位姿和所述里程信息进行融合处理，得到所述机器人的位姿的步骤之后，还包括：

利用超宽带定位技术将所述机器人的位姿调整为所述机器人在全局地图中的位姿。

可选的，所述利用超宽带定位技术将所述机器人的位姿调整为所述机器人在全局地图中的位姿的步骤之后，还包括：

获取所述二维码信息对应的二维码相对于所述第一位姿的第四位姿；

根据所述第四位姿和所述机器人在全局地图中的位姿，在所述全局地图中标记所述二维码，以构建全局二维码地图。

可选的，所述确定所述机器人在预设时长内的里程信息的步骤包括：

利用轮式里程计确定所述机器人在预设时长内的里程信息；

或者，利用正运动学方法计算所述机器人在预设时长内的里程信息。

可选的，所述相机为红外相机，所述机器人包括红外补光灯；所述获取机器人的相机采集到的二维码图像，以及所述机器人的惯性传感器采集到的传感数据，并确定所述机器人在预设时长内的里程信息的步骤之前，还包括：

检测到所述红外相机在采集所述二维码图像时，则开启所述红外补光灯。

为实现上述目的，本申请还提供一种机器人位姿的确定装置，所述机器人位姿的确定装置包括：

采集模块，用于获取机器人的相机采集到的二维码图像，以及所述机器人的惯性传感器采集到的传感数据，并确定所述机器人在预设时长内的里程信息，其中，所述传感数据包括加速度和角速度；

定位模块，用于根据从所述二维码图像中识别得到的二维码信息和姿态估计函数，确定所述相机的第一位姿，并根据所述传感数据确定所述惯性传感器的第二位姿；

处理模块，用于基于所述第一位姿和所述第二位姿进行紧耦合优化处理，得到第三位姿；

融合模块，用于将所述第三位姿和所述里程信息进行融合处理，得到所述机器人的位姿。

为实现上述目的，本申请还提供一种机器人，所述机器人包括：

所述机器人包括存储器、处理器及存储在所述存储器上并可在所述处理器上运行的机器人位姿的确定程序，所述机器人位姿的确定程序被所述处理器执行时实现如上述机器人位姿的确定方法的步骤。

为实现上述目的，本申请还提供一种计算机可读存储介质，所述计算机可读存储介质上存储有机器人位姿的确定程序，所述机器人位姿的确定程序被处理器执行时实现如上述机器人位姿的确定方法的步骤。

本申请提供的机器人位姿的确定方法、机器人位姿的确定装置、机器人以及计算机可读存储介质，采用相机预估机器人位姿，并通过逐步将预估位姿与融合惯性传感数据、里程信息进行融合，以对机器人的位姿进行不断修正，使得即便在光线复杂和多动态物环境等复杂场景中，依然可以得到机器人准确的位姿，从而提高了在复杂场景下定位机器人位姿的准确率。

附图说明

图1为本申请一实施例中机器人位姿的确定方法步骤示意图；

图2为本申请一实施例中机器人位姿的确定方法另一步骤示意图；

图3为本申请一实施例中机器人位姿的确定方法又一步骤示意图；

图4为本申请一实施例中机器人位姿的确定方法再一步骤示意图；

图5为本申请一实施例中机器人位姿的确定装置示意框图；

图6为本申请一实施例的机器人的结构示意框图。

本申请目的的实现、功能特点及优点将结合实施例，参照附图做进一步说明。

具体实施方式

为了使本申请的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本申请进行进一步详细说明。应当理解，此处描述的具体实施例仅仅用以解释本申请，并不用于限定本申请。

参照图1，在一实施例中，所述机器人位姿的确定方法包括：

步骤S10、获取机器人的相机采集到的二维码图像，以及所述机器人的惯性传感器采集到的传感数据，并确定所述机器人在预设时长内的里程信息，其中，所述传感数据包括加速度和角速度；

步骤S20、根据从所述二维码图像中识别得到的二维码信息和姿态估计函数，确定所述相机的第一位姿，并根据所述传感数据确定所述惯性传感器的第二位姿；

步骤S30、基于所述第一位姿和所述第二位姿进行紧耦合优化处理，得到第三位姿；

步骤S40、将所述第三位姿和所述里程信息进行融合处理，得到所述机器人的位姿。

本实施例中，实施例终端包括但不限于机器人，也可以是控制机器人的计算机设备、一种机器人位姿的确定装置等。以下以机器人作为终端为例进行说明。

可选的，所述机器人可以是轮式机器人，也可以是双足机器人。且机器人可以包括诸如物理键盘、鼠标和/或控制杆的一个或多个其它物理用户接口设备。

如步骤S10所述：机器人的机体上设置有相机和惯性传感器，其中，惯性传感器可以是IMU传感器(Inertial Measurement Unit)，IMU传感器是测量物体三轴姿态角(或角速率)以及加速度的装置。

一般情况，一个IMU包含了三个单轴的加速度计和三个单轴的陀螺仪，加速度计检测物体在载体坐标系统独立三轴的加速度信号，而陀螺仪检测载体相对于导航坐标系的角速度信号，基于此可以测量物体在三维空间中的角速度和加速度。

可选的，在机器人运作的过程中，终端可以通过相机采集机器人运作空间内的图像，以及通过惯性传感器采集机器人的传感数据。其中，传感数据包括加速度和角速度。

可选的，终端获取预设时长内通过相机采集到的图像，以及在预设时长内通过惯性传感器采集到的传感数据。其中，所述预设时长可以由工程师根据实际情况需要预先设置，如30秒、一分钟等。

应当理解的是，在预设时长内，机器人可以是不断行进或转进，或者期间有停歇。

可选的，在机器人运作空间内预先随机或按一定规则布置有多个二维码，且各个二维码均不相同，各二维码均有唯一的标识值与其二维码信息关联。当机器人通过相机在采集图像时，可以将采集到的图像中，存在至少一个二维码的图像作为二维码图像。

可选的，终端从相机在预设时长内采集到的至少一帧二维码图像中，识别出其中多个不同的二维码，并进一步提取每个二维码的四个角点像素作为二维码信息。当然，二维码对应的二维码信息还可以包括该二维码对应的标识值。

进一步地，为了对二维码的四个角点像素进行区分，还可以为每个角点像素分配相应的唯一标识符。

可选的，当机器人是轮式机器人时，则机器人安装有轮式里程计，终端可以通过轮式里程计在预设时长内记录的相关信息（例如轮轴转速），确定机器人在预设时长内的里程信息。其中，通过在机器人两侧轮子上分别安装轮式里程计，当机器人移动时，轮式里程计则可以记录机器人轮子的转速，并通过预先记录的轮子周长，从而推算轮子行进距离。而且根据左右两侧轮子的行进距离的比对，即可得到机器人行进的方向，且根据两侧行进距离的具体比值与转动角度的转换关系，还可以进一步得到机器人在转向时所转动的角度。例如当左侧行进距离大于右侧时，则判定机器人在右转；若检测到两侧行进距离相等，则判定机器人在直行；当右侧行进距离大于左侧时，则判定机器人在左转。基于上述轮式里程计的运作原理，即可得到机器人的里程信息。

可选的，当机器人是双足机器人时，则终端可以通过采集预设时长内机器人各关节的变量，采用正运动学方式计算机器人在预设时长内的里程信息。需要说明的是，在机器人运动学中，正运动学是指已知机器人各关节变量，计算机器人的位置姿态的方式。

其中，里程信息包括机器人的里程、加速度和速度，而在里程、加速度和速度基础上，可以进一步估计出机器人的位置和姿态（简称位姿，可记为第五位姿），即里程信息至少包括第五位姿。当然，由于一般采用里程信息估算机器人位姿容易受场地地形影响而存在误差，因此本实施例中不直接采用第五位姿作为最终确定的得到的机器人位姿。

需要说明的是，机器人的位姿，指的机器人所在的位置以及在该位置上机器人的朝向，此时机器人的朝向可以是相当于局部坐标系（或者世界坐标系）而言的。

局部坐标系是以物体的中心为坐标原点，物体的旋转、平移等操作都是围绕局部坐标系进行的坐标系，而世界坐标系是以标志物为世界坐标平面，且定义坐标原点为标志物已知的某一点的坐标系。世界坐标系与局部坐标系之间通常可以通过相应的矩阵变换进行相互转变。

如步骤S20所述：终端在提取得到二维码信息后，可基于二维码信息和姿态估计函数（solvePnP），计算相机在相机坐标系（即相机的局部坐标系）中的第一位姿（或称第一里程信息），并将第一位姿转换到IMU坐标系中（即IMU的局部坐标系）。

需要说明的是，基于相机和IMU传感器之间的相对位置关系，可以建立两者之间的局部坐标系的转换关系，基于该转换关系，可以将彼此局部坐标系中的位姿进行相互转换。而由于相机和IMU传感器均安装在机器人上，因此两者之间的相对位置是在构建机器人时就已经确定的，并可由相关开发人员将两者之间的相对位置保存在机器人的数据库中。

可选的，第一位姿

的具体计算公式如下：

其中，X为系统状态向量，此处表示为相机在世界坐标系中的位姿；

表示观测值，是空间中的特征点投影到图像上的像素坐标（像素坐标是指某个点在图像上的成像位置），用于对X进行修正；j、“l”（英文字母L小写）表示某个常数，P表征为特征点（即二维码的角点对应的像素点）在归一化相机坐标系的坐标，（u，s）则为所述特征点具体的坐标值，因此

，则表征为第“l”（英文字母L小写）个二维码的角点在第“j”个相机在归一化相机坐标系中的观察到的坐标；

，表征为二维码角点在归一化相机坐标系下的坐标；

，是估计第“l”（英文字母L小写）个二维码的角点在第“j”个相机归一化相机坐标系中的可能的坐标；C为图像样本数。其中，相机坐标系中的坐标可先转换到IMU坐标系中，这样最终得到的第一位姿就是参考IMU坐标系的。

可选的，终端在提取得到传感数据后，可基于传感数据进行积分运算，从而计算IMU传感器在IMU坐标系（即IMU的局部坐标系）中的第二位姿（或称第二里程信息）。第二位姿r_B的具体计算公式如下：

其中，α, β, 和γ是IMU预积分量，采用欧拉法离散形式，可分别表示为：

进一步地，在第二位姿的计算公式中，X为系统状态向量，此处表示为IMU传感器在世界坐标系中的位姿；

表示观测值，是IMU在两帧图像之间积分的位姿与由图像通过solvePnP获得的这两帧图像位姿变换之间的一个差，用于对X进行修正；B为IMU的测量数据集，常数向量

，b_k表示为图像的某一帧，故b_k-1即为该帧图像的前一帧，即b_k和b_k-1用于表示前后两帧图像；i表示某一帧IMU数据；θ是旋转角度，δt是IMU帧时间间隔，a和ω分别为IMU输出的加速度和角速度，b_a与b_g分别表示IMU中陀螺仪和加速度计的偏差，R是旋转矩阵，p是位移向量，v是速度向量，q是旋转四元素，g为重力向量。

如步骤S30所述：采用最小二乘法对目标函数进行紧耦合优化处理，可以使得目标函数最小化。其中，目标函数即为第一位姿和第二位姿的组合。

可选的，终端在得到第一位姿和第二位姿后，利用最小二乘法对目标函数进行紧耦合优化处理，得到第三位姿。第三位姿r_A的具体计算公式可如下所示：

其中，

为预积分噪声项协方差，

为噪声协方差。

这样，相当于将第一位姿和第二位姿分别作为两个残差，利用最小二乘法对第一位姿和第二位姿进行拟合，迭代求解出最优值，并将最优值作为第三位姿。

可选的，第一位姿也可以是相对于相机的局部坐标系而言的，并在将第一位姿和第二位姿进行融合之前，将第一位姿和第二位姿均转换至同一世界坐标系后，再将在世界坐标系中的第一位姿和第二位姿进行融合，得到参考世界坐标系的第三位姿。

如步骤S40所述：终端在得到第三位姿和里程信息后，可以是采用滤波处理的方法，以所述里程信息作为观测量，以所述第三位姿作为状态量，利用所述观测量对所述状态量进行校正，从而实现对第三位姿和里程信息进行融合处理，得到基于里程信息校正后的第三位姿，作为最终确定得到的机器人的位姿（可记为第六位姿）。

其中，当第三位姿是参考IMU坐标系时，则先将里程信息对应的第五位姿从其原局部坐标系中转换至IMU坐标系后，再将基于IMU坐标系中的第三位姿和第五位姿进行融合，得到参考IMU坐标系的机器人位姿；当第三位姿是参考世界坐标系时，则先将里程信息对应的第五位姿从其原局部坐标系中转换至世界坐标系后，再将基于世界坐标系中的第三位姿和第五位姿进行融合，得到参考世界坐标系的机器人位姿。

在一实施例中，采用相机预估机器人位姿，并通过逐步将预估位姿与融合惯性传感数据、里程信息进行融合，以对机器人的位姿进行不断修正，使得即便在光线复杂和多动态物环境等复杂场景中，依然可以得到机器人准确的位姿，从而提高了在复杂场景下定位机器人位姿的准确率。

在一实施例中，如图2所示，在上述图1所示的实施例基础上，所述基于所述第一位姿和所述第二位姿进行紧耦合优化处理，得到第三位姿的步骤包括：

步骤S31、根据所述二维码图像和所述传感数据获取边缘化的先验信息；

步骤S32、基于所述边缘化的先验信息、所述第一位姿和所述第二位姿进行紧耦合优化处理，得到第三位姿。

本实施例中，将滑动窗口中的完整状态向量定义为：

其中，X为系统状态向量，

是相机捕获二维码图像时的采集到IMU数据（即每帧二维码图像对应的传感数据），包含IMU在世界坐标系的位置、速度和方向，以及在IMU坐标系中的加速度计偏置和陀螺仪偏置，n是关键帧的总数，m是滑动窗口中的特征总数，λ是观测到的特征的逆深度。

可选的，有选择地将

和特征点λ从滑动窗口边缘化（Marginalization），同时将对应于边缘状态的测量值转换为边缘化的先验信息：{r_e，J_e}。这样，获取边缘化的先验信息的过程，相当于将基于二维码图像，以及二维码图像对应的IMU数据得到的先验信息，进行边缘化后，得到边缘化的先验信息。其中，边缘化是利用舒尔补算法进行的。

终端在得到边缘化的先验信息、第一位姿和第二位姿后，利用最小二乘法对边缘化的先验信息、第一位姿和第二位姿进行紧耦合优化处理，得到第三位姿。第三位姿r_A的具体计算公式可如下所示：

这样，通过将边缘化的先验信息加入到第一位姿和第二位姿的融合处理当中，可以提高基于此得到的第三位姿的准确度。

在一实施例中，如图3所示，在上述图1至图2的实施例基础上，所述将所述第三位姿和所述里程信息进行融合处理，得到所述机器人的位姿的步骤还包括：

步骤S41、利用扩展卡尔曼滤波算法对所述第三位姿和所述里程信息进行融合处理，得到所述机器人的位姿。

本实施例中，终端在得到第三位姿和里程信息后，可以采用扩展卡尔曼滤波算法（Extended kalman filter, EKF），将第三位姿和里程信息进行融合处理。扩展卡尔曼滤波算法是标准卡尔曼滤波在非线性情形下的一种扩展形式，它是一种高效率的递归滤波器（自回归滤波器）。EKF的基本思想是利用泰勒级数展开将非线性系统线性化，然后采用卡尔曼滤波框架对信号进行滤波。

可选的，机器人上安装有EKF滤波器，终端可以将第三位姿和里程信息输入到EKF滤波器中进行融合处理，并获取EKF滤波器经数据处理后输出的第六位姿，作为最终确定得到的机器人的位姿。需要说明的是，EKF滤波器可对多传感器输入的数据进行融合，其中，第三位姿和里程信息可以当作是不同传感器输入到EKF滤波器中的数据。

在终端利用EKF滤波器对第三位姿和里程信息进行融合处理的过程中，将第三位姿和里程信息作为局部坐标系的初始参考值进行数据初始化。

可选的，由于第三位姿和里程信息是基于预设时长内的采集到数据确定得到的，因此，在预设时长内的第三位姿和里程信息均可以有多个，即机器人预设时长中每个时刻对应的第三位姿和里程信息都可能不同。而且在EKF中，定义上一时刻的数据为测量值，当前时刻的数据为预测值（或者预估值），因此在基于第三位姿和里程信息构建的局部坐标系中，确定当前时刻的第三位姿相对于上一时刻的第三位姿的第一协方差矩阵，以及确定当前时刻的里程信息相对于上一时刻的里程信息的第二协方差矩阵。

进一步地，终端还将IMU传感数据输入到EKF滤波器中，并将传感数据作为实际测量值，结合上一时刻的第三位姿、上一时刻的里程信息、第一协方差矩阵、第二协方差矩阵作参考值，输入到EKF滤波算法的融合计算当中，最终得到机器人当前时刻在IMU坐标系中的预估值，以生成机器人的位姿（可记为第六位姿）。当然，也可以将第六位姿转换至世界坐标系中。

需要说明的是，在融合计算过程中，具体涉及到的EKF滤波算法的状态转移方程、观测方程、融合算法（包括泰勒展开式），可采用传统EKF滤波算法提供的相关公式，或者其他EKF滤波算法的变种的公式，本申请对此不作限定。

这样，基于扩展卡尔曼滤波算法融合机器人在预设时长内的第三位姿和里程信息，可以提高最终得到的机器人的位姿的准确度。

在一实施例中，如图4所示，在上述图1至图3的实施例基础上，所述利用扩展卡尔曼滤波算法对所述第三位姿和所述里程信息进行融合处理，得到所述机器人的位姿的步骤之后，还包括：

步骤S50、利用超宽带定位技术将所述机器人的位姿调整为所述机器人在全局地图中的位姿。

本实施例中，机器人安装有超宽带定位模块（UWB-Ultra Wideband，简称UWB），机器人作用空间内预先布置有多个可以UWB模块通信的基站。

可选的，终端可以通过UWB模块与多个基站进行通信测距，并通过分别与至少三个基站进行无线测距得到测距值，可对机器人进行室内定位，从而得到机器人在全局坐标系中的坐标。

需要说明的是，全局坐标系是三维空间物体所在的坐标系，模型的顶点坐标就是基于这个坐标系来表达的。而局部坐标系是一个假想的坐标系，该坐标系与物体的相对位置至始至终是不变的，假想出这个坐标系的目的主要是为了正向理解对三维场景中物体执行的“平移和旋转”操作。应当理解的是，当世界坐标系的参照物与全局坐标系的参照物一致时，则世界坐标系可以等于全局坐标系，即存在世界坐标系等于全局坐标系的可能。

可选的，根据机器人在全局坐标系中的坐标，结合之前得到的机器人在IMU坐标系中的位姿，可以得到机器人在全局坐标系中的位姿。由于机器人在IMU坐标系中的位姿是包括方向和坐标的，因此，虽然IMU坐标系的坐标（记为第一坐标）和全局坐标系中的坐标（记为第二坐标）所表示的形式不同，但这两个坐标在真实世界中所指向的位置是相同的，故，终端可以通过建立IMU坐标系的坐标和全局坐标系中的坐标之间的第一对应关系，并基于第一对应关系，将机器人在局部坐标系中的方向，调整为在全局坐标系中的方向，并结合第二坐标，即可得到机器人在全局坐标系中的位姿。

可选的，终端可以在EKF滤波器输出机器人的位姿之前，将基于UWB技术得到的第二坐标作为全局定位对EKF滤波器进行更新，这样EKF滤波器就可以直接输出机器人在全局坐标系中的位姿。

可选的，终端在利用UWB模块根据多个测距值计算第二坐标前，可以根据以往历史记录（即预设时长之前的记录）的测距值以及历史第二坐标，预估当前时刻的合理测距值范围，并将当前检测到的超出合理测距值范围的测距值剔除后，利用剩余的测距值计算第二坐标。这样，可以提高对机器人定位的准确性。

进一步地，终端在得到机器人在全局坐标系中的位姿后，可以结合真实世界的地图，生成全局地图，并在全局地图中显示机器人的位姿。例如可以是以一个箭头图标显示机器人位姿，且箭头所指方向即为机器人的朝向。

这样，通过利用超宽带定位技术将所述机器人的位姿调整为所述机器人在全局地图中的位姿，可以方便用户直观感受到机器人在真实世界的位姿，从而提高用户体验。

在一实施例中，在上述实施例基础上，所述利用超宽带定位技术将所述机器人的位姿调整为所述机器人在全局地图中的位姿的步骤之后，还包括：

步骤S60、获取所述二维码信息对应的二维码相对于所述第一位姿的第四位姿；

步骤S61、根据所述第四位姿和所述机器人在全局地图中的位姿，在所述全局地图中标记所述二维码，以构建全局二维码地图。

本实施例中，终端在提取得到二维码信息后，可基于二维码信息和姿态估计函数（solvePnP），计算相机在相机坐标系（即相机的局部坐标系）中的第一位姿（或称第一里程信息）。同时，由于用于计算第一位姿的各二维码信息对应的二维码，其位姿其实是与第一位姿相对的（即第一位姿的方向是朝向二维码的，且第一位姿的位置是根据至少三个二维码的位置确定的），因此终端可以通过二维码与相机之间的相对位置关系对第一位姿计算过程进行反向推导，以确定用于计算第一位姿的各二维码信息对应的二维码在相机坐标系中相对于第一位姿的位姿，记为第四位姿。当然，终端可以进一步将第四位姿转换到IMU坐标系或世界坐标系中。

进一步地，终端可以建立第四位姿和第一位姿之间的第二对应关系（包括两者之间的距离关系，和朝向关系），当终端得到机器人在全局地图中的位姿（记为第七位姿）后，可以将第七位姿作为全局地图中的基准点，根据第二对应关系以及第一对应关系，将第四位姿转换为全局地图中的第八位姿，然后在全局地图中标记每个第八位姿对应的二维码，从而构建全局二维码地图。

这样一来，即可提高利用机器人定位构建全局二维码地图的精度，而且当机器人绕场一周后，即可将现场所有二维码的布置位姿（包括位置和朝向）显示到全局二维码地图上，还能节省人工生成全局二维码地图的成本。

在一实施例中，在上述实施例基础上，所述相机为红外相机，所述机器人包括红外补光灯；所述获取机器人的相机采集到的二维码图像，以及所述机器人的惯性传感器采集到的传感数据，并确定所述机器人在预设时长内的里程信息的步骤之前，还包括：

步骤S70、检测到所述红外相机在采集所述二维码图像时，则开启所述红外补光灯。

本实施例中，机器人的相机为红外相机，而且机器人上还安装有红外补光灯。当终端检测到红外相机在采集图像时，则开启红外补光灯，从而在红外相机采集图像的过程中对其进行红外光照补偿。

其中，红外相机可以是采用红外波段940nm窄波段的相机。

这样，在光线变化强烈的场景下，就可以避免可见光波段对相机采集二维码图像造成影响，从而可以得到更为准确的相机位姿，提高了基于相机位姿生成的机器人位姿的准确率。

参照图5，本申请实施例中还提供一种机器人位姿的确定装置100，包括：

采集模块101，用于获取机器人的相机采集到的二维码图像，以及所述机器人的惯性传感器采集到的传感数据，并确定所述机器人在预设时长内的里程信息，其中，所述传感数据包括加速度和角速度；

定位模块102，用于根据从所述二维码图像中识别得到的二维码信息和姿态估计函数，确定所述相机的第一位姿，并根据所述传感数据确定所述惯性传感器的第二位姿；

处理模块103，用于基于所述第一位姿和所述第二位姿进行紧耦合优化处理，得到第三位姿；

融合模块104，用于将所述第三位姿和所述里程信息进行融合处理，得到所述机器人的位姿。

参照图6，本申请实施例中还提供一种机器人，该机器人的内部结构可以如图6所示。该机器人包括通过系统总线连接的处理器、存储器、网络接口和数据库。其中，该机器人的处理器用于提供计算和控制能力。该机器人的存储器包括非易失性存储介质、内存储器。该非易失性存储介质存储有操作系统、计算机程序和数据库。该内存储器为非易失性存储介质中的操作系统和计算机程序的运行提供环境。该机器人的数据库用于机器人位姿的确定程序。该机器人的网络接口用于与外部的终端通过网络连接通信。该计算机程序被处理器执行时以实现一种机器人位姿的确定方法。

本领域技术人员可以理解，图6中示出的结构，仅仅是与本申请方案相关的部分结构的框图，并不构成对本申请方案所应用于其上的机器人的限定。

此外，本申请还提出一种计算机可读存储介质，所述计算机可读存储介质包括机器人位姿的确定程序，所述机器人位姿的确定程序被处理器执行时实现如以上实施例所述的机器人位姿的确定方法的步骤。可以理解的是，本实施例中的计算机可读存储介质可以是易失性可读存储介质，也可以为非易失性可读存储介质。

综上所述，为本申请实施例中提供的机器人位姿的确定方法、机器人位姿的确定装置、机器人和存储介质，采用相机预估机器人位姿，并通过逐步将预估位姿与融合惯性传感数据、里程信息进行融合，以对机器人的位姿进行不断修正，使得即便在光线复杂和多动态物环境等复杂场景中，依然可以得到机器人准确的位姿，从而提高了在复杂场景下定位机器人位姿的准确率。

本领域普通技术人员可以理解实现上述实施例方法中的全部或部分流程，是可以通过计算机程序来指令相关的硬件来完成，所述的计算机程序可存储于一非易失性计算机可读取存储介质中，该计算机程序在执行时，可包括如上述各方法的实施例的流程。其中，本申请所提供的和实施例中所使用的对存储器、存储、数据库或其它介质的任何引用，均可包括非易失性和/或易失性存储器。非易失性存储器可以包括只读存储器（ROM）、可编程ROM（PROM）、电可编程ROM（EPROM）、电可擦除可编程ROM（EEPROM）或闪存。易失性存储器可包括随机存取存储器（RAM）或者外部高速缓冲存储器。作为说明而非局限，RAM通过多种形式可得，诸如静态RAM（SRAM）、动态RAM（DRAM）、同步DRAM（SDRAM）、双速据率SDRAM（SSRSDRAM）、增强型SDRAM（ESDRAM）、同步链路（Synchlink）DRAM（SLDRAM）、存储器总线（Rambus）直接RAM（RDRAM）、直接存储器总线动态RAM（DRDRAM）、以及存储器总线动态RAM（RDRAM）等。

需要说明的是，在本文中，术语“包括”、“包含”或者其任何其它变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、装置、物品或者方法不仅包括那些要素，而且还包括没有明确列出的其它要素，或者是还包括为这种过程、装置、物品或者方法所固有的要素。在没有更多限制的情况下，由语句“包括一个……”限定的要素，并不排除在包括该要素的过程、装置、物品或者方法中还存在另外的相同要素。

以上所述仅为本申请的优选实施例，并非因此限制本申请的专利范围，凡是利用本申请说明书及附图内容所作的等效结构或等效流程变换，或直接或间接运用在其它相关的技术领域，均同理包括在本申请的专利保护范围内。

Claims

1.一种机器人位姿的确定方法，其特征在于，所述机器人位姿的确定方法包括：

2.如权利要求1所述的机器人位姿的确定方法，其特征在于，所述基于所述第一位姿和所述第二位姿进行紧耦合优化处理，得到第三位姿的步骤包括：

3.如权利要求1或2所述的机器人位姿的确定方法，其特征在于，所述将所述第三位姿和所述里程信息进行融合处理，得到所述机器人的位姿的步骤还包括：

4.如权利要求3所述的机器人位姿的确定方法，其特征在于，所述利用扩展卡尔曼滤波算法对所述第三位姿和所述里程信息进行融合处理，得到所述机器人的位姿的步骤之后，还包括：

5.如权利要求4所述的机器人位姿的确定方法，其特征在于，所述利用超宽带定位技术将所述机器人的位姿调整为所述机器人在全局地图中的位姿的步骤之后，还包括：

6.如权利要求1所述的机器人位姿的确定方法，其特征在于，所述确定所述机器人在预设时长内的里程信息的步骤包括：

利用轮式里程计确定所述机器人在预设时长内的里程信息；

7.如权利要求1所述的机器人位姿的确定方法，其特征在于，所述相机为红外相机，所述机器人包括红外补光灯；所述获取机器人的相机采集到的二维码图像，以及所述机器人的惯性传感器采集到的传感数据，并确定所述机器人在预设时长内的里程信息的步骤之前，还包括：

检测到所述红外相机在采集所述二维码图像时，开启所述红外补光灯。

8.一种机器人位姿的确定装置，其特征在于，包括：

9.一种机器人，其特征在于，所述机器人包括存储器、处理器及存储在所述存储器上并可在所述处理器上运行的机器人位姿的确定程序，所述机器人位姿的确定程序被所述处理器执行时实现如权利要求1至7中任一项所述的机器人位姿的确定方法的步骤。

10.一种计算机可读存储介质，其特征在于，所述计算机可读存储介质上存储有机器人位姿的确定程序，所述机器人位姿的确定程序被处理器执行时实现如权利要求1至7中任一项所述的机器人位姿的确定方法的步骤。