一种恒速偏频/机抖激光陀螺惯导系统导航方法

摘要

本发明属于惯性导航领域，针对正反转速率偏频方式激光陀螺惯导系统要求偏频机构瞬间反转、实现难度大，以及恒速偏频方式激光陀螺惯导系统无法克服与偏频旋转轴方向陀螺标度因数误差相关的等效漂移误差、纯惯性导航误差大的问题，公开了一种恒速偏频/机抖激光陀螺惯导系统导航方法。本发明通过机械抖动偏频激光陀螺安装、坐标系及安装关系定义、偏频旋转轴方向等效陀螺采样值计算、精确标校安装关系矩阵、精确标校机械抖动激光陀螺敏感轴的单位矢量、初始对准卡尔曼滤波器设计、纯惯导误差修正补偿的步骤，实现了快速高精度初始对准，减小了纯惯导的北向和东向位置误差，提高了导航精度。

说明书

技术领域：

本发明涉及惯性导航系统领域，特别是一种恒速偏频/机抖激光陀螺惯导系统导 航方法。

背景技术：

单轴旋转调制技术可以平均调制与旋转轴垂直的惯性器件误差，对于旋转轴轴向 的器件误差，有学者采用在线标定与补偿的方法，但标定时间普遍较长，无法满足快速性要 求。速率偏频激光陀螺惯导系统方案不仅可以克服机械抖动偏频激光陀螺过锁区随机游走 误差影响，而且正反转的方式还能抵消与偏频旋转轴方向陀螺标度因数误差相关的等效漂 移误差，提高惯导系统精度，但正反转交替旋转的偏频方式要求偏频机构瞬间反转，实现难 度大。恒速偏频方式可实现高精度的寻北，但由于偏频机构以恒定角速率旋转，造成与偏频 旋转轴方向陀螺标度因数误差相关的等效漂移误差，纯惯性导航误差大。因此，寻找一种单 轴恒速偏频旋转且能抑制偏频旋转轴方向陀螺标度因素误差的惯导系统方案具有重要意 义。

发明内容：

本发明针对现有的单轴恒速偏频激光陀螺导航系统不能解决偏频旋转轴方向陀 螺标度因素误差的问题，提出了一种恒速偏频/机抖激光陀螺惯导系统导航方法。该方案在 三个陀螺、三个加速度计构成的高精度激光陀螺恒速偏频寻北系统的基础上，增加一个不 随偏频机构旋转的固定在基座上的机械抖动偏频激光陀螺，用于敏感偏频旋转轴方向的载 体角运动，避免了沿恒速偏频旋转轴方向的陀螺标度因数误差造成的姿态误差积累。

为解决上述技术问题，本发明采用以下技术方案：

一种恒速偏频/机抖激光陀螺惯导系统导航方法，其步骤为：

步骤一：机械抖动偏频激光陀螺安装：在三个陀螺(g_x、g_y、g_z)、三个石英挠性加速 度计(a_x、a_y、a_z)构成的高精度激光陀螺恒速偏频寻北系统的基础上，安装一个不随偏频机 构旋转的固定在基座上的机械抖动偏频激光陀螺g_dz，其敏感轴与旋转轴一致，提供偏频旋 转轴方向的载体角运动信息；

步骤二：坐标系及安装关系定义：

定义n系为当地水平地理坐标系，N-E-D(北-东-地)方向；

b_g系为斜装的恒速偏频IMU(惯性测量单元)坐标系，其轴与g_x陀螺敏感轴重合， 轴在g_x陀螺与g_y陀螺敏感轴所确定的平面内且g_y陀螺敏感轴方向偏离轴一个小的安 装偏差角，轴方向符合右手规则且g_z陀螺敏感轴方向偏离轴一个小的安装偏差角；

b系为变换后的IMU坐标系，采用前-右-下方向，z_b轴与恒速偏频旋转轴重合，x_b轴、 y_b轴与偏频旋转轴正交，x_b轴与轴及z_b轴在同一平面内，与n系间的方向余弦矩阵为

b_g系、b系固联于旋转台面中并随转台连续转动，转速Ω(转台旋转轴指向下方，顺 时针方向为正)，周期T，t时刻与转台零位夹角α(t)；

b_p系为转台台体坐标系，与b系在t＝0时刻重合，与n系间的方向余弦矩阵为

