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

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

Info

Publication number
CN109211232B
CN109211232B CN201811047751.3A CN201811047751A CN109211232B CN 109211232 B CN109211232 B CN 109211232B CN 201811047751 A CN201811047751 A CN 201811047751A CN 109211232 B CN109211232 B CN 109211232B
Authority
CN
China
Prior art keywords
matrix
cannonball
time
attitude
relative
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN201811047751.3A
Other languages
English (en)
Other versions
CN109211232A (zh
Inventor
刘锡祥
杨文强
许广富
马晓爽
郭小乐
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Southeast University
Original Assignee
Southeast University
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Southeast University filed Critical Southeast University
Priority to CN201811047751.3A priority Critical patent/CN109211232B/zh
Publication of CN109211232A publication Critical patent/CN109211232A/zh
Application granted granted Critical
Publication of CN109211232B publication Critical patent/CN109211232B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/42—Determining position
    • G01S19/45—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement
    • G01S19/47—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement the supplementary measurement being an inertial measurement, e.g. tightly coupled inertial

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Automation & Control Theory (AREA)
  • Navigation (AREA)

Abstract

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

Description

一种基于最小二乘滤波的炮弹姿态估计方法
技术领域
本发明属于导航技术领域，尤其涉及一种基于最小二乘滤波的炮弹姿态估计方法。
背景技术
递推最小二乘参数辨识，是指当被辨识系统在运行时，每取得一次新的观测数据后，就在前一次估计结果的基础上，利用新引入的观测数据对前次估计的结果，根据递推算法进行修正，从而递推地得出新的参数估计值，直到参数估计值达到满意的精度。对于辨识常值参数具有良好的效果。制导炮弹是指在传统炮弹基础上加装制导系统和可供驱动的弹翼或尾舱等空气动力装置，以提高炮弹对目标打击精度的一种低成本、小型化的精确制导武器。增程炮弹在空中易受风力等气象环境影响，弹体的姿态估算是后续导航系统工作的前提，也是当前的难点技术。弹体姿态探测的常用方法，目前包括采用地磁传感器、GPS、惯性系统以及组合的姿态探测方法；根据载体绕质心运动的方程,利用陀螺测量角速度信息对姿态角进行估计；Re‐quest方法等。
发明内容
发明目的：针对以上现有技术存在的问题，本发明提出一种基于最小二乘滤波的炮弹姿态估计方法，该方法目的在于仅利用弹体上安装的IMU和GPS提供的信息，通过最小二乘滤波算法解出最优的姿态估计。
技术方案：为实现本发明的目的，本发明所采用的技术方案是：一种基于最小二乘滤波的炮弹姿态估计方法，该方法包括以下步骤：
(1)根据陀螺仪和GPS提供的炮弹速度和位置，计算t时刻载体惯性系ib相对于载体系b的炮弹姿态矩阵
Figure BDA0001792104630000011
导航系n相对于导航惯性系in的炮弹姿态矩阵
Figure BDA0001792104630000012
(2)计算t时刻炮弹比力在载体惯性系ib和导航惯性系in系下的值
Figure BDA0001792104630000013
和
Figure BDA0001792104630000014
(3)由
Figure BDA0001792104630000015
将
Figure BDA0001792104630000016
的9个元素设为状态变量X，
Figure BDA0001792104630000017
为in系相对于ib系炮弹的姿态矩阵；
(4)结合正交矩阵约束
Figure BDA0001792104630000021
通过最小二乘滤波估计X并计算
Figure BDA0001792104630000022
(5)根据
Figure BDA0001792104630000023
得到
Figure BDA0001792104630000024
并计算t时刻的炮弹姿态角，
Figure BDA0001792104630000025
为t时刻的n系相对于b系炮弹的姿态矩阵。
其中，步骤(1)中，姿态矩阵
Figure BDA0001792104630000026
和
Figure BDA0001792104630000027
计算方法如下：
设初始时刻t0时，b系与ib系间姿态矩阵为
Figure BDA0001792104630000028
I为3×3的单位矩阵；
记t时刻陀螺仪的输出值为
Figure BDA0001792104630000029
即t时刻的b系相对于ib系的角速度在b系上的投影值，由此跟踪b系相对于ib系的变化：
Figure BDA00017921046300000210
其中，
Figure BDA00017921046300000211
为矩阵
Figure BDA00017921046300000212
的变化率，
Figure BDA00017921046300000213
“×”表示三维矢量对应的叉乘矩阵变换，设
Figure BDA00017921046300000214
其中a，b，c分别表示炮弹沿三轴的旋转角速度，则
Figure BDA00017921046300000215
采用毕卡法解式(1)微分方程可得到式(2)：
Figure BDA00017921046300000216
记待解算数据时长为T，将时间段0～T以采样周期dt为间隔划分为多个时刻点t0,t1,t2...tm，k＝0,1,2,...,m，则式(2)中
Figure BDA00017921046300000217
为tk时刻的b系相对于tk-1时刻的b系的姿态矩阵；
Figure BDA00017921046300000218
为tk-1时刻陀螺仪输出，dt为采样周期，最终
Figure BDA00017921046300000219
Figure BDA00017921046300000221
由GPS输出的炮弹位置信息纬度L，东向、北向、天向速度分别为VE,VN,VU，则n系相对in系的角速度
Figure BDA00017921046300000220
可计算如下：
Figure BDA0001792104630000031
其中，RN为地球子午圈曲率半径，ωie为地球自转角速度，RE为地球卯酉圈半径，根据公式(2)的计算方法，由
Figure BDA0001792104630000032
可计算出
Figure BDA0001792104630000033
Figure BDA0001792104630000034
其中，
Figure BDA0001792104630000035
为tk时刻的n系相对于tk-1时刻的n系的姿态矩阵；
Figure BDA00017921046300000321
Figure BDA00017921046300000322
为t＝tk-1时
Figure BDA0001792104630000036
的值，dt为采样周期，则
Figure BDA0001792104630000037
其中，步骤(2)中，
Figure BDA0001792104630000038
和
Figure BDA0001792104630000039
计算法如下：通过式(2)计算得到的姿态矩阵
Figure BDA00017921046300000310
将炮弹加速度计的测量值fb(t)投影到ib系中，得到
Figure BDA00017921046300000311
Figure BDA00017921046300000312
根据炮弹上GPS输出的炮弹在n系下速度vn，对其进行分段曲线拟合，再对拟合曲线求导即得到炮弹在n系下的加速度an，再根据比力方程求出n系下比力fn(t)：
Figure BDA00017921046300000313
其中，
Figure BDA00017921046300000314
为地球自转角速度在n系的投影，
Figure BDA00017921046300000315
为导航系相对于地球的转动角速度，
Figure BDA00017921046300000316
其中，VE,VN分别为炮弹的东向、北向速度，gn＝[0 0 g]T，g为重力加速度，
Figure BDA00017921046300000317
计算方法如下：
Figure BDA00017921046300000318
其中，步骤(3)中，由
Figure BDA00017921046300000319
将
Figure BDA00017921046300000320
的9个元素设为状态变量X，其方法如下：
记
Figure BDA0001792104630000041
其中
Figure BDA0001792104630000042
为炮弹比力在ib系三轴的投影，
Figure BDA0001792104630000043
为炮弹比力在in系三轴的投影；
Figure BDA0001792104630000044
为三阶方阵，记它的9个元素可表示为
Figure BDA0001792104630000045
则有：
Figure BDA0001792104630000046
记X＝[a11 a12 a13 a21 a22 a23 a31 a32 a33]T，
Figure BDA0001792104630000047
O1×3为1×3的零矩阵；
则式(8)改写为
Figure BDA0001792104630000048
由
Figure BDA0001792104630000049
I为三阶单位矩阵，即
Figure BDA00017921046300000410
h(X)表示I关于X的函数，记h对X的导数为H2，则有
Figure BDA00017921046300000411
量测矩阵：
Figure BDA00017921046300000412
其中，步骤(4)中，选择基于递推最小二乘算法作为在线辨识滤波器，构建如下：
Figure BDA0001792104630000051
式中，Kk为tk时刻的增益矩阵；Pk为tk时刻的状态向量协方差阵；Rk为tk时刻的量测噪声阵；Hk为tk时刻式(11)中H阵的取值；Xk为tk时刻的状态变量值；
Figure BDA0001792104630000052
Figure BDA00017921046300000512
为tk时刻比力在ib系的投影，I为单位矩阵。
其中，步骤(4)中，通过最小二乘滤波估计X并计算出
Figure BDA0001792104630000053
方法如下：初值X0＝[0 00 0 0 0 0 0 0]T，P0＝100·I9×9，I9×9为9阶单位矩阵，系统的输入为
Figure BDA0001792104630000054
与
Figure BDA0001792104630000055
Figure BDA00017921046300000513
为tk时刻炮弹比力在in系的投影，依次令k＝1,2,3...m，通过式(12)反复迭代Xk会逐渐趋于真值，Pk逐渐趋于零，将迭代最终的Xk作为辨识出状态变量X，由此计算出
Figure BDA0001792104630000056
其中，步骤(5)中，
Figure BDA0001792104630000057
计算方法如下：
记Xk的各元素可表示为：
Xk＝[x1 x2 x3 x4 x5 x6 x7 x8 x9]T，
则
Figure BDA0001792104630000058
Figure BDA0001792104630000059
其中，步骤(5)中，t时刻炮弹姿态角计算方法如下：矩阵
Figure BDA00017921046300000510
为3阶方阵，其中各元素可记为：
Figure BDA00017921046300000511
则t时刻的炮弹的姿态角由下式解出：
Figure BDA0001792104630000061
其中，φ，θ，γ分别是炮弹的航向角，纵摇角，横滚角。
有益效果：与现有技术相比，本发明的技术方案具有以下有益技术效果：
1)弹体在空中估计时，只需要IMU和GPS提供的信息，无需多余传感器；
2)将姿态矩阵进行链式分解，引入最小二乘滤波计算定值矩阵
Figure BDA0001792104630000062
速度快且精度高。
3)仿真结果表明本方案可适用于高动态的飞行环境下。
附图说明
图1为本发明姿态角误差估计误差图。
具体实施方式
下面对本发明技术方案进行详细说明，但是本发明的保护范围不局限于所述实施例。
本发明适用于炮弹飞行估计。首先定义如下坐标系：
导航系n：原点为炮弹所在位置处，Y轴指向地理北向，X轴指向地理东向，Z轴垂直于大地水平面指向上。
载体系b：原点为弹体质心，Y轴沿弹体前进方向向前，X轴指向右，Z轴指向上。
导航惯性系in：初始时刻的导航系n凝固在惯性空间所得，不随时间变化。
载体惯性系ib：初始时刻的载体系b凝固在惯性空间所得，不随时间变化。
定义如上坐标系后，t时刻的n系相对于b系的姿态矩阵
Figure BDA0001792104630000063
可分解为
Figure BDA0001792104630000064
其中，
Figure BDA0001792104630000065
为t时刻ib系相对于b系的姿态矩阵；
Figure BDA0001792104630000066
为t时刻n系相对于in系的姿态矩阵；
Figure BDA0001792104630000067
为in系相对于ib系的姿态矩阵。根据炮弹上陀螺仪输出和GPS提供的速度位置信息，可计算每时刻的
Figure BDA0001792104630000068
计算t时刻炮弹比力在ib和in系下的值
Figure BDA0001792104630000069
和
Figure BDA00017921046300000610
由
Figure BDA00017921046300000611
将
Figure BDA00017921046300000612
的9个元素设为状态变量，结合正交矩阵约束
Figure BDA0001792104630000071
通过最小二乘滤波辨识出
Figure BDA0001792104630000072
最后根据
Figure BDA0001792104630000073
得到
Figure BDA0001792104630000074
并解出姿态角，其中
Figure BDA0001792104630000075
为3阶方阵。
下面结合附图对本发明实施方法做更详细地描述：
1、由炮弹的陀螺仪和GPS输出计算姿态矩阵
Figure BDA0001792104630000076
具体包括如下步骤：
初始时刻t0时，b系与ib系间姿态矩阵为
Figure BDA0001792104630000077
I3×3为3×3的单位矩阵；
记t时刻陀螺仪的输出值为
Figure BDA0001792104630000078
即t时刻的b系相对于ib系的角速度在b系上的投影值，由此可跟踪b系相对于ib系的变化：
Figure BDA0001792104630000079
其中，
Figure BDA00017921046300000710
为矩阵
Figure BDA00017921046300000711
的变化率，
Figure BDA00017921046300000712
“×”表示三维矢量对应的叉乘矩阵变换，即如果
Figure BDA00017921046300000713
其中a，b，c分别表示炮弹沿三轴的旋转角速度，则
Figure BDA00017921046300000714
采用毕卡法解式(1)微分方程可得到式(2)：
Figure BDA00017921046300000715
记待解算数据时长为T，将时间段0～T以采样周期dt为间隔划分为多个时刻点t0,t1,t2...tm，k＝0,1,2,...,m。则式(2)中
Figure BDA00017921046300000716
为tk时刻的b系相对于tk-1时刻的b系的姿态矩阵；
Figure BDA00017921046300000717
为tk-1时刻陀螺仪输出，dt为采样周期，最终
Figure BDA00017921046300000718
计算时t＝tk，k＝1,2,3,...,m，
Figure BDA00017921046300000719
根据炮弹上搭载的GPS组件可得到弹体位置信息经度λ与纬度L，东向、北向、天向速度分别为VE,VN,VU。则n系相对in系的角速度
Figure BDA00017921046300000720
可计算如下：
Figure BDA0001792104630000081
其中，RN为地球子午圈曲率半径，ωie为地球自转角速度，RE为地球卯酉圈半径。参照公式(2)的计算方法，由
Figure BDA0001792104630000082
可计算出
Figure BDA0001792104630000083
Figure BDA0001792104630000084
其中，
Figure BDA0001792104630000085
为tk时刻的n系相对于tk-1时刻的n系的姿态矩阵；
Figure BDA0001792104630000086
Figure BDA00017921046300000818
为t＝tk-1时
Figure BDA0001792104630000087
的值，dt为采样周期。最终
Figure BDA0001792104630000088
计算时t＝tk，k＝1，2，...，m。
2、炮弹比力在ib和in系下的值
Figure BDA0001792104630000089
和
Figure BDA00017921046300000810
的求法如下：
通过式(2)计算得到的姿态矩阵
Figure BDA00017921046300000811
将炮弹加速度计的测量值fb(t)投影到ib系中，得到
Figure BDA00017921046300000812
Figure BDA00017921046300000813
根据炮弹上GPS组件输出的炮弹在n系下速度vn，对其进行分段曲线拟合，再对拟合曲线求导即得到炮弹在n系下的加速度an，再根据比力方程可求出n系下比力fn(t)：
Figure BDA00017921046300000814
其中，
Figure BDA00017921046300000815
为地球自转角速度在n系的投影，
Figure BDA00017921046300000816
为导航系相对于地球的转动角速度，
Figure BDA00017921046300000817
其中VE,VN分别为炮弹的东向、北向速度，gn＝[0 0 g]T，g为重力加速度。
根据式(4)，
Figure BDA0001792104630000091
求得
Figure BDA0001792104630000092
3、
Figure BDA0001792104630000093
是3*3矩阵，将
Figure BDA0001792104630000094
的9个元素设为状态变量X，结合正交矩阵约束
Figure BDA0001792104630000095
由
Figure BDA0001792104630000096
通过最小二乘滤波估计
Figure BDA0001792104630000097
具体方法如下：
记
Figure BDA0001792104630000098
其中
Figure BDA0001792104630000099
为炮弹比力在ib系三轴的投影，
Figure BDA00017921046300000910
为炮弹比力在in系三轴的投影。
Figure BDA00017921046300000911
为三阶方阵，记它的9个元素可表示为
Figure BDA00017921046300000912
则有：
Figure BDA00017921046300000913
记X＝[a11 a12 a13 a21 a22 a23 a31 a32 a33]T，
Figure BDA00017921046300000914
O1×3为1×3的零矩阵。
则式(8)可改写为
Figure BDA00017921046300000915
另由于
Figure BDA00017921046300000916
应为正交矩阵，则有
Figure BDA00017921046300000917
I为三阶单位矩阵。即
Figure BDA00017921046300000918
h(X)表示I关于X的函数，记h对X的导数为H2，则有：
Figure BDA0001792104630000101
量测矩阵
Figure BDA0001792104630000102
选择基于递推最小二乘算法作为在线辨识滤波器，构建如下：
Figure BDA0001792104630000103
式中，Kk为tk时刻的增益矩阵；Pk为tk时刻的状态向量协方差阵；Rk为tk时刻的量测噪声阵；Hk为tk时刻式(11)中H阵的取值；Xk为tk时刻的状态变量值；
Figure BDA0001792104630000104
Figure BDA0001792104630000109
为tk时刻比力在ib系的投影，I为单位矩阵。
开始计算时k＝0,取初值X0＝[0 0 0 0 0 0 0 0 0]T，P0＝100·I9×9，I9×9为9阶单位矩阵。系统的输入为
Figure BDA0001792104630000105
与
Figure BDA0001792104630000106
Figure BDA00017921046300001010
为tk时刻炮弹比力在in系的投影。依次令k＝1,2,3...m，通过式(12)反复迭代Xk会逐渐趋于真值，Pk逐渐趋于零，最终所有数据处理完成后终止解算，将Xk作为辨识出状态变量X。
4、炮弹的姿态角的计算如下：
根据最后一步迭代得到的Xk，可得到
Figure BDA0001792104630000107
即如果Xk的各元素可表示为：Xk＝[x1 x2x3 x4 x5 x6 x7 x8 x9]T，则
Figure BDA0001792104630000108
结合式(2)(4)，
Figure BDA0001792104630000111
矩阵
Figure BDA0001792104630000112
为3阶方阵，其中各元素可记为：
Figure BDA0001792104630000113
则炮弹的姿态角由下式解出：
Figure BDA0001792104630000114
φ，θ，γ分别是炮弹的航向角，纵摇角，横滚角。
本发明的有益效果通过如下仿真得以验证：
根据运动学定理及捷联惯导反演算法，使用Matlab模拟产生相关导航参数，并在其上叠加相应的仪表误差作为仪表实际采集数据，IMU采样周期0.005s，GPS采样周期0.1s。部分仿真参数如下：
初始位置：东经108.97°、北纬34.25°；
赤道半径：6378165m；
地球椭球度：1/298.3；
地球表面重力加速度：9.8m/s2
地球自转角速度：15.04088°/h
wx滚转陀螺零偏(0.15rad/s)
wy偏航陀螺零偏(0.03rad/s)
wz俯仰陀螺零偏(0.03rad/s)
fx加速度计零偏(0.003m/s2)
fy加速度计零偏(0.003m/s2)
fz加速度计零偏(0.003m/s2)
wx滚转陀螺测量噪声(0.15rad/s)
wy偏航陀螺测量噪声(0.01rad/s)
wz俯仰陀螺测量噪声(0.01rad/s)
fx加速度计测量噪声(0.003m/s2)
fy加速度计测量噪声(0.003m/s2)
fz加速度计测量噪声(0.003m/s2)
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 (7)

