CN108520559B - 一种基于双目视觉的无人机定位导航的方法 - Google Patents

Abstract

本发明公开了一种基于双目视觉的无人机定位导航的方法，根据无人机载控制系统的双目相机获取图像左右视图与相机参数得到矫正后的左右视图进而得到对应像素的深度信息；提取左视图的关键点进行滤除和筛选；然后在当前帧通过光流跟踪寻找匹配关键点集，得到匹配关键点对；根据匹配关键点对计算代价函数，得到最终位姿结果；最后对输入的连续图像帧进行筛选得到关键图像帧，对关键图像帧的关键点集与位姿计算联合代价函数，优化求解代价函数得到更新后的位姿。本发明能够通过双目相机快速获取可靠的深度数据，同时利用光流法快速计算匹配关系，实时性强，算法鲁棒性高，能够在室内外的大中型场景下完成无人机定位导航工作。

Description

一种基于双目视觉的无人机定位导航的方法

技术领域

本发明属于无人机应用技术领域，具体涉及一种基于双目视觉的无人机定位导航的方法。

背景技术

视觉定位导航技术是计算机视觉领域中一个关键技术，在测绘、控制、军事等领域中有着重要的应用。同时涉及了计算机视觉领域中的多项核心技术，如图像处理、模式识别、自动控制、特征工程等等，是一项综合性技术。

利用无人机在空中对地观察时，常常需要解决无人机在未知环境运动时的定位导航问题，即：一个无人机在未知的环境中运动，如何通过对环境的观测确定自身的运动轨迹，同时构建出环境的地图。在基于视觉进行定位导航的过程中，由于相机观察到的场景为二维场景缺乏高度信息，因此单独通过二维场景进行三维地图构建与导航存在较大的不确定性。因此基于视觉的无人机定位导航方法的关键在于图像中高度信息的估计，基于双目相机的双目视觉方法很好的解决了这一问题，通过双目相机恢复出场景中的高度信息从而能够对无人机进行基于双目视觉的定位与导航。

发明内容

本发明所要解决的技术问题在于针对上述现有技术中的不足，提供一种基于双目视觉的无人机定位导航的方法，在室内/室外、动态场景、光照变化下对无人机进行视觉定位导航，同时具有良好实时性能的无人机视觉定位导航方法。

本发明采用以下技术方案：

一种基于双目视觉的无人机定位导航的方法，根据无人机载控制系统的双目相机获取图像左视图、右视图与相机参数得到矫正后的左视图、右视图，通过矫正后的左视图与矫正后的右视图的对应像素匹配关系得到对应像素的深度信息；通过FAST角点算法提取矫正后的左视图关键点，通过非极大值抑制方法对关键点进行滤除，通过卡方检验对关键点进行筛选；然后对上一帧获取到的关键点集，在当前帧通过光流跟踪寻找匹配关键点集，得到匹配关键点对；根据匹配关键点对计算代价函数，通过高斯牛顿法迭代求解代价函数，得到最终位姿结果；最后对输入的连续图像帧进行筛选得到关键图像帧，对关键图像帧的关键点集与位姿计算联合代价函数，优化求解代价函数得到更新后的位姿。

具体的，矫正后的左视图与矫正后的右视图的对应像素匹配关系得到对应像素深度图I_d的步骤如下：

S201、对于矫正后左视图I₀中的任一像素位置p_i，选取p_i的3*3邻域像素集合r_io；

S202、在矫正后右视图I₁中的同样像素位置p_i的7*7邻域R_i中，对每一个像素取3*3邻域像素集合r_i1，对r_i0与r_i1求取绝对差值之和的集合d_i；

S203、步骤S202集合d_i中最小的差值d_min所对应的的像素位置p_min即为p_i的匹配像素点，计算两点的x轴坐标差d，同时计算得到p_i处的深度d_pi；

S204、循环以上步骤直至计算得到深度图I_d。

具体的，任一像素位置p_i处的深度d_pi如下：

其中，f为双目相机给定的焦距，l为双目相机的基线长度。

