CN101696883A - 光纤陀螺捷联惯性导航系统阻尼方法 - Google Patents

Abstract

本发明提供的是一种光纤陀螺捷联惯性导航系统阻尼方法。主要包括：经过初始对准得到载体的初始姿态；测得载体坐标系上的角速度输入和加速度输入；计算初始姿态矩阵；将载体系加速度计输出转化为平台系加速度；得出当前的各速度和角速度值；速度信息进行水平阻尼；计算载体对数学平台系的姿态角速度；使用四元数更新当前的姿态矩阵；输出载体姿态角；进入下一个时间的循环。本发明在系统水平回路的速度信息处加入合适的水平阻尼，以消除系统的舒拉周期振荡以及傅科周期振荡。在地球角速度输入信息处加入合适的方位阻尼网络，以消除系统的24小时周期的地球周期振荡。从而提高船用捷联惯导系统的精度。

Description

光纤陀螺捷联惯性导航系统阻尼方法

(一)技术领域

本发明涉及的是一种消除光纤陀螺捷联惯导系统的振荡误差的方法，具体地说是一种在捷联惯导系统的特定位置加入调节网络改变捷联惯导系统的误差振荡特性，消除系统的三种周期振荡误差的方法。

(二)背景技术

惯性导航系统是一种以陀螺和角速度计为测量元件的自主式的导航定位系统，由于其测量精度高、具有实时性，并且测量过程中不需要任何外部信息，自主性强，被广泛的应用与航空航天，航海等方面。其基本原理主要是利用重力加速度和地球转速信息进行导航，利用舒拉条件屏蔽外部加速度干扰。这些基本原理决定了惯导系统含有振荡性质的系统，这些振荡正是与其导航所利用的原理所造成的舒拉振荡周期、傅科振荡周期和地球振荡周期。对于飞机、火箭等加速度很大，使用时间很短的运载体，振荡偏差和误差的积累不会太严重。而对于船舰等加速度很小，使用时间又长的运载体，这些振荡的误差对系统的影响增大，而且随时间积累，这就需要对振荡误差加以消除。

现今各国所使用的高精度惯导系统多数为平台式惯导系统，一种有效的去除平台式惯导系统中的所固有舒拉振荡周期、傅科振荡周期和地球振荡周期振荡误差的方法就是在系统中使用阻尼技术，从而有效的去除振荡误差。

捷联惯导系统是直接与运载体固联的导航系统。在捷联惯导系统中，陀螺仪不再通过常平架与载体旋转运动隔离，而是完全和载体同步运动。捷联式惯导系统的稳定时间短，可靠性高等优点使其更适应现代化舰船导航的需要。在捷联惯导系统中，一般采用引入其他导航信息组合校正的方法来提高精度。但是在一些特殊场合，外界导航系统不可用或大部分不可用时，利用捷联惯性导航系统本身的信息来提高精度，目前的研究还很少。从理论上来说，平台式惯导系统的阻尼技术也可以用于捷联惯导系统中。然而捷联惯导系统中使用的导航基准是数学平台，不同于平台惯导中的物理平台，阻尼技术的实现方法也有所区别。在2007年4月第39卷第2期《南京航空航天大学学报》中的《航姿系统内阻尼的模糊自适应滤波算法》中，以自适应内阻尼的方式应用于航姿系统中，然而无法消除惯导系统的速度以及位置的误差振荡。

(三)发明内容

本发明的目的在于提供一种完全不受外界信息量制约，能够自主的消除捷联惯导系统中所固有的误差振荡特性的光纤陀螺捷联惯性导航系统阻尼方法。

本发明的目的是这样实现的：主要包括如下步骤：

步骤1、经过初始对准得到载体的初始姿态；

步骤2、由光纤陀螺测得载体坐标系上的角速度输入，由加速度计测得载体坐标系上的加速度输入，

得到的角速度为 ${\mathrm{\ω}}_{\mathrm{ib}}^b=\left[\begin{array}{c}{\mathrm{\ω}}_{\mathrm{ibx}}^b\\ {\mathrm{\ω}}_{\mathrm{iby}}^b\\ {\mathrm{\ω}}_{\mathrm{ibz}}^b\end{array}\right],$ 加速度为 $f^b=\left[\begin{array}{c}{f}_x^b\\ f_y^b\\ f_z^b\end{array}\right]$

