CN107272718A

CN107272718A - 基于卡尔曼滤波的姿态控制方法及装置

Info

Publication number: CN107272718A
Application number: CN201710464239.8A
Authority: CN
Inventors: 陈维亮; 董碧峰
Original assignee: Goertek Techology Co Ltd
Current assignee: Goertek Techology Co Ltd
Priority date: 2017-06-19
Filing date: 2017-06-19
Publication date: 2017-10-20
Anticipated expiration: 2037-06-19
Also published as: CN107272718B

Abstract

本发明公开了一种基于卡尔曼滤波的姿态控制方法及装置，该方法包括：根据当前时刻陀螺仪测量得到的参数和采样时间间隔，得到预测矩阵；获取上一时刻姿态值最优解对应的矩阵，根据预测矩阵和上一时刻姿态值最优解对应的矩阵，得到更新后的预测矩阵；根据当前时刻加速度计测量得到的参数和磁力计测量得到的参数，得到加速度计观测矩阵和磁力计观测矩阵；对更新后的预测矩阵和加速度计观测矩阵进行第一阶卡尔曼滤波计算，得到当前时刻临时姿态最优解对应的矩阵；对磁力计观测矩阵和当前时刻临时姿态最优解对应的矩阵进行第二阶卡尔曼滤波计算，得到当前时刻姿态值最优解对应的矩阵；利用当前时刻姿态值最优解对应的矩阵控制设备的姿态。

Description

基于卡尔曼滤波的姿态控制方法及装置

技术领域

本发明涉及设备空间姿态控制技术领域，更具体地，涉及一种基于卡尔曼滤波的姿态控制方法及装置。

背景技术

卡尔曼滤波是一种信号处理方法，利用线性系统状态方程，通过系统输入观测数据，对系统状态进行最优估计的算法。由于观测数据中包括系统中的噪声，所以最优估计也可以看看作是滤波过程。现有技术中，通过卡尔曼滤波算法对VR手柄、无人机、机器人等设置的传感器测量得到的数据进行计算，得到VR手柄、无人机、机器人等对应的空间姿态值。

现有技术中，基于卡尔曼滤波的九轴姿态控制方法一般都采用一阶卡尔曼滤波器。在采用一阶卡尔曼滤波器计算过程中，利用三轴加速度计和三轴磁力计测量得到的数据构成6维观测变量，使得计算过程复杂，计算量大，计算时间较长。

发明内容

本发明的一个目的是提供一种基于卡尔曼滤波的姿态控制方法及装置的新技术方案。

根据本发明的第一方面，提供了一种基于卡尔曼滤波的姿态控制方法，包括：

采集当前时刻陀螺仪测量得到的参数，根据所述当前时刻陀螺仪测量得到的参数和采样时间间隔，得到预测矩阵；

获取上一时刻姿态值最优解对应的矩阵，根据所述预测矩阵和所述上一时刻姿态值最优解对应的矩阵，得到更新后的预测矩阵；

采集当前时刻加速度计测量得到的参数和磁力计测量得到的参数，根据所述当前时刻加速度计测量得到的参数和磁力计测量得到的参数，得到加速度计观测矩阵和磁力计观测矩阵；

对所述更新后的预测矩阵和所述加速度计观测矩阵进行第一阶卡尔曼滤波计算，得到当前时刻临时姿态最优解对应的矩阵；

对所述磁力计观测矩阵和所述当前时刻临时姿态最优解对应的矩阵进行第二阶卡尔曼滤波计算，得到当前时刻姿态值最优解对应的矩阵。

可选地，根据所述当前时刻加速度计测量得到的参数和磁力计测量得到的参数，得到加速度计观测矩阵和磁力计观测矩阵，包括：

根据所述当前时刻加速度计测量得到的参数和磁力计测量得到的参数，得到当前时刻的俯仰角、翻滚角和航向角；

根据所述俯仰角、所述翻滚角和所述航向角得到一组四元数；

根据所述四元数得到所述加速度计观测矩阵和所述磁力计观测矩阵。

