[发明专利]基于稀疏直接法的四旋翼无人机单目视觉测程方法

权利要求书

1.基于稀疏直接法的四旋翼无人机单目视觉测程方法，其特征在于：对于关键帧做深度估计：以特征点法确定关键帧的特征点，计算相邻两帧之间的本征矩阵，分解本征矩阵并计算相邻两帧之间的旋转矩阵和平移矩阵得到外参矩阵，然后根据三角法计算特征点深度；获得特征点的深度值之后，通过稀疏矩阵直接法来求解四旋翼无人机的位姿，对所有帧做运动估计：提取稀疏的特征点，用直接法计算每个特征点在下一帧的位置，利用特征点周围固定大小的像素块中各像素点的灰度信息在前后相邻的两帧间的灰度差做优化得到相机的运动位姿；

对于关键帧，其深度估计步骤：

A1:通过普通单目相机获取连续两帧图像，以第一帧作为初始帧，以第二帧作为匹配帧，用FAST特征法来检测特征点特征点数至少为8，并设定相机坐标系下的初始帧中各特征点的坐标形成的位置矩阵p＝[x,y,z]^·和相机坐标系下匹配帧中各特征点的坐标形成的的位置矩阵p'＝[x',y',z']^·；

A2：计算本征矩阵E＝R×T，其中R为旋转矩阵，T为平移向量；本征矩阵和p，p'的关系为：p'^TEp＝0；分别使p和p'除以其z轴坐标得到将本征矩阵E齐次化得到E_scale，由于设定的初始帧特征点数远大于八，利用八点法，结合最小二乘分解求出齐次化的本征矩阵E_scale；

A3：奇异值分解齐次化的本征矩阵E_scale：[U,D,V]＝svd(E_scale),E_scale＝Udiag([1,1,0])V^T,其中U,V均为单位化的向量；本征矩阵E_scale须满足两奇异值相等，第三个值为零的充要条件；

A4：R,T各有两个解、因此R,T有四种组合；

A5：设初始帧坐标系I_r为参考坐标系，参考坐标系下的任意特征点p的齐次坐标为P^q＝[x_w,y_w,z_w,1]^T，I_r的外参矩阵为C_r,由R,T求解得到匹配帧坐标系I_m的外参矩阵C_m,初始帧和匹配帧坐标系与参考坐标系的关系为p＝C_rP^q,p'＝C_mP^q；进而p×C_rP^q＝0,p'×C_mP^q＝0，得到线性方程组

A6：根据步骤A5中的线性方程组通过最小二乘法求解可得P^q，P^q＝[x_w,y_w,z_w,1]^T，将每一组R,T代入分别计算得到P^q，选择z轴坐标全为正的一组作为此时计算的点P^q在初始帧坐标系下的坐标点，求得特征点的深度值。

2.如权利要求1所述的基于稀疏直接法的四旋翼无人机单目视觉测程方法，其特征在于：步骤A3中使本征矩阵E_scale满足两奇异值相等，第三个值为零的充要条件的方法为：求解旋转矩阵R和平移向量T，其中R_Z表示绕z轴旋转90度得到旋转矩阵