CN109059914A - 一种基于gps和最小二乘滤波的炮弹滚转角估计方法 - Google Patents

Abstract

本发明公开一种基于GPS和最小二乘滤波的炮弹滚转角估计方法，该方法包括以下步骤：(1)根据陀螺仪和GPS提供的炮弹速度和位置，计算t时刻载体惯性系i_b系相对于载体系b的炮弹姿态矩阵导航系n相对于导航惯性系i_n的炮弹姿态矩阵(2)计算炮弹在载体惯性系i_b和导航惯性系i_n系下的速度值和(3)由GPS提供的速度比值计算初始航向角Y₀与纵摇角P₀，根据初始滚转角设置状态变量X；(4)根据通过最小二乘滤波估计X并计算为i_n系相对于i_b系的炮弹姿态矩阵；(5)根据得到t时的并计算炮弹滚转角；其中为t时刻n系相对b系的姿态矩阵。

Description

一种基于GPS和最小二乘滤波的炮弹滚转角估计方法

技术领域

本发明属于导航技术领域，尤其涉及一种基于GPS和最小二乘滤波的炮弹滚转角估计方法。

背景技术

制导炮弹是一种低成本、小型化的精确制导武器，一般装配有GPS/INS组合。在实现弹药的精确控制飞行技术中，滚转角的实时准确测量是实施制导或修正控制的基础，也是制导弹药实现远程精确飞行的关键，由于制导弹药发射过程中，通常会承受高过载、高转速的恶劣情况，在此高过载冲击环境下IMU无法正常上电工作，导航系统的初始化需要发射后在空中自主完成。且空中易受风力等气象环境影响，滚转角的估计不易精确求得，故修正制导炮弹滚转角是目前国内外研究的热点问题。目前对滚转姿态测量主要包括惯导系统，地磁方法，GPS等。

发明内容

发明目的：针对以上问题，本发明提出一种基于GPS和最小二乘滤波的炮弹滚转角估计方法，该方法的目的在于仅利用陀螺仪，加速度计和GPS提供的信息，通过最小二乘滤波算法解出最优的滚转角。

技术方案：为实现本发明的目的，本发明所采用的技术方案是：一种基于GPS和最小二乘滤波的炮弹滚转角估计方法，该方法包括以下步骤：

(1)根据陀螺仪和GPS提供的炮弹速度和位置，计算t时刻载体惯性系i_b系相对于载体系b的炮弹姿态矩阵导航系n相对于导航惯性系i_n的炮弹姿态矩阵

(2)计算炮弹在载体惯性系i_b和导航惯性系i_n系下的速度值和

(3)由GPS提供的速度比值计算初始航向角Y₀与纵摇角P₀，根据初始滚转角设置状态变量X；

(4)根据通过最小二乘滤波估计X并计算为i_n系相对于i_b系的炮弹姿态矩阵；

(5)根据得到t时的并计算炮弹滚转角，为t时刻n系相对b系的姿态矩阵。

其中，步骤(1)中，炮弹姿态矩阵和计算方法如下：

设初始时刻t₀时，b系相对于i_b系的炮弹姿态矩阵为I为3×3的单位矩阵；

记t时刻陀螺仪的输出值为即t时刻的b系相对于i_b系的角速度在b系上的投影值，由此跟踪b系相对于i_b系的变化：

其中，为矩阵的变化率，“×”表示三维矢量对应的叉乘矩阵变换，设其中a，b，c分别表示炮弹沿三轴的旋转角速度，则采用毕卡法解式(1)微分方程可得到式(2)：

记待解算数据时长为T，将时间段0～T以采样周期dt为间隔划分为多个时刻点t₀,t₁,t₂...t_m，k＝0,1,2,...,m，则式(2)中为t_k时刻的b系相对于t_k-1时刻的b系的姿态矩阵；为t_k-1时刻炮弹陀螺仪输出，dt为采样周期，最终t＝t_k，k＝1,2,3,...,m，