具体的，对矫正后左视图I₀通过FAST角点算法提取关键点集的步骤如下：

S301、对矫正后左视图I₀通过FAST角点算法提取关键点集P；

S302、对关键点集P中的所有关键点p，获取其3*3邻域范围内是否存在其余关键点，若存在则从关键点集P中将其余关键点删除；

S303、对关键点集P中的所有关键点p，根据卡方检验算法计算其卡方值C_p与卡方阈值K，若C_p＞K，则将C_p对应的关键点p从关键点集P中删除。

具体的，通过卡方检验对关键点进行筛选；判断当前帧是否为初始帧，若是则返回重新进行下一帧双目图像的处理，若不是，则继续对上一帧获取到的关键点集，在当前帧通过光流跟踪寻找匹配关键点集，得到匹配关键点对。

进一步的，通过光流跟踪算法得到关键点对集合M₀的步骤如下：

S501、对于关键点集P₀中的任一点p_i，其像素坐标为(x_i，y_i)，设该点的运动为(Δx_i，Δy_i)，则当前帧的矫正后左视图I₀中的对应点p_j为(x_i+Δx_i，y_i+Δy_i)；

S502、根据对应点构建最小二乘模型：

其中，W为p_i的8*8邻域；

S503、通过高斯牛顿法求解该最小二乘问题，得到Δx_i，Δy_i与p_j，将p_i与p_j存入关键点对集合M₀，重复以上步骤直至完成关键点集P₀中所有关键点的跟踪。

具体的，根据得到的关键点对集合M₀，与上一帧左视图I_0last、当前帧左视图I_0now，求解当前帧的对应位姿T_i，具体步骤如下：

S601、对于关键点对集合M₀中的任一点对m_i，提取关键点对p_i(x_i，y_i)、p_j(x_j，y_j)，设T为当前帧的对应位姿；

S602、构建最小二乘模型：

其中，T×I_0now(x_j，y_j)为矩阵乘法，将p_j投影到上一帧左视图I_0last中，I_0last()、I_0now()表示从图像中获取对应像素坐标的像素值；

S603、通过高斯牛顿法求解该最小二乘问题，得到T。

具体的，判断当前帧是否为关键帧，若是则存入关键帧集合Q，若该帧为关键帧，则进行联合位姿优化，否则继续算法循环获取深度图I_d。

进一步的，关键帧判断具体如下：

若上一帧位姿T_last与当前帧位姿T_now之间的平移变换t大于阈值th_t，则该帧为关键帧；

若上一帧位姿T_last与当前帧位姿T_now之间的旋转变换R大于阈值th_R，则该帧为关键帧；

若上一帧得到的关键点集P_last中关键点的数量n_last与当前帧得到的关键点集P_now中关键点的数量n_now比值大于阈值th_n，则该帧为关键帧。

具体的，根据得到的关键帧集合Q，通过局部光束平差法进行联合位姿优化，联合优化完成后，继续算法循环获取深度图I_d，联合位姿优化的具体步骤如下：

S801、对于关键帧集合Q中的每一关键帧Q_i，以及其所对应的关键点对M_i，对于关键点对集合M_i中的任一点对m_i，提取关键点对p_i(x_i，y_i)、p_j(x_j，y_j)，设T_i为点对中p_i对应帧的位姿，T_j为点对中p_j对应帧的位姿；

S802、构建最小二乘模型：

其中，I_0last为p_i对应帧的左视图，I_0now为p_j对应帧的左视图，I_0last()、I_0now()表示从图像中获取对应像素坐标的像素值；

S803、通过高斯牛顿法求解该最小二乘问题，得到T_i与T_j。

与现有技术相比，本发明至少具有以下有益效果：