为b系与b_p系之间的转换矩阵，可由α(t)表示为：

$C_b^{b_p}=\left(\begin{array}{ccc}cos\alpha \left(t\right)& -sin\alpha \left(t\right)& 0\\ sin\alpha \left(t\right)& \cos \alpha \left(t\right)& 0\\ 0& 0& 1\end{array}\right)---\left(1\right)$

步骤三：偏频旋转轴方向等效陀螺采样值计算：

b系下偏频旋转轴方向敏感角速率可表示为，

$\begin{array}{c}{\omega }_{{\mathrm{ib}}_z}^b=\frac{{\omega }_{dz}}{k_{dz}}+\Omega -[k_{dx}\cos \alpha \left(t\right)+k_{dy}\sin \alpha \left(t\right)]\frac{{\omega }_{{\mathrm{ib}}_x}^b}{k_{dz}}-\\ [-k_{dx}\sin \alpha \left(t\right)+k_{dy}\cos \alpha \left(t\right)]\frac{{\omega }_{{\mathrm{ib}}_y}^b}{k_{dz}}\end{array}---\left(2\right)$

式中，是b系下载体角速度，b_p系中机械抖动激光陀螺敏感轴单位矢量为[k_dxk_dyk_dz]^T，其敏感角速率理论值为ω_dz，上标T表示向量或矩阵的转置，Δt是采样时间间隔；

由式(2)积分可得相应的陀螺角增量采样值，根据b系中恒速偏频激光陀螺IMU提 供的垂直于旋转轴的角增量固联于b_p系中的机抖陀螺的角增量 偏频旋转机构的角位置α(t)和角增量Δα(t)，可计算出b系沿偏频旋转轴方向的 角增量

步骤四：精确标校b_g系与b系之间的安装关系矩阵设置b_g系三个恒速偏频激光 陀螺工作在机械抖动偏频模式下，利用水平姿态角γ(滚动角)和θ(俯仰角)的计算结果对 进行精确标校；

步骤五：精确标校机械抖动激光陀螺敏感轴的单位矢量[k_dxk_dyk_dz]：设置三个恒 速偏频激光陀螺工作在机械抖动偏频模式下，保持不变时有：

$\begin{array}{c}{\omega }_{dz}=\left(\begin{array}{ccc}{k}_{dx}& k_{dy}& d_{dz}\end{array}\right){\omega }_{{\mathrm{ib}}_p}^{b_p}\\ =\left(\begin{array}{ccc}{k}_{dx}& k_{dy}& k_{dz}\end{array}\right)C_b^{b_p}{\omega }_{ib}^b\end{array}---\left(3\right)$

式中，表示b_p系的旋转角速率；

整个惯导系统按照不同的倾斜方式置于单轴转台上，单轴转台正反方向旋转，消 除地球自转角速度及陀螺常值漂移误差影响，根据即 可估计出参数[k_dxk_dyk_dz]；

步骤六：初始对准卡尔曼滤波器设计：

选取三个姿态误差(φ_N、φ_E、φ_D)和三个速度误差(δV_N、δV_E、δV_D)作为误差状态，器 件误差中，选取三个加速度计零偏作为误差状态，选取偏频旋转轴方 向等效陀螺采样值常值零偏偏差耦合造成的北向等效陀螺漂移ε_N作为误差状态，即

$x={\left(\begin{array}{ccccccccccc}{\phi }_N& {\phi }_E& {\phi }_D& {\mathrm{\delta V}}_N& {\mathrm{\delta V}}_E& {\mathrm{\delta V}}_D& {\mathrm{\delta f}}_x^b& {\mathrm{\delta f}}_y^b& {\mathrm{\delta f}}_z^b& {\mathrm{\delta \omega }}_{ibz}^b& {ϵ}_N\end{array}\right)}^T---\left(4\right)$

构建系统状态方程如下：

$\stackrel{·}{x}=Fx+Gw---\left(5\right)$

式中，x表示系统误差状态，表示系统误差状态微分，F表示系统误差矩阵，G表示 系统噪声输入矩阵，w表示系统噪声：

w＝[w_gxw_gyw_gzw_axw_ayw_az]^T(6)

