捷联导引头视线角速率估计算法

合集下载
  1. 1、下载文档前请自行甄别文档内容的完整性,平台不提供额外的编辑、内容补充、找答案等附加服务。
  2. 2、"仅部分预览"的文档,不可在线预览部分如存在完整性等问题,可反馈申请退款(可完整预览的文档不适用该条件!)。
  3. 3、如文档侵犯您的权益,请联系客服反馈,我们会尽快为您处理(人工客服工作时间:9:00-18:30)。

Vol. 28 No. 4Apr. 2021
第28卷第4期2021年4月电光与控制Electronics Optics & Control 引用格式:高昊,范军芳•捷联导引头视线角速率估计算法[J] ■电光与控制,2021,28(4) :26-28, 69. GAO H, FAN J F. An algorithm for estimating line- of-sight angular rate of strapdown seeker[ J]. Electronics Optics & Control, 2021, 28(4) :26-2& 69.
捷联导引头视线角速率估计算法
高昊V,范军芳"
(1-北京信息科技大学高动态导航技术北京市重点实验室,北京100089;
2.教育部现代测控技术重点实验室,北京100089)
摘 要:研究了一种用于估计制导弹药与目标之间的视线角速率算法的推导与实现。

低成本的捷联导引头的测量信
息中包含了弹目在相对运动中形成的视线角和弹体姿态两部分信息,首先设计解耦算法将弹体姿态信息去除,然后 采用扩展卡尔曼滤波(EKF)以及a-B 滤波这两种算法分别对视线角和视线角速率进行估计,最终的仿真结果表明采
用EKF 算法能比a-0滤波算法得到更加精确的估计值。

关键词:制导弹药;捷联导引头;视线角速率;扩展卡尔曼滤波;a-0滤波
中图分类号:TJ765 文献标志码:A dot : 10.3969/j. issn. 1671 -637X. 2021.04.006
An Algorithm for Estimating Line-of-Sight Angular
Rate of Strapdown Seeker
GAO Hao 1,2, FAN Junfang 1,2
(1. Beijing Key Laboratory o£ High Dynamic Navigation Technology, Beijing Information Science &
Technology University, Beijing 100089, China ; 2. Key Laboratory of Modern Measurement &
Control Technology, Ministry of Education, Beijing 100089, China)
Abstract : This paper studies the derivation and implementation of an algorithm for estimating the angular
rate of Line of Sight ( LOS) angle between guided munitions and targets. The measurement information of a low-cost strapdown seeker includes two parts, namely, attitude of the missile and LOS angle, which are
formed in relative motion between the missile and the target. Firstly, a decoupling algorithm is designed to
remove the attitude information of the missile. Then, Extended Kalman Filter ( EKF) and a-0 filter are used
to estimate LOS angle and angular rate respectively. The final simulation results show that EKF can obtain a more accurate estimated value than a-fi filter.
Key words : guided munition ; strapdown seeker ; LOS angular rate ; extended Kalman filter ; a-0 filter
0引言
进入21世纪以来,世界局势总体趋于缓和,而集
中在中东地区的局部战争状况则愈演愈烈⑴。

由于大 量高新技术武器装备的投入,现代局部战争物资消耗 仍然十分巨大,这对于高精度、低成本武器的研究就显 得尤为重要,因此新型捷联式导引头应运而生⑵。


联式导引头去除了隔离弹体运动的稳定平台,直接与
弹体捷联安装,大大降低了成本,减少了重量,系统的
收稿日期=2020-04-20 修回日期=2020-05-09
基金项目:国家自然科学基金(61801032);北京市科技新星计划(xxjh
2015B041);北京市委组织部青年拔尖人才计划(2015000026833ZK03) 作者简介:高昊(1994 -),男,河北保定人,硕士,研究方向为导航 与制导。

可靠性也会提高,但同时也存在着无法直接提取视线 角速率的问题。

由于导引头测量信号中还包含了弹体
运动信息,在处理制导信息时具有额外的复杂性,并含 有大量的干扰噪声,因此,需要设计必要的解耦与估计
算法来获得在惯性系下的视线角速率信息。

