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

Abstract

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

Description

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

技术领域

本发明属于伺服控制技术领域，涉及光电跟踪仪，尤其涉及一种适用于光电跟踪仪的高速目标跟踪控制方法。

背景技术

当今，车载、舰载综合体形式的光电跟踪仪（光电跟踪仪安装在火炮炮塔上，光电跟踪仪的瞄准线零位与火炮的炮口指向零位一致），通常都采用方位、俯仰两轴稳定体制，即不设专用的稳定平台来隔离车体、舰船运动的影响，而是将光电跟踪仪的基座直接与炮塔固结在一起，将方位、俯仰速率陀螺均安装在俯仰框架上，通过方位、俯仰伺服***，实现光电跟踪仪瞄准线的指向稳定及对目标的自动跟踪。在自动跟踪目标的过程中，光电跟踪仪始终保持瞄准线对准目标，实现目标的高精度跟踪，输出目标诸元信息，从而保证整个火控***的射击精度。

为了满足光电跟踪仪的跟踪精度指标要求，伺服***必须解决两个关键性的技术问题。一是如何减小或消除车摇、船摇、炮塔旋转及火炮射击引起的瞄准线指向稳定误差；第二个是如何减小或消除由于目标高速运动引起的动态跟踪误差。

传统的光电跟踪仪普遍采用二阶环跟踪方法对目标进行跟踪，在该方法中，速度环利用视频***输出的跟踪误差量并采用数字迭代算法计算跟踪速度量，位置环根据跟踪速度量采用比例法计算位置控制量。虽然二阶环跟踪方法较为简单，易于实现，但是，该方法在整个计算过程中没有涉及运动载体和火炮的相关参数，因而无法补偿运动载体和火炮带来的扰动，导致跟踪精度不高；同时，该方法未引入目标距离量，而对远、近目标均采用同样的控制算法，由于在实际跟踪过程中，随着目标距离的变化，光电跟踪仪在方位、俯仰的跟踪角速度也是不断变化的，即对于同一目标，跟踪距离越近，方位、俯仰的跟踪角速度就会越大，因此，传统的二阶环跟踪方法在目标距离较近、运动速度较快的情况下，会产生较大的跟踪滞后。

在实际的应用过程中，传统的二阶环跟踪方法对于目标距离无法得到且运动速度低于2马赫的慢速来袭目标而言，其跟踪效果尚可；但对于飞行速度大于2马赫的高速来袭目标，其跟踪过程中随着目标距离的不断减小，跟踪动态滞后是不可避免的，加之车摇、船摇、炮塔旋转及火炮射击的扰动，其跟踪精度难以保证，这将影响到整个武器***的射击精度。

发明内容

本发明要解决的技术问题是，针对现有技术存在的缺陷，为光电跟踪仪提供一种在外界扰动条件下针对高速来袭目标的跟踪控制方法。

为解决上述技术问题，本发明提出了一种适用于光电跟踪仪的高速目标跟踪控制方法，该方法是通过存储有跟踪控制软件包的伺服计算机实现的，当伺服计算机上电后，跟踪控制软件包执行以下步骤:

第一步，初始化伺服计算机各通讯端口、各变量寄存器;

第二步，以T为采样周期，实时采集当前时刻火炮方位旋转变压器输出的参数θ，光电跟踪仪方位、俯仰旋转变压器输出的参数(β,ε)、车载惯导***输出的参数(H,P,R)、视频***输出的参数(δ_β,δ_ε)、激光测距机输出的参数D，其中，θ为车体坐标系下的火炮方位角，β、ε分别为光电坐标系下光电跟踪仪瞄准线指向的方位角和俯仰角，H、P、R分别为大地坐标系下车体的偏航角，纵摇角和横滚角，δ_β、δ_ε分别为瞄准线坐标系下光电跟踪仪方位角和俯仰角的跟踪偏差量，D为目标相对于瞄准线坐标系原点的距离;

第三步，读取上级***发出的控制指令，判断指令是否要求光电跟踪仪停止跟踪，如果为“是”，则跳转至第四步；如果为“否”，则跳转至第五步；

第四步，向光电跟踪仪的方位、俯仰位置环发送保持当前位置指令，然后跳转至第三步；

第五步，利用参数D、δ_β、δ_ε，计算当前时刻目标在瞄准线坐标系下的直角坐标分量；

第六步，利用参数β、ε计算瞄准线坐标系到光电坐标系的转换矩阵N₁，利用转换矩阵N₁将目标在瞄准线坐标系下的直角坐标分量转换到光电坐标系；利用光电坐标系原点与炮塔坐标系原点在三个坐标轴方向的固定偏差Ex、Ey、Ez，将目标在光电坐标系下的直角坐标分量转换到炮塔坐标系；利用参数θ计算炮塔坐标系到车体坐标系的转换矩阵P₁，再利用转换矩阵P₁将目标在炮塔坐标系下的直角坐标分量转换到车体坐标系；利用参数H、P、R计算车体坐标系到大地坐标系的转换矩阵A₁，最终利用转换矩阵A₁将目标在车体坐标系下的直角坐标分量转换到大地坐标系；

第七步，判断稳定跟踪时间是否大于2秒，如果为“否”，则跳转至第八步；如果为“是”，则跳转至第九步；

第八步，并行进行以下操作步骤：

8.1、采用数字二阶环跟踪算法计算所述方位速度环和俯仰速度环的前馈补偿量，并叠加到光电跟踪仪的跟踪控制回路；

8.2、采用递推滤波跟踪控制方法计算大地坐标系下，坐标轴X_d、坐标轴Y_d、坐标轴Z_d方向的位置、速度以及加速度滤波值；

递推滤波跟踪控制方法的计算公式如下：

坐标轴X_d方向滤波公式：

Dx=Lb[0]+K₁·(Tmp[3]-Lb[0])

DxV=Lb[1]+K₂·(Tmp[3]-Lb[0])（1）

DxA=Lb[2]+K₃·(Tmp[3]-Lb[0])

坐标轴Y_d方向滤波公式：

Dy=Lb[3]+K₁·(Tmp[4]-Lb[3])

DyV=Lb[4]+K₂·(Tmp[4]-Lb[3])（2）

DyA=Lb[5]+K₃·(Tmp[4]-Lb[3])

坐标轴Z_d方向滤波公式：

Dz=Lb[6]+K₁·(Tmp[5]-Lb[6])

DzV=Lb[7]+K₂·(Tmp[5]-Lb[6])（3）

DzA=Lb[8]+K₃·(Tmp[5]-Lb[6])

递推公式：

Lb[0]=Dx+DxV·T+DxA·T²

Lb[1]=DxV+DxA·T

Lb[2]=DxA

Lb[3]=Dy+DyV·T+DyA·T²

Lb[4]=DyV+DyA·T（4）

Lb[5]=DyA

Lb[6]=Dz+DzV·T+DzA·T²

Lb[7]=DzV+DzA·T

Lb[8]=DzA

其中，Dx、DxV、DxA分别为大地坐标系下坐标轴X_d方向的位置、速度、加速度的滤波值；Dy、DyV、DyA分别为大地坐标系下坐标轴Y_d方向的位置、速度、加速度的滤波值；Dz、DzV、DzA分别为大地坐标系下坐标轴Z_d方向的位置、速度、加速度的滤波值；K₁、K₂、K₃均为暂态系数，取值范围为0.02≤K₁≤0.05，0.3≤K₂≤0.6，1.0≤K₃≤3.0；Tmp[3]、Tmp[4]、Tmp[5]分别为目标在大地坐标系下坐标轴X_d、坐标轴Y_d、坐标轴Z_d方向的直角坐标分量；Lb[0]～Lb[8]为滤波中间变量；

8.3、判断稳定跟踪时间是否大于2秒，如果为“否”，则重复执行第二步至8.2步的操作；如果为“是”，则跳转至第九步；

第九步，采用（1）～（4）式并用稳态系数K₄、K₅、K₆分别代替（1）～（3）式中的暂态系数K₁、K₂、K₃，计算稳态跟踪时大地坐标系下坐标轴X_d、坐标轴Y_d、坐标轴Z_d方向的位置、速度以及加速度滤波值，其中0.003≤K₄≤0.008，0.01≤K₅≤0.03，0.005≤K₆≤0.009；

第十步，计算瞄准线坐标系下，坐标轴X_m、坐标轴Y_m、坐标轴Z_m方向的位置滤波值，具体步骤如下：

