CN113031645B - 一种基于ukf滤波的弹体角速率增稳捷联制导方法 - Google Patents

一种基于ukf滤波的弹体角速率增稳捷联制导方法 Download PDF

Info

Publication number
CN113031645B
CN113031645B CN202110177354.3A CN202110177354A CN113031645B CN 113031645 B CN113031645 B CN 113031645B CN 202110177354 A CN202110177354 A CN 202110177354A CN 113031645 B CN113031645 B CN 113031645B
Authority
CN
China
Prior art keywords
angular rate
projectile
sight
line
frame
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
CN202110177354.3A
Other languages
English (en)
Other versions
CN113031645A (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.)
Shaanxi Zhongtian Rocket Technology Co ltd
Original Assignee
Shaanxi Zhongtian Rocket Technology Co ltd
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 Shaanxi Zhongtian Rocket Technology Co ltd filed Critical Shaanxi Zhongtian Rocket Technology Co ltd
Priority to CN202110177354.3A priority Critical patent/CN113031645B/zh
Publication of CN113031645A publication Critical patent/CN113031645A/zh
Application granted granted Critical
Publication of CN113031645B publication Critical patent/CN113031645B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/10—Simultaneous control of position or course in three dimensions
    • G05D1/107—Simultaneous control of position or course in three dimensions specially adapted for missiles
    • G05D1/108—Simultaneous control of position or course in three dimensions specially adapted for missiles animated with a rolling movement