本发明一种基于双目视觉的无人机定位导航的方法，根据双目相机获取图像左视图、右视图与相机参数得到矫正后的左视图、右视图，通过矫正后的左视图与矫正后的右视图的对应像素匹配关系得到对应像素的深度信息；利用双目视觉方法，改善了由于单目相机无法准确估计目标深度的问题，提高了视觉定位导航的精度，通过FAST角点算法提取矫正后的左视图关键点，通过非极大值抑制方法对关键点进行滤除，通过卡方检验对关键点进行筛选；然后对上一帧获取到的关键点集，在当前帧通过光流跟踪寻找匹配关键点集，得到匹配关键点对；根据匹配关键点对计算代价函数，通过高斯牛顿法迭代求解代价函数，得到最终位姿结果；最后对输入的连续图像帧进行筛选得到关键图像帧，对关键图像帧的关键点集与位姿计算联合代价函数，优化求解代价函数得到更新后的位姿，能够在光照变化、动态场景以及室内/室外不同情况下进行稳定的视觉导航，同时可以应用与多种不同场景与平台，具有稳定性强、实时性好、抗干扰能力强等优点。

进一步的，根据左视图、右视图获取深度图，相比于一般单目相机通过计算多帧视差获取深度图的方法，能够更加有效的利用图像信息，在较短的时间内获取精度较高的深度图，提高后续算法的性能。

进一步的，通过FAST角点算法提取关键点的方法，相比于直接使用像素点作为关键点的方法具有更强的局部描述、选择能力，相比于SIFT等特征点提取方法具有更快的处理速度与实时性，在性能与实时性中取得了较好的平衡。

进一步的，通过对关键点进行筛选判断当前帧是否为初始帧的方法，能够更加有效的提取出图像质量较高的帧作为初始帧，而初始帧的选择对于后续算法的实施有着非常重要的作用，直接影响整体算法的精度。

进一步的，通过光流跟踪算法得到关键点对匹配的方法，相比于直接特征点、描述子匹配的方法具有不需要求取特征点、匹配点对一一对应、计算速度快等优点，能够在较快的时间内取得较好的匹配关键点对结果。

进一步的，加入局部光束平差法对多帧图像位姿进行优化，提升了长时定位的稳定性与可靠性，降低了误差累积，提升了后续定位导航的准确度。

综上所述，本发明通过双目视觉与无人机的结合，在计算能力有限的机载计算单元上能够较为稳定的根据双目相机图像数据对无人机进行位姿解算，并应用于无人机定位与导航任务上去。本发明的主要优势在于能够通过双目相机快速获取可靠的深度数据，同时利用光流法快速计算匹配关系，实时性强，算法鲁棒性高，能够在室内、外的大中型场景下完成无人机定位导航工作。下面通过附图和实施例，对本发明的技术方案做进一步的详细描述。

附图说明

图1为定位导航方法流程图；

图2为无人机载控制系统的组成框图；

图3为本发明所使用的双目相机的示意图；

图4为本发明方法对于室外场景的运行效果图。

具体实施方式

本发明提供了一种基于双目视觉的无人机定位导航的方法，通过用户交互输入初始跟踪目标框并优化，建立以相关滤波为基础的跟踪模型与以随机森林分类器为基础的目标丢失找回模块，实现一种长时稳定的无人机目标跟踪方法。

请参阅图2，无人机载控制系统包括陀螺仪、加速度计、气压计、云台、相机、飞控模块、通信模块和机载计算单元，加速度计用于测量无人机的三轴加速度；陀螺仪用于测量无人机的横滚、偏航、俯仰角；气压计用于获得无人机的飞行高度；加速度计、陀螺仪和气压计分别与机载计算单元连接，相机经由云台与机载计算单元连接，相机用于拍摄图像；云台用于连接相机与无人机并提供一定的稳定作用；通信模块用于无人机与地面站、遥控器传输信息；飞控模块负责接收指令控制飞机运行姿态；机载计算单元为机载计算机，用于处理飞控、图像等信息，机载计算单元分别与飞控模块和通信模块连接，加速度计、陀螺仪、气压计、相机的信息传输给机载计算单元后由其进行目标跟踪处理，并将解算后的目标偏移量和尺度信息传输给飞控模块，进而控制飞机主动跟踪目标。

