一种基于图像标记的无人机视觉姿态和位置解算方法与流程

本发明涉及无人机导航技术领域，尤其涉及一种基于图像标记的无人机视觉姿态和位置解算方法。

背景技术：

无人机导航技术是无人机系统实现自主飞行的关键技术之一。无人机常用的导航技术主要包括惯性导航、gps导航、视觉导航、无线电导航和地形辅助导航等。无人机路径规划、实时避障、精准着陆、着舰和飞行控制等功能的实现需要导航模块提供持续全面的导航信息，其中着舰更是对导航技术要求极高的领域。

无人机自主着舰不同于地面上的着陆。舰船相当于海上不停歇的移动机场，舰船甲板空间狭小，意味着无人机自主着舰控制技术是一项极其复杂且具有挑战性的项目研究。若要实现无人机高精度、安全可靠地着舰，需要充分理解设计无人机下落着舰的各项通道的反馈控制以及高精度的着舰方法。

在自主着舰过程中，由于传统导航方式比如gps等精度不够，无法确定与舰船的相对运动等，会给着舰带来未知的威胁，甚至导致着舰失败。而视觉导航恰好可以弥补此缺陷，提供机、舰相对位置和姿态信息，因此，可以给无人机安装视觉传感器(摄像机)，利用视觉图像来引导无人机着舰，提高着舰精度及自主性。

近年来，随着计算机视觉算法的改进、模式识别理论的进一步完善以及计算机计算和存储能力的提高，视觉图像算法的可靠性和实时性都得到了较大的进步。基于视觉的着舰方式具有自主实时、精度较高、成本低廉、抗干扰能力强等优点。但国内无人机视觉技术引导着舰控制技术研究还尚处于软件仿真和半实物仿真阶段，未能达到实用要求。

技术实现要素：

有鉴于此，本发明提供了一种基于图像标记的无人机视觉姿态和位置解算方法，用以解决基于图像标记的无人机视觉姿态和位置解算问题。

本发明提供的一种基于图像标记的无人机视觉姿态和位置解算方法，包括如下步骤：

s1：采集无人机航拍图像视频流；

s2：利用图像的饱和空间s对采集的图像进行分割，经过色彩空间转换后，通过自适应阈值对图像进行阈值二值化操作，从图像中寻找最大联通区域作为感兴趣区域；

s3：使用一个固定窗口在所述感兴趣区域上进行任意方向上的滑动，比较滑动前后窗口中的像素灰度值；

s4：判断是否任意方向上的滑动都使所述窗口中的像素灰度变化值大于阈值；若是，则所述窗口包含角点，执行步骤s5；若否，则所述窗口不包含角点，返回步骤s1；

s5：从所述感兴趣区域中提取角点坐标；

s6：根据提取的角点坐标，求解单应性矩阵；

s7：根据所述单应性矩阵，基于成像原理求解无人机的相对位置和相对姿态角。

在一种可能的实现方式中，在本发明提供的上述基于图像标记的无人机视觉姿态和位置解算方法中，步骤s5，从所述感兴趣区域中提取角点坐标，具体包括：

harris算子的基本公式如下：

其中，harris算子采用高斯窗口，(u,v)表示沿横纵两个方向的微小滑动量，e(u,v)表示尺寸为n*n的局部检测窗口沿横纵两个方向做微小滑动(u,v)时，窗口的平均能量变化值，n＝3,4,…,10；w(x,y)表示高斯窗口的权重，(x,y)表示当前像素点坐标；i(x+u,y+v)表示窗口滑动后所包含的图像，i(x,y)表示窗口滑动前所包含的图像，ix和iy分别表示图像像素点在水平和垂直方向的梯度；令

其中，m为协方差矩阵，采用如下公式对协方差矩阵m的特征值进行评估：

r＝det(m)-λ(trace(m))²(3)

其中，

det(m)＝λ1λ2(4)

trace(m)＝λ1+λ2(5)

其中，r的值越大，表示角点的响应程度越大；λ＝0.04～0.08；λ1和λ2分别表示矩阵m的特征值；取r最大值的1％为阈值点，在以待判像素点为中心点的n*n矩阵窗口中提取r最大值点作为角点。

在一种可能的实现方式中，在本发明提供的上述基于图像标记的无人机视觉姿态和位置解算方法中，步骤s6中，根据提取的角点坐标，求解单应性矩阵，具体包括：

图像坐标系下图像与世界坐标系下图像的透射关系如下：

其中，表示图像坐标系下点的深度；(xp,yp,1)表示图像坐标系下像素点p的坐标；表示世界坐标系下点的齐次坐标，表示以特征平面为基准的世界坐标系下点的齐次坐标；h为单应性矩阵；

设单应性矩阵h为：

由式(6)与式(7)联立得到：

