一种基于三维点云实现荔枝树智能剪枝装置及其方法

本发明涉及荔枝树剪枝，尤其涉及一种基于三维点云实现荔枝树智能剪枝装置及其方法。

背景技术：

1、在荔枝生长的过程中，荔枝的剪枝是一个非常重要的环节。如何在春季与秋季荔枝树发芽期间和摘果后对荔枝树枝进行剪枝，对荔枝树的茁壮生长及其后生长出高质量荔枝具有非常重要的作用。现在大多地区的荔枝树剪枝都是人工剪枝的方式，在大面积荔枝园需要进行树枝修剪时，为了能够在最佳的剪枝期间内完成荔枝树的剪枝工作，短期间内的需要投入大量的人力，易产生剪枝人员紧缺或产生大量人工费用的问题，进而影响剪枝工作与剪枝成本的激增。因此，需要设计一种基于三维点云实现荔枝树智能剪枝的装置和方法。

技术实现思路

1、本发明的目的在于提供一种基于三维点云实现荔枝树智能剪枝装置及其方法，解决现有荔枝树人工剪枝效率低，人工量大的技术问题。

2、为了实现上述目的，本发明采用的技术方案如下：

3、一种基于三维点云实现荔枝树智能剪枝装置，包括机械修枝剪、机械臂、机械臂升降装置、电源装置、设备控制与数据处理中心、云台摄像机、深度相机ⅰ、深度相机ⅱ、深度相机ⅲ、设备安装平台、平台平衡装置、装置运载车和剪后枝叶垃圾箱，枝叶垃圾箱设置在设备安装平台的下方，平台平衡装置设置在装置运载车上，设备安装平台设置在平台平衡装置的顶端，云台摄像机设置在设备安装平台前方的正中心，数据处理与设备控制中心、电源装置和机械臂升降装置均设置在设备安装平台上，机械修枝剪设置在机械臂上，机械臂设置在机械臂升降装置上，云台摄像机、深度相机ⅰ、深度相机ⅱ、深度相机ⅲ均设置在设备安装平台上，云台摄像机、深度相机ⅰ、深度相机ⅱ和深度相机ⅲ均与设备控制与数据处理中心连接，机械修枝剪、机械臂、机械臂升降装置、平台平衡装置均与设备控制与数据处理中心连接。

4、进一步地，剪枝垃圾箱设置在设备安装平台于车轮之间，垃圾箱内设置有红外感应器，用于感应垃圾箱内树枝的存储量，机械修枝剪、机械臂和机械臂升降装置的数量均为两个。

5、进一步地，机械修枝剪的外部设置为机械手结构，用于抓住树枝，在机械手结构内部设置有收缩刀片，机械手结构固定住剪枝点后用内部刀片间断树枝。

6、进一步地，为保持设备安装平台处于一个相对水平的状态，通过设备控制与数据处理中心内设置的世界坐标与云台相机坐标之间的坐标角度差，获取设备安装平台的倾斜角度，并生成相应的控制指令，在设备控制与数据处理中心控制下，实现所述平台平衡装置不同幅度伸缩，保持设备安装平台的相对水平。

7、一种基于三维点云实现荔枝树智能剪枝装置的剪枝方法，荔枝树智能剪枝装置启动，采用智能剪枝装置自动行进方法，通过云台摄像机获取装置前方与左右两侧的图像信息，经过设备控制与数据处理中心进行处理之后，进行路径规划，设备控制中心发出控制指令，控制车前进至距离荔枝树下方固定距离处，然后采用小车板平衡法，进行平台平衡的调整，使设备安装平台处于一个相对水平的状态，有利于之后的点云采集；

8、设备安装平台平衡后，通过对深度相机进行标定，由深度相机ⅰ、深度相机ⅱ和深度相机ⅲ从正面以及对立面两个不同的角度进行点云采集，点云完成之后在设备控制与数据处理中心对离散点和噪声进行剔除，进行三维重建，重建后采用断点修复法对模型进行补全；

9、将两个角度的点云进行配准、语义分割，将一级树枝和母枝划分出来，通过择优树枝筛选法选取所需要的修剪长度的树枝与多余树枝；

10、在设备控制与数据处理中心提取出每根需要修剪树枝的点云，采用剪枝点确定法，对每根树枝进行剪枝点的判断；

