一种单陀螺单加速度计旋转调制寻北方法

摘要

本发明属于光学陀螺寻北技术领域，具体涉及一种单陀螺单加速度计旋转调制寻北方法。本方法以单轴光学陀螺和单轴加速度计为敏感器件，利用连续旋转的方法消除陀螺漂移等因素对寻北结果的影响，通过设定中间变量建立起陀螺仪输出的角速度及加速度计输出的加速度与初始纬度及航向角之间的关系，用单个加速度计的输出结果补偿初始滚动角及俯仰角的影响；并通过建立以中间变量、陀螺漂移和加速度计零偏为状态量的Kalman滤波误差模型，经滤波估计获得中间变量的数值；并通过数值计算获得初始纬度及航向角的数值。本方法利用单轴陀螺、单轴加速度计以及旋转机构即可完成寻北，不需要初始纬度信息、不需要精确调平，具有简单、实用、可靠性高、成本低等特点。

说明书

技术领域

本发明属于光学陀螺寻北技术领域，具体涉及一种单陀螺单加速度计旋 转调制寻北方法。

背景技术

寻北仪在陆军战车、火炮等军事领域得到广泛的应用。寻北仪的基本原 理是在静态条件下，采用陀螺仪测量北向地球自转角速度，来获得陀螺敏感 轴相对北向的偏角，即航向角。寻北原理决定了其精度容易受到陀螺零位、 不水平角等因素的影响，需要采取一定措施来消除各种因素对寻北精度的影 响。

通常采用的寻北方法是利用两个陀螺仪，配合旋转机构，在0°～360°范 围内等间距位置采集陀螺测量输出的角速度值，来获得陀螺敏感轴相对北向 的偏角，即航向角。常用的寻北方案包括两位置方案、三位置方案以及四位 置方案等。近年来开始出现利用连续旋转调制的方法，来消除陀螺零位等因 素的影响。文献《一种实用旋转调制式陀螺寻北仪的设计》（徐海刚等，兵工 学报，第31卷第5期，2010年5月）以及《采用旋转调制技术的高精度陀 螺寻北方案》（白云超等，中国惯性技术学报，第18卷第4期，2010年8月） 都提出利用连续旋转调制的方案消除陀螺漂移等因素对寻北精度的影响，两 篇文献的共同点都是采用单轴陀螺和两个加速度计并利用旋转机构的连续旋 转来完成寻北计算。

发明内容

本发明需要解决的技术问题为：提出一种利用单轴光学陀螺、单轴加速 度计以及连续旋转调制技术即可实现寻北的方法。

本发明的技术方案如下所述：

一种单陀螺单加速度计旋转调制寻北方法，包括以下步骤：

步骤1.滤波估计

步骤1.1.误差模型

通过对旋转调制下陀螺仪及加速度计输出进行公式推导得如下输出数据 关系：

$\left(\begin{array}{c}{\omega }_x^g=\cos \left(\alpha \right)A^{\prime }+\sin \left(\alpha \right)B^{\prime }+{ϵ}^{\prime }\\ f_x^a=\cos \left(\alpha \right)C^{\prime +}\sin \left(\alpha \right)D^{\prime }+{▿}^{\prime }\end{array}\right)---\left(1\right)$

其中，

A′、B′、C′及D′为中间变量；

ε′为陀螺漂移；

为加速度计零偏；

α为转台旋转角速度，范围可以为5°/s～40°/s；

为陀螺仪输出的角速度信息；

加速度计输出的加速度计信息；

利用上述误差方程，建立Kalman滤波器，取状态量如下：

$X=\left(\begin{array}{cccccc}{A}^{\prime }& B^{\prime }& {ϵ}^{\prime }& C^{\prime }& D^{\prime }& {▿}^{\prime }\end{array}\right)---\left(2\right)$

状态方程由各状态量的定义得Φ=0；

得观测方程：

$\mathrm{hk}=\left(\begin{array}{cccccc}\cos \left(\alpha \right)& \sin \left(\alpha \right)& 1& 0& 0& 0\\ 0& 0& 0& \cos \left(\alpha \right)& \sin \left(\alpha \right)& 1\end{array}\right)---\left(3\right)$

对上式在滤波周期内进行积分，得新的观测方程：

$\mathrm{Zk}=\mathrm{Hk}·\mathrm{Xk}=\underset{i=1}{\overset{n}{\Sigma }}\mathrm{hk}\left(i\right)X\left(i\right)=\left(\underset{i=1}{\overset{n}{\Sigma }}\mathrm{hk}\left(i\right)\right)X\left(n\right)---\left(4\right)$

