[发明专利]基于MEMS传感器的四元数融合姿态估计方法

权利要求书

1.一种基于MEMS传感器的四元数融合姿态估计方法，其特征在于，包括：

将陀螺仪原始测量信号进行预处理，得到预处理后的高频陀螺输出；

使用含遗忘因子的加权最小二乘算法对所述高频陀螺输出进行处理，得到去噪后的陀螺输出；

基于获取的初始姿态四元数和所述去噪后的陀螺输出使用改进的最小二乘递推方法估计陀螺漂移；

将估计得到的陀螺漂移输入最小二乘估计模型得到补偿后的姿态四元数；

基于所述补偿后的姿态四元对陀螺仪的姿态进行更新；

所述将估计得到的陀螺漂移输入最小二乘估计模型得到补偿后的姿态四元数，包括使用四元数融合的动态更新姿态方式对陀螺仪的姿态进行更新，

所述四元数融合的动态更新姿态方式，包括：

设载体坐标系为b系，导航坐标系为n系，由b系到n系的坐标变换矩阵称为姿态转换矩阵；

基于所述姿态转换矩阵表示陀螺输出的角速度信息，得到陀螺四元数；

基于所述姿态转换矩阵表示加速度计输出的加速度信息，得到加速度计四元数；

将所述陀螺四元数与所述加速度计四元数做差，作为滤波的状态量，将所述加速度计四元数作为观测量，则得到四元数最小二乘估计模型；

基于所述四元数最小二乘估计模型得到四元数的姿态补偿模型；

所述四元数最小二乘估计模型为：

其中q_ω为陀螺四元数，q_a为加速度计四元数，T为角速度采样周期，Q_k为上一时刻的四元数矩阵，ω₀为陀螺漂移向量，q_a0加速度计解算中的噪声四元数， k为自然数；为状态估计向量，由四元数误差组成；z为观测向量，由加速度计四元数组成。

2.根据权利要求1所述的基于MEMS传感器的四元数融合姿态估计方法，其特征在于，所述将陀螺仪原始测量信号进行预处理的步骤之前，包括：

将陀螺仪原始测量信号表示为：

s(k)＝h(k)+ε·e(k),k＝0,1,…,n-1，

其中，s(k)为陀螺仪原始测量信号，h(k)为无偏信号，e(k)为噪声，ε为噪声标准差。

3.根据权利要求1所述的基于MEMS传感器的四元数融合姿态估计方法，其特征在于，所述含遗忘因子的加权最小二乘算法，包括：

设定输入与输出服从的函数关系；

基于所述函数关系建立模型输出值和实际观测值的误差平方和方程，并在所述误差平方和方程中引入遗忘因子λ；

基于引入遗忘因子λ的误差平方和方程获取最小二乘问题；

对所述最小二乘问题进行加权处理。

4.根据权利要求3所述的基于MEMS传感器的四元数融合姿态估计方法，其特征在于，所述含遗忘因子的加权最小二乘算法，包括：

设定系统输入与输出所服从的函数关系为y＝f(x,t_i)，其中y为系统输出，t_i为系统输入，x∈Rⁿ，为待定参数或者待定向量；

记基于模型输出值和实际观测值的误差平方和为S，如其中，m为实际观测次数，求得S最小时x的值为一般的最小二乘问题，并引入遗忘因子λ到一般最小二乘估计准则中，如式：

引入残差函数r_i(x)＝y_i-f(x,t_i),i＝1,2,…,m，记于是记最小二乘问题为

将系统量测方程写为矩阵形式z＝Hx+v，对最小二乘估计准则进行加权处理，如式：

其中，z为量测向量，作为无法得到待估向量x的真实值的一种间接测量，选择了真实值x的各分量进行线性组合；H为量测矩阵，v为随机量测噪声，W为加权正定矩阵，得到加权最小二乘估计为如式：

加权最小二乘估计残差如式：

其中测量噪声v满足均值为零，方差阵为R，即且满足W＝R^-1，则加权最小二乘估计如式