CN108731664A

CN108731664A - 机器人状态估计方法、装置、计算机设备和存储介质

Info

Publication number: CN108731664A
Application number: CN201810478673.6A
Authority: CN
Inventors: 刘天瑜; 廖青海; 刘洋; 王鲁佳; 刘明
Original assignee: Shenzhen Yiqing Creative Technology Ltd
Current assignee: Shenzhen Yiqing Creative Technology Ltd
Priority date: 2018-05-18
Filing date: 2018-05-18
Publication date: 2018-11-02
Anticipated expiration: 2038-05-18
Also published as: CN108731664B

Abstract

本申请涉及一种机器人状态估计方法、装置、计算机设备和存储介质。所述方法包括：通过惯性测量单元采集运动参数；将所述惯性测量单元采集的运动参数输入第一滤波器，获得机器人姿态；根据所述机器人姿态确定机器人运动方向；通过第二滤波器，根据所述机器人运动方向和编码器采集的线速度确定机器人位置；输出包含所述机器人姿态、所述线速度和所述机器人位置的状态信息。采用本方法能够避免因轮速差估计的机器人姿态精度低而使机器人状态估计的准确性低的技术问题。

Description

机器人状态估计方法、装置、计算机设备和存储介质

技术领域

本申请涉及机器人技术领域，特别是涉及一种机器人状态估计方法、装置、计算机设备和存储介质。

背景技术

机器人状态估计是机器人实现导航和运动规划等自主能力的前提，传统的机器人状态估计方案中，对于双主动轮机器人，机器人状态(如位置和姿态)的估计主要是：在主动轮上加装编码器读取各轮的轮速，获得轮速差，然后分别通过轮速和轮速差对机器人的位置和姿态进行估计，从而实现对机器人状态的估计。然而，由轮速差估计的机器人姿态精度较低，从而降低对机器人状态估计的准确性。

发明内容

基于此，有必要针对因轮速差估计的机器人姿态精度较低，而使机器人状态估计的准确性低的技术问题，提供一种机器人状态估计方法、装置、计算机设备和存储介质。

一种机器人状态估计方法，包括：

通过惯性测量单元采集运动参数；

将所述惯性测量单元采集的运动参数输入第一滤波器，获得机器人姿态；

根据所述机器人姿态确定机器人运动方向；

通过第二滤波器，根据所述机器人运动方向和编码器采集的线速度确定机器人位置；

输出包含所述机器人姿态、所述线速度和所述机器人位置的状态信息。

一种机器人状态估计装置，包括：

采集模块，用于通过惯性测量单元采集运动参数；

处理模块，用于将所述惯性测量单元采集的运动参数输入第一滤波器，获得机器人姿态；

第一确定模块，用于根据所述机器人姿态确定机器人运动方向；

第二确定模块，用于通过第二滤波器，根据所述机器人运动方向和编码器采集的线速度确定机器人位置；

输出模块，用于输出包含所述机器人姿态、所述线速度和所述机器人位置的状态信息。

一种计算机设备，包括存储器和处理器，所述存储器存储有计算机程序，其特征在于，所述计算机程序被处理器执行时实现上述机器人状态估计方法的步骤。

一种计算机可读存储介质，其上存储有计算机程序，其特征在于，所述计算机程序被处理器执行时实现上述机器人状态估计方法的步骤。

上述机器人状态估计方法、装置、计算机设备和存储介质，通过惯性测量单元采集运动参数，第一滤波器对采集的运动参数进行处理获得机器人姿态，惯性测量单元的精度高，所采集的运动参数具有高准确性，从而提高了对机器人状态估计的准确性。通过第二滤波器对线速度进行处理获得机器人位置，防止线速度跳变对机器人位置产生影响，进一步提高了对机器人状态估计的准确性。

附图说明

图1为一个实施例中机器人的结构框图；

图2为一个实施例中机器人状态估计方法的流程示意图；

图3为一个实施例中对多个惯性测量单元各自采集的运动参数进行加权融合，根据加权融合所得的结果确定机器人姿态步骤的流程示意图；

图4为另一个实施例中对多个惯性测量单元各自采集的角速度和磁力计向量进行加权融合，根据加权融合所得的结果确定机器人姿态步骤的流程示意图；

图5为另一个实施例中对机器人姿态进行更新步骤的流程示意图；

图6为另一个实施例中对第一滤波器的输入参数进行初始化步骤的流程示意图；

图7为另一个实施例中机器人状态估计方法的流程示意图；

图8为另一个实施例中机器人状态估计方法的流程示意图；

图9为一个实施例中机器人状态估计装置的结构框图；

图10为另一个实施例中机器人状态估计装置的结构框图；