请参阅图1，本发明提出一种基于双目视觉的无人机定位导航的方法，根据双目相机获取图像左视图、右视图与相机参数得到矫正后的左视图、右视图，通过矫正后的左视图与矫正后的右视图的对应像素匹配关系得到对应像素的深度信息；通过FAST角点算法提取矫正后的左视图关键点，通过非极大值抑制方法对关键点进行滤除，通过卡方检验对关键点进行筛选；对上一帧获取到的关键点集，在当前帧通过光流跟踪寻找匹配关键点集，得到匹配关键点对；根据匹配关键点对计算代价函数，通过高斯牛顿法迭代求解代价函数，得到最终位姿结果；最后对输入的连续图像帧进行筛选得到关键图像帧，对关键图像帧的关键点集与位姿计算联合代价函数，优化求解代价函数得到更新后的位姿。具体步骤如下：

S1、通过无人机载控制系统的相机连续拍摄地面图像，经过相机参数修正获取矫正后的左视图I₀，矫正后的右视图I₁；

请参阅图3，为所使用的双目相机的示意图，左右相机固定在同一平面上，且与机载计算单元的位置固定，图中Fw为世界坐标系，Fs为无人机机体坐标系，Fc0为双目相机中左目相机坐标系，Fc1为双目相机中右目相机坐标系。

S2、根据I₀、I₁，获取深度图I_d，该过程的具体步骤如下：

S201、对于I₀中的任一像素位置p_i，选取p_i的3*3邻域像素集合r_ic；

S202、在I₁中的同样像素位置p_i的7*7邻域R_i中，对每一个像素取3*3邻域像素集合r_i1，对r_i0与r_i1求取绝对差值之和的集合d_i；

S203、集合d_i中最小的差值d_min所对应的的像素位置p_min即为p_i的匹配像素点，计算两点的x轴坐标差d，同时计算得到p_i处的深度d_pi，公式如下：

其中，f为双目相机给定的焦距，l为双目相机的基线长度；

S204、循环如上步骤直至计算得到深度图I_d；

S3、对矫正后左视图I₀通过FAST角点算法提取关键点集，具体步骤如下：

S301、对矫正后左视图I₀通过FAST角点算法提取关键点集P；

S302、对关键点集P中的所有关键点p，获取其3*3邻域范围内是否存在其余关键点，若存在则从关键点集P中将其余关键点删除；

S303、对关键点集P中的所有关键点p，根据卡方检验算法计算其卡方值C_p与卡方阈值K，若C_p＞K则将C_p对应的关键点p从关键点集P中删除；

S4、判断当前帧是否为初始帧，若是则返回S1步骤继续下一帧双目图像的处理，若否则进入下一步骤S5；

S5、根据步骤S3中得到的上一帧左视图I_0last关键点集P₀与当前帧的左视图I_0now，通过光流跟踪算法得到关键点对集合M₀，具体步骤如下：

S501、对于关键点集P₀中的任一点p_i，其像素坐标为(x_i，y_i)，设该点的运动为(Δx_i，Δy_i)，则当前帧的矫正后左视图I₀中的对应点p_j为(x_i+Δx_i，y_i+Δy_i)；

S502、根据对应点构建最小二乘模型：

其中，W为p_i的8*8邻域；

S503、通过高斯牛顿法求解该最小二乘问题，得到Δx_i，Δy_i与p_j，将p_i与p_j存入关键点对集合M₀，重复如上步骤直至完成关键点集P₀中所有关键点的跟踪；

S6、根据步骤S5中得到的关键点对集合M₀，与上一帧左视图I_0last、当前帧左视图I_0now，求解当前帧的对应位姿T_i，具体步骤如下：

S601、对于关键点对集合M₀中的任一点对m_i，提取关键点对p_i(x_i，y_i)、p_j(x_j，y_j)，设T为当前帧的对应位姿；

S602、构建最小二乘模型：

其中T×I_0now(x_j，y_j)为矩阵乘法，将p_j投影到上一帧左视图I_0last中，I_0last()、I_0now()表示从图像中获取对应像素坐标的像素值；

S603、通过高斯牛顿法求解该最小二乘问题，得到T。

S7、判断当前帧是否为关键帧，若是则存入关键帧集合Q，判断如下：

