CN110849392A - 一种机器人的里程计数据校正方法及机器人 - Google Patents

Abstract

本发明公开一种机器人的里程计数据校正方法及机器人，所述里程计数据校正方法包括：初始化所述机器人的系统状态量以及系统协方差矩阵；计算得出所述机器人的左轮移动的距离和右轮移动的距离，并结合初始化后的所述机器人的系统状态量以及系统协方差矩阵，根据系统状态无噪声模型进行状态预测和系统协方差预测，以输出状态预测后的所述机器人的系统状态量以及系统协方差矩阵；读取惯性测量单元数据，判断当前的惯性测量单元数据与前一时刻的惯性测量单元数据的差值的绝对值是否在第一设定阈值内，以输出所述状态预测后的所述机器人的系统状态量以及系统协方差矩阵作为融合后的信息。本发明的里程计数据校正方法具有实时性高，计算精度比较高。

Description

一种机器人的里程计数据校正方法及机器人

技术领域

本发明涉及机器人轨迹计算技术领域，特别是涉及一种机器人的里程计数据校正方法及机器人。

背景技术

近些年来，随着机器人技术研究的日益成熟以及硬件条件的逐步改善，机器人实时轨迹的计算也有了更高的要求。在保证实时的前提下还要保证位姿的精度，这是机器人轨迹推算的一个基本问题。

现有技术中的机器人轨迹推算方法在轮子打滑的情况下，计算结果明显错误，另外机器人在转弯的时候，计算的相对偏角也不准确，而且由于零漂以及温漂的影响，其数据容易受到影响，从而导致所计算的轨迹具有非常大的累积误差。因此，迫切需要改进。

发明内容

鉴于以上所述现有技术的缺点，本发明的目的在于提供一种机器人的里程计数据校正方法及机器人，用于解决现有技术中的机器人在轮子打滑的情况下，计算结果明显错误，另外机器人在转弯的时候，计算的相对偏角也不准确，而且由于零漂以及温漂的影响，其数据容易受到影响，从而导致所计算的轨迹具有非常大的累积误差的问题。

为实现上述目的及其他相关目的，本发明提供一种机器人的里程计数据校正方法，所述机器人的里程计数据校正方法包括：

初始化所述机器人的系统状态量以及系统协方差矩阵；

计算得出所述机器人的左轮移动的距离和右轮移动的距离，并结合初始化后的所述机器人的系统状态量以及系统协方差矩阵，根据系统状态无噪声模型进行状态预测和系统协方差预测，以输出状态预测后的所述机器人的系统状态量以及系统协方差矩阵；

读取惯性测量单元数据，判断当前的惯性测量单元数据与前一时刻的惯性测量单元数据的差值的绝对值是否在第一设定阈值内，若是，则第一次更新所述机器人的系统状态以及系统协方差矩阵，若否，则输出所述状态预测后的所述机器人的系统状态量以及系统协方差矩阵作为融合后的信息，继续执行计算得出所述机器人的左轮移动的距离和右轮移动的距离的操作。

在本发明的一实施例中，所述机器人的里程计数据校正方法还包括：读取光流数据，判断当前的光流数据与前一时刻的光流数据的差值的绝对值是否在第二设定阈值内，若是，则第二次更新所述机器人的系统状态以及系统协方差矩阵，若否，则输出第一次更新后的所述机器人的系统状态以及系统协方差矩阵作为融合后的信息，继续执行计算得出所述机器人的左轮移动的距离和右轮移动的距离的操作。

在本发明的一实施例中，所述机器人的里程计数据校正方法还包括：输出第二次更新后的所述机器人的系统状态以及系统协方差矩阵，继续执行计算得出所述机器人的左轮移动的距离和右轮移动的距离的操作。

在本发明的一实施例中，读取所述惯性测量单元数据，将所述惯性测量单元数据的输出作为第一观测量和第一观测矩阵。

在本发明的一实施例中，读取所述光流数据，将所述光流数据的输出作为第二观测量和第二观测矩阵。

在本发明的一实施例中，通过多传感器融合模型对所述惯性测量单元数据和光流数据进行机器人里程计数据的联合校正。

