CN110806585B - 一种基于树干聚类跟踪的机器人定位方法及系统 - Google Patents

Abstract

本发明公开了一种基于树干聚类跟踪的机器人定位方法及系统。所述方法包括：获取割胶机器人所处林间环境的激光雷达信息；根据割胶机器人预设距离范围和林间环境中物体的体积，对激光雷达信息进行过滤；采用密度聚类算法对过滤后的激光雷达信息进行聚类，得到激光雷达聚类信息；根据激光雷达聚类信息，采用圆弧拟合算法确定林间环境中每棵树木的树干圆柱面圆心二维坐标和树干位置信息；由树干圆柱面圆心二维坐标和树干位置信息，计算割胶机器人的定位信息；根据树干圆柱面圆心二维坐标和定位信息，采用迪杰斯特拉算法确定割胶机器人的行进路径，以实现割胶机器人的定位。本发明能够提高机器人的定位精度。

Description

一种基于树干聚类跟踪的机器人定位方法及系统

技术领域

本发明涉及机器人定位技术领域，特别是涉及一种基于树干聚类跟踪的机器人定位方法及系统。

背景技术

橡胶树割胶长达30-40年，而割胶的劳动投入占整个橡胶生产劳动总投入的60％以上，因此，割胶是橡胶生产中最重要的环节。割胶技术和割胶制度的好坏，不仅影响橡胶树的产量，甚至影响橡胶树产胶寿命；同时，橡胶树产量与立地环境中的温度、湿度和光照有密切关系，为了保证产胶量，割胶通常都在凌晨进行，繁重的体力加上工作环境的恶劣，使得胶工短缺将成为整个天然橡胶产业发展的新常态，也将严重制约天然橡胶发展。所以，如何降低人工割胶的劳动强度及提高割胶劳动生产率是当前天然橡胶产业发展的瓶颈。因此，机械化、自动化、智能化(即割胶机器人)取代人工割胶是促进天然橡胶生产革命化的一个里程碑。

大部分现有的割胶机器人是“一机一树”式，即在每棵橡胶树上安装一个割胶机器人，这种机器人可以较好的实现割胶操作，但是成本高昂，且在遇到恶劣天气时很容易受到损坏。因此，移动割胶机器人被提出，这种机器人可以实现在胶林中的自主移动并完成割胶动作，一台机器人可以对上百棵树进行割胶，可以弥补“一机一树”机器人存在的缺陷。

在移动割胶机器人的研发过程中，准确的树木定位是正常进行机器人定位导航、准确识别起刀位置和刀痕轨迹控制的基本前提。传统的树木定位方法在路标点较为稀疏的情况下会产生较大的位置不确定性，而这种不确定性会导致系统在长时间运行的状况下定位失败的结果。传统的树木定位方法包括偏航角计算法和三角定位法。林间环境中地形复杂，会出现里程计滑移等情况，传统的偏航角计算法采用里程计读数进行计算，这就导致了在某些情况下会出现无法恢复的“机器人绑架”问题；三角定位法布设繁琐，且在林间环境下非视距效应显著，对定位影响极大。因此，目前树木定位方法存在定位不准确的问题，这就导致了机器人定位方法存在定位精度不高的缺陷。

发明内容

基于此，有必要提供一种基于树干聚类跟踪的机器人定位方法及系统，以提高机器人的定位精度。

为实现上述目的，本发明提供了如下方案：

一种基于树干聚类跟踪的机器人定位方法，包括：

获取割胶机器人所处林间环境的激光雷达信息；所述激光雷达信息是位于所述割胶机器人上的激光雷达通过对林间环境进行扫描得到的；所述激光雷达信息包括距离信息和角度信息；

根据割胶机器人预设距离范围和所述林间环境中物体的体积，对所述激光雷达信息进行过滤，得到过滤后的激光雷达信息；

采用密度聚类算法对所述过滤后的激光雷达信息进行聚类，得到激光雷达聚类信息；所述激光雷达聚类信息表征所述林间环境中每棵树木的轮廓；

根据所述激光雷达聚类信息，采用圆弧拟合算法确定所述林间环境中每棵树木的树干圆柱面圆心二维坐标和树干位置信息；所述树干位置信息包括树干的位置坐标、树干相对割胶机器人的角度以及树干圆柱面的半径；

由所述树干圆柱面圆心二维坐标和所述树干位置信息，计算所述割胶机器人的定位信息；所述定位信息为所述割胶机器人相对树干的相对位置信息；所述相对位置信息包括所述割胶机器人的位置坐标和所述割胶机器人相对树干的角度；

