(19)国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 202210604675.1 (22)申请日 2022.05.28 (71)申请人 麻航翔 地址 325005 浙江省温州市瓯海区泽 雅镇 大坪头村8组 (72)发明人 麻航翔　 (74)专利代理 机构 北京维正专利代理有限公司 11508 专利代理师 吴英杰 (51)Int.Cl. B25J 9/16(2006.01) B25J 9/00(2006.01) (54)发明名称 一种机械臂路径规划方法、 系统及机 械臂 (57)摘要 本申请涉及一种机械臂路径 规划方法、 系统 及机械臂， 其包括获取预设的栅格化地图中的障 碍点位置信息； 通过图搜索算法对机械臂的末端 执行器规划无碰路径； 检测机械臂在以无碰路径 运动时是否会发生碰撞或具有非可达点； 若有， 则将该节点添加入障碍点位置信息中； 若无， 则 将无碰路径进行轨迹规划以生成机械臂的运动 轨迹； 检测机械臂在以该运动轨迹运动时是否会 发生碰撞或具有非可达点； 若有， 则将该节点添 加入障碍点位置信息中； 其中， 若障碍点位置信 息被更新， 则重新规划无碰路径。 本申请 对JPS算 法进行了三维场景的理论拓展。 另外， 根据JPS算 法的搜索特点， 开发了一套适用于机械臂的完备 的规划方案， 相比较传统的路径规划算法， 规划 效率显著提高。 权利要求书3页 说明书11页 附图5页 CN 114986501 A 2022.09.02 CN 114986501 A 1.一种机 械臂路径规划方法， 其特 征在于， 包括： 获取预设的栅格化 地图中的障碍点 位置信息； 根据所需的起点位置、 终点位置以及所获取的障碍点位置信息， 通过图搜索算法对机 械臂的末端执 行器规划无碰路径； 检测机械臂在以无碰路径运动时是否会发生碰撞或具有非可达点； 若机械臂在以无碰路径运动时会发生碰撞或具有非可达点， 则将该节点添加入障碍点 位置信息中； 若无， 则将所规划的无碰路径进行轨 迹规划以生成机 械臂的运动轨 迹； 检测机械臂在以该运动轨 迹运动时是否会发生碰撞或具有非可达点； 若机械臂在以该运动轨迹运动时会发生碰撞或具有非可达点， 则将该节点添加入障碍 点位置信息中； 其中， 若障碍点 位置信息被更新， 则通过图搜索算法重新 规划无碰路径。 2.根据权利要求1所述的机械臂路径规划方法， 其特征在于， 在图搜索算法规划无碰路 径的过程中包括： 以预设的寻找顺序并依据跳跃节点 寻找规则寻找跳跃节点； 在寻找跳跃节点的过程中通过预设的剪枝规则对邻居节点进行剪枝并标记强制邻居 节点； 根据所获取的跳跃节点以及强制邻居节点 规划无碰路径。 3.根据权利要求2所述的机 械臂路径规划方法， 其特 征在于， 预设的剪枝规则包括： 若父节点p(x)移动 至当前节点x的方向为直线， 则在从父节点p(x)移动 至当前节点x的 路径中， 若len(<p(x), …,n>\x)≤len(<p(x),x,n>)， 则节点 n为需要剪枝的邻居节点； 若父节点p(x)移动至当前节点x 的方向为任一平面内的斜线， 则在从父节点p(x)移动 至当前节点x的路径中， 若len(<p(x), …,n>\x)<len(<p(x),x,n>)， 则节点n为需要剪枝的 邻居节点； 其中， 从父节点p(x)抵达父节点p(x)非一步可达 的格点区域， 若从父节点p(x)抵达父 节点p(x)需跨越平面， 则计算路径长度时必须 经过当前节点x； 若父节点p(x)移动至当前节点x 的方向为任一三维斜线方向， 则在从父节点p(x)移动 至当前节点x的路径中， 若len(<p(x), …,n>\x)<len(<p(x),x,n>)， 则该节点n为需要剪枝 的邻居节点； 其中， 从父节点p(x)抵达父节点p(x)非一步可达的格点区域， 若抵达该格点的路径中 经过xy， yz， zx平面相交的棱， 且这些棱均会穿过父节点p(x)， 则计算路径长度时必须经过 当前节点x； 式中， p(x)为父节点， n为判断的节点， x为当前节点， len(<p(x), …,n>\x)表示从父节 点p(x)开始经过各点到节点n但不经过节点x的路径长度， len(<p(x),x,n>)表示从父节点 开始经过节点x到 达节点n的路径长度。 4.根据权利要求3所述的机械臂路径规划方法， 其特征在于， 标记强制邻居节点的规则 需要满足该节点不是当前节点x 所需剪枝的邻居节点， 且满足len((p(x), …,n>\x)<len(<p (x),x,n>)。 5.根据权利要求2 所述的机械臂路径规划方法， 其特征在于， 当从节点x出发并以 方向权　利　要　求　书 1/3 页 2 CN 114986501 A 2抵达节点y， 且 等式的k值最小时的节点y为跳跃节点， 并至少满足： 节点y为目标节点； 或者， 节点y至少有一个邻居节点 为强制邻居节点； 或者， 在斜线移动方向下， 存在一个节点z， 该节点z能够从为目标节点的节点y或是至少一个 邻居节点 为强制邻居节点的节点y沿同一 直线方向移动若干步抵 达。 6.根据权利要求1所述的机械臂路径规划方法， 其特征在于， 在检测机械臂在以无碰路 径运动时是否会发生碰撞或具有非可达点之前， 所述方法还包括对生成的无碰路径进 行路 径优化； 所述路径优化的方法包括： 获取无碰路径上相邻两个节点之间的自由路径 节点； 将各相邻自由路径 节点之间相互连接以形成扩展无碰路径。 7.根据权利要求6所述的机械臂路径规划方法， 其特征在于， 在形成扩展无碰路径之 后， 所述方法还 包括： 在扩展无碰路径的相邻两个节点之间依次选择三个相邻的自由路径节点进行筛选并 迭代， 三个相邻的自由路径 节点依次定义 为p1， p2和p3； 在筛选过程中， 如果dist(p1,p3)<dist(p1,p2)+dist(p2,p3)， 则剔除p2点， 并设定当前的 p1点为前一次迭代的p3点进行迭代； 如果dist(p1,p3)≥dist(p1,p2)+dist(p2,p3)， 则设定当前的p1点为前一次迭代的p2点 进行迭代； 将剩余的相邻自由路径 节点之间相互连接并更新无碰路径； 其中， dist(pi,pj)表示点pi到pj点之间的距离 。 8.根据权利要求1所述的机械臂路径规划方法， 其特征在于， 在每次障碍点位置信 息被 更新的过程中， 判断无碰路径是否 达到最大规划阈值次数， 若是， 则停止无碰路径的规划。 9.一种机 械臂路径规划系统， 其特 征在于， 包括： 初始化模块， 用于初始化 栅格化地图并获取 预设的栅格化 地图中的障碍点 位置信息； 无碰路径规划模块， 用于根据所需的起点位置、 终点位置以及所获取的障碍点位置信 息， 通过图搜索算法规划无碰路径； 无碰路径检测模块， 用于检测机械臂在以无碰路径运动时是否会发生碰撞或具有非可 达点； 若机械臂在以无碰路径运动时会发生碰撞或具有非可达点， 则将该节点添加入障碍点 位置信息中； 若无， 则通过机械臂轨迹规划模块以将所规划的无碰路径进行轨迹规划以生成机械臂 的运动轨 迹； 机械臂轨迹检测模块， 用于检测机械臂在以该运动轨迹运动时是否会发生碰撞或具有 非可达点； 若机械臂在以该运动轨迹运动时会发生碰撞或具有非可达点， 则将该节点添加入障碍 点位置信息中； 其中， 若障碍点 位置信息被更新， 则通过 无碰路径规划模块重新 规划无碰路径。 10.一种机械臂， 其特征在于， 用于执行通过如权利要求1至8中任一种方法所规划的运权　利　要　求　书 2/3 页 3 CN 114986501 A 3