CN105021184A - 一种用于移动平台下视觉着舰导航的位姿估计系统及方法 - Google Patents

Abstract

本发明属于无人机自主着舰领域，具体涉及一种用于移动平台下视觉着舰导航的位姿估计系统及方法。本发明通过对合作地标的处理可实时的解算出移动舰船相对于无人机的位姿信息，算法计算量小，实时性好，实施起来简单，为无人机的安全着舰提供了前提；本方法用摄像机作为主要的传感器，可降低研发成本，缩短研发周期。

Description

一种用于移动平台下视觉着舰导航的位姿估计系统及方法

技术领域

本发明属于无人机自主着舰领域，具体涉及一种用于移动平台下视觉着舰导航的位姿估计系统及方法。

背景技术

无人机自主着降能力是自主飞行能力的核心部分。在现有的着陆导航方式中，惯性导航由于需要通过积分运算来得到飞行器大概位置和速度信息，使得误差会随着时间的推移不断增加；GPS(GlobalPositioning System)卫星导航容易受到电子干扰；INS/GPS(INS:InertialNavigation System)组合导航系统只能定位自身的位姿信息,若在特定地点实施降落,即便在提前知晓降落点经纬度的情况下,也难以保证降落的精度。当需要在移动平台上实施自主降落时,除非能获知平台的实时姿态及位置信息,否则无法完成降落任务。由此可知,单独使用INS/GPS组合导航系统并不能提供无人机自主降落所需的导航信息。

现有的基于计算机视觉着陆导航技术主要解决无人机在静止平台上的着陆问题，很少涉及到移动平台上的着舰问题；并且对于获取全部三维位姿信息的情况，现有技术一般采用双目立体视觉对场景进行三维重建而获得目标的位姿信息，采用双目立体视觉方法可以处理大部分的场景，但重建后获得的深度信息可能有深度丢失的情况从而导致位姿信息计算不完整或错误；同时基于双目视觉的三维重建计算量大，实时性不好。

发明内容

本发明的目的是提供一种用于移动平台下视觉着舰导航的位姿估计系统及方法，以便有效的降低开发成本，缩短视觉导航技术的研发周期，实现无人机在移动平台上安全着陆。

本发明的技术方案是：一种用于移动平台下视觉着舰导航的位姿估计方法，具体步骤是：

步骤101：开始对无人机视觉着舰导航进行位姿估计；

步骤102：设计一个由两个长3米、宽1米的矩形组成的黑色“T”型地标；正视地标时，左上角角点定为地标坐标系原点，坐标为(0,0)；

步骤103：由全球定位系统导航和惯性导航组成的组合导航方式引导无人机飞行；

步骤104：通过目标匹配判断无人机是否到达着舰地标上空，若是则执行步骤105，否则执行步骤103，继续引导飞行；

步骤105：摄像机获取图像；

步骤106：对获取的图像进行二值化、形态学运算、轮廓提取处理，为步骤108提取角点做准备；

步骤107：运用直方图匹配算法进行目标匹配；

步骤108：对匹配成功的图像提取8个Harris角点，结果为像素坐标系下y坐标最小的点为初始点，其余各点依轮廓顺时针排列，并保留每个角点的像素坐标；

步骤109：跟踪步骤108提取的角点；

步骤110：判断步骤108得到的角点排列方式，当a、b两点之间距离大于a、h两点之间距离时步骤109所得排序即为正确顺序，当a、b两点之间距离小于a、h两点之间距离时需要对所有点重新排序，遍历轮廓，取x坐标最小的点为初始点，其余各点按轮廓顺时针依次排序；

步骤111：使用步骤110得到的角点像素坐标、初始化得到的角点世界坐标、摄像机参数矩阵、各坐标系之间的关系求解位姿估计值；

步骤112：对由惯性传感器和全球定位系统估计到的位姿数据和视觉算法解算的位姿数据进行融合，得到最终结果；

步骤113：判断位姿估计算法是否完成，如果是，则视觉着舰导航位姿估计算法结束，否则转至步骤103；

步骤114：视觉着舰导航位姿估计算法结束。

一种用于移动平台下视觉着舰导航的位姿估计系统，其特征是包括：

地标设计模块，用于提供需要的地标；

