CN103345266B - 基于全景图像的车载光电视觉引导方法 - Google Patents

Abstract

本发明公开了一种基于全景图像的车载光电视觉引导方法。该方法由置有视觉引导控制软件包的数据处理系统实现，当数据处理系统上电并接收到坐标数据输出脉冲信号后，根据目标选定指令，利用全景图像中的目标坐标数据、车体的位置姿态数据计算出电视跟踪模块视轴指向目标的理论角度数据，并与当前视轴指向数据进行差值运算后，获得视轴调整量并输出至电视跟踪模块伺服装置，由伺服装置调整电视跟踪模块视轴指向目标，从而实现全景图像中目标的自动视觉引导探测功能。本发明解决了车体周围360°全景自动探测的问题，提高了现有车辆光电平台的探测效率。

Description

基于全景图像的车载光电视觉引导方法

技术领域

本发明属于光电探测领域，主要涉及车载用目标引导方法，尤其涉及一种基于全景图像的车载光电视觉引导方法。

背景技术

车载光电系统主要用于实现战场环境的监测与战场目标的识别和跟踪。目前，车载光电系统采用在二维转塔上安装可见光电视、激光测距机、红外热像仪和跟踪伺服装置等多种光电设备组合实现方位360°、俯仰几十度范围内目标的识别和跟踪。

在实际应用中，车载光电系统对目标的识别与跟踪的步骤包括：1.人眼搜索当前电视/红外图像，判断是否存在目标；2.如果存在，人工操作摇杆引导二维转塔使目标位于可见光电视跟踪波门内；3.人工控制摇杆跟踪目标或按下跟踪按钮自动实现对目标的跟踪；4.如需获取目标坐标，则按下测距按钮，由计算机结合当前目标距离数据和转塔方位/俯仰角度计算输出目标坐标值；5.结束对当前目标的跟踪，人工操作摇杆引导二维转塔至车体其他方向，获取新的电视/红外图像，重复上述步骤对目标进行识别和跟踪。在上述对目标的识别与跟踪过程中，由人眼识别目标并引导电视系统跟踪目标容易受到外部环境、个人因素等影响而造成系统对车体360°范围内的目标识别困难且引导效率低。

现有技术中，计算机视觉引导多用于智能交通、工业测量等领域。中国专利申请03111168.8公开了一种自动引导车的视觉引导方法及其自动引导电动车，该专利中所提出的视觉引导方法通过摄像机摄取在地面敷设的运行路径标线、地址编码标识符，与摄像机相联的计算机，通过图像智能化识别，获得车体与运行路径标线的位置偏差和方向偏差参数，修正车辆行驶路线。在该专利中，摄像机获取的是车辆前部已知运行路径和地址编码，从而引导车辆转向机构按规定路线行驶，该方法不适用于存在多个目标需要引导的车载光电探测领域。中国期刊《光电工程》第37卷第4期1～7页公开了一种新型自动激光经纬仪引导跟踪方法，该方法采用精密二维转台和高分辨率相机代替人眼，利用TM5100A的马达驱动功能将经纬仪的望远镜指向相机视场范围，实现引导跟踪功能。该文献所提出的方法应用于工业测量领域，通过使用高分辨率相机引导双站经纬仪对被测物上已标记的目标进行自动测量，仅对双站经纬仪交汇范围内存在的目标实现引导，不涉及全景引导，同样不适用于车载光电探测领域。

发明内容

本发明要解决的技术问题是，针对现有技术存在的问题，为车载光电系统提供一种借助于全景图像自动进行目标视觉引导的方法。

为了解决上述技术问题，本发明提供的视觉引导方法由置有视觉引导控制软件包的数据处理系统实现，当数据处理系统上电后，执行以下操作步骤：

第一步，查询是否收到坐标数据输出脉冲信号，如果为假，等待；如果为真，执行第二步；

第二步，判断是否接收到目标选定指令，如果为假，等待；如果为真，执行第三步；

第三步，同步采集目标P的坐标数据车体位置姿态数据(L,B,h)和(y,p,r)、电视跟踪模块当前视轴指向数据(α,β)并存入缓存，其中：为目标P在相机坐标系c中的坐标；(y,p,r)为车体在导航坐标系n下的航向角、俯仰角和横滚角；(L,B,h)为车体所在位置在WGS84坐标系e中的经度、纬度和高度数据；(α,β)为当前电视跟踪模块视轴在电视坐标系t中对应的方位角和俯仰角；