可选地，对所述更新后的预测矩阵和所述加速度计观测矩阵进行第一阶卡尔曼滤波计算，得到当前时刻临时姿态最优解对应的矩阵，包括：

获取上一时刻姿态值最优解的误差协方差，根据所述上一时刻姿态值最优解的误差协方差、所述预测矩阵和陀螺仪出厂时的误差系数，得到先验误差协方差；

根据所述加速度计观测矩阵、加速度计的实际误差系数和所述先验误差协方差，得到第一阶卡尔曼增益参数；

根据所述更新后的预测矩阵、所述加速度计观测矩阵、所述第一阶卡尔曼增益参数、上一时刻姿态值最优解对应的矩阵和第一阶迭代状态值，得到当前时刻临时姿态值最优解对应的矩阵。

可选地，根据所述当前时刻加速度计测量得到的参数得到当前时刻的加速度值，根据所述当前时刻的加速度值、重力加速度值和所述加速度计出厂时的误差系数，得到加速度计的实际误差系数。

可选地，根据所述当前时刻加速度计测量得到的参数和磁力计测量得到的参数、所述加速度计观测矩阵和所述加速度计出厂时的误差系数，得到第一阶迭代状态值。

可选地，对所述磁力计观测矩阵和所述当前时刻临时姿态最优解对应的矩阵进行第二阶卡尔曼滤波计算，得到当前时刻姿态值最优解对应的矩阵，包括：

根据所述加速度计观测矩阵、所述先验误差协方差和所述第一阶卡尔曼增益参数，得到第一阶误差协方差；

根据所述磁力计观测矩阵、所述第一阶误差协方差和磁力计出厂时的误差系数，得到第二阶卡尔曼增益参数；

根据所述磁力计观测矩阵、所述临时姿态值最优解对应的矩阵、所述第二阶卡尔曼增益参数、所述上一时刻姿态值最优解对应的矩阵和第二阶迭代状态值，得到当前时刻姿态值最优解。

可选地，根据所述当前时刻加速度计测量得到的参数和磁力计测量得到的参数、所述加速度计观测矩阵和所述磁力计出厂时的误差系数，得到第二阶迭代状态值。

可选地，所述方法还包括：

根据所述磁力计观测矩阵、所述第一阶误差协方差和所述第二阶卡尔曼增益参数，得到第二阶误差协方差，以作为当前时刻姿态值最优解的误差协方差。

可选地，获取上一时刻姿态值最优解对应的矩阵，包括：

如果上一时刻是初始时刻时，根据初始时刻加速度计测量得到的参数和磁力计测量得到的参数，得到初始时刻的俯仰角、翻滚角和航向角；

根据初始时刻的俯仰角、所述翻滚角和所述航向角得到初始时刻对应的一组四元数；

根据所述初始时刻对应的一组四元数，得到一组矩阵，并将所述矩阵作为初始时刻姿态值最优解对应的矩阵。

根据本发明的第二方面，提供了一种基于卡尔曼滤波的姿态控制装置，包括存储器和处理器，其中，所述存储器存储可执行指令，所述可执行指令控制所述处理器进行操作以执行上述任何一项所述的基于卡尔曼滤波的姿态控制方法。

本发明实施例提供的基于卡尔曼滤波的姿态控制方法及装置，进行了两阶卡尔曼滤波计算，对更新后的预测矩阵和加速度计观测矩阵进行第一阶卡尔曼滤波计算，得到当前时刻临时姿态最优解对应的矩阵，然后，对磁力计观测矩阵和当前时刻临时姿态最优解对应的矩阵进行第二阶卡尔曼滤波计算，得到当前时刻姿态最优解对应的矩阵。本发明提供的姿态控制方法，减小了计算量，缩短了计算时间，提高了姿态控制的效率。

通过以下参照附图对本发明的示例性实施例的详细描述，本发明的其它特征及其优点将会变得清楚。

附图说明

被结合在说明书中并构成说明书的一部分的附图示出了本发明的实施例，并且连同其说明一起用于解释本发明的原理。

图1示出了根据本发明一个实施例的基于卡尔曼滤波的姿态控制方法的处理流程图。

