CN102902282A - 基于光轴与惯性轴重合的地理跟踪方法 - Google Patents

Abstract

本发明公开了一种基于光轴与惯性轴重合的地理跟踪方法。本方法根据目标定位数据与自身位置信息采用空间两点几何算法，实时计算载体瞄准线的空间姿态，再结合载体空间姿态和空间坐标变换原理计算瞄准线相对于载体坐标系的空间角度，将该角度送给伺服控制单元，由伺服控制单元调整转台的相应空间姿态，从而完成对目标的地理跟踪。本方法从原理上区别于图像跟踪方法，因此不受云雾和障碍物的影响，若在飞行器上使用可避免因剧烈震动或视线遮挡而丢失目标的问题，显著提高了战场武器***的侦察与搜跟能力。本发明也可用于各种制导导弹的导引装置，以提高对地目标的精确打击能力。

Description

基于光轴与惯性轴重合的地理跟踪方法

技术领域

本发明属于光电侦察与目标跟踪领域，主要涉及一种对目标的地理跟踪方法，尤其涉及一种基于光轴与惯性轴重合的地理跟踪方法。

背景技术

现代武器***的光电跟踪仪具有对目标侦察与搜索跟踪的功能。目前的自动跟踪功能按照目标探测器的形式分为激光、红外、电视三种跟踪方式。这三种跟踪方式的基本原理为：当目标探测器捕获到目标后，由CCD(电荷耦合器件)将目标场景转变为电子图像，再由信号处理单元对图像进行脱靶测量后自动输出脱靶量，然后取差器将脱靶量变为电信号作为角偏差量送入伺服控制单元，最后由伺服控制单元调整光电跟踪仪的瞄准线角度，从而减小跟踪误差，实现对目标的持续跟踪。但是应用这些跟踪方法的前提是探测器必须清晰捕获目标场景，如果目标受到云雾、障碍物遮挡，或背景环境与目标物光谱特性对比度差，就会使得用探测器得到的目标场景效果不理想，导致光电跟踪仪对目标跟踪精度差甚至丢失目标，降低武器***侦察与搜跟能力。后来又出现利用地理信息实现跟踪的方法。

《航空计算机》期刊2012年3月刊登了题目是《无人机光电载荷地理跟踪控制研究》的论文。该论文介绍一种能在光电侦察无人机光电载荷上使用的地理跟踪方法及***。文中提出在有操纵震荡或云层遮挡时，采用图像跟踪技术会丢失目标，所以引入地理跟踪方法做为图像跟踪方法的辅助手段，可解决对目标快速捕获的问题。该方法首先根据载机空间姿态计算载机坐标系与地理坐标系之间的角度旋转关系，随后将地理坐标系下的目标转换成载机坐标系下的“伪目标”，然后在载机坐标系下计算“伪目标”的空间矢量角，再将得到的空间角度值输入给伺服控制单元，由伺服控制单元调整稳定转台角度，即可快速捕获目标。论文提供的技术方案中没有提出光电载荷***的光轴与惯性轴需要重合的要求，因此仅仅依靠计算目标的空间矢量角并将其输入给伺服控制单元，由伺服控制单元调整稳定转台角度，只能使目标出现在摄像机的视场内，不能保证目标在视场的靶心位置。所以单独采用论文中的地理跟踪方法，不能保持对目标持续稳定的跟踪。

发明内容

本发明要解决的技术问题是，针对现有技术存在的问题，提供一种在光轴与惯性轴重合的前提下能单独使用的地理跟踪方法。

本发明提供的地理跟踪方法包括以下步骤：

第一步，采集目标定位装置输出的目标点数据(λ，L，h)和当前载机位置点的数据(λ₁，L₁，h₁，ψ_A，θ_A，γ_A，S)，其中λ，L，h分别是目标点的经度、纬度和高度；λ₁，L₁，h₁分别是载机的当前经度、纬度和高度，ψ_A，θ_A，γ_A分别是载机在地里坐标系下的当前航向角、俯仰角和横滚角，S是当前载机到目标点的距离；

第二步，用以下公式计算载机与目标点在水平面投影中由载机投影到目标点投影矢量线段的北向夹角σ：

$\mathrm{\σ}=\arctan \frac{|\mathrm{\λ}-{\mathrm{\λ}}_1|\×R_M\×\cos L_1}{|L-L_1|\×R_N}$

