买专利卖专利找龙图腾，真高效！ 查专利查商标用IPTOP,全免费！专利年费监控用IP管家,真方便！

摘要：本发明公开了一种考虑动力学特性的水下机械臂轨迹优化方法，属于机械设备控制领域。现有技术对于机械臂的轨迹规划算法只考虑机械臂的速度、加速度等运动学约束，而忽略了机械臂的动力学约束，这就可能导致机械臂运行时出现关节电机力矩超限的情况，这会使机械臂无法准确执行规划轨迹甚至停机的问题。本发明把机械臂的动力学约束加入到轨迹优化算法中来，分入时间缩放因子和前瞻时间，在检测到关节力矩超限时，及时引入时间缩放因子局部调节关节力矩，从而使关节力矩满足动力学限制。相比于现有技术全局性的调速，本发明的轨迹优化避免了不必要的降速，仅在关节力矩超限部分进行调节，在满足动力学限制的同时保证了水下机械臂作业时的效率。

主权项：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为时间缩放因子最小值λmin为若则时间缩放因子的最大值λmax为时间缩放因子最小值λmin为0；当τi＝0时，电机处于停止状态；当τi＜0时，若则时间缩放因子的最大值λmax为时间缩放因子最小值λmin为0；若则时间缩放因子的最大值λmax为时间缩放因子最小值λmin为将求得的时间缩放因子存入由单调队列维护的目标时间缩放因子序列中；步骤8：若有正在执行的时间缩放调节任务，跳到步骤9，若没有正在执行的时间缩放调节任务，跳到步骤10；步骤9：判断目标时间因子序列的队头元素对应的时间缩放调节任务是否已经执行完毕，若执行完毕，弹出队头元素进行下一个时间缩放调节任务；如果没有执行完毕，继续执行当前时间缩放调节任务，根据实际时间缩放因子计算得到插补路径点，执行此插补路径点，并跳转到步骤5；步骤10：如果目标时间因子序列已空，令时间缩放因子为1，无需进行时间缩放调节任务，执行已规划好的插补路径点，跳转到步骤5；如果目标时间因子序列中还有元素，根据队头元素开启新的时间缩放调节任务，根据实际时间缩放因子计算得到插补路径点，得到新的插补路径点，执行此新的插补路径点，跳转到步骤5；步骤11：结束算法，通过修改时间缩放因子的值来改变原来的插补时间，以达到时间缩放的目的，得到优化后的轨迹插补路径点，这些优化后的插补路径点就是新的规划轨迹。

全文数据：

权利要求：

百度查询： 南京航空航天大学 基于动力学的水下机械臂轨迹优化方法