图2示出了根据本发明一个实施例的基于卡尔曼滤波的姿态控制方法的另一种处理流程图。

图3示出了根据本发明一个实施例的基于卡尔曼滤波的姿态控制装置的结构示意图。

具体实施方式

现在将参照附图来详细描述本发明的各种示例性实施例。应注意到：除非另外具体说明，否则在这些实施例中阐述的部件和步骤的相对布置、数字表达式和数值不限制本发明的范围。

以下对至少一个示例性实施例的描述实际上仅仅是说明性的，决不作为对本发明及其应用或使用的任何限制。

对于相关领域普通技术人员已知的技术、方法和设备可能不作详细讨论，但在适当情况下，所述技术、方法和设备应当被视为说明书的一部分。

在这里示出和讨论的所有例子中，任何具体值应被解释为仅仅是示例性的，而不是作为限制。因此，示例性实施例的其它例子可以具有不同的值。

应注意到：相似的标号和字母在下面的附图中表示类似项，因此，一旦某一项在一个附图中被定义，则在随后的附图中不需要对其进行进一步讨论。

本发明的一个实施例提供了一种基于卡尔曼滤波的姿态控制方法。该方法可用于虚拟现实头戴设备、游戏手柄、无人机、机器人的空间姿态解算和控制。

图1示出了根据本发明一个实施例的基于卡尔曼滤波的姿态控制方法的处理流程图。参见图1，该方法至少包括步骤S102至步骤S112。

步骤S102，采集当前时刻陀螺仪测量得到的参数，根据当前时刻陀螺仪测量得到的参数和采样时间间隔，得到预测矩阵；

步骤S104，获取上一时刻姿态值最优解对应的矩阵，根据预测矩阵和上一时刻姿态值最优解对应的矩阵，得到更新后的预测矩阵；

步骤S106，采集当前时刻加速度计测量得到的参数和磁力计测量得到的参数，根据当前时刻加速度计测量得到的参数和磁力计测量得到的参数，得到加速度计观测矩阵和磁力计观测矩阵；

步骤S108，对更新后的预测矩阵和加速度计观测矩阵进行第一阶卡尔曼滤波计算，得到当前时刻临时姿态最优解对应的矩阵；

步骤S110，对磁力计观测矩阵和所述当前时刻临时姿态最优解对应的矩阵进行第二阶卡尔曼滤波计算，得到当前时刻姿态值最优解对应的矩阵；

步骤S112，利用当前时刻姿态值最优解对应的矩阵控制设备的姿态。

本发明实施例提供的基于卡尔曼滤波的姿态控制方法，进行了两阶卡尔曼滤波计算，对更新后的预测矩阵和加速度计观测矩阵进行第一阶卡尔曼滤波计算，得到当前时刻临时姿态最优解对应的矩阵，然后，对磁力计观测矩阵和当前时刻临时姿态最优解对应的矩阵进行第二阶卡尔曼滤波计算，得到当前时刻姿态最优解对应的矩阵。本发明提供的姿态控制方法，减小了计算量，缩短了计算时间，提高了姿态控制的效率。

在本发明的一个实施例中，上述步骤S104中获取上一时刻姿态值最优解对应的矩阵，下面分为两种情况进行说明。

一种情况是当上一时刻是初始时刻时，采集初始时刻加速度计测量得到的参数和磁力计测量得到的参数，根据初始时刻加速度计测量得到的参数和磁力计测量得到的参数，得到初始时刻的俯仰角、翻滚角和航向角，然后，根据初始时刻的俯仰角、翻滚角和航向角得到初始时刻对应的一组四元数，最后，根据初始时刻对应的一组四元数，得到一组矩阵，并将该矩阵作为初始时刻姿态值最优解对应的矩阵。

具体地，初始时刻加速度计测量得到的在三轴上的加速度值分别为G_x、G_y、G_z，磁力计测量得到的在三轴上的磁场强度值分别为M_x、M_y、M_z。本发明实施例中，俯仰角、翻滚角和航向角分别记作为α、β、γ。基于以下计算式，计算得到俯仰角α，

其中，g为重力加速度，

基于以下计算式，计算得到翻滚角β，

