CN103743413A - 倾斜状态下调制寻北仪安装误差在线估计与寻北误差补偿方法 - Google Patents

Abstract

本发明涉及一种倾斜状态下调制寻北仪安装误差在线估计与寻北误差补偿方法，其特征在于：步骤一：建立地理坐标系、载体坐标系、平台坐标系、陀螺坐标系及四者之间的转换关系；步骤二：建立倾斜状态下，转台绕自身垂直中心轴恒速旋转时，陀螺存在安装误差角时的输出模型；步骤三：建立倾斜状态下，转台绕自身垂直中心轴恒速旋转时，加速度计存在安装误差时的输出模型；步骤四：对陀螺和加速度计的输出信号进行处理，并得到载体的倾斜角；步骤五：根据步骤四中对陀螺、加速度计数据处理后的结果对安装误差角进行在线估计；步骤六：根据步骤五中对处理后陀螺的信号和步骤四中得到的载体倾斜角估计出寻北误差，然后对航向角粗估计值进行修正，得到准确的航向角，完成载体的寻北。

Description

倾斜状态下调制寻北仪安装误差在线估计与寻北误差补偿方法

技术领域

本发明涉及一种倾斜状态下调制寻北仪安装误差在线估计与寻北误差补偿方法。

背景技术

在人类社会生活实践中，地理方位是不可或缺的信息，寻北就是要找出载体相对地球的北向的方位关系，即确定载体的航向角。捷联式寻北又称解析式寻北，它利用陀螺直接敏感地球自转角速度分量，具有反应迅速、结构简单等优点，还可以直接应用先进的光学陀螺作为理想的寻北惯性元件。捷联寻北采用的寻北方法主要有静态寻北和动态寻北。静态寻北法包括单位置、二位置、三位置、四位置和多位置寻北，其中二位置和四位置寻北方法较常见。目前，对静态寻北方案的研究基本成熟，在寻北方案、信号滤波、误差分析及补偿、工程样机研制方面均取得了一定成果。

旋转调制式寻北是近年来新兴的一种动态寻北方案，在寻北过程中，惯性测量单元随转台绕其垂直中心轴连续恒速转动，利用陀螺在水平面或倾斜面内的输出信号解算出载体方位角。相比传统寻北方案，它将静态测量变为动态测量，利用转台的恒速旋转对陀螺和加速度计的输出进行调制，可以有效抑制陀螺常值漂移、随机漂移和加速度计漂移，满足了寻北的快速性和高精度定位要求。目前，相关理论、技术和方法仅局限于旋转调制寻北的基本原理、倾斜状态时的补偿算法以及数据处理方面，并未考虑实际寻北中陀螺的安装误差角。

在实际工程应用中，随着寻北仪工作时的湿度、温度等外界环境因素的变化，陀螺与转台之间的机械连接会因受力的变化而变形，造成陀螺坐标系与平台坐标系之间存在安装误差角，而且该误差角的大小与外界干扰因素密切相关，无法用确定的变化的规律来描述。目前尚无具体方法克服由安装误差角引起的寻北误差，使得实际应用中寻北精度受到严重制约。

发明内容

本发明目的在于提供一种倾斜状态下调制寻北仪安装误差在线估计与寻北误差补偿方法，能够对陀螺坐标系与平台坐标系之间的安装误差角进行估计，并对寻北误差进行补偿，有效提高寻北精度。

实现本发明目的技术方案：

一种倾斜状态下调制寻北仪安装误差在线估计与寻北误差补偿方法，其特征在于：

步骤一：建立地理坐标系、载体坐标系、平台坐标系、陀螺坐标系及四者之间的转换关系；

步骤二：建立倾斜状态下，转台绕自身垂直中心轴恒速旋转时，陀螺存在安装误差角时的输出模型；

步骤三：建立倾斜状态下，转台绕自身垂直中心轴恒速旋转时，加速度计存在安装误差时的输出模型；

步骤四：对陀螺和加速度计的输出信号进行处理，并得到载体的倾斜角；