设h33＝1，则式(8)和式(9)化为：

由于单应性矩阵的自由度为8，因此，至少需要图像坐标系下四个角点坐标(xip,yip)及其对应点在世界坐标系下的齐次坐标求解单应性矩阵；

根据式(12)，利用线性求解方式求解单应性矩阵。

在一种可能的实现方式中，在本发明提供的上述基于图像标记的无人机视觉姿态和位置解算方法中，步骤s7，根据所述单应性矩阵，基于成像原理求解无人机的相对位置和相对姿态角，具体包括：

根据以下公式计算相对姿态角：

其中，表示俯仰角，θ表示横滚角；f表示相机焦距；为的逆矩阵；

在图像坐标系下的π1平面的y轴表示齐次坐标为：

y0＝(1,0,0)^t(15)

单应性矩阵与图像坐标系下π1平面的y轴的表示向量相乘得到：

求得在世界坐标系下的π2平面的直线lw为：

lw＝(h11,h12,h13)^t(17)

利用lw的齐次坐标的性质，求得lw的斜率，为相对偏航角ψ；

下面计算相对位置：

由图像坐标系与世界坐标系的对应关系可知：

其中，(xw,yw)表示世界坐标系下点的坐标；

已知世界坐标系下原点ow的坐标为(0,0,1)^t，根据单应性矩阵对应在图像坐标系下的o'w点的坐标为：

π1平面的像素点p点与π2平面的xw轴无穷远处映射，得到：

光源点oc到o'w点的距离为：

光源点oc到p点的距离为：

设q点在世界坐标系下的坐标为且经过q点投影到π1平面的无穷远处，根据单应性矩阵得到：

其中，(xq,yq)表示q点在图像坐标系下的坐标；根据式(24)得到：

根据三角形相似δoco'wp～δowocq，得到如下等式：

其中，ocow表示光源点oc与ow点之间的距离，oco'w表示光源点oc与o'w点之间的距离，ocp表示光源点oc与p点之间的距离，|xqw|表示xqw的绝对值；根据式(26)得到：

过ow点做垂直于zc轴的直线并交于o”点，oco”为世界坐标系下原点ow在相机坐标系下沿zc方向的位置信息，得到：

其中，oco”表示光源点oc与o”点之间的距离；根据计算的图像坐标系和相机坐标系的转换关系，得到：

其中，(xc,y,zc)表示p点在相机坐标系下的位置，

将式(19)带入式(30)得到：

根据标定相机的焦距和主点坐标、12个目标特征点在世界坐标系下的坐标以及透射到图像坐标系下的坐标，求解出世界坐标系相对于相机坐标系的三个位置距离信息。

本发明提供的上述基于图像标记的无人机视觉姿态和位置解算方法，采集无人机航拍图像视频流，从图像中提取感兴趣区域，从感兴趣区域中提取角点坐标，根据角点坐标求解单应性矩阵，根据单应性矩阵基于成像原理求解无人机的相对位置和相对姿态角。考虑到全局进行角点检测会存在时间较长的问题，因此，利用检索圆环区域，对圆环内部的区域进行角点检测，这样可以减少计算量，减少运算时间，提高位姿确定的实时性，还可以避免圆环外杂点的干扰，提高抗干扰能力，同时采用三个矩形，能够高效地提取目标角点，也便于对于角点进行标记和匹配。本发明提供的上述基于图像标记的无人机视觉姿态和位置解算方法，能够长时间保持较高航向测量精度和较高位置测量精度，适用于着舰等两个运动载体汇合时对无人机相对位置和姿态精度要求较高的应用场合。

附图说明

图1为本发明提供的一种基于图像标记的无人机视觉姿态和位置解算方法的流程图；

图2为视觉图像标志图；

图3感兴趣区域提取流程及结果图；

图4为相对航向角测量图；

图5a为在637mm相对距离下的相对位置测量图；

图5b为在890mm相对距离下的相对位置测量图。

具体实施方式

下面将结合本发明实施方式中的附图，对本发明实施方式中的技术方案进行清楚、完整的描述，显然，所描述的实施方式仅仅是作为例示，并非用于限制本发明。

本发明提供的一种基于图像标记的无人机视觉姿态和位置解算方法，如图1所示，包括如下步骤：

s1：采集无人机航拍图像视频流；

s2：利用图像的饱和空间s对采集的图像进行分割，经过色彩空间转换后，通过自适应阈值对图像进行阈值二值化操作，从图像中寻找最大联通区域作为感兴趣区域；

s3：使用一个固定窗口在感兴趣区域上进行任意方向上的滑动，比较滑动前后窗口中的像素灰度值；

s4：判断是否任意方向上的滑动都使窗口中的像素灰度变化值大于阈值；若是，则窗口包含角点，执行步骤s5；若否，则窗口不包含角点，返回步骤s1；