基于以下计算式，计算得到航向角γ，

根据俯仰角α、翻滚角β和航向角γ计算得到初始时刻对应的一组四元数q₁、q₂、q₃和q₄，计算过程如下，

根据计算得到的四元数q₁、q₂、q₃和q₄，得到一个4*1的矩阵，并将该矩阵作为初始时刻姿态值最优解对应的矩阵，该矩阵具体如下：

另一种情况是直接获取上一时刻姿态值最优解对应的矩阵。上一时刻姿态值最优解对应的矩阵是根据本发明实施例提供的姿态控制方法计算得到的。

在本发明的一个实施例中，上述步骤S102涉及的预测矩阵的计算过程如下：

其中，ω_x、ω_y、ω_z为当前时刻陀螺仪在三轴上测量得到的参数，△t为采样时间间隔。

在本发明的一个实施例中，上述步骤S104涉及的更新后的预测矩阵的计算过程如下：

q_预测(k+1)＝A_k+1q_k—计算式(9)，其中，q_k为上一时刻姿态值最优解对应的矩阵。

在本发明的一个实施例中，上述步骤S106中采集当前时刻加速度计测量得到的在三轴上的加速度值G_x、G_y、G_z，以及磁力计测量得到的在三轴上的磁场强度值M_x、M_y、M_z。根据当前时刻加速度计和磁力计测量得到的参数得到一组四元数，具体计算过程参见上述计算式(1)-(7)，并根据计算得到的一组四元数得到一个4*1的矩阵，该矩阵具体如下：

然后，根据该矩阵计算得到加速度计观测矩阵H_1(K+1)和磁力计观测矩阵H_2(K+1)。加速度计观测矩阵是利用重力加速度在地理坐标系和载体坐标系分别投影来建立观测矩阵的，具体计算过程如下：

其中，是的转置矩阵，

磁力计观测矩阵是利用磁场方向在地理坐标系和载体坐标系中的投影来建立的观测矩阵，具体计算过程如下：

在本发明的一个实施例中，上述步骤S108具体为获取上一时刻姿态值最优解的误差协方差，根据上一时刻姿态值最优解的误差协方差、预测矩阵和陀螺仪出厂误差系数，计算得到先验误差协方差P_先验(k+1)，计算过程如下：

P_先验(k+1)＝A_k+1P_2(k)A_k+1 ^T+I₁Q—计算式(15)，其中，P_2(k)为上一时刻姿态值最优解的误差协方差，Q为陀螺仪出厂误差系数。

然后，根据加速度计观测矩阵、加速度计的实际误差系数和先验误差协方差，计算得到第一阶卡尔曼增益参数K_1(k+1)，具体计算过程如下：

K_1(k+1)＝P_先验(k+1)H_1(k+1) ^T(H_1(k+1)P_先验(k+1)H_1(k+1) ^T+I₂R_1(k+1))^-1—计算式(16)，其中，R_1(k+1)为加速度计的实际误差系数。

接着，根据更新后的预测矩阵、加速度计观测矩阵、第一阶卡尔曼增益参数、上一时刻姿态值最优解对应的矩阵和第一阶迭代状态值，计算得到当前时刻临时姿态值最优解对应的矩阵，具体计算过程如下：

q_临时(k+1)＝q_预测(k+1)+K_1(k+1)(a_(k+1)-H_1(k+1)q_最优(k))—计算式(17)，其中，a_(k+1)为第一阶迭代状态值，q_最优(k)为上一时刻姿态值最优解对应的矩阵。

在本发明的一个实施例中，根据当前时刻加速度计测量得到的参数计算得到当前时刻的加速度值，然后根据当前时刻的加速度值、重力加速度值和加速度计出厂时的误差系数，计算得到加速度计的实际误差系数，具体计算过程如下：

R_1(k+1)＝(a-g)R/a+R—计算式(18)，其中，a为当前时刻的加速度值，g为重力加速度，R为加速度计出厂时的误差系数。现有技术中，对于加速度计、磁力计和陀螺仪三个传感器，加速度计在运动过程中混合其他加速度后输出值将不准确，陀螺仪存在积累误差，磁力计相对效果较好，可信度较高的特点。本发明实施例中，提出了自适应的加速度计误差协方差，即上述计算式计算得出的加速度计的实际误差系数，同时根据三个传感器可信度和误差相对大小关系，降低了磁力计的误差，提高了陀螺仪的误差，最终使姿态值最优解得到较平稳的效果。

