CN109211231B - 一种基于牛顿迭代法的炮弹姿态估计方法 - Google Patents

一种基于牛顿迭代法的炮弹姿态估计方法 Download PDF

Info

Publication number
CN109211231B
CN109211231B CN201811047725.0A CN201811047725A CN109211231B CN 109211231 B CN109211231 B CN 109211231B CN 201811047725 A CN201811047725 A CN 201811047725A CN 109211231 B CN109211231 B CN 109211231B
Authority
CN
China
Prior art keywords
matrix
time
attitude
relative
projectile
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
CN201811047725.0A
Other languages
English (en)
Other versions
CN109211231A (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 CN201811047725.0A priority Critical patent/CN109211231B/zh
Publication of CN109211231A publication Critical patent/CN109211231A/zh
Application granted granted Critical
Publication of CN109211231B publication Critical patent/CN109211231B/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 DDA0001792104440000011
导航系n相对于导航惯性系in的炮弹姿态矩阵
Figure DDA0001792104440000012
(2)由陀螺仪和加速度计测量值计算ib系和in系下的炮弹速度v1(t)和v2(t)；(3)定义变量X＝[qT u]T，并构建非线性函数F(X)＝0；其中，q是4维列向量，代表in系至ib系的变化四元数，u为待定系数；(4)通过求函数F(X)一阶偏导数和二阶偏导数,得到Jacobian矩阵和Hessian矩阵；(5)利用牛顿法迭代解出四元数q；根据
Figure DDA0001792104440000013
计算出姿态角。

Description

一种基于牛顿迭代法的炮弹姿态估计方法
技术领域
本发明属于导航技术领域，尤其涉及一种基于牛顿迭代法的炮弹姿态估计方法。
背景技术
制导炮弹是指在传统炮弹基础上加装制导系统和可供驱动的弹翼或尾舱等空气动力装置，以提高炮弹对目标打击精度的一种低成本、小型化的精确制导武器。GPS/INS组合制导弹药从平台发射过程中，通常会承受高过载、高转速的恶劣情况，在此高过载冲击环境下陀螺仪、加速度计等弹上导航系统组件是无法正常上电工作的，所有弹上设备必须在经受此冲击出管后上电工作，导航系统的初始化需要发射后在空中自主完成。且空中易受风力等气象环境影响，弹体的姿态估算是后续导航系统工作的前提，也是当前的难点技术。弹体姿态探测的常用方法，主要包括采用地磁传感器、GPS、惯性系统以及组合的姿态探测方法，根据载体绕质心运动的方程,利用陀螺测量角速度信息对姿态角进行估计等。
然而上述方法在实际使用时需要引入地磁传感器，增大了成本，且一般的组合导航估计算法在高动态复杂的环境中效果不佳。利用陀螺测量角速度信息对滚转角进行估计时将陀螺测量值近似为载体相对于导航系运动的角速度信息,在高速飞行下,这种近似将带来很大的计算误差，甚至不能满足粗估计的要求。
发明内容
发明目的：针对以上现有技术存在的问题，本发明提出一种基于牛顿迭代法的炮弹姿态估计方法，该方法目的在仅利用陀螺仪，加速度计和GPS提供的信息，通过牛顿迭代法解出炮弹最优的姿态估计。
技术方案：为实现本发明的目的，本发明所采用的技术方案是：一种基于牛顿迭代法的炮弹姿态估计方法，该方法包括以下步骤：
(1)根据陀螺仪和GPS提供的炮弹速度和位置，计算t时刻载体惯性系ib系相对于载体系b的炮弹姿态矩阵
Figure BDA0001792104420000011
导航系n相对于导航惯性系in的炮弹姿态矩阵
Figure BDA0001792104420000012
(2)由陀螺仪和加速度计测量值计算ib系和in系下的炮弹速度v1(t)和v2(t)；
(3)定义变量X＝[qT u]T，并构建非线性函数F(X)＝0；其中，q是4维列向量，代表in系至ib系的变化四元数，u为待定系数；
(4)通过函数F(X)一阶偏导数和二阶偏导数，求Jacobian矩阵和Hessian矩阵；
(5)利用牛顿法迭代解出四元数q，根据
Figure BDA0001792104420000021
计算出姿态角。
其中，步骤(1)中，炮弹姿态矩阵
Figure BDA0001792104420000022
和
Figure BDA0001792104420000023
计算方法如下：
设初始时刻t0时，b系相对于ib系的炮弹姿态矩阵为
Figure BDA0001792104420000024
I为3×3的单位矩阵；
记t时刻陀螺仪的输出值为
Figure BDA0001792104420000025
即t时刻的b系相对于ib系的角速度在b系上的投影值，由此跟踪b系相对于ib系的变化：
Figure BDA0001792104420000026
其中，
Figure BDA0001792104420000027
为矩阵
Figure BDA0001792104420000028
的变化率，
Figure BDA0001792104420000029
“×”表示三维矢量对应的叉乘矩阵变换，设
Figure BDA00017921044200000210
其中a，b，c分别表示炮弹沿三轴的旋转角速度，则
Figure BDA00017921044200000211
采用毕卡法解式(1)微分方程可得到式(2)：
Figure BDA00017921044200000212
记待解算数据时长为T，将时间段0～T以采样周期dt为间隔划分为多个时刻点t0,t1,t2...tm，k＝0,1,2,...,m，则式(2)中
Figure BDA00017921044200000213
为tk时刻的b系相对于tk-1时刻的b系的姿态矩阵；
Figure BDA00017921044200000214
为tk-1时刻陀螺仪输出，dt为采样周期，最终
Figure BDA00017921044200000215
t＝tk，k＝1,2,3,...,m，
Figure BDA00017921044200000216
由GPS输出的炮弹位置信息纬度L，东向、北向、天向速度分别为VE,VN,VU，则n系相对in系的角速度
Figure BDA0001792104420000031
可计算如下：
Figure BDA0001792104420000032
其中，RN为地球子午圈曲率半径，ωie为地球自转角速度，RE为地球卯酉圈半径，根据公式(2)的计算方法，由
Figure BDA0001792104420000033
计算出
Figure BDA0001792104420000034
Figure BDA0001792104420000035
其中，
Figure BDA0001792104420000036
为tk时刻的n系相对于tk-1时刻的n系的姿态矩阵；
Figure BDA0001792104420000037
为t＝tk-1时
Figure BDA0001792104420000038
的值，dt为采样周期，
Figure BDA0001792104420000039
t＝tk，k＝12,3,...,m。
其中，步骤(2)中，炮弹速度v1(t)和v2(t)计算方法如下：
Figure BDA00017921044200000310
其中，fb(t)为t时刻炮弹上加速度计的输出，表示炮弹三轴加速度，通过姿态矩阵
Figure BDA00017921044200000311
将fb(t)投影到ib系中，得到
Figure BDA00017921044200000312
Figure BDA00017921044200000313
其中，Vn(t)为t时刻的n系炮弹速度，Vn(0)为起始时刻的n系炮弹速度，
Figure BDA00017921044200000314
gn＝[0 0 g]T，g为重力加速度，
Figure BDA00017921044200000315
Figure BDA00017921044200000316
ωie为地球自转角速度。
其中，步骤(3)中，定义变量X＝[qT u]T，并构建非线性函数F(X)＝0，具体方法如下：
记四元数q＝[s ηT]T，q*＝[s -ηT]T，s为q的标量部分，η为q的矢量部分，q*表示q的共轭四元数，定义四元数q的变换矩阵如下：
Figure BDA0001792104420000041
其中，I为3×3单位矩阵，η×为η对应的叉乘矩阵；
将v1(t)和v2(t)扩充成零标量四元数，即定义V1(t)＝[0 v1(t)T]T，V2(t)＝[0 v2(t)T]T，定义W＝M(V2(t))q-M'(V1(t))q＝(M(V2(t))-M'(V1(t)))q，且对q进行模值约束qqT＝1，引入拉格朗日乘子式，构造函数：
F(X)＝∑WTW-u(qTq-1) (7)
令X＝[qT u]T。
其中，步骤(4)中，通过Jacobian矩阵和Hessian矩阵，求函数F(X)一阶偏导数和二阶偏导数，具体方法如下：
Figure BDA0001792104420000042
Figure BDA0001792104420000043
其中，V＝M(V2(t))-M'(V1(t)),I4×4为四阶单位矩阵；
则Jacobian矩阵J和Hessian矩阵H可记为：
Figure BDA0001792104420000044
其中，步骤(4)中，利用牛顿法迭代，解出四元数q，具体方法如下：
起始时，取X0＝[1 0 0 0 0]T，令k＝0,2,3,...,m-1，每次迭代时计算J与H；
Xk+1＝Xk-H-1J (9)
由式(9)可不断递推Xk，直到所有数据全部解算完毕，从最终得到的Xk中取前4个元素组成q，即为in系至ib系的变化四元数，记q＝[q0 q1 q2 q3]T,q0,q1,q2,q3为q的四个元素。
其中，步骤(5)中，
Figure BDA0001792104420000051
计算方法如下：
计算
Figure BDA0001792104420000052
Figure BDA0001792104420000053
由下式得到
Figure BDA0001792104420000054
Figure BDA0001792104420000055
其中，
Figure BDA0001792104420000056
为t时刻n系相对b系的姿态矩阵；
Figure BDA0001792104420000057
为t时刻ib系相对b系的姿态矩阵、
Figure BDA0001792104420000058
为t时刻n系相对in系的姿态矩阵。
其中，步骤(5)中，t时刻炮弹姿态角计算方法如下：矩阵
Figure BDA0001792104420000059
为3阶方阵，其中各元素记为：
Figure BDA00017921044200000510
则t时刻炮弹的姿态角由下式解出：
Figure BDA00017921044200000511
φ，θ，γ分别是炮弹的航向角，纵摇角，横滚角。
有益效果：与现有技术相比，本发明的技术方案具有以下有益技术效果：
(1)估算弹体在空中的姿态时，只需要IMU和GPS提供的信息，无需多余传感器；
(2)引入牛顿迭代算法进行寻优计算，速度快且精度高；
(3)仿真结果表明本方案在高动态的飞行环境下效果良好。
附图说明
图1为本发明姿态角误差估计误差图。
具体实施方式
下面对本发明技术方案进行详细说明，但是本发明的保护范围不局限于所述实施例。
实施例：
本发明适用于炮弹飞行估计。首先定义如下坐标系：
导航系n：原点为炮弹所在位置处，Y轴指向地理北向，X轴指向地理东向，Z轴垂直于大地水平面指向上。
载体系b：原点为弹体质心，Y轴沿弹体前进方向向前，X轴指向右，Z轴指向上。
导航惯性系in：初始时刻的导航系n凝固在惯性空间所得，不随时间变化。
载体惯性系ib：初始时刻的载体系b凝固在惯性空间所得，不随时间变化。
定义如上坐标系后，t时刻的n系相对于b系的姿态矩阵
Figure BDA0001792104420000061
可分解为
Figure BDA0001792104420000062
其中
Figure BDA0001792104420000063
为t时刻ib系相对于b系的姿态矩阵；
Figure BDA0001792104420000064
为t时刻n系相对于in系的姿态矩阵；
Figure BDA0001792104420000065
为in系相对于ib系的姿态矩阵,
Figure BDA0001792104420000066
为3阶方阵。由炮弹上陀螺仪和加速度计测量值计算炮弹速度分别在ib系和in系下的值v1(t)和v2(t)，根据四元数相关性质，定义状态变量X＝[qT u]T，q是4维列向量，代表in系至ib系的变化四元数，u为待定系数。构建非线性函数，通过Jacobian矩阵和Hessian矩阵，求函数一阶偏导数和二阶偏导数，利用牛顿法迭代，解出四元数q，进而计算出炮弹姿态角。
下面结合附图对本发明实施方法做更详细地描述：
1、由陀螺仪和加速度计测量值计算ib系和in系下的炮弹速度v1(t)和v2(t)，具体包括如下步骤：
初始时刻t0时，b系与ib系间姿态矩阵为
Figure BDA0001792104420000067
I3×3为3阶单位矩阵；
记t时刻陀螺仪的输出值为
Figure BDA0001792104420000068
即t时刻的b系相对于ib系的炮弹角速度在b系上的投影值，由此可跟踪b系相对于ib系的变化：
Figure BDA0001792104420000071
其中，
Figure BDA0001792104420000072
为矩阵
Figure BDA0001792104420000073
的变化率，
Figure BDA0001792104420000074
“×”表示三维矢量对应的叉乘矩阵变换，即如果
Figure BDA0001792104420000075
其中a,b,c分别表示炮弹沿三轴的旋转角速度，则
Figure BDA0001792104420000076
采用毕卡法解式(1)微分方程可得到式(2)：
Figure BDA0001792104420000077
记待解算数据时长为T，将时间段0～T以采样周期dt为间隔划分为多个时刻点t0,t1,t2...tm，k＝0,1,2,...,m.则式(2)中
Figure BDA0001792104420000078
为tk时刻的b系相对于tk-1时刻的b系的姿态矩阵；
Figure BDA0001792104420000079
为tk-1时刻陀螺仪输出，dt为采样周期。最终
Figure BDA00017921044200000710
计算时t＝tk，k＝1,2,3,...,m。
Figure BDA00017921044200000711
fb(t)为t时刻炮弹上加速度计的输出，表示炮弹三轴加速度。通过姿态矩阵
Figure BDA00017921044200000712
将fb(t)投影到ib系中，得到
Figure BDA00017921044200000713
则：
Figure BDA00017921044200000714
根据炮弹上搭载的GPS组件可得到弹体位置信息纬度L，东向、北向、天向速度分别为VE,VN,VU。则n系相对in系的角速度
Figure BDA00017921044200000715
可计算如下：
Figure BDA00017921044200000716
其中,RN为地球子午圈曲率半径，ωie为地球自转角速度，RE为地球卯酉圈半径。参照公式(2)的计算方法，由
Figure BDA00017921044200000717
可计算出
Figure BDA00017921044200000718
Figure BDA0001792104420000081
其中，
Figure BDA0001792104420000082
为tk时刻的n系相对于tk-1时刻的n系的姿态矩阵；
Figure BDA0001792104420000083
为t＝tk-1时
Figure BDA0001792104420000084
的值，dt为采样周期。最终
Figure BDA0001792104420000085
计算时t＝tk，k＝1,2,3,...,m。
则
Figure BDA0001792104420000086
其中Vn(t)为t时刻的n系炮弹速度，Vn(0)为起始时刻的n系炮弹速度，
Figure BDA0001792104420000087
gn＝[0 0 g]T，g为重力加速度。
Figure BDA0001792104420000088
Figure BDA0001792104420000089
ωie为地球自转角速度。
2、根据四元数相关性质，定义变量X＝[qT u]T，并构建非线性函数F(X)＝0具体包括：
记四元数q＝[s ηT]T，q*＝[s -ηT]T，s为q的标量部分，η为q的矢量部分，q*表示q的共轭四元数。定义四元数q的变换矩阵如下
Figure BDA00017921044200000810
其中，I为3×3单位矩阵，η×为η对应的叉乘矩阵。
将v1(t)和v2(t)扩充成零标量四元数，即定义V1(t)＝[0 v1(t)T]T，V2(t)＝[0 v2(t)T]T。定义W＝M(V2(t))q-M'(V1(t))q＝(M(V2(t))-M'(V1(t)))q，且对q进行模值约束qqT＝1，引入拉格朗日乘子式，可构造函数：
F(X)＝∑WTW-u(qTq-1) (7)
令X＝[qT u]T，
则用牛顿迭代法解F(X)＝0即解出q，方法如下：
3、通过求函数F(X)一阶偏导数和二阶偏导数，计算Jacobian矩阵和Hessian矩阵方法如下：
Figure BDA0001792104420000091
Figure BDA0001792104420000092
其中,V＝M(V2(t))-M'(V1(t)),I4×4为四阶单位矩阵。
则Jacobian矩阵J和Hessian矩阵H可记为:
Figure BDA0001792104420000093
4、X的递推方法如下：
起始时，取X0＝[1 0 0 0 0]T，令k＝0,2,3,...,m-1，每次迭代时计算J与H.
Xk+1＝Xk-H-1J (9)
由式(9)可不断递推Xk。直到所有数据全部解算完毕。从最终得到的Xk中取前4个元素组成q，即为in系至ib系的变化四元数。记q＝[q0 q1 q2 q3]T,q0,q1,q2,q3为q的四个元素，则可计算
Figure BDA0001792104420000094
Figure BDA0001792104420000095
再由下式可得到
Figure BDA0001792104420000096
Figure BDA0001792104420000097
其中
Figure BDA0001792104420000098
为t时刻n系相对b系的姿态矩阵；
Figure BDA0001792104420000099
为t时刻ib系相对b系的姿态矩阵、
Figure BDA00017921044200000910
为t时刻n系相对in系的姿态矩阵，均已在前文求出。
矩阵
Figure BDA00017921044200000911
为3阶方阵，若其中各元素可记为：
Figure BDA00017921044200000912
则炮弹的姿态角由下式解出
Figure BDA0001792104420000101
φ，θ，γ分别是炮弹的航向角，纵摇角，横滚角。
本发明的有益效果通过如下仿真得以验证：
根据运动学定理及捷联惯导反演算法，使用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中各曲线表明，在仿真时间内，本发明设计的方法有效估计出姿态角，其中解算结束时航向角误差基本在-3.5°左右，纵摇角误差在0.5°以内，横滚角误差稳定在-6.5°左右。
如上所述，尽管参照特定的优选实施例已经表示和表述了本发明，但其不得解释为对本发明自身的限制。在不脱离所附权利要求定义的本发明的精神和范围前提下，可对其在形式上和细节上作出各种变化。