式中，w_gx、w_gy和w_gz为陀螺测量噪声，w_ax、w_ay、w_az为加速度计测量噪声；

选取n系下速度误差δV_N、δV_E和δV_D作为观测量，构建观测方程如下：

z＝Hx+υ(7)

式中，z为观测量，H为测量矩阵，υ为测量噪声；

步骤七：纯惯导误差修正补偿：

利用初始对准卡尔曼滤波对和ε_N的估计值，在纯惯导过 程中进行修正补偿，完成导航解算，修正补偿方法如下：

${\hat{f}}^b={\tilde{f}}^b-\delta {\hat{f}}^b---\left(8\right)$

${\hat{\omega }}_{{\mathrm{ib}}_z}^b={\tilde{\omega }}_{{\mathrm{ib}}_z}^b-\delta {\hat{\omega }}_{{\mathrm{ib}}_z}^b---\left(9\right)$

${\hat{\omega }}_{in,N}^n={\tilde{\omega }}_{in,N}^n-{\widehat{ϵ}}_N.---\left(10\right)$

步骤三中偏频旋转轴方向等效陀螺采样值计算中求解方法为：

机抖陀螺敏感角速率理论值可表示为：

${\omega }_{dz}=k_{dx}{\omega }_{{\mathrm{ib}}_{px}}^{b_p}+k_{dy}{\omega }_{{\mathrm{ib}}_{py}}^{b_p}+k_{dz}{\omega }_{{\mathrm{ib}}_{pz}}^{b_p}---\left(11\right)$

又${\omega }_{{\mathrm{ib}}_p}^{b_p}=C_b^{b_p}{\omega }_{ib}^b-{\omega }_{b_{p}b}^{b_p},$式中，${\omega }_{b_{p}b}^{b_p}={\left(\begin{array}{ccc}0& 0& \Omega \end{array}\right)}^T,$则有：

${\omega }_{dz}=-\left(\begin{array}{ccc}{k}_{dx}& k_{dy}& k_{dz}\end{array}\right){\left(\begin{array}{ccc}0& 0& \Omega \end{array}\right)}^T+\left(\begin{array}{ccc}{k}_{dx}& k_{dy}& k_{dz}\end{array}\right)C_b^{b_p}{\left(\begin{array}{ccc}{\omega }_{{\mathrm{ib}}_x}^b& {\omega }_{{\mathrm{ib}}_y}^b& {\omega }_{{\mathrm{ib}}_z}^b\end{array}\right)}^T---\left(12\right)$

把式(1)代入式(12)变换后可得的表达式。

步骤四中所述的精确标校方法为：

设真实的IMU坐标系为b′系，则有：

${\tilde{C}}_{b_p}^n=C_{b^{\prime }}^n{C}_{b_p}^b---\left(13\right)$

式中，是b_p与n系间的方向余弦矩阵的真实值，可表示为：

${\tilde{C}}_{b_p}^n=C_{b_p}^n\left(\begin{array}{ccc}1& 0& \delta \theta \\ 0& 1& -\delta \gamma \\ -\delta \theta & \delta \gamma & 1\end{array}\right)---\left(14\right)$

是b′系与n系间的方向余弦矩阵，可表示为：

$C_{b^{\prime }}^n=C_{b_p}^n{C}_b^{b_p}{C}_{b^{\prime }}^b---\left(15\right)$

将式(14)和(15)代入式(13)两边同时消去得：

$\left(\begin{array}{ccc}1& 0& \delta \theta \\ 0& 1& -\delta \gamma \\ -\delta \theta & \delta \gamma & 1\end{array}\right)=C_b^{b_p}{C}_{b^{\prime }}^b{C}_{b_p}^b---\left(16\right)$

设ξ_x和ξ_y为b系和b′系之间的转换的小角度偏差，则

$C_{b^{\prime }}^b=\left(\begin{array}{ccc}1& 0& {\xi }_y\\ 0& 1& -{\xi }_x\\ -{\xi }_y& {\xi }_x& 1\end{array}\right)---\left(17\right)$

将式(1)和式(17)代入式(16)解得：

$\left(\begin{array}{c}\delta \gamma ={\xi }_x\cos \alpha \left(t\right)+{\xi }_y\sin \alpha \left(t\right)\\ \delta \theta ={\xi }_y\cos \alpha \left(t\right)-{\xi }_x\sin \alpha \left(t\right)\end{array}\right)---\left(18\right)$

