CN110780669B - 林间机器人导航与信息采集方法 - Google Patents

Abstract

本发明公开了一种林间机器人导航与信息采集方法。本发明一种林间机器人导航与信息采集方法，包括：路径规划：根据机器人导航利用的数据类别和其完成动作的不同，把导航划分为三个阶段：“直行阶段”、“转弯阶段”和“换行阶段”；以上三个阶段构成了机器人导航的整个循环过程，当机器人遇到超出上述三种之外的情况时，机器人将原地不动。本发明的有益效果：1.本发明针对树林的特定作业情况，开发了基于激光雷达和陀螺仪信息融合的且不依赖于先验地图的机器人林间导航及信息采集系统。本发明具有较强的鲁棒性，也可用于果园中果树的修剪、喷药、采摘等作业中。

Description

林间机器人导航与信息采集方法

技术领域

本发明涉及林间机器人领域，具体涉及一种林间机器人导航与信息采集方法。

背景技术

我国是农业大国，随着科技的发展、人口老龄化加剧和劳动力成本的上升，农业机械化、精细化、信息化发展的要求越来越迫切。智能农业装备是未来农业发展的趋势，而农业装备智能化不仅要求农业车辆可以自主导航，同时要求其对周围农业信息进行有效的提取和处理，如橡胶林中实现自动化割胶作业，果园中实现自动化修剪、喷药、采摘，大棚中实现自动化采收、精准化管理等，都需要能够自主导航的机器人搭载相应的自动化作业设备来实现。

传统技术存在以下技术问题：

现有的林间机器人无法自主完成采集信息和导航。

发明内容

本发明要解决的技术问题是提供一种林间机器人导航与信息采集方法。

为了解决上述技术问题，本发明提供了1.一种林间机器人导航与信息采集方法，其特征在于，包括：

路径规划：

根据机器人导航利用的数据类别和其完成动作的不同，把导航划分为三个阶段：“直行阶段”、“转弯阶段”和“换行阶段”；

以上三个阶段构成了机器人导航的整个循环过程，当机器人遇到超出上述三种之外的情况时，机器人将原地不动；

数据提取时把激光雷达扫描到的全部数据按照角度递增的原则进行排序，然后提取出0—180°范围内的扫描数据，即机器人右侧激光雷达数据；然后通过聚类的方式取出提取距离机器人最近的树行中距离机器人最近的三棵树上的点集；

聚类完成后，如果一个集群类中元素的数量大于给定的阈值N，那么它们就代表了一棵树：

Num(class_i)>N→class_i＝TreeTrunk

机器人导航所利用的数据均来自距离机器人最近的树行，因此该系统可允许作业树林行距与株距不同。由于树行间地形不平坦，机器人可能会出现波动。故陀螺仪实时获取机器人姿态信息，用以修正基于横摇和俯仰角度的激光雷达数据。

利用“高斯-牛顿法”对所提取的树干点集进行圆拟合，得到各棵树圆心坐标；用最小二乘法拟合出过圆心的直线即为机器人树行间行走导航线。

机器人系统控制：

将横向偏差E和航向偏差θ作为模糊控制器的输入量。选取左右两侧直流电机PWM控制差值U作为输出变量，从而控制机器人差速转向。把航向偏差θ、横向偏差E和输出量U分别划分为7个等级，负大(NB)，负中(NM)，负小(NS)，零(Z0)，正小(PS)，正中(PM)，正大(PB)。隶属度函数都选择三角型，选用MIN-MAX-重心法作为模糊控制器的解模糊方法,根据经验和试验情况制定模糊控制规则；采用基于特征的扩展卡尔曼滤波(EKF)为移动机器人进行姿态估计；

机器人信息采集：

采集林中树直径、株距、行距、位置信息；为了提高树直径的采集精度，选择当激光雷达离树距离最近且机器人暂停时，即当机器人停在树前方进行作业时，取出该棵树上的扫描点集合以及下棵需要作业的树与该棵树的相对位置坐标；利用“高斯-牛顿法”对每棵树的点集进行圆拟合，使各个点的误差平方之和最小，得到的圆直径作为树直径；因考虑实际情况中按照此方法拟合出的树半径整体偏小，故采取把每棵拟合的树半径加固定常数进行补偿。