若上一帧位姿T_last与当前帧位姿T_now之间的平移变换t大于阈值th_t，则该帧为关键帧；

若上一帧位姿T_last与当前帧位姿T_now之间的旋转变换R大于阈值th_R，则该帧为关键帧；

若上一帧得到的关键点集P_last中关键点的数量n_last与当前帧得到的关键点集P_now中关键点的数量n_now比值大于阈值th_n，则该帧为关键帧；

若该帧为关键帧，则进入步骤S8，否则进入步骤S2，继续算法循环。

S8、根据步骤S7中得到的关键帧集合Q，通过局部光束平差法进行联合位姿优化，具体步骤如下：

S801、对于关键帧集合Q中的每一关键帧Q_i，以及其所对应的关键点对M_i，对于关键点对集合M_i中的任一点对m_i，提取关键点对p_i(x_i，y_i)、p_j(x_j，y_j)，设T_i为点对中p_i对应帧的位姿，T_j为点对中p_j对应帧的位姿；

S802、构建最小二乘模型：

其中，I_0last为p_i对应帧的左视图，I_0now为p_j对应帧的左视图，I_0last()、I_0now()表示从图像中获取对应像素坐标的像素值；

S803、通过高斯-牛顿法求解该最小二乘问题，得到T_i与T_j；

联合优化完成后，进入步骤S2，继续算法循环。

如图4所示，对于室外场景，双目相机能够较为准确的估计出场景的实际深度(右下窗口为深度图)，为后续的位姿解算提供有力帮助，有效提高了视觉导航的精度与稳定性。同时该方法能够在特征显著的图像区域进行关键点提取(见图4右上)，增强了对于复杂场景的描述能力，最终解算出的位姿轨迹较好的还原了无人机运动的路径(见图4左下)。

以上内容仅为说明本发明的技术思想，不能以此限定本发明的保护范围，凡是按照本发明提出的技术思想，在技术方案基础上所做的任何改动，均落入本发明权利要求书的保护范围之内。

Claims (9)

1.一种基于双目视觉的无人机定位导航的方法，其特征在于，根据无人机载控制系统的双目相机获取图像左视图、右视图与相机参数得到矫正后的左视图、右视图，通过矫正后的左视图与矫正后的右视图的对应像素匹配关系得到对应像素的深度信息；通过FAST角点算法提取矫正后的左视图关键点，通过非极大值抑制方法对关键点进行滤除，通过卡方检验对关键点进行筛选；然后对上一帧获取到的关键点集，在当前帧通过光流跟踪寻找匹配关键点集，得到匹配关键点对；根据匹配关键点对计算代价函数，通过高斯牛顿法迭代求解代价函数，得到最终位姿结果；最后对输入的连续图像帧进行筛选得到关键图像帧，对关键图像帧的关键点集与位姿计算联合代价函数，优化求解代价函数得到更新后的位姿；

矫正后的左视图与矫正后的右视图的对应像素匹配关系得到对应像素深度图I_d的步骤如下：

S201、对于矫正后左视图I₀中的任一像素位置p_i，选取p_i的3*3邻域像素集合r_i0；

S202、在矫正后右视图I₁中的同样像素位置p_i的7*7邻域R_i中，对每一个像素取3*3邻域像素集合r_i1，对r_i0与r_i1求取绝对差值之和的集合d_i；

S203、步骤S202集合d_i中最小的差值d_min所对应的像素位置p_min即为p_i的匹配像素点，计算两点的x轴坐标差d，同时计算得到p_i处的深度d_pi；

S204、循环以上步骤直至计算得到深度图I_d。

2.根据权利要求1所述的一种基于双目视觉的无人机定位导航的方法，其特征在于，任一像素位置p_i处的深度d_pi如下：

其中，f为双目相机给定的焦距，l为双目相机的基线长度。

3.根据权利要求1所述的一种基于双目视觉的无人机定位导航的方法，其特征在于，对矫正后左视图I₀通过FAST角点算法提取关键点集的步骤如下：

