基于三维点云的自动对靶调节姿态修剪系统及其修剪方法

本发明涉及农业机械的，尤其是指一种基于三维点云的自动对靶调节姿态修剪系统及其修剪方法。

背景技术：

1、在果树种植及管理环节中，整形修剪环节不可或缺，通过合理剪除果树营养器官的一部分，既可以调节果树营养化物质和激素的分配与运输、积累与消化；又可以训练树形、调整结果区域，从而降低采收、施药等管理环节成本。目前传统修剪主要采用手动修剪机、背负式修剪机、气动修剪机、电动修剪机等进行作业，这类修剪机械对修剪人员技术要求高、修剪效率较低，常以人工背负助力源的方式提高作业效率；近些年，国内研究人员研究了拖拉机悬挂往复式、回转式、圆盘锯式等末端执行修剪刀具进行作业的机械化作业，该方法适用于规范化树木园，对种植规划与作业地形环境要求较高，智能化程度亟待提高，而本发明系统能够根据探测到的树木点云构建出树木的冠幅、株高等参数，计算树木目标修剪部位，从而实现根据树木冠层参数改变修剪姿态，精准修剪。

技术实现思路

1、本发明的目的在于克服现有技术的不足，提出了一种基于三维点云的自动对靶调节姿态修剪系统及其修剪方法，能够根据探测到的树木点云构建出树木的冠幅、株高、行距等参数，计算树木目标修剪部位，从而实现根据树木冠层参数改变修剪姿态，精准修剪的目的，减轻修剪过程中的工作量，提高修剪效率。

2、为实现上述目的，本发明所提供的技术方案为：基于三维点云的自动对靶调节姿态修剪系统，包括：

3、修剪装备，用于对目标树木进行修剪；

4、信息采集装置，用于获取目标树木和周围环境的点云信息以及修剪装备位姿信息，并传输给数据处理单元；

5、数据处理单元，用于对信息采集装置传输过来的信息进行处理，获得目标树木修剪部位的三维坐标和修剪角度以及修剪装备的位置和姿态信息，并传输给控制单元；

6、控制单元，根据数据处理单元传输过来的信息，控制修剪装备进行精准修剪。

7、进一步，所述修剪装备包括履带底盘和安装在该履带底盘顶部底座上的修剪作业机构，其中，所述履带底盘用于承载与移动修剪作业机构，所述修剪作业机构包括修剪机械臂及装于该修剪机械臂末端的刀盘，用于对树木进行切割，所述修剪作业机构通过液压缸或电动推杆驱动。

8、进一步，所述修剪机械臂由两节机械臂连接而成，分别为第一机械臂和第二机械臂。

9、进一步，所述信息采集装置包括点云获取装置和修剪装备位姿获取装置；

10、所述点云获取装置采用深度相机和/或激光雷达，用于获取目标树木和周围环境的点云信息；

11、所述修剪装备位姿获取装置采用拉绳传感器、角度传感器、旋转编码器和imu姿态传感器；所述拉绳传感器用于采集液压缸或电动推杆的伸出距离；所述角度传感器用于采集机械臂和刀盘角度变化；所述旋转编码器用于采集履带底盘行驶距离和行驶速度；所述imu姿态传感器用于测量履带底盘的姿态。

12、进一步，所述数据处理单元包括点云处理单元，所述点云处理单元具体执行以下操作：

13、从点云获取装置获取的信息中提取树木表面的点云数据，对点云数据进行降采样、去噪和平滑处理，将多个视角的点云数据进行配准，得到完整的树木点云数据，对完整的树木点云数据进行分割，得到单个树木的点云数据，从单个树木的点云数据中提取冠幅、株高、行距和体积参数，计算目标修剪部位的中心三维坐标以及修剪角度，具体实现为：在pcl库中利用passthrough算法设定阈值滤除目标远程噪声点云和ransac算法滤除地面点云，使用radiusoutlierremoval算法筛除离群点云，得到树木群信息，然后以目标集合的聚类误差平方和最小为优化目标，结合最小最大距离法，通过k-means聚类将处理后的树木点云数据划分为k个簇，每个簇为一棵树木的三维点云；以激光雷达或深度相机初始点为原点建立三维坐标系，利用passthrough filter滤波算法分别提取树干和树冠部分点云，并投影至垂直xoy平面，采用ransac算法拟合圆，以树冠拟合圆的直径为树冠直径，以树干拟合圆的圆心为树木位置，为使得果树具备更好的光照条件，需将果树的高度和直径控制在行距的预设倍数，行距的计算方法参照如下：分别将相邻两条果树行的果树树干的投影圆中心拟合为直线，以两条直线之间的间距为果树行距，以行距的预设倍数为约束，在果树的三维点云中，将不满足这一约束的树冠层定义为修剪对象，在三维点云中滤除，将滤除后的树冠层点云投影至与修剪装备前进方向垂直的平面，设定线段长度拟合线段，将该线段的中心位置(x，y，z)及倾角θ定为刀盘切割部位及角度，即目标修剪部位的中心三维坐标以及修剪角度。

14、进一步，所述数据处理单元还包括修剪装备位姿构建单元，其包括：

15、底盘位姿计算模块，采用卡尔曼滤波算法融合旋转编码器信息和imu姿态传感器信息，构建底盘行驶里程计；

16、修剪机械臂位姿计算模块，用于融合角度传感器信息和拉绳传感器信息，构建修剪作业机构的位姿信息；

17、进一步，所述控制单元包括：

18、路径规划模块，用于根据目标修剪部位的中心三维坐标以及修剪角度，规划履带底盘和修剪作业机构的移动路径和姿态调节策略；

