CN117325149A - 基于卡尔曼滤波的姿态调整方法及系统 - Google Patents

Abstract

本发明提供一种基于卡尔曼滤波的姿态调整方法及系统，包括：获取一维姿态数据；对所述一维姿态数据进行卡尔曼一维滤波，以得到一维姿态值；建立姿态运动学模型，以得到姿态预测值；对所述一维姿态值和所述姿态预测值进行卡尔曼二维融合，以得到最佳估计值；利用所述最佳估计值进行姿态调整。通过卡尔曼一维滤波，使得在对姿态调整时，不仅能够有效地取出噪声和误差，还可以处理非线性的动态参数和测量值；通过卡尔曼二维融合，使得能够提高数据的精度、稳定性和鲁棒性；顺次进行卡尔曼一维滤波和卡尔曼二维融合，能够有效提高姿态调整的控制和运动规划能力，解决了如何快速、精准地实现机器人的姿态调整的问题。

Description

基于卡尔曼滤波的姿态调整方法及系统

技术领域

本发明涉及姿态调整及数据处理技术领域，特别涉及一种基于卡尔曼滤波的姿态调整方法及系统。

背景技术

随着工业化程度的不断深化，机器人应用于越来越多的场景，从而对机器人的姿态精度提出了越来越高的要求。一般，对机器人作业端安装陀螺仪，通过获取陀螺仪数据并对该数据进行处理分析来获得机器人作业端的当前姿态，进而根据作业进程对机器人作业端的姿态进行调整，以实现机器人姿态的高精度调节。

然而在机器人实际应用场景中，不可避免的会存在噪声，若不对噪声进行处理，则很可能导致机器人的不准确运动、失控、不安全等问题的产生，严重的还可能会造成重大的损失和伤害。因此在对陀螺仪数据处理前必须进行滤波处理，以避免噪声导致的数据处理结果异常。

在陀螺仪数据处理技术方面，传统的方法包括基于IIR(无限冲击脉冲)滤波、FIR(有限冲激响应滤波器)滤波、中值滤波等滤波方法，以及基于机器学习和神经网络的方法等。但这些方法无法快速、精准地将噪声滤除，可靠性较差。

发明内容

本发明的目的在于提供一种基于卡尔曼滤波的姿态调整方法及系统，以解决如何快速、精准地实现机器人的姿态调整的问题。

为解决上述技术问题，本发明提供一种基于卡尔曼滤波的姿态调整方法，所述基于卡尔曼滤波的姿态调整方法包括：

获取一维姿态数据；

对所述一维姿态数据进行卡尔曼一维滤波，以得到一维姿态值；

建立姿态运动学模型，以得到姿态预测值；

对所述一维姿态值和所述姿态预测值进行卡尔曼二维融合，以得到最佳估计值；

利用所述最佳估计值进行姿态调整。

可选的，在所述的基于卡尔曼滤波的姿态调整方法中，获取至少两组所述一维姿态数据，不同组的所述一维姿态数据来源于不同的姿态传感器。

可选的，在所述的基于卡尔曼滤波的姿态调整方法中，所述对所述一维姿态数据进行卡尔曼一维滤波，以得到一维姿态值的方法包括：

获取卡尔曼一维滤波参数，所述卡尔曼一维滤波参数包括状态转移矩阵、控制矩阵、外部控制向量、状态估计误差协方差矩阵、观测矩阵、观测信息、过程噪声协方差矩阵和观测噪声协方差矩阵；

利用所述卡尔曼一维滤波参数对所述一维姿态数据进行标准卡尔曼滤波处理，其中：

预测方程为：

更新方程为：

P_k ^′＝P_k-K^′H_kP

其中，表示第k时刻的系统状态向量，F_k表示从第k-1时刻到第k时刻的状态转移矩阵，B_k表示第k时刻的控制矩阵，/>表示第k时刻的外部控制向量，P_k表示第k时刻的状态估计误差协方差矩阵，Q_k表示第k时刻的一维过程噪声协方差矩阵；K^′表示新的卡尔曼系数，H_k表示第k时刻的观测矩阵，R_k表示第k时刻的一维观测噪声协方差矩阵，/>表示第k时刻的观测信息，P_k ^′表示新的状态估计误差协方差矩阵。