Landscapes

  • Engineering & Computer Science (AREA)
  • Aviation & Aerospace Engineering (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Aiming, Guidance, Guns With A Light Source, Armor, Camouflage, And Targets (AREA)

Abstract

本发明提供一种基于UKF滤波的弹体角速率增稳捷联制导方法，通过建立视线角速率滤波模型，补偿弹体GPS与发射平台GPS之间的误差，再使用UKF滤波算法提取视线角速率，最后采用基于比例导引算法的增稳捷联制导律对弹体进行制导，通过弹体角速率反馈进行增稳，能够减小导航误差，降低导航误差对制导精度的影响，通过能够在不增加硬件成本的前提下，有效提高小型捷联导引头制导武器的制导算法精度与算法鲁棒性，具备工程可实施性，将平台导引头头替换成捷联头可大大节约武器制造成本，提升产品竞争力。

Description

一种基于UKF滤波的弹体角速率增稳捷联制导方法
技术领域
本发明属于制导控制系统设计领域，尤其涉及一种基于UKF滤波的弹体角速率增稳捷联制导方法。
背景技术
现代战争越来越追求伤亡最小化，察打一体无人作战方式受到的关注度越来越高。国内外市场上，各种军用无人机、无人船以及无人车不断涌现，此类无人平台对搭载的武器系统需求同步增长。半主动激光制导武器作为无人作战平台的主要武器之一，不同厂家研制的型号越来越多，同类产品的竞争也越来越激烈。
考虑到精准杀伤需求以及作战效费比，对精确制导武器命中精度与生产成本等提出了越来越高的要求。平台导引头由于价格较高，产品成本难以控制，捷联头体制得到越来越多的关注。
由于捷联导引头体制受产品量测噪声与导航精度影响，武器命中精度与脱靶量稳定性很难保证，针对捷联导引头制导算法的研究很多。积分比例导引直接利用导引头输出视线角进行制导，该算法简单，但制导精度略差、对弹体气动特性与导航误差较为敏感。速度追踪与弹体追踪制导算法均不需要提取视线角速率，算法结构简单，计算量小，但制导回路快速性不足，制导精度差且对导航误差非常敏感。基于扩张状态观测器、超螺旋等滤波算法提取实现角速率用于比例导引时，由于没有根据动力学进行状态预测，滤波器带宽稍微提高，算法鲁棒性相应的快速变差，对导航误差较为敏感。基于卡尔曼滤波与扩展卡尔曼滤波算法提取视线角速率时，由于滤波方程非线性，滤波精度有所损失，制导精度不能满足使用需求。基于无迹卡尔曼(UKF)与容积卡尔曼滤波(CKF)提取视线角速率方法有效提高了理论算法精度，但当前研究结果对导引头角度误差与寄生回路影响较为敏感。
发明内容
为解决上述问题，本发明提供一种基于UKF滤波的弹体角速率增稳捷联制导方法，能够有效提高制导算法精度与算法鲁棒性，具备工程可实施性，将平台导引头替换成捷联头可大大节约武器制造成本，提升产品竞争力。
一种基于UKF滤波的弹体角速率增稳捷联制导方法，采用设定的弹体角速率增稳捷联制导律获取弹体的纵向过载ayc_ter和侧向过载azc_ter，再根据纵向过载ayc_ter和侧向过载azc_ter控制舵机的舵偏角，实现弹体的捷联制导；
其中，所述弹体角速率增稳捷联制导律为：
Figure BDA0002941076340000021
Figure BDA0002941076340000022
其中，N为设定的比例系数，
Figure BDA0002941076340000023
为弹目视线角速率在弹体系下投影在俯仰方向的分量，
Figure BDA0002941076340000024
为弹目视线角速率在弹体系下投影在偏航方向的分量，g为重力加速度，θ为弹体俯仰角，γ为弹体滚转角，k为设定的角速率增稳项系数，ωbi_z为弹体偏航角速率，ωbi_y为弹体俯仰角速率，Vm为弹体合速度。
进一步地，所述弹目视线角速率在弹体系下投影在俯仰方向的分量
Figure BDA0002941076340000025
和偏航方向的分量
Figure BDA0002941076340000026
的获取方法为：
S1：获取导航坐标系至虚拟框架坐标系的方向余弦矩阵
Figure BDA0002941076340000027
所述虚拟框架为虚拟平台头上用于安装光学镜头的结构件，且方向余弦矩阵
Figure BDA0002941076340000028
为：
Figure BDA0002941076340000031
其中，Ly(qβ)为绕y轴旋转qβ度的旋转矩阵，Lz(qα)为绕z轴旋转qα度的旋转矩阵，qβ为弹体系下的视线方位角，qα为弹体系下的视线高低角，L(γ,θ,ψ)为弹体姿态旋转矩阵，γ为弹体的滚转角，θ为弹体的俯仰角，ψ为弹体的偏航角；
S2：根据方向余弦矩阵
Figure BDA0002941076340000032
获取虚拟框架的姿态如下：
Figure BDA0002941076340000033
Figure BDA0002941076340000034
Figure BDA0002941076340000035
其中，qθf为虚拟框架在惯性系下的俯仰角，qψf为虚拟框架在惯性系下的偏航角，qγf为虚拟框架在惯性系下的滚转角，
Figure BDA0002941076340000036
为方向余弦矩阵
Figure BDA0002941076340000037
中第一行第一列的元素，
Figure BDA0002941076340000038
为方向余弦矩阵
Figure BDA0002941076340000039
中第一行第二列的元素，
Figure BDA00029410763400000310
为方向余弦矩阵
Figure BDA00029410763400000311
中第二行第二列的元素，
Figure BDA00029410763400000312
为方向余弦矩阵
Figure BDA00029410763400000313
中第一行第三列的元素，
Figure BDA00029410763400000314
为方向余弦矩阵
Figure BDA00029410763400000315
中第三行第二列的元素，asin为反正弦函数，atan为反正切函数；
S3：根据虚拟框架的姿态获取虚拟框架坐标系下的框架角速率ωframe＝[0ωframe_y ωframe_z]T，且框架角速率ωframe满足：
Figure BDA00029410763400000316
其中，ωframe_y为框架角速率ωframe在虚拟框架坐标系y轴上的分量，ωframe_z为框架角速率ωframe在虚拟框架坐标系z轴上的分量，
Figure BDA00029410763400000317
为惯性系下的俯仰视线角速率，
Figure BDA00029410763400000318
为惯性系下的偏航视线角速率；
S4：对框架角速率ωframe进行坐标转换，得到弹体系下的框架角速率
Figure BDA00029410763400000319
Figure BDA0002941076340000041
其中，T表示转置；
S5：将框架角速率
Figure BDA0002941076340000042
的第三个元素
Figure BDA0002941076340000043
作为弹目视线角速率在弹体系下投影在俯仰方向的分量
Figure BDA0002941076340000044
框架角速率
Figure BDA0002941076340000045
的第二个元素
Figure BDA0002941076340000046
的相反数作为弹目视线角速率在弹体系下投影在偏航方向的分量
Figure BDA0002941076340000047
进一步地，所述俯仰视线角速率
Figure BDA0002941076340000048
与偏航视线角速率
Figure BDA0002941076340000049
的获取方法为：
S31：根据设定的状态向量
Figure BDA00029410763400000410
构建状态方程如下：
Figure BDA00029410763400000411
其中，x1为俯仰视线角qe，x2为俯仰视线角速率
Figure BDA00029410763400000412
x3为偏航视线角qb，x4为偏航视线角速率
Figure BDA00029410763400000428
Figure BDA00029410763400000430
为x1的导数，
Figure BDA00029410763400000415
为x2的导数，
Figure BDA00029410763400000416
为x3的导数，
Figure BDA00029410763400000417
为x4的导数，r为弹目斜距，
Figure BDA00029410763400000418
为弹目相对速度，ays为弹体的过载在视线系y轴上的分量，azs为弹体的过载在视线系z轴上的分量；
S32：通过第一观测量y1和第二观测量y2构建观测方程如下：
Figure BDA00029410763400000419
其中，
Figure BDA00029410763400000420
为弹体系下的单位视线向量
Figure BDA00029410763400000421
的第一个元素，
Figure BDA00029410763400000422
为弹体系下的单位视线向量
Figure BDA00029410763400000423
的第二个元素，
Figure BDA00029410763400000424
为弹体系下的单位视线向量
Figure BDA00029410763400000425
的第三个元素；
S33：采用UKF滤波算法从状态方程与观测方程提取俯仰视线角速率
Figure BDA00029410763400000426
与偏航视线角速率
Figure BDA00029410763400000427
进一步地，所述弹目斜距r的获取方法为：
Figure BDA0002941076340000051
其中，(Δx,Δy,Δz)为弹体位置与目标位置之间的差值，且所述弹体位置的获取方法为：
从弹上GPS收星稳定的k时刻开始，找出连续的M个弹上GPS未失锁的时刻，其中，M至少为3；
分别将所述M个弹上GPS未失锁的时刻作为当前时刻执行差值获取操作，得到M个经纬高差值，其中，所述差值获取操作为：弹体发射后，弹上计算机以发射平台初始装订的GPS坐标作为弹体初始位置进行导航解算，得到当前时刻k0对应的弹体GPS经纬高LBH_nav(k0)；同时，弹上GPS在当前时刻k0量测弹体所在位置，得到弹体经纬高LBH_m(k0)；将LBH_nav(k0)与LBH_m(k0)之间的差值作为当前时刻对应的经纬高差值；
将M个经纬高差值的平均值作为误差量LBH_c；
将误差量LBH_c与弹上GPS在后续时刻量测得到的弹体经纬高的和值分别作为弹体在各后续时刻的弹体位置。
进一步地，所述单位视线向量
Figure BDA0002941076340000052
且有：
Figure BDA0002941076340000053
其中，
Figure BDA0002941076340000054
为惯性系下的单位视线向量。
有益效果：
本发明提供一种基于UKF滤波的弹体角速率增稳捷联制导方法，通过建立视线角速率滤波模型，补偿弹体GPS与发射平台GPS之间的误差，再使用UKF滤波算法提取视线角速率，最后采用基于比例导引算法的增稳捷联制导律对弹体进行制导，通过弹体角速率反馈进行增稳，能够减小导航误差，降低导航误差对制导精度的影响，通过能够在不增加硬件成本的前提下，有效提高小型捷联导引头制导武器的制导算法精度与算法鲁棒性，具备工程可实施性，将平台导引头头替换成捷联头可大大节约武器制造成本，提升产品竞争力。
附图说明
图1为本发明计算方法的数据流程框图；
图2为本发明中涉及的坐标系示意图；
图3为本发明中UKF滤波算法流程框图。
具体实施方式
为了使本技术领域的人员更好地理解本申请方案，下面将结合本申请实施例中的附图，对本申请实施例中的技术方案进行清楚、完整地描述。
如图1所示，本发明提供一种基于UKF滤波的弹体角速率增稳捷联制导方法，采用设定的弹体角速率增稳捷联制导律获取弹体的纵向过载ayc_ter和侧向过载azc_ter，再根据纵向过载ayc_ter和侧向过载azc_ter控制舵机的舵偏角，实现弹体的捷联制导；
其中，所述弹体角速率增稳捷联制导律为：
Figure BDA0002941076340000061
Figure BDA0002941076340000062
其中，N为设定的比例系数，如N＝3，
Figure BDA0002941076340000063
为弹目视线角速率在弹体系下投影在俯仰方向的分量，
Figure BDA0002941076340000064
为弹目视线角速率在弹体系下投影在偏航方向的分量，g为重力加速度，θ为弹体俯仰角，γ为弹体滚转角，k为设定的角速率增稳项系数，如k＝0.1，ωbi_z为弹体偏航角速率，ωbi_y为弹体俯仰角速率，Vm为弹体合速度，其中，ωbi_z与ωbi_y由陀螺直接输出得到。
需要说明的是，弹体上设置有特定的控制模块来根据纵向过载ayc_ter和侧向过载azc_ter来生成控制舵机的舵偏角的控制指令，属于本领域技术惯用手段，本发明对此不作赘述。
进一步地，所述弹目视线角速率在弹体系下投影在俯仰方向的分量
Figure BDA0002941076340000071
和偏航方向的分量
Figure BDA0002941076340000072
的获取方法为：
S1：如图2所示，根据弹体姿态以及弹体系下的视线高低角qα与方位角qβ，获取导航坐标系至虚拟框架坐标系的方向余弦矩阵
Figure BDA0002941076340000073
所述虚拟框架为虚拟平台头上用于安装光学镜头的结构件，且方向余弦矩阵
Figure BDA0002941076340000074
为：
Figure BDA0002941076340000075
其中，Ly(qβ)为绕y轴旋转qβ度的旋转矩阵，Lz(qα)为绕z轴旋转qα度的旋转矩阵，qβ为弹体系下的视线方位角，qα为弹体系下的视线高低角，L(γ,θ,ψ)为弹体姿态旋转矩阵，γ为弹体的滚转角，θ为弹体的俯仰角，ψ为弹体的偏航角。
S2：根据方向余弦矩阵
Figure BDA0002941076340000076
获取虚拟框架的姿态如下：
Figure BDA0002941076340000077
Figure BDA0002941076340000078
Figure BDA0002941076340000079
其中，qθf为虚拟框架在惯性系下的俯仰角，qψf为虚拟框架在惯性系下的偏航角，qγf为虚拟框架在惯性系下的滚转角，
Figure BDA00029410763400000710
为方向余弦矩阵
Figure BDA00029410763400000711
中第一行第一列的元素，
Figure BDA00029410763400000712
为方向余弦矩阵
Figure BDA00029410763400000713
中第一行第二列的元素，
Figure BDA00029410763400000714
为方向余弦矩阵
Figure BDA00029410763400000715
中第二行第二列的元素，
Figure BDA00029410763400000716
为方向余弦矩阵
Figure BDA00029410763400000717
中第一行第三列的元素，
Figure BDA00029410763400000718
为方向余弦矩阵
Figure BDA00029410763400000719
中第三行第二列的元素，asin为反正弦函数，atan为反正切函数。
S3：根据虚拟框架的姿态获取虚拟框架坐标系下的框架角速率ωframe＝[0ωframe_y ωframe_z]T，且框架角速率ωframe满足：
Figure BDA0002941076340000081
其中，ωframe_y为框架角速率ωframe在框架坐标系y轴上的分量，ωframe_z为框架角速率ωframe在框架坐标系z轴上的分量，
Figure BDA00029410763400000810
为惯性系下的俯仰视线角速率，
Figure BDA00029410763400000811
为惯性系下的偏航视线角速率。
进一步地，下面介绍所述俯仰视线角速率
Figure BDA00029410763400000812
与偏航视线角速率
Figure BDA00029410763400000813
的获取方法，其中，需要说明的是，在构建状态方程和观测方程前，需要建立视线角速率滤波微分方程，且视线坐标系相对惯性系旋转角速率
Figure BDA0002941076340000082
在视线系下的表示为：
Figure BDA0002941076340000083
其中：qb为视线方位角，qe为视线高低角。这里视线系为惯性系经过2-3转序得到的坐标系，沿视线方向无滚转，
Figure BDA0002941076340000084
分别为沿视线系x、y、z轴是单位矢量。
在上述视线系下，弹体指向目标的矢量为
Figure BDA0002941076340000085
对上式求一阶、二阶导数可得微分方程：
Figure BDA0002941076340000086
Figure BDA0002941076340000087
Figure BDA0002941076340000088
其中：axs,ays,azs为弹体过载在视线系各坐标轴上的分量；r为弹目斜距；
Figure BDA0002941076340000089
为弹目相对速度；
然后，根据上述微分方程构建状态方程和观测方程，具体参见步骤S31～S32。
S31：根据设定的状态向量
Figure BDA0002941076340000091
构建状态方程如下：
Figure BDA0002941076340000092
其中，x1为俯仰视线角qe，x2为俯仰视线角速率
Figure BDA0002941076340000093
x3为偏航视线角qb，x4为偏航视线角速率
Figure BDA00029410763400000912
Figure BDA00029410763400000911
为x1的导数，
Figure BDA0002941076340000096
为x2的导数，
Figure BDA0002941076340000097
为x3的导数，
Figure BDA0002941076340000098
为x4的导数，r为弹目斜距，
Figure BDA00029410763400000910
为弹目相对速度，ays为弹体的过载在视线系y轴上的分量，azs为弹体的过载在视线系z轴上的分量。
其中，所述弹目斜距r的获取方法为：
Figure BDA0002941076340000099
其中，(Δx,Δy,Δz)为弹体位置与目标位置之间的差值，且所述弹体位置的获取方法为：
从弹上GPS收星稳定的k时刻开始，找出连续的M个弹上GPS未失锁的时刻，其中，M至少为3；
分别将所述M个弹上GPS未失锁的时刻作为当前时刻执行差值获取操作，得到M个经纬高差值，其中，所述差值获取操作为：弹体发射后，弹上计算机以发射平台初始装订的GPS坐标作为弹体初始位置进行导航解算，得到当前时刻k0对应的弹体GPS经纬高LBH_nav(k0)＝[L_nav(k0)B_nav(k0)H_nav(k0)]，其中，L_nav(k0)为弹体在k0时刻的解算经度，B_nav(k0)为弹体在k0时刻的解算纬度，H_nav(k0)为弹体在k0时刻的解算高度；同时，弹上GPS在当前时刻k0量测弹体所在位置，得到弹体经纬高LBH_m(k0)＝[L_m(k0)B_m(k0)H_m(k0)]，L_m(k0)为弹体在k0时刻的量测经度，B_m(k0)为弹体在k0时刻的量测纬度，H_m(k0)为弹体在k0时刻的量测高度；将LBH_nav(k0)与LBH_m(k0)之间的差值作为当前时刻对应的经纬高差值LBH_miss(k0)＝LBH_nav(k0)-LBH_m(k0)；
将M个经纬高差值的平均值作为误差量LBH_c；例如，当M＝3时，误差量LBH_c＝[LBH_miss(k)+LBH_miss(k+1)+LBH_miss(k+2)]/3；
将误差量LBH_c与弹上GPS在后续时刻量测得到的弹体经纬高的和值分别作为弹体在各后续时刻的弹体位置。
S32：通过第一观测量y1和第二观测量y2构建观测方程如下：
Figure BDA0002941076340000101
其中，
Figure BDA0002941076340000102
为弹体系下的单位视线向量
Figure BDA0002941076340000103
的第一个元素，
Figure BDA0002941076340000104
为弹体系下的单位视线向量
Figure BDA0002941076340000105
的第二个元素，
Figure BDA0002941076340000106
为弹体系下的单位视线向量
Figure BDA0002941076340000107
的第三个元素。
S33：采用UKF滤波算法从状态方程与观测方程提取俯仰视线角速率
Figure BDA0002941076340000108
与偏航视线角速率
Figure BDA0002941076340000109
S4：对框架角速率ωframe进行坐标转换，得到弹体系下的框架角速率
Figure BDA00029410763400001010
Figure BDA00029410763400001011
其中，T表示转置；
S5：将框架角速率
Figure BDA00029410763400001012
的第三个元素
Figure BDA00029410763400001013
作为视线角速率在弹体系下投影在俯仰方向的分量
Figure BDA00029410763400001014
框架角速率
Figure BDA00029410763400001015
的第二个元素
Figure BDA00029410763400001016
的相反数作为视线角速率在弹体系下投影在偏航方向的分量
Figure BDA00029410763400001017
如图3所示，下面详细介绍如何采用UKF滤波算法从状态方程与观测方程提取俯仰视线角速率
Figure BDA0002941076340000117
与偏航视线角速率
Figure BDA0002941076340000111
步骤1：将状态方程和观测方程离散化为如下形式：
Figure BDA0002941076340000112
其中：axs,ays,azs为弹体过载在弹体系各坐标轴上的分量；r为弹目斜距；
Figure BDA0002941076340000113
为弹目相对速度；wk、vk为白噪声。
步骤2：状态量U变换
首先对状态量进行U变换，构造sigma点(2n+1组，n为状态量维数)
Figure BDA0002941076340000114
其中：
Figure BDA0002941076340000115
为矩阵第i列数据，c＝n+l，n为状态量维数，l＝α2(n+λ)-n。α＝0.1。
步骤3：计算出下一时刻状态量估计值以及输出量
对sigma点非线性变换，然后加权求和计算出下一时刻状态量估计值以及输出量：
Figure BDA0002941076340000116
步骤4：更新状态量及其方差，提取视线角速率
计算状态量与观测量方差、协方差，并对当前状态量及其方差进行更新：
Figure BDA0002941076340000121
Figure BDA0002941076340000122
与
Figure BDA0002941076340000123
定义如下：
Figure BDA0002941076340000124
l含义与前述相同，α、β、λ取值为α＝0.1、λ＝0、β＝2。
步骤5：俯仰视线角速率
Figure BDA0002941076340000125
偏航视线角速率
Figure BDA0002941076340000126
需要说明的是，在采用UKF滤波器进行解算时，需要设定一些外部输入量，其中，外部输入量包括滤波初值视线坐标系下的弹体加速度、斜距及其变化率以及观测量，具体如下：
先设定滤波初值，对初始状态
Figure BDA0002941076340000127
与方差阵P0进行赋值：
Q＝diag([1 1000 1 1000])×10-7
R＝diag([6 6])×10-6
视线坐标系下的弹体加速度为：
Figure BDA0002941076340000128
其中：Lz为绕z轴旋转矩阵；Ly为绕y轴旋转矩阵；L(γ,θ,ψ)为弹体姿态旋转矩阵，
Figure BDA0002941076340000129
为加计输出加速度。
斜距为：
Figure BDA00029410763400001210
其中：Δx、Δy、Δz为弹体与目标位置差；
斜距变化率为：
Figure BDA0002941076340000131
Figure BDA0002941076340000132
其中：Ry、Rz与上述相同；
Figure BDA0002941076340000133
为惯性系下弹体速度。
捷联导引头输出两个角度为视线与体系下XOY、XOZ两个面夹角esm、bsm，这里将其转化为弹体系下的高低角qα与方位角qβ：
qα＝asin(sin(esm)/cos(bsm))
qβ＝-bsm
由此可见，本发明首先通过建立视线角速率滤波模型，选择惯性系下的视线方位角与视线高低角为状态量，选择弹体系下的视线方位角与视线高低角为观测量；补偿弹体GPS与发射平台GPS之间的误差，再使用UKF滤波算法提取视线角速率，最后采用比例导引算法进行制导，通过弹体角速率反馈进行增稳，能够有效提高小型捷联导引头制导武器的制导算法精度与算法鲁棒性，具备工程可实施性，将平台导引头头替换成捷联头可大大节约武器制造成本，提升产品竞争力。
当然，本发明还可有其他多种实施例，在不背离本发明精神及其实质的情况下，熟悉本领域的技术人员当然可根据本发明作出各种相应的改变和变形，但这些相应的改变和变形都应属于本发明所附的权利要求的保护范围。

Claims (3)

1.一种基于UKF滤波的弹体角速率增稳捷联制导方法，其特征在于，采用设定的弹体角速率增稳捷联制导律获取弹体的纵向过载ayc_ter和侧向过载azc_ter，再根据纵向过载ayc_ter和侧向过载azc_ter控制舵机的舵偏角，实现弹体的捷联制导；
其中，所述弹体角速率增稳捷联制导律为：
Figure FDA0003808052900000011
Figure FDA0003808052900000012
其中，N为设定的比例系数，
Figure FDA0003808052900000013
为弹目视线角速率在弹体系下投影在俯仰方向的分量，
Figure FDA0003808052900000014
为弹目视线角速率在弹体系下投影在偏航方向的分量，g为重力加速度，θ为弹体在导航系下的俯仰角，γ为弹体在导航系下的滚转角，k为设定的角速率增稳项系数，ωbi_z为弹体偏航角速率，ωbi_y为弹体俯仰角速率，Vm为弹体合速度；
其中，所述弹目视线角速率在弹体系下投影在俯仰方向的分量
Figure FDA0003808052900000015
和偏航方向的分量
Figure FDA0003808052900000016
的获取方法为：
S1：获取导航系至虚拟框架坐标系的方向余弦矩阵
Figure FDA0003808052900000017
所述虚拟框架为虚拟平台头上用于安装光学镜头的结构件，且方向余弦矩阵
Figure FDA0003808052900000018
为：
Figure FDA0003808052900000019
其中，Ly(qβ)为绕y轴旋转qβ度的旋转矩阵，Lz(qα)为绕z轴旋转qα度的旋转矩阵，qβ为弹体系下的视线方位角，qα为弹体系下的视线高低角，L(γ,θ,ψ)为弹体姿态旋转矩阵，γ为弹体在导航系下的滚转角，θ为弹体在导航系下的俯仰角，ψ为弹体在导航系下的偏航角；
S2：根据方向余弦矩阵
Figure FDA00038080529000000110
获取虚拟框架的姿态如下：
Figure FDA0003808052900000021
Figure FDA0003808052900000022
Figure FDA0003808052900000023
其中，qθf为虚拟框架在惯性系下的俯仰角，qψf为虚拟框架在惯性系下的偏航角，qγf为虚拟框架在惯性系下的滚转角，
Figure FDA0003808052900000024
为方向余弦矩阵
Figure FDA0003808052900000025
中第一行第一列的元素，
Figure FDA0003808052900000026
为方向余弦矩阵
Figure FDA0003808052900000027
中第一行第二列的元素，
Figure FDA0003808052900000028
为方向余弦矩阵
Figure FDA0003808052900000029
中第二行第二列的元素，
Figure FDA00038080529000000210
为方向余弦矩阵
Figure FDA00038080529000000211
中第一行第三列的元素，
Figure FDA00038080529000000212
为方向余弦矩阵
Figure FDA00038080529000000213
中第三行第二列的元素，asin为反正弦函数，atan为反正切函数；
S3：根据虚拟框架的姿态获取虚拟框架坐标系下的框架角速率ωframe＝[0 ωframe_yωframe_z]T，且框架角速率ωframe满足：
Figure FDA00038080529000000214
其中，ωframe_y为框架角速率ωframe在虚拟框架坐标系y轴上的分量，ωframe_z为框架角速率ωframe在虚拟框架坐标系z轴上的分量，
Figure FDA00038080529000000215
为惯性系下的俯仰视线角速率，
Figure FDA00038080529000000216
为惯性系下的偏航视线角速率；
S4：对框架角速率ωframe进行坐标转换，得到弹体系下的框架角速率
Figure FDA00038080529000000217
Figure FDA00038080529000000218
其中，T表示转置；
S5：将框架角速率
Figure FDA00038080529000000219
的第三个元素
Figure FDA00038080529000000220
作为弹目视线角速率在弹体系下投影在俯仰方向的分量
Figure FDA00038080529000000221
框架角速率
Figure FDA00038080529000000222
的第二个元素
Figure FDA00038080529000000223
的相反数作为弹目视线角速率在弹体系下投影在偏航方向的分量
Figure FDA00038080529000000224
其中，所述俯仰视线角速率
Figure FDA0003808052900000031
与偏航视线角速率
Figure FDA0003808052900000032
的获取方法为：
S31：根据设定的状态向量
Figure FDA0003808052900000033
构建状态方程如下：
Figure FDA0003808052900000034
其中，x1为俯仰视线角qe，x2为俯仰视线角速率
Figure FDA0003808052900000035
x3为偏航视线角qb，x4为偏航视线角速率
Figure FDA0003808052900000036
Figure FDA0003808052900000037
为x1的导数，
Figure FDA0003808052900000038
为x2的导数，
Figure FDA0003808052900000039
为x3的导数，
Figure FDA00038080529000000310
为x4的导数，r为弹目斜距，
Figure FDA00038080529000000311
为弹目相对速度，ays为弹体的过载在视线系y轴上的分量，azs为弹体的过载在视线系z轴上的分量；
S32：通过第一观测量y1和第二观测量y2构建观测方程如下：
Figure FDA00038080529000000312
其中，
Figure FDA00038080529000000313
为弹体系下的单位视线向量
Figure FDA00038080529000000314
的第一个元素，
Figure FDA00038080529000000315
为弹体系下的单位视线向量
Figure FDA00038080529000000316
的第二个元素，
Figure FDA00038080529000000317
为弹体系下的单位视线向量
Figure FDA00038080529000000318
的第三个元素；
S33：采用UKF滤波算法从状态方程与观测方程提取俯仰视线角速率
Figure FDA00038080529000000319
与偏航视线角速率
Figure FDA00038080529000000320
2.如权利要求1所述的一种基于UKF滤波的弹体角速率增稳捷联制导方法，其特征在于，所述弹目斜距r的获取方法为：
Figure FDA00038080529000000321
其中，(Δx,Δy,Δz)为弹体位置与目标位置之间的差值，且所述弹体位置的获取方法为：
从弹上GPS收星稳定的k时刻开始，找出连续的M个弹上GPS未失锁的时刻，其中，M至少为3；
分别将所述M个弹上GPS未失锁的时刻作为当前时刻执行差值获取操作，得到M个经纬高差值，其中，所述差值获取操作为：弹体发射后，弹上计算机以发射平台初始装订的GPS坐标作为弹体初始位置进行导航解算，得到当前时刻k0对应的弹体GPS经纬高LBH_nav(k0)；同时，弹上GPS在当前时刻k0量测弹体所在位置，得到弹体经纬高LBH_m(k0)；将LBH_nav(k0)与LBH_m(k0)之间的差值作为当前时刻对应的经纬高差值；
将M个经纬高差值的平均值作为误差量LBH_c；
将误差量LBH_c与弹上GPS在后续时刻量测得到的弹体经纬高的和值分别作为弹体在各后续时刻的弹体位置。
3.如权利要求1所述的一种基于UKF滤波的弹体角速率增稳捷联制导方法，其特征在于，所述单位视线向量
Figure FDA0003808052900000041
且有：
Figure FDA0003808052900000042
其中，
Figure FDA0003808052900000043
为惯性系下的单位视线向量。
CN202110177354.3A 2021-02-09 2021-02-09 一种基于ukf滤波的弹体角速率增稳捷联制导方法 Active CN113031645B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110177354.3A CN113031645B (zh) 2021-02-09 2021-02-09 一种基于ukf滤波的弹体角速率增稳捷联制导方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110177354.3A CN113031645B (zh) 2021-02-09 2021-02-09 一种基于ukf滤波的弹体角速率增稳捷联制导方法

Publications (2)

Publication Number Publication Date
CN113031645A CN113031645A (zh) 2021-06-25
CN113031645B true CN113031645B (zh) 2022-12-09

Family

ID=76460699

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110177354.3A Active CN113031645B (zh) 2021-02-09 2021-02-09 一种基于ukf滤波的弹体角速率增稳捷联制导方法

Country Status (1)

Country Link
CN (1) CN113031645B (zh)

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US6064332A (en) * 1994-04-26 2000-05-16 The United States Of America As Represented By The Secretary Of The Air Force Proportional Guidance (PROGUIDE) and Augmented Proportional Guidance (Augmented PROGUIDE)
CN101270993A (zh) * 2007-12-12 2008-09-24 北京航空航天大学 一种远程高精度自主组合导航定位方法
CN105157705A (zh) * 2015-07-07 2015-12-16 西安电子工程研究所 一种半捷联雷达导引头视线角速度提取方法
CN110895418A (zh) * 2019-12-30 2020-03-20 北京理工大学 补偿舵机动力学滞后的低速旋转飞行器控制方法及系统
WO2020087845A1 (zh) * 2018-10-30 2020-05-07 东南大学 基于gpr与改进的srckf的sins初始对准方法
CN111238474A (zh) * 2020-02-05 2020-06-05 哈尔滨工业大学 基于倾斜坐标系的捷联导引头非奇异视线角速度提取方法

Family Cites Families (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN107255924B (zh) * 2017-06-14 2018-07-17 哈尔滨工业大学 基于扩维模型的容积卡尔曼滤波提取捷联导引头制导信息的方法
CN107908895B (zh) * 2017-12-01 2020-11-13 哈尔滨工业大学 一种基于imm滤波器主动防御视线策略协同制导律设计方法

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US6064332A (en) * 1994-04-26 2000-05-16 The United States Of America As Represented By The Secretary Of The Air Force Proportional Guidance (PROGUIDE) and Augmented Proportional Guidance (Augmented PROGUIDE)
CN101270993A (zh) * 2007-12-12 2008-09-24 北京航空航天大学 一种远程高精度自主组合导航定位方法
CN105157705A (zh) * 2015-07-07 2015-12-16 西安电子工程研究所 一种半捷联雷达导引头视线角速度提取方法
WO2020087845A1 (zh) * 2018-10-30 2020-05-07 东南大学 基于gpr与改进的srckf的sins初始对准方法
CN110895418A (zh) * 2019-12-30 2020-03-20 北京理工大学 补偿舵机动力学滞后的低速旋转飞行器控制方法及系统
CN111238474A (zh) * 2020-02-05 2020-06-05 哈尔滨工业大学 基于倾斜坐标系的捷联导引头非奇异视线角速度提取方法

Non-Patent Citations (6)

* Cited by examiner, † Cited by third party
Title
Adaptive Unscented Kalman Filter Based Line of Sight Rate for Strapdown Seeker;W. Mi, et al.;《2018 Chinese Automation Congress (CAC)》;20181202;第886-891页 *
Integrated guidance and control for missile with narrow field-of-view strapdown seeker;Jiayi Tian, et al.;《ISA Transactions》;20201130;第124-137页 *
Line of sight rate estimation of strapdown imaging guidance system based on unscented Kalman filter;Guo-Jiang Zhang, et al.;《2005 International Conference on Machine Learning and Cybernetics》;20050821;第1574-1578页 *
Research on Line-of-Sight rate extraction of strapdown seeker;P. Wang, et al.;《Proceedings of the 33rd Chinese Control Conference》;20140730;第859-863页 *
单兵火箭弹捷联导引滤波方法研究;陈伟 等;《飞行力学》;20170630;第35卷(第03期);第68-72页 *
某型靶弹制导控制方法研究;郑旭 等;《舰船电子工程》;20190430;第39卷(第04期);第40-44页 *

Also Published As

Publication number Publication date
CN113031645A (zh) 2021-06-25

Similar Documents

Publication Publication Date Title
US7500636B2 (en) Processes and devices to guide and/or steer a projectile
CN107255924B (zh) 基于扩维模型的容积卡尔曼滤波提取捷联导引头制导信息的方法
US8606435B2 (en) Method and a system for estimating a trajectory of a moving body
CN105157705B (zh) 一种半捷联雷达导引头视线角速度提取方法
CN109709537A (zh) 一种基于卫星编队的非合作目标位置速度跟踪方法
CN105180728B (zh) 基于前数据的旋转制导炮弹快速空中对准方法
CN105486308B (zh) 估计弹目视线角速率的快收敛Kalman滤波器的设计方法
CN105115508A (zh) 基于后数据的旋转制导炮弹快速空中对准方法
CN105486307A (zh) 针对机动目标的视线角速率估计方法
CN111238474A (zh) 基于倾斜坐标系的捷联导引头非奇异视线角速度提取方法
CN108534783A (zh) 一种基于北斗导航技术的飞行器导航方法
CN113847913A (zh) 一种基于弹道模型约束的弹载组合导航方法
CN102878872A (zh) 一种针对导引头失锁情况的制导信息处理方法
CN111238469A (zh) 一种基于惯性/数据链的无人机编队相对导航方法
CN115388890A (zh) 基于视觉的多无人机协同对地目标定位方法
CN114510065A (zh) 一种多旋翼无人机地面目标跟踪控制方法
CN109857140A (zh) 运载火箭俯仰程序角计算方法、系统、设备及存储介质
Khamis Advanced tracking strategies for linear and nonlinear control systems: Theory and applications
CN113031645B (zh) 一种基于ukf滤波的弹体角速率增稳捷联制导方法
CN117111615A (zh) 一种仅基于方位角测量的多无人艇目标包围协同控制方法
Solano-López et al. Strategies for high performance GNSS/IMU Guidance, Navigation and Control of Rocketry
CA3064640A1 (en) Navigation augmentation system and method
CN116242343A (zh) 一种机载高精度光纤惯性测量装置及组合导航方法
CN114295145B (zh) 一种基于车载发射平台的捷联惯导系统轨迹发生器设计方法
CN112747743B (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