CN110298914B - 一种建立果园中果树冠层特征地图的方法 - Google Patents

Abstract

本发明公开了一种建立果园中果树冠层特征地图的方法，包括使用RGB‑D相机实时采集机器人运动过程中周围环境图像的彩色信息与深度信息，并使用语义分割网络分割果树与其他环境特征，根据分割后的图像保留深度图像中果树的信息，从而生成果树的点云数据；提取彩色图像中的特征角点，计算相邻两帧图像相机的运动，并与IMU数据进行融合，从而估计出相机的运动状态；将多帧图像及相机位姿估计采用因子图构建优化问题，优化相机的位姿；根据果树点云数据与相机位姿构建果树冠层的八叉树地图。采集果园丰富的环境信息，提高机器人对果园环境的理解，满足机器人完成更高级的作业要求，比如自动导航，变量喷雾，水果采摘等。

Description

一种建立果园中果树冠层特征地图的方法

技术领域

本发明涉及视觉SLAM技术与语义分割技术，特别提供一种建立果园中果树冠层特征地图的方法。

背景技术

从土壤分析，到果园管理，再到气候检测、水果采摘，多种精准农业技术正在应用到果园生产的各个环节，精准农业正在成为现代果园发展的未来。果园自主作业机器人正逐渐替代人完成果园中繁复的劳动，自主作业机器人的关键技术之一是环境感知技术，根据采集的数据创建数字地图，从而指导机器人自主移动。

SLAM(同步定位与建图)被认为是实现机器人真正全自主移动的关键，传统的果园SLAM方法大多采用激光雷达传感器提取果树树干特征，建立果树树干的特征地图，但是果园中的杂草和立柱会增加果树树干特征提取的错误率，同时果树树干特征地图提供的信息太过单一，不能对果园环境进行更高层次的理解，从而满足精准农业的要求，而且激光雷达的成本也远远高于视觉传感器。

除此以外，还有采用单目相机、双目相机、深度相机的视觉SLAM方法。单目相机无法通过单张图像深度信息，需要在运动过程中估计图像深度，不适用于果园移动机器人。双目相机计算量巨大，需要采用硬件资源加速才能实时输出图像的深度信息。而深度相机则可以非常容易的实时获取彩色图像与深度图像，大大减少了计算量。但是基于传统机器视觉方法的视觉SLAM不能适应果园中果树树叶的颜色、生长的变化。

当相机运动过快或者视觉识别错误时，单独采用视觉传感器来估计机器人的运动会产生较大误差，不能确保果园作业机器人的可靠性。

发明内容

为了解决上述问题，本发明提供了一种建立果园中果树冠层特征地图的方法，从而满足精准农业中果园作业机器人自主导航作业的需求。

本发明的技术方案如下：

基于深度相机与IMU融合的果园果树冠层特征地图建立方法包括以下步骤：

步骤1：采用基于RGB-D相机实时采集环境的彩色信息和深度信息，采用高频率IMU(惯性测量单元)测量角速度和线性加速度，对IMU数据进行积分，得到相机位姿的测量向量M。步骤2：采用全卷积神经网络FCN，实现像素级别端到端的语义分割，然后利用条件随机场CRF优化语义分割结果，将果树冠层彩色图像和深度图像转换为点云数据。步骤3：提取分割后彩色图像果树的ORB特征点，对相邻两帧图像中的特征点配对，剔除两帧图像误匹配的特征点，计算相机位姿的变换矩阵T，构建图优化问题，融合RGB-D相机与IMU，优化相机位姿的变换矩阵T。步骤4：根据步骤3得到的相机位姿变换矩阵T，对步骤2得到的关键帧的点云数据进行拼接，采用八叉树建立果树冠层特征地图。

进一步，所述步骤2具体包括：

采用全卷积神经网络FCN，实现像素级别端到端(end-to-end)的语义分割，然后利用条件随机场CRF优化语义分割结果，区分彩色图像中的果树冠层与地面、天空等无关特征，每个像素都有一个类别标签，以每个像素点作为节点，像素与像素间的联系作为边，就构成了一个条件随机场CRF，通过观测变量x来推测像素对应的类别标签y，最大化条件概率函数

