3D LiDAR感知的植物行信息提取方法与试验

赵润茂，朱 政，陈建能，范国帅，王麒程，黄培奎

(1 浙江理工大学 机械与自动控制学院,浙江 杭州 310018；2 浙江省种植装备技术重点实验室,浙江 杭州 310018；3 华南农业大学 工程学院,广东 广州 510642)

无人农场是智慧农业的一种生产方式，也是实现智慧农业的重要途径。农业机械自动导航作业是无人农场的关键技术[1]，而车体航向和位置的精准获取则是农机自动导航的前提[2]，目前，主要有全球导航卫星系统[3-4](Global navigation satellite system，GNSS)、立体相机[5-6]和激光雷达(Light detecting and ranging，LiDAR) 3 种感知系统。其中，采用载波相位差分(Real-time kinematic，RTK)技术进行定位解算的GNSS 导航应用最为成熟并已在国际市场商业化，然而卫星通信因易受遮挡影响出现差分失锁或多路径效应，通常用于开阔地作业场景，不适于如林间或冠层下卫星信号盲区的特殊作业环境[7-8]。立体视觉通过彩色相机对环境成像，采用数字图像处理算法或神经网络深度学习模型分析提取图像中的有用信息，为机器人路径规划提供参考[9-10]，但视觉成像质量极易受光照影响，定位精度表现不稳定[11]。利用飞行时间(Time of flight，ToF)原理，2D LiDAR 可全天候获得传感器极坐标系中载体周围环境的二维坐标，但由于点云数据稀疏，小尺寸目标特征信息易缺失，因此更多用于植保[12]、收获[13]或变量作业[14]时大表面积冠层检测[15]，如蔡吉晨[16]依据果树冠层网格化体积模型设计了变量喷药策略，试制了基于2D LiDAR 的果园变量喷药机样机；薛秀云等[17]通过 LiDAR 检测技术提出表征树冠特征参数的叶墙面积及其关联变量喷雾模型，比采用纯对靶定量喷雾省药32.77%；侯加林等[18]采用基于机器人操作系统(Robot operating system，ROS)框架工作的开源cartographer算法，研制了双激光雷达温室运输机器人导航系统，但导航路径规划必须在地图信息已知的前提下进行。

3D LiDAR 具有更大视场，能提供更丰富的环境感知信息，可精准地表达三维模型表面特性和几何形状[19]，广泛用于轿车自动驾驶系统的局部路径规划[20-22]；因为硬件成本和数据计算负担限制，在农业工程领域，3D LiDAR 主要用于基础科学研究，如农植物几何参数获取[23-24]、表型[25]和逆向重构[26]等，苏伟等[27]提出一种基于地面激光扫描技术的大田玉米个体表型信息提取方法，陈日强等[28]利用无人机搭载3D LiDAR 测量每棵果树的树冠面积和直径，并分析了空间分辨率对单木树冠检测与提取结果的影响；刘路等[29]采用了 Velodyne 16 线激光雷达(VLP-16)解析高遮挡环境下玉米植物行中心导航线，但传感器成本较高且未考虑载体姿态变化对植株定位的影响；为实现低成本3D 感知，刘宽[30]、Gasparino 等[31]采用舵机驱动2D LiDAR 定轴摆动，研制了摆头式3D LiDAR 探测系统，由于需要实时测量LiDAR 姿态，并涉及多坐标系统配准，工程化实施难度大。

本文针对林间或冠层下GNSS 信号遮挡严重区域的农业机器人导航需求，提出一种低成本3D LiDAR 感知的点云数据处理与植物行信息提取方法。该方法不仅能够补偿载体姿态变化对点云空间坐标的影响，还可实现不同直径的植株主干自适应分割，避免重叠交错枝叶影响植株中心求取；最后，开展模拟试验和实地试验对算法精度的有效性进行验证。

1 系统组成与坐标标定

1.1 试验材料

1.1.1 3D LiDAR 采用Livox Horizon 3D LiDAR 作为农业机器人导航的感知传感器，3D LiDAR 的扫描量程为0.5 ～ 260.0 m，水平视场为81.7°，垂直视场为25.1°，每秒可输出点云240 000个。当LiDAR 激光束遇到障碍物时发生反射，测量反射时间来确定距离[32]。内置惯性测量单元(Inertial measurement unit，IMU)，通过测量农业机器人行驶过程中的横滚角 θr，俯仰角θp，航向角 ψ，对LiDAR 输出的点云坐标xlidar、ylidar、zlidar进行实时姿态校正，使测量结果不受载体运动姿态影响，在出厂前已完成LiDAR 与IMU 的外参标定和同步。校正公式如下：