10.1、利用参数H、P、R计算大地坐标系到车体坐标系的转换矩阵A₂，并利用转换矩阵A₂将大地坐标系下的位置滤波值(Dx,Dy,Dz)转换到车体坐标系；

10.2、利用参数θ计算车体坐标系到炮塔坐标系的转换矩阵Q₁，并利用转换矩阵Q₁将车体坐标系下坐标轴X_c、坐标轴Y_c、坐标轴Z_c方向的位置滤波值转换到炮塔坐标系；

10.3、利用光电坐标系原点与炮塔坐标系原点在三个坐标轴方向的固定偏差Ex、Ey、Ez，将炮塔坐标系下坐标轴X_p、坐标轴Y_p、坐标轴Z_p方向的位置滤波值转换到光电坐标系；

10.4、利用参数β、ε计算光电坐标系到瞄准线坐标系的转换矩阵L₁，并利用转换矩阵L₁将光电坐标系下坐标轴X_g、坐标轴Y_g、坐标轴Z_g方向的位置滤波值转换到瞄准线坐标系；

第十一步，计算瞄准线坐标系下坐标轴X_m、坐标轴Y_m、坐标轴Z_m方向的速度滤波值，具体步骤如下：

11.1、利用转换矩阵A₂将大地坐标系下的速度滤波值(DxV,DyV,DzV)转换到车体坐标系；

11.2、利用转换矩阵Q₁将车体坐标系下坐标轴X_c、坐标轴Y_c、坐标轴Z_c方向的速度滤波值转换到炮塔坐标系；

11.3、利用光电坐标系原点与炮塔坐标系原点在三个坐标轴方向的固定偏差Ex、Ey、Ez，将炮塔坐标系下坐标轴X_p、坐标轴Y_p、坐标轴Z_p方向的速度滤波值转换到光电坐标系；

11.4、利用转换矩阵L₁将光电坐标系下坐标轴X_g、坐标轴Y_g、坐标轴Z_g方向的速度滤波值转换到瞄准线坐标系；

第十二步，计算瞄准线坐标系下方位速度环和俯仰速度环控制回路中不含加速度分量的速度补偿量，计算公式如下：

Est_FwjV=-Tmp[9]/Tmp[12]

Est_FyjV=Tmp[10]/Tmp[13]

式中， $\mathrm{Tmp}\left[12\right]=\sqrt{{\left(\mathrm{Tmp}\right[6\left]\right)}^2+{\left(\mathrm{Tmp}\right[7\left]\right)}^2}$

$\mathrm{Tmp}\left[13\right]=\sqrt{{\left(\mathrm{Tmp}\right[6\left]\right)}^2+{\left(\mathrm{Tmp}\right[7\left]\right)}^2+{\left(\mathrm{Tmp}\right[8\left]\right)}^2}$

其中，Est_FwjV为光电跟踪仪在瞄准线坐标系下不含加速度分量的方位速度补偿量，Est_FyjV为光电跟踪仪在瞄准线坐标系下不含加速度分量的俯仰速度补偿量；Tmp[6]、Tmp[7]、Tmp[8]分别为瞄准线坐标系下坐标轴X_m、坐标轴Y_m、坐标轴Z_m方向的位置滤波值；Tmp[9]、Tmp[10]分别为瞄准线坐标系下坐标轴X_m、坐标轴Y_m方向的速度滤波值；Tmp[12]、Tmp[13]为中间变量；

第十三步，计算瞄准线坐标系下坐标轴X_m、坐标轴Y_m、坐标轴Z_m方向的加速度滤波值，具体步骤如下：

13.1、利用转换矩阵A₂将大地坐标系下的加速度滤波值(DxA,DyA,DzA)转换到车体坐标系；

13.2、利用转换矩阵Q₁将车体坐标系下坐标轴X_c、坐标轴Y_c、坐标轴Z_c方向的加速度滤波值转换到炮塔坐标系；

13.3、利用光电坐标系原点与炮塔坐标系原点在三个方向的固定偏差Ex、Ey、Ez，将炮塔坐标系下坐标轴X_p、坐标轴Y_p、坐标轴Z_p方向的加速度滤波值转换到光电坐标系；

13.4、利用转换矩阵L₁将光电坐标系下坐标轴X_g、坐标轴Y_g、坐标轴Z_g方向的加速度滤波值转换到瞄准线坐标系；

第十四步，计算瞄准线坐标系下方位、俯仰速度补偿量中的加速度分量，计算公式如下：

Est_FwjA=(2·Tmp[11]·Tmp[18]/Tmp[13]+Tmp[17]·Tmp[19]-Tmp[14]/Tmp[13])/L₂₃

Est_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]为瞄准线坐标系下坐标轴Z_m方向的速度滤波值；Tmp[14]、Tmp[15]分别为瞄准线坐标系下坐标轴X_m、坐标轴Y_m方向的加速度滤波值；L₂₃为转换矩阵L₁中第二行、第三列的元素；Tmp[17]～Tmp[19]为中间变量；

第十五步，计算目标跟踪时，瞄准线坐标系下方位速度环和俯仰速度环的最终前馈补偿参数，计算公式如下：

FwCon=(Est_FwjV+K₇·Est_FwjA)/L₃₃

FyCon=Est_FyjV+K₈·Est_FyjA

其中，FwCon为瞄准线坐标系下方位速度环的跟踪前馈补偿参数，FyCon为瞄准线坐标系下俯仰速度环的跟踪前馈补偿参数；K₇、K₈为修正系数，取值范围为0<K₇≤0.1，0<K₈≤0.1；L₃₃为转换矩阵L₁中第三行、第三列的元素；

第十六步，重复执行第二步到第十五步，实时计算光电跟踪仪跟踪目标时方位、俯仰速度环的前馈补偿量及跟踪参数，实现对高速运动目标的稳定跟踪。

本发明根据目标的特性，在自动跟踪开始的前2秒内采用数字二阶环跟踪算法进行跟踪，在跟踪时间达到2秒后，采用递推滤波跟踪控制方法进行目标跟踪。递推滤波跟踪控制方法利用光电跟踪仪、火炮、车载惯导***、视频***和激光测距机输出的相关参数，通过一系列坐标转换计算出目标在大地坐标系下的直角坐标分量，并将该分量代入大地坐标系下的递推迭代公式，计算出大地坐标系下的位置、速度和加速度滤波值；然后通过坐标反变换，将大地坐标系下的位置、速度和加速度滤波值转换到瞄准线坐标系，最终利用这些滤波值计算出目标跟踪时瞄准线坐标系下方位速度环和俯仰速度环的跟踪前馈补偿参数，并将这些参数叠加到伺服控制回路中，参与跟踪控制。由于这些前馈补偿参数含有位置、速度和加速度滤波的分量以及目标相对于光电跟踪仪的距离量，这些量都是目标跟踪过程中体现目标运动特性的关键参数，将这些参数处理后作为目标跟踪时的前馈补偿参数叠加到跟踪控制回路中，不仅提高了光电跟踪仪对高速运动目标的响应速度和跟踪能力，而且解决了传统跟踪方式对于高速来袭目标跟踪动态滞后较大的缺陷；同时，本发明在计算跟踪前馈补偿参数时，采用了运动载体和火炮的相关参数，对光电跟踪仪跟踪目标过程中运动载体和火炮带来的扰动进行了补偿，有效减小了外界扰动对光电跟踪仪跟踪稳定性和精度的影响，增加了***的抗外界扰动能力，提高了***的鲁棒性。由此可见，本发明不仅提高了光电跟踪仪对高速来袭目标的响应速度和跟踪精度，而且增强了光电跟踪仪目标跟踪过程中的抗外界扰动能力，这对于提高火控***的目标解算精度和整个武器***的作战效能都具有十分重要的意义。

附图说明

图1是本发明跟踪控制方法的工作流程图。

图2是火炮炮塔坐标系与车体坐标系的定义。

图3是光电跟踪仪光电坐标系的定义。

图4是车体坐标系与大地坐标系的定义。

图5是光电跟踪仪瞄准线坐标系的定义。

图6是采用本发明跟踪控制方法的模拟航路跟踪误差曲线。

图7是采用传统二阶环跟踪算法的模拟航路跟踪误差曲线。

具体实施方式

下面结合附图和优选实施例对本发明作进一步的详述。

