CN113984090A - 一种轮式机器人imu误差在线标定与补偿方法及装置 - Google Patents

Abstract

本发明公开了一种轮式机器人IMU误差在线标定与补偿方法及装置，该方法包括：获取第一校正数据；其中，第一校正数据为安装有IMU的轮式机器人在水平面上处于静置状态时，IMU采集的一段连续的数据，包括X、Y、Z方向的加速度和角速度；获取第二校正数据；其中，第二校正数据为轮式机器人原地旋转过程中，IMU采集的一段连续的数据，包括X、Y、Z方向的角速度；建立加速度计与陀螺仪在线标定误差模型，计算标定的参数；通过计算出的参数，对IMU的输出进行补偿。本发明考虑到了IMU存在安装误差的情况，通过简单的操作便可对轮式机器人IMU进行在线标定及补偿，进而提高IMU测量的精度。

Description

一种轮式机器人IMU误差在线标定与补偿方法及装置

技术领域

本发明涉及轮式机器人自动控制技术领域，特别涉及一种轮式机器人IMU误差在线标定与补偿方法及装置。

背景技术

在服务机器人越来越普及的今天，轮式机器人占据举足轻重的地位，而在轮式机器人行驶过程中，惯性测量单元(Inertial Measurement Unit,IMU)是轮式机器人实现定位、测速、姿态估计的一种关键传感器。IMU作为一种测量元件，在出厂安装、运输和用户使用过程中，对冲击、振动、应力、温度等不可避免的外界因素产生的干扰较为敏感，进而会引起时变的、不可忽视的IMU误差。IMU的误差的累积若不进行处理，会严重降低机器人的姿态和导航精度，从而影响机器人的性能。因此，需要对IMU误差进行在线标定与补偿，确保IMU的测量精度，从而确保机器人的性能。

IMU在安装在轮式机器人之前，一般需要将IMU固定在转台上进行出厂标定，其标定参数包括刻度因子和零偏。经测试，当IMU出厂安装于机器人上之后，加速度计和陀螺仪的刻度因子由于安装、运输、温度变化和老化等因素引起的变化很小，不需要在线标定；然而陀螺仪、加速度计的零偏因子却会因为这些因素而引起较大的变化，需要在线标定及补偿。

一般来说，现有技术中，为了对安装在轮式机器人中的陀螺仪和加速度计的零偏进行在线标定，只需使机器人静置于水平地面，由于陀螺仪理论输出为0，加速度计理论输出为重力加速度g，便可以计算出零偏。

然而，考虑到IMU在轮式机器人上的安装并不水平，IMU与机器人之间存在安装误差角，所以即使将机器人置于水平地面，IMU中的理论水平轴也很难与地面保持平行。又由于重力加速度g的存在，所以加速度计的3个轴上都会存在重力加速度g的分量，这便给求取加速度计零偏带来难度。所以仅通过在水平地面上静置，难以估计加速度计零偏。因此，现有的对安装在轮式机器人中的IMU的零偏标定的方法无法准确实现轮式机器人IMU误差的标定与补偿。

发明内容

本发明提供了一种轮式机器人IMU误差在线标定与补偿方法及装置，以解决现有的对安装在轮式机器人中的IMU的零偏标定的方法无法准确实现轮式机器人IMU误差的标定与补偿的技术问题。

为解决上述技术问题，本发明提供了如下技术方案：

一方面，本发明提供了一种轮式机器人IMU误差在线标定与补偿方法，该轮式机器人IMU误差在线标定与补偿方法包括：

获取第一校正数据；其中，所述第一校正数据为安装有惯性测量单元IMU的轮式机器人在水平面上处于静置状态时，所述IMU在第一预设时长内采集的一段连续的数据，所述第一校正数据包括X、Y、Z方向的加速度和角速度；

获取第二校正数据；其中，所述第二校正数据为所述轮式机器人在第二预设时长内，以预设速度在水平面上原地旋转预设角度的过程中，所述IMU采集的一段连续的数据，所述第二校正数据包括X、Y、Z方向的角速度；