根据所述树干圆柱面圆心二维坐标和所述定位信息，采用迪杰斯特拉算法确定所述割胶机器人的行进路径，以实现所述割胶机器人的定位。

可选的，所述根据割胶机器人预设距离范围和所述林间环境中物体的体积，对所述激光雷达信息进行过滤，得到过滤后的激光雷达信息，具体包括：

将所述激光雷达信息中位于割胶机器人预设距离范围之外的信息删除，得到初步过滤后的激光雷达信息；所述割胶机器人预设距离范围为预设矩形框围成的区域；所述预设矩形框为在以所述割胶机器人为原点，所述割胶机器人右侧为x轴正向，所述割胶机器人正前方为y轴正向所构建的平面直角坐标系中，以坐标点(-1m,0)、(2m,0)、(-1m,8m)、(2m,8m)为顶点的矩形；

计算所述林间环境中各物体的体积；所述体积的计算公式为

R表示激光雷达遇到物体后反馈的距离，n表示连续扫描到物体的激光线数；

将所述初步过滤后的激光雷达信息中，所述体积小于预设体积阈值的信息删除，得到过滤后的激光雷达信息。

可选的，所述采用密度聚类算法对所述过滤后的激光雷达信息进行聚类，得到激光雷达聚类信息，具体包括：

根据所述过滤后的激光雷达信息的密集程度，采用DBSCAN聚类算法对所述过滤后的激光雷达信息进行聚类，得到激光雷达聚类信息。

可选的，所述根据所述激光雷达聚类信息，采用圆弧拟合算法确定所述林间环境中每棵树木的树干圆柱面圆心二维坐标和树干位置信息，具体包括：

计算所述激光雷达聚类信息中每个点到预设圆弧的距离；

由所述距离确定每棵树木对应的树干曲线；所述树干曲线是由树木的轮廓点中到所述预设圆弧的距离均相等的点构成的；

由所述树干曲线确定对应树木的树干圆柱面圆心二维坐标和树干位置信息。

可选的，根据所述树干圆柱面圆心二维坐标和所述定位信息，采用迪杰斯特拉算法确定所述割胶机器人的行进路径，以实现所述割胶机器人的定位，具体包括：

根据所述定位信息确定所述割胶机器人的当前位置；

判断位于所述当前位置的割胶机器人是否对当前树木完成割胶操作；

若否，则继续执行割胶操作；

若是，则将位于所述割胶机器人的正前方，且距离当前树木的树干圆柱面圆心二维坐标的长度为0.5m的点作为所述割胶机器人的下一位置，并将下一位置对应的树木作为当前树木，再返回所述判断位于所述当前位置的割胶机器人是否对当前树木完成割胶操作，直至完成对所述林间环境中所有树木的割胶。

本发明还提供了一种基于树干聚类跟踪的机器人定位系统，包括：

雷达信息获取模块，用于获取割胶机器人所处林间环境的激光雷达信息；所述激光雷达信息是位于所述割胶机器人上的激光雷达通过对林间环境进行扫描得到的；所述激光雷达信息包括距离信息和角度信息；

过滤模块，用于根据割胶机器人预设距离范围和所述林间环境中物体的体积，对所述激光雷达信息进行过滤，得到过滤后的激光雷达信息；

聚类模块，用于采用密度聚类算法对所述过滤后的激光雷达信息进行聚类，得到激光雷达聚类信息；所述激光雷达聚类信息表征所述林间环境中每棵树木的轮廓；

圆弧拟合模块，用于根据所述激光雷达聚类信息，采用圆弧拟合算法确定所述林间环境中每棵树木的树干圆柱面圆心二维坐标和树干位置信息；所述树干位置信息包括树干的位置坐标、树干相对割胶机器人的角度以及树干圆柱面的半径；

定位信息计算模块，用于由所述树干圆柱面圆心二维坐标和所述树干位置信息，计算所述割胶机器人的定位信息；所述定位信息为所述割胶机器人相对树干的相对位置信息；所述相对位置信息包括所述割胶机器人的位置坐标和所述割胶机器人相对树干的角度；

行进路径确定模块，用于根据所述树干圆柱面圆心二维坐标和所述定位信息，采用迪杰斯特拉算法确定所述割胶机器人的行进路径，以实现所述割胶机器人的定位。

可选的，所述过滤模块，具体包括：

