CN108303099A - 基于三维视觉slam的无人机室内自主导航方法 - Google Patents

Abstract

本发明提供一种基于三维视觉SLAM的无人机室内自主导航方法，包括：RGB‑D相机获取无人机周围环境的彩色图像和深度数据；无人机机载运算系统提取特征点；无人机机载运算系统判断是否具有足够的特征点，若特征点的数量大于30，表示具有足够的特征点，则进行无人机姿态推算过程，否则进行重定位；使用捆集优化方法全局优化；构造增量式地图。仅使用一台RGB‑D相机给出无人机位姿信息，重建周围环境三维地图，避免单目相机求解深度信息的复杂过程，解决了双目相机中匹配算法的复杂度和鲁棒性问题；使用迭代最近点法结合重投影误差算法，使无人机位姿估计更加准确；使无人机在室内及其它未知环境下进行定位、导航和自主飞行，避免没有GPS信号时无法定位的问题。

Description

基于三维视觉SLAM的无人机室内自主导航方法

技术领域

本发明属于无人机室内定位导航技术领域，尤其涉及一种基于三维视觉SLAM的无人机室内自主导航方法。

背景技术

目前无人机定位的主流导航系统是GPS和惯性导航系统的组合导航系统，但在室内或无法使用GPS的未知环境下，这种方法无法使用。SLAM研究的问题就是机器人在未知环境移动过程中根据位置估计和地图进行自身定位，同时在自身定位的基础上构造增量式地图，实现机器人的自主定位和导航。

研究SLAM使用的传感器通常是激光和相机，激光定位较准确，但成本高，且无法识别丰富的特征，普通的相机，对光强敏感，无法在较弱光线下使用。通常基于图像特征点的无人机位姿估计只使用一种特征匹配方法，很容易出现特征丢失或位姿估计不准的问题。

发明内容

为解决上述技术问题，本发明提供一种基于三维视觉SLAM的无人机室内自主导航方法。为了对披露的实施例的一些方面有一个基本的理解，下面给出了简单的概括。该概括部分不是泛泛评述，也不是要确定关键／重要组成元素或描绘这些实施例的保护范围。其唯一目的是用简单的形式呈现一些概念，以此作为后面的详细说明的序言。

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

在一些可选的实施例中，提供一种基于三维视觉SLAM的无人机室内自主导航方法，包括：