根据树P₁的位置及整个树林的行列走势建立绝对坐标系XOY，此时树P₁的绝对坐标已知，为(x₁，y₁)。当机器人在树P₁正前方停下进行作业时，测量树P₂在机器人坐标系下的位置，然后根据陀螺仪Z轴数据，旋转机器人坐标系使其与绝对坐标系平行，根据激光雷达数据求得树P₂在绝对坐标系下的坐标(x₂，y₂)。依次递推，可获得整个作业树林的位置、株距、行距信息。

在其中一个实施例中，“直行阶段”此阶段中，利用激光雷达和陀螺仪数据融合，基于聚类的方式提取稀疏树干点云信息，然后用“高斯-牛顿法”对所提取的点云进行圆拟合，得到各棵树圆心坐标，再用最小二乘法拟合出过圆心的直线作为机器人林间导航线。“直行阶段”根据导航线以固定的横向距离沿着树行行走和在每棵树前定点停靠。

在其中一个实施例中，“转弯阶段”，此阶段利用陀螺仪Z轴数据及电机差速驱动使机器人右转弯90°。

在其中一个实施例中，“换行阶段”利用陀螺仪进行右转弯动作或者进行左转弯动作。

在其中一个实施例中，“然后通过聚类的方式取出提取距离机器人最近的树行中距离机器人最近的三棵树上的点集；”中，两点若属于同一类需满足以下条件：

(1)机器人坐标系中，两点与激光雷达的距离差小于阈值δ，即L₁-L₂＜δ；

(2)机器人坐标系中，两点与激光雷达的角度差小于阈值α，即α₁-α₂＜α；

(3)机器人坐标系中，两点与导航线的距离差小于阈值ε(ε根据树行距确定)，即L₁sinα₁-L₂ sinα₂＜ε。

一种计算机设备，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，所述处理器执行所述程序时实现任一项所述方法的步骤。

一种计算机可读存储介质，其上存储有计算机程序，该程序被处理器执行时实现任一项所述方法的步骤。

一种处理器，所述处理器用于运行程序，其中，所述程序运行时执行任一项所述的方法。

本发明的有益效果：

1.本发明针对树林的特定作业情况，开发了基于激光雷达和陀螺仪信息融合的且不依赖于先验地图的机器人林间导航及信息采集系统。本发明具有较强的鲁棒性，也可用于果园中果树的修剪、喷药、采摘等作业中。

2.在作业过程中，可通过设置阈值大小，避开因直径太小而不需作业的小树或树桩。

3.本发明基于聚类的方式提取稀疏树干点云信息，然后用“高斯-牛顿法”对所提取的点云进行圆拟合，得到各棵树圆心坐标，再用最小二乘法拟合出过圆心的直线作为机器人林间导航线。这种导航线提取方法在保证实时性的同时，提高了系统稳定性。

4.用扩展卡尔曼滤波(EKF)算法对机器人进行定位，用模糊控制器实现机器人沿着所规划出的导航线在林间以固定横向距离沿着树行行走、定点停靠、行尾转弯及信息采集等多种功能。

5.本发明通过设置激光雷达扫描距离为两倍最大株距，利用机器人右侧距离机器人最近的树行中距离最近的三棵树进行自主导航，提高了机器人适应性，允许作业林中行距、株距不同，且速度快、实时性好。

6.本发明利用激光雷达通过采集、提取林中树直径、株距、行距、位置等信息进行导航，又在导航的过程中采集、完善、精确树林信息，效率很高，所采集的信息最终以特征地图的方式呈现。所构建的特征地图不仅为树木的信息化、精准化管理提供了依据，还为基于先验地图的机器人林间导航提供了参考。

附图说明

图1是本发明林间机器人导航与信息采集方法中系统工作原理图。

图2是本发明林间机器人导航与信息采集方法中的导航路径规划示意图。

图3是本发明林间机器人导航与信息采集方法的流程示意图。

具体实施方式

