CN109211232A - 一种基于最小二乘滤波的炮弹姿态估计方法 - Google Patents

Abstract

本发明公开一种基于最小二乘滤波的炮弹姿态估计方法，该方法包括以下步骤：(1)根据陀螺仪和GPS提供的炮弹速度和位置，计算t时刻载体惯性系i_b相对于载体系b的炮弹姿态矩阵导航系n相对于导航惯性系i_n的炮弹姿态矩阵(2)计算t时刻炮弹比力在载体惯性系i_b和导航惯性系i_n系下的值和(3)由将的9个元素设为状态变量X，为i_n系相对于i_b系炮弹的姿态矩阵；(4)结合正交矩阵约束通过最小二乘滤波估计X并计算(5)根据得到并计算t时刻的炮弹姿态角，为t时刻的n系相对于b系炮弹的姿态矩阵。

Description

一种基于最小二乘滤波的炮弹姿态估计方法

技术领域

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

背景技术

递推最小二乘参数辨识，是指当被辨识系统在运行时，每取得一次新的观测数据后，就在前一次估计结果的基础上，利用新引入的观测数据对前次估计的结果，根据递推算法进行修正，从而递推地得出新的参数估计值，直到参数估计值达到满意的精度。对于辨识常值参数具有良好的效果。制导炮弹是指在传统炮弹基础上加装制导系统和可供驱动的弹翼或尾舱等空气动力装置，以提高炮弹对目标打击精度的一种低成本、小型化的精确制导武器。增程炮弹在空中易受风力等气象环境影响，弹体的姿态估算是后续导航系统工作的前提，也是当前的难点技术。弹体姿态探测的常用方法，目前包括采用地磁传感器、GPS、惯性系统以及组合的姿态探测方法；根据载体绕质心运动的方程,利用陀螺测量角速度信息对姿态角进行估计；Re‐quest方法等。

发明内容

发明目的：针对以上现有技术存在的问题，本发明提出一种基于最小二乘滤波的炮弹姿态估计方法，该方法目的在于仅利用弹体上安装的IMU和GPS提供的信息，通过最小二乘滤波算法解出最优的姿态估计。

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

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

(2)计算t时刻炮弹比力在载体惯性系i_b和导航惯性系i_n系下的值和

(3)由将的9个元素设为状态变量X，为i_n系相对于i_b系炮弹的姿态矩阵；

(4)结合正交矩阵约束通过最小二乘滤波估计X并计算

(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为采样周期，最终

由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为采样周期，则

其中，步骤(2)中，和计算法如下：通过式(2)计算得到的姿态矩阵将炮弹加速度计的测量值f^b(t)投影到i_b系中，得到

根据炮弹上GPS输出的炮弹在n系下速度vⁿ，对其进行分段曲线拟合，再对拟合曲线求导即得到炮弹在n系下的加速度aⁿ，再根据比力方程求出n系下比力fⁿ(t)：

其中，为地球自转角速度在n系的投影，为导航系相对于地球的转动角速度，其中，V_E,V_N分别为炮弹的东向、北向速度，gⁿ＝[0 0 g]^T，g为重力加速度，计算方法如下：

其中，步骤(3)中，由将的9个元素设为状态变量X，其方法如下：

记其中为炮弹比力在i_b系三轴的投影，为炮弹比力在i_n系三轴的投影；

为三阶方阵，记它的9个元素可表示为则有：

记X＝[a₁₁ a₁₂ a₁₃ a₂₁ a₂₂ a₂₃ a₃₁ a₃₂ a₃₃]^T，

O_1×3为1×3的零矩阵；

则式(8)改写为

由I为三阶单位矩阵，即

h(X)表示I关于X的函数，记h对X的导数为H₂，则有

量测矩阵：

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

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

其中，步骤(4)中，通过最小二乘滤波估计X并计算出方法如下：初值X₀＝[0 00 0 0 0 0 0 0]^T，P₀＝100·I_9×9，I_9×9为9阶单位矩阵，系统的输入为与 为t_k时刻炮弹比力在i_n系的投影，依次令k＝1,2,3...m，通过式(12)反复迭代X_k会逐渐趋于真值，P_k逐渐趋于零，将迭代最终的X_k作为辨识出状态变量X，由此计算出

其中，步骤(5)中，计算方法如下：

记X_k的各元素可表示为：

X_k＝[x₁ x₂ x₃ x₄ x₅ x₆ x₇ x₈ x₉]^T，

则

其中，步骤(5)中，t时刻炮弹姿态角计算方法如下：矩阵为3阶方阵，其中各元素可记为：则t时刻的炮弹的姿态角由下式解出：

其中，φ，θ，γ分别是炮弹的航向角，纵摇角，横滚角。

有益效果：与现有技术相比，本发明的技术方案具有以下有益技术效果：

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

2)将姿态矩阵进行链式分解，引入最小二乘滤波计算定值矩阵速度快且精度高。

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系的姿态矩阵。根据炮弹上陀螺仪输出和GPS提供的速度位置信息，可计算每时刻的计算t时刻炮弹比力在i_b和i_n系下的值和由将的9个元素设为状态变量，结合正交矩阵约束通过最小二乘滤波辨识出最后根据得到并解出姿态角，其中为3阶方阵。

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

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

初始时刻t₀时，b系与i_b系间姿态矩阵为I_3×3为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，...，m。

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

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