步骤五：根据步骤四中对陀螺、加速度计数据处理后的结果对安装误差角进行在线估计；

步骤六：根据步骤五中对处理后陀螺的信号和步骤四中得到的载体倾斜角估计出寻北误差，然后对航向角粗估计值进行修正，得到准确的航向角，完成载体的寻北。

步骤二中，陀螺存在安装误差角时的输出模型为，

ω_yi＝K_y{-[(cosγsinH+sinθsinγcosH)ω_N-cosθsinγω_H]sinα_i

+(ω_NcosθcosH+ω_Hsinθ)cosα_i

-η[(cosγsinH+sinθsinγcosH)ω_Ncosα_i-cosθsinγω_Hcosα_i

+(ω_NcosθcosH+ω_Hsinθ)sinα_i]}+ε_yi

式中H为载体的航向角，θ和γ分别为载体的俯仰角和翻滚角，η为平台坐标系p与陀螺坐标系g之间的安装误差角，角位置α_i＝Ω·t_i，其中t_i表示转台转动到第i个位置的时间，Ω为转台恒定角速度，

为当地纬度，ω_ie为地球自转角速度，ω_yi表示转台转动中第i个位置陀螺的输出值，K_y表示陀螺的标度因数，ε_yi表示转台转动中第i个位置的陀螺漂移，i＝1,2,....,n。

步骤四中，对陀螺和加速度计的输出信号进行处理，并得到载体的倾斜角，通过如下方法实现,

令 ${\mathrm{\Ω}}_x=\frac{2}{n}\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{\mathrm{\ω}}_{\mathrm{yi}}\sin {\mathrm{\α}}_i,{\mathrm{\Ω}}_y=\frac{2}{n}\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{\mathrm{\ω}}_{\mathrm{yi}}\cos {\mathrm{\α}}_i,$ 有