本发明跟踪控制方法的优选实施例用于综合体形式的车载光电跟踪仪。光电跟踪仪为两轴两框架结构，分别为方位轴（框架）和俯仰轴（框架），俯仰框架安装于方位框架之上，激光测距机和视频探测器均安装于俯仰框架的光具座上，方位、俯仰框架分别安装各自的旋转变压器，方位速率陀螺和俯仰速率陀螺均安装在俯仰框架上，光电跟踪仪的瞄准线指向平行于各探测器的视轴；光电跟踪仪安装于炮塔之上，其瞄准线零位指向与火炮炮口零位指向一致；炮塔安装于车体之上，炮塔坐标系与车体坐标系的坐标原点重合；火炮方位、俯仰机构分别安装有各自的旋转变压器；车体的摇摆中心安装有惯导***，由于惯导***安装于车体摇摆中心，因此，惯导***输出的参数值均反映车体在大地坐标系下的运动参数；光电跟踪仪方位角和俯仰角的跟踪偏差量由视频***输出。

本发明提出的跟踪控制方法是通过存储有跟踪控制软件包的伺服计算机实现的，当伺服计算机上电后，跟踪控制软件包按照图1所示的工作流程执行以下步骤。

第一步，初始化伺服计算机各通讯端口、各变量寄存器。

第二步，以T为采样周期，实时采集当前时刻火炮方位旋转变压器输出的参数θ，光电跟踪仪方位、俯仰旋转变压器输出的参数(β,ε)、车载惯导***输出的参数(H,P,R)、视频***输出的参数(δ_β,δ_ε)、激光测距机输出的参数D，在本实施例中，采样周期T=1ms。

其中，θ为车体坐标系下的火炮方位角（参见图2），车体坐标系的原点O_c为车体的摇摆中心，Y_c轴平行于炮塔安装基面且指向车体前进方向；X_c轴平行于炮塔安装基面且垂直于Y_c轴，沿车体前进方向指向右侧；Z_c轴垂直于平面O_cX_cY_c且铅垂向上；炮塔方位角θ按左手定则定义即炮塔绕Z_c轴顺时针旋转为正。

β、ε分别为光电坐标系下光电跟踪仪瞄准线指向的方位角和俯仰角（参见图3），光电坐标系的原点O_g为方位旋转轴与俯仰旋转轴的交点，Y_g轴为光电跟踪仪瞄准线零位时的指向，其平行于光电跟踪仪的安装基面，X_g轴平行于光电跟踪仪的安装基面且垂直于Y_g轴，沿瞄准线零位方向指向右侧，Z_g轴垂直于平面O_gX_gY_g，铅垂向上；方位角β按左手定则定义即方位机构绕Z_g轴顺时针旋转为正，俯仰角ε以O_gX_gY_g平面为零位，向上抬起为正。

H、P、R分别为大地坐标系下车体的偏航角、纵摇角和横滚角（参见图4），大地坐标系按照“东-北-天”坐标系定义，原点O_d与车体坐标系的原点O_c重合，Y_d轴为车体的指北线，其平行于水平面指向正北，X_d轴平行于水平面指向为正东，Z_d轴垂直于平面O_dX_dY_d，铅垂向上，其中偏航角是“东-北”平面上的指北线与车体航向线之间的夹角，顺时针为正；纵摇角是铅垂平面上车体纵轴与“东-北”平面的夹角，车头抬起为正；横摇角是车体绕Y_d轴旋转时的旋转角，X_d轴平行于“东-北”平面时为0，左侧抬起为正。

δ_β、δ_ε分别为瞄准线坐标系下光电跟踪仪方位角和俯仰角的跟踪偏差量（参见图5），瞄准线坐标系的原点O_m与光电坐标系的原点O_g重合，为光电跟踪仪方位旋转轴与俯仰旋转轴的交点，Y_m轴为光电跟踪仪的跟踪瞄准线，指向目标方向，X_m轴为光电跟踪仪的俯仰轴，平行于光电跟踪仪的安装基面且垂直于Y_m轴，沿Y_m轴方向指向右侧，Z_m轴垂直于平面O_mX_mY_m，铅垂向上；方位角跟踪偏差量δ_β按右手定则定义即方位机构绕Z_m轴顺时针旋转为负，俯仰角跟踪偏差量δ_ε以O_mX_mY_m平面为零位，向上抬起为负。由于光电跟踪仪在实际目标跟踪过程中存在跟踪误差，因此瞄准线通常不通过目标。

D为目标相对于瞄准线坐标系原点的距离。

第三步，读取上级***发出的控制指令，根据控制指令判断光电跟踪仪是否停止跟踪，如果为“是”，则跳转至第四步；如果为“否”，则跳转至第五步。

第四步，向方位、俯仰位置环发送保持当前位置指令，方位、俯仰机构根据各自的控制回路，使光电跟踪仪停止跟踪，保持在当前位置，转入停当前状态，然后跳转至第三步，等待其它指令。

第五步，计算当前时刻目标M在瞄准线坐标系下的直角坐标分量Tmp₁[0]'、Tmp₁[1]'、Tmp₁[2]'（参见图5），计算公式如下：

Tmp₁[0]'=D·cos(δ_ε)·sin(δ_β)

Tmp₁[1]'=D·cos(δ_ε)·cos(δ_β)

Tmp₁[2]'=D·sin(δ_ε)

其中，Tmp₁[0]'、Tmp₁[1]'、Tmp₁[2]'分别为当前时刻目标M在瞄准线坐标系下X_m轴方向、Y_m轴方向、Z_m轴方向的直角坐标分量。

第六步，将目标M在瞄准线坐标系下的直角坐标分量Tmp₁[0]'、Tmp₁[1]'、Tmp₁[2]'转换到大地坐标系。

6.1、将目标M在瞄准线坐标系下的直角坐标分量Tmp₁[0]'、Tmp₁[1]'、Tmp₁[2]'转换到光电坐标系，计算公式为：E₁=N₁·F₁。

式中， $E_1=\left[\begin{array}{c}{\mathrm{Tmp}}_1{\left[0\right]}^{\&prime;\&prime;}\\ {\mathrm{Tmp}}_1{\left[1\right]}^{\&prime;\&prime;}\\ {\mathrm{Tmp}}_1{\left[2\right]}^{\&prime;\&prime;}\end{array}\right],$ $N_1=\left[\begin{array}{ccc}{N}_{11}& N_{12}& N_{13}\\ N_{21}& N_{22}& N_{23}\\ N_{31}& N_{32}& N_{33}\end{array}\right],$ $F_1=\left[\begin{array}{c}{\mathrm{Tmp}}_1{\left[0\right]}^{\&prime;}\\ {\mathrm{Tmp}}_1{\left[1\right]}^{\&prime;}\\ {\mathrm{Tmp}}_1{\left[2\right]}^{\&prime;}\end{array}\right].$

矩阵N₁的九项元素分别为：N₁₁=cos(β)，N₁₂=sin(β)cos(ε)，N₁₃=-sin(β)sin(ε)，N₂₁=-sin(β)，N₂₂=cos(β)cos(ε)，N₂₃=-cos(β)sin(ε)，N₃₁=0，N₃₂=sin(ε)，N₃₃=cos(ε)。

矩阵E₁的三项元素计算结果如下：

Tmp₁[0]''=Tmp₁[0]'·N₁₁+Tmp₁[1]'·N₁₂+Tmp₁[2]'·N₁₃

Tmp₁[1]''=Tmp₁[0]'·N₂₁+Tmp₁[1]'·N₂₂+Tmp₁[2]'·N₂₃

Tmp₁[2]''=Tmp₁[0]'·N₃₁+Tmp₁[1]'·N₃₂+Tmp₁[2]'·N₃₃

其中，E₁为当前时刻目标M在光电坐标系下的直角坐标分量矩阵，Tmp₁[0]''、Tmp₁[1]''、Tmp₁[2]''分别为目标M在光电坐标系下X_g轴方向、Y_g轴方向、Z_g轴方向的直角坐标分量；N₁为瞄准线坐标系到光电坐标系的转换矩阵；F₁为当前时刻目标M在瞄准线坐标系下的直角坐标分量矩阵。

6.2、将目标M在光电坐标系下的直角坐标分量Tmp₁[0]''、Tmp₁[1]''、Tmp₁[2]''转换到炮塔坐标系，计算公式如下：

Tmp₁[0]=Tmp₁[0]''+Ex=Tmp₁[0]'·N₁₁+Tmp₁[1]'·N₁₂+Tmp₁[2]'·N₁₃+Ex

Tmp₁[1]=Tmp₁[1]''+Ey=Tmp₁[0]'·N₂₁+Tmp₁[1]'·N₂₂+Tmp₁[2]'·N₂₃+Ey

Tmp₁[2]=Tmp₁[2]''+Ez=Tmp₁[0]'·N₃₁+Tmp₁[1]'·N₃₂+Tmp₁[2]'·N₃₃+Ez