图像处理模块，用于对当前帧图像进行图像处理，使用多边形拟合算法对着陆地标进行多边形拟合并进行直方图匹配，匹配成功后提取8个Harris角点，对所得角点顺时针排序与着陆地标对应后将结果传至位姿解算模块；

数据融合模块，用于提高位姿估计的准确性；

位姿解算模块，用于计算移动舰船相对于无人机的位姿数据；首先计算无人机相对于着陆地标的旋转矩阵和平移矩阵，然后通过欧拉角与旋转矩阵关系得到姿态角估计值，通过平移矩阵和系统坐标系转换关系计算得到位置估计值，最后，用求得的位姿数据减去由惯性传感器和全球定位系统得到的无人机的位姿数据即可得到船相对于无人机的位姿数据，实时估计位姿数据；

角点提取模块，对摄像机模块传送的图像数据进行角点提取，把提取到的8个角点送至角点跟踪模块；

角点跟踪模块，把跟踪的角点送至角点排序模块；

角点排序模块，对所得到地标角点进行排序，与物理坐标系下着陆地标各角点一一对应并将角点序列传送至位姿解算模块。

所述的位姿解算模块使用摄像机内参矩阵、三维空间到二维平面的投影关系计算摄像机坐标系相对于着舰地标坐标系的旋转平移矩阵RT，并利用欧拉角与旋转矩阵关系得到姿态角估计值；使用姿态角估计所计算得到的平移矩阵和系统坐标系转换关系计算得到位置估计值，再用求得的位姿数据减去由惯性传感器和全球定位系统得到的无人机的位姿数据即可得到船相对于无人机的位姿数据，然后经过数据融合模块得到最终数据。

所述的地标设计模块设计了由两个长3米、宽1米的矩形组成的“T”型地标，地标的颜色为黑色，比值长3米、宽1米可以保证8个角点的提取和排序；正视地标时，左上角角点定为地标坐标系原点，坐标为(0,0)，其余各点顺时针排序，坐标依次为(3,0)、(3,1)、(2,1)、(2,4)、(1,4)、(1,1)、(0,1)。

所述图像处理模块，包括图像预处理模块、角点提取模块、角点跟踪模块和角点排序模块；图像预处理模块首先对摄像机拍摄到的图像进行灰度化、二值化、形态学运算、轮廓提取、直方图匹配处理；角点提取模块用于提取“T”型地标的8个Harris角点，从左上角开始顺时针依次标记为a,b,c,d,e,f,g,h；角点跟踪模块是运用高斯光流法跟踪运动中的角点，防止目标丢失；角点排序模块用于将所保留的角点顺序为像素坐标系下y坐标最小的点为初始点，其余各点顺时针依次排列；当角点排序模块所得图像角点a、b两点之间距离大于a、h两点之间距离时的角点排序即为正确顺序；当角点排序所得图像角点a、b两点之间距离小于a、h两点之间距离时需要对所有点重新排序，遍历轮廓，取图像像素坐标系下x坐标最小的点为初始点，其余各点按顺时针依次排序；角点排序模块所得图像角点最终排序结果和物理坐标系下地标各角点一一对应。

所述的位姿解算模块，规定无人机的俯仰角、滚转角、偏航角分别记为Ω₂、α₂，船的俯仰角、滚转角、偏航角分别记为Ω₁、α₁，其中无人机自身的姿态角可通过陀螺仪和加速度计这样的传感器得到，船相对于无人机的姿态角通过位姿解算模块计算得到；可通过根据图像坐标系、摄像机坐标系、机体坐标系和世界坐标系之间的关系可推导出像素坐标系和世界坐标系之间的关系：

如下式

XY^T＝x_d

式中X＝[x_wy_d y_wy_d z_wy_d y_d-x_wx_d-y_wx_d-z_wx_d]是已知的，Y＝[p₁/t_y p₂/t_y P₃/t_y t_x/t_y P₄/t_y P₅/t_y P₆/t_y]是待求参数；(x_d,y_d)是角点像素坐标，(x_w,y_w,z_w)为角点世界坐标；当存在N>＝7个已知点时，方程具有唯一解或最小二乘解；视觉算法中提取了8个角点进行解算，在保证有解的前提下计算量最小；因此把摄像机内参矩阵Μ、8个地标角点物理坐标(x_d,y_d)和世界坐标(x_w,y_w,z_w)，并带入公式从而得到式子Y中的未知参数；从而得到旋转平移矩阵RT；