Claims (2)

1.一种基于牛顿迭代法的炮弹姿态估计方法，其特征在于，该方法包括以下步骤：
(1)根据陀螺仪和GPS提供的炮弹速度和位置，计算t时刻载体惯性系ib系相对于载体系b的炮弹姿态矩阵
Figure FDA0003415722050000011
导航系n相对于导航惯性系in的炮弹姿态矩阵
Figure FDA0003415722050000012
(2)由陀螺仪和加速度计测量值计算ib系和in系下的炮弹速度v1(t)和v2(t)；
(3)定义变量X＝[qT u]T，并构建非线性函数F(X)＝0；其中，q是4维列向量，代表in系至ib系的变化四元数，u为待定系数；
(4)通过函数F(X)一阶偏导数和二阶偏导数，求Jacobian矩阵和Hessian矩阵；
(5)利用牛顿法迭代解出四元数q，根据
Figure FDA0003415722050000013
计算出姿态角；
步骤(1)中，炮弹姿态矩阵
Figure FDA0003415722050000014
和
Figure FDA0003415722050000015
计算方法如下：
设初始时刻t0时，b系相对于ib系的炮弹姿态矩阵为
Figure FDA0003415722050000016
I为3×3的单位矩阵；
记t时刻陀螺仪的输出值为
Figure FDA0003415722050000017
即t时刻的b系相对于ib系的角速度在b系上的投影值，由此跟踪b系相对于ib系的变化：
Figure FDA0003415722050000018
其中，
Figure FDA0003415722050000019
为矩阵
Figure FDA00034157220500000110
的变化率，
Figure FDA00034157220500000111
“×”表示三维矢量对应的叉乘矩阵变换，设
Figure FDA00034157220500000112
其中a，b，c分别表示炮弹沿三轴的旋转角速度，则
Figure FDA00034157220500000113
采用毕卡法解式(1)微分方程可得到式(2)：
Figure FDA00034157220500000114
记待解算数据时长为T，将时间段0～T以采样周期dt为间隔划分为多个时刻点t0,t1,t2...tm，k＝0,1,2,...,m，则式(2)中
Figure FDA00034157220500000115
为tk时刻的b系相对于tk-1时刻的b系的姿态矩阵；
Figure FDA00034157220500000116
为tk-1时刻陀螺仪输出，dt为采样周期，最终
Figure FDA00034157220500000117
由GPS输出的炮弹位置信息纬度L，东向、北向、天向速度分别为VE,VN,VU，则n系相对in系的角速度
Figure FDA0003415722050000021
可计算如下：
Figure FDA0003415722050000022
其中，RN为地球子午圈曲率半径，ωie为地球自转角速度，RE为地球卯酉圈半径，根据公式(2)的计算方法，由
Figure FDA0003415722050000023
计算出
Figure FDA0003415722050000024
Figure FDA0003415722050000025
其中，
Figure FDA0003415722050000026
为tk时刻的n系相对于tk-1时刻的n系的姿态矩阵；
Figure FDA0003415722050000027
Figure FDA0003415722050000028
为t＝tk-1时
Figure FDA0003415722050000029
的值，dt为采样周期，
Figure FDA00034157220500000210
Figure FDA00034157220500000211
步骤(2)中，炮弹速度v1(t)和v2(t)计算方法如下：
Figure FDA00034157220500000212
其中，fb(t)为t时刻炮弹上加速度计的输出，表示炮弹三轴加速度，通过姿态矩阵
Figure FDA00034157220500000213
将fb(t)投影到ib系中，得到
Figure FDA00034157220500000214
Figure FDA00034157220500000215
其中，Vn(t)为t时刻的n系炮弹速度，Vn(0)为起始时刻的n系炮弹速度，
Figure FDA00034157220500000216
gn＝[0 0 g]T，g为重力加速度，
Figure FDA00034157220500000217
Figure FDA00034157220500000218
ωie为地球自转角速度；
步骤(3)中，定义变量X＝[qT u]T，并构建非线性函数F(X)＝0，具体方法如下：
记四元数q＝[s ηT]T，q*＝[s -ηT]T，s为q的标量部分，η为q的矢量部分，q*表示q的共轭四元数，定义四元数q的变换矩阵如下：
Figure FDA0003415722050000031
其中，I为3×3单位矩阵，η×为η对应的叉乘矩阵；
将v1(t)和v2(t)扩充成零标量四元数，即定义V1(t)＝[0 v1(t)T]T，V2(t)＝[0 v2(t)T]T，定义W＝M(V2(t))q-M'(V1(t))q＝(M(V2(t))-M'(V1(t)))q，且对q进行模值约束qqT＝1，引入拉格朗日乘子式，构造函数：
F(X)＝∑WTW-u(qTq-1) (7)
令X＝[qT u]T；
步骤(4)中，通过Jacobian矩阵和Hessian矩阵，求函数F(X)一阶偏导数和二阶偏导数，具体方法如下：
Figure FDA0003415722050000032
Figure FDA0003415722050000033
其中，V＝M(V2(t))-M'(V1(t)),I4×4为四阶单位矩阵；
则Jacobian矩阵J和Hessian矩阵H可记为：
Figure FDA0003415722050000034
步骤(4)中，利用牛顿法迭代，解出四元数q，具体方法如下：
起始时，取X0＝[1 0 0 0 0]T，令k＝0,2,3,...,m-1，每次迭代时计算J与H；
Xk+1＝Xk-H-1J (9)
由式(9)可不断递推Xk，直到所有数据全部解算完毕，从最终得到的Xk中取前4个元素组成q，即为in系至ib系的变化四元数，记q＝[q0 q1 q2 q3]T,q0,q1,q2,q3为q的四个元素；
步骤(5)中，
Figure FDA0003415722050000035
计算方法如下：
计算
Figure FDA0003415722050000041
Figure FDA0003415722050000042
由下式得到
Figure FDA0003415722050000043
Figure FDA0003415722050000044
其中，
Figure FDA0003415722050000045
为t时刻n系相对b系的姿态矩阵；
Figure FDA0003415722050000046
为t时刻ib系相对b系的姿态矩阵、
Figure FDA0003415722050000047
为t时刻n系相对in系的姿态矩阵。
2.根据权利要求1所述的一种基于牛顿迭代法的炮弹姿态估计方法，其特征在于，步骤(5)中，t时刻炮弹姿态角计算方法如下：矩阵
Figure FDA0003415722050000048
为3阶方阵，其中各元素记为：
Figure FDA0003415722050000049
则t时刻炮弹的姿态角由下式解出：
Figure FDA00034157220500000410
φ，θ，γ分别是炮弹的航向角，纵摇角，横滚角。
CN201811047725.0A 2018-09-07 2018-09-07 一种基于牛顿迭代法的炮弹姿态估计方法 Active CN109211231B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201811047725.0A CN109211231B (zh) 2018-09-07 2018-09-07 一种基于牛顿迭代法的炮弹姿态估计方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201811047725.0A CN109211231B (zh) 2018-09-07 2018-09-07 一种基于牛顿迭代法的炮弹姿态估计方法