静基座下分别选取α(t)＝0和α(t)＝π两个时刻，从而解得：

$\left(\begin{array}{c}{\xi }_x=\frac{1}{2}({\tilde{\gamma }}_{\alpha \left(t\right)=0}-{\tilde{\gamma }}_{\alpha \left(t\right)=\pi })\\ {\xi }_y=\frac{1}{2}({\tilde{\theta }}_{\alpha \left(t\right)=0}-{\tilde{\theta }}_{\alpha \left(t\right)=\pi })\end{array}\right)---\left(19\right)$

则可根据式对进行精确标校。

步骤六中所述初始对准卡尔曼滤波器设计中系统状态方程和观测方程构建方法 为：

系统状态方程构建如下：

$\left(\begin{array}{c}{\stackrel{·}{\phi }}^n={\omega }_{in}^n\times {\phi }^n+\left(\begin{array}{c}{ϵ}_N\\ 0\\ 0\end{array}\right)-C_b^n\left(\begin{array}{c}0\\ 0\\ {\mathrm{\delta \omega }}_{ibz}^b\end{array}\right)\\ \delta {\stackrel{·}{v}}^n=f^n\times {\phi }^n+C_b^n{\mathrm{\delta f}}^b\\ \delta {\stackrel{·}{f}}^b=0\\ \delta {\stackrel{·}{\omega }}_{ibz}^b=0,{\stackrel{·}{ϵ}}_N=0\end{array}\right)---\left(20\right)$

式中，φⁿ＝[φ_Nφ_Eφ_D]^T代表失准角，fⁿ表示n系下的比力值，是n系的旋转角 速率；

根据系统误差方程得到的系统状态方程中系统误差矩阵F和系统噪声 输入矩阵G相应为：

$F={\left(\begin{array}{ccccc}{\left[-{\omega }_{in}^n\times \right]}_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& {\left(\begin{array}{ccc}1& 0& 0\end{array}\right)}^T& C_b^n{\left(\begin{array}{ccc}0& 0& 1\end{array}\right)}^T\\ {\left[f^n\times \right]}_{3\times 3}& 0_{3\times 3}& C_b^n& 0_{3\times 1}& 0_{3\times 1}\\ 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 1}& 0_{3\times 1}\\ 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0\end{array}\right)}_{11\times 11}---\left(21\right)$

$G={\left(\begin{array}{cc}{(-C_b^n{C}_g^b)}_{3\times 3}& 0_{3\times 3}\\ 0_{3\times 3}& {\left(C_b^n{C}_g^b\right)}_{3\times 3}\\ 0_{5\times 3}& 0_{5\times 3}\end{array}\right)}_{11\times 6}---\left(22\right)$

卡尔曼滤波观测方程构建如下：

观测量z＝[δV_NδV_EδV_D]^T，则在观测方程z＝Hx+υ中：

$H={\left(\begin{array}{ccccccccccc}0& 0& 0& 1& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 1& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 1& 0& 0& 0& 0& 0\end{array}\right)}_{3\times 11}---\left(23\right)$

υ＝[υ_Nυ_Eυ_D]^T。(24)

与现有技术相比，本发明具有以下优点：

(1)在三个陀螺三个加速度计构成的高精度激光陀螺恒速偏频寻北系统的基础上 增加一个不随偏频机构旋转的固定在基座上的机械抖动偏频激光陀螺，改善偏频轴方向载 体角速度测量精度，采用单轴恒速偏频旋转方式，避免了速率偏频技术要求偏频机构瞬间 反转的难点；

(2)以姿态角误差、速度误差、加速度计零偏、偏频旋转轴方向陀螺漂移和偏差耦 合造成的北向等效陀螺漂移为误差状态，以速度误差为观测量，构建卡尔曼滤波器，对加速 度计零偏和偏差耦合造成的北向等效陀螺漂移进行在线标定补偿，对偏频旋转轴方向陀螺 漂移进行预先标定补偿，改善了现有的卡尔曼滤波初始对准方位角波动幅度大的问题，进 一步提高了恒速偏频激光陀螺惯导系统的导航定位精度；

(3)IMU与转台安装关系的精确标校，改善水平姿态角波动幅度。