R_M＝R_e(1-2e+3esin²L₁)

R_N＝R_e(1+esin²L₁)

其中，R_M是载机当前位置的子午圈曲率半径，R_N是载机当前位置的卯酉圈曲率半径，e是扁率，R_e是地球椭球体长轴半径；

第三步，用以下公式计算瞄准线指向目标点的航向角ψ_b：

当λ＞λ₁且L＞L₁  ψ_b＝σ

当λ＞λ₁且L＜L₁  ψ_b＝180°-σ

当λ＜λ₁且L＜L₁  ψ_b＝180°+σ

当λ＜λ₁且L＞L₁  ψ_b＝-σ

当λ＝λ₁且L＞L₁  ψ_b＝0°

当λ＝λ₁且L＜L₁  ψ_b＝180°

当λ＜λ₁且L＝L₁  ψ_b＝-90°

当λ＞λ₁且L＝L₁  ψ_b＝90°

第四步，用以下公式计算瞄准线与水平面夹角δ：

$\mathrm{\δ}=\arcsin \frac{|h-h_1|}{S}$

第五步，用以下公式计算瞄准线指向目标点的俯仰角θ_b：

当h＞h₁  θ_b＝δ

当h＜h₁  θ_b＝-δ

第六步，计算地理坐标系到载机坐标系的姿态矩阵

$C_A^n=\left[\begin{array}{ccc}\cos {\mathrm{\γ}}_A\cos {\mathrm{\ψ}}_A+\sin {\mathrm{\γ}}_A\sin {\mathrm{\ψ}}_A\sin {\mathrm{\θ}}_A& \sin {\mathrm{\ψ}}_A\cos {\mathrm{\θ}}_A& \sin {\mathrm{\γ}}_A\cos {\mathrm{\ψ}}_A-\cos {\mathrm{\γ}}_A\sin {\mathrm{\ψ}}_A\sin {\mathrm{\θ}}_A\\ -\cos {\mathrm{\γ}}_A\sin {\mathrm{\ψ}}_A+\sin {\mathrm{\γ}}_A\cos {\mathrm{\ψ}}_A\sin {\mathrm{\θ}}_A& \cos {\mathrm{\ψ}}_A\cos {\mathrm{\θ}}_A& -\sin {\mathrm{\γ}}_A\sin {\mathrm{\ψ}}_A-\cos {\mathrm{\γ}}_A\cos {\mathrm{\ψ}}_A\sin {\mathrm{\θ}}_A\\ -\sin {\mathrm{\γ}}_A\cos {\mathrm{\θ}}_A& \sin {\mathrm{\θ}}_A& \cos {\mathrm{\γ}}_A\cos {\mathrm{\θ}}_A\end{array}\right]$

第七步，计算瞄准线到地理坐标系的姿态矩阵

$C_n^b=\left[\begin{array}{ccc}\cos {\mathrm{\ψ}}_b& -\sin {\mathrm{\ψ}}_b& 0\\ \sin {\mathrm{\ψ}}_b\cos {\mathrm{\θ}}_b& \cos {\mathrm{\ψ}}_b\cos {\mathrm{\θ}}_b& \sin {\mathrm{\θ}}_b\\ -\sin {\mathrm{\ψ}}_b\sin {\mathrm{\θ}}_b& -\cos {\mathrm{\ψ}}_b\sin {\mathrm{\θ}}_b& \cos {\mathrm{\θ}}_b\end{array}\right]$

第八步，计算瞄准线到载机坐标系的姿态矩阵

$C_A^b=C_n^b*C_A^n=\left[\begin{array}{ccc}{C}_1\left(\mathrm{1,1}\right)& C_1\left(\mathrm{2,1}\right)& C_1\left(\mathrm{3,1}\right)\\ C_1\left(\mathrm{1,2}\right)& C_1\left(\mathrm{2,2}\right)& C_1\left(\mathrm{3,2}\right)\\ C_1\left(\mathrm{1,3}\right)& C_1\left(\mathrm{2,3}\right)& C_1\left(\mathrm{3,3}\right)\end{array}\right]$

式中，C₁(i，j)且(i＝1，2，3 j＝1，2，3)是矩阵

的各项元素；

第九步，计算载机光轴稳定转台的俯仰角β、方位角α和横滚角χ：

9.1用以下公式计算载机光轴稳定转台的俯仰角β、参考横滚角γ_C和参考方位角ψ_C：

