(19)国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 202210799210.6 (22)申请日 2022.07.08 (71)申请人 浙江大学 地址 315400 浙江省宁波市余 姚市凤山 街 道冶山路479号科创大厦12 楼 (72)发明人 朱世强　邵珺　宋伟　廖建峰　 (74)专利代理 机构 杭州浙科专利事务所(普通 合伙) 33213 专利代理师 陈洁 (51)Int.Cl. B25J 9/16(2006.01) (54)发明名称 一种结合抓取网络和抓取位姿选择与调整 的机械臂轨迹规划方法 (57)摘要 本发明属于机器人运动规划领域， 公开了一 种结合抓取网络和抓取位姿选择与调整的机械 臂轨迹规划方法， 包括以下步骤： 步骤1： 定义机 械臂轨迹规划的优化问题， 并给出求解方法； 步 骤2： 设计抓取位姿选 择策略； 步骤3： 针对抓取位 姿调整设计无约束的多目标优化问题， 并给出求 解方法； 步骤4： 根据上述步骤提供的方法， 设计 结合抓取位姿选择与调整的机械臂轨迹规划方 法。 本发明以抓取网络的方法生 成对物体的抓取 姿态集后， 能够借助抓取姿态集， 根据场景适时 优化轨迹、 改变并调整抓取位姿， 实现了轨迹平 滑和轨迹 无碰撞的性能均衡， 具有领先的抓取规 划成功率， 并且抓取位姿调整的近似求解思路和 梯度保存的策略极大降低了计算成本， 方法实时 性强。 权利要求书4页 说明书11页 附图1页 CN 115302502 A 2022.11.08 CN 115302502 A 1.一种结合抓取网络和抓取位姿选择与调整的机械臂轨迹规划方法， 其特征在于， 包 括以下步骤： 步骤1： 定义机 械臂轨迹规划的优化问题， 并给 出求解方法； 步骤2： 设计抓取位姿选择 策略； 步骤3： 针对抓取位姿调整设计无约束的多目标优化问题， 并给 出求解方法； 步骤4： 根据上述步骤提供的方法， 设计结合抓取位姿选择与调整的机械臂轨迹规划方 法。 2.根据权利要求1所述的结合抓取网络和抓取位姿选择与调整的机械臂轨迹规划方 法， 其特征在于， 所述 步骤1包括如下 具体步骤： 将优化问题定义 为： s.t.h( ξ,g)＝0 其中， 轨迹ξ定义为 是从时间[0,1]到关节位姿的映射， ξ*是优化问题的局 部最优解， h( ξ,g)为 等式约束函数， ftraj( ξ )为轨迹规划目标函数， 定义 为： ftraj( ξ )＝fobs( ξ )+λ fsmooth( ξ ) 其中， λ为平 滑目标函数权 重， fsmooth( ξ )为平滑目标函数， 定义 为轨迹的速度平方和： fobs( ξ )为避障函数， 定义 为机械臂上所有点的碰撞 代价沿轨 迹之和： 其中， 是机器人外部 点的集合， 代表t时刻机械臂位姿 ξ(t)下， 机械 臂上某一点 到任务空间的正运动学映射； c(x( ξ(t),u))代表该点的碰撞代价函数， 用 于惩罚机 械臂上靠 近或进入障碍物的点； 等式约束用于使 整个轨迹投影到该约束上， 具体为： h( ξ,g)＝ξ(1) ‑g＝0 其中， ξ(1)为轨 迹 ξ 中最后一个位姿， g为当前从抓取点 集G中选择的目标位姿； 该轨迹规划的优化问题求解方法为， 在第i次迭代时， 轨迹做如下更新， 直至找到可行 解： 其中， ξi与 ξi+1分别为当前轨迹与更新后轨迹， 为轨迹规划目标函数在 ξi处的梯权　利　要　求　书 1/4 页 2 CN 115302502 A 2度， 为轨迹规划的更新步长， A为轨迹的加速度差分矩阵， 将每次更新量均匀叠加在原轨 迹上， 为等式约束在ξi处关于ξ的偏导， b＝h( ξi,g)为ξi处的等式约束函数 值。 3.根据权利要求1所述的结合抓取网络和抓取位姿选择与调整的机械臂轨迹规划方 法， 其特征在于， 所述 步骤2具体分为两个阶段： 在第一阶段， 在第i次迭代时的流 程为： Step1做轨 迹规划的无约束更新： ξi'+1为轨迹规划无约束更新后的轨 迹； Step2选出前k个距当前轨迹末端 位姿最近的抓取位姿gj∈G(j＝1,...,k)， 计算这些位 姿的代价 函数值c(gj)： c(gj)＝fobs(gj)+λ||gj‑ξ(1)|| 其中， fobs(gj)为在gj处的避障函数值； Step3选择代价 最小的抓取位姿作为 新的轨迹末端位姿； 在第二阶段， 在第i次迭代时的流程中， Step1和Step3与第一阶段完全相同， Step2变 为： 选出前k个距当前轨迹末端位姿最近的位姿 gj∈G(j＝1,...,k)， 计算这些位姿的第二 阶段代价 函数值c'(gj)： 是从轨迹的第i个点到gi的插值轨 迹， 是 的轨迹规划目标函数值。 4.根据权利要求1所述的结合抓取网络和抓取位姿选择与调整的机械臂轨迹规划方 法， 其特征在于， 所述 步骤3包括如下 具体步骤： 抓取位姿调整的优化问题定义如下： 其中， g*为求解出局部最优的抓取位姿， f( ξ )为 抓取位姿调整的优化问题目标函数， 定义为f( ξ )＝fobs( ξ )+λ fsmooth( ξ )+ μfgoal( ξ )， μ为抓取位姿调整目标函数权 重， fgoal( ξ )为抓取位姿调整目标函数， 定义如下： fgoal( ξ )＝||W(FK( ξ(1))‑FK(g))||2‑logg( ξ ) 其中， FK(·)是机械臂位姿到末端执行器在任务空间下坐标的正运动学映射， W＝di ag (w1,...,w6)是权重矩阵， 可以自由旋转的自由度设为0， g( ξ )定义 为： φ为限制最大旋转角度， 可以自由旋转的自由度设为1， 其余为 0；权　利　要　求　书 2/4 页 3 CN 115302502 A 3