CN102538821A - 一种快速、参数分段式捷联惯性导航***自对准方法 - Google Patents

Abstract

一种快速、参数分段式捷联惯性导航***自对准方法，利用解析法完成粗对准过程，接着进行水平精对准和方位精对准，方位精对准分为三个阶段，每个阶段根据该阶段的特性选用不同的调整时间和衰减系数来确定对准参数，基于对准参数计算加速度修正量和角速度修正量，对输入的加速度信息和角速度信息进行修正，利用修正后的信息进行四元数解算得到初始姿态角，完成精对准过程。本发明通过采集陀螺仪和加速度计数据利用解析法和罗经法完成初始对准，无需利用其他传感器信息，保证初始对准的安全性和保密性；本发明针对不同阶段的对准特性选用最佳的调整时间和衰减系数，从而计算其他的对准参数，提高对准曲线的快速收敛性以及抗干扰性能。

Description

一种快速、参数分段式捷联惯性导航***自对准方法

技术领域

本发明属于捷联惯性导航技术领域，涉及初始对准，为一种快速、参数分段式捷联惯性导航***自对准方法。

背景技术

捷联惯性导航，简称捷联惯导，捷联惯导***的一个重要的问题是初始对准。在尽可能短的时间内达到最高的对准精度，是捷联惯导初始对准技术所追求的目标。捷联惯导的初始对准就是确定初始时刻的姿态矩阵。

初始对准有精度和快速性两个方面的要求。为了满足高精度要求，希望惯性传感器具有尽可能高的精度和稳定性，并希望***对外界扰动不敏感。为提高***的精度，还希望初始对准时能对陀螺漂移、加速度计零偏以及它们的标度系数进行标定和补偿。显然，精度和快速性这两方面的要求是互相矛盾的，因此需要合理地进行***设计，尽可能兼顾这两方面的要求，以期求得满意的效果。

利用罗经效应进行自主对准的过程一般分为粗对准和精对准两步进行。粗对准要求尽快将数学平台调整到某一个精度范围内，主要指标是较短的对准时间；精对准在粗对准的基础上进行，一般先调水平，然后再进行方位精对准，主要指标是对准精度。精对准结束时的精度就是惯导***进入导航工作模式时的初始精度。

本发明针对传统的自对准方法中出现的对准过程中抖动、对准时间较长以及抗干扰能力不高等问题，本发明将精对准过程分为了三个阶段，每个阶段选用不同的调整时间和衰减系数以根据对准不同阶段的特性设计最佳的参数已达到快速、抗干扰的目的。

发明内容

本发明的技术解决问题是：针对传统的自对准方法中出现的对准过程抖动、对准时间较长以及抗干扰能力差等问题，提供一种快速、参数分段式捷联惯性导航***自对准方法，特别适用于捷联惯性导航***完成无源自主快速对准过程。

本发明的技术解决方案为：一种快速、参数分段式捷联惯性导航***自对准方法，分为粗对准和精对准两步，首先利用解析法完成粗对准过程，然后进行水平精对准和方位精对准，基于精对准参数计算加速度修正量和角速度修正量，对输入捷联惯性导航***的加速度信息和角速度信息进行修正，利用修正后的信息进行四元数解算得到初始姿态角，完成精对准过程，包括以下步骤：

1)对准前首先确定自对准的精对准参数，包括各阶段阻尼比ξ、衰减系数σ、调整时间t_s、对准时间T_i以及由所述参数计算出的各阶段其他阻尼参数；

精对准分段进行，按照选用对准参数的不同设计水平精对准为一个阶段，方位精对准为三个阶段，具体如下：

水平精对准阶段：对准时间为T₁＝100s

$\mathrm{\ξ}=\frac{1}{\sqrt{2}},$ σ＝0.045， $t_s=\frac{3}{\mathrm{\σ}}=66.7$

方位精对准第一阶段：对准时间T₂＝300s

$\mathrm{\ξ}=\frac{1}{\sqrt{2}},$ σ＝0.02， $t_s=\frac{3}{\mathrm{\σ}}=150$

方位精对准第二阶段：对准时间T₃＝300s

$\mathrm{\ξ}=\frac{1}{\sqrt{2}},$ σ＝0.015， $t_s=\frac{3}{\mathrm{\σ}}=200$

方位精对准第三阶段：对准时间T₄＝300s

$\mathrm{\ξ}=\frac{1}{\sqrt{2}},$ σ＝0.01， $t_s=\frac{3}{\mathrm{\σ}}=300$

根据阻尼比ξ和衰减系数σ按下式计算各阶段其他阻尼参数：

捷联惯导***采用东北天坐标系，根据捷联惯导***水平罗经对准原理，得到水平精对准阶段***各阶的阻尼参数：

K₁＝3σ

$K_2=\frac{{\mathrm{\σ}}^2}{{\mathrm{\ω}}_s^2}(2+\frac{1}{{\mathrm{\ξ}}^2})-1$

$K_3=\frac{{\mathrm{\σ}}^3}{{\mathrm{g\ξ}}^2}$

