CN111098335B - 一种双轮差速驱动机器人里程计标定方法及装置 - Google Patents

Abstract

本申请提出一种双轮差速驱动机器人里程计标定方法及装置。该方法包括：获取机器人在第一运动测量时刻的左右轮运动增量及时间增量；确定所述机器人的运动参数及运动参数的协方差矩阵；预测所述机器人在第一运动测量时刻、第一位姿测量时刻的状态向量和状态向量的协方差矩阵；获取所述机器人在第一位姿测量时刻的位姿及位姿测量误差；修正所述机器人在第一位姿测量时刻的预测结果；若确定最新的连续多个位姿测量时刻修正得到的状态向量满足收敛条件，则根据所述所述最新的连续多个位姿测量时刻修正得到的状态向量确定所述机器人里程计的标定结果。

Description

一种双轮差速驱动机器人里程计标定方法及装置

技术领域

本申请涉及人工智能领域，尤其涉及一种双轮差速驱动机器人里程计标定方法及装置。

背景技术

对于双轮差速移动机器人，常利用安装在左右两个驱动轮电机上的光电编码器来检测车轮在一定时间内转过的弧度，并结合机器人的运动学参数(左右轮径、轮距)来推算机器人相对位姿的变化。但是，由于制造误差、安装误差、参数测量误差以及运动过程中的磨损等因素的影响，机器人运动学参数存在系统误差，运动学参数的实际值和标称值不一致，从而导致基于这些运动参数的位姿和轨迹计算存在偏差。而里程计作为计算移动机器人相对位姿变化和机器人运动轨迹推演的方法和装置，具有广泛的应用前景与研究价值。

目前，工程技术人员主要通过将里程计推算得到的位姿轨迹信息与其他方式得到的轨迹位姿信息进行对比，并根据比较结果对里程计参数进行修正或者补偿，这种方法存在两个局限性：

(1)标定结果依赖于其他方式获取的轨迹位姿信息的准确度，忽略了测量误差的影响；

(2)自动化程度低，通常需要手动输入轨迹参数，标定效率低。

因此，需要提供一种用于提高双轮差速驱动机器人标定精度与效率的方法。

发明内容

本申请实施例提供了一种双轮差速驱动机器人里程计标定方法及装置，用以提高双轮差速驱动机器人的标定精度。

本申请实施例提供的一种双轮差速驱动机器人里程计标定方法，包括：

获取机器人按照标定轨迹运动时在第一运动测量时刻的左右轮运动增量；

根据所述左右轮运动增量确定所述机器人的运动参数及运动参数的协方差矩阵，根据所述运动参数以及所述机器人在上一测量时刻的状态向量预测所述机器人在第一运动测量时刻的状态向量，并根据所述运动参数及运动参数的协方差矩阵、所述机器人在上一测量时刻的状态向量和状态向量的协方差矩阵，预测所述机器人在第一运动测量时刻的状态向量的协方差矩阵；其中，所述上一测量时刻包括运动测量时刻和位姿测量时刻；

预测机器人在第一位姿测量时刻的状态向量及状态向量的协方差矩阵，所述第一位姿测量时刻不小于第一运动测量时刻；

获取所述机器人在第一位姿测量时刻的位姿及位姿测量误差；

若第一运动测量时刻和第一位姿测量时刻之间不存在其他运动测量时刻，根据第一位姿测量时刻获取到的所述位姿和位姿测量误差，对预测得到的所述机器人在第一位姿测量时刻的状态向量、状态向量的协方差矩阵进行修正；

若确定最新的连续多个位姿测量时刻修正得到的状态向量满足收敛条件，则根据所述最新的连续多个位姿测量时刻修正得到的状态向量确定所述机器人里程计标定结果；其中，所述最新的连续多个位姿测量时刻包括第一位姿测量时刻及距离第一位姿测量时刻最近的至少一个位姿测量时刻。

可选的，获取机器人按照标定轨迹运动时在第一运动测量时刻的左右轮运动增量及在第一位姿测量时刻的位姿、位姿测量误差后，还包括：

获取机器人在第一运动测量时刻的左右轮运动增量、第一运动测量时刻相对于上一运动测量时刻的时间增量、第一运动测量时刻相对于上一测量时刻的时间增量，确定机器人在第一运动测量时刻的运动参数和运动参数的协方差矩阵；根据机器人在第一运动测量时刻的运动参数和运动参数的协方差矩阵、机器人在第一位姿测量时刻经过修正的状态向量和状态向量的协方差矩阵，预测机器人在第一运动测量时刻的状态向量和状态向量的协方差矩阵；其中，所述上一测量时刻为第一位姿测量时刻；

获取机器人在第二运动测量时刻的左右轮运动增量、第二运动测量时刻相对于第一运动测量时刻的时间增量、第二运动测量时刻相对于上一测量时刻的时间增量，确定机器人在第二运动测量时刻的运动参数和运动参数的协方差矩阵；根据机器人在第二运动测量时刻的运动参数和运动参数的协方差矩阵、机器人在第一运动测量时刻预测得到的状态向量和状态向量的协方差矩阵，预测机器人在第二运动测量时刻的状态向量和状态向量的协方差矩阵；其中，所述上一测量时刻为第一运动测量时刻；

