[发明专利]基于自适应的四元数的姿态估计方法及装置

说明书

本发明公开了一种基于四元数的姿态估计方法，包括如下步骤：根据四元数进行卡尔曼滤波器构建；获取磁强计和加速度计的观测量；判断加速度计的观测量是否超限，如果加速度计和磁强计的观测量未超限，则对卡尔曼滤波器进行的观测值更新以得更新后的四元数；将更新后的四元数进行归一化以及协方差阵对称化处理后作为最终的姿态信息。本发明还公开了一种电子设备和计算机可读存储介质。本发明的基于四元数的姿态估计方法，可以进行全姿态估计，不必对四元数进行线性化，且对载体初始姿态不敏感；且本发明不单融合了多种传感器的观测结果，而且考虑了陀螺工况时存在的零偏，并对零偏的噪声进行建模，提高了姿态估计精度、准度和鲁棒性。

技术领域

本发明涉及姿态检测技术领域，尤其涉及一种基于自适应的四元数的姿态估计方法及装置。

背景技术

目前，多种传感器已经大规模综合应用于具体的载体，对于单一传感器，融合多种创拿起进行载体姿态估计，对提高姿态估计结果的精度、准度和鲁棒性均有重要意义。

1、融合多种传感器进行姿态估计需求背景

得益于微机电系统技术的发展(Micro-Electro-Mechanical System,MEMS)，越来越多的传感器变得微型化，且成本低廉，容易获得，如智能手机就集成了陀螺仪、加速度计、磁强计、红外传感器、摄像头等。又随着嵌入式平台算力不断提升，使得在嵌入平台的板载CPU上融合这些传感器的观测数据成为可能。

2、全姿态估计需求背景

在全姿态估计的场景中，如可无人机，穿戴设备，手持设备等的工况，如果载体的姿态以欧拉角的方式估计，载体的俯仰角是有可能出现90°的情况的，此时欧拉角的有效参数将会从三个退化为两个，无法确定载体此时的姿态。

3、复杂多变环境下的定向问题

在无遮挡环境下，定向问题可以由动态的GNSS接收机，或两台静态的 GNSS组成基线解决。在部分遮挡或全遮挡的环境下实现定向，较多的方法是使用地磁定向。但是地磁容易受到干扰。同时，载体的线性加速度也会干扰加速度计观测载体的姿态。因此，设计一种能够更为准确测量姿态的方法成为本领域技术人员亟待解决的技术问题。

发明内容

为了克服现有技术的不足，本发明的目的之一在于提供一种基于四元数的姿态估计方法，其能提升姿态估计的精准性。

本发明的目的之二在于提供一种电子设备，其能提升姿态估计的精准性。

本发明的目的之三在于提供一种计算机可读存储介质，其能提升姿态估计的精准性。

本发明的目的之一采用如下技术方案实现：

一种基于四元数的姿态估计方法，包括如下步骤：

模型构建步骤：根据四元数进行卡尔曼滤波器构建，确定载体坐标系和导航坐标系，根据已确定的坐标系确定姿态矩阵；所述卡尔曼滤波器中采用陀螺仪的角度随机游走系数ARW对四元数的过程噪声进行建模，且对陀螺仪的零偏噪声进行建模；

数据获取步骤：获取磁强计和加速度计的观测量；

更新步骤：判断加速度计的观测量是否超限，如果加速度计的观测量未超限，则对卡尔曼滤波器进行加速度计的观测值更新；如果磁强计的观测量未超限，则对卡尔曼滤波器进行磁强计的观测值更新以得更新后的四元数；

结果输出步骤：将更新后的四元数进行归一化以及协方差阵对称化处理后作为最终的姿态信息，并根据最终姿态信息输出姿态估计的观测值。

进一步地，所述卡尔曼滤波器通过如下步骤构建得到：

步骤1：对于任意四 维向量q，有运算符Ξ(q)：q＝[q₀ q₁ q₂ q₃]^T