可选的，在所述的基于卡尔曼滤波的姿态调整方法中，状态转移矩阵F_k＝1；

控制矩阵B_k＝0；

外部控制向量或a，其中，a表示电机加速度，且a由电机参数计算得到；

观测矩阵H_k＝1；

过程噪声协方差矩阵Q_k和观测噪声协方差矩阵R_k的取值范围为0.001～0.5。

可选的，在所述的基于卡尔曼滤波的姿态调整方法中，所述电机加速度a的计算方法包括：

获取电机参数，所述电机参数包括初始角度和初始角速度；

利用所述电机参数计算电机加速度，其中计算方程为：

X＝X₀+V×Δt+0.5a(Δt)²

V＝V₀+a×Δt

得，电机加速度为：

其中，X表示角度，X₀表示初始角度，V表示角速度，V₀表示初始角速度，Δt表示时间间隔。

可选的，在所述的基于卡尔曼滤波的姿态调整方法中，所述建立姿态运动学模型，以得到姿态预测值的方法包括：

利用所述一维姿态数据构建姿态运动学模型，所述姿态运动学模型为匀速直线运动模型；

利用所述姿态运动学模型进行仿真模拟，以得到姿态预测值。

可选的，在所述的基于卡尔曼滤波的姿态调整方法中，所述对所述一维姿态值和所述姿态预测值进行卡尔曼二维融合，以得到最佳估计值的方法包括：

获取卡尔曼二维融合参数，所述卡尔曼二维融合参数包括平衡点方程、状态转移矩阵、外部控制向量、状态估计误差协方差矩阵、观测矩阵、观测信息、过程噪声协方差矩阵和观测噪声协方差矩阵；

利用所述卡尔曼二维融合参数对所述一维姿态值和所述姿态预测值进行卡尔曼二维融合处理，其中：

预测方程为：

更新方程为：

其中，f表示平衡点方程，W_k表示第k时刻的二维过程噪声协方差矩阵，V_k表示第k时刻的二维观测噪声协方差矩阵，I表示单位矩阵

可选的，在所述的基于卡尔曼滤波的姿态调整方法中，所述利用所述卡尔曼二维融合参数对所述一维姿态值和所述姿态预测值进行卡尔曼二维融合处理的方法还包括：

建立状态方程：

其中，An1_k和An2_k分别表示第k时刻的两个角度，Ve1_k和Ve2_k分别表示第k时刻的两个角速度；

得，状态转移矩阵为：

其中，Δt表示时间间隔。

为解决上述技术问题，本发明还提供一种基于卡尔曼滤波的姿态调整系统，用于实现如上任一项所述的基于卡尔曼滤波的姿态调整方法，所述姿态调整系统包括：

一维姿态值获取模块，用于获取一维姿态数据，并对所述一维姿态数据进行卡尔曼一维滤波得到一维姿态值；

姿态预测值获取模块，包括姿态运动学模型，以利用所述姿态运动学模型得到姿态预测值；

姿态数据处理模块，用于对所述一维姿态值和所述姿态预测值进行卡尔曼二维融合得到最佳估计值；

姿态控制调整模块，用于根据所述最佳估计值进行姿态调整。

可选的，在所述的基于卡尔曼滤波的姿态调整系统中，所述一维姿态值获取模块包括：

传感器单元，用于接收至少两组所述一维姿态数据；

滤波单元，用于对所述一维姿态数据进行卡尔曼一维滤波，以得到一维姿态值。

可选的，在所述的基于卡尔曼滤波的姿态调整系统中，所述姿态数据处理模块包括：

融合单元，用于对所述一维姿态值和所述姿态预测值进行卡尔曼二维融合，以得到最佳估计值；

解算单元，用于根据所述最佳估计值对姿态进行运动学逆解，以得到姿态调整参数；

控制单元，用于根据所述最佳估计值和所述姿态调整参数进行姿态调整。