根据捷联惯导***方位罗经对准原理，得到方位精对准阶段***各阶的阻尼参数：

K′₁＝K′₄＝2σ

${K^{\′}}_2=\frac{{4\mathrm{\σ}}^2}{{\mathrm{\ω}}_s^2}-1$

${K^{\′}}_3=\frac{{4\mathrm{\σ}}^4}{g}$

其中，

g为重力加速度，R为地球半径；

2)利用解析法进行粗对准，确定初始姿态矩阵以及初始航向角

初始纵摇角θ₀和初始横摇角γ₀；

3)在步骤2)粗对准的基础上开始精对准过程，按照精对准所分阶段，每个阶段均由对准回路计算出导航坐标系下加速度修正信息

和角速度修正信息所述导航坐标系为东北天坐标系；

4)将步骤3)中计算所得的加速度修正信息

和角速度修正信息

转换到载体坐标系上，即

为导航坐标到载体坐标的转换矩阵，从而对捷联惯性导航***中的加速度计的比力

和陀螺仪的角速度

进行修正，即

5)利用步骤4)中修正过的比力信息

和角速度信息

使用四元数更新算法计算本姿态解算周期的姿态矩阵，从而确定对应的纵摇角、横摇角和航向角，输出更新后的姿态；

6)步骤3)至5)为一个姿态解算周期，重复步骤3)至5)进行下一个姿态解算周期的计算，得到对应姿态解算周期载体的纵摇角、横摇角和航向角，直到周期总时间达到步骤1)中所确定的各阶段的对准时间T_i，完成精对准，输出最后一个周期更新的姿态；

7)完成水平精对准和方位精对准后对准过程结束，得到精确的姿态角，实现捷联惯性导航***自对准。

所述的解析法粗对准为：

当地纬度L为已知量，导航坐标系采用东北天坐标系，则重力加速度g和地球自转角速度ω_ie在导航***中的分量都是确定已知的，表示为：

gⁿ＝[0 0 -g]^T

${\mathrm{\ω}}_{\mathrm{ie}}^n={\left[\begin{array}{ccc}0& \mathrm{\Ω}\cos L& \mathrm{\Ω}\sin L\end{array}\right]}^T$

其中，gⁿ表示重力加速度在导航坐标系下的投影，

表示地球自转角速度在导航坐标系下的投影，Ω为地球自转角速率，g为重力加速度的幅值；

解析粗对准利用已知的gⁿ，

和捷联惯性导航***传感器的测量值来估计粗对准初始姿态矩阵

对传感器测量值取平均以提高对准精度，用a^b和

分别表示加速度计和陀螺仪的测量平均值；

解析法粗对准方法采用以下两种方法的一种：

(a)利用三个互相正交的向量g，g×ω_ie和(g×ω_ie)×g以及三个互相正交的向量a^b，a^b×ω^b和(a^b×ω^b)×a^b计算

${C^{\′}}_{b\left(0\right)}^n={\left[\begin{array}{c}{\left(g^n\right)}^T\\ {(g^n\×{\mathrm{\ω}}_{\mathrm{ie}}^n)}^T\\ {[(g^n\×{\mathrm{\ω}}_{\mathrm{ie}}^n)\×g^n]}^T\end{array}\right]}^{-1}\left[\begin{array}{c}{\left(a^b\right)}^T\\ {(a^b\×{\mathrm{\ω}}_{\mathrm{ie}}^b)}^T\\ {[(a^b\×{\mathrm{\ω}}_{\mathrm{ie}}^b)\×a^b]}^T\end{array}\right]$

对姿态矩阵

进行归一化处理得到粗对准初始姿态矩阵

$C_{b\left(0\right)}^n=C_{b\left(0\right)}^n{\left[{\left(C_{b\left(0\right)}^n\right)}^T{C}_{b\left(0\right)}^n\right]}^{-\frac{1}{2}}$

(b)由测量值直接计算

将

写成列向量的形式：

$C_{b\left(0\right)}^n=\left[\begin{array}{ccc}{C}_1& C_2& C_3\end{array}\right]$

其中C₁、C₂、C₃为构成的三个列向量，且彼此间具有正交约束；

设k时刻陀螺仪和加速度计的测量值分别记为ω^b(k)和a^b(k)，采样次数为N次，采用正交约束下的最小二乘法估计

的三个分列向量：

$C_3=-\frac{1}{\mathrm{\λ}}\underset{k=1}{\overset{N}{\mathrm{\Σ}}}{a}^b\left(k\right)$

$C_2=\frac{1}{\mathrm{\η}}(\underset{k=1}{\overset{N}{\mathrm{\Σ}}}{\mathrm{\ω}}^b\left(k\right)+\mathrm{\ρ}\hat{z})$

$C_1=C_2\×C_3=\frac{1}{\mathrm{\η}}\underset{k=1}{\overset{N}{\mathrm{\Σ}}}{\mathrm{\ω}}^b\left(k\right)\×C_3$