根据炮弹上GPS组件输出的炮弹在n系下速度vⁿ，对其进行分段曲线拟合，再对拟合曲线求导即得到炮弹在n系下的加速度aⁿ，再根据比力方程可求出n系下比力fⁿ(t)：

其中，为地球自转角速度在n系的投影，为导航系相对于地球的转动角速度，其中V_E,V_N分别为炮弹的东向、北向速度，gⁿ＝[0 0 g]^T，g为重力加速度。

根据式(4)，

求得

3、是3*3矩阵，将的9个元素设为状态变量X，结合正交矩阵约束由通过最小二乘滤波估计具体方法如下：

记其中为炮弹比力在i_b系三轴的投影，为炮弹比力在i_n系三轴的投影。

为三阶方阵，记它的9个元素可表示为则有：

记X＝[a₁₁ a₁₂ a₁₃ a₂₁ a₂₂ a₂₃ a₃₁ a₃₂ a₃₃]^T，

O_1×3为1×3的零矩阵。

则式(8)可改写为

另由于应为正交矩阵，则有I为三阶单位矩阵。即

h(X)表示I关于X的函数，记h对X的导数为H₂，则有：

量测矩阵

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

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

开始计算时k＝0,取初值X₀＝[0 0 0 0 0 0 0 0 0]^T，P₀＝100·I_9×9，I_9×9为9阶单位矩阵。系统的输入为与 为t_k时刻炮弹比力在i_n系的投影。依次令k＝1,2,3...m，通过式(12)反复迭代X_k会逐渐趋于真值，P_k逐渐趋于零，最终所有数据处理完成后终止解算，将X_k作为辨识出状态变量X。

4、炮弹的姿态角的计算如下：

根据最后一步迭代得到的X_k，可得到即如果X_k的各元素可表示为：X_k＝[x₁ x₂x₃ x₄ x₅ x₆ x₇ x₈ x₉]^T，则结合式(2)(4)，

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

则炮弹的姿态角由下式解出：

φ，θ，γ分别是炮弹的航向角，纵摇角，横滚角。

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

根据运动学定理及捷联惯导反演算法，使用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中各曲线表明，在仿真时间内，本发明设计的方法有效估计出姿态角，其中解算结束时航向角误差基本在±2°以内，纵摇角误差在±0.2°以内，横滚角误差在-4°左右。

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

Claims (8)

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

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

(2)计算t时刻炮弹比力在载体惯性系i_b和导航惯性系i_n系下的值和

(3)由将的9个元素设为状态变量X，为i_n系相对于i_b系炮弹的姿态矩阵；

(4)结合正交矩阵约束通过最小二乘滤波估计X并计算

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

2.根据权利要求1所述的一种基于最小二乘滤波的炮弹姿态估计方法，其特征在于，步骤(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。

3.根据权利要求2所述的一种基于最小二乘滤波的炮弹姿态估计方法，其特征在于，步骤(2)中，和计算法如下：通过式(2)计算得到的姿态矩阵将炮弹加速度计的测量值f^b(t)投影到i_b系中，得到

根据炮弹上GPS输出的炮弹在n系下速度vⁿ，对其进行分段曲线拟合，再对拟合曲线求导即得到炮弹在n系下的加速度aⁿ，再根据比力方程求出n系下比力fⁿ(t)：

其中，为地球自转角速度在n系的投影，为导航系相对于地球的转动角速度，其中，V_E,V_N分别为炮弹的东向、北向速度，gⁿ＝[0 0 g]^T，g为重力加速度，计算方法如下：

4.根据权利要求1所述的一种基于最小二乘滤波的炮弹姿态估计方法，其特征在于，步骤(3)中，由将的9个元素设为状态变量X，其方法如下：

记其中，为炮弹比力在i_b系三轴的投影，为炮弹比力在i_n系三轴的投影；

为三阶方阵，记它的9个元素表示为则有：

记X＝[a₁₁ a₁₂ a₁₃ a₂₁ a₂₂ a₂₃ a₃₁ a₃₂ a₃₃]^T，

O_1×3为1×3的零矩阵；

则式(8)改写为

由I为三阶单位矩阵，即

h(X)表示I关于X的函数，记h对X的导数为H₂，则有

量测矩阵：

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

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

6.根据权利要求5所述的一种基于最小二乘滤波的炮弹姿态估计方法，其特征在于，步骤(4)中，通过最小二乘滤波估计X并计算出方法如下：初值X₀＝[0 0 0 0 0 0 0 0 0]^T，P₀＝100·I_9×9，I_9×9为9阶单位矩阵，系统的输入为与 为t_k时刻炮弹比力在i_n系的投影，依次令k＝1,2,3...m，通过式(12)反复迭代X_k会逐渐趋于真值，P_k逐渐趋于零，将迭代最终的X_k作为辨识出状态变量X，由此计算出

7.根据权利要求6所述的一种基于最小二乘滤波的炮弹姿态估计方法，其特征在于，步骤(5)中，计算方法如下：

记X_k的各元素可表示为：

X_k＝[x₁ x₂ x₃ x₄ x₅ x₆ x₇ x₈ x₉]^T，

则

8.根据权利要求7所述的一种基于最小二乘滤波的炮弹姿态估计方法，其特征在于，步骤(5)中，t时刻炮弹姿态角计算方法如下：矩阵为3阶方阵，其中各元素可记为：则t时刻的炮弹的姿态角由下式解出：

其中，φ，θ，γ分别是炮弹的航向角，纵摇角，横滚角。