获取机器人在第二位姿测量时刻的位姿和位姿测量误差、第二位姿测量时刻相对于上一测量时刻的时间增量、上一测量时刻的运动参数，确定机器人在第二位姿测量时刻的运动参数和运动参数的协方差矩阵；根据机器人在第二位姿测量时刻的运动参数和运动参数的协方差矩阵、机器人在第二运动测量时刻预测得到的状态向量和状态向量的协方差矩阵，预测机器人在第二位姿测量时刻的状态向量和状态向量的协方差矩阵；根据机器人在第二位姿测量时刻的位姿和位姿测量误差，对机器人在第二位姿测量时刻预测得到的状态向量和状态向量的协方差矩阵进行修正，得到修正后的机器人在第二位姿测量时刻的状态向量和状态向量的协方差矩阵；其中，所述上一测量时刻为第二运动测量时刻；

其中，所述第一位姿测量时刻、第二位姿测量时刻为相邻的位姿测量时刻，所述第一运动测量时刻和第二运动测量时刻为相邻的运动测量时刻，且所述第一运动测量时刻和第二运动测量时刻位于所述第一位姿测量时刻和第二位姿测量时刻之间。

可选的，确定最新的连续多个位姿测量时刻修正得到的状态向量满足收敛条件，具体包括：

根据最新的连续多个位姿测量时刻修正得到的状态向量，确定机器人的右轮直径系数、左轮直径系数以及左右轮距修正系数的标准差；

若机器人的右轮直径系数、左轮直径系数以及左右轮距修正系数的标准差均小于设定的阈值，则确定满足收敛条件。

可选的，根据最新的连续多个位姿测量时刻修正得到的状态向量确定机器人里程计的标定结果，具体包括：

若所述机器人的右轮直径系数、左轮直径系数以及左右轮距修正系数的标准差均小于设定的阈值，计算最新的连续多个位姿测量时刻修正得到的状态向量中的运动学参数修正系数的均值；

将该均值确定为机器人在第一位姿测量时刻的运动学参数修正系数的标定结果；

本申请的实施例中，状态向量，包括机器人的运动位姿、运动学参数的修正系数。

本申请实施例提供的一种双轮差速驱动机器人里程计标定装置，包括：

运动测量模块，用于获取运动机器人按照控制模块的标定轨迹运动时里程计在任一运动测量时刻输出的左右轮运动增量；

位姿测量模块，用于获取机器人在任一位姿测量时刻的位姿及位姿测量误差；

标定确定模块，用于根据左右轮运动增量确定机器人的运动参数及运动参数的协方差矩阵，根据运动参数以及机器人在上一测量时刻的状态向量预测机器人在第一运动测量时刻的状态向量，并根据运动参数及运动参数的协方差矩阵、机器人在上一测量时刻的状态向量和状态向量的协方差矩阵，预测机器人在第一运动测量时刻的状态向量的协方差矩阵；其中，上一测量时刻包括运动测量时刻和位姿测量时刻；

预测所述机器人第一位姿测量时刻的状态向量及状态向量的协方差矩阵，其中，所述第一位姿测量时刻不小于第一运动测量时刻；

获取所述机器人在第一位姿测量时刻的位姿及位姿测量误差；

若确定最新的连续多个位姿测量时刻修正得到的状态向量满足收敛条件，则根据所述最新的连续多个位姿测量时刻修正得到的状态向量确定所述机器人里程计标定结果；其中，所述最新的连续多个位姿测量时刻包括第一位姿测量时刻及距离第一位姿测量时刻最近的至少一个位姿测量时刻。

可选的，运动测量模块、位姿测量模块、标定确定模块还用于，包括：

位姿测量模块，还用于获取机器人在第一运动测量时刻的左右轮运动增量、第一运动测量时刻相对于上一运动测量时刻的时间增量、第一运动测量时刻相对于上一测量时刻的时间增量；

获取机器人在第二运动测量时刻的左右轮运动增量、第二运动测量时刻相对于第一运动测量时刻的时间增量、第二运动测量时刻相对于上一测量时刻的时间增量；

位姿测量模块，还用于获取机器人在第二位姿测量时刻的位姿和位姿测量误差；获取机器人在第二位姿测量时刻相对于上一测量时刻的时间增量；

标定确定模块，还用于根据运动测量模块确定机器人在第一运动测量时刻的运动参数和运动参数的协方差矩阵；根据机器人在第一运动测量时刻的运动参数和运动参数的协方差矩阵、机器人在第一位姿测量时刻经过修正的状态向量和状态向量的协方差矩阵，预测机器人在第一运动测量时刻的状态向量和状态向量的协方差矩阵；