其中，Ex、Ey、Ez为光电坐标系向炮塔坐标系转换时X_p、Y_p、Z_p三方向的转换参数；Tmp₁[0]、Tmp₁[1]、Tmp₁[2]分别为当前时刻目标M在炮塔坐标系下X_p轴方向、Y_p轴方向、Z_p轴方向的直角坐标分量。

由于光电跟踪仪安装在炮塔之上，光电跟踪仪的瞄准线零位与炮口零位指向一致，因此光电跟踪仪的光电坐标系与炮塔坐标系的坐标原点不重合，但各轴平行，即光电坐标系与炮塔坐标系之间的坐标变换只涉及坐标平移，因此需要得到光电坐标系与炮塔坐标系坐标原点之间的偏差，该偏差是光电跟踪仪在炮塔上安装的固定偏差。按照光电跟踪仪和炮塔的结构设计参数，炮塔总体设计单位可以提供Ex、Ey、Ez的具体数值。

6.3、将目标M在炮塔坐标系下的直角坐标分量Tmp₁[0]、Tmp₁[1]、Tmp₁[2]转换到车体坐标系，计算公式为：B₁=P₁·T₁。

式中， $B_1=\left[\begin{array}{c}\mathrm{Tmp}\left[0\right]\\ \mathrm{Tmp}\left[1\right]\\ \mathrm{Tmp}\left[2\right]\end{array}\right],$ $P_1=\left[\begin{array}{ccc}{P}_{11}& P_{12}& P_{13}\\ P_{21}& P_{22}& P_{23}\\ P_{31}& P_{32}& P_{33}\end{array}\right],$ $T_1=\left[\begin{array}{c}\mathrm{Tm}{p}_1\left[0\right]\\ \mathrm{Tm}{p}_1\left[1\right]\\ \mathrm{Tm}{p}_1\left[2\right]\end{array}\right].$

矩阵P₁的九项元素分别为：P₁₁=cos(θ)，P₁₂=sin(θ)，P₁₃=0，P₂₁=-sin(θ)，P₂₂=cos(θ)，P₂₃=0，P₃₁=0，P₃₂=0，P₃₃=1。

矩阵B₁的三项元素计算结果如下：

Tmp[0]=Tmp₁[0]·P₁₁+Tmp₁[1]·P₁₂+Tmp₁[2]·P₁₃

Tmp[1]=Tmp₁[0]·P₂₁+Tmp₁[1]·P₂₂+Tmp₁[2]·P₂₃

Tmp[2]=Tmp₁[0]·P₃₁+Tmp₁[1]·P₃₂+Tmp₁[2]·P₃₃

其中，B₁为当前时刻目标M在车体坐标系下的直角坐标分量矩阵，Tmp[0]、Tmp[1]、Tmp[2]分别为目标M在车体坐标系下X_c轴方向、Y_c轴方向、Z_c轴方向的直角坐标分量；P₁为炮塔坐标系到车体坐标系的转换矩阵；T₁为当前时刻目标M在炮塔坐标系下的直角坐标分量矩阵。

本实施例中，炮塔安装于车体之上，炮塔坐标系与车体坐标系的坐标原点重合（参见图2），炮塔坐标系的原点O_p为车体的摇摆中心，Y_p轴平行于炮塔安装基面且为炮口高低角为零时的指向；X_p轴平行于炮塔安装基面且垂直于Y_p轴，沿炮口方向指向右侧；Z_p轴垂直于平面O_pX_pY_p，铅垂向上。炮塔坐标系O_pX_pY_pZ_p与车体坐标系O_cX_cY_cZ_c之间仅存在火炮方位角θ的差异，当θ=0时，两坐标系重合，因此炮塔坐标系与车体坐标系之间的坐标转换只涉及到火炮方位角θ。

6.4、将目标M在车体坐标系下的直角坐标分量Tmp[0]、Tmp[1]、Tmp[2]转换到大地坐标系，计算公式为：C₁=A₁·B₁。

式中， $C_1=\left[\begin{array}{c}\mathrm{Tmp}\left[3\right]\\ \mathrm{Tmp}\left[4\right]\\ \mathrm{Tmp}\left[5\right]\end{array}\right],$ $A_1=\left[\begin{array}{ccc}{X}_{11}& X_{12}& X_{13}\\ X_{21}& X_{22}& X_{23}\\ X_{31}& X_{32}& X_{33}\end{array}\right],$ $B_1=\left[\begin{array}{c}\mathrm{Tmp}\left[0\right]\\ \mathrm{Tmp}\left[1\right]\\ \mathrm{Tmp}\left[2\right]\end{array}\right].$

矩阵A₁的九项元素计算公式如下：

X₁₁=cos(R)cos(H)+sin(R)sin(P)sin(H)

X₁₂=cos(P)sin(H)

X₁₃=sin(R)cos(H)-cos(R)sin(P)sin(H)

X₂₁=-cos(R)sin(H)+sin(R)sin(P)cos(H)

X₂₂=cos(P)cos(H)

X₂₃=-sin(R)sin(H)-cos(R)sin(P)cos(H)

X₃₁=-sin(R)cos(P)

X₃₂=sinP

X₃₃=cos(R)cos(P)

矩阵C₁的三项元素计算结果如下：

Tmp[3]=Tmp[0]·X₁₁+Tmp[1]·X₁₂+Tmp[2]·X₁₃

Tmp[4]=Tmp[0]·X₂₁+Tmp[1]·X₂₂+Tmp[2]·X₂₃

Tmp[5]=Tmp[0]·X₃₁+Tmp[1]·X₃₂+Tmp[2]·X₃₃

其中，C₁为当前时刻目标M在大地坐标系下的直角坐标分量矩阵，Tmp[3]、Tmp[4]、Tmp[5]分别为目标M在大地坐标系下X_d轴方向、Y_d轴方向、Z_d轴方向的直角坐标分量；A₁为车体坐标系到大地坐标系的转换矩阵；B₁为当前时刻目标M在车体坐标系下的直角坐标分量矩阵。

第七步，判断稳定跟踪时间是否大于2秒，如果为“否”，则跳转至第八步；如果为“是”，则跳转至第九步。

2秒计时采用外部中断计数实现，伺服计算机每接收到一次时钟中断，跟踪控制软件中的接收计数变量加1，由于本实施例采用1ms的时钟中断，因此当接收计数变量从0累加到2000时，表明2秒计时时间到。

第八步，进入数字二阶环跟踪模式，同时利用暂态系数(K₁,K₂,K₃)计算大地坐标系下X_d、Y_d、Z_d三方向的位置(Dx,Dy,Dz)、速度(DxV,DyV,DzV)以及加速度滤波值(DxA,DyA,DzA)，直到稳定跟踪时间大于2秒。

8.1、采用数字二阶环跟踪算法计算方位、俯仰速度环的前馈补偿量，并叠加到跟踪控制回路。

由于大地坐标系下X_d、Y_d、Z_d三方向的位置、速度和加速度的滤波值采用递推迭代法计算，因此需要有一定的过渡时间，即计算收敛时间，通常取2～3秒，本实施例设定2秒为计算收敛时间。

在光电跟踪仪刚进入跟踪状态且稳定跟踪目标的时间未达到2秒时，由于迭代计算的滤波参数没有收敛，而不能作为跟踪控制参数进行目标跟踪，因此该时间段内采用数字二阶环算法计算跟踪控制量，并将计算结果作为方位、俯仰速度环的前馈补偿参数，输入到控制回路中，完成对目标的跟踪；通常在跟踪起始段内，目标距离较远，因此目标相对于光电跟踪仪的运动速度较慢，二阶环跟踪模式完全能够满足跟踪精度要求。而在稳定跟踪目标时间超过2秒钟后，则需采用递推滤波跟踪控制方法实现目标跟踪。

8.2、利用暂态系数(K₁,K₂,K₃)，并采用递推滤波跟踪控制方法计算大地坐标系下X_d、Y_d、Z_d三方向的位置(Dx,Dy,Dz)、速度(DxV,DyV,DzV)以及加速度滤波值(DxA,DyA,DzA)。

虽然在稳定跟踪2秒后，才使用递推滤波跟踪控制方法进行跟踪控制，但由于该方法采用递推迭代形式进行参数计算，因此，在采用数字二阶环跟踪模式时，一直在进行滤波参数的迭代计算，但其结果并不参与跟踪控制，只是作为后续迭代计算跟踪控制参数的基础。

