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

本发明涉及无人机导航定位技术领域，特别是一种无人机单目视觉测程方法。

背景技术：

四旋翼无人机的实时位姿数据是定位、控制等方面的前提。携带单个或多个相机的载体，仅利用其图像输入对自身运动进行估计的过程叫做视觉测程(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])v^t,其中u,v均为单位化的向量；本征矩阵escale须满足两奇异值相等，第三个值为零的充要条件；

a4：r,t各有两个解、因此r,t有四种组合；

a5：设初始帧坐标系ir为参考坐标系，参考坐标系下的任意特征点p的齐次坐标为p^q＝[xw,yw,zw,1]^t，ir的外参矩阵为由r,t求解得到匹配帧坐标系im的外参矩阵初始帧和匹配帧坐标系与参考坐标系的关系为p＝crp^q,p'＝cmp^q；进而p×crp^q＝0,p'×cmp^q＝0，得到线性方程组

a6：根据步骤a5中的线性方程组通过最小二乘法求解可得p^q，p^q＝[xw,yw,zw,1]^t，将每一组r,t代入分别计算得到p^q，选择z轴坐标全为正的一组作为此时计算的点p^q在初始帧坐标系下的坐标点，求得特征点的深度值。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∈r^4×4；

b3：初始帧中的像素点坐标为匹配帧中的像素点坐标为p^qi＝[p^qixp^qiyp^qiz1]^·，p^qi为关键帧深度估计求得的初始帧中特征点的齐次坐标；其中：k∈r^3×4，k为相机内参矩阵，cx表示相机光轴在图像坐标系中x轴的偏移量，cy表示相机光轴在图像坐标系中y轴的偏移量，fx表示x轴的焦距，fy表示y轴的焦距，k在相机标定中就可确定。

注意这里的像素点坐标为非齐次坐标，因此要引入矩阵d∈r^2×3去掉齐次坐标p^qi的第三项。

b4：r,t在李群上无加法运算，不好求导，因此引入李代数的指数表征形式表征cm＝e^ξ，其中ξ＝(w,v)^·，w＝(w1,w2,w3)^·表征旋转运动，v＝(v1,v2,v3)^·表征平移运动，通过优化观察到相同特征点的对应连续两帧图像的像素的光度误差来求解相机的位姿；

b5:考虑单个像素误差，给定一个微小的扰动e^δξ，得到

定义增量部分则上述公式变为，利用泰勒展开得，

则雅克比矩阵(误差项对自变量的梯度)为

这三项导数分别为：

1.像素梯度

其中为u的增量部分，是一个扰动量；

2.像素点对空间点的导数

对空间展开有其中为p^q的增量部分，是一个扰动量；

3.空间点对李代数增量的导数其中为的非齐次坐标表达形式；

b6：求出了单个像素误差的雅克比矩阵ji,将采用稀疏矩阵直接法，提取特征点后选择在该点处给定范围的像素块来表征点特征点像素灰度值，然后选择空间点在两帧中均可视的那部分对应的在图像中的坐标点累加计算像素误差的雅克比矩阵，记作

b7：给定姿态初始值ξ0，重复步骤3-5，直至求得满足设定阈值条件的四旋翼无人机的位姿ξ*，即得到旋转矩阵和平移矩阵，从而积分后得到四旋翼无人机的位置和姿态信息。

进一步，步骤b4中优化光度误差求解相机位姿的方法包括以下步骤：首先给定光度误差

优化光度误差的二范数的平方，

其中代表特征点p^qi在初始帧和匹配帧图像均可视的区域。

进一步，步骤b6中，给定范围的像素块为4×4的像素块，因该处梯度明显。

针对一般场景下的四旋翼无人机的状态估计问题，本专利提出了一种基于稀疏直接法的单目视觉里程计的设计方法，分为深度估计和运动估计两个部分。在深度估计部分，根据特征点法确定对应特征点，计算两帧之间的本征矩阵，分解本征矩阵，计算两帧间的旋转和平移，得到外参矩阵。然后根据三角法计算特征点深度。在运动估计部分，只提取稀疏的特征点但并不计算描述子，然后只用直接法计算特征点在下一时刻图像的位置，这种方法相较特征法来说去掉了计算描述子的时间，大大提高了计算速率。最后，利用像素的灰度信息在两帧间的灰度差作优化得到相机的运动位姿。对于一般帧，每帧图都会选择特征点附近固定大小的像素块，这样减少了整个图像的特征点检测和优化计算量，大大提高运算的速度。

由于使用特征点方法的匹配在特征的检测和描述子计算上会消耗极大地时间，基于实时性的考虑，并不是在每一帧都使用特征点方法求解位姿，而仅在关键帧时选择使用以下述方法求解特征点深度。关键帧采用每隔一段时间进行，由于两关键帧连续采样间隔较短，两帧之间的深度信息可以假设不变。

本发明的有益效果是：

1、四旋翼飞行器载荷能力有限，其搭载传感器数量和重量均受到限制。仅携带普通相机的四旋翼无人机，尽可能地减少机身本身的重量，延长四旋翼无人机的续航能力。

2、四旋翼飞行器具有快速的机动能力。为满足四旋翼无人机实时地位姿解算，采用稀疏直接法的单目视觉里程计，计算和优化量比传统的特征点法少很多，可以在机载板卡中实时计算，无需地面站实时处理数据，免除了地面站的外部传输，保证了实时性能。