S301、对矫正后左视图I₀通过FAST角点算法提取关键点集P；

S302、对关键点集P中的所有关键点p，获取其3*3邻域范围内是否存在其余关键点，若存在则从关键点集P中将其余关键点删除；

S303、对关键点集P中的所有关键点p，根据卡方检验算法计算其卡方值C_p与卡方阈值K，若C_p＞K，则将C_p对应的关键点p从关键点集P中删除。

4.根据权利要求1所述的一种基于双目视觉的无人机定位导航的方法，其特征在于，通过卡方检验对关键点进行筛选；判断当前帧是否为初始帧，若是则返回重新进行下一帧双目图像的处理，若不是，则继续对上一帧获取到的关键点集，在当前帧通过光流跟踪寻找匹配关键点集，得到匹配关键点对。

5.根据权利要求4所述的一种基于双目视觉的无人机定位导航的方法，其特征在于，通过光流跟踪算法得到关键点对集合M₀的步骤如下：

S501、对于关键点集P₀中的任一点p_i，其像素坐标为(x_i，y_i)，设该点的运动为(Δx_i，Δy_i)，则当前帧的矫正后左视图I₀中的对应点p_j为(x_i+Δx_i，y_i+Δy_i)；

S502、根据对应点构建最小二乘模型：

其中，W为p_i的8*8邻域；

S503、通过高斯牛顿法求解该最小二乘问题，得到Δx_i，Δy_i与p_j，将p_i与p_j存入关键点对集合M₀，重复以上步骤直至完成关键点集P₀中所有关键点的跟踪。

6.根据权利要求1所述的一种基于双目视觉的无人机定位导航的方法，其特征在于，根据得到的关键点对集合M₀，与上一帧左视图I_0last、当前帧左视图I_0now，求解当前帧的对应位姿T_i，具体步骤如下：

S601、对于关键点对集合M₀中的任一点对m_i，提取关键点对p_i(x_i，y_i)、p_j(x_j，y_j)，设T为当前帧的对应位姿；

S602、构建最小二乘模型：

其中，T×I_0now(x_j，y_j)为矩阵乘法，将p_j投影到上一帧左视图I_0last中，I_0last()、I_0now()表示从图像中获取对应像素坐标的像素值；

S603、通过高斯牛顿法求解该最小二乘问题，得到T。

7.根据权利要求1所述的一种基于双目视觉的无人机定位导航的方法，其特征在于，判断当前帧是否为关键帧，若是则存入关键帧集合Q，若该帧为关键帧，则进行联合位姿优化，否则继续算法循环获取深度图I_d。

8.根据权利要求7所述的一种基于双目视觉的无人机定位导航的方法，其特征在于，关键帧判断具体如下：

若上一帧位姿T_last与当前帧位姿T_now之间的平移变换t大于阈值th_t，则该帧为关键帧；

若上一帧位姿T_last与当前帧位姿T_now之间的旋转变换R大于阈值th_R，则该帧为关键帧；

若上一帧得到的关键点集P_last中关键点的数量n_last与当前帧得到的关键点集P_now中关键点的数量n_now比值大于阈值th_n，则该帧为关键帧。

9.根据权利要求1所述的一种基于双目视觉的无人机定位导航的方法，其特征在于，根据得到的关键帧集合Q，通过局部光束平差法进行联合位姿优化，联合优化完成后，继续算法循环获取深度图I_d，联合位姿优化的具体步骤如下：

S801、对于关键帧集合Q中的每一关键帧Q_i，以及其所对应的关键点对M_i，对于关键点对集合M_i中的任一点对m_i，提取关键点对p_i(x_i，y_i)、p_j(x_j，y_j)，设T_i为点对中p_i对应帧的位姿，T_j为点对中p_j对应帧的位姿；

S802、构建最小二乘模型：

其中，I_0last为p_i对应帧的左视图，I_0now为p_j对应帧的左视图，I_0last()、I_0now()表示从图像中获取对应像素坐标的像素值；

S803、通过高斯牛顿法求解该最小二乘问题，得到T_i与T_j。

Images