估计弹目视线角速率的快收敛Kalman滤波器的设计方法
估计弹目视线角速率的快收敛Kalman滤波器的设计方法
【技术领域】
[0001] 本发明设及导弹的精确制导方法，属于导弹制导控制技术领域，具体设及大目 标一导弹相对运动信息精确估计方法。
【背景技术】
[0002] 近些年来，为了降低其成本，提高其可靠性，精确制导武器越来越多地采用小型捷 联或半捷联导引头，运种导引头的特点是可W测量目标一导弹之间的相对视线角，如果是 主动式导引头还可W测量距离信息，但捷联或半捷联导引头不能直接输出实际制导律需要 的视线角速率信息。
[0003] 针对安装捷联导引头的导弹运一应用背景，极坐标形式的目标一导弹相对运动运 动学和动力学方程构造二维的Kalman滤波器，可W得到高精度的视线角速率估计。为了进 一步提高运种滤波器的收敛速度，我们提出了一种通过自适应调整滤波器中的状态噪声协 方差阵和测量噪声协方差阵，并恰当选用初值来提高收敛速度的估计视线角速率的快收敛 Kalman滤波器。
【发明内容】
[0004] 本发明的目的是提供一种估计弹目视线角速率的快收敛Kalman滤波器的设计方 法，W提高现有的目标一导弹视线角速度估计Kalman滤波方法的收敛速度。所述弹目视线 角速率是指导弹到目标的视线和发射点惯性坐标的夹角的变化率。
[000引本发明为解决上述技术问题采取的技术方案是：
[0006] 一、滤波器初始估计中的视线俯仰角和视线偏航角的初值表(0巧日如(0)取为根据 导引头第1拍测量值计算出来的视线俯仰角和视线偏航角，视线俯仰角速率和视线偏航角 速率的初值是.(0)和^(0)利用几何方法计算出来，即
[0009] 其中，Αχ、Ay、A Z为末制导初始时刻目标一导弹相对位置在末制导惯性参考坐 标系Ξ个轴上的分量，Ai、Aj、： Δ?为末制导初始时刻目标一导弹相对速度在末制导惯性 参考坐标系Ξ个轴上的分量，它们的具体计算过程如下：设末制导惯性参考坐标系ooxoyozo 在末制导过程中相对于惯性空间固化不动，它与地屯、惯性坐标系W及发射点惯性坐标系的 关系如图1所不。
[0010] 末制导惯性参考坐标系的原点为末制导初始时刻导弹的质屯、00,00X0沿着末制导 初始时刻视线方向，指向目标为正，ooyo轴位于包含00X0轴的铅垂面内，指向上为正，O0Z0轴 由右手定则确定。发射点惯性坐标系的原点为发射点〇F，〇FyF轴铅键向上，OF祉轴和OFZF轴位 于水平面内，其指向按需要选取，通常OFXFyF为射击平面。地心肢性坐标系的原点ΟΙ位于地 屯、上，ΟΙΧΙ轴位于赤道平面内，指向某一恒星，ΟΙΖΙ轴指向北极方向，oiyi按右手定则确定。地 屯、惯性坐标系与发射点惯性坐标系之间的转换取决于稱、λ〇和αρΞ个角度，它们的定义见 图1。图1中，qso和qeo分别为末制导初始时刻视线相对于发射点惯性坐标系所成的视线仰角 和视线偏角。
[0011] 设导弹在末制导惯性参考坐标系的位置为(x、y、z)，其速度在该坐标系中的投影 为(X、来、3)，运些信息由弹上导航系统提供；目标在末制导惯性参考坐标系的位置为 (xt、yt、zO，其速度在该坐标系中的投影为（?、拓、為），运些信息由地面上的目标测量系统 提供，则A又二又广又，A y = y广y，A z = z广Z，化二文(-史'，Δ歹=义-，Δ? =与:-i；。
[0012] 运样，在很大程度上降低了滤波器的初始动态过程。
[0013] 二、测算当前时刻目标与导弹之间的相对距离和相对速度，具体过程如下:记当前 时刻相对位置在末制导惯性参考坐标系中的分量为Δ x = x广X，Ay = y广y，Δ z = Z广Z，当前 时刻相对速度在末制导惯性参考坐标系中的分量为馬-X，Aj = J，Δ?二S，则 目标一导弹相对距离为
，相对速度为度二（心Μ + Δ}'轉+心公)/巧。; 在末制导过程中，始终有R〉〇，人' < Cl;
[0014];、测算导弹加速度在视线坐标系c/ Χ4Υ4Ζ4(见图2)c/ Y4和c/ Z4轴方向上的分量ae 和ae，W及目标加速度在视线坐标系〇/y4和〇/Z4轴方向上的分量ate和ate;导弹上加速度计 输出的加速度α是沿着弹体坐标系oxmzi(见图1)投影的，把它投影到视线坐标系即可求 得导弹法向加速度3ε和ae;由图3所示，导弹弹体坐标系oxiyizi的原点0位于导弹质屯、上。0X1 轴与弹体纵轴重合，指向头部为正，oyi轴在弹体纵向对称平面内，垂直0X1轴，指向上方为 正，OZ1轴方向按右手定则确定；由图2,视线坐标系c/x4y4Z4原点oM立于导引头回转中屯、，〇/ X4轴与目标一导弹视线一致，由导引头回转中屯、指向目标为正，c/y4轴位于包含c/x4轴的铅 键面内，与C/X4轴垂直，指向上方为正，c/z4轴按右手定则确定。由末制导惯性坐标系转换到 视线坐标系定义了视线俯仰角qe和视线偏航角qe(见图2)。根据目标轨道测量数据可W计算 出目标加速度嗓在地屯、惯性坐标系中的Ξ个分量，然后投影到视线坐标系求得ate和曰证；
[0015] 四、由导弹的俯仰通道视线运动状态方程和俯仰通道视线角测量方程构造俯仰通 道快收敛Kalman滤波算法，从而求出目标与导弹之间的视线俯仰角速率，由导弹的偏航通 道视线运动状态方程和偏航通道视线角测量方程构造偏航通道快收敛Kalman滤波算法，从 而求出目标与导弹之间的视线偏航角速率。
[0016] 俯仰通道视线运动离散化状态方程为
[0017]
掛
[0018] 其中，k代表第k个采样时刻，At为采样周期；
[0019] 俯仰通道视线角测量方程可W写作
[0020]
(4)
[0021]其中，Ηε=[1 0]，νε化)为视线俯仰角的测量噪声，假设其均值为零。
[0024] 则俯仰通道快收敛Kalman滤波器为
[0025]
[0026] 其中，i,(X.)和丈，W分别代表Χε的滤波估计和预报估计，丫选择一个充分大的正 值
[0029] λ是一个正的标量。若丫选择一个充分大的正值，可W满足在最坏的初始条件下也 具有较好的滤波收敛特性。但γ选取得过大会对滤波器的稳态特性有一定影响，在应用中 应该采取一种折中的选择。另外，为了确保稳态阶段，Rk更接近于或等于化〇，λ不易选择过 大。
[0030] 根据导弹的偏航通道视线运动状态方程和偏航通道视线角测量方程可W设计出 估计目标与导弹之间偏航通道视线角速率的快收敛Kalman滤波器：
[0031] 偏航通道视线运动离散化状态方程写作
[0032] 站化+1) = Φβ化)抽化)+B β化)ι?β化)+WP化）（5)
[0033] 其中，
[0036] 偏航视线角测量方程写作
[0037]
巧）
[0038] 其中，He=[l 0]，ve(k)为视线俯仰角的测量噪声，假设其均值为零，
[0039] 偏航通道Kalman滤波器为
[0040]
[00川其中，^从)和马炒)分别代表站的滤波估计和预报估计，
[004引
，λ和丫（为两个参量，而 且在滤波过程中取为常量)的取值规律与俯仰通道快收敛Kalman滤波器相同。
[0044] 本发明的有益效果是：
[0045] 由导弹的俯仰通道视线运动状态方程和俯仰通道视线角测量方程构造俯仰通道 快收敛Kalman滤波器，从而求出目标与导弹之间的视线俯仰角速率，由导弹的偏航通道视 线运动状态方程和偏航通道视线角测量方程构造偏航通道快收敛Kalman滤波器，从而求出 目标与导弹之间的视线偏航角速率，最终完成目标与导弹之间的视线角速率的估计。
[0046] 本发明提出的估计视线角速度的快收敛Kalman滤波器的收敛速度高于现有的 Kalman滤波器。本发明通过自适应调整滤波器中的状态噪声协方差阵和测量噪声协方差 阵，并恰当选用初值来提高收敛速度的估计视线角速率的快收敛Kalman滤波器。提高了极 坐标形式的目标一导弹相对运动运动学和动力学方程构造二维的Kalman滤波器的收敛速 度，得到了高精度的视线角速率估计。
【附图说明】
[0047] 图1是地屯、惯性坐标系和发射点惯性坐标系关系图；图2是发射点惯性坐标系和视 线坐标系关系图；图3是导弹弹体坐标系示意图；
[004引图4是快收敛Kalman滤波器视线俯仰角速率完整跟踪过程图（丫 =102, /^ > Γ );
[0049] 图5是快收敛Kalman滤波器视线俯仰角速率初段动态跟踪过程图（丫 =102, r>r );
[0050] 图6是快收敛Kalman滤波器视线俯仰角速率稳态跟踪过程图（丫 =102，r'>7·);
[0051 ]图7是快收敛Kalman滤波器视线俯仰角速率末段动态跟踪过程图（丫 = 102，r > r
[0052] 图8是快收敛Kalman滤波器视线偏航角速率完整跟踪过程图（丫 =102，/^ > r );
[0053] 图9是快收敛Kalman滤波器视线偏航角速率初段动态跟踪过程图（丫 = 10"，./· );
[0054]图10是快收敛Kalman滤波器视线偏航角速率稳态跟踪过程图（丫 = ΙΟ2，r >/·);
[00巧]图11是快收敛Kalman滤波器视线偏航角速率末段动态跟踪过程图（丫 = iO'，；>/' );
[0056] 图12是Kalman滤波器视线俯仰角速率完整跟踪过程图（/^ >;·);
[0057] 图13是Kalman滤波器视线俯仰角速率初段动态跟踪过程图（/">;'）；
[005引图14是Kalman滤波器视线俯仰角速率稳态跟踪过程图护>/·);
[0059] 图15是Kalman滤波器视线俯仰角速率末段动态跟踪过程图(/^ >〇 ;
[0060] 图16是Kalman滤波器视线偏航角速率完整跟踪过程图巧> Γ)
[0061] 图17是Kalman滤波器视线偏航角速率初段动态跟踪过程(/=> Γ);
[0062] 图18是Kalman滤波器视线偏航角速率稳态跟踪过程图).
[0063] 图19是Kalman滤波器视线偏航角速率末段动态跟踪过程图（/^ >;'）。
【具体实施方式】
[0064] 下面通过数学推导演示本发明Kalman滤波器的由来：
[0065] (一）目标一导弹相对运动动力学
[0066] 设目标一导弹视线矢量可W表示为
[0067] k -二 P, - P
[006引其中，Λ和衣分别代表目标和导弹在发射点惯性坐标系中的位置矢量。上式相对 时间微分，得到
[0069]
[0070] 其中，边《/从和忍分别代表视线矢量在惯性坐标系和视线坐标系中相对时间求 导，括代表视线坐标系相对惯性坐标系的旋转角速度，巧和尹分别代表目标和导弹速度矢 量。上式写成在视线坐标系中的投影形式：
[0074] 则
[0075]
[0084] 其中，ar、ae和ae是导弹加速度在视线坐标系；个轴上的分量，atr、ate和ate是目标 加速度在视线坐标系Ξ个轴上的分量。
[0085] 在末制导过程中，视线角速率为小量，因此，可W忽略式(7)中第二式中的二阶小 量每1，则俯仰视线变化动力学方程简化为
[0086]
(8)
[0087] 而忽略式(7)中第Ξ式中的二阶小量东如:，而且W末制导惯性参考坐标系为基准， qs为小角度，则偏航视线变化动力学方程简化为
[0088]
鹤
[0089] 俯仰通道视线运动状态方程为
[0090]
(10)
[0091] 令Δ t为采样周期，式(10)离散化后就得到式(3)，即
[009引则式(3)可W写作
[0096] Xe(k+l) = 〇e(k)Xe 化）+Be(k)Ue 化）
[0097] 考虑模型误差等原因，在上式中加入了一个零均值随机过程WE化），就得到 [009引 Xe(k+1) = Φε(10Χε化）+B e(k)Ue化）+We(k)
[0099] 俯仰通道视线角测量方程可w写作式(4)，即
[0100] 2,的）二扣的）十(/,、(失）二//,,.[如(/0扣的)]?-^,,.(弁）
[0101] 其中，Ηε = [1 0]，VE(k)为视线俯仰角的测量噪声，假设其均值为零， 左["''.(从< (叫二么。从)，尼 k'avj (叫二 ，
[0102] 同理，偏航通道视线运动状态方程为
[0103]
(11)
[0104] 式(11)离散化后得到
[0105]
(。）
[0106] 由式(12)，偏航通道视线运动离散化状态方程写作式(5)，即
[0107] 站化+1) = Φβ化)抽化)+ B β化)ι?β化)+WP化）
[010引 其中，
[0109]
[0110] we化)是一个零均值随机过程，
[0111] 偏航视线角测量方程写作式(6)，即
[0112]
[0113] 其中，He = [1 0]，ve(k)为视线俯仰角的测量噪声，假设其均值为零，
[0114] (二)快收敛Kalman滤波算法
[011引在末制导Kalman滤波算法设计中，设ζω和^(*)分别代表Χε化)和Xe化)的一步 预报估计，定义新息
根据新息自适 应地调节滤波算法中的状态噪声协方差阵Qe化)和化化），并定义状态一步预报估计协方差 阵分别为
[0118] 根据它们在线地调整滤波算法中的测量噪声协方差阵Re化)和Re化）。通过自适应 地调节滤波算法中的运两个矩阵，可W扩大滤波器稳定的吸引区域，令其快速收敛，即构成 快速收敛末制导Kalman滤波器。
[0119] 俯仰通道快收敛Kalman滤波器表达式为
[0123]在上述滤波算法中，λ是一个正的标量。若丫选择一个充分大的正值，可W满足在 最坏的初始条件下也具有较好的滤波收敛特性。但γ选取得过大会对滤波器的稳态特性有 一定影响，在应用中应该采取一种折中的选择。另外，为了确保稳态阶段，化更接近于Rk〇，A 不易选择过大。算法中的Ρε化+1)和Pe化+1)代表状态的滤波估计协方差阵，即
[0126] 实现上述滤波器需要知道相对距离和相对速度信息。记拦截器在发射点惯性坐标 系的位置为(x、y、z)，其速度在该坐标系中的投影为（文、莱、f)，运些信息由弹上导航系统 提供；目标在发射点惯性坐标系的位置为（xt、yt、zt)，其速度在该坐标系中的投影为 (i·,、和与），:运些信息由轨道预报系统提供。记Δχ = χ广X，Ay = y广y，Δζ = Ζ广Z，
则目标一导弹相对距离为
相 对速度为及二+ Δ>'Δ>' + Δ2心)/Κ。在末制导过程中，始终有R〉0,灰<0。实现上述滤 波器还需要知道导弹法向加速度ae和ae，W及目标法向加速度ate和ate。导弹上加速度计输 出的加速度《是沿着弹体坐标系投影的，把它投影到视线坐标系即可求得导弹法向加速度 ae和ae。而根据轨道预报数据可W用公式计算出目标加速度成在地屯、惯性坐标系中的Ξ个 分量，然后投影到视线坐标系求得atE和ate。
[0127] (Ξ)视线俯仰角和视线偏航角的提取算法
[0128] 导引头输出的只是目标一导弹视线相对导引头测量坐标系的视线角Aqs和Aqe。 导引头测量坐标系的圆点在导引头回转中屯、〇/，在不考虑导引头安装误差及导引头安装位 置晓性变形的情况下，其〇/χ6轴、c/ys轴和c/z6轴分别平行于弹体坐标系的0X1轴、oyi轴和 0Z1 轴。
[0129] 设导引头测量坐标系与弹体坐标系重合，则目标一导弹视线的单位向量在导引头 测量坐标系中的投影为
[0130]
(13)
[0131] 而它在发射点惯性坐标系中的投影为
[0135]其中，Cf4为由发射点惯性坐标系到弹体坐标系的转换矩阵，其定义为
[0136]
[0137] 其中，巧、Φ和丫分别为弹体的俯仰角，偏航角和滚动角，由惯导系统测算得到。
[0138] 根据式(14)，可W计算出视线俯仰角和视线偏航角，即
[0141] 滤波器初始估计中的视线俯仰角和视线偏航角的初值扣(0)和游供)取为根据导 引头第1拍测量值计算出来的视线俯仰角和视线偏航角，视线俯仰角速率和视线偏航角速 率的初值支(0巧的八0)利用几何方法计算出来，即
[0142]
[0143] 记Δ X、Δ y、Δ Ζ为末制导初始时刻目标一导弹相对位置在末制导惯性参考坐标系 ^个轴上的分量，始、AT、若为末制导初始时刻目标一导弹相对速度在末制导惯性参考 坐标系Ξ个轴上的分量。导弹在末制导惯性参考坐标系的位置为(x、y、z)，其速度在该坐标 系中的投影为(*、旁、i )，运些信息由弹上导航系统提供；目标在发射点惯性坐标系的 位置为(?、yt、zO，其速度在该坐标系中的投影为（-V义、ミ,），运些信息由地面上的目标测 量系统提供，则Ax = xt-X，Ay = y广y，Az = Z广Z，馬-丈，聲=义-J)，M =為-遣。
[0144] (四）下面通过数值仿真结果说明本实施方式的效果：
[0145] 设仿真初始时刻，目标的高度为Ht = 307.49km，飞行速度为4242m/s，导弹的高度 为254.12km，飞行速度3000m/s，二者相对距离112.196km，相对速度为-6680.7m/s。
[0146] 导弹的初始姿态角速率取为零，初始姿态大致指向初始视线方向，滚转角基本等 于零。
[0147] 仿真中，取惯导系统数据更新周期为5ms，导引头数据更新周期为15ms，轨控周期 15ms，姿控周期5ms。
[0148] 导引头测量数据Aq沛Aqe中包含〇.17mrad量化噪声。导引头误差角中分别包含 安装误差角0.2°和-0.2° (常值），同时又分别包含轨控发动机开关机所引起的导引头安装 位置晓性变形产生的误差。
[0149] 惯导系 统初始姿态误差用标准差为0.1°的高斯白噪声描述，初始速度误差用标准 差为Im/s的高斯白噪声描述，在发射点惯性坐标系X轴上的初始位置误差在(2km~5km)范 围内均匀分布，或在(-2km~-5km)范围内均匀分布;在发射点惯性坐标系y轴和Z轴上的初 始位置误差在±lkm内均匀分布。设目标轨道位置预报数据在地屯、惯性坐标系XI轴上的位 置误差用均值为1km，标准差为1km的高斯白噪声表述，yi轴和zi轴方向上的位置误差分别用 均值为1km，标准差为1km的高斯白噪声表述;Ξ个方向上的速度误差均取上述相应数据的 l/(1000s)。轨道预报数据每Is输出一次，而滤波器的采样周期为15ms，运样在每两个预报 数据之间用插值方法获得更多数据。
[0150] 在快收敛Kalman滤波器设计中，滤波器初始估计中的视线俯仰角和视线偏航角的 初值表.(〇巧日私(0)取为根据导引头第1拍测量值计算出来的视线俯仰角和视线偏航角，视线 俯仰角速率和视线偏航角速率的初值?^,(Ο)和Is巧)利用几何方法计算出来。
[0151] 动态噪声方差阵中的Qeo化)和Qeo化)分别取为
[0160] 中，参数丫是一个需要选取的重要参数，丫选取得大有利于扩大滤波器的稳定区 域，但可能影响稳态的滤波效果。折中考虑后选取丫 =1〇2,运是一个较大的参数，它可W保 证滤波器的稳定区域足够大。
[0161] 在测量噪声方差阵计算公式
[0162]
[0163] 中，选取λ = 3。
[0164] 在运种情况下，快收敛Kalman滤波器估计的视线俯仰角速率跟踪其真实值的全过 程、初段动态过程、稳态跟踪过程，W及末段跟踪过程分别如图4-7所示，而快收敛Kalman滤 波器估计的视线偏航角速率跟踪其真实值的全过程、初段动态过程、稳态跟踪过程，W及末 段跟踪过程如图8-11所示。从运些仿真结果图可W看出，滤波器在Is之后已经保证跟踪误 差小于0.005°/s。随后的稳态跟踪过程中，由于丫减小后，滤波器的稳态增益上升，消除了 静差。在末段跟踪阶段也仅在最后的0.1s内没有跟踪上真值，对制导精度影响不大。
[0165] 为了进行对比，我们在图12-15中给出了标准Kalman滤波器估计的视线俯仰角速 率跟踪其真实值的全过程、初段动态过程、稳态跟踪过程，W及末段跟踪过程，在图16-19中 给出了标准Kalman滤波器估计的视线偏航角速率跟踪其真实值的全过程、初段动态过程、 稳态跟踪过程，W及末段跟踪过程。从运些图中可W发现，标准Kalman滤波器稳态跟踪性能 好，但初段动态跟踪阶段进入稳态的时间稍长，大约为2.5s。其末段跟踪性能尚可，本次仿 真脱祀量为0.056m，轨控发动机开机时间5.929s。
【主权项】
1.估计弹目视线角速率的快收敛Kalman滤波器的设计方法，其特征在于:所述方法的 实现过程为： 步骤一、滤波器初始估计值的设定： 定义末制导惯性参考坐标系oQxoyozo、发射点惯性坐标系OFXFyFZF、地心惯性坐标系 OIXiyizi，以二者之间的关系： 设末制导惯性参考坐标系ooxoyozo在末制导过程中相对于惯性空间固化不动，末制导惯 性参考坐标系的原点为末制导初始时刻导弹的质心〇〇,其OOXO轴沿着末制导初始时刻视线 方向，指向目标为正，其ooyo轴位于包含OQXQ轴的铅垂面内，指向上为正，OQZQ轴由右手定则 确定；发射点惯性坐标系的原点为发射点〇F，OFyF轴铅锤向上，OFXF轴和OFZF轴位于水平面 内，其指向按需要选取，设OFXFyF为射击平面;地心惯性坐标系的原点OI位于地心上，ΟΙ?轴 位于赤道平面内，指向某一恒星，OIZI轴指向北极方向，Oiyi按右手定则确定;地心惯性坐标 系与发射点惯性坐标系之间的转换取决于外、和a F三个角度，q ε 〇和qM分别为末制导初始 时刻视线相对于发射点惯性坐标系所成的视线仰角和视线偏角； 滤波器初始估计中的视线俯仰角和视线偏航角的初值&(〇)和.後)取为根据导引头第 1拍测量值计算出来的视线俯仰角和视线偏航角，视线俯仰角速率和视线偏航角速率的初 值i. (〇).和1(〇)利用几何方法计算出来，即I 其中，么.(0)和<^(0)括号中的0表不0时刻； Ax、Ay、Az为末制导初始时刻目标一导弹相对位置在末制导惯性参考坐标系三个轴 上的分量，M、~、为末制导初始时刻目标一导弹相对速度在末制导惯性参考坐标系 三个轴上的分量，M、Ar、M的具体计算过程如下： 设导弹在末制导惯性参考坐标系的位置为(X、y、Z)，其速度在该坐标系中的投影为(文、 j、i>，上述信息由弹上导航系统提供；目标在末制导惯性参考坐标系的位置为（Xt、yt、 zt)，其速度在该坐标系中的投影为（i,)，上述信息由地面上的目标测量系统提供，步骤二、测算当前时刻目标与导弹之间的相对距离和相对速度，具体过程如下： 记当前时刻相对位置在末制导惯性参考坐标系中的分量为Ax = xt-x，Ay = yt_y，Az = zt-z，当前时刻相对速度在末制导惯性参考坐标系中的分量为Δτ= >r -X，Aj = -少，则目标一导弹相对距离为相对速度为在末制导过程中，始终有R>〇，及<0; 步骤三、测算导弹加速度在视线坐标系c/ X4y4Z4的c/ y4和c/ Z4轴方向上的分量ae和ap，以 及目标加速度在视线坐标系的c/ y4和c/ Z4轴方向上的分量ει?和Eltf!;导弹上加速度计输出的 加速度5是沿着弹体坐标系OXiyizi投影的，投影到视线坐标系即可求得导弹法向加速度as 和_; 导弹弹体坐标系oxiyizi的原点〇位于导弹质心上;?χι轴与弹体纵轴重合，指向头部为 正，oyi轴在弹体纵向对称平面内，垂直?χι轴，指向上方为正，Oz1轴方向按右手定则确定;视 线坐标系C/W4Z4原点c/位于导引头回转中心，C/X4轴与目标一导弹视线一致，由导引头回 转中心指向目标为正，c/ y4轴位于包含c/ X4轴的铅锤面内，与c/ X4轴垂直，指向上方为正，c/ Z4轴按右手定则确定；由末制导惯性坐标系转换到视线坐标系定义视线俯仰角qdP视线偏 航角qe;根据目标轨道测量数据可计算出目标加速度或在地心惯性坐标系中的三个分量， 然后投影到视线坐标系求得iitdPati!; 步骤四、由导弹的俯仰通道视线运动状态方程和俯仰通道视线角测量方程构造俯仰通 道快收敛Kalman滤波器，从而求出目标与导弹之间的视线俯仰角速率： 俯仰通道视线运动离散化状态方程为：其中，k代表第k个采样时刻，At为采样周期； 俯仰通道视线角测量方程为：其中，Ηε = [1 0]，VE(k)为视线俯仰角的测量噪声，假设其均值为零；则俯仰通道快收敛Kalman滤波器为：其中，幻和足.(幻分别代表Χε的滤波估计和预报估计，γ选择一个充分大的正值；λ是一个正的标量； 步骤五、根据导弹的偏航通道视线运动状态方程和偏航通道视线角测量方程可设计出 估计目标与导弹之间偏航通道视线角速率的快收敛Kalman滤波器： 偏航通道视线运动离散化状态方程为Wi!(k)是一个零均值随机过程，偏航视线角测量方程为其中，H e = [ I O ]，V e ( k )为视线俯仰角的测量噪声，假设其均值为零，由导弹的偏航通道视线运动状态方程和偏航通道视线角测量方程构造偏航通道快收 敛Kalman滤波器，从而求出目标与导弹之间的视线偏航角速率； 偏航通道Kalman滤波器为：其中，(幻分别代表Xe的滤波估计和预报估计，γ选择一个充分大的正值；λ是一个正的标量。2. 根据权利要求1所述的估计弹目视线角速率的快收敛Kalman滤波器的设计方法，其 特征在于:偏航通道快收敛Kalman滤波器与俯仰通道快收敛Kalman滤波器中的λ和γ的取 值规律相同。3. 根据权利要求1或2所述的估计弹目视线角速率的快收敛Kalman滤波器的设计方法， 其特征在于:在步骤四中，γ选取规则如下： 若γ选择一个充分大的正值，可满足在最坏的初始条件下也具有较好的滤波收敛特 性;若γ选取得过大会对滤波器的稳态特性有一定影响，在应用中应该采取一种折中的选 择，通常选择不大于300; 为了确保稳态阶段，Rk接近于Rko，λ不大于?ο。
【专利摘要】估计弹目视线角速率的快收敛Kalman滤波器的设计方法，属于导弹制导控制技术领域。本发明为了提高现有的目标—导弹视线角速度估计Kalman滤波方法的收敛速度。首先设置滤波器初始估计值；然后测算当前时刻目标与导弹之间的相对距离和相对速度；测算导弹加速度在视线坐标系o′y4和o′z4轴方向上的分量；最后由导弹俯仰通道视线运动状态方程和俯仰通道视线角测量方程构造俯仰通道快收敛Kalman滤波器，由导弹偏航通道视线运动状态方程和偏航通道视线角测量方程构造偏航通道快收敛Kalman滤波器，从而分别求出目标与导弹之间的视线俯仰角速率和视线偏航角速率。本发明提高Kalman滤波器的收敛速度，得到高精度的视线角速率估计。
【IPC分类】G01C21/16, G01C21/20
【公开号】CN105486308
【申请号】CN201510829848
【发明人】周荻, 邹昕光, 张中磊, 朱蕊蘋
【申请人】哈尔滨工业大学
【公开日】2016年4月13日
【申请日】2015年11月25日
【技术领域】
[0001] 本发明设及导弹的精确制导方法，属于导弹制导控制技术领域，具体设及大目 标一导弹相对运动信息精确估计方法。
【背景技术】
[0002] 近些年来，为了降低其成本，提高其可靠性，精确制导武器越来越多地采用小型捷 联或半捷联导引头，运种导引头的特点是可W测量目标一导弹之间的相对视线角，如果是 主动式导引头还可W测量距离信息，但捷联或半捷联导引头不能直接输出实际制导律需要 的视线角速率信息。
[0003] 针对安装捷联导引头的导弹运一应用背景，极坐标形式的目标一导弹相对运动运 动学和动力学方程构造二维的Kalman滤波器，可W得到高精度的视线角速率估计。为了进 一步提高运种滤波器的收敛速度，我们提出了一种通过自适应调整滤波器中的状态噪声协 方差阵和测量噪声协方差阵，并恰当选用初值来提高收敛速度的估计视线角速率的快收敛 Kalman滤波器。
【发明内容】
[0004] 本发明的目的是提供一种估计弹目视线角速率的快收敛Kalman滤波器的设计方 法，W提高现有的目标一导弹视线角速度估计Kalman滤波方法的收敛速度。所述弹目视线 角速率是指导弹到目标的视线和发射点惯性坐标的夹角的变化率。
[000引本发明为解决上述技术问题采取的技术方案是：
[0006] 一、滤波器初始估计中的视线俯仰角和视线偏航角的初值表(0巧日如(0)取为根据 导引头第1拍测量值计算出来的视线俯仰角和视线偏航角，视线俯仰角速率和视线偏航角 速率的初值是.(0)和^(0)利用几何方法计算出来，即
[0009] 其中，Αχ、Ay、A Z为末制导初始时刻目标一导弹相对位置在末制导惯性参考坐 标系Ξ个轴上的分量，Ai、Aj、： Δ?为末制导初始时刻目标一导弹相对速度在末制导惯性 参考坐标系Ξ个轴上的分量，它们的具体计算过程如下：设末制导惯性参考坐标系ooxoyozo 在末制导过程中相对于惯性空间固化不动，它与地屯、惯性坐标系W及发射点惯性坐标系的 关系如图1所不。
[0010] 末制导惯性参考坐标系的原点为末制导初始时刻导弹的质屯、00,00X0沿着末制导 初始时刻视线方向，指向目标为正，ooyo轴位于包含00X0轴的铅垂面内，指向上为正，O0Z0轴 由右手定则确定。发射点惯性坐标系的原点为发射点〇F，〇FyF轴铅键向上，OF祉轴和OFZF轴位 于水平面内，其指向按需要选取，通常OFXFyF为射击平面。地心肢性坐标系的原点ΟΙ位于地 屯、上，ΟΙΧΙ轴位于赤道平面内，指向某一恒星，ΟΙΖΙ轴指向北极方向，oiyi按右手定则确定。地 屯、惯性坐标系与发射点惯性坐标系之间的转换取决于稱、λ〇和αρΞ个角度，它们的定义见 图1。图1中，qso和qeo分别为末制导初始时刻视线相对于发射点惯性坐标系所成的视线仰角 和视线偏角。
[0011] 设导弹在末制导惯性参考坐标系的位置为(x、y、z)，其速度在该坐标系中的投影 为(X、来、3)，运些信息由弹上导航系统提供；目标在末制导惯性参考坐标系的位置为 (xt、yt、zO，其速度在该坐标系中的投影为（?、拓、為），运些信息由地面上的目标测量系统 提供，则A又二又广又，A y = y广y，A z = z广Z，化二文(-史'，Δ歹=义-，Δ? =与:-i；。
[0012] 运样，在很大程度上降低了滤波器的初始动态过程。
[0013] 二、测算当前时刻目标与导弹之间的相对距离和相对速度，具体过程如下:记当前 时刻相对位置在末制导惯性参考坐标系中的分量为Δ x = x广X，Ay = y广y，Δ z = Z广Z，当前 时刻相对速度在末制导惯性参考坐标系中的分量为馬-X，Aj = J，Δ?二S，则 目标一导弹相对距离为
，相对速度为度二（心Μ + Δ}'轉+心公)/巧。; 在末制导过程中，始终有R〉〇，人' < Cl;
[0014];、测算导弹加速度在视线坐标系c/ Χ4Υ4Ζ4(见图2)c/ Y4和c/ Z4轴方向上的分量ae 和ae，W及目标加速度在视线坐标系〇/y4和〇/Z4轴方向上的分量ate和ate;导弹上加速度计 输出的加速度α是沿着弹体坐标系oxmzi(见图1)投影的，把它投影到视线坐标系即可求 得导弹法向加速度3ε和ae;由图3所示，导弹弹体坐标系oxiyizi的原点0位于导弹质屯、上。0X1 轴与弹体纵轴重合，指向头部为正，oyi轴在弹体纵向对称平面内，垂直0X1轴，指向上方为 正，OZ1轴方向按右手定则确定；由图2,视线坐标系c/x4y4Z4原点oM立于导引头回转中屯、，〇/ X4轴与目标一导弹视线一致，由导引头回转中屯、指向目标为正，c/y4轴位于包含c/x4轴的铅 键面内，与C/X4轴垂直，指向上方为正，c/z4轴按右手定则确定。由末制导惯性坐标系转换到 视线坐标系定义了视线俯仰角qe和视线偏航角qe(见图2)。根据目标轨道测量数据可W计算 出目标加速度嗓在地屯、惯性坐标系中的Ξ个分量，然后投影到视线坐标系求得ate和曰证；
[0015] 四、由导弹的俯仰通道视线运动状态方程和俯仰通道视线角测量方程构造俯仰通 道快收敛Kalman滤波算法，从而求出目标与导弹之间的视线俯仰角速率，由导弹的偏航通 道视线运动状态方程和偏航通道视线角测量方程构造偏航通道快收敛Kalman滤波算法，从 而求出目标与导弹之间的视线偏航角速率。
[0016] 俯仰通道视线运动离散化状态方程为
[0017]
掛
[0018] 其中，k代表第k个采样时刻，At为采样周期；
[0019] 俯仰通道视线角测量方程可W写作
[0020]
(4)
[0021]其中，Ηε=[1 0]，νε化)为视线俯仰角的测量噪声，假设其均值为零。
[0024] 则俯仰通道快收敛Kalman滤波器为
[0025]
[0026] 其中，i,(X.)和丈，W分别代表Χε的滤波估计和预报估计，丫选择一个充分大的正 值
[0029] λ是一个正的标量。若丫选择一个充分大的正值，可W满足在最坏的初始条件下也 具有较好的滤波收敛特性。但γ选取得过大会对滤波器的稳态特性有一定影响，在应用中 应该采取一种折中的选择。另外，为了确保稳态阶段，Rk更接近于或等于化〇，λ不易选择过 大。
[0030] 根据导弹的偏航通道视线运动状态方程和偏航通道视线角测量方程可W设计出 估计目标与导弹之间偏航通道视线角速率的快收敛Kalman滤波器：
[0031] 偏航通道视线运动离散化状态方程写作
[0032] 站化+1) = Φβ化)抽化)+B β化)ι?β化)+WP化）（5)
[0033] 其中，
[0036] 偏航视线角测量方程写作
[0037]
巧）
[0038] 其中，He=[l 0]，ve(k)为视线俯仰角的测量噪声，假设其均值为零，
[0039] 偏航通道Kalman滤波器为
[0040]
[00川其中，^从)和马炒)分别代表站的滤波估计和预报估计，
[004引
，λ和丫（为两个参量，而 且在滤波过程中取为常量)的取值规律与俯仰通道快收敛Kalman滤波器相同。
[0044] 本发明的有益效果是：
[0045] 由导弹的俯仰通道视线运动状态方程和俯仰通道视线角测量方程构造俯仰通道 快收敛Kalman滤波器，从而求出目标与导弹之间的视线俯仰角速率，由导弹的偏航通道视 线运动状态方程和偏航通道视线角测量方程构造偏航通道快收敛Kalman滤波器，从而求出 目标与导弹之间的视线偏航角速率，最终完成目标与导弹之间的视线角速率的估计。
[0046] 本发明提出的估计视线角速度的快收敛Kalman滤波器的收敛速度高于现有的 Kalman滤波器。本发明通过自适应调整滤波器中的状态噪声协方差阵和测量噪声协方差 阵，并恰当选用初值来提高收敛速度的估计视线角速率的快收敛Kalman滤波器。提高了极 坐标形式的目标一导弹相对运动运动学和动力学方程构造二维的Kalman滤波器的收敛速 度，得到了高精度的视线角速率估计。
【附图说明】
[0047] 图1是地屯、惯性坐标系和发射点惯性坐标系关系图；图2是发射点惯性坐标系和视 线坐标系关系图；图3是导弹弹体坐标系示意图；
[004引图4是快收敛Kalman滤波器视线俯仰角速率完整跟踪过程图（丫 =102, /^ > Γ );
[0049] 图5是快收敛Kalman滤波器视线俯仰角速率初段动态跟踪过程图（丫 =102, r>r );
[0050] 图6是快收敛Kalman滤波器视线俯仰角速率稳态跟踪过程图（丫 =102，r'>7·);
[0051 ]图7是快收敛Kalman滤波器视线俯仰角速率末段动态跟踪过程图（丫 = 102，r > r
[0052] 图8是快收敛Kalman滤波器视线偏航角速率完整跟踪过程图（丫 =102，/^ > r );
[0053] 图9是快收敛Kalman滤波器视线偏航角速率初段动态跟踪过程图（丫 = 10"，./· );
[0054]图10是快收敛Kalman滤波器视线偏航角速率稳态跟踪过程图（丫 = ΙΟ2，r >/·);
[00巧]图11是快收敛Kalman滤波器视线偏航角速率末段动态跟踪过程图（丫 = iO'，；>/' );
[0056] 图12是Kalman滤波器视线俯仰角速率完整跟踪过程图（/^ >;·);
[0057] 图13是Kalman滤波器视线俯仰角速率初段动态跟踪过程图（/">;'）；
[005引图14是Kalman滤波器视线俯仰角速率稳态跟踪过程图护>/·);
[0059] 图15是Kalman滤波器视线俯仰角速率末段动态跟踪过程图(/^ >〇 ;
[0060] 图16是Kalman滤波器视线偏航角速率完整跟踪过程图巧> Γ)
[0061] 图17是Kalman滤波器视线偏航角速率初段动态跟踪过程(/=> Γ);
[0062] 图18是Kalman滤波器视线偏航角速率稳态跟踪过程图).
[0063] 图19是Kalman滤波器视线偏航角速率末段动态跟踪过程图（/^ >;'）。
【具体实施方式】
[0064] 下面通过数学推导演示本发明Kalman滤波器的由来：
[0065] (一）目标一导弹相对运动动力学
[0066] 设目标一导弹视线矢量可W表示为
[0067] k -二 P, - P
[006引其中，Λ和衣分别代表目标和导弹在发射点惯性坐标系中的位置矢量。上式相对 时间微分，得到
[0069]
[0070] 其中，边《/从和忍分别代表视线矢量在惯性坐标系和视线坐标系中相对时间求 导，括代表视线坐标系相对惯性坐标系的旋转角速度，巧和尹分别代表目标和导弹速度矢 量。上式写成在视线坐标系中的投影形式：
[0074] 则
[0075]
[0084] 其中，ar、ae和ae是导弹加速度在视线坐标系；个轴上的分量，atr、ate和ate是目标 加速度在视线坐标系Ξ个轴上的分量。
[0085] 在末制导过程中，视线角速率为小量，因此，可W忽略式(7)中第二式中的二阶小 量每1，则俯仰视线变化动力学方程简化为
[0086]
(8)
[0087] 而忽略式(7)中第Ξ式中的二阶小量东如:，而且W末制导惯性参考坐标系为基准， qs为小角度，则偏航视线变化动力学方程简化为
[0088]
鹤
[0089] 俯仰通道视线运动状态方程为
[0090]
(10)
[0091] 令Δ t为采样周期，式(10)离散化后就得到式(3)，即
[009引则式(3)可W写作
[0096] Xe(k+l) = 〇e(k)Xe 化）+Be(k)Ue 化）
[0097] 考虑模型误差等原因，在上式中加入了一个零均值随机过程WE化），就得到 [009引 Xe(k+1) = Φε(10Χε化）+B e(k)Ue化）+We(k)
[0099] 俯仰通道视线角测量方程可w写作式(4)，即
[0100] 2,的）二扣的）十(/,、(失）二//,,.[如(/0扣的)]?-^,,.(弁）
[0101] 其中，Ηε = [1 0]，VE(k)为视线俯仰角的测量噪声，假设其均值为零， 左["''.(从< (叫二么。从)，尼 k'avj (叫二 ，
[0102] 同理，偏航通道视线运动状态方程为
[0103]
(11)
[0104] 式(11)离散化后得到
[0105]
(。）
[0106] 由式(12)，偏航通道视线运动离散化状态方程写作式(5)，即
[0107] 站化+1) = Φβ化)抽化)+ B β化)ι?β化)+WP化）
[010引 其中，
[0109]
[0110] we化)是一个零均值随机过程，
[0111] 偏航视线角测量方程写作式(6)，即
[0112]
[0113] 其中，He = [1 0]，ve(k)为视线俯仰角的测量噪声，假设其均值为零，
[0114] (二)快收敛Kalman滤波算法
[011引在末制导Kalman滤波算法设计中，设ζω和^(*)分别代表Χε化)和Xe化)的一步 预报估计，定义新息
根据新息自适 应地调节滤波算法中的状态噪声协方差阵Qe化)和化化），并定义状态一步预报估计协方差 阵分别为
[0118] 根据它们在线地调整滤波算法中的测量噪声协方差阵Re化)和Re化）。通过自适应 地调节滤波算法中的运两个矩阵，可W扩大滤波器稳定的吸引区域，令其快速收敛，即构成 快速收敛末制导Kalman滤波器。
[0119] 俯仰通道快收敛Kalman滤波器表达式为
[0123]在上述滤波算法中，λ是一个正的标量。若丫选择一个充分大的正值，可W满足在 最坏的初始条件下也具有较好的滤波收敛特性。但γ选取得过大会对滤波器的稳态特性有 一定影响，在应用中应该采取一种折中的选择。另外，为了确保稳态阶段，化更接近于Rk〇，A 不易选择过大。算法中的Ρε化+1)和Pe化+1)代表状态的滤波估计协方差阵，即
[0126] 实现上述滤波器需要知道相对距离和相对速度信息。记拦截器在发射点惯性坐标 系的位置为(x、y、z)，其速度在该坐标系中的投影为（文、莱、f)，运些信息由弹上导航系统 提供；目标在发射点惯性坐标系的位置为（xt、yt、zt)，其速度在该坐标系中的投影为 (i·,、和与），:运些信息由轨道预报系统提供。记Δχ = χ广X，Ay = y广y，Δζ = Ζ广Z，
则目标一导弹相对距离为
相 对速度为及二+ Δ>'Δ>' + Δ2心)/Κ。在末制导过程中，始终有R〉0,灰<0。实现上述滤 波器还需要知道导弹法向加速度ae和ae，W及目标法向加速度ate和ate。导弹上加速度计输 出的加速度《是沿着弹体坐标系投影的，把它投影到视线坐标系即可求得导弹法向加速度 ae和ae。而根据轨道预报数据可W用公式计算出目标加速度成在地屯、惯性坐标系中的Ξ个 分量，然后投影到视线坐标系求得atE和ate。
[0127] (Ξ)视线俯仰角和视线偏航角的提取算法
[0128] 导引头输出的只是目标一导弹视线相对导引头测量坐标系的视线角Aqs和Aqe。 导引头测量坐标系的圆点在导引头回转中屯、〇/，在不考虑导引头安装误差及导引头安装位 置晓性变形的情况下，其〇/χ6轴、c/ys轴和c/z6轴分别平行于弹体坐标系的0X1轴、oyi轴和 0Z1 轴。
[0129] 设导引头测量坐标系与弹体坐标系重合，则目标一导弹视线的单位向量在导引头 测量坐标系中的投影为
[0130]
(13)
[0131] 而它在发射点惯性坐标系中的投影为
[0135]其中，Cf4为由发射点惯性坐标系到弹体坐标系的转换矩阵，其定义为
[0136]
[0137] 其中，巧、Φ和丫分别为弹体的俯仰角，偏航角和滚动角，由惯导系统测算得到。
[0138] 根据式(14)，可W计算出视线俯仰角和视线偏航角，即
[0141] 滤波器初始估计中的视线俯仰角和视线偏航角的初值扣(0)和游供)取为根据导 引头第1拍测量值计算出来的视线俯仰角和视线偏航角，视线俯仰角速率和视线偏航角速 率的初值支(0巧的八0)利用几何方法计算出来，即
[0142]
[0143] 记Δ X、Δ y、Δ Ζ为末制导初始时刻目标一导弹相对位置在末制导惯性参考坐标系 ^个轴上的分量，始、AT、若为末制导初始时刻目标一导弹相对速度在末制导惯性参考 坐标系Ξ个轴上的分量。导弹在末制导惯性参考坐标系的位置为(x、y、z)，其速度在该坐标 系中的投影为(*、旁、i )，运些信息由弹上导航系统提供；目标在发射点惯性坐标系的 位置为(?、yt、zO，其速度在该坐标系中的投影为（-V义、ミ,），运些信息由地面上的目标测 量系统提供，则Ax = xt-X，Ay = y广y，Az = Z广Z，馬-丈，聲=义-J)，M =為-遣。
[0144] (四）下面通过数值仿真结果说明本实施方式的效果：
[0145] 设仿真初始时刻，目标的高度为Ht = 307.49km，飞行速度为4242m/s，导弹的高度 为254.12km，飞行速度3000m/s，二者相对距离112.196km，相对速度为-6680.7m/s。
[0146] 导弹的初始姿态角速率取为零，初始姿态大致指向初始视线方向，滚转角基本等 于零。
[0147] 仿真中，取惯导系统数据更新周期为5ms，导引头数据更新周期为15ms，轨控周期 15ms，姿控周期5ms。
[0148] 导引头测量数据Aq沛Aqe中包含〇.17mrad量化噪声。导引头误差角中分别包含 安装误差角0.2°和-0.2° (常值），同时又分别包含轨控发动机开关机所引起的导引头安装 位置晓性变形产生的误差。
[0149] 惯导系 统初始姿态误差用标准差为0.1°的高斯白噪声描述，初始速度误差用标准 差为Im/s的高斯白噪声描述，在发射点惯性坐标系X轴上的初始位置误差在(2km~5km)范 围内均匀分布，或在(-2km~-5km)范围内均匀分布;在发射点惯性坐标系y轴和Z轴上的初 始位置误差在±lkm内均匀分布。设目标轨道位置预报数据在地屯、惯性坐标系XI轴上的位 置误差用均值为1km，标准差为1km的高斯白噪声表述，yi轴和zi轴方向上的位置误差分别用 均值为1km，标准差为1km的高斯白噪声表述;Ξ个方向上的速度误差均取上述相应数据的 l/(1000s)。轨道预报数据每Is输出一次，而滤波器的采样周期为15ms，运样在每两个预报 数据之间用插值方法获得更多数据。
[0150] 在快收敛Kalman滤波器设计中，滤波器初始估计中的视线俯仰角和视线偏航角的 初值表.(〇巧日私(0)取为根据导引头第1拍测量值计算出来的视线俯仰角和视线偏航角，视线 俯仰角速率和视线偏航角速率的初值?^,(Ο)和Is巧)利用几何方法计算出来。
[0151] 动态噪声方差阵中的Qeo化)和Qeo化)分别取为
[0160] 中，参数丫是一个需要选取的重要参数，丫选取得大有利于扩大滤波器的稳定区 域，但可能影响稳态的滤波效果。折中考虑后选取丫 =1〇2,运是一个较大的参数，它可W保 证滤波器的稳定区域足够大。
[0161] 在测量噪声方差阵计算公式
[0162]
[0163] 中，选取λ = 3。
[0164] 在运种情况下，快收敛Kalman滤波器估计的视线俯仰角速率跟踪其真实值的全过 程、初段动态过程、稳态跟踪过程，W及末段跟踪过程分别如图4-7所示，而快收敛Kalman滤 波器估计的视线偏航角速率跟踪其真实值的全过程、初段动态过程、稳态跟踪过程，W及末 段跟踪过程如图8-11所示。从运些仿真结果图可W看出，滤波器在Is之后已经保证跟踪误 差小于0.005°/s。随后的稳态跟踪过程中，由于丫减小后，滤波器的稳态增益上升，消除了 静差。在末段跟踪阶段也仅在最后的0.1s内没有跟踪上真值，对制导精度影响不大。
[0165] 为了进行对比，我们在图12-15中给出了标准Kalman滤波器估计的视线俯仰角速 率跟踪其真实值的全过程、初段动态过程、稳态跟踪过程，W及末段跟踪过程，在图16-19中 给出了标准Kalman滤波器估计的视线偏航角速率跟踪其真实值的全过程、初段动态过程、 稳态跟踪过程，W及末段跟踪过程。从运些图中可W发现，标准Kalman滤波器稳态跟踪性能 好，但初段动态跟踪阶段进入稳态的时间稍长，大约为2.5s。其末段跟踪性能尚可，本次仿 真脱祀量为0.056m，轨控发动机开机时间5.929s。
【主权项】
1.估计弹目视线角速率的快收敛Kalman滤波器的设计方法，其特征在于:所述方法的 实现过程为： 步骤一、滤波器初始估计值的设定： 定义末制导惯性参考坐标系oQxoyozo、发射点惯性坐标系OFXFyFZF、地心惯性坐标系 OIXiyizi，以二者之间的关系： 设末制导惯性参考坐标系ooxoyozo在末制导过程中相对于惯性空间固化不动，末制导惯 性参考坐标系的原点为末制导初始时刻导弹的质心〇〇,其OOXO轴沿着末制导初始时刻视线 方向，指向目标为正，其ooyo轴位于包含OQXQ轴的铅垂面内，指向上为正，OQZQ轴由右手定则 确定；发射点惯性坐标系的原点为发射点〇F，OFyF轴铅锤向上，OFXF轴和OFZF轴位于水平面 内，其指向按需要选取，设OFXFyF为射击平面;地心惯性坐标系的原点OI位于地心上，ΟΙ?轴 位于赤道平面内，指向某一恒星，OIZI轴指向北极方向，Oiyi按右手定则确定;地心惯性坐标 系与发射点惯性坐标系之间的转换取决于外、和a F三个角度，q ε 〇和qM分别为末制导初始 时刻视线相对于发射点惯性坐标系所成的视线仰角和视线偏角； 滤波器初始估计中的视线俯仰角和视线偏航角的初值&(〇)和.後)取为根据导引头第 1拍测量值计算出来的视线俯仰角和视线偏航角，视线俯仰角速率和视线偏航角速率的初 值i. (〇).和1(〇)利用几何方法计算出来，即I 其中，么.(0)和<^(0)括号中的0表不0时刻； Ax、Ay、Az为末制导初始时刻目标一导弹相对位置在末制导惯性参考坐标系三个轴 上的分量，M、~、为末制导初始时刻目标一导弹相对速度在末制导惯性参考坐标系 三个轴上的分量，M、Ar、M的具体计算过程如下： 设导弹在末制导惯性参考坐标系的位置为(X、y、Z)，其速度在该坐标系中的投影为(文、 j、i>，上述信息由弹上导航系统提供；目标在末制导惯性参考坐标系的位置为（Xt、yt、 zt)，其速度在该坐标系中的投影为（i,)，上述信息由地面上的目标测量系统提供，步骤二、测算当前时刻目标与导弹之间的相对距离和相对速度，具体过程如下： 记当前时刻相对位置在末制导惯性参考坐标系中的分量为Ax = xt-x，Ay = yt_y，Az = zt-z，当前时刻相对速度在末制导惯性参考坐标系中的分量为Δτ= >r -X，Aj = -少，则目标一导弹相对距离为相对速度为在末制导过程中，始终有R>〇，及<0; 步骤三、测算导弹加速度在视线坐标系c/ X4y4Z4的c/ y4和c/ Z4轴方向上的分量ae和ap，以 及目标加速度在视线坐标系的c/ y4和c/ Z4轴方向上的分量ει?和Eltf!;导弹上加速度计输出的 加速度5是沿着弹体坐标系OXiyizi投影的，投影到视线坐标系即可求得导弹法向加速度as 和_; 导弹弹体坐标系oxiyizi的原点〇位于导弹质心上;?χι轴与弹体纵轴重合，指向头部为 正，oyi轴在弹体纵向对称平面内，垂直?χι轴，指向上方为正，Oz1轴方向按右手定则确定;视 线坐标系C/W4Z4原点c/位于导引头回转中心，C/X4轴与目标一导弹视线一致，由导引头回 转中心指向目标为正，c/ y4轴位于包含c/ X4轴的铅锤面内，与c/ X4轴垂直，指向上方为正，c/ Z4轴按右手定则确定；由末制导惯性坐标系转换到视线坐标系定义视线俯仰角qdP视线偏 航角qe;根据目标轨道测量数据可计算出目标加速度或在地心惯性坐标系中的三个分量， 然后投影到视线坐标系求得iitdPati!; 步骤四、由导弹的俯仰通道视线运动状态方程和俯仰通道视线角测量方程构造俯仰通 道快收敛Kalman滤波器，从而求出目标与导弹之间的视线俯仰角速率： 俯仰通道视线运动离散化状态方程为：其中，k代表第k个采样时刻，At为采样周期； 俯仰通道视线角测量方程为：其中，Ηε = [1 0]，VE(k)为视线俯仰角的测量噪声，假设其均值为零；则俯仰通道快收敛Kalman滤波器为：其中，幻和足.(幻分别代表Χε的滤波估计和预报估计，γ选择一个充分大的正值；λ是一个正的标量； 步骤五、根据导弹的偏航通道视线运动状态方程和偏航通道视线角测量方程可设计出 估计目标与导弹之间偏航通道视线角速率的快收敛Kalman滤波器： 偏航通道视线运动离散化状态方程为Wi!(k)是一个零均值随机过程，偏航视线角测量方程为其中，H e = [ I O ]，V e ( k )为视线俯仰角的测量噪声，假设其均值为零，由导弹的偏航通道视线运动状态方程和偏航通道视线角测量方程构造偏航通道快收 敛Kalman滤波器，从而求出目标与导弹之间的视线偏航角速率； 偏航通道Kalman滤波器为：其中，(幻分别代表Xe的滤波估计和预报估计，γ选择一个充分大的正值；λ是一个正的标量。2. 根据权利要求1所述的估计弹目视线角速率的快收敛Kalman滤波器的设计方法，其 特征在于:偏航通道快收敛Kalman滤波器与俯仰通道快收敛Kalman滤波器中的λ和γ的取 值规律相同。3. 根据权利要求1或2所述的估计弹目视线角速率的快收敛Kalman滤波器的设计方法， 其特征在于:在步骤四中，γ选取规则如下： 若γ选择一个充分大的正值，可满足在最坏的初始条件下也具有较好的滤波收敛特 性;若γ选取得过大会对滤波器的稳态特性有一定影响，在应用中应该采取一种折中的选 择，通常选择不大于300; 为了确保稳态阶段，Rk接近于Rko，λ不大于?ο。
【专利摘要】估计弹目视线角速率的快收敛Kalman滤波器的设计方法，属于导弹制导控制技术领域。本发明为了提高现有的目标—导弹视线角速度估计Kalman滤波方法的收敛速度。首先设置滤波器初始估计值；然后测算当前时刻目标与导弹之间的相对距离和相对速度；测算导弹加速度在视线坐标系o′y4和o′z4轴方向上的分量；最后由导弹俯仰通道视线运动状态方程和俯仰通道视线角测量方程构造俯仰通道快收敛Kalman滤波器，由导弹偏航通道视线运动状态方程和偏航通道视线角测量方程构造偏航通道快收敛Kalman滤波器，从而分别求出目标与导弹之间的视线俯仰角速率和视线偏航角速率。本发明提高Kalman滤波器的收敛速度，得到高精度的视线角速率估计。
【IPC分类】G01C21/16, G01C21/20
【公开号】CN105486308
【申请号】CN201510829848
【发明人】周荻, 邹昕光, 张中磊, 朱蕊蘋
【申请人】哈尔滨工业大学
【公开日】2016年4月13日
【申请日】2015年11月25日
最新回复(0)