$\mathrm{\λ}={\left[{\left(\underset{k=1}{\overset{N}{\mathrm{\Σ}}}{a}^b\left(k\right)\right)}^T\left(\underset{k=1}{\overset{N}{\mathrm{\Σ}}}{a}^b\left(k\right)\right)\right]}^{\frac{1}{2}}$

$\mathrm{\ρ}=\frac{1}{\mathrm{\λ}}{\left(\underset{k=1}{\overset{N}{\mathrm{\Σ}}}{a}^b\left(k\right)\right)}^T\left(\underset{k=1}{\overset{N}{\mathrm{\Σ}}}{\mathrm{\ω}}^b\left(k\right)\right)$

$\mathrm{\η}={[{\left(\underset{k=1}{\overset{N}{\mathrm{\Σ}}}{\mathrm{\ω}}^b\left(k\right)\right)}^T\left(\underset{k=1}{\overset{N}{\mathrm{\Σ}}}{\mathrm{\ω}}^b\left(k\right)\right)-{\mathrm{\ρ}}^2]}^{\frac{1}{2}}$

此时便得到了

由于已经是正交矩阵，所以无需正交化。

步骤3)中所述的加速度修正量 $f_c^n={\left(\begin{array}{ccc}{f}_{\mathrm{cx}}^n& f_{\mathrm{cy}}^n& f_{\mathrm{cz}}^n\end{array}\right)}^T$ 和角速度修正量 ${\mathrm{\ω}}_c^n={\left(\begin{array}{ccc}{\mathrm{\ω}}_{\mathrm{cx}}^n& {\mathrm{\ω}}_{\mathrm{cy}}^n& {\mathrm{\ω}}_{\mathrm{cz}}^n\end{array}\right)}^T,$ 计算如下：

水平精对准时修正量计算公式：

$\left\{\begin{array}{c}{\mathrm{\ω}}_{\mathrm{cx}}^n=\frac{K_2}{R}{\mathrm{\δV}}_N\\ {\mathrm{\ω}}_{\mathrm{cy}}^n=-\frac{K_2}{R}{\mathrm{\δV}}_E\\ {\mathrm{\ω}}_{\mathrm{cz}}^n=0\end{array}\right.,$ $\left\{\begin{array}{c}{f}_{\mathrm{cx}}^n=K_1{\mathrm{\δV}}_E\\ f_{\mathrm{cy}}^n=K_1{\mathrm{\δV}}_N\\ f_{\mathrm{cz}}^n=0\end{array}\right.$

方位精对准时修正量计算公式：

$\left\{\begin{array}{c}{\mathrm{\ω}}_{\mathrm{cx}}^n=\frac{{K^{\′}}_2}{R}{\mathrm{\δV}}_N\\ {\mathrm{\ω}}_{\mathrm{cy}}^n=-\frac{{K^{\′}}_2}{R}{\mathrm{\δV}}_E\\ {\mathrm{\ω}}_{\mathrm{cz}}^n=\frac{{K^{\′}}_3}{{\mathrm{\ω}}_{\mathrm{ie}}\cos L(s+{K^{\′}}_4)}{\mathrm{\δV}}_N\end{array}\right.,$ $\left\{\begin{array}{c}{f}_{\mathrm{cx}}^n={K^{\′}}_1{\mathrm{\δV}}_E\\ f_{\mathrm{cy}}^n={K^{\′}}_1{\mathrm{\δV}}_N\\ f_{\mathrm{cz}}^n=0\end{array}\right.$

其中，R为地球半径，δV_E为东向速度误差，δV_N为北向速度误差，L为纬度，ω_ie为地球自转角速度，s表示对准***中的积分环节。

本发明利用解析法完成粗对准过程，接着进行水平精对准和方位精对准，方位精对准分为三个阶段，每个阶段根据该阶段的特性选用不同的调整时间和衰减系数从而确定相应的对准参数，基于这些参数计算加速度修正量和角速度修正量，对输入的加速度信息和角速度信息进行修正，利用修正后的信息进行四元数解算得到初始姿态角，完成三个阶段精对准过程即得到了最终的初始姿态角。

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

(1)本发明通过采集陀螺仪和加速度计数据利用解析法和罗经法完成初始对准，此方法无需利用其他传感器信息，保证初始对准的安全性和保密性。

(2)本发明采用方位精对准过程参数分段式方法，针对不同阶段的对准特性选用最佳的调整时间和衰减系数，从而计算其他的对准参数，该方法可以提高对准曲线的快速收敛性以及抗干扰性能。

附图说明

图1为本发明捷联惯性导航***自对准流程图。

图2为本发明水平精对准回路东向通道方块图。

图3为本发明水平精对准回路北向通道方块图。

图4为本发明方位精对准方块图。

图5为本发明捷联惯性导航***自对准方法原理图。

图6为本发明实施例参数未分段自对准姿态角误差曲线。

图7为图6的局部放大图。

图8为本发明实施例参数分段式自对准姿态角误差曲线。

图9为图8的局部放大图。

具体实施方式

如图1，本发明的具体实施步骤如下：

1)对准前确定自对准精对准参数，包括各阶段阻尼比ξ、衰减系数σ、调整时间t_s、对准时间T以及由此参数计算出的其他对准控制回路中其他参数；

2)利用解析法进行粗对准，确定初始姿态矩阵

以及初始航向角

初始纵摇角θ₀和初始横摇角γ₀；

3)在步骤2)粗对准的基础上开始精对准过程，按照精对准所分阶段，每个阶段均由对准回路计算出导航坐标系下加速度修正信息

