CN101187562A - 船用光纤陀螺捷联系统初始姿态确定方法 - Google Patents

Abstract

本发明提供的是一种船用光纤陀螺捷联系统初始姿态确定方法。包括(1)对SINS进行预热准备；(2)SINS预热准备完后，调整SINS进入初始对准的粗对准阶段，采集船用陀螺仪输出和加速度计输出，通过粗对准结束后得到的粗略的初始捷联姿态矩阵计算地理坐标系和真实地理坐标系之间的三个误差失准角φ_x ⁿ、φ_y ⁿ和φ_z ⁿ，当三个误差失准角小于10度时转入精对准步骤、否则重新进行粗对准；(3)精对准，在粗对准的基础上，继续采集船用陀螺仪输出和加速度计输出，采用线性二次型LQ最优控制技术，得到精对准过程中所需要的最佳修正控制角速度。本发明既权衡到了时间，又权衡到了对准的精度，该初始姿态求解方案的对准精度与传统方法相当，但对准时间和收敛速度大大提高。

Description

船用光纤陀螺捷联系统初始姿态确定方法

(一)技术领域

本发明涉及的是一种测量方法，特别是涉及一种捷联航姿系统的初始姿态确定技术。

(二)背景技术

捷联惯导系统SINS是一种自主式导航系统，采用牛顿第二定律在初始信息的基础上进行积分运算，可以连续、实时地提供位置、速度和姿态信息，具有隐蔽性好，不受干扰等优点，广泛应用于军事武器系统中。而初始信息的取得是整个系统的十分关键环节，如何快速准确获得初始信息是惯性导航领域一项非常重要课题。

捷联惯导系统初始姿态确定的过程包括粗对准和精对准阶段；初始姿态确定要求包括精度性和快速性两个方面，但是这两个方面是十分难以协调的，如何在最短的时间内获得最好的对准精度？传统的精对准方法在精度上基本能满足要求，但对准时间一般需要4～5分钟，甚至更多，这对于那些要求快速性的舰船武器导航系统将是一个致命的弱点。

(三)发明内容

本发明的目的在于提供一种既权衡时间，又权衡对准的精度，使在对准精度与传统方法相当的前提下对准时间和收敛速度大大提高的船用光纤陀螺捷联系统初始姿态确定方法。

本发明的目的是这样实现的：

(1)对SINS进行预热准备；

(2)SINS预热准备完后，调整SINS进入初始对准的粗对准阶段，采集船用陀螺仪输出和加速度计输出，通过粗对准结束后得到的粗略的初始捷联姿态矩阵计算地理坐标系和真实地理坐标系之间的三个误差失准角φ_x ⁿ、φ_y ⁿ和φ_z ⁿ，当三个误差失准角小于10度时转入精对准步骤、否则重新进行粗对准；

(3)精对准，在粗对准的基础上，继续采集船用陀螺仪输出和加速度计输出，采用线性二次型LQ最优控制技术，得到精对准过程中所需要的最佳修正控制角速度，让精对准过程快速收敛下来，精度还能与传统方法相当。

所述的精对准包括：

1)建立船用光纤陀螺捷联系统的处于摇摆基座下状态方程和量测方程；

2)进行船用光纤陀螺捷联系统的抗干扰设计；

3)确定最优控制的线性二次型性能指标；

4)采用动态规划的递推最优控制增益；

5)离线算出最优控制增益矩阵L。

本发明的原理是：首先建立船用光纤陀螺捷联系统的空间状态方程；对船用捷联系统的干扰项进行抗干扰设计，得到去除干扰后的空间状态方程；然后给出最优控制的线性二次型性能指标；最后采用动态规划的递推算出最优的控制增益。实际精对准过程中，将离线算出的最优控制增益加入到对准回路中即可。

