CN105865490A - 一种惯性稳定平台固定基座多位置自瞄准方法 - Google Patents

Abstract

一种惯性稳定平台固定基座多位置自瞄准方法，台体在水平面内采用N个位置进行自瞄准，根据X陀螺仪在1,2,…,N个位置的测量值ω_xk和台体转角θ_k，采用最小二乘法计算X陀螺仪漂移ω_gx以及X陀螺仪与当地水平面内的地球北向方位轴的夹角ψ₀的三角函数cosψ₀、sinψ₀，进而得到ψ₀，实现惯性稳定平台固定基座任意位置的自瞄准。确定θ_k的方法分为直接法与间接法。直接法利用框架轴转角解算θ_k；间接法根据期望的θ_k解算所需框架转角，实现平台期望转动。相比现有的三位置、四位置自瞄准算法，该方法可实现对基座倾斜的补偿，克服了固定基座不水平导致的θ_k偏差问题，并且测量位置不再受限于转角三等分或四等分位置，实际测量灵活，对准精度高。

Description

一种惯性稳定平台固定基座多位置自瞄准方法

技术领域

本发明涉及一种惯性稳定平台固定基座多位置自瞄准方法，属于惯性导航自瞄准技术领域。

背景技术

飞行器发射前，其平台惯导系统需要对目标方向或发射方向进行对准。目标方向或发射方向以其与地球真北方向的偏角形式给定，因此为了实现对准，平台惯导系统首先需要确定地球真北方向，此过程便称为平台系统的自瞄准。

自瞄准包括调平和台体转动两部分，前者使台体与地理坐标系水平面平行，后者将台体绕地理系Y轴发生指定转动。目前，惯性稳定平台采用的是三位置或四位置自瞄准方法，将平台基座固定在水平面上，对外环轴转角进行三等分或四等分，并依次进行转动。其具体算法及原理包括：

(1)三/四位置自瞄准方案及算法

惯性稳定平台的台体在水平面内采用三个或四个位置进行自瞄准，定义台体上X轴陀螺仪敏感方位信息，其常值漂移项为ω_gx，设ω_e为北向地速，ψ₀为在第1个位置时X轴陀螺仪的输入轴与当地水平面内的地球北向方位轴的夹角，简记为平台方位角，方向与右手坐标系定义一致，θ_k为台体在第k个位置处X轴陀螺仪的输入轴与第1个位置处X轴陀螺仪的输入轴之间的夹角，有

陀螺仪在各个位置的测量值分别为：

ω_xk＝ω_gx+ω_e cos(ψ₀+θ_k)

1)三位置自瞄准

三位置自瞄方案原理如图1所示。ψ₀求取方法为：

2)四位置自瞄准

四位置自瞄准方案如图2所示。ψ₀求取方法为：