根据运动测量模块确定机器人在第二运动测量时刻的运动参数和运动参数的协方差矩阵；根据机器人在第二运动测量时刻的运动参数和运动参数的协方差矩阵、机器人在第一运动测量时刻预测得到的状态向量和状态向量的协方差矩阵，预测机器人在第二运动测量时刻的状态向量和状态向量的协方差矩阵；

根据位姿测量模块确定机器人在第二位姿测量时刻的运动参数和运动参数的协方差矩阵；根据机器人在第二位姿测量时刻的运动参数和运动参数的协方差矩阵、机器人在第二运动测量时刻预测得到的状态向量和状态向量的协方差矩阵，预测机器人在第二位姿测量时刻的状态向量和状态向量的协方差矩阵；根据机器人在第二位姿测量时刻的位姿和位姿测量误差，对机器人在第二位姿测量时刻预测得到的状态向量和状态向量的协方差矩阵进行修正，得到修正后的机器人在第二位姿测量时刻的状态向量和状态向量的协方差矩阵；

其中，所述第一位姿测量时刻、第二位姿测量时刻为相邻的位姿测量时刻，所述第一运动测量时刻和第二运动测量时刻为相邻的运动测量时刻，且所述第一运动测量时刻和第二运动测量时刻位于所述第一位姿测量时刻和第二位姿测量时刻之间。

可选的，标定确定模块，具体用于：

根据最新的连续多个位姿测量时刻修正得到的状态向量，确定机器人的右轮直径系数、左轮直径系数以及左右轮距修正系数的标准差；若机器人的右轮直径系数、左轮直径系数以及左右轮距修正系数的标准差均小于设定的阈值，则确定满足收敛条件，否则不满足。

可选的，标定确定模块，具体用于：

若机器人的右轮直径系数、左轮直径系数以及左右轮距修正系数的标准差均小于设定的阈值，计算最新的连续多个位姿测量时刻修正得到的状态向量中的运动学参数修正系数的均值，将该均值确定为机器人运动学参数修正系数的标定结果。

可选的，状态向量包括：机器人的运动位姿、运动学参数的修正系数。

本申请实施例提供的一种机器人，包括机器人本体、运动控制模块以及双轮差速驱动机器人里程计标定装置，其中，机器人本体包括左右轮以及里程计，里程计用于输出左右轮运动增量及时间增量，左右轮运动增量是基于左右轮径的标称值得到的；运动控制模块，用于控制左右轮进行运动。

本申请的实施例中，在双轮驱动机器人里程计标定过程中，一方面，利用其他方式获取的机器人的轨迹位姿及位姿测量误差修正里程计预测得到的机器人轨迹位姿信息，提高了里程计的标定精度；另一方面，基于连续若干预测得到的状态向量确定机器人状态向量的标定结果，消除了人为因素的干扰，提高了自动化程度。

附图说明

为了更清楚地说明本申请实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图是本申请的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。

图1示例性示出了本申请实施例提供的标定系统结构图；

图2示例性示出了本申请实施例中的标定确定模块303的结构图；

图3示例性示出了本申请实施例双轮差速驱动机器人里程计标定流程示意图；

图4示例性示出了本申请实施例提供的运动测量和位姿测量过程及时间示意图。

具体实施方式

本申请实施例提供一种双轮差速驱动机器人里程计标定方法及装置，用于在线自动标定双轮差速驱动机器人里程计运动学参数，提高里程计的标定精度与效率。

为使本申请示例性实施例的目的、技术方案和优点更加清楚，下面将结合本申请示例性实施例中的附图，对本申请示例性实施例中的技术方案进行清楚、完整地描述。

图1示例性示出了本申请实施例提供的标定系统结构图。如图1所示，包括机器人本体100、运动控制模块200和双轮差速驱动机器人里程计标定装置300。

具体的，机器人本体100包括左右轮101及里程计102。里程计用于输出左右轮运动增量，左右轮运动增量是基于左右轮径的标称值得到的。

运动控制模块200用于控制左右轮运动。

双轮差速驱动机器人里程计标定装置300包括运动测量模块301、位姿测量模块302和标定确定模块303。其中，运动测量模块301用于获取里程计输出的机器人的左右轮运动增量和时间增量；位姿测量模块302用于获取机器人的位姿及位姿测量误差，该位姿测量模块302可选激光定位、视觉定位、全球定位系统或惯导等能够实现位姿检测的装置；标定确定模块303用于处理运动测量模块301获取的机器人的左右轮运动增量、时间增量和位姿测量模块302获取的机器人的位姿及位姿测量误差，得到该机器人的标定结果。

图2示例性示出了本申请实施例中的标定确定模块303的结构图。如图2所示，包括机器人位姿预测模块3031，机器人位姿解算模块3032，融合更新模块3033，收敛状态判定模块3034，具体的：

机器人位姿预测模块3031，用于根据机器人的左右轮运动增量及时间增量确定机器人的运动参数及运动参数的协方差矩阵，根据机器人的运动参数以及机器人在上一测量时刻的状态向量和状态向量的协方差矩阵预测机器人在当前测量时刻的状态向量和状态向量的协方差矩阵；