$\left\{\begin{array}{c}{\mathrm{\Ω}}_x=K_y[{\mathrm{\ω}}_H\cos \mathrm{\θ}\sin \mathrm{\γ}-{\mathrm{\ω}}_N(\cos \mathrm{\γ}\sin H+\sin \mathrm{\θ}\sin \mathrm{\γ}\cos H)-\mathrm{\η}({\mathrm{\ω}}_N\cos \mathrm{\θ}\cos H+{\mathrm{\ω}}_H\sin \mathrm{\θ})]\\ {\mathrm{\Ω}}_y=K_y\{{\mathrm{\ω}}_N\cos \mathrm{\θ}\cos H+{\mathrm{\ω}}_H\sin \mathrm{\θ}-\mathrm{\η}[{\mathrm{\ω}}_N(\cos \mathrm{\γ}\sin H+\sin \mathrm{\θ}\sin \mathrm{\γ}\cos H)-{\mathrm{\ω}}_H\cos \mathrm{\θ}\sin \mathrm{\γ}\left]\right\}\end{array}\right.$

令 $a_x=\frac{2}{n}\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{f}_{\mathrm{yi}}\sin {\mathrm{\α}}_i,a_y=\frac{2}{n}\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{f}_{\mathrm{yi}}\cos {\mathrm{\α}}_i,$ 有

$\left\{\begin{array}{c}\mathrm{\θ}\≈\arcsin \left(\frac{a_y}{g}\right)\\ \mathrm{\γ}\≈\arcsin \left(\frac{a_x}{g\cos \mathrm{\θ}}\right)\end{array}\right.$

式中g表示地球重力加速度，f_yi表示转台转动中第i个位置加速度计的输出值，i＝1,2,....,n。

步骤五中，在线估计出安装误差角的大小通过如下方法实现,

$\mathrm{\η}=\frac{(a-b\sin \hat{H}-c\cos \hat{H}-{\mathrm{\Ω}}_x/K_y)d\sin \hat{H}-(d\cos \hat{H}+e-{\mathrm{\Ω}}_y/K_y)(b\cos \hat{H}-c\sin \hat{H})}{\left({d\cos \hat{H}+e}_{}\right)d\sin \hat{H}+(a-b\sin \hat{H}-c\cos \hat{H})(b\cos \hat{H}-c\sin \hat{H})}$

式中a＝ω_Hcosθsinγ,b＝ω_Ncosγ,c＝ω_Nsinθsinγ,d＝ω_Ncosθ,e＝ω_Hsinθ， $\hat{H}=\mathrm{atg}\frac{-{\mathrm{\Ω}}_x\cos \mathrm{\θ}-{\mathrm{\Ω}}_y\sin \mathrm{\γ}\sin \mathrm{\θ}+K_y{\mathrm{\ω}}_H\sin \mathrm{\γ}}{\cos \mathrm{\γ}({\mathrm{\Ω}}_y-K_y{\mathrm{\ω}}_H\sin \mathrm{\θ})}.$

步骤六中，对载体航向角的修正通过如下方法实现,

$\mathrm{\ΔH}=\frac{(a-b\sin \hat{H}-c\cos \hat{H}-{\mathrm{\Ω}}_x/K_y)(a-b\sin \hat{H}-c\cos \hat{H})+(d\cos \hat{H}+e-{\mathrm{\Ω}}_y/K_y)(d\cos \hat{H}+e)}{(d\cos \hat{H}+e)d\sin \hat{H}+(a-b\sin \hat{H}-c\cos \hat{H})(b\cos \hat{H}-c\sin \hat{H})}$

$H=\hat{H}+\mathrm{\ΔH}$

n取值为3000。

本发明具有的有益效果：

本发明陀螺和加速度计作为惯性测量单元随转台绕其垂直中心连续恒速转动，陀螺用来测量载体倾斜面内地球自转角速度分量，加速度计用来测量载体的倾斜角，通过建立倾斜状态下陀螺、加速度计存在安装误差角时的输出模型，对陀螺、加速度计输出信号进行处理，实时估计出陀螺的安装误差角，然后对寻北误差进行补偿，完成寻北。本发明对陀螺坐标系与平台坐标系之间的安装误差角进行估计，修正寻北结果，可以显著提高寻北精度。

附图说明

图1是本发明的方法流程图；

图2是实施例中本发明方法与传统忽略安装误差时的寻北解算误差曲线图。

具体实施方式

如图1所示，步骤一：建立地理坐标系、载体坐标系、平台坐标系、陀螺坐标系及四者之间的转换关系；

坐标系的定义：地理坐标系ox_ny_nz_n为东北天坐标系，坐标原点在转台平面中心；载体坐标系ox_by_bz_b，坐标原点与地理坐标系原点重合，ox_b轴和oy_b轴在转台平面内，oz_b轴垂直于转台平面且与ox_b轴、oy_b轴呈右手螺旋，其中ox_b轴、oy_b轴不随转台的旋转而旋转；平台坐标系ox_py_pz_p坐标系原点与地理坐标系原点重合，初始时刻平台坐标系ox_py_pz_p和载体坐标系ox_by_bz_b的x、y、z轴分别对应重合，转台转动时，ox_p轴、oy_p轴随转台旋转而转动；陀螺坐标系ox_gy_gz_g的坐标原点同样也在转台平面中心，随转台的旋转而转动。单轴陀螺寻北时，陀螺的敏感轴与oy_g轴重合，此时考虑寻北仪的安装误差，陀螺坐标系g系的oz_g轴与平台坐标系p系的oz_p轴完全重合，oy_g轴与oy_p轴、ox_g轴与ox_p轴之间存在安装误差角η。

地理坐标系n到载体坐标系b的转移矩阵为

$C_n^b=\left[\begin{array}{ccc}\cos \mathrm{\γ}\cos H-\sin \mathrm{\θ}\sin \mathrm{\γ}\sin H& \cos \mathrm{\γ}\sin H+\sin \mathrm{\θ}\sin \mathrm{\γ}\cos H& -\cos \mathrm{\θ}\sin \mathrm{\γ}\\ -\cos \mathrm{\θ}\sin H& \cos \mathrm{\θ}\cos H& \sin \mathrm{\θ}\\ \sin \mathrm{\γ}\cos H+\sin \mathrm{\θ}\cos \mathrm{\γ}\sin H& \sin \mathrm{\γ}\sin H-\sin \mathrm{\θ}\cos \mathrm{\γ}\cos H& \cos \mathrm{\θ}\cos \mathrm{\γ}\end{array}\right]---\left(1\right)$

式中：H为载体的航向角，θ和γ分别为载体的俯仰角和翻滚角。

当转台以恒定角速度Ω绕oz_b轴（oz_p轴）逆时针转动时，角位置α_i＝Ω·t_i，其中t_i表示转台转动到第i个位置的时间，则载体坐标系到平台坐标系的转移矩阵为

$C_b^p=\left[\begin{array}{ccc}\cos {\mathrm{\α}}_i& \sin {\mathrm{\α}}_i& 0\\ -\sin {\mathrm{\α}}_i& \cos {\mathrm{\α}}_i& 0\\ 0& 0& 1\end{array}\right]---\left(2\right)$

平台坐标系p与陀螺坐标系g之间的安装误差角η为小角度，所以平台坐标系到陀螺坐标系转移矩阵为

$C_p^g=\left[\begin{array}{ccc}1& \mathrm{\η}& 0\\ -\mathrm{\η}& 1& 0\\ 0& 0& 1\end{array}\right]---\left(3\right)$

步骤二：建立倾斜状态下，转台绕自身垂直中心轴恒速旋转时，陀螺存在安装误差角时的输出模型；

考虑陀螺标度因数和陀螺漂移，有

${\mathrm{\ω}}^g=K{C}_p^g(C_b^p{C}_n^b\·{\mathrm{\ω}}_{\mathrm{ie}}^n+\mathrm{\Ω})+\mathrm{\ϵ}---\left(4\right)$

其中，ω^g表示陀螺坐标系下角速度，K＝[K_x K_y K_z]^T,K_gx、K_gy、K_gz表示陀螺各轴的标度因数，表示地球自转角速度在地理坐标系的投影，Ω＝[0 0 Ω]^T为寻北仪转位机构转动的角速度矢量，Ω为转台转动的角速度大小，ε＝[ε_x ε_y ε_z]^T，ε_x、ε_y、ε_z表示陀螺各轴的陀螺漂移，[…]^T表示矩阵的转置。

由于陀螺的敏感轴与oy_g轴，因此陀螺输出为陀螺坐标系下角速度ω^g在y轴的投影。将式（1）、（2）、（3）代入式（4），并取ω^g的y轴分量可得陀螺的输出为

ω_yi＝K_y{-[(cosγsinH+sinθsinγcosH)ω_N-cosθsinγω_H]sinα_i

+(ω_NcosθcosH+ω_Hsinθ)cosα_i              （5）

-η[(cosγsinH+sinθsinγcosH)ω_Ncosα_i-cosθsinγω_Hcosα_i

+(ω_NcosθcosH+ω_Hsinθ)sinα_i]}+ε_yi

式中H为载体的航向角，θ和γ分别为载体的俯仰角和翻滚角，η为平台坐标系p与陀螺坐标系g之间的安装误差角，角位置α_i＝Ω·t_i，其中t_i表示转台转动到第i个位置的时间，Ω为转台恒定角速度，

为当地纬度，ω_ie为地球自转角速度，ω_yi表示转台转动中第i个位置陀螺的输出值，Ky表示陀螺的标度因数，ε_yi＝ε₀+ε_di表示转台转动中第i个位置的陀螺漂移，ε₀是常值漂移，ε_di是随机漂移，i＝1,2,....,n。

步骤三：建立倾斜状态下，转台绕自身垂直中心轴恒速旋转时，加速度计存在安装误差时的输出模型；

在旋转调制式寻北仪中，采用陀螺坐标系下水平方向y轴上的一只加速度计来测量载体的倾斜角，加速度计测得的比力在地理坐标系的投影为fⁿ，在陀螺坐标系的投影为f^g，重力加速度在地理坐标系下的投影为gⁿ。有

f^g＝[f_x f_y f_z]^T         （6）

gⁿ＝[0 0 -g]^T          （7）

因为载体静止，地速在地理坐标系的投影

地理坐标系相对地球坐标系的转动角速度

依据惯导基本方程有

$\begin{array}{c}{f}^n={\stackrel{\·}{V}}_e^n+(2{\mathrm{\ω}}_{\mathrm{ie}}^n+{\mathrm{\ω}}_{\mathrm{en}}^n)\×V_e^n-g^n\\ =-g^n\\ ={\left[\begin{array}{ccc}0& 0& g\end{array}\right]}^T\end{array}---\left(8\right)$

考虑加速度漂移▽＝[▽_x ▽_y ▽_z]^T，依据坐标转换原理，有

$f^g=\left[\begin{array}{c}{f}_x\\ f_y\\ f_z\end{array}\right]=C_p^g{C}_b^p{C}_n^b\·f^n+\▿---\left(9\right)$

将式（1）、（2）、（3）、（8）代入式（9）中，并取f^g的_y轴分量作为加速度计的输出

f_yi＝(gsinθ+ηgcosθsinγ)cosα_i+(gcosθsinγ-ηgsinθ)sinα_i+▽_y  （10）

其中f_yi表示转台转动中第i个位置处oy_g轴的加速度计测得的比力，i＝1,2,....,n。

步骤四：对陀螺和加速度计的输出信号进行处理，并得到载体的倾斜角；

（1）陀螺输出信号数据处理

转台旋转一周，在旋转过程中，陀螺输出不断变化。选取一周中n个对称位置获得陀螺输出采样值，当次数n足够大时，我们可以采用如下近似式来进行数据处理。

$\left\{\begin{array}{c}\underset{i=1}{\overset{n}{\mathrm{\Σ}}}\sin {\mathrm{\α}}_i\≈\frac{n}{2\mathrm{\π}}\underset{0}{\overset{2\mathrm{\π}}{\∫}}\sin \mathrm{\αd\α}=0\\ \underset{i=1}{\overset{n}{\mathrm{\Σ}}}\cos {\mathrm{\α}}_i\≈\frac{n}{2\mathrm{\π}}\underset{0}{\overset{2\mathrm{\π}}{\∫}}\cos \mathrm{\αd\α}=0\\ \underset{i=1}{\overset{n}{\mathrm{\Σ}}}{\sin }^2{\mathrm{\α}}_i\≈\frac{n}{2\mathrm{\π}}\underset{0}{\overset{2\mathrm{\π}}{\∫}}{\sin }^2\mathrm{\αd\α}=0\\ \underset{i=1}{\overset{n}{\mathrm{\Σ}}}{\cos }^2{\mathrm{\α}}_i\≈\frac{n}{2\mathrm{\π}}\underset{0}{\overset{2\mathrm{\π}}{\∫}}{\cos }^2\mathrm{\αd\α}=0\\ \underset{i=1}{\overset{n}{\mathrm{\Σ}}}\sin {\mathrm{\α}}_i\cos {\mathrm{\α}}_i\≈\frac{n}{2\mathrm{\π}}\underset{0}{\overset{2\mathrm{\π}}{\∫}}\sin \mathrm{\α}\cos \mathrm{\αd\α}=0\end{array}\right.---\left(11\right)$

$\left\{\begin{array}{c}\frac{2}{n}\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{\mathrm{\ϵ}}_{\mathrm{di}}\sin {\mathrm{\α}}_i\≈0\\ \frac{2}{n}\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{\mathrm{\ϵ}}_{\mathrm{di}}\cos {\mathrm{\α}}_i\≈0\end{array}\right.---\left(12\right)$

令 ${\mathrm{\Ω}}_x=\frac{2}{n}\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{\mathrm{\ω}}_{\mathrm{yi}}\sin {\mathrm{\α}}_i,{\mathrm{\Ω}}_y=\frac{2}{n}\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{\mathrm{\ω}}_{\mathrm{yi}}\cos {\mathrm{\α}}_i,$ 根据式（5）、（11）、（12）有

$\left\{\begin{array}{c}{\mathrm{\Ω}}_x=K_y[{\mathrm{\ω}}_H\cos \mathrm{\θ}\sin \mathrm{\γ}-{\mathrm{\ω}}_N(\cos \mathrm{\γ}\sin H+\sin \mathrm{\θ}\sin \mathrm{\γ}\cos H)-\mathrm{\η}({\mathrm{\ω}}_N\cos \mathrm{\θ}\cos H+{\mathrm{\ω}}_H\sin \mathrm{\θ})]\\ {\mathrm{\Ω}}_y=K_y\{{\mathrm{\ω}}_N\cos \mathrm{\θ}\cos H+{\mathrm{\ω}}_H\sin \mathrm{\θ}-\mathrm{\η}[{\mathrm{\ω}}_N(\cos \mathrm{\γ}\sin H+\sin \mathrm{\θ}\sin \mathrm{\γ}\cos H)-{\mathrm{\ω}}_H\cos \mathrm{\θ}\sin \mathrm{\γ}\left]\right\}\end{array}\right.---\left(13\right)$

在这里，综合考虑计算量与计算精度，根据多次实践经验，选取n＝3000。

（2）加速度计输出信号数据处理

同（1）中对陀螺输出信号的数据处理，选取转台旋转一周中n个对称位置获得加速度计输出采样值，令 $a_x=\frac{2}{n}\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{f}_{\mathrm{yi}}\sin {\mathrm{\α}}_i,a_y=\frac{2}{n}\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{f}_{\mathrm{yi}}\cos {\mathrm{\α}}_i,$ 根据式（10）、（11）有

$\left\{\begin{array}{c}{a}_x\≈g\cos \mathrm{\θ}\sin \mathrm{\γ}-\mathrm{\η}g\sin \mathrm{\θ}\\ a_y\≈g\sin \mathrm{\θ}+\mathrm{\η}g\cos \mathrm{\θ}\sin \mathrm{\γ}\end{array}\right.---\left(14\right)$

在这里，同理式（13），n＝3000。

实际工作中，寻北仪的倾斜角θ、γ为小角度，安装误差角η也为小角度，因此忽略式（14）中的二阶小量项-ηgsinθ和ηgcosθsinγ，有

$\left\{\begin{array}{c}{a}_x\≈g\cos \mathrm{\θ}\sin \mathrm{\γ}\\ a_y\≈g\sin \mathrm{\θ}\end{array}\right.---\left(15\right)$

根据式（15）可得载体的倾斜角

$\left\{\begin{array}{c}\mathrm{\θ}\≈\arcsin \left(\frac{a_y}{g}\right)\\ \mathrm{\γ}\≈\arcsin \left(\frac{a_x}{g\cos \mathrm{\θ}}\right)\end{array}\right.---\left(16\right)$

步骤五：根据步骤四中对陀螺、加速度计数据处理后的结果对安装误差角进行在线估计。

忽略安装误差角η，根据式（13）对载体航向角进行粗估计

$\hat{H}=\mathrm{atg}\frac{-{\mathrm{\Ω}}_x\cos \mathrm{\θ}-{\mathrm{\Ω}}_y\sin \mathrm{\γ}\sin \mathrm{\θ}+K_y{\mathrm{\ω}}_H\sin \mathrm{\γ}}{\cos \mathrm{\γ}({\mathrm{\Ω}}_y-K_y{\mathrm{\ω}}_H\sin \mathrm{\θ})}---\left(17\right)$

设航向角真值H与估计值

之间的关系为

$H=\hat{H}+\mathrm{\ΔH}---\left(18\right)$

其中ΔH为小量。

将式（18）代入式（13）中，并对三角函数sinH、cosH进行泰勒展开

$\left\{\begin{array}{c}\sin H=\sin \hat{H}+\cos \hat{H}\·\mathrm{\ΔH}+o\left(\mathrm{\ΔH}\right)\\ \cos H=\cos \hat{H}-\sin \hat{H}\·\mathrm{\ΔH}+o\left(\mathrm{\ΔH}\right)\end{array}\right.---\left(19\right)$

其中 $o\left(\mathrm{\ΔH}\right)=\underset{k=2}{\overset{\∞}{\mathrm{\Σ}}}\frac{{\sin }^{\left(k\right)}\left(\hat{H}\right)}{k!}{\left(\mathrm{\ΔH}\right)}^k$ 是高阶小量。

将式（19）代入式（13）中，忽略η和ΔH的高阶小量，有

$\left\{\begin{array}{c}{\mathrm{\Ω}}_x=K_y[a-b\sin \hat{H}-c\cos \hat{H}-(b\cos \hat{H}-c\sin \hat{H})\·\mathrm{\ΔH}-(d\cos \hat{H}+e)\·\mathrm{\η}]\\ {\mathrm{\Ω}}_y=K_y[d\cos \hat{H}+e-d\sin \hat{H}\·\mathrm{\ΔH}+(a-b\sin \hat{H}-c\cos \hat{H})\·\mathrm{\η}]\end{array}\right.---\left(20\right)$

其中，a＝ω_Hcosθsinγ,b＝ω_Ncosγ,c＝ω_Nsinθsinγ,d＝ω_Ncosθ，e＝ω_Hsinθ。

根据式（20）对安装误差角进行在线估计

$\mathrm{\η}=\frac{(a-b\sin \hat{H}-c\cos \hat{H}-{\mathrm{\Ω}}_x/K_y)d\sin \hat{H}-(d\cos \hat{H}+e-{\mathrm{\Ω}}_y/K_y)(b\cos \hat{H}-c\sin \hat{H})}{(d\cos \hat{H}+e)d\sin \hat{H}+(a-b\sin \hat{H}-c\cos \hat{H})(b\cos \hat{H}-c\sin \hat{H})}---\left(21\right)$

步骤六：根据步骤五中对处理后陀螺的信号和步骤四中得到的载体倾斜角估计出寻北误差，然后对航向角粗估计值进行修整，得到准确的航向角，完成载体的寻北。

$\mathrm{\ΔH}=\frac{(a-b\sin \hat{H}-c\cos \hat{H}-{\mathrm{\Ω}}_x/K_y)(a-b\sin \hat{H}-c\cos \hat{H})+(d\cos \hat{H}+e-{\mathrm{\Ω}}_y/K_y)(d\cos \hat{H}+e)}{(d\cos \hat{H}+e)d\sin \hat{H}+(a-b\sin \hat{H}-c\cos \hat{H})(b\cos \hat{H}-c\sin \hat{H})}---\left(22\right)$

$H=\hat{H}+\mathrm{\ΔH}---\left(23\right)$

寻北的实质在于根据平台坐标系中测得的地球自转角速度分量求得载体的方位角。其中，Ω_x、Ω_y等价于在陀螺坐标系下测得的角速度，当安装误差角存在时，陀螺坐标系与平台坐标系并不完全重合。而且寻北仪在实际工作中，安装误差会不可避免的出现，此时再用Ω_x、Ω_y来求载体的方位角，必然会引入寻北误差，本发明可以通过估计出安装误差角和寻北误差，得到准确的航向角，从而避免因安装误差而引起的寻北误差。

结合具体实施例，进一步对本发明进行说明。仿真时暂不考虑陀螺的标度因数误差、转台的转位误差和温度变化等因素。设地球自转角速度ω_ie＝15.04107°/h，当地地理纬度

安装误差角η＝20'，转台转动角速度Ω＝1°/s，转台的倾斜角θ＝5°、γ＝5°，光纤陀螺的常值漂移为0.01°/h，随机漂移为0.01°/h，采样位置数n＝3000。从图2可以看出，在寻北仪旋转一周过程中，寻北仪连续输出载体航向角，传统的忽略安装误差时的寻北解算误差范围为[0.3° 0.4°]，由此可以看出，忽略安装误差导致在要求高精度寻北中严重影响了最终的寻北结果。而本发明方法则可以很好的解决这一问题，由图2可以看出安装误差经过补偿后，寻北解算误差范围为[-0.2° 0.15°]，比未考虑安装误差时的寻北精度有明显提高，说明本发明方法对安装误差起到了很好的消除作用。从本实施例可以看出，在实际工程应用中，本方法可以在寻北仪长期工作导致安装误差的情况下，很好的补偿由安装误差引起的寻北误差，提高寻北的精度。

Claims (6)

1.一种倾斜状态下调制寻北仪安装误差在线估计与寻北误差补偿方法，其特征在于： 

步骤一：建立地理坐标系、载体坐标系、平台坐标系、陀螺坐标系及四者之间的转换关系； 

步骤二：建立倾斜状态下，转台绕自身垂直中心轴恒速旋转时，陀螺存在安装误差角时的输出模型； 

步骤三：建立倾斜状态下，转台绕自身垂直中心轴恒速旋转时，加速度计存在安装误差角时的输出模型； 

步骤四：对陀螺和加速度计的输出信号进行处理，并得到载体的倾斜角； 

步骤五：根据步骤四中对陀螺、加速度计数据处理后的结果对安装误差角进行在线估计； 

步骤六：根据步骤五中对处理后陀螺的信号和步骤四中得到的载体倾斜角估计出寻北误差，然后对航向角粗估计值进行修正，得到准确的航向角，完成载体的寻北。 

2.根据权利要求1所述的倾斜状态下调制寻北仪安装误差在线估计与寻北误差补偿方法，其特征在于：步骤二中，陀螺存在安装误差角时的输出模型为 

ω_yi＝K_y{-[(cosγsinH+sinθsinγcosH)ω_N-cosθsinγω_H]sinα_i

+(ω_NcosθcosH+ω_Hsinθ)cosα_i

-η[(cosγsinH+sinθsinγcosH)ω_Ncosα_i-cosθsinγω_Hcosα_i

+(ω_NcosθcosH+ω_Hsinθ)sinα_i]}+ε_yi

式中H为载体的航向角，θ和γ分别为载体的俯仰角和翻滚角，η为平台坐标系p与陀螺坐标系g之间的安装误差角，角位置α_i＝Ω·t_i，其中t_i表示转台转动到第i个位置的时间，Ω为转台恒定角速度，

为当地纬度，ω_ie为地球自转角速度，ω_yi表示转台转动中第i个位置陀螺的输出值，K_y表示陀螺的标度因数，ε_yi表示转台转动中第i个位置的陀螺漂移， i＝1,2,....,n。 

3.根据权利要求2所述的倾斜状态下调制寻北仪安装误差在线估计与寻北误差补偿方法，其特征在于：步骤四中，对陀螺和加速度计的输出信号进行处理，并得到载体的倾斜角，通过如下方法实现, 

令

有 

令

有 

式中g表示地球重力加速度，f_yi表示转台转动中第i个位置加速度计的输出值，i＝1,2,....,n。 

4.根据权利要求3所述的倾斜状态下调制寻北仪安装误差在线估计与寻北误差补偿方法，其特征在于：步骤五中，安装误差角在线估计通过如下方法实现， 

式中a＝ω_Hcosθsinγ,b＝ω_Ncosγ,c＝ω_Nsinθsinγ,d＝ω_Ncosθ,e＝ω_Hsinθ， 

5.根据权利要求4所述的倾斜状态下调制寻北仪安装误差在线估计与寻北 误差补偿方法，其特征在于：步骤六中对载体航向角的修正通过如下方法实现， 

。

6.根据权利要求2—5任何一项所述的倾斜状态下调制寻北仪安装误差在线估计与误差补偿方法，其特征在于：n取值为3000。 

Images