应用于工业机器人的追踪与实时避障反应规划算法

摘要

本发明提供了一种应用于工业机器人的追踪与实时避障反应规划算法，包括：步骤1，输入待追踪曲线位姿、末端刚体与障碍物间的实时距离；步骤2，通过运动学正解给出从机器人关节空间到工作空间的映射；步骤3，通过视觉传感器检测障碍物并计算与障碍物距离；步骤4，规划路径，工作空间刚体追踪及避障；步骤5，根据关节空间中雅克比伪逆法求关节角；步骤6，利用快速搜索生成树处理角约束问题。步骤7，约束检测，如不满足约束，则重复步骤6；如满足约束，则进入步骤8。步骤8，达到目标，得到关节角并据此计算关节空间控制力矩；达到目标，则结束算法流程；否则进入步骤9；步骤9，更新初始点，重新从步骤3执行。