第四步，根据下式计算目标P在WGS84坐标系e中的坐标：

$\left[\begin{array}{c}{X}_e^P\\ Y_e^P\\ Z_e^P\end{array}\right]=R_n^e{R}_b^n[R_c^b\left[\begin{array}{c}{X}_c^P\\ Y_c^P\\ Z_c^P\end{array}\right]+\left[\begin{array}{c}{X}_b^{O_c}\\ Y_b^{O_c}\\ Z_b^{O_c}\end{array}\right]]+\left[\begin{array}{c}{X}_e^{O_n}\\ Y_e^{O_n}\\ Z_e^{O_n}\end{array}\right]$

$R_n^e=\left[\begin{array}{ccc}-\sin B& -\sin L\cos B& \cos L\cos B\\ \cos L& -\sin L\sin B& \cos L\sin B\\ 0& \cos L& \sin L\end{array}\right]$

$R_b^n=\left[\begin{array}{ccc}\cos y\cos r+\sin y\sin p\sin r& \sin y\cos p& \cos y\sin r-\sin y\sin p\cos r\\ -\sin y\cos r+\cos y\sin p\sin r& \cos y\cos p& -\sin y\sin r-\cos y\sin p\cos r\\ -\cos p\sin r& \sin p& \cos p\cos r\end{array}\right]$

$R_c^b=\left[\begin{array}{ccc}\cos {\mathrm{\θ}}_x\cos {\mathrm{\θ}}_z+\sin {\mathrm{\θ}}_x\sin {\mathrm{\θ}}_y\sin {\mathrm{\θ}}_z& -\cos {\mathrm{\θ}}_x\sin {\mathrm{\θ}}_z+\sin {\mathrm{\θ}}_x\sin {\mathrm{\θ}}_y\sin {\mathrm{\θ}}_z& \sin {\mathrm{\θ}}_x\cos {\mathrm{\θ}}_y\\ \cos {\mathrm{\θ}}_y\sin {\mathrm{\θ}}_z& \cos {\mathrm{\θ}}_y\cos {\mathrm{\θ}}_z& -{\sin \mathrm{\θ}}_y\\ -\sin {\mathrm{\θ}}_x\cos {\mathrm{\θ}}_z+\cos {\mathrm{\θ}}_x\sin {\mathrm{\θ}}_y\sin {\mathrm{\θ}}_z& \sin {\mathrm{\θ}}_x\sin {\mathrm{\θ}}_z+{\cos \mathrm{\θ}}_x\sin {\mathrm{\θ}}_y\cos {\mathrm{\θ}}_z& \cos {\mathrm{\θ}}_x\cos {\mathrm{\θ}}_y\end{array}\right]$

式中，为相机坐标系原点O_c在载车坐标系b中的坐标，由多摄像头视觉感知模块和位置姿态测量模块之间的安装位置偏差确定；为导航坐标系原点O_n在WGS84坐标系e中的坐标；为目标P在WGS84坐标系e中的坐标；(θ_x,θ_y,θ_z)为多摄像头视觉感知模块与位置姿态测量模块之间的基准角度偏差；分别为导航坐标系n到WGS84坐标系e，载车坐标系b到导航坐标系n和相机坐标系c到载车坐标系b的旋转矩阵；

第五步，根据下式计算目标P在电视坐标系t中的坐标：

$\left[\begin{array}{c}{X}_t^P\\ Y_t^P\\ Z_t^P\end{array}\right]=R_t^{b^{\′}}\left[R_b^{n^{\′}}{R}_n^{e^{\′}}\right[\left[\begin{array}{c}{X}_e^P\\ Y_e^P\\ Z_e^P\end{array}\right]-\left[\begin{array}{c}{X}_e^O\\ Y_e^O\\ Z_e^O\end{array}\right]]-\left[\begin{array}{c}{X}_b^{O_t}\\ Y_b^{O_t}\\ Z_b^{O_t}\end{array}\right]]$