和角速度修正信息

所述导航坐标系为东北天坐标系；

4)将步骤3)中计算所得的加速度修正信息

和角速度修正信息转换到载体坐标系上，即

为导航坐标到载体坐标的转换矩阵，从而对捷联惯性导航***中的加速度计的比力和陀螺仪的角速度

进行修正，即

5)利用步骤4)中修正过的比力信息

和角速度信息

使用四元数更新算法计算本姿态解算周期的姿态矩阵，从而确定对应的纵摇角、横摇角和航向角，输出更新后的姿态；

6)步骤3)至5)为一个姿态解算周期，重复步骤3)至5)进行下一个姿态解算周期的计算，得到对应姿态解算周期载体的纵摇角、横摇角和航向角，直到周期总时间达到步骤1)中所确定的各阶段的对准时间T_i，完成精对准，输出最后一个周期更新的姿态；

7)完成水平精对准和方位精对准后对准过程结束，得到精确的姿态角，实现捷联惯性导航***自对准。

本发明捷联惯性导航***自对准的原理图如图5，步骤3)至步骤5)完成一个姿态解算周期，循环周期，实现自对准。

本发明所述的解析法粗对准按照以下方法进行：

粗对准就是建立起一个近似的姿态矩阵的估计值，即初始姿态矩阵

从而使***可以进行下一步的精对准。粗对准为捷联惯性导航初始对准技术领域的公知技术，这里介绍其中两种方法。

当地纬度L为已知量，导航系采用东北天坐标系，则重力加速度g和地球自转角速度ω_ie在导航系中的分量都是确定已知的，可以表示为：

gⁿ＝[0 0 -g]^T

${\mathrm{\ω}}_{\mathrm{ie}}^n={\left[\begin{array}{ccc}0& \mathrm{\Ω}\cos L& \mathrm{\Ω}\sin L\end{array}\right]}^T$

其中gⁿ表示重力加速度在导航坐标系下的投影，

表示地球自转角速度在导航坐标系下的投影，Ω为地球自转角速率，g为重力加速度的幅值。解析粗对准就是利用已知的gⁿ，和捷联惯性导航***传感器的测量值来估计表示载体坐标到导航坐标的转换矩阵，即捷联姿态矩阵。由于粗对准时间较短，通常采用对传感器测量值取平均的办法来提高对准精度。为表达书写，用a^b和

分别表示加速度计和陀螺仪的测量平均值。

步骤2)中所述的解析法粗对准方法可以采用以下两种方法的任一种，均为等价的：

(a)利用三个互相正交的向量g，g×ω_ie和(g×ω_ie)×g和三个互相正交的向量a^b，a^b×ω^b和(a^b×ω^b)×a^b计算

${C^{\′}}_{b\left(0\right)}^n={\left[\begin{array}{c}{\left(g^n\right)}^T\\ {(g^n\×{\mathrm{\ω}}_{\mathrm{ie}}^n)}^T\\ {[(g^n\×{\mathrm{\ω}}_{\mathrm{ie}}^n)\×g^n]}^T\end{array}\right]}^{-1}\left[\begin{array}{c}{\left(a^b\right)}^T\\ {(a^b\×{\mathrm{\ω}}_{\mathrm{ie}}^b)}^T\\ {[(a^b\×{\mathrm{\ω}}_{\mathrm{ie}}^b)\×a^b]}^T\end{array}\right]$

其中，gⁿ表示重力加速度在导航坐标系下的投影，表示地球自转角速度在导航坐标系下的投影，a^b表示加速度计测量值的测量平均值，

表示陀螺仪测量值的测量平均值；

对姿态矩阵进行归一化处理：

$C_{b\left(0\right)}^n={{C^{\′}}_{b\left(0\right)}^n\left[{\left({C^{\′}}_{b\left(0\right)}^n\right)}^T{C^{\′}}_{b\left(0\right)}^n\right]}^{-\frac{1}{2}}$

就是粗对准得到的初始姿态矩阵；

(b)由测量值直接计算

将

写成列向量的形式：

$C_{b\left(0\right)}^n=\left[\begin{array}{ccc}{C}_1& C_2& C_3\end{array}\right]$

其中C₁、C₂、C₃为构成

的三个列向量，且彼此间具有正交约束。

设k时刻陀螺仪和加速度计的测量值分别记为ω^b(k)和a^b(k)，采样次数为N次，采用正交约束下的最小二乘法估计

的三个分列向量：

$C_3=-\frac{1}{\mathrm{\λ}}\underset{k=1}{\overset{N}{\mathrm{\Σ}}}{a}^b\left(k\right)$