3、采用了稀疏直接法方法，改进了特征点和传统直接法，针对关键帧，采用深度估计，用深度估计获得的深度值对所有帧做运动估计。该设计方法既采取了特征点法的优点，又无需每帧计算特征描述子，提高了计算速率。在实时性能上提升较多，保证了四旋翼无人机飞行过程中的实时位姿估计。

4、采用深度估计与运动估计相结合的稀疏直接法，巧妙地将两者的优势结合起来，相比单纯使用光流信息，避免了累计误差，保证长期较高的准确度。

5、在运动估计部分，对一般帧处理，选择特征点周围固定大小的像素块，避免了常规方法需要计算整个图像的较大计算。

附图说明

图1为初始帧和匹配帧匹配原理图。

图2为本发明深度估计部分的场景图。

图3为本发明运动估计部分的流程图。

具体实施方式

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

由于使用特征点方法的匹配在特征的检测和描述子计算上会消耗极大的时间，基于实时性的考虑，并不是在每一帧都使用特征点方法求解位姿，而仅在关键帧时选择使用以下述方法求解特征点深度。关键帧采用每隔一段时间进行，由于两关键帧连续采样间隔很短，两帧之间的深度信息可以假设不变。

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

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])v^t,其中u,v均为单位化的向量；本征矩阵escale须满足两奇异值相等，第三个值为零的充要条件：求解旋转矩阵r和平移向量t，其中rz表示绕z轴旋转90度得到旋转矩阵

a4：r,t各有两个解、因此r,t有四种组合；

a5：设初始帧坐标系ir为参考坐标系，参考坐标系下的任意特征点p的齐次坐标为p^q＝[xw,yw,zw,1]^t，ir的外参矩阵为由r,t求解得到匹配帧坐标系im的外参矩阵初始帧和匹配帧坐标系与参考坐标系的关系为p＝crp^q,p'＝cmp^q；进而p×crp^q＝0,p'×cmp^q＝0，得到线性方程组

a6：根据步骤a5中的线性方程组通过最小二乘法求解可得p^q，p^q＝[xw,yw,zw,1]^t，将每一组r,t代入分别计算得到p^q，选择z轴坐标全为正的一组作为此时计算的点p^q在初始帧坐标系下的坐标点，求得特征点的深度值。zw即为z轴坐标值。将每一组r,t代入分别计算得到p，选择z轴坐标全为正的一组即可。

然而，由于特征点检测和匹配都存在误差，因此深度估计部分的r,t存在较大误差，采用稀疏直接法来求解更准确。得到了特征点的深度值之后，通过稀疏直接法来求解四旋翼无人机的位姿，即运动估计。这种方法只提取稀疏的特征点但并不计算描述子，然后只用直接法计算特征点在下一时刻图像的位置，这种方法相较特征法来说去掉了计算描述子的时间大大提高了计算速率。

对于相机获取的所有帧图像做运动估计的具体步骤：

b1、获取连续两帧图像，以第一帧作为初始帧，以第二帧作为匹配帧，分别设相机坐标系下初始帧为ir、匹配帧为im，直接法的求解问题：给定初始帧图像和匹配帧图像下的特征点深度，特征点深度由关键帧深度估计获得，求解初始帧到匹配帧的位姿r,t，r为旋转矩阵，t为平移向量；

b2、设ir为世界坐标系，则该帧的外参矩阵为ci＝[i3×303×1]，初始帧到匹配帧的转换矩阵为cm∈r^4×4；

b3、初始帧中的像素点坐标为匹配帧中的像素点坐标为其中：

p^qi＝[p^qixp^qiyp^qiz1]^·，pi^q为关键帧深度估计求得的初始帧中特征点的齐次坐标；k∈r^3×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)^·表征平移运动，通过优化观察到相同特征点的对应连续两帧图像的像素的光度误差来求解相机的位姿；

光度误差的优化方法为：

b4-1、给定光度误差

b4-2、优化光度误差的二范数的平方，

其中代表特征点p^qi在两帧图像均可视的区域；

b5、考虑单个像素误差，给定一个微小的扰动e^δξ，得到

定义增量部分则上述公式变为，利用泰勒展开得，

则雅克比矩阵(误差项对自变量的梯度)为

这三项导数分别为：

1.像素梯度

其中为u的增量部分，是一个扰动量。

2.像素点对空间点的导数

对空间展开有其中为p^q的增量部分，是一个扰动量。

3.空间点对李代数增量的导数其中为的非齐次坐标表达形式。

b6、求出了单个像素误差的雅克比矩阵ji,将采用稀疏矩阵直接法，提取特征点后选择在该点处的4×4的像素块(该处梯度明显)来表征点特征点像素灰度值，然后选择空间点在两帧中均可视的那部分对应的在图像中的坐标点累加计算像素误差的雅克比矩阵，记作

b7、给定姿态初始值ξ0，重复步骤3-5，直至求得满足设定阈值条件的四旋翼无人机的位姿ξ*，即得到旋转矩阵和平移矩阵，从而积分后得到四旋翼无人机的位置和姿态信息。

以上所述仅是本发明的优选实施方式，应当指出，对于本技术领域的一般技术人员来说，本发明还可以有各种更改和变化。在不脱离本发明原理的前提下，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。