适用于光电跟踪仪的高速目标跟踪控制方法

摘要

本发明公开了一种适用于光电跟踪仪的高速目标跟踪控制方法，属于伺服控制技术领域。本方法利用光电跟踪仪、火炮、载体惯导系统、视频跟踪器和激光测距机输出的相关参数，通过一系列坐标转换、递推迭代和坐标反变换，完成瞄准线坐标系下方位速度环和俯仰速度环跟踪前馈补偿参数的计算，并将该参数分别叠加到方位、俯仰跟踪控制回路，参与跟踪控制。采用本发明的光电跟踪仪对于来袭的高速运动目标跟踪具有响应速度快、动态滞后小、跟踪精度高、跟踪稳定性好的优点，适合于车载、舰载光电跟踪系统对于来袭导弹、飞机等高速运动目标的跟踪，对于提高火控系统的目标解算精度和整个武器系统的作战效能都具有十分重要的意义。

主权项

一种适用于光电跟踪仪的高速目标跟踪控制方法，其特征在于，该方法是通过存储有跟踪控制软件包的伺服计算机实现的，当伺服计算机上电后，跟踪控制软件包执行以下步骤:第一步，初始化伺服计算机各通讯端口、各变量寄存器;第二步，以T为采样周期，实时采集当前时刻火炮方位旋转变压器输出的参数θ，光电跟踪仪方位、俯仰旋转变压器输出的参数(β,ε)、车载惯导系统输出的参数(H,P,R)、视频跟踪器输出的参数(δβ,δε)、激光测距机输出的参数D，其中，θ为车体坐标系下的火炮方位角，β、ε分别为光电坐标系下光电跟踪仪瞄准线指向的方位角和俯仰角，H、P、R分别为大地坐标系下车体的偏航角，纵摇角和横滚角，δβ、δε分别为瞄准线坐标系下光电跟踪仪方位角和俯仰角的跟踪偏差量，D为目标相对于瞄准线坐标系原点的距离;第三步，读取上级系统发出的控制指令，判断指令是否要求光电跟踪仪停止跟踪，如果为“是”，则跳转至第四步；如果为“否”，则跳转至第五步；第四步，向光电跟踪仪的方位、俯仰位置环发送保持当前位置指令，然后跳转至第三步；第五步，利用参数D、δβ、δε，计算当前时刻目标在瞄准线坐标系下的直角坐标分量；第六步，利用参数β、ε计算瞄准线坐标系到光电坐标系的转换矩阵N1，利用转换矩阵N1将目标在瞄准线坐标系下的直角坐标分量转换到光电坐标系；利用光电坐标系原点与炮塔坐标系原点在三个坐标轴方向的固定偏差Ex、Ey、Ez，将目标在光电坐标系下的直角坐标分量转换到炮塔坐标系；利用参数θ计算炮塔坐标系到车体坐标系的转换矩阵P1，再利用转换矩阵P1将目标在炮塔坐标系下的直角坐标分量转换到车体坐标系；利用参数H、P、R计算车 体坐标系到大地坐标系的转换矩阵A1，最终利用转换矩阵A1将目标在车体坐标系下的直角坐标分量转换到大地坐标系；第七步，判断稳定跟踪时间是否大于2秒，如果为“否”，则跳转至第八步；如果为“是”，则跳转至第九步；第八步，并行进行以下操作步骤：8.1、采用数字二阶环跟踪算法计算所述方位速度环和俯仰速度环的前馈补偿量，并叠加到光电跟踪仪的跟踪控制回路；8.2、采用递推滤波跟踪控制方法计算大地坐标系下坐标轴Xd、坐标轴Yd、坐标轴Zd方向的位置、速度以及加速度滤波值；递推滤波跟踪控制方法的计算公式如下：坐标轴Xd方向滤波公式：Dx=Lb[0]+K1·(Tmp[3]‑Lb[0])DxV=Lb[1]+K2·(Tmp[3]‑Lb[0])         （1）DxA=Lb[2]+K3·(Tmp[3]‑Lb[0])坐标轴Yd方向滤波公式：Dy=Lb[3]+K1·(Tmp[4]‑Lb[3])DyV=Lb[4]+K2·(Tmp[4]‑Lb[3])        （2）DyA=Lb[5]+K3·(Tmp[4]‑Lb[3])坐标轴Zd方向滤波公式：Dz=Lb[6]+K1·(Tmp[5]‑Lb[6])DzV=Lb[7]+K2·(Tmp[5]‑Lb[6])       （3）DzA=Lb[8]+K3·(Tmp[5]‑Lb[6])递推公式：Lb[0]=Dx+DxV·T+DxA·T2Lb[1]=DxV+DxA·TLb[2]=DxALb[3]=Dy+DyV·T+DyA·T2Lb[4]=DyV+DyA·T        （4）Lb[5]=DyALb[6]=Dz+DzV·T+DzA·T2Lb[7]=DzV+DzA·TLb[8]=DzA其中，Dx、DxV、DxA分别为大地坐标系下坐标轴Xd方向的位置、速度、加速度的滤波值；Dy、DyV、DyA分别为大地坐标系下坐标轴Yd方向的位置、速度、加速度的滤波值；Dz、DzV、DzA分别为大地坐标系下坐标轴Zd方向的位置、速度、加速度的滤波值；K1、K2、K3均为暂态系数，取值范围为0.02≤K1≤0.05，0.3≤K2≤0.6，1.0≤K3≤3.0；Tmp[3]、Tmp[4]、Tmp[5]分别为目标在大地坐标系下坐标轴Xd、坐标轴Yd、坐标轴Zd方向的直角坐标分量；Lb[0]～Lb[8]为滤波中间变量；8.3、判断稳定跟踪时间是否大于2秒，如果为“否”，则重复执行第二步至8.2步的操作；如果为“是”，则跳转至第九步；第九步，采用（1）～（4）式并用稳态系数K4、K5、K6分别代替（1）～（3）式中的暂态系数K1、K2、K3，计算稳态跟踪时大地坐标系下坐标轴Xd、坐标轴Yd、坐标轴Zd方向的位置、速度以及加速度滤波值，其中0.003≤K4≤0.008，0.01≤K5≤0.03，0.005≤K6≤0.009；第十步，计算瞄准线坐标系下，坐标轴Xm、坐标轴Ym、坐标轴Zm方向的位置滤波值，具体步骤如下：10.1、利用参数H、P、R计算大地坐标系到车体坐标系的转换矩阵A2，并利用转换矩阵A2将大地坐标系下的位置滤波值(Dx,Dy,Dz)转换到车体坐标系；10.2、利用参数θ计算车体坐标系到炮塔坐标系的转换矩阵Q1，并利用转换矩阵Q1将车体坐标系下坐标轴Xc、坐标轴Yc、坐标轴Zc方向的位置滤波值 转换到炮塔坐标系；10.3、利用光电坐标系原点与炮塔坐标系原点在三个坐标轴方向的固定偏差Ex、Ey、Ez，将炮塔坐标系下坐标轴Xp、坐标轴Yp、坐标轴Zp方向的位置滤波值转换到光电坐标系；10.4、利用参数β、ε计算光电坐标系到瞄准线坐标系的转换矩阵L1，并利用转换矩阵L1将光电坐标系下坐标轴Xg、坐标轴Yg、坐标轴Zg方向的位置滤波值转换到瞄准线坐标系；第十一步，计算瞄准线坐标系下坐标轴Xm、坐标轴Ym、坐标轴Zm方向的速度滤波值，具体步骤如下：11.1、利用转换矩阵A2将大地坐标系下的速度滤波值(DxV,DyV,DzV)转换到车体坐标系；11.2、利用转换矩阵Q1将车体坐标系下坐标轴Xc、坐标轴Yc、坐标轴Zc方向的速度滤波值转换到炮塔坐标系；11.3、利用光电坐标系原点与炮塔坐标系原点在三个坐标轴方向的固定偏差Ex、Ey、Ez，将炮塔坐标系下坐标轴Xp、坐标轴Yp、坐标轴Zp方向的速度滤波值转换到光电坐标系；11.4、利用转换矩阵L1将光电坐标系下坐标轴Xg、坐标轴Yg、坐标轴Zg方向的速度滤波值转换到瞄准线坐标系；第十二步，计算瞄准线坐标系下方位速度环和俯仰速度环控制回路中不含加速度分量的速度补偿量，计算公式如下：Est_FwjV=‑Tmp[9]/Tmp[12]Est_FyjV=Tmp[10]/Tmp[13]式中， Tmp [ 12 ] = ( Tmp [ 6 ] ) 2 + ( Tmp [ 7 ] ) 2 Tmp [ 13 ] = ( Tmp [ 6 ] ) 2 + ( Tmp [ 7 ] ) 2 + ( Tmp [ 8 ] ) 2 其中，Est_FwjV为光电跟踪仪在瞄准线坐标系下不含加速度分量的方位 速度补偿量，Est_FyjV为光电跟踪仪在瞄准线坐标系下不含加速度分量的俯仰速度补偿量；Tmp[6]、Tmp[7]、Tmp[8]分别为瞄准线坐标系下坐标轴Xm、坐标轴Ym、坐标轴Zm方向的位置滤波值；Tmp[9]、Tmp[10]分别为瞄准线坐标系下坐标轴Xm、坐标轴Ym方向的速度滤波值；Tmp[12]、Tmp[13]为中间变量；第十三步，计算瞄准线坐标系下坐标轴Xm、坐标轴Ym、坐标轴Zm方向的加速度滤波值，具体步骤如下：13.1、利用转换矩阵A2将大地坐标系下的加速度滤波值(DxA,DyA,DzA)转换到车体坐标系；13.2、利用转换矩阵Q1将车体坐标系下坐标轴Xc、坐标轴Yc、坐标轴Zc方向的加速度滤波值转换到炮塔坐标系；13.3、利用光电坐标系原点与炮塔坐标系原点在三个方向的固定偏差Ex、Ey、Ez，将炮塔坐标系下坐标轴Xp、坐标轴Yp、坐标轴Zp方向的加速度滤波值转换到光电坐标系；13.4、利用转换矩阵L1将光电坐标系下坐标轴Xg、坐标轴Yg、坐标轴Zg方向的加速度滤波值转换到瞄准线坐标系；第十四步，计算瞄准线坐标系下方位、俯仰速度补偿量中的加速度分量，计算公式如下：Est_FwjA=(2·Tmp[11]·Tmp[18]/Tmp[13]+Tmp[17]·Tmp[19]‑Tmp[14]/Tmp[13])/L23Est_FyjA=2·Tmp[11]·Tmp[19]/Tmp[13]‑Tmp[18]·Tmp[19]+Tmp[15]/Tmp[13]式中，Tmp[17]=‑Tmp[10]/Tmp[13]Tmp[18]=Tmp[9]/Tmp[13]Tmp[19]=Tmp[18]·Tmp[9]/Tmp[12]其中，Est_FwjA为光电跟踪仪在瞄准线坐标系下方位速度补偿量中的加速度分量，Est_FyjA为光电跟踪仪在瞄准线坐标系下俯仰速度补偿量中的加速度分量；Tmp[11]为瞄准线坐标系下坐标轴Zm方向的速度滤波值；Tmp[14]、 Tmp[15]分别为瞄准线坐标系下坐标轴Xm、坐标轴Ym方向的加速度滤波值；L23为转换矩阵L1中第二行、第三列的元素；Tmp[17]～Tmp[19]为中间变量；第十五步，计算目标跟踪时，瞄准线坐标系下方位速度环和俯仰速度环的最终前馈补偿参数，计算公式如下：FwCon=(Est_FwjV+K7·Est_FwjA)/L33FyCon=Est_FyjV+K8·Est_FyjA其中，FwCon为瞄准线坐标系下方位速度环的跟踪前馈补偿参数，FyCon为瞄准线坐标系下俯仰速度环的跟踪前馈补偿参数；K7、K8为修正系数，取值范围为0