[发明专利]捷联惯性导航系统的快速精对准方法

摘要

一种捷联惯性导航系统的快速精对准方法，属捷联惯性导航系统的对准方法。该方法包括如下步骤：采集惯性测量组件的输出信号，得到运载体的角速度和比力信息；捷联惯性导航系统解析式粗对准；捷联惯性导航系统解算；捷联惯性导航系统精对准；滤波输出及修正；精对准完成。本方法针对低成本捷联惯性导航系统，可实现性和可操作性高，成本低，提高了捷联惯性导航系统的对准精度和快速性，并且实现了航向对准。

主权项

1.一种捷联惯性导航系统的快速精对准方法，其特征在于该方法包括下列步骤：(A)采集惯性测量组件的输出信号，得到运载体的角速度和比力信息；(B)捷联惯性导航系统解析式粗对准步骤在粗对准时，利用加速度计测量重力加速度矢量g在机体坐标系b中的分量；利用陀螺仪测量地球角速率矢量Ω在机体坐标系b中的分量，则机体坐标系相对导航坐标系的姿态矩阵通过如下方式进行估计，C_n^b为导航坐标系n相对机体坐标系b的姿态矩阵，C_bⁿ为机体坐标系b相对导航坐标系n的姿态矩阵，重力加速度矢量和地球角速率矢量的变换式为gb=Cnbgn---(1)]]>Ωb=CnbΩn---(2)]]>定义第三个矢量V＝g×Ω，则其变换式为Vb=CnbVn---(3)]]>由于Cbn=(Cnb)-1=(Cnb)T,]]>则由式(1)、式(2)和式(3)的矢量关系得机体坐标系b相对导航坐标系n的姿态矩阵：Cbn=(gn)T(Ωn)T(Vn)T-1(gb)T(Ωb)T(Vb)T---(4)]]>则由步骤(A)和式(4)获得载体初始姿态矩阵C_bⁿ的估计值；(C)捷联惯性导航系统解算步骤利用步骤(A)得到的角速度和比力信息，以及由步骤(B)得到的载体初始姿态矩阵即式(4)，按捷联惯性导航算法的流程，解算出运载体的姿态角、航向角和速度信息，导航坐标系选择东北天地理坐标系，则机体坐标系相对于地理坐标系的角速度在机体系的三个轴向分量依次为绕横滚轴的角速度ω_x，绕俯仰轴的角速度ω_y，绕方位轴的角速度ω_z，计算周期为Δt，Δt时间内机体转过的角度Δθ的三个分量依次为横滚轴角增量Δθ_x，俯仰轴角增量Δθ_y，方位轴角增量Δθ_z，则转过的角度以矩阵方式表示为：Δθ=0-ΔθzΔθyΔθz0-Δθx-ΔθyΔθx0=0-ωzωyωz0-ωx-ωyωx0Δt---(5)]]>t时刻载体的姿态矩阵为：Cbn(t)=sinψsinθsinγ+cosψcosγsinψcosθ-sinψsinθcosγ+cosψsinγcosψsinθsinγ-sinψcosγcosψcosθ-cosψsinθcosγ-sinψsinγ-cosθsinγsinθcosθcosγ---(6)]]>式(6)中，γ为载体的横滚角，θ为载体的俯仰角，ψ为载体的航向角，那么，t+Δt时刻的姿态矩阵由Cbn=(t+Δt)=-Δθ·Cbn(t)]]>从t时刻的姿态矩阵递推求得，在姿态矩阵递推的初始时刻，即t＝0时，C_bⁿ(0)的值由步骤(B)中的式(4)确定，将C_bⁿ(t+Δt)简写为C11C12C13C21C22C23C31C32C33,]]>则t+Δt时刻，捷联惯性导航系统的横滚角γ_I、俯仰角θ_I和航向角ψ_I姿态信息由下式获得：γI=tg-1(C31C33)θI=sin-1(C32)ψI=tg-1(C12C22)---(7)]]>上述式中：C₁₁＝sinψsinθsinγ+cosψcosγ C₁₂＝sinψcosθ C₁₃＝-sinψsinθcosγ+cosψsinγC₂₁＝cosψsinθsinγ-sinψcosγ C₂₂＝cosψcosθ C₂₃＝-cosψsinθcosγ-sinψsinγC₃₁＝-cosθsinγ                C₃₂＝sinθ      C₃₃＝cosθcosγ在姿态解算的基础上，根据当前的姿态和比力，获得地理坐标系中的运动加速度a_E，a_N，a_U，其中，a_E是东向的运动加速度，a_N是北向的运动加速度，a_U是天向的运动加速度，运载体的速度由式(8)递推得到，v_E(t)表示t时刻运载体东向的速度，v_N(t)表示t时刻运载体北向的速度，v_U(t)表示t时刻运载体天向的速度，v_E(t+Δt)，v_N(t+Δt)，v_U(t+Δt)分别表示t+Δt时刻东向、北向和天向的速度，即：vE(t+Δt)=vE(t)+aEΔtvN(t+Δt)=vN(t)+aNΔtvU(t+Δt)=vU(t)+aUΔt---(8)]]>(D)捷联惯性导航系统精对准步骤，包括建立精对准系统的状态方程和观测方程，获取精对准系统的观测量，进行卡尔曼滤波状态估计，①建立精对准系统的状态方程和观测方程在东北天地理坐标系下，静基座捷联惯性导航系统初始对准的状态方程为：X·(t)=A(t)X(t)+G(t)W(t)---(9)]]>式(9)中，X(t)为t时刻系统的状态向量；A(t)，G(t)分别为系统状态转移矩阵和系统噪声系数矩阵；W(t)为系统的噪声向量，系统的状态向量为：X(t)＝[δv_E δv_N φ_E φ_N φ_U __x __y ε_x ε_y ε_z]^T    (10)系统的噪声向量为：W(t)＝[w_ax w_ay w_gx w_gy w_gz]^T    (11)式(10)和式(11)中，δv_E、δv_N表示系统东向和北向的速度误差，φ_E、φ_N、φ_U表示数学平台的东向、北向和方位失准角，__x、__y表示x轴和y轴加速度计的误差，ε_x、ε_y、ε_z表示三轴陀螺仪的误差；w_ax、w_ay表示x轴和y轴加速度计的测量白噪声，w_gx、w_gy、w_gz表示三轴陀螺仪的测量白噪声，系统的状态转移矩阵为：A(t)=02ΩU0-g0C11C12000-2ΩU0g00C21C22000000ΩU-ΩN00C11C12C1300-ΩU0000C21C22C2300ΩN0000C31C32C3300000000000000000000000000000000000000000000000000---(12)]]>系统的噪声系数矩阵为：G(t)=C11C12000C21C2200000C11C12C1300C21C22C2300C31C32C330000000000000000000000000---(13)]]>式(12)和式(13)中，Ω_N、Ω_U表示地球角速率矢量在地理坐标系北向和天向的分量，C_ij为机体坐标系相对地理坐标系的姿态矩阵C_bⁿ的第i行和第j列元素，即C₁₁＝sinψsinθsinγ+cosψcosγ C₁₂＝sinψcosθ C₁₃＝-sinψsinθcosγ+cosψsinγC₂₁＝cosψsinθsinγ-sinψcosγ C₂₂＝cosψcosθ C₂₃＝-cosψsinθcosγ-sinψsinγC₃₁＝-cosθsinγ                C₃₂＝sinθ      C₃₃＝cosθcosγ建立姿态角误差、航向角误差观测量对数学平台失准角的观测关系，实质上，两者存在着如下的转换关系：Cbn=CpnCbp---(14)]]>式(14)中，p代表数学平台坐标系，在东北天地理坐标系下有Cbn=sinψsinθsinγ+cosψcosγsinψcosθ-sinψsinθcosγ+cosψsinγcosψsinθsinγ-sinψcosγcosψcosθ-cosψsinθcosγ-sinψsinγ-cosθsinγsinθcosθcosγ---(15)]]>Cbp=sinψ′sinθ′sinγ′+cosψ′cosγ′sinψ′cosθ′-sinψ′sinθ′cosγ′+cosψ′sinγ′cosψ′sinθ′sinγ′-sinψ′cosγ′cosψ′cosθ′-cosψ′sinθ′cosγ′-sinψ′sinγ′-cosθ′sinγ′sinθ′cosθ′cosγ′---(16)]]>式(15)和式(16)中：γ、θ、ψ分别为运载体理想情况下的横滚角、俯仰角、航向角；γ′、θ′、ψ′分别为运载体实际情况下的横滚角、俯仰角和航向角，定义δγ、δθ和δψ分别为横滚角、俯仰角和航向角误差，则有如下的关系：δγ=γ′-γδθ=θ′-θδψ=ψ′-ψ---(17)]]>通过数学平台的失准角φ建立数学平台系与地理坐标系之间的方向余弦矩阵为：Cpn=1-φUφNφU1-φE-φNφE1---(18)]]>将式(15)、式(16)、式(18)代入式(14)，展开过程中代入式(17)并忽略δγ、δθ和δψ的二阶小量，令式(14)左右两端矩阵元素相等得：δγ=-sinψcosθ·φE-cosψcosθ·φNδθ=-cosψ·φE+sinψ·φNδψ=-tanθsinψ·φE-tanθcosψ·φN+φU---(19)]]>式(19)即为姿态角误差、航向角误差与数学平台失准角之间的观测关系，式(18)和式(19)中φ_E、φ_N、φ_U表示数学平台的东向、北向和方位失准角，视量测噪声为白噪声，建立系统的观测方程如下：Z(t)＝＝H(t)X(t)+V(t)    (20)式(20)中，Z(t)为t时刻系统的观测向量，H(t)为系统的观测矩阵，V(t)为观测白噪声向量，系统的观测向量为：Z(t)＝[δv_E δv_N δγ δθ δψ]^T    (21)系统的观测矩阵为：H(t)=1000000000010000000000-sinψ/cosθ-cosψ/cosθ00000000-cosψsinψ00000000-tanθsinψ-tanθcosψ100000---(22)]]>②获取精对准系统的观测量在静基座条件下，以步骤(C)中式(8)输出的惯性导航系统东向和北向的速度信息作为速度误差观测量，即δv_E＝v_E                  (23)δv_N＝v_N获取姿态误差观测量的特征在于利用加速度计倾角传感器测量原理估计运载体的姿态，即在地理坐标系下，加速度计输出的比力为fn=v·en+(2ωie+ωen)×ven-gn=-gn=00G---(24)]]>式(24)中的v_en为地理坐标系n相对于地球坐标系e的速度，为地理坐标系n相对于地球坐标系e的加速度，ω_en为地理坐标系n相对于地球坐标系e的角速度，g_n为地理坐标系下的重力加速度矢量，G为重量加速度矢量的大小，加速度计的实际测量输出为f_b，而fb=Cnbfn---(25)]]>将式(24)代入式(25)计算横滚角γ_D和俯仰角θ_D得：γD=-arctan(fxfz),]]>θD=arcsin(fyg)---(26)]]>式(26)中的f_x，f_y，f_z为加速度计的实际测量输出f_b的三个轴向分量，则以步骤(C)输出的惯性导航系统姿态即式(7)和加速度计估计的姿态即式(26)两者之差作为姿态误差观测量，即δγ=γI-γDδθ=θI-θD---(27)]]>以步骤(C)输出的惯性导航系统航向ψ_I和外部磁传感器输出的航向ψ_M两者之差作为航向误差观测量，即δψ＝ψ_I-ψ_M                         (28)综合式(23)、式(27)和式(28)，得该精对准系统的观测量为：Z(t)=δvEδvNδγδθδψ=vEvNγI-γDθI-θDψI-ψM---(29)]]>③进行卡尔曼滤波状态估计：在步骤①和②基础上利用标准的卡尔曼滤波方程进行迭代计算，对式(10)中各状态量进行估计；(E)滤波输出及修正步骤：在步骤(C)获得的运载体姿态角、速度信息中，扣除步骤(D)估计出的系统速度误差、姿态角误差，得到滤波输出的姿态角、速度导航参数，同时将步骤(A)输出的角速度和比力信息扣除步骤(D)估计出的加速度计误差和陀螺仪误差作为步骤(C)的输入信号；(F)精对准完成步骤：重复步骤(A)、(C)、(D)、(E)进行迭代计算，直至滤波器收敛，由步骤(D)滤波输出的姿态角和航向角建立精确的初始姿态矩阵，同时即实现了航向的对准。