图11为一个实施例中计算机设备的内部结构图。

具体实施方式

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

本申请提供的机器人状态估计方法，可以应用于如图1所示的机器人中。其中，机器人包括多个IMU(Inertial Measurement Unit，惯性测试单元)、多个编码器、两个滤波器(分别为第一滤波器和第二滤波器)和机器人状态模块。多个IMU分别与两个滤波器连接。多个编码器分别与第二滤波器连接。第一滤波器和第二滤波器连接。第二滤波器与机器人状态模块连接。其中，编码器可以是轮式编码器。滤波器可以是维纳滤波器或卡尔曼滤波器。

在一个实施例中，如图2所示，提供了一种机器人状态估计方法，以该方法应用于图1中的机器人为例进行说明，包括以下步骤：

S202，通过惯性测量单元采集运动参数。

其中，惯性测量单元包含磁力计、加速度计和陀螺仪。磁力计可以测量出机器人与东南西北四个方向上的夹角。加速度计检测机器人在坐标系的各坐标轴上的加速度信号，从而获得机器人在三维空间中的加速度。陀螺仪检测机器人相对于坐标系的各坐标轴上的角速度信号，从而获得机器人在三维空间中的角速度。

在一个实施例中，运动参数包括角速度、加速度和磁力计向量。机器人通过内置的惯性测量单元中的磁力计，测量出机器人与东南西北四个方向中任一个方向的夹角，根据所测量的夹角获得磁力计向量。机器人通过内置的惯性测量单元中的加速度计，测量出机器人运动的加速度。机器人通过内置的惯性测量单元中的陀螺仪，检测出机器人运动的角速度。

S204，将惯性测量单元采集的运动参数输入第一滤波器，获得机器人姿态。

其中，第一滤波器可以是维纳滤波器或卡尔曼滤波器。机器人姿态可以是关于机器人的旋转四元数，即使用旋转四元数表征机器人姿态。旋转四元数可用于描述机器人移动方向的变化，也即机器人自身的坐标系相对于固定的世界坐标系的变化。

在一个实施例中，机器人确定在坐标系中机器人旋转的旋转轴，通过运动参数中的角速度计算机器人的旋转角度，通过旋转轴和旋转角度确定旋转四元数，即机器人姿态。

例如，旋转四元数的表达式如下所示：

q＝cos(a/2)+i(x*sin(a/2))+j(y*sin(a/2))+k(z*sin(a/2))

其中a表示旋转角度，(x,y,z)表示旋转轴。假设旋转轴为RAix＝Z轴，换算成三维空间单位向量即为RAix＝[0,0,1]，旋转角度为60度。那么，转化成旋转四元数为：

q₀＝cos((60/2)*pi/180)＝0.866；

q_x＝RAix.x*sin((60/2)*pi/180)＝0*0.5＝0；

q_y＝RAix.y*sin((60/2)*pi/180)＝0*0.5＝0；

q_z＝RAix.z*sin((60/2)*pi/180)＝1*0.5＝0.5。

因此，可以得出旋转四元数，即机器人姿态q＝0.866+0.5k。

S206，根据机器人姿态确定机器人运动方向。

其中，运动方向可以指在运动过程中机器人相对某个参考方向的夹角。例如，假设机器人自身的坐标系为x_by_bz_b，地理坐标系为x_ty_tz_t，那么机器人相对自身的坐标系y_b轴在地平面上的投影与y_t轴间的夹角即为机器人运动方向。

在一个实施例中，机器人通过机器人姿态计算出航向角(yaw)，将计算出的航向角确定为机器人运动方向。

S208，通过第二滤波器，根据机器人运动方向和编码器采集的线速度确定机器人位置。

其中，第二滤波器可以是维纳滤波器或卡尔曼滤波器。编码器可以是安装于机器人上的轮式编码器，用于测量机器人动轮的线速度。位置可以是世界坐标系的坐标位置。

在一个实施例中，机器人通过内部安装的第二滤波器，根据编码器采集的线速度在机器人运动方向上进行积分运算，最终得到机器人位置。

例如，从三维空间中看，假设机器人在点A＝[0,1,0]，绕RAix＝Z轴旋转30度，通过第二滤波器计算机器人在点A旋转后在世界坐标系下的坐标将是B＝[0.866,0.5,0]。

在一个实施例中，当检测到定时的线速度采集指令，机器人获取编码器实时采集的线速度，计算当前所采集到的线速度与上一次采集到的线速度之间的差值。若所计算的差值小于或等于预设差值时，根据当前所采集的线速度在机器人运动方向上进行积分运算，最终得到机器人位置。从而避免了线速度的跳变对机器人的运动状态产生影响。

