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

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

Attitude control method and device based on Calman filter

The invention discloses a Calman filter for attitude control based method and device, the method comprises: according to the current measurement parameters of gyroscope and sampling interval prediction matrix; obtain a moment attitude value matrix corresponding to the best solution, according to the prediction matrix and a moment attitude value matrix corresponding to the best solution. Get the updated prediction matrix; gauge parameters measured according to current accelerometer measured parameters and magnetic force, get accelerometer and magnetometer observation matrix of observation matrix; matrix prediction and acceleration of the updated meter observation matrix first order Calman filtering calculation, get the current temporary attitude matrix corresponding to the optimal solution; matrix of the magnetometer observation matrix and the current optimal solution of the corresponding temporary attitude for the second order Calman filter. The matrix corresponding to the optimal solution of the current attitude is obtained, and the attitude of the device is controlled by the matrix corresponding to the optimal solution of the attitude value at the present moment.

【技术实现步骤摘要】
基于卡尔曼滤波的姿态控制方法及装置
本专利技术涉及设备空间姿态控制
，更具体地，涉及一种基于卡尔曼滤波的姿态控制方法及装置。
技术介绍
卡尔曼滤波是一种信号处理方法，利用线性系统状态方程，通过系统输入观测数据，对系统状态进行最优估计的算法。由于观测数据中包括系统中的噪声，所以最优估计也可以看看作是滤波过程。现有技术中，通过卡尔曼滤波算法对VR手柄、无人机、机器人等设置的传感器测量得到的数据进行计算，得到VR手柄、无人机、机器人等对应的空间姿态值。现有技术中，基于卡尔曼滤波的九轴姿态控制方法一般都采用一阶卡尔曼滤波器。在采用一阶卡尔曼滤波器计算过程中，利用三轴加速度计和三轴磁力计测量得到的数据构成6维观测变量，使得计算过程复杂，计算量大，计算时间较长。
技术实现思路
本专利技术的一个目的是提供一种基于卡尔曼滤波的姿态控制方法及装置的新技术方案。根据本专利技术的第一方面，提供了一种基于卡尔曼滤波的姿态控制方法，包括：采集当前时刻陀螺仪测量得到的参数，根据所述当前时刻陀螺仪测量得到的参数和采样时间间隔，得到预测矩阵；获取上一时刻姿态值最优解对应的矩阵，根据所述预测矩阵和所述上一时刻姿态值最优解对应的矩阵，得到更新后的预测矩阵；采集当前时刻加速度计测量得到的参数和磁力计测量得到的参数，根据所述当前时刻加速度计测量得到的参数和磁力计测量得到的参数，得到加速度计观测矩阵和磁力计观测矩阵；对所述更新后的预测矩阵和所述加速度计观测矩阵进行第一阶卡尔曼滤波计算，得到当前时刻临时姿态最优解对应的矩阵；对所述磁力计观测矩阵和所述当前时刻临时姿态最优解对应的矩阵进行第二阶卡尔曼滤波计算，得到当前时刻姿态值最优解对应的矩阵。可选地，根据所述当前时刻加速度计测量得到的参数和磁力计测量得到的参数，得到加速度计观测矩阵和磁力计观测矩阵，包括：根据所述当前时刻加速度计测量得到的参数和磁力计测量得到的参数，得到当前时刻的俯仰角、翻滚角和航向角；根据所述俯仰角、所述翻滚角和所述航向角得到一组四元数；根据所述四元数得到所述加速度计观测矩阵和所述磁力计观测矩阵。可选地，对所述更新后的预测矩阵和所述加速度计观测矩阵进行第一阶卡尔曼滤波计算，得到当前时刻临时姿态最优解对应的矩阵，包括：获取上一时刻姿态值最优解的误差协方差，根据所述上一时刻姿态值最优解的误差协方差、所述预测矩阵和陀螺仪出厂时的误差系数，得到先验误差协方差；根据所述加速度计观测矩阵、加速度计的实际误差系数和所述先验误差协方差，得到第一阶卡尔曼增益参数；根据所述更新后的预测矩阵、所述加速度计观测矩阵、所述第一阶卡尔曼增益参数、上一时刻姿态值最优解对应的矩阵和第一阶迭代状态值，得到当前时刻临时姿态值最优解对应的矩阵。可选地，根据所述当前时刻加速度计测量得到的参数得到当前时刻的加速度值，根据所述当前时刻的加速度值、重力加速度值和所述加速度计出厂时的误差系数，得到加速度计的实际误差系数。可选地，根据所述当前时刻加速度计测量得到的参数和磁力计测量得到的参数、所述加速度计观测矩阵和所述加速度计出厂时的误差系数，得到第一阶迭代状态值。可选地，对所述磁力计观测矩阵和所述当前时刻临时姿态最优解对应的矩阵进行第二阶卡尔曼滤波计算，得到当前时刻姿态值最优解对应的矩阵，包括：根据所述加速度计观测矩阵、所述先验误差协方差和所述第一阶卡尔曼增益参数，得到第一阶误差协方差；根据所述磁力计观测矩阵、所述第一阶误差协方差和磁力计出厂时的误差系数，得到第二阶卡尔曼增益参数；根据所述磁力计观测矩阵、所述临时姿态值最优解对应的矩阵、所述第二阶卡尔曼增益参数、所述上一时刻姿态值最优解对应的矩阵和第二阶迭代状态值，得到当前时刻姿态值最优解。可选地，根据所述当前时刻加速度计测量得到的参数和磁力计测量得到的参数、所述加速度计观测矩阵和所述磁力计出厂时的误差系数，得到第二阶迭代状态值。可选地，所述方法还包括：根据所述磁力计观测矩阵、所述第一阶误差协方差和所述第二阶卡尔曼增益参数，得到第二阶误差协方差，以作为当前时刻姿态值最优解的误差协方差。可选地，获取上一时刻姿态值最优解对应的矩阵，包括：如果上一时刻是初始时刻时，根据初始时刻加速度计测量得到的参数和磁力计测量得到的参数，得到初始时刻的俯仰角、翻滚角和航向角；根据初始时刻的俯仰角、所述翻滚角和所述航向角得到初始时刻对应的一组四元数；根据所述初始时刻对应的一组四元数，得到一组矩阵，并将所述矩阵作为初始时刻姿态值最优解对应的矩阵。根据本专利技术的第二方面，提供了一种基于卡尔曼滤波的姿态控制装置，包括存储器和处理器，其中，所述存储器存储可执行指令，所述可执行指令控制所述处理器进行操作以执行上述任何一项所述的基于卡尔曼滤波的姿态控制方法。本专利技术实施例提供的基于卡尔曼滤波的姿态控制方法及装置，进行了两阶卡尔曼滤波计算，对更新后的预测矩阵和加速度计观测矩阵进行第一阶卡尔曼滤波计算，得到当前时刻临时姿态最优解对应的矩阵，然后，对磁力计观测矩阵和当前时刻临时姿态最优解对应的矩阵进行第二阶卡尔曼滤波计算，得到当前时刻姿态最优解对应的矩阵。本专利技术提供的姿态控制方法，减小了计算量，缩短了计算时间，提高了姿态控制的效率。通过以下参照附图对本专利技术的示例性实施例的详细描述，本专利技术的其它特征及其优点将会变得清楚。附图说明被结合在说明书中并构成说明书的一部分的附图示出了本专利技术的实施例，并且连同其说明一起用于解释本专利技术的原理。图1示出了根据本专利技术一个实施例的基于卡尔曼滤波的姿态控制方法的处理流程图。图2示出了根据本专利技术一个实施例的基于卡尔曼滤波的姿态控制方法的另一种处理流程图。图3示出了根据本专利技术一个实施例的基于卡尔曼滤波的姿态控制装置的结构示意图。具体实施方式现在将参照附图来详细描述本专利技术的各种示例性实施例。应注意到：除非另外具体说明，否则在这些实施例中阐述的部件和步骤的相对布置、数字表达式和数值不限制本专利技术的范围。以下对至少一个示例性实施例的描述实际上仅仅是说明性的，决不作为对本专利技术及其应用或使用的任何限制。对于相关领域普通技术人员已知的技术、方法和设备可能不作详细讨论，但在适当情况下，所述技术、方法和设备应当被视为说明书的一部分。在这里示出和讨论的所有例子中，任何具体值应被解释为仅仅是示例性的，而不是作为限制。因此，示例性实施例的其它例子可以具有不同的值。应注意到：相似的标号和字母在下面的附图中表示类似项，因此，一旦某一项在一个附图中被定义，则在随后的附图中不需要对其进行进一步讨论。本专利技术的一个实施例提供了一种基于卡尔曼滤波的姿态控制方法。该方法可用于虚拟现实头戴设备、游戏手柄、无人机、机器人的空间姿态解算和控制。图1示出了根据本专利技术一个实施例的基于卡尔曼滤波的姿态控制方法的处理流程图。参见图1，该方法至少包括步骤S102至步骤S112。步骤S102，采集当前时刻陀螺仪测量得到的参数，根据当前时刻陀螺仪测量得到的参数和采样时间间隔，得到预测矩阵；步骤S104，获取上一时刻姿态值最优解对应的矩阵，根据预测矩阵和上一时刻姿态值最优解对应的矩阵，得到更新后的预测矩阵；步骤S106，采集当前时刻加速度计测量得到的参数和磁力计测量得到的参数，根据当前时刻加速度计测量得到的参数和磁力计测量得到的参数，得到加速度计观测矩阵和本文档来自技高网...

