CN101413800A - 导航/稳瞄一体化系统的导航、稳瞄方法 - Google Patents

Abstract

一种导航/稳瞄一体化系统的导航、稳瞄方法，属惯性导航领域。该导航方法是利用安装在光电稳瞄平台上的惯性测量单元实现载体定姿、定位。具体步骤是：采集惯性测量单元的信号；稳瞄平台下的快速精确初始对准；基于车载稳瞄平台的惯性导航算法；车辆航姿信息解算及误差分析；惯导系统与惯性器件的误差分析、建模与补偿；惯导/里程仪/GPS多信息容错组合导航。本导航方法改进了已有技术中稳瞄和导航系统不能同时工作，以及一体化系统无法提供全面导航信息的不足，能为载体(战车)提供实时、精确、完整的导航姿态信息和导航定位信息，能提高战车的战场生存能力和综合作战能力。

Description

导航/稳瞄一体化系统的导航、稳瞄方法

技术领域

本发明是一种应用于战车导航/稳瞄一体化系统的导航定位、定姿方法，属于惯性导航领域。

背景技术

瞄准线稳定系统和惯性导航系统是近年来广泛应用于坦克等地面战车的两种高精度惯性系统。瞄准线稳定系统主要组成部分为光电稳瞄平台，通过它稳定瞄准线，为战车在行进中提供精确的射击或制导基准，增强了战车的机动性；惯导系统能适时提供载体姿态、速度以及所在位置，引导坦克隐蔽进入战场与打击目标，其自主性是其他定位定向系统无法比拟的。

瞄准线稳定系统主要组成部分为光电稳瞄平台，光电稳瞄平台的功能是捕获、跟踪、瞄准战车所处地域内的目标，如车辆、房屋等。平台主要由光电探测器、跟踪伺服平台、电子设备和惯性元件组成。其中光电探测器一般包括红外成像、激光测距、电视成像系统；跟踪伺服平台是多轴多环架伺服系统，光电探测器安装在平台的内环；惯性元件由陀螺仪、加速度计等组成，用来在车载环境下建立惯性基准。

车载惯性导航系统(Strapdown Inertial Navigation System)是一种基于牛顿运动定律以推算的方式实现导航功能的系统，其核心传感器是由陀螺仪(角运动传感器)和加速度计(线运动传感器)等两类惯性传感器构成的测量单元，由上述两类传感器构成的测量单元直接固联在战车稳瞄平台上。惯性导航系统中的处理器通过模拟/数字转换电路或者直接采用数字接口，读取上述测量单元内角运动和线运动传感器的数据，按惯性导航算法的原理流程，对原始的测量数据进行加工，解算出运动载体的姿态、速度和位置等参数。

针对未来作战环境战车装备的潜在要求，有展开信息感知和火控稳瞄综合运用的强烈需求，但由于涉及到惯性器件、精密光机结构、高精度伺服和复杂的解析算法，在我国现有装备的系统中，稳瞄、导航这两套系统只能分时使用，即稳瞄和导航不能同时工作，且精度不高。国内有稳瞄/寻北一体化系统的相关技术研究，但该系统只能解算车体航向，无法提供全方位的姿态信息以及速度、位置信息，即无法进行完整的导航工作。

惯性导航系统一般在静基座下进行系统初始化，初始化过程中姿态对准特别是航向失准角的精度与对准速度难以满足要求；车载稳瞄平台上的惯性导航系统算法，在光电稳瞄平台进行瞄准线稳定的过程中，解算机理类似于平台惯导系统，而在稳瞄平台进行目标跟踪与人工调转过程中，惯性传感器构成的测量单元跟随稳瞄平台一起转动，此过程中惯性导航系统解算机理类似于捷联惯导，目前常规的平台惯导与捷联惯导解算机理均不适用于车载稳瞄平台上的惯性导航系统，惯导系统与惯性器件的误差分析、建模、补偿方式也需要进一步改进。

发明内容

本发明的主要目的是，改进已有方案中稳瞄和导航系统不能同时工作，以及一体化系统无法提供全面导航信息的不足，在随机旋转的光电稳瞄平台上，安装光纤捷联惯性测量单元，通过提取惯性测量单元的输出信息，结合车载测角机构与导航计算机，探索适用于导航/稳瞄一体化系统的惯导解算方法的新途径，为载体提供实时、精确、完整的导航定位信息。

本发明的内容是导航/稳瞄一体化系统通过光电稳瞄平台上安装完整的惯性测量单元实现载体定姿、定位的方法，其特点通过以下步骤实现：

惯性测量单元信号采集步骤：采集惯性测量单元中光纤陀螺与MEMS加速度计的输出信号，得到惯导系统的角速度和比力；

稳瞄平台下的快速精确初始对准步骤：利用稳瞄平台具有的随机调转特性，设计了一种快速而精确的转动基座初始对准方法，即在稳瞄平台绕其航向轴连续转动的条件下，建立系统状态方程 $\stackrel{.}{X}=\mathrm{AX}+W$ ，误差状态量 $X=\left[\begin{array}{cccccccccc}{\mathrm{\δV}}_N& {\mathrm{\δV}}_E& {\mathrm{\φ}}_N& {\mathrm{\φ}}_E& {\mathrm{\φ}}_D& {\▿}_x& {\▿}_y& {\mathrm{\ϵ}}_x& {\mathrm{\ϵ}}_y& {\mathrm{\ϵ}}_z\end{array}\right],$ 其中δV_N、δV_E为水平速度误差沿北、东方向的分量，φ_N、φ_E、φ_D为平台误差角在北、东、地方向的分量；为加速度计的零偏；ε_x、ε_y、ε_z为陀螺漂移状态转移矩阵，状态转移矩阵 $A=\left[\begin{array}{cc}F& T\\ 0& 0\end{array}\right]$ ，其中 $F=\left[\begin{array}{ccccc}0& 2{\mathrm{\Ω}}_D& 0& g& 0\\ -2{\mathrm{\Ω}}_D& 0& -g& 0& 0\\ 0& 0& 0& {\mathrm{\Ω}}_D& 0\\ 0& 0& -{\mathrm{\Ω}}_D& 0& {\mathrm{\Ω}}_N\\ 0& 0& 0& -{\mathrm{\Ω}}_N& 0\end{array}\right],$ $T=\left[\begin{array}{ccccc}{c}_{11}\′& c_{12}\′& 0& 0& 0\\ c_{21}\′& c_{22}\′& 0& 0& 0\\ 0& 0& c_{11}\′& c_{12}\′& c_{13}\′\\ 0& 0& c_{21}\′& c_{22}\′& c_{23}\′\\ 0& 0& c_{31}\′& c_{32}\′& c_{33}\′\end{array}\right];$ Ω_D，Ω_N为地球自转角速度在地向与北向的分量，c₁₁′＝cosγcosφ+sinγsinθsinφ、c₁₂′＝cosθsinφ、c₁₃′-sinγcosφ-cosγsinθsinφ、c₂₁′＝-cosγsinφ+sinγsinθcosφ、c₂₂′＝cosθcosφ、c₂₃′＝-sinγsinφ-cosγsinθcosφ、c₃₁′＝-sinγcosθ、c₃₂′＝sinθ、c₃₃′＝cosγcosθ分别为

中的各元素，γ为平台横滚角、θ为平台俯仰角、φ为平台航向角，

为平台姿态转移矩阵

的转置矩阵，

由横滚角γ、俯仰角θ、航向角φ的三角函数构成，即 $C_n^b=\left[\begin{array}{ccc}\cos \mathrm{\γ}\cos \mathrm{\φ}+\sin \mathrm{\γ}\sin \mathrm{\θ}\sin \mathrm{\φ}& -\cos \mathrm{\γ}\sin \mathrm{\φ}+\sin \mathrm{\γ}\sin \mathrm{\θ}\cos \mathrm{\φ}& -\sin \mathrm{\γ}\cos \mathrm{\θ}\\ \cos \mathrm{\θ}\sin \mathrm{\φ}& \cos \mathrm{\θ}\cos \mathrm{\φ}& \sin \mathrm{\θ}\\ \sin \mathrm{\γ}\cos \mathrm{\φ}-\cos \mathrm{\γ}\sin \mathrm{\θ}\sin \mathrm{\φ}& -\sin \mathrm{\γ}\sin \mathrm{\φ}-\cos \mathrm{\γ}\sin \mathrm{\θ}\cos \mathrm{\φ}& \cos \mathrm{\γ}\cos \mathrm{\θ}\end{array}\right],$ g为当地重力加速度，W＝[w₁ w₂ w₃ w₄ w₅]，式中w₁、w₂为等效北向、东向的加速度计白噪声分量，w₃、w₄、w₅为等效北向、东向、地向的陀螺仪白噪声分量，再建立系统观测方程 $Z=\left[\begin{array}{c}{\mathrm{\δV}}_N\\ {\mathrm{\δV}}_E\end{array}\right]=\left[\begin{array}{cccccccccc}1& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 1& 0& 0& 0& 0& 0& 0& 0& 0\end{array}\right]X+V$ ，式中δV_N、δV_E分别为水平速度误差沿北、东方向的分量，X定义同上，V为观测噪声矢量，通过增加东向与北向加速度计输出f_N、f_E作为观测量，扩展观测方程为 $Z=\left[\begin{array}{cccccccccc}1& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 1& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& -g& 0& c_{11}\′& c_{12}\′& 0& 0& 0\\ 0& 0& g& 0& 0& c_{21}\′& c_{22}\′& 0& 0& 0\end{array}\right]X+V\′=\mathrm{HX}+V\′,$ 式中 $Z=\left[\begin{array}{c}{\mathrm{\δV}}_N\\ {\mathrm{\δV}}_E\\ f_N\\ f_E\end{array}\right],$ X定义同上，V′为观测量扩展后的观测噪声矢量，c₁₁′、c₁₂′、c₂₁′、c₂₂′定义同上，运用卡尔曼滤波器，快速估计惯导系统的初始平台误差角φ_N、φ_E、φ_D并反馈给导航系统进行补偿，得到惯导系统精确的初始横滚角、俯仰角、航向角信息；

基于车载稳瞄平台的惯性导航解算步骤：该步骤由高精度姿态解算与速度、位置解算组成，惯导系统姿态解算首先利用公式 ${\mathrm{\ω}}_{\mathrm{nb}}^b={\mathrm{\ω}}_{\mathrm{ib}}^b-{\mathrm{\ω}}_{\mathrm{in}}^b$ 计算机体相对导航坐标系角速度

