CN114001602A - 基于四元数卡尔曼滤波去噪融合的火箭炮扰动检测方法 - Google Patents

基于四元数卡尔曼滤波去噪融合的火箭炮扰动检测方法 Download PDF

Info

Publication number
CN114001602A
CN114001602A CN202111245042.8A CN202111245042A CN114001602A CN 114001602 A CN114001602 A CN 114001602A CN 202111245042 A CN202111245042 A CN 202111245042A CN 114001602 A CN114001602 A CN 114001602A
Authority
CN
China
Prior art keywords
disturbance
rocket
angle
rocket gun
sensor
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.)
Pending
Application number
CN202111245042.8A
Other languages
English (en)
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.)
Northeastern University Qinhuangdao Branch
Original Assignee
Northeastern University Qinhuangdao Branch
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 Northeastern University Qinhuangdao Branch filed Critical Northeastern University Qinhuangdao Branch
Priority to CN202111245042.8A priority Critical patent/CN114001602A/zh
Publication of CN114001602A publication Critical patent/CN114001602A/zh
Pending legal-status Critical Current

Links

Images

Classifications

    • F—MECHANICAL ENGINEERING; LIGHTING; HEATING; WEAPONS; BLASTING
    • F42—AMMUNITION; BLASTING
    • F42B—EXPLOSIVE CHARGES, e.g. FOR BLASTING, FIREWORKS, AMMUNITION
    • F42B35/00—Testing or checking of ammunition
    • F42B35/02—Gauging, sorting, trimming or shortening cartridges or missiles
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01D—MEASURING NOT SPECIALLY ADAPTED FOR A SPECIFIC VARIABLE; ARRANGEMENTS FOR MEASURING TWO OR MORE VARIABLES NOT COVERED IN A SINGLE OTHER SUBCLASS; TARIFF METERING APPARATUS; MEASURING OR TESTING NOT OTHERWISE PROVIDED FOR
    • G01D21/00—Measuring or testing not otherwise provided for
    • G01D21/02—Measuring two or more variables by means not covered by a single other subclass
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
    • G06F17/10—Complex mathematical operations
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F30/00—Computer-aided design [CAD]
    • G06F30/20—Design optimisation, verification or simulation

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Theoretical Computer Science (AREA)
  • General Engineering & Computer Science (AREA)
  • Data Mining & Analysis (AREA)
  • Mathematical Physics (AREA)
  • Mathematical Analysis (AREA)
  • Mathematical Optimization (AREA)
  • Pure & Applied Mathematics (AREA)
  • Databases & Information Systems (AREA)
  • Software Systems (AREA)
  • Computational Mathematics (AREA)
  • Algebra (AREA)
  • Computer Hardware Design (AREA)
  • Evolutionary Computation (AREA)
  • Geometry (AREA)
  • Aerodynamic Tests, Hydrodynamic Tests, Wind Tunnels, And Water Tanks (AREA)

Abstract

本发明属于火箭炮扰动检测的军事技术领域，公开了基于四元数卡尔曼滤波去噪融合的火箭炮扰动检测方法，利用其自身结构进行瞬时解析、瞬时更替；采用四元数、卡尔曼滤波、小波阈值去噪算法，消除了外界的干扰融合角度，同时基于上述算法运用积分学计算位移大大消除传统方法所带来的积分误差，使得数据更具有时效性，且更加精准；本发明利用刚体运动学及机器人学的构造进行扰动建模，可测量任意火箭炮炮管的扰动值，更具有效率性和节约成本，操作起来比较容易。

Description