捷联导引头视线角速率估计问题得到了很大的重 视,目前国内外已有很多学者对此开展深入研究。

WIL-
UAMS 等⑷基于最优控制与估计理论的方法设计出了全 捷联制导系统;孙婷婷等⑷采用了无迹卡尔曼滤波(UKF) 的方法,并且搭建了半物理实验系统对该方法进行了
验证;聂聪等⑸利用了陀螺角速率信息,简化系统噪声
模型并提出了基于中央差分卡尔曼滤波(CDKF)的视线
角速率估计方法。

本文首先推导了视线角速率的解耦算法,然后建立
第4期高昊等:捷联导引头视线角速率估计算法27
视线角速率运动学模型并采用扩展卡尔曼滤波(EKF)以及«-/3滤波这两种算法分别对视线角速率进行估计,最终的仿真结果表明采用EKF算法比a-/3滤波算法能得到更加精确的估计值。

1涉及到的坐标系及其关系
在研究视线角速率的解耦与估计算法时,需要用到的坐标系及定义如下所述。

1)惯性坐标系Ox^z,:原点0为地球的中心,Ozj 轴与地球自转轴重合,而0%和0九轴位于地球赤道平面内和0z x轴组成右手笛卡尔坐标系,惯性坐标系不与地球自转一起运动,是静坐标系。

2)弹体坐标系Ox R y R z R:原点0为导弹的质心,0%轴为弹体纵轴,正方向为弹体头部方向;0%轴垂直于轴并且位于弹体纵向对称面内,正方向为上; O zb轴与0讣面相垂直,正方向按右手直角坐标系确定。

弹体坐标系与弹体固连,属于动坐标系。

3)视线坐标系Ox s y s z s:原点0为弹体质心,0农轴与弹目连线重合,指向目标方向为正,。

蚣轴位于惯性坐标系下的0會内并且与&s轴垂直,。

弘轴满足右手定则。

4)体视线坐标系Ox ss y BS z ss:原点0为弹体质心,S bs轴与弹目连线重合,指向目标方向为正,&bs轴位于弹体坐标系下的0弧知内并且与。

%s轴垂直,Q/bs轴满足右手定则。

各坐标系之间的相互转换如图1所示。

图1各坐标系之间的关系
Fig.1Relationship between coordinate systems
2视线角速率解耦算法
根据图1中惯性系到视线系的转换方式可得[@”sin q sh q sh g…,cos q sh]丁=C;(叫+©) +C s a)c(1)式中:g,/为惯性坐标系下的视线角;©,为体视线坐标系相对于弹体坐标系的旋转角速率;©为惯性坐标系相对于弹体坐标系的旋转角速率;亿为体视线坐标系相对于视线坐标系的旋转角速率;C;为弹体坐标系转换到惯性坐标系的方向余弦矩阵;C;为惯性坐标系转换到视线坐标系的方向余弦矩阵。

