(19)国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 202211144194.3 (22)申请日 2022.09.20 (71)申请人 南京航空航天大 学 地址 210016 江苏省南京市秦淮区御道街 29号 (72)发明人 张得礼　王志国　王珉　王小平　 安鲁陵　曾思力　周星雨　 (74)专利代理 机构 南京天华专利代理有限责任 公司 32218 专利代理师 孔金皓　徐冬涛 (51)Int.Cl. B25J 9/16(2006.01) (54)发明名称 基于动力学的水 下机械臂轨迹优化方法 (57)摘要 本发明公开了一种考虑动力学特性的水下 机械臂轨迹优化方法， 属于机械设备控制领域。 现有技术对于机械臂的轨迹规划算法只考虑机 械臂的速度、 加速度等运动学约束， 而忽略了机 械臂的动力学约束， 这就可能导致机械臂运行时 出现关节电机力矩超限的情况， 这会使机械臂无 法准确执行规划轨迹甚至停 机的问题。 本发明把 机械臂的动力学约束加入到轨迹优化算法中来， 分入时间缩放因子和前瞻时间， 在检测到关节力 矩超限时， 及时引入时间缩放因子局部调节关节 力矩， 从而使关节力矩满足动力学限制。 相比于 现有技术全局性的调速， 本发明的轨迹优化避免 了不必要的降速， 仅在关节力矩超限部分进行调 节， 在满足动力学限制的同时保证了水下机械臂 作业时的效率。 权利要求书7页 说明书13页 附图3页 CN 115351790 A 2022.11.18 CN 115351790 A 1.一种基于动力学的水 下机械臂轨迹优化方法， 其特 征在于， 包括以下步骤： 步骤1： 对水下机械臂笛卡尔空间轨迹规划采用直线、 圆弧路径规划以及 s型速度规划； 对水下机械臂关节空间轨迹规划采用s型速度规划， 得到不同插补时间对应的插补路径点， 得到的插补路径点即规划轨 迹； 步骤2： 确定初始前瞻时间， 通过动力学模型计算初始前瞻时间内步骤1得到的插补路 径点对应的关节电机力矩， 将计算出 的关节电机力矩与此关节电机额定的力矩进行对比， 判断此前瞻时间范围内是否 关节电机力矩超限， 若 未超限则 初始时间缩放因子为1， 跳到步 骤4； 若关节电机力矩超限， 则 根据求得的关节电机力矩τi与此关节电机额定力矩上限 和对应的此关节电机额定力矩下限 来计算时间缩放因子， 具体如下： 当τi＞0时， 若 则时间缩放因子的最大值λmax为 时间缩 放因子最小值λmin为 若 则时间缩放因子的最大值λmax为 时间缩放因子最小值 λmin为0； 当τi＝0时， 电机处于停止状态； 当τi＜0时， 若 则时间缩放因子的最大值λmax为 时间缩 放因子最小值λmin为0； 若 则时间缩放因子的最大值λmax为 时 间缩放因子最小值 λmin为 将求得的时间缩放因子存入由单调队列维护的目标时间缩放因子序列， 此序列中的每 一个目标时间缩放因子都是此序列中的一个元素， 此序列中的每一个元素都会开启一个时 间缩放调节任务； 步骤3： 开启一个时间缩放调节任务， 以目标时间缩放因子序列中队头元素作为调节任 务的目标因子， 对实际时间缩放因子进 行调节； 并以实际时间缩放因子为依据进 行调速， 根 据实际时间缩放因子计算得到插补路径点， 此插补路径点是调速后的插补路径点， 机械臂 执行调速后的插补路径点， 跳 到步骤5； 步骤4： 目标时间缩放因子序列中无 元素， 则机械臂按原规划轨 迹执行插补路径点； 步骤5： 机械臂执行完最后一个插补路径点， 则跳到步骤11； 机械臂若没有执行到最后 一个插补路径点则跳 到步骤6； 步骤6： 通过动力学模型计算前瞻时间内剩余插补路径点对应的关节电机力矩， 将计算 出的关节电机力矩与此关节电机的额定力矩进 行对比， 判断此前瞻时间范围内是否 关节电 机力矩超限， 若未超限则跳 到步骤8； 若关节电机力矩超限， 跳 到步骤7； 步骤7： 根据求得的关节电机力矩τi与说明书对应的此关节电机力矩上限 和对应的 此关节电机力矩下限 来计算时间缩放因子， 具体如下： 当τi＞0时， 若 则时间缩放因子的最大值λmax为 时间缩权　利　要　求　书 1/7 页 2 CN 115351790 A 2放因子最小值λmin为 若 则时间缩放因子的最大值λmax为 时间缩放因子最小值 λmin为0； 当τi＝0时， 电机处于停止状态； 当τi＜0时， 若 则时间缩放因子的最大值λmax为 时间缩 放因子最小值 λmin为0； 若 则时间缩放因子的最大值 λmax为 时 间缩放因子最小值 λmin为 将求得的时间缩放因子存 入由单调队列维护的目标时间缩放因子序列中； 步骤8： 若有正在执行的时间缩放调 节任务， 跳到步骤9， 若没有正在执行的时间缩放调 节任务， 跳 到步骤10； 步骤9： 判断目标时间因子序列的队头元素对应的时间缩放调节任务是否已经执行完 毕， 若执行完毕， 弹出队头元素进行下一个时间缩放调节任务； 如果没有执行完毕， 继续执 行当前时间缩放调节任务， 根据实际时间缩放因子计算得到插补路径点， 执行此插补路径 点， 并跳转到步骤5； 步骤10： 如果目标时间因子序列已空， 令时间缩放因子为1， 无需进行时间缩放调节任 务， 执行已规划好的插补路径点， 跳转到步骤5； 如果目标时间因子序列中还有 元素， 根据队 头元素开启新的时间缩放调节任务， 根据实际时间缩放因子计算得到插补路径点， 得到新 的插补路径点， 执 行此新的插补路径点， 跳转到步骤5； 步骤11： 结束算法， 通过修改时间缩放因子的值来改变原来的插补时间， 以达到时间缩 放的目的， 得到优化后的轨 迹插补路径点， 这些优化后的插补路径点 就是新的规划轨 迹。 2.根据权利要求1所述的基于动力学的水下机械臂轨迹优化方法， 其特征在于， 步骤1 中所述的直线路径规划为： 输入参数： 始末两点 坐标(x0， y0， z0， a0， b0， c0)， (x1， y1， z1， a1， b1， c1)； 输出当前时刻末端插补点(xt， yt， zt， at， bt， ct)； 正常情况下， 根据始末两点坐标计算处两点间距离， 对两点距离进行s型速度规划， 并 以s型速度规划得到的距离distancest与总距离distance为依据进行插补， 即 xt＝x0+k·(x1‑x0) yt＝y0+k·(y1‑y0) zt＝z0+k·(z1‑z0) at＝a0+k·(a1‑a0) bt＝b0+k·(b1‑b0) ct＝c0+k·(c1‑c0) k＝distancest/distance 圆弧路径规划为： 输入参数： 圆弧首末两点 坐标， 与中间一 点坐标； 输出当前时刻末端插补点(xt， yt， zt， at， bt， ct)； 根据用户给定的三点建立局部平面坐标系， 并在局部平面坐标中计算 圆弧的圆心坐标权　利　要　求　书 2/7 页 3 CN 115351790 A 3