[发明专利]一种基于最小二乘的INS/GNSS/偏振/地磁组合导航系统对准方法

权利要求书

1.一种基于最小二乘的INS/GNSS/偏振/地磁组合导航系统对准方法，其特征在：于包括以下步骤：

(1)选择失准角、速度误差、位置误差、陀螺仪零偏、加速度计零偏以及磁航向角误差作为INS/GNSS/偏振/地磁组合导航系统的状态变量；

(2)根据偏振传感器输出的偏振方位角与INS输出的方位角匹配融合建立偏振量测方程，得到基于偏振方位角的量测方程；

(3)根据磁强计输出的航向角与INS输出的航向角建立地磁量测方程，得到基于磁强计航向角误差的量测方程；

(4)根据GNSS输出的速度、位置信息与INS输出的速度、位置信息匹配融合建立速度误差与位置误差量测方程，得到基于速度误差与位置误差的量测方程；

(5)在步骤(2)、(3)和(4)建立的量测方程基础上，利用增广技术将上述3个步骤中的量测方程进行向量化，建立统一的INS/GNSS/偏振/地磁组合导航系统量测方程；

(6)根据步骤(5)建立的统一的INS/GNSS/偏振/地磁组合导航系统量测方程，利用最小二乘技术估计INS/GNSS/偏振/地磁组合导航系统未知的状态；

(7)根据步骤(6)中估计得到的失准角、速度误差以及位置误差对INS/GNSS/偏振/地磁组合导航系统进行反馈校正，方法为基于INS获取的姿态、速度及位置上减去步骤(6)中估计得到的失准角、速度误差及位置误差，获取经反馈校正后的INS/GNSS/偏振/地磁组合导航系统姿态、速度及位置；

所述步骤(2)中，根据偏振传感器输出的偏振方位角与INS输出的方位角匹配融合建立偏振量测方程为：

z_polar＝H₁X+υ_polar

其中，为INS/GNSS/偏振/地磁组合导航系统16维状态变量，φ_E，φ_N，φ_U分别为导航系，即n系下的东向失准角、北向失准角和天向失准角；δV_E，δV_N，δV_U分别为导航系下的东向、北向和天向速度误差，这里导航系采用当地地理坐标系，即以载体的质心为原点，当地东向为地理系x轴，北向为y轴，天向为z轴，x轴，y轴和z轴满足右手坐标系；δλ，δL，δh分别为经度、纬度和高度误差；ε_x，ε_y，ε_z为载体坐标系，即b系下三个轴向的陀螺常值漂移，这里b系定义为，其原点O与载体质心重合，x轴正向与载体正向同向，y轴沿载体纵轴指左，z轴垂直xOy构成的平面指上，坐标系满足右手坐标系；分别为b系下加速度常值漂移；为磁强计的航向角测量误差；υ_polar为偏振传感器的量测噪声；z_polar为偏振量测量，表示为：

其中，s^m为单位太阳矢量在模块系m系下的表示，模块系为偏振传感器模块所在的坐标系，定义与载体坐标系基本相同，原点为模块质心，一般情况下与载体坐标系重合；为计算导航系n′系到b系下的姿态转换矩阵，计算导航系n′系为实际导航过程用于计算导航解的导航坐标系，坐标原点与导航系统相同，x轴、y轴和z轴与理想导航系n系存在偏差角，该偏差角即为失准角；为地球系e系到理想导航系n系下的姿态转换矩阵，地球系原点与地心重合，x_e轴指向本初子午线，z_e轴沿极轴方向指向北极，y_e轴在赤道平面内与x_e、z_e轴构成右手系，理想导航系即为“东北天”坐标系，其原点为载体所在点，N轴指向北极方向，E轴指向东，U轴指向天空，满足右手坐标系；s^e为单位太阳矢量在e系下的表示；

H₁是以偏振方位角的量测矩阵，具体表示为：

其中，表示矢量的反对称阵形式；

所述步骤(6)中，利用最小二乘技术估计INS/GNSS/偏振/地磁组合导航系统未知的状态具体为：所设计的最小二乘滤波具体如下：

第n次测量后INS/GNSS/偏振/地磁组合导航系统量测方程为：

Z_n＝H_nX+V_n

其中，Z_n为第n次全部量测的测量值，H_n为第n次INS/GNSS/偏振/地磁组合导航系统统一的量测矩阵，V_n为n时刻噪声向量，其加权最小二乘估计为：

其中，R_n为第n次INS/GNSS/偏振/地磁组合导航系统测量噪声矩阵，为第n次估计值；

估计误差方差为：

其中，P_n为误差方差矩阵，为第n次测量真实值X与估计值之差；

综合上述两式，得：

当得到一个新的测量Z_n+1时，有：

Z_n+1＝H_n+1X+V_n+1

新的加权最小二乘估计为：

把新的测量噪声加入原本的测量噪声矩阵中，此时噪声矩阵的逆矩阵应为对角矩阵：

记展开得：

此时，n+1时刻误差方差矩阵为：

令中间量则n+1时刻状态估计值为：