11、最后采用从下往上剪枝法，通过设备控制与数据处理中心将所需要修剪的树枝距离机械修枝剪的距离进行从小到大排序，从小的距离开始规划路径，然后设备控制与数据处理中心发出指令，通过控制机械臂与机械修枝剪进行剪枝，修剪后，将抓住的残枝放入残枝储存垃圾箱，完成整个剪枝操作。

12、进一步地，智能剪枝装置自动行进的具体过程为：

13、步骤1.1：云台摄像机获取荔枝树智能剪枝装置前方和左右两侧的rgb图像；

14、步骤1.2：设备控制与数据处理中心分析处理rgb图像信息，识别并确定的荔枝树位置，根据荔枝树位置规划到荔枝树下方的路径，荔枝树智能剪枝装置根据规划的路径进行定量前行，并实时识别检测行进路径上是否存在其他障碍物；

15、步骤1.3：根据rgb图像分析前方的路径信息，将路径规划分为三种情形：情形1，前方路径无障碍物；情形2，前方有障碍物，但可以通过左侧或者右侧绕行通过；情形3，前方有障碍物且对障碍物无法避开通行；

16、步骤1.4：当为情形1时，具体处理步骤包括如下：

17、步骤1.4.1：由设备控制与数据处理中心的数据处理中心发出前进指令到设备控制中心；

18、步骤1.4.2：设备控制中心发出指令，控制小车进行运转，实现智能剪枝装置的定量前进和左右转向动作；

19、步骤1.4.3：智能剪枝装置的前进距离按照定量前进的形式，每前进一次都要通过云台摄像机获取当前位置距离目标荔枝树的距离，然后朝着规划的路径继续前进；

20、步骤1.4.4：来到荔枝树下，发出小车板平衡方法与后续剪枝指令，若树枝不需要修剪，则重新识别新的荔枝树，直至所有荔枝树均无剪枝需求；

21、步骤1.5：当为情形2时，具体处理步骤包括如下：

22、步骤1.5.1：智能剪枝装置规划前方障碍物左侧或者右侧的绕行路径；

23、步骤1.5.2：通过云台摄像头左转或右转，判断绕行路径上是否存在其他障碍物，若无障碍物，则规划绕行路线并进行定量前行；

24、步骤1.6：当为情形3时，具体处理步骤包括如下：

25、通过云台摄像机对两侧分析，若存在障碍物，分析是否会影响绕行路线，若不影响，故规划路径并定量向前行驶，若影响，则放弃目标荔枝树，选择新的荔枝树目标，选择新目标时优先级左侧大于右侧，前方若无荔枝树则旋转云台摄像头从左侧开始识别，若左侧也无荔枝树，再旋转云台摄像头至右侧识别荔枝树；

26、步骤1.7：若左、右两侧存在新荔枝树目标，则重复步骤1.2，若前、左、右都没有发现新荔枝树目标，则停驻小车，并通过数据处理中心向工作人发出求助指令；

27、步骤1.4.4中小车板平衡方法，具体流程为：

28、步骤1.4.4.1：平台平衡装置接收到平衡指令；

29、步骤1.4.4.2：由数据处理中心人为指定世界坐标即水平于地面的坐标xw，yw，zw并且规定三个坐标轴的正方向，相机坐标为xc，yc，zc，由于相机坐标存在于斜坡上，则相机坐标与世界坐标存在角度差，其x,y,z轴角度差设置为ψ,θ和φ，将设备安装平台的长度设置为t，宽度设置为w；

30、步骤1.4.4.3：通过两个三维坐标轴上的向量计算出夹角大小，在相机坐标和世界坐标的x,y,z轴上各取一点，夹角计算模型：其中xw,yw,zw为世界坐标的三维坐标系，xc,yc,zc为相机坐标的三维坐标系；

31、步骤1.4.4.4：通过调整平台平衡装置的伸缩幅度l，伸缩幅度l计算模型为：la＝|tsinα|或者|wsinα|，其中la为伸缩长度，t和w代表设备安装平台的长度和宽度，α代表两个坐标之间的夹角；