第一删除单元，用于将所述激光雷达信息中位于割胶机器人预设距离范围之外的信息删除，得到初步过滤后的激光雷达信息；所述割胶机器人预设距离范围为预设矩形框围成的区域；所述预设矩形框为在以所述割胶机器人为原点，所述割胶机器人右侧为x轴正向，所述割胶机器人正前方为y轴正向所构建的平面直角坐标系中，以坐标点(-1m,0)、(2m,0)、(-1m,8m)、(2m,8m)为顶点的矩形；

体积计算单元，用于计算所述林间环境中各物体的体积；所述体积的计算公式为

R表示激光雷达遇到物体后反馈的距离，n表示连续扫描到物体的激光线数；

第二删除单元，用于将所述初步过滤后的激光雷达信息中，所述体积小于预设体积阈值的信息删除，得到过滤后的激光雷达信息。

可选的，所述聚类模块，具体包括：

聚类单元，用于根据所述过滤后的激光雷达信息的密集程度，采用DBSCAN聚类算法对所述过滤后的激光雷达信息进行聚类，得到激光雷达聚类信息。

可选的，所述圆弧拟合模块，具体包括：

距离计算单元，用于计算所述激光雷达聚类信息中每个点到预设圆弧的距离；

树干曲线确定单元，用于由所述距离确定每棵树木对应的树干曲线；所述树干曲线是由树木的轮廓点中到所述预设圆弧的距离均相等的点构成的；

位置信息确定单元，用于由所述树干曲线确定对应树木的树干圆柱面圆心二维坐标和树干位置信息。

可选的，行进路径确定模块，具体包括：

当前位置确定单元，用于根据所述定位信息确定所述割胶机器人的当前位置；

判断单元，用于判断位于所述当前位置的割胶机器人是否对当前树木完成割胶操作；若否，则继续执行割胶操作；若是，则将位于所述割胶机器人的正前方，且距离当前树木的树干圆柱面圆心二维坐标的长度为0.5m的点作为所述割胶机器人的下一位置，并将下一位置对应的树木作为当前树木，再返回所述判断位于所述当前位置的割胶机器人是否对当前树木完成割胶操作，直至完成对所述林间环境中所有树木的割胶。

与现有技术相比，本发明的有益效果是：

本发明提出了一种基于树干聚类跟踪的机器人定位方法及系统。所述方法包括：获取割胶机器人所处林间环境的激光雷达信息；根据割胶机器人预设距离范围和林间环境中物体的体积，对激光雷达信息进行过滤；采用密度聚类算法对过滤后的激光雷达信息进行聚类，得到激光雷达聚类信息；根据激光雷达聚类信息，采用圆弧拟合算法确定林间环境中每棵树木的树干圆柱面圆心二维坐标和树干位置信息；由树干圆柱面圆心二维坐标和树干位置信息，计算割胶机器人的定位信息；根据树干圆柱面圆心二维坐标和定位信息，采用迪杰斯特拉算法确定割胶机器人的行进路径，以实现割胶机器人的定位。本发明相比于传统的偏航角计算法和三角定位法，可以将多棵树的位置信息融合，尽可能多地利用激光雷达所获得的信息量，定位精度高，并且具有计算上的高效性和稳定性。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。

图1为本发明实施例1一种基于树干聚类跟踪的机器人定位方法的流程图；

图2为本发明实施例2激光雷达扫描范围示意图；

图3为本发明实施例2定位信息计算过程的示意图；

图4为本发明实施例3一种基于树干聚类跟踪的机器人定位系统的结构示意图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

为使本发明的上述目的、特征和优点能够更加明显易懂，下面结合附图和具体实施方式对本发明作进一步详细的说明。

实施例1

图1为本发明实施例1一种基于树干聚类跟踪的机器人定位方法的流程图。参见图1，本实施例的基于树干聚类跟踪的机器人定位方法，包括：

步骤S1：获取割胶机器人所处林间环境的激光雷达信息。

所述激光雷达信息是位于所述割胶机器人上的激光雷达通过对林间环境进行扫描得到的；所述激光雷达信息包括距离信息和角度信息。

步骤S2：根据割胶机器人预设距离范围和所述林间环境中物体的体积，对所述激光雷达信息进行过滤，得到过滤后的激光雷达信息。

所述步骤S2，具体包括：

将所述激光雷达信息中位于割胶机器人预设距离范围之外的信息删除，得到初步过滤后的激光雷达信息；所述割胶机器人预设距离范围为预设矩形框围成的区域；所述预设矩形框为在以所述割胶机器人为原点，所述割胶机器人右侧为x轴正向，所述割胶机器人正前方为y轴正向所构建的平面直角坐标系中，以坐标点(-1m,0)、(2m,0)、(-1m,8m)、(2m,8m)为顶点的矩形。