本发明提供的基于卡尔曼滤波的姿态调整方法及系统，包括：获取一维姿态数据；对所述一维姿态数据进行卡尔曼一维滤波，以得到一维姿态值；建立姿态运动学模型，以得到姿态预测值；对所述一维姿态值和所述姿态预测值进行卡尔曼二维融合，以得到最佳估计值；利用所述最佳估计值进行姿态调整。通过卡尔曼一维滤波，使得在对姿态调整时，不仅能够有效地取出噪声和误差，还可以处理非线性的动态参数和测量值；通过卡尔曼二维融合，使得能够提高数据的精度、稳定性和鲁棒性；顺次进行卡尔曼一维滤波和卡尔曼二维融合，能够有效提高姿态调整的控制和运动规划能力，解决了如何快速、精准地实现机器人的姿态调整的问题。

附图说明

图1为本实施例提供的基于卡尔曼滤波的姿态调整方法的流程图；

图2为本实施例提供的基于卡尔曼滤波的姿态调整系统的结构示意图；

图3为本实施例提供的基于卡尔曼滤波的姿态调整系统的静态数据处理实际效果图；

图4为本实施例提供的基于卡尔曼滤波的姿态调整系统的数据融合实际效果图。

具体实施方式

以下结合附图和具体实施例对本发明提出的基于卡尔曼滤波的姿态调整方法及系统作进一步详细说明。需说明的是，附图均采用非常简化的形式且均使用非精准的比例，仅用以方便、明晰地辅助说明本发明实施例的目的。此外，附图所展示的结构往往是实际结构的一部分。特别的，各附图需要展示的侧重点不同，有时会采用不同的比例。

需要说明的是，本发明的说明书和权利要求书及附图说明中的“第一”、“第二”等是用于区别类似的对象，以便描述本发明的实施例，而不用于描述特定的顺序或先后次序，应该理解这样使用的结构在适当情况下可以互换。此外，术语“包括”和“具有”以及他们的任何变形，意图在于覆盖不排他的包含，例如，包含了一系列步骤或单元的过程、方法、系统、产品或设备不必限于清楚地列出的那些步骤或单元，而是可包括没有清楚地列出的或对于这些过程、方法、产品或设备固有的其它步骤或单元。

本实施例提供一种基于卡尔曼滤波的姿态调整方法，如图1所示，所述基于卡尔曼滤波的姿态调整方法包括：

S1，获取一维姿态数据；

S2，对所述一维姿态数据进行卡尔曼一维滤波，以得到一维姿态值；

S3，建立姿态运动学模型，以得到姿态预测值；

S4，对所述一维姿态值和所述姿态预测值进行卡尔曼二维融合，以得到最佳估计值；

S5，利用所述最佳估计值进行姿态调整。

本实施例提供的基于卡尔曼滤波的姿态调整方法，通过卡尔曼一维滤波，使得在对姿态调整时，不仅能够有效地取出噪声和误差，还可以处理非线性的动态参数和测量值；通过卡尔曼二维融合，使得能够提高数据的精度、稳定性和鲁棒性；顺次进行卡尔曼一维滤波和卡尔曼二维融合，能够有效提高姿态调整的控制和运动规划能力，解决了如何快速、精准地实现机器人的姿态调整的问题。

具体的，在本实施例中，步骤S1，获取一维姿态数据时，获取至少两组所述一维姿态数据，不同组的所述一维姿态数据来源于不同的姿态传感器。在实际应用中，可以通过对机器人设置两组或多组陀螺仪，每组陀螺仪获取不同的参数数据，通过对每一组陀螺仪的数据进行获取，便可以得到一维姿态数据。

进一步的，在本实施例中，步骤S2，对所述一维姿态数据进行卡尔曼一维滤波，以得到一维姿态值的方法包括：

