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

权利要求书

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

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

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

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

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

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

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

(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)中，建立的惯性/偏振光组合导航系统非线性状态方程与量测方程为：

y＝h(X)+r

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

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

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

ε^b(k)＝ε^b(k-1)

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

其中，k表示第k个时刻，Δt为采样时间间隔，I_3×3为3×3维单位向量；

所述步骤(6)中，设计的无迹卡尔曼滤波器UKF如下：

①预测步骤：

在时刻k-1处，生成2n-1个sigma点{χ_k-1,i}以及相关的权重{W_i}：

W₀＝κ/(n+κ)

W_i＝1/(2(n+κ)),i＝1,…,n

W_i+n＝1/(2(n+κ))

其中，n为状态维数，κ为尺度参数，调整它提高逼近精度，表示协方差矩阵P_x,k-1|k-1的平方根矩阵的第i列，为k-1时刻状态估计值；

将每一个sigma点经过非线性状态函数ξ＝f(χ)变换后，有：

ξ_k-1,i＝f(χ_k-1,i)i＝0,1,…,2n

ξ_k-1,i的平均值以及方差P_x,k|k-1为：

其中，Q_k-1为k-1时刻噪声方差矩阵；

②更新步骤：

利用生成2n-1个sigma点{χ_k,i}；

将sigma点经过非线性量测函数ξ＝h(χ)变换，有：

ξ_k,i＝h(χ_k,i)

同理，ξ_k,i的平均值自协方差矩阵P_y,k及互协方差矩阵P_xy,k分别为：

其中，R_k为k时刻量测噪声矩阵；

利用Kalman滤波框架来计算k时刻的状态均值及方差：

其中，为k时刻状态估计值，P_x,k|k为k时刻状态协方差矩阵；

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

(1)姿态校正方法

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

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

(2)速度校正方法

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

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