在进行大地坐标系下X_d、Y_d、Z_d三方向位置、速度、加速度递推滤波计算时，需要用到一组系数，即暂态系数(K₁,K₂,K₃)。该组系数是根据α-β-γ滤波算法以及试验修正确定的，其取值范围如下：0.02≤K₁≤0.05，0.3≤K₂≤0.6，1.0≤K₃≤3.0。在本实施例中，暂态系数的取值分别为：K₁=0.04113、K₂=0.57580、K₃=2.68704；该组系数经过多套***的试验验证，适用于两轴光电跟踪仪，可在其它两轴***中直接使用，也可根据实际情况在取值范围内进行修正。

递推滤波跟踪控制方法的计算公式如下：

X_d方向滤波公式：

Dx=Lb[0]+K₁·(Tmp[3]-Lb[0])

DxV=Lb[1]+K₂·(Tmp[3]-Lb[0])

DxA=Lb[2]+K₃·(Tmp[3]-Lb[0])

Y_d方向滤波公式：

Dy=Lb[3]+K₁·(Tmp[4]-Lb[3])

DyV=Lb[4]+K₂·(Tmp[4]-Lb[3])

DyA=Lb[5]+K₃·(Tmp[4]-Lb[3])

Z_d方向滤波公式：

Dz=Lb[6]+K₁·(Tmp[5]-Lb[6])

DzV=Lb[7]+K₂·(Tmp[5]-Lb[6])

DzA=Lb[8]+K₃·(Tmp[5]-Lb[6])

递推公式：

Lb[0]=Dx+DxV·T+DxA·T²

Lb[1]=DxV+DxA·T

Lb[2]=DxA

Lb[3]=Dy+DyV·T+DyA·T²

Lb[4]=DyV+DyA·T

Lb[5]=DyA

Lb[6]=Dz+DzV·T+DzA·T²

Lb[7]=DzV+DzA·T

Lb[8]=DzA

其中，Dx、DxV、DxA分别为大地坐标系下X_d方向的位置、速度、加速度的滤波值；Dy、DyV、DyA分别为大地坐标系下Y_d方向的位置、速度、加速度的滤波值；Dz、DzV、DzA分别为大地坐标系下Z_d方向的位置、速度、加速度的滤波值；Lb[0]～Lb[8]为滤波中间变量。

8.3、判断稳定跟踪时间是否大于2秒，如果为“否”，则重复执行第二步至8.2步的操作；如果为“是”，则跳转至第九步。

第九步，利用稳态系数(K₄,K₅,K₆)，并采用递推滤波跟踪控制方法计算稳态跟踪时，大地坐标系下X_d、Y_d、Z_d三方向的位置(Dx,Dy,Dz)、速度(DxV,DyV,DzV)以及加速度滤波值(DxA,DyA,DzA)。

当稳定跟踪目标时间超过2秒时，则采用递推滤波跟踪控制方法计算跟踪参数，此时，需要用到一组系数，即稳态系数(K₄,K₅,K₆)。该组系数与暂态系数(K₁,K₂,K₃)一样，也是根据α-β-γ滤波算法以及试验修正确定的，其取值范围如下：0.003≤K₄≤0.008，0.01≤K₅≤0.03，0.005≤K₆≤0.009。在本实施例中，稳态系数的取值分别为：K₄=0.005982、K₅=0.011964、K₆=0.00796；该组稳态系数与暂态系数一样，经过多套***的试验验证，适用于两轴光电跟踪仪，可在其它两轴***中直接使用，也可根据实际情况在取值范围内进行修正。

此时，递推滤波跟踪控制方法的计算公式与第8.2步的计算公式形式上完全一样，只需要把第8.2步公式中的暂态系数(K₁,K₂,K₃)对应的更换为稳态系数(K₄,K₅,K₆)即可。

计算开始时，X_d、Y_d、Z_d三方向的位置、速度以及加速度滤波计算的初值分别为2秒计时时间到的那一刻，第8.2步计算出的大地坐标系下X_d、Y_d、Z_d三方向的位置、速度以及加速度滤波值。

第十步，计算瞄准线坐标系下X_m、Y_m、Z_m三方向的位置滤波值。

10.1、将大地坐标系下的位置滤波值(Dx,Dy,Dz)转换到车体坐标系，计算公式为C₂=A₂·B₂。

式中， $C_2=\left[\begin{array}{c}\mathrm{Tm}{p}_1\left[6\right]\\ \mathrm{Tm}{p}_1\left[7\right]\\ \mathrm{Tm}{p}_1\left[8\right]\end{array}\right],$ $A_1=\left[\begin{array}{ccc}{Y}_{11}& Y_{12}& Y_{13}\\ Y_{21}& Y_{22}& Y_{23}\\ Y_{31}& Y_{32}& Y_{33}\end{array}\right],$ $B_2=\left[\begin{array}{c}\mathrm{Dx}\\ \mathrm{Dy}\\ \mathrm{Dz}\end{array}\right].$

矩阵A₂的九项元素计算公式如下：

Y₁₁=cos(R)cos(H)+sin(R)sin(P)sin(H)

Y₁₂=-cos(R)sin(H)+sin(R)sin(P)cos(H)

Y₁₃=-sin(R)cos(P)

Y₂₁=cos(P)sin(H)

Y₂₂=cos(P)cos(H)

Y₂₃=sinP

Y₃₁=sin(R)cos(H)-cos(R)sin(P)sin(H)

Y₃₂=-sin(R)sin(H)-cos(R)sin(P)cos(H)

Y₃₃=cos(R)cos(P)

矩阵C₂的三项元素计算结果如下：

Tmp₁[6]=Dx·Y₁₁+Dy·Y₁₂+Dz·Y₁₃

Tmp₁[7]=Dx·Y₂₁+Dy·Y₂₂+Dz·Y₂₃

Tmp₁[8]=Dx·Y₃₁+Dy·Y₃₂+Dz·Y₃₃

其中，C₂为车体坐标系下X_c、Y_c、Z_c三方向的位置滤波值矩阵，Tmp₁[6]、Tmp₁[7]、Tmp₁[8]分别为车体坐标系下位置滤波值在X_c轴方向、Y_c轴方向、Z_c轴方向的直角坐标分量；A₂为大地坐标系到车体坐标系的转换矩阵；B₂为大地坐标系下X_d、Y_d、Z_d三方向的位置滤波值矩阵。

10.2、将车体坐标系下X_c、Y_c、Z_c三方向的位置滤波值Tmp₁[6]、Tmp₁[7]、Tmp₁[8]转换到炮塔坐标系，计算公式如下：T₂=Q₁·C₂。

式中， $T_2=\left[\begin{array}{c}\mathrm{Tmp}{\left[6\right]}^{\&prime;}\\ \mathrm{Tmp}{\left[7\right]}^{\&prime;}\\ \mathrm{Tmp}{\left[8\right]}^{\&prime;}\end{array}\right],$ $Q_1=\left[\begin{array}{ccc}{Q}_{11}& Q_{12}& Q_{13}\\ Q_{21}& Q_{22}& Q_{23}\\ Q_{31}& Q_{32}& Q_{33}\end{array}\right],$ $C_2=\left[\begin{array}{c}\mathrm{Tm}{p}_1\left[6\right]\\ \mathrm{Tm}{p}_1\left[7\right]\\ \mathrm{Tm}{p}_1\left[8\right]\end{array}\right].$

矩阵Q₁的九项元素分别为：Q₁₁=cos(θ)，Q₁₂=-sin(θ)，Q₁₃=0，Q₂₁=sin(θ)，Q₂₂=cos(θ)，Q₂₃=0，Q₃₁=0，Q₃₂=0，Q₃₃=1。

矩阵T₂的三项元素计算结果如下：

Tmp[6]'=Tmp₁[6]·Q₁₁+Tmp₁[7]·Q₁₂+Tmp₁[8]·Q₁₃

Tmp[7]'=Tmp₁[6]·Q₂₁+Tmp₁[7]·Q₂₂+Tmp₁[8]·Q₂₃

Tmp[8]'=Tmp₁[6]·Q₃₁+Tmp₁[7]·Q₃₂+Tmp₁[8]·Q₃₃

其中，T₂为炮塔坐标系下X_p、Y_p、Z_p三方向的位置滤波值矩阵，Tmp[6]'、Tmp[7]'、Tmp[8]'分别为炮塔坐标系下位置滤波值在X_p轴方向、Y_p轴方向、Z_p轴方向的直角坐标分量；Q₁为车体坐标系到炮塔坐标系的转换矩阵；C₂为车体坐标系下X_c、Y_c、Z_c三方向的位置滤波值矩阵。