计算所述林间环境中各物体的体积；所述体积的计算公式为

R表示激光雷达遇到物体后反馈的距离，n表示连续扫描到物体的激光线数。

将所述初步过滤后的激光雷达信息中，所述体积小于预设体积阈值的信息删除，得到过滤后的激光雷达信息。

步骤S3：采用密度聚类算法对所述过滤后的激光雷达信息进行聚类，得到激光雷达聚类信息。

所述激光雷达聚类信息表征所述林间环境中每棵树木的轮廓。

所述步骤S3，具体包括：

根据所述过滤后的激光雷达信息的密集程度，采用DBSCAN聚类算法(Density-Based Spatial Clustering of Applications with Noise，具有噪声的基于密度的聚类方法)对所述过滤后的激光雷达信息进行聚类，得到激光雷达聚类信息。

步骤S4：根据所述激光雷达聚类信息，采用圆弧拟合算法确定所述林间环境中每棵树木的树干圆柱面圆心二维坐标和树干位置信息。

所述树干位置信息包括树干的位置坐标、树干相对割胶机器人的角度以及树干圆柱面的半径。

所述步骤S4，具体包括：

计算所述激光雷达聚类信息中每个点到预设圆弧的距离。

由所述距离确定每棵树木对应的树干曲线；所述树干曲线是由树木的轮廓点中到所述预设圆弧的距离均相等的点构成的。

由所述树干曲线确定对应树木的树干圆柱面圆心二维坐标和树干位置信息。

步骤S5：由所述树干圆柱面圆心二维坐标和所述树干位置信息，计算所述割胶机器人的定位信息。

所述定位信息为所述割胶机器人相对树干的相对位置信息；所述相对位置信息包括所述割胶机器人的位置坐标和所述割胶机器人相对树干的角度。

步骤S6：根据所述树干圆柱面圆心二维坐标和所述定位信息，采用迪杰斯特拉算法确定所述割胶机器人的行进路径，以实现所述割胶机器人的定位。

所述步骤S6，具体包括：

根据所述定位信息确定所述割胶机器人的当前位置。

判断位于所述当前位置的割胶机器人是否对当前树木完成割胶操作。若否，则继续执行割胶操作；若是，则将位于所述割胶机器人的正前方，且距离当前树木的树干圆柱面圆心二维坐标的长度为0.5m的点作为所述割胶机器人的下一位置，并将下一位置对应的树木作为当前树木，再返回“判断位于所述当前位置的割胶机器人是否对当前树木完成割胶操作”的步骤，直至完成对所述林间环境中所有树木的割胶。

本实施例的基于树干聚类跟踪的机器人定位方法，相比于传统的偏航角计算法和三角定位法，可以将多棵树的位置信息融合，尽可能多地利用激光雷达所获得的信息量，定位精度高，并且具有计算上的高效性和稳定性。

实施例2

本实施例提供了一种更为具体的基于树干聚类跟踪的机器人定位方法。本实施例包括如下步骤。

步骤1：通过割胶机器人的激光雷达得到林间环境的激光雷达信息，激光雷达信息包括机器人与树木之间的距离信息及树木相对机器人的角度信息(机器人正右方为x轴正方向，正前方为y轴正方向)。激光雷达扫描的范围为前方270度(正前方为135度，右后侧为0度，左后侧为270度)、范围为10米内的扇形区域，如图2所示。由于激光雷达信息中除包括上述信息外，还包含杂草、经济作物等各类物体，所以接下来，根据割胶机器人预设距离范围(提前设定，本场景中为机器人左侧1m，右侧2m，前方8m的矩形框，即忽略矩形框外信息)、林间环境中物体的预设体积，也称物体反射激光雷达线数阈值(提前设定，本是实施例中设定为10个，即忽略连续雷达点数小于10个的信息)，对激光雷达信息进行初步滤波，尽可能地过滤杂草和激光雷达噪声。具体滤波过程：激光雷达信息中可用信息包括障碍物距离及角度(机器人坐标系下)，滤波根据这两个参数，利用距离限定可用信息区域，利用某个物体体积(用线数表示，被越多线的雷达扫到，证明体积越大，可用公式

计算，R表示激光雷达遇到物体后反馈的距离，n表示连续扫描到物体的激光线数)，共同过滤掉较细的杂草和噪声(在3米以外区域，连续线数小于9，即直径小于15cm的物体)，以此得到过滤后的激光雷达信息，以用作下一步聚类的激光雷达信息，即被扫描的树木及极少量的其他物体在机器人极坐标系下的距离信息和角度信息。