$C_2=\frac{1}{\mathrm{\η}}(\underset{k=1}{\overset{N}{\mathrm{\Σ}}}{\mathrm{\ω}}^b\left(k\right)+\mathrm{\ρ}\hat{z})$

$C_1=C_2\×C_3=\frac{1}{\mathrm{\η}}\underset{k=1}{\overset{N}{\mathrm{\Σ}}}{\mathrm{\ω}}^b\left(k\right)\×C_3$

$\mathrm{\λ}={\left[{\left(\underset{k=1}{\overset{N}{\mathrm{\Σ}}}{a}^b\left(k\right)\right)}^T\left(\underset{k=1}{\overset{N}{\mathrm{\Σ}}}{a}^b\left(k\right)\right)\right]}^{\frac{1}{2}}$

$\mathrm{\ρ}=\frac{1}{\mathrm{\λ}}{\left(\underset{k=1}{\overset{N}{\mathrm{\Σ}}}{a}^b\left(k\right)\right)}^T\left(\underset{k=1}{\overset{N}{\mathrm{\Σ}}}{\mathrm{\ω}}^b\left(k\right)\right)$

$\mathrm{\η}={[{\left(\underset{k=1}{\overset{N}{\mathrm{\Σ}}}{\mathrm{\ω}}^b\left(k\right)\right)}^T\left(\underset{k=1}{\overset{N}{\mathrm{\Σ}}}{\mathrm{\ω}}^b\left(k\right)\right)-{\mathrm{\ρ}}^2]}^{\frac{1}{2}}$

此时便得到了

由于已经是正交矩阵，所以无需正交化。

上面计算的是初始姿态矩阵，为第一个姿态解算周期，后续的姿态结算周期中，姿态矩阵与初始姿态矩阵的计算方法相同，使用对应周期测量得到的传感器参数计算即可。

本发明的重要特征在于所述的精对准过程分阶段确定对准时间和各对准参数，具体如下：

在设计参数时，一般首先选定阻尼比ξ和调整时间t_s。

调节时间t_s是指单位阶跃响应到达并保持在终值±5％内所需的最短时间，它是反映响应速度和阻尼程度的综合性指标。t_s的简化计算公式如下：

$t_s\≈\frac{3}{{\mathrm{\ξ\ω}}_n}=\frac{3}{\mathrm{\σ}}$

通常取阻尼比ξ＝0.4～0.8为宜，此时超调量适度。由于阻尼系数小时对准过渡的时间长，阻尼系数大时***抑制干扰的能力弱，因此，选取

为加快对准时间，在精对准过程中，分段进行。每段参数选择的基本原则是：调整时间t_s逐渐变长。精对准的第一段调整时间不能太短，否则摇摆运动时会出现振荡。对准时间是所设计的调整时间1～2倍较为合适。

设计时要合理的增加精对准阶段的个数。本发明水平精对准分为一个阶段，方位对准过程分三个阶段完成。各个阶段的参数选取如下：

水平精对准阶段：(对准时间为T₁＝100s)

$\mathrm{\ξ}=\frac{1}{\sqrt{2}},$ σ＝0.045， $t_s=\frac{3}{\mathrm{\σ}}=66.7$

方位精对准第一阶段：(对准时间T₂＝300s)

$\mathrm{\ξ}=\frac{1}{\sqrt{2}},$ σ＝0.02， $t_s=\frac{3}{\mathrm{\σ}}=150$

方位精对准第二阶段：(对准时间T₃＝300s)

$\mathrm{\ξ}=\frac{1}{\sqrt{2}},$ σ＝0.015， $t_s=\frac{3}{\mathrm{\σ}}=200$

方位精对准第三阶段：(对准时间T₄＝300s)

$\mathrm{\ξ}=\frac{1}{\sqrt{2}},$ σ＝0.01， $t_s=\frac{3}{\mathrm{\σ}}=300$

根据阻尼比ξ和衰减系数σ按下式计算各阶段其他阻尼参数：

捷联惯导***采用东北天坐标系，根据捷联惯导***水平罗经对准***，如图2和图3所示，得到水平精对准阶段***各阶的阻尼参数：

K₁＝3σ

$K_2=\frac{{\mathrm{\σ}}^2}{{\mathrm{\ω}}_s^2}(2+\frac{1}{{\mathrm{\ξ}}^2})-1$

$K_3=\frac{{\mathrm{\σ}}^3}{{\mathrm{g\ξ}}^2}$

根据捷联惯导***方位罗经对准***，如图4所述，得到方位精对准阶段***各阶的阻尼参数：

K′₁＝K′₄＝2σ

${K^{\′}}_2=\frac{{4\mathrm{\σ}}^2}{{\mathrm{\ω}}_s^2}-1$

${K^{\′}}_3=\frac{{4\mathrm{\σ}}^4}{g}$

其中，

g为重力加速度，R为地球半径。

(3)本发明中所述的精对准方法如下：

图2为水平精对准回路东向通道方块图，图3为水平精对准回路北向通道方块图，图4为方位精对准方块图。图5为精对准算法原理图。