β＝arcsinC₁(3，2)

${\mathrm{\γ}}_C=\arctan (-\frac{C_1\left(\mathrm{3,1}\right)}{C_1\left(\mathrm{3,3}\right)})$

${\mathrm{\ψ}}_C=\arctan \left(\frac{C_1\left(\mathrm{1,2}\right)}{C_1\left(\mathrm{2,2}\right)}\right)$

9.2用以下判别公式计算载机光轴稳定转台方位角α：

当C₁(2，2)→0且C₁(1，2)＞0  α＝90°

当C₁(2，2)→0且C₁(1，2)＜0  α＝-90°

当C₁(2，2)＞0               α＝ψ_C

当C₁(2，2)＜0且C₁(1，2)＞0  α＝ψ_C+180°

当C₁(2，2)＜0且C₁(1，2)＜0  α＝ψ_C-180°

9.3用以下判别公式计算载机光轴稳定转台横滚角χ：

当C₁(3，3)＞0            χ＝γ_C

当C₁(3，3)＜0且γ_C＞0    χ＝γ_C-180°

当C₁(3，3)＜0且γ_C＜0    χ＝γ_C+180°

第十步，将计算获得的载机光轴稳定转台的方位角α、俯仰角β、横滚角χ传输给伺服控制单元；

第十一步，重复执行第一步到第十步，直到上级***发出地理跟踪结束指令后结束。

本发明的整体技术效果体现为以下四个方面。

(一)本发明利用载体与目标的地理信息并结合载体姿态，根据空间几何算法，实时计算载体瞄准线相对于载体坐标系的空间角度，然后将该角度量送给伺服控制单元，完成对目标的地理跟踪。因此在目标场景理想，或背景环境与目标物光谱特性对比度差，甚至在目标受到遮挡等情况下，本发明方法都能对目标持续稳定的跟踪。从而显著提高***的光电侦查与目标搜跟能力，进一步发挥惯性导航与稳瞄控制的信息融合、协同作战的作用。

(二)本方明具有光轴与惯性轴重合的特点，所以瞄准线的空间姿态角与稳定转台的空间姿态角完全一致，如此对转台的空间姿态控制实质就是对瞄准线的空间姿态控制，从而通过保证对稳定转台的精确控制实现了瞄准线对目标的精确瞄准。

(三)本发明仅用空间几何算法，再经过一次矩阵计算，就能对跟踪目标瞄准线的空间指令角度完成计算。因此本发明的操作步骤简便，数据计算量小，对***硬件资源要求较低。可在多种武器平台的***中移植应用，具有通用性。

(四)本发明充分利用地理信息和惯性信息进行实时计算，不仅用于光电侦察与目标跟踪***，而且适用于需要地理信息获得跟踪指令的攻击武器***，例如为制导导弹提供一种目标导引方法。

附图说明

图1是本发明地理跟踪方法的操作流程图。

图2是载机、光轴稳定转台、地理坐标系之间的关系示意图。

图3是地理跟踪模型空间几何关系示意图。

具体实施方式

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

本发明地理跟踪方法的优选实施例用于对某固定目标跟踪的无人机载光电探测/跟踪***。该***包括光轴稳定转台、惯性导航单元、伺服控制单元、信息处理单元。其中，光轴稳定转台上的光轴与惯性导航单元的惯性轴重合，信息处理单元包含装有地理跟踪模块的计算机。本方法是通过信息处理单元中的计算机实现的。计算机的硬盘中事先存放有参数扁率e和地球椭球体长轴半径R_e。在无人机飞行过程中，若机载光电探测/跟踪***锁定某一目标，且光电探测/跟踪***接到上级***发出地理跟踪指令时，地理跟踪模块将根据图1所示的流程完成以下操作过程。

第一步，采集目标定位装置输出的目标点数据(λ，L，h)和当前载机位置点的数据(λ₁，L₁，h₁，ψ_A，θ_A，γ_A，S)。其中λ，L，h分别是目标点的经度、纬度和高度；λ₁，L₁，h₁分别是载机的当前经度、纬度和高度，ψ_A，θ_A，γ_A分别是载机在地理坐标系下的当前航向角、俯仰角和横滚角，S是当前载机到目标点的距离。