S21，获取卡尔曼一维滤波参数，所述卡尔曼一维滤波参数包括状态转移矩阵、控制矩阵、外部控制向量、状态估计误差协方差矩阵、观测矩阵、观测信息、过程噪声协方差矩阵和观测噪声协方差矩阵。具体的，状态转移矩阵用于描述状态变量之间的关系，通常可以使用动力学模型来获得；控制矩阵可以根据系统的物理特性和控制量的作用方式来设计；外部控制向量用于描述外部控制因素对状态量的影响，可以视具体情况进行设计；过程噪声协方差矩阵和观测噪声协方差矩阵是表述系统模型中存在的噪声和不确定性对系统状态的影响，可以通过实验或者经验来确定过程噪声协方差矩阵和观测噪声协方差矩阵的取值，从而能够获得更加准确的状态预测和估计。

S22，利用所述卡尔曼一维滤波参数对所述一维姿态数据进行标准卡尔曼滤波处理，其中：

预测方程为：

更新方程为：

P_k ^′＝P_k-K^′H_kP

其中，表示第k时刻的系统状态向量，F_k表示从第k-1时刻到第k时刻的状态转移矩阵，B_k表示第k时刻的控制矩阵，/>表示第k时刻的外部控制向量，P_k表示第k时刻的状态估计误差协方差矩阵，Q_k表示第k时刻的一维过程噪声协方差矩阵；K^′表示新的卡尔曼系数，H_k表示第k时刻的观测矩阵，R_k表示第k时刻的一维观测噪声协方差矩阵，/>表示第k时刻的观测信息，P_k ^′表示新的状态估计误差协方差矩阵。具体的卡尔曼一维标准滤波的方式为本领域技术人员所熟知的，此处不再赘述。

较佳的，为了简化模型及运算量，使得姿态调整的效率更高，在本实施例中，状态转移矩阵F_k＝1，这是由于本实施例中此步骤只处理一维数据，例如角度值，因此此处状态转移矩阵F_k＝1；控制矩阵B_k＝0，这是因为在本实施例中省略了自主的外部输入；观测矩阵H_k＝1，这是由于处理的一维数据的值是通过传感器直接测量获得的，例如通过传感器直接获取到角度值；外部控制向量或a，其中，a表示电机加速度，且a由电机参数计算得到，当外部控制向量/>时，表明此处没有考虑自主的外部输入，因此没有外部控制因素对状态量施加影响，当外部控制向量/>时，表明此时考虑了电机带动机器人运动，因此外部控制因素需考虑电机的影响。

具体的，当外部控制向量时，本实施例提供一种a的计算方法，包括：

获取电机参数，所述电机参数包括初始角度和初始角速度；

利用所述电机参数计算电机加速度，其中计算方程为：

X＝X₀+V×Δt+0.5a(Δt)²

V＝V₀+a×Δt

得，电机加速度为：

其中，X表示角度，X₀表示初始角度，V表示角速度，V₀表示初始角速度，Δt表示时间间隔。

以及，通过大量的实验验证及经验，过程噪声协方差矩阵Q_k和观测噪声协方差矩阵R_k的取值范围为0.001～0.5。例如，在实验过程中，若发现测量误差较大时，则可以加大观测噪声协方差矩阵R_k的取值，从而使最佳估计值更接近姿态预测值，反之，若发现测量误差较小时，则可以加大过程噪声协方差矩阵Q_k的取值，从而使最佳估计值更接近一维姿态值。通过调整过程噪声协方差矩阵Q_k和观测噪声协方差矩阵R_k的取值使得最终得到的最佳估计值更贴近于实际。

在本实施例中，步骤S3，建立姿态运动学模型，以得到姿态预测值的方法包括：

S31，利用所述一维姿态数据构建姿态运动学模型，所述姿态运动学模型为匀速直线运动模型。当然，在其他实施例中，考虑更复杂的情形，也可以选择其他姿态运动学模型。本实施例中由于处理的一维姿态数据为角度值，因此可以将姿态运动学模型简化为匀速直线运动模型，即位移-时间模型，如此可以使得模型的计算量较小，实时性高。

S32，利用所述姿态运动学模型进行仿真模拟，以得到姿态预测值。

进一步的，在本实施例中，步骤S4，对所述一维姿态值和所述姿态预测值进行卡尔曼二维融合，以得到最佳估计值的方法包括：