由GPS输出的炮弹位置信息纬度L，东向、北向、天向速度分别为V_E,V_N,V_U，则n系相对i_n系的角速度可计算如下：

其中，R_N为地球子午圈曲率半径，ω_ie为地球自转角速度，R_E为地球卯酉圈半径，根据公式(2)的计算方法，由计算出

其中，为t_k时刻的n系相对于t_k-1时刻的n系的姿态矩阵；为t＝t_k-1时的值，dt为采样周期，t＝t_k，k＝12,3,...,m。

其中，步骤(2)中，速度值和的计算方法如下：

将炮弹上GPS输出的n系下炮弹速度vⁿ(t)投影到i_n系中，得到

由炮弹在n系的速度计算对应的b系速度v^b(t)＝[0 ||vⁿ(t)|| 0]^T，将炮弹的b系速度v^b(t)投影到i_b系中，得到

其中，步骤(3)中，由GPS提供的炮弹速度比值计算初始航向角Y₀与纵摇角P₀，根据初始滚转角设置状态变量X，方法如下：

炮弹初始时刻的航向角与纵摇角由GPS提供的速度求得：

其中，V_E(t₀)，V_N(t₀)，V_U(t₀)分别表示初始时刻GPS输出的炮弹东向速度、北向速度与天向速度；

设初始时刻的滚转角为R₀，则：

设初始状态变量记其中为t时刻炮弹速度在i_n系的三轴分量；记其中为t时刻炮弹速度在i_b系的三轴分量，记将各元素展开，改写为Z＝H·X的形式，其中：

其中，步骤(4)中，选择基于递推最小二乘算法作为在线辨识滤波器，构建如下：

式中，K_k为t_k时刻的增益矩阵；P_k为t_k时刻的状态向量协方差阵；R_k为t_k时刻的量测噪声阵；H_k为t_k时刻式(9)中H阵的取值；X_k为t_k时刻的状态变量值；Z_k为t_k时刻Z的取值，为t_k时刻炮弹速度在i_b系的投影，I为单位矩阵；

其中，步骤(4)中，根据通过最小二乘滤波估计X方法如下：

设取初值X₀＝[1 0]^T，系统的输入为与依次令k＝1,2,3...m，通过式(10)迭代X_k会逐渐趋于真值，P_k逐渐趋于零，将最终的X_k作为状态变量X；并且，在求解出X的基础上，根据式(8)计算出

其中，步骤(5)中，t时刻炮弹滚转角的计算方法如下：由下式计算得到t时刻的n系相对于b系的姿态矩阵

矩阵为3阶方阵，其各元素记为：则炮弹t时刻的滚转角计算如下：

有益效果：与现有技术相比，本发明的有益技术效果如下：

(1)弹体在空中对准时，只需要IMU和GPS提供的信息，无需多余传感器；

(2)将姿态矩阵进行链式分解，引入最小二乘滤波计算初始滚转角，则GPS速度求得航向角与纵摇角，进而得到矩阵精度较高；

(3)优化对象仅为滚转角，精度更高。

附图说明

图1为本发明解算滚转角误差估计图。

具体实施方式

下面对本发明技术方案进行详细说明，但是本发明的保护范围不局限于所述实施例。

本发明适用于炮弹飞行时滚转角的估计，首先定义如下坐标系：

导航系n：原点为炮弹所在位置处，Y轴指向地理北向，X轴指向地理东向，Z轴垂直于大地水平面指向上。

载体系b：原点为弹体质心，Y轴沿弹体前进方向向前，X轴指向右，Z轴指向上。

导航惯性系i_n：初始时刻的导航系n凝固在惯性空间所得，不随时间变化。

载体惯性系i_b：初始时刻的载体系b凝固在惯性空间所得，不随时间变化。

