1.基于稀疏直接法的四旋翼无人机单目视觉测程方法,其特征在于:对于关键帧做深度估计:以特征点法确定关键帧的特征点,计算相邻两帧之间的本征矩阵,分解本征矩阵并计算相邻两帧之间的旋转矩阵和平移矩阵得到外参矩阵,然后根据三角法计算特征点深度;获得特征点的深度值之后,通过稀疏矩阵直接法来求解四旋翼无人机的位姿,对所有帧做运动估计:提取稀疏的特征点,用直接法计算每个特征点在下一帧的位置,利用特征点周围固定大小的像素块中各像素点的灰度信息在前后相邻的两帧间的灰度差做优化得到相机的运动位姿。
2.如权利要求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齐次化得到Escale,由于设定的初始帧特征点数远大于八,利用八点法,结合最小二乘分解求出齐次化的本征矩阵Escale;
A3:奇异值分解齐次化的本征矩阵Escale:[U,D,V]=svd(Escale),Escale=Udiag([1,1,
0])VT,其中U,V均为单位化的向量;本征矩阵Escale须满足两奇异值相等,第三个值为零的充要条件;
A4:R,T各有两个解、因此R,T有四种组合;
A5:设初始帧坐标系Ir为参考坐标系,参考坐标系下的任意特征点p的齐次坐标为Pq=[xw,yw,zw,1]T,Ir的外参矩阵为Cr, 由R,T求解得到匹配帧坐标系Im的外参矩阵Cm, 初始帧和匹配帧坐标系与参考坐标系的关系为p=CrPq,p'=CmPq;进而p×CrPq=0,p'×CmPq=0,得到线性方程组A6:根据步骤A5中的线性方程组通过最小二乘法求解可得Pq,Pq=[xw,yw,zw,1]T,将每一组R,T代入分别计算得到Pq,选择z轴坐标全为正的一组作为此时计算的点Pq在初始帧坐标系下的坐标点,求得特征点的深度值。
3.如权利要求2所述的基于稀疏直接法的四旋翼无人机单目视觉测程方法,其特征在于:步骤A3中使本征矩阵Escale满足两奇异值相等,第三个值为零的充要条件的方法为:求解旋转矩阵R和平移向量T, 其中RZ表示绕z轴旋转90度得到旋转矩阵
4.如权利要求3所述的基于稀疏直接法的四旋翼无人机单目视觉测程方法,其特征在于:对于相机获取的所有帧图像做运动估计的具体步骤:B1:获取连续两帧图像,以第一帧作为初始帧,以第二帧作为匹配帧,分别设相机坐标系下初始帧为Ir、匹配帧为Im,直接法的求解问题:给定初始帧图像和匹配帧图像下的特征点深度,特征点深度由关键帧深度估计获得,求解初始帧到匹配帧的位姿R,T,R为旋转矩阵,T为平移向量;
B2:初始帧Ir的外参矩阵为Ci=[I3×3 03×1],初始帧到匹配帧的转换矩阵为Cm∈R4×4;
B3:初始帧中的像素点坐标为 匹配帧中的像素点坐标为
Pqi=[Pqix Pqiy Pqiz 1]·,Pqi为关键帧深度估计求得的初始帧中特征点的齐次坐标;其中: K∈R3×4,K为相机内参矩阵, cx表示相机光轴在图像坐标系中x轴的偏移量,cy表示相机光轴在图像坐标系中y轴的偏移量,fx表示x轴的焦距,fy表示y轴的焦距,K在相机标定中就可确定;
B4:R,T在李群上无加法运算,不好求导,因此引入李代数的指数表征形式表征Cm=eξ,其中ξ=(w,v)·,w=(w1,w2,w3)·表征旋转运动,v=(v1,v2,v3)·表征平移运动,通过优化观察到相同特征点的对应连续两帧图像的像素的光度误差来求解相机的位姿;
B5:考虑单个像素误差,给定一个微小的扰动eδξ,得到
定义增量部分 则上述公式变为,
利用泰勒展开得,
则雅克比矩阵(误差项对自变量的梯度)为
这三项导数分别为:
1).像素梯度
其中 为u
的增量部分,是一个扰动量;
2).像素点对空间点的导数
对 空间展开有
其中
为Pq的增量部分,是一个扰动量;
3).空间点对李代数增量的导数 其中 为 的非齐次坐标表
达形式;
B6:求出了单个像素误差的雅克比矩阵Ji,将采用稀疏矩阵直接法,提取特征点后选择在该点处给定范围的像素块来表征点特征点像素灰度值,然后选择空间点在两帧中均可视的那部分对应的在图像中的坐标点累加计算像素误差的雅克比矩阵,记作B7:给定姿态初始值ξ0,重复步骤3-5,直至求得满足设定阈值条件的四旋翼无人机的位姿ξ*,即得到旋转矩阵和平移矩阵,从而积分后得到四旋翼无人机的位置和姿态信息。
5.如权利要求6所述的基于稀疏直接法的四旋翼无人机单目视觉测程方法,其特征在于:步骤B4中优化光度误差求解相机位姿的方法包括以下步骤:首先给定光度误差优化光度误差的二范数的平方,其中 代表特征点Pqi在初始帧
和匹配帧图像均可视的区域。
6.如权利要求5所述的基于稀疏直接法的四旋翼无人机单目视觉测程方法,其特征在于:步骤B6中,给定范围的像素块为4×4的像素块。