建立加速度计与陀螺仪在线标定误差模型；

基于所述第一校正数据和第二校正数据，通过所述加速度计与陀螺仪在线标定误差模型，计算出陀螺仪和加速度计的零偏，完成所述IMU误差在线标定；

通过计算出的陀螺仪和加速度计的零偏对所述IMU的输出数据进行补偿。

进一步地，所述加速度计与陀螺仪在线标定误差模型的表达式如下：

其中，

表示角速度测量值，ω表示角速度理论值，w_g表示陀螺仪噪声，其类型为高斯白噪声，

表示加速度测量值，a表示加速度理论值，w_a表示加速度计噪声，其类型为高斯白噪声，b_g表示陀螺仪零偏，b_a表示加速度计零偏。

进一步地，所述基于所述第一校正数据和第二校正数据，通过所述加速度计与陀螺仪在线标定误差模型，计算出陀螺仪和加速度计的零偏，包括：

计算出所述第一校正数据中的加速度的平均值

以及所述第一校正数据中的角速度的平均值

由于IMU处于静置状态，且高斯白噪声数学期望为0，所以ω＝0，陀螺仪噪声平均值

加速度计噪声平均值

由此得到：

其中，

表示陀螺仪零偏在第一预设时长内的平均值；

计算出所述第二校正数据中的角速度的平均值

由于机器人在水平面上旋转，故旋转轴方向与重力方向相同，从而有：

又由于：

所以：

进一步地，所述通过计算出的陀螺仪和加速度计的零偏对所述IMU的输出数据进行补偿，包括：

通过计算出的陀螺仪和加速度计的零偏，基于所述加速度计与陀螺仪在线标定误差模型，利用下式对所述IMU的输出数据进行补偿：

其中，ω₂为补偿后的陀螺仪输出数据，a₂为补偿后的加速度计输出数据。

另一方面，本发明还提供了一种轮式机器人IMU误差在线标定与补偿装置，该轮式机器人IMU误差在线标定与补偿装置包括：

第一校正数据获取模块，用于获取第一校正数据；其中，所述第一校正数据为安装有惯性测量单元IMU的轮式机器人在水平面上处于静置状态时，所述IMU在第一预设时长内采集的一段连续的数据，所述第一校正数据包括X、Y、Z方向的加速度和角速度；

第二校正数据获取模块，用于获取第二校正数据；其中，所述第二校正数据为所述轮式机器人在第二预设时长内，以预设速度在水平面上原地旋转预设角度的过程中，所述IMU采集的一段连续的数据，所述第二校正数据包括X、Y、Z方向的角速度；

标定误差模型构建模块，用于建立加速度计与陀螺仪在线标定误差模型；

在线标定模块，用于基于所述第一校正数据获取模块获取的第一校正数据和所述第二校正数据获取模块获取的第二校正数据，通过所述标定误差模型构建模块所建立的加速度计与陀螺仪在线标定误差模型，计算出陀螺仪和加速度计的零偏，完成所述IMU误差在线标定；

补偿模块，用于通过所述在线标定模块计算出的陀螺仪和加速度计的零偏对所述IMU的输出数据进行补偿。

进一步地，标定误差模型构建模块建立的在线标定误差模型的表达式如下：

其中，

表示角速度测量值，ω表示角速度理论值，w_g表示陀螺仪噪声，其类型为高斯白噪声，

表示加速度测量值，a表示加速度理论值，w_a表示加速度计噪声，其类型为高斯白噪声，b_g表示陀螺仪零偏，b_a表示加速度计零偏。

进一步地，所述在线标定模块具体用于：

计算出所述第一校正数据中的加速度的平均值

以及所述第一校正数据中的角速度的平均值

由于IMU处于静置状态，且高斯白噪声数学期望为0，所以ω＝0，陀螺仪噪声平均值

加速度计噪声平均值

由此得到：

其中，