根据式(1)可得到在惯性坐标系下的视线角速率
么”和
0”=(加sin q bh+)(C n sin q sh+C31cos q sh)+ (Qbh+)(C^sin q sh+C32cos q sh)+
(加cos q bh+w d)(C13sin q sh+C33cos q sh)(2) (Lsin q bh+co,l)[C2l+tan q”(C31sin q sh-C n cos)]+ (Qbh+)[C n+tan9…(C32sin q sh-C12cos q sh)]+ (O»cos q bk+)[C23+tan?OT(C33sin q th-C l3cos q th)~\
(3)式中,C”为矩阵C;中对应的表达式。

设导弹与目标之间的距离为d,目标位置为(d0 0),目标在弹体坐标系和惯性坐标系中的表示方法分别为
[%y B ZB「=C;s[d o0]T(4)
[省如T=c;[d o o]T(5)联立式(4)、式(5)并根据惯性系和弹体系之间的转换关系可得
[%i Ji z i]T=Cg[%b X b2b]T o(6)代入C;后可得惯性系下的视线角的表示方法「sin q…=C21cos g»cos q hh+C22sin q“_C23cos g^sin q hh 仁=Gros+Csztan血-Csssin°
I m Cucos q bh+C12tan g» -C13sin q bh
(7) 3视线角速率估计算法
与捷联导引头相对应的框架式导引头可直接提取视线角速率信息,因此不需要滤波算法。

但是捷联导引头获取的信息具有很强的非线性,无法直接通过微分运算获得体视线角速率,因此需要采用非线性滤波算法估计视线角速率。

3.1视线角速率的运动学模型
在视线坐标系中,弹目相对运动的关系可表示为/Zg=+dg+(Ug^/g+2(Ug^/g+O(8)设%,Qsv5Qsh,0皿分别作为状态变里光1,光2,尤3,光4°根据弹目运动关系可建立状态方程
一厉1=x2
•B T
•小丄2(/%%
x±-Zx^xAan x,一—+------一—
I d dcos d
兀3—先4
式中,風和/分别表示在视线坐标系下目标和弹体的加速度。

惯性视线角[q”?.,a]T与旋转角速率处之间的关系可以表示为
1—X—-0--0-
=C;Qsh+0
-0-_q sv_
(10)
28电光与控制第28卷
设体视线角弘,血分别作为观测量X,建立观测方程
r Ti=arcsm(C12cos g”cos么+色血q,-C^cos g”sin%) -HcLui G?009q*+氐18"q”_"㉛血*q*°戶皿C n cos q“+C21tan g”-C31sm q*
(11) 3.2扩展卡尔曼滤波(EKF)算法
传统的卡尔曼滤波只能处理线性系统中的问题,在处理非线性问题时就需要用到扩展卡尔曼滤波(EKF)算法。

其原理是:首先使用泰勒级数展开的方式进行线性化处理撚后再煎卡尔曼的融计系统炖。

设/(斗)和人(斗)分别为非线性系统的状态函数和量测函数,是观测量,为了使用卡尔曼方程,/(孔)和E(孔)需要取一阶导数进行线性化处理
(12)
利用泰勒级数展开的方法,线性化的状态转移矩阵可以通过F($)得到
W+...+!W^©(f*)=/j=l+F(g”+
(13)
忽略高阶项,设离散步长为T,可取离散状态转移矩阵的近似值为
©(気)i+F(E)T。

(14)经泰勒级数展开的方式处理过的系统状态转移矩阵以及观测矩阵不再是常数矩阵,而是随”的变化而变化。

3.3a-0滤波算法
a-p滤波算法的递推思想是假定目标沿直线做匀速运动,然后根据观测到的运动状态和运动速度来估计系统状态⑷。

在“时刻,系统的状态为X*,假设系统在间隔为T 的时间区间内做匀速直线运动,在t小时刻的状态可通过h时刻来预测
=x*+丘*?1(15)式中,丸是系统在S时刻的速度主是斗的估计值是左*的估计值,在匀速直线运动的前提下,系统运动速度预测值A+讥为
=x t+x t(16)式(15)和式(16)就是a-p滤波器的状态方程。

耳是系统状态在S+i时刻的量测值,对于该时刻a-B滤波估计值为
■^4+1=X k^\/k+8[x4+1一斗+i/j]
•:_:Pk r-_入i(17)
X*+l=+1/4+〒L X*+l—X&+1/1」
式中:弘和民为滤波增益;t中时刻的系统状态估计值左中和速度估计值:屮可通过预测误差X tt,-x itl/i来修正。

滤波增益可根据不同情况进行调整。

4仿真分析
以某微小型导弹为例,导弹的发射坐标为[0m1m 0m]T,发射点初速度为50m/s,目标的位置为[1100m 0m0mF,转动惯量J r=J*=0.45kg•m2,分别采用EKF算法以及a-p滤波算法对视线角、视线角速率进行估计,仿真结果如图2与图3所示。

伍)2”
t/s
⑹g*
图2视线角估计曲线
Fig.2Estimation curve of LOS angle
(
一*
人。

))
/
^
1.0
05
o.o
-05
-1.0
-15
0.005 1.015 2.0Z5 3.035 4.04<5 5.055
t/s
伍)鼻
3
2
(
2
(。

))


51----<_I_._t x1_.__.__.—1_._I___>_!_._I__l
0.005 1.015 2.025 3.035 4.045 5.055
-2
-3
-4
t/s

图3视线角速率估计曲线
Fig.3Estimation curve of LOS angular rate
(下转第69
页)
第4期王迷迷等:基于前沿点复合检测的机器人未知环境探索69
International Conference on Robotics and Automation,
2004:4688-4694.
[5]FREDA L,ORIOLO G.Frontier-based probabilistic$trate・
gies for sensor-based exploration[C]//Proceedings of the
IEEE International Conference on Robotics and Automa­
tion,2005:3881-3887.
[6]GONZALEZ-BANOS H H,LATOMBE J C.Navigation
strategies for exploring indoor environments[J].Interna­
tional Journal of Robotics Research,2002,21(10/11):
829-848.
[7]STACHNISS C,BURGARD W.Exploring unknown envi­
ronments with mobile robots using coverage maps[C]//
Proceedings of the18th International Joint Conference on
Artificial Intelligence,2003:1127-1132.
[8]王东署,段谊海,王佳.未知环境中移动机器人的环境
探索与地图构建[J].郑州大学学报(理学版),2014,
46(3):96-101.
(上接第28页)
从图2和图3可以看出,与真实值相比,采用EKF 算法估计的视线角以及视线角速率误差明显小于a-p 滤波算法的估计误差,并且随着时间的推移,EKF算法估计的视线角速率的发散程度要比a-0滤波算法的低。

其中,EKF算法估计精度能够满足比例导引需要的视线角速率,具体数值见表1。

表1仿真误差分析表
Table1Analysis of simulation errors
EKF算法a-0滤波算法
最大值精度最大值精度9»/(°) 1.040.0535 1.800.0957
%/(°)0.870.0613 1.810.1016
°/((。

)・s・i)0.150.00870.300.0164
祗/((。

)・L)0.650.0241 1.630.0835
5结论
视线角速率是实现比例导引制导方案所需的信号,由于使用的低成本捷联导引头的输出信号包含弹体运动,使其无法直接测量视线角速率,因此本文设计了视线角速率的解耦算法,并采用EKF算法和a-p滤[9]HENRY P,KRAININ M,HERBST E,et al.RGB-D map-
ping:using Kinect-style depth cameras for dense3D mod­
eling of indoor environments[J].International Journal of
Robotics Research,2012,31(5):647-663.
[10]ENDRES F,HESS J,ENGELHARD N,et al.An evalua­
tion of the RGB-D SLAM system[C]//IEEE Interna­
tional Conference on Robotics and Automation,2012:
1691-1696.
[11]MUNARO M,BASSO F,MENEGAT11E.OpenPTrack:open
source multi-camera calibration and people tracking for
RGB-D camera networks[J].Robotics and Autonomous
Systems,2016,75:525-538.
[12]YU N B,WANG S R.Enhanced autonomous exploration
and mapping of an unknown environment with the fusion
of dual RGB-D sensors[J].Engineering,2019,5(1):
355-373.
波算法对导引头的输出信息进行估计。

仿真结果表明,采用EKF算法能比a-p滤波算法更好地估计视线角速率,对于以后的捷联制导技术研究具有一定的工程应用价值。

参考文献
[1]丛敏,张婵•国外微小型导弹发展现状[J]•飞航导弹,
2011(10):1-8.
[2]刘振亚,高敏,许路铁.捷联制导武器发展现状及关键
技术[J].飞航导弹,2017(4)=55-59.
[3]WILLIAMS D E,RICHMAN J,FRIEDLAND B.Design of
an integrated strapdown guidance and control system for a
tactical missile[C]//AIAA Guidance and Control Confer­
ence,1983.doi:10.2514/6.1983-2169.
[4]孙婷婷,储海荣,贾宏光,等.捷联式光学图像导引头
视线角速率估计[J]•光学学报,2014,34(6):171-177.
[5]聂聪,张科.全捷联导引头视线角速率估计方法[J].
计算机仿真,2017,34(1):70-73,108.
[6]赵飞,韩卫杰.基于a-0滤波的塔康距离跟踪算法
[J]•科技视界,2018(16)=249-250.。

相关文档
最新文档