(19)国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 202211054710.3 (22)申请日 2022.08.31 (71)申请人 江苏大学 地址 212013 江苏省镇江市京口区学府路 301号 (72)发明人 夏长高　倪万磊　许秋月　黎晓帆　 盛志鹏　马鑫杰　金宝龙　 (74)专利代理 机构 南京智造力知识产权代理有 限公司 32382 专利代理师 屠志炜 (51)Int.Cl. B25J 9/16(2006.01) B25J 11/00(2006.01) (54)发明名称 一种果园采摘机械手的路径规划方法及其 控制系统 (57)摘要 本发明提供了一种果园采摘机械手的路径 规划方法及其控制系统， 包括如下步骤： 根据机 械手初始点、 机械手终点和扩展 步长利用RRT*算 法得出以初始点为起点、 机械手终点为终点的扩 展树T； 确定目标偏移概率λ； 确定随机数； 获取 采样点； 求得新节点； 对节点和新节点之间得路 径进行碰撞检测： 以新节点为中心， 建立 半径为R 的球形搜索空间， 球形搜索空间内的节点设为近 邻节点； 计算新节点经过近邻节 点到初始点的全 部路径的欧式距离； 重新选择父节点： 将新节点 添加到扩展 树T中； 更新扩展 树； 若目标点和新节 点之间的距离小于扩展步长， 则输出路径P。 本发 明能够短时间内寻找到一条无碰撞路径， 且规划 路径效率高， 有利于采摘机 械手的实时控制。 权利要求书2页 说明书5页 附图2页 CN 115302516 A 2022.11.08 CN 115302516 A 1.一种果园采摘机 械手的路径规划方法， 其特 征在于， 包括如下步骤： S01： 确定机械手初始点xstart、 机械手终点和扩展步长Δl； 根据机械手初始点xstart、 机 械手终点和扩展步长Δl利用RRT*算法得出以初始点xstart为起点、 机械手终点为终点的扩 展树T； 确定目标偏移概 率 λ； 设机 械手终点 为目标点xgoal； S02： 使用rand函数随机生成0 ‑1之间的随机数； S03： 获取采样点Xrand， 具体为： 如果随机数小于 λ， 则将目标点xgoal设置为采样点Xrand； 如果随机 数大于 λ， 则在采样空间中随机采样得到一个随机采样点x ′rand； 所述采样空间 是包含起点、 终点、 扩展树T内各个节点以及各种障碍物组成的一个空间； 目标点xgoal对节 点xnear产生吸引力， 其中节点xnear是扩展树T上距离随机采样点x ′rand最近的点； 根据目标点xgoal对节点xnear的吸引力大小， 得到临时路径点xtp； 根据随机采样点x ′rand和临时路径点xtp确定采样点Xrand； S04： 求得新节点xnew； 以节点xnear为起点， Δl为扩展 步长生成新节点xnew， 新节点xnew为等待加入扩展树T的一 个节点， S05： 对节点xnear和新节点xnew之间得路径进行碰撞检测： 若节点xnear和新节点xnew发生碰撞， 则重复步骤S02至步骤S04， 直至无碰撞； S06： 确定节点xnear和新节点xnew之间无碰撞后， 以新节点xnew为中心， 建立半径为R的球 形搜索空间， 球形搜索空间内的节点设为近邻节点； S07： 计算 新节点xnew经过近邻节点到初始点xstart的全部路径的欧式距离； S08： 重新选择 父节点： 确定在新节点xnew经过近邻节点到初始点xstart中欧式距离最小且无碰撞的近邻节点 x1； 将近邻节点x1设置为新节点xnew的父节点； 所述父节点为在扩展树T上可向下延伸的一 个节点； S09： 将新节点xnew添加到扩展树T中； S10： 如果球形搜索空间内其余近邻节点的父节点改为新节点xnew后， 可使新节点xnew经 过近邻节点到初始点xstart的欧式距离减小， 则将该近邻节点的父节点更改为 新节点xnew； S11： 更新扩展树； S12： 若目标点xgoal和新节点xnew之间的距离大于扩展步长Δl， 则回到S02继续开始； 若 目标点xgoal和新节点xnew之间的距离小于扩展步长 Δl， 则输出路径P。 2.根据权利要求1所述的果园采摘 机械手的路径规划方法， 其特征在于， 根据随机采样 点xr′and和临时路径点xtp确定采样点Xrand， 具体为： 权　利　要　求　书 1/2 页 2 CN 115302516 A 23.根据权利要求1所述的果园采摘机械手的路径规划方法， 其特征在于， 根据目标点 xgoal对节点xnear的吸引力大小得到临时路径点xtp， 具体如下： 其中katt为引力增益。 4.根据权利要求1所述的果园采摘 机械手的路径规划方法， 其特征在于， 搜索空间的半 径R根据下面公式计算： 式中： R为搜索空间的半径； γ是基于环境的规划常数； n表示近邻节点的数量； d表示 规划空间维数。 5.一种果园采摘 机械手的路径规划的控制系统， 其特征在于， 包括期望轨迹模块、 控制 模块和机械手； 所述期望轨迹模块内集成权利要求1 ‑4任一项所述的果园采摘机械手的路 径规划方法的程序， 所述期 望轨迹模块将期 望的路径P 输入控制模块， 所述控制模块将期 望 的路径P转换成机械手需要转动的角度和移动的位移， 根据设定移动速度， 确定机械手上驱 动单元的响应时间； 所述控制模块控制机 械手根据期望的路径P移动。权　利　要　求　书 2/2 页 3 CN 115302516 A 3