表示陀螺仪零偏在第一预设时长内的平均值；

计算出所述第二校正数据中的角速度的平均值

由于机器人在水平面上旋转，故旋转轴方向与重力方向相同，从而有：

又由于：

所以：

进一步地，所述补偿模块具体用于：

通过计算出的陀螺仪和加速度计的零偏，基于所述加速度计与陀螺仪在线标定误差模型，利用下式对所述IMU的输出数据进行补偿：

其中，ω₂为补偿后的陀螺仪输出数据，a₂为补偿后的加速度计输出数据。

再一方面，本发明还提供了一种电子设备，其包括处理器和存储器；其中，存储器中存储有至少一条指令，所述指令由处理器加载并执行以实现上述方法。

又一方面，本发明还提供了一种计算机可读存储介质，所述存储介质中存储有至少一条指令，所述指令由处理器加载并执行以实现上述方法。

本发明提供的技术方案带来的有益效果至少包括：

1、本发明通过实时采数计算便可完成对轮式机器人IMU的在线标定，标定结果较好，可有效提升定位定姿性能。

2、本发明中的数学公式简单易懂，程序方便实现；实施操作简单，易于实现，操作时间短。只需一块水平地面即可操作，不需要其它专业工具。

3、本发明提供的轮式机器人IMU误差在线标定与补偿方法，适用于存在较大IMU安装误差的情况，实用性好。

附图说明

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

图1是本发明实施例提供的轮式机器人IMU误差在线标定与补偿方法的执行流程示意图。

具体实施方式

为使本发明的目的、技术方案和优点更加清楚，下面将结合附图对本发明实施方式作进一步地详细描述。

第一实施例

本实施例提供了一种轮式机器人IMU误差在线标定与补偿方法，该方法可以由电子设备实现。该方法的执行流程如图1所示，包括以下步骤：

S1，获取第一校正数据；其中，所述第一校正数据为安装有惯性测量单元IMU的轮式机器人在水平面上处于静置状态时，所述IMU在第一预设时长内采集的一段连续的数据，所述第一校正数据包括X、Y、Z方向的加速度和角速度；

需要说明的是，在本实施例中，所述第一校正数据的获取方式为：将安装IMU的轮式机器人置于水平面上，并使其处于静置状态，然后获取IMU输出的数据；其中，IMU输出的数据是经过出厂标定系数校正后的IMU数据。

S2，获取第二校正数据；其中，所述第二校正数据为所述轮式机器人在第二预设时长内，以预设速度在水平面上原地旋转预设角度的过程中，所述IMU采集的一段连续的数据，所述第二校正数据包括X、Y、Z方向的角速度；

需要说明的是，在本实施例中，所述第二校正数据的获取方式为：将安装IMU的轮式机器人置于水平面上，驱动机器人在水平面上顺时针(或逆时针)以一定速度原地旋转一定角度，获取IMU在原地旋转状态下采集的一段连续的数据；其中，IMU输出的数据是经过出厂标定系数校正后的IMU数据。

S3，建立加速度计与陀螺仪在线标定误差模型；

需要说明的是，在本实施例中，对加速度计和陀螺仪的误差模型进行建模时，考虑到陀螺仪、加速度计的刻度因子在出厂时已经标定完成，出厂后安装、运输、温度变化和老化等因素引起的变化很小，不需要在线标定；然而陀螺仪、加速度计的零偏因子却会因为这些因素而引起较大的变化，需要在线标定。

基于上述，本实施例建立加速度计和陀螺仪误差模型如下：

其中，

表示角速度测量值，ω表示角速度理论值，w_g表示陀螺仪噪声，其类型为高斯白噪声，

表示加速度测量值，a表示加速度理论值，w_a表示加速度计噪声，其类型为高斯白噪声，b_g表示陀螺仪零偏，b_a表示加速度计零偏。

S4，基于所述第一校正数据和第二校正数据，通过所述加速度计与陀螺仪在线标定误差模型，计算出陀螺仪和加速度计的零偏，完成IMU误差在线标定；

