[发明专利]一种基于图像标记的无人机视觉姿态和位置解算方法

权利要求书

1.一种基于图像标记的无人机视觉姿态和位置解算方法，其特征在于，包括如下步骤：

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

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

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

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

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

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

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

步骤S6中，根据提取的角点坐标，求解单应性矩阵，具体包括：

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

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

设单应性矩阵H为：

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

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

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

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

步骤S7，根据所述单应性矩阵，基于成像原理求解无人机的相对位置和相对姿态角，具体包括：

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

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

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

y₀＝(1,0,0)^T (15)

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

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

L_w＝(h₁₁,h₁₂,h₁₃)^T (17)

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

下面计算相对位置：

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

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

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

π₁平面的像素点p点与π₂平面的X_w轴无穷远处映射，得到：

光源点O_c到O'_w点的距离为：

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

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

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

根据三角形相似ΔO_cO'_wp～ΔO_wO_cq，得到如下等式：

其中，O_cO_w表示光源点O_c与O_w点之间的距离，O_cO'_w表示光源点O_c与O'_w点之间的距离，O_cp表示光源点O_c与p点之间的距离，表示的绝对值；根据式(26)得到：

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

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

其中，(X_c,Y,Z_c)表示p点在相机坐标系下的位置，

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

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

2.如权利要求1所述的基于图像标记的无人机视觉姿态和位置解算方法，其特征在于，步骤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)表示窗口滑动前所包含的图像，I_x和I_y分别表示图像像素点在水平和垂直方向的梯度；令

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

R＝det(M)-λ(trace(M))² (3)

其中，

det(M)＝λ₁λ₂ (4)

trace(M)＝λ₁+λ₂ (5)

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