基于稀疏直接法的四旋翼无人机单目视觉测程方法技术

基于稀疏直接法的四旋翼无人机单目视觉测程方法，其特征在于：对于关键帧做深度估计：以特征点法确定关键帧的特征点，计算相邻两帧之间的本征矩阵，分解本征矩阵并计算相邻两帧之间的旋转矩阵和平移矩阵得到外参矩阵，然后根据三角法计算特征点深度；获得特征点的深度值之后，通过稀疏矩阵直接法来求解四旋翼无人机的位姿，对所有帧做运动估计：提取稀疏的特征点，用直接法计算每个特征点在下一帧的位置，利用特征点周围固定大小的像素块中各像素点的灰度信息在前后相邻的两帧间的灰度差做优化得到相机的运动位姿。本发明专利技术具有既能避免累积误差，保持长期较高的准确度，又能降低计算量的优点。

Monocular vision measurement method for four rotor UAV Based on sparse direct method

Sparse direct method of four rotor UAV monocular visual odometry based method, which is characterized in that the key frame depth estimation: feature point method to determine the feature points of key frames, calculate the eigen matrix between two adjacent frames, the eigen matrix decomposition and calculation between two adjacent frames of the rotation matrix and translation matrix get the external parameters matrix, then calculate the feature point depth according to the triangulation; after obtaining feature point depth values, through direct sparse matrix method to solve the pose four rotor UAV, all the frames of the motion estimation: feature extraction and sparse, calculate the position of each feature point in the next frame with the direct method. Do the optimized movement of the camera pose using gray feature points around the gray information of each pixel pixel blocks of fixed size before and after the two adjacent frame difference. The invention has the advantages of avoiding accumulated error, maintaining long term higher accuracy and reducing calculation amount.