$RT=\left[\begin{array}{cccc}{P}_1& P_2& P_3& t_x\\ P_4& P_5& P_6& t_y\\ P_7& P_8& P_9& t_z\\ 0& 0& 0& 1\end{array}\right]$

式中T＝[t_x t_y t_z]^T为3×1的平移向量，t_x、t_y、t_z分别是x、y、z方向的平移量；p＝[P_x P_y P_z]为3×3的旋转矩阵，其中p_x＝[P₁ P₄ P₇]^T，P_y＝[P₂ P₅ P₈]^T，P_z＝[P₃ P₆ P₉]^T分别为世界坐标系X_w轴，Y_w轴，Z_w轴在摄像机坐标系中的方向向量，其中p₁、p₄、p₇对应世界坐标系X_w轴在摄像机坐标系中方向向量的x、y、z坐标值，p₂、p₅、p₈对应世界坐标系Y_w轴在摄像机坐标系中方向向量的x、y、z坐标值，p₃、p₆、p₉对应世界坐标系Z_w轴在摄像机坐标系中方向向量的x、y、z坐标值；根据各约束对应关系可得

式中，求得的总俯仰角滚转角Ω、偏航角α；由相对关系知船的姿态角为(Ω-Ω₂ α-α₂ )，位置估计用于计算舰船相对于无人机的位置，已知着舰地标的原点在世界坐标系中的坐标为(x,y,z)和姿态估计所求得的平移向量(t_x,t_y,t_z)，由坐标系转换关系知船位置为(x-t_x,y+t_y,z+t_z)。

所述数据融合模块是利用卡尔曼滤波融合视觉算法解算出的位姿数据和由惯性传感器和全球定位系统得到的数据，提高位姿估计结果的准确性。

本发明的优点是通过对合作地标的处理可实时的解算出移动舰船相对于无人机的位姿信息，算法计算量小，实时性好，实施起来简单，为无人机的安全着舰提供了前提；本方法用摄像机作为主要的传感器，可降低研发成本，缩短研发周期。

下面结合实施例对本发明做进一步的说明：

附图说明

图1一种用于移动平台下视觉着舰导航的位姿估计系统及方法流程图；

图2一种用于移动平台下视觉着舰导航的位姿估计系统及方法结构图；

图3系统坐标系关系图；

图4角点排序结果图。

具体实施方式

如图1所示，一种用于视觉着舰导航的位姿估计方法，具体步骤是：

步骤101：开始对无人机视觉着舰导航进行位姿估计；

步骤102：设计一个由两个长3米、宽1米的矩形组成的黑色“T”型地标；正视地标时，左上角角点定为地标坐标系原点，坐标为(0,0)；

步骤103：由全球定位系统导航和惯性导航组成的组合导航方式引导无人机飞行；

步骤104：通过目标匹配判断无人机是否到达着舰地标上空，若是则执行步骤105，否则执行步骤103，继续引导飞行；

步骤105：摄像机获取图像；

步骤106：对获取的图像进行二值化、形态学运算、轮廓提取处理，为步骤108提取角点做准备；

步骤107：运用直方图匹配算法进行目标匹配；

步骤108：对匹配成功的图像提取8个Harris角点，结果为像素坐标系下y坐标最小的点为初始点，其余各点依轮廓顺时针排列，并保留每个角点的像素坐标；

步骤109：跟踪步骤108提取的角点；

步骤110：判断步骤108得到的角点排列方式，当a、b两点之间距离大于a、h两点之间距离时步骤109所得排序即为正确顺序，当a、b两点之间距离小于a、h两点之间距离时需要对所有点重新排序，遍历轮廓，取x坐标最小的点为初始点，其余各点按轮廓顺时针依次排序；

步骤111：使用步骤110得到的角点像素坐标、初始化得到的角点世界坐标、摄像机参数矩阵、各坐标系之间的关系求解位姿估计值；

步骤112：对由惯性传感器和全球定位系统估计到的位姿数据和视觉算法解算的位姿数据进行融合，得到最终结果；

步骤113：判断位姿估计算法是否完成，如果是，则视觉着舰导航位姿估计算法结束，否则转至步骤103；

步骤114：视觉着舰导航位姿估计算法结束。

无人机的整个着舰过程分为几个阶段，本发明假设无人机依靠全球定位系统导航和惯性导航组成的组合导航方式引导其飞行至着舰地标上空，然后无人机与船以相同的速度在运动，并且地标总是在摄像机图像范围内。

如图2所示，一种用于视觉着舰导航的位姿估计系统，其特征是：包括地标设计模块1、图像处理模块2、位姿解算模块3和数据融合模块4；

地标设计模块1用于提供需要的地标；

图像处理模块2用于对当前帧图像进行图像处理，使用多边形拟合算法对着陆地标进行多边形拟合并进行直方图匹配，匹配成功后提取8个Harris角点，对所得角点顺时针排序与着陆地标对应后将结果传至位姿解算模块3；

数据融合模块4用于提高位姿估计的准确性；

位姿解算模块3用于计算移动舰船相对于无人机的位姿数据；首先计算无人机相对于着陆地标的旋转矩阵和平移矩阵，然后通过欧拉角与旋转矩阵关系得到姿态角估计值，通过平移矩阵和系统坐标系转换关系计算得到位置估计值，最后，用求得的位姿数据减去由惯性传感器和全球定位系统得到的无人机的位姿数据(由其它传感器得到)即可得到船相对于无人机的位姿数据，实时估计位姿数据；

角点提取模块6对摄像机模块传送的图像数据进行角点提取，把提取到的8个角点送至角点跟踪模块7；

角点跟踪模块7把跟踪的角点送至角点排序模块8；

角点排序模块8对所得到地标角点进行排序，与物理坐标系下着陆地标各角点一一对应并将角点序列传送至位姿解算模块3。

所述的位姿解算模块3使用摄像机内参矩阵、三维空间到二维平面的投影关系计算摄像机坐标系相对于着舰地标坐标系的旋转平移矩阵RT，并利用欧拉角与旋转矩阵关系得到姿态估计值；使用姿态估计所计算得到的平移矩阵和系统坐标系转换关系计算得到位置估计值；然后经过数据融合模块4得到最终数据。

所述的地标设计模块1由两个长3米、宽1米的矩形组成的“T”型地标，地标的颜色为黑色，比值长3米、宽1米可以保证8个角点的提取和排序；正视地标时，左上角角点定为地标坐标系原点，坐标为(0,0)，其余各点顺时针排序，坐标依次为(3,0)、(3,1)、(2,1)、(2,4)、(1,4)、(1,1)、(0,1)。

如图4所示，其中，(a)图为初始点X坐标最小，(b)图为初始点Y坐标最小；所述图像处理模块2，包括图像预处理模块5、角点提取模块6、角点跟踪模块7和角点排序模块8；图像预处理模块5首先对摄像机拍摄到的图像进行灰度化、二值化、形态学运算、轮廓提取、直方图匹配处理；角点提取模块6用于提取“T”型地标的8个Harris角点，从左上角开始顺时针依次标记为a,b,c,d,e,f,g,h；角点跟踪模块7是运用高斯光流法跟踪运动中的角点，防止目标丢失；角点排序模块8用于将所保留的角点顺序为像素坐标系下y坐标最小的点为初始点，其余各点顺时针依次排列；当角点排序模块8所得图像角点a、b两点之间距离大于a、h两点之间距离时的角点排序即为正确顺序；当角点排序所得图像角点a、b两点之间距离小于a、h两点之间距离时需要对所有点重新排序，遍历轮廓，取图像像素坐标系下x坐标最小的点为初始点，其余各点按顺时针依次排序；角点排序模块8所得图像角点最终排序结果和物理坐标系下地标各角点一一对应。

如图3所示，其中，(a)图为空间示意图，(b)图为转换关系；所述的位姿解算模块3，规定无人机的俯仰角、滚转角、偏航角分别记为Ω₂、α₂，船的俯仰角、滚转角、偏航角分别记为Ω₁、α₁，其中无人机自身的姿态角可通过陀螺仪和加速度计这样的传感器得到，船相对于无人机的姿态角通过位姿解算模块3计算得到；可通过根据图像坐标系、摄像机坐标系、机体坐标系和世界坐标系之间的关系可推导出像素坐标系和世界坐标系之间的关系：