$R_t^b=\left[\begin{array}{ccc}\cos {\mathrm{\φ}}_x\cos {\mathrm{\φ}}_z+\sin {\mathrm{\φ}}_x\sin {\mathrm{\φ}}_y\sin {\mathrm{\φ}}_z& -\cos {\mathrm{\φ}}_x\sin {\mathrm{\φ}}_z+\sin {\mathrm{\φ}}_x\sin {\mathrm{\φ}}_y\cos {\mathrm{\φ}}_z& \sin {\mathrm{\φ}}_x\cos {\mathrm{\φ}}_y\\ \cos {\mathrm{\φ}}_y\sin {\mathrm{\φ}}_z& \cos {\mathrm{\φ}}_y\cos {\mathrm{\φ}}_z& -\sin {\mathrm{\φ}}_y\\ -\sin {\mathrm{\φ}}_x\cos {\mathrm{\φ}}_z+\cos {\mathrm{\φ}}_x\sin {\mathrm{\φ}}_y\sin {\mathrm{\φ}}_z& \sin {\mathrm{\φ}}_x\sin {\mathrm{\φ}}_z+\cos {\mathrm{\φ}}_x\sin {\mathrm{\φ}}_y\cos {\mathrm{\φ}}_z& \cos {\mathrm{\φ}}_x\cos {\mathrm{\φ}}_y\end{array}\right]$

式中，为电视坐标系原点O_t在载车坐标系b中的坐标，由位置姿态测量模块和电视跟踪模块之间的安装位置偏差确定；为目标P在电视坐标系t中的坐标；为电视坐标系t到载车坐标系b的旋转矩阵；(φ_x,φ_y,φ_z)为位置姿态测量模块与电视跟踪模块之间的基准角度偏差； 分别为的转置矩阵；

第六步，根据下式计算电视跟踪模块视轴指向目标P时在电视坐标系t中对应的方位角α'和俯仰角β'：

${\mathrm{\β}}^{\′}=\mathrm{arctg}(-\frac{Z_t^P}{Y_t^P}\sin {\mathrm{\α}}^{\′})$

第七步，根据下式计算电视跟踪模块的视轴调整量(Δα,Δβ)，并输出到电视跟踪模块的伺服装置：

Δα＝α-α',Δβ＝β-β'

第八步，查询电视跟踪模块是否输出跟踪锁定脉冲信号，如果为假，返回执行第三步；如果为真，执行第九步；

第九步，判断是否要引导下一个目标，如果为真，返回执行第二步；如果为假，引导程序结束。

本发明在传统以人力实现车载光电系统跟踪与识别基础上，引入了视觉引导算法，即数据处理系统利用全景图像中的目标坐标数据、车体的位置姿态数据和电视跟踪模块当前视轴的指向数据计算出电视跟踪模块视轴指向目标所需要的视轴调整量，电视跟踪模块伺服装置根据这些信息，调整电视跟踪模块视轴指向目标，从而实现全景图像中目标的自动视觉引导探测功能。因此，本发明不需要操作者手动调整，最大程度的利用全景图像的覆盖范围，实现了对目标的自动引导探测，从而使现有车载光电平台的探测效率得到了显著提高。

附图说明

图1是车载光电系统组成示意图。

图2是数据处理系统与外部设备信息交互示意图。

图3是本发明视觉引导方法的操作流程图。

图4是本发明中所涉及的坐标系示意图。

具体实施方式

以下结合附图对本发明的优选实施例进行说明，应当理解，此处所描述的优选实施例仅用于说明和解释本发明，并不用于限定本发明。

正如图1所示，车载光电系统包括：多摄像头视觉感知模块1、前景选取模块2、位置姿态测量模块3、数据处理系统4、电视跟踪模块5和车体6。其中，多摄像头视觉感知模块1、前景选取模块2、位置姿态测量模块3、数据处理系统4和电视跟踪模块5均与车体6固连；多摄像头视觉感知模块1与位置姿态测量模块3、位置姿态测量模块3与电视跟踪模块5之间的安装位置偏差和基准角度偏差在光电系统装调时进行标定；多摄像头视觉感知模块包含多个光学摄像头，其安装可以采用集成式，也可采用分布式，但不限于此。