【技术实现步骤摘要】
基于稀疏直接法的四旋翼无人机单目视觉测程方法
本专利技术涉及无人机导航定位
，特别是一种无人机单目视觉测程方法。
技术介绍
四旋翼无人机的实时位姿数据是定位、控制等方面的前提。携带单个或多个相机的载体，仅利用其图像输入对自身运动进行估计的过程叫做视觉测程(VisualOdometry,VO)。视觉测程通过感知输入图像的变化，增量地估计载体位姿。视觉测程算法的有效运行要求环境中光照充足，场景纹理足够丰富。单目视觉测程仅利用单个相机作为输入，系统配置简单，适应环境尺度变化的能力强于多目视觉系统。现有的单目视觉测程方法一般利用分为特征点法和直接法。特征法是提取每一帧图像特征中的的稀疏信息集合(如点，直线等)，然后在连续帧间利用特征描述子(有多种定义)匹配，使用对极几何方法鲁棒地恢复相机运动和场景的结构信息，最终通过优化重投影误差来修正相机位姿和场景结构。此方法也存在一些缺点，比如无法适用于缺少特征点的场景；并且它强烈依赖特征检测和匹配阈值的准确性，当出现错匹配时对鲁棒估计技术的需求。直接法是通过图像的光度值直接估计场景结构和相机运动，利用了图像中的所有信息。相较于特征点法，在缺乏纹理特征的环境或者当运动模糊、相机失焦的情况下更具优势。且直接在图像上操作，没有特征检测和特征描述子计算的步骤，相较基于特征的方法来说计算量更小。然而，计算光度误差相较于计算重投影误差，计算任务更加繁重，因为它涉及整个图像区域。本专利技术涉及的术语说明如下：帧：在视觉测程领域，称获得的一幅图像为一帧，如：相机前一时刻获得的图像称为前一帧，相机当前时刻获得的图像称为当前帧，相机获得的连续两幅图像称为相邻帧等。关键帧：由于当前相机的帧率较高，相邻帧之间的位姿变化往往比较小，为了增强运动估计的准确性，一般采取关键帧的策略，即在一定的位姿变化范围内，新得到的图像只与某一特定的帧进行对齐以估计当前的位姿，而只有当超出了一定的范围后，才采取新的特定的帧进行下一阶段的图像对齐，称这些用来进行图像对齐的特定帧为关键帧；参考帧：用来对齐当前图像的帧成为当前图像的参考帧。
技术实现思路
本专利技术的目的在于提供一种既能避免累积误差，保持长期较高的准确度，又能降低计算量的基于稀疏直接法的四旋翼无人机单目视觉测程方法。基于稀疏直接法的四旋翼无人机单目视觉测程方法，其特征在于：对于关键帧做深度估计：以特征点法确定关键帧的特征点，计算相邻两帧之间的本征矩阵，分解本征矩阵并计算相邻两帧之间的旋转矩阵和平移矩阵得到外参矩阵，然后根据三角法计算特征点深度；获得特征点的深度值之后，通过稀疏矩阵直接法来求解四旋翼无人机的位姿，对所有帧做运动估计：提取稀疏的特征点，用直接法计算每个特征点在下一帧的位置，利用特征点周围固定大小的像素块中各像素点的灰度信息在前后相邻的两帧间的灰度差做优化得到相机的运动位姿。进一步，对于关键帧，其深度估计步骤：A1:通过普通单目相机获取连续两帧图像，以第一帧作为初始帧，以第二帧作为匹配帧，用FAST(FeaturesFromAcceleratedSegementTest,加速分段测试特征)特征法来检测特征点特征点数至少为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的外参矩阵为由R,T求解得到匹配帧坐标系Im的外参矩阵初始帧和匹配帧坐标系与参考坐标系的关系为p＝CrPq,p'＝CmPq；进而p×CrPq＝0,p'×CmPq＝0，得到线性方程组A6：根据步骤A5中的线性方程组通过最小二乘法求解可得Pq，Pq＝[xw,yw,zw,1]T，将每一组R,T代入分别计算得到Pq，选择z轴坐标全为正的一组作为此时计算的点Pq在初始帧坐标系下的坐标点，求得特征点的深度值。zw即为z轴坐标值。将每一组R,T代入分别计算得到P，选择z轴坐标全为正的一组即可。进一步，步骤A3中使本征矩阵Escale满足两奇异值相等，第三个值为零的充要条件的方法为：求解旋转矩阵R和平移向量T，其中RZ表示绕z轴旋转90度得到旋转矩阵然而，由于特征点检测和匹配都存在误差，因此深度估计部分的R，T存在较大误差，采用稀疏矩阵直接法来求解更准确。在得到了特征点的深度值之后，通过稀疏直接法对所有帧做运动估计来求解四旋翼无人机的位姿。这种方法只提取稀疏的特征点但并不计算描述子，然后只用直接法计算特征点在下一时刻图像的位置，这种方法相较特征法来说去掉了计算描述子的时间大大提高了计算速率。对于相机获取的所有帧图像做运动估计的具体步骤：B1：获取连续两帧图像，以第一帧作为初始帧，以第二帧作为匹配帧，分别设相机坐标系下初始帧为Ir、匹配帧为Im，直接法的求解问题：给定初始帧图像和匹配帧图像下的特征点深度，特征点深度由关键帧深度估计获得，求解初始帧到匹配帧的位姿R,T，R为旋转矩阵，T为平移向量；B2：初始帧Ir的外参矩阵为Ci＝[I3×303×1]，初始帧到匹配帧的转换矩阵为Cm∈R4×4；B3：初始帧中的像素点坐标为匹配帧中的像素点坐标为Pqi＝[PqixPqiyPqiz1]·，Pqi为关键帧深度估计求得的初始帧中特征点的齐次坐标；其中：K∈R3×4，K为相机内参矩阵，cx表示相机光轴在图像坐标系中x轴的偏移量，cy表示相机光轴在图像坐标系中y轴的偏移量，fx表示x轴的焦距，fy表示y轴的焦距，K在相机标定中就可确定。注意这里的像素点坐标为非齐次坐标，因此要引入矩阵D∈R2×3去掉齐次坐标Pqi的第三项。B4：R,T在李群上无加法运算，不好求导，因此引入李代数的指数表征形式表征Cm＝eξ，其中ξ＝(w,v)·，w＝(w1,w2,w3)·表征旋转运动，v＝(v1,v2,v3)·表征平移运动，通过优化观察到相同特征点的对应连续两帧图像的像素的光度误差来求解相机的位姿；B5:考虑单个像素误差，给定一个微小的扰动eδξ，得到定义增量部分则上述公式变为，利用泰勒展开得，则雅克比矩阵(误差项对自变量的梯度)为这三项导数分别为：1.像素梯度其中为u的增量部分，是一个扰动量；2.像素点对空间点的导数对空间展开有其中为Pq的增量部分，是一个扰动量；3.空间点对李代数增量的导数其中为的非齐次坐标表达形式；B6：求出了单个像素误差的雅克比矩阵Ji,将采用稀疏矩阵直接法，提取特征点后选择在该点处给定范围的像素块来表征点特征点像素灰度值，然后选择空间点在两帧中均可本文档来自技高网...

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

【技术特征摘要】
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度得到旋...

【专利技术属性】
技术研发人员：陈特欢，叶波，
申请(专利权)人：宁波大学，
类型：发明
国别省市：浙江,33