CN103439977B - 适用于光电跟踪仪的高速目标跟踪控制方法 - Google Patents
适用于光电跟踪仪的高速目标跟踪控制方法 Download PDFInfo
- Publication number
- CN103439977B CN103439977B CN201310373992.8A CN201310373992A CN103439977B CN 103439977 B CN103439977 B CN 103439977B CN 201310373992 A CN201310373992 A CN 201310373992A CN 103439977 B CN103439977 B CN 103439977B
- Authority
- CN
- China
- Prior art keywords
- tmp
- coordinate axis
- coordinate
- under
- coordinate system
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
Landscapes
- Aiming, Guidance, Guns With A Light Source, Armor, Camouflage, And Targets (AREA)
Abstract
本发明公开了一种适用于光电跟踪仪的高速目标跟踪控制方法,属于伺服控制技术领域。本方法利用光电跟踪仪、火炮、载体惯导***、视频***和激光测距机输出的相关参数,通过一系列坐标转换、递推迭代和坐标反变换,完成瞄准线坐标系下方位速度环和俯仰速度环跟踪前馈补偿参数的计算,并将该参数分别叠加到方位、俯仰跟踪控制回路,参与跟踪控制。采用本发明的光电跟踪仪对于来袭的高速运动目标跟踪具有响应速度快、动态滞后小、跟踪精度高、跟踪稳定性好的优点,适合于车载、舰载光电跟踪***对于来袭导弹、飞机等高速运动目标的跟踪,对于提高火控***的目标解算精度和整个武器***的作战效能都具有十分重要的意义。
Description
技术领域
本发明属于伺服控制技术领域,涉及光电跟踪仪,尤其涉及一种适用于光电跟踪仪的高速目标跟踪控制方法。
背景技术
当今,车载、舰载综合体形式的光电跟踪仪(光电跟踪仪安装在火炮炮塔上,光电跟踪仪的瞄准线零位与火炮的炮口指向零位一致),通常都采用方位、俯仰两轴稳定体制,即不设专用的稳定平台来隔离车体、舰船运动的影响,而是将光电跟踪仪的基座直接与炮塔固结在一起,将方位、俯仰速率陀螺均安装在俯仰框架上,通过方位、俯仰伺服***,实现光电跟踪仪瞄准线的指向稳定及对目标的自动跟踪。在自动跟踪目标的过程中,光电跟踪仪始终保持瞄准线对准目标,实现目标的高精度跟踪,输出目标诸元信息,从而保证整个火控***的射击精度。
为了满足光电跟踪仪的跟踪精度指标要求,伺服***必须解决两个关键性的技术问题。一是如何减小或消除车摇、船摇、炮塔旋转及火炮射击引起的瞄准线指向稳定误差;第二个是如何减小或消除由于目标高速运动引起的动态跟踪误差。
传统的光电跟踪仪普遍采用二阶环跟踪方法对目标进行跟踪,在该方法中,速度环利用视频***输出的跟踪误差量并采用数字迭代算法计算跟踪速度量,位置环根据跟踪速度量采用比例法计算位置控制量。虽然二阶环跟踪方法较为简单,易于实现,但是,该方法在整个计算过程中没有涉及运动载体和火炮的相关参数,因而无法补偿运动载体和火炮带来的扰动,导致跟踪精度不高;同时,该方法未引入目标距离量,而对远、近目标均采用同样的控制算法,由于在实际跟踪过程中,随着目标距离的变化,光电跟踪仪在方位、俯仰的跟踪角速度也是不断变化的,即对于同一目标,跟踪距离越近,方位、俯仰的跟踪角速度就会越大,因此,传统的二阶环跟踪方法在目标距离较近、运动速度较快的情况下,会产生较大的跟踪滞后。
在实际的应用过程中,传统的二阶环跟踪方法对于目标距离无法得到且运动速度低于2马赫的慢速来袭目标而言,其跟踪效果尚可;但对于飞行速度大于2马赫的高速来袭目标,其跟踪过程中随着目标距离的不断减小,跟踪动态滞后是不可避免的,加之车摇、船摇、炮塔旋转及火炮射击的扰动,其跟踪精度难以保证,这将影响到整个武器***的射击精度。
发明内容
本发明要解决的技术问题是,针对现有技术存在的缺陷,为光电跟踪仪提供一种在外界扰动条件下针对高速来袭目标的跟踪控制方法。
为解决上述技术问题,本发明提出了一种适用于光电跟踪仪的高速目标跟踪控制方法,该方法是通过存储有跟踪控制软件包的伺服计算机实现的,当伺服计算机上电后,跟踪控制软件包执行以下步骤:
第一步,初始化伺服计算机各通讯端口、各变量寄存器;
第二步,以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·T2
Lb[1]=DxV+DxA·T
Lb[2]=DxA
Lb[3]=Dy+DyV·T+DyA·T2
Lb[4]=DyV+DyA·T(4)
Lb[5]=DyA
Lb[6]=Dz+DzV·T+DzA·T2
Lb[7]=DzV+DzA·T
Lb[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]
式中,
其中,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])/L23
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]为瞄准线坐标系下坐标轴Zm方向的速度滤波值;Tmp[14]、Tmp[15]分别为瞄准线坐标系下坐标轴Xm、坐标轴Ym方向的加速度滤波值;L23为转换矩阵L1中第二行、第三列的元素;Tmp[17]~Tmp[19]为中间变量;
第十五步,计算目标跟踪时,瞄准线坐标系下方位速度环和俯仰速度环的最终前馈补偿参数,计算公式如下:
FwCon=(Est_FwjV+K7·Est_FwjA)/L33
FyCon=Est_FyjV+K8·Est_FyjA
其中,FwCon为瞄准线坐标系下方位速度环的跟踪前馈补偿参数,FyCon为瞄准线坐标系下俯仰速度环的跟踪前馈补偿参数;K7、K8为修正系数,取值范围为0<K7≤0.1,0<K8≤0.1;L33为转换矩阵L1中第三行、第三列的元素;
第十六步,重复执行第二步到第十五步,实时计算光电跟踪仪跟踪目标时方位、俯仰速度环的前馈补偿量及跟踪参数,实现对高速运动目标的稳定跟踪。
本发明根据目标的特性,在自动跟踪开始的前2秒内采用数字二阶环跟踪算法进行跟踪,在跟踪时间达到2秒后,采用递推滤波跟踪控制方法进行目标跟踪。递推滤波跟踪控制方法利用光电跟踪仪、火炮、车载惯导***、视频***和激光测距机输出的相关参数,通过一系列坐标转换计算出目标在大地坐标系下的直角坐标分量,并将该分量代入大地坐标系下的递推迭代公式,计算出大地坐标系下的位置、速度和加速度滤波值;然后通过坐标反变换,将大地坐标系下的位置、速度和加速度滤波值转换到瞄准线坐标系,最终利用这些滤波值计算出目标跟踪时瞄准线坐标系下方位速度环和俯仰速度环的跟踪前馈补偿参数,并将这些参数叠加到伺服控制回路中,参与跟踪控制。由于这些前馈补偿参数含有位置、速度和加速度滤波的分量以及目标相对于光电跟踪仪的距离量,这些量都是目标跟踪过程中体现目标运动特性的关键参数,将这些参数处理后作为目标跟踪时的前馈补偿参数叠加到跟踪控制回路中,不仅提高了光电跟踪仪对高速运动目标的响应速度和跟踪能力,而且解决了传统跟踪方式对于高速来袭目标跟踪动态滞后较大的缺陷;同时,本发明在计算跟踪前馈补偿参数时,采用了运动载体和火炮的相关参数,对光电跟踪仪跟踪目标过程中运动载体和火炮带来的扰动进行了补偿,有效减小了外界扰动对光电跟踪仪跟踪稳定性和精度的影响,增加了***的抗外界扰动能力,提高了***的鲁棒性。由此可见,本发明不仅提高了光电跟踪仪对高速来袭目标的响应速度和跟踪精度,而且增强了光电跟踪仪目标跟踪过程中的抗外界扰动能力,这对于提高火控***的目标解算精度和整个武器***的作战效能都具有十分重要的意义。
附图说明
图1是本发明跟踪控制方法的工作流程图。
图2是火炮炮塔坐标系与车体坐标系的定义。
图3是光电跟踪仪光电坐标系的定义。
图4是车体坐标系与大地坐标系的定义。
图5是光电跟踪仪瞄准线坐标系的定义。
图6是采用本发明跟踪控制方法的模拟航路跟踪误差曲线。
图7是采用传统二阶环跟踪算法的模拟航路跟踪误差曲线。
具体实施方式
下面结合附图和优选实施例对本发明作进一步的详述。
本发明跟踪控制方法的优选实施例用于综合体形式的车载光电跟踪仪。光电跟踪仪为两轴两框架结构,分别为方位轴(框架)和俯仰轴(框架),俯仰框架安装于方位框架之上,激光测距机和视频探测器均安装于俯仰框架的光具座上,方位、俯仰框架分别安装各自的旋转变压器,方位速率陀螺和俯仰速率陀螺均安装在俯仰框架上,光电跟踪仪的瞄准线指向平行于各探测器的视轴;光电跟踪仪安装于炮塔之上,其瞄准线零位指向与火炮炮口零位指向一致;炮塔安装于车体之上,炮塔坐标系与车体坐标系的坐标原点重合;火炮方位、俯仰机构分别安装有各自的旋转变压器;车体的摇摆中心安装有惯导***,由于惯导***安装于车体摇摆中心,因此,惯导***输出的参数值均反映车体在大地坐标系下的运动参数;光电跟踪仪方位角和俯仰角的跟踪偏差量由视频***输出。
本发明提出的跟踪控制方法是通过存储有跟踪控制软件包的伺服计算机实现的,当伺服计算机上电后,跟踪控制软件包按照图1所示的工作流程执行以下步骤。
第一步,初始化伺服计算机各通讯端口、各变量寄存器。
第二步,以T为采样周期,实时采集当前时刻火炮方位旋转变压器输出的参数θ,光电跟踪仪方位、俯仰旋转变压器输出的参数(β,ε)、车载惯导***输出的参数(H,P,R)、视频***输出的参数(δβ,δε)、激光测距机输出的参数D,在本实施例中,采样周期T=1ms。
其中,θ为车体坐标系下的火炮方位角(参见图2),车体坐标系的原点Oc为车体的摇摆中心,Yc轴平行于炮塔安装基面且指向车体前进方向;Xc轴平行于炮塔安装基面且垂直于Yc轴,沿车体前进方向指向右侧;Zc轴垂直于平面OcXcYc且铅垂向上;炮塔方位角θ按左手定则定义即炮塔绕Zc轴顺时针旋转为正。
β、ε分别为光电坐标系下光电跟踪仪瞄准线指向的方位角和俯仰角(参见图3),光电坐标系的原点Og为方位旋转轴与俯仰旋转轴的交点,Yg轴为光电跟踪仪瞄准线零位时的指向,其平行于光电跟踪仪的安装基面,Xg轴平行于光电跟踪仪的安装基面且垂直于Yg轴,沿瞄准线零位方向指向右侧,Zg轴垂直于平面OgXgYg,铅垂向上;方位角β按左手定则定义即方位机构绕Zg轴顺时针旋转为正,俯仰角ε以OgXgYg平面为零位,向上抬起为正。
H、P、R分别为大地坐标系下车体的偏航角、纵摇角和横滚角(参见图4),大地坐标系按照“东-北-天”坐标系定义,原点Od与车体坐标系的原点Oc重合,Yd轴为车体的指北线,其平行于水平面指向正北,Xd轴平行于水平面指向为正东,Zd轴垂直于平面OdXdYd,铅垂向上,其中偏航角是“东-北”平面上的指北线与车体航向线之间的夹角,顺时针为正;纵摇角是铅垂平面上车体纵轴与“东-北”平面的夹角,车头抬起为正;横摇角是车体绕Yd轴旋转时的旋转角,Xd轴平行于“东-北”平面时为0,左侧抬起为正。
δβ、δε分别为瞄准线坐标系下光电跟踪仪方位角和俯仰角的跟踪偏差量(参见图5),瞄准线坐标系的原点Om与光电坐标系的原点Og重合,为光电跟踪仪方位旋转轴与俯仰旋转轴的交点,Ym轴为光电跟踪仪的跟踪瞄准线,指向目标方向,Xm轴为光电跟踪仪的俯仰轴,平行于光电跟踪仪的安装基面且垂直于Ym轴,沿Ym轴方向指向右侧,Zm轴垂直于平面OmXmYm,铅垂向上;方位角跟踪偏差量δβ按右手定则定义即方位机构绕Zm轴顺时针旋转为负,俯仰角跟踪偏差量δε以OmXmYm平面为零位,向上抬起为负。由于光电跟踪仪在实际目标跟踪过程中存在跟踪误差,因此瞄准线通常不通过目标。
D为目标相对于瞄准线坐标系原点的距离。
第三步,读取上级***发出的控制指令,根据控制指令判断光电跟踪仪是否停止跟踪,如果为“是”,则跳转至第四步;如果为“否”,则跳转至第五步。
第四步,向方位、俯仰位置环发送保持当前位置指令,方位、俯仰机构根据各自的控制回路,使光电跟踪仪停止跟踪,保持在当前位置,转入停当前状态,然后跳转至第三步,等待其它指令。
第五步,计算当前时刻目标M在瞄准线坐标系下的直角坐标分量Tmp1[0]'、Tmp1[1]'、Tmp1[2]'(参见图5),计算公式如下:
Tmp1[0]'=D·cos(δε)·sin(δβ)
Tmp1[1]'=D·cos(δε)·cos(δβ)
Tmp1[2]'=D·sin(δε)
其中,Tmp1[0]'、Tmp1[1]'、Tmp1[2]'分别为当前时刻目标M在瞄准线坐标系下Xm轴方向、Ym轴方向、Zm轴方向的直角坐标分量。
第六步,将目标M在瞄准线坐标系下的直角坐标分量Tmp1[0]'、Tmp1[1]'、Tmp1[2]'转换到大地坐标系。
6.1、将目标M在瞄准线坐标系下的直角坐标分量Tmp1[0]'、Tmp1[1]'、Tmp1[2]'转换到光电坐标系,计算公式为:E1=N1·F1。
式中,
矩阵N1的九项元素分别为:N11=cos(β),N12=sin(β)cos(ε),N13=-sin(β)sin(ε),N21=-sin(β),N22=cos(β)cos(ε),N23=-cos(β)sin(ε),N31=0,N32=sin(ε),N33=cos(ε)。
矩阵E1的三项元素计算结果如下:
Tmp1[0]''=Tmp1[0]'·N11+Tmp1[1]'·N12+Tmp1[2]'·N13
Tmp1[1]''=Tmp1[0]'·N21+Tmp1[1]'·N22+Tmp1[2]'·N23
Tmp1[2]''=Tmp1[0]'·N31+Tmp1[1]'·N32+Tmp1[2]'·N33
其中,E1为当前时刻目标M在光电坐标系下的直角坐标分量矩阵,Tmp1[0]''、Tmp1[1]''、Tmp1[2]''分别为目标M在光电坐标系下Xg轴方向、Yg轴方向、Zg轴方向的直角坐标分量;N1为瞄准线坐标系到光电坐标系的转换矩阵;F1为当前时刻目标M在瞄准线坐标系下的直角坐标分量矩阵。
6.2、将目标M在光电坐标系下的直角坐标分量Tmp1[0]''、Tmp1[1]''、Tmp1[2]''转换到炮塔坐标系,计算公式如下:
Tmp1[0]=Tmp1[0]''+Ex=Tmp1[0]'·N11+Tmp1[1]'·N12+Tmp1[2]'·N13+Ex
Tmp1[1]=Tmp1[1]''+Ey=Tmp1[0]'·N21+Tmp1[1]'·N22+Tmp1[2]'·N23+Ey
Tmp1[2]=Tmp1[2]''+Ez=Tmp1[0]'·N31+Tmp1[1]'·N32+Tmp1[2]'·N33+Ez
其中,Ex、Ey、Ez为光电坐标系向炮塔坐标系转换时Xp、Yp、Zp三方向的转换参数;Tmp1[0]、Tmp1[1]、Tmp1[2]分别为当前时刻目标M在炮塔坐标系下Xp轴方向、Yp轴方向、Zp轴方向的直角坐标分量。
由于光电跟踪仪安装在炮塔之上,光电跟踪仪的瞄准线零位与炮口零位指向一致,因此光电跟踪仪的光电坐标系与炮塔坐标系的坐标原点不重合,但各轴平行,即光电坐标系与炮塔坐标系之间的坐标变换只涉及坐标平移,因此需要得到光电坐标系与炮塔坐标系坐标原点之间的偏差,该偏差是光电跟踪仪在炮塔上安装的固定偏差。按照光电跟踪仪和炮塔的结构设计参数,炮塔总体设计单位可以提供Ex、Ey、Ez的具体数值。
6.3、将目标M在炮塔坐标系下的直角坐标分量Tmp1[0]、Tmp1[1]、Tmp1[2]转换到车体坐标系,计算公式为:B1=P1·T1。
式中,
矩阵P1的九项元素分别为:P11=cos(θ),P12=sin(θ),P13=0,P21=-sin(θ),P22=cos(θ),P23=0,P31=0,P32=0,P33=1。
矩阵B1的三项元素计算结果如下:
Tmp[0]=Tmp1[0]·P11+Tmp1[1]·P12+Tmp1[2]·P13
Tmp[1]=Tmp1[0]·P21+Tmp1[1]·P22+Tmp1[2]·P23
Tmp[2]=Tmp1[0]·P31+Tmp1[1]·P32+Tmp1[2]·P33
其中,B1为当前时刻目标M在车体坐标系下的直角坐标分量矩阵,Tmp[0]、Tmp[1]、Tmp[2]分别为目标M在车体坐标系下Xc轴方向、Yc轴方向、Zc轴方向的直角坐标分量;P1为炮塔坐标系到车体坐标系的转换矩阵;T1为当前时刻目标M在炮塔坐标系下的直角坐标分量矩阵。
本实施例中,炮塔安装于车体之上,炮塔坐标系与车体坐标系的坐标原点重合(参见图2),炮塔坐标系的原点Op为车体的摇摆中心,Yp轴平行于炮塔安装基面且为炮口高低角为零时的指向;Xp轴平行于炮塔安装基面且垂直于Yp轴,沿炮口方向指向右侧;Zp轴垂直于平面OpXpYp,铅垂向上。炮塔坐标系OpXpYpZp与车体坐标系OcXcYcZc之间仅存在火炮方位角θ的差异,当θ=0时,两坐标系重合,因此炮塔坐标系与车体坐标系之间的坐标转换只涉及到火炮方位角θ。
6.4、将目标M在车体坐标系下的直角坐标分量Tmp[0]、Tmp[1]、Tmp[2]转换到大地坐标系,计算公式为:C1=A1·B1。
式中,
矩阵A1的九项元素计算公式如下:
X11=cos(R)cos(H)+sin(R)sin(P)sin(H)
X12=cos(P)sin(H)
X13=sin(R)cos(H)-cos(R)sin(P)sin(H)
X21=-cos(R)sin(H)+sin(R)sin(P)cos(H)
X22=cos(P)cos(H)
X23=-sin(R)sin(H)-cos(R)sin(P)cos(H)
X31=-sin(R)cos(P)
X32=sinP
X33=cos(R)cos(P)
矩阵C1的三项元素计算结果如下:
Tmp[3]=Tmp[0]·X11+Tmp[1]·X12+Tmp[2]·X13
Tmp[4]=Tmp[0]·X21+Tmp[1]·X22+Tmp[2]·X23
Tmp[5]=Tmp[0]·X31+Tmp[1]·X32+Tmp[2]·X33
其中,C1为当前时刻目标M在大地坐标系下的直角坐标分量矩阵,Tmp[3]、Tmp[4]、Tmp[5]分别为目标M在大地坐标系下Xd轴方向、Yd轴方向、Zd轴方向的直角坐标分量;A1为车体坐标系到大地坐标系的转换矩阵;B1为当前时刻目标M在车体坐标系下的直角坐标分量矩阵。
第七步,判断稳定跟踪时间是否大于2秒,如果为“否”,则跳转至第八步;如果为“是”,则跳转至第九步。
2秒计时采用外部中断计数实现,伺服计算机每接收到一次时钟中断,跟踪控制软件中的接收计数变量加1,由于本实施例采用1ms的时钟中断,因此当接收计数变量从0累加到2000时,表明2秒计时时间到。
第八步,进入数字二阶环跟踪模式,同时利用暂态系数(K1,K2,K3)计算大地坐标系下Xd、Yd、Zd三方向的位置(Dx,Dy,Dz)、速度(DxV,DyV,DzV)以及加速度滤波值(DxA,DyA,DzA),直到稳定跟踪时间大于2秒。
8.1、采用数字二阶环跟踪算法计算方位、俯仰速度环的前馈补偿量,并叠加到跟踪控制回路。
由于大地坐标系下Xd、Yd、Zd三方向的位置、速度和加速度的滤波值采用递推迭代法计算,因此需要有一定的过渡时间,即计算收敛时间,通常取2~3秒,本实施例设定2秒为计算收敛时间。
在光电跟踪仪刚进入跟踪状态且稳定跟踪目标的时间未达到2秒时,由于迭代计算的滤波参数没有收敛,而不能作为跟踪控制参数进行目标跟踪,因此该时间段内采用数字二阶环算法计算跟踪控制量,并将计算结果作为方位、俯仰速度环的前馈补偿参数,输入到控制回路中,完成对目标的跟踪;通常在跟踪起始段内,目标距离较远,因此目标相对于光电跟踪仪的运动速度较慢,二阶环跟踪模式完全能够满足跟踪精度要求。而在稳定跟踪目标时间超过2秒钟后,则需采用递推滤波跟踪控制方法实现目标跟踪。
8.2、利用暂态系数(K1,K2,K3),并采用递推滤波跟踪控制方法计算大地坐标系下Xd、Yd、Zd三方向的位置(Dx,Dy,Dz)、速度(DxV,DyV,DzV)以及加速度滤波值(DxA,DyA,DzA)。
虽然在稳定跟踪2秒后,才使用递推滤波跟踪控制方法进行跟踪控制,但由于该方法采用递推迭代形式进行参数计算,因此,在采用数字二阶环跟踪模式时,一直在进行滤波参数的迭代计算,但其结果并不参与跟踪控制,只是作为后续迭代计算跟踪控制参数的基础。
在进行大地坐标系下Xd、Yd、Zd三方向位置、速度、加速度递推滤波计算时,需要用到一组系数,即暂态系数(K1,K2,K3)。该组系数是根据α-β-γ滤波算法以及试验修正确定的,其取值范围如下:0.02≤K1≤0.05,0.3≤K2≤0.6,1.0≤K3≤3.0。在本实施例中,暂态系数的取值分别为:K1=0.04113、K2=0.57580、K3=2.68704;该组系数经过多套***的试验验证,适用于两轴光电跟踪仪,可在其它两轴***中直接使用,也可根据实际情况在取值范围内进行修正。
递推滤波跟踪控制方法的计算公式如下:
Xd方向滤波公式:
Dx=Lb[0]+K1·(Tmp[3]-Lb[0])
DxV=Lb[1]+K2·(Tmp[3]-Lb[0])
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])
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])
DzA=Lb[8]+K3·(Tmp[5]-Lb[6])
递推公式:
Lb[0]=Dx+DxV·T+DxA·T2
Lb[1]=DxV+DxA·T
Lb[2]=DxA
Lb[3]=Dy+DyV·T+DyA·T2
Lb[4]=DyV+DyA·T
Lb[5]=DyA
Lb[6]=Dz+DzV·T+DzA·T2
Lb[7]=DzV+DzA·T
Lb[8]=DzA
其中,Dx、DxV、DxA分别为大地坐标系下Xd方向的位置、速度、加速度的滤波值;Dy、DyV、DyA分别为大地坐标系下Yd方向的位置、速度、加速度的滤波值;Dz、DzV、DzA分别为大地坐标系下Zd方向的位置、速度、加速度的滤波值;Lb[0]~Lb[8]为滤波中间变量。
8.3、判断稳定跟踪时间是否大于2秒,如果为“否”,则重复执行第二步至8.2步的操作;如果为“是”,则跳转至第九步。
第九步,利用稳态系数(K4,K5,K6),并采用递推滤波跟踪控制方法计算稳态跟踪时,大地坐标系下Xd、Yd、Zd三方向的位置(Dx,Dy,Dz)、速度(DxV,DyV,DzV)以及加速度滤波值(DxA,DyA,DzA)。
当稳定跟踪目标时间超过2秒时,则采用递推滤波跟踪控制方法计算跟踪参数,此时,需要用到一组系数,即稳态系数(K4,K5,K6)。该组系数与暂态系数(K1,K2,K3)一样,也是根据α-β-γ滤波算法以及试验修正确定的,其取值范围如下:0.003≤K4≤0.008,0.01≤K5≤0.03,0.005≤K6≤0.009。在本实施例中,稳态系数的取值分别为:K4=0.005982、K5=0.011964、K6=0.00796;该组稳态系数与暂态系数一样,经过多套***的试验验证,适用于两轴光电跟踪仪,可在其它两轴***中直接使用,也可根据实际情况在取值范围内进行修正。
此时,递推滤波跟踪控制方法的计算公式与第8.2步的计算公式形式上完全一样,只需要把第8.2步公式中的暂态系数(K1,K2,K3)对应的更换为稳态系数(K4,K5,K6)即可。
计算开始时,Xd、Yd、Zd三方向的位置、速度以及加速度滤波计算的初值分别为2秒计时时间到的那一刻,第8.2步计算出的大地坐标系下Xd、Yd、Zd三方向的位置、速度以及加速度滤波值。
第十步,计算瞄准线坐标系下Xm、Ym、Zm三方向的位置滤波值。
10.1、将大地坐标系下的位置滤波值(Dx,Dy,Dz)转换到车体坐标系,计算公式为C2=A2·B2。
式中,
矩阵A2的九项元素计算公式如下:
Y11=cos(R)cos(H)+sin(R)sin(P)sin(H)
Y12=-cos(R)sin(H)+sin(R)sin(P)cos(H)
Y13=-sin(R)cos(P)
Y21=cos(P)sin(H)
Y22=cos(P)cos(H)
Y23=sinP
Y31=sin(R)cos(H)-cos(R)sin(P)sin(H)
Y32=-sin(R)sin(H)-cos(R)sin(P)cos(H)
Y33=cos(R)cos(P)
矩阵C2的三项元素计算结果如下:
Tmp1[6]=Dx·Y11+Dy·Y12+Dz·Y13
Tmp1[7]=Dx·Y21+Dy·Y22+Dz·Y23
Tmp1[8]=Dx·Y31+Dy·Y32+Dz·Y33
其中,C2为车体坐标系下Xc、Yc、Zc三方向的位置滤波值矩阵,Tmp1[6]、Tmp1[7]、Tmp1[8]分别为车体坐标系下位置滤波值在Xc轴方向、Yc轴方向、Zc轴方向的直角坐标分量;A2为大地坐标系到车体坐标系的转换矩阵;B2为大地坐标系下Xd、Yd、Zd三方向的位置滤波值矩阵。
10.2、将车体坐标系下Xc、Yc、Zc三方向的位置滤波值Tmp1[6]、Tmp1[7]、Tmp1[8]转换到炮塔坐标系,计算公式如下:T2=Q1·C2。
式中,
矩阵Q1的九项元素分别为:Q11=cos(θ),Q12=-sin(θ),Q13=0,Q21=sin(θ),Q22=cos(θ),Q23=0,Q31=0,Q32=0,Q33=1。
矩阵T2的三项元素计算结果如下:
Tmp[6]'=Tmp1[6]·Q11+Tmp1[7]·Q12+Tmp1[8]·Q13
Tmp[7]'=Tmp1[6]·Q21+Tmp1[7]·Q22+Tmp1[8]·Q23
Tmp[8]'=Tmp1[6]·Q31+Tmp1[7]·Q32+Tmp1[8]·Q33
其中,T2为炮塔坐标系下Xp、Yp、Zp三方向的位置滤波值矩阵,Tmp[6]'、Tmp[7]'、Tmp[8]'分别为炮塔坐标系下位置滤波值在Xp轴方向、Yp轴方向、Zp轴方向的直角坐标分量;Q1为车体坐标系到炮塔坐标系的转换矩阵;C2为车体坐标系下Xc、Yc、Zc三方向的位置滤波值矩阵。
10.3、将炮塔坐标系下Xp、Yp、Zp三方向的位置滤波值Tmp[6]'、Tmp[7]'、Tmp[8]'转换到光电坐标系,计算结果如下:
Tmp[6]''=Tmp[6]'-Ex=Tmp1[6]·Q11+Tmp1[7]·Q12+Tmp1[8]·Q13-Ex
Tmp[7]''=Tmp[7]'-Ey=Tmp1[6]·Q21+Tmp1[7]·Q22+Tmp1[8]·Q23-Ey
Tmp[8]''=Tmp[8]'-Ez=Tmp1[6]·Q31+Tmp1[7]·Q32+Tmp1[8]·Q33-Ez
其中,Tmp[6]''、Tmp[7]''、Tmp[8]''分别为光电坐标系下Xg、Yg、Zg三方向的位置滤波值;Ex、Ey、Ez分别为光电坐标系与炮塔坐标系的坐标原点在Xp、Yp、Zp三方向的固定偏差。
10.4、将光电坐标系下Xg、Yg、Zg三方向的位置滤波值Tmp[6]''、Tmp[7]''、Tmp[8]''转换到瞄准线坐标系,计算公式如下:T3=L1·C3。
式中,
矩阵L1的九项元素分别为:L11=cos(β),L12=-sin(β),L13=0,L21=sin(β)cos(ε),L22=cos(β)cos(ε),L23=sin(ε),L31=-sin(β)sin(ε),L32=-cos(β)sin(ε),L33=cos(ε)。
矩阵T3的三项元素计算结果如下:
Tmp[6]=Tmp[6]''·L11+Tmp[7]''·L12+Tmp[8]''·L13
Tmp[7]=Tmp[6]''·L21+Tmp[7]''·L22+Tmp[8]''·L23
Tmp[8]=Tmp[6]''·L31+Tmp[7]''·L32+Tmp[8]''·L33
其中,T3为瞄准线坐标系下位置滤波值的直角坐标分量矩阵,Tmp[6]、Tmp[7]、Tmp[8]分别为瞄准线坐标系下Xm、Ym、Zm三方向的位置滤波值;L1为光电坐标系到瞄准线坐标系的转换矩阵;C3为光电坐标系下Xg、Yg、Zg三方向位置滤波值的直角坐标分量矩阵。
第十一步,计算瞄准线坐标系下Xm、Ym、Zm三方向的速度滤波值。
11.1、利用转换矩阵A2将大地坐标系下的速度滤波值(DxV,DyV,DzV)转换到车体坐标系,计算结果如下:
Tmp1[9]=DxV·Y11+DyV·Y12+DzV·Y13
Tmp1[10]=DxV·Y21+DyV·Y22+DzV·Y23
Tmp1[11]=DxV·Y31+DyV·Y32+DzV·Y33
其中,Tmp1[9]、Tmp1[10]、Tmp1[11]分别为车体坐标系下Xc、Yc、Zc三方向的速度滤波值。
11.2、利用转换矩阵Q1将车体坐标系下Xc、Yc、Zc三方向的速度滤波值Tmp1[9]、Tmp1[10]、Tmp1[11]转换到炮塔坐标系,计算结果如下:
Tmp[9]'=Tmp1[9]·Q11+Tmp1[10]·Q12+Tmp1[11]·Q13
Tmp[10]'=Tmp1[9]·Q21+Tmp1[10]·Q22+Tmp1[11]·Q23
Tmp[11]'=Tmp1[9]·Q31+Tmp1[10]·Q32+Tmp1[11]·Q33
其中,Tmp[9]'、Tmp[10]'、Tmp[11]'分别为炮塔坐标系下Xp、Yp、Zp三方向的速度滤波值。
11.3、利用光电坐标系与炮塔坐标系的坐标原点在Xp、Yp、Zp三方向的固定偏差Ex、Ey、Ez,将炮塔坐标系下Xp、Yp、Zp三方向的速度滤波值Tmp[9]'、Tmp[10]'、Tmp[11]'转换到光电坐标系,计算结果如下:
Tmp[9]''=Tmp[9]'-Ex=Tmp1[9]·Q11+Tmp1[10]·Q12+Tmp1[11]·Q13-Ex
Tmp[10]''=Tmp[10]'-Ey=Tmp1[9]·Q21+Tmp1[10]·Q22+Tmp1[11]·Q23-Ey
Tmp[11]''=Tmp[11]'-Ez=Tmp1[9]·Q31+Tmp1[10]·Q32+Tmp1[11]·Q33-Ez
其中,Tmp[9]''、Tmp[10]''、Tmp[11]''分别为光电坐标系下Xg、Yg、Zg三方向的速度滤波值。
11.4、利用转换矩阵L1将光电坐标系下Xg、Yg、Zg三方向的速度滤波值Tmp[9]''、Tmp[10]''、Tmp[11]''转换到瞄准线坐标系,计算结果如下:
Tmp[9]=Tmp[9]''·L11+Tmp[10]''·L12+Tmp[11]''·L13
Tmp[10]=Tmp[9]''·L21+Tmp[10]''·L22+Tmp[11]''·L23
Tmp[11]=Tmp[9]''·L31+Tmp[10]''·L32+Tmp[11]''·L33
其中,Tmp[9]、Tmp[10]、Tmp[11]分别为瞄准线坐标系下Xm、Ym、Zm三方向的速度滤波值。
第十二步,计算瞄准线坐标系下方位速度环、俯仰速度环不含加速度分量的速度补偿量,计算公式如下:
Est_FwjV=-Tmp[9]/Tmp[12]
Est_FyjV=Tmp[10]/Tmp[13]
式中,
其中,Est_FwjV为光电跟踪仪在瞄准线坐标系下不含加速度分量的方位速度补偿量,Est_FyjV为光电跟踪仪在瞄准线坐标系下不含加速度分量的俯仰速度补偿量;Tmp[12]、Tmp[13]为中间变量。
第十三步,计算瞄准线坐标系下Xm、Ym、Zm三方向的加速度滤波值。
13.1、利用转换矩阵A2将大地坐标系下的加速度滤波值(DxA,DyA,DzA)转换到车体坐标系,计算结果如下:
Tmp1[14]=DxA·Y11+DyA·Y12+DzA·Y13
Tmp1[15]=DxA·Y21+DyA·Y22+DzA·Y23
Tmp1[16]=DxA·Y31+DyA·Y32+DzA·Y33
其中,Tmp1[14]、Tmp1[15]、Tmp1[16]分别为车体坐标系下Xc、Yc、Zc三方向的加速度滤波值。
13.2、利用转换矩阵Q1将车体坐标系下Xc、Yc、Zc三方向的加速度滤波值Tmp1[14]、Tmp1[15]、Tmp1[16]转换到炮塔坐标系,计算结果如下:
Tmp[14]'=Tmp1[14]·Q11+Tmp1[15]·Q12+Tmp1[16]·Q13
Tmp[15]'=Tmp1[14]·Q21+Tmp1[15]·Q22+Tmp1[16]·Q23
Tmp[16]'=Tmp1[14]·Q31+Tmp1[15]·Q32+Tmp1[16]·Q33
其中,Tmp[14]'、Tmp[15]'、Tmp[16]'分别为炮塔坐标系下Xp、Yp、Zp三方向的加速度滤波值。
13.3、利用光电坐标系与炮塔坐标系的坐标原点在Xp、Yp、Zp三方向的固定偏差Ex、Ey、Ez,将炮塔坐标系下Xp、Yp、Zp三方向的加速度滤波值Tmp[14]'、Tmp[15]'、Tmp[16]'转换到光电坐标系,计算结果如下:
Tmp[14]''=Tmp[14]'-Ex=Tmp1[14]·Q11+Tmp1[15]·Q12+Tmp1[16]·Q13-Ex
Tmp[15]''=Tmp[15]'-Ey=Tmp1[14]·Q21+Tmp1[15]·Q22+Tmp1[16]·Q23-Ey
Tmp[16]''=Tmp[16]'-Ez=Tmp1[14]·Q31+Tmp1[15]·Q32+Tmp1[16]·Q33-Ez
其中,Tmp[14]''、Tmp[15]''、Tmp[16]''分别为光电坐标系下Xg、Yg、Zg三方向的加速度滤波值。
13.4、利用转换矩阵L1将光电坐标系下Xg、Yg、Zg三方向的加速度滤波值Tmp[14]''、Tmp[15]''、Tmp[16]''转换到瞄准线坐标系,计算结果如下:
Tmp[14]=Tmp[14]''·L11+Tmp[15]''·L12+Tmp[16]''·L13
Tmp[15]=Tmp[14]''·L21+Tmp[15]''·L22+Tmp[16]''·L23
Tmp[16]=Tmp[14]''·L31+Tmp[15]''·L32+Tmp[16]''·L33
其中,Tmp[14]、Tmp[15]、Tmp[16]分别为瞄准线坐标系下Xm、Ym、Zm三方向的加速度滤波值。
第十四步、计算瞄准线坐标系下方位、俯仰速度补偿量中的加速度分量,计算公式如下:
Est_FwjA=(2·Tmp[11]·Tmp[18]/Tmp[13]+Tmp[17]·Tmp[19]-Tmp[14]/Tmp[13])/L23
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+K7·Est_FwjA)/L33
FyCon=Est_FyjV+K8·Est_FyjA
其中,FwCon为瞄准线坐标系下方位速度环的跟踪前馈补偿参数,FyCon为瞄准线坐标系下俯仰速度环的跟踪前馈补偿参数。这两个参数分别作为方位、俯仰速度环的前馈量叠加到跟踪控制回路中,对跟踪过程中,方位、俯仰的速度环参数进行修正和补偿。
公式中有两个修正系数K7和K8,其取值范围如下:0<K7≤0.1,0<K8≤0.1,该系数针对不同的***可以进行修改,可通过模拟跟踪航路测试或对真实目标的动飞试验确定。在模拟航路测试或动飞试验过程中,可以通过对跟踪数据的分析来调整和确定修正系数K7和K8,方位、俯仰回路可按照以下规律进行调整:当方位回路跟踪滞后时,增大系数K7的取值,当俯仰回路跟踪滞后时,增大系数K8的取值;当方位回路跟踪超前时,减小K7的取值,当俯仰回路跟踪超前时,减小系数K8的取值;最终在***误差指标范围内确定合适的系数。本实施例分别确定为K7=0.03、K8=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、δβ、δε,计算当前时刻目标在瞄准线坐标系下的直角坐标分量;
第六步,利用参数β、ε计算瞄准线坐标系到光电坐标系的转换矩阵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·T2
Lb[1]=DxV+DxA·T
Lb[2]=DxA
Lb[3]=Dy+DyV·T+DyA·T2
Lb[4]=DyV+DyA·T(4)
Lb[5]=DyA
Lb[6]=Dz+DzV·T+DzA·T2
Lb[7]=DzV+DzA·T
Lb[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]
式中,
其中,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])/L23
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]为瞄准线坐标系下坐标轴Zm方向的速度滤波值;Tmp[14]、Tmp[15]分别为瞄准线坐标系下坐标轴Xm、坐标轴Ym方向的加速度滤波值;L23为转换矩阵L1中第二行、第三列的元素;Tmp[17]~Tmp[19]为中间变量;
第十五步,计算目标跟踪时,瞄准线坐标系下方位速度环和俯仰速度环的最终前馈补偿参数,计算公式如下:
FwCon=(Est_FwjV+K7·Est_FwjA)/L33
FyCon=Est_FyjV+K8·Est_FyjA
其中,FwCon为瞄准线坐标系下方位速度环的跟踪前馈补偿参数,FyCon为瞄准线坐标系下俯仰速度环的跟踪前馈补偿参数;K7、K8为修正系数,取值范围为0<K7≤0.1,0<K8≤0.1;L33为转换矩阵L1中第三行、第三列的元素;
第十六步,重复执行第二步到第十五步,实时计算光电跟踪仪跟踪目标时方位、俯仰速度环的前馈补偿量及跟踪参数,实现对高速运动目标的稳定跟踪。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201310373992.8A CN103439977B (zh) | 2013-08-23 | 2013-08-23 | 适用于光电跟踪仪的高速目标跟踪控制方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201310373992.8A CN103439977B (zh) | 2013-08-23 | 2013-08-23 | 适用于光电跟踪仪的高速目标跟踪控制方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN103439977A CN103439977A (zh) | 2013-12-11 |
CN103439977B true CN103439977B (zh) | 2016-03-02 |
Family
ID=49693672
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201310373992.8A Active CN103439977B (zh) | 2013-08-23 | 2013-08-23 | 适用于光电跟踪仪的高速目标跟踪控制方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN103439977B (zh) |
Families Citing this family (20)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN104035448A (zh) * | 2014-05-21 | 2014-09-10 | 北京航天发射技术研究所 | 瞄准仪方位、俯仰电控跟踪瞄准装置及方法 |
CN104534926A (zh) * | 2014-12-08 | 2015-04-22 | 河北汉光重工有限责任公司 | 高射速小口径火炮自动跟踪瞄具*** |
CN104793631B (zh) * | 2015-02-12 | 2017-06-27 | 中国科学院光电技术研究所 | 一种基于动态扫描方式的目标捕获方法 |
CN105547295A (zh) * | 2016-01-25 | 2016-05-04 | 西安应用光学研究所 | 基于陀螺测速的机载光电观瞄***用地面目标无源测速方法 |
CN108614273B (zh) * | 2016-12-09 | 2021-01-08 | 中国人民解放军92232部队 | 一种机载双波段光电广域侦察与跟踪装置及方法 |
CN106845394A (zh) * | 2017-01-19 | 2017-06-13 | 捷洁通(北京)技术服务有限公司 | 一种光电侦察平台及其目标追踪方法 |
CN107065560A (zh) * | 2017-05-15 | 2017-08-18 | 北京环境特性研究所 | 一种两轴奇异路径光电跟踪控制方法 |
CN108050887B (zh) * | 2017-10-30 | 2022-06-10 | 中国北方车辆研究所 | 一种坦克装甲车辆火控***瞄准线平移的补偿方法及*** |
CN107728648A (zh) * | 2017-11-03 | 2018-02-23 | 南京长峰航天电子科技有限公司 | 一种伺服转台跟踪精度的检测方法 |
CN109584183B (zh) * | 2018-12-05 | 2020-05-29 | 吉林大学 | 一种激光雷达点云去畸变方法及*** |
CN109752023B (zh) * | 2018-12-20 | 2020-12-29 | 北京恒星箭翔科技有限公司 | 一种目标运动状态快速估计方法 |
CN110207723B (zh) * | 2019-06-16 | 2023-05-23 | 西安应用光学研究所 | 一种光电跟踪仪复合轴控制***控制精度测试方法 |
CN110658854B (zh) * | 2019-09-29 | 2023-03-14 | 凯迈(洛阳)测控有限公司 | 一种基于组合惯导信息应用的光电转塔视频跟踪前馈补偿方法 |
CN111198561B (zh) * | 2019-12-05 | 2021-10-22 | 浙江大华技术股份有限公司 | 目标跟踪的运动控制方法、装置、计算机设备及存储介质 |
CN111490819B (zh) * | 2020-03-13 | 2021-05-18 | 威海市赢海通信技术有限公司 | 基于光纤陀螺的舰载激光通信光束跟踪控制方法 |
CN112130134B (zh) * | 2020-08-17 | 2023-12-05 | 河北汉光重工有限责任公司 | 一种基于时间补偿的实时基线修正方法 |
CN112346485B (zh) * | 2020-12-24 | 2021-05-25 | 华中光电技术研究所(中国船舶重工集团公司第七一七研究所) | 一种光电跟踪控制方法、***、电子设备及存储介质 |
CN113985778B (zh) * | 2021-10-28 | 2024-01-19 | 北京环境特性研究所 | 车载光电传感器的控制***及方法 |
CN114003045B (zh) * | 2021-12-30 | 2022-04-15 | 成都星宇融科电力电子股份有限公司 | 一种光电跟踪仪的目标跟踪方法、终端、可读存储介质 |
CN116400603B (zh) * | 2023-05-25 | 2023-09-12 | 南京信息工程大学 | 基于伪前馈改进的Smith预估器的激光跟踪控制方法 |
Citations (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US4266463A (en) * | 1978-01-18 | 1981-05-12 | Aktiebolaget Bofors | Fire control device |
JPH07167592A (ja) * | 1993-12-13 | 1995-07-04 | Nissan Motor Co Ltd | 移動体の誘導方法 |
US6808139B1 (en) * | 1996-11-30 | 2004-10-26 | Daimler-Benz Aerospace Ag | Guidance for missle systems with target tracker and additional manual track point correction |
CN102999048A (zh) * | 2011-09-08 | 2013-03-27 | 中国航天科工集团第二研究院二0七所 | 一种车载动基座光电搜索跟踪转台的伺服控制方法 |
CN103090883A (zh) * | 2013-01-15 | 2013-05-08 | 中国电子科技集团公司第二十七研究所 | 一种光电跟踪***动态跟踪精度校准装置及方法 |
CN103226327A (zh) * | 2013-04-09 | 2013-07-31 | 西安应用光学研究所 | 光电稳瞄***瞄准线静态漂移的自动补偿方法 |
-
2013
- 2013-08-23 CN CN201310373992.8A patent/CN103439977B/zh active Active
Patent Citations (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US4266463A (en) * | 1978-01-18 | 1981-05-12 | Aktiebolaget Bofors | Fire control device |
JPH07167592A (ja) * | 1993-12-13 | 1995-07-04 | Nissan Motor Co Ltd | 移動体の誘導方法 |
US6808139B1 (en) * | 1996-11-30 | 2004-10-26 | Daimler-Benz Aerospace Ag | Guidance for missle systems with target tracker and additional manual track point correction |
CN102999048A (zh) * | 2011-09-08 | 2013-03-27 | 中国航天科工集团第二研究院二0七所 | 一种车载动基座光电搜索跟踪转台的伺服控制方法 |
CN103090883A (zh) * | 2013-01-15 | 2013-05-08 | 中国电子科技集团公司第二十七研究所 | 一种光电跟踪***动态跟踪精度校准装置及方法 |
CN103226327A (zh) * | 2013-04-09 | 2013-07-31 | 西安应用光学研究所 | 光电稳瞄***瞄准线静态漂移的自动补偿方法 |
Non-Patent Citations (4)
Title |
---|
Effect of the Sight Line Shift when a Head-Mounted Display is used;Hiroaki Kudo, Noboru Ohnishi;《Proceedings of the 22nd Annual EMBS International Conference》;20001231;第548-550页 * |
Sight Line Rate Estimation in Missile Seeker Using Disturbance Observer-Based Technique;Smita Sadhu, T.K.Ghoshal;《IEEE TANSACTIONS ON CONTROL SYSTEMS TECHNOLOGY》;20110331;第19卷(第2期);第449-454页 * |
一种新颖的瞄准线高精度稳定补偿方法研究;王惠林等;《应用光学》;20110531;第22卷(第5期);第40-42页 * |
基于dSPACE平台的稳瞄控制***建模方法研究;胥青青等;《兵工学报》;20111130;第32卷(第11期);第1421-1427页 * |
Also Published As
Publication number | Publication date |
---|---|
CN103439977A (zh) | 2013-12-11 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN103439977B (zh) | 适用于光电跟踪仪的高速目标跟踪控制方法 | |
CN103700286B (zh) | 一种舰载无人机自动着舰引导方法 | |
CN107544500A (zh) | 一种考虑约束的无人艇靠泊行为轨迹规划方法 | |
CN105202972B (zh) | 一种基于模型预测控制技术的多导弹协同作战制导方法 | |
CN101807081A (zh) | 一种用于无人飞机的自主导航制导方法 | |
CN110244735A (zh) | 移动机器人跟踪预定轨迹的启发式动态规划控制方法 | |
CN102927851A (zh) | 一种基于轨迹在线规划的末制导方法 | |
CN104390640B (zh) | 一种基于理想流体数值计算的无人机三维航路规划方法 | |
CN102878872B (zh) | 一种针对导引头失锁情况的制导信息处理方法 | |
CN104597911A (zh) | 空中加油受油机自适应最优对接轨迹跟踪飞行控制方法 | |
CN103472849B (zh) | 基于闭环模式合作目标跟踪的卫星姿态机动跟踪方法 | |
CN110764523B (zh) | 基于抗饱和平滑变换的比例积分前置导引攻击目标的方法 | |
CN109471454A (zh) | 一种指定攻击倾角的微型作业飞行器的末端制导段进入方法 | |
CN105180728A (zh) | 基于前数据的旋转制导炮弹快速空中对准方法 | |
CN104483977A (zh) | 一种舵机云台的无速度反馈动态面控制方法 | |
CN106054612A (zh) | 一种btt导弹飞行轨迹自动控制方法 | |
CN102425980A (zh) | 利用加速度计实现过载驾驶仪的控制方法 | |
CN106643297B (zh) | 一种运动平台矢量脱靶量参数估计修正方法 | |
CN105628045A (zh) | 一种无人机跟拍路径规划与跟踪方法 | |
CN107943079A (zh) | 一种剩余飞行时间在线估计方法 | |
CN105197200B (zh) | 一种基于航迹引导的气垫船进坞过程自动控制***及控制方法 | |
CN109917373B (zh) | 运动补偿搜索的动平台雷达的动态规划检测前跟踪方法 | |
CN105333873B (zh) | 一种着陆点在线选取的行星安全着陆制导方法 | |
CN106646507A (zh) | 基于惯性引导的激光跟踪测量设备多目标测量方法与装置 | |
CN101403593B (zh) | 基于横滚/偏摆结构的两轴捷联平台光轴超半球稳定方法 |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
C06 | Publication | ||
PB01 | Publication | ||
C10 | Entry into substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
C14 | Grant of patent or utility model | ||
GR01 | Patent grant |