10.3、将炮塔坐标系下X_p、Y_p、Z_p三方向的位置滤波值Tmp[6]'、Tmp[7]'、Tmp[8]'转换到光电坐标系，计算结果如下：

Tmp[6]''=Tmp[6]'-Ex=Tmp₁[6]·Q₁₁+Tmp₁[7]·Q₁₂+Tmp₁[8]·Q₁₃-Ex

Tmp[7]''=Tmp[7]'-Ey=Tmp₁[6]·Q₂₁+Tmp₁[7]·Q₂₂+Tmp₁[8]·Q₂₃-Ey

Tmp[8]''=Tmp[8]'-Ez=Tmp₁[6]·Q₃₁+Tmp₁[7]·Q₃₂+Tmp₁[8]·Q₃₃-Ez

其中，Tmp[6]''、Tmp[7]''、Tmp[8]''分别为光电坐标系下X_g、Y_g、Z_g三方向的位置滤波值；Ex、Ey、Ez分别为光电坐标系与炮塔坐标系的坐标原点在X_p、Y_p、Z_p三方向的固定偏差。

10.4、将光电坐标系下X_g、Y_g、Z_g三方向的位置滤波值Tmp[6]''、Tmp[7]''、Tmp[8]''转换到瞄准线坐标系，计算公式如下：T₃=L₁·C₃。

式中， $T_3=\left[\begin{array}{c}\mathrm{Tmp}\left[6\right]\\ \mathrm{Tmp}\left[7\right]\\ \mathrm{Tmp}\left[8\right]\end{array}\right],$ $L_1=\left[\begin{array}{ccc}{L}_{11}& L_{12}& L_{13}\\ L_{21}& L_{22}& L_{23}\\ L_{31}& L_{32}& L_{33}\end{array}\right],$ $C_3=\left[\begin{array}{c}\mathrm{Tmp}{\left[6\right]}^{\&prime;\&prime;}\\ \mathrm{Tmp}{\left[7\right]}^{\&prime;\&prime;}\\ \mathrm{Tmp}{\left[8\right]}^{\&prime;\&prime;}\end{array}\right].$

矩阵L₁的九项元素分别为：L₁₁=cos(β)，L₁₂=-sin(β)，L₁₃=0，L₂₁=sin(β)cos(ε)，L₂₂=cos(β)cos(ε)，L₂₃=sin(ε)，L₃₁=-sin(β)sin(ε)，L₃₂=-cos(β)sin(ε)，L₃₃=cos(ε)。

矩阵T₃的三项元素计算结果如下：

Tmp[6]=Tmp[6]''·L₁₁+Tmp[7]''·L₁₂+Tmp[8]''·L₁₃

Tmp[7]=Tmp[6]''·L₂₁+Tmp[7]''·L₂₂+Tmp[8]''·L₂₃

Tmp[8]=Tmp[6]''·L₃₁+Tmp[7]''·L₃₂+Tmp[8]''·L₃₃

其中，T₃为瞄准线坐标系下位置滤波值的直角坐标分量矩阵，Tmp[6]、Tmp[7]、Tmp[8]分别为瞄准线坐标系下X_m、Y_m、Z_m三方向的位置滤波值；L₁为光电坐标系到瞄准线坐标系的转换矩阵；C₃为光电坐标系下X_g、Y_g、Z_g三方向位置滤波值的直角坐标分量矩阵。

第十一步，计算瞄准线坐标系下X_m、Y_m、Z_m三方向的速度滤波值。

11.1、利用转换矩阵A₂将大地坐标系下的速度滤波值(DxV,DyV,DzV)转换到车体坐标系，计算结果如下：

Tmp₁[9]=DxV·Y₁₁+DyV·Y₁₂+DzV·Y₁₃

Tmp₁[10]=DxV·Y₂₁+DyV·Y₂₂+DzV·Y₂₃

Tmp₁[11]=DxV·Y₃₁+DyV·Y₃₂+DzV·Y₃₃

其中，Tmp₁[9]、Tmp₁[10]、Tmp₁[11]分别为车体坐标系下X_c、Y_c、Z_c三方向的速度滤波值。

11.2、利用转换矩阵Q₁将车体坐标系下X_c、Y_c、Z_c三方向的速度滤波值Tmp₁[9]、Tmp₁[10]、Tmp₁[11]转换到炮塔坐标系，计算结果如下：

Tmp[9]'=Tmp₁[9]·Q₁₁+Tmp₁[10]·Q₁₂+Tmp₁[11]·Q₁₃

Tmp[10]'=Tmp₁[9]·Q₂₁+Tmp₁[10]·Q₂₂+Tmp₁[11]·Q₂₃

Tmp[11]'=Tmp₁[9]·Q₃₁+Tmp₁[10]·Q₃₂+Tmp₁[11]·Q₃₃

其中，Tmp[9]'、Tmp[10]'、Tmp[11]'分别为炮塔坐标系下X_p、Y_p、Z_p三方向的速度滤波值。

11.3、利用光电坐标系与炮塔坐标系的坐标原点在X_p、Y_p、Z_p三方向的固定偏差Ex、Ey、Ez，将炮塔坐标系下X_p、Y_p、Z_p三方向的速度滤波值Tmp[9]'、Tmp[10]'、Tmp[11]'转换到光电坐标系，计算结果如下：

Tmp[9]''=Tmp[9]'-Ex=Tmp₁[9]·Q₁₁+Tmp₁[10]·Q₁₂+Tmp₁[11]·Q₁₃-Ex

Tmp[10]''=Tmp[10]'-Ey=Tmp₁[9]·Q₂₁+Tmp₁[10]·Q₂₂+Tmp₁[11]·Q₂₃-Ey

Tmp[11]''=Tmp[11]'-Ez=Tmp₁[9]·Q₃₁+Tmp₁[10]·Q₃₂+Tmp₁[11]·Q₃₃-Ez

其中，Tmp[9]''、Tmp[10]''、Tmp[11]''分别为光电坐标系下X_g、Y_g、Z_g三方向的速度滤波值。

11.4、利用转换矩阵L₁将光电坐标系下X_g、Y_g、Z_g三方向的速度滤波值Tmp[9]''、Tmp[10]''、Tmp[11]''转换到瞄准线坐标系，计算结果如下：

Tmp[9]=Tmp[9]''·L₁₁+Tmp[10]''·L₁₂+Tmp[11]''·L₁₃

Tmp[10]=Tmp[9]''·L₂₁+Tmp[10]''·L₂₂+Tmp[11]''·L₂₃

Tmp[11]=Tmp[9]''·L₃₁+Tmp[10]''·L₃₂+Tmp[11]''·L₃₃

其中，Tmp[9]、Tmp[10]、Tmp[11]分别为瞄准线坐标系下X_m、Y_m、Z_m三方向的速度滤波值。

第十二步，计算瞄准线坐标系下方位速度环、俯仰速度环不含加速度分量的速度补偿量，计算公式如下：

Est_FwjV=-Tmp[9]/Tmp[12]

Est_FyjV=Tmp[10]/Tmp[13]

式中， $\mathrm{Tmp}\left[12\right]=\sqrt{{\left(\mathrm{Tmp}\right[6\left]\right)}^2+{\left(\mathrm{Tmp}\right[7\left]\right)}^2}$

$\mathrm{Tmp}\left[13\right]=\sqrt{{\left(\mathrm{Tmp}\right[6\left]\right)}^2+{\left(\mathrm{Tmp}\right[7\left]\right)}^2+{\left(\mathrm{Tmp}\right[8\left]\right)}^2}$

其中，Est_FwjV为光电跟踪仪在瞄准线坐标系下不含加速度分量的方位速度补偿量，Est_FyjV为光电跟踪仪在瞄准线坐标系下不含加速度分量的俯仰速度补偿量；Tmp[12]、Tmp[13]为中间变量。

第十三步，计算瞄准线坐标系下X_m、Y_m、Z_m三方向的加速度滤波值。

13.1、利用转换矩阵A₂将大地坐标系下的加速度滤波值(DxA,DyA,DzA)转换到车体坐标系，计算结果如下：

Tmp₁[14]=DxA·Y₁₁+DyA·Y₁₂+DzA·Y₁₃

Tmp₁[15]=DxA·Y₂₁+DyA·Y₂₂+DzA·Y₂₃

Tmp₁[16]=DxA·Y₃₁+DyA·Y₃₂+DzA·Y₃₃

其中，Tmp₁[14]、Tmp₁[15]、Tmp₁[16]分别为车体坐标系下X_c、Y_c、Z_c三方向的加速度滤波值。