在本发明的一个实施例中，根据当前时刻加速度计测量得到的参数和磁力计测量得到的参数、加速度计观测矩阵和加速度计出厂时的误差系数，计算得到第一阶迭代状态值，具体计算过程如下：

其中，q为上述根据当前时刻加速度计测量得到的在三轴上的加速度值G_x、G_y、G_z以及磁力计测量得到的在三轴上的磁场强度值M_x、M_y、M_z计算得到的矩阵，具体计算过程参见上述实施例。

在本发明的一个实施例中，上述步骤S110具体为：首先，根据加速度计观测矩阵、先验误差协方差和第一阶卡尔曼增益参数，计算得出第一阶误差协方差P_1(k+1)，具体计算过程如下，

P_1(k+1)＝(I₁-K_1(k+1)H_1(k+1))P_先验(k+1)—计算式(20)，其中，然后，根据磁力计观测矩阵、第一阶误差协方差和磁力计出厂时的误差系数，计算得出第二阶卡尔曼增益参数K_2(k+1)，具体计算过程如下：

K_2(k+1)＝P_1(k+1)H_2(k+1) ^T(H_2(k+1)P_1(k+1)H_2(k+1) ^T+I₂R₂)^-1—计算式(21)，其中，R₂为磁力计出厂时的误差系数，

最后，根据磁力计观测矩阵、临时姿态值最优解对应的矩阵、第二阶卡尔曼增益参数、上一时刻姿态值最优解对应的矩阵和第二阶迭代值，计算得出当前时刻姿态值最优解，具体计算过程如下：

q_最优(k+1)＝q_临时(k+1)+K_2(k+1)(m_(k+1)-H_2(k+1)q_最优(k))—计算式(22)，其中，m_(k+1)为第二阶迭代状态值，q_最优(k)为上一时刻姿态值最优解对应的矩阵。

在本发明的一个实施例中，第二阶迭代状态值m_(k+1)可根据当前时刻加速度计测量得到的参数和磁力计测量得到的参数、加速度计观测矩阵和磁力计出厂时的误差系数计算得到，具体计算过程如下：

其中，q为上述根据当前时刻加速度计测量得到的在三轴上的加速度值G_x、G_y、G_z以及磁力计测量得到的在三轴上的磁场强度值M_x、M_y、M_z计算得到的矩阵，具体计算过程参见上述实施例，R₂为磁力计出厂时的误差系数。

在本发明的一个实施例中，在上述步骤S110之后，该方法还包括：根据磁力计观测矩阵、第一阶误差协方差和第二阶卡尔曼增益参数，计算得出第二阶误差协方差P_2(k+1)，具体计算过程如下：

P_2(k+1)＝(I₁-K_2(k+1)H_2(k+1))P_1(k+1)—计算式(24)，本发明实施例计算得出的第二阶误差协方差作为本次循环的姿态值最优解的误差协方差，可用于下一时刻姿态值最优解的计算。

表1示出了根据本发明实施例的加速度计、磁力计和陀螺仪出厂时的误差系数。

表1

	出厂时误差系数
		磁力计	2
加速度计	0.398
		陀螺仪	0.07

图2示出了根据本发明一个实施例的基于卡尔曼滤波的姿态控制方法的另一种处理流程图。参见图2，该方法至少包括步骤S201至步骤S207。步骤S202涉及的陀螺仪预测姿态可根据上述计算式(8)计算得到。步骤S203涉及的预测值可根据上述计算式(9)计算得到。步骤S204涉及的第一阶卡尔曼滤波和步骤S205得到的当前时刻临时姿态值最优解可根据上述计算式(10)-计算式(17)计算得到。步骤S206涉及的第二阶卡尔曼滤波和步骤S207得到的当前时刻姿态值最优解可根据上述计算式(18)-计算式(24)计算得到。

