[发明专利]一种大失准角下惯性/偏振光组合导航系统EKF对准方法

权利要求书

1.一种大失准角下惯性/偏振光组合导航系统EKF对准方法，其特征在于，惯性/偏振光组合导航系统由惯性导航系统和偏振光导航系统组成，包括以下步骤：

(1)选择失准角、速度误差、陀螺漂移及加速度计常值偏置作为惯性/偏振光组合导航系统的状态矢量，并建立大失准角下初始对准惯性/偏振光组合导航系统非线性误差状态方程；

(2)根据偏振光传感器测量的偏振方位角计算太阳矢量，建立偏振光非线性量测方程；

(3)根据惯性导航系统的速度输出，建立速度误差量测方程；

(4)利用增广技术将步骤(2)建立的偏振光非线性量测方程和步骤(3)建立的速度误差量测方程进行向量化，建立统一的惯性/偏振光组合导航系统非线性量测方程；

(5)在步骤(1)和(4)基础上，建立惯性/偏振光组合导航系统非线性状态方程与量测方程，对所建立的惯性/偏振光组合导航系统非线性状态方程与量测方程进行离散化，得到离散的惯性/偏振光组合导航系统状态方程和量测方程；

(6)根据步骤(5)建立的离散惯性/偏振光组合导航系统状态方程和量测方程，设计扩展卡尔曼滤波器，估计惯性/偏振光组合导航系统未知的状态，获得惯性/偏振光组合导航系统的失准角、速度误差、陀螺漂移和加速度计常值偏差；

(7)根据步骤(6)中估计的失准角及速度误差，对基于惯性导航系统的姿态和速度进行补偿，使用反馈校正方法，获得校正后的惯性/偏振光组合导航系统的姿态和速度；

所述步骤(1)中，选择失准角、速度误差、陀螺漂移及加速度计常值偏置作为惯性/偏振光组合导航系统的状态矢量，并建立大失准角下初始对准惯性/偏振光组合导航系统非线性误差状态方程，所选择惯性/偏振光组合导航系统初始对准的状态矢量为:

其中，φ_E，φ_N和φ_U为分别为导航坐标系n系下东向、北向和天向的失准角，表示为计算导航坐标系n′系与n系之间的误差角；和为n系下三轴速度误差；和为载体坐标系b系下三轴陀螺常值偏置；和为b系下的三轴加速度计常值偏置；

大失准角初始对准下，惯性/偏振光组合导航系统的非线性误差状态方程为：

其中，

为n系与计算导航系n′系间的姿态转换矩阵，表示为：

f^b为b系下比力，由加速度计得到；gⁿ为当地的重力矢量；为b系与n′系的姿态转换矩阵；为地球系e系相对于惯性系i系的角速度在n系下的表示，由地球自转产生；为矢量的对反对称阵形式，即：

其中，和为的三轴分量；I_3×3为3×3维单位矩阵，0_3×1为3×1维全0矢量；与分别为陀螺与加速度计的噪声矢量，令状态噪声矢量为且服从均值为0，方差为Q的高斯白噪声分布，即q～N(0_12×1,Q)，Q＝E[qq^T]，0_1×6为1×6维全0矢量，0_12×1为12×1维全0矢量；

所述步骤(2)中，建立偏振光非线性量测方程为：

其中，为利用偏振光传感器m₁获得的量测矢量，为偏振光传感器模块系m₁与b系间的姿态转换关系，即为偏振光传感器m₁与载体的安装关系，可在对准前标定获得；S^m1为太阳矢量在m₁系下的表示，可由偏振光传感器计算获得；Sⁿ太阳矢量在n系下的表示，利用当地位置、时间，根据天文年历计算得到；δS^m1为S^m1的量测误差；为偏振光量测噪声矢量，服从均值为0，方差为R₁的高斯白噪声分布，即r_pol～N(0_3×1,R₁)，0_3×1为3×1维全0矢量；

所述步骤(3)中，建立的速度误差量测方程为：

δv＝[0_3×3 H_v 0_3×3 0_3×3]X+r_v

其中，为惯性导航系统的加速度计输出经计算后得到的三轴速度值，0_3×3为3×3维全0矩阵，r_v为速度误差噪声矢量，服从均值为0，方差为R₂的高斯白噪声分布，即r_v～N(0_3×1,R₂)，

所述步骤(4)中，建立的统一的惯性/偏振光组合导航非线性量测方程为：

y＝h(X)+r

其中，h(·)为惯性/偏振光组合导航系统非线性量测函数，其具体形式为：

r为量测噪声矢量，且0_6×1为6×1维全0矢量；

所述步骤(5)中，在步骤(1)和(4)基础上，建立惯性/偏振光组合导航系统非线性状态方程与量测方程，对所建立的惯性/偏振光组合导航系统非线性状态方程与量测方程进行离散化，得到离散的惯性/偏振光组合导航系统状态方程和量测方程，所建立的惯性/偏振光组合导航系统非线性状态方程与量测方程为：

y＝h(X)+r

其中，f(·)为惯性/偏振光组合导航系统非线性状态函数，其具体形式为：

q为状态噪声矢量，服从均值为0，方差为Q的高斯白噪声分布，即q～N(0_12×1,Q)，Q＝E[qq^T]，0_12×1为12×1维全0矢量；

将惯性/偏振光组合导航系统非线性状态方程与量测方程进行离散化，得到离散的惯性/偏振光组合导航系统状态方程和量测方程：

y(k)＝h(X(k))+r(k)

其中，k表示第k个时刻，Δt为采样时间间隔，为非线性状态函数f(·)在X(k-1)处的一阶泰勒级数展开；

所述步骤(6)中，根据步骤(5)建立的离散惯性/偏振光组合导航系统状态方程和量测方程，设计扩展卡尔曼滤波器，估计惯性/偏振光组合导航系统未知的状态，获得惯性/偏振光组合导航系统的失准角、速度误差、陀螺漂移和加速度计常值偏差，所设计的扩展卡尔曼滤波器如下：

①时间更新：

状态一步预测方程：

其中，为k-1时刻估计值，为k时刻一步预测值；

一步预测状态误协方差方程：

P(k|k-1)＝Φ(k|k-1)P(k-1|k-1)Φ^T(k|k-1)+Q(k-1)

其中，P(k-1|k-1)为k-1时刻的误差协方差矩阵，P(k|k-1)为k时刻状态的一步预测误差协方差矩阵，状态转移矩阵Φ(k|k-1)＝I+A(X(k-1|k-1))Δt，Q(k-1)为k-1时刻状态噪声的协方差矩阵；

②量测更新：

状态估计方程：

其中，为k时刻的状态最优估计，K(k)为卡尔曼滤波增益矩阵，其计算方法为

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

其中，为非线性量测函数h(·)在处的一阶泰勒级数展开，R(k)为k时刻量测噪声的协方差矩阵；

估计误差协方差方程：

P(k|k)＝(I-K(k)H(k))P(k|k-1)(I-K(k)H(k))^T+K(k)R(k)K^T(k)

其中，I为与协方差矩阵P(k|k-1)同维度的单位矩阵；

所述步骤(7)中，采用的反馈校正方法如下：

(1)姿态校正方法

首先计算n′系与n系的转换矩阵

其中，和为状态估计后的失准角，则校正后的姿态矩阵为：

(2)速度校正方法

设校正后的三轴载体速度为V_x、V_y和V_z，则速度校正表示为：

其中，和分别为n系下惯性导航系统的三轴速度值；和为n系下状态估计的三轴速度误差。