(19)国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 202210706510.5 (22)申请日 2022.06.21 (71)申请人 实时侠智能控制技 术有限公司 地址 201702 上海市青浦区徐民路3 08弄50 号中建锦绣广场6号楼8层 (72)发明人 王乾　钱进　李睿钦　王启超　 (74)专利代理 机构 上海专利商标事务所有限公 司 31100 专利代理师 骆希聪 (51)Int.Cl. B25J 9/16(2006.01) (54)发明名称 末端工具有效工作空间的规划方法、 装置及 存储介质 (57)摘要 本申请涉及一种末端工具有效工作空间的 规划方法、 装置及存储介质， 该方法包括： 设置基 坐标系中的第一坐标轴的极限值为有效工作空 间的设计变量， 其中极限值包括 极限最大值和极 限最小值； 基于约束条件确定有效工作空间的非 线性等式， 以及根据非线性等式确定设计变量的 目标函数， 其中约束条件包括末端工具的极限朝 向的空间向量集合； 在基坐标系中的第二坐标轴 和第三坐标轴组成的平面上等间隔取点， 通过优 化目标函数计算出平面上每个点对应的末端工 具所能达到的第一坐标轴上的最大值和最小值； 根据平面上所有的点对应的最大值和最小值确 定最大有效工作空间。 本发明实现了末端工具与 机械臂末端不共轴时规划最大的有效工作空间。 权利要求书2页 说明书10页 附图3页 CN 114872053 A 2022.08.09 CN 114872053 A 1.一种末端工具有效工作空间的规划方法， 其特 征在于， 包括： 设置基坐标系中的第 一坐标轴的极限值为有 效工作空间的设计变量， 其中所述极限值 包括极限最大值和极限最小值； 基于约束条件确定所述有 效工作空间的非线性等式， 以及根据 所述非线性等式确定所 述设计变量的目标函数， 其中所述约束条件 包括末端工具的极限朝向的空间向量 集合； 在基坐标系中的第 二坐标轴和第 三坐标轴组成的平面上等间隔取点， 通过优化所述目 标函数计算出所述平面上每个点对应的所述末端工具所能达到的第一坐标轴上的最大值 和最小值； 根据所述平面上 所有的点对应的所述 最大值和所述 最小值确定最大有效工作空间。 2.如权利要求1所述的规划方法， 其特征在于， 所述约束条件还包括机械臂的结构参数 和末端工具参数。 3.如权利要求1所述的规划方法， 其特 征在于， 所述非线性 等式为： 其中， Ttool为基坐标系到工 具坐标系的坐标转换矩阵， F( ·)是机械臂运动学正解函数， q是机械臂轴角向量， tool是末端工具参数， [z1， z2， z3]为所述末端工具的末端朝向在基坐 标系下的投影， [xq， yq， zq]为根据机械臂轴角向量确定的基坐标系下的坐标。 4.如权利要求3所述的规划方法， 其特 征在于， 所述目标函数为： 其中， min(x)为第一坐标轴的最小值， max(x)为第一坐标轴的最大值， f( ·)是第一坐 标轴的值与 机械臂轴角向量的关系函数， qa是对应第一坐标轴的最小值的最优机械臂轴角 向量， qb是对应第一 坐标轴的最大值的最优机 械臂轴角向量。 5.如权利要求4所述的规划方法， 其特征在于， 通过优化所述目标函数计算出所述平面 上每个点对应的所述末端工具 所能达到的第一 坐标轴上的最大值和最小值的步骤 包括： 以下列等式约束与不等式约束为所述目标函数的KKT条件， 通过顺序二次规划优化所 述目标函数 得到每个点对应的一组最优机 械臂轴角向量； qmin＜q＜qmax 其中， [z1， z2， z3]为所述末端工具的末端朝向在基坐标系下的投影， [zn1， zn2， zn3]为第n 组限制的末端朝向在基坐标系下的投影， yq和zq为根据所述机械臂轴角向量确定的第二坐 标轴上的坐标和第三坐标轴上的坐标， y和z为所述平面上的点在第二坐标轴上的坐标和在 第三坐标轴上的坐标， qmin为机械臂关节的最小可达角度所构成的向量， qmax为机械臂关节权　利　要　求　书 1/2 页 2 CN 114872053 A 2的最大可达角度所构成的向量， q是机 械臂轴角向量； 以所述一组最优机械臂轴角向量对应的的一组第一坐标轴的值的其中一个为所述最 大值， 另一个为所述 最小值。 6.如权利要求1所述的规划方法， 其特征在于， 在基坐标系中的第 二坐标轴和第 三坐标 轴组成的平面上等间隔取点的步骤 包括： 在所述第二 坐标轴上从原点 开始以第一 步长递增 确定各个点的第二 坐标； 在所述第三坐标轴上从原点开始以第 二步长递增确定各个点的第 三坐标， 直到由所述 第二坐标和所述第三坐标确定的点对应的第一坐标轴的最大值和 最小值的差值小于第一 阈值， 则在所述第三 坐标轴上从原点 开始以第二 步长递减继续确定各个点的第三 坐标。 7.如权利要求1所述的规划方法， 其特征在于， 还包括： 根据工况需求确定所述末端工 具在所述第二坐标轴所需要的第一工作范围和所述末端工具在所述第三坐标轴所需要的 第二工作范围， 根据所述第一工作范围和所述第二工作范围在基坐标系中的第二坐标轴和 第三坐标轴组成的平面上等间隔取点。 8.如权利要求7所述的规划方法， 其特征在于， 根据 所述第一工作范围和所述第 二工作 范围在基坐标系中的第二 坐标轴和第三 坐标轴组成的平面上等间隔取点的步骤 包括： 在所述第二坐标轴上从原点开始到二分之一的所述第一工作范围内以第一步长递增 确定各个点的第二 坐标， 其中所述有效工作空间在第二 坐标轴上关于原点对称； 在所述第三坐标轴上从原点开始以所述第二工作范围为步长递增确定各个点的第三 坐标， 直到由所述第二坐标和所述第三坐标确定的点对应的第一坐标轴的最大值和最小值 的差值小于第一阈值， 则 在所述第三坐标轴上从原 点开始以所述第二工作范围为步长递减 继续确定各个点的第三 坐标。 9.如权利要求1所述的规划方法， 其特征在于， 根据 所述平面上所有的点对应的所述最 大值和所述 最小值确定最大有效工作空间的步骤 包括： 从所述平面上所有的点对应的所述最大值中取最小者作为所述极限最大值， 从所述平 面上所有的点对应的所述 最小值中取最大者作为所述极限最小值； 根据所述极限最大值和所述极限最小值确定最大有效工作空间。 10.如权利要求1所述的规划方法， 其特征在于， 根据所述极限最大值和所述极限最小 值得到最大有效工作空间的步骤 包括： 生成所述第 二坐标轴和所述第 三坐标轴组成的平面对应的极限最大值曲面， 生成所述 第二坐标轴和所述第三坐标轴组成的平面对应的极限最小值曲面， 得到最大有效工作空 间。 11.如权利要求2所述的规划方法， 其特征在于， 还包括： 判断所述最大有 效工作空间是 否大于需求工作空间， 如果否， 则调整所述机械臂结构参数并重新获取所述最大有效工作 空间。 12.一种末端工具有效工作空间的规划装置， 包括： 存储器， 用于存 储可由处 理器执行的指令； 处理器， 用于执 行所述指令以实现如权利要求1 ‑11任一项所述的方法。 13.一种存储有计算机程序代码 的计算机可读介质， 所述计算机程序代码在由处理器 执行时实现如权利要求1 ‑11任一项所述的方法。权　利　要　求　书 2/2 页 3 CN 114872053 A 3