S210，输出包含机器人姿态、线速度和机器人位置的状态信息。

其中，状态信息用于表示机器人的姿态、位置和速度等。

在一个实施例中，机器人计算出机器人姿态、线速度和机器人位置时，将机器人姿态、线速度和机器人位置作为机器人的状态信息进行输出，即将状态信息传输至机器人状态模块。其中，机器人状态模块可以用于展示机器人的状态信息，例如展示机器人的姿态、线速度和机器人位置等。

上述实施例中，通过惯性测量单元采集运动参数，第一滤波器对采集的运动参数进行处理获得机器人姿态，惯性测量单元的精度高，所采集的运动参数具有高准确性，从而提高了对机器人状态估计的准确性。通过第二滤波器对线速度进行处理获得机器人位置，防止线速度跳变对机器人位置产生影响，进一步提高了对机器人状态估计的准确性。

在一个实施例中，如图3所示，S204具体可以包括：

S302，将多个惯性测量单元各自采集的运动参数输入第一滤波器。

S304，通过第一滤波器，将多个惯性测量单元各自采集的运动参数加权融合。

其中，加权融合可以指对运动参数进行加权处理，将加权处理后所得的运动参数进行求和。

在一个实施例中，当运动参数为角速度时，机器人通过第一滤波器对各个惯性测量单元各自采集的角速度进行加权处理，将加权处理后所得的角速度进行求和。

例如，若第i个惯性测量单元的在各惯性测量单元中的权重为λ_i时，使用λ_i对角速度w_i进行加权，加权得到λ_iw_i，然后对加权所得的角速度进行求和。

S306，根据加权融合所得的运动参数确定机器人姿态。

在一个实施例中，机器人确定在坐标系中机器人旋转的旋转轴，通过加权融合所得的运动参数计算机器人的旋转角度，通过旋转轴和旋转角度确定旋转四元数，即机器人姿态。

上述实施例中，对各惯性测量单元各自采集的运动参数进行加权求和，根据求和后的运动参数计算机器人状态，避免了单个惯性测量单元所采集的运动参数出现较大误差对机器人状态的估计产生影响，提高了估计机器人状态的准确性。

在一个实施例中，运动参数包括磁力计向量和角速度；如图4所示，S304具体可以包括：

S402，获取多个惯性测量单元各自的性能系数。

其中，性能系数可以称为输入系数，根据各惯性测量单元的误差值进行确定。误差值是根据各惯性测量单元输入第一滤波器的运动参数确定的。例如，根据校正后的各惯性测量单元预先所采集的运动参数对第一滤波器的观测量输入系数δ₁δ₂,...δ_i,...,δ_m进行初始化，其中m为惯性测量单元的数量。初始化过程所使用的表达式为：

e_i为第i个传感器的误差值，为惯性测量单元在世界坐标系下t时刻的旋转四元数，w_t为在t时刻下惯性测量单元的角速度，T为标定的总时间。

S404，获取磁力计向量之间的角度值。

在一个实施例中，机器人从各惯性测量单元中获取各磁力计向量，根据获取的各磁力计向量计算出各磁力计向量的夹角，并求各夹角的平均值，将该平均值确定为各磁力计向量之间的角度值。

S406，根据角度值和性能系数，确定多个惯性测量单元各自的权重。

其中，权重可以随角度值变化而发生变化。

S408，将各惯性测量单元的磁力计向量按照相应惯性测量单元的权重加权求和。

在一个实施例中，当机器人获得角度值和性能系数时，机器人根据角度值和性能系数确定多个惯性测量单元各自的权重，将各惯性测量单元的磁力计向量按照相应惯性测量单元的权重加权求和，加权求和所得的结果为磁力计向量的球面插值。

例如，假设惯性测量单元的个数为m，各惯性测量单元的磁力计向量分别为b₁,b₂,...b_i,...b_m，那么，可以获得磁力计向量的球面插值，如下所示：

S410，将各惯性测量单元的角速度按照相应惯性测量单元的权重加权求和。

在一个实施例中，当机器人获得角度值和性能系数时，机器人根据角度值和性能系数确定多个惯性测量单元各自的权重，将各惯性测量单元的角速度按照相应惯性测量单元的权重加权求和，加权求和所得的结果为磁力计向量的球面插值。

例如，假设惯性测量单元的个数为m，各惯性测量单元的角速度分别为w₁,w₂,...w_i,...w_m，那么，可以获得磁力计向量的球面插值，如下所示：

其中，为第i个惯性测量单元的权重。

S412，获取重力向量。

在一个实施例中，机器人通过重力计或重力传感器获取重力方向，根据重力方向获得重力向量。

例如，在地理坐标系x_ty_tz_t中，重力方向与Z轴平行，换算成三维空间单位向量即为[0,0,1]。

