CN109211230B - 一种基于牛顿迭代法的炮弹姿态和加速度计常值误差估计方法 - Google Patents

一种基于牛顿迭代法的炮弹姿态和加速度计常值误差估计方法 Download PDF

Info

Publication number
CN109211230B
CN109211230B CN201811041674.0A CN201811041674A CN109211230B CN 109211230 B CN109211230 B CN 109211230B CN 201811041674 A CN201811041674 A CN 201811041674A CN 109211230 B CN109211230 B CN 109211230B
Authority
CN
China
Prior art keywords
matrix
attitude
time
accelerometer
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
CN201811041674.0A
Other languages
English (en)
Other versions
CN109211230A (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 CN201811041674.0A priority Critical patent/CN109211230B/zh
Publication of CN109211230A publication Critical patent/CN109211230A/zh
Application granted granted Critical
Publication of CN109211230B publication Critical patent/CN109211230B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

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)将初始时刻的载体系b和导航系n凝固为载体惯性系ib和导航惯性系in，则t时刻的姿态矩阵可分解为
Figure DDA0001792262560000011
由陀螺仪和加速度计测量值计算ib系和in系下的速度v1(t)和v2(t)；(2)根据四元数相关性质，定义变量X＝[qT ba T u]T，并构建非线性函数F(X)＝0；(3)v1(t)和v2(t)，计算F(X)对应的Jacobian矩阵和Hessian矩阵，求函数F(X)一阶偏导数和二阶偏导数；(4)利用牛顿法迭代，解出

Description

