[发明专利]基于卡尔曼滤波的姿态控制方法及装置

说明书

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

技术领域

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

背景技术

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

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

发明内容

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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