S41，获取卡尔曼二维融合参数，所述卡尔曼二维融合参数包括平衡点方程、状态转移矩阵、外部控制向量、状态估计误差协方差矩阵、观测矩阵、观测信息、过程噪声协方差矩阵和观测噪声协方差矩阵。具体的，在卡尔曼二维融合过程中，选择的参数部分来源于卡尔曼一维滤波，例如状态转移矩阵、外部控制向量、状态估计误差协方差矩阵、观测矩阵、观测信息等；过程噪声协方差矩阵包括在卡尔曼一维滤波过程中产生的噪声所对应的一维过程噪声协方差矩阵，还包括在卡尔曼二维融合过程中产生的噪声所对应的二维过程噪声协方差矩阵；同理，观测噪声协方差矩阵也同样包括在卡尔曼一维滤波过程中产生的噪声所对应的一维观测噪声协方差矩阵和在卡尔曼二维融合过程中产生的噪声所对应的二维观测噪声协方差矩阵。各个参数的获得可以参考卡尔曼一维滤波过程，此处不再赘述。

S42，利用所述卡尔曼二维融合参数对所述一维姿态值和所述姿态预测值进行卡尔曼二维融合处理，其中：

预测方程为：

更新方程为：

其中，f表示平衡点方程，W_k表示第k时刻的二维过程噪声协方差矩阵，V_k表示第k时刻的二维观测噪声协方差矩阵，I表示单位矩阵。

具体的，在本实施例中，为了获得状态转移矩阵，利用所述卡尔曼二维融合参数对所述一维姿态值和所述姿态预测值进行卡尔曼二维融合处理的方法还包括：

建立状态方程：

其中，An1_k和An2_k分别表示第k时刻的两个角度，Ve1_k和Ve2_k分别表示第k时刻的两个角速度；

得，状态转移矩阵为：

其中，Δt表示时间间隔，即采样时间。

本实施例提供的基于卡尔曼滤波的姿态调整方法，其原理过程大致如下：

首先利用传感器获得一维姿态数据，并建立姿态运动学模型为姿态预测值的输出做好准备；然后利用卡尔曼一维滤波对一维姿态数据进行滤波以得到一维姿态值；进而利用卡尔曼二维融合将一维姿态值和姿态预测值融合在一起，从而得到最佳估计值；最后将得到的最佳估计值导入至下一周期中，进行下一轮滤波。也即，利用卡尔曼一维滤波和卡尔曼二维融合来预测机器人下一时刻的状态值，然后在平衡点线性化后得到更新后的卡尔曼系数，根据更新后的卡尔曼系数修正当前的状态值，最后输出最佳估计值，并将该最佳估计值反馈至下一周期，利用该最佳估计值进行下一时刻状态值的预测。

本实施例还提供一种基于卡尔曼滤波的姿态调整系统，用于实现如上所述的基于卡尔曼滤波的姿态调整方法，如图2所示，所述姿态调整系统包括：一维姿态值获取模块，用于获取一维姿态数据，并对所述一维姿态数据进行卡尔曼一维滤波得到一维姿态值；姿态预测值获取模块，包括姿态运动学模型，以利用所述姿态运动学模型得到姿态预测值；姿态数据处理模块，用于对所述一维姿态值和所述姿态预测值进行卡尔曼二维融合得到最佳估计值；姿态控制调整模块，用于根据所述最佳估计值进行姿态调整。

具体的，在本实施例中，所述一维姿态值获取模块包括：传感器单元，用于接收至少两组所述一维姿态数据；滤波单元，用于对所述一维姿态数据进行卡尔曼一维滤波，以得到一维姿态值。

以及，在本实施例中，所述姿态预测值获取模块包括：姿态运动学模型，用于通过对姿态运动学模型的仿真得到姿态预测值；输出端，用于输出姿态预测值。

以及，在本实施例中，所述姿态数据处理模块包括：融合单元，用于对所述一维姿态值和所述姿态预测值进行卡尔曼二维融合，以得到最佳估计值；解算单元，用于根据所述最佳估计值对姿态进行运动学逆解，以得到姿态调整参数；控制单元，用于根据所述最佳估计值和所述姿态调整参数进行姿态调整。