机器人位姿解算模块3032，用于利用其他定位装置或方法获取机器人在位姿测量时刻的位姿和位姿测量误差；

融合更新模块3033，用于根据机器人位姿预测模块3031预测得到的机器人的状态向量、状态向量的协方差矩阵，以及机器人位姿解算模块3032获取的机器人的位姿和位姿测量误差，修正机器人位姿预测模块3031预测得到的机器人在位姿测量时刻的状态向量和状态向量的协方差矩阵；

收敛状态判定模块3034，用于确定最新的连续多个位姿测量时刻修正得到的状态向量满足收敛条件，则根据最新的连续多个位姿测量时刻修正得到的状态向量确定机器人状态向量的标定结果；其中，最新的连续多个测量时刻包括当前位姿测量时刻及距离当前姿测量时刻最近的至少一个位姿测量时刻。

具体的，收敛状态判定模块3034根据机器人最新的连续多个位姿测量时刻修正得到的状态向量，确定机器人运动学参数修正系数的右轮直径系数、左轮直径系数以及左右轮距修正系数的标准差，若确定机器人运动学参数修正系数的右轮直径系数、左轮直径系数以及左右轮距修正系数的标准差均小于设定的阈值，则满足收敛条件，机器人标定结果中状态向量的运动学参数修正系数为机器人最新的连续多个位姿测量时刻修正得到的状态向量的运动学参数修正系数的均值。

本申请的实施例中，机器人本体100的状态向量用二维平面向量X表示为：

X＝[x_r，y_r，θ_r，λ_R,λ_L,λ_W]^T....................[1]

其中，X_r＝[x_r,y_r,θ_r]^T表示机器人在世界坐标系下的位姿，X_λ＝[λ_R,λ_L,λ_W]^T表示机器人运动学参数的修正系数，λ_R为右轮直径修正系数，λ_L为左轮直径修正系数，λ_W为轮距修正系数。

本申请的实施例中，机器人状态向量的协方差矩阵用C表示为：

图3示例性示出了本申请实施例双轮差速驱动机器人里程计标定流程示意图。该流程包括：

S301：确定机器人在上一测量时刻k的状态向量X^k及状态向量的协方差矩阵C^k。

该步骤中，状态向量

包括机器人位姿

及修正系数

可选的，在起始时刻，即k＝0时，将机器人的当前位姿作为世界坐标系的原点，此时，

运动学参数修正系数初始值

S302：控制机器人运动。

该步骤可由运动控制模块执行，被控机器人的运动轨迹可根据实际标定场景进行设定，运动形式包含直线运动和旋转运动。

可选的，直线运动和旋转运动相互独立，旋转运动为原地旋转。运动过程速度尽量平缓，避免较大的加速度，以提高机器人位姿预测模块的预测精度。

S303：获取机器人按照标定轨迹运动时在第一运动测量时刻的左右轮运动增量、第一运动测量时刻与上一运动测量时刻的时间增量为ΔT、第一运动测量时刻与上一测量时刻的时间增量Δt，确定机器人的运动参数U₁及运动参数的协方差矩阵Q₁。

该步骤可由机器人位姿预测模块执行，左右轮运动增量即机器人右轮行走的距离ΔS_R和左轮行走的距离ΔS_L，可基于左右轮径和左右轮距得到，则机器人位姿预测模块获取机器人的速度参数的公式为：

其中，υ₁为机器人的线速度，ω₁为机器人的角速度，W为机器人的左右轮距标称值，

为k时刻右轮直径修正系数，

为k时刻左轮直径修正系数，

为k时刻左右轮距修正系数。

S304：根据机器人的运动参数U₁及机器人在上一测量时刻k的状态向量X^k预测机器人在第一运动测量时刻的状态向量X₁，根据机器人的运动参数U₁、运动参数的协方差矩阵Q₁及机器人在上一测量时刻k的状态向量X^k、状态向量的协方差矩阵C^k预测机器人在第一运动测量时刻的状态向量的协方差矩阵C₁。

该步骤可由机器人位姿预测模块执行，预测公式为：

X₁＝f(X^k，U₁)....................[5]

其中，运动参数U₁＝[υ₁,ω₁,Δt]。

S305：预测机器人在第一位姿测量时刻的状态向量及状态向量的协方差矩阵。

该步骤中，当第一运动测量时刻和第一位姿测量时刻同步时，第一位姿测量时刻预测的状态向量

等于第一运动测量时刻预测得到的状态向量X₁，第一位姿测量时刻预测的状态向量的协方差矩阵

等于第一运动测量时刻预测得到的状态向量的协方差矩阵C₁。

S306：获取机器人在第一位姿测量时刻的位姿Z^k+1及位姿测量误差R^k+1。

该步骤可由机器人位姿解算模块执行，在第一位姿测量时刻，可获取来自于其他装置的机器人的位姿Z^k+1及位姿测量误差R^k+1。

S307：修正更新机器人在第一位姿测量时刻预测得到的状态向量

和状态向量的协方差矩阵