采用基于水平三阶回路的精对准方法，精对准算法分为水平部分和方位部分， ${\mathrm{\ω}}_c^n={\left(\begin{array}{ccc}{\mathrm{\ω}}_{\mathrm{cx}}^n& {\mathrm{\ω}}_{\mathrm{cy}}^n& {\mathrm{\ω}}_{\mathrm{cz}}^n\end{array}\right)}^T$ 为角速度修正量， $f_c^n={\left(\begin{array}{ccc}{f}_{\mathrm{cx}}^n& f_{\mathrm{cy}}^n& f_{\mathrm{cz}}^n\end{array}\right)}^T$ 为加速度修正量，计算方法如下：

水平精对准时修正量计算公式：

$\left\{\begin{array}{c}{\mathrm{\ω}}_{\mathrm{cx}}^n=(\frac{K_2}{R}+\frac{K_3}{s}){\mathrm{\δV}}_N\\ {\mathrm{\ω}}_{\mathrm{cy}}^n=(-\frac{K_2}{R}+\frac{K_3}{s}){\mathrm{\δV}}_E\\ {\mathrm{\ω}}_{\mathrm{cz}}^n=0\end{array}\right.,$ $\left\{\begin{array}{c}{f}_{\mathrm{cx}}^n=K_1{\mathrm{\δV}}_E\\ f_{\mathrm{cy}}^n=K_1{\mathrm{\δV}}_N\\ f_{\mathrm{cz}}^n=0\end{array}\right.$

K₁、K₂、K₃为图2和图3中水平精对准阶段***各阶的阻尼参数：

方位精对准时修正量计算公式：

$\left\{\begin{array}{c}{\mathrm{\ω}}_{\mathrm{cx}}^n=\frac{{K^{\′}}_2}{R}{\mathrm{\δV}}_N\\ {\mathrm{\ω}}_{\mathrm{cy}}^n=-\frac{{K^{\′}}_2}{R}{\mathrm{\δV}}_E\\ {\mathrm{\ω}}_{\mathrm{cz}}^n=\frac{{K^{\′}}_3}{{\mathrm{\ω}}_{\mathrm{ie}}\cos L(s+{K^{\′}}_4)}{\mathrm{\δV}}_N\end{array}\right.,$ $\left\{\begin{array}{c}{f}_{\mathrm{cx}}^n={K^{\′}}_1{\mathrm{\δV}}_E\\ f_{\mathrm{cy}}^n={K^{\′}}_1{\mathrm{\δV}}_N\\ f_{\mathrm{cz}}^n=0\end{array}\right.$

K′₁、K′₂、K′₃、K′₄为图4方位精对准中引入的方位精对准阶段***各阶的阻尼参数。

R为地球半径，δV_E为东向速度误差，δV_N为北向速度误差，L为纬度，ω_ie为地球自转角速率，

s表示对准***中的积分环节。。

将修正量转换为载体坐标系下的值对载体系下的加速度信息和角速度信息进行补偿，从而用更新后的信息进行四元数计算得到初始的姿态角。

本发明采用一种快速、参数分段式捷联惯性导航***自对准方法，下面实验室三轴摇摆台条件下验证该发明的有益效果。

惯性传感器的性能指标如下：陀螺常值漂移：0.04°/h；陀螺随机漂移：0.04°/h；加速度计常值偏置：50μg；加速度计随机偏置：50μg。初始失准角：纵摇失准角0.15°、横摇失准角0.15°、航向失准角0.7°。

实验中，三轴摇摆的运动方式为：内框摆幅为15°，频率为0.125Hz；中框摆幅为8°，频率为0.15Hz；外框摆幅为5°，频率为0.2Hz。三轴同时进行摇摆运动，以此运动状态模拟舰船实际的应用环境，每组试验进行1000s。

图6和图7显示了参数未分段的自对准姿态角误差曲线，图8和图9显示了参数分段的自对准姿态角误差曲线。实验结果表明参数分段式自对准方法较传统自对准方法能够缩短对准时间，减小对准过程中的振荡以及误差角的均方差和均值，从而使对准精度提高。

Claims (3)

1.一种快速、参数分段式捷联惯性导航***自对准方法，其特征是分为粗对准和精对准两步，首先利用解析法完成粗对准过程，然后进行水平精对准和方位精对准，基于精对准参数计算加速度修正量和角速度修正量，对输入捷联惯性导航***的加速度信息和角速度信息进行修正，利用修正后的信息进行四元数解算得到初始姿态角，完成精对准过程，包括以下步骤：

1)对准前首先确定自对准的精对准参数，包括各阶段阻尼比ξ、衰减系数σ、调整时间t_s、对准时间T_i以及由所述参数计算出的各阶段其他阻尼参数；

精对准分段进行，按照选用对准参数的不同设计水平精对准为一个阶段，方位精对准为三个阶段，具体如下：

水平精对准阶段：对准时间为T₁＝100s

$\mathrm{\ξ}=\frac{1}{\sqrt{2}},$ σ＝0.045， $t_s=\frac{3}{\mathrm{\σ}}=66.7$

方位精对准第一阶段：对准时间T₂＝300s

$\mathrm{\ξ}=\frac{1}{\sqrt{2}},$ σ＝0.02， $t_s=\frac{3}{\mathrm{\σ}}=150$

方位精对准第二阶段：对准时间T₃＝300s

$\mathrm{\ξ}=\frac{1}{\sqrt{2}},$ σ＝0.015， $t_s=\frac{3}{\mathrm{\σ}}=200$

方位精对准第三阶段：对准时间T₄＝300s

$\mathrm{\ξ}=\frac{1}{\sqrt{2}},$ σ＝0.01， $t_s=\frac{3}{\mathrm{\σ}}=300$

根据阻尼比ξ和衰减系数σ按下式计算各阶段其他阻尼参数：

捷联惯导***采用东北天坐标系，根据捷联惯导***水平罗经对准原理，得到水平精对准阶段***各阶的阻尼参数：

K₁＝3σ

$K_2=\frac{{\mathrm{\σ}}^2}{{\mathrm{\ω}}_s^2}(2+\frac{1}{{\mathrm{\ξ}}^2})-1$

$K_3=\frac{{\mathrm{\σ}}^3}{{\mathrm{g\ξ}}^2}$

根据捷联惯导***方位罗经对准原理，得到方位精对准阶段***各阶的阻尼参数：

K′₁＝K′₄＝2σ

${K^{\′}}_2=\frac{{4\mathrm{\σ}}^2}{{\mathrm{\ω}}_s^2}-1$

${K^{\′}}_3=\frac{{4\mathrm{\σ}}^4}{g}$

其中，

g为重力加速度，R为地球半径；

2)利用解析法进行粗对准，确定初始姿态矩阵

以及初始航向角

初始纵摇角θ₀和初始横摇角γ₀；

3)在步骤2)粗对准的基础上开始精对准过程，按照精对准所分阶段，每个阶段均由对准回路计算出导航坐标系下加速度修正信息

