CN103954301A - 惯性测量系统基于火箭橇轨道坐标系的定位方法 - Google Patents

Abstract

惯性测量系统基于火箭橇轨道坐标系的定位方法，(1)设置火箭橇轨道坐标系；(2)惯性测量系统进行自对准或者进行传递对准，得到火箭橇橇体的轨道坐标系下的三个姿态角初值；(3)计算地球转速以及重力加速度在火箭橇轨道坐标系下的分量，结合惯性测量系统中捷联陀螺的输出，更新火箭橇橇体在火箭橇轨道坐标系下的三个姿态角；(4)利用步骤(3)中更新后的姿态角计算火箭橇轨道坐标系到捷联本体坐标系的姿态变换矩阵；(5)利用步骤(4)中的姿态变换矩阵、步骤(3)中的重力加速度在火箭橇轨道坐标系下的分量，结合惯性测量系统中捷联加表的输出，得到火箭橇橇体在火箭橇轨道的加速度，进而得到火箭橇橇体在火箭橇轨道的速度以及位置。

Description

惯性测量系统基于火箭橇轨道坐标系的定位方法

技术领域

本发明涉及一种惯性测量系统基于火箭橇发射点轨道坐标系的导航计算方法，可用于惯性测量系统精度评定和惯性导航系统落点精度评估中。

背景技术

火箭橇试验具有产生大过载、高速度、强振动和冲击等综合条件的能力，可以最逼真地模拟导弹真实飞行环境。通过试验能够考核惯性测量系统在综合环境条件下的各项性能指标和精度，验证惯性测量系统误差模型在高动态条件下的正确性，特别是在大过载情况下，高次项放大作用，能够确定惯性测量系统高次误差项对导航性能的影响，是实现惯性测量系统动态性能验证的最佳途径。

捷联式惯导系统的最大特点是依靠算法建立起导航坐标系，即平台坐标系以数学平台形式存在，这样就省略了复杂的物理实体平台，一般情况下的导航坐标系是在地理坐标系下进行导航解算，

在惯性测量系统火箭橇试验中，目前主要采用基于地理坐标系的导航计算方法，速度信息取东向速度v_e、北向速度v_n和天向速度v_u，位置信息取纬度经度λ和高度h，导航计算公式为

其中，V^L＝(v_e，v_n，v_u)。

在计算火箭橇橇体运行轨迹时，需要把东向速度和北向速度合成为轨道速度，即

$v=\sqrt{v_e^2+v_n^2+v_u^2}$

在求取橇体运行距离时，有

$S={\∫}_0^{t}v\left(t\right)\mathrm{dt}={\∫}_0^t\sqrt{v_e^2+v_n^2}\mathrm{dt}$

从以上计算过程可以看出，在惯性测量系统火箭橇试验时，基于理坐标系的导航计算方法存在以下缺点：

(1)由于天向速度v_u发散，所以在计算橇体速度时，采用如下简化方法

$v=\sqrt{v_e^2+v_n^2}$

这种方法适合考虑到曲率半径后与地球水准面平行的轨道，但对直线轨道来说存在高度误差。

(2)由于速度解算存在误差，会引起位置误差。另外，由于是标量运算，没有方向信息，求解得到的位置和速度信息都是一维信息，缺少三维空间的位置误差、速度误差和姿态角误差信息。

因此，需要研究一种适合于火箭橇试验的导航解算方法。

发明内容

本发明的技术解决问题：精确解算火箭橇橇体沿轨道运行时的姿态、位置、速度，为最终进行惯性测量系统的精度评定和惯性导航落点精度评估提供精确的导航结果。

本发明的技术解决方案：惯性测量系统基于火箭橇轨道坐标系的定位方法，步骤如下：

(1)设置火箭橇轨道坐标系(OX_lY_lZ_l)，该坐标系的原点为火箭橇轨道起始点，OX_l轴指向火箭橇橇体运动前进方向，OZ_l轴朝上垂直于轨道，OY_l轴在水平面内垂直于轨道，且三者满足右手坐标系；

(2)惯性测量系统进行自对准或者进行传递对准，得到火箭橇橇体的轨道坐标系下的三个姿态角初值；

(3)计算地球转速以及重力加速度在火箭橇轨道坐标系下的分量，结合惯性测量系统中捷联陀螺的输出，更新火箭橇橇体在火箭橇轨道坐标系下的三个姿态角；

(4)利用步骤(3)中更新后的姿态角计算火箭橇轨道坐标系到捷联本体坐标系的姿态变换矩阵；