得到经过修正的第一位姿测量时刻的状态向量X^k+1和状态向量的协方差矩阵C^k+1。

该步骤可由融合更新模块执行，可采用自适应卡尔曼滤波(Adaptive KalmanFilter，AKF)算法，利用第一位姿测量时刻的位姿Z^k+1和位姿测量误差R^k+1对第一位姿测量时刻预测得到的状态向量

和状态向量的协方差矩阵

进行修正，得到第一位姿测量时刻经过修正的状态向量X^k+1和状态向量的协方差矩阵C^k+1。

修正公式为：

S308：确定最新的连续多个位姿测量时刻修正得到的状态向量满足收敛条件，则根据最新的连续多个位姿测量时刻修正得到的状态向量确定机器人里程计标定结果。

其中，最新的连续多个位姿测量时刻包括第一位姿测量时刻及距离第一位姿测量时刻最近的至少一个位姿测量时刻。

该步骤可由收敛状态判定模块执行，随着机器人的持续运动，机器人的状态向量不断进行预测和更新，运动学参数的修正系数不断变化并向真值收敛。

该步骤中，基于连续若干修正得到的状态向量确定机器人里程计的标定结果，消除了人为因素的干扰，提高了自动化程度。

可选地，在一些实施例中，步骤S308中，可通过最新的连续多个位姿测量时刻修正得到的状态向量的运动学修正系数的右轮直径修正系数、左轮直径修正系数、左右轮距修正系数的标准差，判定状态向量的收敛性，判定步骤包括：

步骤1：获取机器人最新的连续N个位姿测量时刻修正得到的状态向量

步骤2：得到最新的连续N个位姿测量时刻修正得到的状态向量中的右轮直径修正系数、左轮直径修正系数、左右轮距修正系数的标准差，其中N为大于1的整数。标准差计算公式为：

其中

为右轮直径修正系数的标准差，λ_Ri为第i次测量时刻的右轮直径修正系数，

为N次测量时刻的右轮直径修正系数的均值；

为左轮直径修正系数的标准差，λ_Li为第i次测量时刻的左轮直径修正系数，

为N次测量时刻的左轮直径修正系数的均值；

为左右轮距修正系数的标准差，λ_Wi为第i次测量时刻的左右轮距修正系数，

为N次测量时刻的左右轮距修正系数的均值。

步骤3：根据右轮直径修正系数、左轮直径修正系数、左右轮距修正系数的标准差与设定的阈值比较结果确定状态向量的收敛性，若右轮直径修正系数、左轮直径修正系数、左右轮距修正系数的标准差均小于设定的阈值，则状态向量满足收敛条件，标定完成，此时机器人状态向量的运动学参数修正系数标定结果为N个位姿测量时刻修正得到的状态向量的运动学参数修正系数的均值；若右轮直径修正系数、左轮直径修正系数、左右轮距修正系数的标准差未同时小于设定的阈值，则状态向量不满足收敛条件，需重复步骤S302～S307，直到满足收敛条件。

该步骤中，通过最新的连续N个位姿测量时刻修正得到的状态向量的运动学修正系数的右轮直径修正系数、左轮直径修正系数、左右轮距修正系数的标准差作为判定修正得到的状态向量的收敛条件，提高了标定精度。

以上是以运动测量模块301和位姿测量模块302测量时间同步的情况为例描述的，但存在测量左右轮运动增量的时间(运动测量时间)与获取机器人位姿及位姿测量误差的时间(位姿测量时间)不同步的情况，图4示例性示出了本申请实施例提供的运动测量和位姿测量过程及时间示意图。如图4所示，Δt₁、Δt₂、Δt₃、Δt₄为位姿测量和运动测量中的时间间隔。

考虑到S305中位姿测量模块和运动测量模块在时间上不同步，可利用运动参数更新预测结果,降低了对运动测量模块和位姿测量模块时间同步的要求，从而提高了标定确定模块303标定的精度。

以第一位姿测量时刻(k时刻)、第二位姿测量时刻(k+1时刻)为相邻的位姿测量时刻，第一运动测量时刻和第二运动测量时刻为相邻的运动测量时刻，且第一运动测量时刻和第二运动测量时刻位于第一位姿测量时刻和第二位姿测量时刻之间为例说明预测修正状态向量和状态向量的协方差矩阵的过程。

具体的，获取机器人在第一运动测量时刻的左右轮运动增量、第一运动测量时刻相对于上一运动测量时刻的时间增量ΔT₁、第一运动测量时刻相对于上一测量时刻的时间增量Δt₁，确定机器人在第一运动测量时刻的运动参数U₁和运动参数的协方差矩阵Q₁,；根据机器人在第一运动测量时刻的运动参数U₁和运动参数的协方差矩阵Q₁、机器人在第一位姿测量时刻经过修正的状态向量X^k和状态向量的协方差矩阵C^k，预测机器人在第一运动测量时刻的状态向量X₁和状态向量的协方差矩阵C₁；其中，所述上一测量时刻为第一位姿测量时刻。