其中ω_ib ^b为b系即载体坐标系相对于i系即地球惯性坐标系的角速度向量在b系上的投影；f_b载体所受到的非引力加速度向量在b系上的投影；

步骤3、利用步骤1中所得到的初始姿态信息计算初始姿态矩阵T，

$T=\left[\begin{array}{ccc}\cos \left(\mathrm{\ψ}\right)\cos \left(\mathrm{\γ}\right)+\sin \left(\mathrm{\ψ}\right)\sin \left(\mathrm{\θ}\right)\sin \left(\mathrm{\γ}\right)& \sin \left(\mathrm{\ψ}\right)\cos \left(\mathrm{\θ}\right)& \cos \left(\mathrm{\ψ}\right)\sin \left(\mathrm{\γ}\right)-\sin \left(\mathrm{\ψ}\right)\sin \left(\mathrm{\θ}\right)\cos \left(\mathrm{\γ}\right)\\ \cos \left(\mathrm{\ψ}\right)\sin \left(\mathrm{\θ}\right)\sin \left(\mathrm{\γ}\right)-\sin \left(\mathrm{\ψ}\right)\cos \left(\mathrm{\γ}\right)& \cos \left(\mathrm{\θ}\right)\cos \left(\text{\ψ}\right)& -\sin \left(\mathrm{\ψ}\right)\sin \left(\mathrm{\γ}\right)-\cos \left(\mathrm{\ψ}\right)\sin \left(\mathrm{\θ}\right)\cos \left(\mathrm{\γ}\right)\\ -\cos \left(\mathrm{\θ}\right)\sin \left(\mathrm{\γ}\right)& \sin \left(\mathrm{\θ}\right)& \cos \left(\mathrm{\θ}\right)\cos \left(\mathrm{\γ}\right)\end{array}\right]$

其中，θ、γ、ψ分别为欧拉角意义下的俯仰、横滚和偏航姿态角；

步骤4、利用姿态矩阵T将步骤2中所得的载体系加速度计输出转化为平台系加速度f_p＝Tf_b；

步骤5、利用步骤1中得到的初始姿态信息，得出当前的各速度和角速度值，其中，速度与位置已知，北向与东向速度投影为V_x和V_t，经纬度为λ与

其中，ω_ie为地球自转角速度，R为地球半径；ω_ie ^p为e系即地球坐标系相对于i系的角速度向量在p系即平台坐标系上的投影；ω_ep ^p为p系相对于e系的角速度向量在p系上的投影；

步骤6、利用前一时刻速度信息通过速度微分方程修正在本时刻速度，并对速度进行积分更新本时刻位置信息，并输出惯导系统计算所得的载体速度和位置信息，

速度更新微分方程为：

${\stackrel{\·}{V}}_x=f_x^p+({2\mathrm{\ω}}_{\mathrm{iez}}^p+{\mathrm{\ω}}_{\mathrm{epz}}^p)V_y$

${\stackrel{\·}{V}}_y=f_y^p-({2\mathrm{\ω}}_{\mathrm{iez}}^p+{\mathrm{\ω}}_{\mathrm{epz}}^p)V_x$

经纬度位置计算方程为：

步骤7、将步骤6中输出的速度信息进行水平阻尼，先经过合适的水平阻尼网络H，然后再将经过阻尼的速度信息引入捷联惯导系统；将地球角速度信息经过方位阻尼网络，然后再将经过阻尼的地球角速度信息引入捷联惯导系统，

${\mathrm{\ω}}_{\mathrm{ep}}^p=\left[\begin{array}{c}-\frac{V_y}{R_M}H\left(s\right)\\ \frac{V_x}{R_N}H\left(s\right)\\ \frac{V_x}{R_N}\tan \mathrm{\φH}\left(s\right)\end{array}\right],$ ${\mathrm{\ω}}_{\mathrm{ie}}^p=\left[\begin{array}{c}0\\ \mathrm{\Ω}\cos \mathrm{\φ}\·Y\left(s\right)\\ \mathrm{\Ω}\sin \mathrm{\φ}\·Y\left(s\right)\end{array}\right]$

所述水平阻尼网络H为：

$H\left(s\right)=\frac{(s+8.50\×{10}^{-4})(s+9.412\×{10}^{-2})}{(s+8.0\×{10}^{-3})(s+1.0\×{10}^{-2})}$

所述方位阻尼网络Y为：