本发明的精对准方法，采用现代最优控制理论中的线性二次型(LQ)最优控制系统，在二次型性能指标中，将精对准的最终对准精度、对准的时间和最优控制量的大小进行最优控制，使得二次型性能指标函数取得极小值。既权衡到了时间，又权衡到了对准的精度，该初始姿态求解方案的对准精度与传统方法相当，但对准时间和收敛速度大大提高，相对于传统的方案，该方法只花费3秒钟左右就完成了整个精对准过程。

本发明的初始姿态求解新方案与现有技术相比的优点和自身的优势：

(1)在二次型性能指标中，将精对准的最终对准精度、对准的时间和最优控制量的大小进行最优控制，使得二次型性能指标函数取得极小值。既权衡到了时间，又权衡到了对准的精度，相对于其它传统的对准方法，方案的对准时间短，收敛速度快，整个过程只需几秒钟；

(2)针对船用捷联系统中的干扰，通过进行差分变换，将阶跃型干扰项从方程去除，达到抗干扰的效果；

(3)反馈控制增益采用动态规划的递推法离线计算，由于系统能观能控，反馈增益矩阵的系数都趋于常值(曲线从后往前看)，实际精对准时将增益矩阵的系数都取常值，并计算出最优控制量加入对准回路中就可完成用捷联系统的精对准过程，这样的做法减轻实际系统的计算量，不用在线计算控制增益也可以减少计算时间，提高整个系统的计算速度。

(四)附图说明

图1是本发明的捷联系统精对准的最优控制结构图；

图2是反馈控制矩阵L中L11、L21、L31的曲线图；

图3是反馈控制矩阵L中L12、L22、L32的曲线图；

图4是反馈控制矩阵L中L13、L23、L33的曲线图；

图5是反馈控制矩阵L中L14、L24、L34的曲线图；

图6是反馈控制矩阵L中L15、L25、L35的曲线图；

图7是反馈控制矩阵L中L16、L26、L36的曲线图；

图8是船用光纤陀螺捷联系统初始姿态确定具体实施方式；

图9是摇摆基座下的初始精对准误差失准角曲线。

(五)具体实施方式

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

本发明的原理是：首先建立船用光纤陀螺捷联系统的空间状态方程；对船用捷联系统的干扰项进行抗干扰设计，得到去除干扰后的空间状态方程；然后给出最优控制的线性二次型性能指标；最后采用动态规划的递推算出最优的控制增益。实际精对准过程中，将离线算出的最优控制增益加入到对准回路中即可。

1)建立船用光纤陀螺捷联系统的处于摇摆基座下状态方程和量测方程：

其中：ω_x ^c、ω_y ^c、ω_z ^c为修正控制角速率，φ_x ⁿ、φ_y ⁿ、φ_z ⁿ为误差角失准角；δω^d、δa^d为干扰项。

为加速度在惯性系内的三周投影，

由于对准过程只需几秒钟，所以C_n ⁱ中的t可以取常值(比如取t＝2)。

令状态向量 $x={\left[\begin{array}{ccc}{\mathrm{\φ}}_x^n& {\mathrm{\φ}}_y^n& {\mathrm{\φ}}_z^n\end{array}\right]}^T,$ $\mathrm{\δ}{\mathrm{\ω}}^d={\left[\begin{array}{ccc}{\mathrm{\δ\ω}}_x^d& {\mathrm{\δ\ω}}_x^d& {\mathrm{\δ\ω}}_x^d\end{array}\right]}^T,$ $u={\left[\begin{array}{ccc}{\mathrm{\ω}}_x^c& {\mathrm{\ω}}_x^c& {\mathrm{\ω}}_x^c\end{array}\right]}^T,$ $y={\left[\begin{array}{ccc}{z}_1& z_2& z_3\end{array}\right]}^T,$ $\mathrm{\δ}{a}^d={\left[\begin{array}{ccc}\mathrm{\δ}{a}_x^d& \mathrm{\δ}{a}_y^d& \mathrm{\δ}{a}_z^d\end{array}\right]}^T,$ 则上述捷联系统初始对准的状态方程和量测方程可写成：