定义如上坐标系后，t时刻的n系相对于b系的姿态矩阵可分解为其中为t时刻i_b系相对于b系的姿态矩阵；为t时刻n系相对于i_n系的姿态矩阵；为i_n系相对于i_b系的姿态矩阵,为3阶方阵。根据炮弹上陀螺仪输出和GPS提供的速度位置信息，可计算每时刻的计算t时刻炮弹速度在i_b和i_n系下的值和由GPS提供的速度比值计算炮弹的初始航向角Y₀与纵摇角P₀，设炮弹的初始滚转角为R₀，根据通过递推最小二乘算法可得到进而得到并解出任意时刻的炮弹滚转角。

下面结合附图对本发明实施方法做更详细地描述：

1、由炮弹的陀螺仪和GPS组件输出计算姿态矩阵具体包括如下步骤：

初始时刻t₀时，b系与i_b系间姿态矩阵为I为3×3的单位矩阵；

记t时刻陀螺仪的输出值为即t时刻的b系相对于i_b系的角速度在b系上的投影值，由此可跟踪b系相对于i_b系的变化：

其中，为矩阵的变化率，“×”表示三维矢量对应的叉乘矩阵变换，即如果其中a,b,c分别表示炮弹沿三轴的旋转角速度，则采用毕卡法解式(1)微分方程可得到式(2)：

记待解算数据时长为T，将时间段0～T以采样周期dt为间隔划分为多个时刻点t₀,t₁,t₂...t_m，k＝0,1,2,...,m，则式(2)中为t_k时刻的b系相对于t_k-1时刻的b系的姿态矩阵；为t_k-1时刻陀螺仪输出，dt为采样周期。最终计算时t＝t_k，k＝1,2,3,...,m，

根据炮弹上搭载的GPS组件可得到弹体位置信息经度λ与纬度L，东向、北向、天向速度分别为V_E,V_N,V_U。则n系相对i_n系的角速度可计算如下：

其中，R_N为地球子午圈曲率半径，ω_ie为地球自转角速度，R_E为地球卯酉圈半径。参照公式(2)的计算方法，由可计算出

其中，为t_k时刻的n系相对于t_k-1时刻的n系的姿态矩阵；为t＝t_k-1时的值，dt为采样周期。最终计算时t＝t_k，k＝1,2,3,...,m。

2、炮弹速度在i_b和i_n系下的值和的求法如下：

由式(4)，根据炮弹上GPS组件输出的n系下炮弹速度vⁿ(t)，投影到i_n系中，得到

由于||vⁿ(t)||＝||v^b(t)||，且b系速度只有前向分量不为0，可由炮弹在n系的速度计算对应的b系速度v^b(t)＝[0 ||vⁿ(t)|| 0]^T。

则通过式(2)计算得到的姿态矩阵将炮弹的b系速度v^b(t)投影到i_b系中，得到

3、由GPS提供的速度比值计算初始时刻T₀航向角Y₀与纵摇角P₀，设初始滚转角为R₀，根据通过递推最小二乘算法可计算出为i_n系相对于i_b系的姿态矩阵，具体方法如下：

初始时刻的航向角与纵摇角由GPS提供的速度求得：

其中V_E(t₀)，V_N(t₀)，V_U(t₀)分别表示初始时刻GPS输出的炮弹东向速度，北向速度与天向速度。

设初始滚转角为R₀，则：

设状态变量则中仅R₀为未知量。记其中为t时刻炮弹速度在i_n系的三轴分量；记其中为t时刻炮弹速度在i_b系的三轴分量，记将各元素展开，可改写为Z＝H·X的形式，其中：

选择基于递推最小二乘算法作为在线辨识滤波器，构建如下：

式中，K_k为t_k时刻的增益矩阵；P_k为t_k时刻的状态向量协方差阵；R_k为t_k时刻的量测噪声阵；H_k为t_k时刻式(9)中H阵的取值；X_k为t_k时刻的状态变量值；Z_k为t_k时刻Z的取值，为t_k时刻炮弹速度在i_b系的投影；I为单位矩阵。