附图说明：

1、图1为本发明方案的流程图；

2、图2为滚动角收敛曲线对比图；

3、图3为俯仰角收敛曲线对比图；

4、图4为改进卡尔曼滤波与标准卡尔曼滤波初始对准方位角收敛曲线对比图；

5、图5为4小时纯惯导北向速度误差曲线对比图；

6、图6为4小时纯惯导东向速度误差曲线对比图；

7、图7为4小时纯惯导位置误差曲线对比图；

8、图8为第一组24小时纯惯导速度误差曲线图；

9、图9为第一组24小时纯惯导位置误差曲线图；

10、图10为第二组24小时纯惯导速度误差曲线图；

11、图11为第二组24小时纯惯导位置误差曲线图。

具体实施方式：

下面结合附图对本发明中的方法作进一步详细描述。

步骤一：机械抖动偏频激光陀螺安装：在三个陀螺(g_x、g_y、g_z)、三个石英挠性加速 度计(a_x、a_y、a_z)构成的高精度激光陀螺恒速偏频寻北系统的基础上，安装一个不随偏频机 构旋转的固定在基座上的机械抖动偏频激光陀螺g_dz，其敏感轴与旋转轴一致，提供偏频旋 转轴方向的载体角运动信息；

步骤二：坐标系及安装关系定义：

定义n系为当地水平地理坐标系，N-E-D(北-东-地)方向；

b_g系为斜装的恒速偏频IMU(惯性测量单元)坐标系，其轴与g_x陀螺敏感轴重合， 轴在g_x陀螺与g_y陀螺敏感轴所确定的平面内且g_y陀螺敏感轴方向偏离轴一个小的安 装偏差角，轴方向符合右手规则且g_z陀螺敏感轴方向偏离轴一个小的安装偏差角；

b系为变换后的IMU坐标系，采用前-右-下方向，z_b轴与恒速偏频旋转轴重合，x_b轴、 y_b轴与偏频旋转轴正交，x_b轴与轴及z_b轴在同一平面内，与n系间的方向余弦矩阵为

b_g系、b系固联于旋转台面中并随转台连续转动，转速Ω(转台旋转轴指向下方，顺 时针方向为正)，周期T，t时刻与转台零位夹角α(t)；

b_p系为转台台体坐标系，与b系在t＝0时刻重合，与n系间的方向余弦矩阵为

为b系与b_p系之间的转换矩阵，可由α(t)表示为：

$C_b^{b_p}=\left(\begin{array}{ccc}cos\alpha \left(t\right)& -sin\alpha \left(t\right)& 0\\ sin\alpha \left(t\right)& \cos \alpha \left(t\right)& 0\\ 0& 0& 1\end{array}\right)---\left(25\right)$

步骤三：偏频旋转轴方向等效陀螺采样值计算：

b系下偏频旋转轴方向敏感角速率可表示为，

$\begin{array}{c}{\omega }_{{\mathrm{ib}}_z}^b=\frac{{\omega }_{dz}}{k_{dz}}+\Omega -[k_{dx}\cos \alpha \left(t\right)+k_{dy}\sin \alpha \left(t\right)]\frac{{\omega }_{{\mathrm{ib}}_x}^b}{k_{dz}}-\\ [-k_{dx}\sin \alpha \left(t\right)+k_{dy}\cos \alpha \left(t\right)]\frac{{\omega }_{{\mathrm{ib}}_y}^b}{k_{dz}}\end{array}---\left(26\right)$

式中，是b系下载体角速度，b_p系中机械抖动激光陀螺敏感轴单位矢量为[k_dxk_dyk_dz]^T，其敏感角速率理论值为ω_dz，上标T表示向量或矩阵的转置，Δt是采样时间间隔；

由式(2)积分可得相应的陀螺角增量采样值，根据b系中恒速偏频激光陀螺IMU提 供的垂直于旋转轴的角增量固联于b_p系中的机抖陀螺的角增量 偏频旋转机构的角位置α(t)和角增量Δα(t)，可计算出b系沿偏频旋转轴方向的 角增量

偏频旋转轴方向等效陀螺采样值计算中求解方法为：

机抖陀螺敏感角速率理论值可表示为：