获取机器人在第二运动测量时刻的左右轮运动增量、第二运动测量时刻相对于第一运动测量时刻的时间增量ΔT₂、第二运动测量时刻相对于上一测量时刻的时间增量Δt₂，确定机器人在第二运动测量时刻的运动参数U₂和运动参数的协方差矩阵Q₂；根据机器人在第二运动测量时刻的运动参数U₂和运动参数的协方差矩阵Q₂、机器人在第一运动测量时刻预测得到的状态向量X₁和状态向量的协方差矩阵C₁，预测机器人在第二运动测量时刻的状态向量X₂和状态向量的协方差矩阵C₂；其中，所述上一测量时刻为第一运动测量时刻。

获取机器人在第二位姿测量时刻的位姿Z^k+1和位姿测量误差R^k+1、第二位姿测量时刻相对于上一测量时刻的时间增量Δt₃、上一测量时刻的运动参数U₂、确定机器人在第二位姿测量时刻的运动参数U^k+1和运动参数的协方差矩阵Q^k+1；根据机器人在第二位姿测量时刻的运动参数U^k+1和运动参数的协方差矩阵Q^k+1、机器人在第二运动测量时刻的状态向量X₂和状态向量的协方差矩阵C₂；预测机器人在第二位姿测量时刻的状态向量

和状态向量的协方差矩阵

其中，所述上一测量时刻为第二运动测量时刻。

上述实施例的预测公式为：

X₁＝f(X^k，U₁)....................[13]

X₂＝f(X₁，U₂)....................[12]

进一步，根据自适应卡尔玛滤波算法，利用第二位姿测量时刻的位姿Z^k+1和位姿测量误差R^k+1对第二次位姿测量时刻预测得到的状态向量

和状态向量的协方差矩阵

进行修正，得到第二次位姿测量时刻经过修正的状态向量X^k+1和状态向量的协方差矩阵C^k+1。

上述实施例中，基于自适应卡尔曼滤波算法，步骤S306利用重新预测的第二位姿测量时刻的状态向量

及状态向量的协方差矩阵

和获取的机器人在第二时刻的位姿Z^k+1和位姿测量误差R^k+1，修正机器人在第二时刻预测得到的状态向量，提高了标定精度。

本领域内的技术人员应明白，本申请的实施例可提供为方法、系统、或计算机程序产品。因此，本申请可采用完全硬件实施例、完全软件实施例、或结合软件和硬件方面的实施例的形式。而且，本申请可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质(包括但不限于磁盘存储器、CD-ROM、光学存储器等)上实施的计算机程序产品的形式。

本申请是参照根据本申请的方法、设备(系统)、和计算机程序产品的流程图和/或方框图来描述的。应理解可由计算机程序指令实现流程图和/或方框图中的每一流程和/或方框、以及流程图和/或方框图中的流程和/或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理设备的处理器以产生一个机器，使得通过计算机或其他可编程数据处理设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的装置。

这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理设备以特定方式工作的计算机可读存储器中，使得存储在该计算机可读存储器中的指令产生包括指令装置的制造品，该指令装置实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能。

这些计算机程序指令也可装载到计算机或其他可编程数据处理设备上，使得在计算机或其他可编程设备上执行一系列操作步骤以产生计算机实现的处理，从而在计算机或其他可编程设备上执行的指令提供用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的步骤。

显然，本领域的技术人员可以对本申请进行各种改动和变型而不脱离本申请的精神和范围。这样，倘若本申请的这些修改和变型属于本申请权利要求及其等同技术的范围之内，则本申请也意图包含这些改动和变型在内。

Claims (9)

1.一种双轮差速驱动机器人里程计标定方法，其特征在于，包括：

若第一位姿测量时刻等于第一运动测量时刻，则：

获取机器人按照标定轨迹运动时在第一运动测量时刻的左轮、右轮运动增量；

根据所述左轮、右轮运动增量确定所述机器人的运动参数及运动参数的协方差矩阵，根据所述运动参数以及所述机器人在上一测量时刻的状态向量预测所述机器人在第一运动测量时刻的状态向量，并根据所述运动参数及运动参数的协方差矩阵、所述机器人在上一测量时刻的状态向量和状态向量的协方差矩阵，预测所述机器人在第一运动测量时刻的状态向量的协方差矩阵；其中，所述上一测量时刻包括运动测量时刻或位姿测量时刻；

预测机器人在第一位姿测量时刻的状态向量及状态向量的协方差矩阵，所述机器人在所述第一位姿测量时刻的状态向量等于所述机器人在所述第一运动测量时刻的状态向量，以及所述机器人在所述第一位姿测量时刻的状态向量协方差矩阵等于所述机器人在所述第一运动测量时刻的状态向量的协方差矩阵；

获取所述机器人在第一位姿测量时刻的位姿及位姿测量误差；

根据第一位姿测量时刻获取到的所述位姿和位姿测量误差，对预测得到的所述机器人在第一位姿测量时刻的状态向量、状态向量的协方差矩阵进行修正；