1.一种基于最小二乘滤波的炮弹姿态估计方法，其特征在于，该方法包括以下步骤：
(1)根据陀螺仪和GPS提供的炮弹速度和位置，计算t时刻载体惯性系ib相对于载体系b的炮弹姿态矩阵
Figure FDA0003103423410000011
导航系n相对于导航惯性系in的炮弹姿态矩阵
Figure FDA0003103423410000012
(2)计算t时刻炮弹比力在载体惯性系ib和导航惯性系in系下的值
Figure FDA0003103423410000013
和
Figure FDA0003103423410000014
(3)由
Figure FDA0003103423410000015
将
Figure FDA0003103423410000016
的9个元素设为状态变量X，
Figure FDA0003103423410000017
为in系相对于ib系炮弹的姿态矩阵；
(4)结合正交矩阵约束
Figure FDA0003103423410000018
通过最小二乘滤波估计X并计算
Figure FDA0003103423410000019
(5)根据
Figure FDA00031034234100000110
得到
Figure FDA00031034234100000111
并计算t时刻的炮弹姿态角，
Figure FDA00031034234100000112
为t时刻的n系相对于b系炮弹的姿态矩阵；
步骤(1)中，姿态矩阵
Figure FDA00031034234100000113
和
Figure FDA00031034234100000114
计算方法如下：
设初始时刻t0时，b系与ib系间姿态矩阵为
Figure FDA00031034234100000115
I为3×3的单位矩阵；
记t时刻陀螺仪的输出值为
Figure FDA00031034234100000116
即t时刻的b系相对于ib系的角速度在b系上的投影值，由此跟踪b系相对于ib系的变化：
Figure FDA00031034234100000117
其中，
Figure FDA00031034234100000118
为矩阵
Figure FDA00031034234100000119
的变化率，
Figure FDA00031034234100000120
“×”表示三维矢量对应的叉乘矩阵变换，设
Figure FDA00031034234100000121
其中a，b，c分别表示炮弹沿三轴的旋转角速度，则
Figure FDA00031034234100000122
采用毕卡法解式(1)微分方程可得到式(2)：
Figure FDA00031034234100000123
记待解算数据时长为T，将时间段0～T以采样周期dt为间隔划分为多个时刻点t0,t1,t2...tm，k＝0,1,2,...,m，则式(2)中
Figure FDA00031034234100000124
为tk时刻的b系相对于tk-1时刻的b系的姿态矩阵；
Figure FDA00031034234100000125
Figure FDA00031034234100000126
为tk-1时刻陀螺仪输出，dt为采样周期，最终
Figure FDA00031034234100000127
由GPS输出的炮弹位置信息纬度L，东向、北向、天向速度分别为VE,VN,VU，则n系相对in系的角速度
Figure FDA0003103423410000021
可计算如下：
Figure FDA0003103423410000022
其中，RN为地球子午圈曲率半径，ωie为地球自转角速度，RE为地球卯酉圈半径，根据公式(2)的计算方法，由
Figure FDA0003103423410000023
可计算出
Figure FDA0003103423410000024
Figure FDA0003103423410000025
其中，
Figure FDA0003103423410000026
为tk时刻的n系相对于tk-1时刻的n系的姿态矩阵；
Figure FDA0003103423410000027
Figure FDA0003103423410000028
为t＝tk-1时
Figure FDA0003103423410000029
的值，dt为采样周期，则
Figure FDA00031034234100000210
k＝12,3,...,m。
2.根据权利要求1所述的一种基于最小二乘滤波的炮弹姿态估计方法，其特征在于，步骤(2)中，
Figure FDA00031034234100000211
和
Figure FDA00031034234100000212
计算法如下：通过式(2)计算得到的姿态矩阵
Figure FDA00031034234100000213
将炮弹加速度计的测量值fb(t)投影到ib系中，得到
Figure FDA00031034234100000214
Figure FDA00031034234100000215
根据炮弹上GPS输出的炮弹在n系下速度vn，对其进行分段曲线拟合，再对拟合曲线求导即得到炮弹在n系下的加速度an，再根据比力方程求出n系下比力fn(t)：
Figure FDA00031034234100000216
其中，
Figure FDA00031034234100000217
为地球自转角速度在n系的投影，
Figure FDA00031034234100000218
为导航系相对于地球的转动角速度，
Figure FDA00031034234100000219
其中，VE,VN分别为炮弹的东向、北向速度，gn＝[0 0 g]T，g为重力加速度，
Figure FDA00031034234100000220
计算方法如下：
Figure FDA0003103423410000031
3.根据权利要求1所述的一种基于最小二乘滤波的炮弹姿态估计方法，其特征在于，步骤(3)中，由
Figure FDA0003103423410000032
将
Figure FDA0003103423410000033
的9个元素设为状态变量X，其方法如下：
记
Figure FDA0003103423410000034
其中，
Figure FDA0003103423410000035
为炮弹比力在ib系三轴的投影，
Figure FDA0003103423410000036
为炮弹比力在in系三轴的投影；
Figure FDA0003103423410000037
为三阶方阵，记它的9个元素表示为
Figure FDA0003103423410000038
则有：
Figure FDA0003103423410000039
记X＝[a11 a12 a13 a21 a22 a23 a31 a32 a33]T，
Figure FDA00031034234100000310
O1×3为1×3的零矩阵；
则式(8)改写为
Figure FDA00031034234100000311
由
Figure FDA00031034234100000312
I为三阶单位矩阵，即
Figure FDA00031034234100000313
h(X)表示I关于X的函数，记h对X的导数为H2，则有
Figure FDA0003103423410000041
量测矩阵：
Figure FDA0003103423410000042
4.根据权利要求3所述的一种基于最小二乘滤波的炮弹姿态估计方法，其特征在于，步骤(4)中，选择基于递推最小二乘算法作为在线辨识滤波器，构建如下：
Figure FDA0003103423410000043
式中，Kk为tk时刻的增益矩阵；Pk为tk时刻的状态向量协方差阵；Rk为tk时刻的量测噪声阵；Hk为tk时刻式(11)中H阵的取值；Xk为tk时刻的状态变量值；
Figure FDA0003103423410000044
Figure FDA0003103423410000045
为tk时刻比力在ib系的投影，I为单位矩阵。
5.根据权利要求4所述的一种基于最小二乘滤波的炮弹姿态估计方法，其特征在于，步骤(4)中，通过最小二乘滤波估计X并计算出
Figure FDA0003103423410000046
方法如下：初值X0＝[0 0 0 0 0 0 0 0 0]T，P0＝100·I9×9，I9×9为9阶单位矩阵，系统的输入为
Figure FDA0003103423410000047
与
Figure FDA0003103423410000048
Figure FDA0003103423410000049
为tk时刻炮弹比力在in系的投影，依次令k＝1,2,3...m，通过式(12)反复迭代Xk会逐渐趋于真值，Pk逐渐趋于零，将迭代最终的Xk作为辨识出状态变量X，由此计算出
Figure FDA00031034234100000410
6.根据权利要求5所述的一种基于最小二乘滤波的炮弹姿态估计方法，其特征在于，步骤(5)中，
Figure FDA00031034234100000411
计算方法如下：
记Xk的各元素可表示为：
Xk＝[x1 x2 x3 x4 x5 x6 x7 x8 x9]T，
则
Figure FDA0003103423410000051
Figure FDA0003103423410000052
7.根据权利要求6所述的一种基于最小二乘滤波的炮弹姿态估计方法，其特征在于，步骤(5)中，t时刻炮弹姿态角计算方法如下：矩阵
Figure FDA0003103423410000053
为3阶方阵，其中各元素可记为：
Figure FDA0003103423410000054
则t时刻的炮弹的姿态角由下式解出：
Figure FDA0003103423410000055
其中，φ，θ，γ分别是炮弹的航向角，纵摇角，横滚角。
CN201811047751.3A 2018-09-07 2018-09-07 一种基于最小二乘滤波的炮弹姿态估计方法 Active CN109211232B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201811047751.3A CN109211232B (zh) 2018-09-07 2018-09-07 一种基于最小二乘滤波的炮弹姿态估计方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201811047751.3A CN109211232B (zh) 2018-09-07 2018-09-07 一种基于最小二乘滤波的炮弹姿态估计方法