步骤2：将步骤1中滤波后的激光雷达信息进行聚类，树干的激光雷达扫描数据具有明显的密度集中规律，所以我们需要结合这一密度集中规律从激光雷达扫描到的二维点云数据中提取橡胶树的轮廓。本实施例在聚类算法上，选用DBSCAN聚类算法。具体的

过程为：根据被聚类点的密度分布状况将密度大于设定阈值的数据称作中心点、将密度低于设定阈值的数据称为边缘点，并将相互接近的中心点聚为一类，最后再将所有边缘点和距离其最近的聚类中心点聚为一类。按照此算法将所有的激光雷达信息遍历，则完成了所有信息的聚类。

步骤3：在得到聚类的信息后，树干(树木)和树干之间有了分界线，为了进一步得到更高的定位精度，采用圆弧拟合算法得到林间环境中每棵树木的树干圆柱面圆心二维坐标和树干位置信息。具体过程为：将步骤2中已经聚为一类的激光雷达信息用一条圆弧去遍历，即计算每个点相对于待优化圆弧的误差，该误差由每个测量点到假设圆弧所在位置的距离所决定，当同一个聚类中的所有的数据点基本位于同一条圆弧线上时，误差值趋近于0，该圆弧线就是计算得到的树干曲线。由该树干曲线可确定每棵树木的树干圆柱面圆心二维坐标和树干位置信息；所述树干位置信息包括树干的位置坐标、树干相对割胶机器人的角度以及树干圆柱面的半径。

步骤4：对机器人进行定位。在经过上述步骤1、2、3之后得到树干圆柱面圆心二维坐标和树干位置信息，树干位置信息即树干相对于机器人的相对位置信息。依据上述信息，由坐标系变换公式可直接反推机器人相对于树干的相对位置信息，从而根据树木在世界坐标系下的位置信息直接计算机器人在世界坐标系下的位置，即得到机器人的定位信息，机器人的定位信息包括割胶机器人的位置坐标和割胶机器人相对树干的角度，具体计算过程如图3所示。图3的(a)部分中，实线框表示当前时刻机器人的位姿，当前时刻机器人的定位信息为(x_t,y_t,θ_t)，x_t表示当前时刻机器人在X轴的位置，y_t表示当前时刻机器人在Y轴的位置，θ_t表示当前时刻机器人的偏航角；虚线框表示上一时刻机器人的位姿，上一时刻机器人的定位信息为(x_t-1,y_t-1,θ_t-1)。图3的(b)部分中，实线箭头表示真实观测，虚线箭头表示预测观测，圆圈表示树的位置。

下面距离说明定位信息的计算过程。例如我们已知树A和树B在世界中的位置及其在机器人观测下的位置A’和B’，我们就可以根据上述方法直接计算机器人在世界坐标系下的位置R。在已经有了关于树在世界坐标系下的位置信息的情况下，还需要进行地图和传感器扫描信息的数据关联，思路如下：这一二维激光雷达的数据关联问题可以抽象为一个指派问题，指派问题有很多较为成熟的算法可以解决，其中匈牙利算法是最通用和有效的算法之一。关于二维激光雷达的跟踪问题，以匈牙利算法和卡尔曼滤波算法为基础衍生出了全局最近邻算法(GNN Algorithm)。

步骤5：根据所述树干圆柱面圆心二维坐标和所述定位信息，采用迪杰斯特拉算法确定所述割胶机器人的行进路径，以实现所述割胶机器人的定位。具体过程为：首先，确定150m*150m的空白地图(代价值处处等于0)，机器人初始位置设置为空白地图的中心(即默认初始位置为map系下原点)。然后，根据步骤3得到的树干圆柱面圆心二维坐标，构建出林间环境的坐标地图(仅包含地图范围、及树干圆柱面圆心二维坐标在世界坐标系下的坐标)，即确定林间环境的相对位置。接下来，不断通过步骤4更新跟踪移动后的割胶机器人的位置，计算机聚类线程会实时发布割胶机器人前方第一棵树木的圆心坐标，导航线程在收到该位置后，根据割胶线程反馈判断是否进行完成割胶操作，若未完成，则等待；若完成，则将距离圆心坐标的左侧(y轴正方向)0.5m处作为目标点，再将当前机器人位姿设置为初始点，依次把地图上离初始点最近的点(采用栅格地图，以像素点的形式储存，即完成始末位置的像素点连线)找到并加入集合S，最终，集合S中所有的点都包含该点到初始点最短路径长度，以实现实时路径规划，并控制割胶机器人按照该路径行进。