和角速度修正信息

所述导航坐标系为东北天坐标系；

4)将步骤3)中计算所得的加速度修正信息

和角速度修正信息转换到载体坐标系上，即

为导航坐标到载体坐标的转换矩阵，从而对捷联惯性导航***中的加速度计的比力

和陀螺仪的角速度

进行修正，即

5)利用步骤4)中修正过的比力信息

和角速度信息

使用四元数更新算法计算本姿态解算周期的姿态矩阵，从而确定对应的纵摇角、横摇角和航向角，输出更新后的姿态；

6)步骤3)至5)为一个姿态解算周期，重复步骤3)至5)进行下一个姿态解算周期的计算，得到对应姿态解算周期载体的纵摇角、横摇角和航向角，直到周期总时间达到步骤1)中所确定的各阶段的对准时间T_i，完成精对准，输出最后一个周期更新的姿态；

7)完成水平精对准和方位精对准后对准过程结束，得到精确的姿态角，实现捷联惯性导航***自对准。

2.根据权利要求1所述的一种快速、参数分段式捷联惯性导航***自对准方法，其特征是所述的解析法粗对准为：

当地纬度L为已知量，导航坐标系采用东北天坐标系，则重力加速度g和地球自转角速度ω_ie在导航***中的分量都是确定已知的，表示为：

gⁿ＝[0 0 -g]^T

${\mathrm{\ω}}_{\mathrm{ie}}^n={\left[\begin{array}{ccc}0& \mathrm{\Ω}\cos L& \mathrm{\Ω}\sin L\end{array}\right]}^T$

其中，gⁿ表示重力加速度在导航坐标系下的投影，

表示地球自转角速度在导航坐标系下的投影，Ω为地球自转角速率，g为重力加速度的幅值；

解析粗对准利用已知的gⁿ，

和捷联惯性导航***传感器的测量值来估计粗对准初始姿态矩阵

对传感器测量值取平均以提高对准精度，用a^b和

分别表示加速度计和陀螺仪的测量平均值；

解析法粗对准方法采用以下两种方法的一种：

(a)利用三个互相正交的向量g，g×ω_ie和(g×ω_ie)×g以及三个互相正交的向量a^b，a^b×ω^b和(a^b×ω^b)×a^b计算

${C^{\′}}_{b\left(0\right)}^n={\left[\begin{array}{c}{\left(g^n\right)}^T\\ {(g^n\×{\mathrm{\ω}}_{\mathrm{ie}}^n)}^T\\ {[(g^n\×{\mathrm{\ω}}_{\mathrm{ie}}^n)\×g^n]}^T\end{array}\right]}^{-1}\left[\begin{array}{c}{\left(a^b\right)}^T\\ {(a^b\×{\mathrm{\ω}}_{\mathrm{ie}}^b)}^T\\ {[(a^b\×{\mathrm{\ω}}_{\mathrm{ie}}^b)\×a^b]}^T\end{array}\right]$

对姿态矩阵

进行归一化处理得到粗对准初始姿态矩阵