根据图2所示，多摄像头视觉感知模块和前景选取模块输出全景图像中目标的坐标数据，位置姿态测量模块输出车体的位置姿态数据，电视跟踪模块输出当前视轴的指向数据，上述三组数据同步传输至数据处理系统，由该系统输出角度调整指令到电视跟踪模块，引导电视跟踪模块视轴指向目标。

不难看出，数据处理系统是实现本发明视觉引导方法的核心，数据处理系统中置有视觉引导控制软件包。当数据处理系统上电后，视觉引导控制软件包将按照图3所示的操作流程图，执行以下操作步骤：

第一步，查询是否收到坐标数据输出脉冲信号，如果为假，等待；如果为真，执行第二步。

第二步，等待接收目标选定指令，如果接收到目标选定指令，执行第三步。

第三步，同步采集目标P的坐标数据当前车体位置姿态数据(L,B,h)和(y,p,r)、电视跟踪模块当前视轴指向数据(α,β)并存入缓存，其中：为目标P在相机坐标系c中的坐标，(y,p,r)为车体在导航坐标系n下的航向角、俯仰角和横滚角，(L,B,h)为车体所在位置在WGS84坐标系e中的经度、纬度和高度数据，(α,β)为当前电视跟踪模块视轴在电视坐标系t中对应的方位角和俯仰角。

各坐标系的定义参见图4。

相机坐标系c定义为：原点O_c为多摄像头视觉感知模块质心，Y_c沿模块内摄像头1-1的视轴向外，Z_c沿模块与车体安装面指向天顶，X_c由右手规则确定。

载车坐标系b定义为：原点O_b为位置姿态测量模块几何中心，X_b沿车体横轴向右，即车体前进方向右侧；Y_b沿车体纵轴向前，即车体前进方向；Z_b由右手规则确定。

导航坐标系n定义为：原点O_n与载车坐标系原点O_b重合，X_n沿参考椭球指向东，Y_n沿椭球子午圈方向并指向北，Z_n沿椭球外法线方向指向天顶。

电视坐标系t定义为：原点O_t位于电视跟踪模块二维转塔方位回转轴与视轴的交点，X_t沿方位回转轴编码器的初始零位指向外，Z_t与方位回转轴重合，Y_t由右手规则确定；方位角α为二维转塔绕Z_t旋转的角度，规定向右旋转为正；俯仰角β为二维转塔绕Y_t轴旋转的角度，规定向上旋转为正。

WGS84坐标系e定义为：原点O_t位于地球质心，Z_e指向(国际时间局)BIH1984.0定义的协议地球极(CTP)方向，X_e指向BIH1984.0的零度子午面和CTP赤道的交点，Y_e由右手规则确定，并与其他两轴构成右手地心地固(ECEF)直角坐标系。根据需要，也可将地心地固直角坐标系转换为空间大地坐标(经纬高)。

第四步，根据下式计算目标P在WGS84坐标系e中的坐标。

$\left[\begin{array}{c}{X}_e^P\\ Y_e^P\\ Z_e^P\end{array}\right]=R_n^e{R}_b^n[R_c^b\left[\begin{array}{c}{X}_c^P\\ Y_c^P\\ Z_c^P\end{array}\right]+\left[\begin{array}{c}{X}_b^{O_c}\\ Y_b^{O_c}\\ Z_b^{O_c}\end{array}\right]]+\left[\begin{array}{c}{X}_e^{O_n}\\ Y_e^{O_n}\\ Z_e^{O_n}\end{array}\right]$

$R_n^e=\left[\begin{array}{ccc}-\sin B& -\sin L\cos B& \cos L\cos B\\ \cos L& -\sin L\sin B& \cos L\sin B\\ 0& \cos L& \sin L\end{array}\right]$

$R_b^n=\left[\begin{array}{ccc}\cos y\cos r+\sin y\sin p\sin r& \sin y\cos p& \cos y\sin r-\sin y\sin p\cos r\\ -\sin y\cos r+\cos y\sin p\sin r& \cos y\cos p& -\sin y\sin r-\cos y\sin p\cos r\\ -\cos p\sin r& \sin p& \cos p\cos r\end{array}\right]$