${\omega }_{dz}=k_{dx}{\omega }_{{\mathrm{ib}}_{px}}^{b_p}+k_{dy}{\omega }_{{\mathrm{ib}}_{py}}^{b_p}+k_{dz}{\omega }_{{\mathrm{ib}}_{pz}}^{b_p}---\left(27\right)$

又${\omega }_{{\mathrm{ib}}_p}^{b_p}=C_b^{b_p}{\omega }_{ib}^b-{\omega }_{b_{p}b}^{b_p},$式中，${\omega }_{b_{p}b}^{b_p}={\left(\begin{array}{ccc}0& 0& \Omega \end{array}\right)}^T,$则有：

${\omega }_{dz}=-\left(\begin{array}{ccc}{k}_{dx}& k_{dy}& k_{dz}\end{array}\right){\left(\begin{array}{ccc}0& 0& \Omega \end{array}\right)}^T+\left(\begin{array}{ccc}{k}_{dx}& k_{dy}& k_{dz}\end{array}\right)C_b^{b_p}{\left(\begin{array}{ccc}{\omega }_{{\mathrm{ib}}_x}^b& {\omega }_{{\mathrm{ib}}_y}^b& {\omega }_{{\mathrm{ib}}_z}^b\end{array}\right)}^T---\left(28\right)$

把式(25)代入式(28)变换后可得的表达式；

步骤四：精确标校b_g系与b系之间的安装关系矩阵设置b_g系三个恒速偏频激光 陀螺工作在机械抖动偏频模式下，利用水平姿态角γ(滚动角)和θ(俯仰角)的计算结果对 进行精确标校；

精确标校具体方法为：

设真实的IMU坐标系为b′系，则有：

${\tilde{C}}_{b_p}^n=C_{b^{\prime }}^n{C}_{b_p}^b---\left(29\right)$

式中，是b_p与n系间的方向余弦矩阵的真实值，可表示为：

${\tilde{C}}_{b_p}^n=C_{b_p}^n\left(\begin{array}{ccc}1& 0& \delta \theta \\ 0& 1& -\delta \gamma \\ -\delta \theta & \delta \gamma & 1\end{array}\right)---\left(30\right)$

是b′系与n系间的方向余弦矩阵，可表示为：

$C_{b^{\prime }}^n=C_{b_p}^n{C}_b^{b_p}{C}_{b^{\prime }}^b---\left(31\right)$

将式(30)和代(31)入式(29)两边同时消去得：

$\left(\begin{array}{ccc}1& 0& \delta \theta \\ 0& 1& -\delta \gamma \\ -\delta \theta & \delta \gamma & 1\end{array}\right)=C_b^{b_p}{C}_{b^{\prime }}^b{C}_{b_p}^b---\left(32\right)$

设ξ_x和ξ_y为b系和b′系之间的转换的小角度偏差，则

$C_{b^{\prime }}^b=\left(\begin{array}{ccc}1& 0& {\xi }_y\\ 0& 1& -{\xi }_x\\ -{\xi }_y& {\xi }_x& 1\end{array}\right)---\left(33\right)$

将式(25)和式(33)代入式(32)解得：

$\left(\begin{array}{c}\delta \gamma ={\xi }_x\cos \alpha \left(t\right)+{\xi }_y\sin \alpha \left(t\right)\\ \delta \theta ={\xi }_y\cos \alpha \left(t\right)-{\xi }_x\sin \alpha \left(t\right)\end{array}\right)---\left(34\right)$

静基座下分别选取α(t)＝0和α(t)＝π两个时刻，从而解得：

$\left(\begin{array}{c}{\xi }_x=\frac{1}{2}({\tilde{\gamma }}_{\alpha \left(t\right)=0}-{\tilde{\gamma }}_{\alpha \left(t\right)=\pi })\\ {\xi }_y=\frac{1}{2}({\tilde{\theta }}_{\alpha \left(t\right)=0}-{\tilde{\theta }}_{\alpha \left(t\right)=\pi })\end{array}\right)---\left(35\right)$

则可根据式对进行精确标校；

步骤五：精确标校机械抖动激光陀螺敏感轴的单位矢量[k_dxk_dyk_dz]：设置三个恒 速偏频激光陀螺工作在机械抖动偏频模式下，保持不变时有：