s5：从感兴趣区域中提取角点坐标；

s6：根据提取的角点坐标，求解单应性矩阵；

s7：根据单应性矩阵，基于成像原理求解无人机的相对位置和相对姿态角。

下面通过一个具体的实施例对本发明提供的上述基于图像标记的无人机视觉姿态和位置解算方法的具体实施进行详细说明。

实施例1：

第一步：视觉标记的特征标志提取，细分为如下步骤：

(1)采集无人机航拍图像视频流

(2)提取感兴趣区域

考虑到全局的角点提取的时间较长，利用感兴趣区域来缩短遍历全局的特征角点检测。在hsv的颜色空间下，其中，h代表色调，其范围为0～360°；v代表图像的亮度，其值通常取0～1，1(或者100％)表示亮度最大，0表示亮度最暗；s代表饱和度，表示颜色接近光谱色的程度，表示其与所对应亮度空间的比值，其值取为0～1，当1(或者100％)时表示其饱和度最大，对应图像色彩最为鲜艳，0表示只有亮度对应的灰度值。本发明利用图像的饱和空间s，在理想情况下，利用s空间对采集的图像进行分割，可以有效地将图像的色彩饱和区域较高的区域进行提取。原始图像经过色彩空间转换后，通过自适应阈值对图像进行阈值二值化操作，从图像中寻找最大联通区域作为感兴趣区域。

图2给出了视觉图像标志图，充分考虑到全局进行角点检测时会存在时间较长的问题，因此，利用检索圆环区域(圆环区域的颜色可以是红色、蓝色或黑色等任意单一颜色)，对圆环内部的区域进行角点检测，这样做如下优势：a)可以减少计算量，从而可以减少运算时间，提高位姿确定的实时性；b)可以避免圆环外杂点的干扰，使抗干扰的能力大幅度提高；c)同时采用三个矩形，能够高效地提取目标角点，也便于对于角点进行标记和匹配。

图3给出了提取感兴趣区域的流程和结果，首先获得饱和度v空间图像，据此进行图像二值化处理，根据圆环区域提取感兴趣区域(roi)，再在灰度图上进行区域分割，最终得到roi区域图像。

(3)提取感兴趣区域内特征角点

特征角点具有位姿估计所需要的特征信息，并且其本身具有旋转不变性，能够有效地减少遍历全局信息的数据量，从而提高运算速度，并且信息的可靠匹配也能得到保证，使得实时处理成为可能。harris特征角点检测是指使用一个固定窗口在感兴趣区域上进行任意方向上的滑动，比较滑动前与滑动后两种情况下窗口中像素灰度的变化程度，如果存在任意方向上的滑动都使所述窗口中的像素灰度变化值大于阈值，即存在任意方向上的滑动都使窗口中的像素有着较大的灰度变化，那么可以认为该窗口包含角点。

harris算子的基本公式如下：

其中，harris算子采用高斯窗口，(u,v)表示沿横纵两个方向的微小滑动量，e(u,v)表示尺寸为n*n的局部检测窗口沿横纵两个方向做微小滑动(u,v)时，窗口的平均能量变化值，n＝3,4,…,10；w(x,y)表示高斯窗口的权重，(x,y)表示当前像素点坐标；i(x+u,y+v)表示窗口滑动后所包含的图像，i(x,y)表示窗口滑动前所包含的图像，ix和iy分别表示图像像素点在水平和垂直方向的梯度；令

其中，m为协方差矩阵；对于协方差矩阵m来说，椭圆形的焦点对应于矩阵m的特征值。当特征值都比较大的时候，窗口包含角点；当一个特征值较大，另一个特征值较小的时候，窗口包含边缘；当特征值都比较小的时候，图像的灰度变化较小，窗口不包含角点；采用如下公式对协方差矩阵m的特征值进行评估：

r＝det(m)-λ(trace(m))²(3)

其中，

det(m)＝λ1λ2(4)

trace(m)＝λ1+λ2(5)

其中，r的值越大，表示角点的响应程度越大；λ＝0.04～0.08；λ1和λ2分别表示矩阵m的特征值；取r最大值的1％为阈值点，在以待判像素点为中心点的n*n矩阵窗口中提取r最大值点作为角点。

第二步：基于图像的角点信息确定相对位姿，细分为如下步骤：

(1)求解单应性矩阵

由于特征图像是一个平面，经过透射变换于像平面仍保留其相似的某些特征，因此，可以根据单目相机进行位姿解算。在位姿解算的过程中，提出用单应性矩阵来进行位姿解算。单应性矩阵h是指在透射变换中，一个物体在不同视角透射平面或者在相机模型中，三维物体或者平面到另一个平面的映射。图像坐标系下图像与世界坐标系下图像的透射关系如下：