$R_c^b=\left[\begin{array}{ccc}\cos {\mathrm{\θ}}_x\cos {\mathrm{\θ}}_z+\sin {\mathrm{\θ}}_x\sin {\mathrm{\θ}}_y\sin {\mathrm{\θ}}_z& -\cos {\mathrm{\θ}}_x\sin {\mathrm{\θ}}_z+\sin {\mathrm{\θ}}_x\sin {\mathrm{\θ}}_y\sin {\mathrm{\θ}}_z& \sin {\mathrm{\θ}}_x\cos {\mathrm{\θ}}_y\\ \cos {\mathrm{\θ}}_y\sin {\mathrm{\θ}}_z& \cos {\mathrm{\θ}}_y\cos {\mathrm{\θ}}_z& -{\sin \mathrm{\θ}}_y\\ -\sin {\mathrm{\θ}}_x\cos {\mathrm{\θ}}_z+\cos {\mathrm{\θ}}_x\sin {\mathrm{\θ}}_y\sin {\mathrm{\θ}}_z& \sin {\mathrm{\θ}}_x\sin {\mathrm{\θ}}_z+{\cos \mathrm{\θ}}_x\sin {\mathrm{\θ}}_y\cos {\mathrm{\θ}}_z& \cos {\mathrm{\θ}}_x\cos {\mathrm{\θ}}_y\end{array}\right]$

式中，为相机坐标系原点O_c在载车坐标系b中的坐标，由多摄像头视觉感知模块1和位置姿态测量模块3之间的安装位置偏差确定；为导航坐标系原点O_n在WGS84坐标系e中的坐标；为目标P在WGS84坐标系e中的坐标；(θ_x,θ_y,θ_z)为多摄像头视觉感知模块1与位置姿态测量模块3之间的基准角度偏差；分别为导航坐标系n到WGS84坐标系e，载车坐标系b到导航坐标系n和相机坐标系c到载车坐标系b的旋转矩阵。

第五步，根据下式计算目标P在电视坐标系t中的坐标。

$\left[\begin{array}{c}{X}_t^P\\ Y_t^P\\ Z_t^P\end{array}\right]=R_t^{b^{\′}}\left[R_b^{n^{\′}}{R}_n^{e^{\′}}\right[\left[\begin{array}{c}{X}_e^P\\ Y_e^P\\ Z_e^P\end{array}\right]-\left[\begin{array}{c}{X}_e^O\\ Y_e^O\\ Z_e^O\end{array}\right]]-\left[\begin{array}{c}{X}_b^{O_t}\\ Y_b^{O_t}\\ Z_b^{O_t}\end{array}\right]]$

$R_t^b=\left[\begin{array}{ccc}\cos {\mathrm{\φ}}_x\cos {\mathrm{\φ}}_z+\sin {\mathrm{\φ}}_x\sin {\mathrm{\φ}}_y\sin {\mathrm{\φ}}_z& -\cos {\mathrm{\φ}}_x\sin {\mathrm{\φ}}_z+\sin {\mathrm{\φ}}_x\sin {\mathrm{\φ}}_y\cos {\mathrm{\φ}}_z& \sin {\mathrm{\φ}}_x\cos {\mathrm{\φ}}_y\\ \cos {\mathrm{\φ}}_y\sin {\mathrm{\φ}}_z& \cos {\mathrm{\φ}}_y\cos {\mathrm{\φ}}_z& -\sin {\mathrm{\φ}}_y\\ -\sin {\mathrm{\φ}}_x\cos {\mathrm{\φ}}_z+\cos {\mathrm{\φ}}_x\sin {\mathrm{\φ}}_y\sin {\mathrm{\φ}}_z& \sin {\mathrm{\φ}}_x\sin {\mathrm{\φ}}_z+\cos {\mathrm{\φ}}_x\sin {\mathrm{\φ}}_y\cos {\mathrm{\φ}}_z& \cos {\mathrm{\φ}}_x\cos {\mathrm{\φ}}_y\end{array}\right]$

式中，为电视坐标系原点O_t在载车坐标系b中的坐标，由位置姿态测量模块3和电视跟踪模块5之间的安装位置偏差确定；为目标P在电视坐标系t中的坐标；为电视坐标系t到载车坐标系b的旋转矩阵；(φ_x,φ_y,φ_z)为位置姿态测量模块3与电视跟踪模块5之间的基准角度偏差；分别为的转置矩阵。