$\begin{array}{c}\left\{\begin{array}{c}{\mathrm{\ω}}_{x_1}={\mathrm{\ω}}_{gx}+{\mathrm{\ω}}_{e}cos{\mathrm{\ψ}}_0\\ {\mathrm{\ω}}_{x_2}={\mathrm{\ω}}_{gx}-{\mathrm{\ω}}_e{\mathrm{cos\ψ}}_0\\ {\mathrm{\ω}}_{x_3}={\mathrm{\ω}}_{gx}-{\mathrm{\ω}}_e{\mathrm{sin\ψ}}_0\\ {\mathrm{\ω}}_{x_4}={\mathrm{\ω}}_{gx}+{\mathrm{\ω}}_e{\mathrm{sin\ψ}}_0\end{array}\right.\\ \⇒{\mathrm{\ψ}}_0=arctan\left(\frac{{\mathrm{\ω}}_{x4}-{\mathrm{\ω}}_{x3}}{{\mathrm{\ω}}_{x1}-{\mathrm{\ω}}_{x2}}\right)\end{array}$

由三/四位置自瞄算法可以看出，现有三/四位置算法采用角度均分的方法，要求θ_k分别以120°和90°为增量进行转动，降低了测量的灵活性，同时传统三/四位置算法依赖于θ_k的精确转动。

(2)θ_k转动实现方法

不失一般性，假设惯性稳定平台系统为三框架四轴结构，定义θ_x′、θ_y、θ_x、θ_z分别为惯性稳定平台的随动轴相对基座、外环轴相对随动轴、内环轴相对外环轴以及台体轴相对内环轴的转角，为叙述方便，分别简称为随动轴、外环轴、内环轴以及台体轴转角。当基座水平时，在第1个位置，θ_k＝0°，有θ_x′＝θ_x＝θ_y＝θ_z＝0°；第k个位置，为了实现θ_k转动，将外环轴转动θ_yk＝θ_k。

θ_k的上述转动实现方法仅适用于平台基座为固定水平基座，当基座不水平时，这种转动外框架的方法，将会使实际转角θ_k*与θ_k存在偏差，当转动存在较大误差或转动不准时，采用现有三/四位置自瞄算法解算得到的ψ₀将存在较大偏差甚至无法对准。

发明内容

本发明解决的技术问题是：克服现有技术的不足，提供一种惯性稳定平台固定基座多位置自瞄准方法，该方法克服了固定基座不水平导致的θ_k偏差问题，同时不再依赖θ_k对转角的三等分或四等分，对准精度高。

本发明解决的技术方案是：一种惯性稳定平台固定基座多位置自瞄准方法，所述惯性稳定平台为三框架四轴平台，步骤如下：

(1.1)惯性稳定平台的台体在水平面内采用N个位置进行自瞄准，定义X轴陀螺仪敏感方位信息，其常值漂移项为ω_gx，在第一个位置时X轴陀螺仪的输入轴与当地水平面内的地球北向方位轴的夹角为ψ₀，方向与右手坐标系定义一致，其中N≥3；

(1.2)根据X轴陀螺仪在1,2,…,N个位置的测量值ω_xk，采用最小二乘法计算ω_gx、cosψ₀、sinψ₀，计算公式为：

$\left[\begin{array}{c}{\mathrm{\ω}}_{gx}\\ {\mathrm{cos\ψ}}_0\\ {\mathrm{sin\ψ}}_0\end{array}\right]={\left(A^{T}A\right)}^{-1}{A}^T\left[\begin{array}{c}{\mathrm{\ω}}_{x_1}\\ {\mathrm{\ω}}_{x_2}\\ \·\\ \·\\ \·\\ {\mathrm{\ω}}_{x_N}\end{array}\right]$

其中：

$A=\left[\begin{array}{ccc}1& {\mathrm{\ω}}_e{\mathrm{cos\θ}}_1& -{\mathrm{\ω}}_e{\mathrm{sin\θ}}_1\\ 1& {\mathrm{\ω}}_e{\mathrm{cos\θ}}_2& -{\mathrm{\ω}}_e{\mathrm{sin\θ}}_2\\ \·& \·& \·\\ \·& \·& \·\\ \·& \·& \·\\ 1& {\mathrm{\ω}}_e{\mathrm{cos\θ}}_{N-1}& -{\mathrm{\ω}}_e{\mathrm{sin\θ}}_{N-1}\\ 1& {\mathrm{\ω}}_e{\mathrm{cos\θ}}_N& -{\mathrm{\ω}}_e{\mathrm{sin\θ}}_N\end{array}\right]$

ω_e为北向地速，θ_k为台体在第k个位置处X轴陀螺仪的输入轴与第1个位置处X轴陀螺仪的输入轴之间的夹角，k＝1,…,N；

(1.3)根据步骤(2)，利用公式

计算ψ₀，从而实现惯性稳定平台固定基座的自瞄准。

所述台体在第k个位置处X轴陀螺仪的输入轴与第1个位置处X轴陀螺仪的输入轴之间的夹角θ_k按照如下方式获得：

(2.1)定义θ_x′、θ_y、θ_x、θ_z分别为惯性稳定平台的随动轴相对基座的转角、外环轴相对随动轴的转角、内环轴相对外环轴的转角以及台体轴相对内环轴的转角，则基座坐标系到台体坐标系的转换矩阵满足：

$R_b^p({\mathrm{\θ}}_{x^{\′}},{\mathrm{\θ}}_y,{\mathrm{\θ}}_x,{\mathrm{\θ}}_z)=\left[\begin{array}{ccc}{\mathrm{cos\θ}}_z& {\mathrm{sin\θ}}_z& 0\\ -{\mathrm{sin\θ}}_z& {\mathrm{cos\θ}}_z& 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{ccc}1& 0& 0\\ 0& {\mathrm{cos\θ}}_x& {\mathrm{sin\θ}}_x\\ 0& -{\mathrm{sin\θ}}_x& {\mathrm{cos\θ}}_x\end{array}\right]\left[\begin{array}{ccc}{\mathrm{cos\θ}}_y& 0& -{\mathrm{sin\θ}}_y\\ 0& 1& 0\\ {\mathrm{sin\θ}}_y& 0& {\mathrm{cos\θ}}_y\end{array}\right]\left[\begin{array}{ccc}1& 0& 0\\ 0& {\mathrm{cos\θ}}_{x^{\′}}& {\mathrm{sin\θ}}_{x^{\′}}\\ 0& -{\mathrm{sin\θ}}_{x^{\′}}& {\mathrm{cos\θ}}_{x^{\′}}\end{array}\right]$

台体坐标系到基座坐标系的转换矩阵满足：

$R_p^b({\mathrm{\θ}}_{x^{\′}},{\mathrm{\θ}}_y,{\mathrm{\θ}}_x,{\mathrm{\θ}}_z)=\left[\begin{array}{ccc}1& 0& 0\\ 0& {\mathrm{cos\θ}}_{x^{\′}}& -{\mathrm{sin\θ}}_{x^{\′}}\\ 0& {\mathrm{sin\θ}}_{x^{\′}}& {\mathrm{cos\θ}}_{x^{\′}}\end{array}\right]\left[\begin{array}{ccc}{\mathrm{cos\θ}}_y& 0& {\mathrm{sin\θ}}_y\\ 0& 1& 0\\ -{\mathrm{sin\θ}}_y& 0& {\mathrm{cos\θ}}_y\end{array}\right]\left[\begin{array}{ccc}1& 0& 0\\ 0& {\mathrm{cos\θ}}_x& -{\mathrm{sin\θ}}_x\\ 0& {\mathrm{sin\θ}}_x& {\mathrm{cos\θ}}_x\end{array}\right]\left[\begin{array}{ccc}{\mathrm{cos\θ}}_z& -{\mathrm{sin\θ}}_z& 0\\ {\mathrm{sin\θ}}_z& {\mathrm{cos\θ}}_z& 0\\ 0& 0& 1\end{array}\right]$

(2.2)根据基座坐标系到台体坐标系的转换矩阵以及台体坐标系到基座坐标系的转换矩阵，利用如下公式计算cosθ_k和sinθ_k：

$R_{p_0}^{p_1}=R_b^p({\mathrm{\θ}}_{x^{\′}k},{\mathrm{\θ}}_{yk},{\mathrm{\θ}}_{xk},{\mathrm{\θ}}_{zk})\×R_p^b({\mathrm{\θ}}_{x^{\′}1},{\mathrm{\θ}}_{y1},{\mathrm{\θ}}_{x1},{\mathrm{\θ}}_{z1})=\left[\begin{array}{ccc}{\mathrm{cos\θ}}_k& 0& -{\mathrm{sin\θ}}_k\\ 0& 1& 0\\ {\mathrm{sin\θ}}_k& 0& {\mathrm{cos\θ}}_k\end{array}\right];$

其中

$R_b^p({\mathrm{\θ}}_{x^{\′}k},{\mathrm{\θ}}_{yk},{\mathrm{\θ}}_{xk},{\mathrm{\θ}}_{zk})=\left[\begin{array}{ccc}{\mathrm{cos\θ}}_{zk}& {\mathrm{sin\θ}}_{zk}& 0\\ -{\mathrm{sin\θ}}_{zk}& {\mathrm{cos\θ}}_{zk}& 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{ccc}1& 0& 0\\ 0& {\mathrm{cos\θ}}_{xk}& {\mathrm{sin\θ}}_{xk}\\ 0& -{\mathrm{sin\θ}}_{xk}& {\mathrm{cos\θ}}_{xk}\end{array}\right]\left[\begin{array}{ccc}{\mathrm{cos\θ}}_{yk}& 0& -{\mathrm{sin\θ}}_{yk}\\ 0& 1& 0\\ {\mathrm{sin\θ}}_{yk}& 0& {\mathrm{cos\θ}}_{yk}\end{array}\right]\left[\begin{array}{ccc}1& 0& 0\\ 0& {\mathrm{cos\θ}}_{x^{\′}k}& {\mathrm{sin\θ}}_{x^{\′}k}\\ 0& -{\mathrm{sin\θ}}_{x^{\′}k}& {\mathrm{cos\θ}}_{x^{\′}k}\end{array}\right]$

$R_p^b({\mathrm{\θ}}_{x^{\′}1},{\mathrm{\θ}}_{y1},{\mathrm{\θ}}_{x1},{\mathrm{\θ}}_{z1})=\left[\begin{array}{ccc}1& 0& 0\\ 0& {\mathrm{cos\θ}}_{x^{\′}1}& -{\mathrm{sin\θ}}_{x^{\′}1}\\ 0& {\mathrm{sin\θ}}_{x^{\′}1}& {\mathrm{cos\θ}}_{x^{\′}1}\end{array}\right]\left[\begin{array}{ccc}{\mathrm{cos\θ}}_{y1}& 0& {\mathrm{sin\θ}}_{y1}\\ 0& 1& 0\\ -{\mathrm{sin\θ}}_{y1}& 0& {\mathrm{cos\θ}}_{y1}\end{array}\right]\left[\begin{array}{ccc}1& 0& 0\\ 0& {\mathrm{cos\θ}}_{x1}& -{\mathrm{sin\θ}}_{x1}\\ 0& {\mathrm{sin\θ}}_{x1}& {\mathrm{cos\θ}}_{x1}\end{array}\right]\left[\begin{array}{ccc}{\mathrm{cos\θ}}_{z1}& -{\mathrm{sin\θ}}_{z1}& 0\\ {\mathrm{sin\θ}}_{z1}& {\mathrm{cos\θ}}_{z1}& 0\\ 0& 0& 1\end{array}\right]$

θ_x′k、θ_yk、θ_xk、θ_zk分别为惯性稳定平台在第k个位置时随动轴相对基座的转角、外环轴相对随动轴的转角、内环轴相对外环轴的转角以及台体轴相对内环轴的转角，θ_x′1、θ_y1、θ_x1、θ_z1分别为惯性稳定平台在第1个位置时随动轴相对基座的转角、外环轴相对随动轴的转角、内环轴相对外环轴的转角以及台体轴相对内环轴的转角；

(2.3)根据步骤(2.2)得到的cosθ_k和sinθ_k，利用如下公式解算θ_k：

所述步骤(2.2)中θ_x′1、θ_y1、θ_x1、θ_z1、θ_x′k、θ_yk、θ_xk、θ_zk按照如下方式确定：

(3.1)惯性稳定平台在第1个位置时，平台工作处于锁零调平状态，此时有：

θ_y1＝θ_x1＝0°

台体调平后，通过轴端姿态角传感器采集θ_x′1、θ_z1；

(3.2)当惯性稳定平台在第k个位置时，外环轴相对随动轴的转角θ_yk由控制指令解算算法求得，并将θ_yk作为指令输入至平台控制系统，之后平台控制系统判断平台处于随锁还是随动状态，若为随锁状态，则θ_x′k＝θ_x′(k-1)，台体调平后，通过轴端姿态角传感器采集θ_xk和θ_zk；若为随动状态，则θ_xk＝θ_x(k-1)，台体调平后，通过轴端姿态角传感器采集θ_x′k和θ_zk。

所述步骤(3.2)中判断平台处于随锁还是随动状态的方法为：

判断θ_yk是否属于[-90°,-80°]U[80°,90°]，若属于，则平台处于随锁状态，否则，平台处于随动状态。

所述步骤(3.2)中θ_yk直接由控制指令任意给定。

所述步骤(3.2)中θ_yk按照如下方式给定：

(6.1)首先假设平台为随动状态，根据期望的θ_k，利用如下公式计算随动状态下的外环轴转角θ_yk：

${\mathrm{\θ}}_{yk}=-arcsin\frac{c_{31}}{{\mathrm{cos\θ}}_{x(k-1)}}$

其中

$\left[\begin{array}{ccc}{\mathrm{cos\θ}}_k& 0& -{\mathrm{sin\θ}}_k\\ 0& 1& 0\\ {\mathrm{sin\θ}}_k& 0& {\mathrm{cos\θ}}_k\end{array}\right]R_b^p({\mathrm{\θ}}_{x^{\′}1},{\mathrm{\θ}}_{y1},{\mathrm{\θ}}_{x1},{\mathrm{\θ}}_{z1})=\left[\begin{array}{ccc}{c}_{11}& c_{12}& c_{13}\\ c_{21}& c_{22}& c_{23}\\ c_{31}& c_{32}& c_{33}\end{array}\right]$

(6.2)判断平台处于随锁还是随动状态

1)若平台处于随锁状态，按照以下公式计算θ_yk：