【技术保护点】
一种基于卡尔曼滤波的姿态控制方法，其特征在于，包括：采集当前时刻陀螺仪测量得到的参数，根据所述当前时刻陀螺仪测量得到的参数和采样时间间隔，得到预测矩阵；获取上一时刻姿态值最优解对应的矩阵，根据所述预测矩阵和所述上一时刻姿态值最优解对应的矩阵，得到更新后的预测矩阵；采集当前时刻加速度计测量得到的参数和磁力计测量得到的参数，根据所述当前时刻加速度计测量得到的参数和磁力计测量得到的参数，得到加速度计观测矩阵和磁力计观测矩阵；对所述更新后的预测矩阵和所述加速度计观测矩阵进行第一阶卡尔曼滤波计算，得到当前时刻临时姿态最优解对应的矩阵；对所述磁力计观测矩阵和所述当前时刻临时姿态最优解对应的矩阵进行第二阶卡尔曼滤波计算，得到当前时刻姿态值最优解对应的矩阵；利用所述当前时刻姿态值最优解对应的矩阵控制设备的姿态。

【技术特征摘要】
1.一种基于卡尔曼滤波的姿态控制方法，其特征在于，包括：采集当前时刻陀螺仪测量得到的参数，根据所述当前时刻陀螺仪测量得到的参数和采样时间间隔，得到预测矩阵；获取上一时刻姿态值最优解对应的矩阵，根据所述预测矩阵和所述上一时刻姿态值最优解对应的矩阵，得到更新后的预测矩阵；采集当前时刻加速度计测量得到的参数和磁力计测量得到的参数，根据所述当前时刻加速度计测量得到的参数和磁力计测量得到的参数，得到加速度计观测矩阵和磁力计观测矩阵；对所述更新后的预测矩阵和所述加速度计观测矩阵进行第一阶卡尔曼滤波计算，得到当前时刻临时姿态最优解对应的矩阵；对所述磁力计观测矩阵和所述当前时刻临时姿态最优解对应的矩阵进行第二阶卡尔曼滤波计算，得到当前时刻姿态值最优解对应的矩阵；利用所述当前时刻姿态值最优解对应的矩阵控制设备的姿态。2.根据权利要求1所述的方法，其特征在于，根据所述当前时刻加速度计测量得到的参数和磁力计测量得到的参数，得到加速度计观测矩阵和磁力计观测矩阵，包括：根据所述当前时刻加速度计测量得到的参数和磁力计测量得到的参数，得到当前时刻的俯仰角、翻滚角和航向角；根据所述俯仰角、所述翻滚角和所述航向角得到一组四元数；根据所述四元数得到所述加速度计观测矩阵和所述磁力计观测矩阵。3.根据权利要求1所述的方法，其特征在于，对所述更新后的预测矩阵和所述加速度计观测矩阵进行第一阶卡尔曼滤波计算，得到当前时刻临时姿态最优解对应的矩阵，包括：获取上一时刻姿态值最优解的误差协方差，根据所述上一时刻姿态值最优解的误差协方差、所述预测矩阵和陀螺仪出厂时的误差系数，得到先验误差协方差；根据所述加速度计观测矩阵、加速度计的实际误差系数和所述先验误差协方差，得到第一阶卡尔曼增益参数；根据所述更新后的预测矩阵、所述加速度计观测矩阵、所述第一阶卡尔曼增益参数、上一时刻姿态值最优解对应的矩阵和第一阶迭代状态值，得到当前时刻临时姿态值最优解对应的矩阵。4.根据权利要求3所述的方法，其特征在于，根据所述当前时刻加速度计测量得到的参数得到当前时刻的加速度值，根据所述当前时...

【专利技术属性】
技术研发人员：陈维亮，董碧峰，
申请(专利权)人：歌尔科技有限公司，
类型：发明
国别省市：山东,37