[发明专利]一种基于零空间的机械臂速度层轨迹规划方法

说明书

本发明涉及一种基于零空间的协作型机械臂速度层轨迹规划方法。设计了吸引速度函数和排斥速度函数，避免了势场法在特殊场景下存在的几种问题；根据机械臂和障碍物移动方向与夹角判断障碍物运动趋势，并通过在原本避障方向的基础上叠加一个额外的方向向量实现动态避障轨迹规划；利用机械臂零空间的特性，将末端作业任务和避障任务划分优先级处理，通过将低优先级任务映射到高优先级任务的零空间中，机械臂可以在满足高优先级任务的条件下完成低优先级的任务。本发明基于机械臂的零空间特性，能够实现在动态非结构化环境下，机械臂在保证自身和环境安全的条件下同时完成机械臂末端的作业任务和本体的避障任务。

技术领域

本发明涉及智能机器人技术领域，尤其涉及一种机械臂在动态非结构化环境下的速度层轨迹规划方法。

背景技术

协作型机器人具有人机融合、安全易用、灵敏精准及灵活通用等特征，不仅适应工业领域中小批量、多品种、用户定制的柔性制造需求，在服务、医疗和电力等领域也有潜在的应用前景，已经成为引领下一代工业机器人发展的重要方向。在动态非结构化的任务环境中，如何使其能够根据环境变化实时动态调整运动轨迹，实现自主避障并到达目标位置，这是协作型机器人应用研究中尚待解决的一个关键问题。

当机械臂感知到工作空间中的障碍物并且障碍物有可能干扰到机械臂的作业任务时，机械臂应该及时地调整自己的运动轨迹，从而避免发生碰撞。机械臂自身可以看做是由一系列连杆串联而成的运动链，因此在轨迹规划过程中除了需要考虑末端执行器的避障外，还需要考虑本体连杆的避障策略。

发明内容

本发明的目的是提供一种基于零空间的机械臂速度层轨迹规划方法。

本发明为实现上述目的所采用的技术方案是：

一种基于零空间的机械臂速度层轨迹规划方法，包括以下步骤：

1)根据机械臂末端执行器的坐标判断机械臂是否达到目标点，若达到目标点，结束任务，否则，进行步骤2)；

2)根据机械臂与障碍物的最近距离，判断机械臂是否进入障碍物范围，若进入障碍物范围，进入步骤3)，否则，进入步骤7)；

3)根据机械臂移动的单位方向向量与障碍物移动的单位方向向量所成的夹角，判断障碍物的运动趋势，若障碍物接近机械臂，进行步骤4)，若障碍物远离机械臂，进入步骤5)；

4)求解机械臂的排斥速度矢量，并修正机械臂的避障方向，进入步骤6)；

5)求解机械臂的排斥速度矢量，进入步骤6)；

6)根据优先级策略计算吸引速度矢量和机械臂各关节控制量，进入步骤9)；

7)求解机械臂的吸引速度矢量，进入步骤8)；

8)计算机械臂各关节控制量，进入步骤9)；

9)上位机根据机械臂各关节控制量，控制机械臂运动，进入步骤1)。

步骤3)为：

机械臂移动的单位方向向量为障碍物的最近点到机械臂的单位方向向量为障碍物移动的单位方向向量为当时，障碍物朝向远离机械臂的方向移动，不需要调整避障方向，此时当时，障碍物朝接近机械臂的方向移动。

所述排斥速度表达式为：

其中，为排斥速度矢量，为机械臂的避障方向，d₀为机械臂本体与障碍物最近距离的大小，d_m为障碍物作用范围的半径大小，γ为人为设定的函数参数。

所述修正机械臂的避障方向为在原避障方向的基础上，叠加一个额外的方向向量，具体为：