CN102168978B - 一种船用惯性导航系统摇摆基座开环对准方法 - Google Patents

Abstract

一种船用惯性导航系统摇摆基座开环对准方法，是指舰船处于系泊状态时，通过在惯性导航系统内部建立数学平台从而隔离载体角运动，并以惯导的数学平台偏角为量测量，利用递推最小二乘法进行对准与测漂。本发明直接以惯导系统的数学平台偏角为量测量，利用递推最小二乘法进行偏角和漂移估计，可大大简化初始对准过程的计算，具有准确、高效、易于实现、高通用性等特点。

Description

一种船用惯性导航系统摇摆基座开环对准方法

技术领域

本发明涉及一种准确、快速、稳定可靠、易于实现的船用惯性导航系统摇摆基座开环对准方法，该方法同样也可用于其它应用场合惯性惯导系统的静基座及摇摆基座初始对准，属于惯性导航领域。

技术背景

惯性导航系统具有全自主、高隐蔽性、高带宽、连续输出等特点，在国防上具有战略意义，是航空、航天、航海等领域中最重要的设备之一。

初始对准是在惯导进入导航状态前确定其初始状态的过程，初始状态包括初始速度、位置、姿态和陀螺漂移。初始对准的误差在整个导航过程中会影响系统的导航性能，其水平是影响惯性导航精度最为主要的因素之一。舰船等应用环境一般需要惯导系统能够在基座摇摆的条件下实现高精度对准，因此必须要求在惯导系统内部建立起隔离载体运动的数学/物理平台。传统的对准办法需要惯导处于导航状态，根据位置误差和速度误差反推平台偏角及陀螺漂移。利用这种方法需要较长的对准时间，且误差模型本身对线性近似较敏感。

发明内容

本发明解决的技术问题是：克服现有技术的不足，提供一种船用惯性导航系统摇摆基座开环对准方法，具有准确、快速、稳定可靠、易于实现等特点。

本发明的技术解决方案是：一种船用惯性导航系统摇摆基座开环对准方法，实现步骤如下：

(1)输入初始纬度，根据初始纬度计算当地地球自转角速度；

(2)利用粗对准得到的姿态矩阵计算地球自转角速度在载体坐标系上的投影，并将载体坐标系上地球自转角速度的投影从陀螺仪输出中扣除，得到载体的相对运动角速度；

(3)根据步骤(2)中得到的载体相对运动角速度更新四元数及载体姿态矩阵和姿态；

(4)根据步骤(3)中得到载体姿态矩阵将机体坐标系加速度计的输出转换到数学平台坐标系下，得到数学平台坐标系下载体的东向及北向加速度；

(5)根据步骤(4)得到的数学平台坐标系下载体的东向及北向加速度，计算东向、北向数学平台偏角；

(6)根据步骤(5)得到的东向、北向数学平台偏角作为量测量，对东向北向数学平台偏角误差方程进行最小二乘拟合，得到平台坐标系下的等效陀螺漂移；

(7)根据步骤(6)得到的平台坐标系下的等效陀螺漂移计算转导航时刻的数学平台偏角并进行补偿，利用(3)中计算的载体姿态矩阵，将步骤(6)拟合得到的平台坐标系下的等效陀螺漂移转换到载体坐标系下并进行补偿。

本发明的原理是：通过建立运动隔离平台，将载体的运动角速度从陀螺输出中扣除，将载体姿态角引起的重力加速度投影从加速度计输出中扣除，从而得到隔离载体运动后的平台坐标系下的加速度及偏角量测；推导此种解算条件下惯导系统误差模型，对偏角量测进行拟合，从而估计出陀螺漂移的平均值和初始平台偏角。

本发明与现有技术相比的优点在于：

(1)传统的船用惯导系统摇摆基座条件下初始对准时需要惯导处于导航状态，利用导航位置和速度误差反推平台偏角和陀螺漂移，因此误差传递过程时间较长，且模型对非线性误差较敏感。本发明利用初始时刻输入的位置作为整个对准过程中的载体位置，舰船摇摆情况下，视其速度为零，在此条件下进行姿态更新，并构建隔离平台，计算平台坐标系下的加速度和平台偏角。推导在以上解算条件下惯导的误差方程，并以平台偏角为量测量，利用最小二乘法估计对准过程中的平均漂移及平台偏角，在转导航时刻予以修正。与传统方法相比，这种方法减少了速度更新和位置更新两个环节，可以极大缩短对准时间和提高对准精度。本发明所指的惯性导航系统摇摆基座开环对准方法具有准确、快速、稳定可靠、易于实现等优点。