基于四元数卡尔曼滤波去噪融合的火箭炮扰动检测方法
技术领域
本发明属于火箭炮扰动检测的军事技术领域，具体涉及基于四元数卡尔曼滤波去噪融合的火箭炮扰动检测方法。
背景技术
火箭武器系统发射过程中，火箭弹点火后开始沿着定向器向前滑动。在此过程中，存在推力、偏心力、闭锁解脱力、弹管间撞击力和摩擦力及火箭弹移动形成的重力对耳轴的力矩等各类载荷。火箭弹与定向器组成的系统在这些载荷的共同作用下发生振动，其自身的运动受到干扰，相应的运动参数和姿态会发生变化。以火箭弹后定心部离开定向器的瞬间为初始时刻，自该时刻开始，火箭弹弹轴由于受到外部干扰，在俯仰、偏航和滚转三个方向上各产生角度、角速度和角加速度三个变动量。通常称这一时刻为起始时刻，这种干扰为起始扰动，扰动的产生会导致火箭弹命中精度产生影响，从而降低炮弹的命中精度。
传统的测扰动方法多数为测某一炮管的扰动，这样做能保证单一炮管扰动检测较标准，但是实施起来多为不方便，且火箭炮炮管居多，若一一安装传感器实施起来较为费力；单一进行模型的建立不具备普遍性，且只能测得一个炮管的扰动量无法进行多个炮管的测量,进而成本费用较高。
发明内容
为解决上述问题，本发明提供基于四元数卡尔曼滤波去噪融合的火箭炮扰动检测方法，利用其自身结构进行瞬时解析、瞬时更替。利用四元数及火箭炮旋转矩阵这一特性消除了瞬时加速度的变化，采用四元数、卡尔曼滤波、小波阈值去噪算法，消除了外界的干扰融合角度，同时基于上述算法运用积分学计算位移大大消除传统方法所带来的积分误差，使得数据更具有时效性，且更加精准，避免了因其他设备如CCD摄像机，如高敏探测器PSD等所带来的机器误差，且比非接触式测量方式更具有说服力。因数据时刻反映着扰动的测量值，使本发明的扰动模型与其他模型相比更具有灵活性；本发明利用刚体运动学及机器人学的构造进行扰动建模，可测量任意火箭炮炮管的扰动值，更具有效率性和节约成本，操作起来比较容易。
本发明的基于四元数卡尔曼滤波去噪融合的火箭炮扰动检测方法具体步骤如下：
步骤1、根据火箭炮起落架的旋转轴和旋转角确定旋转矩阵，采集三轴加速度计传感器所测加速度数据，采用四元数法进行空间坐标转换，获得火箭炮起落架传感器的加速度数据；
步骤2、采集三轴陀螺仪传感器所测角速度数据、三轴加速度计传感器所测加速度数据、三轴磁力计传感器所测磁力计数据，利用四元数法、卡尔曼滤波算法及小波阈值去噪算法计算俯仰角、航向角及滚转角的三轴角度数据；
步骤3、通过步骤2获得的三轴角度数据，利用数据融合位移算法求得火箭炮起落架传感器待测位置的三轴扰动位移量；
步骤4、根据步骤1和步骤3获得的火箭炮起落架传感待测位置的三轴位移量和角度数据，建立火箭炮扰动模型，以推导出火箭炮定向器的扰动量；
进一步的，所述步骤2中所述小波阈值去噪算法需建立一个含噪声的一维信号可用的模型：
S(t)＝f(t)+σ.e(t)t＝0,1,2...,n-1
f(t)为真实的信号，e(t)为噪声，S(t)为含噪声的信号。
进一步的，所述小波阈值去噪具体过程如下：
(1)对含噪信号进行预处理；
(2)采用小波变换进行多尺度分解；
(3)对各尺度上的小波系数进行除噪处理；
(4)小波逆变换重构信号，得到除噪后的信号；
进一步的，所述数据融合位移算法是根据牛顿第二定律对物体加速度积分一次得到速度值，同时根据数学微积分原理对三轴加速度数据计算扰动位移量，具体过程为：
设定
Figure BDA0003320658280000031
Figure BDA0003320658280000032
进一步的，所述火箭炮扰动模型的建立方法为：定义传感器的位置坐标；计算传感器3的三个轴的角度变化量。
进一步的，所述传感器的位置坐标，即为P(x0，y0，z0)，根据起落架定点扰动量的三轴位移量以及三轴角度值，通过刚体运动学的微分运动推导微分平移和微分旋转如下：
c～cos，s～sin，vers～1-cos
Figure BDA0003320658280000033
Figure BDA0003320658280000034
Δ＝Trans(dx，dy，dz)·Rot(f，dθ)-I
其中I为单位阵；
Figure BDA0003320658280000041
进一步的，所述传感器3的三个轴的角度变化量分别为δx，δy，δz；火箭炮炮口某一点在坐标系ox1y1z1下的坐标为P1(x1，y1，z1)，火箭炮炮筒坐标系ox2y2z2是由ox1y1z1根据实际调整高低机和回转机的角度，按照PY(θ1，Ψ1)旋转得到的，根据刚体运动学可知A：
Figure BDA0003320658280000042
引入微分运动的等价变换，根据刚体运动学的dT＝ΔT和dT＝TTΔ，当两个坐标系等价时，ΔT＝TTΔ，T-1ΔT＝TΔ，根据dT＝ΔT，dA＝ΔA，把A带入上式有A-1ΔA＝TA其中：
Figure BDA0003320658280000043
Figure BDA0003320658280000044
可求得TA；根据TΔ的表达形式，可表达TA，结果如下：
Figure BDA0003320658280000051
其中：
Figure BDA0003320658280000052
公式中Tdx，Tdy，Tdz，Tδx，Tδy，Tδz为出炮口x、y、z位置坐标的扰动量。
本发明的有益效果是：
1、通过引入火箭炮发射瞬时其起落架振动所产生的旋转轴及旋转角度而推出的发射瞬时火箭炮起落架所产生的旋转矩阵，传感器的检测位置根据火箭炮的炮体结构安放，利用三轴加速度传感计检测出来的数据及四元数法进行空间转换，可计算转换后的加速度数据；采用了四元数空间坐标转换和角度补偿有效的减少了积分带来的误差积累，在火箭炮发射瞬时的情况下其时间足够小，更加有效的提高了位移的精度；
2、结合三轴陀螺仪传感器，三轴加速度传感器，三轴磁力计传感器所采集的数据，利用四元数法、卡尔曼滤波算法、小波阈值去噪算法，更加准确的融合角度数据；根据转换后的加速度数据、解析出来的角度数据及物理学加速度和位移的关系，运用数学积分学可得到测距位移结果。采用小波阈值去噪算法提高了因外界因素带来的噪声误差，更加有效的提高了角度的测量；
3、利用四元数法、卡尔曼滤波算法、小波阈值去噪算法可得到火箭炮发射瞬时起落架所产生的扰动量即三轴角度偏移量和三轴运动的位移量，根据机器人学及刚体运动学理论对火箭炮的起落架和定向器进行扰动建模；
4、该扰动模型可根据刚体运动学知识由起落架发射瞬时产生的扰动量，推其火箭炮任意定向器发射时瞬时振动所产生的扰动量，可进一步有效的提高火箭弹的命中精度。
附图说明
图1是本发明提供的基于四元数卡尔曼滤波去噪融合的火箭炮扰动检测方法流程图；
图2是本发明提供的基于四元数卡尔曼滤波去噪融合的火箭炮扰动检测方法对俯仰角去噪前后对比视图；
图3是本发明提供的基于四元数卡尔曼滤波去噪融合的火箭炮扰动检测方法对航向角去噪前后对比视图；
图4是本发明提供的基于四元数卡尔曼滤波去噪融合的火箭炮扰动检测方法对滚转角去噪前后对比视图；
图5是本发明提供的基于四元数卡尔曼滤波去噪融合的火箭炮扰动检测方法的火箭炮车体坐标空间示意图。
图5中元件标号说明：
1-定向器；2-起落架；3-传感器；4-高低机；回转机。
具体实施方式
以下通过特定的具体实例说明的实施方式，本领域技术人员可由本说明书所揭露的内容轻易地了解本发明的其他优点与功效，本发明还可以通过另外不同的具体实施方式加以实施或应用，本说明书中的各项细节也可以基于不同观点与应用，在没有背离本发明的精神下进行各种修饰或改变。需说明的是，在不冲突的情况下，以下实施例及实施中的特征可以相互组合。需要说明的是，以下实施例中所提供的图示仅以示意方式说明本发明的基本构想。
实施例：
本发明采用四元数算法解算火箭炮发射时空间三轴坐标转换问题，根据火箭炮车体结构安放传感器具体位置为起落架上的定点处，结合三轴加速度传感器所采集的数据利用四元数法进行空间转换计算过滤后的加速度数据，实行瞬时动态替更。
如图1所示：
步骤1、根据火箭炮起落架的旋转轴和旋转角确定旋转矩阵，采集三轴加速度计传感器所测加速度数据，采用四元数法进行空间坐标转换，获得火箭炮起落架传感器的加速度数据；
1)火箭炮起落架的旋转轴和旋转角的旋转矩阵的推导参考《火炮炮管静态定向角测量系统研究》[1]推得旋转矩阵S的过程；
Figure BDA0003320658280000071
其中pxpypz是虚轴量；
2)采集三轴加速度计传感器所测加速度数据；
3)采用四元数法进行空间坐标转换，进行火箭炮起落架传感器待测位置的加速度数据计算；
四元数是由实数元素加上三个复数元素i、j、k组成的，即：
Q＝q0+g1i+q2j+q3k
四元数法的矩阵法为：
Figure BDA0003320658280000081
其中q0表示转角一半的余弦值，rx ry rz分别表示瞬时转动轴与坐标系间的方向余弦值。坐标系转换中，任意坐标系都可以由围绕3轴旋转角度来获得。以载体坐标系作为动坐标系，以导航坐标系作为定标系，姿态角即为欧拉角，通常用导航坐标系到载体坐标系的转换矩阵算欧拉角，其包括：俯仰角、航向角及滚转角。
因此定义
Figure BDA0003320658280000082
把四元数法的矩阵法代入到旋转矩阵S中得：
Figure BDA0003320658280000083
将采集的三轴加速度计传感器所测加速度数据输入到旋转矩阵s中，即可得出转换后的火箭炮起落架传感器待测位置的三轴加速度值。
步骤2、采集三轴陀螺仪传感器所测角速度数据、三轴加速度计传感器所测加速度数据、三轴磁力计传感器所测磁力计数据，利用四元数法、卡尔曼滤波算法及小波阈值去噪算法计算俯仰角、航向角及滚转角。
1)采用四元数算法
四元数法上述过程已经说明，这里不在进行描述。
2)采用卡尔曼滤波算法
卡尔曼滤波算法有5大核心公式。
先验估计以及协方差矩阵的预测方程如下，卡尔曼滤波器估计的状态x由下式决定：
xk＝Axk-1+Buk-1+wk-1
其中A为状态转移矩阵，B为控制输入量u与状态x的关联阵，w为过程噪声矩阵。其中估计值与测量值是由传感器输出的数据，结合四元数法进行解姿作为估计角度，同时根据加速度计和磁力计求解姿态来确定的，这里不再描述加速度计和磁力计的解姿过程。
zk＝Hxk+vk
Z为测量向量，H为状态向量x和z的关联矩阵，v为测量噪声矩阵。其中w和v可看作相互独立且满足正态分布的白噪声。
p(w)～N(0，Q)，p(v)～N(0，R)其中过程噪声协方差矩阵Q和测量噪声协方差矩阵R是随时间变化的，可看作是常量。
协方差预测方程：
Pk-＝APk-1AT+Q
R和Q的取值会影响滤波效果和响应速度，通常情况下R值取得越小，响应越快，收敛越迅速。Q取值越小，抑制噪声的能力越强，同时收敛和响应也会变慢，因此有必要设定优化初值。
卡尔曼增益方程：
Figure BDA0003320658280000091
方差更新方程：
Figure BDA0003320658280000092
协方差更新方程：
Figure BDA0003320658280000093
如图2所示，以俯仰角为例，根据卡尔曼滤波算法将采集到的三轴陀螺仪传感器所测角速度数据、三轴加速度计传感器所测加速度数据、三轴磁力计传感器所测磁力计数据拷贝到MATLAB.mat文件里。对四元数算法和卡尔曼滤波器来进行仿真。从仿真结果可看出，带有白噪声解算出来的俯仰角对于实际选取角度有很大的影响，因此需要进行去噪过程的处理。
3)采用小波阈值去噪算法
传统的去噪方法通常有线性滤波和非线性滤波等方法，传统方法去噪的缺点在于信号变换后它的熵增高，无法刻画信号的非平稳特性。因此采用小波阈值去噪使其信号具有良好的时频特性。
我们建立一个含噪声的一维信号可用的模型：
S(t)＝f(t)+σ.e(t)t＝0，1，2...，n-1
f(t)为真实的信号，e(t)为噪声，S(t)为含噪声的信号。对于一维信号数据，所述小波阈值去噪具体过程如下：
(1)对含噪信号进行预处理；
(2)采用小波变换进行多尺度分解；
(3)对各尺度上的小波系数进行除噪处理；
(4)小波逆变换重构信号，得到除噪后的信号；
如图2-4所示，通过上述仿真可看出去噪效果比较明显。结合卡尔曼滤波算法优化的俯仰角、航向角及滚转角，再通过小波阈值去噪算法、在避免因角速度积分带来的角度累计误差的同时避免因外界环境带来的干扰，因此可获得多轴传感器融合的角度值。
步骤3、通过步骤2获得的三轴角度数据，利用数据融合位移算法求得火箭炮起落架传感器待测位置的三轴扰动位移量，进行多传感器融合新角度。
数据融合位移算法是根据牛顿第二定律对物体加速度积分一次得到速度值，同时根据数学微积分原理对三轴加速度数据计算扰动位移量，具体过程为：
设定
Figure BDA0003320658280000101
Figure BDA0003320658280000111
将步骤2获得的三轴角度数据代入该数据融合位移算法中即可得到火箭炮起落架传感器待测位置的三轴扰动位移量。
步骤4、根据步骤1和步骤3获得的火箭炮起落架传感器待测位置的三轴位移量和角度数据，进行扰动模型的建立，以推导出火箭炮定向器1的扰动量。
1)定义火箭炮坐标系
运用刚体运动学旋转矩阵来计算坐标位置，首先建立如图5所示的坐标系：地面坐标系oJLE。其中o点为火箭炮炮车的质量中心点，地面坐标系下的oJ方向为火箭炮车车头的正方向，即前进方向，也是基准射向，L轴为铅锤轴，向上为正，oL方向与地面垂直，oE方向与车体的侧面垂直即垂直于火箭炮炮车的正方向，上述坐标系为基准坐标系。
如图5所示，水平旋转回转机5及竖直调整高低机4可以确定起落架2的角度和高度。根据地面坐标系oJLE，可知图中回转机5在EoJ面上水平逆时针移动了ψ1，即航向角Ψ1，在LoJ面上竖直移动了θ1，即俯仰角θ1，上述过程为调校过程。
坐标系ox1y1z1的建立位置可根据实际起落架2与定向器1的长度、宽度和高度来建立，本发明选择在起落架2上的某点作为原点并建立ox1y1z1坐标系。传感器3的位置在坐标系ox1y1z1上方，这样可避免传感器3的位置时是(0，0，0)时带入扰动模型没有计算意义。其中ox1y1z1坐标系与地面坐标系oJLE完全平行，炮管坐标系ox2y2z2坐标系的方向建立是由坐标系ox1y1z1根据实际回转机5的平移ψ1，以及高低机4的竖直移动θ1来确立的。
2)建立火箭炮扰动模型：
根据ox1y1z1坐标系可得到传感器3的位置坐标，即为P(x0，y0，z0)，根据运动坐标系oxyz可知，由于扰动的影响产生了新的位置，上述算法已解析起落架2定点扰动量的三轴位移量以及三轴角度值。
根据刚体运动学的微分运动推导微分平移和微分旋转如下：
c～cos，s～sin，vers～1-cos
Figure BDA0003320658280000121
Figure BDA0003320658280000122
Δ＝Trans(dx，dy，dz)·Rot(f，dθ)-I
其中I为单位阵。
Figure BDA0003320658280000123
Figure BDA0003320658280000124
对于传感器3，其三个轴的角度变化量分别为δx，δy，δz。设测量点即火箭炮炮口某一点在坐标系ox1y1z1下的坐标为P1(x1，y1，z1)，火箭炮炮筒坐标系ox2y2z2是由ox1y1z1根据实际调整高低机4和回转机5的角度，按照PY(θ1，ψ1)旋转得到的，根据刚体运动学可知A为：
Figure BDA0003320658280000131
引入微分运动的等价变换，根据刚体运动学的dT＝ΔT和dT＝TTΔ，当两个坐标系等价时，ΔT＝TTΔ，T-1ΔT＝TΔ，根据dT＝ΔT，dA＝ΔA，把A带入上式有A-1ΔA＝TA其中：
Figure BDA0003320658280000132
Figure BDA0003320658280000133
可求得TA。根据TΔ的表达形式，可表达TA，结果如下：
Figure BDA0003320658280000134
其中：
Figure BDA0003320658280000135
公式中Tdx，Tdy，Tdz，Tδx，Tδy，Tδz为出炮口x、y、z位置坐标的扰动量。根据上述公式的分析，可推导扰动模型。
3)任意火箭炮定向器的扰动量；
当火箭炮点火发射时，将传感器获得的数据代入扰动模型中，可得到发射架的扰动、出炮口的扰动、出炮口的位置，可带入到火箭弹弹道轨迹方程中，从而为分析命中精度提供依据。
根据扰动模型的建立，结合信号处理的方法进行具体的分析：
定义初始值如下，传感器的位置坐标：P(X，Y，Z)＝(1，1，1)(单位：m)，其初始值的三个轴向角的扰动量：(φ，ψ，θ)＝(0.01，0.015，0.012)(单位：°)，发射管坐标系下的预测点坐标P1(x1，y1，z1)为(1.3，1.3，1.3)(单位：m)，根据定义坐标系下的两个位置坐标P和P1及根据信号检测分析得出的三轴角度及调校过程中炮管实际转动的角度，即可得到出炮口时的扰动变化量及位置坐标。
计算结果表明传感器位置的扰动量分别为3.4915e-05，-8.7225e-05，5.2304e-05(单位：m)；发射管预测点位置扰动量为-2.8339e-05，-1.0284e-04，2.322e-04(单位：m)；三轴角度扰动量为：6.1707e-05，3.3565e-04，1.6248e-04(单位：rad/s)折算成角度分别为0.003537°；0.0192°；0.009314°。
根据火箭炮管其位置的扰动量，可分析命中精度。根据上述扰动模型可知出炮口的位置，带入到炮弹弹道轨迹方程即可得到着弹点的位置。
为了分析着弹点的命中精度，引入火箭炮炮弹弹道轨迹模型，便于研究问题。模型如下：
Figure BDA0003320658280000151
其中θ为俯仰角，ψ为航向角，m可看做定质量，g为重力加速度9.8N/m2，G为物体的重力，P为推力，V为初速度，α、β为攻角与侧滑角，X为阻力，Y为升力，Z为侧力，X、Y、Z受空气密度ρ、实际的飞行速度V、自身结构的气动力参考面积s以及阻力系数cx、升力系数cy、侧力系数cz有关。其中cx、cy、cz为攻角和马赫数的函数，马赫数是实际飞行速度V与cs的函数，可根据大气象数据采取差值拟合的方式合成。
通常发射的第一发炮弹往往是精确的，可采用第一发炮弹作为精准射程，早期火箭炮的精度为射程的1/1000，随即火箭炮的精度为射程的1/300。采用精度为射程的1/300作为评估指标，出炮口的位置为P(1.3，1.3，1.3)，俯仰角初值为45°，航向角为30°，滚转角为15°。着弹点坐标为(223.5，0.0005563，-274.2)单位：m，其中y＝0.0005563可认为到达地面，由计算得出射程为353.7483m，其精度为1.1792m。可用坐标(223.5，-274.2)为圆心，以半径r＝1.1792(单位：m)做圆，在此圆内的点作为在精度域内的着弹点，此圆外的点作为在精度域外的着弹点为评估标准。
由于第一发炮弹发射后，导致后续发射炮弹的位置坐标和三轴角度都会发生变化。第二个炮口的初始坐标为(1.3-2.8339e-05,1.3-1.0284e-04,1.3+2.2322e-04),三轴角度为俯仰角45+0.009314°,航向角30+0.0192°,滚转角15+0.003537°,结合弹道轨迹模型得到着弹点位置坐标为散点1(223.8，0.000682，-273.9)，可知在精度范围内。第三发炮弹出炮口坐标可通过在炮筒坐标系下确立其它出炮口坐标，例如：位置坐标(1.3,1.3,1.5)及传感器测得的扰动角度为俯仰角0.3°、航向角0.2°、滚转角0.5°，带入到扰动模型。计算得出第三个炮口的初始条件为(1.3-0.0088,1.3+0.0060,1.5-0.0176)出炮口的俯仰角为45+0.5102°，航向角为30+0.2809°，滚转角为15-0.2121°，结合弹道轨迹模型得到着弹点位置坐标为散点2(224.2,0.001934,-271.7),可知在精度范围外。
通过上述计算结果验证了火箭炮扰动模型及炮弹命中精度算法的准确性，可见本发明对优化火箭炮的结构设计以及分析命中精度提供依据。
显然，本发明的上述实施例仅仅是为清楚地说明本发明所作的举例，而并非是对本发明的实施方式的限定。对于所属普通技术人员来说，在上述说明的基础上还可以做出其它不同形式的变化或变动。这里无需也无法对所有的实施方式予以穷举。凡在本发明的精神和原则之内所作的任何修改，等同替换和改进等，均应包含在本发明权利要求的保护范围之内。
[1]本发明参考文献：《兵工学报》第25卷第6期2004年11日出版的《火炮炮管静态定向角测量系统研究》的文章；作者：王春艳、王志坚(长春理工大学，吉林长春)。