32、步骤1.4.4.5：平台平衡装置的伸缩分为以下四种情形：情形1：当相机坐标与世界坐标x、z轴坐标夹角α>0时，表示小车前侧偏高，则平台平衡装置ⅲ与平台平衡装置ⅳ延伸la＝|tsinψ|或|tsinφ|幅度，情形2：当相机坐标与世界坐标x轴坐标夹角α<0时，表示小车后侧偏高，则平台平衡装置ⅰ与平台平衡装置ⅱ延伸la＝|tsin(-φ)|或|tsin(-ψ)|幅度，情形3：当相机坐标与世界坐标y、z轴坐标夹角α>0时，表示小车左侧偏高，则平台平衡装置ⅱ与平台平衡装置ⅳ延伸la＝|wsinθ|或|wsinφ|幅度，情形4：当相机坐标与世界坐标y轴坐标夹角α<0时，表示小车右侧偏高，平台平衡装置ⅰ与平台平衡装置ⅲ延伸la＝|wsin(-θ)|或|wsin(-φ)|幅度；

33、步骤1.4.4.6：经过伸缩平台平衡装置后，再次获取两坐标夹角α，若α＝0，结束平衡调整。

34、进一步地，断点修复法的具体过程为：

35、步骤2.1：首先对相机进行标定，然后通过深度相机获取到深度图像，转换为点云数据，并对所收集的点云进行预处理，去除掉噪声和离散点，对点云三维重建构成一棵三维荔枝树；

36、步骤2.2：通过三维重建得到一棵存在若干处断点的荔枝树，由数据处理中心发出断点修复指令，将所有存在断点的点云做切片操作，获取断点附近的点云截面，并求出该截面上所有点云的中心坐标m，计算中心点的算式为：

37、

38、其中xi,yi,zi为每个点云的三维坐标，n为截面点云总个数；

39、步骤2.3：点云断点可分为以下两种情形：情形1，点云的两侧都存在断点，情形2，点云只有一侧存在断点；

40、步骤2.3.1：当为情形1时，将点云两侧中心坐标设置为[m1，m2]，并存放在一个集合中；

41、步骤2.3.2：当为情形2时，将点云中心坐标设置为c1、c2、c3、c4......cn，n为正整数；

42、步骤2.4：随机选择一个两侧存在断点的点云部分，由两侧的中心点确定一条直线l1＝a1x+b1y+c1z+d1，规定朝z值小的中心点的直线方向为修复方向，并确立z值小的中心点为修复点mf；

43、步骤2.5：以mf为圆心画圆，判断其距离3cm范围内是否存在其他断点m[i]或c[i]；

44、步骤2.6：判断断点分为以下四种情形：情形1，在距离范围内既存在断点m[i]也存在断点c[i]，情形2，在距离范围内不存在断点，情形3，在距离范围内，只存在一个或多个断点m[i]，情形4，在距离范围内，只存在一个或若干个断点c[i]；

45、步骤2.6.1：当为情形1时，将mf与多个断点连接，并通过两点构成多条直线l2、l3......ln，n为正整数，求出若干条直线与直线l1之间的夹角α[i]，夹角公式为：

46、

47、其中a1,b1,c1为直线l1的参数，aibici为直线li的参数。

48、由于树枝是一条比较光滑的曲线，人工拟定夹角大于20°的剔除掉，故剩下的断点直线与l1之间的夹角αs<＝20°，接下来选择夹角最小的断点直线作为连接点，若存在相同的夹角度数，则计算断点m[i]或c[i]到mf的距离s；三维点云距离公式为：

49、

50、其中x1,y1,z1为修复点mf的三维坐标，x2,y2,z2为点m[i]或c[i]的三维坐标。选择出夹角最小或夹角相同距离较短的断点为新的连接点，沿新连接点直线进行点云修复，新连接点存在以下两种情形：情形1，确定的新连接是m[i]，情形2，确定的新连接是c[i]；

51、步骤2.6.1.1：若为情形1时，确定新的连接点为m[i]，连接m[i]集合中的两个断点并确立此直线为新的l1，并判断m[i]集合中z值较小的点作为为新的修复点mf与l1的修复方向，并再次执行步骤2.5；

52、步骤2.6.1.2：若为情形2，确定新的连接为c[i]，则沿着连接点直线进行点云修复，因为新连接点只存在一个断点，说明其另一端已经连接树干，即修复后mf与树干树枝相连接，故代表修复完成；