基于同一发明构思，本发明还提供了一种基于卡尔曼滤波的姿态控制装置。图3示出了根据本发明一个实施例的基于卡尔曼滤波的姿态控制装置的结构示意图。参见图3，该装置至少包括处理器310和存储器320。存储器320存储可执行指令，该可执行指令控制处理器310进行操作以执行上述任一实施例所述的基于卡尔曼滤波的姿态控制方法。

本发明可以是系统、方法和/或计算机程序产品。计算机程序产品可以包括计算机可读存储介质，其上载有用于使处理器实现本发明的各个方面的计算机可读程序指令。

计算机可读存储介质可以是可以保持和存储由指令执行设备使用的指令的有形设备。计算机可读存储介质例如可以是――但不限于――电存储设备、磁存储设备、光存储设备、电磁存储设备、半导体存储设备或者上述的任意合适的组合。计算机可读存储介质的更具体的例子(非穷举的列表)包括：便携式计算机盘、硬盘、随机存取存储器(RAM)、只读存储器(ROM)、可擦式可编程只读存储器(EPROM或闪存)、静态随机存取存储器(SRAM)、便携式压缩盘只读存储器(CD-ROM)、数字多功能盘(DVD)、记忆棒、软盘、机械编码设备、例如其上存储有指令的打孔卡或凹槽内凸起结构、以及上述的任意合适的组合。这里所使用的计算机可读存储介质不被解释为瞬时信号本身，诸如无线电波或者其他自由传播的电磁波、通过波导或其他传输媒介传播的电磁波(例如，通过光纤电缆的光脉冲)、或者通过电线传输的电信号。

这里所描述的计算机可读程序指令可以从计算机可读存储介质下载到各个计算/处理设备，或者通过网络、例如因特网、局域网、广域网和/或无线网下载到外部计算机或外部存储设备。网络可以包括铜传输电缆、光纤传输、无线传输、路由器、防火墙、交换机、网关计算机和/或边缘服务器。每个计算/处理设备中的网络适配卡或者网络接口从网络接收计算机可读程序指令，并转发该计算机可读程序指令，以供存储在各个计算/处理设备中的计算机可读存储介质中。

用于执行本发明操作的计算机程序指令可以是汇编指令、指令集架构(ISA)指令、机器指令、机器相关指令、微代码、固件指令、状态设置数据、或者以一种或多种编程语言的任意组合编写的源代码或目标代码，所述编程语言包括面向对象的编程语言—诸如Smalltalk、C++等，以及常规的过程式编程语言—诸如“C”语言或类似的编程语言。计算机可读程序指令可以完全地在用户计算机上执行、部分地在用户计算机上执行、作为一个独立的软件包执行、部分在用户计算机上部分在远程计算机上执行、或者完全在远程计算机或服务器上执行。在涉及远程计算机的情形中，远程计算机可以通过任意种类的网络—包括局域网(LAN)或广域网(WAN)—连接到用户计算机，或者，可以连接到外部计算机(例如利用因特网服务提供商来通过因特网连接)。在一些实施例中，通过利用计算机可读程序指令的状态信息来个性化定制电子电路，例如可编程逻辑电路、现场可编程门阵列(FPGA)或可编程逻辑阵列(PLA)，该电子电路可以执行计算机可读程序指令，从而实现本发明的各个方面。

这里参照根据本发明实施例的方法、装置(系统)和计算机程序产品的流程图和/或框图描述了本发明的各个方面。应当理解，流程图和/或框图的每个方框以及流程图和/或框图中各方框的组合，都可以由计算机可读程序指令实现。

这些计算机可读程序指令可以提供给通用计算机、专用计算机或其它可编程数据处理装置的处理器，从而生产出一种机器，使得这些指令在通过计算机或其它可编程数据处理装置的处理器执行时，产生了实现流程图和/或框图中的一个或多个方框中规定的功能/动作的装置。也可以把这些计算机可读程序指令存储在计算机可读存储介质中，这些指令使得计算机、可编程数据处理装置和/或其他设备以特定方式工作，从而，存储有指令的计算机可读介质则包括一个制造品，其包括实现流程图和/或框图中的一个或多个方框中规定的功能/动作的各个方面的指令。