，式中

为稳瞄平台上陀螺仪的直接输出，

可通过车体速度在导航下投影、地球自转角速度ω_ie以及上个周期的姿态转移矩阵

求得，然后采用四元数微分方程 $q(t+1)=\{\cos \frac{{\mathrm{\Δ\Φ}}_0}{2}I+\frac{\sin \frac{{\mathrm{\Δ\Φ}}_0}{2}}{{\mathrm{\Δ\Φ}}_0}\left[\mathrm{\Δ\Φ}\right]\}q\left(t\right)$ 求解姿态转移矩阵

对应的的四元数，式中 $q=q_0+q_1\stackrel{\→}{i}+q_2\stackrel{\→}{j}+q_3\stackrel{\→}{k},$ l为四元数计算的时间间隔， $\mathrm{\Δ\Φ}=\mathrm{\Δ\θ}+\frac{7}{45}{h}^2{\mathrm{\ω}}_1\×{\mathrm{\ω}}_2+\frac{1}{180}{h}^2{\mathrm{\ω}}_1\×{\mathrm{\ω}}_3,$ ω₁、ω₂、ω₃分别为在一次姿态解算周期内对陀螺仪输出进行3次采样的角速度，即采用3子样采样方法补偿陀螺非定轴转动时，角速度矢量积分产生的计算误差。姿态更新周期h＝0.02s， $\mathrm{\Δ\θ}=\left[\begin{array}{c}{\mathrm{\Δ\θ}}_x\\ {\mathrm{\Δ\θ}}_y\\ {\mathrm{\Δ\θ}}_z\end{array}\right]={\∫}_t^{t+h}\left[\begin{array}{c}{\mathrm{\ω}}_{\mathrm{nbx}}^b\\ {\mathrm{\ω}}_{\mathrm{nby}}^b\\ {\mathrm{\ω}}_{\mathrm{nbz}}^b\end{array}\right]\mathrm{dt},$ $\left[\mathrm{\Δ\Φ}\right]=\left[\begin{array}{cccc}0& -\mathrm{\Δ}{\mathrm{\Φ}}_x& -{\mathrm{\Δ\Φ}}_y& -{\mathrm{\Δ\Φ}}_z\\ {\mathrm{\Δ\Φ}}_x& 0& {\mathrm{\Δ\Φ}}_z& -{\mathrm{\Δ\Φ}}_y\\ {\mathrm{\Δ\Φ}}_y& -{\mathrm{\Δ\Φ}}_z& 0& {\mathrm{\Δ\Φ}}_x\\ {\mathrm{\Δ\Φ}}_z& {\mathrm{\Δ\Φ}}_y& -{\mathrm{\Δ\Φ}}_x& 0\end{array}\right],$ 将得到的四元数利用公式 $q_i=\frac{q_i^{\′}}{{\left(\underset{j=0}{\overset{3}{\mathrm{\Σ}}}{q}_j^{\′2}\right)}^{1/2}}$ i＝0，1，2，3进行规范化，式中 $q\′=q_0^{\′}+q_1^{\′}\stackrel{\→}{i}+q_2^{\′}\stackrel{\→}{j}+q_3^{\′}\stackrel{\→}{k}$ 为直接由四元数微分方程计算得到的四元数，而设规范化后的四元数为 $q=q_0+q_1\stackrel{\→}{i}+q_2\stackrel{\→}{j}+q_3\stackrel{\→}{k},$ 其中为相互正交的单位食量，然后将规范化后的四元数通过公式 $C_n^b=\left[\begin{array}{ccc}{q}_1^2+q_0^2-q_3^2-q_2^2& 2(q_1{q}_2+q_0{q}_3)& 2(q_1{q}_3-q_0{q}_2)\\ 2(q_1{q}_2-q_0{q}_3)& q_2^2-q_3^2+q_0^2-q_1^2& 2(q_2{q}_3+q_0{q}_1)\\ 2(q_1{q}_3+q_0{q}_2)& 2(q_2{q}_3-q_0{q}_1)& q_3^2-q_2^2-q_1^2+q_0^2\end{array}\right]$ 生成姿态转移矩阵

定义同上，最后利用公式 $\mathrm{\θ}=\arctan \left(\frac{{c\′}_{23}}{\sqrt{c_{21}^{\′2}+c_{22}^{\′2}}}\right),$ $\mathrm{\γ}=\arctan (-\frac{{c\′}_{13}}{{c\′}_{33}}),$ $\mathrm{\φ}=\arctan \left(\frac{{c\′}_{21}}{{c\′}_{22}}\right)$ 从

中提取横滚角γ、俯仰角θ、航向角φ信息，其中c′₁₁、c′₁₂、c′₁₃、c′₂₁、c′₂₂、c′₂₃、c′₃₁、c′₃₂、c′₃₃分别为姿态转移矩阵

的各元素，定义同上。惯导系统(即车辆)速度、位置解算方法则将加速度计输出通过平台

的转置矩阵

从载体坐标系转换到导航坐标系，并通过解算比力微分方程 $f=\stackrel{\·}{v_{\mathrm{ep}}}+(2{\mathrm{\ω}}_{\mathrm{ie}}+{\mathrm{\ω}}_{\mathrm{ep}})\×v_{\mathrm{ep}}-g$ 得到导航系统速度v_ep，再将导航系统速度v_ep经过一次积分得到导航系统位置，式中f为加速度计输出，ω_ie为地球自转角速度，ω_ep为稳瞄平台相对于地球的角速度；

车辆航姿信息解算及误差分析步骤：借助稳定平台的测角元件和炮塔转角测量元件，测量出平台相对于炮塔姿态转移矩阵

$C_B^b=\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos {\mathrm{\λ}}_P& \sin {\mathrm{\λ}}_P\\ 0& -\sin {\mathrm{\λ}}_P& \cos {\mathrm{\λ}}_P\end{array}\right]\left[\begin{array}{ccc}\cos {\mathrm{\θ}}_P& 0& -\sin {\mathrm{\θ}}_P\\ 0& 1& 0\\ \sin {\mathrm{\θ}}_P& 0& \cos {\mathrm{\θ}}_P\end{array}\right]\left[\begin{array}{ccc}\cos {\mathrm{\ψ}}_P& \sin {\mathrm{\ψ}}_P& 0\\ -\sin {\mathrm{\ψ}}_P& \cos {\mathrm{\ψ}}_P& 0\\ 0& 0& 1\end{array}\right],$ ψ_P，θ_P，γ_P为稳瞄平台相对于炮塔的三个转角；炮塔相对于车辆的姿态转移矩阵

$C_V^B=\left[\begin{array}{ccc}\cos {\mathrm{\ψ}}_{\mathrm{VB}}& \sin {\mathrm{\ψ}}_{\mathrm{VB}}& 0\\ -\sin {\mathrm{\ψ}}_{\mathrm{VB}}& \cos {\mathrm{\ψ}}_{\mathrm{VB}}& 0\\ 0& 0& 1\end{array}\right],$ ψ_VB为炮塔相对于车体的航向角；同时稳定平台上的惯导系统输出上述平台的姿态转移矩阵

利用上述平台相对于炮塔姿态转移矩阵、炮塔相对于车辆的姿态转移矩阵

、平台的姿态转移矩阵三个矩阵信息可以解算出车辆的横滚角、俯仰角、航向角信息，并对姿态误差进行分析与修正；