具体地，在本实施例中，加速度计和陀螺仪的误差参数求解过程如下：

将第一校正数据中的加速度和角速度分别求平均，得到：第一校正数据中的加速度的平均值

以及第一校正数据中的角速度的平均值

由于IMU处于静置状态，且高斯白噪声数学期望为0，所以ω＝0，陀螺仪噪声平均值

加速度计噪声平均值

由此得到：

为当地重力加速度。

其中，

表示陀螺仪零偏在第一预设时长内的平均值。

将第二校正数据的角速度求平均，得到第二校正数据中角速度的平均值

由于机器人在水平面上旋转，故旋转轴方向与重力方向相同，从而有：

又因高斯白噪声数学期望为0，

将误差模型带入上式得：

又由于：

所以：

至此，求解出陀螺仪、加速度计的零偏，完成式机器人IMU误差在线标定。

S5，通过计算出的陀螺仪和加速度计的零偏对IMU的输出数据进行补偿。

具体地，当机器人在实际应用中运行时，IMU误差补偿方法为：

其中，ω₂为补偿后的陀螺仪输出数据，a₂为补偿后的加速度计输出数据；b_g为所求陀螺仪的零偏误差，b_a为所求加速度计的零偏误差，w_g为陀螺仪的数据噪声，为高斯白噪声，w_a为加速度计的数据噪声，为高斯白噪声；

分别为经过出厂校正后的陀螺仪和加速度计输出数据，它们可由下式表示：

其中，K_g为3阶方阵，B_g为3维列向量，分别为出厂标定的陀螺仪刻度系数和零偏；K_a为3阶方阵，B_a为3维列向量，分别为出厂标定的加速度计刻度系数和零偏。

分别为陀螺仪和加速度计输出的原始数据。

综上，本实施例的轮式机器人IMU误差在线标定与补偿方法，利用机器人静置时陀螺仪的输出的平均值来求解零偏；利用当机器人在水平面上旋转时，旋转轴方向与重力方向相同的特征，来求解加速度计的零偏。此方法通过简单操作并实时采数计算便可完成对轮式机器人IMU的在线标定及补偿，结果较好，可有效提升定位定姿性能；且数学公式简单易懂，程序方便实现，实施操作简单，易于实现，操作时间短，只需一块水平地面即可操作，不需要其它专业工具。而且，本标定及补偿方法适用于存在较大IMU安装误差的情况，实用性好。

第二实施例

本实施例提供了一种轮式机器人IMU误差在线标定与补偿装置，包括：

第一校正数据获取模块，用于获取第一校正数据；其中，所述第一校正数据为安装有惯性测量单元IMU的轮式机器人在水平面上处于静置状态时，所述IMU在第一预设时长内采集的一段连续的数据，所述第一校正数据包括X、Y、Z方向的加速度和角速度；

第二校正数据获取模块，用于获取第二校正数据；其中，所述第二校正数据为所述轮式机器人在第二预设时长内，以预设速度在水平面上原地旋转预设角度的过程中，所述IMU采集的一段连续的数据，所述第二校正数据包括X、Y、Z方向的角速度；

标定误差模型构建模块，用于建立加速度计与陀螺仪在线标定误差模型；

在线标定模块，用于基于所述第一校正数据获取模块获取的第一校正数据和所述第二校正数据获取模块获取的第二校正数据，通过所述标定误差模型构建模块所建立的加速度计与陀螺仪在线标定误差模型，计算出陀螺仪和加速度计的零偏，完成所述IMU误差在线标定；

补偿模块，用于通过所述在线标定模块计算出的陀螺仪和加速度计的零偏对所述IMU的输出数据进行补偿。

本实施例的轮式机器人IMU误差在线标定与补偿装置与上述第一实施例的轮式机器人IMU误差在线标定与补偿方法相对应；其中，本实施例的轮式机器人IMU误差在线标定与补偿装置中的各功能模块所实现的功能与上述轮式机器人IMU误差在线标定与补偿方法中的各流程步骤一一对应；故，在此不再赘述。