式中x_ij是单个像素点的观测变量，y_ij是单个像素点的类别标签，y_(i+1)j，y_(i-1)j，y_i(j+1)，y_i(j+1)是相邻四个像素点的类别标签，Z是归一化因子。

得到彩色图像像素级别的分割后，遍历彩色图像只保留彩色图像中的果树信息，然后遍历彩色图像对应的深度图像，只保留深度图像中果树的深度信息，然后读取果树的颜色信息和距离信息，计算出像素在相机坐标系下的坐标，根据相机内参计算一对彩色图像和深度图像对应的点云。

进一步，所述步骤3具体包括：

提取分割后彩色图像果树的ORB特征，首先找出Oriented FAST关键点，然后计算BRIEF描述子，从而对特征点周围图像区域进行描述，采用RANSAC算法找出相邻帧之间的特征点对应关系，从而推算出相邻帧间相机的运动，然后融合步骤1得到的相机位姿的测量向量M，采用因子图构建优化问题，通过最小化下面的函数，优化相机位姿的变换矩阵T和特征点的坐标P：

式中h是测量方程，F_i、G_j是有关ξ_j的雅克比矩阵。

进一步，所述步骤4包括：

根据相机内参计算一对彩色图像和深度图像对应的点云，然后根据步骤3中得到的相机位姿T和特征点的坐标P，将深度信息转换到世界坐标系下，构成点云地图，然后使用八叉树形式存储地图，使用概率占用估计来对占用空间和自由空间进行建模，大大节省存储空间。生成八叉树地图先是用8个立体体元划分整幅点云，若节点不为空，则该节点继续划分，直至体元内无点，判断的是否继续划分的依据是叶节点被占用的概率，方程如下：

式中P(Z_i)是叶节点被占用的概率，根据叶节点内的点云数据来计算P(z_i)。

八叉树地图生成后，通过搜索计算机器人周围节点的占用状态L(n)，判断节点是否可行，从而机器人可以规划出可行路径，实现自主导航，节点的占用概率值采用如下方程计算：

本发明的有益效果为：

1.采集果园丰富的环境信息，提高机器人对果园环境的理解，满足机器人完成更高级的作业要求，比如自动导航，变量喷雾，水果采摘等。

2.采用RGB-D相机与IMU融合，解决相机运动过快、特征缺失带来的问题，以及减小运动物体的干扰，采用非线性图优化方法优化相机位姿，提高相机位姿变换矩阵T估计的准确性。

3.采用八叉树建立灵活的、可压缩的、可随时更新的地图，可以按照需要调节地图的分辨率，满足较大的环境的建图，相比点云地图节省了大量的存储空间，可以通过检测地图网格的占据信息，完成导航路径的规划。

除此之外，步骤1的创新点：其他方法单纯利用相机估计相机运动，没有融合IMU，当相机快速运动图像模糊不能估计相机运动。步骤3的创新点：采用因子图优化算法，迭代优化，减少测量噪声的影响。步骤4中的创新点：采用八叉树结构存储地图数据，方便导航算法搜索可路径，减少地图数据存储空间。通过对数运算将地图网格占用概率值转化为计算机可以方便判断的bool值。

附图说明

图1语义分割分类标签示意图

图2基于RGB-D相机与IMU建立果树冠层特征地图的方法流程图

图3优化相机位姿的变换矩阵T的因子图

图4八叉树地图存储结构示意图

图5地图网格占用状态判断函数曲线示意图

具体实施方式

下面结合实施例及附图对本发明作进一步的描述，但本发明的实施方式不限于此。

本发明包括以下步骤：

步骤1：采用基于RGB-D相机实时采集环境的彩色信息和深度信息，采用高频率IMU测量角速度和线性加速度，对IMU数据进行积分，得到相机位姿的测量向量M；

步骤2：采用全卷积神经网络FCN，实现像素级别端到端的语义分割，然后利用条件随机场CRF优化语义分割结果，将果树冠层彩色图像和深度图像转换为点云数据；