在本发明的一实施例中，所述第一观测量为[pitch,roll,yaw]^T，其中，pitch表示机器人在三维空间中的俯仰角，roll表示机器人在三维空间中的横滚角，yaw表示机器人在三维空间中的偏航角；所述第一观测矩阵为

在本发明的一实施例中，所述第二观测量为[x,y,z,yaw]^T，其中，x表示机器人在三维空间中的横坐标，y表示机器人在三维空间中的纵坐标，z表示机器人在三维空间中的竖坐标，yaw表示机器人在三维空间中的偏航角；所述第二观测矩阵为

在本发明的一实施例中，根据所述机器人的左轮的码盘数据计算得出所述机器人的左轮移动的距离，根据所述机器人的右轮的码盘数据计算得出所述机器人的右轮移动的距离。

为实现上述目的，本发明还提供一种机器人，所述机器人运行程序指令实现上述的机器人的里程计数据校正方法。

如上所述，本发明的一种机器人的里程计数据校正方法及机器人，具有以下有益效果：

本发明的机器人的里程计数据校正方法包括：读取惯性测量单元数据作为第一观测量及第一观测矩阵，若数据符合一定的条件，则进行状态更新过程，读取光流数据作为第二观测量及第二观测矩阵，若数据符合一定的条件，则进行状态更新过程。本发明的里程计数据校正方法具有实时性高，计算精度比较高的特点，另外机器人在转弯的时候，计算的相对偏角准确，不受零漂以及温漂的影响，其数据容易受到影响而且在计算相对平移时不受打滑的影响。本发明可以实时高效地对里程计数据进行校正，进而得到比较好的状态估计。

通过多传感器融合模型读取所述惯性测量单元数据和光流数据，本发明提出的多传感器融合模型可以实时高效地对里程计数据进行校正，进而得到比较好的状态估计，具有很强的应用价值。

附图说明

图1为本申请一个实施例提供的一种机器人的里程计数据校正方法的工作流程图。

图2为本申请又一个实施例提供的一种机器人的里程计数据校正方法的工作流程图。

图3为本申请再一个实施例提供的一种机器人的里程计数据校正方法的工作流程图。

图4为本申请再一个实施例提供的一种机器人的里程计数据校正方法的工作流程图。

图5为本申请实施例提供的机器人的结构图。

元件标号说明

1 机器人壳体

2 右轮

3 左轮

具体实施方式

以下通过特定的具体实例说明本发明的实施方式，本领域技术人员可由本说明书所揭露的内容轻易地了解本发明的其他优点与功效。本发明还可以通过另外不同的具体实施方式加以实施或应用，本说明书中的各项细节也可以基于不同观点与应用，在没有背离本发明的精神下进行各种修饰或改变。需说明的是，在不冲突的情况下，以下实施例及实施例中的特征可以相互组合。

需要说明的是，以下实施例中所提供的图示仅以示意方式说明本发明的基本构想，遂图示中仅显示与本发明中有关的组件而非按照实际实施时的组件数目、形状及尺寸绘制，其实际实施时各组件的型态、数量及比例可为一种随意的改变，且其组件布局型态也可能更为复杂。

