[发明专利]一种类球形果实的采摘顺序动态规划方法

权利要求书

1.一种类球形果实的采摘顺序动态规划方法，其特征在于，所述采摘顺序规划方法应用于采摘类球形果实的采摘机器人；具体包括步骤如下：

第一步，在第一感知点进行感知；

采摘机器人到达采摘位置后，此时采摘机械臂处于初始位置，双目相机在第一感知点即初始位置进行环境感知，双目相机将采集到的环境RGBD图像通过ROS进行发布，利用深度学习yolov3算法对双目相机采集到的RGBD图像进行订阅并处理；使用yolov3算法对环境RGBD图像进行识别，将识别到的果实i的中心点三维位置坐标(x_i,y_i,z_i)通过ROS进行发布；将所有识别到的果实位置三维坐标组成一个果实位置的集合V_a{a,b,c,…,n}，将采摘机械臂需要进行感知的所有感知点坐标组成一个感知点集合V_s{a_s，b_s，c_s…，n_s}，则机械臂末端需要移动到的所有位置的三维坐标集合V＝V_a+V_s；

第二步，检测果实重叠情况；

提取果实位置坐标的x-y平面位置信息，所述x-y平面即垂直于双目相机法线，双目相机拍摄的RGB图像对应的平面；提取后的果实i中心点平面坐标为(x_i,y_i)，所有检测到的果实位置组成果实位置的二维位置集合V_b{a_b,b_b,c_b,…,n_b}；检测集合V_b中所有果实在x-y平面的重叠情况，即依次判断V_b中所有果实两两之间的距离l_ij，判断依据如式(3)所示：

式中(x_i，y_i)和(x_j,y_j)分别为i果实的中心点和j果实的中心点在x-y平面的坐标；r为较大的成熟果实半径；

根据式(3)判断两个果实在x-y平面的重叠情况，当两个果实间的距离l_ij≤2.5r，则判定两个果实发生重叠；根据式(3)，对集合V_b中所有的果实位置进行两两计算，判断所有果实之间的重叠关系，并记录所有互为遮挡的果实组组成集合V_c；

第三步，果实间权重的计算及动态更新；

令权重c_ij等于i点到j点的距离，如式(4)所示：

其中i，j为集合V中的点，根据式(4)计算集合V中所有点两两之间的权重；

权重的计算：根据式(4)计算第一步中检测到的所有果实位置和感知点集合V中所有点两两之间的权重；

权重的动态更新：对在第二步中判定为重叠的果实集合V_c中所有互为遮挡的果实进行权重更新；首先比较互为重叠的果实i和j的深度信息，如果z_i-z_j0，则i果实在j果实之前，令深度信息更大的果实即被遮挡果实j与集合V中其他位置点之间的权重为无穷大，与遮挡j果实的果实i之间的权重为0，如式(5 )所示：

当遮挡果实i被采摘，则判定先前被遮挡果实j不再被遮挡，即式(3)不再成立，则更新j果实的权重，使j果实的权重值再次以式(4)计算得到；

除了遮挡果实的权重更新外，机械臂在新的感知位置感知到了新的果实后，也需要对集合V中所有点进行权重更新；

第四步，采用动态收获感知方法对采摘机器人采摘路径进行规划；令位置集合V_m为除了点m之外的所有采摘点和感知点的集合，即V_m＝V-m；设采摘机械臂末端从初始位置s出发，令d(m,V_m)表示机械臂末端从采摘点m出发经过位置集合V_m中各个采摘点仅且一次，最后回到初始位置s的最短路径长度，则：

1)当移动位置集合V_m为空集时，那么d(m,V_m)表示采摘机械臂只有一个需要运动到达的位置点，机械臂采摘完成后直接从m回到了初始位置s，此时d(m,V_m)＝c_ms(m≠s)；c_ms为m点和s点的之间权重；

2)当移动位置集合V_m不为空，在V_m这个位置点集合中，尝试对每一个位置点进行求解，枚举出所有的子路径，并得出最优解，依次求得子路径、子路径的子路径的最优解，直到集合V_m为空集，则问题转化为1)，如式(6)所示：

d(m,V) =min(c_mk+d(k,V_m-(k))) （ 6 ）

其中d(k,V_m-(k))为d(m,V_m)的子问题；c_mk为m点和k点之间的权重；

通过1)和2)的不断求解，对所有点依次迭代，求出所有路径的权重之和，则权重之和最小的路径即为所求的最短路径；最终求得采摘机械臂末端走完集合V中所有点的最短路径；

第五步，采摘机器人按照第四步 所求得的最短采摘路径进行采摘或感知，在每一次采摘或感知之后进行判断，如果存在剩余的果实或感知点，则采摘机器人再次进行第二步，判断果实重叠情况有无变化或有无新增的果实，如果有，则继续依次执行第三步-第五步；如果判断没有新增果实且果实重叠情况没有发生变化，则继续执行第五步 ，直到判断此区域没有剩余的果实或感知点，则采摘机器人进入下一采摘地点进行采摘。