第三实施例

本实施例提供一种电子设备，其包括处理器和存储器；其中，存储器中存储有至少一条指令，所述指令由处理器加载并执行，以实现第一实施例的方法。

该电子设备可因配置或性能不同而产生比较大的差异，可以包括一个或一个以上处理器(central processing units，CPU)和一个或一个以上的存储器，其中，存储器中存储有至少一条指令，所述指令由处理器加载并执行上述方法。

第四实施例

本实施例提供一种计算机可读存储介质，该存储介质中存储有至少一条指令，所述指令由处理器加载并执行，以实现上述第一实施例的方法。其中，该计算机可读存储介质可以是ROM、随机存取存储器、CD-ROM、磁带、软盘和光数据存储设备等。其内存储的指令可由终端中的处理器加载并执行上述方法。

此外，需要说明的是，本发明可提供为方法、装置或计算机程序产品。因此，本发明实施例可采用完全硬件实施例、完全软件实施例或结合软件和硬件方面的实施例的形式。而且，本发明实施例可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质上实施的计算机程序产品的形式。

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

这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理终端设备以特定方式工作的计算机可读存储器中，使得存储在该计算机可读存储器中的指令产生包括指令装置的制造品，该指令装置实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能。这些计算机程序指令也可装载到计算机或其他可编程数据处理终端设备上，使得在计算机或其他可编程终端设备上执行一系列操作步骤以产生计算机实现的处理，从而在计算机或其他可编程终端设备上执行的指令提供用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的步骤。

还需要说明的是，在本文中，诸如第一和第二等之类的关系术语仅仅用来将一个实体或者操作与另一个实体或操作区分开来，而不一定要求或者暗示这些实体或操作之间存在任何这种实际的关系或者顺序。术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者终端设备不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、物品或者终端设备所固有的要素。在没有更多限制的情况下，由语句“包括一个……”限定的要素，并不排除在包括所述要素的过程、方法、物品或者终端设备中还存在另外的相同要素。

最后需要说明的是，以上所述是本发明优选实施方式，应当指出，尽管已描述了本发明优选实施例，但对于本技术领域的技术人员来说，一旦得知了本发明的基本创造性概念，在不脱离本发明所述原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也应视为本发明的保护范围。所以，所附权利要求意欲解释为包括优选实施例以及落入本发明实施例范围的所有变更和修改。

Claims (8)

1.一种轮式机器人IMU误差在线标定与补偿方法，其特征在于，包括：

获取第一校正数据；其中，所述第一校正数据为安装有惯性测量单元IMU的轮式机器人在水平面上处于静置状态时，所述IMU在第一预设时长内采集的一段连续的数据，所述第一校正数据包括X、Y、Z方向的加速度和角速度；

获取第二校正数据；其中，所述第二校正数据为所述轮式机器人在第二预设时长内，以预设速度在水平面上原地旋转预设角度的过程中，所述IMU采集的一段连续的数据，所述第二校正数据包括X、Y、Z方向的角速度；

建立加速度计与陀螺仪在线标定误差模型；

基于所述第一校正数据和第二校正数据，通过所述加速度计与陀螺仪在线标定误差模型，计算出陀螺仪和加速度计的零偏，完成所述IMU误差在线标定；

通过计算出的陀螺仪和加速度计的零偏对所述IMU的输出数据进行补偿。

2.如权利要求1所述的轮式机器人IMU误差在线标定与补偿方法，其特征在于，所述加速度计与陀螺仪在线标定误差模型的表达式如下：

其中，

表示角速度测量值，ω表示角速度理论值，w_g表示陀螺仪噪声，其类型为高斯白噪声，

表示加速度测量值，a表示加速度理论值，w_a表示加速度计噪声，其类型为高斯白噪声，b_g表示陀螺仪零偏，b_a表示加速度计零偏。