以及，在本实施例中，所述姿态控制调整模块包括：上位机，用于将姿态数据处理模块中控制单元下达的指令输送至机器人，以使机器人实现姿态的调整。

本实施例提供的基于卡尔曼滤波的姿态调整系统，通过一维姿态值获取模块获取一维姿态数据，并对所述一维姿态数据进行卡尔曼一维滤波得到一维姿态值；通过姿态预测值获取模块利用姿态运动学模型得到姿态预测值；然后通过姿态数据处理模块对所述一维姿态值和所述姿态预测值进行卡尔曼二维融合得到最佳估计值；最后通过姿态控制调整模块根据所述最佳估计值进行姿态调整，使得在对姿态调整时，不仅能够有效地取出噪声和误差，还可以处理非线性的动态参数和测量值，能够提高数据的精度、稳定性和鲁棒性，以及能够有效提高姿态调整的控制和运动规划能力，解决了如何快速、精准地实现机器人的姿态调整的问题。

以下，以一具体实施例说明本申请提供的基于卡尔曼滤波的姿态调整方法和系统在机器人的姿态调整方面较现有技术所取得的优势。

本实施例中机器人采用四足机器人。

在本实施例中，采用两个传感器获得一维姿态数据，两个传感器分别为MPU6050和JY931。MPU6050为全球首例整合性六轴运动处理组件，相较于多组件方案，免除了组合陀螺仪与加速器时间轴之差的问题，减少了大量的封装空间。JY931为高速率九轴姿态角度传感器，其通过获取芯片三轴加速度、陀螺仪、磁场数据，通过卡尔曼滤波融合算法解算出三轴角度数据，角度精度非常高，且支持二次开发应用。

两个传感器提供的数据有很多，在本实施例中，一维姿态数据只提取两个传感器中的角度值，从而能够在保证获知姿态的准确度的同时，减少计算量，提高姿态调整系统的响应速度。

另外，在本实施例中，由于传感器安装于四足机器人的背部，因此以匀速直线运动位移公式建立姿态运动学模型。本实施例中采用的姿态运动学模型相较简单，因此计算量较少，实时性高。

在获取到一维姿态数据(角度值)后，按照上述方式进行卡尔曼标准一维滤波，从而得到一维姿态值，该一维姿态值是基于实际测量的值。具体的，在本实施例中，状态转移矩阵F_k＝1、控制矩阵B_k＝0、外部控制向量观测矩阵H_k＝1，从而最大限度的简化卡尔曼一维滤波，同时，过程噪声协方差矩阵Q_k和观测噪声协方差矩阵R_k的取值范围控制在0.001～0.5范围内，并根据实际实验结果对其进行调整，以获得最佳的输出值。卡尔曼标准一维滤波的具体方法为本领域技术人员所熟知的，此处不再赘述。

之后，将卡尔曼一维滤波输出的一维姿态值和姿态运动学模型输出的姿态预测值按照上述方式进行卡尔曼二维融合，从而得到最佳估计值，该最佳估计值用于对四足机器人进行姿态调整，并作为下一轮的一维姿态数据。

如图3所示，为本实施例提供的四足机器人静态数据处理实际效果图。从图中可以看出，当机器人处于静止不动的状态时，由于机器人内部机械构造原因，会产生小幅度抖动，从而造成噪声(如图中紫色曲线所示)，这些噪声会对姿态数据的计算产生影响，造成姿态数据计算偏差。因此，需要对此噪声进行滤波处理。在采用本实施例提供的方式进行滤波处理后，可以发现(如图中红色曲线所示)该方式有效消除了这些噪声，从而能够有效提高姿态数据计算处理的精度。

如图4所示，为本实施例提供的四足机器人数据融合实际效果图，其中蓝色曲线代表仅使用MPU6050得到的原始数据，紫色曲线代表仅使用JY931得到的原始数据，绿色曲线代表经过了卡尔曼一维滤波和卡尔曼二维融合后得到的数据。从图中可以看出，通过对传感器采集到的一维姿态数据进行卡尔曼一维滤波和卡尔曼二维融合，能够有效提高数据的精度和稳定性，此方式在姿态调整领域具有实用性和推广价值。