19、运动控制模块，用于向履带底盘和修剪作业机构发送移动指令和姿态调节指令，并接收反馈信息；

20、修剪控制模块，用于向刀盘发送启停指令，并接收反馈信息。

21、进一步，所述的基于三维点云的自动对靶调节姿态修剪系统，还包括控制柜，所述控制柜上设置有触控屏，用于系统操作，所述数据处理单元和控制单元布置于控制柜中，所述点云获取装置布置于控制柜上方，并位于修剪装备正前方。

22、本发明也提供了上述基于三维点云的自动对靶调节姿态修剪系统的修剪方法，包括单一修剪模式和差异化对靶修剪模式，若选择单一修剪模式，则执行步骤s1；若选择差异化对靶修剪模式，则执行步骤s2-s7；

23、s1、在单一修剪模式下，开启修剪装备位姿获取装置，获取当前修剪装备的三维姿态并显示于触摸屏，通过触摸屏多次拖动选择修剪装备的各个机械臂和刀盘的角度，确定为修剪姿态，设置完成后，将设定姿态的第一机械臂、第二机械臂及刀盘的转动角度(θ1，θ2，θ3)信息反馈至控制单元，调节修剪装备姿态至目标姿态，驱动履带底盘进行修剪，该修剪模式的效果适用于多株统一的整形修剪场景；

24、s2、在差异化对靶修剪模式下，触摸屏上设有差异化对靶修剪按钮，点击此按钮，便能开启差异化对靶修剪；启动修剪装备的履带底盘运动，开启点云获取装置，启动数据处理单元，对目标树木进行扫描，获取目标树木和周围环境的三维点云数据；

25、s3、对三维点云数据进行降采样、去噪和平滑处理，再将多个视角的三维点云数据进行配准，得到完整的树木点云数据，对完整的树木点云数据进行分割，得到单个树木的点云数据，提取树木的冠幅、株高、行距和体积参数，根据农艺要求计算目标修剪部位，并计算所有目标部位的三维坐标以及修剪角度；

26、s4、根据目标修剪部位的三维坐标以及修剪角度，从部位高到低规划履带底盘路径和修剪作业机构的姿态调节策略；

27、s5、获取修剪装备当前的状态，根据目标修剪部位的修剪路径规划，控制履带底盘移动到正确的位置，并调节修剪机械臂的位置姿态，使其与目标修剪部位对准；

28、s6、启动刀盘，履带底盘运动，对目标修剪部位进行整形修剪处理；

29、s7、重复步骤s4-s6，直到完成所有树木的修剪。

30、进一步，在步骤s5中，在底座、各机械臂前端及刀盘前端和中心固结坐标系，通过坐标系之间的变换矩阵，如式(1)所示，实现根据当前第一机械臂、第二机械臂及刀盘的转动角度(θ1，θ2，θ3)，求解出末端刀盘中心坐标系在现实中的位置；

31、

32、式中，i为杆的编号，取值为0至3，杆0为底座，杆1为第一机械臂，杆2为第二机械臂，杆3为刀盘，为杆i坐标系{i}相对于杆i-1坐标系{i-1}的变换矩阵，为杆i-1坐标系的xi-1轴旋转矩阵，αi-1为从杆i-1坐标系的zi-1轴到杆i坐标系的zi轴绕杆i-1坐标系的xi-1轴的旋转角度；为杆i-1坐标系的xi-1轴移动矩阵，ai-1为移动距离，即为连杆i-1的长度；为zi轴旋转矩阵，θi为从杆i-1坐标系的xi-1轴到杆i坐标系的xi轴绕杆i坐标系的zi轴的旋转角度，即为θi为连杆i与连杆i-1之间的夹角；为杆i坐标系的zi轴移动矩阵，di为移动距离，即为连杆i相对于连杆i-1的偏距；

33、将目标切割点的位置通过坐标转换为刀盘坐标系的坐标点，通过逆运动学求解第一机械臂、第二机械臂及刀盘的运动角度，建立逆运动学几何求解模型，逆向运动学解析目的在于给定末端杆件的三维坐标及转动角度，反求出所有杆件的各自转动角度，具体求解过程如下：根据目标修剪部位的三维坐标变换为刀盘中心坐标系在底座坐标系x、z轴的位置同时结合刀盘姿态角度θt，根据公式(2)将坐标转换为刀盘前端坐标系在底座坐标系x、z轴的坐标

34、

35、式中，tx、tz为刀盘前端坐标系与刀盘中心坐标系初始位置x、z方向上的差值；

36、根据第一机械臂坐标系、第二机械臂坐标系、刀盘前端坐标原点所构成的三角形关系，根据式(3)、(4)、(5)解出第一机械臂转动角度θ1以及第二机械臂的转动角度θ2，最后，根据给出的刀盘姿态角度θt与θ1、θ2的差值，能得到刀盘的转动角度θ3；

37、

38、

39、

40、式中，l为第一机械臂坐标系与刀盘前端坐标系的距离；d0为第一机械臂坐标系与底座坐标系的距离；∠1为l与x轴的夹角；∠2为l与第一机械臂夹角；∠3为第二机械臂与第一机械臂夹角；a1为第一机械臂长度；a2为第二机械臂长度。

41、本发明与现有技术相比，具有如下优点与有益效果：

42、本发明能够根据探测到的树木点云构建出树木的冠幅、株高等参数，结合修剪需求，计算树木目标修剪部位，从而实现根据树木冠层参数改变修剪姿态，精准修剪的目的，减轻修剪过程中的工作量，提高修剪效率。本发明不仅能够适用于多株统一的整形修剪场景，还能根据果树的差异化进行对靶精准修剪，具有实际应用价值，值得推广。