一种基于牛顿迭代法的炮弹姿态和加速度计常值误差估计 方法
技术领域
本发明属于导航技术领域，尤其涉及一种基于牛顿迭代法的炮弹姿态和加速度计常值误差估计方法。
背景技术
制导炮弹是指在传统炮弹基础上加装制导系统和可供驱动的弹翼或尾舱等空气动力装置，其上一般装配有GPS/INS组合。炮弹的姿态估计是后续精确打击的关键，但在制导弹药从平台发射过程中，由于环境恶劣，在高过载冲击环境下陀螺仪、加速度计等弹上导航系统组件是无法正常上电工作的，导航系统的初始化需要发射后在空中自主完成。且空中易受风力等气象环境影响，弹体的姿态估算也是当前的难点技术，提供仪表的常值误差估计则对进一步提升对准精度有重要意义。
发明内容
发明目的：针对以上问题，本发明提出一种基于牛顿迭代法的炮弹姿态和加速度计常值误差估计方法，该方法的目的在于仅利用陀螺仪，加速度计和GPS提供的信息，通过牛顿迭代法解出最优的炮弹姿态估计和加速度计常值误差。
技术方案：为实现本发明的目的，本发明所采用的技术方案是：一种基于牛顿迭代法的炮弹姿态和加速度计常值误差方法，该方法包括以下步骤：
(1)根据陀螺仪和GPS提供的炮弹速度和位置，计算t时刻载体惯性系ib系相对于载体系b的炮弹姿态矩阵
Figure BDA0001792262550000011
导航系n相对于导航惯性系in的炮弹姿态矩阵
Figure BDA0001792262550000012
(2)由陀螺仪和加速度计测量值计算ib系和in系下的炮弹速度v1(t)和v2(t)；
(3)定义变量X＝[qT ba T u]T，并构建非线性函数F(X)＝0，其中，q为in系至ib系的变化四元数，ba＝[0 BaT]T，Ba为炮弹上三轴加速度计的常值误差，u为待定系数；
(4)根据v1(t)和v2(t)，计算F对应的Jacobian矩阵和Hessian矩阵，求函数F(X)一阶偏导数和二阶偏导数；
(5)利用牛顿法迭代法并根据
Figure BDA0001792262550000013
计算
Figure BDA0001792262550000014
对应的姿态角四元数q与加速度计常值误差，
Figure BDA0001792262550000015
为t时刻n系相对b系的姿态矩阵，
Figure BDA0001792262550000016
为in系相对于ib系的姿态矩阵。
其中，步骤(1)中，炮弹姿态矩阵
Figure BDA0001792262550000021
和
Figure BDA0001792262550000022
计算方法如下：
设初始时刻t0时，b系与ib系间姿态矩阵为
Figure BDA0001792262550000023
I为3×3的单位矩阵；
记t时刻陀螺仪的输出值为
Figure BDA0001792262550000024
即t时刻的b系相对于ib系的角速度在b系上的投影值，由此跟踪b系相对于ib系的变化：
Figure BDA0001792262550000025
其中，
Figure BDA0001792262550000026
为矩阵
Figure BDA0001792262550000027
的变化率，
Figure BDA0001792262550000028
“×”表示三维矢量对应的叉乘矩阵变换，设
Figure BDA0001792262550000029
其中a，b，c分别表示炮弹沿三轴的旋转角速度，则
Figure BDA00017922625500000210
采用毕卡法解式(1)微分方程可得到式(2)：
Figure BDA00017922625500000211
记待解算数据时长为T，将时间段0～T以采样周期dt为间隔划分为多个时刻点t0,t1,t2...tm，k＝0,1,2,...,m，则式(2)中
Figure BDA00017922625500000212
为tk时刻的b系相对于tk-1时刻的b系的姿态矩阵；
Figure BDA00017922625500000213
为tk-1时刻陀螺仪输出，dt为采样周期，最终
Figure BDA00017922625500000214
由GPS输出的炮弹位置信息纬度L，东向、北向、天向速度分别为VE,VN,VU，则n系相对in系的角速度
Figure BDA00017922625500000215
可计算如下：
Figure BDA00017922625500000216
其中，RN为地球子午圈曲率半径，ωie为地球自转角速度，RE为地球卯酉圈半径，根据公式(2)的计算方法，由
Figure BDA00017922625500000217
可计算出
Figure BDA00017922625500000218
Figure BDA0001792262550000031
其中，
Figure BDA0001792262550000032
为tk时刻的n系相对于tk-1时刻的n系的姿态矩阵；
Figure BDA0001792262550000033
Figure BDA0001792262550000034
为t＝tk-1时
Figure BDA0001792262550000035
的值，dt为采样周期，则
Figure BDA0001792262550000036
Figure BDA0001792262550000037
其中，步骤(2)中，炮弹速度v1(t)和v2(t)计算方法如下：
Figure BDA0001792262550000038
取t＝tk，k＝12,3,...,m，fb为炮弹加速度计输出；将
Figure BDA0001792262550000039
记为α0，
Figure BDA00017922625500000310
记为χ0；
Figure BDA00017922625500000311
其中，Vn(t)为t时刻的n系炮弹速度，Vn(0)为起始时刻的n系炮弹速度，
Figure BDA00017922625500000312
gn＝[0 0 g]T，g为重力加速度，
Figure BDA00017922625500000313
Figure BDA00017922625500000314
ωie为地球自转角速度。
其中，步骤(3)中，定义变量X＝[qT ba T u]T，并构建非线性函数F(X)＝0，具体方法如下：定义α＝[0 α0 T]T，ba＝[0 BaT]T,V2(t)＝[0 v2(t)T]T，
Figure BDA00017922625500000315
其中，O1×3与O3×1分别为1×3和3×1的零矩阵，定义变量X＝[qT ba T u]T，q是4维列向量，代表in系至ib系的变化四元数，u为待定系数；
记四元数q＝[s ηT]T，q*＝[s -ηT]T，s为q的标量部分，η为q的矢量部分，定义四元数q的变换矩阵如下：
Figure BDA00017922625500000316
其中，I为3×3单位矩阵，η×为η对应的叉乘矩阵；
根据
Figure BDA0001792262550000041
定义W＝(M'(α)-M(V2(t)))q+M(q)(χba)，
对q进行模值约束qqT＝1，引入拉格朗日乘子式，构建非线性造函数如下：
F(X)＝∑WTW-u(qTq-1)＝0 (7)。
其中，步骤(4)中，根据v1(t)和v2(t)，求函数F(X)一阶偏导数和二阶偏导数，方法如下：
F(X)一阶偏导数：
Figure BDA0001792262550000042
Figure BDA0001792262550000043
Figure BDA0001792262550000044
F(X)二阶偏导数：
Figure BDA0001792262550000045
Figure BDA0001792262550000046
其中，I4×4是4阶单位矩阵，g1＝[1 0 0 0]T，g2＝[0 1 0 0]T，g3＝[0 0 1 0]T，g4＝[0 0 0 1]T；
Figure BDA0001792262550000047
Figure BDA0001792262550000048
Figure BDA0001792262550000051
Figure BDA0001792262550000052
其中，步骤(4)中，根据v1(t)和v2(t)，计算F(x)对应的Jacobian矩阵和Hessian矩阵方法如下：Jacobian矩阵J和Hessian矩阵H记为：
Figure BDA0001792262550000053
其中，步骤(5)中，利用牛顿法迭代法，计算四元数q，方法如下：
起始时，取X0＝[1 0 0 0 0 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 BDA0001792262550000054
为t时刻n系相对b系的姿态矩阵计算方法如下：
计算
Figure BDA0001792262550000055
Figure BDA0001792262550000056
由下式得到
Figure BDA0001792262550000057
Figure BDA0001792262550000058
其中，
Figure BDA0001792262550000059
为t时刻n系相对b系的姿态矩阵；
Figure BDA00017922625500000510
为t时刻ib系相对b系的姿态矩阵、
Figure BDA00017922625500000511
为t时刻n系相对in系的姿态矩阵，
Figure BDA00017922625500000512
为in系相对于ib系的姿态矩阵；
其中，步骤(5)中，炮弹姿态角计算方法如下：矩阵
Figure BDA0001792262550000061
为3阶方阵，其各个元素记为：
Figure BDA0001792262550000062
则t时刻的炮弹的姿态角由下式解出:
Figure BDA0001792262550000063
其中，φ，θ，γ分别是炮弹的航向角，纵摇角，横滚角。
并且，步骤(5)中，加速度计的常值误差计算方法如下：从最终得到的Xk中取第6，7，8个元素组成Ba，即为t时刻的炮弹三轴加速度计的常值误差。
有益效果：与现有技术相比，本发明的技术方案具有以下有益技术效果：
(1)弹体在空中对准时，只需要IMU和GPS提供的信息，无需多余传感器；
(2)引入牛顿迭代算法进行寻优计算，速度快且精度高；
(3)可估算出加速度计常值误差，有利于进一步提高对准精度。
具体实施方式
下面对本发明技术方案进行详细说明，但是本发明的保护范围不局限于所述实施例。
本发明适用于炮弹飞行对准。首先定义如下坐标系：
导航系n：原点为炮弹所在位置处，Y轴指向地理北向，X轴指向地理东向，Z轴垂直于大地水平面指向上。
载体系b：原点为弹体质心，Y轴沿弹体前进方向向前，X轴指向右，Z轴指向上。
导航惯性系in：初始时刻的导航系n凝固在惯性空间所得，不随时间变化。
载体惯性系ib：初始时刻的载体系b凝固在惯性空间所得，不随时间变化。
定义如上坐标系后，t时刻的n系相对于b系的姿态矩阵
Figure BDA0001792262550000064
可分解为
Figure BDA0001792262550000065
其中
Figure BDA0001792262550000066
为t时刻ib系相对于b系的姿态矩阵；
Figure BDA0001792262550000067
为t时刻n系相对于in系的姿态矩阵；
Figure BDA0001792262550000068
为in系相对于ib系的姿态矩阵，
Figure BDA0001792262550000069
为3阶方阵。由炮弹上陀螺仪和加速度计测量值计算炮弹速度分别在ib系和in系下的值v1(t)和v2(t)，根据四元数相关性质，定义包含q,Ba,u的变量X，其中q是4维列向量，代表in系至ib系的变化四元数，Ba为3维列向量，代表炮弹上三轴加速度计的常值误差，u为待定系数。并构建非线性函数，通过Jacobian矩阵和Hessian矩阵，求函数一阶偏导数和二阶偏导数，利用牛顿法迭代，解出四元数q和炮弹加速度计常值误差，进而得到炮弹姿态角。
下面对本发明实施方法做更详细地描述：
1、由陀螺仪和加速度计测量值计算ib系和in系下的炮弹速度v1(t)和v2(t)，具体包括如下步骤：
初始时刻t0时，b系与ib系间姿态矩阵为
Figure BDA0001792262550000071
I3×3为3阶单位矩阵；
记t时刻陀螺仪的输出值为
Figure BDA0001792262550000072
即t时刻的b系相对于ib系的炮弹角速度在b系上的投影值，由此可跟踪b系相对于ib系的变化：
Figure BDA0001792262550000073
其中，
Figure BDA0001792262550000074
为矩阵
Figure BDA0001792262550000075
的变化率，
Figure BDA0001792262550000076
“×”表示三维矢量对应的叉乘矩阵变换，即如果
Figure BDA0001792262550000077
其中a，b，c分别表示炮弹沿三轴的旋转角速度，则
Figure BDA0001792262550000078
采用毕卡法解式(1)微分方程可得到式(2)：
Figure BDA0001792262550000079
记待解算数据时长为T，将时间段0～T以采样周期dt为间隔划分为多个时刻点t0,t1,t2...tm，k＝0,1,2,...,m。则式(2)中
Figure BDA00017922625500000710
为tk时刻的b系相对于tk-1时刻的b系的姿态矩阵；
Figure BDA00017922625500000711
为tk-1时刻陀螺仪输出，dt为采样周期。最终
Figure BDA00017922625500000712
计算时t＝tk，k＝1,2,3,...,m。
Figure BDA00017922625500000713
炮弹速度v1(t)和v2(t)计算方法如下：
考虑到加速度计常值误差Ba，有：
Figure BDA0001792262550000081
上式计算时取t＝tk，fb为炮弹加速度计输出；将
Figure BDA0001792262550000082
记为α0，
Figure BDA0001792262550000083
记为χ0。
根据炮弹上搭载的GPS组件可得到弹体位置信息经度λ与纬度L，东向、北向、天向速度分别为VE,VN,VU。则n系相对in系的角速度
Figure BDA0001792262550000084
可计算如下：
Figure BDA0001792262550000085
其中，RN为地球子午圈曲率半径，ωie为地球自转角速度，RE为地球卯酉圈半径。参照式(2)的方法，由
Figure BDA0001792262550000086
可计算出
Figure BDA00017922625500000814
Figure BDA0001792262550000087
其中，
Figure BDA00017922625500000815
为tk时刻的n系相对于tk-1时刻的n系的姿态矩阵；
Figure BDA0001792262550000088
为t＝tk-1时
Figure BDA0001792262550000089
的值，dt为采样周期。最终
Figure BDA00017922625500000810
计算时t＝tk，k＝1,2,3,...,m。则
Figure BDA00017922625500000811
其中，Vn(t)为t时刻的n系炮弹速度，Vn(0)为起始时刻的n系炮弹速度，
Figure BDA00017922625500000812
gn＝[0 0 g]T，g为重力加速度。
Figure BDA00017922625500000813
Figure BDA00017922625500000816
ωie为地球自转角速度。
2、根据四元数相关性质，定义变量X，并构建非线性函数F(X)＝0具体包括：
将α0，Ba和v2(t)扩充成零标量四元数，即定义α＝[0 α0 T]T，ba＝[0 BaT]T,V2(t)＝[0 v2(t)T]T。χ0扩充成4阶方阵，即定义
Figure BDA0001792262550000091
其中O1×3与O3×1分别为1×3和3×1的零矩阵。定义变量X＝[qT ba T u]T，q是4维列向量，代表in系至ib系的变化四元数，u为待定系数。
记四元数q＝[s ηT]T，q*＝[s -ηT]T，s为q的标量部分，η为q的矢量部分，定义四元数q的变换矩阵如下
Figure BDA0001792262550000092
其中，I为3×3单位矩阵，η×为η对应的叉乘矩阵。
根据
Figure BDA0001792262550000093
可定义W＝(M'(α)-M(V2(t)))q+M(q)(χba)，
再对q进行模值约束qqT＝1，引入拉格朗日乘子式，可构造函数：
F(X)＝∑WTW-u(qTq-1) (7)
则用牛顿迭代法解F(X)＝0，即解出q与ba，方法如下:
3、根据v1(t)和v2(t)，计算F(X)对应的Jacobian矩阵和Hessian矩阵，求函数F(X)一阶偏导数和二阶偏导数，方法如下：
F(X)一阶偏导数：
Figure BDA0001792262550000094
Figure BDA0001792262550000095
Figure BDA0001792262550000096
F(X)二阶偏导数：
Figure BDA0001792262550000101
Figure BDA0001792262550000102
其中，g1＝[1 0 0 0]T，g2＝[0 1 0 0]T，g3＝[0 0 1 0]T，g4＝[0 0 0 1]T。
Figure BDA0001792262550000103
Figure BDA0001792262550000104
Figure BDA0001792262550000105
Figure BDA0001792262550000106
Jacobian矩阵J和Hessian矩阵H记为
Figure BDA0001792262550000107
4、X的递推方法如下：
起始时，取X0＝[1 0 0 0 0 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 BDA0001792262550000108
Figure BDA0001792262550000111
再由下式可得到
Figure BDA0001792262550000112
Figure BDA0001792262550000113
其中，
Figure BDA0001792262550000114
为t时刻n系相对b系的姿态矩阵；
Figure BDA0001792262550000115
为t时刻ib系相对b系的姿态矩阵、
Figure BDA0001792262550000116
为t时刻n系相对in系的姿态矩阵，均已在前文求出。矩阵
Figure BDA0001792262550000117
为3阶方阵，若其中各元素可记为：
Figure BDA0001792262550000118
则炮弹的姿态角由下式解出:
Figure BDA0001792262550000119
φ，θ，γ分别是炮弹的航向角，纵摇角，横滚角。
从最终得到的Xk中取第6，7，8个元素组成Ba，即为炮弹三轴加速度计的常值误差。
如上所述，尽管参照特定的优选实施例已经表示和表述了本发明，但其不得解释为对本发明自身的限制。在不脱离所附权利要求定义的本发明的精神和范围前提下，可对其在形式上和细节上作出各种变化。

Claims (9)

1.一种基于牛顿迭代法的炮弹姿态和加速度计常值误差方法，其特征在于，该方法包括以下步骤：
(1)根据陀螺仪和GPS提供的炮弹速度和位置，计算t时刻载体惯性系ib系相对于载体系b的炮弹姿态矩阵
Figure FDA0003416913830000011
导航系n相对于导航惯性系in的炮弹姿态矩阵
Figure FDA0003416913830000012
(2)由陀螺仪和加速度计测量值计算ib系和in系下的炮弹速度v1(t)和v2(t)；
(3)定义变量X＝[qT ba T u]T，并构建非线性函数F(X)＝0，其中，q为in系至ib系的变化四元数，ba＝[0 BaT]T，Ba为炮弹上三轴加速度计的常值误差，u为待定系数；
(4)根据v1(t)和v2(t)，计算F(X)对应的Jacobian矩阵和Hessian矩阵，求函数F(X)一阶偏导数和二阶偏导数；
(5)利用牛顿法迭代法并根据
Figure FDA0003416913830000013
计算
Figure FDA0003416913830000014
对应的姿态角四元数q与加速度计常值误差，
Figure FDA0003416913830000015
为t时刻n系相对b系的姿态矩阵，
Figure FDA0003416913830000016
为in系相对于ib系的姿态矩阵；
步骤(1)中，炮弹姿态矩阵
Figure FDA0003416913830000017
和
Figure FDA0003416913830000018
计算方法如下：
设初始时刻t0时，b系与ib系间姿态矩阵为
Figure FDA0003416913830000019
I为3×3的单位矩阵；
记t时刻陀螺仪的输出值为
Figure FDA00034169138300000110
即t时刻的b系相对于ib系的角速度在b系上的投影值，由此跟踪b系相对于ib系的变化：
Figure FDA00034169138300000111
其中，
Figure FDA00034169138300000112
为矩阵
Figure FDA00034169138300000113
的变化率，
Figure FDA00034169138300000114
“×”表示三维矢量对应的叉乘矩阵变换，设
Figure FDA00034169138300000115
其中a，b，c分别表示炮弹沿三轴的旋转角速度，则
Figure FDA00034169138300000116
采用毕卡法解式(1)微分方程可得到式(2)：
Figure FDA00034169138300000117
记待解算数据时长为T，将时间段0～T以采样周期dt为间隔划分为多个时刻点t0,t1,t2...tm，k＝0,1,2,...,m，则式(2)中
Figure FDA0003416913830000021
为tk时刻的b系相对于tk-1时刻的b系的姿态矩阵；
Figure FDA0003416913830000022
Figure FDA0003416913830000023
为tk-1时刻陀螺仪输出，dt为采样周期，最终
Figure FDA0003416913830000024
由GPS输出的炮弹位置信息纬度L，东向、北向、天向速度分别为VE,VN,VU，则n系相对in系的角速度
Figure FDA0003416913830000025
可计算如下：
Figure FDA0003416913830000026
其中，RN为地球子午圈曲率半径，ωie为地球自转角速度，RE为地球卯酉圈半径，根据公式(2)的计算方法，由
Figure FDA0003416913830000027
可计算出
Figure FDA0003416913830000028
Figure FDA0003416913830000029
其中，
Figure FDA00034169138300000210
为tk时刻的n系相对于tk-1时刻的n系的姿态矩阵；
Figure FDA00034169138300000211
Figure FDA00034169138300000212
为t＝tk-1时
Figure FDA00034169138300000213
的值，dt为采样周期，则
Figure FDA00034169138300000214
Figure FDA00034169138300000215
2.根据权利要求1所述的一种基于牛顿迭代法的炮弹姿态和加速度计常值误差方法，其特征在于，步骤(2)中，炮弹速度v1(t)和v2(t)计算方法如下：
Figure FDA00034169138300000216
取t＝tk，k＝12,3,...,m，fb为炮弹加速度计输出；将
Figure FDA00034169138300000217
记为α0，
Figure FDA00034169138300000218
记为χ0；
Figure FDA0003416913830000031
其中，Vn(t)为t时刻的n系炮弹速度，Vn(0)为起始时刻的n系炮弹速度，
Figure FDA0003416913830000032
gn＝[0 0 g]T，g为重力加速度，
Figure FDA0003416913830000033
Figure FDA0003416913830000034
ωie为地球自转角速度。
3.根据权利要求2所述的一种基于牛顿迭代法的炮弹姿态和加速度计常值误差方法，其特征在于，步骤(3)中，定义变量X＝[qT ba T u]T，并构建非线性函数F(x)＝0，具体方法如下：定义α＝[0 α0 T]T，ba＝[0 BaT]T,V2(t)＝[0 v2(t)T]T，
Figure FDA0003416913830000035
其中，O1×3与O3×1分别为1×3和3×1的零矩阵，定义变量X＝[qT ba T u]T，q是4维列向量，代表in系至ib系的变化四元数，u为待定系数；
记四元数q＝[s ηT]T，q*＝[s -ηT]T，s为q的标量部分，η为q的矢量部分，定义四元数q的变换矩阵如下：
Figure FDA0003416913830000036
其中，I为3×3单位矩阵，η×为η对应的叉乘矩阵；
根据
Figure FDA0003416913830000037
定义W＝(M'(α)-M(V2(t)))q+M(q)(χba)；
对q进行模值约束qqT＝1，引入拉格朗日乘子式，构建非线性函数如下：
F(X)＝∑WTW-u(qTq-1)＝0 (7)。
4.根据权利要求3所述的一种基于牛顿迭代法的炮弹姿态和加速度计常值误差方法，其特征在于，步骤(4)中，根据v1(t)和v2(t)，求函数F(X)一阶偏导数和二阶偏导数，方法如下：
F(X)一阶偏导数：
Figure FDA0003416913830000038
Figure FDA0003416913830000041
Figure FDA0003416913830000042
F(X)二阶偏导数：
Figure FDA0003416913830000043
Figure FDA0003416913830000044
其中，I4×4是4阶单位矩阵，g1＝[1 0 0 0]T，g2＝[0 1 0 0]T，g3＝[0 0 1 0]T，g4＝[0 00 1]T；
Figure FDA0003416913830000045
Figure FDA0003416913830000046
Figure FDA0003416913830000047
Figure FDA0003416913830000048
5.根据权利要求4所述的一种基于牛顿迭代法的炮弹姿态和加速度计常值误差方法，其特征在于，步骤(4)中，根据v1(t)和v2(t)，计算F(X)对应的Jacobian矩阵和Hessian矩阵方法如下：Jacobian矩阵J和Hessian矩阵H记为：
Figure FDA0003416913830000049
6.根据权利要求5所述的一种基于牛顿迭代法的炮弹姿态和加速度计常值误差方法，其特征在于，步骤(5)中，利用牛顿法迭代法，计算四元数q，方法如下：
起始时，取X0＝[1 0 0 0 0 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的四个元素。
7.根据权利要求6所述的一种基于牛顿迭代法的炮弹姿态和加速度计常值误差方法，其特征在于，步骤(5)中，
Figure FDA00034169138300000512
为t时刻n系相对b系的姿态矩阵计算方法如下：
计算
Figure FDA0003416913830000051
Figure FDA0003416913830000052
由下式得到
Figure FDA0003416913830000053
Figure FDA0003416913830000054
其中，
Figure FDA0003416913830000055
为t时刻n系相对b系的姿态矩阵；
Figure FDA0003416913830000056
为t时刻ib系相对b系的姿态矩阵、
Figure FDA0003416913830000057
为t时刻n系相对in系的姿态矩阵，
Figure FDA0003416913830000058
为in系相对于ib系的姿态矩阵。
8.根据权利要求7所述的一种基于牛顿迭代法的炮弹姿态和加速度计常值误差方法，其特征在于，步骤(5)中，炮弹姿态角计算方法如下：矩阵
Figure FDA0003416913830000059
为3阶方阵，其各个元素记为：
Figure FDA00034169138300000510
则t时刻的炮弹的姿态角由下式解出：
Figure FDA00034169138300000511
其中，φ，θ，γ分别是炮弹的航向角，纵摇角，横滚角。
9.根据权利要求7所述的一种基于牛顿迭代法的炮弹姿态和加速度计常值误差方法，其特征在于，步骤(5)中，加速度计的常值误差计算方法如下：从最终得到的Xk中取第6，7，8个元素组成Ba，即为t时刻的炮弹三轴加速度计的常值误差。
CN201811041674.0A 2018-09-07 2018-09-07 一种基于牛顿迭代法的炮弹姿态和加速度计常值误差估计方法 Active CN109211230B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201811041674.0A CN109211230B (zh) 2018-09-07 2018-09-07 一种基于牛顿迭代法的炮弹姿态和加速度计常值误差估计方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201811041674.0A CN109211230B (zh) 2018-09-07 2018-09-07 一种基于牛顿迭代法的炮弹姿态和加速度计常值误差估计方法

Publications (2)

Publication Number Publication Date
CN109211230A CN109211230A (zh) 2019-01-15
CN109211230B true CN109211230B (zh) 2022-02-15

Family

ID=64987125

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201811041674.0A Active CN109211230B (zh) 2018-09-07 2018-09-07 一种基于牛顿迭代法的炮弹姿态和加速度计常值误差估计方法

Country Status (1)

Country Link
CN (1) CN109211230B (zh)

Families Citing this family (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109916399B (zh) * 2019-04-04 2019-12-24 中国人民解放军火箭军工程大学 一种阴影下的载体姿态估计方法
CN110006456B (zh) * 2019-04-24 2021-05-14 北京星网宇达科技股份有限公司 一种检测车对准方法、装置和设备
CN110108301B (zh) * 2019-05-14 2020-12-01 苏州大学 模值检测动基座鲁棒对准方法
CN110061675A (zh) * 2019-05-30 2019-07-26 东南大学 一种永磁同步电机全速范围无位置传感器控制方法
CN114090944A (zh) * 2021-10-26 2022-02-25 北京大学 一种基于非易失性存储器阵列的偏微分方程求解器及方法

Citations (11)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101413800A (zh) * 2008-01-18 2009-04-22 南京航空航天大学 导航/稳瞄一体化系统的导航、稳瞄方法
CN102486377A (zh) * 2009-11-17 2012-06-06 哈尔滨工程大学 一种光纤陀螺捷联惯导系统初始航向的姿态获取方法
CN103235328A (zh) * 2013-04-19 2013-08-07 黎湧 一种gnss与mems组合导航的方法
CN103344259A (zh) * 2013-07-11 2013-10-09 北京航空航天大学 一种基于杆臂估计的ins/gps组合导航系统反馈校正方法
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传感器组的姿态检测数据融合方法
CN105180728A (zh) * 2015-08-27 2015-12-23 北京航天控制仪器研究所 基于前数据的旋转制导炮弹快速空中对准方法
CN105258698A (zh) * 2015-10-13 2016-01-20 北京航天控制仪器研究所 一种高动态自旋制导炮弹空中组合导航方法
CN105865455A (zh) * 2016-06-08 2016-08-17 中国航天空气动力技术研究院 一种利用gps与加速度计计算飞行器姿态角的方法
CN108051866A (zh) * 2017-10-30 2018-05-18 中国船舶重工集团公司第七0七研究所 基于捷联惯性/gps组合辅助水平角运动隔离的重力测量方法

Patent Citations (11)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101413800A (zh) * 2008-01-18 2009-04-22 南京航空航天大学 导航/稳瞄一体化系统的导航、稳瞄方法
CN102486377A (zh) * 2009-11-17 2012-06-06 哈尔滨工程大学 一种光纤陀螺捷联惯导系统初始航向的姿态获取方法
CN103235328A (zh) * 2013-04-19 2013-08-07 黎湧 一种gnss与mems组合导航的方法
CN103344259A (zh) * 2013-07-11 2013-10-09 北京航空航天大学 一种基于杆臂估计的ins/gps组合导航系统反馈校正方法
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传感器组的姿态检测数据融合方法
CN105180728A (zh) * 2015-08-27 2015-12-23 北京航天控制仪器研究所 基于前数据的旋转制导炮弹快速空中对准方法
CN105258698A (zh) * 2015-10-13 2016-01-20 北京航天控制仪器研究所 一种高动态自旋制导炮弹空中组合导航方法
CN105865455A (zh) * 2016-06-08 2016-08-17 中国航天空气动力技术研究院 一种利用gps与加速度计计算飞行器姿态角的方法
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
CN109211230A (zh) 2019-01-15

Similar Documents

Publication Publication Date Title
CN109211230B (zh) 一种基于牛顿迭代法的炮弹姿态和加速度计常值误差估计方法
CN106990426B (zh) 一种导航方法和导航装置
CN109211231B (zh) 一种基于牛顿迭代法的炮弹姿态估计方法
CN109059914B (zh) 一种基于gps和最小二乘滤波的炮弹滚转角估计方法
CN104374388B (zh) 一种基于偏振光传感器的航姿测定方法
CN107314718A (zh) 基于磁测滚转角速率信息的高速旋转弹姿态估计方法
CN105180728B (zh) 基于前数据的旋转制导炮弹快速空中对准方法
CN105486307B (zh) 针对机动目标的视线角速率估计方法
CN108007477B (zh) 一种基于正反向滤波的惯性行人定位系统误差抑制方法
US8185261B2 (en) Systems and methods for attitude propagation for a slewing angular rate vector
CN107036598A (zh) 基于陀螺误差修正的对偶四元数惯性/天文组合导航方法
CN105737823A (zh) 基于五阶ckf的gps/sins/cns组合导航方法
CN109211232B (zh) 一种基于最小二乘滤波的炮弹姿态估计方法
CN103398713A (zh) 一种星敏感器/光纤惯性设备量测数据同步方法
CN105806363A (zh) 基于srqkf的sins/dvl水下大失准角对准方法
CN105115508A (zh) 基于后数据的旋转制导炮弹快速空中对准方法
CN113340298B (zh) 一种惯导和双天线gnss外参标定方法
CN109343550A (zh) 一种基于滚动时域估计的航天器角速度的估计方法
CN111189474A (zh) 基于mems的marg传感器的自主校准方法
CN105241319B (zh) 一种高速自旋制导炮弹空中实时对准方法
CN111680462A (zh) 基于空间目标在光学相平面位置变化的制导方法和系统
CN111121820B (zh) 基于卡尔曼滤波的mems惯性传感器阵列融合方法
CN103123487B (zh) 一种航天器姿态确定方法
CN108871319B (zh) 一种基于地球重力场与地磁场序贯修正的姿态解算方法
CN104613984B (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