[发明专利]一种运动姿态传感器的数据融合与解算方法

权利要求书

1.一种运动姿态传感器的数据融合与解算方法，其特征在于：

所述运动姿态传感器能够输出的数据包括：宿主原始X、Y、Z三个轴向的角速度、加速度、复合磁场强度ADC数据，经过融合但未经处理的宿主四元数数据，经过解算和处理的宿主姿态角数据，包括航向角、俯仰角、横滚角、倾斜角；

所述原始X、Y、Z三个轴向的角速度、加速度、复合磁场强度ADC数据是运动姿态传感器的角速度计、加速度计和磁力计将采集到的X、Y、Z三个轴向的角速度、加速度、地球磁场和环境磁场之复合磁场强度等模拟数据发送给微处理器后进行模数转换后的数字化数据；

所述经过融合但未经处理的宿主四元数数据是微处理器对原始X、Y、Z三个轴向的角速度、加速度、复合磁场强度ADC数据按预设好的融合算法进行数据融合而得到的超复数，它由实数加上三个虚数单位i、j、k组成，四元数一般可表示为

其中n为旋转方向，θ/2为转角大小，i，j，k关系为但该四元数据没有经过欧拉角解算和数据滤波、修正补偿等，四元数表示三维空间中的一个旋转；

所述经过解算和处理的宿主姿态角数据是微处理器对经过融合的四元数按预设好的数据解算算法解算出初始欧拉角，即航向角、俯仰角、横滚角、倾斜角，这些姿态数据再经过预设的滤波修正算法进行处理，得到相对精确的宿主姿态角数据。

2.根据权利要求1所述的一种运动姿态传感器的数据融合与解算方法，其特征在于：所述的数据融合算法是将原始X、Y、Z三个轴向的角速度、加速度、复合磁场强度ADC数据进行空间融合而形成四元数的算法，包括如下计算方法和步骤：

(1)四元数微分

已知四元数对时间微分

已知E为地理坐标系，b为宿主坐标系

因为角速度计在宿主坐标系上测得的角速度为故将转换成会比较方便

展开得

整理为

其中

(2)四元数更新

假设有一微分方程

则其解为

X(t+Δt)＝X(t)+Δt·f(X(t)，w(t))

其中Δt为取样周期，套用四元数

Q(t+Δt)＝Q(t)+Δt·Ω_b·Q(t)

展开上式

其中w_xw_yw_z为宿主坐标系下的三轴角速度值；

(3)角速度积分偏差修正，

通过角速度微分方程可以得到四元数，问题是角速度计会有积分漂移，得到的角度会有误差，这时候需引入加速度计对角速度数据进行校正，以得到更准确的角度；不过这时航向也会存在误差，在误差不能忽略的使用情况下，需要用磁力计校正，观察矩阵为重力加速度和正北方向的磁场，通过这个误差向量来修正角速度数据，将融合后的角速度输入到四元数微分方程就得到四元数；

系统模型的旋转矩阵

其中

R_1，1＝q₀²+q₁²-q₂²-q₃²

R_1，2＝2(q₁q₂+q₀q₃)

R_1，3＝2(q₁q₃-q₂q₀)

R_2，1＝2(q₁q₂-q₀q₃)

R_2，2＝q₀²-q₁²+q₂²-q₃²

R_2，3＝2(q₃q₂+q₀q₁)

R_3，1＝2(q₁q₃+q₂q₀)

R_3，2＝2(q₃q₂-q₀q₁)

R_3，3＝q₀²-q₁²-q₂²-q₃²

四元数微分方程

其中

输入方程

为角速度计输出值，宿主坐标系下真实的角速度，传感器噪声矢量，角速度漂移矢量，则有状态方程

因为

所以

输出方程

其中，为重力加速度矢量，为正北磁场的矢量

线性化下的雅可比矩阵

离散化模型

离散时间的一阶近似

Φ≌I+FT

Γ≌GT

滤波过程

P_k|k-1＝(I+FT)P_k|k-1(I+FT)^T+T²GQG^T

K＝P_k|k-1H^T(HP_k|k-1H^T+R)^-1

P_k＝P_k|k-1-KHP_k|k-1

其中，为状态预测值，P_k|k-1为均方差预测，K为最优卡尔曼增益，α_k为新息向量，为状态估计，P_k为均方差估计，α_k又被称为残差向量，它反映了滤波模型的准确性。

3.根据权利要求1所述的一种运动姿态传感器的数据融合与解算方法，其特征在于：所述的数据解算算法是将四元数根据运动姿态传感器的宿主运动特征按一定的规则进行欧拉角的计算，得到相应取值范围的姿态角数据的算法，包括如下计算方法和步骤：

(1)四元数乘法

设有两四元数QA和QB，其乘法关系为

旋转四元数

设一向量及旋转后向量四元数满足R′＝Q^-1·R·Q

四元数倒数

(2)四元数归一

当四元数归一化后|Q|＝1

Q^-1＝Q^*

可得到

R′＝Q^*·R·Q

将其展开

另四元数旋转矩阵为M_q

(3)将四元数转换成欧拉角，

由M_q可得出

其中θ为横滚角，Φ为俯仰角，为航向角

(4)倾角计算

其中θ为倾角，ax，ay，az分别为三轴重力加速度的值。