根据图2所示，地理坐标系n定义为：原点O是载机所在的地球某点处，X轴从原点O指向正东方向，Y轴从原点O指向正北方向，Z轴垂直于地面指向天顶，X轴、Y轴和Z轴构成右手坐标系。载机坐标系A定义为：原点O是飞机质心，X轴沿载机横轴指向右，Y轴沿载机纵轴指向机头，Z轴在纵向对称平面内由机腹指向机背，X轴、Y轴和Z轴构成右手坐标系。

载机航向角ψ_A定义为：载机在地理坐标系下绕Z轴旋转的角度，规定向右旋转为正；载机俯仰角θ_A定义为：载机在地理坐标系下绕X轴旋转的角度，规定向上旋转为正；载机横滚角γ_A定义为：载机在地理坐标系下绕Y轴旋转的角度，规定逆时针旋转为正。

本优选实施例中当前目标点数据是：(λ，L，h)＝(108.8929，34.1504，5.2)

当前载机位置点数据是：

(λ₁，L₁，h₁，ψ_A，θ_A，γ_A，S)＝(108.8552，34.13，1235.3，3.49221°，1.03811°，-0.0002°，4320.5)

第二步，根据目标点和载机位置点的经度λ、λ₁和纬度L、L₁，用以下公式计算载机与目标点在水平面投影中由载机投影到目标点投影矢量线段的北向夹角σ。

$\mathrm{\σ}=\arctan \frac{|\mathrm{\λ}-{\mathrm{\λ}}_1|\×R_M\×\cos L_1}{|L-L_1|\×R_N}$

R_M＝R_e(1-2e+3esin²L₁)

R_N＝R_e(1+esin²L₁)

$e=\frac{1}{298.257}$

R_e＝6378137

其中，R_M是载机当前位置的子午圈曲率半径，R_N是载机当前位置的卯酉圈曲率半径，e是地球的扁率，R_e是地球的长轴半径。

图3是本优选实施例中载机与目标空间位置关系及空间几何计算示意图。该图建立在地理坐标系下，F表示当前载机位置点，T表示目标点，M和N分别是F点和T点在水平面上的投影，NK表示在地理坐标系下与Y轴平行的线段，所以点M、N和K构成水平面上的直角三角形ΔMNK，JT是与MN平行的线段。北向夹角σ在图中为线段MN和线段NK的夹角，即∠MNK。

第三步，根据第二步的计算结果，并结合目标点的经度λ与载机位置点的经度λ₁，用以下判别公式计算瞄准线指向目标点的航向角ψ_b。

当λ＞λ₁且L＞L₁  ψ_b＝σ

当λ＞λ₁且L＜L₁  ψ_b＝180°-σ

当λ＜λ₁且L＜L₁  ψ_b＝180°+σ

当λ＜λ₁且L＞L₁  ψ_b＝-σ

当λ＝λ₁且L＞L₁  ψ_b＝0°

当λ＝λ₁且L＜L₁  ψ_b＝180°

当λ＜λ₁且L＝L₁  ψ_b＝-90°

当λ＞λ₁且L＝L₁  ψ_b＝90°

瞄准线指向目标点的方位角ψ_b定义为：对准目标点的瞄准线在地理坐标系下绕Z轴旋转的角度，规定向右旋转为正。

在本优选实施例中，由第三步计算得到瞄准线指向目标点的航向角ψ_b是：

ψ_b＝56.70638°

第四步，根据目标点和载机位置点的高度h、h₁及载机到目标点之间的距离S，用以下公式计算瞄准线与水平面夹角δ：

$\mathrm{\δ}=\arcsin \frac{|h-h_1|}{S}$

在图3中，线段FM表示载机位置点高度h，线段TN表示目标点高度h₁，线段FT是载机到目标点之间的距离S。瞄准线与水平面夹角δ在图中为线段FT和线段JT的夹角，即∠FTJ。

第五步，根据第四步计算结果，并结合目标点与载机位置点的高度h、h₁，用以下判别公式计算瞄准线指向目标点的俯仰角θ_b：

当h＞h₁  θ_b＝δ

当h＜h₁  θ_b＝-δ

瞄准线指向目标点的俯仰角θ_b定义为：在地理坐标系下对准目标点的瞄准线绕X轴旋转的角度，规定向上旋转为正。

本优选实施例中，由第五步计算得到瞄准线指向目标点的俯仰角θ_b是：

θ_b＝-16.54163°