S414，根据重力向量、加权求和所得的磁力计向量和角速度确定机器人姿态。

在一个实施例中，机器人根据重力向量和磁力计向量的球面插值(即磁力计向量进行加权求和后所得的结果)，确定目标方向，根据目标方向计算出目标向量。其中，该目标方向与重力方向垂直，且与磁力计方向垂直。

在一个实施例中，由于机器人在地面上运动，将俯仰角方向为与水平面平行。根据磁力计向量、目标向量和重力向量确定世界坐标系，根据角速度确定机器人的旋转四元数，即机器人姿态。

上述实施例中，对各惯性测量单元各自采集的角速度和磁力计向量进行加权求和，分别获得加权求和后的角速度和磁力计向量，即角速度和磁力计向量的球面插值，根据加权求和后的角速度和磁力计向量计算机器人状态，避免了单个惯性测量单元所采集的运动参数出现较大误差对机器人状态的估计产生影响，提高了估计机器人状态的准确性。

在一个实施例中，如图5所示，该方法还可以包括以下步骤：

S502，通过第二滤波器获取第一滤波器输出的、对于机器人姿态更新的更新姿态。

在一个实施例中，当第一滤波器根据惯性测量单元新采集到的运动参数时，机器人通过第一滤波器对新采集到的运动参数进行处理，获得对于机器人姿态的更新姿态。其中，该更新姿态为新获得的机器人姿态。

S504，确定更新前机器人姿态与检测到的更新姿态之间的差值。

在一个实施例中，机器人计算更新前机器人姿态与检测到的更新姿态之间的差值。

S506，当差值小于预设差值时，输出包含更新姿态、线速度和机器人位置的状态信息。

在一个实施例中，机器人计算出机器人姿态、线速度和机器人位置，且差值小于预设差值时，将机器人姿态、线速度和机器人位置作为机器人的状态信息进行输出，即将状态信息传输至机器人状态模块。其中，机器人状态模块可以用于展示机器人的状态信息，例如展示机器人的姿态、线速度和机器人位置等。

上述实施例中，在输出机器人状态之前，计算更新前机器人姿态与检测到的更新姿态之间的差值，当差值小于预设差值时进行机器人状态的输出，确保了所输出的机器人姿态未发生跳变，从而提高了机器人状态估计的准确性。

在一个实施例中，在执行S202之前，需要对第一滤波器进行初始化，如图6所示，该方法还包括：

S602，通过第一滤波器，根据多个惯性测量单元各自预先采集的运动参数确定对于机器人的多个姿态。

其中，机器人的多个姿态指的是分别根据多个惯性测量单元各自预先采集的运动参数所计算出来的多个姿态。即根据惯性测量单元A所采集的运动参数计算出一个姿态，然后根据惯性测量单元B所采集的运动参数计算出一个姿态，即可获得机器人的多个姿态。

S604，根据所确定的多个姿态和各自预先采集的运动参数，确定各惯性测量单元的误差值。

在一个实施例中，运动参数为角速度，机器人根据多个惯性测量单元预先采集预设时长的角速度，通过第一滤波器对预先采集的角速度进行处理，获得预设时长下的机器人姿态。机器人根据预设时长下的角速度和机器人姿态确定各惯性测量单元的误差值。

S606，根据误差值确定各惯性测量单元的性能系数。

其中，性能系数可以称为输入系数，根据各惯性测量单元的误差值进行确定。例如，根据校正后的各惯性测量单元预先所采集的运动参数对第一滤波器的观测量输入系数δ₁δ₂,...δ_i,...,δ_m进行初始化，其中m为惯性测量单元的数量。初始化过程所使用的表达式为：

e_i为第i个传感器的误差值，为惯性测量单元在世界坐标系下t时刻的旋转四元数，w_it为在t时刻下惯性测量单元的角速度，T为标定的总时间。

S608，根据运动参数中的磁力计向量获得各磁力计方向之间的角度值。

S610，根据性能系数和角度值，确定多个惯性测量单元各自的权重；权重用于对输入第一滤波器的运动参数进行加权融合。

其中，权重可以随角度值变化而发生变化。

上述实施例中，对各惯性测量单元进行误差值的确定，进而得到各惯性测量单元的权值，用于对后续所采集到的运动参数进行加权融合，避免了单个惯性测量单元所采集的运动参数出现较大误差对机器人状态的估计产生影响，提高了估计机器人状态的准确性。