式中：RIMU为IMU 的旋转矩阵，xlidar、ylidar、zlidar为LiDAR 的点云坐标，xmix、ymix、zmix为校正后的LiDAR 点云坐标。

相比于常规多线机械旋转式3D LiDAR，Livox Horizon 采用非重复扫描方式获取传感器与环境的距离，扫描时间超过0.1 s 的时候，可以达到常见64 线机械旋转式激光雷达扫描覆盖率；达到0.5 s左右时，视场覆盖率将接近100%，价格仅有国外同类64 线产品的1/50 ～ 1/100；因此，研究选用Livox Horizon 作为植物行感知传感器，且其小视场、非重复式扫描的特点非常适于封闭、低速的农业机器人导航环境。

1.1.2 GNSS 定位系统 为评价本文提出的作物行提取精度，以GNSS 天线定位数据作为环境信息位置真值。GNSS 采用天宝定位板卡BD 970 接收解析卫星信号，千寻连续运行参考站(Continuously operating reference stations，CORS) 提供基于RTK 的定位信息，能实现1～2 cm 的定位精度。通过将位置已知基准站测量的伪距修正值或相位信息发送到移动站来提高精度，并将定位数据通过串口以GNGGA 、GNVTG 的形式输送至上位机。

1.1.3 机器人底盘 采用的四轮差速转向机器人如图1所示，结构尺寸(长×宽×高)为830 mm×650 mm×370 mm，底盘高度为120 mm，轮胎为直径300 mm的真空胎，最高时速为8 km/h，轮胎减震且具有自适应悬挂，提高了在越野路面的行驶通过性，适用于农业环境下工作。

图1 轮式机器人与传感器坐标系Fig.1 Wheeled robot and sensor coordinate system

1.2 采用的坐标系统

1.2.1 坐标系统定义 WGS-84 大地坐标系：该坐标反映了地球表面各点的空间分布，是为全球定位系统(Global positioning system，GPS)的使用而建立的一种通用地心坐标系；研究使用的GNSS 接收机输出的经纬度坐标即是 WGS-84 坐标下天线的定位。

传感器坐标系：即LiDAR 坐标系，如图1所示，原点在高于LiDAR 底部50.1 mm 的透镜中心，在笛卡尔右手坐标系下定义Y轴正方向指向传感器正前方，Z轴过传感器形心垂直于水平面。LiDAR内置的IMU 坐标原点OI在LiDAR 坐标系上的坐标为(-55.12，-22.26，29.70)(单位：mm)。

导航坐标系：通过高斯吕格投影先将WGS-84 大地坐标系的经纬度信息转化到局部切平面坐标系，X轴指向地理北方，Y轴指向东方，再经坐标平移至导航坐标系，本研究拟进行的数据处理及分析均在导航坐标系下进行。

1.2.2 坐标系统转换 IMU 校正后的LiDAR 输出数据是IMU 系下的环境信息坐标，已与导航系东北平面平行，还需进行航向标定。如图2所示，E、F、G是GNSS 天线在导航坐标系下的位置，分别记为(xi，yi，zi)(i=1,2,3)，采用隐式(3)计算LiDAR 坐标系原点在导航系下的位置，记为(x，y，z)。LiDAR 坐标系与导航坐标系的航向偏差yaw 为的航向角与航向角的差值，代入式(4)求解导航坐标系下LiDAR 原点坐标(xN，yN，zN)。

图2 导航坐标系与 IMU 坐标系相对位置Fig.2 Relative position of navigation coordinate system and IMU coordinate system

1.3 研究方法

1.3.1 点云信息预处理 LiDAR 视场为圆锥面，每秒最多获得240 000 个点，最大探测距离可达260 m，在数据采集的过程中会引入大量的目标无关点，比如杂草、地面、周围的建筑物，保留其原始探测范围会在探测到更多目标信息的同时增加处理工作量，延长数据处理时间[33]；另外，距离传感器越远，LiDAR 测得的点云变得稀疏，所能提供可利用的有效信息变少，因此，需要在不改变点云特征的前提下实时对数据进行预处理，具体步骤如下。

1) 感兴趣区域和降采样