若第二位姿测量时刻大于第二运动测量时刻，所述第二运动测量时刻和所述第一运动测量时刻为相邻的运动测量时刻，所述第二运动测量时刻大于所述第一运动测量时刻，所述第一运动测量时刻大于所述第一位姿测量时刻，则：

获取机器人在第一运动测量时刻的左轮、右轮运动增量、第一运动测量时刻相对于上一运动测量时刻的时间增量、第一运动测量时刻相对于所述第一位姿测量时刻的时间增量，确定机器人在第一运动测量时刻的运动参数和运动参数的协方差矩阵；根据机器人在第一运动测量时刻的运动参数和运动参数的协方差矩阵、机器人在第一位姿测量时刻经过修正的状态向量和状态向量的协方差矩阵，预测机器人在第一运动测量时刻的状态向量和状态向量的协方差矩阵；

获取机器人在第二运动测量时刻的左轮、右轮运动增量、第二运动测量时刻相对于第一运动测量时刻的时间增量，确定机器人在第二运动测量时刻的运动参数和运动参数的协方差矩阵；根据机器人在第二运动测量时刻的运动参数和运动参数的协方差矩阵、机器人在第一运动测量时刻预测得到的状态向量和状态向量的协方差矩阵，预测机器人在第二运动测量时刻的状态向量和状态向量的协方差矩阵；

获取机器人在第二位姿测量时刻的位姿和位姿测量误差、第二位姿测量时刻相对于第二运动测量时刻的时间增量、第二运动测量时刻的运动参数，确定机器人在第二位姿测量时刻的运动参数和运动参数的协方差矩阵；根据机器人在第二位姿测量时刻的运动参数和运动参数的协方差矩阵、机器人在第二运动测量时刻预测得到的状态向量和状态向量的协方差矩阵，预测机器人在第二位姿测量时刻的状态向量和状态向量的协方差矩阵；根据机器人在第二位姿测量时刻的位姿和位姿测量误差，对机器人在第二位姿测量时刻预测得到的状态向量和状态向量的协方差矩阵进行修正，得到修正后的机器人在第二位姿测量时刻的状态向量和状态向量的协方差矩阵；

所述方法还包括：

若确定最新的连续多个位姿测量时刻修正得到的状态向量中的运动学参数修正系数满足收敛条件，则根据所述最新的连续多个位姿测量时刻修正得到的状态向量中的运动学参数修正系数确定所述机器人里程计标定结果；其中，所述最新的连续多个位姿测量时刻包括第一位姿测量时刻及距离第一位姿测量时刻最近的至少一个位姿测量时刻。

2.如权利要求1所述的方法，其特征在于，确定最新的连续多个位姿测量时刻修正得到的状态向量中的运动学参数修正系数满足收敛条件，具体包括：

根据所述最新的连续多个位姿测量时刻修正得到的状态向量，确定所述机器人的右轮直径修正系数、左轮直径修正系数以及左右轮距修正系数的标准差；

若所述机器人的右轮直径修正系数、左轮直径修正系数以及左右轮距修正系数的标准差均小于设定的阈值，则确定满足收敛条件。

3.如权利要求1所述的方法，其特征在于，根据所述最新的连续多个位姿测量时刻修正得到的状态向量中的运动学参数修正系数确定所述机器人里程计标定结果，具体包括：

若所述机器人的右轮直径修正系数、左轮直径修正系数以及左右轮距修正系数的标准差均小于设定的阈值，计算所述最新的连续多个位姿测量时刻修正得到的状态向量中的运动学参数修正系数的均值；

将所述均值确定为所述机器人在第一位姿测量时刻的运动学参数修正系数的标定结果。

4.如权利要求1-3任一所述的方法，其特征在于，所述状态向量，包括：

所述机器人的运动位姿以及所述机器人的运动学参数的修正系数。

5.一种双轮差速驱动机器人里程计标定装置，其特征在于，包括：

若第一位姿测量时刻等于第一运动测量时刻，则：

运动测量模块，用于获取机器人按照控制模块的标定轨迹运动时里程计在第一运动测量时刻输出的左轮、右轮运动增量；

位姿测量模块，用于获取所述机器人在第一位姿测量时刻的位姿及位姿测量误差；

标定确定模块，用于根据左轮、右轮运动增量确定机器人的运动参数及运动参数的协方差矩阵，根据运动参数以及机器人在上一测量时刻的状态向量预测机器人在第一运动测量时刻的状态向量，并根据运动参数及运动参数的协方差矩阵、机器人在上一测量时刻的状态向量和状态向量的协方差矩阵，预测机器人在第一运动测量时刻的状态向量的协方差矩阵；其中，上一测量时刻包括运动测量时刻或位姿测量时刻；

预测所述机器人第一位姿测量时刻的状态向量及状态向量的协方差矩阵，所述机器人在所述第一位姿测量时刻的状态向量等于所述机器人在所述第一运动测量时刻的状态向量，以及所述机器人在所述第一位姿测量时刻的状态向量协方差矩阵等于所述机器人在所述第一运动测量时刻的状态向量的协方差矩阵；