$C_{b\left(0\right)}^n=C_{b\left(0\right)}^n{\left[{\left(C_{b\left(0\right)}^n\right)}^T{C}_{b\left(0\right)}^n\right]}^{-\frac{1}{2}}$

(b)由测量值直接计算

将

写成列向量的形式：

$C_{b\left(0\right)}^n=\left[\begin{array}{ccc}{C}_1& C_2& C_3\end{array}\right]$

其中C₁、C₂、C₃为构成

的三个列向量，且彼此间具有正交约束；

设k时刻陀螺仪和加速度计的测量值分别记为ω^b(k)和a^b(k)，采样次数为N次，采用正交约束下的最小二乘法估计

的三个分列向量：

$C_3=-\frac{1}{\mathrm{\λ}}\underset{k=1}{\overset{N}{\mathrm{\Σ}}}{a}^b\left(k\right)$

$C_2=\frac{1}{\mathrm{\η}}(\underset{k=1}{\overset{N}{\mathrm{\Σ}}}{\mathrm{\ω}}^b\left(k\right)+\mathrm{\ρ}\hat{z})$

$C_1=C_2\×C_3=\frac{1}{\mathrm{\η}}\underset{k=1}{\overset{N}{\mathrm{\Σ}}}{\mathrm{\ω}}^b\left(k\right)\×C_3$

$\mathrm{\λ}={\left[{\left(\underset{k=1}{\overset{N}{\mathrm{\Σ}}}{a}^b\left(k\right)\right)}^T\left(\underset{k=1}{\overset{N}{\mathrm{\Σ}}}{a}^b\left(k\right)\right)\right]}^{\frac{1}{2}}$

$\mathrm{\ρ}=\frac{1}{\mathrm{\λ}}{\left(\underset{k=1}{\overset{N}{\mathrm{\Σ}}}{a}^b\left(k\right)\right)}^T\left(\underset{k=1}{\overset{N}{\mathrm{\Σ}}}{\mathrm{\ω}}^b\left(k\right)\right)$

$\mathrm{\η}={[{\left(\underset{k=1}{\overset{N}{\mathrm{\Σ}}}{\mathrm{\ω}}^b\left(k\right)\right)}^T\left(\underset{k=1}{\overset{N}{\mathrm{\Σ}}}{\mathrm{\ω}}^b\left(k\right)\right)-{\mathrm{\ρ}}^2]}^{\frac{1}{2}}$

此时便得到了

由于已经是正交矩阵，所以无需正交化。

3.根据权利要求1所述的一种快速、参数分段式捷联惯性导航***自对准方法，其特征是：

步骤3)中所述的加速度修正量 $f_c^n={\left(\begin{array}{ccc}{f}_{\mathrm{cx}}^n& f_{\mathrm{cy}}^n& f_{\mathrm{cz}}^n\end{array}\right)}^T$ 和角速度修正量 ${\mathrm{\ω}}_c^n={\left(\begin{array}{ccc}{\mathrm{\ω}}_{\mathrm{cx}}^n& {\mathrm{\ω}}_{\mathrm{cy}}^n& {\mathrm{\ω}}_{\mathrm{cz}}^n\end{array}\right)}^T,$ 计算如下：

水平精对准时修正量计算公式：

$\left\{\begin{array}{c}{\mathrm{\ω}}_{\mathrm{cx}}^n=\frac{K_2}{R}{\mathrm{\δV}}_N\\ {\mathrm{\ω}}_{\mathrm{cy}}^n=-\frac{K_2}{R}{\mathrm{\δV}}_E\\ {\mathrm{\ω}}_{\mathrm{cz}}^n=0\end{array}\right.,$ $\left\{\begin{array}{c}{f}_{\mathrm{cx}}^n=K_1{\mathrm{\δV}}_E\\ f_{\mathrm{cy}}^n=K_1{\mathrm{\δV}}_N\\ f_{\mathrm{cz}}^n=0\end{array}\right.$

方位精对准时修正量计算公式：

$\left\{\begin{array}{c}{\mathrm{\ω}}_{\mathrm{cx}}^n=\frac{{K^{\′}}_2}{R}{\mathrm{\δV}}_N\\ {\mathrm{\ω}}_{\mathrm{cy}}^n=-\frac{{K^{\′}}_2}{R}{\mathrm{\δV}}_E\\ {\mathrm{\ω}}_{\mathrm{cz}}^n=\frac{{K^{\′}}_3}{{\mathrm{\ω}}_{\mathrm{ie}}\cos L(s+{K^{\′}}_4)}{\mathrm{\δV}}_N\end{array}\right.,$ $\left\{\begin{array}{c}{f}_{\mathrm{cx}}^n={K^{\′}}_1{\mathrm{\δV}}_E\\ f_{\mathrm{cy}}^n={K^{\′}}_1{\mathrm{\δV}}_N\\ f_{\mathrm{cz}}^n=0\end{array}\right.$

其中，R为地球半径，δV_E为东向速度误差，δV_N为北向速度误差，L为纬度，ω_ie为地球自转角速度，s表示对准***中的积分环节。

Images