(5)惯导系统和惯性器件的误差分析、建模和补偿步骤：惯导系统的误差分为数学平台误差、速度误差和位置误差，数学平台误差模型为 $\left\{\begin{array}{c}{\stackrel{.}{\mathrm{\φ}}}_n=\frac{\mathrm{\δ}{V}_e}{R_N+h}-{\mathrm{\ω}}_{\mathrm{ie}}\sin \mathrm{L\δL}-({\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{V_e}{R_N+h}\mathrm{tgL}){\mathrm{\φ}}_e+\frac{V_n}{R_M+h}{\mathrm{\φ}}_d-{\mathrm{\ϵ}}_n^n\\ {\stackrel{.}{\mathrm{\φ}}}_e=-\frac{\mathrm{\δ}{V}_n}{R_M+h}+({\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{V_e}{R_N+h}\mathrm{tgL}){\mathrm{\φ}}_n+({\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_e}{R_N+h}){\mathrm{\φ}}_d-{\mathrm{\ϵ}}_e^n\\ {\stackrel{.}{\mathrm{\φ}}}_d=-\frac{\mathrm{\δ}{V}_e}{R_N+h}\mathrm{tgL}-({\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_e}{R_N+h}{\sec }^{2}L)\mathrm{\δL}-({\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_e}{R_e}){\mathrm{\φ}}_e-\frac{V_n}{R_M+h}{\mathrm{\φ}}_n-{\mathrm{\ϵ}}_d^n\end{array}\right.$ ，式中φ_n、φ_e和φ_d分别为北向、东向、地向的平台误差角；v_e、v_n分别为东向与北向速度；ω_ie为地球自转角速度，L为当地纬度，δL为纬度误差，R_M为地球子午圈半径，R_N为地球卯酉圈半径，h为当地海拔高度，

分别为北向、东向和地向陀螺仪噪声，速度误差模型为 $\left\{\begin{array}{c}\mathrm{\δ}{\stackrel{.}{V}}_n=-f_d{\mathrm{\φ}}_e+f_e{\mathrm{\φ}}_d+\frac{V_d}{R_M+h}\mathrm{\δ}{V}_n-(2{\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{V_e}{R_N+h}\mathrm{tgL})\mathrm{\δ}{V}_e+\frac{V_n}{R_M+h}\mathrm{\δ}{V}_d\\ -(2{\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_e}{R_N+h}{\sec }^{2}L)V_e\mathrm{\δL}+{\▿}_n\\ \mathrm{\δ}{\stackrel{.}{V}}_e=f_d{\mathrm{\φ}}_n+(2{\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{V_e}{R_N+h}\mathrm{tgL})\mathrm{\δ}{V}_n+(\frac{V_n}{R_M+h}\mathrm{tgL}+\frac{V_d}{R_M+h})\mathrm{\δ}{V}_e+(2{\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_e}{R_N+h})\mathrm{\δ}{V}_d\\ +(2{\mathrm{\ω}}_{\mathrm{ie}}{V}_n\cos L+\frac{V_e}{R_N+h}{V}_n{\sec }^{2}L-2{\mathrm{\ω}}_{\mathrm{ie}}{V}_d\sin L)\mathrm{\δL}-2{\mathrm{\ω}}_{\mathrm{ie}}{V}_d\sin L)\mathrm{\δL}-f_n{\mathrm{\φ}}_d+{\▿}_e\\ \mathrm{\δ}{\stackrel{.}{V}}_d=-f_e{\mathrm{\φ}}_n+f_n{\mathrm{\φ}}_e\frac{2V_n}{R_M+h}\mathrm{\δ}{V}_n-2({\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_e}{R_N+h})\mathrm{\δ}{V}_e\\ +2V_e{\mathrm{\ω}}_{\mathrm{ie}}\sin \mathrm{L\δL}-\frac{2g}{R_M+h}\mathrm{\δh}+{\▿}_d\end{array}\right.$ ，式中δV_n、δV_e、δV_d为北向、东向、地向的速度误差，f_n、f_e、f_d分别为北向、东向、地向的加速度计输出，δh为海拔高度误差；

分别为北向、东向、天向加速度计误差，其余变量定义同上， $\mathrm{\δ}\stackrel{.}{L}=\mathrm{\δ}{V}_n/(R_M+h)$ 位置误差方程为 $\mathrm{\δ}\stackrel{.}{\mathrm{\λ}}=\mathrm{\δ}{V}_e/(R_N+h)\cos L+V_e/(R_N+h)\sec \mathrm{LtgL\δL}$ ，式中 $\mathrm{\δ}\stackrel{.}{h}=-\mathrm{\δ}{V}_d$ δλ为经度误差，其他变量定义同上，惯性器件误差则是一类强非线性误差，陀螺漂移误差考虑主要由三部分组成：ε^b＝ω_g+ε_c+ε_r，式中ω_g为随机白噪声漂移，其均方差为σ_g，ε_c为随机常值漂移，ε_r为随机一阶马尔柯夫过程漂移，设三个轴向的陀螺误差模型相同，均为： $\left.\begin{array}{c}{\stackrel{.}{\mathrm{\ϵ}}}_c=0\\ {\stackrel{.}{\mathrm{\ϵ}}}_r=-\frac{1}{T_r}{\mathrm{\ϵ}}_r+{\mathrm{\ω}}_r\end{array}\right\},$ 式中T_r为相关时间，ω_r是均方差为σ_r的马尔柯夫过程驱动白噪声，加速度计误差模型

一般考虑为一阶马尔柯夫过程，且三个轴向的加速度计误差模型相同： ${\stackrel{.}{\▿}}_{\mathrm{\α}}=-\frac{1}{T_a}{\▿}_{\mathrm{\α}}+{\mathrm{\ω}}_{\mathrm{\α}},$ ，式中T_a是马尔柯夫过程的相关时间，ω_a是均方差为σ_a的驱动白噪声，本发明采用里程仪(或GPS)与惯导之间的速度误差、位置误差作为观测量，以卡尔曼滤波的方式对姿态、速度、位置等系统误差以及陀螺仪漂移、加速度计偏置等惯性器件误差进行零均值最小方差估计，通过闭环反馈修正进行误差补偿；

惯导/里程仪/GPS多信息容错组合导航步骤：该步骤采用两种方案实现，第一种方案采用间断地利用GPS信息对惯导/里程仪组合导航系统进行重调，首先构建一套捷联惯导与里程仪进行速度/位置组合的组合导航系统，实时输出位置与速度信息，在GPS信号可用时，将GPS的位置、速度信息对捷联惯导/里程仪组合导航系统输出进行校正，提高整体导航精度。经过一次校正，间隔一定时间后继续运用上述方法校正系统；第二种方案采用惯导/里程仪/GPS多信息组合联邦滤波结构，该方案将捷联惯导与里程仪进行了位置速度组合滤波解算，同时将捷联惯导与GPS进行了位置、速度组合滤波解算，最后将两个子滤波器的误差状态量估计值经过主滤波器加权平均，求得误差状态量的零均值最小方差估计，并反馈至捷联惯导系统进行闭环校正，提高惯导系统导航定位精度。

说明书附图

图1是导航/稳瞄一体化系统结构原理图。

图2是捷联式惯导系统原理。

图3捷联惯导系统的算法流程。图中γ₀、θ₀、ψ₀分别为稳瞄平台的初始横滚角、初始俯仰角与初始航向角，Q(Λ₀)为初始化四元数，

为初始化姿态转移矩阵的转置矩阵，

为陀螺仪输出，

为导航坐标系相对于惯性坐标系的角速度，Q(Λ_k)为前一时刻的四元数，Q(Λ_k+1)为当前时刻的四元数，

为当前时刻姿态转移矩阵的转置矩阵，γ、θ、ψ分别为当前时刻平台的横滚角、俯仰角以及航向角，

为加速度计输出，为加速度计输出在导航坐标系中的投影，L(0)、λ(0)、h(0)、Vt(0)分别为上一时刻载体的纬度、经度、高度与速度，

为当前时刻导航坐标系相对于惯性坐标系的角速度在导航坐标系中的投影，L、λ、h、V_t分别为当前时刻载体的纬度、经度、高度以及速度。

图4GPS间断对惯导/里程仪组合导航系统重调方式结构图。

图5惯导/里程仪/GPS多信息组合联邦滤波结构图

具体实施方式

传统的车载一体化系统中，或无法提供全方位的姿态信息以及速度、位置信息，或导航系统与稳瞄系统无法同时工作。本发明在传统的一体化系统基础上进行改进，如图1所示，将惯性测量组件(IMU)安装在随机调转的光电稳瞄平台上，IMU由3个正交安装的光纤陀螺与3个正交安装的加速度计组成，稳瞄平台具有3自由度的转动功能，IMU中的陀螺信号输出给平台稳定回路，以实现对稳瞄平台稳定的功能。同时，IMU中的陀螺仪和加速度计的信号输出给惯导解算部分解算出稳瞄平台的姿态、速度和位置信息，并适时与里程仪与GPS进行组合导航。利用平台测角机构测量出平台和车体之间的角位置，并利用惯导输出的稳瞄平台的姿态角信息，进一步解算出车辆的的姿态信息。

为了完成导航/稳瞄一体化系统的精确定位与测姿，需要完成工作：

5.1　惯性测量单元信号采集步骤

采集惯性测量单元中光纤陀螺与MEMS加速度计的输出信号，得到惯导系统的角速度和比力。

5.2　稳瞄平台下的快速精确初始对准步骤

在稳瞄控制单元的输入口处，设立一个转换开关，当综合体需要初始对准时，该开关拨向导航单元，惯导控制单元根据IMU检测的地球自转和重力加速度变化信息，判断概略稳瞄平台的姿态和航向，以此为依据，向稳瞄控制单元发出旋转指令，后者立即控制伺服电路将平台旋转和调平，实现快速初始粗对准。

瞄平台的水平对准精度较高，初始精确对准的关键是方位对准。由于稳瞄平台可以相对车体在方位上转动，本发明提出了一种采用转动稳瞄准平台的快速初始精对准方法，该方法通过以一定角速度连续转动稳瞄平台的方位轴，以使整个初始精对准在航向角不断改变的过程中完成，从而在提高精度的同时又提高了对准的速度。这种方法的理论分析如下所示。

采用北-东-地地理坐标系为导航坐标系，惯导系统初始对准误差模型的状态方程为：

$\stackrel{.}{X}=\mathrm{Ax}+W---\left(1\right)$

式中，误差状态量 $X=\left[\begin{array}{cccccccccc}{\mathrm{\δV}}_N& {\mathrm{\δV}}_E& {\mathrm{\φ}}_N& {\mathrm{\φ}}_E& {\mathrm{\φ}}_D& {\▿}_x& {\▿}_y& {\mathrm{\ϵ}}_x& {\mathrm{\ϵ}}_y& {\mathrm{\ϵ}}_z\end{array}\right],$

$F=\left[\begin{array}{ccccc}0& 2{\mathrm{\Ω}}_D& 0& g& 0\\ -2{\mathrm{\Ω}}_D& 0& -g& 0& 0\\ 0& 0& 0& {\mathrm{\Ω}}_D& 0\\ 0& 0& -{\mathrm{\Ω}}_D& 0& {\mathrm{\Ω}}_N\\ 0& 0& 0& -{\mathrm{\Ω}}_N& 0\end{array}\right],$ $T=\left[\begin{array}{ccccc}{c}_{11}\′& c_{12}\′& 0& 0& 0\\ c_{21}\′& c_{22}\′& 0& 0& 0\\ 0& 0& c_{11}\′& c_{12}\′& c_{13}\′\\ 0& 0& c_{21}\′& c_{22}\′& c_{23}\′\\ 0& 0& c_{31}\′& c_{32}\′& c_{33}\′\end{array}\right];$

δV_N、δV_E为水平速度误差沿北、东方向的分量；φ_N、φ_E、φ_D为平台误差角在北、东、地方向的分量；

为加速度计的零偏；ε_x、ε_y、ε_z为陀螺漂移；Ω_D，Ω_N为地球自转角速度在地向与北向的分量c′₁₁、c′₁₂、c′₁₃、c′₂₁、c′₂₂、c′₂₃、c′₃₁、c′₃₂、c′₃₃为姿态转移矩阵的转置矩阵的各元素，c₁₁′＝cosγcosφ+sinγsinθsinφ、c₁₂′＝cosθsinφ、c₁₃′-sinγcosφ-cosγsinθsinφ、c₂₁′＝-cosγsinφ+sinγsinθcosφ、c₂₂′＝cosθcosφ、c₂₃′＝-sinγsinφ-cosγsinθcosφ、c₃₁′＝-sinγcosθ、c₃₂′＝sinθ、c₃₃′＝cosγcosθ，γ为平台的横滚角，θ为平台的俯仰角，φ为平台航向角，姿态转移矩阵 $C_n^b=\left[\begin{array}{ccc}\cos \mathrm{\γ}\cos \mathrm{\φ}+\sin \mathrm{\γ}\sin \mathrm{\θ}\sin \mathrm{\φ}& -\cos \mathrm{\γ}\sin \mathrm{\φ}+\sin \mathrm{\γ}\sin \mathrm{\θ}\cos \mathrm{\φ}& -\sin \mathrm{\γ}\cos \mathrm{\θ}\\ \cos \mathrm{\θ}\sin \mathrm{\φ}& \cos \mathrm{\θ}\cos \mathrm{\φ}& \sin \mathrm{\θ}\\ \sin \mathrm{\γ}\cos \mathrm{\φ}-\cos \mathrm{\γ}\sin \mathrm{\θ}\sin \mathrm{\φ}& -\sin \mathrm{\γ}\sin \mathrm{\φ}-\cos \mathrm{\γ}\sin \mathrm{\θ}\cos \mathrm{\φ}& \cos \mathrm{\γ}\cos \mathrm{\θ}\end{array}\right],$ g为当地重力加速度，W＝[w₁ w₂ w₃ w₄ w₅]，w₁、w₂为等效北向、东向的加速度计白噪声分量，w₃、w₄、w₅为等效北向、东向、地向的陀螺仪白噪声分量。

应用卡尔曼滤波器进行状态矢量的零均值最小方差估计，建立系统观测方程。首先选取两个水平速度误差δV_N、δV_E作为观测量，所建立的系统观测方程为：

$Z=\left[\begin{array}{c}{\mathrm{\δV}}_N\\ {\mathrm{\δV}}_E\end{array}\right]=\left[\begin{array}{cccccccccc}1& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 1& 0& 0& 0& 0& 0& 0& 0& 0\end{array}\right]X+V---\left(2\right)$

其中，δV_N、δV_E、X的定义同上。V是系统观测噪声矢量，为高斯白噪声过程。

初步仿真中发现，基于转动基座的初始对准精度受到加速度计偏置影响较大，为了提高加速度计随机常值偏置的估计精度，利用关系式：

$f_E=-g{\mathrm{\φ}}_N+{\▿}_E,$ $f_N=g{\mathrm{\φ}}_E+{\▿}_N---\left(3\right)$

将观测方程扩展为“速度+比力”匹配方式。其中f_E和f_N分别为等效东向和北向加速度计输出，可利用稳瞄平台加速度计输出并通过

转换得到，φ_N和φ_E的定义同上，

与为等效东向和北向加速度计误差，从而观测方程可扩展为

$Z=\left[\begin{array}{cccccccccc}1& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 1& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& -g& 0& c_{11}\′& c_{12}\′& 0& 0& 0\\ 0& 0& g& 0& 0& c_{21}\′& c_{22}\′& 0& 0& 0\end{array}\right]X+V\′=\mathrm{HX}+V\′---\left(4\right)$

式中，Z为两个水平速度误差δV_N、δV_E，与等效东向和北向加速度计输出f_E和f_N组成的观测矢量，V'为原观测噪声V与等效东向与北向加速度计白噪声分量组成的总测噪声，也考虑为高斯白噪声过程，服从正态分布。对上述系统误差方程与观测方程，应用线性Kalman滤波器对姿态失准角与惯性器件误差进行最优估计与补偿。

5.3　基于车载稳瞄平台的惯性导航解算步骤

导航稳瞄一体化系统将IMU放在稳瞄平台上，稳瞄平台在控制回路的作用下按照要求进行三维的角运动，同时稳瞄平台随着车辆的运动进行三维的线运动，其中车辆三维的角运动被稳瞄平台的框架所隔离。因此，可以将该系统近似看作是一个装载在稳瞄平台上的捷联惯导系统，而捷联惯导系统的载体是稳瞄平台，通过捷联惯性导航算法可以解算出稳瞄平台的姿态转移矩阵及其在地理坐标系下的速度和位置。由于稳瞄平台和载车之间只有角运动，因此，通过稳瞄平台上的惯导系统解算出来的稳瞄平台在地理坐标系下的速度和位置就是载车在同一坐标系下的速度和位置。由此可以实现对车辆的速度测量和导航定位。

5.3.1　捷联式惯导系统基本原理

由于稳瞄平台上的惯导系统近似于捷联式惯导系统，其基本原理如图2所示，该原理图主要突出表明了数学平台在捷联导航系统中的作用和关系。在捷联惯导系统中把陀螺仪和加速度计的组合体通常称为惯性组件(IMU—InertialMeasuring Unit)。三轴陀螺仪和加速度计的指向安装时要保持严格正交，该组件直接安装在载体上时也要保持与机体坐标系完全一致。同时还可以看到，IMU对捷联惯导系统而言是开环式的，仅起到了惯性传感器信号输入的作用，不需要任何信号再对IMU进行反馈控制，所有的信号处理也都在计算机内实现，因此工程实现方便。

捷联惯导系统的算法主要包括姿态转移矩阵的计算(即数学平台的计算部分)和导航计算(包括位置与速度的计算)两部分组成，其中姿态转移矩阵的计算是捷联惯性导航系统算法的核心。由于四元素姿态算法具有运算量小和全姿态解算的优点，因此，一般采用四元素法进行姿态转移矩阵的解算。

5.3.2　基于车载稳瞄平台的捷联式惯导系统高精度姿态解算

导航/稳瞄一体化系统的特征是利用同一个IMU同时实现导航和稳瞄功能，因此需要将IMU安装在稳瞄平台上。虽然稳瞄平台可以将车辆的颠簸抖动隔离掉，但是稳瞄平台在其搜索过程中，具有很高的机动性，角速度可达400度/秒。因此，要保证高动态下的高精度的姿态解算是本系统姿态解算的关键技术。通常姿态转移矩阵的求解有欧拉角法、方向余弦法和四元数法。由于四元数法具有可以全姿态工作、计算工作量小等特点，故本系统采用四元数法。四元数法解姿态角主要有以下几个步骤：

1)机体相对导航坐标系角速度

的计算

${\mathrm{\ω}}_{\mathrm{nb}}^b={\mathrm{\ω}}_{\mathrm{ib}}^b-{\mathrm{\ω}}_{\mathrm{in}}^b---\left(5\right)$

其中

为稳瞄平台上陀螺仪的直接输出，

可通过车体速度在导航下投影、地球自转角速度ω_ie以及上个周期的姿态转移矩阵

求得。

2)四元数微分方程的求解

四元数微分方程可以获得四元数迭代计算公式。

$q(t+1)=\{\cos \frac{{\mathrm{\Δ\Φ}}_0}{2}I+\frac{\sin \frac{{\mathrm{\Δ\Φ}}_0}{2}}{{\mathrm{\Δ\Φ}}_0}\left[\mathrm{\Δ\Φ}\right]\}q\left(t\right)---\left(6\right)$

其中， $\mathrm{\Δ\Φ}=\mathrm{\Δ\θ}+\frac{7}{45}{h}^2{\mathrm{\ω}}_1\×{\mathrm{\ω}}_2+\frac{1}{180}{h}^2{\mathrm{\ω}}_1\×{\mathrm{\ω}}_3,$ ω₁、ω₂、ω₃分别为在一次姿态解算周期内对陀螺仪输出进行3次采样的角速度，即采用3子样采样方法补偿陀螺非定轴转动时，角速度矢量积分产生的计算误差。姿态更新周期h＝0.02s。

$\mathrm{\Δ\θ}=\left[\begin{array}{c}{\mathrm{\Δ\θ}}_x\\ {\mathrm{\Δ\θ}}_y\\ {\mathrm{\Δ\θ}}_z\end{array}\right]={\∫}_t^{t+h}\left[\begin{array}{c}{\mathrm{\ω}}_{\mathrm{nbx}}^b\\ {\mathrm{\ω}}_{\mathrm{nby}}^b\\ {\mathrm{\ω}}_{\mathrm{nbz}}^b\end{array}\right]\mathrm{dt}---\left(7\right)$

$\left[\mathrm{\Δ\Φ}\right]=\left[\begin{array}{cccc}0& -\mathrm{\Δ}{\mathrm{\Φ}}_x& -{\mathrm{\Δ\Φ}}_y& -{\mathrm{\Δ\Φ}}_z\\ {\mathrm{\Δ\Φ}}_x& 0& {\mathrm{\Δ\Φ}}_z& -{\mathrm{\Δ\Φ}}_y\\ {\mathrm{\Δ\Φ}}_y& -{\mathrm{\Δ\Φ}}_z& 0& {\mathrm{\Δ\Φ}}_x\\ {\mathrm{\Δ\Φ}}_z& {\mathrm{\Δ\Φ}}_y& -{\mathrm{\Δ\Φ}}_x& 0\end{array}\right]---\left(8\right)$

上式中[]表示向量的矩阵扩展，ΔΦ₀ ²＝ΔΦ_x ²+ΔΦ_y ²+ΔΦ_z ²。

3)四元数规范化

由于计算误差使得计算的变换四元数的范数不再等于1，即计算的四元数失去规范性，因此对计算的四元数必须周期地进行规范化处理。设 $q\′=q_0^{\′}+q_1^{\′}\stackrel{\→}{i}+q_2^{\′}\stackrel{\→}{j}+q_3^{\′}\stackrel{\→}{k}$ 为直接由四元数微分方程计算得到的四元数，而设规范化后的四元数为 $q=q_0+q_1\stackrel{\→}{i}+q_2\stackrel{\→}{j}+q_3\stackrel{\→}{k},$ 其中

为相互正交的单位食量，利用最小二乘判据可以求得最优规范化的变换四元数。计算公式如下：

$\begin{array}{cc}{q}_i=\frac{q_i^{\′}}{{\left(\underset{j=0}{\overset{3}{\mathrm{\Σ}}}{q}_j^{\′2}\right)}^{1/2}}& i=\mathrm{0,1,2,3}\end{array}---\left(9\right)$

4)由四元数计算姿态转移矩阵