下面结合附图和具体实施例对本发明作进一步说明，以使本领域的技术人员可以更好地理解本发明并能予以实施，但所举实施例不作为对本发明的限定。

针对林间智能农业装备自动化作业过程中需要机器人移动平台在每棵树前特定区域停车的需求，本发明基于激光雷达和陀螺仪所采集数据，根据拟合出的导航线，以设定的横向距离沿着树行行走的同时，实时采集激光雷达在90°方向的数据，并根据采集的数据控制机器人是否停车。如需改变在每棵树前停下的区域范围，只需修改所设定的沿树行横向距离的阈值和激光雷达判断角度阈值即可，具有较高的适应性。

针对一些因直径太小而机器人平台不需停下，即不需作业的小树或树桩，本发明根据所提取出的每棵树上的元素点集，判断该棵树直径，从而指令机器人是否忽略该目标。

如要实现智能装备在整片树林自主行驶作业，需要实现其自动转向和跨行行驶。本发明根据所提取的机器人平台右前方树的数量，判断是否是树行的行尾，是否需要转弯换行。如需要转弯换行，则利用陀螺仪使机器人旋转固定角度值，实现其左右转弯、跨行作业。

针对机器人在不同行距和株距的树林中适应性降低的问题，本发明通过设置激光雷达扫描距离为两倍最大株距，利用机器人右侧距离最近的树行中距离最近的三棵树的数据进行自主导航，提高了机器人适应性，允许作业林中行距、株距不同。

为实现树木的信息化、精准化管理，本发明利用激光雷达通过采集林中树直径、株距、行距、位置等信息进行导航，又在导航的过程中完善、精确所采集的信息，所采集的信息最终以特征地图的方式呈现。所构建的特征地图不仅为树木的信息化、精准化管理提供了依据，还为基于先验地图的机器人林间导航提供了参考。系统控制组成如图1所示。

1路径规划

根据机器人导航利用的数据类别和其完成动作的不同，把导航划分为三个阶段：“直行阶段”、“转弯阶段”和“换行阶段”。

“直行阶段”如图2中AB段所示。此阶段中，利用激光雷达和陀螺仪数据融合，基于聚类的方式提取稀疏树干点云信息，然后用“高斯-牛顿法”对所提取的点云进行圆拟合，得到各棵树圆心坐标，再用最小二乘法拟合出过圆心的直线作为机器人林间导航线。“直行阶段”根据导航线以固定的横向距离沿着树行行走和在每棵树前定点停靠。

“转弯阶段”如图2中BC段所示。此阶段利用陀螺仪Z轴数据及电机差速驱动使机器人右转弯90°。

“换行阶段”如图2中DE和FG段所示。DE段与AB段相似，不同点是此阶段无定点停靠，且当走过一个行距时将利用陀螺仪进行右转弯动作；FG段与BC段相似，不同点是此阶段进行左转弯。

以上三个阶段构成了机器人导航的整个循环过程，当机器人遇到超出上述三种之外的情况时，机器人将原地不动，如此一方面可确保机器人的安全性，另一方面当机器人完成整个橡胶林作业时，可自动停止动作。

数据提取时把激光雷达扫描到的全部数据按照角度递增的原则进行排序，然后提取出0—180°范围内的扫描数据，即机器人右侧激光雷达数据。然后通过聚类的方式取出提取距离机器人最近的树行中距离机器人最近的三棵树上的点集。两点若属于同一类需满足以下条件：

(1)机器人坐标系中，两点与激光雷达的距离差小于阈值δ，即L₁-L₂＜δ；

(2)机器人坐标系中，两点与激光雷达的角度差小于阈值α，即α₁-α₂＜α；

(3)机器人坐标系中，两点与导航线的距离差小于阈值ε(ε根据树行距确定)，即L₁sinα₁-L₂ sinα₂＜ε

聚类完成后，如果一个集群类中元素的数量大于给定的阈值N，那么它们就代表了一棵树：

Num(class_i)>N→class_i＝TreeTrunk

若树行中有因直径太小而不需作业的小树或者树桩时，可通过设置阈值N使机器人自动忽略。机器人导航所利用的数据均来自距离机器人最近的树行，因此该系统可允许作业树林行距与株距不同。由于树行间地形不平坦，机器人可能会出现波动。故陀螺仪实时获取机器人姿态信息，用以修正基于横摇和俯仰角度的激光雷达数据。