$\left\{\begin{array}{c}{\mathrm{\&theta;}}_{x^{\&prime;}k}={\mathrm{\&theta;}}_{x^{\&prime;}(k-1)}\\ {\mathrm{\&theta;}}_{yk}=\left\{\begin{array}{cc}\arctan \frac{c_{31}}{{\mathrm{sin\&theta;}}_{x^{\&prime;}k}{c}_{32}+{\mathrm{cos\&theta;}}_{x^{\&prime;}k}{c}_{33}}& \left(\begin{array}{c}{c}_{31}\&GreaterEqual;0\\ {\mathrm{sin\&theta;}}_{x^{\&prime;}k}{c}_{32}+{\mathrm{cos\&theta;}}_{x^{\&prime;}k}{c}_{33}>0\end{array}\right)\\ \mathrm{\&pi;}+\arctan \frac{c_{31}}{{\mathrm{sin\&theta;}}_{x^{\&prime;}k}{c}_{32}+{\mathrm{cos\&theta;}}_{x^{\&prime;}k}{c}_{33}}& \left(\begin{array}{c}{c}_{31}>0\\ {\mathrm{sin\&theta;}}_{x^{\&prime;}k}{c}_{32}+{\mathrm{cos\&theta;}}_{x^{\&prime;}k}{c}_{33}\&le;0\end{array}\right)\\ \arctan \frac{c_{31}}{{\mathrm{sin\&theta;}}_{x^{\&prime;}k}{c}_{32}+{\mathrm{cos\&theta;}}_{x^{\&prime;}k}{c}_{33}}& \left(\begin{array}{c}{c}_{31}<0\\ {\mathrm{sin\&theta;}}_{x^{\&prime;}k}{c}_{32}+{\mathrm{cos\&theta;}}_{x^{\&prime;}k}{c}_{33}\&GreaterEqual;0\end{array}\right)\\ \arctan \frac{c_{31}}{{\mathrm{sin\&theta;}}_{x^{\&prime;}k}{c}_{32}+{\mathrm{cos\&theta;}}_{x^{\&prime;}k}{c}_{33}}-\mathrm{\&pi;}& \left(\begin{array}{c}{c}_{31}\&le;0\\ {\mathrm{sin\&theta;}}_{x^{\&prime;}k}{c}_{32}+{\mathrm{cos\&theta;}}_{x^{\&prime;}k}{c}_{33}<0\end{array}\right)\end{array}\right.\end{array}\right.$

2)若平台为随动状态，按照以下方法计算θ_yk：

a.

$\left\{\begin{array}{c}{\mathrm{\&theta;}}_{x^{\&prime;}k1}=\mathrm{sgn}\left({\mathrm{sin\&theta;}}_{x^{\&prime;}k1}\right)\arccos \left({\mathrm{cos\&theta;}}_{x^{\&prime;}k1}\right)\\ {\mathrm{\&theta;}}_{yk1}=-\arcsin \frac{c_{31}}{{\mathrm{cos\&theta;}}_{x(k-1)}}\\ {\mathrm{\&theta;}}_{xk}={\mathrm{\&theta;}}_{x(k-1)}\\ {\mathrm{\&theta;}}_{zk1}=\mathrm{sgn}\left({\mathrm{sin\&theta;}}_{zk1}\right)\arccos \left({\mathrm{cos\&theta;}}_{zk1}\right)\end{array}\right.,\left\{\begin{array}{c}{\mathrm{\&theta;}}_{x^{\&prime;}k2}=\mathrm{sgn}\left({\mathrm{sin\&theta;}}_{x^{\&prime;}k2}\right)\arccos \left({\mathrm{cos\&theta;}}_{x^{\&prime;}k2}\right)\\ {\mathrm{\&theta;}}_{yk2}=\left\{\begin{array}{cc}-\arcsin \frac{c_{31}}{{\mathrm{cos\&theta;}}_{x(k-1)}}-\mathrm{\&pi;}& \frac{c_{31}}{{\mathrm{cos\&theta;}}_{x(k-1)}}\&GreaterEqual;0\\ \mathrm{\&pi;}-\arcsin \frac{c_{31}}{{\mathrm{cos\&theta;}}_{x(k-1)}}& \frac{c_{31}}{{\mathrm{cos\&theta;}}_{x(k-1)}}<0\end{array}\right.\\ {\mathrm{\&theta;}}_{xk}={\mathrm{\&theta;}}_{x(k-1)}\\ {\mathrm{\&theta;}}_{zk2}=\mathrm{sgn}\left({\mathrm{sin\&theta;}}_{zk2}\right)\arccos \left({\mathrm{cos\&theta;}}_{zk2}\right)\end{array}\right.$

其中：

$\left\{\begin{array}{c}{\mathrm{sin\&theta;}}_{x^{\&prime;}k1}=\frac{c33{\mathrm{sin\&theta;}}_{xk}-c32{\mathrm{cos\&theta;}}_{xk}{\mathrm{cos\&theta;}}_{yk1}}{{\cos }^2{\mathrm{\&theta;}}_{xk}{\mathrm{cos\&theta;}}_{yk1}+{\sin }^2{\mathrm{\&theta;}}_{xk}}\\ {\mathrm{cos\&theta;}}_{x^{\&prime;}k1}=\frac{c32{\mathrm{sin\&theta;}}_{xk}+c32{\mathrm{cos\&theta;}}_{xk}{\mathrm{cos\&theta;}}_{yk1}}{{\cos }^2{\mathrm{\&theta;}}_{xk}{\mathrm{cos\&theta;}}_{yk2}+{\sin }^2{\mathrm{\&theta;}}_{xk}}\\ {\mathrm{sin\&theta;}}_{zk1}=\frac{c\mathrm{11}{\mathrm{sin\&theta;}}_{xk}{\mathrm{sin\&theta;}}_{yk1}-c21{\mathrm{cos\&theta;}}_{yk1}}{{\cos }^2{\mathrm{\&theta;}}_{yk1}+{\sin }^2{\mathrm{\&theta;}}_{xk}{\sin }^2{\mathrm{\&theta;}}_{yk1}}\\ {\mathrm{cos\&theta;}}_{zk1}=\frac{c\mathrm{11}{\mathrm{cos\&theta;}}_{yk}+c21{\mathrm{sin\&theta;}}_{xk}{\mathrm{sin\&theta;}}_{yk}}{{\cos }^2{\mathrm{\&theta;}}_{yk1}+{\sin }^2{\mathrm{\&theta;}}_{xk}{\sin }^2{\mathrm{\&theta;}}_{yk1}}\end{array}\right.,\left\{\begin{array}{c}{\mathrm{sin\&theta;}}_{x^{\&prime;}k2}=\frac{c33{\mathrm{sin\&theta;}}_{xk}-c32{\mathrm{cos\&theta;}}_{xk}{\mathrm{cos\&theta;}}_{yk2}}{{\cos }^2{\mathrm{\&theta;}}_{xk}{\mathrm{cos\&theta;}}_{yk2}+{\sin }^2{\mathrm{\&theta;}}_{xk}}\\ {\mathrm{cos\&theta;}}_{x^{\&prime;}k2}=\frac{c32{\mathrm{sin\&theta;}}_{xk}+c33{\mathrm{cos\&theta;}}_{xk}{\mathrm{cos\&theta;}}_{yk2}}{{\cos }^2{\mathrm{\&theta;}}_{xk}{\mathrm{cos\&theta;}}_{yk2}+{\sin }^2{\mathrm{\&theta;}}_{xk}}\\ {\mathrm{sin\&theta;}}_{zk2}=\frac{c\mathrm{11}{\mathrm{sin\&theta;}}_{xk}{\mathrm{sin\&theta;}}_{yk2}-c21{\mathrm{cos\&theta;}}_{yk2}}{{\cos }^2{\mathrm{\&theta;}}_{yk2}+{\sin }^2{\mathrm{\&theta;}}_{xk}{\sin }^2{\mathrm{\&theta;}}_{yk2}}\\ {\mathrm{cos\&theta;}}_{zk2}=\frac{c\mathrm{11}{\mathrm{cos\&theta;}}_{yk}+c21{\mathrm{sin\&theta;}}_{xk}{\mathrm{sin\&theta;}}_{yk}}{{\cos }^2{\mathrm{\&theta;}}_{yk2}+{\sin }^2{\mathrm{\&theta;}}_{xk}{\sin }^2{\mathrm{\&theta;}}_{yk2}}\end{array}\right.$

b.

本发明与现有技术相比的优点如下：

(1)现有的三/四位置自瞄采用角度均分的方法，要求θ_k分别以120°和90°为增量进行转动，降低了测量的灵活性。本发明基于最小二乘法，不再依赖角度均分，只要陀螺仪测量位置数足够多(不小于3)，便可求解ψ₀，具有很高的通用性，同时大大增加了测量的灵活性，对准精度高。

(2)利用基座坐标系与台体坐标系的转换矩阵，给出了固定基座下实现转角θ_k的两种方法，即直接法与间接法。前者可任意给定外环轴转角θ_y，进而根据各轴转角求得θ_k；后者则根据期望的θ_k求解得到外环轴转角θ_y，从而实现转角θ_k的转动。由于各轴转角中包含了基座不水平信息，因此两种方法均弥补了现有自瞄方法中由于基座倾斜造成的θ_k转角偏差问题。

附图说明

图1为三位置自瞄准方案示意图；

图2为四位置自瞄准方案示意图；

图3为本发明方法流程图；

图4为台体坐标系与基座坐标系的坐标变换示意图。

具体实施方式

本发明的实现原理是：建立基座坐标系与台体坐标系的转换矩阵，该矩阵为框架轴各转角的三角函数表达式，利用该矩阵可以求出任意框架轴转角下台体实际转角以及期望台体转角下框架轴转角，之后通过多位置测量，基于最小二乘法求解得到ψ₀。相比现有的三位置、四位置自瞄算法，本发明的自瞄方法具有转角准确、实施灵活的优点。

如图3所示，本发明的自瞄准方法步骤如下：

(1)惯性稳定平台的台体在水平面内采用N个位置进行自瞄准，定义X轴陀螺仪敏感方位信息，其常值漂移项为ω_gx，在第1个位置时X轴陀螺仪的输入轴与当地水平面内的地球北向方位轴的夹角为ψ₀，方向与右手坐标系定义一致，其中N≥3；θ_k为台体在第k个位置处X轴陀螺仪的输入轴与第1个位置处X轴陀螺仪的输入轴之间的夹角。

(2)X轴陀螺仪在1,2,…,N个位置的测量值分别为：

ω_xk＝ω_gx+ω_e cos(ψ₀+θ_k)k＝1,…,N

矩阵表达式为

$\left[\begin{array}{c}{\mathrm{\&omega;}}_{x_1}\\ {\mathrm{\&omega;}}_{x_2}\\ \&CenterDot;\\ \&CenterDot;\\ \&CenterDot;\\ {\mathrm{\&omega;}}_{x_{N-1}}\\ {\mathrm{\&omega;}}_{x_N}\end{array}\right]=\left[\begin{array}{ccc}1& {\mathrm{\&omega;}}_e{\mathrm{cos\&theta;}}_1& -{\mathrm{\&omega;}}_e{\mathrm{sin\&theta;}}_1\\ 1& {\mathrm{\&omega;}}_e{\mathrm{cos\&theta;}}_2& -{\mathrm{\&omega;}}_e{\mathrm{sin\&theta;}}_2\\ \&CenterDot;& \&CenterDot;& \&CenterDot;\\ \&CenterDot;& \&CenterDot;& \&CenterDot;\\ \&CenterDot;& \&CenterDot;& \&CenterDot;\\ 1& {\mathrm{\&omega;}}_e{\mathrm{cos\&theta;}}_{N-1}& -{\mathrm{\&omega;}}_e{\mathrm{sin\&theta;}}_{N-1}\\ 0& {\mathrm{\&omega;}}_e{\mathrm{cos\&theta;}}_N& -{\mathrm{\&omega;}}_e{\mathrm{sin\&theta;}}_N\end{array}\right]\left[\begin{array}{c}{\mathrm{\&omega;}}_{gx}\\ {\mathrm{cos\&psi;}}_0\\ {\mathrm{sin\&psi;}}_0\end{array}\right]$

采用最小二乘法求解ω_gx、cosψ₀、sinψ₀，见下式：

$\left[\begin{array}{c}{\mathrm{\&omega;}}_{gx}\\ {\mathrm{cos\&psi;}}_0\\ {\mathrm{sin\&psi;}}_0\end{array}\right]={\left(A^{T}A\right)}^{-1}{A}^T\left[\begin{array}{c}{\mathrm{\&omega;}}_{x_1}\\ {\mathrm{\&omega;}}_{x_2}\\ \&CenterDot;\\ \&CenterDot;\\ \&CenterDot;\\ {\mathrm{\&omega;}}_{x_N}\end{array}\right]$

式中：

$A=\left[\begin{array}{ccc}1& {\mathrm{\&omega;}}_e{\mathrm{cos\&theta;}}_1& -{\mathrm{\&omega;}}_e{\mathrm{sin\&theta;}}_1\\ 1& {\mathrm{\&omega;}}_e{\mathrm{cos\&theta;}}_2& -{\mathrm{\&omega;}}_e{\mathrm{sin\&theta;}}_2\\ \&CenterDot;& \&CenterDot;& \&CenterDot;\\ \&CenterDot;& \&CenterDot;& \&CenterDot;\\ \&CenterDot;& \&CenterDot;& \&CenterDot;\\ 1& {\mathrm{\&omega;}}_e{\mathrm{cos\&theta;}}_{N-1}& -{\mathrm{\&omega;}}_e{\mathrm{sin\&theta;}}_{N-1}\\ 1& {\mathrm{\&omega;}}_e{\mathrm{cos\&theta;}}_N& -{\mathrm{\&omega;}}_e{\mathrm{sin\&theta;}}_N\end{array}\right]$

(3)根据步骤(2)，利用公式

解算ψ₀，从而实现惯性稳定平台固定基座的自瞄准。

台体在第k个位置处X轴陀螺仪的输入轴与第1个位置处X轴陀螺仪的输入轴之间的夹角θ_k按照如下方式获得：

(1)不失一般性，假设惯性稳定平台系统为三框架四轴结构，定义θ_x′、θ_y、θ_x、θ_z分别为惯性稳定平台的随动轴相对基座、外环轴相对随动轴、内环轴相对外环轴以及台体轴相对内环轴的转角，为叙述方便，分别简称为随动轴、外环轴、内环轴以及台体轴转角。为台体坐标系到基座坐标系的转换矩阵，为基座坐标系到台体坐标系的转换矩阵。

基座坐标系到台体坐标系的转换矩阵为：

$R_b^p({\mathrm{\&theta;}}_{x^{\&prime;}},{\mathrm{\&theta;}}_y,{\mathrm{\&theta;}}_x,{\mathrm{\&theta;}}_z)=\left[\begin{array}{ccc}{\mathrm{cos\&theta;}}_z& {\mathrm{sin\&theta;}}_z& 0\\ -{\mathrm{sin\&theta;}}_z& {\mathrm{cos\&theta;}}_z& 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{ccc}1& 0& 0\\ 0& {\mathrm{cos\&theta;}}_x& {\mathrm{sin\&theta;}}_x\\ 0& -{\mathrm{sin\&theta;}}_x& {\mathrm{cos\&theta;}}_x\end{array}\right]\left[\begin{array}{ccc}{\mathrm{cos\&theta;}}_y& 0& -{\mathrm{sin\&theta;}}_y\\ 0& 1& 0\\ {\mathrm{sin\&theta;}}_y& 0& {\mathrm{cos\&theta;}}_y\end{array}\right]\left[\begin{array}{ccc}1& 0& 0\\ 0& {\mathrm{cos\&theta;}}_{x^{\&prime;}}& {\mathrm{sin\&theta;}}_{x^{\&prime;}}\\ 0& -{\mathrm{sin\&theta;}}_{x^{\&prime;}}& {\mathrm{cos\&theta;}}_{x^{\&prime;}}\end{array}\right]$

台体坐标系到基座坐标系的坐标转换矩阵为：

$R_p^b({\mathrm{\&theta;}}_{x^{\&prime;}},{\mathrm{\&theta;}}_y,{\mathrm{\&theta;}}_x,{\mathrm{\&theta;}}_z)=\left[\begin{array}{ccc}1& 0& 0\\ 0& {\mathrm{cos\&theta;}}_{x^{\&prime;}}& -{\mathrm{sin\&theta;}}_{x^{\&prime;}}\\ 0& {\mathrm{sin\&theta;}}_{x^{\&prime;}}& {\mathrm{cos\&theta;}}_{x^{\&prime;}}\end{array}\right]\left[\begin{array}{ccc}{\mathrm{cos\&theta;}}_y& 0& {\mathrm{sin\&theta;}}_y\\ 0& 1& 0\\ -{\mathrm{sin\&theta;}}_y& 0& {\mathrm{cos\&theta;}}_y\end{array}\right]\left[\begin{array}{ccc}1& 0& 0\\ 0& {\mathrm{cos\&theta;}}_x& -{\mathrm{sin\&theta;}}_x\\ 0& {\mathrm{sin\&theta;}}_x& {\mathrm{cos\&theta;}}_x\end{array}\right]\left[\begin{array}{ccc}{\mathrm{cos\&theta;}}_z& -{\mathrm{sin\&theta;}}_z& 0\\ {\mathrm{sin\&theta;}}_z& {\mathrm{cos\&theta;}}_z& 0\\ 0& 0& 1\end{array}\right]$

如图4所示为坐标系转换示意图，其中Ox_py_pz_p表示台体坐标系,Ox_by_bz_b表示基座坐标系。图4中第一步由Ox_by_bz_b绕x_b转动θ_x′角到Ox₁y₁z₁，第二步由Ox₁y₁z₁绕y₁转动θ_y角到Ox₂y₂z₂，第三步由Ox₂y₂z₂绕x₂转动θ_x角到Ox₃y₃z₃，第四步由Ox₃y₃z₃绕z₃转动θ_z角到Ox_py_pz_p。

(2)根据基座坐标系到台体坐标系的转换矩阵以及台体坐标系到基座坐标系的转换矩阵，利用如下公式计算cosθ_k和sinθ_k：

$R_{p_0}^{p_1}=R_b^p({\mathrm{\&theta;}}_{x^{\&prime;}k},{\mathrm{\&theta;}}_{yk},{\mathrm{\&theta;}}_{xk},{\mathrm{\&theta;}}_{zk})\&times;R_p^b({\mathrm{\&theta;}}_{x^{\&prime;}1},{\mathrm{\&theta;}}_{y1},{\mathrm{\&theta;}}_{x1},{\mathrm{\&theta;}}_{z1})=\left[\begin{array}{ccc}{\mathrm{cos\&theta;}}_k& 0& -{\mathrm{sin\&theta;}}_k\\ 0& 1& 0\\ {\mathrm{sin\&theta;}}_k& 0& {\mathrm{cos\&theta;}}_k\end{array}\right];$

其中，θ_x′k、θ_yk、θ_xk、θ_zk分别为惯性稳定平台在第k个位置时随动轴、外环轴、内环轴和台体轴的转角，θ_x′1、θ_y1、θ_x1、θ_z1分别为惯性稳定平台在第1个位置时随动轴、外环轴、内环轴和台体轴的转角。

(3)根据步骤(2)得到的cosθ_k和sinθ_k，利用如下公式解算θ_k：

上述过程中，θ_x′1、θ_y1、θ_x1、θ_z1、θ_x′k、θ_yk、θ_xk、θ_zk按照如下方式确定：

(1)惯性稳定平台在第1个位置时，平台工作处于锁零调平状态，此时有：

θ_y1＝θ_x1＝0°

平台调平回路将台体调平，由轴端姿态角传感器输出相应θ_x′1、θ_z1。

(2)当惯性稳定平台在第k个位置时，外环轴转角θ_yk由控制指令解算算法求得，并作为指令输入至平台控制系统。之后平台控制系统判断平台处于随锁还是随动状态。若为随锁状态，则θ_x′k＝θ_x′(k-1)，平台调平回路将台体调平，由轴端姿态角传感器输出相应θ_xk和θ_zk；若为随动状态，则θ_xk＝θ_x(k-1)，平台调平回路将台体调平，由轴端姿态角传感器输出相应θ_x′k和θ_zk。

对于外环轴转角θ_yk，按照如下方式确定：

(1)直接法：当惯性稳定平台在第k位置时，外环轴转角θ_yk由控制指令任意给定。

(2)间接法：

间接法的主要特点是：惯性稳定平台在第k位置的X轴陀螺仪输入轴与第1个位置X轴陀螺仪输入轴的夹角θ_k根据期望给定，而外框架轴转角θ_yk由θ_k反解得到，从而实现转角θ_k的转动。其具体方法与步骤如下：

1)首先假设平台为随锁状态，利用如下公式计算随动状态下的外环轴转角θ_yk：

${\mathrm{\&theta;}}_{yk}=-\arcsin \frac{c_{31}}{{\mathrm{cos\&theta;}}_{x(k-1)}}$

其中

$\left[\begin{array}{ccc}{\mathrm{cos\&theta;}}_k& 0& -{\mathrm{sin\&theta;}}_k\\ 0& 1& 0\\ {\mathrm{sin\&theta;}}_k& 0& {\mathrm{cos\&theta;}}_k\end{array}\right]R_b^p({\mathrm{\&theta;}}_{x^{\&prime;}1},{\mathrm{\&theta;}}_{y1},{\mathrm{\&theta;}}_{x1},{\mathrm{\&theta;}}_{z1})=\left[\begin{array}{ccc}{c}_{11}& c_{12}& c_{13}\\ c_{21}& c_{22}& c_{23}\\ c_{31}& c_{32}& c_{33}\end{array}\right]$

2)判断θ_yk是否属于[-90°,-80°]U[80°,90°]

①若属于，则平台处于随锁状态，按照以下公式计算θ_yk：

$\left\{\begin{array}{c}{\mathrm{\&theta;}}_{x^{\&prime;}k}={\mathrm{\&theta;}}_{x^{\&prime;}(k-1)}\\ {\mathrm{\&theta;}}_{yk}=\left\{\begin{array}{cc}\arctan \frac{c_{31}}{{\mathrm{sin\&theta;}}_{x^{\&prime;}k}{c}_{32}+{\mathrm{cos\&theta;}}_{x^{\&prime;}k}{c}_{33}}& \left(\begin{array}{c}{c}_{31}\&GreaterEqual;0\\ {\mathrm{sin\&theta;}}_{x^{\&prime;}k}{c}_{32}+{\mathrm{cos\&theta;}}_{x^{\&prime;}k}{c}_{33}>0\end{array}\right)\\ \mathrm{\&pi;}+\arctan \frac{c_{31}}{{\mathrm{sin\&theta;}}_{x^{\&prime;}k}{c}_{32}+{\mathrm{cos\&theta;}}_{x^{\&prime;}k}{c}_{33}}& \left(\begin{array}{c}{c}_{31}>0\\ {\mathrm{sin\&theta;}}_{x^{\&prime;}k}{c}_{32}+{\mathrm{cos\&theta;}}_{x^{\&prime;}k}{c}_{33}\&le;0\end{array}\right)\\ \arctan \frac{c_{31}}{{\mathrm{sin\&theta;}}_{x^{\&prime;}k}{c}_{32}+{\mathrm{cos\&theta;}}_{x^{\&prime;}k}{c}_{33}}& \left(\begin{array}{c}{c}_{31}<0\\ {\mathrm{sin\&theta;}}_{x^{\&prime;}k}{c}_{32}+{\mathrm{cos\&theta;}}_{x^{\&prime;}k}{c}_{33}\&GreaterEqual;0\end{array}\right)\\ \arctan \frac{c_{31}}{{\mathrm{sin\&theta;}}_{x^{\&prime;}k}{c}_{32}+{\mathrm{cos\&theta;}}_{x^{\&prime;}k}{c}_{33}}-\mathrm{\&pi;}& \left(\begin{array}{c}{c}_{31}\&le;0\\ {\mathrm{sin\&theta;}}_{x^{\&prime;}k}{c}_{32}+{\mathrm{cos\&theta;}}_{x^{\&prime;}k}{c}_{33}<0\end{array}\right)\end{array}\right.\end{array}\right.$

②若不属于，则平台为随动状态，按照以下方法计算θ_yk：

a.

$\left\{\begin{array}{c}{\mathrm{\&theta;}}_{x^{\&prime;}k1}=\mathrm{sgn}\left({\mathrm{sin\&theta;}}_{x^{\&prime;}k1}\right)\arccos \left({\mathrm{cos\&theta;}}_{x^{\&prime;}k1}\right)\\ {\mathrm{\&theta;}}_{yk1}=-\arcsin \frac{c_{31}}{{\mathrm{cos\&theta;}}_{x(k-1)}}\\ {\mathrm{\&theta;}}_{xk}={\mathrm{\&theta;}}_{x(k-1)}\\ {\mathrm{\&theta;}}_{zk1}=\mathrm{sgn}\left({\mathrm{sin\&theta;}}_{zk1}\right)\arccos \left({\mathrm{cos\&theta;}}_{zk1}\right)\end{array}\right.,\left\{\begin{array}{c}{\mathrm{\&theta;}}_{x^{\&prime;}k2}=\mathrm{sgn}\left({\mathrm{sin\&theta;}}_{x^{\&prime;}k2}\right)\arccos \left({\mathrm{cos\&theta;}}_{x^{\&prime;}k2}\right)\\ {\mathrm{\&theta;}}_{yk2}=\left\{\begin{array}{cc}-\arcsin \frac{c_{31}}{{\mathrm{cos\&theta;}}_{x(k-1)}}-\mathrm{\&pi;}& \frac{c_{31}}{{\mathrm{cos\&theta;}}_{x(k-1)}}\&GreaterEqual;0\\ \mathrm{\&pi;}-\arcsin \frac{c_{31}}{{\mathrm{cos\&theta;}}_{x(k-1)}}& \frac{c_{31}}{{\mathrm{cos\&theta;}}_{x(k-1)}}<0\end{array}\right.\\ {\mathrm{\&theta;}}_{xk}={\mathrm{\&theta;}}_{x(k-1)}\\ {\mathrm{\&theta;}}_{zk2}=\mathrm{sgn}\left({\mathrm{sin\&theta;}}_{zk2}\right)\arccos \left({\mathrm{cos\&theta;}}_{zk2}\right)\end{array}\right.$

其中：

$\left\{\begin{array}{c}{\mathrm{sin\&theta;}}_{x^{\&prime;}k1}=\frac{c33{\mathrm{sin\&theta;}}_{xk}-c32{\mathrm{cos\&theta;}}_{xk}{\mathrm{cos\&theta;}}_{yk1}}{{\cos }^2{\mathrm{\&theta;}}_{xk}{\mathrm{cos\&theta;}}_{yk1}+{\sin }^2{\mathrm{\&theta;}}_{xk}}\\ {\mathrm{cos\&theta;}}_{x^{\&prime;}k1}=\frac{c32{\mathrm{sin\&theta;}}_{xk}+c32{\mathrm{cos\&theta;}}_{xk}{\mathrm{cos\&theta;}}_{yk1}}{{\cos }^2{\mathrm{\&theta;}}_{xk}{\mathrm{cos\&theta;}}_{yk2}+{\sin }^2{\mathrm{\&theta;}}_{xk}}\\ {\mathrm{sin\&theta;}}_{zk1}=\frac{c\mathrm{11}{\mathrm{sin\&theta;}}_{xk}{\mathrm{sin\&theta;}}_{yk1}-c21{\mathrm{cos\&theta;}}_{yk1}}{{\cos }^2{\mathrm{\&theta;}}_{yk1}+{\sin }^2{\mathrm{\&theta;}}_{xk}{\sin }^2{\mathrm{\&theta;}}_{yk1}}\\ {\mathrm{cos\&theta;}}_{zk1}=\frac{c\mathrm{11}{\mathrm{cos\&theta;}}_{yk}+c21{\mathrm{sin\&theta;}}_{xk}{\mathrm{sin\&theta;}}_{yk}}{{\cos }^2{\mathrm{\&theta;}}_{yk1}+{\sin }^2{\mathrm{\&theta;}}_{xk}{\sin }^2{\mathrm{\&theta;}}_{yk1}}\end{array}\right.,\left\{\begin{array}{c}{\mathrm{sin\&theta;}}_{x^{\&prime;}k2}=\frac{c33{\mathrm{sin\&theta;}}_{xk}-c32{\mathrm{cos\&theta;}}_{xk}{\mathrm{cos\&theta;}}_{yk2}}{{\cos }^2{\mathrm{\&theta;}}_{xk}{\mathrm{cos\&theta;}}_{yk2}+{\sin }^2{\mathrm{\&theta;}}_{xk}}\\ {\mathrm{cos\&theta;}}_{x^{\&prime;}k2}=\frac{c32{\mathrm{sin\&theta;}}_{xk}+c33{\mathrm{cos\&theta;}}_{xk}{\mathrm{cos\&theta;}}_{yk2}}{{\cos }^2{\mathrm{\&theta;}}_{xk}{\mathrm{cos\&theta;}}_{yk2}+{\sin }^2{\mathrm{\&theta;}}_{xk}}\\ {\mathrm{sin\&theta;}}_{zk2}=\frac{c\mathrm{11}{\mathrm{sin\&theta;}}_{xk}{\mathrm{sin\&theta;}}_{yk2}-c21{\mathrm{cos\&theta;}}_{yk2}}{{\cos }^2{\mathrm{\&theta;}}_{yk2}+{\sin }^2{\mathrm{\&theta;}}_{xk}{\sin }^2{\mathrm{\&theta;}}_{yk2}}\\ {\mathrm{cos\&theta;}}_{zk2}=\frac{c\mathrm{11}{\mathrm{cos\&theta;}}_{yk}+c21{\mathrm{sin\&theta;}}_{xk}{\mathrm{sin\&theta;}}_{yk}}{{\cos }^2{\mathrm{\&theta;}}_{yk2}+{\sin }^2{\mathrm{\&theta;}}_{xk}{\sin }^2{\mathrm{\&theta;}}_{yk2}}\end{array}\right.$

b.

由上面的步骤可以看到，采用基于最小二乘法的多位置测量不再依赖于θ_k的转角三/四等分，只要测量位置数不小于3，就可以利用最小二乘法对ψ₀进行解算；同时在调平回路作用下，各轴转角包含了基座不水平信息，因此可对基座倾斜进行有效补偿。

需要说明的是：测量位置选取的越多，测量点数越多，相应的自瞄时间也将增加，因此实际应用中应根据具体情况合理选择测量位置数。

本发明自瞄准方法使用更为灵活，同时在基座倾斜时也能准确进行ψ₀的解算。

实施例：

为验证本发明方法的正确性和有效性，对本发明中的直接法、间接法与现有三位置自瞄方法进行对比测试试验。试验中，基座相对水平面存在15°倾角，直接法选取了外环轴转角θ_yk＝0°,120°,240°三个测量位置，然后根据本发明给出的方法计算ψ₀。间接法确定期望θ_k＝0°,120°,240°，然后利用本发明间接法反解出θ_yk，实现θ_k的转动得到上述期望值的三个测量位置，然后利用本发明方法计算ψ₀。表1给出了三种方法的计算结果。

表1某转台基座倾斜下的方位角计算结果

从表1中的数据可以看出，当基座存在倾斜时，现有三位置自瞄算法在0°、±45°、±135°、±180°方位角处存在较大偏差，最大偏差可达约7°；而本发明设计的多位置自瞄准方法，仍然可以准确得到平台方位角，证明了本发明方法的正确性和有效性。

本发明说明书中未作详细描述的内容属于本领域专业技术人员的公知技术。

Claims (6)

1.一种惯性稳定平台固定基座多位置自瞄准方法，所述惯性稳定平台为三框架四轴平台，其特征在于步骤如下：

(1.1)惯性稳定平台的台体在水平面内采用N个位置进行自瞄准，定义X轴陀螺仪敏感方位信息，其常值漂移项为ω_gx，在第一个位置时X轴陀螺仪的输入轴与当地水平面内的地球北向方位轴的夹角为ψ₀，方向与右手坐标系定义一致，其中N≥3；

(1.2)根据X轴陀螺仪在1,2,…,N个位置的测量值ω_xk，采用最小二乘法计算ω_gx、cosψ₀、sinψ₀，计算公式为：

$\left[\begin{array}{c}{\mathrm{\&omega;}}_{gx}\\ {\mathrm{cos\&psi;}}_0\\ {\mathrm{sin\&psi;}}_0\end{array}\right]={\left(A^{T}A\right)}^{-1}{A}^T\left[\begin{array}{c}{\mathrm{\&omega;}}_{x_1}\\ {\mathrm{\&omega;}}_{x_2}\\ \begin{array}{c}.\\ .\\ .\end{array}\\ {\mathrm{\&omega;}}_{x_N}\end{array}\right]$

其中：

$A=\left[\begin{array}{ccc}1& {\mathrm{\&omega;}}_e{\mathrm{cos\&theta;}}_1& -{\mathrm{\&omega;}}_e{\mathrm{sin\&theta;}}_1\\ 1& {\mathrm{\&omega;}}_e{\mathrm{cos\&theta;}}_2& -{\mathrm{\&omega;}}_e{\mathrm{sin\&theta;}}_2\\ \begin{array}{c}.\\ .\\ .\end{array}& \begin{array}{c}.\\ .\\ .\end{array}& \begin{array}{c}.\\ .\\ .\end{array}\\ 1& {\mathrm{\&omega;}}_e{\mathrm{cos\&theta;}}_{N-1}& -{\mathrm{\&omega;}}_e{\mathrm{sin\&theta;}}_{N-1}\\ 1& {\mathrm{\&omega;}}_e{\mathrm{cos\&theta;}}_N& -{\mathrm{\&omega;}}_e{\mathrm{sin\&theta;}}_N\end{array}\right]$

ω_e为北向地速，θ_k为台体在第k个位置处X轴陀螺仪的输入轴与第1个位置处X轴陀螺仪的输入轴之间的夹角，k＝1,…,N；

(1.3)根据步骤(1.2)，利用公式

计算ψ₀，从而实现惯性稳定平台固定基座的自瞄准。

2.根据权利要求1所述的一种惯性稳定平台固定基座多位置自瞄准方法，其特征在于：所述台体在第k个位置处X轴陀螺仪的输入轴与第1个位置处X轴陀螺仪的输入轴之间的夹角θ_k按照如下方式获得：

(2.1)定义θ_x′、θ_y、θ_x、θ_z分别为惯性稳定平台的随动轴相对基座的转角、外环轴相对随动轴的转角、内环轴相对外环轴的转角以及台体轴相对内环轴的转角，则基座坐标系到台体坐标系的转换矩阵满足：

$R_b^p\left({\mathrm{\&theta;}}_{x^{\&prime;}},{\mathrm{\&theta;}}_y,{\mathrm{\&theta;}}_x,{\mathrm{\&theta;}}_z\right)=\left[\begin{array}{ccc}{\mathrm{cos\&theta;}}_z& {\mathrm{sin\&theta;}}_z& 0\\ -{\mathrm{sin\&theta;}}_z& {\mathrm{cos\&theta;}}_z& 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{ccc}1& 0& 0\\ 0& {\mathrm{cos\&theta;}}_x& {\mathrm{sin\&theta;}}_x\\ 0& -{\mathrm{sin\&theta;}}_x& {\mathrm{cos\&theta;}}_x\end{array}\right]\left[\begin{array}{ccc}{\mathrm{cos\&theta;}}_y& 0& -{\mathrm{sin\&theta;}}_y\\ 0& 1& 0\\ {\mathrm{sin\&theta;}}_y& 0& {\mathrm{cos\&theta;}}_y\end{array}\right]\left[\begin{array}{ccc}1& 0& 0\\ 0& {\mathrm{cos\&theta;}}_{x^{\&prime;}}& {\mathrm{sin\&theta;}}_{x^{\&prime;}}\\ 0& -{\mathrm{sin\&theta;}}_{x^{\&prime;}}& {\mathrm{cos\&theta;}}_{x^{\&prime;}}\end{array}\right]$

台体坐标系到基座坐标系的转换矩阵满足：

$R_p^b\left({\mathrm{\&theta;}}_{x^{\&prime;}},{\mathrm{\&theta;}}_y,{\mathrm{\&theta;}}_x,{\mathrm{\&theta;}}_z\right)=\left[\begin{array}{ccc}1& 0& 0\\ 0& {\mathrm{cos\&theta;}}_{x^{\&prime;}}& -{\mathrm{sin\&theta;}}_{x^{\&prime;}}\\ 0& {\mathrm{sin\&theta;}}_{x^{\&prime;}}& {\mathrm{cos\&theta;}}_{x^{\&prime;}}\end{array}\right]\left[\begin{array}{ccc}{\mathrm{cos\&theta;}}_y& 0& {\mathrm{sin\&theta;}}_y\\ 0& 1& 0\\ -{\mathrm{sin\&theta;}}_y& 0& {\mathrm{cos\&theta;}}_y\end{array}\right]\left[\begin{array}{ccc}1& 0& 0\\ 0& {\mathrm{cos\&theta;}}_x& -{\mathrm{sin\&theta;}}_x\\ 0& {\mathrm{sin\&theta;}}_x& {\mathrm{cos\&theta;}}_x\end{array}\right]\left[\begin{array}{ccc}{\mathrm{cos\&theta;}}_z& -{\mathrm{sin\&theta;}}_z& 0\\ {\mathrm{sin\&theta;}}_z& {\mathrm{cos\&theta;}}_z& 0\\ 0& 0& 1\end{array}\right]$

(2.2)根据基座坐标系到台体坐标系的转换矩阵以及台体坐标系到基座坐标系的转换矩阵，利用如下公式计算cosθ_k和sinθ_k：

$R_{p_0}^{p_1}=R_b^p({\mathrm{\&theta;}}_{x^{\&prime;}k},{\mathrm{\&theta;}}_{yk},{\mathrm{\&theta;}}_{xk},{\mathrm{\&theta;}}_{zk})\&times;R_p^b({\mathrm{\&theta;}}_{x^{\&prime;}1},{\mathrm{\&theta;}}_{y1},{\mathrm{\&theta;}}_{x1},{\mathrm{\&theta;}}_{z1})=\left[\begin{array}{ccc}{\mathrm{cos\&theta;}}_k& 0& -{\mathrm{sin\&theta;}}_k\\ 0& 1& 0\\ {\mathrm{sin\&theta;}}_k& 0& {\mathrm{cos\&theta;}}_k\end{array}\right];$

其中

$R_b^p\left({\mathrm{\&theta;}}_{x^{\&prime;}k},{\mathrm{\&theta;}}_{yk},{\mathrm{\&theta;}}_{xk},{\mathrm{\&theta;}}_{zk}\right)=\left[\begin{array}{ccc}{\mathrm{cos\&theta;}}_{zk}& {\mathrm{sin\&theta;}}_{zk}& 0\\ -{\mathrm{sin\&theta;}}_{zk}& {\mathrm{cos\&theta;}}_{zk}& 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{ccc}1& 0& 0\\ 0& {\mathrm{cos\&theta;}}_{xk}& {\mathrm{sin\&theta;}}_{xk}\\ 0& -{\mathrm{sin\&theta;}}_{xk}& {\mathrm{cos\&theta;}}_{xk}\end{array}\right]\left[\begin{array}{ccc}{\mathrm{cos\&theta;}}_{yk}& 0& -{\mathrm{sin\&theta;}}_{yk}\\ 0& 1& 0\\ {\mathrm{sin\&theta;}}_{yk}& 0& {\mathrm{cos\&theta;}}_{yk}\end{array}\right]\left[\begin{array}{ccc}1& 0& 0\\ 0& {\mathrm{cos\&theta;}}_{x^{\&prime;}k}& {\mathrm{sin\&theta;}}_{x^{\&prime;}k}\\ 0& -{\mathrm{sin\&theta;}}_{x^{\&prime;}k}& {\mathrm{cos\&theta;}}_{x^{\&prime;}k}\end{array}\right]$

$R_p^b\left({\mathrm{\&theta;}}_{x^{\&prime;}1},{\mathrm{\&theta;}}_{y1},{\mathrm{\&theta;}}_{x1},{\mathrm{\&theta;}}_{z1}\right)=\left[\begin{array}{ccc}1& 0& 0\\ 0& {\mathrm{cos\&theta;}}_{x^{\&prime;}1}& -{\mathrm{sin\&theta;}}_{x^{\&prime;}1}\\ 0& {\mathrm{sin\&theta;}}_{x^{\&prime;}1}& {\mathrm{cos\&theta;}}_{x^{\&prime;}1}\end{array}\right]\left[\begin{array}{ccc}{\mathrm{cos\&theta;}}_{y1}& 0& {\mathrm{sin\&theta;}}_{y1}\\ 0& 1& 0\\ -{\mathrm{sin\&theta;}}_{y1}& 0& {\mathrm{cos\&theta;}}_{y1}\end{array}\right]\left[\begin{array}{ccc}1& 0& 0\\ 0& {\mathrm{cos\&theta;}}_{x1}& -{\mathrm{sin\&theta;}}_{x1}\\ 0& {\mathrm{sin\&theta;}}_{x1}& {\mathrm{cos\&theta;}}_{x1}\end{array}\right]\left[\begin{array}{ccc}{\mathrm{cos\&theta;}}_{z1}& -{\mathrm{sin\&theta;}}_{z1}& 0\\ {\mathrm{sin\&theta;}}_{z1}& {\mathrm{cos\&theta;}}_{z1}& 0\\ 0& 0& 1\end{array}\right]$

θ_x′k、θ_yk、θ_xk、θ_zk分别为惯性稳定平台在第k个位置时随动轴相对基座的转角、外环轴相对随动轴的转角、内环轴相对外环轴的转角以及台体轴相对内环轴的转角，θ_x′1、θ_y1、θ_x1、θ_z1分别为惯性稳定平台在第1个位置时随动轴相对基座的转角、外环轴相对随动轴的转角、内环轴相对外环轴的转角以及台体轴相对内环轴的转角；

(2.3)根据步骤(2.2)得到的cosθ_k和sinθ_k，利用如下公式解算θ_k：

3.根据权利要求2所述的一种惯性稳定平台固定基座多位置自瞄准方法，其特征在于：所述步骤(2.2)中θ_x′1、θ_y1、θ_x1、θ_z1、θ_x′k、θ_yk、θ_xk、θ_zk按照如下方式确定：

(3.1)惯性稳定平台在第1个位置时，平台工作处于锁零调平状态，此时有：

θ_y1＝θ_x1＝0°

台体调平后，通过轴端姿态角传感器采集θ_x′1、θ_z1；

(3.2)当惯性稳定平台在第k个位置时，外环轴相对随动轴的转角θ_yk由控制指令解算算法求得，并将θ_yk作为指令输入至平台控制系统，之后平台控制系统判断平台处于随锁还是随动状态，若为随锁状态，则θ_x′k＝θ_x′(k-1)，台体调平后，通过轴端姿态角传感器采集θ_xk和θ_zk；若为随动状态，则θ_xk＝θ_x(k-1)，台体调平后，通过轴端姿态角传感器采集θ_x′k和θ_zk。

4.根据权利要求3所述的一种惯性稳定平台固定基座多位置自瞄准方法，其特征在于：所述步骤(3.2)中判断平台处于随锁还是随动状态的方法为：

判断θ_yk是否属于[-90°,-80°]U[80°,90°]，若属于，则平台处于随锁状态，否则，平台处于随动状态。

5.根据权利要求4所述的一种惯性稳定平台固定基座多位置自瞄准方法，其特征在于：所述步骤(3.2)中θ_yk直接由控制指令任意给定。

6.根据权利要求4所述的一种惯性稳定平台固定基座多位置自瞄准方法，其特征在于：所述步骤(3.2)中θ_yk按照如下方式给定：

(6.1)首先假设平台为随动状态，根据期望的θ_k，利用如下公式计算随动状态下的外环轴转角θ_yk：

${\mathrm{\&theta;}}_{yk}=-\arcsin \frac{c_{31}}{{\mathrm{cos\&theta;}}_{x(k-1)}}$

其中

$\left[\begin{array}{ccc}{\mathrm{cos\&theta;}}_k& 0& -{\mathrm{sin\&theta;}}_k\\ 0& 1& 0\\ {\mathrm{sin\&theta;}}_k& 0& {\mathrm{cos\&theta;}}_k\end{array}\right]R_b^p({\mathrm{\&theta;}}_{x^{\&prime;}1},{\mathrm{\&theta;}}_{y1},{\mathrm{\&theta;}}_{x1},{\mathrm{\&theta;}}_{z1})=\left[\begin{array}{ccc}{c}_{11}& c_{12}& c_{13}\\ c_{21}& c_{22}& c_{23}\\ c_{31}& c_{32}& c_{33}\end{array}\right]$

(6.2)判断平台处于随锁还是随动状态

1)若平台处于随锁状态，按照以下公式计算θ_yk：

$\left\{\begin{array}{c}{\mathrm{\&theta;}}_{x^{\&prime;}k}={\mathrm{\&theta;}}_{x^{\&prime;}\left(k-1\right)}\\ {\mathrm{\&theta;}}_{yk}=\left\{\begin{array}{cc}\arctan \frac{c_{31}}{{\mathrm{sin\&theta;}}_{x^{\&prime;}k}{c}_{32}+{\mathrm{cos\&theta;}}_{x^{\&prime;}k}{c}_{33}}& \left(\begin{array}{c}{c}_{31}\&GreaterEqual;0\\ {\mathrm{sin\&theta;}}_{x^{\&prime;}k}{c}_{32}+{\mathrm{cos\&theta;}}_{x^{\&prime;}k}{c}_{33}>0\end{array}\right)\\ \mathrm{\&pi;}+\arctan \frac{c_{31}}{{\mathrm{sin\&theta;}}_{x^{\&prime;}k}{c}_{32}+{\mathrm{cos\&theta;}}_{x^{\&prime;}k}{c}_{33}}& \left(\begin{array}{c}{c}_{31}>0\\ {\mathrm{sin\&theta;}}_{x^{\&prime;}k}{c}_{32}+{\mathrm{cos\&theta;}}_{x^{\&prime;}k}{c}_{33}\&le;0\end{array}\right)\\ \arctan \frac{c_{31}}{{\mathrm{sin\&theta;}}_{x^{\&prime;}k}{c}_{32}+{\mathrm{cos\&theta;}}_{x^{\&prime;}k}{c}_{33}}& \left(\begin{array}{c}{c}_{31}<0\\ {\mathrm{sin\&theta;}}_{x^{\&prime;}k}{c}_{32}+{\mathrm{cos\&theta;}}_{x^{\&prime;}k}{c}_{33}\&GreaterEqual;0\end{array}\right)\\ \arctan \frac{c_{31}}{{\mathrm{sin\&theta;}}_{x^{\&prime;}k}{c}_{32}+{\mathrm{cos\&theta;}}_{x^{\&prime;}k}{c}_{33}}-\mathrm{\&pi;}& \left(\begin{array}{c}{c}_{31}\&le;0\\ {\mathrm{sin\&theta;}}_{x^{\&prime;}k}{c}_{32}+{\mathrm{cos\&theta;}}_{x^{\&prime;}k}{c}_{33}<0\end{array}\right)\end{array}\right.\end{array}\right.$

2)若平台为随动状态，按照以下方法计算θ_yk：

a.

$\left\{\begin{array}{c}{\mathrm{\&theta;}}_{x^{\&prime;}k1}=\mathrm{sgn}\left({\mathrm{sin\&theta;}}_{x^{\&prime;}k1}\right)\arccos \left({\mathrm{cos\&theta;}}_{x^{\&prime;}k1}\right)\\ {\mathrm{\&theta;}}_{yk1}=-\arcsin \frac{c_{31}}{{\mathrm{cos\&theta;}}_{x\left(k-1\right)}}\\ {\mathrm{\&theta;}}_{xk}={\mathrm{\&theta;}}_{x\left(k-1\right)}\\ {\mathrm{\&theta;}}_{zk1}=\mathrm{sgn}\left({\mathrm{sin\&theta;}}_{zk1}\right)\arccos \left({\mathrm{cos\&theta;}}_{zk1}\right)\end{array}\right.,\left\{\begin{array}{c}{\mathrm{\&theta;}}_{x^{\&prime;}k2}=\mathrm{sgn}\left({\mathrm{sin\&theta;}}_{x^{\&prime;}k2}\right)\arccos \left({\mathrm{cos\&theta;}}_{x^{\&prime;}k2}\right)\\ {\mathrm{\&theta;}}_{yk2}=\left\{\begin{array}{cc}-\arcsin \frac{c_{31}}{{\mathrm{cos\&theta;}}_{x\left(k-1\right)}}-\mathrm{\&pi;}& \frac{c_{31}}{{\mathrm{cos\&theta;}}_{x\left(k-1\right)}}\&GreaterEqual;0\\ \mathrm{\&pi;}-\arcsin \frac{c_{31}}{{\mathrm{cos\&theta;}}_{x\left(k-1\right)}}& \frac{c_{31}}{{\mathrm{cos\&theta;}}_{x\left(k-1\right)}}<0\end{array}\right.\\ {\mathrm{\&theta;}}_{xk}={\mathrm{\&theta;}}_{x\left(k-1\right)}\\ {\mathrm{\&theta;}}_{zk2}=\mathrm{sgn}\left({\mathrm{sin\&theta;}}_{zk2}\right)\arccos \left({\mathrm{cos\&theta;}}_{zk2}\right)\end{array}\right.$

其中：

$\left\{\begin{array}{c}{\mathrm{sin\&theta;}}_{x^{\&prime;}k1}=\frac{c33{\mathrm{sin\&theta;}}_{xk}-c32{\mathrm{cos\&theta;}}_{xk}{\mathrm{cos\&theta;}}_{yk1}}{{\cos }^2{\mathrm{\&theta;}}_{xk}{\mathrm{cos\&theta;}}_{yk1}+{\sin }^2{\mathrm{\&theta;}}_{xk}}\\ {\mathrm{cos\&theta;}}_{x^{\&prime;}k1}=\frac{c32{\mathrm{sin\&theta;}}_{xk}+c33{\mathrm{cos\&theta;}}_{xk}{\mathrm{cos\&theta;}}_{yk1}}{{\cos }^2{\mathrm{\&theta;}}_{xk}{\mathrm{cos\&theta;}}_{yk2}+{\sin }^2{\mathrm{\&theta;}}_{xk}}\\ {\mathrm{sin\&theta;}}_{zk1}=\frac{c11{\mathrm{sin\&theta;}}_{xk}{\mathrm{sin\&theta;}}_{yk1}-c21{\mathrm{cos\&theta;}}_{yk1}}{{\cos }^2{\mathrm{\&theta;}}_{yk1}+{\sin }^2{\mathrm{\&theta;}}_{xk}{\sin }^2{\mathrm{\&theta;}}_{yk1}}\\ {\mathrm{cos\&theta;}}_{zk1}=\frac{c11{\mathrm{cos\&theta;}}_{yk}+c21{\mathrm{sin\&theta;}}_{xk}{\mathrm{sin\&theta;}}_{yk}}{{\cos }^2{\mathrm{\&theta;}}_{yk1}+{\sin }^2{\mathrm{\&theta;}}_{xk}{\sin }^2{\mathrm{\&theta;}}_{yk1}}\end{array}\right.,\left\{\begin{array}{c}{\mathrm{sin\&theta;}}_{x^{\&prime;}k2}=\frac{c33{\mathrm{sin\&theta;}}_{xk}-c32{\mathrm{cos\&theta;}}_{xk}{\mathrm{cos\&theta;}}_{yk2}}{{\cos }^2{\mathrm{\&theta;}}_{xk}{\mathrm{cos\&theta;}}_{yk2}+{\sin }^2{\mathrm{\&theta;}}_{xk}}\\ {\mathrm{cos\&theta;}}_{x^{\&prime;}k2}=\frac{c32{\mathrm{sin\&theta;}}_{xk}+c33{\mathrm{cos\&theta;}}_{xk}{\mathrm{cos\&theta;}}_{yk2}}{{\cos }^2{\mathrm{\&theta;}}_{xk}{\mathrm{cos\&theta;}}_{yk2}+{\sin }^2{\mathrm{\&theta;}}_{xk}}\\ {\mathrm{sin\&theta;}}_{zk2}=\frac{c11{\mathrm{sin\&theta;}}_{xk}{\mathrm{sin\&theta;}}_{yk2}-c21{\mathrm{cos\&theta;}}_{yk2}}{{\cos }^2{\mathrm{\&theta;}}_{yk2}+{\sin }^2{\mathrm{\&theta;}}_{xk}{\sin }^2{\mathrm{\&theta;}}_{yk2}}\\ {\mathrm{cos\&theta;}}_{zk2}=\frac{c11{\mathrm{cos\&theta;}}_{yk}+c21{\mathrm{sin\&theta;}}_{xk}{\mathrm{sin\&theta;}}_{yk}}{{\cos }^2{\mathrm{\&theta;}}_{yk2}+{\sin }^2{\mathrm{\&theta;}}_{xk}{\sin }^2{\mathrm{\&theta;}}_{yk2}}\end{array}\right.$

b.