依据四元数表示的固定矢量之间的变换关系，可以获得四元数矩阵与方向余弦的关系为：

$C_n^b=\left[\begin{array}{ccc}{q}_1^2+q_0^2-q_3^2-q_2^2& 2(q_1{q}_2+q_0{q}_3)& 2(q_1{q}_3-q_0{q}_2)\\ 2(q_1{q}_2-q_0{q}_3)& q_2^2-q_3^2+q_0^2-q_1^2& 2(q_2{q}_3+q_0{q}_1)\\ 2(q_1{q}_3+q_0{q}_2)& 2(q_2{q}_3-q_0{q}_1)& q_3^2-q_2^2-q_1^2+q_0^2\end{array}\right]---\left(10\right)$

5)由姿态转移矩阵

提取姿态角

$\mathrm{\θ}=\arctan \left(\frac{{c\′}_{23}}{\sqrt{c_{21}^{\′2}+c_{22}^{\′2}}}\right)$

$\mathrm{\γ}=\arctan (-\frac{{c\′}_{13}}{{c\′}_{33}})---\left(11\right)$

$\mathrm{\φ}=\arctan \left(\frac{{c\′}_{21}}{{c\′}_{22}}\right)$

其中θ、γ、φ分别为捷联惯导系统的俯仰角、横滚角以及航向角，c′₁₁、c′₁₂、c′₁₃、c′₂₁、c′₂₂、c′₂₃、c′₃₁、c′₃₂、c′₃₃分别为姿态转移矩阵