也可以把计算机可读程序指令加载到计算机、其它可编程数据处理装置、或其它设备上，使得在计算机、其它可编程数据处理装置或其它设备上执行一系列操作步骤，以产生计算机实现的过程，从而使得在计算机、其它可编程数据处理装置、或其它设备上执行的指令实现流程图和/或框图中的一个或多个方框中规定的功能/动作。

附图中的流程图和框图显示了根据本发明的多个实施例的系统、方法和计算机程序产品的可能实现的体系架构、功能和操作。在这点上，流程图或框图中的每个方框可以代表一个模块、程序段或指令的一部分，所述模块、程序段或指令的一部分包含一个或多个用于实现规定的逻辑功能的可执行指令。在有些作为替换的实现中，方框中所标注的功能也可以以不同于附图中所标注的顺序发生。例如，两个连续的方框实际上可以基本并行地执行，它们有时也可以按相反的顺序执行，这依所涉及的功能而定。也要注意的是，框图和/或流程图中的每个方框、以及框图和/或流程图中的方框的组合，可以用执行规定的功能或动作的专用的基于硬件的系统来实现，或者可以用专用硬件与计算机指令的组合来实现。对于本领域技术人员来说公知的是，通过硬件方式实现、通过软件方式实现以及通过软件和硬件结合的方式实现都是等价的。

以上已经描述了本发明的各实施例，上述说明是示例性的，并非穷尽性的，并且也不限于所披露的各实施例。在不偏离所说明的各实施例的范围和精神的情况下，对于本技术领域的普通技术人员来说许多修改和变更都是显而易见的。本文中所用术语的选择，旨在最好地解释各实施例的原理、实际应用或对市场中的技术改进，或者使本技术领域的其它普通技术人员能理解本文披露的各实施例。本发明的范围由所附权利要求来限定。

Claims

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

对所述磁力计观测矩阵和所述当前时刻临时姿态最优解对应的矩阵进行第二阶卡尔曼滤波计算，得到当前时刻姿态值最优解对应的矩阵；

利用所述当前时刻姿态值最优解对应的矩阵控制设备的姿态。

2.根据权利要求1所述的方法，其特征在于，根据所述当前时刻加速度计测量得到的参数和磁力计测量得到的参数，得到加速度计观测矩阵和磁力计观测矩阵，包括：

3.根据权利要求1所述的方法，其特征在于，对所述更新后的预测矩阵和所述加速度计观测矩阵进行第一阶卡尔曼滤波计算，得到当前时刻临时姿态最优解对应的矩阵，包括：

4.根据权利要求3所述的方法，其特征在于，根据所述当前时刻加速度计测量得到的参数得到当前时刻的加速度值，根据所述当前时刻的加速度值、重力加速度值和所述加速度计出厂时的误差系数，得到加速度计的实际误差系数。

5.根据权利要求3或4所述的方法，其特征在于，根据所述当前时刻加速度计测量得到的参数和磁力计测量得到的参数、所述加速度计观测矩阵和所述加速度计出厂时的误差系数，得到第一阶迭代状态值。

6.根据权利要求5所述的方法，其特征在于，对所述磁力计观测矩阵和所述当前时刻临时姿态最优解对应的矩阵进行第二阶卡尔曼滤波计算，得到当前时刻姿态值最优解对应的矩阵，包括：

7.根据权利要求6所述的方法，其特征在于，根据所述当前时刻加速度计测量得到的参数和磁力计测量得到的参数、所述加速度计观测矩阵和所述磁力计出厂时的误差系数，得到第二阶迭代状态值。

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

9.根据权利要求1所述的方法，其特征在于，获取上一时刻姿态值最优解对应的矩阵，包括：

10.一种基于卡尔曼滤波的姿态控制装置，其特征在于，包括存储器和处理器，其中，所述存储器存储可执行指令，所述可执行指令控制所述处理器进行操作以执行根据权利要求1-9中的任何一项所述的基于卡尔曼滤波的姿态控制方法。