Claims (7)

1.基于四元数卡尔曼滤波去噪融合的火箭炮扰动检测方法，其特征在于：该方法的具体步骤如下：
步骤1、根据火箭炮起落架的旋转轴和旋转角确定旋转矩阵，采集三轴加速度计传感器所测加速度数据，采用四元数法进行空间坐标转换，获得火箭炮起落架传感器的加速度数据；
步骤2、采集三轴陀螺仪传感器所测角速度数据、三轴加速度计传感器所测加速度数据、三轴磁力计传感器所测磁力计数据，利用四元数法、卡尔曼滤波算法及小波阈值去噪算法计算俯仰角、航向角及滚转角的三轴角度数据；
步骤3、通过步骤2获得的三轴角度数据，利用数据融合位移算法求得火箭炮起落架传感器待测位置的三轴扰动位移量；
步骤4、根据步骤1和步骤3获得的火箭炮起落架传感待测位置的三轴位移量和角度数据，建立火箭炮扰动模型，以推导出火箭炮定向器的扰动量。
2.根据权利要求1所述的基于四元数卡尔曼滤波去噪融合的火箭炮扰动检测方法，其特征在于：所述步骤2中所述小波阈值去噪算法需建立一个含噪声的一维信号可用的模型：
S(t)＝f(t)+σ.e(t) t＝0,1,2...,n-1
f(t)为真实的信号，e(t)为噪声，S(t)为含噪声的信号。
3.根据权利要求2所述的基于四元数卡尔曼滤波去噪融合的火箭炮扰动检测方法，其特征在于：所述小波阈值去噪具体过程如下：
(1)对含噪信号进行预处理；
(2)采用小波变换进行多尺度分解；
(3)对各尺度上的小波系数进行除噪处理；
(4)小波逆变换重构信号，得到除噪后的信号。
4.根据权利要求1所述的基于四元数卡尔曼滤波去噪融合的火箭炮扰动检测方法，其特征在于：所述数据融合位移算法是根据牛顿第二定律对物体加速度积分一次得到速度值，同时根据数学微积分原理对三轴加速度数据计算扰动位移量，具体过程为：
设定
Figure FDA0003320658270000021
Figure FDA0003320658270000022
5.根据权利要求1所述的基于四元数卡尔曼滤波去噪融合的火箭炮扰动检测方法，其特征在于：所述火箭炮扰动模型的建立方法为：定义传感器的位置坐标；计算传感器3的三个轴的角度变化量。
6.根据权利要求5所述的基于四元数卡尔曼滤波去噪融合的火箭炮扰动检测方法，其特征在于：所述传感器的位置坐标，即为P(x0,y0,z0),根据起落架定点扰动量的三轴位移量以及三轴角度值，通过刚体运动学的微分运动推导微分平移和微分旋转如下：
c～cos，s～sin,vers～1-cos
Figure FDA0003320658270000023
Figure FDA0003320658270000024
Δ＝Trans(dx,dy,dz)·Rot(f,dθ)-I
其中I为单位阵；
Figure FDA0003320658270000031
Figure FDA0003320658270000032
7.根据权利要求5所述的基于四元数卡尔曼滤波去噪融合的火箭炮扰动检测方法，其特征在于：所述传感器3的三个轴的角度变化量分别为δx,δy,δz；火箭炮炮口某一点在坐标系ox1y1z1下的坐标为P1(x1,y1,z1)，火箭炮炮筒坐标系ox2y2z2是由ox1y1z1根据实际调整高低机和回转机的角度，按照PY(θ1,ψ1)旋转得到的，根据刚体运动学可知A：
Figure FDA0003320658270000033
引入微分运动的等价变换，根据刚体运动学的dT＝ΔT和dT＝TTΔ，当两个坐标系等价时，ΔT＝TTΔ，T-1ΔT＝TΔ,根据dT＝ΔT,dΑ＝ΔΑ，把A带入上式有Α-1ΔΑ＝TΑ其中：
Figure FDA0003320658270000034
Figure FDA0003320658270000035
可求得TΑ；根据TΔ的表达形式，可表达TΑ,结果如下：
Figure FDA0003320658270000041
其中：
Figure FDA0003320658270000042
公式中Tdx，Tdy,Tdz,Tδx,Tδy,Tδz为出炮口x、y、z位置坐标的扰动量。
CN202111245042.8A 2021-10-26 2021-10-26 基于四元数卡尔曼滤波去噪融合的火箭炮扰动检测方法 Pending CN114001602A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202111245042.8A CN114001602A (zh) 2021-10-26 2021-10-26 基于四元数卡尔曼滤波去噪融合的火箭炮扰动检测方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202111245042.8A CN114001602A (zh) 2021-10-26 2021-10-26 基于四元数卡尔曼滤波去噪融合的火箭炮扰动检测方法