Publications (2)

Publication Number Publication Date
CN109211232A CN109211232A (zh) 2019-01-15
CN109211232B true CN109211232B (zh) 2021-07-27

Family

ID=64987283

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201811047751.3A Active CN109211232B (zh) 2018-09-07 2018-09-07 一种基于最小二乘滤波的炮弹姿态估计方法

Country Status (1)

Country Link
CN (1) CN109211232B (zh)

Families Citing this family (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110514200B (zh) * 2019-08-13 2023-03-14 中国航空工业集团公司西安飞行自动控制研究所 一种惯性导航系统及高转速旋转体姿态测量方法
CN114353784B (zh) * 2022-03-17 2022-06-07 西北工业大学 一种基于运动矢量的制导炮弹空中姿态辨识方法
CN116150552A (zh) * 2023-02-20 2023-05-23 北京自动化控制设备研究所 一种制导炮弹初始姿态计算方法

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US4754280A (en) * 1982-09-10 1988-06-28 The Charles Stark Draper Laboratory, Inc. Attitude sensing system
CN104457446A (zh) * 2014-11-28 2015-03-25 北京航天控制仪器研究所 一种自旋制导炮弹的空中自对准方法
CN105115508A (zh) * 2015-08-27 2015-12-02 北京航天控制仪器研究所 基于后数据的旋转制导炮弹快速空中对准方法
CN105180728A (zh) * 2015-08-27 2015-12-23 北京航天控制仪器研究所 基于前数据的旋转制导炮弹快速空中对准方法
CN105241319A (zh) * 2015-08-27 2016-01-13 北京航天控制仪器研究所 一种高速自旋制导炮弹空中实时对准方法

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US4754280A (en) * 1982-09-10 1988-06-28 The Charles Stark Draper Laboratory, Inc. Attitude sensing system
CN104457446A (zh) * 2014-11-28 2015-03-25 北京航天控制仪器研究所 一种自旋制导炮弹的空中自对准方法
CN105115508A (zh) * 2015-08-27 2015-12-02 北京航天控制仪器研究所 基于后数据的旋转制导炮弹快速空中对准方法
CN105180728A (zh) * 2015-08-27 2015-12-23 北京航天控制仪器研究所 基于前数据的旋转制导炮弹快速空中对准方法
CN105241319A (zh) * 2015-08-27 2016-01-13 北京航天控制仪器研究所 一种高速自旋制导炮弹空中实时对准方法

Also Published As

Publication number Publication date
CN109211232A (zh) 2019-01-15

Similar Documents

Publication Publication Date Title
CN110207697B (zh) 基于角加速度计/陀螺/加速度计的惯性导航解算方法
CN109059914B (zh) 一种基于gps和最小二乘滤波的炮弹滚转角估计方法
CN109813311B (zh) 一种无人机编队协同导航方法
CN109211230B (zh) 一种基于牛顿迭代法的炮弹姿态和加速度计常值误差估计方法
CN106643737B (zh) 风力干扰环境下四旋翼飞行器姿态解算方法
CN109211231B (zh) 一种基于牛顿迭代法的炮弹姿态估计方法
CN106990426B (zh) 一种导航方法和导航装置
CN109211232B (zh) 一种基于最小二乘滤波的炮弹姿态估计方法
CN105486307B (zh) 针对机动目标的视线角速率估计方法
CN105180728B (zh) 基于前数据的旋转制导炮弹快速空中对准方法
CN104713555A (zh) 应用全天域中性点辅助定向的车辆自主导航方法
CN105737823A (zh) 基于五阶ckf的gps/sins/cns组合导航方法
CN104697526A (zh) 用于农业机械的捷联惯导系统以及控制方法
CN113340298B (zh) 一种惯导和双天线gnss外参标定方法
JP2007232443A (ja) 慣性航法装置およびその誤差補正方法
CN104764467A (zh) 空天飞行器惯性传感器误差在线自适应标定方法
CN105910606A (zh) 一种基于角速度差值的方向修正方法
CN113847913A (zh) 一种基于弹道模型约束的弹载组合导航方法
CN105929836A (zh) 用于四旋翼飞行器的控制方法
CN107101636A (zh) 一种使用卡尔曼滤波器辨识多旋翼动力学模型参数的方法
CN105115508A (zh) 基于后数据的旋转制导炮弹快速空中对准方法
CN104296780B (zh) 一种基于重力视运动的sins自对准与纬度计算方法
CN108759814B (zh) 一种四旋翼飞行器横滚轴角速度和俯仰轴角速度估计方法
CN105241319B (zh) 一种高速自旋制导炮弹空中实时对准方法
CN108871319A (zh) 一种基于地球重力场与地磁场序贯修正的姿态解算方法

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant