[发明专利]基于全景图像的车载光电视觉引导方法

权利要求书

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

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

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

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

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

XePYePZeP=RneRbn[RcbXcPYcPZcP+XbOcYbOcZbOc]+XeOnYeOnZeOn]]>

Rne=-sinB-sinLcosBcosLcosBcosL-sinLsinBcosLsinB0cosLsinL]]>

Rbn=cosycosr+sinysinpsinrsinycospcosysinr-sinysinpcosr-sinycosr+cosysinpsinrcosycosp-sinysinr-cosysinpcosr-cospsinrsinpcospcosr]]>

Rcb=cosθxcosθz+sinθxsinθysinθz-cosθxsinθz+sinθxsinθycosθzsinθxcosθycosθysinθzcosθycosθz-sinθy-sinθxcosθz+cosθxsinθysinθzsinθxsinθz+cosθxsinθycosθzcosθxcosθy]]>

式中，为相机坐标系原点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中的坐标：

XtPYtPZtP=Rtb′[Rbn′Rne′[XePYePZeP-XeOnYeOnZeOn]-XbOtYbOtZbOt]]]>

Rtb=cosφxcosφz+sinφxsinφysinφz-cosφxsinφz+sinφxsinφycosφzsinφxcosφycosφysinφzcosφycosφz-sinφy-sinφxcosφz+cosφxsinφysinφzsinφxsinφz+cosφxsinφycosφzcosφxcosφy]]>

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

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

β′=arctg(-ZtPYtPsinα′)]]>

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

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

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

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