利用“高斯-牛顿法”对所提取的树干点集进行圆拟合，得到各棵树圆心坐标。然后用最小二乘法拟合出过圆心的直线即为机器人树行间行走导航线。

2机器人系统控制

将横向偏差E和航向偏差θ作为模糊控制器的输入量。选取左右两侧直流电机PWM控制差值U作为输出变量，从而控制机器人差速转向。把航向偏差θ、横向偏差E和输出量U分别划分为7个等级，负大(NB)，负中(NM)，负小(NS)，零(Z0)，正小(PS)，正中(PM)，正大(PB)。隶属度函数都选择三角型，选用MIN-MAX-重心法作为模糊控制器的解模糊方法,根据经验和试验情况制定模糊控制规则。基于特征的扩展卡尔曼滤波(EKF)为移动机器人姿态估计提供了一种有效的方法，因此本系统将其用于机器人林间定位。

3机器人信息采集

采集林中树直径、株距、行距、位置信息。为了提高树直径的采集精度，选择当激光雷达离树距离最近且机器人暂停时，即当机器人停在树前方进行作业时，取出该棵树上的扫描点集合以及下棵需要作业的树与该棵树的相对位置坐标。利用“高斯-牛顿法”对每棵树的点集进行圆拟合，使各个点的误差平方之和最小，得到的圆直径作为树直径。因考虑实际情况中按照此方法拟合出的树半径整体偏小，故采取把每棵拟合的树半径加固定常数进行补偿。

根据树P₁的位置及整个树林的行列走势建立绝对坐标系XOY，此时树P₁的绝对坐标已知，为(x₁，y₁)。当机器人在树P₁正前方停下进行作业时，测量树P₂在机器人坐标系下的位置，然后根据陀螺仪Z轴数据，旋转机器人坐标系使其与绝对坐标系平行，根据激光雷达数据求得树P₂在绝对坐标系下的坐标(x₂，y₂)。依次递推，可获得整个作业树林的位置、株距、行距信息。系统原理如图3所示。

下面介绍一下工作过程：

激光雷达和陀螺仪数据信息通过USB串口实时传递给工控机，工控机用陀螺仪数据修正基于横摇和俯仰角度的激光雷达数据。然后以聚类的方式提取距离机器人最近的树行中距离最近的三棵树树干点云信息，用“高斯-牛顿法”对所提取的点云进行圆拟合，得到各棵树圆心坐标，再用最小二乘法拟合出过圆心的直线作为机器人林间导航线。用扩展卡尔曼滤波(EKF)算法对机器人进行定位，用模糊控制器实现机器人沿着所规划出的导航线行走，同时采集、完善树林信息。所采集的信息最终以特征地图的方式呈现。

以上所述实施例仅是为充分说明本发明而所举的较佳的实施例，本发明的保护范围不限于此。本技术领域的技术人员在本发明基础上所作的等同替代或变换，均在本发明的保护范围之内。本发明的保护范围以权利要求书为准。

Claims (9)

1.一种林间机器人导航与信息采集方法，其特征在于，包括：

路径规划：

根据机器人导航利用的数据类别和其完成动作的不同，把导航划分为三个阶段：“直行阶段”、“转弯阶段”和“换行阶段”；

以上三个阶段构成了机器人导航的整个循环过程，当机器人遇到超出上述三种之外的情况时，机器人将原地不动；

数据提取时把激光雷达扫描到的全部数据按照角度递增的原则进行排序，然后提取出0—180°范围内的扫描数据，即机器人右侧激光雷达数据；然后通过聚类的方式提取距离机器人最近的树行中距离机器人最近的三棵树上的点集；

聚类完成后，如果一个集群类中元素的数量大于给定的阈值N，那么它们就代表了一棵树：

Num(class_i)>N→class_i＝TreeTrunk

机器人导航所利用的数据均来自距离机器人最近的树行，允许作业树林行距与株距不同；由于树行间地形不平坦，机器人可能会出现波动；故陀螺仪实时获取机器人姿态信息，用以修正基于横摇和俯仰角度的激光雷达数据；

