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

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

Info

Publication number
CN109059914B
CN109059914B CN201811047724.6A CN201811047724A CN109059914B CN 109059914 B CN109059914 B CN 109059914B CN 201811047724 A CN201811047724 A CN 201811047724A CN 109059914 B CN109059914 B CN 109059914B
Authority
CN
China
Prior art keywords
time
projectile
gps
matrix
velocity
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
CN201811047724.6A
Other languages
English (en)
Other versions
CN109059914A (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 CN201811047724.6A priority Critical patent/CN109059914B/zh
Publication of CN109059914A publication Critical patent/CN109059914A/zh
Application granted granted Critical
Publication of CN109059914B publication Critical patent/CN109059914B/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
    • 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/20—Instruments for performing navigational calculations
    • 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)
  • Automation & Control Theory (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Navigation (AREA)
  • Position Fixing By Use Of Radio Waves (AREA)

Abstract

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

Description

一种基于GPS和最小二乘滤波的炮弹滚转角估计方法
技术领域
本发明属于导航技术领域，尤其涉及一种基于GPS和最小二乘滤波的炮弹滚转角估计方法。
背景技术
制导炮弹是一种低成本、小型化的精确制导武器，一般装配有GPS/INS组合。在实现弹药的精确控制飞行技术中，滚转角的实时准确测量是实施制导或修正控制的基础，也是制导弹药实现远程精确飞行的关键，由于制导弹药发射过程中，通常会承受高过载、高转速的恶劣情况，在此高过载冲击环境下IMU无法正常上电工作，导航系统的初始化需要发射后在空中自主完成。且空中易受风力等气象环境影响，滚转角的估计不易精确求得，故修正制导炮弹滚转角是目前国内外研究的热点问题。目前对滚转姿态测量主要包括惯导系统，地磁方法，GPS等。
发明内容
发明目的：针对以上问题，本发明提出一种基于GPS和最小二乘滤波的炮弹滚转角估计方法，该方法的目的在于仅利用陀螺仪，加速度计和GPS提供的信息，通过最小二乘滤波算法解出最优的滚转角。
技术方案：为实现本发明的目的，本发明所采用的技术方案是：一种基于GPS和最小二乘滤波的炮弹滚转角估计方法，该方法包括以下步骤：
(1)根据陀螺仪和GPS提供的炮弹速度和位置，计算t时刻载体惯性系ib系相对于载体系b的炮弹姿态矩阵
Figure BDA0001792104480000011
导航系n相对于导航惯性系in的炮弹姿态矩阵
Figure BDA0001792104480000012
(2)计算炮弹在载体惯性系ib和导航惯性系in系下的速度值
Figure BDA0001792104480000013
和
Figure BDA0001792104480000014
(3)由GPS提供的速度比值计算初始航向角Y0与纵摇角P0，根据初始滚转角设置状态变量X；
(4)根据
Figure BDA0001792104480000015
通过最小二乘滤波估计X并计算
Figure BDA0001792104480000016
为in系相对于ib系的炮弹姿态矩阵；
(5)根据
Figure BDA0001792104480000021
得到t时的
Figure BDA0001792104480000022
并计算炮弹滚转角，
Figure BDA0001792104480000023
为t时刻n系相对b系的姿态矩阵。
其中，步骤(1)中，炮弹姿态矩阵
Figure BDA0001792104480000024
和
Figure BDA0001792104480000025
计算方法如下：
设初始时刻t0时，b系相对于ib系的炮弹姿态矩阵为
Figure BDA0001792104480000026
I为3×3的单位矩阵；
记t时刻陀螺仪的输出值为
Figure BDA0001792104480000027
即t时刻的b系相对于ib系的角速度在b系上的投影值，由此跟踪b系相对于ib系的变化：
Figure BDA0001792104480000028
其中，
Figure BDA0001792104480000029
为矩阵
Figure BDA00017921044800000210
的变化率，
Figure BDA00017921044800000211
“×”表示三维矢量对应的叉乘矩阵变换，设
Figure BDA00017921044800000212
其中a，b，c分别表示炮弹沿三轴的旋转角速度，则
Figure BDA00017921044800000213
采用毕卡法解式(1)微分方程可得到式(2)：
Figure BDA00017921044800000214
记待解算数据时长为T，将时间段0～T以采样周期dt为间隔划分为多个时刻点t0,t1,t2...tm，k＝0,1,2,...,m，则式(2)中
Figure BDA00017921044800000215
为tk时刻的b系相对于tk-1时刻的b系的姿态矩阵；
Figure BDA00017921044800000216
为tk-1时刻炮弹陀螺仪输出，dt为采样周期，最终
Figure BDA00017921044800000217
t＝tk，k＝1,2,3,...,m，
Figure BDA00017921044800000218
由GPS输出的炮弹位置信息纬度L，东向、北向、天向速度分别为VE,VN,VU，则n系相对in系的角速度
Figure BDA00017921044800000219
可计算如下：
Figure BDA0001792104480000031
其中，RN为地球子午圈曲率半径，ωie为地球自转角速度，RE为地球卯酉圈半径，根据公式(2)的计算方法，由
Figure BDA0001792104480000032
计算出
Figure BDA0001792104480000033
Figure BDA0001792104480000034
其中，
Figure BDA0001792104480000035
为tk时刻的n系相对于tk-1时刻的n系的姿态矩阵；
Figure BDA0001792104480000036
为t＝tk-1时
Figure BDA0001792104480000037
的值，dt为采样周期，
Figure BDA0001792104480000038
t＝tk，k＝12,3,...,m。
其中，步骤(2)中，速度值
Figure BDA0001792104480000039
和
Figure BDA00017921044800000310
的计算方法如下：
将炮弹上GPS输出的n系下炮弹速度vn(t)投影到in系中，得到
Figure BDA00017921044800000311
Figure BDA00017921044800000312
由炮弹在n系的速度计算对应的b系速度vb(t)＝[0 ||vn(t)|| 0]T，将炮弹的b系速度vb(t)投影到ib系中，得到
Figure BDA00017921044800000313
Figure BDA00017921044800000314
其中，步骤(3)中，由GPS提供的炮弹速度比值计算初始航向角Y0与纵摇角P0，根据初始滚转角设置状态变量X，方法如下：
炮弹初始时刻的航向角与纵摇角由GPS提供的速度求得：
Figure BDA00017921044800000315
其中，VE(t0)，VN(t0)，VU(t0)分别表示初始时刻GPS输出的炮弹东向速度、北向速度与天向速度；
设初始时刻的滚转角为R0，则：
Figure BDA0001792104480000041
设初始状态变量
Figure BDA0001792104480000042
记
Figure BDA0001792104480000043
其中
Figure BDA0001792104480000044
为t时刻炮弹速度在in系的三轴分量；记
Figure BDA0001792104480000045
其中
Figure BDA0001792104480000046
为t时刻炮弹速度在ib系的三轴分量，记
Figure BDA0001792104480000047
将
Figure BDA0001792104480000048
各元素展开，改写为Z＝H·X的形式，其中：
Figure BDA0001792104480000049
其中，步骤(4)中，选择基于递推最小二乘算法作为在线辨识滤波器，构建如下：
Figure BDA00017921044800000410
式中，Kk为tk时刻的增益矩阵；Pk为tk时刻的状态向量协方差阵；Rk为tk时刻的量测噪声阵；Hk为tk时刻式(9)中H阵的取值；Xk为tk时刻的状态变量值；Zk为tk时刻Z的取值，
Figure BDA00017921044800000411
为tk时刻炮弹速度在ib系的投影，I为单位矩阵；
其中，步骤(4)中，根据
Figure BDA00017921044800000412
通过最小二乘滤波估计X方法如下：
设取初值X0＝[1 0]T，
Figure BDA00017921044800000413
系统的输入为
Figure BDA00017921044800000414
与
Figure BDA00017921044800000415
依次令k＝1,2,3...m，通过式(10)迭代Xk会逐渐趋于真值，Pk逐渐趋于零，将最终的Xk作为状态变量X；并且，在求解出X的基础上，根据式(8)计算出
Figure BDA0001792104480000051
其中，步骤(5)中，t时刻炮弹滚转角的计算方法如下：由下式计算得到t时刻的n系相对于b系的姿态矩阵
Figure BDA0001792104480000052
Figure BDA0001792104480000053
矩阵
Figure BDA0001792104480000054
为3阶方阵，其各元素记为：
Figure BDA0001792104480000055
则炮弹t时刻的滚转角计算如下：
Figure BDA0001792104480000056
有益效果：与现有技术相比，本发明的有益技术效果如下：
(1)弹体在空中对准时，只需要IMU和GPS提供的信息，无需多余传感器；
(2)将姿态矩阵进行链式分解，引入最小二乘滤波计算初始滚转角，则GPS速度求得航向角与纵摇角，进而得到矩阵
Figure BDA0001792104480000057
精度较高；
(3)优化对象仅为滚转角，精度更高。
附图说明
图1为本发明解算滚转角误差估计图。
具体实施方式
下面对本发明技术方案进行详细说明，但是本发明的保护范围不局限于所述实施例。
本发明适用于炮弹飞行时滚转角的估计，首先定义如下坐标系：
导航系n：原点为炮弹所在位置处，Y轴指向地理北向，X轴指向地理东向，Z轴垂直于大地水平面指向上。
载体系b：原点为弹体质心，Y轴沿弹体前进方向向前，X轴指向右，Z轴指向上。
导航惯性系in：初始时刻的导航系n凝固在惯性空间所得，不随时间变化。
载体惯性系ib：初始时刻的载体系b凝固在惯性空间所得，不随时间变化。
定义如上坐标系后，t时刻的n系相对于b系的姿态矩阵
Figure BDA0001792104480000061
可分解为
Figure BDA0001792104480000062
其中
Figure BDA0001792104480000063
为t时刻ib系相对于b系的姿态矩阵；
Figure BDA0001792104480000064
为t时刻n系相对于in系的姿态矩阵；
Figure BDA0001792104480000065
为in系相对于ib系的姿态矩阵,
Figure BDA0001792104480000066
为3阶方阵。根据炮弹上陀螺仪输出和GPS提供的速度位置信息，可计算每时刻的
Figure BDA0001792104480000067
计算t时刻炮弹速度在ib和in系下的值
Figure BDA0001792104480000068
和
Figure BDA0001792104480000069
由GPS提供的速度比值计算炮弹的初始航向角Y0与纵摇角P0，设炮弹的初始滚转角为R0，根据
Figure BDA00017921044800000610
通过递推最小二乘算法可得到
Figure BDA00017921044800000611
进而得到
Figure BDA00017921044800000612
并解出任意时刻的炮弹滚转角。
下面结合附图对本发明实施方法做更详细地描述：
1、由炮弹的陀螺仪和GPS组件输出计算姿态矩阵
Figure BDA00017921044800000613
具体包括如下步骤：
初始时刻t0时，b系与ib系间姿态矩阵为
Figure BDA00017921044800000614
I为3×3的单位矩阵；
记t时刻陀螺仪的输出值为
Figure BDA00017921044800000615
即t时刻的b系相对于ib系的角速度在b系上的投影值，由此可跟踪b系相对于ib系的变化：
Figure BDA00017921044800000616
其中，
Figure BDA00017921044800000617
为矩阵
Figure BDA00017921044800000618
的变化率，
Figure BDA00017921044800000619
“×”表示三维矢量对应的叉乘矩阵变换，即如果
Figure BDA00017921044800000620
其中a,b,c分别表示炮弹沿三轴的旋转角速度，则
Figure BDA00017921044800000621
采用毕卡法解式(1)微分方程可得到式(2)：
Figure BDA00017921044800000622
记待解算数据时长为T，将时间段0～T以采样周期dt为间隔划分为多个时刻点t0,t1,t2...tm，k＝0,1,2,...,m，则式(2)中
Figure BDA00017921044800000623
为tk时刻的b系相对于tk-1时刻的b系的姿态矩阵；
Figure BDA0001792104480000071
为tk-1时刻陀螺仪输出，dt为采样周期。最终
Figure BDA0001792104480000072
计算时t＝tk，k＝1,2,3,...,m，
Figure BDA0001792104480000073
根据炮弹上搭载的GPS组件可得到弹体位置信息经度λ与纬度L，东向、北向、天向速度分别为VE,VN,VU。则n系相对in系的角速度
Figure BDA0001792104480000074
可计算如下：
Figure BDA0001792104480000075
其中，RN为地球子午圈曲率半径，ωie为地球自转角速度，RE为地球卯酉圈半径。参照公式(2)的计算方法，由
Figure BDA0001792104480000076
可计算出
Figure BDA0001792104480000077
Figure BDA0001792104480000078
其中，
Figure BDA0001792104480000079
为tk时刻的n系相对于tk-1时刻的n系的姿态矩阵；
Figure BDA00017921044800000710
为t＝tk-1时
Figure BDA00017921044800000711
的值，dt为采样周期。最终
Figure BDA00017921044800000712
计算时t＝tk，k＝1,2,3,...,m。
2、炮弹速度在ib和in系下的值
Figure BDA00017921044800000713
和
Figure BDA00017921044800000714
的求法如下：
由式(4)，根据炮弹上GPS组件输出的n系下炮弹速度vn(t)，投影到in系中，得到
Figure BDA00017921044800000715
Figure BDA00017921044800000716
由于||vn(t)||＝||vb(t)||，且b系速度只有前向分量不为0，可由炮弹在n系的速度计算对应的b系速度vb(t)＝[0 ||vn(t)|| 0]T。
则通过式(2)计算得到的姿态矩阵
Figure BDA00017921044800000717
将炮弹的b系速度vb(t)投影到ib系中，得到
Figure BDA00017921044800000718
Figure BDA0001792104480000081
3、由GPS提供的速度比值计算初始时刻T0航向角Y0与纵摇角P0，设初始滚转角为R0，根据
Figure BDA0001792104480000082
通过递推最小二乘算法可计算出
Figure BDA0001792104480000083
为in系相对于ib系的姿态矩阵，具体方法如下：
初始时刻的航向角与纵摇角由GPS提供的速度求得：
Figure BDA0001792104480000084
其中VE(t0)，VN(t0)，VU(t0)分别表示初始时刻GPS输出的炮弹东向速度，北向速度与天向速度。
设初始滚转角为R0，则：
Figure BDA0001792104480000085
设状态变量
Figure BDA0001792104480000086
则
Figure BDA0001792104480000087
中仅R0为未知量。记
Figure BDA0001792104480000088
其中
Figure BDA0001792104480000089
为t时刻炮弹速度在in系的三轴分量；记
Figure BDA00017921044800000810
其中
Figure BDA00017921044800000811
为t时刻炮弹速度在ib系的三轴分量，记
Figure BDA00017921044800000812
将
Figure BDA00017921044800000813
各元素展开，可改写为Z＝H·X的形式，其中：
Figure BDA00017921044800000814
选择基于递推最小二乘算法作为在线辨识滤波器，构建如下：
Figure BDA00017921044800000815
式中，Kk为tk时刻的增益矩阵；Pk为tk时刻的状态向量协方差阵；Rk为tk时刻的量测噪声阵；Hk为tk时刻式(9)中H阵的取值；Xk为tk时刻的状态变量值；Zk为tk时刻Z的取值，
Figure BDA0001792104480000091
为tk时刻炮弹速度在ib系的投影；I为单位矩阵。
开始计算时k＝0,取初值X0＝[1 0]T，
Figure BDA0001792104480000092
系统的输入为
Figure BDA0001792104480000093
与
Figure BDA0001792104480000094
依次令k＝1,2,3...m，通过式(10)反复迭代Xk会逐渐趋于真值，Pk逐渐趋于零，最终所有数据处理完成后终止解算，辨识出状态变量X。
4、滚转角的计算如下：
根据最后一步迭代得到的Xk，可得到sin(R0)与cos(R0)。根据式(8)可计算出
Figure BDA0001792104480000095
再由下式可得到
Figure BDA0001792104480000096
Figure BDA0001792104480000097
其中，
Figure BDA0001792104480000098
为t时刻n系相对b系的姿态矩阵；
Figure BDA0001792104480000099
为t时刻ib系相对b系的姿态矩阵、
Figure BDA00017921044800000910
为t时刻n系相对in系的姿态矩阵，均已在前文求出。
矩阵
Figure BDA00017921044800000911
为3阶方阵，其中各元素可记为：
Figure BDA00017921044800000912
则炮弹t时刻的滚转角由下式解出
Figure BDA00017921044800000913
本发明的有益效果通过如下仿真得以验证：
根据运动学定理及捷联惯导反演算法，使用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中各曲线表明，在仿真时间内，本发明设计的方法有效估计出滚转角，且横滚角误差稳定在-6°以内，解算结束时基本在-3.5°左右。表明本发明设计的方法有效完成了横滚角的跟踪。
如上所述，尽管参照特定的优选实施例已经表示和表述了本发明，但其不得解释为对本发明自身的限制。在不脱离所附权利要求定义的本发明的精神和范围前提下，可对其在形式上和细节上作出各种变化。

Claims (7)

1.一种基于GPS和最小二乘滤波的炮弹滚转角估计方法，其特征在于，该方法包括以下步骤：
步骤 (1)根据陀螺仪和GPS提供的炮弹速度和位置，计算t时刻载体惯性系ib系相对于载体系b的炮弹姿态矩阵
Figure FDA0003247730160000011
导航系n相对于导航惯性系in的炮弹姿态矩阵
Figure FDA0003247730160000012
步骤 (2)计算炮弹在载体惯性系ib和导航惯性系in系下的速度值
Figure FDA0003247730160000013
和
Figure FDA0003247730160000014
步骤 (3)由GPS提供的速度比值计算初始航向角Y0与纵摇角P0，根据初始滚转角设置状态变量X；
步骤 (4)根据
Figure FDA0003247730160000015
通过最小二乘滤波估计X并计算
Figure FDA0003247730160000016
为in系相对于ib系的炮弹姿态矩阵；
步骤 (5)根据
Figure FDA0003247730160000017
得到t时的
Figure FDA0003247730160000018
并计算炮弹滚转角，
Figure FDA0003247730160000019
为t时刻n系相对b系的姿态矩阵；
步骤(1)中，炮弹姿态矩阵
Figure FDA00032477301600000110
和
Figure FDA00032477301600000111
计算方法如下：
设初始时刻t0时，b系相对于ib系的炮弹姿态矩阵为
Figure FDA00032477301600000112
I为3×3的单位矩阵；
记t时刻陀螺仪的输出值为
Figure FDA00032477301600000113
即t时刻的b系相对于ib系的角速度在b系上的投影值，由此跟踪b系相对于ib系的变化：
Figure FDA00032477301600000114
其中，
Figure FDA00032477301600000115
为矩阵
Figure FDA00032477301600000116
的变化率，
Figure FDA00032477301600000117
“×”表示三维矢量对应的叉乘矩阵变换，设
Figure FDA00032477301600000118
其中a，b，c分别表示炮弹沿三轴的旋转角速度，则
Figure FDA00032477301600000119
采用毕卡法解式(1)微分方程可得到式(2)：
Figure FDA00032477301600000120
记待解算数据时长为T，将时间段0～T以采样周期dt为间隔划分为多个时刻点t0,t1,t2...tm，k＝0,1,2,...,m，则式(2)中
Figure FDA00032477301600000121
为tk时刻的b系相对于tk-1时刻的b系的姿态矩阵；
Figure FDA0003247730160000021
Figure FDA0003247730160000022
为tk-1时刻炮弹陀螺仪输出，dt为采样周期，最终
Figure FDA0003247730160000023
由GPS输出的炮弹位置信息纬度L，东向、北向、天向速度分别为VE,VN,VU，则n系相对in系的角速度
Figure FDA0003247730160000024
可计算如下：
Figure FDA0003247730160000025
其中，RN为地球子午圈曲率半径，ωie为地球自转角速度，RE为地球卯酉圈半径，根据公式(2)的计算方法，由
Figure FDA0003247730160000026
计算出
Figure FDA0003247730160000027
Figure FDA0003247730160000028
其中，
Figure FDA0003247730160000029
为tk时刻的n系相对于tk-1时刻的n系的姿态矩阵；
Figure FDA00032477301600000210
Figure FDA00032477301600000219
为t＝tk-1时
Figure FDA00032477301600000211
的值，dt为采样周期，
Figure FDA00032477301600000212
k＝12,3,...,m。
2.根据权利要求1所述的一种基于GPS和最小二乘滤波的炮弹滚转角估计方法，其特征在于，步骤(2)中，速度值
Figure FDA00032477301600000213
和
Figure FDA00032477301600000214
的计算方法如下：
将炮弹上GPS输出的n系下炮弹速度vn(t)投影到in系中，得到
Figure FDA00032477301600000215
Figure FDA00032477301600000216
由炮弹在n系的速度计算对应的b系速度vb(t)＝[0 ||vn(t)|| 0]T，将炮弹的b系速度vb(t)投影到ib系中，得到
Figure FDA00032477301600000217
Figure FDA00032477301600000218
3.根据权利要求2所述的一种基于GPS和最小二乘滤波的炮弹滚转角估计方法，其特征在于，步骤(3)中，由GPS提供的炮弹速度比值计算初始航向角Y0与纵摇角P0，
根据初始滚转角设置状态变量X，方法如下：
炮弹初始时刻的航向角与纵摇角由GPS提供的速度求得：
Figure FDA0003247730160000031
其中，VE(t0)，VN(t0)，VU(t0)分别表示初始时刻GPS输出的炮弹东向速度、北向速度与天向速度；
设初始时刻的滚转角为R0，则：
Figure FDA0003247730160000032
设初始状态变量
Figure FDA0003247730160000033
其中
Figure FDA0003247730160000034
为t时刻炮弹速度在in系的三轴分量；记
Figure FDA0003247730160000035
其中
Figure FDA0003247730160000036
为t时刻炮弹速度在ib系的三轴分量，记
Figure FDA0003247730160000037
将
Figure FDA0003247730160000038
各元素展开，改写为Z＝H·X的形式，其中：
Figure FDA0003247730160000039
4.根据权利要求3所述的一种基于GPS和最小二乘滤波的炮弹滚转角估计方法，其特征在于，步骤(4)中，选择基于递推最小二乘算法作为在线辨识滤波器，构建如下：
Figure FDA00032477301600000310
式中，Kk为tk时刻的增益矩阵；Pk为tk时刻的状态向量协方差阵；Rk为tk时刻的量测噪声阵；Hk为tk时刻式(9)中H阵的取值；Xk为tk时刻的状态变量值；Zk为tk时刻Z的取值，
Figure FDA00032477301600000311
为tk时刻炮弹速度在ib系的投影，I为单位矩阵。
5.根据权利要求4所述的一种基于GPS和最小二乘滤波的炮弹滚转角估计方法，其特征在于，步骤(4)中，根据
Figure FDA00032477301600000410
通过最小二乘滤波估计X方法如下：
设取初值X0＝[1 0]T，
Figure FDA0003247730160000041
系统的输入为
Figure FDA0003247730160000042
与
Figure FDA0003247730160000043
依次令k＝1,2,3...m，通过式(10)迭代Xk会逐渐趋于真值，Pk逐渐趋于零，将最终的Xk作为状态变量X。
6.根据权利要求5所述的一种基于GPS和最小二乘滤波的炮弹滚转角估计方法，其特征在于，步骤(4)中，在求解出X的基础上，根据式(8)计算出
Figure FDA0003247730160000044
7.根据权利要求6所述的一种基于GPS和最小二乘滤波的炮弹滚转角估计方法，其特征在于，步骤(5)中，t时刻炮弹滚转角的计算方法如下：由下式计算得到t时刻的n系相对于b系的姿态矩阵
Figure FDA0003247730160000045
Figure FDA0003247730160000046
矩阵
Figure FDA0003247730160000047
为3阶方阵，其各元素记为：
Figure FDA0003247730160000048
则炮弹t时刻的滚转角计算如下：
Figure FDA0003247730160000049
CN201811047724.6A 2018-09-07 2018-09-07 一种基于gps和最小二乘滤波的炮弹滚转角估计方法 Active CN109059914B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201811047724.6A CN109059914B (zh) 2018-09-07 2018-09-07 一种基于gps和最小二乘滤波的炮弹滚转角估计方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201811047724.6A CN109059914B (zh) 2018-09-07 2018-09-07 一种基于gps和最小二乘滤波的炮弹滚转角估计方法

Publications (2)

Publication Number Publication Date
CN109059914A CN109059914A (zh) 2018-12-21
CN109059914B true CN109059914B (zh) 2021-11-02

Family

ID=64760034

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201811047724.6A Active CN109059914B (zh) 2018-09-07 2018-09-07 一种基于gps和最小二乘滤波的炮弹滚转角估计方法

Country Status (1)

Country Link
CN (1) CN109059914B (zh)

Families Citing this family (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110081883B (zh) * 2019-04-29 2021-05-18 北京理工大学 适用于高速滚转飞行器的低成本组合导航系统及方法
CN111504256A (zh) * 2020-04-29 2020-08-07 中国北方工业有限公司 一种基于最小二乘法的滚转角实时估计方法
CN111912402B (zh) * 2020-07-22 2022-09-09 北京自动化控制设备研究所 基于地磁信息辅助gps的高旋转载体的测姿方法及装置
CN114353784B (zh) * 2022-03-17 2022-06-07 西北工业大学 一种基于运动矢量的制导炮弹空中姿态辨识方法
CN116150552A (zh) * 2023-02-20 2023-05-23 北京自动化控制设备研究所 一种制导炮弹初始姿态计算方法

Citations (6)

* 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 北京航天控制仪器研究所 一种高速自旋制导炮弹空中实时对准方法
CN105258698A (zh) * 2015-10-13 2016-01-20 北京航天控制仪器研究所 一种高动态自旋制导炮弹空中组合导航方法

Patent Citations (6)

* 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 北京航天控制仪器研究所 一种高速自旋制导炮弹空中实时对准方法
CN105258698A (zh) * 2015-10-13 2016-01-20 北京航天控制仪器研究所 一种高动态自旋制导炮弹空中组合导航方法

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
制导炮弹用MINS/GPS滚转角在线对准方法;徐云等;《探测与控制学报》;20151231;第37卷(第6期);全文 *

Also Published As

Publication number Publication date
CN109059914A (zh) 2018-12-21

Similar Documents

Publication Publication Date Title
CN109059914B (zh) 一种基于gps和最小二乘滤波的炮弹滚转角估计方法
CN108180925B (zh) 一种里程计辅助车载动态对准方法
CN107621264B (zh) 车载微惯性/卫星组合导航系统的自适应卡尔曼滤波方法
CN109211230B (zh) 一种基于牛顿迭代法的炮弹姿态和加速度计常值误差估计方法
CN106643737A (zh) 风力干扰环境下四旋翼飞行器姿态解算方法
CN109211231B (zh) 一种基于牛顿迭代法的炮弹姿态估计方法
CN110243377B (zh) 一种基于分层式结构的集群飞行器协同导航方法
CN104697526A (zh) 用于农业机械的捷联惯导系统以及控制方法
CN105910606A (zh) 一种基于角速度差值的方向修正方法
CN109959374B (zh) 一种行人惯性导航全时全程逆向平滑滤波方法
CN113847913A (zh) 一种基于弹道模型约束的弹载组合导航方法
CN109211232B (zh) 一种基于最小二乘滤波的炮弹姿态估计方法
CN113340298B (zh) 一种惯导和双天线gnss外参标定方法
JP2007232443A (ja) 慣性航法装置およびその誤差補正方法
CN102944241A (zh) 基于多胞型线性微分包含的航天器相对姿态确定方法
CN104697520A (zh) 一体化无陀螺捷联惯导系统与gps系统组合导航方法
CN105115508A (zh) 基于后数据的旋转制导炮弹快速空中对准方法
CN104296780B (zh) 一种基于重力视运动的sins自对准与纬度计算方法
CN108871319B (zh) 一种基于地球重力场与地磁场序贯修正的姿态解算方法
CN105241319B (zh) 一种高速自旋制导炮弹空中实时对准方法
CN104482942B (zh) 一种基于惯性系的最优两位置对准方法
CN110514200B (zh) 一种惯性导航系统及高转速旋转体姿态测量方法
Wang et al. State transformation extended Kalman filter for SINS based integrated navigation system
CN107894240A (zh) 一种用于水下无人航行器在极区导航的初始粗对准方法
CN106595669A (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