$\left\{\begin{array}{c}\stackrel{.}{x}\left(t\right)=\mathrm{Ax}\left(t\right)+\mathrm{Bu}\left(t\right)+\mathrm{\δ}{\mathrm{\ω}}^d\left(t\right)\\ y\left(t\right)=\mathrm{Cx}\left(t\right)+\mathrm{\δ}{a}^d\left(t\right)\end{array}\right.---\left(2\right)$

其中：

$B=\left[\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\\ 0& 0& 1\end{array}\right]$

$C=\left[\begin{array}{ccc}0& g_z^i& -g_y^i\\ -g_z^i& 0& g_x^i\\ g_y^i& {-g}_x^i& 0\end{array}\right]C_n^i$

假设系统的采样间隔为T秒，那么可得到上述状态方程和量测方程的离散化形式：

$\left\{\begin{array}{c}x(k+1)=\mathrm{Fx}\left(k\right)+\mathrm{Gu}\left(k\right)+v\left(k\right)\\ y\left(k\right)=\mathrm{Cx}+\mathrm{\δ}{a}^d\left(k\right)\end{array}\right.---\left(3\right)$

其中F＝e^AT， $G={\∫}_0^T{e}^{\mathrm{AT}}\mathrm{dtB},$ $v\left(k\right)={\∫}_0^T{e}^{\mathrm{AT}}\mathrm{dt\δ\ω}\left(k\right).$ F和G可以通过矩阵指数转化为用矩阵级数求和法进行求解，v(k)、δa^d(k)不用实时求解，它可以通过下面的抗干扰设计而除去。

2)船用光纤陀螺捷联系统的抗干扰设计

由离散化状态方程式(3)可知，有一干扰项v(k)，δa^d(k)，其中v(k)为陀螺的常值漂移和随机漂移等扰动，而δa^d(k)为加速度计常值偏置和随机干扰等扰动。为了分析问题，我们完全可以认为v(k)、δa^d(k)为近似阶跃型干扰。这样我们可以通过下面的差分变换将v(k)、δa^d(k)进行消除。

定义各量的差分为：

$\left\{\begin{array}{c}\mathrm{\Δx}\left(k\right)=x\left(k\right)-x(k-1)\\ \mathrm{\Δu}\left(k\right)=u\left(k\right)-u(k-1)\\ \mathrm{\Δy}\left(k\right)=y\left(k\right)-y(k-1)\\ \mathrm{\Δv}\left(k\right)=v\left(k\right)-v(k-1)\\ \mathrm{\Δ\δ}{a}^d\left(k\right)=\mathrm{\δ}{a}^d\left(k\right)-\mathrm{\δ}{a}^d(k-1)\end{array}\right.---\left(4\right)$

因为v(k)、δa^d(k)为近似阶跃型干扰，显然当k≥1时，有Δδa^d(k)＝0，Δv(k)＝0，对式(3)两边进行差分变换，得：

Δx(k+1)＝FΔx(k)+GΔu(k)         (5)

Δy(k+1)＝CΔx(k+1)               (6)

式(6)可进一步改写为：

y(k+1)-y(k)＝CΔx(k+1)            (7)

将式(5)代入(7)式，可得：

y(k+1)＝y(k)+CFΔx(k)+CGΔu(k)    (8)

令 $\stackrel{\‾}{X}\left(k\right)=\left[\begin{array}{c}y\left(k\right)\\ \mathrm{\Δx}\left(k\right)\end{array}\right],$ 结合式(5)、(8)，可写出新的关于的状态方程为：

$\stackrel{\‾}{X}(k+1)=\stackrel{\‾}{F}\stackrel{\‾}{X}\left(k\right)+\stackrel{\‾}{G}\mathrm{\Δu}\left(k\right),k\≥1---\left(9\right)$

其中： $\stackrel{\‾}{F}={\left[\begin{array}{cc}I& \mathrm{CF}\\ 0& F\end{array}\right]}_{6\×6},$ $\stackrel{\‾}{G}={\left[\begin{array}{c}\mathrm{CG}\\ G\end{array}\right]}_{6\×3}.$

新的量测方程可写为：

$\stackrel{\‾}{Z}\left(k\right)=I_{6\×6}\stackrel{\‾}{X}\left(k\right)---\left(10\right)$

3)给出最优控制的线性二次型性能指标

我们知道，进行捷联惯导初始对准的实质上是让误差失准角快速收敛到零，通过分析，我们发现当三个误差角收敛趋近于零时，系统的观测量也将趋于零，这正好符合最优控制中的输出调节器问题——即用不大的控制量，使输出趋向零值附近。所以我们要进行最优控制的目的就是在给定的时间内让y(t)的各分量趋近于零。输出误差向量e(t)为：

e(t)＝0-y(t)＝-y(f)    (11)

因此，控制任务就变为寻求一个控制u(t)，在有限的时间内，以使得系统的输出误差尽可能的小，而且对于u(t)也有要求，有时对u(t)有幅值要求，或者对于u(t)内各分量的权值有所要求。把这些性能要求用离散形式二次型性能指标来表示的话，可得：

$J=\frac{1}{2}{\stackrel{\‾}{X}}^T\left(N\right){\stackrel{\‾}{Q}}_0\stackrel{\‾}{X}\left(N\right)+\frac{1}{2}\underset{k=0}{\overset{k=N-1}{\mathrm{\Σ}}}[{\stackrel{\‾}{X}}^T\left(k\right){\stackrel{\‾}{Q}}_1\stackrel{\‾}{X}\left(k\right)+\mathrm{\Δ}{u}^T\left(k\right){\stackrel{\‾}{Q}}_2\mathrm{\Δu}\left(k\right)]---\left(12\right)$

其中

为6×6半正定常数阵，

为6×6半正定常数阵，

为3×3正定常数阵。式中第一项是控制姿态的精度，第二项控制系统的收敛速度，第三项协调修正控制角速度的大小。

4)采用动态规划的递推最优控制增益

由式(9)，式(10)和式(12)的离散化二次型性能指标可以看出，这符合最优调节器问题：用最优的控制量，在有限的时间内使输出趋向于零值附近。

取采样时间T＝0.01秒，取 ${\stackrel{\‾}{Q}}_0=I_{6\×6},$ ${\stackrel{\‾}{Q}}_1=I_{6\×6},$ ${\stackrel{\‾}{Q}}_2=\mathrm{diag}\left\{\begin{array}{ccc}b1& b2& b3\end{array}\right\},$ b1、b2、b2分别为常数(用于控制控制量的权值)，令 ${\stackrel{\‾}{Q}}_1=D^{T}D,$ 为了便于分析，可取D＝I_6×6，计算出

对(9)、(10)的线性离散化系统进行能控性和能观性判定。

能控性判定： $\mathrm{rank}\left[\begin{array}{cccccc}\stackrel{\‾}{G}& \stackrel{\‾}{F}\stackrel{\‾}{G}& {\stackrel{\‾}{F}}^2\stackrel{\‾}{G}& {\stackrel{\‾}{F}}^3\stackrel{\‾}{G}& {\stackrel{\‾}{F}}^4\stackrel{\‾}{G}& {\stackrel{\‾}{F}}^5\stackrel{\‾}{G}\end{array}\right]=6,$ 所以能控；

能观性判定： $\mathrm{rank}{\left[\begin{array}{cccccc}D& D\stackrel{\‾}{F}& D{\stackrel{\‾}{F}}^2& D{\stackrel{\‾}{F}}^3& D{\stackrel{\‾}{F}}^4& D{\stackrel{\‾}{F}}^5\end{array}\right]}^T=6,$ 所以能观。