RGB-D相机获取无人机周围环境的彩色图像和深度数据并在机器人软件平台(ROS,Robot Operating System）上进行时间对齐的配准；

无人机机载运算系统提取所述RGB-D相机获取的彩色图像中的特征点；

所述无人机机载运算系统判断是否具有足够的所述特征点，若所述特征点的数量大于30，表示具有足够的特征点，则进行无人机姿态推算过程，否则进行重定位；

所述无人机机载运算系统使用捆集优化方法全局优化通过所述无人机姿态推算过程获取的最优位姿和地图点，从而得到最佳位姿；

所述无人机机载运算系统将自所述RGB-D相机获取的三维稠密点云进行滤波，使用所述最佳位姿将三维点云变换到世界坐标系下进行拼接，再进行逐步叠加，构造增量式三维地图信息。

在一些可选的实施例中，所述的基于三维视觉SLAM的无人机室内自主导航方法，还包括：

所述无人机机载运算系统发送位姿信息及路径规划信息至无人机飞行控制模块；

无人机飞行控制模块依据所述位姿信息及路径规划信息控制无人机进行全自主飞行。

在一些可选的实施例中，所述重定位的过程包括：将当前帧与所有候选关键帧进行匹配；当匹配点数大于15时重定位成功，则进行RANSAC迭代，RANSAC是根据一组包含异常数据的样本数据集，计算出数据的数学模型参数，得到有效样本数据的算法。

在一些可选的实施例中，所述无人机姿态推算过程包括：

使用迭代最近点法结合重投影误差方法进行两两帧间匹配，从而求得所述RGB-D相机的三维空间位置和姿态，同时提取关键帧图像中关键点作为地图点；

使用通用图优化方法(G2O，General Graph Optimization)把位姿和空间特征点构建成节点，将位姿变换后的点与变换前的点坐标的误差方程构建成边，将优化问题表达成位姿图，利用通用图优化方法中优化求解器来优化位姿和空间特征点，得到最优位姿和地图点；

使用当前帧的位姿将相机坐标系下的当前帧变换到世界坐标系下时，与存储的候选帧进行比较，如果两帧位置信息和特征点的比较值收敛，则认为是同一个位置，闭合路径，更新里程计和三维点云地图。

本发明所带来的有益效果：本发明仅使用一台RGB-D相机，给出无人机的位姿信息，重建出周围环境的三维地图，避免了单目相机中求解深度信息的复杂过程，解决了双目相机中匹配算法的复杂度和鲁棒性问题；使用迭代最近点法结合重投影误差方法算法，使无人机位姿的估计更加准确，并建立周围环境下小场景的特征丰富的三维地图；本发明使无人机在室内及其它未知环境下进行定位、导航和自主飞行，避免了没有GPS信号时无法定位的问题；使用更准确和更鲁棒性的位姿优化算法，使视觉导航定位适用于更多的如光线较暗的场景中。

为了上述以及相关的目的，一个或多个实施例包括后面将详细说明并在权利要求中特别指出的特征。下面的说明以及附图详细说明某些示例性方面，并且其指示的仅仅是各个实施例的原则可以利用的各种方式中的一些方式。其它的益处和新颖性特征将随着下面的详细说明结合附图考虑而变得明显，所公开的实施例是要包括所有这些方面以及它们的等同。

附图说明

图1是本发明基于三维视觉SLAM的无人机室内自主导航方法的流程示意图。

图2是本发明无人机SLAM的系统结构框图。

具体实施方式

以下描述和附图充分地示出本发明的具体实施方案，以使本领域的技术人员能够实践它们。其他实施方案可以包括结构的、逻辑的、电气的、过程的以及其他的改变。实施例仅代表可能的变化。除非明确要求，否则单独的部件和功能是可选的，并且操作的顺序可以变化。一些实施方案的部分和特征可以被包括在或替换其他实施方案的部分和特征。本发明的实施方案的范围包括权利要求书的整个范围，以及权利要求书的所有可获得的等同物。

如图2所示，SLAM系统包括：无人机机载传感器系统1、无人机机载运算系统2及无人机机载飞行控制单元3。

无人机机载传感器系统1的RGB-D相机用于获取无人机周围环境的彩色图像和深度数据。无人机机载运算系统2包括位姿估计、三维建图、回环检测和路径规划部分，这四个过程在无人机机载运算系统2上处理，无人机机载运算系统2是一个体量很轻的高性能电脑棒。无人机机载传感器系统2获取的位姿信息和路径规划发送到无人机机载飞行控制单元3中的无人机飞行控制模块指示无人机进行全自主飞行。

本发明仅使用一台RGB-D相机作为无人机SLAM的传感器系统。RGB-D相机小型轻量，相对于单目相机可以提供三维点信息，相对于双目相机减少匹配算法的复杂度和提高了鲁棒性。

如图1所示，在一些说明性的实施例中，针对小型无人机在室内或其它GPS无法使用的未知环境下，本发明提供一种基于三维视觉SLAM的无人机室内自主导航方法，包括：

101：RGB-D相机获取无人机周围环境的彩色图像和深度数据，并在ROS系统上进行时间对齐的配准。

102：无人机机载运算系统提取RGB-D相机获取的彩色图像中的特征点。

103：无人机机载运算系统判断是否具有足够的特征点，若特征点的数量大于30，表示具有足够的特征点，则进行无人机姿态推算过程，否则进行重定位。

104：无人机姿态推算过程。

105：重定位。

106：无人机机载运算系统使用捆集优化方法（BA，Bundle Adjustment）全局优化通过无人机姿态推算过程104获取的最优位姿和地图点，从而得到最佳位姿。

当无人机机载运算系统启动时，初始位置设置为原点，从所有关键帧中求得的位姿及地图点建立一个局部初步地图，对建立的局部初步地图进行一次局部BA优化，删除冗余地图点。随着无人机的运动，G2O构建的位姿图也会越大，把无人机位姿和三维空间的地图点不断地添加到图中，最后对所添加的图进行全局BA优化，得到最佳的位姿和三维点云地图。

107：构造增量式地图。

无人机机载运算系统将自RGB-D相机获取的三维稠密点云进行滤波，使用最佳位姿将三维点云变换到世界坐标系下进行拼接，再进行逐步叠加，构造增量式三维地图信息。提取关键帧的特征点作为地图点建立稀疏地图，对于关键帧加上关键帧的深度信息可以建立稠密的具有丰富特征的三维地图。

108：无人机机载运算系统发送位姿信息及路径规划信息至无人机飞行控制模块。

在获得无人机位姿信息和周围三维点云地图后，进行无人机飞行轨迹规划，轨迹规划包括两个阶段：学习阶段和查询阶段。通过环境地图可以确认出周边的区域中的危险区域及安全区域。在学习阶段可以根据这些区域随机产生路径点组成路径点集合，然后通过简单快速的局部路径规划器将相邻的路径点连接，从而构成路径图。在查询阶段，首先在可行路径集中找出一条满足约束条件的最优路径，然后判断该路径是否满足期望的性能指标。如果该路径的性能指标满足要求，则算法结束，否则将增加随机路径点，然后重新得到最优路径，并重复执行上述过程，直到找出满足期望的性能指标的最优路径为止。

109：无人机飞行控制模块依据所述位姿信息及路径规划信息控制无人机进行全自主飞行。

本发明使用RGB-D相机提取每帧彩色图像的特征点，加入深度信息进行相邻帧特征点匹配，利用迭代最近点法(ICP，Iterative Closest Points）优化得到一个无人机的姿态信息和三维点云，对于无效深度的像素点进行重投影误差方法( PnP，Pespective-n-point Projection)优化，提高了位姿求解的准确率。

本发明利用G2O法对姿态信息和地图结果进行全局优化，得到最优的位姿估计，进而更新地图。将算法得到的RGB-D相机位姿信息发送到无人机的飞控部分，实现无人机定位，将相关的地图信息存储在无人机机载运算系统，利用建好的地图进行路径规划，实现无人机室内自主飞行。只使用一台RGB-D相机利用ICP/PnP方法进行位姿估计、构造增量式地图和进行路径规划。

在一些说明性的实施例中，无人机姿态推算过程104包括：

1041：两两帧间匹配。

1042：得到RGB-D相机的三维空间位置和姿态估计。

使用ICP结合PnP进行两两帧间匹配，从而求得RGB-D相机的三维空间位置和位姿估计，同时提取关键帧图像中关键点作为地图点。

两两帧间匹配使用ICP和PnP两种法结合求解RGB-D相机的位姿，这种匹配使无人机的定位更具有鲁棒性。两个匹配方法如下：

非线性的ICP方法：ICP方法用于特征点深度信息确定的情况下，这里迭代的两组点并不是最近邻点而是两组匹配好的在相机坐标下的三维点。对于两帧图像中的两组匹配的特征点集p和p', 通过求解相机的位姿，即旋转矩阵R和平移向量t使公式（1）成立，公式（1）如下：

其中pi 和pi'是匹配前后对应的特征点，且pip, pi'p' 。

对于n个点使用ICP方法利用对公式（1）的误差求解，寻找最优位姿的值，构建求解优化位姿的目标函数。由于旋转矩转R是SO(3)正交群，满足约束条件：；因此可以将有约束条件的位姿R和t用无约束的李代数表示，构建位姿变换后的点与原来点的误差，使位姿求解变成一个最小二乘问题，如公式（2），公式（2）如下：

其中为李代数表示的位姿。

计算重投影误差的PnP方法：PnP方法用于RGB-D相机中像素点的深度值无效的时候，将地图点投影到相机坐标系下，与图像中的像素点作差，再使用最小二乘法优化位姿。

空间点Pi在世界坐标系下的齐次坐标表示为，它的像素点zi齐次坐标为。像素与空间点的坐标关系如公式（3），公式（3）如下：

其中K为RGB-D相机的内参数矩阵,si为特征点对应的深度，在这种情况下是未知的，求解优化问题时可以消去，为李代数表示的位姿。由公式（3）建立最小二乘问题如公式（4）误差最小时得到最优的相机位姿，公式（4）如下：

从而将问题转化为非线性的ICP方法中同样的非线性优化问题。

1043：位姿图优化。

使用G2O把位姿和空间特征点构建成节点，将位姿变换后的点与变换前的点坐标的误差方程构建成边，将优化问题表达成位姿图，利用通用图优化方法中优化求解器来优化位姿和空间特征点，得到最优位姿和地图点。

公式（2）和公式（4）分别给出三维空间点坐标和像素坐标与它们进行位姿变换后坐标的误差，当误差值极小时，得到最优的位姿解。本发明使用G2O库将位姿和地图点构建成位姿图的顶点，公式（2）和公式（4）的误差构建成位姿图的边，之后不断添加顶点和边，构建完位姿图后使用G2O法优化中的LM算法对构建的图进行优化。其中LM算法是牛顿法和梯度法融合的Levenberg-Marquardt算法,它给位姿变化量加一个信赖区域，直到函数收敛时，可以得到所求的位姿即为最优的位姿值。

1044：闭环检测，将最优位姿与位姿图的其他位姿比较，检测是否有闭环。如果有闭环，则将更新地图点和位姿。

使用当前帧的位姿将相机坐标系下的当前帧变换到世界坐标系下时，与存储的候选帧进行比较，如果两帧位置信息和特征点的比较值收敛，则认为是同一个位置，闭合路径，更新里程计和三维点云地图。

位姿估计部分利用ICP法结合PnP法的特征点法进行非线性优化，再使用LM算法计算优化问题的雅可比矩阵，得到最优的无人机位姿信息，发送到无人机机载飞行控制单元。当相邻帧跟踪失败后，将当前帧与之前关键帧库的帧进行匹配形成闭环，求解位姿和叠加地图点，这样计算提高了位姿的准确性和地图稳定性，防止因无人机进行回环时地图点会乱掉。

在一些说明性的实施例中，重定位105的过程包括：

1051：将当前帧与所有候选关键帧进行匹配。

1052：当匹配点数大于15时重定位成功，则进行RANSAC迭代。

本领域技术人员还应当理解，结合本文的实施例描述的各种说明性的逻辑框、模块、电路和算法步骤均可以实现成电子硬件、计算机软件或其组合。

为了清楚地说明硬件和软件之间的可交换性，上面对各种说明性的部件、框、模块、电路和步骤均围绕其功能进行了一般地描述。至于这种功能是实现成硬件还是实现成软件，取决于特定的应用和对整个系统所施加的设计约束条件。熟练的技术人员可以针对每个特定应用，以变通的方式实现所描述的功能，但是，这种实现决策不应解释为背离本公开的保护范围。

Claims (4)

1.基于三维视觉SLAM的无人机室内自主导航方法，其特征在于，包括：

RGB-D相机获取无人机周围环境的彩色图像和深度数据并在ROS系统上进行时间对齐的配准；

无人机机载运算系统提取所述RGB-D相机获取的彩色图像中的特征点；

所述无人机机载运算系统判断是否具有足够的所述特征点，若所述特征点的数量大于30，表示具有足够的特征点，则进行无人机姿态推算过程，否则进行重定位；

所述无人机机载运算系统使用捆集优化方法全局优化通过所述无人机姿态推算过程获取的最优位姿和地图点，从而得到最佳位姿；

所述无人机机载运算系统将自所述RGB-D相机获取的三维稠密点云进行滤波，使用所述最佳位姿将三维点云变换到世界坐标系下进行拼接，再进行逐步叠加，构造增量式三维地图信息。

2.根据权利要求1所述的基于三维视觉SLAM的无人机室内自主导航方法，其特征在于，还包括：

所述无人机机载运算系统发送位姿信息及路径规划信息至无人机飞行控制模块；

无人机飞行控制模块依据所述位姿信息及路径规划信息控制无人机进行全自主飞行。

3.根据权利要求1或2所述的基于三维视觉SLAM的无人机室内自主导航方法，其特征在于，所述重定位的过程包括：

将当前帧与所有候选关键帧进行匹配；

当匹配点数大于15时重定位成功，则进行RANSAC迭代。

4.根据权利要求3所述的基于三维视觉SLAM的无人机室内自主导航方法，其特征在于，所述无人机姿态推算过程包括：

使用迭代最近点法结合重投影误差方法进行两两帧间匹配，从而求得所述RGB-D相机的三维空间位置和姿态，同时提取关键帧图像中关键点作为地图点；

使用通用图优化方法把位姿和空间特征点构建成节点，将位姿变换后的点与变换前的点坐标的误差方程构建成边，将优化问题表达成位姿图，利用通用图优化方法中优化求解器来优化位姿和空间特征点，得到最优位姿和地图点；

使用当前帧的位姿将相机坐标系下的当前帧变换到世界坐标系下时，与存储的候选帧进行比较，如果两帧位置信息和特征点的比较值收敛，则认为是同一个位置，闭合路径，更新里程计和三维点云地图。