请参阅图1，图1为本申请一个实施例提供的一种机器人的里程计数据校正方法的工作流程图。一种机器人的里程计数据校正方法，所述机器人的里程计数据校正方法包括：S1、初始化所述机器人的系统状态量以及系统协方差矩阵。S2、计算得出所述机器人的左轮3移动的距离和右轮4移动的距离，并结合初始化后的所述机器人的系统状态量以及系统协方差矩阵，根据系统状态无噪声模型进行状态预测和协方差矩阵预测，以输出状态预测后的所述机器人的系统状态量以及系统协方差矩阵。具体的，可以但不限于根据所述机器人的左轮3的码盘数据计算得出机器人左轮3移动的距离，根据所述机器人的右轮2的码盘数据计算得出机器人右轮2移动的距离。S3、读取惯性测量单元数据(IMU数据)。S4、判断当前的惯性测量单元数据与前一时刻的惯性测量单元数据的差值的绝对值是否在第一设定阈值内。若当前的惯性测量单元数据与前一时刻的惯性测量单元数据的差值的绝对值在第一设定阈值内，则执行步骤S5操作，若当前的惯性测量单元数据与前一时刻的惯性测量单元数据的差值的绝对值不在第一设定阈值内，则执行步骤S6操作。S5、第一次更新所述机器人的系统状态以及系统协方差矩阵。S6、输出所述状态预测后的所述机器人的系统状态量以及系统协方差矩阵作为融合后的信息，继续执行步骤S2的计算得出所述机器人的左轮3移动的距离和右轮4移动的距离，并结合初始化后的所述机器人的系统状态量以及系统协方差矩阵，根据系统状态无噪声模型进行状态预测和系统协方差预测，以输出状态预测后的所述机器人的系统状态量以及系统协方差矩阵的操作。具体的，步骤S4中的：所述当前的惯性测量单元数据与前一时刻的惯性测量单元数据的差值的绝对值可以通过计算得出，即通过计算得到当前的光流数据与前一时刻的光流数据的差值，再判断当前的惯性测量单元数据与前一时刻的惯性测量单元数据的差值的绝对值是否在第一设定阈值内。具体的，通过多传感器融合模型读取所述惯性测量单元数据和光流数据。所述多传感器融合模型为：

其中，X_k表示状态量，Y_k表示观测量，U_k表示控制量，B表示控制系数，A表示状态转移矩阵，C_i表示观测矩阵，ξ表示服从正态分布N(0,Q)的状态噪声，σ_i表示服从正态分布N(0,R)的观测噪声。通过IMU数据更新系统的状态量及其系统协方差矩阵，将二者作为光流更新过程的系统预测状态量和系统预测协方差矩阵进行更新，输出最优系统状态量和最优系统协方差矩阵，将二者用于预测过程进入下一次迭代更新过程。在本发明的多传感器融合模型中，系统状态量为[x,y,z,pitch,roll,yaw]^T，表示机器人在三维空间中的坐标[x y z]^T，以及角度[pitch roll yaw]^T，pitch表示机器人在三维空间中的俯仰角，roll表示机器人在三维空间中的横滚角，yaw表示机器人在三维空间中的偏航角。系统输入为编码器的左轮右轮路程U＝[S_l,S_r]^T，S_l表示机器人左轮3移动的距离，S_r表示机器人右轮2移动的距离。对于室内地面上的自主移动机器人，为了得到尽量简化的模型，本发明取t时刻的状态量为X(t)＝[x(t) y(t) yaw(t)]^T，根据前后两个状态即状态X(t)和X(t+1)之间的几何关系建立如下系统状态无噪声模型：

其中，D表示两主动轮之间的距离。本发明的多传感器融合模型中观测量有IMU数据和光流数据，其中IMU数据输出的第一观测量为[pitch,roll,yaw]^T，其第一观测矩阵为

所述光流数据输出的第二观测量为[x,y,z,yaw]^T，其中，x表示机器人在三维空间中的横坐标，y表示机器人在三维空间中的纵坐标，z表示机器人在三维空间中的竖坐标，yaw表示机器人在三维空间中的偏航角。其第二观测矩阵为此观测矩阵即为多传感器融合模型中的C_i。上述两个观测向量即构成了所述观测向量Y(t)。