(2)本发明的对准方法不更新惯导位置和速度，减少了对准量测计算过程，且对非线性误差不敏感；同时还适用于静基座及车载等其它应用环境的惯导系统初始对准，具有一定的通用性。

附图说明

图1为本发明所指的新对准方法流程示意图；

图2为本发明实施例中动基座对准等效北向陀螺漂移估计曲线；

图3为本发明实施例中动基座对准等效天向陀螺漂移估计曲线；

图4为本发明实施例中动基座对准初始方位平台偏角估计曲线；

图5为本发明实施例中对准结束后202小时导航东向位置误差曲线；

图6为本发明实施例中对准结束后202小时导航北向位置误差曲线。

具体实施方式

下面以某型激光陀螺惯性导航系统海上试验系泊状态下初始对准过程为例来阐述本发明的具体实施过程。

图1为发明所提的新对准方法流程图，具体过程如下：

1、输入初始纬度，根据初始纬度计算当地地球自转角速度；

2、利用粗对准得到的姿态矩阵计算地球自转角速度在载体坐标系上的投影，并将载体坐标系上地球自转角速度的投影从陀螺仪输出中扣除，得到载体的相对运动角速度，如下式所示：

${\mathrm{\ω}}_{\mathrm{nb}}^b=\left[\begin{array}{c}{\mathrm{\ω}}_X^b\\ {\mathrm{\ω}}_Y^b\\ {\mathrm{\ω}}_Z^b\end{array}\right]-C_n^b\left[\begin{array}{c}0\\ {\mathrm{\ω}}_N\\ {\mathrm{\ω}}_U\end{array}\right]---\left(9\right)$

3、根据步骤(2)中得到的载体相对运动角速度

更新四元数及载体姿态矩阵和姿态，四元数更新过程的表达式为：

$q\left(t_k\right)=(\cos \frac{{\mathrm{\Δ\θ}}_0}{2}I+\frac{\sin \frac{{\mathrm{\Δ\θ}}_0}{2}}{{\mathrm{\Δ\θ}}_0}[\mathrm{\Δ\θ}\left]\right)q\left(t_{k-1}\right)---\left(10\right)$

其中，q(t_k-1)和q(t_k)分别为t_k-1和t_k时刻的四元数，I为单位矩阵，Δθ为等效旋转矢量，其中：