本实施例中的基于树干聚类跟踪的机器人定位方法，相比于传统的树林间导航算法，具有更高的精度和鲁棒性；该方法树木的位置为参照物，特征更加明显，不易丢失；在得到树干信息后，将其转化为稳定的树干圆柱面圆心二维坐标可以增加树干路标点的位置稳定性和机器人的定位精度；基于非线性优化的方式相比于传统的偏航角计算法和三角定位法具有计算上的高效性和稳定性，可以将多棵树的位置信息融合，尽可能多地利用激光雷达所获得的信息量且高效地计算出定位结果。

实施例3

本实施例提供了一种基于树干聚类跟踪的机器人定位系统，图4为本发明实施例3一种基于树干聚类跟踪的机器人定位系统的结构示意图。

本实施例的基于树干聚类跟踪的机器人定位系统，包括：

雷达信息获取模块401，用于获取割胶机器人所处林间环境的激光雷达信息；所述激光雷达信息是位于所述割胶机器人上的激光雷达通过对林间环境进行扫描得到的；所述激光雷达信息包括距离信息和角度信息。

过滤模块402，用于根据割胶机器人预设距离范围和所述林间环境中物体的体积，对所述激光雷达信息进行过滤，得到过滤后的激光雷达信息。

聚类模块403，用于采用密度聚类算法对所述过滤后的激光雷达信息进行聚类，得到激光雷达聚类信息；所述激光雷达聚类信息表征所述林间环境中每棵树木的轮廓。

圆弧拟合模块404，用于根据所述激光雷达聚类信息，采用圆弧拟合算法确定所述林间环境中每棵树木的树干圆柱面圆心二维坐标和树干位置信息；所述树干位置信息包括树干的位置坐标、树干相对割胶机器人的角度以及树干圆柱面的半径。

定位信息计算模块405，用于由所述树干圆柱面圆心二维坐标和所述树干位置信息，计算所述割胶机器人的定位信息；所述定位信息为所述割胶机器人相对树干的相对位置信息；所述相对位置信息包括所述割胶机器人的位置坐标和所述割胶机器人相对树干的角度。

行进路径确定模块406，用于根据所述树干圆柱面圆心二维坐标和所述定位信息，采用迪杰斯特拉算法确定所述割胶机器人的行进路径，以实现所述割胶机器人的定位。

作为一种可选的实施方式，所述过滤模块402，具体包括：

第一删除单元，用于将所述激光雷达信息中位于割胶机器人预设距离范围之外的信息删除，得到初步过滤后的激光雷达信息；所述割胶机器人预设距离范围为预设矩形框围成的区域；所述预设矩形框为在以所述割胶机器人为原点，所述割胶机器人右侧为x轴正向，所述割胶机器人正前方为y轴正向所构建的平面直角坐标系中，以坐标点(-1m,0)、(2m,0)、(-1m,8m)、(2m,8m)为顶点的矩形。

体积计算单元，用于计算所述林间环境中各物体的体积；所述体积的计算公式为

R表示激光雷达遇到物体后反馈的距离，n表示连续扫描到物体的激光线数。

第二删除单元，用于将所述初步过滤后的激光雷达信息中，所述体积小于预设体积阈值的信息删除，得到过滤后的激光雷达信息。

作为一种可选的实施方式，所述聚类模块403，具体包括：

聚类单元，用于根据所述过滤后的激光雷达信息的密集程度，采用DBSCAN聚类算法对所述过滤后的激光雷达信息进行聚类，得到激光雷达聚类信息。

作为一种可选的实施方式，所述圆弧拟合模块404，具体包括：

距离计算单元，用于计算所述激光雷达聚类信息中每个点到预设圆弧的距离。

树干曲线确定单元，用于由所述距离确定每棵树木对应的树干曲线；所述树干曲线是由树木的轮廓点中到所述预设圆弧的距离均相等的点构成的。

位置信息确定单元，用于由所述树干曲线确定对应树木的树干圆柱面圆心二维坐标和树干位置信息。

作为一种可选的实施方式，行进路径确定模块406，具体包括：

当前位置确定单元，用于根据所述定位信息确定所述割胶机器人的当前位置。

判断单元，用于判断位于所述当前位置的割胶机器人是否对当前树木完成割胶操作；若否，则继续执行割胶操作；若是，则将位于所述割胶机器人的正前方，且距离当前树木的树干圆柱面圆心二维坐标的长度为0.5m的点作为所述割胶机器人的下一位置，并将下一位置对应的树木作为当前树木，再返回所述判断位于所述当前位置的割胶机器人是否对当前树木完成割胶操作，直至完成对所述林间环境中所有树木的割胶。

