[发明专利]一种在线实时估计补偿磁强计误差的定位测姿方法

权利要求书

1.一种在线实时估计补偿磁强计误差的定位测姿方法，其特征在于，

包括以下步骤：

步骤1：首先对GNSS/MINS组合导航系统进行初始化，初始化包括位置和姿态的初始化；

步骤2：实时导航过程中记录估计磁强计误差所需要的数据，包括磁强计的实时输出和磁强计的理论输出值；具体为：

设定磁强计的数学模型为：

或者：

式中，M矩阵代表磁强计的误差矩阵，其中对角线元素对应磁强计的比例因子误差，非对角线元素代表磁强计的交轴耦合误差，b_mx,b_my,b_mz分别代表磁强计x,y,z轴的零偏误差；

设定某一时刻载体的姿态信息为[φ₁ θ₁ ψ₁]，则在该时刻磁强计输出的理论值表示为：

其中，m′₁代表当地磁场在b系中的投影，即磁强计的理论输出值，m_bx1,m_by1,m_bz1依次代表当地磁场在磁强计x，y，z上的分量，为载体坐标系到导航坐标系的方向余弦矩阵，cθ、cφ、cψ依次代表横滚角、俯仰角以及航向角的余弦值，sθ、sφ、sψ依次代表横滚、俯仰以及航向角的正弦值，m_n为地磁场在导航坐标系的投影；

得到多组磁强计输出的理论值：

同时记录载体对应的[φ_iθ_iψ_i]角度时的磁强计的实际输出，组成如下所示的观测矩阵u_i:

其中m_xi,m_yi,m_zi依次代表磁强计x，y，z轴的输出，i组相互独立的磁强计实际输出记为：

步骤3：根据步骤2记录的磁强计的实时输出和磁强计的理论输出值通过最小二乘方法实时计算磁强计的误差,所述的磁强计的误差包括磁强计零偏、比例因子以及交轴耦合误差；

步骤4：当GNSS信号中断时，先用步骤3中计算得到的磁强计的误差补偿磁强计输出，再利用补偿后磁强计信息辅助组合导航系统的航向。

2.根据权利要求1所述的一种在线实时估计补偿磁强计误差的定位测姿方法，其特征在于，步骤3中通过最小二乘方法实时计算磁强计的误差具体为：

根据步骤2记录的磁强计的实时输出和磁强计的理论输出值，构建最小二乘的系数矩阵如下：

最小二乘的观测矩阵，如下：

U＝[u₁ u₂ ... u_i]；

通过最小二乘方法计算得到误差矩阵：

M＝U·A^T·(AA^T)^-1；

根据误差矩阵实时计算磁强计的误差。

3.根据权利要求1所述的一种在线实时估计补偿磁强计误差的定位测姿方法，其特征在于，步骤4具体为：

当GNSS信号中断时，首先利用步骤3中计算得到的磁强计误差补偿磁强计输出，再用补偿后的磁强计输出计算航向信息，航向信息具体计算方法如下：

其中，

θ、φ分别为载体实时的横滚角和俯仰角；ψ_mag为利用磁强计输出计算得到的载体的航向信息，γ_m为当地磁偏角，其值结合纬度信息查阅资料得到；M_x,M_y,M_z分别为经过补偿后的磁强计输出的X，Y，Z轴的输出；

利用磁强计和惯导计算的航向信息进行加权平均得到精度更高的航向信息，计算方法如下：

其中ψ_int为利用磁强计和惯导计算的航向值进行组合后计算的得到的航向值，ψ_ins为惯导计算的航向信息，而ψ_mag为磁强计计算的航向信息，σ_ins、σ_mag为惯导和磁强计计算得到的航向的方差。