步骤3：提取分割后彩色图像果树的ORB特征点，对相邻两帧图像中的特征点配对，剔除两帧图像误匹配的特征点，计算相机位姿的变换矩阵T，构建图优化问题，融合RGB-D相机与IMU，优化相机位姿的变换矩阵T；

步骤4：根据步骤3得到的相机位姿变换矩阵T，对步骤2得到的关键帧的点云数据进行拼接，采用八叉树建立果树冠层特征地图。

上述步骤1中，相机运动角度向量φ、平移向量ρ，相机位姿向量

相机位姿测量向量M是一个6维的向量。

上述步骤2中彩色图像经过全卷积神经网络FCN后，每个像素具有类别标签，以每个像素点作为节点，像素与像素间的联系作为边，就构成了一个条件随机场CRF，通过观测变量x来推测像素对应的类别标签y，最大化条件概率函数

式中x_ij是单个像素点的观测变量，y_ij是单个像素点的类别标签，y_(i+1)j，y_(i-1)j，y_i(j+1)，y_i(j+1)是相邻四个像素点的类别标签，Z是归一化因子。

上述步骤3中融合RGB-D相机与IMU，采用因子图优化算法，通过最小化下面的函数，优化相机位姿的变换矩阵T和特征点的坐标P：

式中h是测量方程，F_i、G_j是有关ξ_j的雅克比矩阵。

上述步骤4中点云拼接是根据相机内参计算一对彩色图像和深度图像对应的点云，然后根据步骤3中得到的相机位姿T和特征点的坐标P，将深度信息转换到世界坐标系下，构成点云地图，然后使用八叉树形式存储地图，先是用8个立体体元划分整幅点云，若节点不为空，则该节点继续划分，直至体元内无点，判断的是否继续划分的依据是叶节点被占用的概率，方程如下：

式中P(z_i)是叶节点被占用的概率，根据叶节点内的点云数据来计算P(z_i)；

八叉树地图生成后，通过搜索计算机器人周围地图网格的占用状态L(n)，判断节点是否可行，从而机器人可以规划出可行路径，实现自主导航，节点的占用概率值采用如下方程计算：

式中P(n)表示地图中单个网格的占用概率值，通过对数运算转换为该地图网格占用状态L(n)，当P(n)＞0.5时，L(n)＞0则该地图网格可以通行，否则L(n)＝0该地图网格不可通行。

实施例

采用微软的KinectV2相机采集图像序列，实时输出同一时刻的彩色图像，深度图像，采用MEMS陀螺仪采集IMU数据，并将起始状态图像帧与IMU数据进行同步，输出30FPS的图像数据序列和200Hz的IMU数据序列。

利用DeepLabv3网络对彩色图像中的果树冠层进行分割，对每个像素进行分类，预测目标采用one-hot编码，即为每一个可能的类创建一个输出通道。通过取每个像素点在各个channel的argmax可以得到最终的预测分割图。

计算Oriented FAST关键点和BRIEF描述子，提取出图像中的ORB特征点，对相邻两帧图像中的特征点配对，采用RANSAC(随机抽样一致性算法)剔除两帧图像误匹配的特征点。

然后根据匹配的特征点像素位置，求出本质矩阵E，对本质矩阵E采用SVD分解，对上一次位置求解相机位姿的变换矩阵T。

然后根据图像中的一些特征点的深度信息和相机的内参矩阵，利用对极几何来确定三维空间点的位置。

最后利用计算得到的三维点的坐标和计算得到的相机位姿进行重投影。

利用g2o构建因子图优化问题，优化变量为相机的位姿及特征点的位置，优化因子为相机观测Z、相机变换矩阵T、IMU预积分量、回环检测，通过调整优化变量的值，来使优化因子的乘积最大化。

根据相机位姿T和一一对应的彩色图像和深度图像，生成果树冠层的点云数据，利用PCL完成点云数据的拼接，生成局部的三维点云地图。

利用Octomap将生成的局部三维点云地图转化为带占据信息的八叉树地图，结合相机位姿变换矩阵T，建立相机的全局地图。

本发明未尽事宜为公知技术。