开始计算时k＝0,取初值X₀＝[1 0]^T，系统的输入为与依次令k＝1,2,3...m，通过式(10)反复迭代X_k会逐渐趋于真值，P_k逐渐趋于零，最终所有数据处理完成后终止解算，辨识出状态变量X。

4、滚转角的计算如下：

根据最后一步迭代得到的X_k，可得到sin(R₀)与cos(R₀)。根据式(8)可计算出再由下式可得到

其中，为t时刻n系相对b系的姿态矩阵；为t时刻i_b系相对b系的姿态矩阵、为t时刻n系相对i_n系的姿态矩阵，均已在前文求出。

矩阵为3阶方阵，其中各元素可记为：

则炮弹t时刻的滚转角由下式解出

本发明的有益效果通过如下仿真得以验证：

根据运动学定理及捷联惯导反演算法，使用Matlab模拟产生相关导航参数，并在其上叠加相应的仪表误差作为仪表实际采集数据，IMU采样周期0.005s，GPS采样周期0.1s。部分仿真参数如下：

初始位置：东经108.97°、北纬34.25°；

赤道半径：6378165m；

地球椭球度：1/298.3；

地球表面重力加速度：9.8m/s²

地球自转角速度：15.04088°/h

wx滚转陀螺零偏(0.15rad/s)

wy偏航陀螺零偏(0.03rad/s)

wz俯仰陀螺零偏(0.03rad/s)

fx加速度计零偏(0.003m/s²)

fy加速度计零偏(0.003m/s²)

fz加速度计零偏(0.003m/s²)

wx滚转陀螺测量噪声(0.15rad/s)

wy偏航陀螺测量噪声(0.01rad/s)

wz俯仰陀螺测量噪声(0.01rad/s)

fx加速度计测量噪声(0.003m/s²)

fy加速度计测量噪声(0.003m/s²)

fz加速度计测量噪声(0.003m/s²)

GPS解算误差(纬度)(5m)

GPS解算误差(经度)(5m)

GPS解算误差(高度)(10m)

GPS解算误差(北向速度)(0.15m/s)

GPS解算误差(天向速度)(0.3m/s)

GPS解算误差(东向速度)(0.15m/s)

选取80s数据进行解算，结果如图所示。图1中各曲线表明，在仿真时间内，本发明设计的方法有效估计出滚转角，且横滚角误差稳定在-6°以内，解算结束时基本在-3.5°左右。表明本发明设计的方法有效完成了横滚角的跟踪。

如上所述，尽管参照特定的优选实施例已经表示和表述了本发明，但其不得解释为对本发明自身的限制。在不脱离所附权利要求定义的本发明的精神和范围前提下，可对其在形式上和细节上作出各种变化。

Claims (8)

1.一种基于GPS和最小二乘滤波的炮弹滚转角估计方法，其特征在于，该方法包括以下步骤：

(1)根据陀螺仪和GPS提供的炮弹速度和位置，计算t时刻载体惯性系i_b系相对于载体系b的炮弹姿态矩阵导航系n相对于导航惯性系i_n的炮弹姿态矩阵

(2)计算炮弹在载体惯性系i_b和导航惯性系i_n系下的速度值和

(3)由GPS提供的速度比值计算初始航向角Y₀与纵摇角P₀，根据初始滚转角设置状态变量X；

(4)根据通过最小二乘滤波估计X并计算 为i_n系相对于i_b系的炮弹姿态矩阵；

(5)根据得到t时的并计算炮弹滚转角，为t时刻n系相对b系的姿态矩阵。

2.根据权利要求1所述的一种基于GPS和最小二乘滤波的炮弹滚转角估计方法，其特征在于，步骤(1)中，炮弹姿态矩阵和计算方法如下：

设初始时刻t₀时，b系相对于i_b系的炮弹姿态矩阵为I为3×3的单位矩阵；

记t时刻陀螺仪的输出值为即t时刻的b系相对于i_b系的角速度在b系上的投影值，由此跟踪b系相对于i_b系的变化：

