[发明专利]一种基于相对安装位置关系的多惯导协同导航方法及装置

权利要求书

1.一种基于相对安装位置关系的多惯导协同导航方法，其特征在于，包括：

步骤S1：基于多惯导系统中各惯导的相对安装位置测量，建立主用惯导与备用惯导的导航参数之间的数学关系；

步骤S2：获取所述主用惯导与备用惯导之间的相对距离信息和相对姿态信息，基于所述主用惯导与备用惯导的导航参数之间的数学关系构建协同导航系统的量测方程；

步骤S3：对所述多惯导系统的误差进行扩展卡尔曼滤波，构建扩展卡尔曼滤波状态方程；基于所述扩展卡尔曼滤波状态方程及所述协同导航系统的量测方程，生成导航信息。

2.如权利要求1所述的方法，其特征在于，所述步骤S1，包括：

步骤S11：通过测量各惯导的相对距离Δr₁₂，构建所述主用惯导与所述备用惯导位置间的数学关系，则有

式中，

和分别表示所述主用惯导和所述备用惯导实际输出的纬度、经度信息，L₁、λ₁和L₂、λ₁分别表示所述主用惯导和所述备用惯导理想或预期输出的纬度、经度信息，δL₁、δλ₁和δL₂、δλ₂分别表示所述主用惯导和所述备用惯导的纬度误差、经度误差；R_n、R_e分别表示地球子午圈、卯酉圈曲率半径；h＝(h₁+h₂)/2表示惯导输出载体的平均高度信息；表示惯导输出载体的平均纬度信息，h₁、h₂分别表示所述主用惯导、所述备用惯导输出载体的高度；

步骤S12：使用标校装置进行安装标校，使得构建所述主用惯导的姿态矩阵和所述备用惯导的姿态矩阵之间的数学关系，则有

式中，

b₁和b₂分别表示所述主用惯导和所述备用惯导的载体坐标系；I表示单位阵，表示所述主用惯导载体坐标系相对所述备用惯导载体坐标系的姿态矩阵；为所述主用惯导载体坐标系b₁相对导航坐标系n的计算姿态矩阵，为所述备用惯导载体坐标系b₂相对导航坐标系n的计算姿态矩阵，φ₁ⁿ为所述主用惯导的姿态失准角,为所述备用惯导的姿态失准角，为所述主用惯导载体坐标系b₁相对导航坐标系n的理想姿态矩阵，为所述备用惯导载体坐标系b₂相对导航坐标系n的理想姿态矩阵。

3.如权利要求2所述的方法，其特征在于，所述步骤S2，包括：

步骤S21：选取状态变量，所述状态变量包括所述主用惯导及所述备用惯导的姿态误差、速度误差、位置误差、陀螺常值误差和加速度计常值误差，构建的状态变量X表示如下：

式中，φ_iⁿ表示姿态失准角，δV_iⁿ表示速度误差，表示位置误差，δh_i为高度误差，表示陀螺常值误差，表示加速度计常值误差，1≤i≤num，num为所述主用惯导数量与所述备用惯导数量的和，num＝2；

步骤S22：构建协同导航系统的量测方程：

令

则有

其中，Δθ₁₂为构建的所述主用惯导与所述备用惯导间姿态误差角向量，Δθ_x、Δθ_y、Δθ_z分别为姿态误差角向量Δθ₁₂三个元素；

获得协同导航系统的量测方程：

Z＝[Δθ_x Δθ_y Δθ_z Δr₁₂]^T＝h(X)+V (9)

式中，Z为量测变量，V为量测噪声向量；h(X)为量测函数，基于所述主用惯导与所述备用惯导的导航参数之间的数学关系获取。