第六步，根据载机位置点的航向角ψ_A、俯仰角θ_A和横滚角γ_A，用以下公式计算地理坐标系到载机坐标系的姿态矩阵

$C_A^n=\left[\begin{array}{ccc}\cos {\mathrm{\γ}}_A\cos {\mathrm{\ψ}}_A+\sin {\mathrm{\γ}}_A\sin {\mathrm{\ψ}}_A\sin {\mathrm{\θ}}_A& \sin {\mathrm{\ψ}}_A\cos {\mathrm{\θ}}_A& \sin {\mathrm{\γ}}_A\cos {\mathrm{\ψ}}_A-\cos {\mathrm{\γ}}_A\sin {\mathrm{\ψ}}_A\sin {\mathrm{\θ}}_A\\ -\cos {\mathrm{\γ}}_A\sin {\mathrm{\ψ}}_A+\sin {\mathrm{\γ}}_A\cos {\mathrm{\ψ}}_A\sin {\mathrm{\θ}}_A& \cos {\mathrm{\ψ}}_A\cos {\mathrm{\θ}}_A& -\sin {\mathrm{\γ}}_A\sin {\mathrm{\ψ}}_A-\cos {\mathrm{\γ}}_A\cos {\mathrm{\ψ}}_A\sin {\mathrm{\θ}}_A\\ -\sin {\mathrm{\γ}}_A\cos {\mathrm{\θ}}_A& \sin {\mathrm{\θ}}_A& \cos {\mathrm{\γ}}_A\cos {\mathrm{\θ}}_A\end{array}\right]$

第七步，根据第三步和第五步的计算结果，用以下公式计算瞄准线到地理坐标系的姿态矩阵

$C_n^b=\left[\begin{array}{ccc}\cos {\mathrm{\ψ}}_b& -\sin {\mathrm{\ψ}}_b& 0\\ \sin {\mathrm{\ψ}}_b\cos {\mathrm{\θ}}_b& \cos {\mathrm{\ψ}}_b\cos {\mathrm{\θ}}_b& \sin {\mathrm{\θ}}_b\\ -\sin {\mathrm{\ψ}}_b\sin {\mathrm{\θ}}_b& -\cos {\mathrm{\ψ}}_b\sin {\mathrm{\θ}}_b& \cos {\mathrm{\θ}}_b\end{array}\right]$

第八步，根据第六步和第七步的计算结果，用以下公式计算瞄准线到载机坐标系的姿态矩阵

$C_A^b=C_n^b*C_A^n=\left[\begin{array}{ccc}{C}_1\left(\mathrm{1,1}\right)& C_1\left(\mathrm{2,1}\right)& C_1\left(\mathrm{3,1}\right)\\ C_1\left(\mathrm{1,2}\right)& C_1\left(\mathrm{2,2}\right)& C_1\left(\mathrm{3,2}\right)\\ C_1\left(\mathrm{1,3}\right)& C_1\left(\mathrm{2,3}\right)& C_1\left(\mathrm{3,3}\right)\end{array}\right]$

式中，C₁(i，j)且(i＝1，2，3  j＝1，2，3)是矩阵

的各项元素。

第九步，计算载机光轴稳定转台的俯仰角β、方位角α和横滚角χ。

9.1根据第八步计算姿态矩阵

得到的C₁(3，2)、C₁(3，1)、C₁(3，3)、C₁(1，2)和C₁(2，2)，用以下公式计算载机光轴稳定转台的俯仰角β、参考横滚角γ_C和参考方位角ψ_C

β＝arcsinC₁(3，2)

${\mathrm{\γ}}_C=\arctan (-\frac{C_1\left(\mathrm{3,1}\right)}{C_1\left(\mathrm{3,3}\right)})$

${\mathrm{\ψ}}_C=\arctan \left(\frac{C_1\left(\mathrm{1,2}\right)}{C_1\left(\mathrm{2,2}\right)}\right)$

光电探测/跟踪***的光轴稳定转台坐标系b定义为：原点O是光轴稳定转台回转中心，X轴是其主横线，即转动俯仰轴，Y轴沿光轴稳定转台的瞄准线指向瞄准目标，即转动横滚轴，Z轴垂直于XY平面且指向天顶，即转动方位轴，X轴、Y轴和Z轴构成右手坐标系。

载机光轴稳定转台俯仰角β定义为：光轴稳定转台在载机坐标系下绕X轴旋转的角度，规定向上旋转为正。