Publications (1)

Publication Number Publication Date
CN114001602A true CN114001602A (zh) 2022-02-01

Family

ID=79924053

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202111245042.8A Pending CN114001602A (zh) 2021-10-26 2021-10-26 基于四元数卡尔曼滤波去噪融合的火箭炮扰动检测方法

Country Status (1)

Country Link
CN (1) CN114001602A (zh)

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20220178658A1 (en) * 2019-04-11 2022-06-09 Israel Aerospace Industries Ltd. Method of controlling ejection of a missile from a canister and system therefor
CN116592816A (zh) * 2023-07-18 2023-08-15 中国人民解放军海军工程大学 基于旋转机构的数据获取方法、装置、设备及存储介质

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN105608865A (zh) * 2015-12-17 2016-05-25 安徽诺安信息科技有限公司 一种关联气象数据的imu数据采集处理系统
US20160349026A1 (en) * 2014-08-07 2016-12-01 U.S. Army Research Laboratory Attn: Rdrl-Loc-I Method and apparatus for gps-denied navigation of spin-stabilized projectiles
CN109373832A (zh) * 2018-12-07 2019-02-22 惠州学院 基于磁测滚转的旋转弹炮口初始参数测量方法
CN111854747A (zh) * 2020-08-25 2020-10-30 东南大学 一种载体大机动情况下的dvl辅助sins粗对准方法
CN112611394A (zh) * 2020-12-16 2021-04-06 西北工业大学 一种在发射坐标系下的飞行器姿态对准方法及系统
CN113483765A (zh) * 2021-05-24 2021-10-08 航天科工空间工程发展有限公司 一种卫星自主姿态确定方法

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20160349026A1 (en) * 2014-08-07 2016-12-01 U.S. Army Research Laboratory Attn: Rdrl-Loc-I Method and apparatus for gps-denied navigation of spin-stabilized projectiles
CN105608865A (zh) * 2015-12-17 2016-05-25 安徽诺安信息科技有限公司 一种关联气象数据的imu数据采集处理系统
CN109373832A (zh) * 2018-12-07 2019-02-22 惠州学院 基于磁测滚转的旋转弹炮口初始参数测量方法
CN111854747A (zh) * 2020-08-25 2020-10-30 东南大学 一种载体大机动情况下的dvl辅助sins粗对准方法
CN112611394A (zh) * 2020-12-16 2021-04-06 西北工业大学 一种在发射坐标系下的飞行器姿态对准方法及系统
CN113483765A (zh) * 2021-05-24 2021-10-08 航天科工空间工程发展有限公司 一种卫星自主姿态确定方法

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20220178658A1 (en) * 2019-04-11 2022-06-09 Israel Aerospace Industries Ltd. Method of controlling ejection of a missile from a canister and system therefor
CN116592816A (zh) * 2023-07-18 2023-08-15 中国人民解放军海军工程大学 基于旋转机构的数据获取方法、装置、设备及存储介质
CN116592816B (zh) * 2023-07-18 2023-09-29 中国人民解放军海军工程大学 基于旋转机构的数据获取方法、装置、设备及存储介质