以LiDAR 的安装位置为原点，移动机器人的航向为Y轴，建立直角坐标系。根据葡萄田环境，通过直通滤波分别在X轴、Y轴和Z轴设置6、12和2 m 的感兴趣区域(Region of interest,ROI)。直通滤波处理点云的具体计算式如式(5)所示。并采用体素滤波对ROI 区域内的点云进行降采样，体素单元大小为0.1 m×0.1 m×0.1 m。估计每个立方体的形心坐标，以其代表立方体内的所有点云输出。直通滤波与体素滤波能在避免缺失点云特征的前提下有效降低点云数据，加快数据处理速度。

式中，(u,v,w) 为雷达点云的位置坐标，m；dst(u,v,w)为ROI 区域内的点云坐标，m；src(u,v,w)为原始点云坐标，m；dmin为直通滤波下限，m；dmax为直通滤波上限，m；dmax取值为LiDAR 正前方3～4 株植物株距。

2)地面点云分割

采用Ray Ground Filter 分割地面点云，具体方法如下：首先，按照LiDAR 水平角分辨率将点云剖分为360÷0.2=1 800 个纵截面，并将每个纵截面内的点按照距离远近排序；其次，设定同条扫描线上邻近两点的坡度阈值(local_max_slope)和整个地面的坡度阈值(general_max_slop)，并根据这2 个坡度阈值以及当前点的半径(到LiDAR 的水平距离)求得高度阈值；最后，若当前点的高度值在地面加减高度阈值范围内，则判定为地面点并删除。

3)半径滤波

受系统测量噪声和环境特性等干扰因素的影响，3D LiDAR 点云存在部分离群点，在数据特征上表现为远离目标点云，应当滤除。采用半径滤波去除主干周围的离群点云。半径滤波方法运行速度快，并且依序迭代留下的点一定是最密集的，极其适用于点云数据量大、实时性要求高的场景，流程如图3所示。

图3 半径滤波流程图Fig.3 Flow chart of radius filtering

1.3.2 植物中心点提取 为获取植物主干区域，提取中心点，刘路等[29]采用了将三维点云投影到二维平面后再进行k-mean 聚类的方式，对于未知环境的点云，这种方法难以预先确定聚类簇的数量，部分枝叶的点云也会因相互遮挡难以采集，导致聚类得到的中心点会有较大误差；而DBSCAN 聚类算法，由于离LiDAR 较近的数据点密度高，在聚类半径相同的情况下，会存在距离较近且较细的杂草枝叶与距离较远且直径较大的树干相比接收到的数据点数量接近甚至更多的现象，从而出现大量误判与漏检；而均值漂移聚类算法受异常值的影响较小，而且能在未知环境下对数据进行聚类，具体流程如下：

1) 在随机一块植物点云中任取一点x作为中心点；

2) 找出离中心点x距离在带宽之内的所有点xi，记做集合M，认为这些点属于簇c；

3) 如公式(6)所示，计算从中心点开始到集合M 中每个元素的向量，将这些向量相加，得到偏移向量M(x)；

4) 中心点沿着漂移均值点的方向移动，移动距离是偏移向量的模；

5) 重复步骤2)～4)，直到偏移向量的大小满足设定的阈值要求并且逐渐收敛，记住此时的中心点；

6) 重复1) ～ 5)直到所有的点都被归类。

式中，Sr为以x为圆心、r为半径的圆形区域，m；k为包含在Sr范围内的点的个数；xi为包含在Sr范围内的点。

由于植物间交错的枝叶会影响到植物中心点的判断，因此，必须利用植物的主干信息提取植物行；同时，每株植物枝叶位置随机生长，如图4所示，机器人在前进过程中，3D LiDAR 视场随载体运动状态发生变化，而固定参数的直通滤波器不能准确分割植物主干部分的点云。综上，研究提出一种扫描区域自适应的主干中心点提取方法：如图5所示，对每一棵植物的点云沿着高度方向取每层h进行层切[34]，分别搜索每层切片点云中点集在X轴和Y轴方向的最大值xmax、ymax与最小值xmin、ymin，根据植物的主干直径与枝叶直径设定阈值[35]dx、dy,保留同时满足X轴、Y轴阈值条件的点云，自动筛选植物主干部分点云，目标点云的具体表达式如式(7)所示：

图4 不同位置的LiDAR 扫描植物Fig.4 Scanning plants by LiDAR at different positions