在一个实施例中，如图7所示，提供了另一种机器人状态估计方法，以该方法应用于图1中的机器人为例进行说明，其中，图1中的机器人按功能可划分为输入层、决策层和输出层。其中，输入层包括惯性测量单元和编码器，输入层的数据为惯性测量单元和编码器采集到的原始数据。决策层包括两个滤波器，分别为第一滤波器和第二滤波器。第一滤波器对惯性测量单元所采集到的运动参数进行在世界坐标系下机器人姿态的预估计。第二个滤波器对机器人在世界坐标系下的位置和机器人姿态的估计。输出层包括机器人状态模块，为用户展示可以直接使用的机器人位置、姿态和速度的状态信息。如图7所示，机器人状态估计方法包括以下步骤：

S702，对第一滤波器进行初始化。

第一滤波器的主要功能是进行惯性测量单元所采集的运动参数进行处理，对惯性测量单元采集到的运动参数，即线加速度、角速度以及指向地磁北的磁力计向量进行处理。状态量为观测量为[w,a,b]^T。其中，q,w,a,b分别表示惯性测量单元在同一坐标系下的旋转四元数，沿三个正交轴的角速度，沿三个正交轴的线加速度和指向地磁北的三维向量磁力计的读数。和分别为沿三个正交轴的角加速度和线加加速度。其中，角加速度为角速度在时间上的微分，线加加速度为线加速度在时间上的微分。输入为多个惯性测量单元所采集的运动参数经过融合后得到的数据，输出为一个固定频率的旋转四元数。

S702具体可以包括以下三个步骤：

S7021，系统上电后会进行自标定。包括各个惯性测量单元在三个正交轴的漂移系数d_x,d_y,d_z。

S7022，根据校正后惯性测量单元采集的运动参数对第一滤波器的输入系数δ₁δ₂,...δ_i,...,δ_m进行初始化，其中m为惯性测量单元的数量，其中m为传感器的数量。初始化过程所使用的表达式为：

S7023，根据输入系数对观测量进行球面插值，得到球面空间上的第一滤波器的球面加权观测据此计算出机器人的初始姿态，对坐标系进行初始化，初始化完成。

以两个IMU所得到的磁力计向量b₁,b₂为例，输入系数分别为δ₁δ₂，两向量的夹角为Ω。则球面插值描述为：

得到的初始磁力计向量为重力计测量得到方向为参考正东方向为h，初始姿态为q₀。则有：

初始化位置p₀为(0,0,0)。

初始化完成后，当获取到惯性测量单元新采集的运动参数后，对该次获得的观测值进行权值评估。在第一滤波器更新过程中，计算滤波器预测值q_est∈so(3)与观测值q_mes∈so(3)的流形空间距离作为误差值e＝q_est-q_mes。使用误差值计算得到输入系数计算过程与初始化过程中求取输入系数的过程相同。通过更新过程可得到机器人的当前的旋转四元数。

S704，对第二滤波器进行初始化。

第二滤波器的主要功能是对惯性测量单元采集的运动参数、第一滤波器输出的数据和编码器采集的数据(如线速度)进行融合。如图1所示，第二滤波器输入数据由输入层(惯性测量单元和编码器)的原始数据与第一滤波器的输出数据(可称为预测值)共同构成。其中，第二滤波器的状态量包括惯性测量单元和编码器在世界坐标系下的[p,v,q,w,a]，分别代表传感器坐标下的位置p，线速度v，旋转四元数q，角速度w以及线加速度a。初始化过程包括以下两步：

S7041，当第一滤波器初始化完成时，将根据接收到第一个由第一滤波器计算所得到的旋转四元数作为初始的机器人姿态。

S7042，对传感器接收队列进行初始化。例如，当接收到惯性测量单元新采集到的运动参数时，将采集到的运动参数送入到待处理的IMU队列中，用于对系统的w和a进行更新。当收到编码器新采集到的线速度信息时，将线速度信息送入到待处理的线速度队列中，用于对系统的w和v进行更新。当收到第一滤波器根据惯性测量单元新采集到的运动参数所获得的旋转四元数时，将旋转四元数送入到待处理的旋转四元数队列，用于对q的更新。

S706，初始化完成之后，当获取到编码器、惯性测量单元所采集的数据以及第一滤波器的输出时，对机器人的状态进行更新。并且以固定的频率对机器人的位置进行预测，将预测结果作为输出结果。

输出层的数据为用户可以直接利用的数据，为机器人当前在世界坐标系下的位置、线速度和机器人姿态[p,v,q]，频率为150Hz。

上述实施例中，通过第一滤波器使用多个惯性测量单元所采集的运动参数进行机器人姿态估计，惯性测量单元的精度高，所采集的运动参数具有高准确性，从而提高了对机器人状态估计的准确性。通过第二滤波器对多惯性测量单元所采集的运动参数与编码器所采集的线速度融合进行机器人姿态以及位置的估计，实现了机器人姿态的二次估计，避免了机器人姿态的累积误差，提高了机器人状态估计的准确性。