请参阅图2、图3、图4，图2为本申请又一个实施例提供的一种机器人的里程计数据校正方法的工作流程图。图3为本申请再一个实施例提供的一种机器人的里程计数据校正方法的工作流程图。图4为本申请再一个实施例提供的一种机器人的里程计数据校正方法的工作流程图。所述机器人的里程计数据校正方法还包括步骤S7，步骤S7、读取光流数据。S8、判断当前的光流数据与前一时刻的光流数据的差值的绝对值是否在第二设定阈值内，若当前的光流数据与前一时刻的光流数据的差值的绝对值在第二设定阈值内，则进行步骤S9操作，若当前的光流数据与前一时刻的光流数据的差值的绝对值不在第二设定阈值内，则进行步骤S10操作。S9、第二次更新所述机器人的系统状态以及系统协方差矩阵。S10、输出第一次更新后的所述机器人的系统状态以及系统协方差矩阵作为融合后的信息，继续执行步骤S2的计算得出所述机器人的左轮3移动的距离和右轮4移动的距离，并结合初始化后的所述机器人的系统状态量以及系统协方差矩阵，根据系统状态无噪声模型进行状态预测和系统协方差矩阵预测，以输出状态预测后的所述机器人的系统状态量以及系统协方差矩阵的操作。具体的，步骤S8中的：所述当前的光流数据与前一时刻的光流数据的差值的绝对值可以通过计算得出，即通过计算得到当前的光流数据与前一时刻的光流数据的差值，再判断当前的光流数据与前一时刻的光流数据的差值的绝对值是否在第二设定阈值内。所述机器人的里程计数据校正方法还包括步骤S11，步骤S11、输出第二次更新后的所述机器人的系统状态以及系统协方差矩阵，继续执行步骤S2的计算得出所述机器人的左轮3移动的距离和右轮4移动的距离，并结合初始化后的所述机器人的系统状态量以及系统协方差矩阵，根据系统状态无噪声模型进行状态预测和系统协方差矩阵预测，以输出状态预测后的所述机器人的系统状态量以及系统协方差矩阵的操作。具体的，所述第一设定阈值和第二设定阈值可以但不限于为平移阈值0.1米至0.3米，角度阈值20度至40度。所述平移阈值可以为0.18米，所述角度阈值可以为30度。以一个传感器为例，进行融合得到最优状态量的步骤如下：步骤一：初始化状态量X(0|0)以及协方差矩阵P(0|0)，表示相对于0时刻时的量。步骤二：根据当前t时刻的输入量U(t)进行状态预测，即通过当前时刻t的最优状态X(t|t)估计下一时刻的状态X(t+1|t)：X(t+1|t)＝A*X(t|t)+B*U(t)。步骤三：根据误差传播原理得到预测协方差矩阵，即根据t时刻状态方程的协方差矩阵Q(t)和观测状态的协方差矩阵P(t|t)计算下一时刻的估计状态协方差矩阵P(t+1|t)：P(t+1|t)＝A*P(t|t)*A^T+Q(t)。步骤四：根据上述所说的观测矩阵C和观测协方差矩阵R(t)计算Kalman增益：Gain＝P(t+1|t)*C^T*(C*P(t+1|t)*C^T+R(t))^-1。步骤五：根据t时刻的观测量Y(t)计算最优状态估计：X(t+1|t+1)＝X(t+1|t)+Gain*(Y(t)-X(t+1|t))。步骤六：更新估计状态的协方差矩阵：P(t+1|t+1)＝(I-Gain*C)*P(t+1|t)，其中，I表示与Gain*C具有相同维数的单位矩阵。本发明的多传感器融合模型可以应用于地面上的自主移动机器人。

请参阅图5，图5为本申请实施例提供的机器人的结构图。本发明提供了一种机器人，所述机器人运行程序指令实现上述的机器人的里程计数据校正方法。所述机器人包括机器人壳体1、右轮2以及左轮3。在融合过程的初始化阶段，应将机器人静止一段时间以收集传感器的数据，根据这些数据统计其均值，作为消除漂移的基准。读取到传感器数据之后，应先对数据进行有效性判断，本发明采用该数据是否在传感器噪声分布范围内以及相邻数据之间的跳变程度作为判断标准。本发明不要求各传感器输出数据时的时间是同步的，也即本发明中的多传感器的融合模型是一个分步融合模型，即使某一个传感器数据出现故障，传感器的融合模型是仍能继续计算。本发明中的状态噪声和观测噪声都假设是正比于时间变化的，因此，本发明的融合模型中的状态模型协方差和观测模型的协方差都是动态变化的。

综上所述，本发明的机器人的里程计数据校正方法包括：读取惯性测量单元数据作为第一观测量及第一观测矩阵，若数据符合一定的条件，则进行状态更新过程，读取光流数据作为第二观测量及第二观测矩阵，若数据符合一定的条件，则进行状态更新过程。本发明的里程计数据校正方法具有实时性高，计算精度比较高的特点，另外机器人在转弯的时候，计算的相对偏角准确，不受零漂以及温漂的影响，其数据容易受到影响而且在计算相对平移时不受打滑的影响。本发明可以实时高效地对里程计数据进行校正，进而得到比较好的状态估计。