本优选实施例中，由第八步计算得到的数据是：

C₁(3，2)＝-0.2950682

C₁(3，1)＝0.01450774

C₁(3，3)＝0.955366

C₁(1，2)＝0.7677324

C₁(2，2)＝0.5687895

据此计算得：

β＝-17.16163°

γ_C＝-0.8699°

ψ_C＝53.46638°

9.2根据第9.1步计算得到的参考方位角ψ_C，并结合姿态矩阵

中的C₁(1，2)和C₁(2，2)两项元素，用以下判别公式计算载机光轴稳定转台方位角α：

当C₁(2，2)→0且C₁(1，2)＞0  α＝90°

当C₁(2，2)→0且C₁(1，2)＜0    α＝-90°

当C₁(2，2)＞0                 α＝ψ_C

当C₁(2，2)＜0且C₁(1，2)＞0    α＝ψ_C+180°

当C₁(2，2)＜0且C₁(1，2)＜0    α＝ψ_C-180°

载机光轴稳定转台方位角α定义为：光轴稳定转台在载机坐标系下绕Z轴旋转的角度，规定向右旋转为正；

本优选实施例中，α＝53.46638°

9.3根据第9.1步计算得到的参考横滚角γ_C，并结合姿态矩阵

中的C₁(3，3)，用以下判别公式计算载机光轴稳定转台横滚角χ：

当C₁(3，3)＞0          χ＝γ_C

当C₁(3，3)＜0且γ_C＞0  χ＝γ_C-180°

当C₁(3，3)＜0且γ_C＜0  χ＝γ_C+180°

载机光轴稳定转台横滚角χ定义为：光轴稳定转台在载机坐标系下绕Y轴旋转的角度，规定逆时针旋转为正。

本优选实施例中，χ＝-0.8699°。

第十步，将得到的载机光轴稳定转台方位角α、俯仰角β、横滚角χ传输给伺服控制单元进行角位置伺服控制。

第十一步，重复执行第一步到第十步，直到上级***发出地理跟踪结束指令后结束。

Claims (1)

1.一种基于光轴与惯性轴重合的地理跟踪方法，其特征在于：该方法包括以下操作步骤：

第一步，采集目标定位装置输出的目标点数据(λ，L，h)和当前载机位置点的数据(λ₁，L₁，h₁，ψ_A，θ_A，γ_A，S)，其中λ，L，h分别是目标点的经度、纬度和高度；λ₁，L₁，h₁分别是载机的当前经度、纬度和高度，ψ_A，θ_A，γ_A分别是载机在地里坐标系下的当前航向角、俯仰角和横滚角，S是当前载机到目标点的距离；

第二步，用以下公式计算载机与目标点在水平面投影中由载机投影到目标点投影矢量线段的北向夹角σ：

$\mathrm{\σ}=\arctan \frac{|\mathrm{\λ}-{\mathrm{\λ}}_1|\×R_M\×\cos L_1}{|L-L_1|\×R_N}$

R_M＝R_e(1-2e+3esin²L₁)

R_N＝R_e(1+esin²L₁)

其中，R_M是载机当前位置的子午圈曲率半径，R_N是载机当前位置的卯酉圈曲率半径，e是扁率，R_e是地球椭球体长轴半径；

第三步，用以下公式计算瞄准线指向目标点的航向角ψ_b：

当λ＞λ₁且L＞L₁  ψ_b＝σ

当λ＞λ₁且L＜L₁  ψ_b＝180°-σ

当λ＜λ₁且L＜L₁  ψ_b＝180°+σ

当λ＜λ₁且L＞L₁  ψ_b＝-σ

当λ＝λ₁且L＞L₁  ψ_b＝0°

当λ＝λ₁且L＜L₁  ψ_b＝180°

当λ＜λ₁且L＝L₁  ψ_b＝-90°

当λ＞λ₁且L＝L₁  ψ_b＝90°

第四步，用以下公式计算瞄准线与水平面夹角δ：

$\mathrm{\δ}=\arcsin \frac{|h-h_1|}{S}$

第五步，用以下公式计算瞄准线指向目标点的俯仰角θ_b：

当h＞h₁  θ_b＝δ

当h＜h₁  θ_b＝-δ

第六步，计算地理坐标系到载机坐标系的姿态矩阵