其中，

观测量为：

$\mathrm{Zk}=\left(\begin{array}{c}\underset{i=1}{\overset{n}{\Sigma }}{\omega }_x^g\left(i\right)\\ \underset{i=1}{\overset{n}{\Sigma }}{f}_x^a\left(i\right)\end{array}\right)---\left(5\right);$

步骤1.2.Kalman滤波方程

建立上述误差模型后，利用Kalman滤波方法作为参数辨识方法，完成 对状态量的估计：

状态一步预测：

${\stackrel{^^}{X}}_{k,k-1}={\Phi }_{k,k-1}{X}_{k-1}---\left(6\right)$

状态估计：

$\stackrel{^^^}{X_k}=X_{k,k-1}+K_k[Z_k-H_k{X}_{k,k-1}]---\left(7\right)$

滤波增益矩阵：

$K_k=P_{k,k-1}{H}_k^T[H_k{P}_{k,k-1}{H}_k^T+R_k{]}^{-1}---\left(8\right)$

一步预测误差方差阵：

$P_{k,k-1}={\Phi }_{k,k-1}{P}_{k-1}{\Phi }_{k,k-1}^T+{\Gamma }_{k,k-1}{Q}_{k-1}{\Gamma }_{k,k-1}^T---\left(9\right)$

估计误差方差阵：

P_k=[I-K_kH_k]P_k,k-1  (10)

其中，

为一步状态预测值；

为状态估计矩阵；

Φ_k,k-1为状态一步转移矩阵；

H_k为量测矩阵；

Z_k为量测量；

K_k为滤波增益矩阵；

R_k为观测噪声阵；

P_k,k-1为一步预测误差方差阵；

P_k为估计误差方差阵；

Γ_k,k-1为系统噪声驱动阵；

Q_k-1为系统噪声阵；

步骤2.寻北计算

由步骤1的Kalman滤波得到中间变量A′、B′、C′、D′的数值，由安装 误差角经下式计算获得新的中间变量A、B、C、D：

$\left(\begin{array}{c}A=A^{\prime }/\cos \left({\theta }_1\right)\\ B=B^{\prime }/\cos \left({\theta }_1\right)\\ C=C^{\prime }/\cos \left({\theta }_2\right)\\ D=D^{\prime }/\cos \left({\theta }_2\right)\end{array}\right)---\left(11\right)$

其中，

θ₁为陀螺仪敏感轴与转台平面的夹角；

θ₂为加速度计敏感轴与转台平面的夹角；

则初始姿态角由下式计算获得：

$\left(\begin{array}{c}\theta =\arcsin \left[\frac{C\cos \left(\phi \right)-D\sin \left(\phi \right)}{g}\right]\\ \gamma =\arcsin \left[\frac{C\sin \left(\phi \right)+D\cos \left(\phi \right)}{g\cos \left(\theta \right)}\right]\end{array}\right)---\left(12\right)$

其中，

φ为陀螺仪敏感轴与加速度计敏感轴在转台平面投影的夹角；

θ为初始俯仰角；

γ为初始滚动角；

通过下式计算纬度和初始航向角：

其中，

$\left(\begin{array}{c}a=\cos \left(\theta \right)\\ b=\sin \left(\theta \right)\\ d=\sin \left(\gamma \right)\cos \left(\theta \right)\\ e=\sin \left(\gamma \right)\sin \left(\theta \right)\\ f=\cos \left(\gamma \right)\end{array}\right)---\left(14\right)$

$\left(\begin{array}{c}m=b^2{f}^2+a^2{f}^2+{(\mathrm{ad}+\mathrm{be})}^2\\ n=-2[{\mathrm{Af}}^{2}b+(\mathrm{ad}+\mathrm{be})(\mathrm{eA}+\mathrm{aB})]\\ l=A^2{f}^2+{(\mathrm{eA}+\mathrm{aB})}^2-a^2{f}^2{\omega }_{\mathrm{ie}}^2\end{array}\right)---\left(15\right)$

其中，

ω_ie为地球自转角速率；

L为寻北计算获得的纬度信息；

ψ为寻北计算获得的初始航向角信息。

本发明的有益效果为：

本发明的一种单陀螺单加速度计旋转调制寻北方法，利用单轴陀螺、单 轴加速度计以及旋转机构即可完成寻北。