13.2、利用转换矩阵Q₁将车体坐标系下X_c、Y_c、Z_c三方向的加速度滤波值Tmp₁[14]、Tmp₁[15]、Tmp₁[16]转换到炮塔坐标系，计算结果如下：

Tmp[14]'=Tmp₁[14]·Q₁₁+Tmp₁[15]·Q₁₂+Tmp₁[16]·Q₁₃

Tmp[15]'=Tmp₁[14]·Q₂₁+Tmp₁[15]·Q₂₂+Tmp₁[16]·Q₂₃

Tmp[16]'=Tmp₁[14]·Q₃₁+Tmp₁[15]·Q₃₂+Tmp₁[16]·Q₃₃

其中，Tmp[14]'、Tmp[15]'、Tmp[16]'分别为炮塔坐标系下X_p、Y_p、Z_p三方向的加速度滤波值。

13.3、利用光电坐标系与炮塔坐标系的坐标原点在X_p、Y_p、Z_p三方向的固定偏差Ex、Ey、Ez，将炮塔坐标系下X_p、Y_p、Z_p三方向的加速度滤波值Tmp[14]'、Tmp[15]'、Tmp[16]'转换到光电坐标系，计算结果如下：

Tmp[14]''=Tmp[14]'-Ex=Tmp₁[14]·Q₁₁+Tmp₁[15]·Q₁₂+Tmp₁[16]·Q₁₃-Ex

Tmp[15]''=Tmp[15]'-Ey=Tmp₁[14]·Q₂₁+Tmp₁[15]·Q₂₂+Tmp₁[16]·Q₂₃-Ey

Tmp[16]''=Tmp[16]'-Ez=Tmp₁[14]·Q₃₁+Tmp₁[15]·Q₃₂+Tmp₁[16]·Q₃₃-Ez

其中，Tmp[14]''、Tmp[15]''、Tmp[16]''分别为光电坐标系下X_g、Y_g、Z_g三方向的加速度滤波值。

13.4、利用转换矩阵L₁将光电坐标系下X_g、Y_g、Z_g三方向的加速度滤波值Tmp[14]''、Tmp[15]''、Tmp[16]''转换到瞄准线坐标系，计算结果如下：

Tmp[14]=Tmp[14]''·L₁₁+Tmp[15]''·L₁₂+Tmp[16]''·L₁₃

Tmp[15]=Tmp[14]''·L₂₁+Tmp[15]''·L₂₂+Tmp[16]''·L₂₃

Tmp[16]=Tmp[14]''·L₃₁+Tmp[15]''·L₃₂+Tmp[16]''·L₃₃

其中，Tmp[14]、Tmp[15]、Tmp[16]分别为瞄准线坐标系下X_m、Y_m、Z_m三方向的加速度滤波值。

第十四步、计算瞄准线坐标系下方位、俯仰速度补偿量中的加速度分量，计算公式如下：

Est_FwjA=(2·Tmp[11]·Tmp[18]/Tmp[13]+Tmp[17]·Tmp[19]-Tmp[14]/Tmp[13])/L₂₃

Est_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[17]～Tmp[19]为中间变量。

第十五步，利用光电跟踪仪在瞄准线坐标系下不含加速度分量的方位、俯仰速度补偿量Est_FwjV、Est_FyjV和方位、俯仰速度补偿量中的加速度分量Est_FwjA、Est_FyjA，计算目标跟踪时，瞄准线坐标系下方位速度环和俯仰速度环的最终跟踪前馈补偿参数，计算公式如下：

FwCon=(Est_FwjV+K₇·Est_FwjA)/L₃₃

FyCon=Est_FyjV+K₈·Est_FyjA

其中，FwCon为瞄准线坐标系下方位速度环的跟踪前馈补偿参数，FyCon为瞄准线坐标系下俯仰速度环的跟踪前馈补偿参数。这两个参数分别作为方位、俯仰速度环的前馈量叠加到跟踪控制回路中，对跟踪过程中，方位、俯仰的速度环参数进行修正和补偿。

公式中有两个修正系数K₇和K₈，其取值范围如下：0<K₇≤0.1，0<K₈≤0.1，该系数针对不同的***可以进行修改，可通过模拟跟踪航路测试或对真实目标的动飞试验确定。在模拟航路测试或动飞试验过程中，可以通过对跟踪数据的分析来调整和确定修正系数K₇和K₈，方位、俯仰回路可按照以下规律进行调整：当方位回路跟踪滞后时，增大系数K₇的取值，当俯仰回路跟踪滞后时，增大系数K₈的取值；当方位回路跟踪超前时，减小K₇的取值，当俯仰回路跟踪超前时，减小系数K₈的取值；最终在***误差指标范围内确定合适的系数。本实施例分别确定为K₇=0.03、K₈=0.01。

第十六步，重复执行第二步到第十五步，实时计算光电跟踪仪跟踪目标时方位、俯仰速度环的前馈补偿量及跟踪参数，实现对高速运动目标的稳定跟踪。

利用本发明提出的跟踪控制方法进行模拟航路测试，对于跟踪航路高度300米，航路捷径500米，目标速度900米的高速来袭目标，全航路统计，跟踪***误差：方位为0.08mrad，俯仰为0.05mrad，跟踪随机误差：方位为0.11mrad，俯仰0.08mard，误差曲线如图6所示。而采用传统二阶环跟踪方式，对于同一条航路，全航路统计，跟踪***误差：方位为0.15mrad，俯仰为0.11mrad，跟踪随机误差：方位为0.19mrad，俯仰0.15mard，误差曲线如图7所示。可见，采用本发明提出的跟踪控制方法，对提高光电跟踪仪对于高速运动目标的跟踪精度具有非常明显的效果。

Claims (1)

1.一种适用于光电跟踪仪的高速目标跟踪控制方法，其特征在于，该方法包括以下步骤:

第一步，初始化伺服计算机各通讯端口、各变量寄存器；

第二步，以T为采样周期，实时采集当前时刻火炮方位旋转变压器输出的参数θ，光电跟踪仪方位旋转变压器输出的参数β,光电跟踪仪俯仰旋转变压器输出的参数ε,车载惯导***输出的参数H、P和R，视频***输出的参数δ_β和δ_ε，激光测距机输出的参数D，其中，θ为车体坐标系下的火炮方位角，β、ε分别为光电坐标系下光电跟踪仪瞄准线指向的方位角和俯仰角，H、P、R分别为大地坐标系下车体的偏航角，纵摇角和横滚角，δ_β、δ_ε分别为瞄准线坐标系下光电跟踪仪方位角和俯仰角的跟踪偏差量，D为目标相对于瞄准线坐标系原点的距离；

第三步，读取上级***发出的控制指令，判断指令是否要求光电跟踪仪停止跟踪，如果为“是”，则跳转至第四步；如果为“否”，则跳转至第五步；

第四步，向光电跟踪仪的方位、俯仰位置环发送保持当前位置指令，然后跳转至第三步；

第五步，利用参数D、δ_β、δ_ε，计算当前时刻目标在瞄准线坐标系下的直角坐标分量；

第六步，利用参数β、ε计算瞄准线坐标系到光电坐标系的转换矩阵N₁，利用转换矩阵N₁将目标在瞄准线坐标系下的直角坐标分量转换到光电坐标系；利用光电坐标系原点与炮塔坐标系原点在三个坐标轴方向的固定偏差Ex、Ey、Ez，将目标在光电坐标系下的直角坐标分量转换到炮塔坐标系；利用参数θ计算炮塔坐标系到车体坐标系的转换矩阵P₁，再利用转换矩阵P₁将目标在炮塔坐标系下的直角坐标分量转换到车体坐标系；利用参数H、P、R计算车体坐标系到大地坐标系的转换矩阵A₁，最终利用转换矩阵A₁将目标在车体坐标系下的直角坐标分量转换到大地坐标系；

第七步，判断稳定跟踪时间是否大于2秒，如果为“否”，则跳转至第八步；如果为“是”，则跳转至第九步；

第八步，并行进行以下操作步骤：

8.1、采用数字二阶环跟踪算法计算方位速度环和俯仰速度环的前馈补偿量，并叠加到光电跟踪仪的跟踪控制回路；

8.2、采用递推滤波跟踪控制方法计算大地坐标系下坐标轴X_d、坐标轴Y_d、坐标轴Z_d方向的位置、速度以及加速度滤波值；

递推滤波跟踪控制方法的计算公式如下：

坐标轴X_d方向滤波公式：