Similar Documents

Publication Publication Date Title
CN107255924B (zh) 基于扩维模型的容积卡尔曼滤波提取捷联导引头制导信息的方法
CN107560613B (zh) 基于九轴惯性传感器的机器人室内轨迹跟踪系统及方法
CN106643737B (zh) 风力干扰环境下四旋翼飞行器姿态解算方法
CN101846510B (zh) 一种基于星敏感器和陀螺的高精度卫星姿态确定方法
CN106354901B (zh) 一种运载火箭质量特性及动力学关键参数在线辨识方法
CN114001602A (zh) 基于四元数卡尔曼滤波去噪融合的火箭炮扰动检测方法
CN109000642A (zh) 一种改进的强跟踪容积卡尔曼滤波组合导航方法
CN108731676B (zh) 一种基于惯性导航技术的姿态融合增强测量方法及系统
CN105865448A (zh) 一种基于imu的室内定位方法
CN110672078B (zh) 一种基于地磁信息的高旋弹丸姿态估计方法
CN101915580A (zh) 一种基于微惯性和地磁技术的自适应三维姿态定位方法
CN105486307B (zh) 针对机动目标的视线角速率估计方法
CN103453806B (zh) 基于雷达多普勒数据的弹丸章动角提取方法
CN108444468B (zh) 一种融合下视视觉与惯导信息的定向罗盘
CN110702113B (zh) 基于mems传感器的捷联惯导系统数据预处理和姿态解算的方法
Liu et al. A complementary filter based on multi-sample rotation vector for attitude estimation
CN108050999A (zh) 一种新息正交性的红外与地磁复合旋转弹体测姿方法
CN112697131B (zh) 基于视觉和惯性导航系统的井下移动装备定位方法及系统
CN111238474A (zh) 基于倾斜坐标系的捷联导引头非奇异视线角速度提取方法
CN109186614B (zh) 一种航天器间近距离自主相对导航方法
CN117932894A (zh) 一种多模导引头信息融合目标状态估计方法
CN103869097B (zh) 旋转弹航向角、俯仰角角速率测量方法
CN116611160A (zh) 基于一段实测弹道参数的无控飞行器在线实时特征参数辨识与弹道预报方法
CN104075710A (zh) 一种基于航迹预测的机动扩展目标轴向姿态实时估计方法
Ko et al. Moving vehicle attitude tracking algorithm based on mems inertial navigation system

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
WD01 Invention patent application deemed withdrawn after publication
WD01 Invention patent application deemed withdrawn after publication

Application publication date: 20220201