53、步骤2.6.2：若为情形2，在判断距离内不存在任何断点时，将点云沿修复方向修复3cm，在延长时存在两种情形：情形1，点云延长后没有与任何树干或母枝点云相交，情形2，点云点云延长过程中与树干或母枝点云相交；

54、步骤2.6.2.1：当为情形1时，在延长后的端点截面上，求新的点云中点，并确立为新的修复点mf，修复方向不变，并再次执行步骤2.5；

55、步骤2.6.2.2：当为情形2时：若在点云修复的过程中与母枝或树干的点云相交，则停止修复，即修复完成；

56、步骤2.6.3：当为情形3时，进行步骤2.6.1与步骤2.6.1.1；

57、步骤2.6.4：当为情形4时，进行步骤2.6.1与步骤2.6.1.2；

58、步骤2.7：每当修复完一根树枝，重复步骤2.4的操作，直至所有断点都被修复，即完成所有点云断点的修复工作，数据处理中心执行后续点云配对、语义分割指令。

59、进一步地，择优树枝筛选法的具体过程为：

60、步骤3.1：当通过语义分割，分割出母枝与一级分支，数据处理中心设置参数count，来计算每一根母枝上的一级分支；

61、步骤3.2：一个母枝上面最多存在4个一级分支，且一级分支的大小规定在40-50cm之间，若存在过多分支，会影响荔枝树的发育与后续荔枝的产量；

62、步骤3.3：通过数据处理中心，对三维点云进行切片，提取出母枝中一级分支长度data[i]，i为正整数，并将他们都放入集合rm中，则rm＝{data1、data2、data3、data4......}，首先将data[i]>50cm一级树枝放入修剪集合r中，data[i]<40cm的一级树枝放入修剪集合r2中；

63、步骤3.4：经过第一轮筛选后，计算集合rm中树枝个数，若count<4，则此母枝筛选完毕，若count>4，则需要采用择优筛选法；

64、步骤3.5：经过对长枝和短枝的剔除，剩下的树枝长度位于40-50cm之间，但由于最多保留4根，需要进行修剪，因为一级树枝最优长度为40-50cm，故取平均值为45cm，对rm中data[i]进行data＝|data-45|操作，此操作是选择与平均值差值大的树枝进行修剪，对rm中data[i]进行一次更新，然后对集合rm进行由大到小的排序，将前count-4根树枝提取出来放入集合r2；

65、步骤3.6：对一根母枝进行筛选判断完成后，需要重复步骤3.3、3.4，对荔枝树上所有的一级树枝进行筛选，并放入集合r和集合r2中，筛选完后，数据处理中心发出下一条指令，将集合r与集合r2中的树枝提取出来进行剪枝点选择。

66、进一步地，剪枝点确定法的具体过程为：

67、步骤4.1：通过数据处理中心将集合r和r2中树枝的参数提取出来，将树枝的三维点云通过最小二乘法的方式来拟合曲线，由于树枝是弯曲的，选择弯折点作为进行剪枝的剪枝点，进行自动识别剪枝点；

68、步骤4.2：选出r中的一根树枝，并且提取对该树枝在根部、弯折点、树枝顶端附近的点云坐标，通过深度学习训练出如何识别弯折点、根部、以及顶端点，然后对弯折点做点云切片处理，得到横截面的点云，通过横截面上的点云坐标求出横截面点云中心坐标f：

69、

70、其中xi,yi,zi为每个横截面上点云的三维坐标，n为当前截面的点云个数；

71、步骤4.3：通过数据处理中心对每一个中心点f[i]赋予权重，f0＝(x0+a0，y0+b0，z0+c0)，f1＝(x1+a1，y1+b1，z1+c1)，f[i]＝(x[i]+ai，y[i]+bi，z[i]+ci)......，采用最小二乘法构建出一个距离优化问题，即其中di是当前截面第i个点云到当前截面中心点f的距离，d是当前截面中心点f到当前截面每个点云的平均距离，求解a[i]、b[i]、c[i]，找到离截面上所有点云距离最近的最优点，即f[i]不再是截面的中心点，而是截面的最优点；

72、步骤4.4：通过最小二乘法的方法确定了根部、弯折点、顶端点的最优点f[i]，将每个最优点连接起来，就拟合为树枝原本的线性形状；

73、步骤4.5：得到曲线之后，将剪枝点的目标放在弯折点上，由于已知最优点点云坐标，则可以求出每两个最优点连接的直线长度len[i]，计算len[i]公式为：