3.如权利要求2所述的轮式机器人IMU误差在线标定与补偿方法，其特征在于，所述基于所述第一校正数据和第二校正数据，通过所述加速度计与陀螺仪在线标定误差模型，计算出陀螺仪和加速度计的零偏，包括：

计算出所述第一校正数据中的加速度的平均值

以及所述第一校正数据中的角速度的平均值

由于IMU处于静置状态，且高斯白噪声数学期望为0，所以ω＝0，陀螺仪噪声平均值

加速度计噪声平均值

由此得到：

其中，

表示陀螺仪零偏在第一预设时长内的平均值；

计算出所述第二校正数据中的角速度的平均值

由于机器人在水平面上旋转，故旋转轴方向与重力方向相同，从而有：

又由于：

所以：

4.如权利要求3所述的轮式机器人IMU误差在线标定与补偿方法，其特征在于，所述通过计算出的陀螺仪和加速度计的零偏对所述IMU的输出数据进行补偿，包括：

通过计算出的陀螺仪和加速度计的零偏，基于所述加速度计与陀螺仪在线标定误差模型，利用下式对所述IMU的输出数据进行补偿：

其中，ω₂为补偿后的陀螺仪输出数据，a₂为补偿后的加速度计输出数据。

5.一种轮式机器人IMU误差在线标定与补偿装置，其特征在于，包括：

第一校正数据获取模块，用于获取第一校正数据；其中，所述第一校正数据为安装有惯性测量单元IMU的轮式机器人在水平面上处于静置状态时，所述IMU在第一预设时长内采集的一段连续的数据，所述第一校正数据包括X、Y、Z方向的加速度和角速度；

第二校正数据获取模块，用于获取第二校正数据；其中，所述第二校正数据为所述轮式机器人在第二预设时长内，以预设速度在水平面上原地旋转预设角度的过程中，所述IMU采集的一段连续的数据，所述第二校正数据包括X、Y、Z方向的角速度；

标定误差模型构建模块，用于建立加速度计与陀螺仪在线标定误差模型；

在线标定模块，用于基于所述第一校正数据获取模块获取的第一校正数据和所述第二校正数据获取模块获取的第二校正数据，通过所述标定误差模型构建模块所建立的加速度计与陀螺仪在线标定误差模型，计算出陀螺仪和加速度计的零偏，完成所述IMU误差在线标定；

补偿模块，用于通过所述在线标定模块计算出的陀螺仪和加速度计的零偏对所述IMU的输出数据进行补偿。

6.如权利要求5所述的轮式机器人IMU误差在线标定与补偿装置，其特征在于，所述标定误差模型构建模块建立的在线标定误差模型的表达式如下：

其中，

表示角速度测量值，ω表示角速度理论值，w_g表示陀螺仪噪声，其类型为高斯白噪声，

表示加速度测量值，a表示加速度理论值，w_a表示加速度计噪声，其类型为高斯白噪声，b_g表示陀螺仪零偏，b_a表示加速度计零偏。

7.如权利要求6所述的轮式机器人IMU误差在线标定与补偿装置，其特征在于，所述在线标定模块具体用于：

计算出所述第一校正数据中的加速度的平均值

以及所述第一校正数据中的角速度的平均值

由于IMU处于静置状态，且高斯白噪声数学期望为0，所以ω＝0，陀螺仪噪声平均值

加速度计噪声平均值

由此得到：

其中，

表示陀螺仪零偏在第一预设时长内的平均值；

计算出所述第二校正数据中的角速度的平均值

由于机器人在水平面上旋转，故旋转轴方向与重力方向相同，从而有：

又由于：

所以：

8.如权利要求7所述的轮式机器人IMU误差在线标定与补偿装置，其特征在于，所述补偿模块具体用于：

通过计算出的陀螺仪和加速度计的零偏，基于所述加速度计与陀螺仪在线标定误差模型，利用下式对所述IMU的输出数据进行补偿：

其中，ω₂为补偿后的陀螺仪输出数据，a₂为补偿后的加速度计输出数据。

Images