Dx＝Lb[0]+K₁·(Tmp[3]-Lb[0])

DxV＝Lb[1]+K₂·(Tmp[3]-Lb[0])(1)

DxA＝Lb[2]+K₃·(Tmp[3]-Lb[0])

坐标轴Y_d方向滤波公式：

Dy＝Lb[3]+K₁·(Tmp[4]-Lb[3])

DyV＝Lb[4]+K₂·(Tmp[4]-Lb[3])(2)

DyA＝Lb[5]+K₃·(Tmp[4]-Lb[3])

坐标轴Z_d方向滤波公式：

Dz＝Lb[6]+K₁·(Tmp[5]-Lb[6])

DzV＝Lb[7]+K₂·(Tmp[5]-Lb[6])(3)

DzA＝Lb[8]+K₃·(Tmp[5]-Lb[6])

递推公式：

Lb[0]＝Dx+DxV·T+DxA·T²

Lb[1]＝DxV+DxA·T

Lb[2]＝DxA

Lb[3]＝Dy+DyV·T+DyA·T²

Lb[4]＝DyV+DyA·T(4)

Lb[5]＝DyA

Lb[6]＝Dz+DzV·T+DzA·T²

Lb[7]＝DzV+DzA·T

Lb[8]＝DzA

其中，Dx、DxV、DxA分别为大地坐标系下坐标轴X_d方向的位置、速度、加速度的滤波值；Dy、DyV、DyA分别为大地坐标系下坐标轴Y_d方向的位置、速度、加速度的滤波值；Dz、DzV、DzA分别为大地坐标系下坐标轴Z_d方向的位置、速度、加速度的滤波值；K₁、K₂、K₃均为暂态系数，取值范围为0.02≤K₁≤0.05，0.3≤K₂≤0.6，1.0≤K₃≤3.0；Tmp[3]、Tmp[4]、Tmp[5]分别为目标在大地坐标系下坐标轴X_d、坐标轴Y_d、坐标轴Z_d方向的直角坐标分量；Lb[0]～Lb[8]为滤波中间变量；

8.3、判断稳定跟踪时间是否大于2秒，如果为“否”，则重复执行第二步至8.2步的操作；如果为“是”，则跳转至第九步；

第九步，采用(1)～(4)式并用稳态系数K₄、K₅、K₆分别代替(1)～(3)式中的暂态系数K₁、K₂、K₃，计算稳态跟踪时大地坐标系下坐标轴X_d、坐标轴Y_d、坐标轴Z_d方向的位置、速度以及加速度滤波值，其中0.003≤K₄≤0.008，0.01≤K₅≤0.03，0.005≤K₆≤0.009；

第十步，计算瞄准线坐标系下，坐标轴X_m、坐标轴Y_m、坐标轴Z_m方向的位置滤波值，具体步骤如下：

10.1、利用参数H、P、R计算大地坐标系到车体坐标系的转换矩阵A₂，并利用转换矩阵A₂将大地坐标系下的位置滤波值(Dx,Dy,Dz)转换到车体坐标系；

10.2、利用参数θ计算车体坐标系到炮塔坐标系的转换矩阵Q₁，并利用转换矩阵Q₁将车体坐标系下坐标轴X_c、坐标轴Y_c、坐标轴Z_c方向的位置滤波值转换到炮塔坐标系；

10.3、利用光电坐标系原点与炮塔坐标系原点在三个坐标轴方向的固定偏差Ex、Ey、Ez，将炮塔坐标系下坐标轴X_p、坐标轴Y_p、坐标轴Z_p方向的位置滤波值转换到光电坐标系；

10.4、利用参数β、ε计算光电坐标系到瞄准线坐标系的转换矩阵L₁，并利用转换矩阵L₁将光电坐标系下坐标轴X_g、坐标轴Y_g、坐标轴Z_g方向的位置滤波值转换到瞄准线坐标系；

第十一步，计算瞄准线坐标系下坐标轴X_m、坐标轴Y_m、坐标轴Z_m方向的速度滤波值，具体步骤如下：

11.1、利用转换矩阵A₂将大地坐标系下的速度滤波值(DxV,DyV,DzV)转换到车体坐标系；

11.2、利用转换矩阵Q₁将车体坐标系下坐标轴X_c、坐标轴Y_c、坐标轴Z_c方向的速度滤波值转换到炮塔坐标系；

11.3、利用光电坐标系原点与炮塔坐标系原点在三个坐标轴方向的固定偏差Ex、Ey、Ez，将炮塔坐标系下坐标轴X_p、坐标轴Y_p、坐标轴Z_p方向的速度滤波值转换到光电坐标系；

11.4、利用转换矩阵L₁将光电坐标系下坐标轴X_g、坐标轴Y_g、坐标轴Z_g方向的速度滤波值转换到瞄准线坐标系；

第十二步，计算瞄准线坐标系下方位速度环和俯仰速度环控制回路中不含加速度分量的速度补偿量，计算公式如下：

Est_FwjV＝-Tmp[9]/Tmp[12]

Est_FyjV＝Tmp[10]/Tmp[13]

式中， $Tmp\&lsqb;12\&rsqb;=\sqrt{{(Tmp\&lsqb;6\&rsqb;)}^2+{(Tmp\&lsqb;7\&rsqb;)}^2}$

$Tmp\&lsqb;13\&rsqb;=\sqrt{{(Tmp\&lsqb;6\&rsqb;)}^2+{(Tmp\&lsqb;7\&rsqb;)}^2+{(Tmp\&lsqb;8\&rsqb;)}^2}$

其中，Est_FwjV为光电跟踪仪在瞄准线坐标系下不含加速度分量的方位速度补偿量，Est_FyjV为光电跟踪仪在瞄准线坐标系下不含加速度分量的俯仰速度补偿量；Tmp[6]、Tmp[7]、Tmp[8]分别为瞄准线坐标系下坐标轴X_m、坐标轴Y_m、坐标轴Z_m方向的位置滤波值；Tmp[9]、Tmp[10]分别为瞄准线坐标系下坐标轴X_m、坐标轴Y_m方向的速度滤波值；Tmp[12]、Tmp[13]为中间变量；

第十三步，计算瞄准线坐标系下坐标轴X_m、坐标轴Y_m、坐标轴Z_m方向的加速度滤波值，具体步骤如下：

13.1、利用转换矩阵A₂将大地坐标系下的加速度滤波值(DxA,DyA,DzA)转换到车体坐标系；

13.2、利用转换矩阵Q₁将车体坐标系下坐标轴X_c、坐标轴Y_c、坐标轴Z_c方向的加速度滤波值转换到炮塔坐标系；

13.3、利用光电坐标系原点与炮塔坐标系原点在三个方向的固定偏差Ex、Ey、Ez，将炮塔坐标系下坐标轴X_p、坐标轴Y_p、坐标轴Z_p方向的加速度滤波值转换到光电坐标系；

13.4、利用转换矩阵L₁将光电坐标系下坐标轴X_g、坐标轴Y_g、坐标轴Z_g方向的加速度滤波值转换到瞄准线坐标系；

第十四步，计算瞄准线坐标系下方位、俯仰速度补偿量中的加速度分量，计算公式如下：

Est_FwjA＝(2·Tmp[11]·Tmp[18]/Tmp[13]+Tmp[17]·Tmp[19]-Tmp[14]/Tmp[13])/L₂₃

Est_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]为瞄准线坐标系下坐标轴Z_m方向的速度滤波值；Tmp[14]、Tmp[15]分别为瞄准线坐标系下坐标轴X_m、坐标轴Y_m方向的加速度滤波值；L₂₃为转换矩阵L₁中第二行、第三列的元素；Tmp[17]～Tmp[19]为中间变量；

第十五步，计算目标跟踪时，瞄准线坐标系下方位速度环和俯仰速度环的最终前馈补偿参数，计算公式如下：

FwCon＝(Est_FwjV+K₇·Est_FwjA)/L₃₃

FyCon＝Est_FyjV+K₈·Est_FyjA

其中，FwCon为瞄准线坐标系下方位速度环的跟踪前馈补偿参数，FyCon为瞄准线坐标系下俯仰速度环的跟踪前馈补偿参数；K₇、K₈为修正系数，取值范围为0＜K₇≤0.1，0＜K₈≤0.1；L₃₃为转换矩阵L₁中第三行、第三列的元素；

第十六步，重复执行第二步到第十五步，实时计算光电跟踪仪跟踪目标时方位、俯仰速度环的前馈补偿量及跟踪参数，实现对高速运动目标的稳定跟踪。