在本说明书的描述中，参考术语“一个实施例”、“一些实施例”、“示意性实施例”、“示例”、“具体示例”、或“一些示例”等的描述意指结合该实施例或示例描述的具体特征、结构、材料或者特点包含于本发明的至少一个实施例或示例中。在本说明书中，对上述术语的示意性表述不一定指的是相同的实施例或示例。而且，描述的具体特征、结构、材料或者特点可以在任何的一个或多个实施例或示例中以合适的方式结合。

尽管已经示出和描述了本发明的实施例，本领域的普通技术人员可以理解：在不脱离本发明的原理和宗旨的情况下可以对这些实施例进行多种变化、修改、替换和变型，本发明的范围由权利要求及其等同物限定。

Claims (5)

1.一种基于RGB-D相机与IMU建立果树冠层特征地图的方法，其特征在于：所述方法包括以下步骤：

步骤1：采用基于RGB-D相机实时采集环境的彩色信息和深度信息，采用高频率IMU测量角速度和线性加速度，对IMU数据进行积分，得到相机位姿的测量向量M；

步骤2：采用全卷积神经网络FCN，实现像素级别端到端的语义分割，然后利用条件随机场CRF优化语义分割结果，将果树冠层彩色图像和深度图像转换为点云数据；

步骤3：提取分割后彩色图像果树的ORB特征点，对相邻两帧图像中的特征点配对，剔除两帧图像误匹配的特征点，计算相机位姿的变换矩阵T，构建图优化问题，融合RGB-D相机与IMU，优化相机位姿的变换矩阵T；

步骤4：根据步骤3得到的相机位姿变换矩阵T，对步骤2得到的关键帧的点云数据进行拼接，采用八叉树建立果树冠层特征地图。

2.按照权利要求1所述的基于RGB-D相机与IMU建立果树冠层特征地图的方法，其特征在于：所述步骤1中，相机运动角度向量φ、平移向量ρ,相机位姿向量

相机位姿测量向量M是一个6维的向量。

3.按照权利要求1所述的基于RGB-D相机与IMU建立果树冠层特征地图的方法，其特征在于：所述步骤2中彩色图像经过全卷积神经网络FCN后，每个像素具有类别标签，以每个像素点作为节点，像素与像素间的联系作为边，就构成了一个条件随机场CRF，通过观测变量x来推测像素对应的类别标签y，最大化条件概率函数

式中x_ij是单个像素点的观测变量，y_ij是单个像素点的类别标签，y_(i+1)j，y_(i-1)j，y_i(j+1)，y_i(j+1)是相邻四个像素点的类别标签，Z是归一化因子。

4.按照权利要求1所述的基于RGB-D相机与IMU建立果树冠层特征地图的方法，其特征在于：所述步骤3中融合RGB-D相机与IMU，采用因子图优化算法，通过最小化下面的函数，优化相机位姿的变换矩阵T和特征点的坐标P：

式中h是测量方程，F_i、G_j是有关ξ_j的雅克比矩阵。

5.按照权利要求1所述的基于RGB-D相机与IMU建立果树冠层特征地图的方法，其特征在于：所述步骤4中点云拼接是根据相机内参计算一对彩色图像和深度图像对应的点云，然后根据步骤3中得到的相机位姿T和特征点的坐标P，将深度信息转换到世界坐标系下，构成点云地图，然后使用八叉树形式存储地图，先是用8个立体体元划分整幅点云，若节点不为空，则该节点继续划分，直至体元内无点，判断的是否继续划分的依据是叶节点被占用的概率，方程如下：

式中P(z_i)是叶节点被占用的概率，根据叶节点内的点云数据来计算P(z_i)；

八叉树地图生成后，通过搜索计算机器人周围地图网格的占用状态L(n)，判断节点是否可行，从而机器人可以规划出可行路径，实现自主导航，节点的占用概率值采用如下方程计算：

式中P(n)表示地图中单个网格的占用概率值，通过对数运算转换为该地图网格占用状态L(n)，当P(n)＞0.5时，L(n)＞0则该地图网格可以通行，否则L(n)＝0该地图网格不可通行。

Images