的各元素，定义同上。

5.3.3　速度与位置计算

a)速度计算

由于加速度计固连在稳瞄平台上，它的输出是稳瞄平台系相对于惯性空间的加速度在稳瞄平台系上的投影。因此需要把稳瞄平台上的加速度计原始输出

通过姿态转移矩阵

转换到导航坐标系中，记为即

$f_{\mathrm{ib}}^n=C_b^n\·f_{\mathrm{ib}}^b---\left(12\right)$

从而可以解算出载体在地理系中的速度，其微分方程为：

$\left.\begin{array}{c}{\stackrel{.}{V}}_e=f_e^n-(\stackrel{.}{\mathrm{\λ}}+2{\mathrm{\ω}}_{\mathrm{ie}})\sin L{V}_e+\stackrel{.}{L}{V}_d\\ {\stackrel{.}{V}}_n=f_n^n-(\stackrel{.}{\mathrm{\λ}}+2{\mathrm{\ω}}_{\mathrm{ie}})(\sin L{V}_n+\cos L{V}_d)\\ {\stackrel{.}{V}}_d=f_d^n-\stackrel{.}{L}{V}_n-(\stackrel{.}{\mathrm{\λ}}+2{\mathrm{\ω}}_{\mathrm{ie}})\cos L{V}_e+g\end{array}\right\}---\left(13\right)$

上式中，

分别是

在东向、北向和地向的投影分量，V_e、V_n、V_d分别是东向、北向和地向的速度分量，λ、L分别为当地的精度与纬度，g为当地重力加速度。

b)位置计算

由于载体在地球表面运动，因此导航计算时必须考虑地球曲率的影响。以经纬度和高度作为导航定位单位，由位置微分方程可求得载体的实时位置，其中要用到地球子午面内的曲率半径R_n，垂直于子午面的法线平面内的曲率半径R_m。

在北东地指向的地理坐标系下，水平指北编排的捷联惯导系统的算法流程如图3所示，捷联惯导系统算法流程分为3个部分，即惯性测量单元部分，“数学平台”计算部分以及导航计算部分。捷联系统经过初始对准解算已得到了稳瞄平台的初始横滚角γ₀，初始俯仰角θ₀，初始航向角ψ₀，并得到初始化四元数Q(Λ₀)与初始化姿态转移矩阵的转置矩阵

，通过四元数微分方程与四元数规范化，结合陀螺仪输出

、导航坐标系相对于惯性坐标系的角速度

以及前一时刻的四元数Q(Λ_k)得到当前时刻的四元数Q(Λ_k+1)，从而由Q(Λ_k+1)四元数求解得到姿态转移矩阵的转置矩阵

，并求解出平台的横滚角γ，俯仰角θ以及航向角ψ。将加速度计输出

通过矩阵转换至

，结合上个时刻载体的纬度L(0)、经度λ(0)、高度h(0)与速度V_t(0)求解当前时刻导航坐标系相对于惯性坐标系的角速度在导航坐标系中的投影

载体的纬度L、经度λ、高度h以及速度V_t，完成整个捷联惯导系统的算法流程。

5.4　车辆航姿信息解算及误差分析步骤

在利用里程仪信息进行车辆航位推算过程中，由于里程仪输出的是车体坐标系下的信息，因此必须转换到地理坐标系下才能进行航位推算。因此，在进行基于里程仪信息的车辆航位推算以前，需要确定车辆的姿态角，即确定车辆的姿态转移矩阵

借助稳定平台的测角元件和炮塔转角测量元件，可以测量出平台相对于稳定平台相对于炮塔的姿态角以及炮塔相对于车辆的航向角；同时，稳定平台上的捷联惯导可以输出平台的姿态角，利用上述信息可以解算出车辆的姿态角信息。在解算姿态角时所用的坐标系如下：

b系：稳瞄平台坐标系；

B系：炮塔坐标系；

V系：车辆坐标系。

5.4.1　车辆航姿信息求解

根据上述各坐标系的物理连接和运动关系，可以得到车辆坐标系和稳瞄平台坐标系之间的变换关系：

$C_V^n=C_B^n{C}_V^B---\left(14\right)$

式中，

由炮塔相对于车体的航向角ψ_VB来确定，该数值可由炮塔的测角元件测定，所以

$C_V^B=\left[\begin{array}{ccc}\cos {\mathrm{\ψ}}_{\mathrm{VB}}& \sin {\mathrm{\ψ}}_{\mathrm{VB}}& 0\\ -\sin {\mathrm{\ψ}}_{\mathrm{VB}}& \cos {\mathrm{\ψ}}_{\mathrm{VB}}& 0\\ 0& 0& 1\end{array}\right]---\left(15\right)$

由稳瞄平台相对于炮塔的三个转角(ψ_P，θ_P，γ_P)来确定，该三个数值可由稳瞄平台的测角元件输出，所以

$C_B^b=\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos {\mathrm{\λ}}_P& \sin {\mathrm{\λ}}_P\\ 0& -\sin {\mathrm{\λ}}_P& \cos {\mathrm{\λ}}_P\end{array}\right]\left[\begin{array}{ccc}\cos {\mathrm{\θ}}_P& 0& -\sin {\mathrm{\θ}}_P\\ 0& 1& 0\\ \sin {\mathrm{\θ}}_P& 0& \cos {\mathrm{\θ}}_P\end{array}\right]\left[\begin{array}{ccc}\cos {\mathrm{\ψ}}_P& \sin {\mathrm{\ψ}}_P& 0\\ -\sin {\mathrm{\ψ}}_P& \cos {\mathrm{\ψ}}_P& 0\\ 0& 0& 1\end{array}\right]$

捷联惯导解算出来的姿态阵为稳瞄平台的姿态，即

与

互为转置，而车辆在姿态阵为因此

$C_V^n=C_b^n{C}_V^b=C_b^n{C}_B^b{C}_V^B---\left(16\right)$

由式(16)可以解算出车辆的姿态转移矩阵，从而可以实现在车辆坐标系内输出的里程仪信息转化为导航坐标系内处理，完成车辆的航位推算。

5.4.2　车辆姿态误差分析

在车辆姿态解算过程中，由于稳瞄平台的航姿角、稳瞄平台的测角元件输出的数值和炮塔的测角元件输出的数值都存在误差，即存在稳瞄平台误差角ψ_n、稳瞄平台相对于炮塔的转角误差角ψ_b以及炮塔的航向误差角ψ_B，因此，利用式(16)进行车辆姿态解算将会把上述误差传递给车辆的姿态转移矩阵，即解算出来的车辆姿态也存在误差。该误差会对利用里程仪解算得到的速度和位置产生影响，因此需要对该姿态误差进行分析。

由式(16)可知：

${\hat{C}}_V^n={\hat{C}}_b^n{\hat{C}}_B^b{\hat{C}}_V^B$

$=(I-{\mathrm{\ψ}}_n\×)C_b^n(I-{\mathrm{\ψ}}_b\×)C_B^b(I-{\mathrm{\ψ}}_B\×)C_V^B---\left(17\right)$

$\≈C_b^n{C}_B^b{C}_V^B+C_b^n\×{\mathrm{\ψ}}_n{C}_B^b{C}_V^B+C_b^n{C}_B^b\×{\mathrm{\ψ}}_b{C}_V^B+C_b^n{C}_B^b{C}_V^B\×{\mathrm{\ψ}}_B$

其中

为带有误差角的车辆姿态转移矩阵，

为带有误差角ψ_n的稳瞄平台姿态转移矩阵，

为带有误差角ψ_b的稳瞄平台相对于炮塔的姿态转移矩阵，为带有误差角ψ_B的炮塔相对于车体的姿态转移矩阵，故车辆姿态误差方程为：

$\mathrm{\ψ}=C_P^n\×{\mathrm{\ψ}}_n{C}_B^P{C}_V^B+C_P^n{C}_B^P\×{\mathrm{\ψ}}_P{C}_V^B+C_P^n{C}_B^P{C}_V^B\×{\mathrm{\ψ}}_B---\left(18\right)$

上式中ψ为车辆总的姿态误差角，其他变量同上。

5.5　惯导系统和惯性器件的误差分析、建模和补偿步骤

高精度的导航定位需要对整个系统和惯性器件进行误差分析，并建立合理的数学模型，采用合适的方式对误差进行估计和补偿，以进一步提高和确保高精度惯导系统的实现，满足载体对系统的导航定位的精度要求。因此，对惯导系统及其惯性器件进行误差分析和建模是导航定位过程中重要的内容。

捷联惯性导航系统的误差包括两个方面：惯性传感器误差和捷联惯导系统的基本导航参数误差。因此对误差的建模也就是对这两类的误差建模。

1.惯性传感器的误差模型

捷联惯性传感器是一类随机性非常强的器件，因此模型主要是对随机误差建模。该随机误差在物理上主要来源于标定、正交、温度、原理等多方面，通常采取抽象的方法进行处理，对IMU的随机误差进行建模处理后，再根据大量的实测数据进行校核以保证模型及其参数的正确性。

a)陀螺仪漂移数学模型

对于光纤陀螺，陀螺漂移误差考虑主要由三部分组成：

ε^b＝ω_g+ε_c+ε_r            (19)

其中：ω_g——随机白噪声漂移，其均方差为σ_g；

      ε_c——随机常值漂移；

      ε_r——随机一阶马尔柯夫过程漂移。

假定三个轴向的陀螺误差模型均相同，均为：