$\begin{array}{c}{\omega }_{dz}=\left(\begin{array}{ccc}{k}_{dx}& k_{dy}& d_{dz}\end{array}\right){\omega }_{{\mathrm{ib}}_p}^{b_p}\\ =\left(\begin{array}{ccc}{k}_{dx}& k_{dy}& k_{dz}\end{array}\right)=C_b^{b_p}{\omega }_{ib}^b\end{array}---\left(36\right)$

式中，表示b_p系的旋转角速率；

整个惯导系统按照不同的倾斜方式置于单轴转台上，单轴转台正反方向旋转，消 除地球自转角速度及陀螺常值漂移误差影响，根据即 可估计出参数[k_dxk_dyk_dz]；、

步骤六：初始对准卡尔曼滤波器设计：

选取三个姿态误差(φ_N、φ_E、φ_D)和三个速度误差(δV_N、δV_E、δV_D)作为误差状态，器 件误差中，选取三个加速度计零偏作为误差状态，选取偏频旋转轴方 向等效陀螺采样值常值零偏偏差耦合造成的北向等效陀螺漂移ε_N作为误差状态，即

$x={\left(\begin{array}{ccccccccccc}{\phi }_N& {\phi }_E& {\phi }_D& {\mathrm{\delta V}}_N& {\mathrm{\delta V}}_E& {\mathrm{\delta V}}_D& {\mathrm{\delta f}}_x^b& {\mathrm{\delta f}}_y^b& {\mathrm{\delta f}}_z^b& {\mathrm{\delta \omega }}_{ibz}^b& {ϵ}_N\end{array}\right)}^T---\left(37\right)$

构建系统状态方程如下：

$\stackrel{·}{x}=Fx+Gw---\left(38\right)$

式中，x表示系统误差状态，表示系统误差状态微分，F表示系统误差矩阵，G表示 系统噪声输入矩阵，w表示系统噪声：

w＝[w_gxw_gyw_gzw_axw_ayw_az]^T(39)

式中，w_gx、w_gy和w_gz为陀螺测量噪声，w_ax、w_ay、w_az为加速度计测量噪声；

选取n系下速度误差δV_N、δV_E和δV_D作为观测量，构建观测方程如下：

z＝Hx+υ(40)

式中，z为观测量，H为测量矩阵，υ为测量噪声；

卡尔曼滤波系统状态方程和观测方程构建方法为：

系统状态方程构建如下：

$\left(\begin{array}{c}{\stackrel{·}{\phi }}^n=-{\omega }_{in}^n\times {\phi }^n+\left(\begin{array}{c}{ϵ}_N\\ 0\\ 0\end{array}\right)-C_b^n\left(\begin{array}{c}0\\ 0\\ {\mathrm{\delta \omega }}_{ibz}^b\end{array}\right)\\ \delta {\stackrel{·}{v}}^n=f^n\times {\phi }^n+C_b^n{\mathrm{\delta f}}^b\\ \delta {\stackrel{·}{f}}^b=0\\ \delta {\stackrel{·}{\omega }}_{ibz}^b=0,{\stackrel{·}{ϵ}}_N=0\end{array}\right)---\left(41\right)$

式中，φⁿ＝[φ_Nφ_Eφ_D]^T代表失准角，fⁿ表示n系下的比力值，是n系的旋转角 速率；

根据系统误差方程得到的系统状态方程中系统误差矩阵F和系统噪声 输入矩阵G相应为：

$F={\left(\begin{array}{ccccc}{\left[-{\omega }_{in}^n\times \right]}_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& {\left(\begin{array}{ccc}1& 0& 0\end{array}\right)}^T& C_h^n{\left(\begin{array}{ccc}0& 0& 1\end{array}\right)}^T\\ {\left[f^n\times \right]}_{3\times 3}& 0_{3\times 3}& C_b^n& 0_{3\times 1}& 0_{3\times 1}\\ 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 1}& 0_{3\times 1}\\ 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0\end{array}\right)}_{11\times 11}---\left(42\right)$

$G={\left(\begin{array}{cc}{(-C_b^n{C}_g^b)}_{3\times 3}& 0_{3\times 3}\\ O_{3\times 3}& {\left(C_b^n{C}_g^b\right)}_{3\times 3}\\ O_{5\times 3}& 0_{5\times 3}\end{array}\right)}_{11\times 6}---\left(43\right)$

