针对机动目标的视线角速率估计方法
针对机动目标的视线角速率估计方法
【技术领域】
[0001 ]本发明设及一种视线角速率估计方法。
【背景技术】
[0002] 在制导技术领域，目标的持续机动在很大程度上增加了视线角速率高精度提取的 难度，现有的视线角速率提取方法不能满足拦截导弹或拦截器直接碰撞目标的精确制导要 求，必须研究与目标机动条件相适应的新的高精度视线角速率提取方法。
[0003] 近些年来，为了降低其成本，提高其可靠性，精确制导武器越来越多地采用小型捷 联或半捷联导引头，运种导引头的特点是可W测量目标一导弹之间的相对视线角，如果是 主动式导引头还可W距离信息，但捷联或半捷联导引头不能直接输出实际制导律需要的视 线角速率信息。
[0004] 现有的视线角速率估计方法均是针对非机动目标的，在对付机动目标时视线角速 率提取精度有限。所W针对安装捷联或半捷联导引头的导弹拦截机动目标运一应用背景， 目前还没有一种行之有效的视线角速率高精度提取方法。
【发明内容】
[0005] 本发明为了解决现有的针对非机动目标的视线角速率估计方法提取机动目标视 线角速率的精度有限的问题，进而提出了一种专口针对捷联或半捷联导引头和机动目标的 视线角速率高精度Kalman滤波方法。
[0006] 针对机动目标的视线角速率估计方法，包括下述步骤：
[0007] 步骤一、测算目标一导弹相对距离R和目标一导弹相对速度R;
[000引步骤二、测算导弹加速度在视线坐标系0' Χ4Υ4Ζ4的0' Y4和0' Z4轴方向上的分量3ε和 日e;
[0009] 步骤Ξ、估计目标加速度在视线坐标系0' Χ4Υ4Ζ4的0' Y4和0' Z4轴方向上的分量ate 和 ate;
[0010] 步骤四、计算出视线俯仰角qs和视线偏航角qe;
[0011] 步骤五、将目标一导弹相对距离R和目标一导弹相对速度R，导弹加速度分量ae和 ae，目标加速度估计结果ate和ateW及视线俯仰角和视线偏航角计算结果qe和qe分别代入到 导弹的俯仰通道视线角速率Kalman滤波器和偏航通道视线角速率Kalman滤波器中，从而精 确求出目标与导弹之间的视线俯仰角速率和视线偏航角速率。
[0012] 本发明具有W下有益效果：
[0013] 本发明将目标加速度估计Kalman滤波器与视线角速率估计Kalman滤波器相结合， 得到了针对机动目标的捷联或半捷联导引头视线角速率估计Kalman滤波器，其精度高于现 有的针对非机动目标的捷联或半捷联导引头视线角速率估计Kalman滤波器，精度提高了 0.03Vs。
【附图说明】
[0014] 图1为地屯、惯性坐标系、发射点惯性坐标系和末制导初始视线坐标系关系示意图；
[0015] 图2为末制导初始视线坐标系和视线坐标系关系示意图；
[0016] 图3为导弹的弹体坐标系示意图；
[0017] 图4为导弹拦截目标的纵向飞行弹道仿真效果图；
[0018] 图5为导弹拦截目标的侧向飞行弹道仿真效果图；、
[0019] 图6为目标一导弹视线俯仰角速率仿真效果图；
[0020] 图7为目标一导弹视线偏航角速率仿真效果图；
[0021 ]图8发射点惯性坐标系XF轴目标加速度及其估计仿真效果图；
[0022] 图9发射点惯性坐标系yF轴目标加速度及其估计仿真效果图；
[0023] 图10发射点惯性坐标系ZF轴目标加速度及其估计仿真效果图。
【具体实施方式】
【具体实施方式】 [0024] 一：
[0025] 针对机动目标的视线角速率估计方法，包括下述步骤：
[0026] 步骤一、测算目标一导弹相对距离R和目标一导弹相对速度R;
[0027] 步骤二、测算导弹加速度在视线坐标系c/ Χ4Υ4Ζ4的c/ Y4和c/ Z4轴方向上的分量ae和 日e;
[00%] 步骤Ξ、估计目标加速度在视线坐标系c/ Χ4Υ4Ζ4的c/ y4和c/ Z4轴方向上的分量ate 和 ate;
[0029] 步骤四、计算出视线俯仰角qs和视线偏航角qe;
[0030] 步骤五、将目标一导弹相对距离R和目标一导弹相对速度R，导弹加速度分量ae和 ae，目标加速度估计结果ate和ateW及视线俯仰角和视线偏航角计算结果qe和qe分别代入到 导弹的俯仰通道视线角速率Kalman滤波器和偏航通道视线角速率Kalman滤波器中，从而精 确求出目标与导弹之间的视线俯仰角速率和视线偏航角速率。
[0031 ]【具体实施方式】二:结合图1说明本实施方式，
[0032] 本实施方式步骤一所述测算目标一导弹相对距离R和目标一导弹相对速度R的具 体步骤如下；
[0033] 定义地屯、惯性坐标系：地屯、惯性坐标系oixiyizi的原点01位于地屯、上，OUI轴位于 赤道平面内，指向某一恒星，OIZI轴指向北极方向，oiyi按右手定则确定；
[0034] 定义发射点惯性坐标系：设发射点惯性坐标系OFXFyFZF在导弹发射瞬间相对于地 屯、惯性坐标系固化不动，它与地屯、惯性坐标系的关系如图1所示，发射点惯性坐标系的原点 为发射点〇F，〇FyF轴铅键向上，OF祉轴和OFZF轴位于水平面内，其指向按需要选取，通常OFXFyF 为射击平面；
[0035] 见图1，地屯、惯性坐标系与发射点惯性坐标系之间的转换取绝于狗)、λ〇和αρΞ个角 度；妍、λ〇和αρ分别是发射点相对于地屯、惯性坐标系的缔度、经度和发射方位角；
[00%]定义末制导初始视线坐标系：图1中ooxoyozo为末制导初始视线坐标系，〇日位于末制 导初始时刻导弹导引头回转中屯、上;O0X日为初始视线方向，指向目标为正;ooyo位于包含00X0 的铅垂面内，垂直于00X0,指向上方为正；oozo按照右手定则确定；在整个末制导过程中， ooxoy日Z日坐标系相对于发射点惯性坐标系0阳FyFZ個化不动;qe日和q邮分别为视线相对于发射 点惯性坐标系所成的初始视线俯仰角和视线偏航角；
[0037] 导弹在发射点惯性坐标系的位置为(x、y、z)，其速度在该坐标系中的投影为(Vx、 Vy、Vz )，运些信息由弹上导航系统提供；
[0038] 目标在发射点惯性坐标系的位置为（xt、yt、zt)，其速度在该坐标系中的投影为 (Vtx、Vty、Vtz)，运些信息由弹上的目标跟踪滤波器提供；
[0039] 根据 Ax = x-xt，Ay = y-yt，Az = z-zt，AVx=Vx-Vtx，AVy = Vy-Vty，AVz = Vz-Vtz， 计算目标一导弹相对距离
和目标一导弹相对速度
在末制导过程中，始终有R〉〇，度< 0。
[0040] 其他步骤和参数与【具体实施方式】一相同。
【具体实施方式】 [0041] Ξ:
[0042] 本实施方式步骤二所述测算导弹加速度在视线坐标系〇/ X4y4Z4的〇/ y4和〇/ Z4轴方 向上的分量ae和ae的具体步骤如下：
[00创定义视线坐标系:视线坐标系c/x4y4Z4原点oM立于导引头回转中屯、;C/X4轴与当前 时刻目标一导弹视线一致，由导引头回转中屯、指向目标为正;c/y4轴位于包含C/X4轴的铅键 面内，与C/X4轴垂直，指向上方为正;C/Z4轴按右手定则确定;如图2所示，由末制导初始视线 坐标系转换到视线坐标系定义视线俯仰角qs和视线偏航角qe;
[0044] 定义弹体坐标系；由图3所示，导弹弹体坐标系oxiyizi的原点0位于导弹质屯、上； 0X1轴与弹体纵轴重合，指向头部为正;oyi轴在弹体纵向对称平面内，垂直0X1轴，指向上方 为正;OZ1轴方向按右手定则确定；
[0045] 导弹上加速度计输出的加速度召是沿着弹体坐标系oxiyizi投影的，将导弹上加速 度计输出的加速度α投影到视线坐标系〇/x4y4Z4的〇/y4和〇/Z4轴方向上得到导弹视线坐标 系0' Χ4Υ4Ζ；4的0' Y4和0' Z4轴方向上的加速度ae和ae。
[0046] 其他步骤和参数与【具体实施方式】一或二相同。
【具体实施方式】 [0047] 四：
[004引本实施方式步骤Ξ所述估计目标加速度在视线坐标系0/ X4y4Z4的0/ y4和0/ Z4轴方 向上的分量ate和ate的具体步骤如下：
[0049]步骤3.1、在发射点惯性坐标系的Ξ个轴上，均用Singer模型来分别描述目标加速 度分量atx、aty和atz:
[0053]其中，λχ、λγ和λζ分别代表发射点惯性坐标系Ξ个轴方向上目标机动时间常数的倒 数;Wx、Wy和Wz分别代表Ξ个轴上的零均值高斯白噪声；％、如和'也表示atx、aty和atz的导 数，参数上带·均表示参数的导数；目标加速度分量atx、a巧和atz既包含气动力产生的加速 度，又包含重力产生的加速度；
[0化4] 建立起如下Ξ组运动方程
[0058] 步骤3.2、针对于方程（1)，定义目标加速度分量atx在发射点惯性坐标系的OFXF轴 的状态变量Xx=[xt Vtx atx]T，则得到如下状态方程
[0059] 去*=巧馬 (4)
[0060] 其中，
[0061 ]
(5)
[0062] 对公式(5) W采样周期At(该周期与制导周期一致)进行离散化后，得到
[0063]
(..6)
[0064] 则离散化后的状态方程为
[00化]Xx(k+1) =巫xXx 化） （7)
[0066] 其中，k表示第k个采样时刻；
[0067] 目标的位置由地面雷达测量装置测量得到，则测量方程为
[0068] IU化）=xt化） （8)该测量信息数据更新率较低，例如0.2s更新一次；
[0069] 根据如公式（1)的模型，设计如下发射点惯性坐标系XF轴的目标跟踪Kalman滤波 器，其预报方程为
[0070]
(9)
[0071] 其中，义、.脚和玄,作)分别代表Χχ的滤波估计和预报估计;Qx为模型预报误差协方 差矩阵；
[0072]
[0073]
[0074] 式中E{ · }代表数学期望；
[0075] 滤波器的测量修正方程为
[0076]
(10)
[0077] 其中，Κχ化+1)是该滤波器在k+1时刻的滤波增益;rix化+1)是k+1时刻的测量信息； 13为3 X 3维单位矩阵;Rx为测量误差协方差矩阵;测量矩阵为
[007引出=[1 0 0] (11)
[0079] 当没有测量信息时，只运行预报方程(9)，有测量信息时，则同时运行预报方程(9) 和测量修正方程(10)，得到发射点惯性坐标系的OFXF轴的目标加速度atx的估计值；
[0080] 步骤3.3、根据步骤3.2，设计发射点惯性坐标系yF轴和ZF轴的目标跟踪Kalman滤波 器并得到发射点惯性坐标系的yF轴和ZF轴的目标加速度aty和atz的估计值；
[0081] 步骤3.4、把目标加速度atx、a巧和atz的估计值从发射点惯性坐标系投影到视线坐 标系后，即得到目标加速度在视线坐标系的〇/ y4和〇/ Z4轴方向上的分量ate和ate。
[0082] 其他步骤和参数与【具体实施方式】一至Ξ之一相同。
【具体实施方式】 [0083] 五：
[0084] 本实施方式步骤四所述计算出视线俯仰角qs和视线偏航角qe的具体步骤如下：
[0085] 导引头测量坐标系的圆点在导引头回转中屯、〇/，在不考虑导引头安装误差及导引 头安装位置晓性变形的情况下，其Ξ个轴分别平行于弹体坐标系的0X1轴、oyi轴和OZ1轴；导 引头输出目标一导弹视线相对导引头测量坐标系的视线俯仰角和视线偏航角Aqs和Aqe;
[0086] 设导引头测量坐标系与弹体坐标系重合，则目标一导弹视线的单位向量在导引头 测量坐标系中的投影为
[0087]
(12)
[0088] 其中，cix、ciy、ciz分别为目标一导弹视线的单位向量在导引头测量坐标系Ξ个轴 上的投影；
[0089] 而目标一导弹视线的单位向量在发射点惯性坐标系中的投影为
[0090]
(13)
[0091] 其中，c〇x、c〇y、c〇z分别为目标一导弹视线的单位向量在发射点惯性坐标系Ξ个轴 上的投影；
[0092] 则
[0093]
(14)
[0094] 式中，Cf4为由发射点惯性坐标系到弹体坐标系的转换矩阵，
[0095]
[0096] 其中，沒、Φ和丫分别为弹体的俯仰角，偏航角和滚动角，由惯性导航系统测算得 到；
[0097] 根据式(13)计算出视线俯仰角和视线偏航角，即
[0100] 其他步骤和参数与【具体实施方式】一至四之一相同。
【具体实施方式】 [0101] 六：
[0102] 本实施方式步骤五所述求出目标与导弹之间的视线俯仰角速率和视线偏航角速 率的具体步骤如下：
[0103] 步骤5.1、俯仰通道视线运动状态方程为
[0104]
(1:8)
[0105] 对公式（18)进行离散化，得到
[0106]
(19)
[0107] 其中，k代表第k个采样时刻，At为采样周期；
[0108] 俯仰通道视线角测量方程为
[0109]
(锁）
[0110] 其中，Ηε = [1 0]，νε化)为视线俯仰角的测量噪声，假设其均值为零；
[0113] 则俯仰通道Kalman滤波器为
[0114]
(21 >
[0"引其中，疋.(0和是(A-)分别代表Χε的滤波估计和预报估计，KE(k+l)是俯仰通道 Kalman滤波器k+1时刻的滤波器增益;12为2 X 2维的单位矩阵；
[0118]步骤5.2、偏航通道视线运动离散化状态方程写作
[0123] 其中，He = [1 0]，ve(k)为视线俯仰角的测量噪声，假设其均值为零，
[0124] 偏航通道Kalman滤波器为
[0125]
[0126] 其中，iy斜和馬心）分别代表Xe的滤波估计和预报估计，Ke(k+U是偏航通道 Kalman滤波器k+1时刻的滤波器增益；
[0129]步骤5.3、根据步骤5.1和步骤5.2的滤波器精确求出目标与导弹之间的视线俯仰 角速率和视线偏航角速率。
[0130] 其他步骤和参数与【具体实施方式】一至五之一相同。
[0131] 实施例
[0132] 下面通过数值仿真结果说明本发明实施效果。
[0133] 对地基拦截导弹拦截空中高速机动目标的过程进行全弹道仿真。导弹惯导系统的 数据更新周期为2.5ms，导引头数据更新周期为10ms，其Kalman滤波器滤波周期和制导周期 均为10ms。
[0134] 在视线角速率Kalman滤波器中，状态估计方差阵的初值取为
[0135]
[0136] ii噪声方差取Reo化）= 1〇ΛΚρο化）= 1〇Λ动态噪声方差阵取为
[0137]
[0138] 滤波器的初始估计值矣(0巧日如(0)取为根据导引头第1拍测量值计算出来的视线 角，ii〇)和i(〇)用几何方法计算:即
[0141] 针对在约20km高度飞行的目标，考查一条拦截斜距为90km左右的弹道：
[0142] 第Os时，在发射点惯性坐标系中，导弹速度Vx = Om/s，Vy = Om/s，Vz = Om/s，导弹位 置x = 0m，y = 0m，z = 0m，导弹角速度 Wx = 〇°/s, Wy = 〇°/s，Wz = 〇°/s，姿态角#二'90。，Φ = 0。，丫=0。；目标位置xt = 220km，yt=16.2km，zt = 0km，目标速率为Vxt = -1499.113m/s，Vパ =51.5776m/s，Vzt = 0m/s，目标飞行高度Htc=19.983km。目标作侧向常值机动，机动幅值at =邑。
[0143] 图4和图5所示分别为导弹拦截目标纵向飞行弹道仿真效果图和导弹拦截目标侧 向飞行弹道仿真效果图。图5中的纵坐标Η代表飞行高度，单位为km。目标高度计算公式为出 = ；Tti-Re,其中
巧表目t不的地心距，xti二xt, yti二yt+Re, zti二zt代表目t不在 图1所示的地心肢性坐标系中的坐标，Re为地球半径；导弹高度计算公式为H = ri-Re，其中
代表导弹的地屯、距，XI = x，yi = y+Re，ZI = Z代表导弹在图1所示的地屯、惯性 坐标系中的坐标。此弹道下，终端拦截时间为88.138s，拦截斜距为92.494km，拦截高度 19.9829km，终端脱祀量 0.1102m。
[0144] 图6为目标一导弹视线俯仰角速率仿真效果图，图7为目标一导弹视线偏航角速率 仿真效果图。图6和图7中，true代表视线角速率的真实值，Kail代表本文所提出的机动目标 跟踪视线角速率Kalman滤波器，KalO代表原有的非机动目标跟踪视线角速率Kalman滤波 器，图6和图7表明机动目标跟踪视线角速率Kalman滤波器的收敛速度很快，Is后估计值可 用，而且其估计值能够很好地跟踪视线角速率的真实值。图6和图7还表明，机动目标跟踪视 线角速率Kalman滤波器的估计结果明显优于原有的非机动目标跟踪视线角速率Kalman滤 波器的估计结果，稳态精度最大提高了约0.03 Vs。
[0145]图8-图10中绘出了目标跟踪滤波器对目标机动加速度在发射点惯性坐标系Ξ个 轴上的分量的估计结果，由图8-图 10可W看出，对目标加速度的估计误差小于2m/s2。
【主权项】
1. 针对机动目标的视线角速率估计方法，其特征在于包括下述步骤： 步骤一、测算目标一导弹相对距离R和目标一导弹相对速度Λ ; 步骤二、测算导弹加速度在视线坐标系c/ X4y4Z4的c/ y4和c/ Ζ4轴方向上的分量as和ae; 步骤三、估计目标加速度在视线坐标系c/ X4Y4Z4的c/ y4和c/ Z4轴方向上的分量ει?和ate; 步骤四、计算出视线俯仰角qE和视线偏航角qe; 步骤五、将目标一导弹相对距离R和目标一导弹相对速度导弹加速度分量ajPae，目 标加速度估计结果atdPate以及视线俯仰角和视线偏航角计算结果qjPqe分别代入到导弹 的俯仰通道视线角速率Kalman滤波器和偏航通道视线角速率Kalman滤波器中，从而精确求 出目标与导弹之间的视线俯仰角速率和视线偏航角速率。2. 根据权利要求1所述的针对机动目标的视线角速率估计方法，其特征在于步骤一所 述测算目标一导弹相对距离R和目标一导弹相对速度左的具体步骤如下： 定义地心惯性坐标系:地心惯性坐标系omyizi的原点οι位于地心上，οι?轴位于赤道平 面内，指向某一恒星，Oizi轴指向北极方向，oiyi按右手定则确定； 定义发射点惯性坐标系:设发射点惯性坐标系〇FXFyFZF在导弹发射瞬间相对于地心惯性 坐标系固化不动，发射点惯性坐标系的原点为发射点OF，OFyF轴铅锤向上，OFXF轴和OFZF轴位 于水平面内，其指向按需要选取， 〇FXFyF为射击平面； 外、λ〇和aF分别是发射点相对于地心惯性坐标系的炜度、经度和发射方位角； 定义末制导初始视线坐标系:〇〇x〇y〇z〇为末制导初始视线坐标系，〇〇位于末制导初始时 刻导弹导引头回转中心上;OQXQ为初始视线方向，指向目标为正;OQyo位于包含OQXQ的铅垂面 内，垂直于00X0,指向上方为正;OQZQ按照右手定则确定;在整个末制导过程中，OQXQyQZQ坐标 系相对于发射点惯性坐标系OFXFyFZF固化不动;q E〇和q?)分别为视线相对于发射点惯性坐标 系所成的初始视线俯仰角和视线偏航角； 导弹在发射点惯性坐标系的位置为（x、y、z)，其速度在该坐标系中的投影为（Vx、vy、 Vz); 目标在发射点惯性坐标系的位置为(^、^、^)，其速度在该坐标系中的投影为(1、^、 VtZ )； 根据计算 目标一导弹相对距离和目标一导弹相对速度在末制导过程中，始终有R>〇，左< 0。3. 根据权利要求2所述的针对机动目标的视线角速率估计方法，其特征在于步骤二所 述测算导弹加速度在视线坐标系c/ X4y4Z4的c/ y4和c/ Z4轴方向上的分量a￡和ae的具体步骤 如下： 定义视线坐标系:视线坐标系c/ X4y4Z4原点c/位于导引头回转中心;c/ X4轴与当前时刻 目标一导弹视线一致，由导引头回转中心指向目标为正;〇、4轴位于包含C/X4轴的铅锤面 内，与C/ X4轴垂直，指向上方为正;C/ Z4轴按右手定则确定；由末制导初始视线坐标系转换到 视线坐标系定义视线俯仰角qE和视线偏航角q e; 定义弹体坐标系；导弹弹体坐标系oxiyizi的原点〇位于导弹质心上;?χι轴与弹体纵轴重 合，指向头部为正;oyi轴在弹体纵向对称平面内，垂直?χι轴，指向上方为正;ozi轴方向按右 手定则确定； 导弹上加速度计输出的加速度.是沿着弹体坐标系oxiyizi投影的，将导弹上加速度计 输出的加速度5投影到视线坐标系c/ X4y4Z4的c/ y4和c/ Ζ4轴方向上得到导弹视线坐标系c/ X4y4Z4的c/ y4和c/ Z4轴方向上的加速度ae和ap。4.根据权利要求3所述的针对机动目标的视线角速率估计方法，其特征在于步骤三所 述估计目标加速度在视线坐标系c/ X4y4Z4的c/ y4和c/ Z4轴方向上的分量a?和atf!的具体步骤 如下： 步骤3.1、在发射点惯性坐标系的三个轴上，均用Singer模型来分别描述目标加速度分 里_￡1七\、Elty矛口Eltz ：其中，λχ、λ4Ρλζ分别代表发射点惯性坐标系三个轴方向上目标机动时间常数的倒数； wx、wy和^分别代表三个轴上的零均值高斯白噪声；<、：<^.和（表示 atx、aty和atz的导数， 参数上带·均表示参数的导数； 建立起如下三组运动方程步骤3.2、针对于方程(1 )，定义目标加速度分量在发射点惯性坐标系的OFXF轴的状态 变量Xx= [xt Vtx ￡itx]T，则得到如下状态方程对公式(5)以采样周期At进行离散化后，得到则离散化后的状态方程为 Xx(k+l) = ?xXx(k) (7) 其中，k表示第k个采样时刻； 测量方程为 qx(k)=xt(k) (8) 根据如公式（1)的模型，设计如下发射点惯性坐标系XF轴的目标跟踪Kalman滤波器，其 预报方程为其中，i N).和兄(U分别代表Xx的滤波估计和预报估计;Qx为模型预报误差协方差矩 阵；式中Ε{ · }代表数学期望； 滤波器的测量修正方程为其中，Kx(k+1)是该滤波器在k+Ι时刻的滤波增益;qx(k+l)是k+Ι时刻的测量信息；I 3为3 X 3维单位矩阵;Rx为测量误差协方差矩阵;测量矩阵为 Hx=[l O 0] (11) 当没有测量信息时，只运行预报方程(9)，有测量信息时，则同时运行预报方程(9)和测 量修正方程(10)，得到发射点惯性坐标系的〇FXF轴的目标加速度atx的估计值； 步骤3.3、根据步骤3.2，设计发射点惯性坐标系yF轴和ZF轴的目标跟踪Kalman滤波器并 得到发射点惯性坐标系的yF轴和ZF轴的目标加速度Elty和Eltz的估计值； 步骤3.4、把目标加速度atx、aty和atz的估计值从发射点惯性坐标系投影到视线坐标系 后，即得到目标加速度在视线坐标系的V y4和c/ Z4轴方向上的分量a?和ate。5.根据权利要求4所述的针对机动目标的视线角速率估计方法，其特征在于步骤四所 述计算出视线俯仰角qs和视线偏航角qe的具体步骤如下： 导引头测量坐标系的圆点在导引头回转中心c/，其三个轴分别平行于弹体坐标系的〇X1 轴、Oy1轴和OZ1轴;导引头输出目标一导弹视线相对导引头测量坐标系的视线俯仰角和视线 偏航角Aqe和Aqe; 设导引头测量坐标系与弹体坐标系重合，则目标一导弹视线的单位向量在导引头测量 坐标系中的投影为其中，clx、cly、clz分别为目标一导弹视线的单位向量在导引头测量坐标系三个轴上的 投影； 而目标一导弹视线的单位向量在发射点惯性坐标系中的投影为其中，CQX、CQy、CQZ分别为目标一导弹视线的单位向量在发射点惯性坐标系三个轴上的 投影； 则式中，Cp1为由发射点惯性坐标系到弹体坐标系的转换矩阵，其中，θ、φ和γ分别为弹体的俯仰角，偏航角和滚动角； 根据式(13)计算出视线俯仰角和视线偏航角，BP6.根据权利要求5所述的针对机动目标的视线角速率估计方法，其特征在于步骤五所 述求出目标与导弹之间的视线俯仰角速率和视线偏航角速率的具体步骤如下： 步骤5.1、俯仰通道视线运动状态方程为对公式(18)进行离散化，得到其中，k代表第k个采样时刻，At为采样周期； 俯仰通道视线角测量方程为其中，Ηε = [1 0]，VE(k)为视线俯仰角的测量噪声，假设其均值为零；则俯仰通道Kal man滤波器为其中，之⑷和.Y,..⑷分别代表L的滤波估计和预报估计，Kjk+l)是俯仰通道Kalman滤 波器k+Ι时刻的滤波器增益;I2为2 X 2维的单位矩阵；步骤5.2、偏航通道视线运动离散化状态方程写作Wi!(k)是一个零均值随机过程，偏航视线角测量方程为其中，H e = [ I O ]，V e ( k )为视线俯仰角的测量噪声，假设其均值为零，偏航通道Kalman滤波器为其中，±#(1)和J/fc)分别代表Xe的滤波估计和预报估计，Ke(k+1)是偏航通道Kalman滤 波器k+Ι时刻的滤波器增益；步骤5.3、根据步骤5.1和步骤5.2的滤波器精确求出目标与导弹之间的视线俯仰角速 率和视线偏航角速率。
【专利摘要】针对机动目标的视线角速率估计方法，涉及一种视线角速率估计方法。本发明为了解决现有的针对非机动目标的视线角速率估计方法提取机动目标视线角速率的精度有限的问题，首先测算目标—导弹相对距离R和目标—导弹相对速度然后测算导弹加速度分量aε和aβ和估计目标加速度的分量atε和atβ，并计算出视线俯仰角qε和视线偏航角qβ；然后将R、aε、aβ、atε、atβ以及qε和qβ分别代入到导弹的俯仰通道视线角速率Kalman滤波器和偏航通道视线角速率Kalman滤波器中，从而精确求出目标与导弹之间的视线俯仰角速率和视线偏航角速率。本发明适用制导领域中机动目标的视线角速率估计。
【IPC分类】G01C21/20, G01C21/16
【公开号】CN105486307
【申请号】CN201510829839
【发明人】周荻, 邹昕光, 张中磊, 朱蕊蘋
【申请人】哈尔滨工业大学
【公开日】2016年4月13日
【申请日】2015年11月25日
【技术领域】
[0001 ]本发明设及一种视线角速率估计方法。
【背景技术】
[0002] 在制导技术领域，目标的持续机动在很大程度上增加了视线角速率高精度提取的 难度，现有的视线角速率提取方法不能满足拦截导弹或拦截器直接碰撞目标的精确制导要 求，必须研究与目标机动条件相适应的新的高精度视线角速率提取方法。
[0003] 近些年来，为了降低其成本，提高其可靠性，精确制导武器越来越多地采用小型捷 联或半捷联导引头，运种导引头的特点是可W测量目标一导弹之间的相对视线角，如果是 主动式导引头还可W距离信息，但捷联或半捷联导引头不能直接输出实际制导律需要的视 线角速率信息。
[0004] 现有的视线角速率估计方法均是针对非机动目标的，在对付机动目标时视线角速 率提取精度有限。所W针对安装捷联或半捷联导引头的导弹拦截机动目标运一应用背景， 目前还没有一种行之有效的视线角速率高精度提取方法。
【发明内容】
[0005] 本发明为了解决现有的针对非机动目标的视线角速率估计方法提取机动目标视 线角速率的精度有限的问题，进而提出了一种专口针对捷联或半捷联导引头和机动目标的 视线角速率高精度Kalman滤波方法。
[0006] 针对机动目标的视线角速率估计方法，包括下述步骤：
[0007] 步骤一、测算目标一导弹相对距离R和目标一导弹相对速度R;
[000引步骤二、测算导弹加速度在视线坐标系0' Χ4Υ4Ζ4的0' Y4和0' Z4轴方向上的分量3ε和 日e;
[0009] 步骤Ξ、估计目标加速度在视线坐标系0' Χ4Υ4Ζ4的0' Y4和0' Z4轴方向上的分量ate 和 ate;
[0010] 步骤四、计算出视线俯仰角qs和视线偏航角qe;
[0011] 步骤五、将目标一导弹相对距离R和目标一导弹相对速度R，导弹加速度分量ae和 ae，目标加速度估计结果ate和ateW及视线俯仰角和视线偏航角计算结果qe和qe分别代入到 导弹的俯仰通道视线角速率Kalman滤波器和偏航通道视线角速率Kalman滤波器中，从而精 确求出目标与导弹之间的视线俯仰角速率和视线偏航角速率。
[0012] 本发明具有W下有益效果：
[0013] 本发明将目标加速度估计Kalman滤波器与视线角速率估计Kalman滤波器相结合， 得到了针对机动目标的捷联或半捷联导引头视线角速率估计Kalman滤波器，其精度高于现 有的针对非机动目标的捷联或半捷联导引头视线角速率估计Kalman滤波器，精度提高了 0.03Vs。
【附图说明】
[0014] 图1为地屯、惯性坐标系、发射点惯性坐标系和末制导初始视线坐标系关系示意图；
[0015] 图2为末制导初始视线坐标系和视线坐标系关系示意图；
[0016] 图3为导弹的弹体坐标系示意图；
[0017] 图4为导弹拦截目标的纵向飞行弹道仿真效果图；
[0018] 图5为导弹拦截目标的侧向飞行弹道仿真效果图；、
[0019] 图6为目标一导弹视线俯仰角速率仿真效果图；
[0020] 图7为目标一导弹视线偏航角速率仿真效果图；
[0021 ]图8发射点惯性坐标系XF轴目标加速度及其估计仿真效果图；
[0022] 图9发射点惯性坐标系yF轴目标加速度及其估计仿真效果图；
[0023] 图10发射点惯性坐标系ZF轴目标加速度及其估计仿真效果图。
【具体实施方式】
【具体实施方式】 [0024] 一：
[0025] 针对机动目标的视线角速率估计方法，包括下述步骤：
[0026] 步骤一、测算目标一导弹相对距离R和目标一导弹相对速度R;
[0027] 步骤二、测算导弹加速度在视线坐标系c/ Χ4Υ4Ζ4的c/ Y4和c/ Z4轴方向上的分量ae和 日e;
[00%] 步骤Ξ、估计目标加速度在视线坐标系c/ Χ4Υ4Ζ4的c/ y4和c/ Z4轴方向上的分量ate 和 ate;
[0029] 步骤四、计算出视线俯仰角qs和视线偏航角qe;
[0030] 步骤五、将目标一导弹相对距离R和目标一导弹相对速度R，导弹加速度分量ae和 ae，目标加速度估计结果ate和ateW及视线俯仰角和视线偏航角计算结果qe和qe分别代入到 导弹的俯仰通道视线角速率Kalman滤波器和偏航通道视线角速率Kalman滤波器中，从而精 确求出目标与导弹之间的视线俯仰角速率和视线偏航角速率。
[0031 ]【具体实施方式】二:结合图1说明本实施方式，
[0032] 本实施方式步骤一所述测算目标一导弹相对距离R和目标一导弹相对速度R的具 体步骤如下；
[0033] 定义地屯、惯性坐标系：地屯、惯性坐标系oixiyizi的原点01位于地屯、上，OUI轴位于 赤道平面内，指向某一恒星，OIZI轴指向北极方向，oiyi按右手定则确定；
[0034] 定义发射点惯性坐标系：设发射点惯性坐标系OFXFyFZF在导弹发射瞬间相对于地 屯、惯性坐标系固化不动，它与地屯、惯性坐标系的关系如图1所示，发射点惯性坐标系的原点 为发射点〇F，〇FyF轴铅键向上，OF祉轴和OFZF轴位于水平面内，其指向按需要选取，通常OFXFyF 为射击平面；
[0035] 见图1，地屯、惯性坐标系与发射点惯性坐标系之间的转换取绝于狗)、λ〇和αρΞ个角 度；妍、λ〇和αρ分别是发射点相对于地屯、惯性坐标系的缔度、经度和发射方位角；
[00%]定义末制导初始视线坐标系：图1中ooxoyozo为末制导初始视线坐标系，〇日位于末制 导初始时刻导弹导引头回转中屯、上;O0X日为初始视线方向，指向目标为正;ooyo位于包含00X0 的铅垂面内，垂直于00X0,指向上方为正；oozo按照右手定则确定；在整个末制导过程中， ooxoy日Z日坐标系相对于发射点惯性坐标系0阳FyFZ個化不动;qe日和q邮分别为视线相对于发射 点惯性坐标系所成的初始视线俯仰角和视线偏航角；
[0037] 导弹在发射点惯性坐标系的位置为(x、y、z)，其速度在该坐标系中的投影为(Vx、 Vy、Vz )，运些信息由弹上导航系统提供；
[0038] 目标在发射点惯性坐标系的位置为（xt、yt、zt)，其速度在该坐标系中的投影为 (Vtx、Vty、Vtz)，运些信息由弹上的目标跟踪滤波器提供；
[0039] 根据 Ax = x-xt，Ay = y-yt，Az = z-zt，AVx=Vx-Vtx，AVy = Vy-Vty，AVz = Vz-Vtz， 计算目标一导弹相对距离
和目标一导弹相对速度
在末制导过程中，始终有R〉〇，度< 0。
[0040] 其他步骤和参数与【具体实施方式】一相同。
【具体实施方式】 [0041] Ξ:
[0042] 本实施方式步骤二所述测算导弹加速度在视线坐标系〇/ X4y4Z4的〇/ y4和〇/ Z4轴方 向上的分量ae和ae的具体步骤如下：
[00创定义视线坐标系:视线坐标系c/x4y4Z4原点oM立于导引头回转中屯、;C/X4轴与当前 时刻目标一导弹视线一致，由导引头回转中屯、指向目标为正;c/y4轴位于包含C/X4轴的铅键 面内，与C/X4轴垂直，指向上方为正;C/Z4轴按右手定则确定;如图2所示，由末制导初始视线 坐标系转换到视线坐标系定义视线俯仰角qs和视线偏航角qe;
[0044] 定义弹体坐标系；由图3所示，导弹弹体坐标系oxiyizi的原点0位于导弹质屯、上； 0X1轴与弹体纵轴重合，指向头部为正;oyi轴在弹体纵向对称平面内，垂直0X1轴，指向上方 为正;OZ1轴方向按右手定则确定；
[0045] 导弹上加速度计输出的加速度召是沿着弹体坐标系oxiyizi投影的，将导弹上加速 度计输出的加速度α投影到视线坐标系〇/x4y4Z4的〇/y4和〇/Z4轴方向上得到导弹视线坐标 系0' Χ4Υ4Ζ；4的0' Y4和0' Z4轴方向上的加速度ae和ae。
[0046] 其他步骤和参数与【具体实施方式】一或二相同。
【具体实施方式】 [0047] 四：
[004引本实施方式步骤Ξ所述估计目标加速度在视线坐标系0/ X4y4Z4的0/ y4和0/ Z4轴方 向上的分量ate和ate的具体步骤如下：
[0049]步骤3.1、在发射点惯性坐标系的Ξ个轴上，均用Singer模型来分别描述目标加速 度分量atx、aty和atz:
[0053]其中，λχ、λγ和λζ分别代表发射点惯性坐标系Ξ个轴方向上目标机动时间常数的倒 数;Wx、Wy和Wz分别代表Ξ个轴上的零均值高斯白噪声；％、如和'也表示atx、aty和atz的导 数，参数上带·均表示参数的导数；目标加速度分量atx、a巧和atz既包含气动力产生的加速 度，又包含重力产生的加速度；
[0化4] 建立起如下Ξ组运动方程
[0058] 步骤3.2、针对于方程（1)，定义目标加速度分量atx在发射点惯性坐标系的OFXF轴 的状态变量Xx=[xt Vtx atx]T，则得到如下状态方程
[0059] 去*=巧馬 (4)
[0060] 其中，
[0061 ]
(5)
[0062] 对公式(5) W采样周期At(该周期与制导周期一致)进行离散化后，得到
[0063]
(..6)
[0064] 则离散化后的状态方程为
[00化]Xx(k+1) =巫xXx 化） （7)
[0066] 其中，k表示第k个采样时刻；
[0067] 目标的位置由地面雷达测量装置测量得到，则测量方程为
[0068] IU化）=xt化） （8)该测量信息数据更新率较低，例如0.2s更新一次；
[0069] 根据如公式（1)的模型，设计如下发射点惯性坐标系XF轴的目标跟踪Kalman滤波 器，其预报方程为
[0070]
(9)
[0071] 其中，义、.脚和玄,作)分别代表Χχ的滤波估计和预报估计;Qx为模型预报误差协方 差矩阵；
[0072]
[0073]
[0074] 式中E{ · }代表数学期望；
[0075] 滤波器的测量修正方程为
[0076]
(10)
[0077] 其中，Κχ化+1)是该滤波器在k+1时刻的滤波增益;rix化+1)是k+1时刻的测量信息； 13为3 X 3维单位矩阵;Rx为测量误差协方差矩阵;测量矩阵为
[007引出=[1 0 0] (11)
[0079] 当没有测量信息时，只运行预报方程(9)，有测量信息时，则同时运行预报方程(9) 和测量修正方程(10)，得到发射点惯性坐标系的OFXF轴的目标加速度atx的估计值；
[0080] 步骤3.3、根据步骤3.2，设计发射点惯性坐标系yF轴和ZF轴的目标跟踪Kalman滤波 器并得到发射点惯性坐标系的yF轴和ZF轴的目标加速度aty和atz的估计值；
[0081] 步骤3.4、把目标加速度atx、a巧和atz的估计值从发射点惯性坐标系投影到视线坐 标系后，即得到目标加速度在视线坐标系的〇/ y4和〇/ Z4轴方向上的分量ate和ate。
[0082] 其他步骤和参数与【具体实施方式】一至Ξ之一相同。
【具体实施方式】 [0083] 五：
[0084] 本实施方式步骤四所述计算出视线俯仰角qs和视线偏航角qe的具体步骤如下：
[0085] 导引头测量坐标系的圆点在导引头回转中屯、〇/，在不考虑导引头安装误差及导引 头安装位置晓性变形的情况下，其Ξ个轴分别平行于弹体坐标系的0X1轴、oyi轴和OZ1轴；导 引头输出目标一导弹视线相对导引头测量坐标系的视线俯仰角和视线偏航角Aqs和Aqe;
[0086] 设导引头测量坐标系与弹体坐标系重合，则目标一导弹视线的单位向量在导引头 测量坐标系中的投影为
[0087]
(12)
[0088] 其中，cix、ciy、ciz分别为目标一导弹视线的单位向量在导引头测量坐标系Ξ个轴 上的投影；
[0089] 而目标一导弹视线的单位向量在发射点惯性坐标系中的投影为
[0090]
(13)
[0091] 其中，c〇x、c〇y、c〇z分别为目标一导弹视线的单位向量在发射点惯性坐标系Ξ个轴 上的投影；
[0092] 则
[0093]
(14)
[0094] 式中，Cf4为由发射点惯性坐标系到弹体坐标系的转换矩阵，
[0095]
[0096] 其中，沒、Φ和丫分别为弹体的俯仰角，偏航角和滚动角，由惯性导航系统测算得 到；
[0097] 根据式(13)计算出视线俯仰角和视线偏航角，即
[0100] 其他步骤和参数与【具体实施方式】一至四之一相同。
【具体实施方式】 [0101] 六：
[0102] 本实施方式步骤五所述求出目标与导弹之间的视线俯仰角速率和视线偏航角速 率的具体步骤如下：
[0103] 步骤5.1、俯仰通道视线运动状态方程为
[0104]
(1:8)
[0105] 对公式（18)进行离散化，得到
[0106]
(19)
[0107] 其中，k代表第k个采样时刻，At为采样周期；
[0108] 俯仰通道视线角测量方程为
[0109]
(锁）
[0110] 其中，Ηε = [1 0]，νε化)为视线俯仰角的测量噪声，假设其均值为零；
[0113] 则俯仰通道Kalman滤波器为
[0114]
(21 >
[0"引其中，疋.(0和是(A-)分别代表Χε的滤波估计和预报估计，KE(k+l)是俯仰通道 Kalman滤波器k+1时刻的滤波器增益;12为2 X 2维的单位矩阵；
[0118]步骤5.2、偏航通道视线运动离散化状态方程写作
[0123] 其中，He = [1 0]，ve(k)为视线俯仰角的测量噪声，假设其均值为零，
[0124] 偏航通道Kalman滤波器为
[0125]
[0126] 其中，iy斜和馬心）分别代表Xe的滤波估计和预报估计，Ke(k+U是偏航通道 Kalman滤波器k+1时刻的滤波器增益；
[0129]步骤5.3、根据步骤5.1和步骤5.2的滤波器精确求出目标与导弹之间的视线俯仰 角速率和视线偏航角速率。
[0130] 其他步骤和参数与【具体实施方式】一至五之一相同。
[0131] 实施例
[0132] 下面通过数值仿真结果说明本发明实施效果。
[0133] 对地基拦截导弹拦截空中高速机动目标的过程进行全弹道仿真。导弹惯导系统的 数据更新周期为2.5ms，导引头数据更新周期为10ms，其Kalman滤波器滤波周期和制导周期 均为10ms。
[0134] 在视线角速率Kalman滤波器中，状态估计方差阵的初值取为
[0135]
[0136] ii噪声方差取Reo化）= 1〇ΛΚρο化）= 1〇Λ动态噪声方差阵取为
[0137]
[0138] 滤波器的初始估计值矣(0巧日如(0)取为根据导引头第1拍测量值计算出来的视线 角，ii〇)和i(〇)用几何方法计算:即
[0141] 针对在约20km高度飞行的目标，考查一条拦截斜距为90km左右的弹道：
[0142] 第Os时，在发射点惯性坐标系中，导弹速度Vx = Om/s，Vy = Om/s，Vz = Om/s，导弹位 置x = 0m，y = 0m，z = 0m，导弹角速度 Wx = 〇°/s, Wy = 〇°/s，Wz = 〇°/s，姿态角#二'90。，Φ = 0。，丫=0。；目标位置xt = 220km，yt=16.2km，zt = 0km，目标速率为Vxt = -1499.113m/s，Vパ =51.5776m/s，Vzt = 0m/s，目标飞行高度Htc=19.983km。目标作侧向常值机动，机动幅值at =邑。
[0143] 图4和图5所示分别为导弹拦截目标纵向飞行弹道仿真效果图和导弹拦截目标侧 向飞行弹道仿真效果图。图5中的纵坐标Η代表飞行高度，单位为km。目标高度计算公式为出 = ；Tti-Re,其中
巧表目t不的地心距，xti二xt, yti二yt+Re, zti二zt代表目t不在 图1所示的地心肢性坐标系中的坐标，Re为地球半径；导弹高度计算公式为H = ri-Re，其中
代表导弹的地屯、距，XI = x，yi = y+Re，ZI = Z代表导弹在图1所示的地屯、惯性 坐标系中的坐标。此弹道下，终端拦截时间为88.138s，拦截斜距为92.494km，拦截高度 19.9829km，终端脱祀量 0.1102m。
[0144] 图6为目标一导弹视线俯仰角速率仿真效果图，图7为目标一导弹视线偏航角速率 仿真效果图。图6和图7中，true代表视线角速率的真实值，Kail代表本文所提出的机动目标 跟踪视线角速率Kalman滤波器，KalO代表原有的非机动目标跟踪视线角速率Kalman滤波 器，图6和图7表明机动目标跟踪视线角速率Kalman滤波器的收敛速度很快，Is后估计值可 用，而且其估计值能够很好地跟踪视线角速率的真实值。图6和图7还表明，机动目标跟踪视 线角速率Kalman滤波器的估计结果明显优于原有的非机动目标跟踪视线角速率Kalman滤 波器的估计结果，稳态精度最大提高了约0.03 Vs。
[0145]图8-图10中绘出了目标跟踪滤波器对目标机动加速度在发射点惯性坐标系Ξ个 轴上的分量的估计结果，由图8-图 10可W看出，对目标加速度的估计误差小于2m/s2。
【主权项】
1. 针对机动目标的视线角速率估计方法，其特征在于包括下述步骤： 步骤一、测算目标一导弹相对距离R和目标一导弹相对速度Λ ; 步骤二、测算导弹加速度在视线坐标系c/ X4y4Z4的c/ y4和c/ Ζ4轴方向上的分量as和ae; 步骤三、估计目标加速度在视线坐标系c/ X4Y4Z4的c/ y4和c/ Z4轴方向上的分量ει?和ate; 步骤四、计算出视线俯仰角qE和视线偏航角qe; 步骤五、将目标一导弹相对距离R和目标一导弹相对速度导弹加速度分量ajPae，目 标加速度估计结果atdPate以及视线俯仰角和视线偏航角计算结果qjPqe分别代入到导弹 的俯仰通道视线角速率Kalman滤波器和偏航通道视线角速率Kalman滤波器中，从而精确求 出目标与导弹之间的视线俯仰角速率和视线偏航角速率。2. 根据权利要求1所述的针对机动目标的视线角速率估计方法，其特征在于步骤一所 述测算目标一导弹相对距离R和目标一导弹相对速度左的具体步骤如下： 定义地心惯性坐标系:地心惯性坐标系omyizi的原点οι位于地心上，οι?轴位于赤道平 面内，指向某一恒星，Oizi轴指向北极方向，oiyi按右手定则确定； 定义发射点惯性坐标系:设发射点惯性坐标系〇FXFyFZF在导弹发射瞬间相对于地心惯性 坐标系固化不动，发射点惯性坐标系的原点为发射点OF，OFyF轴铅锤向上，OFXF轴和OFZF轴位 于水平面内，其指向按需要选取， 〇FXFyF为射击平面； 外、λ〇和aF分别是发射点相对于地心惯性坐标系的炜度、经度和发射方位角； 定义末制导初始视线坐标系:〇〇x〇y〇z〇为末制导初始视线坐标系，〇〇位于末制导初始时 刻导弹导引头回转中心上;OQXQ为初始视线方向，指向目标为正;OQyo位于包含OQXQ的铅垂面 内，垂直于00X0,指向上方为正;OQZQ按照右手定则确定;在整个末制导过程中，OQXQyQZQ坐标 系相对于发射点惯性坐标系OFXFyFZF固化不动;q E〇和q?)分别为视线相对于发射点惯性坐标 系所成的初始视线俯仰角和视线偏航角； 导弹在发射点惯性坐标系的位置为（x、y、z)，其速度在该坐标系中的投影为（Vx、vy、 Vz); 目标在发射点惯性坐标系的位置为(^、^、^)，其速度在该坐标系中的投影为(1、^、 VtZ )； 根据计算 目标一导弹相对距离和目标一导弹相对速度在末制导过程中，始终有R>〇，左< 0。3. 根据权利要求2所述的针对机动目标的视线角速率估计方法，其特征在于步骤二所 述测算导弹加速度在视线坐标系c/ X4y4Z4的c/ y4和c/ Z4轴方向上的分量a￡和ae的具体步骤 如下： 定义视线坐标系:视线坐标系c/ X4y4Z4原点c/位于导引头回转中心;c/ X4轴与当前时刻 目标一导弹视线一致，由导引头回转中心指向目标为正;〇、4轴位于包含C/X4轴的铅锤面 内，与C/ X4轴垂直，指向上方为正;C/ Z4轴按右手定则确定；由末制导初始视线坐标系转换到 视线坐标系定义视线俯仰角qE和视线偏航角q e; 定义弹体坐标系；导弹弹体坐标系oxiyizi的原点〇位于导弹质心上;?χι轴与弹体纵轴重 合，指向头部为正;oyi轴在弹体纵向对称平面内，垂直?χι轴，指向上方为正;ozi轴方向按右 手定则确定； 导弹上加速度计输出的加速度.是沿着弹体坐标系oxiyizi投影的，将导弹上加速度计 输出的加速度5投影到视线坐标系c/ X4y4Z4的c/ y4和c/ Ζ4轴方向上得到导弹视线坐标系c/ X4y4Z4的c/ y4和c/ Z4轴方向上的加速度ae和ap。4.根据权利要求3所述的针对机动目标的视线角速率估计方法，其特征在于步骤三所 述估计目标加速度在视线坐标系c/ X4y4Z4的c/ y4和c/ Z4轴方向上的分量a?和atf!的具体步骤 如下： 步骤3.1、在发射点惯性坐标系的三个轴上，均用Singer模型来分别描述目标加速度分 里_￡1七\、Elty矛口Eltz ：其中，λχ、λ4Ρλζ分别代表发射点惯性坐标系三个轴方向上目标机动时间常数的倒数； wx、wy和^分别代表三个轴上的零均值高斯白噪声；<、：<^.和（表示 atx、aty和atz的导数， 参数上带·均表示参数的导数； 建立起如下三组运动方程步骤3.2、针对于方程(1 )，定义目标加速度分量在发射点惯性坐标系的OFXF轴的状态 变量Xx= [xt Vtx ￡itx]T，则得到如下状态方程对公式(5)以采样周期At进行离散化后，得到则离散化后的状态方程为 Xx(k+l) = ?xXx(k) (7) 其中，k表示第k个采样时刻； 测量方程为 qx(k)=xt(k) (8) 根据如公式（1)的模型，设计如下发射点惯性坐标系XF轴的目标跟踪Kalman滤波器，其 预报方程为其中，i N).和兄(U分别代表Xx的滤波估计和预报估计;Qx为模型预报误差协方差矩 阵；式中Ε{ · }代表数学期望； 滤波器的测量修正方程为其中，Kx(k+1)是该滤波器在k+Ι时刻的滤波增益;qx(k+l)是k+Ι时刻的测量信息；I 3为3 X 3维单位矩阵;Rx为测量误差协方差矩阵;测量矩阵为 Hx=[l O 0] (11) 当没有测量信息时，只运行预报方程(9)，有测量信息时，则同时运行预报方程(9)和测 量修正方程(10)，得到发射点惯性坐标系的〇FXF轴的目标加速度atx的估计值； 步骤3.3、根据步骤3.2，设计发射点惯性坐标系yF轴和ZF轴的目标跟踪Kalman滤波器并 得到发射点惯性坐标系的yF轴和ZF轴的目标加速度Elty和Eltz的估计值； 步骤3.4、把目标加速度atx、aty和atz的估计值从发射点惯性坐标系投影到视线坐标系 后，即得到目标加速度在视线坐标系的V y4和c/ Z4轴方向上的分量a?和ate。5.根据权利要求4所述的针对机动目标的视线角速率估计方法，其特征在于步骤四所 述计算出视线俯仰角qs和视线偏航角qe的具体步骤如下： 导引头测量坐标系的圆点在导引头回转中心c/，其三个轴分别平行于弹体坐标系的〇X1 轴、Oy1轴和OZ1轴;导引头输出目标一导弹视线相对导引头测量坐标系的视线俯仰角和视线 偏航角Aqe和Aqe; 设导引头测量坐标系与弹体坐标系重合，则目标一导弹视线的单位向量在导引头测量 坐标系中的投影为其中，clx、cly、clz分别为目标一导弹视线的单位向量在导引头测量坐标系三个轴上的 投影； 而目标一导弹视线的单位向量在发射点惯性坐标系中的投影为其中，CQX、CQy、CQZ分别为目标一导弹视线的单位向量在发射点惯性坐标系三个轴上的 投影； 则式中，Cp1为由发射点惯性坐标系到弹体坐标系的转换矩阵，其中，θ、φ和γ分别为弹体的俯仰角，偏航角和滚动角； 根据式(13)计算出视线俯仰角和视线偏航角，BP6.根据权利要求5所述的针对机动目标的视线角速率估计方法，其特征在于步骤五所 述求出目标与导弹之间的视线俯仰角速率和视线偏航角速率的具体步骤如下： 步骤5.1、俯仰通道视线运动状态方程为对公式(18)进行离散化，得到其中，k代表第k个采样时刻，At为采样周期； 俯仰通道视线角测量方程为其中，Ηε = [1 0]，VE(k)为视线俯仰角的测量噪声，假设其均值为零；则俯仰通道Kal man滤波器为其中，之⑷和.Y,..⑷分别代表L的滤波估计和预报估计，Kjk+l)是俯仰通道Kalman滤 波器k+Ι时刻的滤波器增益;I2为2 X 2维的单位矩阵；步骤5.2、偏航通道视线运动离散化状态方程写作Wi!(k)是一个零均值随机过程，偏航视线角测量方程为其中，H e = [ I O ]，V e ( k )为视线俯仰角的测量噪声，假设其均值为零，偏航通道Kalman滤波器为其中，±#(1)和J/fc)分别代表Xe的滤波估计和预报估计，Ke(k+1)是偏航通道Kalman滤 波器k+Ι时刻的滤波器增益；步骤5.3、根据步骤5.1和步骤5.2的滤波器精确求出目标与导弹之间的视线俯仰角速 率和视线偏航角速率。
【专利摘要】针对机动目标的视线角速率估计方法，涉及一种视线角速率估计方法。本发明为了解决现有的针对非机动目标的视线角速率估计方法提取机动目标视线角速率的精度有限的问题，首先测算目标—导弹相对距离R和目标—导弹相对速度然后测算导弹加速度分量aε和aβ和估计目标加速度的分量atε和atβ，并计算出视线俯仰角qε和视线偏航角qβ；然后将R、aε、aβ、atε、atβ以及qε和qβ分别代入到导弹的俯仰通道视线角速率Kalman滤波器和偏航通道视线角速率Kalman滤波器中，从而精确求出目标与导弹之间的视线俯仰角速率和视线偏航角速率。本发明适用制导领域中机动目标的视线角速率估计。
【IPC分类】G01C21/20, G01C21/16
【公开号】CN105486307
【申请号】CN201510829839
【发明人】周荻, 邹昕光, 张中磊, 朱蕊蘋
【申请人】哈尔滨工业大学
【公开日】2016年4月13日
【申请日】2015年11月25日
最新回复(0)