第六步，根据下式计算电视跟踪模块视轴指向目标P时在电视坐标系t中对应的方位角α'和俯仰角β'：

${\mathrm{\β}}^{\′}=\mathrm{arctg}(-\frac{Z_t^P}{Y_t^P}\sin {\mathrm{\α}}^{\′})$

第七步，根据下式计算电视跟踪模块的视轴调整量(Δα,Δβ)，并输出到电视跟踪模块的伺服装置：

Δα＝α-α',Δβ＝β-β'

第八步，查询电视跟踪模块是否输出跟踪锁定脉冲信号，如果为假，返回执行第三步；如果为真，执行第九步。

第九步，判断是否要引导下一个目标，如果为真，返回执行第二步；如果为假，引导程序结束。

Claims (1)

1.一种基于全景图像的车载光电视觉引导方法，其特征在于：该方法包括以下操作步骤：

第一步，查询是否收到坐标数据输出脉冲信号，如果为假，等待；如果为真，执行第二步；

第二步，判断是否接收到目标选定指令，如果为假，等待；如果为真，执行第三步；

第三步，同步采集目标P的坐标数据车体位置姿态数据(L,B,h)和(y,p,r)、电视跟踪模块当前视轴指向数据(α,β)并存入缓存，其中：为目标P在相机坐标系c中的坐标；(y,p,r)为车体在导航坐标系n下的航向角、俯仰角和横滚角；(L,B,h)为车体所在位置在WGS84坐标系e中的经度、纬度和高度数据；(α,β)为当前电视跟踪模块视轴在电视坐标系t中对应的方位角和俯仰角；

第四步，根据下式计算目标P在WGS84坐标系e中的坐标：

$\left[\begin{array}{c}{X}_e^P\\ Y_e^P\\ Z_e^P\end{array}\right]=R_n^e{R}_b^n[R_c^b\left[\begin{array}{c}{X}_c^P\\ Y_c^P\\ Z_c^P\end{array}\right]+\left[\begin{array}{c}{X}_b^{O_c}\\ Y_b^{O_c}\\ Z_b^{O_c}\end{array}\right]]+\left[\begin{array}{c}{X}_e^{O_n}\\ Y_e^{O_n}\\ Z_e^{O_n}\end{array}\right]$

$R_n^e=\left[\begin{array}{ccc}-\sin B& -\sin L\cos B& \cos L\cos B\\ \cos L& -\sin L\sin B& \cos L\sin B\\ 0& \cos L& \sin L\end{array}\right]$

$R_b^n=\left[\begin{array}{ccc}\cos y\cos r+\sin y\sin p\sin r& \sin y\cos p& \cos y\sin r-\sin y\sin p\cos r\\ -\sin y\cos r+\cos y\sin p\sin r& \cos y\cos p& -\sin y\sin r-\cos y\sin p\cos r\\ -\cos p\sin r& \sin p& \cos p\cos r\end{array}\right]$

$R_c^b=\left[\begin{array}{ccc}\cos {\mathrm{\θ}}_x\cos {\mathrm{\θ}}_z+\sin {\mathrm{\θ}}_x\sin {\mathrm{\θ}}_y\sin {\mathrm{\θ}}_z& -\cos {\mathrm{\θ}}_x\sin {\mathrm{\θ}}_z+\sin {\mathrm{\θ}}_x\sin {\mathrm{\θ}}_y\cos {\mathrm{\θ}}_z& \sin {\mathrm{\θ}}_x\cos {\mathrm{\θ}}_y\\ \cos {\mathrm{\θ}}_y\sin {\mathrm{\θ}}_z& \cos {\mathrm{\θ}}_y\cos {\mathrm{\θ}}_z& -\sin {\mathrm{\θ}}_y\\ -\sin {\mathrm{\θ}}_x\cos {\mathrm{\θ}}_z+\cos {\mathrm{\θ}}_x\sin {\mathrm{\θ}}_y\sin {\mathrm{\θ}}_z& \sin {\mathrm{\θ}}_x\sin {\mathrm{\θ}}_z+\cos {\mathrm{\θ}}_x\sin {\mathrm{\θ}}_y\cos {\mathrm{\θ}}_z& \cos {\mathrm{\θ}}_x\cos {\mathrm{\θ}}_y\end{array}\right]$

