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

权利要求书

1.一种基于四元数的姿态估计方法，其特征在于，包括如下步骤：

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

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

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

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

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

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

步骤2：四元数卡尔曼滤波的状态为四元数q_k和陀螺零偏ε_k组合，也即是且四元数关于角增量的状态转移矩阵为：

步骤3：定义四元数卡尔曼滤波的状态协方差更新方程以及观测噪声矩阵分别为：

其中四元数卡尔曼滤波的过程噪声为：

且其中ARW为陀螺仪的角度随机游走系数，σ为陀螺仪的测量噪声标准差，τ为陀螺仪的测量噪声相关时间；R_k是与传感器直接相关的观测噪声矩阵，是这两个矩阵的Kronecker积；在步骤3中，由于

则简化计算方法为：

其中，

且为的四元数协方差阵

步骤4：记传感器三轴单位化的观测矢量为对应的在参考矢量在导航系的投影为可以得到四元数卡尔曼滤波的乘性观测矩阵：

最终得到四元数卡尔曼滤波的量测更新如下：

2.如权利要求1所述的一种基于四元数的姿态估计方法，其特征在于，在步骤3中观测噪声的简化计算方法为：

中表示这个观测噪声矩阵是针对单位化观测量的。

3.如权利要求1所述的一种基于四元数的姿态估计方法，其特征在于，所述更新步骤具体为：

如果加速度计未超限，对卡尔曼滤波器进行加速度计观测值更新；

如果磁强计未超限，对滤波器进行磁强计观测值更新，进而从中得到

根据公式：计算得到其中*表示四元数乘法，是的共轭四元数，包括横滚角和俯仰角信息；

计算TRIAD算法，并忽略时间下标k，

将得到的结果进行量测更新以得到和

4.如权利要求1所述的一种基于四元数的姿态估计方法，其特征在于，在所述更新步骤中，若GNSS接收机地速大于设定阈值，采用给出的航向角ψ作为观测值代替不稳定的磁场观测量，具体实现过程如下：

b_r＝[v_E v_N 0]^T同样的将b_r代入TRIAD算法中进行量测更新。