74、

75、其中x1,y1,z1与x2,y2,z2，是两个相邻最优点的三维坐标。

76、从根部最优点出发，累加每条直线的长度ls，计算公式为：

77、

78、其中n为第n段树枝；

79、步骤4.6：从筛选好的树枝集合r中提取树枝，进行每段累加，当ls>50时，停止累加，并将第n-1个弯折点确定为修剪点；

80、步骤4.7：对于集合r2中的树枝是因为母枝上的一级树枝大于设定值、与小于40cm的病枝，需要整根剪掉，故直接选取集合r2中所有树枝的根部最优点作为剪枝点，并且将所有确定好剪枝点的树枝放入集合r3中；

81、步骤4.8：确定集合r与集合r2中树枝的剪枝点，复步骤4.2与步骤4.6、4.7，即完成剪枝点的确定，接下来数据处理中心发出对集合r3中的树枝进行机械臂到剪枝点距离计算的指令，进行从下往上的修剪规划。

82、进一步地，下往上剪枝法的具体过程为：

83、步骤5.1：求出剪枝点到机械臂距离后，进行从小到大排序，从最下方的树枝开始进行剪枝，这样能够达到最高效的剪枝，并且可以避免许多不必要的路程规划；

84、步骤5.2：设定机械臂坐标，并给集合r3中每一个树枝定义一个集合q，其中包含一个编号x[i]和距离d[i]，故q[i]＝{x[i]，d[i]}；

85、步骤5.3：世界坐标(xw，yw，zw)与相机坐标(xc，yc，zc)之间的转换关系可表示为：

86、

87、其中r为世界坐标系到相机坐标系的旋转矩阵，其中r的具体表达式为：

88、

89、其中θ、φ为世界坐标到相机坐标x、y、z轴旋转角度。t＝[t1t2 t3]t为世界坐标系到相机坐标系的平移向量；

90、步骤5.4：然后通过相机坐标与图像坐标g(x，y)之间转换：

91、

92、其中f为镜头焦距；

93、步骤5.5：通过图像g(x,y)与像素坐标u(xu,yu)的转换，图像坐标原点在像素映射坐标为g’(u，v)：

94、

95、其中dx与dy为单个像素点在x轴、y轴方向上的对应的物理尺寸；

96、步骤5.6：通过机械臂坐标在世界坐标中的位置通过坐标转换可以计算出机械臂到剪枝点的距离，通过三维坐标计算出距离其中x1,y1,z1为机械臂坐标，xi,yi,zi为剪枝点坐标。并放入集合r4。确定好所有修剪距离d[i]之后，对集合r4进行从小到大快速排序，排序后根据d[i]在集合q[i]中对应的编号x[i]来确定是哪一根树枝，通过数据处理中心确定好树枝剪枝点，向设备控制中心发出剪枝命令，设备控制中心向机械臂升降台、机械臂、机械修枝剪发出移动到合适位置进行修剪的命令来完成从下往上修剪树枝。

97、本发明由于采用了上述技术方案，具有以下有益效果：

98、本发明采用t字履带，实现了在陡坡、不平稳山路的行驶，通过云台摄像机实别路况，对规划小车路径，识别荔枝树，然后采用小车板平衡方法，实现对设备安装平台进行平衡，并利用两个单目深度相机和一个双目深度相机实现精准点云处理，在进行三维建模后运用树枝断点修复法，实现对整棵荔枝树存在遮挡部分点云收集不完全地方的三维的补全，便于后续对树枝长度、树枝数量等进行了准确的判断，对荔枝树进行配准、语义分割后，通过择优树枝筛选法，实现对树枝进行精准的自动筛选。提取树枝数据后，采用剪枝点确认法，实现了智能的树枝修剪点选择，最后利用从下往上修剪法，实现了机械臂从树的底部树枝开始规划剪枝路径，避免了先剪顶部树枝需要绕过太多杂乱枝叶的状况，在修剪树枝时采用机械手与刀片的结合，实现了固定剪枝点进行剪断，修剪后将残枝收入垃圾箱。利用机器视觉技术实现了荔枝树枝的自动点云，利用plc自动控制技术，实现了多机械臂协同工作，可同时进行两根树枝的剪枝，加快了剪枝的速度。