根据第一位姿测量时刻获取到的所述位姿和位姿测量误差，对预测得到的所述机器人在第一位姿测量时刻的状态向量、状态向量的协方差矩阵进行修正；

若第二位姿测量时刻大于第二运动测量时刻，所述第二运动测量时刻和所述第一运动测量时刻为相邻的运动测量时刻，所述第二运动测量时刻大于所述第一运动测量时刻，所述第一运动测量时刻大于所述第一位姿测量时刻，则：

所述运动测量模块，用于获取机器人在第一运动测量时刻的左轮、右轮运动增量、第一运动测量时刻相对于上一运动测量时刻的时间增量、第一运动测量时刻相对于所述第一位姿测量时刻的时间增量；获取机器人在第二运动测量时刻的左轮、右轮运动增量、第二运动测量时刻相对于第一运动测量时刻的时间增量；

所述位姿测量模块，用于获取机器人在第二位姿测量时刻的位姿和位姿测量误差；获取机器人在第二位姿测量时刻相对于第二运动测量时刻的时间增量、第二运动测量时刻的运动参数；

所述标定确定模块，用于根据运动测量模块获取的第一运动测量时刻的左轮、右轮运动增量、第一运动测量时刻相对于上一运动测量时刻的时间增量、第一运动测量时刻相对于所述第一位姿测量时刻的时间增量，确定机器人在第一运动测量时刻的运动参数和运动参数的协方差矩阵；根据机器人在第一运动测量时刻的运动参数和运动参数的协方差矩阵、机器人在第一位姿测量时刻经过修正的状态向量和状态向量的协方差矩阵，预测机器人在第一运动测量时刻的状态向量和状态向量的协方差矩阵；

根据运动测量模块获取的第二运动测量时刻的左轮、右轮运动增量、第二运动测量时刻相对于第一运动测量时刻的时间增量，确定机器人在第二运动测量时刻的运动参数和运动参数的协方差矩阵；根据机器人在第二运动测量时刻的运动参数和运动参数的协方差矩阵、机器人在第一运动测量时刻预测得到的状态向量和状态向量的协方差矩阵，预测机器人在第二运动测量时刻的状态向量和状态向量的协方差矩阵；

根据位姿测量模块获取的第二位姿测量时刻的位姿和位姿测量误差、第二位姿测量时刻相对于第二运动测量时刻的时间增量、第二运动测量时刻的运动参数，确定机器人在第二位姿测量时刻的运动参数和运动参数的协方差矩阵；根据机器人在第二位姿测量时刻的运动参数和运动参数的协方差矩阵、机器人在第二运动测量时刻预测得到的状态向量和状态向量的协方差矩阵，预测机器人在第二位姿测量时刻的状态向量和状态向量的协方差矩阵；根据机器人在第二位姿测量时刻的位姿和位姿测量误差，对机器人在第二位姿测量时刻预测得到的状态向量和状态向量的协方差矩阵进行修正，得到修正后的机器人在第二位姿测量时刻的状态向量和状态向量的协方差矩阵；

所述标定确定模块，还用于若确定最新的连续多个位姿测量时刻修正得到的状态向量中的运动学参数修正系数满足收敛条件，则根据所述最新的连续多个位姿测量时刻修正得到的状态向量中的运动学参数修正系数确定所述机器人里程计标定结果；其中，所述最新的连续多个位姿测量时刻包括第一位姿测量时刻及距离第一位姿测量时刻最近的至少一个位姿测量时刻。

6.如权利要求5所述的装置，其特征在于，所述标定确定模块，具体用于：

根据所述最新的连续多个位姿测量时刻修正得到的状态向量，确定所述机器人的右轮直径修正系数、左轮直径修正系数以及左右轮距修正系数的标准差；若所述机器人的右轮直径修正系数、左轮直径修正系数以及左右轮距修正系数的标准差均小于设定的阈值，则确定满足收敛条件，否则不满足。

7.如权利要求5所述的装置，其特征在于，所述标定确定模块，具体用于：

若所述机器人的右轮直径修正系数、左轮直径修正系数以及左右轮距修正系数的标准差均小于设定的阈值，计算所述最新的连续多个位姿测量时刻修正得到的状态向量中的运动学参数修正系数的均值，将所述均值确定为所述机器人运动学参数修正系数的标定结果。

8.如权利要求5-7任一所述的装置，其特征在于，所述状态向量，包括：

所述机器人的运动位姿以及所述机器人的运动学参数的修正系数。

9.一种机器人，其特征在于，包括：机器人本体、运动控制模块以及如权利要求5-7中任一所述的双轮差速驱动机器人里程计标定装置；

所述机器人本体包括左轮、右轮以及里程计，所述里程计用于输出左轮、右轮运动增量及时间增量，左轮运动增量是基于左轮径的标称值得到的，右轮运动增量是基于右轮径的标称值得到的；

所述运动控制模块，用于控制所述左轮、右轮进行运动。

Images