本发明的方法通过旋转机构的连续旋转补偿陀螺漂移等因素对寻北计算 的影响，通过加速度计输出补偿不水平角的影响，并建立中间变量与纬度及 初始航向角之间的关系，通过Kalman滤波估计中间变量的数值，完成寻北计 算。

本发明的方法不需输入初始纬度信息，不需对台面进行精确调平，具有 简单、实用、可靠性高以及成本低的特点，可满足不同精度寻北仪的需求。

附图说明

图1为实现本发明方法的陀螺仪及加速度计与转台平面的安装关系示意 图。

具体实施方式

下面结合附图和实施例对本发明的一种单陀螺单加速度计旋转调制寻北 方法进行详细说明。

本发明的方法利用旋转调制技术消除陀螺漂移等因素对寻北精度的影 响，通过加速度计零偏计算并补偿不水平角对寻北结果的影响。本寻北方法 主要分两个步骤：一是滤波估计，主要是通过Kalman滤波完成中间变量的估 计；二是寻北计算，即利用滤波估计出的中间变量的数值，经过公式计算获 得寻北结果。

步骤1.滤波估计

滤波估计主要是建立中间变量的误差模型，利用Kalman滤波方法完成 中间变量的数值估计。

步骤1.1.误差模型

通过对旋转调制下陀螺仪及加速度计输出进行公式推导可得输出数据 关系如下：

$\left(\begin{array}{c}{\omega }_x^g=\cos \left(\alpha \right)A^{\prime }+\sin \left(\alpha \right)B^{\prime }+{ϵ}^{\prime }\\ f_x^a=\cos \left(\alpha \right)C^{\prime }+\sin \left(\alpha \right)D^{\prime }+{▿}^{\prime }\end{array}\right)---\left(13\right)$

其中，

A′、B′、C′及D′为中间变量，没有特定物理意义，均为定值；

ε′为陀螺漂移；

为加速度计零偏；

α为转台旋转角速度，该值的选取与所用的陀螺仪精度相关，稳定性 差的陀螺α的取值应稍大，范围通常在5°/s～40°/s；

为陀螺仪输出的角速度信息；

加速度计输出的加速度计信息。

利用上述误差方程，建立Kalman滤波器，取状态量如下：

$X=\left(\begin{array}{cccccc}{A}^{\prime }& B^{\prime }& {ϵ}^{\prime }& C^{\prime }& D^{\prime }& {▿}^{\prime }\end{array}\right)---\left(14\right)$

状态方程由各状态量的定义可知Φ=0。

可得观测方程：

$\mathrm{hk}=\left(\begin{array}{cccccc}\cos \left(\alpha \right)& \sin \left(\alpha \right)& 1& 0& 0& 0\\ 0& 0& 0& \cos \left(\alpha \right)& \sin \left(\alpha \right)& 1\end{array}\right)---\left(15\right)$

为了消除角速率及加速度噪声对滤波器的影响，对上式在滤波周期内 进行积分，可得新的观测方程为：

$\mathrm{Zk}=\mathrm{Hk}·\mathrm{Xk}=\underset{i=1}{\overset{n}{\Sigma }}\mathrm{hk}\left(i\right)X\left(i\right)=\left(\underset{i=1}{\overset{n}{\Sigma }}\mathrm{hk}\left(i\right)\right)X\left(n\right)---\left(16\right)$

其中，如当陀螺仪和加速度 计的输出周期为0.005s，滤波周期为1s时，n的取值为200。

观测量为：

$\mathrm{Zk}=\left(\begin{array}{c}\underset{i=1}{\overset{n}{\Sigma }}{\omega }_x^g\left(i\right)\\ \underset{i=1}{\overset{n}{\Sigma }}{f}_x^a\left(i\right)\end{array}\right)---\left(17\right)$

步骤1.2.Kalman滤波方程

建立上述误差模型后，利用Kalman滤波方法作为参数辨识方法，完成对 状态量的估计。Kalman滤波方程采用文献《卡尔曼滤波和组合导航原理》 （第一版，秦永元等编著）中的形式，具体公式如下：

状态一步预测：

${\stackrel{^^}{X}}_{k,k-1}={\Phi }_{k,k-1}{X}_{k-1}---\left(18\right)$

状态估计：

$\stackrel{^^^}{X_k}=X_{k,k-1}+K_k[Z_k-H_k{X}_{k,k-1}]---\left(19\right)$