$Y\left(s\right)=1.669\·\frac{s^2+7.173\×{10}^{-5}s+21.53\×{10}^{-10}}{s^2+12\×{10}^{-5}s+36\×{10}^{-10}}$

步骤8、利用步骤7所得的各角速度以及步骤2中测得的角速度计算载体对数学平台系的姿态角速度ω_pb ^b，

${\mathrm{\ω}}_{\mathrm{pb}}^b={\mathrm{\ω}}_{\mathrm{ib}}^b+T^{-1}({\mathrm{\ω}}_{\mathrm{ie}}^p+{\mathrm{\ω}}_{\mathrm{ep}}^p);$

步骤9、利用步骤8中所提供的当前姿态角速度ω_pb ^b使用四元数更新当前的姿态矩阵T，

通过更新四元数值对应跟新姿态矩阵T：

$\left[\begin{array}{c}{\stackrel{\·}{q}}_0\\ {\stackrel{\·}{q}}_1\\ {\stackrel{\·}{q}}_2\\ {\stackrel{\·}{q}}_3\end{array}\right]=\frac{1}{2}\left[\begin{array}{cccc}0& {-\mathrm{\ω}}_{\mathrm{pbx}}^b& -{\mathrm{\ω}}_{\mathrm{pby}}^b& -{\mathrm{\ω}}_{\mathrm{pbz}}^b\\ {\mathrm{\ω}}_{\mathrm{pbx}}^b& & {\mathrm{\ω}}_{\mathrm{pbz}}^b& -{\mathrm{\ω}}_{\mathrm{pby}}^b\\ {\mathrm{\ω}}_{\mathrm{pby}}^b& -{\mathrm{\ω}}_{\mathrm{pbz}}^b& & {\mathrm{\ω}}_{\mathrm{pbx}}^b\\ {\mathrm{\ω}}_{\mathrm{pbz}}^b& {\mathrm{\ω}}_{\mathrm{pby}}^b& -{\mathrm{\ω}}_{\mathrm{pbx}}^b& \end{array}\right]\left[\begin{array}{c}{q}_0\\ q_1\\ q_2\\ q_3\end{array}\right]$

将计算所得四元数归一化，

$\left[\begin{array}{c}{q}_0\\ q_1\\ q_2\\ q_3\end{array}\right]=\frac{1}{\sqrt{q_0^2+q_1^2+q_2^2+q_3^2}}\left[\begin{array}{c}{q}_0\\ q_1\\ q_2\\ q_3\end{array}\right]$

然后更新姿态矩阵T：

$T=\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_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_0^2-q_1^2-q_2^2+q_3^2\end{array}\right];$

步骤10、利用步骤9中所得当前姿态矩阵T得到当前姿态，并输出载体姿态角，

θ＝arcsinT₃₂

$\mathrm{\γ}=\arctan \frac{{-T}_{31}}{T_{33}}$

$\mathrm{\ψ}=\arctan \frac{{-T}_{12}}{T_{22}}$

角度修正如下：

步骤11、循环至步骤4进入下一个时间的循环，在每一个系统周期中在步骤6与步骤10的位置输出本时刻载体的姿态，速度和位置信息。

本发明是针对捷联惯导系统的振荡特性进行捷联惯导系统改进的技术，成功的去除了捷联惯导系统输出姿态、速度以及位置中的三种周期振荡误差，提高了捷联惯导系统的精度。

本发明提供了一种完全不受外界信息量制约的，能够自主的消除捷联惯导系统中所固有的误差振荡特性的阻尼技术。在无阻尼捷联惯导系统的基础上进行系统改进，将阻尼技术应用于捷联惯导系统。

设计阻尼网络既时要保证系统的稳定性，又要保证阻尼网络能有效的消除系统振荡，还要保证阻尼网络在稳态时不影响惯导系统的舒拉条件。为了满足以上要求，经过分析及多次尝试后选择阻尼网络设计。

本发明在系统水平回路的速度信息处加入合适的水平阻尼，以消除系统的舒拉周期振荡以及傅科周期振荡。在地球角速度输入信息处加入合适的方位阻尼网络，以消除系统的24小时周期的地球周期振荡。从而提高船用捷联惯导系统的精度。

(四)附图说明

图1为本发明的技术方案的流程图。

图2为本发明中阻尼方案的阻尼原理图。