(5)利用步骤(4)中的姿态变换矩阵、步骤(3)中的重力加速度在火箭橇轨道坐标系下的分量，结合惯性测量系统中捷联加表的输出，得到火箭橇橇体在火箭橇轨道的加速度，进而得到火箭橇橇体在火箭橇轨道的速度以及位置。

所述步骤(2)中的火箭橇橇体的轨道坐标系下三个方向的姿态角初值计算公式如下：

$\left\{\begin{array}{c}{\mathrm{\φ}}_x=\arcsin \left[R_l^b\left(\mathrm{2,3}\right)\right]\\ {\mathrm{\φ}}_y=-\arctan \left[\frac{R_l^b\left(\mathrm{1,3}\right)}{R_l^b\left(\mathrm{3,3}\right)}\right]\\ {\mathrm{\φ}}_z=-\arctan \left[\frac{R_l^b\left(\mathrm{2,1}\right)}{R_l^b\left(\mathrm{2,2}\right)}\right]\end{array}\right.$

其中，为载体坐标系到轨道坐标系的姿态变换矩阵，

所述步骤(3)中三个姿态角利用下述公式积分得到，

其中，为第n次更新时的时间tn对应的捷联陀螺的输出；为地球转速在轨道坐标系中的投影分量；将第n次更新时的时间tn对应的三个姿态角 根据上述积分即可得到下一更新时间t_n+1对应的姿态角

所述步骤(5)中重力加速度在火箭橇轨道坐标系下的分量如下：

$\left[\begin{array}{c}{g}_x^l\\ g_y^l\\ g_z^l\end{array}\right]=\left[\begin{array}{c}-(g_0+b_1{r}_x+b_2{r}_x^2)\sin \frac{\left|P_{0}P\right|-r_x}{N+h_p}\\ 0\\ (g_0+b_1{r}_x+b_2{r}_x^2)\cos \frac{\left|P_{0}P\right|-r_x}{N+h_p}\end{array}\right]$

其中，a为地球长半轴，e²为地球偏心率，为火箭橇橇体所在点的纬度值；h_p为橇体相对水准面的高度；|P₀P|＝(N+h_p)β′，β′为矢量OP₀和OP的夹角，其中矢量OP₀为地球中心O到轨道初始点P₀的矢量，OP为地球中心O到P点的矢量，P为轨道与地球表面切点，为火箭橇轨道重力加速度模型，其中r_x为橇体沿轨道运行的距离；g₀为发射点位置的重力加速度，b₁和b₂为常值。

所述步骤(5)中火箭橇橇体在火箭橇轨道的加速度计算公式如下：

${\stackrel{\·}{V}}^l\left(t_n\right)=R_b^l\left(t_{n+1}\right)f^b\left(t_n\right)-2{\mathrm{\Ω}}_{\mathrm{ie}}^l{V}^l\left(t_n\right)+g^l$

其中，g^l为重力加速度在轨道坐标系下的投影，为步骤中更新后的姿态变换矩阵，f^b(t_n)为捷联加表的测量值，为地球转速在轨道坐标系投影分量的反对称矩阵。

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

(1)基于火箭撬发射点的轨道坐标系的导航算法中，对于直线轨道来说，基于火箭撬发射点的轨道坐标系OX轴与轨道平行，因此，可以直接得到橇体沿轨道运行的距离信息；对于与地球水平面平行的弯曲轨道，就可在基于火箭撬发射点的轨道坐标系OXZ平面内描述橇体沿轨道运行的距离信息。该计算方法简单，且物理意义明确。

(2)基于火箭撬发射点的轨道坐标系的导航算法姿态角更新时只需考虑陀螺仪组合的测量值即可，不需要考虑速度的影响，算法简单。而基于地理坐标系的导航算法在姿态更新时需要考虑速度的影响。

(3)重力加速度模型是一个只与运行距离有关的二次项方程，而不是与纬度、高度相关的高阶方程。

附图说明

图1为本发明基于火箭撬发射点轨道坐标系的导航方法的实现流程图；

图2为火箭橇轨道在大地坐标系中的仿真示意图；

图3为轨道偏航角和俯仰角示意图；

图4为火箭橇轨道俯仰角变化示意图；

图5为重力加速度仿真比较结果示意图；

图6为本发明导航的结果与外测数据对比图。

具体实施方式

下面首先对方法中涉及的坐标系进行说明。

橇体坐标系b与橇体固连，原点为橇体中心，x轴指向运动方向，z轴指天，y轴分别与x、z轴垂直，且满足右手准则。

地理坐标系原点为橇体中心，其中x、y、z三轴满足东北天准则。

本发明方法如图1所示，是基于轨道坐标系的状态空间模型、初始姿态对准、姿态角更新、坐标变换矩阵求取、速度更新以及位置更新等组成，具体步骤如下：

(1)设置火箭橇轨道坐标系(OX_lY_lZ_l)，该坐标系的原点为火箭橇轨道起始点，OX_l轴指向火箭橇橇体运动前进方向，OZ_l轴朝上垂直于轨道，OY_l轴在水平面内垂直于轨道，且三者满足右手坐标系；

(2)惯性测量系统进行自对准或者进行传递对准，得到火箭橇橇体的轨道坐标系下的三个姿态角初值；

一般来讲，对于高精度惯性测量系统，可采用自对准方案，而对于精度相对较低的惯性测量系统，采用外部水平和方位传递对准的方式。

从地理坐标系到轨道坐标系的转换矩阵由大地测量得到，可以认为没有误差，

$R_{L_0}^l=\left[\begin{array}{ccc}\cos {\mathrm{\β}}_0& 0& \sin {\mathrm{\β}}_0\\ 0& 1& 0\\ -\sin {\mathrm{\β}}_0& 0& \cos {\mathrm{\β}}_0\end{array}\right]\left[\begin{array}{ccc}\cos {\mathrm{\γ}}_0& \sin {\mathrm{\γ}}_0& 0\\ -\sin {\mathrm{\γ}}_0& \cos {\mathrm{\γ}}_0& 0\\ 0& 0& 1\end{array}\right]$

其中，β₀和γ₀分别为轨道相对发射点地理坐标系的俯仰角和偏航角，其示意图如图3所示。

在初始对准时，保持惯组静止一段时间，记录这段时间内的在橇体坐标系三个轴向测量到的平均加速度为平均角速度为因此可以得到橇体坐标系b和地理坐标系L₀之间的姿态角为

$\left\{\begin{array}{c}{\mathrm{\φ}}_x^{b,L_0}=\arcsin \frac{A_y^b}{g_0}\\ {\mathrm{\φ}}_y^{b,L_0}=-\arcsin \frac{A_x^b}{A_z^b}\\ {\mathrm{\φ}}_z^{b,L_0}=\arcsin \frac{G_x^b\cos {\mathrm{\φ}}_y^{b,L}+G_z^b\sin {\mathrm{\φ}}_y^{b,L}}{{\mathrm{\ω}}_{\mathrm{ie}}\cos L}\end{array}\right.$

其中，g₀为测试点重力加速度，ω_ie为地球自转角速度，L为测试点纬度。

地理坐标系L₀到橇体坐标系b的姿态变换矩阵为

$R_{L_0}^b=\left[\begin{array}{ccc}\cos {\mathrm{\φ}}_y^{b,L_0}& 0& -\sin {\mathrm{\φ}}_y^{b,L_0}\\ 0& 1& 0\\ -\sin {\mathrm{\φ}}_y^{b,L_0}& 0& \cos {\mathrm{\φ}}_y^{b,L_0}\end{array}\right]\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos {\mathrm{\φ}}_x^{b,L_0}& \sin {\mathrm{\φ}}_x^{b,L_0}\\ 0& -\sin {\mathrm{\φ}}_x^{b,L_0}& \cos {\mathrm{\φ}}_x^{b,L_0}\end{array}\right]\left[\begin{array}{ccc}\cos {\mathrm{\φ}}_z^{b,L_0}& {\sin \mathrm{\φ}}_z^{b,L_0}& 0\\ -\sin {\mathrm{\φ}}_z^{b,L_0}& \cos {\mathrm{\φ}}_z^{b,L_0}& 0\\ 0& 0& 1\end{array}\right]$

且

$R_b^{L_0}={\left(R_{L_0}^b\right)}^T$

由此可得从橇体坐标系到轨道坐标系的姿态变换矩阵初值以及 $R_l^b={\left(R_b^l\right)}^{-1}.$ 因此，有

$\left\{\begin{array}{c}{\mathrm{\φ}}_x=\arcsin \left[R_l^b\left(\mathrm{2,3}\right)\right]\\ {\mathrm{\φ}}_y=-\arctan \left[\frac{R_l^b\left(\mathrm{1,3}\right)}{R_l^b\left(\mathrm{3,3}\right)}\right]\\ {\mathrm{\φ}}_z=-\arctan \left[\frac{R_l^b\left(\mathrm{2,1}\right)}{R_l^b\left(\mathrm{2,2}\right)}\right]\end{array}\right.$

(3)计算地球转速以及重力加速度在火箭橇轨道坐标系下的分量，结合惯性测量系统中捷联陀螺的输出，更新火箭橇橇体在火箭橇轨道坐标系下的三个姿态角；

由于轨道坐标系与地球固联，则地球转速在轨道坐标系的分量为常值向量

其中，为发射点的地理纬度，ω_ie为地球自转速度。

在捷联陀螺测量得到后，就可利用下面微分方程对三个姿态角进行更新：

其中，为第n次更新时的时间tn对应的捷联陀螺的输出；为地球转速在轨道坐标系中的投影分量；

将第n次更新时的时间t_n对应的三个姿态角根据上述积分即可得到下一更新时间t_n+1对应的姿态角其中，当n＝0时，三个姿态角的取值为步骤(2)的结果。

(4)利用步骤(3)中更新后的姿态角计算火箭橇轨道坐标系到捷联本体坐标系的姿态变换矩阵；

(5)利用步骤(4)中的姿态变换矩阵、步骤(3)中的重力加速度在火箭橇轨道坐标系下的分量，结合惯性测量系统中捷联加表的输出，得到火箭橇橇体在火箭橇轨道的加速度，进而得到火箭橇橇体在火箭橇轨道的速度以及位置。

发射点轨道坐标系的重力加速度分量为

$\left[\begin{array}{c}{g}_x^l\\ g_y^l\\ g_z^l\end{array}\right]=\left[\begin{array}{c}g\left(r_x\right)\sin \mathrm{\β}\left(r_x\right)\\ 0\\ g\left(r_x\right)\cos \mathrm{\β}\left(r_x\right)\end{array}\right]=\left[\begin{array}{c}-(g_0+b_1{r}_x+b_2{r}_x^2)\sin \frac{\left|P_{0}P\right|-r_x}{N+h_p}\\ 0\\ (g_0+b_1{r}_x+b_2{r}_x^2)\cos \frac{\left|P_{0}P\right|-r_x}{N+h_p}\end{array}\right]$

其中，地球长半轴a＝6378137±2m，地球偏心率e²，为载体所在点的纬度值；h_p为橇体相对水准面的高度；|P₀P|＝(N+h_p)β′，β′为矢量OP₀和OP的夹角，其中矢量OP₀为地球中心O到轨道初始点P₀的矢量，OP为地球中心O到P点的矢量，P为轨道与地球表面切点，为火箭橇轨道重力加速度模型，其中r_x为橇体沿轨道运行的距离；g₀为发射点位置的重力加速度，b₁和b₂为常值，由轨道重力测量值拟合确定，具体拟合即采用现有方法每隔固定距离(例如60m)测量重力加速度，将所有测量值采用最小二乘法进行拟合后即可得到b₁和b₂值。

惯性导航系统中的捷联加表测量值为f^b(t_n)，就可利用下述速度微分方程

${\stackrel{\·}{V}}^l\left(t_n\right)=R_b^l\left(t_{n+1}\right)f^b\left(t_n\right)-2{\mathrm{\Ω}}_{\mathrm{ie}}^l{V}^l\left(t_n\right)+g^l$

对速度进行更新，就可得到一个新的在轨道坐标系下的速度矢量V^l(t_n+1)。其中g^l为重力矢量在轨道坐标系中的投影，为地球转速在轨道坐标系投影分量的反对称矩阵，

在得到速度矢量V^l(t_n+1)后，就可利用位置微分方程

${\stackrel{\·}{r}}^l\left(t_n\right)=V^l\left(t_{n+1}\right)$

对位置进行更新，就可得到一个新的在轨道坐标系下的位置矢量r^l(t_n+1)

在导航时间内，重复步骤(3)-(5)，完成惯性测量系统基于火箭橇轨道坐标系的定位。

实施例

火箭橇试验所用的轨道为一条平直的直线，如图2中P₀P₁表示火箭橇轨道，其中P点为轨道与地球表面的相切点。图3中描述了轨道坐标系与地理坐标系的关系，从而可以确定轨道坐标系到地理坐标系的转换矩阵轨道俯仰角与运行距离之间的关系如图4所示，从图中可以看出，俯仰角从初始位置到末区由负值变为正值。图5中分别描述了拟合重力加速度模型和拟合重力加速度残差，由图5中可以看出拟合曲线与实测值之间的差值小于1μGal。采用上述轨道参数和重力加速度模型后的导航解算结果如图6所示，可以得到三维位置、速度和姿态角信息。

本发明未详细描述内容为本领域技术人员公知技术。

Claims (5)

1.惯性测量系统基于火箭橇轨道坐标系的定位方法，其特征在于步骤如下：

(1)设置火箭橇轨道坐标系(OX_lY_lZ_l)，该坐标系的原点为火箭橇轨道起始点，OX_l轴指向火箭橇橇体运动前进方向，OZ_l轴朝上垂直于轨道，OY_l轴在水平面内垂直于轨道，且三者满足右手坐标系；

(2)惯性测量系统进行自对准或者进行传递对准，得到火箭橇橇体的轨道坐标系下的三个姿态角初值；

(3)计算地球转速以及重力加速度在火箭橇轨道坐标系下的分量，结合惯性测量系统中捷联陀螺的输出，更新火箭橇橇体在火箭橇轨道坐标系下的三个姿态角；

(4)利用步骤(3)中更新后的姿态角计算火箭橇轨道坐标系到捷联本体坐标系的姿态变换矩阵；

(5)利用步骤(4)中的姿态变换矩阵、步骤(3)中的重力加速度在火箭橇轨道坐标系下的分量，结合惯性测量系统中捷联加表的输出，得到火箭橇橇体在火箭橇轨道的加速度，进而得到火箭橇橇体在火箭橇轨道的速度以及位置。

2.根据权利要求1所述的惯性测量系统基于火箭橇轨道坐标系的定位方法，其特征在于：所述步骤(2)中的火箭橇橇体的轨道坐标系下三个方向的姿态角初值计算公式如下：

$\left\{\begin{array}{c}{\mathrm{\φ}}_x=\arcsin \left[R_l^b\left(\mathrm{2,3}\right)\right]\\ {\mathrm{\φ}}_y=-\arctan \left[\frac{R_l^b\left(\mathrm{1,3}\right)}{R_l^b\left(\mathrm{3,3}\right)}\right]\\ {\mathrm{\φ}}_z=-\arctan \left[\frac{R_l^b\left(\mathrm{2,1}\right)}{R_l^b\left(\mathrm{2,2}\right)}\right]\end{array}\right.$

其中，为载体坐标系到轨道坐标系的姿态变换矩阵，

3.根据权利要求1所述的惯性测量系统基于火箭橇轨道坐标系的定位方法，其特征在于：所述步骤(3)中三个姿态角利用下述公式积分得到，

其中，为第n次更新时的时间tn对应的捷联陀螺的输出；为地球转速在轨道坐标系中的投影分量；将第n次更新时的时间t_n对应的三个姿态角 根据上述积分即可得到下一更新时间t_n+1对应的姿态角

4.根据权利要求1所述的惯性测量系统基于火箭橇轨道坐标系的定位方法，其特征在于：所述步骤(5)中重力加速度在火箭橇轨道坐标系下的分量如下：

$\left[\begin{array}{c}{g}_x^l\\ g_y^l\\ g_z^l\end{array}\right]=\left[\begin{array}{c}-(g_0+b_1{r}_x+b_2{r}_x^2)\sin \frac{\left|P_{0}P\right|-r_x}{N+h_p}\\ 0\\ (g_0+b_1{r}_x+b_2{r}_x^2)\cos \frac{\left|P_{0}P\right|-r_x}{N+h_p}\end{array}\right]$

其中，a为地球长半轴，e²为地球偏心率，为火箭橇橇体所在点的纬度值；h_p为橇体相对水准面的高度；|P₀P|＝(N+h_p)β′，β′为矢量OP₀和OP的夹角，其中矢量OP₀为地球中心O到轨道初始点P₀的矢量，OP为地球中心O到P点的矢量，P为轨道与地球表面切点，为火箭橇轨道重力加速度模型，其中r_x为橇体沿轨道运行的距离；g₀为发射点位置的重力加速度，b₁和b₂为常值。

5.根据权利要求1所述的惯性测量系统基于火箭橇轨道坐标系的定位方法，其特征在于：所述步骤(5)中火箭橇橇体在火箭橇轨道的加速度计算公式如下：

${\stackrel{\·}{V}}^l\left(t_n\right)=R_b^l\left(t_{n+1}\right)f^b\left(t_n\right)-2{\mathrm{\Ω}}_{\mathrm{ie}}^l{V}^l\left(t_n\right)+g^l$

其中，g^l为重力加速度在轨道坐标系下的投影，为步骤中更新后的姿态变换矩阵，f^b(t_n)为捷联加表的测量值，为地球转速在轨道坐标系投影分量的反对称矩阵。