$\left[\mathrm{\Δ\θ}\right]=\left[\begin{array}{cccc}0& -{\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]$

${\mathrm{\Δ\θ}}_0=\sqrt{{\mathrm{\Δ\θ}}_X^2+{\mathrm{\Δ\θ}}_Y^2+{\mathrm{\Δ\θ}}_Z^2}$

${\mathrm{\Δ\θ}}_X=\underset{t_k}{\overset{t_{k+1}}{\∫}}{\mathrm{\ω}}_{\mathrm{nbx}}^b\mathrm{dt}$

${\mathrm{\Δ\θ}}_Y=\underset{t_k}{\overset{t_{k+1}}{\∫}}{\mathrm{\ω}}_{\mathrm{nby}}^b\mathrm{dt}$

${\mathrm{\Δ\θ}}_Z=\underset{t_k}{\overset{t_{k+1}}{\∫}}{\mathrm{\ω}}_{\mathrm{nbz}}^b\mathrm{dt}$

其中，

为

中的三个分量。

姿态矩阵由更新后的四元数给出：

$C_n^b=\left[\begin{array}{ccc}{q}_0^2+q_1^2-q_2^2-q_3^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_0^2-q_1^2+q_2^2-q_3^2& 2(q_0{q}_1+q_2{q}_3)\\ 2(q_1{q}_3+q_0{q}_2)& 2(q_2{q}_3-q_0{q}_1)& q_0^2-q_1^2-q_2^2+q_3^2\end{array}\right]$

其中，q₀，q₁，q₂和q₃分别为更新后的四元数q(t_k)的四个分量，即q(t_k)＝[q₀ q₁ q₂ q₃]。

4、将加速度计测量的重力加速度投影到数学平台坐标系上，得到惯导的误差加速度和平台偏角：

$\left[\begin{array}{c}{\mathrm{\ΔA}}_E\left(t_k\right)\\ \mathrm{\Δ}{A}_N\left(t_k\right)\\ {\mathrm{\ΔA}}_U\left(t_k\right)\end{array}\right]=C_b^n\left(t_k\right)\left[\begin{array}{c}{A}_X^b\left(t_k\right)\\ A_Y^b\left(t_k\right)\\ A_Z^b\left(t_k\right)\end{array}\right]-\left[\begin{array}{c}0\\ 0\\ g\end{array}\right]---\left(11\right)$

$\left[\begin{array}{c}{\mathrm{\Δ\φ}}_E\left(t_k\right)\\ {\mathrm{\Δ\φ}}_N\left(t_k\right)\end{array}\right]=\left[\begin{array}{c}{\mathrm{\ΔA}}_N\left(t_k\right)/g\\ -{\mathrm{\ΔA}}_E\left(t_k\right)/g\end{array}\right]---\left(12\right)$

5、在步骤(1)、(2)、(3)、(4)所述的解算条件下，惯导误差方程可以表示为：

$\mathrm{\Δ}{\stackrel{\·}{\mathrm{\φ}}}_E={\mathrm{\ω}}_u{\mathrm{\Δ\φ}}_N-{\mathrm{\ω}}_N{\mathrm{\Δ\φ}}_U+{\mathrm{\Δ\ϵ}}_E---\left(13\right)$

$\mathrm{\Δ}{\stackrel{\·}{\mathrm{\φ}}}_N=-{\mathrm{\ω}}_U{\mathrm{\Δ\φ}}_E+{\mathrm{\Δ\ϵ}}_N---\left(14\right)$

$\mathrm{\Δ}{\stackrel{\·}{\mathrm{\φ}}}_U={\mathrm{\ω}}_N{\mathrm{\Δ\φ}}_E+{\mathrm{\Δ\ϵ}}_U---\left(15\right)$

解(13)～(15)式所示的微分方程组，得到对准过程中平台误差角的表达式为：

${\mathrm{\Δ\φ}}_E={\mathrm{\Δ\φ}}_{E0}\cos {\mathrm{\ω}}_{e}t+\frac{{\mathrm{\Δ\ϵ}}_E-{\mathrm{\ω}}_N{\mathrm{\Δ\φ}}_{U0}+{\mathrm{\ω}}_U{\mathrm{\Δ\φ}}_{N0}}{{\mathrm{\ω}}_e}\sin {\mathrm{\ω}}_{e}t---\left(16\right)$

$+\frac{{\mathrm{\ω}}_U{\mathrm{\Δ\ϵ}}_N-{\mathrm{\ω}}_N{\mathrm{\Δ\ϵ}}_U}{{{\mathrm{\ω}}_e}^2}(1-\cos {\mathrm{\ω}}_{e}t)$

${\mathrm{\Δ\φ}}_N={\mathrm{\Δ\φ}}_{N0}+{\mathrm{\Δ\ϵ}}_{N}t-\frac{{\mathrm{\Δ\ϵ}}_E-{\mathrm{\ω}}_N{\mathrm{\Δ\φ}}_{U0}+{\mathrm{\ω}}_N{\mathrm{\Δ\φ}}_{N0}}{{{\mathrm{\ω}}_e}^2}{\mathrm{\ω}}_U(1-\cos {\mathrm{\ω}}_{e}t)---\left(17\right)$

$-\frac{{\mathrm{\ω}}_U{\mathrm{\Δ\ϵ}}_N-{\mathrm{\ω}}_N{\mathrm{\Δ\ϵ}}_U}{{{\mathrm{\ω}}_e}^2}{\mathrm{\ω}}_U(t-\frac{1}{{\mathrm{\ω}}_e}\sin {\mathrm{\ω}}_{e}t)$

利用式(12)得到的t_k时刻东向北向数学平台偏角作为量测，对式(16)和式(17)进行最小二乘拟合，即可得到整个对准过程中的平均漂移以及初始平台偏角。

6、对准结束时刻，将估计出的平台偏角及陀螺漂移进行修正。假设(3)中估计出的陀螺漂移和初始平台偏角分别为

和

则转导航时刻平台误差角计算公式如下：

${{\mathrm{\Δ\φ}}_E}^{\′}=\stackrel{\‾}{{\mathrm{\Δ\φ}}_{E0}}\cos {\mathrm{\ω}}_{e}t+\frac{\stackrel{\‾}{{\mathrm{\Δ\ϵ}}_E}-{\mathrm{\ω}}_N\stackrel{\‾}{{\mathrm{\Δ\φ}}_{U0}}+{\mathrm{\ω}}_U\stackrel{\‾}{{\mathrm{\Δ\φ}}_{N0}}}{{\mathrm{\ω}}_e}\sin {\mathrm{\ω}}_{e}t$

(18)

$+\frac{{\mathrm{\ω}}_U\stackrel{\‾}{{\mathrm{\Δ\ϵ}}_N}-{\mathrm{\ω}}_N\stackrel{\‾}{{\mathrm{\Δ\ϵ}}_U}}{{{\mathrm{\ω}}_e}^2}(1-\cos {\mathrm{\ω}}_{e}t)$

${{\mathrm{\Δ\φ}}_N}^{\′}=\stackrel{\‾}{{\mathrm{\Δ\φ}}_{N0}}+\stackrel{\‾}{{\mathrm{\Δ\ϵ}}_{N}t}-\frac{\stackrel{\‾}{{\mathrm{\Δ\ϵ}}_E}-{\mathrm{\ω}}_N\stackrel{\‾}{{\mathrm{\Δ\φ}}_{U0}}+{\mathrm{\ω}}_U\stackrel{\‾}{{\mathrm{\Δ\φ}}_{N0}}}{{{\mathrm{\ω}}_e}^2}{\mathrm{\ω}}_U(1-\cos {\mathrm{\ω}}_{e}t)$

(19)

$-\frac{{\mathrm{\ω}}_U\stackrel{\‾}{{\mathrm{\Δ\ϵ}}_N}-{\mathrm{\ω}}_N\stackrel{\‾}{{\mathrm{\Δ\ϵ}}_U}}{{{\mathrm{\ω}}_e}^2}{\mathrm{\ω}}_U(t-\frac{1}{{\mathrm{\ω}}_e}\sin {\mathrm{\ω}}_{e}t)$

${{\mathrm{\Δ\φ}}_U}^{\′}=\stackrel{\‾}{{\mathrm{\Δ\φ}}_{U0}}+\stackrel{\‾}{{\mathrm{\Δ\ϵ}}_U}t+\frac{\stackrel{\‾}{{\mathrm{\Δ\ϵ}}_E}-{\mathrm{\ω}}_N\stackrel{\‾}{{\mathrm{\Δ\φ}}_{U0}}+{\mathrm{\ω}}_N\stackrel{\‾}{{\mathrm{\Δ\φ}}_{N0}}}{{{\mathrm{\ω}}_e}^2}{\mathrm{\ω}}_N(1-\cos {\mathrm{\ω}}_{e}t)+\frac{{\mathrm{\ω}}_N}{{\mathrm{\ω}}_e}\stackrel{\‾}{{\mathrm{\Δ\φ}}_{E0}}\sin {\mathrm{\ω}}_{e}t---\left(20\right)$

$+\frac{{\mathrm{\ω}}_U\stackrel{\‾}{{\mathrm{\Δ\ϵ}}_N}-{\mathrm{\ω}}_N\stackrel{\‾}{{\mathrm{\Δ\ϵ}}_U}}{{{\mathrm{\ω}}_e}^2}{\mathrm{\ω}}_N(t-\frac{1}{{\mathrm{\ω}}_e}\sin {\mathrm{\ω}}_{e}t)$

转导航时刻按照式(21)修正姿态矩阵：

$C_b^n=\left[\begin{array}{ccc}1& -{{\mathrm{\Δ\φ}}_U}^{\′}& {{\mathrm{\Δ\φ}}_N}^{\′}\\ {{\mathrm{\Δ\φ}}_U}^{\′}& 1& -{{\mathrm{\Δ\φ}}_E}^{\′}\\ -{{\mathrm{\Δ\φ}}_N}^{\′}& {{\mathrm{\Δ\φ}}_E}^{\′}& 1\end{array}\right]C_b^{n^{\′}}---\left(21\right)$

其中，

为修正前的姿态矩阵，

为修正以后的姿态矩阵。

四元数以修正后的姿态矩阵元素计算：

$q_0=0.5\sqrt{1+C_b^n\left(\mathrm{1,1}\right)+C_b^n\left(\mathrm{2,2}\right)+C_b^n\left(\mathrm{3,3}\right)}---\left(22\right)$

$q_1=(C_b^n\left(\mathrm{2,3}\right)-C_b^n\left(\mathrm{3,2}\right))/\left(4q_0\right)---\left(23\right)$

$q_2=(C_b^n\left(\mathrm{3,1}\right)-C_b^n\left(\mathrm{1,3}\right))/\left(4q_0\right)---\left(24\right)$

$q_3=(C_b^n\left(\mathrm{1,2}\right)-C_b^n\left(\mathrm{2,1}\right))/\left(4q_0\right)---\left(25\right)$

其中，

表示姿态矩阵的第m行第n列，漂移修正公式为：

$\left[\begin{array}{c}{\mathrm{\Δ\ϵ}}_X\\ {\mathrm{\Δ\ϵ}}_Y\\ {\mathrm{\Δ\ϵ}}_Z\end{array}\right]=C_n^b\left[\begin{array}{c}{\mathrm{\Δ\ϵ}}_E\\ {\mathrm{\Δ\ϵ}}_N\\ {\mathrm{\Δ\ϵ}}_U\end{array}\right]---\left(18\right)$

其中，Δε_X、Δε_Y、Δε_Z分别为待补偿的机体坐标系下等效陀螺漂移。

如图2～4所示，惯导系统在舰船摇摆基座条件下利用本发明所述方法进行8小时初始对准，等效北向陀螺漂移收敛到了-0.0005度/小时，后2小时稳定到了0.0001度/小时，等效天向陀螺漂移收敛到了+0.0018度/小时，后两小时稳定到了0.0003度/小时，初始方位平台偏角收敛到了38角秒，后两小时稳定到了10角秒以内；

如图5～6所示，利用本发明所述的方法对准完之后导航202小时的东向北向位置误差，右图可见，202小时东向位置误差最大4200米，北向误差最大4150米；由整个曲线的趋势来看，对准过程中估计出的陀螺漂移精度小于0.0002度/小时；

本发明说明书中未作详细描述的内容属于本领域专业技术人员公知的现有技术。

最后所应说明的是：以上实施实例仅用以说明而非限制本发明的技术方案，所有的不脱离本发明的精神和范围的修改或局部替换，均应涵盖在本发明的权利要求范围当中。

Claims (2)

1.一种船用惯性导航系统摇摆基座开环对准方法，其特征在于实现步骤如下：

步骤(1)、输入初始纬度，根据初始纬度计算当地地球自转角速度；

步骤(2)、利用粗对准得到的姿态矩阵计算地球自转角速度在载体坐标系上的投影，并将载体坐标系上地球自转角速度的投影从陀螺仪输出中扣除，得到载体相对运动角速度；

步骤(3)、根据步骤(2)中得到的载体相对运动角速度更新四元数及载体姿态矩阵和姿态；

步骤(4)、根据步骤(3)中得到载体姿态矩阵，将载体坐标系加速度计的输出转换到数学平台坐标系下，得到数学平台坐标系下载体的东向及北向加速度；

步骤(5)、根据步骤(4)得到的数学平台坐标系下载体的东向及北向加速度，计算东向、北向数学平台偏角；

步骤(6)、根据步骤(5)得到的东向、北向数学平台偏角作为量测量，对东向、北向数学平台偏角的误差方程进行最小二乘拟合，得到数学平台坐标系下的等效陀螺漂移；

步骤(7)、根据步骤(6)得到的数学平台坐标系下的等效陀螺漂移计算转导航时刻的数学平台偏角并进行补偿，利用步骤(3)中计算的载体姿态矩阵，将步骤(6)拟合得到的数学平台坐标系下的等效陀螺漂移转换到载体坐标系下并进行补偿。

2.根据权利要求1所述的船用惯性导航系统摇摆基座开环对准方法，其特征在于：所述步骤(2)的载体相对运动角速度计算公式如下：

${\mathrm{\ω}}_{\mathrm{nb}}^b=\left[\begin{array}{c}{\mathrm{\ω}}_X^b\\ {\mathrm{\ω}}_Y^b\\ {\mathrm{\ω}}_Z^b\end{array}\right]-C_n^b\left[\begin{array}{c}0\\ {\mathrm{\ω}}_N\\ {\mathrm{\ω}}_U\end{array}\right]---\left(1\right)$

上式(1)中

为载体相对运动角速度，为三个方向陀螺仪的输出角速度，

为载体的姿态矩阵，ω_N和ω_U分别为当地地球自转角速度的北向和天向分量。

Images