图3到图5为无阻尼捷联惯导系统与水平阻尼方案捷联惯导系统的姿态角、速度和位置误差输出比较。其中，虚线表示无阻尼捷联惯导系统输出，实线表示加入水平阻尼的捷联惯导系统输出。

图6到图8为无阻尼捷联惯导系统与水平阻尼方案捷联惯导系统的姿态角、速度和位置误差输出比较。其中，虚线表示加入水平阻尼的捷联惯导系统输出，实线表示加入水平阻尼和方位阻尼的全阻尼捷联惯导系统输出。

(五)具体实施方式

下面结合附图举例对本发明做更详细地描述：

结合图1，能够自主消除捷联惯导系统固有震荡的阻尼技术方案如下：

步骤1、经过初始对准得到载体的初始姿态。

步骤2、由光纤陀螺测得载体坐标系上的角速度输入，由加速度计测得载体坐标系上的加速度输入。

得到的角速度为 ${\mathrm{\ω}}_{\mathrm{ib}}^b=\left[\begin{array}{c}{\mathrm{\ω}}_{\mathrm{ibx}}^b\\ {\mathrm{\ω}}_{\mathrm{iby}}^b\\ {\mathrm{\ω}}_{\mathrm{ibz}}^b\end{array}\right],$ 加速度为 $f^b=\left[\begin{array}{c}{f}_x^b\\ f_y^b\\ f_z^b\end{array}\right]$

其中ω_ib ^b为b(载体坐标系)相对于i系(地球惯性坐标系)的角速度向量在b系上的投影；f_b载体所受到的非引力加速度(比力)向量在b系上的投影。后文所用的向量的描述方式与此相同。

步骤3、利用步骤1中所得到的姿态信息计算初始姿态矩阵T。

$T=\left[\begin{array}{ccc}\cos \left(\mathrm{\ψ}\right)\cos \left(\mathrm{\γ}\right)+\sin \left(\mathrm{\ψ}\right)\sin \left(\mathrm{\θ}\right)\sin \left(\mathrm{\γ}\right)& \sin \left(\mathrm{\ψ}\right)\cos \left(\mathrm{\θ}\right)& \cos \left(\mathrm{\ψ}\right)\sin \left(\mathrm{\γ}\right)-\sin \left(\mathrm{\ψ}\right)\sin \left(\mathrm{\θ}\right)\cos \left(\mathrm{\γ}\right)\\ \cos \left(\mathrm{\ψ}\right)\sin \left(\mathrm{\θ}\right)\sin \left(\mathrm{\γ}\right)-\sin \left(\mathrm{\ψ}\right)\cos \left(\mathrm{\γ}\right)& \cos \left(\mathrm{\θ}\right)\cos \left(\text{\ψ}\right)& -\sin \left(\mathrm{\ψ}\right)\sin \left(\mathrm{\γ}\right)-\cos \left(\mathrm{\ψ}\right)\sin \left(\mathrm{\θ}\right)\cos \left(\mathrm{\γ}\right)\\ -\cos \left(\mathrm{\θ}\right)\sin \left(\mathrm{\γ}\right)& \sin \left(\mathrm{\θ}\right)& \cos \left(\mathrm{\θ}\right)\cos \left(\mathrm{\γ}\right)\end{array}\right]---\left(1\right)$

其中，θ、γ、ψ分别为欧拉角意义下的俯仰、横滚和偏航姿态角

步骤4、利用姿态矩阵T将步骤2中所得的载体系加速度计输出转化为平台系(数学平台系跟踪地理坐标系)加速度f_p＝Tf_b。

步骤5、利用步骤1中得到的初始信息，得出当前的各速度和角速度值。其中，速度与位置已知，北向与东向速度投影为V_x和V_y，经纬度为λ与

那么可得：

其中，ω_ie为地球自转角速度，R为地球半径；ω_ie ^p为e系(地球坐标系)相对于i系的角速度向量在p系(平台坐标系)上的投影；ω_ep ^p为p系相对于e系的角速度向量在p系上的投影；

步骤6、利用前一时刻速度信息通过速度微分方程修正在本时刻速度，并对速度进行积分更新本时刻位置信息，并输出惯导系统计算所得的载体速度和位置信息。

速度更新微分方程为：

${\stackrel{\·}{V}}_x=f_x^p+({2\mathrm{\ω}}_{\mathrm{iez}}^p+{\mathrm{\ω}}_{\mathrm{epz}}^p)V_y$ (3)