上述实施例仅例示性说明本发明的原理及其功效，而非用于限制本发明。任何熟悉此技术的人士皆可在不违背本发明的精神及范畴下，对上述实施例进行修饰或改变。因此，举凡所属技术领域中具有通常知识者在未脱离本发明所揭示的精神与技术思想下所完成的一切等效修饰或改变，仍应由本发明的权利要求所涵盖。

Claims (10)

1.一种机器人的里程计数据校正方法，其特征在于，所述机器人的里程计数据校正方法包括：

初始化所述机器人的系统状态量以及系统协方差矩阵；

计算得出所述机器人的左轮移动的距离和右轮移动的距离，并结合初始化后的所述机器人的系统状态量以及系统协方差矩阵，根据系统状态无噪声模型进行状态预测和系统协方差预测，以输出状态预测后的所述机器人的系统状态量以及系统协方差矩阵；

读取惯性测量单元数据，判断当前的惯性测量单元数据与前一时刻的惯性测量单元数据的差值的绝对值是否在第一设定阈值内，若是，则第一次更新所述机器人的系统状态以及系统协方差矩阵，若否，则输出所述状态预测后的所述机器人的系统状态量以及系统协方差矩阵作为融合后的信息，继续执行计算得出所述机器人的左轮移动的距离和右轮移动的距离的操作。

2.根据权利要求1所述的一种机器人的里程计数据校正方法，其特征在于，所述机器人的里程计数据校正方法还包括：读取光流数据，判断当前的光流数据与前一时刻的光流数据的差值的绝对值是否在第二设定阈值内，若是，则第二次更新所述机器人的系统状态以及系统协方差矩阵，若否，则输出第一次更新后的所述机器人的系统状态以及系统协方差矩阵作为融合后的信息，继续执行计算得出所述机器人的左轮移动的距离和右轮移动的距离的操作。

3.根据权利要求2所述的一种机器人的里程计数据校正方法，其特征在于，所述机器人的里程计数据校正方法还包括：输出第二次更新后的所述机器人的系统状态以及系统协方差矩阵，继续执行计算得出所述机器人的左轮移动的距离和右轮移动的距离的操作。

4.根据权利要求1所述的一种机器人的里程计数据校正方法，其特征在于：读取所述惯性测量单元数据，将所述惯性测量单元数据的输出作为第一观测量和第一观测矩阵。

5.根据权利要求2所述的一种机器人的里程计数据校正方法，其特征在于：读取所述光流数据，将所述光流数据的输出作为第二观测量和第二观测矩阵。

6.根据权利要求2所述的一种机器人的里程计数据校正方法，其特征在于：通过多传感器融合模型对所述惯性测量单元数据和光流数据进行机器人里程计数据的联合校正。

7.根据权利要求4所述的一种机器人的里程计数据校正方法，其特征在于：所述第一观测量为[pitch,roll,yaw]^T，其中，pitch表示机器人在三维空间中的俯仰角，roll表示机器人在三维空间中的横滚角，yaw表示机器人在三维空间中的偏航角；所述第一观测矩阵为

8.根据权利要求5所述的一种机器人的里程计数据校正方法，其特征在于：所述第二观测量为[x,y,z,yaw]^T，其中，x表示机器人在三维空间中的横坐标，y表示机器人在三维空间中的纵坐标，z表示机器人在三维空间中的竖坐标，yaw表示机器人在三维空间中的偏航角；所述第二观测矩阵为

9.根据权利要求1所述的一种机器人的里程计数据校正方法，其特征在于：根据所述机器人的左轮的码盘数据计算得出所述机器人的左轮移动的距离，根据所述机器人的右轮的码盘数据计算得出所述机器人的右轮移动的距离。

10.一种机器人，其特征在于：所述机器人运行程序指令实现如权利要求1至权利要求9任一项所述的机器人的里程计数据校正方法。

Images