Publications (2)

Publication Number Publication Date
CN109211231A CN109211231A (zh) 2019-01-15
CN109211231B true CN109211231B (zh) 2022-02-15

Family

ID=64987262

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201811047725.0A Active CN109211231B (zh) 2018-09-07 2018-09-07 一种基于牛顿迭代法的炮弹姿态估计方法

Country Status (1)

Country Link
CN (1) CN109211231B (zh)

Families Citing this family (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110455288A (zh) * 2019-08-06 2019-11-15 东南大学 一种基于角速度高次多项式的姿态更新方法
CN112966612B (zh) * 2021-03-10 2022-06-03 广东海洋大学 基于牛顿积分神经动力学的北极海冰遥感图像提取方法
CN114353784B (zh) * 2022-03-17 2022-06-07 西北工业大学 一种基于运动矢量的制导炮弹空中姿态辨识方法
CN116070066B (zh) * 2023-02-20 2024-03-15 北京自动化控制设备研究所 一种制导炮弹滚动角计算方法

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101413800A (zh) * 2008-01-18 2009-04-22 南京航空航天大学 导航/稳瞄一体化系统的导航、稳瞄方法
CN103235328A (zh) * 2013-04-19 2013-08-07 黎湧 一种gnss与mems组合导航的方法
CN103471616A (zh) * 2013-09-04 2013-12-25 哈尔滨工程大学 一种动基座sins大方位失准角条件下初始对准方法
CN103644911A (zh) * 2013-11-27 2014-03-19 南京城际在线信息技术有限公司 陀螺仪辅助定位方法
CN104296745A (zh) * 2014-09-29 2015-01-21 杭州电子科技大学 一种基于9-dof传感器组的姿态检测数据融合方法
CN108051866A (zh) * 2017-10-30 2018-05-18 中国船舶重工集团公司第七0七研究所 基于捷联惯性/gps组合辅助水平角运动隔离的重力测量方法

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101413800A (zh) * 2008-01-18 2009-04-22 南京航空航天大学 导航/稳瞄一体化系统的导航、稳瞄方法
CN103235328A (zh) * 2013-04-19 2013-08-07 黎湧 一种gnss与mems组合导航的方法
CN103471616A (zh) * 2013-09-04 2013-12-25 哈尔滨工程大学 一种动基座sins大方位失准角条件下初始对准方法
CN103644911A (zh) * 2013-11-27 2014-03-19 南京城际在线信息技术有限公司 陀螺仪辅助定位方法
CN104296745A (zh) * 2014-09-29 2015-01-21 杭州电子科技大学 一种基于9-dof传感器组的姿态检测数据融合方法
CN108051866A (zh) * 2017-10-30 2018-05-18 中国船舶重工集团公司第七0七研究所 基于捷联惯性/gps组合辅助水平角运动隔离的重力测量方法

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
基于UKF 的四元数载体姿态确定;刘海颖，等;《南京航空航天大学学报》;20060228;第38卷(第1期);全文 *
基于数据存储与循环解算的SINS快速对准方法;刘锡祥，等;《中国惯性技术学报》;20131231;第21卷(第6期);全文 *

Also Published As

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

Similar Documents

Publication Publication Date Title
CN109211231B (zh) 一种基于牛顿迭代法的炮弹姿态估计方法
CN109211230B (zh) 一种基于牛顿迭代法的炮弹姿态和加速度计常值误差估计方法
CN109059914B (zh) 一种基于gps和最小二乘滤波的炮弹滚转角估计方法
CN106990426B (zh) 一种导航方法和导航装置
US7957899B2 (en) Method for determining the attitude, position, and velocity of a mobile device
CN105486307B (zh) 针对机动目标的视线角速率估计方法
CN105180728B (zh) 基于前数据的旋转制导炮弹快速空中对准方法
CN105737823A (zh) 基于五阶ckf的gps/sins/cns组合导航方法
CN109211232B (zh) 一种基于最小二乘滤波的炮弹姿态估计方法
CN113340298B (zh) 一种惯导和双天线gnss外参标定方法
CN105806363A (zh) 基于srqkf的sins/dvl水下大失准角对准方法
JP2007232443A (ja) 慣性航法装置およびその誤差補正方法
CN113847913A (zh) 一种基于弹道模型约束的弹载组合导航方法
CN109708663B (zh) 基于空天飞机sins辅助的星敏感器在线标定方法
CN103123487B (zh) 一种航天器姿态确定方法
JP2014240266A (ja) センサドリフト量推定装置及びプログラム
RU2564379C1 (ru) Бесплатформенная инерциальная курсовертикаль
CN108871319B (zh) 一种基于地球重力场与地磁场序贯修正的姿态解算方法
CN109029499A (zh) 一种基于重力视运动模型的加速度计零偏迭代寻优估计方法
CN112325878A (zh) 基于ukf与空中无人机节点辅助的地面载体组合导航方法
Fiot et al. Estimation of air velocity for a high velocity spinning projectile using transerse accelerometers
CN107036595A (zh) 基于交互式多模型滤波的船体变形角估计方法
CN114295145A (zh) 一种基于车载发射平台的捷联惯导系统轨迹发生器设计方法
RU2620786C1 (ru) Способ восстановления параметров движения летательного аппарата
CN117073472B (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