一种捷联激光导引头视线角速度初值的在线解算算法
捷联导引头视线角速率估计算法
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.006An Algorithm for Estimating Line-of-Sight AngularRate of Strapdown SeekerGAO 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 angularrate 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 areformed in relative motion between the missile and the target. Firstly, a decoupling algorithm is designed toremove the attitude information of the missile. Then, Extended Kalman Filter ( EKF) and a-0 filter are usedto 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 filter0引言进入21世纪以来，世界局势总体趋于缓和，而集中在中东地区的局部战争状况则愈演愈烈⑴。
捷联惯性导航系统的解算方法ppt课件
的 分量构成的矩阵，则
r rnT n r nT Cnb n rnT r nT Cbn
由于坐标系不动而是矢量转动，它 相应于矢量固定时坐标系方向转动
rn
n
C
n b
r
2010-03-19
方向余弦矩阵微分方程
由矢量相对导数和绝对导数的关系式
dr dt
n
dr dt
b
ω nb r
假定地理坐标系为参考坐标系，作为参考 坐标系认为它在空间是不动的，即
如把OXbYbZb作为动坐标系， ENU作为参考坐标系，则航向 角H，纵摇角（俯仰角）P和横 摇角（横滚角、倾斜角）R。就 是一组欧拉角。
Zb Zb'' U Zb' P
欧拉角没有严格的定义，根 据
需要，可以选用不同的欧拉 角
组。第一次转动，可以绕三 个
轴中的任一个转动，故有3种 可
能，第二次有2种可能，第三 次
启动
自检 测
初始 化
返回9
姿态
迭 数
代
次阵计 算
N O
YES
导 航 计 控 制 信 息算提 取
3.2 姿态矩阵的计算
捷联式惯导中，载体地理位置就是地理坐标系相对 地球坐标系的方位。而载体的姿态和航向则是载体 坐标系相对于地理坐标系的方位关系。确定两个坐 标系的方位关系问题，是力学中的刚体定点转到理 论。在刚体定点转动理论中，描述动坐标系相对参 考坐标系方位关系的方法有多种。
求解方程可以直接得到航向和姿态信息，
欧 拉角法得到的姿态阵永远是正交阵，用这
个矩 阵将比力fb→fn信息的坐标变换时，变换后的信 息
中不存在非正交误差。因此，用欧拉角法得 到的姿态矩阵无
捷联惯性导航系统的解算方法课件
02
CATALOGUE
捷联惯性导航系统组成及工作 原理
主要组成部分介绍
惯性测量单元
包括加速度计和陀螺仪，用于测量载体在三个正交轴上的加速度 和角速度。
导航计算机
用于处理惯性测量单元的测量数据，解算出载体的姿态、速度和 位置信息。
控制与显示单元
用于实现人机交互，包括设置导航参数、显示导航信息等。
工作原理简述
学生自我评价报告
知识掌握情况
学生对捷联惯性导航系统的基本原理、解算 方法和实现技术有了深入的理解和掌握。
实践能力提升
通过实验和仿真，学生的动手实践能力得到了提升 ，能够独立完成相关的实验和仿真验证。
团队协作能力
在课程项目中，学生之间的团队协作能力得 到了锻炼和提升，能够相互协作完成项目任 务。
对未来发展趋势的预测和建议
捷联惯性导航系统的解算 方法课件
CATALOGUE
目 录
• 捷联惯性导航系统概述 • 捷联惯性导航系统组成及工作原理 • 捷联惯性导航系统解算方法 • 误差分析及补偿策略 • 实验验证与结果展示 • 总结与展望
01
CATALOGUE
捷联惯性导航系统概述
定义与基本原理
定义
捷联惯性导航系统是一种基于惯性测量元件（加速度计和陀螺仪）来测量载体（如飞机、导弹等）的加速度和角 速度，并通过积分运算得到载体位置、速度和姿态信息的自主导航系统。
01
高精度、高可靠性
02
多传感器融合技术
随着科技的发展和应用需求的提高， 捷联惯性导航系统需要进一步提高精 度和可靠性，以满足更高层次的应用 需求。
为了克服单一传感器的局限性，可以 采用多传感器融合技术，将捷联惯性 导航系统与其他传感器进行融合，提 高导航系统的性能和鲁棒性。
一种捷联寻的导引头的制导信息提取方法[发明专利]
![一种捷联寻的导引头的制导信息提取方法[发明专利]](https://img.taocdn.com/s3/m/3371dbe54a7302768f99396b.png)
专利名称：一种捷联寻的导引头的制导信息提取方法
专利类型：发明专利
发明人：田源,黄朝东,方海红,吴昭辉,赵洋,李涛,鞠晓燕,孟刚,刘佳琪,刘志轩,苏晓东,闫新峰,秦雪,孙月光,赵春明,
孙忠旭
申请号：CN201510373110.7
申请日：20150630
公开号：CN105021092A
公开日：
20151104
专利内容由知识产权出版社提供
摘要：本发明提供了一种捷联寻的导引头的制导信息提取方法，包括如下步骤：(1)获取载体坐标系o-xyz下导引头的体视线方位角和体视线高低角；(2)处理捷联导引头测量信息；(3)计算载体坐标系下弹目相对距离矢量以及目标相对飞行器的运动速度在载体坐标系的分量；(4)计算载体坐标系下当前时刻飞行器的惯性视线方位角速率和当前时刻飞行器的惯性视线高低角速率；(5)将步骤(4)获得的结果作为制导信息送至捷联寻的导引头。
本发明针对现有技术的不足，直接使用体视线角测量信息和陀螺角速度测量信息提取惯性视线角速度，提高了制导精度，降低制导系统的设计难度，能够广泛应用于各种捷联寻的制导武器中。
申请人：北京航天长征飞行器研究所,中国运载火箭技术研究院
地址：100076 北京市丰台区北京9200信箱76分箱6号
国籍：CN
代理机构：中国航天科技专利中心
代理人：陈鹏
更多信息请下载全文后查看。
捷联式光学导引头视线角速率解耦与估计_孙婷婷
1 坐标系及其关系
捷联导引头模型和解耦算法需要在地理坐标 系 、弹 体 坐 标 系 、视 线 坐 标 系 和 体 视 线 坐 标 系 中 进 行 研究。 各坐标系定义如下：
(1) 地 理 坐 标 系 Oxeyeze ： 取 载 体 和 地 球 中 心 连 线
与 地 球 表 面 交 点 O 为 坐 标 系 原 点 ， Oxe 轴 取 在 当 地 水 平 面 内 指 向 东 ， Oye 轴 沿 当 地 地 垂 线 方 向 并 且 指 向 天 顶 ， Oze 轴 方 向 按 右 手 法 则 确武器的发展提出了较 高 要 求 ，除 了 要 求 命 中 精 度 高 外 ，还 要 求 其 具 备 重 量 轻 、体 积 小 、成 本 低 、可 靠 性 高 等 特 点 。 传 统 框 架 式 导 引头具有较大的总视场角， 且能够直接提取制导系 统所需的视线角 速 率 信 息 ，但 由 于 框 架 的 存 在 ，使 其 结 构 复 杂 ，重 量 增 大 ，且 降 低 了 系 统 的 可 靠 性 。 随 着 大 面 阵 、高 分 辨 率 成 像 器 件 的 迅 速 发 展 ，捷 联 式 导 引 头在制导系统中应用成为重要的研究方向， 并在美 国 “长 钉 ”导 弹 上 得 到 较 好 应 用 。 捷 联 式 光 学 导 引 头 将光学系统、 相机和图像处理等部分直接固联在载 体基座上， 降低了结构复杂性与成本， 提高了可靠 性 ；取 消 框 架 使 总 的 视 场 角 降 低 ，可 以 采 用 高 分 辨 率 成像器件提高角分辨率； 但捷联导引头不能直接提 取制导系统所需 的 视 线 角 速 率 信 号 ，因 此 ，需 要 设 计 合适的解耦算法与估计算法。 目前国内外学者已开展对捷联式光学导引头视 线 角 速 率 估 计 方 面 的 研 究 。 R.D.Ehrich [1] 采 用 附 加 速 率 补 偿 方 法 进 行 单 通 道 视 线 角 速 率 重 构 ； Emmert R
全捷联导引头视线角速率估计方法
文章编号 ： １ ０ ０ ６ — ９ ３ ４ ８ （ ２ ０ １ ７ ） Ｏ ｌ 一 ０ ０ ７ ０ ～ ０ ４
计
算
机
仿
真
２ ０ １ ７ 年１ 月
全 捷 联 导 引 头 视 线 角 速 率 估 计 方 法
聂 聪， 张 科
（ 西北工业大学航天学 院， 陕西 西安 ７ １ ０ ０ ７ ２ ） 摘要 ： 针对全捷联导引头只能测得 相对于弹体 的视线角而不能测得制导 系统需要 的视线 角速率信息 的问题 ， 提 出 了一 种基 于中央差分卡尔曼滤波（ Ｃ Ｄ Ｋ Ｆ ） 的视线角速率估计方法 。上述方法充分利用 了惯 导系统角速率陀螺 的信 息 ， 简化 了系统噪
ｂ ｕ ｔ ｃ ａ ｎ ｎ ｏ ｔ ｏ ｂ ｔ ｍｎ ｔ ｈ ｅ Ｌ ＯＳ ａ ｎ ｕｌ ｇ ａ ｒ ｒ ａ ｔ ｅ ｎ ｅ ｅ ｄ ｅ ｄ ｆ ｏ ｒ ｇ ｕ ｉ ｄ ａ ｎ ｃ ｅ ｓ ｙ ｓ ｔ ｅ ｍ．Ａｉ ｍｉ ｎ ｇ ａ ｔ ｔ ｈ ｉ ｓ ｐ ｒ ｏ ｂ ｌ ｅ ｍ ，ａ Ｌ Ｏ Ｓ ａ ｎ ｇ ｕ ｌ ｒ ａ ｅ ｓ ｔ ｉ ｍａ ｔ ｉ ｏ ｎ
ｏ ｕ ｔ ｐ ｕ ｔ ｏ ｆ ｔ ｈ ｅ ｉ ｎ ｅ ｔ ｒ ｉ ａ ｌ ｎ ａ ｖ ｉ ｇ ａ ｔ ｉ ｏ ｎ ｓ ｙ ｓ ｔ ｅ ｍ，ａ ｎ ｄ ｔ ｈ ｅ ｓ ｙ ｓ ｔ ｅ ｍ ｎ ｏ ｉ ｓ ｅ ｍｏ ｄ ｅ ｌ ｉ ｓ ｓ ｉ ｍｐ ｌ ｉ ｉｅ ｆ ｄ ．Ａ ｆ ｔ ｅ ｒ ｓ ｔ ｕ ｄ ｙ ｉ ｎ ｇ ｔ ｈ ｅ ｒ ｅ ｌ ａ ｔ ｉ ｏ ｎ ｓ ｈ ｉ ｐ ｏ ｆ
ＮＩ Ｅ Ｃｏ ｎ ｇ．ＺＨＡＮＧ Ｋｅ
（ Ｓ ｃ ｈ ｏ ｏ ｌ ｏ ｆ Ａ ｓ ｔ ｒ ｏ ｎ ａ ｕ ｔ ｉ ｃ ｓ ，Ｎ ｏ ｒ ｔ ｈ ｗ ｅ ｓ ｔ ｅ ｒ ｎ Ｐ ｏ ｌ ｙ ｔ ｅ ｃ ｈ ｎ ｉ ｃ ａ ｌ Ｕ ｎ ｉ ｖ ｅ ｒ ｓ ｉ ｔ ｙ ， Ｘ ｉ ’ ａ ｎ Ｓ ｈ ａ ｎ ｘ ｉ ７ １ ０ ０ ７ ２ ， Ｃ ｈ ｉ ｎ ａ ）
捷联成像导引头视线角速率提取方法与仿真分析
捷联成像导引头视线角速率提取方法与仿真分析姚秀娟;王宏宇;李力文【摘要】本文以工程应用为出发点,对捷联成像导引头视线角速率的提取及滤波方法进行了有条理的叙述.建立模型进行仿真并对仿真结果进行了分析.【期刊名称】《电子世界》【年(卷),期】2016(000)001【总页数】3页(P92-94)【关键词】捷联;视线角速率;滤波【作者】姚秀娟;王宏宇;李力文【作者单位】西南技术物理研究所;西南技术物理研究所;西南技术物理研究所【正文语种】中文捷联成像导引头结构上与弹体固连，使其失去了直接测定视线角速率的能力，只能测量目标相对于弹体的体视线角。
捷联成像导引头测量的体视线角中包含了目标相对惯性空间的视线角和弹体运动两部分信息，制导系统要实现制导律则必须将导引头测量信号中耦合的弹体运动信息去除，提取出目标相对惯性空间的视线角及角速率。
目标视线角速率解算中用到的坐标系有：地理坐标系Oxeyeze、弹体坐标系Oxbybzb、视线坐标系Oxsyszs、体视线坐标系Oxlylzl。
其中，θ为弹体姿态俯仰角，Ψ为弹体姿态偏航角，γ为弹体姿态滚动角，qλ为视线方位角，qγ为视线高低角，qβ为体视线方位角，qα为体视线高低角。
以上各定义及坐标系之间的转换关系见文献[1]。
在捷联导引头中，导引头传感器可测得体视线高低角qα、体视线方位角qβ，要求取视线高低角qγ、视线方位角qλ转动的角速度qγ和qλ。
根据文献[1]中坐标系定义，目标在体视线坐标系和视线坐标系中的位置坐标均为( R 0 0)，则目标在体坐标系以及地理坐标系中的坐标分别为：又根据关系：得：可得：其中：式（5）（6）即为捷联成像导引头系统目标视线角解耦算法。
文献[1]中根据目标视线角、体视线角及其微分利用个坐标系之间的推导关系求取目标视线角速率。
为了避免体视线角微分误差在复杂的计算中被放大。
本文将qγ和qλ进行微分直接得到目标在地理坐标系的俯仰视线角速度和方位视线角速度。
捷联式光学导引头视线角速率解耦与估计_孙婷婷
进行 视 线 角 速 率解 耦 与 估计算法 研究。首先 ， 建立 了 全 捷 联 导 引头 数 学 模型 ， 并利 用 Taylor 级 数对 其 进行线性化； 接着 ， 根 据 弹 目 运 动 几 何 学 与 坐 标 系 相 对 关 系 推 导 视 线 角 速 率解 耦 算法 ； 然后， 针对 捷 联 导 引头无 法 直 接 测 量 体 视 线 角 速 率 的 问题 , 提 出 微 分 + 稳 态 Kalman 滤 波 方法估计 体 视 线 角 速 率； 最 后， 建立 视 线 角 速 率解 耦 与 估计算法 验 证 系 统 并 进行 仿真 实验， 结果表明： 解 耦 算法 绝 对 误 差 小 于 5 × 10 -5 rad/s ， 相 对 误 差 小 于 0.3% ， 验 证 了 解 耦 算法 的 正 确性； 在 包含 导 引头 数 学 模型 的 条 件 下 ， 采用角 频 率为 19.2 rad/s 的 稳 态 Kalman 滤 波 器， 视 线 角 速 率 估计 误 差 小 于 4 × 10 -3 rad/s ， 较 直 接 微 分方法 的 估 计 误 差 提高 近 一个量 级 。 视 线 角 速 率解 耦 与 估计算法 同时 能 满 足 制 导系 统 对 精度 与 动态 性能 的 要求 。
(2) 弹 体 坐 标 系 Oxbybzb ： 原 点 O 选 在 导 弹 瞬 时 质
心 上 ； Oxb 轴 与 导 弹 纵 轴 重 合 ， 指 向 导 弹 头 部 为 正 ；
Oyb 轴 位 于 导 弹 纵 向 对 称 面 内 且 与 Oxb 轴 垂 直 ， 指 向
捷联惯导的解算程序
%====本程序为捷联惯导的解算程序(由惯性器件的输出解算出飞行器的位置、速度、姿态信息)======clear all;close all;clc;deg_rad=pi/180; %由度转化成弧度rad_deg=180/pi; %由弧度转化成度%-------------------------------从源文件中读入数据----------------------------------fid_read=fopen('IMUout.txt','r'); %path1_Den.dat 是由轨迹发生器产生的数据[AllDataNumofAllData]=fscanf(fid_read,'%g %g %g %g %g %g %g %g %g %g %g %g %g %g %g %g',[17 inf]);AllData=AllData';NumofEachData=round(NumofAllData/17);Time=AllData(:,1);longitude=AllData(:,2); %经度单位：弧度latitude=AllData(:,3); %纬度单位：弧度High=AllData(:,4); %高度单位：米Ve=-AllData(:,6); % 东向、北向、天向速度单位：米/妙Vn=AllData(:,5);Vu=AllData(:,7);fb_x=AllData(:,9); %比力（fx，fy，fz）fb_y=AllData(:,8); %指向右机翼方向为x正方向，指向机头方向为y正向，z轴与x轴和y轴构成右手坐标系单位：米/秒2fb_z=-AllData(:,10); %右前上pitch=AllData(:,11); %俯仰角（向上为正）单位:弧度head=-AllData(:,13); %偏航角（偏西为正）roll=AllData(:,12); %滚转角（向右为正）omigax=AllData(:,15); %陀螺输出（单位：弧度/秒，坐标轴的定义与比力的相同）omigay=AllData(:,14);omigaz=-AllData(:,16);%-------------------------------程序初始化--------------------------------------latitude0=latitude(1);longitude0=longitude(1); %初始位置High0=High(1);Ve0=Ve(1);Vn0=Vn(1); %初始速度Vu0=Vu(1);pitch0=pitch(1);head0=head(1); %初始姿态roll0=roll(1);TimeEach=0.005; %周期和仿真总时间TimeAll=(NumofEachData-1)*TimeEach;Omega_ie=0.7292115147E-4;%0.00007272205216643040; %地球自转角速度单位：弧度每妙g0=9.78;%------------------------------导航解算开始--------------------------------------%假设没有初始对准误差pitch_err0=pitch0+0*deg_rad;head_err0=head0+0*deg_rad;roll_err0=roll0+0*deg_rad;%初始捷联矩阵的计算《捷联惯导系统》P63 旋转顺序 head - pitch - roll %导航坐标系n为东北天方向载体坐标系b为右前上偏航角北偏西为正Tbn(1,1)=cos(roll_err0)*cos(head_err0)-sin(roll_err0)*sin(pitch_err0) *sin(head_err0);Tbn(1,2)=cos(roll_err0)*sin(head_err0)+sin(roll_err0)*sin(pitch_err0) *cos(head_err0);Tbn(1,3)=-sin(roll_err0)*cos(pitch_err0);Tbn(2,1)=-cos(pitch_err0)*sin(head_err0);Tbn(2,2)=cos(pitch_err0)*cos(head_err0);Tbn(2,3)=sin(pitch_err0);Tbn(3,1)=sin(roll_err0)*cos(head_err0)+cos(roll_err0)*sin(pitch_err0) *sin(head_err0);Tbn(3,2)=sin(roll_err0)*sin(head_err0)-cos(roll_err0)*sin(pitch_err0) *cos(head_err0);Tbn(3,3)=cos(roll_err0)*cos(pitch_err0);Tnb=Tbn';%位置矩阵的初始化《捷联惯导系统》P46 其中游动方位角 a＝0 假使初始经纬度确知Cne(1,1) = - sin(longitude0);Cne(1,2) = cos(longitude0);Cne(1,3) = 0;Cne(2,1) = - sin(latitude0) * cos(longitude0);Cne(2,2) = - sin(latitude0) * sin(longitude0);Cne(2,3) = cos(latitude0);Cne(3,1) = cos(latitude0) * cos(longitude0);Cne(3,2) = cos(latitude0) * sin(longitude0);Cne(3,3) = sin(latitude0);Cen=Cne';%初始四元数的确定《捷联惯导系统》 P151-152 方法本身保证了q1^2+q2^2+q3^2+q4^2=1q(2,1) = sqrt(abs(1.0 + Tnb(1,1) - Tnb(2,2) - Tnb(3,3))) / 2.0; q(3,1) = sqrt(abs(1.0 - Tnb(1,1) + Tnb(2,2) - Tnb(3,3))) / 2.0; q(4,1) = sqrt(abs(1.0 - Tnb(1,1) - Tnb(2,2) + Tnb(3,3))) / 2.0;q(1,1) = sqrt(abs(1.0 - q(2,1) ^2 - q(3,1) ^2 - q(4,1) ^2));% 判断q(1,1)的符号flag_q11=cos(head_err0/2.0)*cos(pitch_err0/2.0)*cos(roll_err0/2.0)-sin(head_err0/2.0)*sin(pitch_err0/2.0)*sin(roll_err0/2.0);if (flag_q11 >0) %此时q(1,1)取正if (Tnb(3,2) < Tnb(2,3))q(2,1) = - q(2,1);endif (Tnb(1,3) < Tnb(3,1))q(3,1) = - q(3,1);endif (Tnb(2,1) < Tnb(1,2))q(4,1) = - q(4,1);endelse %此时q(1,1)取负或0q(1,1) = - q(1,1);if (Tnb(3,2) > Tnb(2,3))q(2,1) = - q(2,1);endif (Tnb(1,3) > Tnb(3,1))q(3,1) = - q(3,1);endif (Tnb(2,1) > Tnb(1,2))q(4,1) = - q(4,1);endend%-------------------------迭代推算用到的参数的初始化------------------------Wiee_e = 0;Wiee_n = 0;Wiee_u = Omega_ie;Wiee = [Wiee_e Wiee_n Wiee_u]'; %地球速率在地球系中的投影东－北－天Lat_err(1)=latitude0;Lon_err(1)=longitude0;High_err(1)=High0;Ve_err(1)=Ve0;Vn_err(1)=Vn0;Vu_err(1)=Vu0;pitch_err(1)=pitch_err0;head_err(1)=head_err0;roll_err(1)=roll_err0;Re=6378137.0;%6378245.0; %地球长轴《惯性导航系统》 P28e=0.0033528106647474807198455286185206; %地球扁率精确值ee=0.00669437999014131699614;%----------------------------迭代推算开始-----------------------------------for i=1:NumofEachData%----------------------------惯性仪表数据的获得------------------------Wibb(1,1)=omigax(i); %指向右机翼方向为x正方向，指向机头方向为y正向，z轴与x轴和y轴构成右手坐标系Wibb(2,1)=omigay(i); %单位：弧度/妙Wibb(3,1)=omigaz(i); %右前上fb(1,1)=fb_x(i); %指向右机翼方向为x正方向，指向机头方向为y正向，z轴与x轴和y轴构成右手坐标系fb(2,1)=fb_y(i); %单位：米/秒2fb(3,1)=fb_z(i); %右前上%--------计算在姿态矩阵和位置矩阵更新时用到的参数------------------ RM=Re*(1.0-2.0*e+3.0*e*Cne(3,3)^2)+High_err(i); %《捷联惯导系统》P233 P235RN=Re*(1.0+e*Cne(3,3)^2)+High_err(i);% RN=Re*(1-ee)/(sqrt(1-ee*sin(Lat_err(i))))^3+High_err(i);% RM=Re/sqrt(1-ee*sin(Lat_err(i)))+High_err(i);%实验当地重力加速度计算《捷联惯导系统》P150 《惯性导航系统》 P35g=g0*((1.0+0.0052884*Cne(3,3)^2)-0.0000059*(1-(1-2*Cne(3,3)^2)^2))*(1 .0-2.0*High_err(i)/Re);tmp_slat=sin(Lat_err(i))*sin(Lat_err(i));Wien = Cne * Wiee; %地球速率在导航系中的投影Wenn(1,1) = -Vn_err(i)/RM;Wenn(2,1) = Ve_err(i)/RN; % <<惯性导航系统>> P45 考虑了地球转动的影响.Wenn(3,1) = Ve_err(i)*tan(Lat_err(i))/RN; %计算Wenn(不太精确)，更新速度和位置矩阵时用Winn=Wien+Wenn;Winb=Tbn*Winn;Wnbb=Wibb-Winb; %姿态速率在姿态更新时用到fn=Tnb*fb; % x-y-z 东－北－天% 速度的更新《捷联惯导系统》 P30 33 东－北－天difVe_err=fn(1,1)+(2*Wien(3,1)+Wenn(3,1))*Vn_err(i)-(2*Wien(2,1)+Wenn (2,1))*Vu_err(i);difVn_err=fn(2,1)-(2*Wien(3,1)+Wenn(3,1))*Ve_err(i)+(2*Wien(1,1)+Wenn (1,1))*Vu_err(i);difVu_err=fn(3,1)+(2*Wien(2,1)+Wenn(2,1))*Ve_err(i)-(2*Wien(1,1)+Wenn (1,1))*Vn_err(i)-g;Ve_err(i+1)=Ve_err(i)+difVe_err*TimeEach;Vn_err(i+1)=Vn_err(i)+difVn_err*TimeEach;Vu_err(i+1)=Vu_err(i)+difVu_err*TimeEach;High_err(i+1)=High_err(i)+Vu_err(i)*TimeEach;% 位置矩阵的实时更新《惯性导航系统》 P190Cne(1,1)=Cne(1,1)+TimeEach*(Wenn(3,1)*Cne(2,1)-Wenn(2,1)*Cne(3,1)); Cne(1,2)=Cne(1,2)+TimeEach*(Wenn(3,1)*Cne(2,2)-Wenn(2,1)*Cne(3,2)); Cne(1,3)=Cne(1,3)+TimeEach*(Wenn(3,1)*Cne(2,3)-Wenn(2,1)*Cne(3,3)); Cne(2,1)=Cne(2,1)+TimeEach*(-Wenn(3,1)*Cne(1,1)+Wenn(1,1)*Cne(3,1)); Cne(2,2)=Cne(2,2)+TimeEach*(-Wenn(3,1)*Cne(1,2)+Wenn(1,1)*Cne(3,2)); Cne(2,3)=Cne(2,3)+TimeEach*(-Wenn(3,1)*Cne(1,3)+Wenn(1,1)*Cne(3,3)); Cne(3,1)=Cne(3,1)+TimeEach*(Wenn(2,1)*Cne(1,1)-Wenn(1,1)*Cne(2,1)); Cne(3,2)=Cne(3,2)+TimeEach*(Wenn(2,1)*Cne(1,2)-Wenn(1,1)*Cne(2,2)); Cne(3,3)=Cne(3,3)+TimeEach*(Wenn(2,1)*Cne(1,3)-Wenn(1,1)*Cne(2,3));% Mat_Wenn(1,1)=0;% Mat_Wenn(1,2)=Wenn(3,1);% Mat_Wenn(1,3)=-Wenn(2,1); %Wenn的反对阵矩阵取负% Mat_Wenn(2,1)=-Wenn(3,1); %这里位置矩阵的及时修正为:dCne/dt=Mat_Wenn*Cne% Mat_Wenn(2,2)=0;% Mat_Wenn(2,3)=Wenn(1,1);% Mat_Wenn(3,1)=Wenn(2,1);% Mat_Wenn(3,2)=-Wenn(1,1);% Mat_Wenn(3,3)=0;%% Mat_Wenn=Mat_Wenn*Cne*TimeEach;% Cne=Cne+Mat_Wenn;Cen=Cne';% 计算经纬度Lat_err(i+1)=asin(Cne(3,3));Lon_err(i+1)=atan(Cne(3,2)/Cne(3,1)); %这是经度的主值if (Cne(3,1) < 0)if (Lon_err(i+1) > 0)Lon_err(i+1) = Lon_err(i+1) - pi;elseLon_err(i+1) = Lon_err(i+1) + pi;endend% 四元数的及时修正《惯性导航系统》 P194% Mat_Wnbb=[ 0, -Wnbb(1,1), -Wnbb(2,1), -Wnbb(3,1); % Wnbb(1,1), 0, Wnbb(3,1), -Wnbb(2,1); % Wnbb(2,1), -Wnbb(3,1), 0, Wnbb(1,1); % Wnbb(3,1), Wnbb(2,1), -Wnbb(1,1), 0];% q=q+Mat_Wnbb*q*TimeEach/2.0;q(1,1)=q(1,1)+TimeEach*(-Wnbb(1,1)*q(2,1)-Wnbb(2,1)*q(3,1)-Wnbb(3,1)* q(4,1))/2.0;q(2,1)=q(2,1)+TimeEach*(Wnbb(1,1)*q(1,1)+Wnbb(3,1)*q(3,1)-Wnbb(2,1)*q (4,1))/2.0;q(3,1)=q(3,1)+TimeEach*(Wnbb(2,1)*q(1,1)-Wnbb(3,1)*q(2,1)+Wnbb(1,1)*q (4,1))/2.0;q(4,1)=q(4,1)+TimeEach*(Wnbb(3,1)*q(1,1)+Wnbb(2,1)*q(2,1)-Wnbb(1,1)*q (3,1))/2.0;% 四元数归一化处理q_norm=sqrt(sum(q.*q));q=q/q_norm;% 计算姿态矩阵 TnbTnb(1,1) = q(1,1) ^2 + q(2,1) ^2 - q(3,1)^2 - q(4,1)^2;Tnb(1,2) = 2.0 * (q(2,1) * q(3,1) - q(1,1) * q(4,1));Tnb(1,3) = 2.0 * (q(2,1) * q(4,1) + q(1,1) * q(3,1));Tnb(2,1) = 2.0 * (q(2,1) * q(3,1) + q(1,1) * q(4,1));Tnb(2,2) = q(1,1)^2 - q(2,1)^2 + q(3,1)^2 - q(4,1)^2;Tnb(2,3) = 2.0 * (q(3,1) * q(4,1) - q(1,1) * q(2,1));Tnb(3,1) = 2.0 * (q(2,1) * q(4,1) - q(1,1) * q(3,1));Tnb(3,2) = 2.0 * (q(3,1) * q(4,1) + q(1,1) * q(2,1));Tnb(3,3) = q(1,1)^2 - q(2,1)^2 - q(3,1)^2 + q(4,1)^2;Tbn=Tnb';flag_pitch=asin(Tnb(3,2));flag_roll=atan(-Tnb(3,1)/Tnb(3,3));flag_head=atan(-Tnb(1,2)/Tnb(2,2));if(Tnb(3,3)<0)if(flag_roll<0)flag_roll=flag_roll+pi;endif(flag_roll>0)flag_roll=flag_roll-pi;endend% 偏航角范围 -180度——180度北偏西为正if(Tnb(2,2)<0)if(flag_head<0)flag_head=flag_head+pi;endif(flag_head>0)flag_head=flag_head-pi;endend% 姿态角更新pitch_err(i+1)=flag_pitch;head_err(i+1)=flag_head;roll_err(i+1)=flag_roll;% 解算完毕由对准结果、陀螺、加表的输出解算出载体的位置、速度、姿态%----------------------计算解算误差------------------ddLat(i)=(Lat_err(i)-latitude(i))*rad_deg; %纬度误差单位：度ddLog(i)=(Lon_err(i)-longitude(i))*rad_deg; %经度误差单位：度ddHigh(i)=High_err(i)-High(i); %高度误差单位：米ddVe(i)=Ve_err(i)-Ve(i);ddVn(i)=Vn_err(i)-Vn(i); % 速度误差单位：米/妙2ddVu(i)=Vu_err(i)-Vu(i);ddpitch(i)=(pitch_err(i)-pitch(i))*rad_deg*3600; %姿态误差单位：度ddhead(i)=(head_err(i)-head(i))*rad_deg*3600;ddroll(i)=(roll_err(i)-roll(i))*rad_deg*3600;endfclose(fid_read);%---------------------------绘图开始--------------------------------- figure(1)plot(Time,ddLog)ylabel('经度误差(度)'),xlabel('时间(秒)');figure(2)plot(Time,ddLat)ylabel('纬度误差(度)'),xlabel('时间(秒)');figure(3)plot(Time,ddHigh);ylabel('高度误差(米)'),xlabel('时间(秒)');figure(4)plot(Time,ddhead)ylabel('偏航角误差(角妙)'),xlabel('时间(秒)'); figure(5)plot(Time,ddpitch)ylabel('俯仰角误差(角妙)'),xlabel('时间(秒'); figure(6)plot(Time,ddroll);ylabel('滚转角误差(角妙)'),xlabel('时间(秒)'); figure(7)plot(Time,ddVe);ylabel('东向速度误差(米/秒)'),xlabel('时间(秒)'); figure(8)plot(Time,ddVn)ylabel('北向速度误差(米/秒)'),xlabel('时间(秒)'); figure(9)plot(Time,ddVu)ylabel('天向速度误差(米/秒)'),xlabel('时间(秒)');%------------------------------绘图结束-------------------------------。
- 1、下载文档前请自行甄别文档内容的完整性，平台不提供额外的编辑、内容补充、找答案等附加服务。
- 2、"仅部分预览"的文档,不可在线预览部分如存在完整性等问题,可反馈申请退款(可完整预览的文档不适用该条件!)。
- 3、如文档侵犯您的权益，请联系客服反馈,我们会尽快为您处理(人工客服工作时间：9:00-18:30)。
0 引言
捷联激光导引头去除了传统导引头的万向支架和复杂的伺 服机构以及陀螺等感应器，使导引头刚性地与弹体连接，降低导 引头的设计复杂度，减轻了重量，降低了成本。捷联导引头由于上 述特点得到了越来越多的重视。
不过捷联激光导引头只能输出视线误差角，不能输出视线角 速度，而且视线误差角存在严重的非线性和复杂的测量噪声，这 就要求对系统设计合适的滤波器。近年来，基于 UT 变换的 UKF 算 法得到快速发展，其以不需要求导、估计精度高等优点，用于全捷 联激光导引头视线角速度的提取，可以解决视线角速度提取过程 中非线性以及噪声的干扰问题，大大提高制导炸弹的精度。
算法的初值在线解算算法。仿真结果表明算法可以大大提高视线角速度算法的收敛速度和制导精度。
关键词 ：捷联激光导引头 ；视线误差角 ；UKF ；视线角速度初值 ；bp 神经网络
DOI:10.16520/ki.1000-8519.2016.18.026
An online algorithm for the initial value of line of sight velocity of Strapdown laser seeker
(1)
其中，
。
在观测方程的建立中，捷联导引头的测量信息有实现误差角
、 ，将其作为观测量，有观测方程为 ：
图 1 导弹与目标的空间几何关系
54
上式中的 为矩阵
(2) 的元素， 为俯仰角， 为偏航
2016.18
理论与算法
角， 为滚转角。且式中 、 为测量量中的噪声信号。
（3）
1.2.2 UKF 算法流程 建立以下 UKF 计算流程 ： （1）初始化。初始状态 的统计学特性为 ：
理论与算法
2016.18
一种捷联激光导引头视线角速度初值的在线解算算法
高薇薇 （中国空间技术研究院，北京 ,100094）
摘要 ：捷联式激光导引头与弹体固联，导引头只能提供视线误差角信息。本人采用了 UKF 算法得到视线角速度。不过当攻击移
动目标时，UKF 算法的制导精度和收敛时间均下降。为了提高算法的制导精度和收敛时间，本文提出了一种基于 bp 神经网络
(4)
（2）根据 的统计特性计算 sigma 采样点。
(5)
（3）计算经非线性函数 传递后的 sigma 点。 (6)
（4）计算状态量一步预测值 和误源自方差阵 。(7)（5）计算经非线性函数 传递后的 sigma 点。 (8)
（6）计算观测预测值 和相应的方差阵
(9)
（7）计算滤波增益矩阵 （8）计算更新后的状态量和方差阵
视线高低角及视线高低角速率，视线方位角及视线方位角速率作 为状态变量，建立状态方程。导弹与目标的空间几何关系见图 1。 图中，T 为弹目视线，M 为弹体。
1.2 UKF 滤波算法
1.2.1 建立状态方程和观测方程
设
为 系 统 的 状 态 量，
为
视线高低角、视线高低角速度、视线方位角、视线方位角速度。 状态方程如下 ：
Gao Weiwei
(China Academy of space technology,Beijing,100094)
Abstract ：The strapdown laser seeker and missile body are combined,and the seeker can only provide the information of line of sight error.I used the UKF algorithm to get the line of sight speed.However,when the target is attacked,the guidance precision and convergence time of the UKF algorithm are decreased.In order to improve the guidance precision and convergence time of the algorithm,this paper presents a new algorithm based on BP neural network algorithm. The simulation results show that the algorithm can greatly improve the convergence speed and precision of the line of sight. Keywords ：strapdown seeker ；Los angles ；UKF ；initial value of Los angles ；BP neural network
(12) 其中， 为非线性函数， 为 6 个输入，分别为根据装订 目标计算出两个视线角和视线角速度以及根据全捷联导引头计 算出的两个视线角 ； 为 2 个输出，即视线角速度的初值。 为了实现公式 (12) 中的函数，并保证实现角速度提取精度 和收敛速度，必须选择合适的算法。bp 神经网络算法具有强大 的非线形映射能力，可以表示多输入和多输出的非线性映射，并 且计算过程简单，计算复杂度低，计算量小，因此采用了 bp 神经 网络算法来实现上述目的。论研究表明，三层的 BP 网络能以任意 精度逼近任意的非线性函数。
(10) (11)
2 UKF 算法初值选取的优化方法
当攻击目标为移动目标时，UKF 算法得到视线角速度的精 度有所下降。通过分析，发现攻击移动目标，UKF 算法的状态变
量的初值解算精度下降造成的。 对于固定目标以及运动目标来说，弹目之间视线角和角速度
的真实值与根据装订目标的位置信息计算出的视线角和视线角 速度之间存在某种映射关系。可以根据这些关系以及导引头提供 的视线误差角度信息，求解出视线角速度的初值。该映射关系是 一个非线性的函数。需要找到一种算法影射出这种非线性关系。 该算法需要在弹载计算机上实时解算，因此要求算法的计算量不 能大。该关系可以由下面的函数表示 ：
根据数学仿真分析发现，状态变量 中视线角速度的初值对 于 UKF 滤波方法的收敛时间及精度的都有较大的影响，因此本文 提出一种基于 bp 神经网络且可以在弹上进行实时解算状态变量 初值的算法，用来提高 UKF 算法的精度。
1 视线角速度提取算法
1.1 导引头视线误差角模型
捷联激光导引头测得的视线误差角信息，将导引头包含噪声 的测量量 、 作为观测量，建立观测方程 ；将需要解算得到的