其中，为矩阵的变化率，“×”表示三维矢量对应的叉乘矩阵变换，设其中a，b，c分别表示炮弹沿三轴的旋转角速度，则采用毕卡法解式(1)微分方程可得到式(2)：

记待解算数据时长为T，将时间段0～T以采样周期dt为间隔划分为多个时刻点t₀,t₁,t₂...t_m，k＝0,1,2,...,m，则式(2)中为t_k时刻的b系相对于t_k-1时刻的b系的姿态矩阵； 为t_k-1时刻炮弹陀螺仪输出，dt为采样周期，最终

由GPS输出的炮弹位置信息纬度L，东向、北向、天向速度分别为V_E,V_N,V_U，则n系相对i_n系的角速度可计算如下：

其中，R_N为地球子午圈曲率半径，ω_ie为地球自转角速度，R_E为地球卯酉圈半径，根据公式(2)的计算方法，由计算出

其中，为t_k时刻的n系相对于t_k-1时刻的n系的姿态矩阵； 为t＝t_k-1时的值，dt为采样周期，

3.根据权利要求2所述的一种基于GPS和最小二乘滤波的炮弹滚转角估计方法，其特征在于，步骤(2)中，速度值和的计算方法如下：

将炮弹上GPS输出的n系下炮弹速度vⁿ(t)投影到i_n系中，得到

由炮弹在n系的速度计算对应的b系速度v^b(t)＝[0 ||vⁿ(t)|| 0]^T，将炮弹的b系速度v^b(t)投影到i_b系中，得到

4.根据权利要求3所述的一种基于GPS和最小二乘滤波的炮弹滚转角估计方法，其特征在于，步骤(3)中，由GPS提供的炮弹速度比值计算初始航向角Y₀与纵摇角P₀，根据初始滚转角设置状态变量X，方法如下：

炮弹初始时刻的航向角与纵摇角由GPS提供的速度求得：

其中，V_E(t₀)，V_N(t₀)，V_U(t₀)分别表示初始时刻GPS输出的炮弹东向速度、北向速度与天向速度；

设初始时刻的滚转角为R₀，则：

设初始状态变量记其中为t时刻炮弹速度在i_n系的三轴分量；记其中为t时刻炮弹速度在i_b系的三轴分量，记将各元素展开，改写为Z＝H·X的形式，其中：

5.根据权利要求4所述的一种基于GPS和最小二乘滤波的炮弹滚转角估计方法，其特征在于，步骤(4)中，选择基于递推最小二乘算法作为在线辨识滤波器，构建如下：

式中，K_k为t_k时刻的增益矩阵；P_k为t_k时刻的状态向量协方差阵；R_k为t_k时刻的量测噪声阵；H_k为t_k时刻式(9)中H阵的取值；X_k为t_k时刻的状态变量值；Z_k为t_k时刻Z的取值，为t_k时刻炮弹速度在i_b系的投影，I为单位矩阵。

6.根据权利要求5所述的一种基于GPS和最小二乘滤波的炮弹滚转角估计方法，其特征在于，步骤(4)中，根据通过最小二乘滤波估计X方法如下：

设取初值X₀＝[1 0]^T，系统的输入为与依次令k＝1,2,3...m，通过式(10)迭代X_k会逐渐趋于真值，P_k逐渐趋于零，将最终的X_k作为状态变量X。

7.根据权利要求6所述的一种基于GPS和最小二乘滤波的炮弹滚转角估计方法，其特征在于，步骤(4)中，在求解出X的基础上，根据式(8)计算出

8.根据权利要求7所述的一种基于GPS和最小二乘滤波的炮弹滚转角估计方法，其特征在于，步骤(5)中，t时刻炮弹滚转角的计算方法如下：由下式计算得到t时刻的n系相对于b系的姿态矩阵

矩阵为3阶方阵，其各元素记为：则炮弹t时刻的滚转角计算如下：