本实施例的基于树干聚类跟踪的机器人定位系统，相比于传统的偏航角计算法和三角定位法，可以将多棵树的位置信息融合，尽可能多地利用激光雷达所获得的信息量，定位精度高，并且具有计算上的高效性和稳定性。

本说明书中各个实施例采用递进的方式描述，每个实施例重点说明的都是与其他实施例的不同之处，各个实施例之间相同相似部分互相参见即可。对于实施例公开的系统而言，由于其与实施例公开的方法相对应，所以描述的比较简单，相关之处参见方法部分说明即可。

本文中应用了具体个例对本发明的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本发明的方法及其核心思想；同时，对于本领域的一般技术人员，依据本发明的思想，在具体实施方式及应用范围上均会有改变之处。综上所述，本说明书内容不应理解为对本发明的限制。

Claims (8)

1.一种基于树干聚类跟踪的机器人定位方法，其特征在于，包括：

获取割胶机器人所处林间环境的激光雷达信息；所述激光雷达信息是位于所述割胶机器人上的激光雷达通过对林间环境进行扫描得到的；所述激光雷达信息包括距离信息和角度信息；

根据割胶机器人预设距离范围和所述林间环境中物体的体积，对所述激光雷达信息进行过滤，得到过滤后的激光雷达信息；

采用密度聚类算法对所述过滤后的激光雷达信息进行聚类，得到激光雷达聚类信息；所述激光雷达聚类信息表征所述林间环境中每棵树木的轮廓；

根据所述激光雷达聚类信息，采用圆弧拟合算法确定所述林间环境中每棵树木的树干圆柱面圆心二维坐标和树干位置信息；所述树干位置信息包括树干的位置坐标、树干相对割胶机器人的角度以及树干圆柱面的半径；

由所述树干圆柱面圆心二维坐标和所述树干位置信息，计算所述割胶机器人的定位信息；所述定位信息为所述割胶机器人相对树干的相对位置信息；所述相对位置信息包括所述割胶机器人的位置坐标和所述割胶机器人相对树干的角度；

根据所述树干圆柱面圆心二维坐标和所述定位信息，采用迪杰斯特拉算法确定所述割胶机器人的行进路径，以实现所述割胶机器人的定位；

所述根据所述激光雷达聚类信息，采用圆弧拟合算法确定所述林间环境中每棵树木的树干圆柱面圆心二维坐标和树干位置信息，具体包括：

计算所述激光雷达聚类信息中每个点到预设圆弧的距离；

由所述距离确定每棵树木对应的树干曲线；所述树干曲线是由树木的轮廓点中到所述预设圆弧的距离均相等的点构成的；

由所述树干曲线确定对应树木的树干圆柱面圆心二维坐标和树干位置信息。

2.根据权利要求1所述的一种基于树干聚类跟踪的机器人定位方法，其特征在于，所述根据割胶机器人预设距离范围和所述林间环境中物体的体积，对所述激光雷达信息进行过滤，得到过滤后的激光雷达信息，具体包括：

将所述激光雷达信息中位于割胶机器人预设距离范围之外的信息删除，得到初步过滤后的激光雷达信息；所述割胶机器人预设距离范围为预设矩形框围成的区域；所述预设矩形框为在以所述割胶机器人为原点，所述割胶机器人右侧为x轴正向，所述割胶机器人正前方为y轴正向所构建的平面直角坐标系中，以坐标点(-1m,0)、(2m,0)、(-1m,8m)、(2m,8m)为顶点的矩形；

计算所述林间环境中各物体的体积；所述体积的计算公式为

R表示激光雷达遇到物体后反馈的距离，n表示连续扫描到物体的激光线数；

将所述初步过滤后的激光雷达信息中，所述体积小于预设体积阈值的信息删除，得到过滤后的激光雷达信息。

3.根据权利要求1所述的一种基于树干聚类跟踪的机器人定位方法，其特征在于，所述采用密度聚类算法对所述过滤后的激光雷达信息进行聚类，得到激光雷达聚类信息，具体包括：

根据所述过滤后的激光雷达信息的密集程度，采用DBSCAN聚类算法对所述过滤后的激光雷达信息进行聚类，得到激光雷达聚类信息。

4.根据权利要求1所述的一种基于树干聚类跟踪的机器人定位方法，其特征在于，根据所述树干圆柱面圆心二维坐标和所述定位信息，采用迪杰斯特拉算法确定所述割胶机器人的行进路径，以实现所述割胶机器人的定位，具体包括：

根据所述定位信息确定所述割胶机器人的当前位置；