式中，为相机坐标系原点O_c在载车坐标系b中的坐标，由多摄像头视觉感知模块(1)和位置姿态测量模块(3)之间的安装位置偏差确定；为导航坐标系原点O_n在WGS84坐标系e中的坐标；为目标P在WGS84坐标系e中的坐标；(θ_x,θ_y,θ_z)为多摄像头视觉感知模块(1)与位置姿态测量模块(3)之间的基准角度偏差；分别为导航坐标系n到WGS84坐标系e，载车坐标系b到导航坐标系n和相机坐标系c到载车坐标系b的旋转矩阵；

第五步，根据下式计算目标P在电视坐标系t中的坐标：

$\left[\begin{array}{c}{X}_t^P\\ Y_t^P\\ Z_t^P\end{array}\right]=R_t^{b^{\′}}\left[R_b^{n^{\′}}{R}_n^{e^{\′}}\right[\left[\begin{array}{c}{X}_e^P\\ Y_e^P\\ Z_e^P\end{array}\right]-\left[\begin{array}{c}{X}_e^{O_n}\\ Y_e^{O_n}\\ Z_e^{O_n}\end{array}\right]]-\left[\begin{array}{c}{X}_b^{O_t}\\ Y_b^{O_t}\\ Z_b^{O_t}\end{array}\right]]$

$R_t^b=\left[\begin{array}{ccc}\cos {\mathrm{\φ}}_x\cos {\mathrm{\φ}}_z+\sin {\mathrm{\φ}}_x\sin {\mathrm{\φ}}_y\sin {\mathrm{\φ}}_z& -\cos {\mathrm{\φ}}_x\sin {\mathrm{\φ}}_z+\sin {\mathrm{\φ}}_x\sin {\mathrm{\φ}}_y\cos {\mathrm{\φ}}_z& \sin {\mathrm{\φ}}_x\cos {\mathrm{\φ}}_y\\ \cos {\mathrm{\φ}}_y\sin {\mathrm{\φ}}_z& \cos {\mathrm{\φ}}_y\cos {\mathrm{\φ}}_z& -\sin {\mathrm{\φ}}_y\\ -\sin {\mathrm{\φ}}_x\cos {\mathrm{\φ}}_z+\cos {\mathrm{\φ}}_x\sin {\mathrm{\φ}}_y\sin {\mathrm{\φ}}_z& \sin {\mathrm{\φ}}_x\sin {\mathrm{\φ}}_z+\cos {\mathrm{\φ}}_x\sin {\mathrm{\φ}}_y\cos {\mathrm{\φ}}_z& \cos {\mathrm{\φ}}_x\cos {\mathrm{\φ}}_y\end{array}\right]$

式中，为电视坐标系原点O_t在载车坐标系b中的坐标，由位置姿态测量模块(3)和电视跟踪模块(5)之间的安装位置偏差确定；为目标P在电视坐标系t中的坐标；为电视坐标系t到载车坐标系b的旋转矩阵；(φ_x,φ_y,φ_z)为位置姿态测量模块(3)与电视跟踪模块(5)之间的基准角度偏差；分别为的转置矩阵；

第六步，根据下式计算电视跟踪模块视轴指向目标P时在电视坐标系t中对应的方位角α'和俯仰角β'：

${\mathrm{\α}}^{\′}=\mathrm{arctg}(-\frac{{Y_t}^P}{X_t^P}),$

${\mathrm{\β}}^{\′}=\mathrm{arctg}(-\frac{{Z_t}^P}{Y_t^P}\sin {\mathrm{\α}}^{\′})$

第七步，根据下式计算电视跟踪模块的视轴调整量(Δα,Δβ)，并输出到电视跟踪模块的伺服装置：

Δα＝α-α',Δβ＝β-β'

第八步，查询电视跟踪模块是否输出跟踪锁定脉冲信号，如果为假，返回执行第三步；如果为真，执行第九步；

第九步，判断是否要引导下一个目标，如果为真，返回执行第二步；如果为假，引导程序结束。