图5 高度自适应点云分割Fig.5 Highly adaptive point cloud segmentation

式中，(x,y,z) 为分类后每层点云的位置坐标，m；dst(x,y,z)为目标点云坐标，m；src(x,y,z)为原始点云坐标，m；dx为X轴方向的阈值，m；dy为Y轴方向的阈值，m。

主干分割后再进行垂直投影，得到的点云团相对稠密且离群点少，避免了枝叶点云的影响，通过点云团中的点取平均值得到植物中心，精度高，速度快。

1.3.3 植物行拟合 目前，用于植物行识别和导航定位基准线检测的方法主要包括随机样本一致性(Random sample consensus,RANSAC)、Hough 变换法和最小二乘法等。其中RANSAC 运算速度较慢，随机性较高[36]，且LiDAR 扫描点云的疏密程度受植株距离的影响，导致RANSAC 拟合的路径与真实的路径有所偏差；Hough 变换法虽然是应用最广泛的直线监测方法，有较好的鲁棒性并且受噪声影响小，但Hough 变换时间复杂度与空间复杂度较高；与上述2 种方法相比，根据植物中心点通过最小二乘法检测路径，误差小，速度快，更符合实际应用需求，具体计算式为(8)～(10)：

式中，y为假设植物行的线性方程；Se2为总误差平方和；xi为植物主干中心点x坐标；yi为植物主干中心点y坐标；a、b为常数。

1.4 植物行提取方法的精度验证

为验证通过上述方法拟合的植物行精度，进行了LiDAR 识别植物行的精度验证试验。常用作真值参考的RTK-GNSS 定位方法在树林场景中失效，一些常用的室内定位手段，如超宽带(Ultra wide band，UWB)技术，精度较低，为5～10 cm。因此，参照文献[29]和[37]中的方法，在开阔地建立仿真果园，使用RTK 定位系统验证所提出算法的精度。如图6a 所示，采用粘贴有芦苇叶且直径不同的塑料圆管模拟植物，每根圆管高为1 m，直径25～50 mm，其中一次试验排列方式如图6b 所示。使用LiDAR 获取模拟植物的点云信息并通过LiDAR 内置IMU 进行姿态校正，得到导航坐标系下的点云；再将GNSS 天线分别放置在每个圆管的圆心处得到每个圆管圆心处的经纬度信息，采用高斯吕格投影[38]，将GNSS 天线采集的坐标数据从WGS-84 坐标系也转化至导航坐标系下以获取模拟植物位置真值，测量一段时间，取平均值；最后用最小二乘法分别对LiDAR 点云和植物坐标真值进行直线拟合，并根据拟合的直线倾角评价二者结果，验证算法的精度。

图6 搭建仿真果园进行模拟试验验证提取的植物行精度Fig.6 Build a simulation orchard for simulation experiments to verify the accuracy of extracted plant rows

每完成一次扫描感知后，任意改变圆管的排布位置，进行下一次扫描，试验共进行3 次。

2 结果与分析

2.1 LiDAR 植物行识别结果

其中一组试验的算法各步处理结果如图7所示，具体步骤如下：

图7 植物点云拟合流程Fig.7 Plant point cloud fitting process

1) LiDAR 扫描模拟植物并经过直通滤波选择ROI 如图7a 所示，此时点云图中有较多的大尺度噪点，使用半径滤波去除噪点得到图7b 所示的点云，完成点云的预处理；

2) 通过均值漂移聚类将每株植物进行分类得到图7c 结果，将每棵模拟植物进行高度自适应分割点云，去除枝叶部分点云，保留主干部分点云，效果如图7d；

3) 将得到的主干部分点云投影到大地平面，分别取平均值找到每棵模拟植物的中心点，如图7e；

4) 利用最小二乘法拟合模拟植物中心直线方程，结果如图7f 所示。

由于采用直尺测量模拟作物间的相对位置作为定位真值，易产生较大的累计或坐标系对齐误差，因此，本试验以RTK-GNSS 定位为真值，以直线方程与导航坐标系正东方向夹角为指标，评价算法精度。针对其中一次试验，分别采用LiDAR 和RTK-GNSS 天线定位，采集模拟植物的定位信息。根据上述试验方法，调整模拟植物的间距以及排列方式，控制农业机器人收集点云数据并提取植物行，如图8a 所示。用GNSS 天线采集每组模拟植物的坐标真值，如表1所示，并同样利用最小二乘法将坐标真值拟合成直线方程，如图8b 所示。分析并统计每次试验中农业机器人识别到的植物行与行真值直线方程间的误差。