判断位于所述当前位置的割胶机器人是否对当前树木完成割胶操作；

若否，则继续执行割胶操作；

若是，则将位于所述割胶机器人的正前方，且距离当前树木的树干圆柱面圆心二维坐标的长度为0.5m的点作为所述割胶机器人的下一位置，并将下一位置对应的树木作为当前树木，再返回所述判断位于所述当前位置的割胶机器人是否对当前树木完成割胶操作，直至完成对所述林间环境中所有树木的割胶。

5.一种基于树干聚类跟踪的机器人定位系统，其特征在于，包括：

雷达信息获取模块，用于获取割胶机器人所处林间环境的激光雷达信息；所述激光雷达信息是位于所述割胶机器人上的激光雷达通过对林间环境进行扫描得到的；所述激光雷达信息包括距离信息和角度信息；

过滤模块，用于根据割胶机器人预设距离范围和所述林间环境中物体的体积，对所述激光雷达信息进行过滤，得到过滤后的激光雷达信息；

聚类模块，用于采用密度聚类算法对所述过滤后的激光雷达信息进行聚类，得到激光雷达聚类信息；所述激光雷达聚类信息表征所述林间环境中每棵树木的轮廓；

圆弧拟合模块，用于根据所述激光雷达聚类信息，采用圆弧拟合算法确定所述林间环境中每棵树木的树干圆柱面圆心二维坐标和树干位置信息；所述树干位置信息包括树干的位置坐标、树干相对割胶机器人的角度以及树干圆柱面的半径；

定位信息计算模块，用于由所述树干圆柱面圆心二维坐标和所述树干位置信息，计算所述割胶机器人的定位信息；所述定位信息为所述割胶机器人相对树干的相对位置信息；所述相对位置信息包括所述割胶机器人的位置坐标和所述割胶机器人相对树干的角度；

行进路径确定模块，用于根据所述树干圆柱面圆心二维坐标和所述定位信息，采用迪杰斯特拉算法确定所述割胶机器人的行进路径，以实现所述割胶机器人的定位；

所述圆弧拟合模块，具体包括：

距离计算单元，用于计算所述激光雷达聚类信息中每个点到预设圆弧的距离；

树干曲线确定单元，用于由所述距离确定每棵树木对应的树干曲线；所述树干曲线是由树木的轮廓点中到所述预设圆弧的距离均相等的点构成的；

位置信息确定单元，用于由所述树干曲线确定对应树木的树干圆柱面圆心二维坐标和树干位置信息。

6.根据权利要求5所述的一种基于树干聚类跟踪的机器人定位系统，其特征在于，所述过滤模块，具体包括：

第一删除单元，用于将所述激光雷达信息中位于割胶机器人预设距离范围之外的信息删除，得到初步过滤后的激光雷达信息；所述割胶机器人预设距离范围为预设矩形框围成的区域；所述预设矩形框为在以所述割胶机器人为原点，所述割胶机器人右侧为x轴正向，所述割胶机器人正前方为y轴正向所构建的平面直角坐标系中，以坐标点(-1m,0)、(2m,0)、(-1m,8m)、(2m,8m)为顶点的矩形；

体积计算单元，用于计算所述林间环境中各物体的体积；所述体积的计算公式为

R表示激光雷达遇到物体后反馈的距离，n表示连续扫描到物体的激光线数；

第二删除单元，用于将所述初步过滤后的激光雷达信息中，所述体积小于预设体积阈值的信息删除，得到过滤后的激光雷达信息。

7.根据权利要求5所述的一种基于树干聚类跟踪的机器人定位系统，其特征在于，所述聚类模块，具体包括：

聚类单元，用于根据所述过滤后的激光雷达信息的密集程度，采用DBSCAN聚类算法对所述过滤后的激光雷达信息进行聚类，得到激光雷达聚类信息。

8.根据权利要求5所述的一种基于树干聚类跟踪的机器人定位系统，其特征在于，行进路径确定模块，具体包括：

当前位置确定单元，用于根据所述定位信息确定所述割胶机器人的当前位置；

判断单元，用于判断位于所述当前位置的割胶机器人是否对当前树木完成割胶操作；若否，则继续执行割胶操作；若是，则将位于所述割胶机器人的正前方，且距离当前树木的树干圆柱面圆心二维坐标的长度为0.5m的点作为所述割胶机器人的下一位置，并将下一位置对应的树木作为当前树木，再返回所述判断位于所述当前位置的割胶机器人是否对当前树木完成割胶操作，直至完成对所述林间环境中所有树木的割胶。

Images