从本实施例中可以看出，相比于传统的滤波，本实施例提供的基于卡尔曼滤波的姿态调整方法及系统能够有效地对数据进行噪声和误差的去除，能够处理非线性系统的动态参数和测量值，并且该方法能够将多个传感器(如陀螺仪、加速度计和磁力计)的数据进行融合，提高数据(Roll、Yaw、Pitch)的可靠性和精度，并能够对不同传感器之间的误差和漂移进行补偿，具有较高的精度、稳定性和鲁棒性，能够有效地应用于四足机器人的姿态数据处理中，提高机器人的姿态控制和运动规划能力，并为机器人在不同环境和任务下的应用提供有力的支持。

需要说明的是，本说明书中各个实施例采用递进的方式描述，每个实施例重点说明的都是与其他实施例的不同之处，各个实施例之间相同相似部分互相参见即可，此外，各个实施例之间不同的部分也可互相组合使用，本发明对此不作限定。

本实施例提供的基于卡尔曼滤波的姿态调整方法及系统，包括：获取一维姿态数据；对所述一维姿态数据进行卡尔曼一维滤波，以得到一维姿态值；建立姿态运动学模型，以得到姿态预测值；对所述一维姿态值和所述姿态预测值进行卡尔曼二维融合，以得到最佳估计值；利用所述最佳估计值进行姿态调整。通过卡尔曼一维滤波，使得在对姿态调整时，不仅能够有效地取出噪声和误差，还可以处理非线性的动态参数和测量值；通过卡尔曼二维融合，使得能够提高数据的精度、稳定性和鲁棒性；顺次进行卡尔曼一维滤波和卡尔曼二维融合，能够有效提高姿态调整的控制和运动规划能力，解决了如何快速、精准地实现机器人的姿态调整的问题。

上述描述仅是对本发明较佳实施例的描述，并非对本发明范围的任何限定，本发明领域的普通技术人员根据上述揭示内容做的任何变更、修饰，均属于权利要求书的保护范围。

Claims (10)

1.一种基于卡尔曼滤波的姿态调整方法，其特征在于，所述基于卡尔曼滤波的姿态调整方法包括：

获取一维姿态数据；

对所述一维姿态数据进行卡尔曼一维滤波，以得到一维姿态值；

建立姿态运动学模型，以得到姿态预测值；

对所述一维姿态值和所述姿态预测值进行卡尔曼二维融合，以得到最佳估计值；

利用所述最佳估计值进行姿态调整。

2.根据权利要求1所述的基于卡尔曼滤波的姿态调整方法，其特征在于，获取至少两组所述一维姿态数据，不同组的所述一维姿态数据来源于不同的姿态传感器。

3.根据权利要求1所述的基于卡尔曼滤波的姿态调整方法，其特征在于，所述对所述一维姿态数据进行卡尔曼一维滤波，以得到一维姿态值的方法包括：

获取卡尔曼一维滤波参数，所述卡尔曼一维滤波参数包括状态转移矩阵、控制矩阵、外部控制向量、状态估计误差协方差矩阵、观测矩阵、观测信息、过程噪声协方差矩阵和观测噪声协方差矩阵；

利用所述卡尔曼一维滤波参数对所述一维姿态数据进行标准卡尔曼滤波处理，其中：

预测方程为：

更新方程为：

P_k ^′＝P_k-K^′H_kP

其中，表示第k时刻的系统状态向量，F_k表示从第k-1时刻到第k时刻的状态转移矩阵，B_k表示第k时刻的控制矩阵，/>表示第k时刻的外部控制向量，P_k表示第k时刻的状态估计误差协方差矩阵，Q_k表示第k时刻的一维过程噪声协方差矩阵；K′表示新的卡尔曼系数，H_k表示第k时刻的观测矩阵，R_k表示第k时刻的一维观测噪声协方差矩阵，/>表示第k时刻的观测信息，P′_k表示新的状态估计误差协方差矩阵。