利用“高斯-牛顿法”对所提取的树干点集进行圆拟合，得到各棵树圆心坐标；用最小二乘法拟合出过圆心的直线即为机器人树行间行走导航线；

机器人系统控制：

将横向偏差E和航向偏差θ作为模糊控制器的输入量；选取左右两侧直流电机PWM控制差值U作为输出变量，从而控制机器人差速转向；把航向偏差θ、横向偏差E和输出量U分别划分为7个等级，负大(NB)，负中(NM)，负小(NS)，零(Z0)，正小(PS)，正中(PM)，正大(PB)；隶属度函数都选择三角型，选用MI N-MAX-重心法作为模糊控制器的解模糊方法,根据经验和试验情况制定模糊控制规则；采用基于特征的扩展卡尔曼滤波(EKF)为移动机器人进行姿态估计；

机器人信息采集：

采集林中树直径、株距、行距、位置信息；为了提高树直径的采集精度，选择当激光雷达离树距离最近且机器人暂停时，即当机器人停在树前方进行作业时，取出该棵树上的扫描点集合以及下棵需要作业的树与该棵树的相对位置坐标；利用“高斯-牛顿法”对每棵树的点集进行圆拟合，使各个点的误差平方之和最小，得到的圆直径作为树直径；因考虑实际情况中按照此方法拟合出的树半径整体偏小，故采取把每棵拟合的树半径加固定常数进行补偿；

根据树P₁的位置及整个树林的行列走势建立绝对坐标系XOY，此时树P₁的绝对坐标已知，为(x₁，y₁)；当机器人在树P₁正前方停下进行作业时，测量树P₂在机器人坐标系下的位置，然后根据陀螺仪Z轴数据，旋转机器人坐标系使其与绝对坐标系平行，根据激光雷达数据求得树P₂在绝对坐标系下的坐标(x₂，y₂)；依次递推，可获得整个作业树林的位置、株距、行距信息。

2.如权利要求1所述的林间机器人导航与信息采集方法，其特征在于，所述直行阶段利用激光雷达和陀螺仪数据融合，基于聚类的方式提取稀疏树干点云信息，然后用“高斯-牛顿法”对所提取的点云进行圆拟合，得到各棵树圆心坐标，再用最小二乘法拟合出过圆心的直线作为机器人林间导航线；所述直行阶段根据导航线以固定的横向距离沿着树行行走和在每棵树前定点停靠。

3.如权利要求1所述的林间机器人导航与信息采集方法，其特征在于，所述转弯阶段利用陀螺仪Z轴数据及电机差速驱动使机器人右转弯90°。

4.如权利要求1所述的林间机器人导航与信息采集方法，其特征在于，所述换行阶段利用陀螺仪进行右转弯动作或者进行左转弯动作。

5.如权利要求1所述的林间机器人导航与信息采集方法，其特征在于，所述聚类的方式提取距离机器人最近的树行中距离机器人最近的三棵树上的点集中，两点若属于同一类需满足以下条件：

(1)机器人坐标系中，两点与激光雷达的距离差小于阈值δ，即L₁-L₂＜δ；

(2)机器人坐标系中，两点与激光雷达的角度差小于阈值α，即α₁-α₂＜α；

(3)机器人坐标系中，两点与导航线的距离差小于阈值ε，即L₁sinα₁-L₂sinα₂＜ε，其中，ε根据树行距确定。

6.如权利要求1所述的林间机器人导航与信息采集方法，其特征在于，若树行中有因直径太小而不需作业的小树或者树桩时，可通过设置阈值N使机器人自动忽略。

7.一种计算机设备，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，其特征在于，所述处理器执行所述程序时实现权利要求1到6任一项所述方法的步骤。

8.一种计算机可读存储介质，其上存储有计算机程序，其特征在于，该程序被处理器执行时实现权利要求1到6任一项所述方法的步骤。

9.一种处理器，其特征在于，所述处理器用于运行程序，其中，所述程序运行时执行权利要求1到6任一项所述的方法。

Images