系统能控又能观，则最优控制一定存在，且求解出来的控制系统是渐进稳定的。

设反馈控制矩阵 $L=\left[\begin{array}{cccccc}L11& L12& L13& L14& L15& L16\\ L21& L22& L23& L24& L25& L26\\ L31& L32& L33& L34& L35& L36\end{array}\right],$ 则可得如下递推公式：

$\left\{\begin{array}{c}\mathrm{\Δu}\left(k\right)=-L\left(k\right)\stackrel{\‾}{X}\left(k\right)\\ L\left(k\right)={[{\stackrel{\‾}{Q}}_2+{\stackrel{\‾}{G}}^{T}S(k+1)\stackrel{\‾}{G}]}^{-1}{\stackrel{\‾}{G}}^{T}S(k+1)\stackrel{\‾}{F}\\ S\left(k\right)={[\stackrel{\‾}{F}-\stackrel{\‾}{G}L\left(k\right)]}^{T}S(k+1)[\stackrel{\‾}{F}-\stackrel{\‾}{G}L\left(k\right)]+{\stackrel{\‾}{Q}}_1+L^T\left(k\right){\stackrel{\‾}{Q}}_{2}L\left(k\right)\\ S\left(N\right)={\stackrel{\‾}{Q}}_0\\ k=N-1,N-2,L0\end{array}\right.---\left(11\right)$

将 $\stackrel{\‾}{X}\left(k\right)=\left[\begin{array}{c}y\left(k\right)\\ \mathrm{\Δx}\left(k\right)\end{array}\right]$ 代入式(11)中的 $\mathrm{\Δu}\left(k\right)=-L\left(k\right)\stackrel{\‾}{X}\left(k\right)$ 可得：

$\mathrm{\Δu}\left(k\right)=-L\left(k\right)\stackrel{\‾}{X}\left(k\right)=-L1\left(k\right)y\left(k\right)-L2\left(k\right)\mathrm{\Δx}\left(k\right)---\left(12\right)$

其中：L＝[L1L2]，L1、L2是相应的3×3分块矩阵，对式(12)两边作求和运算并整理可以得到：

$u\left(k\right)=-L1\left(k\right)\underset{i=1}{\overset{K}{\mathrm{\Σ}}}y\left(i\right)-L2\left(k\right)x\left(k\right)+L2\left(0\right)x\left(0\right)+u\left(0\right)---\left(13\right)$

由上式看出，最优控制u(k)与初始条件x(0)、u(0)有关。实际上，x(0)和u(0)是难以预先知道的，因此通常只实现如下的控制规律：

$u\left(k\right)=-L1\left(k\right)\underset{i=1}{\overset{k}{\mathrm{\Σ}}}y\left(i\right)-L2\left(k\right)x\left(k\right)---\left(14\right)$

由上式(14)可见，u(k)由两部分组成，一部分是状态x(k)的比例调节，另一部分是输出量的求和，可以看成是输出量的积分调节。图1表示了捷联系统初始精对准的最优控制结构。

5)离线算出的最优控制增益矩阵L

反馈控制矩阵L中各项控制增益可以离线计算，所以可以事先用计算机计算出L中各项增益，然后再加入实际系统进行精对准最优控制，这样可以达到减轻实际硬件系统的计算量，也可减少实际船用光纤陀螺捷联系统的精对准时间。

图2-7是按照状态方程和线性二次型性能指标离线计算出来的反馈控制增益矩阵，实际精对准中不需要在计算。

结合图8，本发明的具体实施方法如下：

1.用PC机离线计算出LQ最优控制的控制增益矩阵的系数

按照上面介绍的方法，先得到船用光纤陀螺捷联系统的状态空间方程和二次型性能指标，进行抗干扰处理，采用动态规划的递推法计算出反馈增益系数。

2.船用光纤陀螺捷联系统的准备

SINS开机后，进入准备状态。

3.船用光纤陀螺捷联系统的粗对准阶段