${\stackrel{\·}{V}}_y=f_y^p-({2\mathrm{\ω}}_{\mathrm{iez}}^p+{\mathrm{\ω}}_{\mathrm{epz}}^p)V_x$

经纬度位置计算方程：

步骤7、将步骤6中输出的速度信息进行水平阻尼，令其先经过合适的水平阻尼网络H，然后再将经过阻尼的速度信息引入捷联惯导系统。将地球角速度信息经过方位阻尼网络，然后再将经过阻尼的地球角速度信息引入捷联惯导系统。

${\mathrm{\ω}}_{\mathrm{ep}}^p=\left[\begin{array}{c}-\frac{V_y}{R_M}H\left(s\right)\\ \frac{V_x}{R_N}H\left(s\right)\\ \frac{V_x}{R_N}\tan \mathrm{\φH}\left(s\right)\end{array}\right],$ ${\mathrm{\ω}}_{\mathrm{ie}}^p=\left[\begin{array}{c}0\\ \mathrm{\Ω}\cos \mathrm{\φ}\·Y\left(s\right)\\ \mathrm{\Ω}\sin \mathrm{\φ}\·Y\left(s\right)\end{array}\right]---\left(5\right)$

设计阻尼网络既时要保证系统的稳定性，又要保证阻尼网络能有效的消除系统振荡，还要保证阻尼网络在稳态时不影响惯导系统的舒拉条件。为了满足以上要求，经过分析及多次尝试得到阻尼网络设计如下：

水平阻尼网络H设计为：

$H\left(s\right)=\frac{(s+8.50\×{10}^{-4})(s+9.412\×{10}^{-2})}{(s+8.0\×{10}^{-3})(s+1.0\×{10}^{-2})}$

方位阻尼网络Y设计为：

$Y\left(s\right)=1.669\·\frac{s^2+7.173\×{10}^{-5}s+21.53\×{10}^{-10}}{s^2+12\×{10}^{-5}s+36\×{10}^{-10}}$

步骤8、利用步骤7算得的各角速度以及步骤2中测得的角速度计算载体对数学平台系的姿态角速度ω_pb ^b。

${\mathrm{\ω}}_{\mathrm{pb}}^b={\mathrm{\ω}}_{\mathrm{ib}}^b+T^{-1}({\mathrm{\ω}}_{\mathrm{ie}}^p+{\mathrm{\ω}}_{\mathrm{ep}}^p)---\left(6\right)$

步骤9、利用步骤8中所提供的当前姿态角速度ω_pb ^b使用四元数更新当前的姿态矩阵T。

通过更新四元数值对应跟新姿态矩阵T：

$\left[\begin{array}{c}{\stackrel{\·}{q}}_0\\ {\stackrel{\·}{q}}_1\\ {\stackrel{\·}{q}}_2\\ {\stackrel{\·}{q}}_3\end{array}\right]=\frac{1}{2}\left[\begin{array}{cccc}0& -{\mathrm{\ω}}_{\mathrm{pbx}}^b& -{\mathrm{\ω}}_{\mathrm{pby}}^b& -{\mathrm{\ω}}_{\mathrm{pbz}}^b\\ {\mathrm{\ω}}_{\mathrm{pbx}}^b& & {\mathrm{\ω}}_{\mathrm{pbz}}^b& -{\mathrm{\ω}}_{\mathrm{pby}}^b\\ {\mathrm{\ω}}_{\mathrm{pby}}^b& -{\mathrm{\ω}}_{\mathrm{pbz}}^b& & {\mathrm{\ω}}_{\mathrm{pbx}}^b\\ {\mathrm{\ω}}_{\mathrm{pbz}}^b& {\mathrm{\ω}}_{\mathrm{pby}}^b& -{\mathrm{\ω}}_{\mathrm{pbx}}^b& \end{array}\right]\left[\begin{array}{c}{q}_0\\ q_1\\ q_2\\ q_3\end{array}\right]=\left(7\right)$

将计算所得四元数归一化。

$\left[\begin{array}{c}{q}_0\\ q_1\\ q_2\\ q_3\end{array}\right]=\frac{1}{\sqrt{q_0^2+q_1^2+q_2^2+q_3^2}}\left[\begin{array}{c}{q}_0\\ q_1\\ q_2\\ q_3\end{array}\right]---\left(8\right)$