在一个实施例中，如图8所示，提供了一种详细的机器人状态估计方法，以该方法应用于图1中的机器人为例进行说明，包括以下步骤：

S802，通过第一滤波器，根据多个惯性测量单元各自预先采集的运动参数确定对于机器人的多个姿态。

S804，根据所确定的多个姿态和各自预先采集的运动参数，确定各惯性测量单元的误差值。

S806，根据误差值确定各惯性测量单元的性能系数。

S808，根据运动参数中的磁力计向量获得各磁力计方向之间的角度值。

S810，根据性能系数和角度值，确定多个惯性测量单元各自的权重；权重用于对输入第一滤波器的运动参数进行加权融合。

S812，通过惯性测量单元采集运动参数。

S814，将惯性测量单元采集的运动参数进行加权融合后输入第一滤波器，获得机器人姿态。

在一个实施例中，将多个惯性测量单元各自采集的运动参数输入第一滤波器；通过第一滤波器，将多个惯性测量单元各自采集的运动参数加权融合；根据加权融合所得的运动参数确定机器人姿态。

在一个实施例中，获取多个惯性测量单元各自的性能系数；获取磁力计向量之间的角度值；根据角度值和性能系数，确定多个惯性测量单元各自的权重；将各惯性测量单元的磁力计向量按照相应惯性测量单元的权重加权求和。将各惯性测量单元的角速度按照相应惯性测量单元的权重加权求和；获取重力向量；根据重力向量、加权求和所得的磁力计向量和角速度确定机器人姿态。

S816，根据机器人姿态确定机器人运动方向。

S818，通过第二滤波器，根据机器人运动方向和编码器采集的线速度确定机器人位置。

S820，通过第二滤波器获取第一滤波器输出的、对于机器人姿态更新的更新姿态。

S822，确定更新前机器人姿态与检测到的更新姿态之间的差值。

S824，当差值小于预设差值时，输出包含更新姿态、线速度和机器人位置的状态信息。

应该理解的是，虽然图2的流程图中的各个步骤按照箭头的指示依次显示，但是这些步骤并不是必然按照箭头指示的顺序依次执行。除非本文中有明确的说明，这些步骤的执行并没有严格的顺序限制，这些步骤可以以其它的顺序执行。而且，图2中的至少一部分步骤可以包括多个子步骤或者多个阶段，这些子步骤或者阶段并不必然是在同一时刻执行完成，而是可以在不同的时刻执行，这些子步骤或者阶段的执行顺序也不必然是依次进行，而是可以与其它步骤或者其它步骤的子步骤或者阶段的至少一部分轮流或者交替地执行。

在一个实施例中，如图9所示，提供了一种机器人状态估计装置，包括：采集模块902、处理模块904、第一确定模块906、第二确定模块908和输出模块910，其中：

采集模块902，用于通过惯性测量单元采集运动参数；

处理模块904，用于将惯性测量单元采集的运动参数输入第一滤波器，获得机器人姿态；

第一确定模块906，用于根据机器人姿态确定机器人运动方向；

第二确定模块908，用于通过第二滤波器，根据机器人运动方向和编码器采集的线速度确定机器人位置；

输出模块910，用于输出包含机器人姿态、线速度和机器人位置的状态信息。

在一个实施例中，处理模块904还用于将多个惯性测量单元各自采集的运动参数输入第一滤波器；通过第一滤波器，将多个惯性测量单元各自采集的运动参数加权融合；根据加权融合所得的运动参数确定机器人姿态。

在一个实施例中，运动参数包括磁力计向量和角速度；处理模块904还用于获取多个惯性测量单元各自的性能系数；获取磁力计向量之间的角度值；根据角度值和性能系数，确定多个惯性测量单元各自的权重；将各惯性测量单元的磁力计向量按照相应惯性测量单元的权重加权求和；将各惯性测量单元的角速度按照相应惯性测量单元的权重加权求和；

第一确定模块还用于获取重力向量；根据重力向量、加权求和所得的磁力计向量和角速度确定机器人姿态。

在一个实施例中，如图10所示，该装置还包括：更新模块912和第三确定模块914；其中：

更新模块912，用于通过第二滤波器获取第一滤波器输出的、对于机器人姿态更新的更新姿态；

第三确定模块914，用于确定更新前机器人姿态与检测到的更新姿态之间的差值；

输出模块910还用于当差值小于预设差值时，输出包含更新姿态、线速度和机器人位置的状态信息。

在一个实施例中，如图10所示，该装置还包括：第四确定模块916、获取模块918；其中：