4.根据权利要求3所述的基于卡尔曼滤波的姿态调整方法，其特征在于，

状态转移矩阵F_k＝1；

控制矩阵B_k＝0；

外部控制向量或a，其中，a表示电机加速度，且a由电机参数计算得到；

观测矩阵H_k＝1；

过程噪声协方差矩阵Q_k和观测噪声协方差矩阵R_k的取值范围为0.001～0.5。

5.根据权利要求4所述的基于卡尔曼滤波的姿态调整方法，其特征在于，所述电机加速度a的计算方法包括：

获取电机参数，所述电机参数包括初始角度和初始角速度；

利用所述电机参数计算电机加速度，其中计算方程为：

X＝X₀+V×Δt+0.5a(Δt)²

V＝V₀+a×Δt

得，电机加速度为：

其中，X表示角度，X₀表示初始角度，V表示角速度，V₀表示初始角速度，Δt表示时间间隔。

6.根据权利要求1所述的基于卡尔曼滤波的姿态调整方法，其特征在于，所述建立姿态运动学模型，以得到姿态预测值的方法包括：

利用所述一维姿态数据构建姿态运动学模型，所述姿态运动学模型为匀速直线运动模型；

利用所述姿态运动学模型进行仿真模拟，以得到姿态预测值。

7.根据权利要求3所述的基于卡尔曼滤波的姿态调整方法，其特征在于，所述对所述一维姿态值和所述姿态预测值进行卡尔曼二维融合，以得到最佳估计值的方法包括：

获取卡尔曼二维融合参数，所述卡尔曼二维融合参数包括平衡点方程、状态转移矩阵、外部控制向量、状态估计误差协方差矩阵、观测矩阵、观测信息、过程噪声协方差矩阵和观测噪声协方差矩阵；

利用所述卡尔曼二维融合参数对所述一维姿态值和所述姿态预测值进行卡尔曼二维融合处理，其中：

预测方程为：

更新方程为：

其中，f表示平衡点方程，W_k表示第k时刻的二维过程噪声协方差矩阵，V_k表示第k时刻的二维观测噪声协方差矩阵，I表示单位矩阵。

8.根据权利要求7所述的基于卡尔曼滤波的姿态调整方法，其特征在于，所述利用所述卡尔曼二维融合参数对所述一维姿态值和所述姿态预测值进行卡尔曼二维融合处理的方法还包括：

建立状态方程：

其中，An1_k和An2_k分别表示第k时刻的两个角度，Ve1_k和Ve2_k分别表示第k时刻的两个角速度；

得，状态转移矩阵为：

其中，Δt表示时间间隔。

9.一种基于卡尔曼滤波的姿态调整系统，用于实现如权利要求1～8任一项所述的基于卡尔曼滤波的姿态调整方法，其特征在于，所述姿态调整系统包括：

一维姿态值获取模块，用于获取一维姿态数据，并对所述一维姿态数据进行卡尔曼一维滤波得到一维姿态值；

姿态预测值获取模块，包括姿态运动学模型，以利用所述姿态运动学模型得到姿态预测值；

姿态数据处理模块，用于对所述一维姿态值和所述姿态预测值进行卡尔曼二维融合得到最佳估计值；

姿态控制调整模块，用于根据所述最佳估计值进行姿态调整。

10.根据权利要求9所述的基于卡尔曼滤波的姿态调整系统，其特征在于，所述一维姿态值获取模块包括：

传感器单元，用于接收至少两组所述一维姿态数据；

滤波单元，用于对所述一维姿态数据进行卡尔曼一维滤波，以得到一维姿态值；

所述姿态数据处理模块包括：

融合单元，用于对所述一维姿态值和所述姿态预测值进行卡尔曼二维融合，以得到最佳估计值；

解算单元，用于根据所述最佳估计值对姿态进行运动学逆解，以得到姿态调整参数；

控制单元，用于根据所述最佳估计值和所述姿态调整参数进行姿态调整。