如下式

XY^T＝x_d

式中X＝[x_wy_d y_wy_d z_wy_d y_d -x_wx_d -y_wx_d -z_wx_d]是已知的，Y＝[p₁/t_y p₂/t_y P₃/t_y t_x/t_y P₄/t_y P₅/t_y P₆/t_y]是待求参数；(x_d,y_d)是角点像素坐标，(x_w,y_w,z_w)为角点世界坐标；当存在N>＝7个已知点时，方程具有唯一解或最小二乘解；视觉算法中提取了8个角点进行解算，在保证有解的前提下计算量最小；因此把摄像机内参矩阵Μ、8个地标角点物理坐标(x_d,y_d)和世界坐标(x_w,y_w,z_w)，并带入公式从而得到式子Y中的未知参数；从而得到旋转平移矩阵RT；

$RT=\left[\begin{array}{cccc}{P}_1& P_2& P_3& t_x\\ P_4& P_5& P_6& t_y\\ P_7& P_8& P_9& t_z\\ 0& 0& 0& 1\end{array}\right]$

式中T＝[t_x t_y t_z]^T为3×1的平移向量，t_x、t_y、t_z分别是x、y、z方向的平移量；p＝[P_x P_y P_z]为3×3的旋转矩阵，其中p_x＝[P₁ P₄ P₇]^T，P_y＝[P₂ P₅ P₈]^T，P_z＝[P₃ P₆ P₉]^T分别为世界坐标系X_w轴，Y_w轴，Z_w轴在摄像机坐标系中的方向向量，其中p₁、p₄、p₇对应世界坐标系X_w轴在摄像机坐标系中方向向量的x、y、z坐标值，p₂、p₅、p₈对应世界坐标系Y_w轴在摄像机坐标系中方向向量的x、y、z坐标值，p₃、p₆、p₉对应世界坐标系Z_w轴在摄像机坐标系中方向向量的x、y、z坐标值；根据各约束对应关系可得

式中，求得的总俯仰角滚转角Ω、偏航角α；由相对关系知船的姿态角为(Ω-Ω₂ α-α₂ )，位置估计用于计算舰船相对于无人机的位置，已知着舰地标的原点在世界坐标系中的坐标为(x,y,z)和姿态估计所求得的平移向量(t_x,t_y,t_z)，由坐标系转换关系知船位置为(x-t_x,y+t_y,z+t_z)。

所述数据融合模块4是利用卡尔曼滤波融合视觉算法解算出位姿数据和惯性传感器及全球定位系统得到的数据，提高位姿估计结果的准确性。

本发明通过地标设计模块1设计出易于识别和处理的地标，摄像机拍摄地标图像并发送图像数据到图像处理模块2；角点提取模块6对摄像机模块传送的图像数据进行角点提取，把提取到的8个角点送至角点跟踪模块7，角点跟踪模块7把跟踪的角点送至角点排序模块8；角点排序模块8对所得到地标角点进行排序，与物理坐标系下着陆地标各角点一一对应并将角点序列传送至位姿解算模块3；位姿解算模块3使用摄像机内参矩阵、三维空间到二维平面的投影关系计算摄像机坐标系相对于着舰地标坐标系的旋转平移矩阵RT，并利用欧拉角与旋转矩阵关系得到姿态估计值；位置估计使用姿态估计所计算得到的平移矩阵和系统坐标系转换关系计算得到位置估计值；然后经过数据融合模块4得到最终数据。

本实施例没有详细叙述的部分属本行业公知的常用手段，这里不一一叙述。以上例举仅是对本发明的举例说明，并不构成对本发明的保护范围的限制，凡是与本发明相同或相似的设计均属于本发明的保护范围之内。

Claims (7)

1.一种用于移动平台下视觉着舰导航的位姿估计方法，具体步骤是：

步骤101：开始对无人机视觉着舰导航进行位姿估计；

步骤102：设计一个由两个长3米、宽1米的矩形组成的黑色“T”型地标；正视地标时，左上角角点定为地标坐标系原点，坐标为(0,0)；

步骤103：由全球定位系统导航和惯性导航组成的组合导航方式引导无人机飞行；

