[发明专利]基于陀螺误差修正的对偶四元数惯性/天文组合导航方法

权利要求书

1.一种基于陀螺误差修正的对偶四元数惯性/天文组合导航方法，其特征在于，包括如下步骤：

步骤1，将陀螺误差和加速度计误差均建模为常值误差与高斯白噪声随机误差，并将陀螺和加速度计的常值误差扩展为系统状态量，由此得到基于加性对偶四元数的惯性/天文组合导航状态方程；

步骤2，天文星敏感器的输出值为机体系相对于惯性系的旋转四元数，将其输出误差建模为加性四元数，并利用陀螺输出的原始信息计算出机体系相对于惯性系的旋转四元数，结合两者的信息，构建基于加性对偶四元数的惯性/天文组合导航量测方程；

步骤3，在步骤1、2的组合导航状态方程和量测方程的基础上，采用基于奇异值分解的可观测度分析方法对该状态方程中的状态变量进行可观测度分析，根据其分析结果，剔除可观测性差和不可观的状态分量，得到降维后的基于加性对偶四元数的惯性/天文组合导航状态方程；

步骤4，对系统状态方程和量测方程进行离散化处理，并采用卡尔曼滤波方法对各状态变量进行开环估计，利用开环估计得到的陀螺常值漂移误差估计值对陀螺原始输出信息进行在线修正，然后基于修正后的陀螺输出信息进行对偶四元数捷联惯性导航解算。

2.根据权利要求1所述基于陀螺误差修正的对偶四元数惯性/天文组合导航方法，其特征在于，步骤1所述陀螺和加速度计误差模型为：

其中，为陀螺误差，为陀螺常值漂移误差，为陀螺高斯白噪声；δf^B为加速度计误差，δf_c^B为加速度计常值误差，δf_s^B为加速度高斯白噪声。

3.根据权利要求1所述基于陀螺误差修正的对偶四元数惯性/天文组合导航方法，其特征在于，步骤1所述的基于加性对偶四元数的惯性/天文组合导航状态方程：

其中X∈R^24×1为系统状态量，F∈R^24×24为系统矩阵，G∈R^24×8为噪声系数矩阵，w∈R^8×1为系统噪声向量，为X的一阶导数，各矩阵分别表示为：

1

系统矩阵F和噪声系数矩阵G中，若将四元数q写成q＝[q₀ q₁ q₂ q₃]^T的形式，我们定义矩阵为q在四元数乘法中的前乘矩阵，为q在四元数乘法中的后乘矩阵，其具体可表示为：

式(3)～(6)中0均为四阶零矩阵，I₄为四阶单位矩阵，各变量均为四元数，其中三维向量表示为标量部分为0的四元数；δq_IT为推力速度对偶四元数误差的实数部分，δq′_IT为推力速度对偶四元数误差的对偶部分，δq′_IG为引力速度对偶四元数误差的对偶部分，δq′_IU为位置对偶四元数误差的对偶部分；为陀螺输出信息，为在四元数乘法中的后乘矩阵；q_IT为推力速度对偶四元数的实数部分，为q_IT在四元数乘法中的前乘矩阵；q′_IT为推力对偶四元数的对偶部分，为q′_IT在四元数乘法中的前乘矩阵；为地球自转角速度，为在四元数乘法中的后乘矩阵；q_IU为位置对偶四元数的实数部分，为其在四元数乘法中的前乘矩阵，为其在四元数乘法中的后乘矩阵；q^*_IT为q_IT的共轭四元数，为其在四元数乘法中的前乘矩阵，为其在四元数乘法中的后乘矩阵；f_B为加速度计输出信息，为其在四元数乘法中的后乘矩阵。

4.根据权利要求1所述基于陀螺误差修正的对偶四元数惯性/天文组合导航方法，其特征在于，步骤2所述的天文星敏感器加性四元数输出误差模型为：

q_c＝q_IB-δq_IB，

其中，q_c为天文星敏感器输出的机体系相对于惯性系的旋转四元数，q_IB为真实的机体系相对于惯性系的旋转四元数，δq_IB为天文星敏感器量测误差，将其建模为高斯白噪声。