法律状态公告日
法律状态信息
法律状态
2018-03-30
授权
授权
2016-05-11
实质审查的生效 IPC(主分类):G01C21/20 申请日:20151125
实质审查的生效
2016-04-13
公开
公开
技术领域
本发明涉及导弹的精确制导方法,属于导弹制导控制技术领域,具体涉及大目 标—导弹相对运动信息精确估计方法。
背景技术
近些年来,为了降低其成本,提高其可靠性,精确制导武器越来越多地采用小型捷 联或半捷联导引头,这种导引头的特点是可以测量目标—导弹之间的相对视线角,如果是 主动式导引头还可以测量距离信息,但捷联或半捷联导引头不能直接输出实际制导律需要 的视线角速率信息。
针对安装捷联导引头的导弹这一应用背景,极坐标形式的目标—导弹相对运动运 动学和动力学方程构造二维的Kalman滤波器,可以得到高精度的视线角速率估计。为了进 一步提高这种滤波器的收敛速度,我们提出了一种通过自适应调整滤波器中的状态噪声协 方差阵和测量噪声协方差阵,并恰当选用初值来提高收敛速度的估计视线角速率的快收敛 Kalman滤波器。
发明内容
本发明的目的是提供一种估计弹目视线角速率的快收敛Kalman滤波器的设计方 法,以提高现有的目标—导弹视线角速度估计Kalman滤波方法的收敛速度。所述弹目视线 角速率是指导弹到目标的视线和发射点惯性坐标的夹角的变化率。
本发明为解决上述技术问题采取的技术方案是:
一、滤波器初始估计中的视线俯仰角和视线偏航角的初值和取为根据 导引头第1拍测量值计算出来的视线俯仰角和视线偏航角,视线俯仰角速率和视线偏航角 速率的初值和利用几何方法计算出来,即
其中,Δx、Δy、Δz为末制导初始时刻目标—导弹相对位置在末制导惯性参考坐 标系三个轴上的分量,为末制导初始时刻目标—导弹相对速度在末制导惯性 参考坐标系三个轴上的分量,它们的具体计算过程如下:设末制导惯性参考坐标系o0x0y0z0在末制导过程中相对于惯性空间固化不动,它与地心惯性坐标系以及发射点惯性坐标系的 关系如图1所示。
末制导惯性参考坐标系的原点为末制导初始时刻导弹的质心o0,o0x0沿着末制导 初始时刻视线方向,指向目标为正,o0y0轴位于包含o0x0轴的铅垂面内,指向上为正,o0z0轴 由右手定则确定。发射点惯性坐标系的原点为发射点oF,oFyF轴铅锤向上,oFxF轴和oFzF轴位 于水平面内,其指向按需要选取,通常oFxFyF为射击平面。地心惯性坐标系的原点oI位于地 心上,oIxI轴位于赤道平面内,指向某一恒星,oIzI轴指向北极方向,oIyI按右手定则确定。地 心惯性坐标系与发射点惯性坐标系之间的转换取决于λ0和αF三个角度,它们的定义见 图1。图1中,qε0和qβ0分别为末制导初始时刻视线相对于发射点惯性坐标系所成的视线仰角 和视线偏角。
设导弹在末制导惯性参考坐标系的位置为(x、y、z),其速度在该坐标系中的投影 为这些信息由弹上导航系统提供;目标在末制导惯性参考坐标系的位置为 (xt、yt、zt),其速度在该坐标系中的投影为这些信息由地面上的目标测量系统 提供,则Δx=xt-x,Δy=yt-y,Δz=zt-z,
这样,在很大程度上降低了滤波器的初始动态过程。
二、测算当前时刻目标与导弹之间的相对距离和相对速度,具体过程如下:记当前 时刻相对位置在末制导惯性参考坐标系中的分量为Δx=xt-x,Δy=yt-y,Δz=zt-z,当前 时刻相对速度在末制导惯性参考坐标系中的分量为则 目标—导弹相对距离为相对速度为
三、测算导弹加速度在视线坐标系o′x4y4z4(见图2)o′y4和o′z4轴方向上的分量aε和aβ,以及目标加速度在视线坐标系o′y4和o′z4轴方向上的分量atε和atβ;导弹上加速度计 输出的加速度是沿着弹体坐标系ox1y1z1(见图1)投影的,把它投影到视线坐标系即可求 得导弹法向加速度aε和aβ;由图3所示,导弹弹体坐标系ox1y1z1的原点o位于导弹质心上。ox1轴与弹体纵轴重合,指向头部为正,oy1轴在弹体纵向对称平面内,垂直ox1轴,指向上方为 正,oz1轴方向按右手定则确定;由图2,视线坐标系o′x4y4z4原点o′位于导引头回转中心,o′ x4轴与目标—导弹视线一致,由导引头回转中心指向目标为正,o′y4轴位于包含o′x4轴的铅 锤面内,与o′x4轴垂直,指向上方为正,o′z4轴按右手定则确定。由末制导惯性坐标系转换到 视线坐标系定义了视线俯仰角qε和视线偏航角qβ(见图2)。根据目标轨道测量数据可以计算 出目标加速度在地心惯性坐标系中的三个分量,然后投影到视线坐标系求得atε和atβ;
四、由导弹的俯仰通道视线运动状态方程和俯仰通道视线角测量方程构造俯仰通 道快收敛Kalman滤波算法,从而求出目标与导弹之间的视线俯仰角速率,由导弹的偏航通 道视线运动状态方程和偏航通道视线角测量方程构造偏航通道快收敛Kalman滤波算法,从 而求出目标与导弹之间的视线偏航角速率。
俯仰通道视线运动离散化状态方程为
其中,k代表第k个采样时刻,Δt为采样周期;
俯仰通道视线角测量方程可以写作
其中,Hε=[10],νε(k)为视线俯仰角的测量噪声,假设其均值为零。
设
则俯仰通道快收敛Kalman滤波器为
其中,和分别代表Xε的滤波估计和预报估计,γ选择一个充分大的正 值
λ是一个正的标量。若γ选择一个充分大的正值,可以满足在最坏的初始条件下也 具有较好的滤波收敛特性。但γ选取得过大会对滤波器的稳态特性有一定影响,在应用中 应该采取一种折中的选择。另外,为了确保稳态阶段,Rk更接近于或等于Rk0,λ不易选择过 大。
根据导弹的偏航通道视线运动状态方程和偏航通道视线角测量方程可以设计出 估计目标与导弹之间偏航通道视线角速率的快收敛Kalman滤波器:
偏航通道视线运动离散化状态方程写作
Xβ(k+1)=Φβ(k)Xβ(k)+Ββ(k)uβ(k)+wβ(k)(5)
其中,
wβ(k)是一个零均值随机过程,
偏航视线角测量方程写作
其中,Hβ=[10],νβ(k)为视线俯仰角的测量噪声,假设其均值为零,
偏航通道Kalman滤波器为
其中,和分别代表Xβ的滤波估计和预报估计,
本发明的有益效果是:
由导弹的俯仰通道视线运动状态方程和俯仰通道视线角测量方程构造俯仰通道 快收敛Kalman滤波器,从而求出目标与导弹之间的视线俯仰角速率,由导弹的偏航通道视 线运动状态方程和偏航通道视线角测量方程构造偏航通道快收敛Kalman滤波器,从而求出 目标与导弹之间的视线偏航角速率,最终完成目标与导弹之间的视线角速率的估计。
本发明提出的估计视线角速度的快收敛Kalman滤波器的收敛速度高于现有的 Kalman滤波器。本发明通过自适应调整滤波器中的状态噪声协方差阵和测量噪声协方差 阵,并恰当选用初值来提高收敛速度的估计视线角速率的快收敛Kalman滤波器。提高了极 坐标形式的目标—导弹相对运动运动学和动力学方程构造二维的Kalman滤波器的收敛速 度,得到了高精度的视线角速率估计。
附图说明
图1是地心惯性坐标系和发射点惯性坐标系关系图;图2是发射点惯性坐标系和视 线坐标系关系图;图3是导弹弹体坐标系示意图;
图4是快收敛Kalman滤波器视线俯仰角速率完整跟踪过程图(γ=102,);
图5是快收敛Kalman滤波器视线俯仰角速率初段动态跟踪过程图(γ=102,);
图6是快收敛Kalman滤波器视线俯仰角速率稳态跟踪过程图(γ=102,);
图7是快收敛Kalman滤波器视线俯仰角速率末段动态跟踪过程图(γ=102,);
图8是快收敛Kalman滤波器视线偏航角速率完整跟踪过程图(γ=102,);
图9是快收敛Kalman滤波器视线偏航角速率初段动态跟踪过程图(γ=102,);
图10是快收敛Kalman滤波器视线偏航角速率稳态跟踪过程图(γ=102,);
图11是快收敛Kalman滤波器视线偏航角速率末段动态跟踪过程图(γ=102,);
图12是Kalman滤波器视线俯仰角速率完整跟踪过程图
图13是Kalman滤波器视线俯仰角速率初段动态跟踪过程图
图14是Kalman滤波器视线俯仰角速率稳态跟踪过程图
图15是Kalman滤波器视线俯仰角速率末段动态跟踪过程图
图16是Kalman滤波器视线偏航角速率完整跟踪过程图
图17是Kalman滤波器视线偏航角速率初段动态跟踪过程
图18是Kalman滤波器视线偏航角速率稳态跟踪过程图
图19是Kalman滤波器视线偏航角速率末段动态跟踪过程图
具体实施方式
下面通过数学推导演示本发明Kalman滤波器的由来:
(一)目标—导弹相对运动动力学
设目标—导弹视线矢量可以表示为
其中,和分别代表目标和导弹在发射点惯性坐标系中的位置矢量。上式相对 时间微分,得到
其中,和分别代表视线矢量在惯性坐标系和视线坐标系中相对时间求 导,代表视线坐标系相对惯性坐标系的旋转角速度,和分别代表目标和导弹速度矢 量。上式写成在视线坐标系中的投影形式:
其中,
则
把公式
相对时间求导,得到
投影到视线坐标系,可得
即
其中,ar、aε和aβ是导弹加速度在视线坐标系三个轴上的分量,atr、atε和atβ是目标 加速度在视线坐标系三个轴上的分量。
在末制导过程中,视线角速率为小量,因此,可以忽略式(7)中第二式中的二阶小 量则俯仰视线变化动力学方程简化为
而忽略式(7)中第三式中的二阶小量而且以末制导惯性参考坐标系为基准, qε为小角度,则偏航视线变化动力学方程简化为
俯仰通道视线运动状态方程为
令Δt为采样周期,式(10)离散化后就得到式(3),即
又设
则式(3)可以写作
Xε(k+1)=Φε(k)Xε(k)+Βε(k)uε(k)
考虑模型误差等原因,在上式中加入了一个零均值随机过程wε(k),就得到
Xε(k+1)=Φε(k)Xε(k)+Βε(k)uε(k)+wε(k)
俯仰通道视线角测量方程可以写作式(4),即
其中,Hε=[10],νε(k)为视线俯仰角的测量噪声,假设其均值为零,
同理,偏航通道视线运动状态方程为
式(11)离散化后得到
由式(12),偏航通道视线运动离散化状态方程写作式(5),即
Xβ(k+1)=Φβ(k)Xβ(k)+Ββ(k)uβ(k)+wβ(k)
其中,
wβ(k)是一个零均值随机过程,
偏航视线角测量方程写作式(6),即
其中,Hβ=[10],νβ(k)为视线俯仰角的测量噪声,假设其均值为零,
(二)快收敛Kalman滤波算法
在末制导Kalman滤波算法设计中,设和分别代表Xε(k)和Xβ(k)的一步 预报估计,定义新息和根据新息自适 应地调节滤波算法中的状态噪声协方差阵Qε(k)和Qβ(k),并定义状态一步预报估计协方差 阵分别为
根据它们在线地调整滤波算法中的测量噪声协方差阵Rε(k)和Rβ(k)。通过自适应 地调节滤波算法中的这两个矩阵,可以扩大滤波器稳定的吸引区域,令其快速收敛,即构成 快速收敛末制导Kalman滤波器。
俯仰通道快收敛Kalman滤波器表达式为
而偏航通道快收敛Kalman滤波器表达式为
在上述滤波算法中,λ是一个正的标量。若γ选择一个充分大的正值,可以满足在 最坏的初始条件下也具有较好的滤波收敛特性。但γ选取得过大会对滤波器的稳态特性有 一定影响,在应用中应该采取一种折中的选择。另外,为了确保稳态阶段,Rk更接近于Rk0,λ 不易选择过大。算法中的Pε(k+1)和Pβ(k+1)代表状态的滤波估计协方差阵,即
实现上述滤波器需要知道相对距离和相对速度信息。记拦截器在发射点惯性坐标 系的位置为(x、y、z),其速度在该坐标系中的投影为这些信息由弹上导航系统 提供;目标在发射点惯性坐标系的位置为(xt、yt、zt),其速度在该坐标系中的投影为 这些信息由轨道预报系统提供。记Δx=xt-x,Δy=yt-y,Δz=zt-z,
(三)视线俯仰角和视线偏航角的提取算法
导引头输出的只是目标—导弹视线相对导引头测量坐标系的视线角Δqε和Δqβ。 导引头测量坐标系的圆点在导引头回转中心o′,在不考虑导引头安装误差及导引头安装位 置挠性变形的情况下,其o′x6轴、o′y6轴和o′z6轴分别平行于弹体坐标系的ox1轴、oy1轴和 oz1轴。
设导引头测量坐标系与弹体坐标系重合,则目标—导弹视线的单位向量在导引头 测量坐标系中的投影为
而它在发射点惯性坐标系中的投影为
则
其中,CF→1为由发射点惯性坐标系到弹体坐标系的转换矩阵,其定义为
其中,ψ和γ分别为弹体的俯仰角,偏航角和滚动角,由惯导系统测算得到。
根据式(14),可以计算出视线俯仰角和视线偏航角,即
qε=sin-1c0y(17)
滤波器初始估计中的视线俯仰角和视线偏航角的初值和取为根据导 引头第1拍测量值计算出来的视线俯仰角和视线偏航角,视线俯仰角速率和视线偏航角速 率的初值和利用几何方法计算出来,即
记Δx、Δy、Δz为末制导初始时刻目标—导弹相对位置在末制导惯性参考坐标系 三个轴上的分量,为末制导初始时刻目标—导弹相对速度在末制导惯性参考 坐标系三个轴上的分量。导弹在末制导惯性参考坐标系的位置为(x、y、z),其速度在该坐标 系中的投影为这些信息由弹上导航系统提供;目标在发射点惯性坐标系的 位置为(xt、yt、zt),其速度在该坐标系中的投影为这些信息由地面上的目标测 量系统提供,则Δx=xt-x,Δy=yt-y,Δz=zt-z,
(四)下面通过数值仿真结果说明本实施方式的效果:
设仿真初始时刻,目标的高度为Ht=307.49km,飞行速度为4242m/s,导弹的高度 为254.12km,飞行速度3000m/s,二者相对距离112.196km,相对速度为-6680.7m/s。
导弹的初始姿态角速率取为零,初始姿态大致指向初始视线方向,滚转角基本等 于零。
仿真中,取惯导系统数据更新周期为5ms,导引头数据更新周期为15ms,轨控周期 15ms,姿控周期5ms。
导引头测量数据Δqε和Δqβ中包含0.17mrad量化噪声。导引头误差角中分别包含 安装误差角0.2°和-0.2°(常值),同时又分别包含轨控发动机开关机所引起的导引头安装 位置挠性变形产生的误差。
惯导系统初始姿态误差用标准差为0.1°的高斯白噪声描述,初始速度误差用标准 差为1m/s的高斯白噪声描述,在发射点惯性坐标系x轴上的初始位置误差在(2km~5km)范 围内均匀分布,或在(-2km~-5km)范围内均匀分布;在发射点惯性坐标系y轴和z轴上的初 始位置误差在±1km内均匀分布。设目标轨道位置预报数据在地心惯性坐标系xI轴上的位 置误差用均值为1km,标准差为1km的高斯白噪声表述,yI轴和zI轴方向上的位置误差分别用 均值为1km,标准差为1km的高斯白噪声表述;三个方向上的速度误差均取上述相应数据的 1/(1000s)。轨道预报数据每1s输出一次,而滤波器的采样周期为15ms,这样在每两个预报 数据之间用插值方法获得更多数据。
在快收敛Kalman滤波器设计中,滤波器初始估计中的视线俯仰角和视线偏航角的 初值和取为根据导引头第1拍测量值计算出来的视线俯仰角和视线偏航角,视线 俯仰角速率和视线偏航角速率的初值和利用几何方法计算出来。
动态噪声方差阵中的Qε0(k)和Qβ0(k)分别取为
测量噪声方差阵中Rε0(k)和Rβ0(k)分别取为
状态估计方差阵的初值取为
在动态噪声方差阵计算公式
中,参数γ是一个需要选取的重要参数,γ选取得大有利于扩大滤波器的稳定区 域,但可能影响稳态的滤波效果。折中考虑后选取γ=102,这是一个较大的参数,它可以保 证滤波器的稳定区域足够大。
在测量噪声方差阵计算公式
中,选取λ=3。
在这种情况下,快收敛Kalman滤波器估计的视线俯仰角速率跟踪其真实值的全过 程、初段动态过程、稳态跟踪过程,以及末段跟踪过程分别如图4-7所示,而快收敛Kalman滤 波器估计的视线偏航角速率跟踪其真实值的全过程、初段动态过程、稳态跟踪过程,以及末 段跟踪过程如图8-11所示。从这些仿真结果图可以看出,滤波器在1s之后已经保证跟踪误 差小于0.005°/s。随后的稳态跟踪过程中,由于γ减小后,滤波器的稳态增益上升,消除了 静差。在末段跟踪阶段也仅在最后的0.1s内没有跟踪上真值,对制导精度影响不大。
为了进行对比,我们在图12-15中给出了标准Kalman滤波器估计的视线俯仰角速 率跟踪其真实值的全过程、初段动态过程、稳态跟踪过程,以及末段跟踪过程,在图16-19中 给出了标准Kalman滤波器估计的视线偏航角速率跟踪其真实值的全过程、初段动态过程、 稳态跟踪过程,以及末段跟踪过程。从这些图中可以发现,标准Kalman滤波器稳态跟踪性能 好,但初段动态跟踪阶段进入稳态的时间稍长,大约为2.5s。其末段跟踪性能尚可,本次仿 真脱靶量为0.056m,轨控发动机开机时间5.929s。
机译: 双目视觉函数测量方法,双目视觉功能测量程序,眼镜镜头设计方法,眼镜镜片制造方法和双目视觉功能测量系统
机译: 双目视觉功能测量方法,双目视觉功能测量程序,眼镜镜头设计方法,眼镜镜片制造方法和双目视觉功能测量系统
机译: 代码量估计方法,帧速率估计方法,代码量估计设备,帧速率估计设备,代码量估计程序,帧速率估计程序和存储该程序的计算机可读记录介质