$\left.\begin{array}{c}{\stackrel{.}{\mathrm{\ϵ}}}_c=0\\ {\stackrel{.}{\mathrm{\ϵ}}}_r=-\frac{1}{T_r}{\mathrm{\ϵ}}_r+{\mathrm{\ω}}_r\end{array}\right\}---\left(20\right)$

上式中，T_r为是相关时间，ω_r是均方差为σ_r的马尔柯夫过程驱动白噪声。

b)加速度计误差模型

对IMU中的加速度计，其误差模型

一般考虑为一阶马尔柯夫过程，且三个轴向的加速度计误差模型相同：

${\stackrel{.}{\▿}}_{\mathrm{\α}}=-\frac{1}{T_a}{\▿}_{\mathrm{\α}}+{\mathrm{\ω}}_{\mathrm{\α}}---\left(21\right)$

其中，T_a是马尔柯夫过程的相关时间，ω_a是均方差为σ_a的驱动白噪声。

2.捷联惯性导航系统的误差模型

捷联惯导系统导航参数误差可分为三部分：姿态误差、速度误差和位置误差，对应的误差模型分别为：平台误差角模型、速度误差模型和位置误差模型。当采用当地水平坐标系为北、东、地时，其误差模型为：

a)数学平台误差模型：

$\left\{\begin{array}{c}{\stackrel{.}{\mathrm{\φ}}}_n=\frac{\mathrm{\δ}{V}_e}{R_N+h}-{\mathrm{\ω}}_{\mathrm{ie}}\sin \mathrm{L\δL}-({\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{V_e}{R_N+h}\mathrm{tgL}){\mathrm{\φ}}_e+\frac{V_n}{R_M+h}{\mathrm{\φ}}_d-{\mathrm{\ϵ}}_n^n\\ {\stackrel{.}{\mathrm{\φ}}}_e=-\frac{\mathrm{\δ}{V}_n}{R_M+h}+({\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{V_e}{R_N+h}\mathrm{tgL}){\mathrm{\φ}}_n+({\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_e}{R_N+h}){\mathrm{\φ}}_d-{\mathrm{\ϵ}}_e^n\\ {\stackrel{.}{\mathrm{\φ}}}_d=-\frac{\mathrm{\δ}{V}_e}{R_N+h}\mathrm{tgL}-({\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_e}{R_N+h}{\sec }^{2}L)\mathrm{\δL}-({\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_e}{R_e}){\mathrm{\φ}}_e-\frac{V_n}{R_M+h}{\mathrm{\φ}}_n-{\mathrm{\ϵ}}_d^n\end{array}\right.---\left(22\right)$

上式中φ_n、φ_e和φ_d分别为北向、东向、地向的平台误差角；v_e、v_n分别为东向与北向速度；ω_ie为地球自转角速度；L为当地纬度，δL为纬度误差；R_M为地球子午圈半径，R_N为地球卯酉圈半径；h为当地海拔高度；

分别为北向、东向和地向陀螺仪噪声。

b)速度误差模型：

$\left\{\begin{array}{c}\mathrm{\δ}{\stackrel{.}{V}}_n=-f_d{\mathrm{\φ}}_e+f_e{\mathrm{\φ}}_d+\frac{V_d}{R_M+h}\mathrm{\δ}{V}_n-(2{\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{V_e}{R_N+h}\mathrm{tgL})\mathrm{\δ}{V}_e+\frac{V_n}{R_M+h}\mathrm{\δ}{V}_d\\ -(2{\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_e}{R_N+h}{\sec }^{2}L)V_e\mathrm{\δL}+{\▿}_n\\ \mathrm{\δ}{\stackrel{.}{V}}_e=f_d{\mathrm{\φ}}_n+(2{\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{V_e}{R_N+h}\mathrm{tgL})\mathrm{\δ}{V}_n+(\frac{V_n}{R_M+h}\mathrm{tgL}+\frac{V_d}{R_M+h})\mathrm{\δ}{V}_e+(2{\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_e}{R_N+h})\mathrm{\δ}{V}_d\\ +(2{\mathrm{\ω}}_{\mathrm{ie}}{V}_n\cos L+\frac{V_e}{R_N+h}{V}_n{\sec }^{2}L-2{\mathrm{\ω}}_{\mathrm{ie}}{V}_d\sin L)\mathrm{\δL}-2{\mathrm{\ω}}_{\mathrm{ie}}{V}_d\sin L)\mathrm{\δL}-f_n{\mathrm{\φ}}_d+{\▿}_e\\ \mathrm{\δ}{\stackrel{.}{V}}_d=-f_e{\mathrm{\φ}}_n+f_n{\mathrm{\φ}}_e\frac{2V_n}{R_M+h}\mathrm{\δ}{V}_n-2({\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_e}{R_N+h})\mathrm{\δ}{V}_e\\ +2V_e{\mathrm{\ω}}_{\mathrm{ie}}\sin \mathrm{L\δL}-\frac{2g}{R_M+h}\mathrm{\δh}+{\▿}_d\end{array}\right.---\left(23\right)$ δV_n、δV_e、δV_d为北向、东向、地向的速度误差，f_n、f_e、f_d分别为北向、东向、地向的加速度计输出；δh为海拔高度误差；

分别为北向、东向、天向加速度计误差；其余变量同上。

c)位置误差模型：

$\left\{\begin{array}{c}\mathrm{\δ}\stackrel{.}{L}=\mathrm{\δ}{V}_n/(R_M+h)\\ \mathrm{\δ}\stackrel{.}{\mathrm{\λ}}=\mathrm{\δ}{V}_e/(R_N+h)\cos L+V_e/(R_N+h)\sec \mathrm{LtgL\δL}\\ \mathrm{\δ}\stackrel{.}{h}=-\mathrm{\δ}{V}_d\end{array}\right.---\left(24\right)$

上式中，δλ为经度误差，其他变量同上。

在得到惯性器件和捷联导航系统的误差模型以后，就可以在系统中设法消除或减小各种误差对导航系统精度的影响，从而提高系统的导航定位精度。一般采用外观测量的方式对系统误差和器件误差进行最优估计，将估计结果反馈到系统解算过程中进行误差补偿。

5.6　惯导/里程仪/GPS多信息容错组合导航步骤

由于里程仪具有长距离工作时误差积累的缺点，因此捷联惯导/里程仪组合导航系统在长距离工作时其导航定位精度会下降。如果利用卫星导航系统误差不随时间和地点积累的优点进行进一步组合，组成捷联惯导/里程仪/GPS多信息组合导航系统，可始终保持对车辆的导航定位性能。为方便起见，在研究阶段，采用GPS系统作为卫星导航系统。考虑到卫星导航系统在实用时的可用性，捷联惯导/里程仪/GPS多信息组合导航可以采用以下两种方案来实现。

1.第一种方案：间断地利用GPS信息对捷联惯导/里程仪组合导航系统进行重调。该方案简单易行，比较适合于GPS间歇工作模式。在GPS工作期间，采用GPS信息对该组合导航系统进行一步校正，使导航定位误差减小到卫星定位系统的误差；GPS不工作时，捷联惯导/里程仪在此基础上继续进行组合导航。该方案的系统结构图如图4所示，首先构建一套捷联惯导与里程仪进行速度/位置组合的组合导航系统，实时输出位置与速度信息，在该组合导航系统因长距离工作误差积累超出性能指标要求，且GPS信号可用时，将GPS的位置、速度信息对捷联惯导/里程仪组合导航系统进行一步校正，即将GPS的位置、速度信息对捷联惯导/里程仪组合导航系统相应输出进行重置，提高整体导航精度。经过一次校正，间隔一定时间后可继续运用上述方法校正系统。

2.第二种方案：捷联惯导/里程仪/GPS多信息组合联邦滤波结构。该种方案适合于GPS可以持续使用的环境，也适用于GPS可以间断地持续工作一段时间的环境。在联邦滤波结构中，原捷联惯导/里程仪组合导航系统作为子系统1，捷联惯导/GPS组合导航系统作为子系统2，两个子系统分别进行分布式滤波，然后对子系统1和子系统2的滤波结果进行数据融合(联邦滤波)，给出捷联惯导系统的误差估计，最后利用估计误差反馈校正惯导系统。

联邦滤波算法的结构图如图5所示，该方案将捷联惯导与里程仪进行了位置速度组合滤波解算，同时将捷联惯导与GPS进行了位置、速度组合滤波解算，最后将两个子滤波器的误差状态量估计值经过主滤波器加权平均，求得误差状态量的零均值最小方差估计，并反馈至捷联惯导系统进行闭环校正，提高惯导系统导航定位精度。该方案相对第一种方案具有更强的容错能力。当GPS信息不可用或GPS系统出现故障时，通过联邦滤波器的故障检测与隔离单元检测GPS信号的合理性，并将子系统2隔离掉，这样整个系统就变为捷联惯导/里程仪组合导航系统；当GPS信号可用时，再次将子系统2恢复使用，构成捷联惯导/里程仪/GPS多信息组合联邦滤波系统。该方案不仅使原捷联惯导/里程仪组合导航系统改造方便，而且可以实现在有无GPS信号时整个系统可以平滑地进行组合状态的转换，特别适合于卫星定位系统可用性差的环境，由于GPS的权限问题，该系统中的GPS接收机还可以换成北斗或伽利略卫星定位系统的接收机，以增强信号的可靠性。

同理，当里程仪出现故障时，也采用同样的方式隔离捷联惯导/里程仪组合导航子系统1，使系统转换成捷联惯导/GPS组合导航系统。

发明的效果

本发明从未来作战环境战车的导航/稳瞄一体化系统的性能需求入手，基于卡尔曼滤波最优加权平均的本质，结合稳瞄平台的转动，通过对惯导速度和比力输出的观测，实现对惯导系统平台失准角的最优估计与补偿，为战车提供高精度的姿态信息，并通过与里程仪和卫星导航系统的组合，为战车提供高精度的定位信息，提高战车的战场生存与综合作战能力。

对于中高精度光纤陀螺惯性导航系统，静基座初始对准的时间一般在10分钟以上，对准精度一般在十几角分。本发明在稳瞄平台上安装了0.01°/h光纤陀螺与1×10^-4g的加速度计的惯导系统，初始对准方案采取了转动惯导系统基座的方法，大大提高了平台失准角的可观测性，对准时间在5分钟左右，对准精度高于5.4角分。与GPS、里程仪进行组合后，定位精度可达8米以上，速度精度0.4米/秒，完全满足战车车载导航系统的定位精度要求。本发明具有很强的工程应用价值。

Claims (1)

1.一种导航/稳瞄一体化系统的导航、稳瞄方法，其特征在于稳瞄平台上安装惯性测量单元实现载体定姿、定位，通过以下步骤实现：

(1)惯性测量单元信号采集步骤：采集惯性测量单元中光纤陀螺与MEMS加速度计的输出信号，得到惯导系统的角速度和比力；

(2)稳瞄平台下的快速精确初始对准步骤：利用稳瞄平台具有的随机调转特性，设计了一种快速而精确的转动基座初始对准方法，即在稳瞄平台绕其航向轴连续转动的条件下，建立系统状态方程 $\stackrel{.}{X}=\mathrm{Ax}+W,$ 误差状态量 $X=\left[\begin{array}{cccccccccc}{\mathrm{\δV}}_N& {\mathrm{\δV}}_E& {\mathrm{\φ}}_N& {\mathrm{\φ}}_E& {\mathrm{\φ}}_D& {\▿}_x& {\▿}_y& {\mathrm{\ϵ}}_x& {\mathrm{\ϵ}}_y& {\mathrm{\ϵ}}_z\end{array}\right],$ 其中δV_N、δV_E为水平速度误差沿北、东方向的分量，φ_N、φ_E、φ_D为平台误差角在北、东、地方向的分量；

为加速度计的零偏；ε_x、ε_y、ε_z为陀螺漂移状态转移矩阵，状态转移矩阵 $A=\left[\begin{array}{cc}F& T\\ 0& 0\end{array}\right],$ 其中 $F=\left[\begin{array}{ccccc}0& 2{\mathrm{\Ω}}_D& 0& g& 0\\ -2{\mathrm{\Ω}}_D& 0& -g& 0& 0\\ 0& 0& 0& {\mathrm{\Ω}}_D& 0\\ 0& 0& -{\mathrm{\Ω}}_D& 0& {\mathrm{\Ω}}_N\\ 0& 0& 0& -{\mathrm{\Ω}}_N& 0\end{array}\right],$ $T=\left[\begin{array}{ccccc}{c}_{11}\′& c_{12}\′& 0& 0& 0\\ c_{21}\′& c_{22}\′& 0& 0& 0\\ 0& 0& c_{11}\′& c_{12}\′& c_{13}\′\\ 0& 0& c_{21}\′& c_{22}\′& c_{23}\′\\ 0& 0& c_{31}\′& c_{32}\′& c_{33}\′\end{array}\right];$ Ω_D，Ω_N为地球自转角速度在地向与北向的分量，c₁₁′＝cosγcosφ+sinγsinθsinφ、c₁₂′＝cosθsinφ、c₁₃′-sinγcosφ-cosγsinθsinφ、c₂₁′＝-cosγsinφ+sinγsinθcosφ、c₂₂′＝cosθcosφ、c₂₃′＝-sinγsinφ-cosγsinθcosφ、c₃₁′＝-sinγcosθ、c₃₂′＝sinθ、c₃₃′＝cosγcosθ分别为

中的各元素，γ为平台横滚角、θ为平台俯仰角、φ为平台航向角，

为平台姿态转移矩阵

的转置矩阵，

由横滚角γ、俯仰角θ、航向角φ的三角函数构成，即 $C_n^b=\left[\begin{array}{ccc}\cos \mathrm{\γ}\cos \mathrm{\φ}+\sin \mathrm{\γ}\sin \mathrm{\θ}\sin \mathrm{\φ}& -\cos \mathrm{\γ}\sin \mathrm{\φ}+\sin \mathrm{\γ}\sin \mathrm{\θ}\cos \mathrm{\φ}& -\sin \mathrm{\γ}\cos \mathrm{\θ}\\ \cos \mathrm{\θ}\sin \mathrm{\φ}& \cos \mathrm{\θ}\cos \mathrm{\φ}& \sin \mathrm{\θ}\\ \sin \mathrm{\γ}\cos \mathrm{\φ}-\cos \mathrm{\γ}\sin \mathrm{\θ}\sin \mathrm{\φ}& -\sin \mathrm{\γ}\sin \mathrm{\φ}-\cos \mathrm{\γ}\sin \mathrm{\θ}\cos \mathrm{\φ}& \cos \mathrm{\γ}\cos \mathrm{\θ}\end{array}\right],$ g为当地重力加速度，W＝[w₁ w₂ w₃ w₄ w₅]，式中w₁、w₂为等效北向、东向的加速度计白噪声分量，w₃、w₄、w₅为等效北向、东向、地向的陀螺仪白噪声分量，再建立系统观测方程 $Z=\left[\begin{array}{c}{\mathrm{\δV}}_N\\ {\mathrm{\δV}}_E\end{array}\right]=\left[\begin{array}{cccccccccc}1& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 1& 0& 0& 0& 0& 0& 0& 0& 0\end{array}\right]X+V,$ 式中δV_N、δV_E分别为水平速度误差沿北、东方向的分量，X定义同上，V为观测噪声矢量，通过增加东向与北向加速度计输出f_N、f_E作为观测量，扩展观测方程为 $Z=\left[\begin{array}{cccccccccc}1& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 1& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& -g& 0& c_{11}\′& c_{12}\′& 0& 0& 0\\ 0& 0& g& 0& 0& c_{21}\′& c_{22}\′& 0& 0& 0\end{array}\right]X+V\′=\mathrm{HX}+V\′,$ 式中 $Z=\left[\begin{array}{c}{\mathrm{\δV}}_N\\ {\mathrm{\δV}}_E\\ f_N\\ f_E\end{array}\right],$ X定义同上，V′为观测量扩展后的观测噪声矢量，c₁₁′、c₁₂′、c₂₁′、c₂₂′定义同上，运用卡尔曼滤波器，快速估计惯导系统的初始平台误差角φ_N、φ_E、φ_D并反馈给导航系统进行补偿，得到惯导系统精确的初始横滚角、俯仰角、航向角信息；

(3)基于车载稳瞄平台的惯性导航解算步骤：该步骤由高精度姿态解算与速度、位置解算组成，惯导系统姿态解算首先利用公式 ${\mathrm{\ω}}_{\mathrm{nb}}^b={\mathrm{\ω}}_{\mathrm{ib}}^b-{\mathrm{\ω}}_{\mathrm{in}}^b$ 计算机体相对导航坐标系角速率

式中

为稳瞄平台上陀螺仪的直接输出，

可通过车体速度在导航下投影、地球自转角速度ω_ie以及上个周期的姿态转移矩阵

求得，然后采用四元数微分方程 $q(t+1)=\{\cos \frac{{\mathrm{\Δ\Φ}}_0}{2}I+\frac{\sin \frac{{\mathrm{\Δ\Φ}}_0}{2}}{{\mathrm{\Δ\Φ}}_0}\left[\mathrm{\Δ\Φ}\right]\}q\left(t\right)$ 求解姿态转移矩阵对应的的四元数，式中 $q=q_0+q_1\stackrel{\→}{i}+q_2\stackrel{\→}{j}+q_3\stackrel{\→}{k},$ l为四元数计算的时间间隔， $\mathrm{\Δ\Φ}=\mathrm{\Δ\θ}+\frac{7}{45}{h}^2{\mathrm{\ω}}_1\×{\mathrm{\ω}}_2+\frac{1}{180}{h}^2{\mathrm{\ω}}_1\×{\mathrm{\ω}}_3,$ ω₁、ω₂、ω₃分别为在一次姿态解算周期内对陀螺仪输出进行3次采样的角速率，即采用3子样采样方法补偿陀螺非定轴转动时，角速度矢量积分产生的计算误差。姿态更新周期h＝0.02s， $\mathrm{\Δ\θ}=\left[\begin{array}{c}{\mathrm{\Δ\θ}}_x\\ {\mathrm{\Δ\θ}}_y\\ {\mathrm{\Δ\θ}}_z\end{array}\right]={\∫}_t^{t+h}\left[\begin{array}{c}{\mathrm{\ω}}_{\mathrm{nbx}}^b\\ {\mathrm{\ω}}_{\mathrm{nby}}^b\\ {\mathrm{\ω}}_{\mathrm{nbz}}^b\end{array}\right]\mathrm{dt},$ $\left[\mathrm{\Δ\Φ}\right]=\left[\begin{array}{cccc}0& -\mathrm{\Δ}{\mathrm{\Φ}}_x& -{\mathrm{\Δ\Φ}}_y& -{\mathrm{\Δ\Φ}}_z\\ {\mathrm{\Δ\Φ}}_x& 0& {\mathrm{\Δ\Φ}}_z& -{\mathrm{\Δ\Φ}}_y\\ {\mathrm{\Δ\Φ}}_y& -{\mathrm{\Δ\Φ}}_z& 0& {\mathrm{\Δ\Φ}}_x\\ {\mathrm{\Δ\Φ}}_z& {\mathrm{\Δ\Φ}}_y& -{\mathrm{\Δ\Φ}}_x& 0\end{array}\right],$ 将得到的四元数利用公式 $q_i=\frac{q_i^{\′}}{{\left(\underset{j=0}{\overset{3}{\mathrm{\Σ}}}{q}_j^{\′2}\right)}^{1/2}}$ i＝0，1，2，3进行规范化，式中 $q\′=q_0^{\′}+q_1^{\′}\stackrel{\→}{i}+q_2^{\′}\stackrel{\→}{j}+q_3^{\′}\stackrel{\→}{k}$ 为直接由四元数微分方程计算得到的四元数，而设规范化后的四元数为 $q=q_0+q_1\stackrel{\→}{i}+q_2\stackrel{\→}{j}+q_3\stackrel{\→}{k},$ 其中

为相互正交的单位食量，然后将规范化后的四元数通过公式 $C_n^b=\left[\begin{array}{ccc}{q}_1^2+q_0^2-q_3^2-q_2^2& 2(q_1{q}_2+q_0{q}_3)& 2(q_1{q}_3-q_0{q}_2)\\ 2(q_1{q}_2-q_0{q}_3)& q_2^2-q_3^2+q_0^2-q_1^2& 2(q_2{q}_3+q_0{q}_1)\\ 2(q_1{q}_3+q_0{q}_2)& 2(q_2{q}_3-q_0{q}_1)& q_3^2-q_2^2-q_1^2+q_0^2\end{array}\right]$ 生成姿态转移矩阵

定义同上，最后利用公式 $\mathrm{\θ}=\arctan \left(\frac{{c\′}_{23}}{\sqrt{c_{21}^{\′2}+c_{22}^{\′2}}}\right),$ $\mathrm{\γ}=\arctan (-\frac{{c\′}_{13}}{{c\′}_{33}}),$ $\mathrm{\φ}=\arctan \left(\frac{{c\′}_{21}}{{c\′}_{22}}\right)$ 从

中提取横滚角γ、俯仰角θ、航向角φ信息，其中c′₁₁、c′₁₂、c′₁₃、c′₂₁、c′₂₂、c′₂₃、c′₃₁、c′₃₂、c′₃₃分别为姿态转移矩阵

的各元素，定义同上。惯导系统(即车辆)速度、位置解算方法则将加速度计输出通过平台的转置矩阵从载体坐标系转换到导航坐标系，并通过解算比力微分方程 $f=\stackrel{\·}{v_{\mathrm{ep}}}+(2{\mathrm{\ω}}_{\mathrm{ie}}+{\mathrm{\ω}}_{\mathrm{ep}})\×v_{\mathrm{ep}}-g$ 得到导航系统速度v_ep，再将导航系统速度v_ep经过一次积分得到导航系统位置，式中f为加速度计输出，ω_ie为地球自转角速度，ω_ep为稳瞄平台相对于地球的角速度；

(4)车辆航姿信息解算及误差分析步骤：借助稳定平台的测角元件和炮塔转角测量元件，测量出平台相对于炮塔姿态转移矩阵

$C_B^b=\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos {\mathrm{\λ}}_P& \sin {\mathrm{\λ}}_P\\ 0& -\sin {\mathrm{\λ}}_P& \cos {\mathrm{\λ}}_P\end{array}\right]\left[\begin{array}{ccc}\cos {\mathrm{\θ}}_P& 0& -\sin {\mathrm{\θ}}_P\\ 0& 1& 0\\ \sin {\mathrm{\θ}}_P& 0& \cos {\mathrm{\θ}}_P\end{array}\right]\left[\begin{array}{ccc}\cos {\mathrm{\ψ}}_P& \sin {\mathrm{\ψ}}_P& 0\\ -\sin {\mathrm{\ψ}}_P& \cos {\mathrm{\ψ}}_P& 0\\ 0& 0& 1\end{array}\right],$ ψ_P，θ_P，γ_P为稳瞄平台相对于炮塔的三个转角；炮塔相对于车辆的姿态转移矩阵

$C_V^B=\left[\begin{array}{ccc}\cos {\mathrm{\ψ}}_{\mathrm{VB}}& \sin {\mathrm{\ψ}}_{\mathrm{VB}}& 0\\ -\sin {\mathrm{\ψ}}_{\mathrm{VB}}& \cos {\mathrm{\ψ}}_{\mathrm{VB}}& 0\\ 0& 0& 1\end{array}\right],$ ψ_VB为炮塔相对于车体的航向角；同时稳定平台上的惯导系统输出上述平台的姿态转移矩阵

利用上述平台相对于炮塔姿态转移矩阵

炮塔相对于车辆的姿态转移矩阵

平台的姿态转移矩阵

三个矩阵信息可以解算出车辆的横滚角、俯仰角、航向角信息，并对姿态误差进行分析与修正；

(5)惯导系统和惯性器件的误差分析、建模和补偿步骤：惯导系统的误差分为数学平台误差、速度误差和位置误差，数学平台误差模型为 $\left\{\begin{array}{c}{\stackrel{.}{\mathrm{\φ}}}_n=\frac{\mathrm{\δ}{V}_e}{R_N+h}-{\mathrm{\ω}}_{\mathrm{ie}}\sin \mathrm{L\δL}-({\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{V_e}{R_N+h}\mathrm{tgL}){\mathrm{\φ}}_e+\frac{V_n}{R_M+h}{\mathrm{\φ}}_d-{\mathrm{\ϵ}}_n^n\\ {\stackrel{.}{\mathrm{\φ}}}_e=-\frac{\mathrm{\δ}{V}_n}{R_M+h}+({\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{V_e}{R_N+h}\mathrm{tgL}){\mathrm{\φ}}_n+({\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_e}{R_N+h}){\mathrm{\φ}}_d-{\mathrm{\ϵ}}_e^n\\ {\stackrel{.}{\mathrm{\φ}}}_d=-\frac{\mathrm{\δ}{V}_e}{R_N+h}\mathrm{tgL}-({\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_e}{R_N+h}{\sec }^{2}L)\mathrm{\δL}-({\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_e}{R_e}){\mathrm{\φ}}_e-\frac{V_n}{R_M+h}{\mathrm{\φ}}_n-{\mathrm{\ϵ}}_d^n\end{array}\right.$ ，式中φ_n、φ_e和φ_d分别为北向、东向、地向的平台误差角；v_e、v_n分别为东向与北向速度；ω_ie为地球自转角速度；L为当地纬度，δL为纬度误差；R_M为地球子午圈半径，R_N为地球卯酉圈半径；h为当地海拔高度；

分别为北向、东向和地向陀螺仪噪声，速度误差模型为 $\left\{\begin{array}{c}\mathrm{\δ}{\stackrel{.}{V}}_n=-f_d{\mathrm{\φ}}_e+f_e{\mathrm{\φ}}_d+\frac{V_d}{R_M+h}\mathrm{\δ}{V}_n-(2{\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{V_e}{R_N+h}\mathrm{tgL})\mathrm{\δ}{V}_e+\frac{V_n}{R_M+h}\mathrm{\δ}{V}_d\\ -(2{\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_e}{R_N+h}{\sec }^{2}L)V_e\mathrm{\δL}+{\▿}_n\\ \mathrm{\δ}{\stackrel{.}{V}}_e=f_d{\mathrm{\φ}}_n+(2{\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{V_e}{R_N+h}\mathrm{tgL})\mathrm{\δ}{V}_n+(\frac{V_n}{R_M+h}\mathrm{tgL}+\frac{V_d}{R_M+h})\mathrm{\δ}{V}_e+(2{\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_e}{R_N+h})\mathrm{\δ}{V}_d\\ +(2{\mathrm{\ω}}_{\mathrm{ie}}{V}_n\cos L+\frac{V_e}{R_N+h}{V}_n{\sec }^{2}L-2{\mathrm{\ω}}_{\mathrm{ie}}{V}_d\sin L)\mathrm{\δL}-2{\mathrm{\ω}}_{\mathrm{ie}}{V}_d\sin L)\mathrm{\δL}-f_n{\mathrm{\φ}}_d+{\▿}_e\\ \mathrm{\δ}{\stackrel{.}{V}}_d=-f_e{\mathrm{\φ}}_n+f_n{\mathrm{\φ}}_e-\frac{2V_n}{R_M+h}\mathrm{\δ}{V}_n-2({\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_e}{R_N+h})\mathrm{\δ}{V}_e\\ +2V_e{\mathrm{\ω}}_{\mathrm{ie}}\sin \mathrm{L\δL}-\frac{2g}{R_M+h}\mathrm{\δh}+{\▿}_d\end{array}\right.$ ，式中δV_n、δV_e、δV_d为北向、东向、地向的速度误差，f_n、f_e、f_d分别为北向、东向、地向的加速度计输出；δh为海拔高度误差；

分别为北向、东向、天向加速度计误差；其余变量定义同上，

$\mathrm{\δ}\stackrel{.}{L}=\mathrm{\δ}{V}_n/(R_M+h)$

$\mathrm{\δ}\stackrel{.}{\mathrm{\λ}}=\mathrm{\δ}{V}_e/(R_N+h)\cos L+V_e/(R_N+h)\sec \mathrm{LtgL\δL}$

位置误差方程为 $\mathrm{\δ}\stackrel{.}{h}=-\mathrm{\δ}{V}_d$ ，式中δλ为经度误差，其他变量定义同上，惯性器件误差则是一类强非线性误差，陀螺漂移误差考虑主要由三部分组成：ε^b＝ω_g+ε_c+ε_r，式中ω_g为随机白噪声漂移，其均方差为σ_g，ε_c为随机常值漂移，ε_r为随机一阶马尔柯夫过程漂移，设三个轴向的陀螺误差模型相同，均为： $\left.\begin{array}{c}{\stackrel{.}{\mathrm{\ϵ}}}_c=0\\ {\stackrel{.}{\mathrm{\ϵ}}}_r=-\frac{1}{T_r}{\mathrm{\ϵ}}_r+{\mathrm{\ω}}_r\end{array}\right\},$ 式中T_r为相关时间，ω_r是均方差为σ_r的马尔柯夫过程驱动白噪声，加速度计误差模型

一般考虑为一阶马尔柯夫过程，且三个轴向的加速度计误差模型相同： ${\stackrel{.}{\▿}}_{\mathrm{\α}}=-\frac{1}{T_a}{\▿}_{\mathrm{\α}}+{\mathrm{\ω}}_{\mathrm{\α}},$ 式中，T_a是马尔柯夫过程的相关时间，ω_a是均方差为σ_a的驱动白噪声，本发明采用里程仪(或GPS)与惯导之间的速度误差、位置误差作为观测量，以卡尔曼滤波的方式对姿态、速度、位置等系统误差以及陀螺仪漂移、加速度计偏置等惯性器件误差进行零均值最小方差估计，通过闭环反馈修正进行误差补偿；

(6)惯导/里程仪/GPS多信息容错组合导航步骤：该步骤采用两种方案实现，第一种方案采用间断地利用GPS信息对惯导/里程仪组合导航系统进行重调，首先构建一套捷联惯导与里程仪进行速度/位置组合的组合导航系统，实时输出位置与速度信息，在GPS信号可用时，将GPS的位置、速度信息对捷联惯导/里程仪组合导航系统输出进行校正，提高整体导航精度。经过一次校正，间隔一定时间后继续运用上述方法校正系统；第二种方案采用惯导/里程仪/GPS多信息组合联邦滤波结构，该方案将捷联惯导与里程仪进行了位置速度组合滤波解算，同时将捷联惯导与GPS进行了位置、速度组合滤波解算，最后将两个子滤波器的误差状态量估计值经过主滤波器加权平均，求得误差状态量的零均值最小方差估计，并反馈至捷联惯导系统进行闭环校正，提高惯导系统导航定位精度。

Images