第四确定模块916，用于通过第一滤波器，根据多个惯性测量单元各自预先采集的运动参数确定对于机器人的多个姿态；根据所确定的多个姿态和各自预先采集的运动参数，确定各惯性测量单元的误差值；根据误差值确定各惯性测量单元的性能系数；

获取模块918，用于根据运动参数中的磁力计向量获得各磁力计方向之间的角度值；

第四确定模块916还用于根据性能系数和角度值，确定多个惯性测量单元各自的权重；权重用于对输入第一滤波器的运动参数进行加权融合。

关于机器人状态估计装置的具体限定可以参见上文中对于机器人状态估计方法的限定，在此不再赘述。上述机器人状态估计装置中的各个模块可全部或部分通过软件、硬件及其组合来实现。上述各模块可以硬件形式内嵌于或独立于计算机设备中的处理器中，也可以以软件形式存储于计算机设备中的存储器中，以便于处理器调用执行以上各个模块对应的操作。

在一个实施例中，提供了一种计算机设备，该计算机设备可以是终端，其内部结构图可以如图11所示。该计算机设备包括通过系统总线连接的处理器、存储器、网络接口、显示屏和输入装置。其中，该计算机设备的处理器用于提供计算和控制能力。该计算机设备的存储器包括非易失性存储介质、内存储器。该非易失性存储介质存储有操作系统和计算机程序。该内存储器为非易失性存储介质中的操作系统和计算机程序的运行提供环境。该计算机设备的网络接口用于与外部的终端通过网络连接通信。该计算机程序被处理器执行时以实现一种机器人状态估计方法。该计算机设备的显示屏可以是液晶显示屏或者电子墨水显示屏，该计算机设备的输入装置可以是显示屏上覆盖的触摸层，也可以是计算机设备外壳上设置的按键、轨迹球或触控板，还可以是外接的键盘、触控板或鼠标等。

本领域技术人员可以理解，图11中示出的结构，仅仅是与本申请方案相关的部分结构的框图，并不构成对本申请方案所应用于其上的计算机设备的限定，具体的计算机设备可以包括比图中所示更多或更少的部件，或者组合某些部件，或者具有不同的部件布置。

在一个实施例中，提供了一种计算机设备，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，处理器执行计算机程序时实现以下步骤：通过惯性测量单元采集运动参数；将惯性测量单元采集的运动参数输入第一滤波器，获得机器人姿态；根据机器人姿态确定机器人运动方向；通过第二滤波器，根据机器人运动方向和编码器采集的线速度确定机器人位置；输出包含机器人姿态、线速度和机器人位置的状态信息。

在一个实施例中，计算机程序被处理器执行将惯性测量单元采集的运动参数输入第一滤波器，获得机器人姿态的步骤时，使得处理器具体执行以下步骤：将多个惯性测量单元各自采集的运动参数输入第一滤波器；通过第一滤波器，将多个惯性测量单元各自采集的运动参数加权融合；根据加权融合所得的运动参数确定机器人姿态。

在一个实施例中，运动参数包括磁力计向量和角速度；计算机程序被处理器执行将多个惯性测量单元各自采集的运动参数加权融合的步骤时，使得处理器具体执行以下步骤：获取多个惯性测量单元各自的性能系数；根据磁力计向量确定各磁力计方向之间的角度值；根据角度值和性能系数，确定多个惯性测量单元各自的权重；将各惯性测量单元的磁力计向量按照相应惯性测量单元的权重加权求和；将各惯性测量单元的角速度按照相应惯性测量单元的权重加权求和；

计算机程序被处理器执行根据加权融合所得的运动参数确定机器人姿态的步骤时，使得处理器具体执行以下步骤：获取重力向量；根据重力向量、加权求和所得的磁力计向量和角速度确定机器人姿态。

在一个实施例中，处理器执行计算机程序时还实现以下步骤：通过第二滤波器获取第一滤波器输出的、对于机器人姿态更新的更新姿态；确定更新前机器人姿态与检测到的更新姿态之间的差值；

计算机程序被处理器执行输出包含机器人姿态、线速度和机器人位置的状态信息的步骤时，使得处理器具体执行以下步骤：当差值小于预设差值时，输出包含更新姿态、线速度和机器人位置的状态信息。

在一个实施例中，处理器执行计算机程序时还实现以下步骤：通过第一滤波器，根据多个惯性测量单元各自预先采集的运动参数确定对于机器人的多个姿态；根据所确定的多个姿态和各自预先采集的运动参数，确定各惯性测量单元的误差值；根据误差值确定各惯性测量单元的性能系数；根据运动参数中的磁力计向量获得各磁力计方向之间的角度值；根据性能系数和角度值，确定多个惯性测量单元各自的权重；权重用于对输入第一滤波器的运动参数进行加权融合。