$C_A^n=\left[\begin{array}{ccc}\cos {\mathrm{\γ}}_A\cos {\mathrm{\ψ}}_A+\sin {\mathrm{\γ}}_A\sin {\mathrm{\ψ}}_A\sin {\mathrm{\θ}}_A& \sin {\mathrm{\ψ}}_A\cos {\mathrm{\θ}}_A& \sin {\mathrm{\γ}}_A\cos {\mathrm{\ψ}}_A-\cos {\mathrm{\γ}}_A\sin {\mathrm{\ψ}}_A\sin {\mathrm{\θ}}_A\\ -\cos {\mathrm{\γ}}_A\sin {\mathrm{\ψ}}_A+\sin {\mathrm{\γ}}_A\cos {\mathrm{\ψ}}_A\sin {\mathrm{\θ}}_A& \cos {\mathrm{\ψ}}_A\cos {\mathrm{\θ}}_A& -\sin {\mathrm{\γ}}_A\sin {\mathrm{\ψ}}_A-\cos {\mathrm{\γ}}_A\cos {\mathrm{\ψ}}_A\sin {\mathrm{\θ}}_A\\ -\sin {\mathrm{\γ}}_A\cos {\mathrm{\θ}}_A& \sin {\mathrm{\θ}}_A& \cos {\mathrm{\γ}}_A\cos {\mathrm{\θ}}_A\end{array}\right]$

第七步，计算瞄准线到地理坐标系的姿态矩阵

$C_n^b=\left[\begin{array}{ccc}\cos {\mathrm{\ψ}}_b& -\sin {\mathrm{\ψ}}_b& 0\\ \sin {\mathrm{\ψ}}_b\cos {\mathrm{\θ}}_b& \cos {\mathrm{\ψ}}_b\cos {\mathrm{\θ}}_b& \sin {\mathrm{\θ}}_b\\ -\sin {\mathrm{\ψ}}_b\sin {\mathrm{\θ}}_b& -\cos {\mathrm{\ψ}}_b\sin {\mathrm{\θ}}_b& \cos {\mathrm{\θ}}_b\end{array}\right]$

第八步，计算瞄准线到载机坐标系的姿态矩阵

$C_A^b=C_n^b*C_A^n=\left[\begin{array}{ccc}{C}_1\left(\mathrm{1,1}\right)& C_1\left(\mathrm{2,1}\right)& C_1\left(\mathrm{3,1}\right)\\ C_1\left(\mathrm{1,2}\right)& C_1\left(\mathrm{2,2}\right)& C_1\left(\mathrm{3,2}\right)\\ C_1\left(\mathrm{1,3}\right)& C_1\left(\mathrm{2,3}\right)& C_1\left(\mathrm{3,3}\right)\end{array}\right]$

式中，C₁(i，j)且(i＝1，2，3  j＝1，2，3)是矩阵的各项元素；

第九步，计算载机光轴稳定转台的俯仰角β、方位角α和横滚角χ：

9.1用以下公式计算载机光轴稳定转台的俯仰角β、参考横滚角γ_C和参考方位角ψ_C：

β＝arcsin C₁(3，2)

${\mathrm{\γ}}_C=\arctan (-\frac{C_1\left(\mathrm{3,1}\right)}{C_1\left(\mathrm{3,3}\right)})$

${\mathrm{\ψ}}_C=\arctan \left(\frac{C_1\left(\mathrm{1,2}\right)}{C_1\left(\mathrm{2,2}\right)}\right)$

9.2用以下判别公式计算载机光轴稳定转台方位角α：

当C₁(2，2)→0且C₁(1，2)＞0  α＝90°

当C₁(2，2)→0且C₁(1，2)＜0    α＝-90°

当C₁(2，2)＞0                 α＝ψ_C

当C₁(2，2)＜0且C₁(1，2)＞0    α＝ψ_C+180°

当C₁(2，2)＜0且C₁(1，2)＜0    α＝ψ_C-180°

9.3用以下判别公式计算载机光轴稳定转台横滚角χ：

当C₁(3，3)＞0            χ＝γ_C

当C₁(3，3)＜0且γ_C＞0    χ＝γ_C-180°

当C₁(3，3)＜0且γ_C＜0    χ＝γ_C+180°

第十步，将计算获得的载机光轴稳定转台的方位角α、俯仰角β、横滚角χ传输给伺服控制单元；

第十一步，重复执行第一步到第十步，直到上级***发出地理跟踪结束指令后结束。

Images