卡尔曼滤波观测方程构建如下：

观测量z＝[δV_NδV_EδV_D]^T，则在观测方程z＝Hx+υ中：

$H={\left(\begin{array}{ccccccccccc}0& 0& 0& 1& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 1& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 1& 0& 0& 0& 0& 0\end{array}\right)}_{3\times 11}---\left(44\right)$

υ＝[υ_Nυ_Eυ_D]^T(45)

步骤七：纯惯导误差修正补偿：

利用初始对准卡尔曼滤波对和ε_N的估计值，在纯惯导过 程中进行修正补偿，完成导航解算，修正补偿方法如下：

${\hat{f}}^b={\tilde{f}}^b-\delta {\hat{f}}^b---\left(46\right)$

${\hat{\omega }}_{{\mathrm{ib}}_z}^b={\tilde{\omega }}_{{\mathrm{ib}}_z}^b-\delta {\hat{\omega }}_{{\mathrm{ib}}_z}^b---\left(47\right)$

${\hat{\omega }}_{in,N}^n={\tilde{\omega }}_{in,N}^n-{\widehat{ϵ}}_N.---\left(48\right)$

下面是利用实验室自主设计的高精度恒速偏频激光陀螺寻北系统对本文提出的 参数标校方法、初始对准算法和纯惯导精度进行半实物仿真验证评估。方法如下：

1)在实验室内进行恒速偏频静基座实验，转台转速设定为40°/s，采样频率设定为 500Hz，采集5组实际的5小时数据，两组实际的24小时数据。

2)机抖陀螺角增量输出从机械抖动激光陀螺静态数据中获得，采用理论值加机抖 陀螺静态噪声和漂移实际采样数据的方式仿真生成。

3)用本专利提出的方案进行初始对准及纯惯性导航实验，进行验证评估。

初始对准水平姿态角收敛曲线如图2、图3，其中(a)为用本文方法精确标校后 的结果，(b)为原标校方法的结果。精确标校后，水平姿态角收敛曲线波动从10角秒减小 到3角秒，从而有效地改善了初始对准水平姿态角精度。

方位角收敛曲线如图4，粗实线是采用本专利提出的改进的卡尔曼滤波初始对准 算法，细实线是采用标准卡尔曼滤波初始对准算法。

5组数据初始对准10分钟方位角结果如表1所示：

表15组数据初始对准10分钟方位角结果

实验编号 方位角(°) 1 264.167 2 264.165 3 264.164 4 264.166 5 264.168 标准差(1σ) 5.7角秒

由图4和表1可知，本专利提出的初始对准算法不仅能有效地减小方位角周期性波 动幅度，且10min精度优于10角秒。

图5、图6和图7中，粗实线是20分钟初始对准补偿加速度计零偏、偏差耦合的北向 等效陀螺漂移和预先标定补偿偏频旋转轴方向陀螺漂移后的纯惯导4小时结果。细实线是 10分钟初始对准仅补偿加速度计零偏的纯惯导4小时结果。由图7可以看出，补偿陀螺漂移 后北向位置误差最大值从774米减小到96米，东向位置误差最大值从252米减小到89米。

5组数据纯惯导4小时位置误差最大值如表2所示：

表25组数据纯惯导4小时位置误差结果

实验编号 北向位置误差(m) 东向位置误差(m) 1 153 122 2 144 37 3 158 111 4 96 89 5 127 184 均值 135.6 108.6

由表2可知5次实验结果表明，4小时纯惯导北向、东向位置误差最大值均小于 200m。

图8、图9和图10、图11分别是两组长时间数据进行20分钟初始对准补偿加速度计 零偏、偏差耦合的北向等效陀螺漂移和预先标定补偿偏频旋转轴方向陀螺漂移后的纯惯导 24小时结果，速度误差均小于0.2米/秒，北向位置误差优于450米/24小时，东向位置误差优 于750米/24小时。

以上仅是本发明的优选实施方式，本发明的保护范围并不仅限于上述实施例，凡 属于本发明思路下的技术方案均属于本发明的保护范围。应当指出，对于本技术领域的普 通技术人员来说，在不脱离本发明原理前提下的若干改进和润饰，应当视为落入本发明的 保护范围。