[发明专利]一种基于低成本MEMS航姿参考系统的船舶运动姿态解算方法

权利要求书

1.一种基于低成本MEMS航姿参考系统的船舶运动姿态解算方法，其特征在于，包括：S1：获取航姿参考系统信息并进行误差补偿；

S2：构建陀螺仪误差模型；

S3：运用四元数矩阵表示导航坐标系与载体坐标系的转换关系；

S4：结合从陀螺仪获取的航姿参考系统信息与陀螺仪误差模型构建系统状态方程，结合从加速度计以及磁力计获取的航姿参考系统信息构建量测方程；

S5：计算船舶姿态角的初值及确定卡尔曼滤波器的参数初值；

S6：采用CKF算法作为融合算法并进行鲁棒性改进；

S7：计算更新后的船舶姿态角；

所述船舶运动姿态解算方法还包括：结合所述系统状态方程和量测方程构建卡尔曼滤波器，根据所述卡尔曼滤波器计算船舶姿态角；

所述S2包括：根据陀螺仪的随机误差种类，建立陀螺仪误差模型；

所述陀螺仪误差模型为：

ε＝b_ω+ε_r+w_g；

其中，ε表示陀螺仪的主要误差，b_ω表示随机常值漂移，ε_γ表示陀螺仪一阶马尔可夫随机漂移，w_g表示陀螺仪解算姿态角的白噪声，w_b表示随机常值漂移的白噪声，τ_g表示相关时间常数，w_r表示马尔可夫过程附加的白噪声；

所述S6包括：基于强跟踪滤波方法调节预测状态误差协方差矩阵；基于Huber鲁棒滤波方法调节测量噪声协方差矩阵；

所述基于强跟踪滤波方法调节预测状态误差协方差矩阵包括：在预测状态误差协方差矩阵中引入多衰因子矩阵Λ_k+1：

Λ_k+1＝diag(λ_1，k+1，λ_2，k+1，…，λ_n，k+1)；

λ_1，k+1＝μ_ic_k；

其中，λ_i，k+1表示第i个状态分量对应的衰落因子，μ_i表示第i个状态分量相对应的衰落因子的权重，c_k是标准因子；

所述基于Huber鲁棒滤波方法调节测量噪声协方差矩阵包括：引入代价函数，修正测量噪声协方差矩阵的权重。

2.根据权利要求1所述的一种基于低成本MEMS航姿参考系统的船舶运动姿态解算方法，其特征在于，所述S1包括：通过陀螺仪、加速度计以及磁力计获取航姿参考系统信息，并对标定出来的误差进行补偿。

3.根据权利要求1所述的一种基于低成本MEMS航姿参考系统的船舶运动姿态解算方法，其特征在于，所述S3具体包括：定义单位四元数向量q＝[q₀，q₁，q₂，q₃]T，利用四元数坐标变换矩阵表示导航坐标系n系与载体坐标系b系之间的坐标转换关系；

4.根据权利要求1所述的一种基于低成本MEMS航姿参考系统的船舶运动姿态解算方法，其特征在于，所述S4包括：根据磁力计的输出模型与加速度计的输出模型，得到量测方程；

加速度计的输出模型为：

其中，表示四元数坐标变换矩阵，a^b表示加速度计在b系测量的加速度信号，g表示地球引力常数的绝对值，v_a为加速度计的加性传感器噪声；

磁力计的输出模型为：

其中表示磁力计在b系输出的磁场信号，v_h表示磁力计的噪声；[E_x 0 E_z]表示当地的地球磁场。