然后更新姿态矩阵T：

$T=\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_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_0^2-q_1^2-q_2^2+q_3^2\end{array}\right]---\left(9\right)$

步骤10、利用步骤9中所得当前姿态矩阵T得到当前姿态，并输出载体姿态角。

θ＝arcsinT₃₂

$\mathrm{\γ}=\arctan \frac{{-T}_{31}}{T_{33}}$

$\mathrm{\ψ}=\arctan \frac{{-T}_{12}}{T_{22}}---\left(10\right)$

角度修正如下：

步骤11、循环至步骤4进入下一个时间的循环，在每一个系统周期中在步骤6与步骤10的位置输出本时刻载体的姿态，速度和位置信息。

Claims (1)

1.一种光纤陀螺捷联惯性导航系统阻尼方法，其特征是主要包括如下步骤：

步骤1、经过初始对准得到载体的初始姿态；

步骤2、由光纤陀螺测得载体坐标系上的角速度输入，由加速度计测得载体坐标系上的加速度输入，

得到的角速度为

加速度为

其中ω_ib ^b为b系即载体坐标系相对于i系即地球惯性坐标系的角速度向量在b系上的投影；f_b载体所受到的非引力加速度向量在b系上的投影；

步骤3、利用步骤1中所得到的初始姿态信息计算初始姿态矩阵T，

$T=\left[\begin{array}{ccc}\cos \left(\mathrm{\ψ}\right)\cos \left(\mathrm{\γ}\right)+\sin \left(\mathrm{\ψ}\right)\sin \left(\mathrm{\θ}\right)\sin \left(\mathrm{\γ}\right)& \sin \left(\mathrm{\ψ}\right)\cos \left(\mathrm{\θ}\right)& \cos \left(\mathrm{\ψ}\right)\sin \left(\mathrm{\γ}\right)-\sin \left(\mathrm{\ψ}\right)\sin \left(\mathrm{\θ}\right)\cos \left(\mathrm{\γ}\right)\\ \cos \left(\mathrm{\ψ}\right)\sin \left(\mathrm{\θ}\right)\sin \left(\mathrm{\γ}\right)-\sin \left(\mathrm{\ψ}\right)\cos \left(\mathrm{\γ}\right)& \cos \left(\mathrm{\θ}\right)\cos \left(\mathrm{\ψ}\right)& -\sin \left(\mathrm{\ψ}\right)\sin \left(\mathrm{\γ}\right)-\cos \left(\mathrm{\ψ}\right)\sin \left(\mathrm{\θ}\right)\cos \left(\mathrm{\γ}\right)\\ -\cos \left(\mathrm{\θ}\right)\sin \left(\mathrm{\γ}\right)& \sin \left(\mathrm{\θ}\right)& \cos \left(\mathrm{\θ}\right)\cos \left(\mathrm{\γ}\right)\end{array}\right]$

其中，θ、γ、ψ分别为欧拉角意义下的俯仰、横滚和偏航姿态角；

步骤4、利用姿态矩阵T将步骤2中所得的载体系加速度计输出转化为平台系加速度f_p＝Tf_b；

步骤5、利用步骤1中得到的初始姿态信息，得出当前的各速度和角速度值，其中，速度与位置已知，北向与东向速度投影为V_x和V_y，经纬度为λ与，

其中，ω_ie为地球自转角速度，R为地球半径；ω_ie ^p为e系即地球坐标系相对于i系的角速度向量在p系即平台坐标系上的投影；ω_ep ^p为p系相对于e系的角速度向量在p系上的投影；

步骤6、利用前一时刻速度信息通过速度微分方程修正在本时刻速度，并对速度进行积分更新本时刻位置信息，并输出惯导系统计算所得的载体速度和位置信息，

速度更新微分方程为：

${\stackrel{\·}{V}}_x=f_x^p+(2{\mathrm{\ω}}_{\mathrm{iez}}^p+{\mathrm{\ω}}_{\mathrm{epz}}^p)V_y$

${\stackrel{\·}{V}}_y=f_y^p-({2\mathrm{\ω}}_{\mathrm{iez}}^p+{\mathrm{\ω}}_{\mathrm{epz}}^p)V_x$

经纬度位置计算方程为：