表1 模拟植物坐标GNSS 测量值1)Table 1 GNSS measurements of simulated plant coordinates

图8 第3 组试验中模拟植物在全局坐标系下的坐标并通过最小二乘法提取植物行Fig.8 The coordinates of plants in the global coordinate system are simulated and the plant rows are extracted by least square method in the third experiment

2.2 精度验证结果

将导航坐标系和LiDAR 坐标系标定、投影之后，直线方程斜率即为与正东方夹角，根据上述试验的结果，通过计算每组试验对应的植物行与正东方的夹角判断试验的精度，试验共进行3 次，每次对2 行模拟植物进行扫描估计，得到6 条直线方程，结果如表2所示。基于植物行与正东方的夹角，LiDAR 识别植物行夹角平均绝对误差为0.79°，最小绝对误差为0.12°，最大绝对误差为1.49°。误差产生的可能原因包括传感器与GNSS 的测量误差、LiDAR 坐标系与导航坐标系的标定误差、LiDAR 的安装误差、数值计算误差以及最小二乘法拟合产生的误差等，但不影响导航或者小车通过。

表2 模拟试验中基于植物行与正东方的夹角判断LiDAR识别植物行精度Table 2 Aaccuracy evaluation of LiDAR recognition of plant rows based on the angle between the plant rows and due east in simulation experiment

2.3 田间试验

为验证提出算法的有效性，在浙江理工大学的水杉树林(图9，树的直径为200～400 mm)开展田间验证试验，将小车行驶至树林某一位置利用3D LiDAR 扫描树木，记录3 组试验的点云数据，并以小车初始位置为原点，建立导航坐标系，通过卷尺测量水杉树到小车的距离作为植物坐标真值，利用最小二乘法对植株真值坐标进行拟合，与处理LiDAR 点云(图10a)提取的植物行对比，验证田间试验精度。植株行拟合结果如图10b所示。

图9 机器人在水杉树林进行田间试验Fig.9 Robots conducting field experiments in metasequoia forests

图10 点云数据采集与处理结果Fig.10 Point cloud data collection and processing result

试验数据如表3所示，田间试验中采用本文提出的方法得到的植物行角度与真实的植物行角度平均绝对误差为1.48°，最小绝对误差为0.88°，最大绝对误差为2.33°。相比于模拟试验，误差有所提升；误差提升的可能原因包括卷尺测量实际植物中心产生的测量误差、小车重复试验产生的初始偏差、LiDAR 坐标系与导航坐标系的标定误差、LiDAR 的安装误差、数值计算误差以及最小二乘法拟合产生的误差等，但不影响导航或者小车通过。田间试验表明，利用本文提出的方法，农业机器人能实现在未知环境下对植物行信息的有效估计。

表3 田间试验中基于植物行与正东方的夹角判断LiDAR识别植物行精度Table 3 Accuracy evaluation the of LiDAR recognition of plant rows based on the angle between the plant rows and due east in field experiment

3 结论

1) 针对林间或冠层下GNSS 信号遮挡严重的区域，提出一种低成本3D LiDAR 感知点云数据处理与植物行信息提取方法，该方法可利用3D LiDAR 内置IMU 补偿载体运动姿态变化对采集的点云空间坐标的影响。

2) 为避免枝叶影响算法对植株中心的判断，提高算法参数对由于机器人运动导致的LiDAR 视场变化的适应性，设计了一种植物行估计算法。先后采用均值漂移聚类、扫描区域自适应的方法分割每棵植物的主干，垂直投影主干点云估算植物中心点，最后利用最小二乘法提取出植物行信息。

3) 为评价研究提出的基于3D LiDAR 的植物行识别精度，进行了精度验证试验与田间试验，以植物行向量与正东方夹角为指标，比较研究提出的方法识别的植物行信息与卫星天线定位测得的植物行真值间角度误差。结果表明，采用提出的3D LiDAR 点云信息处理与植物行估计方法，模拟试验与田间试验对植物行识别误差平均值分别为0.79°、1.48°，最小值为0.12°、0.88°，最大值为1.49°、2.33°。因此，利用本文提出的方法，农业机器人能实现在未知环境下对植物行信息的有效估计。