步骤104：通过目标匹配判断无人机是否到达着舰地标上空，若是则执行步骤105，否则执行步骤103，继续引导飞行；

步骤105：摄像机获取图像；

步骤106：对获取的图像进行二值化、形态学运算、轮廓提取处理，为步骤108提取角点做准备；

步骤107：运用直方图匹配算法进行目标匹配；

步骤108：对匹配成功的图像提取8个Harris角点，结果为像素坐标系下y坐标最小的点为初始点，其余各点依轮廓顺时针排列，并保留每个角点的像素坐标；

步骤109：跟踪步骤108提取的角点；

步骤110：判断步骤108得到的角点排列方式，当a、b两点之间距离大于a、h两点之间距离时步骤109所得排序即为正确顺序，当a、b两点之间距离小于a、h两点之间距离时需要对所有点重新排序，遍历轮廓，取x坐标最小的点为初始点，其余各点按轮廓顺时针依次排序；

步骤111：使用步骤110得到的角点像素坐标、初始化得到的角点世界坐标、摄像机参数矩阵、各坐标系之间的关系求解位姿估计值；

步骤112：对由惯性传感器和全球定位系统估计到的位姿数据和视觉算法解算的位姿数据进行融合，得到最终结果；

步骤113：判断位姿估计算法是否完成，如果是，则视觉着舰导航位姿估计算法结束，否则转至步骤103；

步骤114：视觉着舰导航位姿估计算法结束。

2.一种用于移动平台下视觉着舰导航的位姿估计系统，其特征

是包括：

地标设计模块(1)，用于提供需要的地标；

图像处理模块(2)，用于对当前帧图像进行图像处理，使用多边形拟合算法对着陆地标进行多边形拟合并进行直方图匹配，匹配成功后提取8个Harris角点，对所得角点顺时针排序与着陆地标对应后将结果传至位姿解算模块(3)；

数据融合模块(4)，用于提高位姿估计的准确性；

位姿解算模块(3)，用于计算移动舰船相对于无人机的位姿数据；首先计算无人机相对于着陆地标的旋转矩阵和平移矩阵，然后通过欧拉角与旋转矩阵关系得到姿态角估计值，通过平移矩阵和系统坐标系转换关系计算得到位置估计值，最后，用求得的位姿数据减去由惯性传感器和全球定位系统得到的无人机的位姿数据即可得到船相对于无人机的位姿数据，实时估计位姿数据；

角点提取模块(6)，对摄像机模块传送的图像数据进行角点提取，把提取到的8个角点送至角点跟踪模块(7)；

角点跟踪模块(7)，把跟踪的角点送至角点排序模块(8)；

角点排序模块(8)，对所得到地标角点进行排序，与物理坐标系下着陆地标各角点一一对应并将角点序列传送至位姿解算模块(3)。

3.根据权利要求2所述的一种用于移动平台下视觉着舰导航的位姿估计系统，其特征是：所述的位姿解算模块(3)使用摄像机内参矩阵、三维空间到二维平面的投影关系计算摄像机坐标系相对于着舰地标坐标系的旋转平移矩阵RT，并利用欧拉角与旋转矩阵关系得到姿态角估计值；使用姿态角估计所计算得到的平移矩阵和系统坐标系转换关系计算得到位置估计值，再用求得的位姿数据减去由惯性传感器和全球定位系统得到的无人机的位姿数据即可得到船相对于无人机的位姿数据，然后经过数据融合模块(4)得到最终数据。

4.根据权利要求2所述的一种用于移动平台下视觉着舰导航的位姿估计系统，其特征是：所述的地标设计模块(1)设计了由两个长3米、宽1米的矩形组成的“T”型地标，地标的颜色为黑色，比值长3米、宽1米可以保证8个角点的提取和排序；正视地标时，左上角角点定为地标坐标系原点，坐标为(0,0)，其余各点顺时针排序，坐标依次为(3,0)、(3,1)、(2,1)、(2,4)、(1,4)、(1,1)、(0,1)。

