(19)国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 202211162608.5 (22)申请日 2022.09.23 (71)申请人 清华大学 地址 100084 北京市海淀区清华园1号 (72)发明人 邵珠峰　刘汉擎　段金昊　霍晔　 姚铭　 (74)专利代理 机构 北京清亦华知识产权代理事 务所(普通 合伙) 11201 专利代理师 顾鲜红 (51)Int.Cl. G06F 30/20(2020.01) G06Q 10/04(2012.01) G06F 111/04(2020.01) G06F 119/14(2020.01) (54)发明名称 用于平行绳索约束的索并联机器人动态轨 迹规划方法 (57)摘要 本申请公开了一种用于平行绳索约束的索 并联机器人动态轨迹规划方法， 方法包括： 根据 预设的平行绳索约束的索并联机器人构型， 建立 索并联机器人的运动学与动力学模 型； 基于同方 向索力可合并的原则， 对索并联机器人的运动学 与动力学模 型简化； 基于平行索的力等效模型和 扭矩分析， 获取用于评价动平台抗扭转能力的性 能评价指标； 针对码垛任务要求的门型动态轨 迹， 基于轨迹模型建立轨迹方程， 并以轨迹功耗 与扭转性能综合指标为优化目标， 根据约束条件 优化求解得到门型动态轨迹， 实现对轨迹门型特 征的控制。 由此， 解决相关技术中难以丰富索并 联机器人的构型和自由度形式， 具有一定局限 性， 降低绳索的约束能力， 无法满足索并联机器 人的使用需求。 权利要求书3页 说明书13页 附图3页 CN 115455716 A 2022.12.09 CN 115455716 A 1.一种用于平行绳索约束的索并联机器人动态轨迹规划方法， 其特征在于， 包括以下 步骤： 根据预设的平行绳索约束的索并联机器人构型， 建立索并联机器人的运动学与动力学 模型； 基于同方向索力可合并的原则， 对所述索并联机器人的运动学与动力学模型进行简 化， 得到所述索并联机器人的最终运动学与动力学模型； 基于平行索的力等效模型和扭矩分析， 获取用于评价动平台抗扭转能力的性 能评价指 标； 以及 基于所述最终运动学与动力学模型， 针对码垛任务要求的门型动态轨迹， 基于预设的 轨迹模型建立轨迹方程， 并以由所述性能评价指标得到的轨迹功耗与扭转性能综合指标为 优化目标， 根据预设约束条件优化求解所述索 并联机器人 的门型动态轨迹， 实现对轨迹门 型特征的控制。 2.根据权利要求1所述的方法， 其特 征在于， 所述 最终运动学与动力学模型为： Mk+Mq+M0＝0， 其中， F1与F2为平行索合力或单索索力， e1与e2为绳索方向向量， m为动平台及负载质量， g为重力加速度， 为动平台加速度， Mk为平行索的抗扭转力矩， Mq为机构的自倾力矩。 3.根据权利要求1所述的方法， 其特征在于， 所述用于评价动平台抗扭转 能力的性能评 价指标包括 抗扭转能力指标与抗扭转均衡指标， 其中， 所述抗扭转能力指标TRAI定义 为： TRAI＝||Mkm||/||Mg||， 其中， Mg为负载参 考力矩。 所述抗扭转均衡指标TRBI定义 为： TRBI＝||Mq||/||Mkm||， 其中， Mq为机构的自倾力矩。 4.根据权利要求3所述的方法， 其特征在于， 所述轨迹功耗与扭转性能综合指标TPTI定 义为： 其中， W为整个动态轨 迹的功耗。 5.根据权利要求1所述的方法， 其特征在于， 所述基于预设的轨迹模型建立轨迹方程， 并以由所述性能评价指标得到的轨迹功 耗与扭转性能综合指标为优化目标， 根据预设约束 条件优化求解所述索并联机器人的门型动态轨 迹， 包括： 在所述轨迹模型为傅里叶级数模型时， 根据所述预设约束条件选取傅里叶级数阶次， 对所述轨迹模型求导， 得到轨迹各点处的第一速度与第一加速度， 对轨迹端点处的速度约 束条件分析以简化模型， 根据端点处的位置和加速度约束求解傅里叶级数 的系数参数， 以 TPTI最小为优化目标对动态轨 迹的频率进行优化 求解。 6.根据权利要求1所述的方法， 其特征在于， 所述基于预设的轨迹模型建立轨迹方程，权　利　要　求　书 1/3 页 2 CN 115455716 A 2并以由所述性能评价指标得到的轨迹功 耗与扭转性能综合指标为优化目标， 根据预设约束 条件优化求解所述索并联机器人的门型动态轨 迹， 包括： 在所述轨迹模型为多项式函数模型时， 根据所述预设约束条件选取多项式函数阶次， 对所述轨迹模型求导， 得到轨迹各点处的第二速度与第二加速度， 对轨迹端点处的速度约 束条件分析以简化模型， 以TPTI最小为优化目标对每段动态轨迹的时间和终止点处的水平 加速度进行优化 求解， 根据端点处的位置和 加速度约束求 解多项式系数参数。 7.一种用于平行绳索约束的索并联机器人动态轨 迹规划装置， 其特 征在于， 包括： 建立模块， 用于根据预设的平行绳索约束的索并联机器人构型， 建立索并联机器人的 运动学与动力学模型； 处理模块， 用于基于同方向索力可合并的原则， 对所述索并联机器人的运动学与动力 学模型进行简化， 得到所述索并联机器人的最终运动学与动力学模型； 获取模块， 用于基于平行索的力等效模型和扭矩分析， 获取用于评价动平台抗扭转能 力的性能评价指标； 以及 控制模块， 用于基于所述最终运动学与动力学模型， 针对码垛任务要求的门型动态轨 迹， 基于预设的轨迹模型建立轨迹方程， 并以由所述性能评价指标得到的轨迹功耗与扭转 性能综合指标为优化目标， 根据预设约束条件优化求解所述索并联机器人的门型动态轨 迹， 实现对轨 迹门型特征的控制。 8.根据权利要求7 所述的装置， 其特 征在于， 所述 最终运动学与动力学模型为： Mk+Mq+M0＝0， 其中， F1与F2为平行索合力或单索索力， e1与e2为绳索方向向量， m为动平台及负载质量， g为重力加速度， 为动平台加速度， Mk为平行索的抗扭转力矩， Mq为机构的自倾力矩。 9.根据权利要求7所述的装置， 其特征在于， 所述用于评价动平台抗扭转 能力的性能评 价指标包括 抗扭转能力指标与抗扭转均衡指标， 其中， 所述抗扭转能力指标TRAI定义 为： TRAI＝||Mkm||/||Mg||， 其中， Mg为负载参 考力矩。 所述抗扭转均衡指标TRBI定义 为： TRBI＝||Mq||/||Mkm||， 其中， Mq为机构的自倾力矩。 10.根据权利要求9所述的装置， 其特征在于， 所述轨迹功耗与扭转性能综合指标TPTI 定义为： 其中， W为整个动态轨 迹的功耗。 11.根据权利要求7所述的装置， 其特征在于， 所述控制模块进一步用于在所述轨迹模 型为傅里叶级数模型时， 根据所述预设约束条件选取傅里叶级数阶次， 对所述轨迹模型求 导， 得到轨迹各点处的第一速度与第一加速度， 对轨迹端点处的速度约束条件分析以简化权　利　要　求　书 2/3 页 3 CN 115455716 A 3