SINS准备完毕，采集陀螺仪输出和加速度输出，采用二阶调平+方位估算的粗对准方式对系统进行粗对准，获得一个粗略的初始捷联姿态矩阵，使真实地理系和计算地理系的失准角在10度以内(一般的粗对准方法都没有任何问题)。

4.船用光纤陀螺捷联系统的精对准阶段

粗对准完毕后，进入系统的精对准过程。将用PC机离线计算出的反馈增益系数加入对准控制回路中，通过对准回路得到实时的最佳的修正控制角速度，以此修正角速度达到精对准的目的。

5.初始姿态确定完毕，对准过程结束。

对本发明的有益效果说明如下：

假设有如下环境：

(1)加速度计和陀螺的性能如下：

①陀螺的常值漂移为：ε_x＝ε_y＝ε_z＝0.1o/h；陀螺的随机漂移为：0.01o/h；

②陀螺刻度系数误差：K_gx＝K_gy＝K_gz＝10^-4；

③加速度计零位偏差：_x＝_y＝_z＝10^-4g；加速度计随机干扰：10^-5g；

④加速度计刻度系数误差：K_ax＝K_ay＝K_az＝10^-4；

⑤粗对准结束后姿态误差角：(4°5°5°)；

 ⑥取采样时间T＝0.01秒，各加权矩阵取 ${\stackrel{\‾}{Q}}_0{=I}_{6\×6},$ ${\stackrel{\‾}{Q}}_1=I_{6\×6},$ ${\stackrel{\‾}{Q}}_2=\mathrm{diag}\left\{\begin{array}{ccc}1& 1& 3\end{array}\right\};$

(2)假设船舶在海浪激励下分别绕方位轴、纵摇轴和横摇轴作摇摆运动。其模型为：

由垂荡、纵荡和横荡引起的线加速度为

其中，A_Dx＝0.02m，A_Dy＝0.02m，A_Dz＝0.16m；ω_Di＝2π/T_Di，且T_Dx＝8s，T_Dy＝10s，T_Dz＝10s；_Di为[0，2π]上服从均匀分布的随机相位；

载体初始位置：北纬45.7796°，东经126.6705°；

通过计算机模拟仿真，如果初始时刻计算平台坐标系和真实平台坐标系间的三个误差角小于10°的话，整个对准过程就能在几秒钟内完成。

摇摆基座下的初始精对准误差失准角曲线如图9所示。

图中只用了3秒就已经达到了对准的精度，经过3秒后，北向水平误差角收敛到0.05°左右，东向水平误差失准角收敛到0.08°左右，方位误差失准角收敛到0.r左右。

Claims (2)

1.一种船用光纤陀螺捷联系统初始姿态确定方法，其特征是：

(1)对SINS进行预热准备；

(2)SINS预热准备完后，调整SINS进入初始对准的粗对准阶段，采集船用陀螺仪输出和加速度计输出，通过粗对准结束后得到的粗略的初始捷联姿态矩阵计算地理坐标系和真实地理坐标系之间的三个误差失准角φ_x ⁿ、φ_y ⁿ和φ_z ⁿ，当三个误差失准角小于10度时转入精对准步骤、否则重新进行粗对准；

(3)精对准，在粗对准的基础上，继续采集船用陀螺仪输出和加速度计输出，采用线性二次型LQ最优控制技术，得到精对准过程中所需要的最佳修正控制角速度，让精对准过程快速收敛下来，精度还能与传统方法相当。

2.根据权利要求1所述的船用光纤陀螺捷联系统初始姿态确定方法，其特征是：所述的精对准包括：

1)建立船用光纤陀螺捷联系统的处于摇摆基座下状态方程和量测方程；

2)进行船用光纤陀螺捷联系统的抗干扰设计；

3)确定最优控制的线性二次型性能指标；

4)采用动态规划的递推最优控制增益；

5)离线算出最优控制增益矩阵L。

Images