在一个实施例中，提供了一种计算机可读存储介质，其上存储有计算机程序，计算机程序被处理器执行时实现以下步骤：通过惯性测量单元采集运动参数；将惯性测量单元采集的运动参数输入第一滤波器，获得机器人姿态；根据机器人姿态确定机器人运动方向；通过第二滤波器，根据机器人运动方向和编码器采集的线速度确定机器人位置；输出包含机器人姿态、线速度和机器人位置的状态信息。

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

以上实施例的各技术特征可以进行任意的组合，为使描述简洁，未对上述实施例中的各个技术特征所有可能的组合都进行描述，然而，只要这些技术特征的组合不存在矛盾，都应当认为是本说明书记载的范围。

以上所述实施例仅表达了本申请的几种实施方式，其描述较为具体和详细，但并不能因此而理解为对发明专利范围的限制。应当指出的是，对于本领域的普通技术人员来说，在不脱离本申请构思的前提下，还可以做出若干变形和改进，这些都属于本申请的保护范围。因此，本申请专利的保护范围应以所附权利要求为准。

Claims

1.一种机器人状态估计方法，包括：

通过惯性测量单元采集运动参数；

根据所述机器人姿态确定机器人运动方向；

2.根据权利要求1所述的方法，其特征在于，所述将所述惯性测量单元采集的运动参数输入第一滤波器，获得机器人姿态，包括：

将多个惯性测量单元各自采集的运动参数输入第一滤波器；

通过所述第一滤波器，将多个惯性测量单元各自采集的运动参数加权融合；

根据加权融合所得的运动参数确定机器人姿态。

3.根据权利要求2所述的方法，其特征在于，所述运动参数包括磁力计向量和角速度；所述将多个惯性测量单元各自采集的运动参数加权融合包括：

获取多个惯性测量单元各自的性能系数；

根据所述磁力计向量确定各磁力计方向之间的角度值；

根据所述角度值和所述性能系数，确定多个惯性测量单元各自的权重；

将各惯性测量单元的磁力计向量按照相应惯性测量单元的权重加权求和；

将各惯性测量单元的角速度按照相应惯性测量单元的权重加权求和；

所述根据加权融合所得的运动参数确定机器人姿态包括：

获取重力向量；根据所述重力向量、加权求和所得的磁力计向量和角速度确定机器人姿态。

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

通过所述第二滤波器获取所述第一滤波器输出的、对于所述机器人姿态更新的更新姿态；

确定更新前所述机器人姿态与检测到的更新姿态之间的差值；

所述输出包含所述机器人姿态、所述线速度和所述机器人位置的状态信息,包括：

当所述差值小于预设差值时，输出包含所述更新姿态、所述线速度和所述机器人位置的状态信息。

5.根据权利要求1至4任一项所述的方法，其特征在于，所述通过惯性测量单元采集运动参数之前，所述方法还包括：

通过所述第一滤波器，根据多个惯性测量单元各自预先采集的运动参数确定对于机器人的多个姿态；

根据所确定的多个姿态和所述各自预先采集的运动参数，确定各惯性测量单元的误差值；

根据所述误差值确定各惯性测量单元的性能系数；

根据所述运动参数中的磁力计向量获得各磁力计方向之间的角度值；

根据所述性能系数和所述角度值，确定多个惯性测量单元各自的权重；所述权重用于对输入所述第一滤波器的运动参数进行加权融合。

6.一种机器人状态估计装置，包括：

采集模块，用于通过惯性测量单元采集运动参数；

7.根据权利要求6所述的装置，其特征在于，所述处理模块还用于将多个惯性测量单元各自采集的运动参数输入第一滤波器；通过所述第一滤波器，将多个惯性测量单元各自采集的运动参数加权融合；根据加权融合所得的运动参数确定机器人姿态。

8.根据权利要求7所述的装置，其特征在于，所述运动参数包括磁力计向量和角速度；所述处理模块还用于获取多个惯性测量单元各自的性能系数；获取所述磁力计向量之间的角度值；根据所述角度值和所述性能系数，确定多个惯性测量单元各自的权重；将各惯性测量单元的磁力计向量按照相应惯性测量单元的权重加权求和；将各惯性测量单元的角速度按照相应惯性测量单元的权重加权求和；

所述第一确定模块还用于获取重力向量；根据所述重力向量、加权求和所得的磁力计向量和角速度确定机器人姿态。

9.一种计算机设备，包括存储器和处理器，所述存储器存储有计算机程序，其特征在于，所述计算机程序被处理器执行时实现权利要求1至5中任一项所述方法的步骤。

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