5.根据权利要求2所述的一种用于移动平台下视觉着舰导航的位姿估计系统，其特征是：所述图像处理模块(2)，包括图像预处理模块(5)、角点提取模块(6)、角点跟踪模块(7)和角点排序模块(8)；图像预处理模块(5)首先对摄像机拍摄到的图像进行灰度化、二值化、形态学运算、轮廓提取、直方图匹配处理；角点提取模块(6)用于提取“T”型地标的8个Harris角点，从左上角开始顺时针依次标记为a,b,c,d,e,f,g,h；角点跟踪模块(7)是运用高斯光流法跟踪运动中的角点，防止目标丢失；角点排序模块(8)用于将所保留的角点顺序为像素坐标系下y坐标最小的点为初始点，其余各点顺时针依次排列；当角点排序模块(8)所得图像角点a、b两点之间距离大于a、h两点之间距离时的角点排序即为正确顺序；当角点排序所得图像角点a、b两点之间距离小于a、h两点之间距离时需要对所有点重新排序，遍历轮廓，取图像像素坐标系下x坐标最小的点为初始点，其余各点按顺时针依次排序；角点排序模块(8)所得图像角点最终排序结果和物理坐标系下地标各角点一一对应。

6.根据权利要求2所述的一种用于移动平台下视觉着舰导航的位姿估计系统，其特征是：所述的位姿解算模块(3)，规定无人机的俯仰角、滚转角、偏航角分别记为Ω₂、α₂，船的俯仰角、滚转角、偏航角分别记为Ω₁、α₁，其中无人机自身的姿态角可通过陀螺仪和加速度计这样的传感器得到，船相对于无人机的姿态角通过位姿解算模块(3)计算得到；可通过根据图像坐标系、摄像机坐标系、机体坐标系和世界坐标系之间的关系可推导出像素坐标系和世界坐标系之间的关系：

如下式

XY^T＝x_d

式中X＝[x_wy_d y_wy_d z_wy_d y_d -x_wx_d -y_wx_d -z_wx_d]是已知的，Y＝[p₁/t_y p₂/t_y P₃/t_y t_x/t_y P₄/t_y P₅/t_y P₆/t_y]是待求参数；(x_d,y_d)是角点像素坐标，(x_w,y_w,z_w)为角点世界坐标；当存在N>＝7个已知点时，方程具有唯一解或最小二乘解；视觉算法中提取了8个角点进行解算，在保证有解的前提下计算量最小；因此把摄像机内参矩阵Μ、8个地标角点物理坐标(x_d,y_d)和世界坐标(x_w,y_w,z_w)，并带入公式从而得到式子Y中的未知参数；从而得到旋转平移矩阵RT；

$RT=\left[\begin{array}{cccc}{P}_1& P_2& P_3& t_x\\ P_4& P_5& P_6& t_y\\ P_7& P_8& P_9& t_z\\ 0& 0& 0& 1\end{array}\right]$

式中T＝[t_x t_y t_z]^T为3×1的平移向量，t_x、t_y、t_z分别是x、y、z方向的平移量；p＝[P_x P_y P_z]为3×3的旋转矩阵，其中p_x＝[P₁ P₄ P₇]^T，P_y＝[P₂ P₅ P₈]^T，P_z＝[P₃ P₆ P₉]^T分别为世界坐标系X_w轴，Y_w轴，Z_w轴在摄像机坐标系中的方向向量，其中p₁、p₄、p₇对应世界坐标系X_w轴在摄像机坐标系中方向向量的x、y、z坐标值，p₂、p₅、p₈对应世界坐标系Y_w轴在摄像机坐标系中方向向量的x、y、z坐标值，p₃、p₆、p₉对应世界坐标系Z_w轴在摄像机坐标系中方向向量的x、y、z坐标值；根据各约束对应关系可得

式中，求得的总俯仰角滚转角Ω、偏航角α；由相对关系知船的姿态角为位置估计用于计算舰船相对于无人机的位置，已知着舰地标的原点在世界坐标系中的坐标为(x,y,z)和姿态估计所求得的平移向量(t_x,t_y,t_z)，由坐标系转换关系知船位置为(x-t_x,y+t_y,z+t_z)。

7.根据权利要求2所述的一种用于移动平台下视觉着舰导航的位姿估计系统，其特征是：所述数据融合模块(4)是利用卡尔曼滤波融合视觉算法解算出的位姿数据和由惯性传感器和全球定位系统得到的数据，提高位姿估计结果的准确性。