(19)国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 202210864926.X (22)申请日 2022.07.21 (71)申请人 北京工业大 学 地址 100124 北京市朝阳区平乐园10 0号 (72)发明人 许静静　裴艳虎　刘志峰　刘天天　 (74)专利代理 机构 北京思海天达知识产权代理 有限公司 1 1203 专利代理师 王兆波 (51)Int.Cl. B25J 9/16(2006.01) (54)发明名称 一种基于性能约束项阶段降维的工业机器 人运动规划优化方法 (57)摘要 本发明公开了一种基于性能约束项阶段降 维的工业机器人运动规划优化方法， 该方法首先 针对机器人与目标结构间最短距离， 分别采用基 于简单几何体描述和切片描述方法建立了距离 粗估计和精确估计模型， 形成了安全性约束项函 数； 结合机器人运动学、 各关节输出力矩和各关 节扭转刚度建立了机器人末端柔性误差模型， 形 成了精度性能约束项函数； 进行目标连续轨迹作 业任务分解和各阶段轨迹插值， 进行分阶段轨迹 运动性能需求分析进行各阶段约束项降维， 进行 了运动规划优化建模及启发式优化求解方法。 该 方法可有效提升优化求解效率和综合运动性能， 为机器人高端制造 应用提供了方法支撑 。 权利要求书3页 说明书5页 附图1页 CN 115179291 A 2022.10.14 CN 115179291 A 1.一种基于性能约束项阶段降维的工业机器人运动规划优化方法， 其特征在于： 该方 法包括如下步骤， S1、 将工业机器人结构分解并简化为圆柱体或长方体包络的臂杆组合， 采用球体包络 方法描述操作目标结构， 建立机器人与目标间最短距离粗估计函数， 为建立安全性能指标 提供部分函数基础； S2、 将工业机器人结构分解为变截面圆柱体和凸多边体， 采用基于切片的方法进行臂 杆描述， 同样采用凸多边形平面包络描述 目标结构， 建立机器人与目标间最短距离精确估 计函数， 为建立 安全性能指标提供部分函数基础； S3、 基于机器人运动学模型和关节输出力矩值， 结合各关节扭转刚度值建立机器人末 端柔性误差模型， 形成运动误差估计函数； 在实际应用中， 关节输出力矩可通过传感器测量 或基于动力学模型计算得到， 关节扭转刚度体现了关节柔性， 可通过实验测 量和数学建模 的方法估计， 为建立精度性能指标提供函数基础； S4、 以机器人目标连续轨迹为任务， 将任务具体分解为由初始位置到预作业点、 由预作 业点到连续轨 迹起点、 由轨迹起点到终点的三阶段任务过程， 进行 各关键路径点 位姿； S5、 基于路径点位姿进行插值规划， 具体针对前两个阶段进行关节空间五次多项式插 值规划， 第三个阶段进行关节空间三次样条插值 规划， 确定分段关节角位移时间函数； S6、 确定分段规划变量、 各段轨迹运动性能要求， 设计分段性能约束项组合， 进行整体 运动规划约束项分段降维， 以整体运动时间最小为优化 目标， 建立机器人运动规划优化问 题模型； S7、 采用粒子群算法依据 经验确定算法参数， 进行优化模型求解， 得到工业机器人优化 运动轨迹。 2.根据权利要求1所述的一种基于性 能约束项阶段降维的工业机器人运动 规划优化方 法， 其特征在于： 该 方法包括如下步骤， 步骤(1)建立机器人与目标 结构间最短距离粗估计模型； 将工业机器人进行简化分解， 进行长方体包络描述各臂杆， 采用球体包络描述目标结 构； 则机器人ROB( θ )与目标 结构OBJ间的最短距离粗估计模型定义如下： DIS1[ROB( θ ),OBJ]＝mi n{dis1(Po lyHDi,Sphere),1≤i≤N} (1) 式中PolyHDi表示第i个长 方体结构， Sphere表示球体 结构， dis1表示长 方体与球体间最 短距离计算 函数， N为长方体臂杆个数； 步骤(2)建立机器人与目标 结构间最短距离精确估计模型； 将工业机器人进行分解， 根据各臂杆特点， 通过臂杆切片形成由圆形切片 和凸 多边形切片 集合整体描述的机器人结构， 以及由凸多边形平面Planek集合描述的 目标结构， 则机器人ROB( θ )与目标 结构OBJ间的最短距离精确估计模型 可定义如下： 式中dis2表示圆形切片与凸多边形平面间最短距离函数， dis3表示凸多边形切片与凸权　利　要　求　书 1/3 页 2 CN 115179291 A 2多边形平面间的最短距离计算函数， 和 分别表示机械臂中第ic个圆形切片 和第ip个凸多边形切片， Planej表示机器人作业空间中第j个障碍平面， Ic,Ip和J分别表示 机器人切片集合中所包含的圆形切片数量、 凸多边形切片数量、 目标结构 中凸多边形平面 数量； 步骤(3)建立 考虑机构柔 性的工业机器人末端运动误差估计模型； 将工业机器 人正运动学模型描述为Forw( θ )， 第k个关节的输出扭矩表示为τj， 扭转刚度 表示为Kj， 则考虑关节柔性的机器人末端实际位姿为gsta＝Forw( θ +Δθ )， 其中Δθ 为各 关节 柔性误差集合， Δθ＝(Δθ1,Δθ2,…,ΔθK)， K为机器人关节总数， 第k个关节的柔性误差为 Δθk＝ τk/Kk； 工业机器人末端实际位置Pa由位姿gsta得到， 则末端运动误差表示如下： Error(t)＝| |Pa‑Pd|| (3) 式中Pd为工业机器人末端目标理想位置； 步骤(4)进行目标 连续轨迹任务分解； 将目标连续任务分为三段， 由初始位置到预作业点， 将初始位置和预作业点处的机器 人末端位姿分别表示为gst0和gst1； 由预作业点到连续轨迹 起点， 将连续轨迹 起点处机器人 末端目标位姿表示为gst2； 由连续轨迹起点到终点， 将终点处的机器人末端目标位姿表示 为gst3； 步骤(5)进行机器人任务整体轨 迹进行分段插值 规划； 建立机械臂运动学逆解方程， 并形成函数Inv(gst)， 针对第一段和第二段非连续轨迹 追踪， 初始位置处各关节角位移为θ0＝(0,0,0,0,0,0)， 通过逆解计算各路径点处的关节角 位移逆解， θ1＝Inv(gst1)和θ2＝Inv(gst2)； 对前两段轨迹采用三次多项式进行插值规划， 则某关节角位移的时间t相关函数表示如下， 式中θs和 θe分别为某关节在一段轨迹中的初始角位移和目标角位移， T为该段轨迹的运 动总时间； 对第三段轨迹采用三次样条曲线进行连续轨迹插值规划， 首先在连续轨迹上取M个路 径点， 将第m个 路径点处的位姿表示为gst3,m， 进行各路径点逆解计算， θ3,m＝Inv(gst3,m)， 则 第三段轨迹会被细分为M段子轨迹， 第m段子轨迹的时间区间为[tm‑1,tm]， 某关节角位移函 数表示如下： 式中 和 为第m段子轨迹的起始和终止加速度值， Tm为该段子轨迹的运动时间， Tm ＝tm‑tm‑1； 则第三段轨 迹运动时间 步骤(6)构建基于约束 项降维的机器人分段运动规划优化模型； 在该轨迹规划中， 规划变量为第一段轨迹运动时间Td1、 第二段轨迹运动时间Td2和第三 段轨迹中各子轨迹段的运动时间Tm， 整体考虑效率指标， 并根据第k关节角位移允许范围Dk， 角速度和角加速度允许最大值Vk,max和Ak,max， 建立统一的运动学约束条件；权　利　要　求　书 2/3 页 3 CN 115179291 A 3