其中，表示图像坐标系下点的深度；(xp,yp,1)表示图像坐标系下像素点p的坐标；表示世界坐标系下点的齐次坐标，表示以特征平面为基准的世界坐标系下点的齐次坐标；h为单应性矩阵；

设单应性矩阵h为：

由式(6)与式(7)联立得到：

设h33＝1，则式(8)和式(9)化为：

由于单应性矩阵的自由度为8，因此，至少需要图像坐标系下四个角点坐标(xip,yip)及其对应点在世界坐标系下的齐次坐标求解单应性矩阵；

根据式(12)，利用线性求解方式(dlt)求解单应性矩阵。例如，可以给出12组特征匹配点，对于此方程组求解属于超定方程组的求解方式，对于越多的匹配点，运行时间会越长，但与此同时其求解精度也会增加。

(2)基于成像原理求解相对位置和姿态

得到单应性矩阵后，根据以下公式计算相对姿态角：

其中，表示俯仰角，θ表示横滚角；f表示相机焦距；为的逆矩阵；

在图像坐标系下的π1平面的y轴表示齐次坐标为：

y0＝(1,0,0)^t(15)

单应性矩阵与图像坐标系下π1平面的y轴的表示向量相乘得到：

求得在世界坐标系下的π2平面的直线lw为：

lw＝(h11,h12,h13)^t(17)

利用lw的齐次坐标的性质，求得lw的斜率，为相对偏航角ψ；

下面计算相对位置：

由图像坐标系与世界坐标系的对应关系可知：

其中，(xw,yw)表示世界坐标系下点的坐标；

已知世界坐标系下原点ow的坐标为(0,0,1)^t，根据单应性矩阵对应在图像坐标系下的o'w点的坐标为：

π1平面的像素点p点与π2平面的xw轴无穷远处映射，得到：

光源点oc到o'w点的距离为：

光源点oc到p点的距离为：

设q点在世界坐标系下的坐标为(xqw,0,1)，且经过q点投影到π1平面的无穷远处，根据单应性矩阵得到：

其中，(xq,yq)表示q点在图像坐标系下的坐标；根据式(24)得到：

根据三角形相似δoco'wp～δowocq，得到如下等式：

其中，ocow表示光源点oc与ow点之间的距离，oco'w表示光源点oc与o'w点之间的距离，ocp表示光源点oc与p点之间的距离，表示的绝对值；根据式(26)得到：

过ow点做垂直于zc轴的直线并交于o”点，oco”为世界坐标系下原点ow在相机坐标系下沿zc方向的位置信息，即深度信息，得到：

其中，oco”表示光源点oc与o”点之间的距离；根据计算的图像坐标系和相机坐标系的转换关系，得到：

其中，(xc,y,zc)表示p点在相机坐标系下的位置，

将式(19)带入式(30)得到：

根据标定相机的焦距和主点坐标、12个目标特征点在世界坐标系下的坐标以及透射到图像坐标系下的坐标，求解出世界坐标系相对于相机坐标系的三个位置距离信息。

图4给出了相对航向角测量图，结果表明，本发明提供的上述基于图像标记的无人机视觉姿态和位置解算方法，能够长时间保持较高航向测量精度，半物理仿真结果显示，相对航向准确度在1°内，相对航向误差标准差在0.05°内。图5a和图5b分别给出了637mm相对距离和890mm相对距离下的相对位置测量图，结果表明，本发明提供的上述基于图像标记的无人机视觉姿态和位置解算方法，能够长时间保持较高位置测量精度，半物理仿真结果显示，相对距离误差优于1.5％。

本发明提供的上述基于图像标记的无人机视觉姿态和位置解算方法，采集无人机航拍图像视频流，从图像中提取感兴趣区域，从感兴趣区域中提取角点坐标，根据角点坐标求解单应性矩阵，根据单应性矩阵基于成像原理求解无人机的相对位置和相对姿态角。考虑到全局进行角点检测会存在时间较长的问题，因此，利用检索圆环区域，对圆环内部的区域进行角点检测，这样可以减少计算量，减少运算时间，提高位姿确定的实时性，还可以避免圆环外杂点的干扰，提高抗干扰能力，同时采用三个矩形，能够高效地提取目标角点，也便于对于角点进行标记和匹配。本发明提供的上述基于图像标记的无人机视觉姿态和位置解算方法，能够长时间保持较高航向测量精度和较高位置测量精度，适用于着舰等两个运动载体汇合时对无人机相对位置和姿态精度要求较高的应用场合。

显然，本领域的技术人员可以对本发明进行各种改动和变型而不脱离本发明的精神和范围。这样，倘若本发明的这些修改和变型属于本发明权利要求及其等同技术的范围之内，则本发明也意图包含这些改动和变型在内。