滤波增益矩阵：

$K_k=P_{k,k-1}{H}_k^T[H_k{P}_{k,k-1}{H}_k^T+R_k{]}^{-1}---\left(20\right)$

一步预测误差方差阵：

$P_{k,k-1}={\Phi }_{k,k-1}{P}_{k-1}{\Phi }_{k,k-1}^T+{\Gamma }_{k,k-1}{Q}_{k-1}{\Gamma }_{k,k-1}^T---\left(21\right)$

估计误差方差阵：

P_k=[I-K_kH_k]P_k,k-1  (22)

其中，

为一步状态预测值；

为状态估计矩阵；

Φ_k,k-1为状态一步转移矩阵；

H_k为量测矩阵；

Z_k为量测量；

K_k为滤波增益矩阵；

R_k为观测噪声阵；

P_k,k-1为一步预测误差方差阵；

P_k为估计误差方差阵；

Γ_k,k-1为系统噪声驱动阵；

Q_k-1为系统噪声阵。

步骤2.寻北计算

由步骤1的Kalman滤波可得中间变量A′、B′、C′、D′的数值，接下来 即可进行寻北结果的计算。

设θ₁，θ₂及φ为陀螺仪和加速度计敏感轴与转台平面之间的安装误差角， 可由标定获得，安装误差角的定义如图1所示：θ₁为陀螺仪敏感轴与转台平 面的夹角，θ₂为加速度计敏感轴与转台平面的夹角，φ为陀螺仪敏感轴与加速 度计敏感轴在转台平面投影的夹角。

由安装误差角经下式计算获得新的中间变量A、B、C、D。

$\left(\begin{array}{c}A=A^{\prime }/\cos \left({\theta }_1\right)\\ B=B^{\prime }/\cos \left({\theta }_1\right)\\ C=C^{\prime }/\cos \left({\theta }_2\right)\\ D=D^{\prime }/\cos \left({\theta }_2\right)\end{array}\right)---\left(23\right)$

则初始姿态角可由下式计算获得：

$\left(\begin{array}{c}\theta =\arcsin \left[\frac{C\cos \left(\phi \right)-D\sin \left(\phi \right)}{g}\right]\\ \gamma =\arcsin \left[\frac{C\sin \left(\phi \right)+D\cos \left(\phi \right)}{g\cos \left(\theta \right)}\right]\end{array}\right)---\left(24\right)$

其中θ为初始俯仰角，γ为初始滚动角。

按如下方法计算如下变量：

$\left(\begin{array}{c}a=\cos \left(\theta \right)\\ b=\sin \left(\theta \right)\\ d=\sin \left(\gamma \right)\cos \left(\theta \right)\\ e=\sin \left(\gamma \right)\sin \left(\theta \right)\\ f=\cos \left(\gamma \right)\end{array}\right)---\left(25\right)$

$\left(\begin{array}{c}m=b^2{f}^2+a^2{f}^2+{(\mathrm{ad}+\mathrm{be})}^2\\ n=-2[{\mathrm{Af}}^{2}b+(\mathrm{ad}+\mathrm{be})(\mathrm{eA}+\mathrm{aB})]\\ l=A^2{f}^2+{(\mathrm{eA}+\mathrm{aB})}^2-a^2{f}^2{\omega }_{\mathrm{ie}}^2\end{array}\right)---\left(26\right)$

其中，

a、b、d、e、f以及m、n、l都为中间变量，没有特定物理意义，均为定值；

ω_ie为地球自转角速率。

由上述中间变量作如下计算：

其中L及ψ即为寻北计算获得的纬度及初始航向角信息。

本发明的关键是通过设定中间变量建立起陀螺仪输出的角速度及加速度 计输出的加速度与初始纬度及航向角之间的关系，利用旋转过程中俯仰角和 滚动角存在相关转换的特点，用单个加速度计的输出结果补偿初始滚动角及 俯仰角的影响，并通过建立以中间变量、陀螺漂移和加速度计零偏为状态量 的Kalman滤波误差模型，经过滤波估计获得中间变量的数值，并通过数值计 算获得初始纬度及航向角的数值，实现单轴陀螺仪及单轴加速度计连续旋转 的寻北计算。本方法在实施时首先建立Kalman滤波器的误差模型，通过滤波 计算完成对中间变量的估计，并在得到估计值后，利用公式计算获得初始纬 度及航向角的数值。