步骤7、将步骤6中输出的速度信息进行水平阻尼，先经过合适的水平阻尼网络H(s)，然后再将经过阻尼的速度信息引入捷联惯导系统；将地球角速度信息经过方位阻尼网络Y(s)，然后再将经过阻尼的地球角速度信息引入捷联惯导系统，

${\mathrm{\ω}}_{\mathrm{ep}}^p=\left[\begin{array}{c}-\frac{V_y}{R_M}H\left(s\right)\\ \frac{V_x}{R_N}H\left(s\right)\\ \frac{V_x}{R_N}\tan \mathrm{\φH}\left(s\right)\end{array}\right],$ ${\mathrm{\ω}}_{\mathrm{ie}}^p=\left[\begin{array}{c}0\\ \mathrm{\Ω}\cos \mathrm{\φ}\·Y\left(s\right)\\ \mathrm{\Ω}\sin \mathrm{\φ}\·Y\left(s\right)\end{array}\right]$

所述水平阻尼网络H(s)为：

$H\left(s\right)=\frac{(s+8.50\×{10}^{-4})(s+9.412\×{10}^{-2})}{(s+8.0\×{10}^{-3})(s+1.0\×{10}^{-2})}$

所述方位阻尼网络Y(s)为：

$Y\left(s\right)=1.669\·\frac{s^2+7.173\×{10}^{-5}s+21.53\×{10}^{-10}}{s^2+12\×{10}^{-5}s+36\×{10}^{-10}}$

步骤8、利用步骤7所得的各角速度以及步骤2中测得的角速度计算载体对数学平台系的姿态角速度ω_pb ^b，

${\mathrm{\ω}}_{\mathrm{pb}}^b={\mathrm{\ω}}_{\mathrm{ib}}^b+T^{-1}({\mathrm{\ω}}_{\mathrm{ie}}^p+{\mathrm{\ω}}_{\mathrm{ep}}^p);$

步骤9、利用步骤8中所提供的当前姿态角速度ω_pb ^b使用四元数更新当前的姿态矩阵T，

通过更新四元数值对应跟新姿态矩阵T：

$\left[\begin{array}{c}{\stackrel{\·}{q}}_0\\ {\stackrel{\·}{q}}_1\\ {\stackrel{\·}{q}}_2\\ {\stackrel{\·}{q}}_3\end{array}\right]=\frac{1}{2}\left[\begin{array}{cccc}0& -{\mathrm{\ω}}_{\mathrm{pbx}}^b& -{\mathrm{\ω}}_{\mathrm{pby}}^b& -{\mathrm{\ω}}_{\mathrm{pbz}}^b\\ {\mathrm{\ω}}_{\mathrm{pbx}}^b& & {\mathrm{\ω}}_{\mathrm{pbz}}^b& -{\mathrm{\ω}}_{\mathrm{pby}}^b\\ {\mathrm{\ω}}_{\mathrm{pby}}^b& -{\mathrm{\ω}}_{\mathrm{pbz}}^b& & {\mathrm{\ω}}_{\mathrm{pbx}}^b\\ {\mathrm{\ω}}_{\mathrm{pbz}}^b& {\mathrm{\ω}}_{\mathrm{pby}}^b& -{\mathrm{\ω}}_{\mathrm{pbx}}^b& \end{array}\right]\left[\begin{array}{c}{q}_0\\ q_1\\ q_2\\ q_3\end{array}\right]$

将计算所得四元数归一化，

$\left[\begin{array}{c}{q}_0\\ q_1\\ q_2\\ q_3\end{array}\right]=\frac{1}{\sqrt{q_0^2+q_1^2+q_2^2+q_3^2}}\left[\begin{array}{c}{q}_0\\ q_1\\ q_2\\ q_3\end{array}\right]$

然后更新姿态矩阵T：

$T=\begin{array}{c}\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_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_0^2-q_1^2-q_2^2+q_3^2\end{array}\right]\end{array};$

步骤10、利用步骤9中所得当前姿态矩阵T得到当前姿态，并输出载体姿态角，

θ＝arcsinT₃₂

$\mathrm{\γ}=\arctan \frac{-T_{31}}{T_{33}}$

$\mathrm{\ψ}=\arctan \frac{-T_{12}}{T_{22}}$

角度修正如下：

步骤11、循环至步骤4进入下一个时间的循环，在每一个系统周期中在步骤6与步骤10的位置输出本时刻载体的姿态，速度和位置信息。

Images