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

权利要求书

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

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

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

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

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

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

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

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

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

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

所述优先级策略为：将机械臂的避障任务设置为高优先级，机械臂末端执行器作业任务即机械臂的行为任务设置为低优先级，同时满足机械臂末端执行器作业任务以及机械臂本体避障任务的表达式为：

其中，为机械臂避障任务所需的控制量，为低优先级任务即目标任务减去由避障任务所造成的运动耦合后的控制量，的秩对应避障任务所对应的零空间自由度的大小，的秩则对应末端目标任务所需的自由度大小，为机械臂各关节的控制量，为避障点处雅克比矩阵的伪逆，为排斥速度矢量，I为单位矩阵，J₀为避障点处的雅克比矩阵，J_e为作业任务所对应的雅克比矩阵，为吸引速度矢量，α_h为平滑因子，其大小取决于机械臂与障碍物之间的最小距离，表达式为：

其中γ和β为可调的参数，_x为机械臂与障碍物之间的最小距离；

所述排斥速度表达式为：

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

其中，为吸引速度矢量，d_a为的大小增大到常数的阈值，d为机械臂末端与目标点的距离。

2.根据权利要求1所述的一种基于零空间的机械臂速度层轨迹规划方法，其特征在于,步骤3)为：

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

3.根据权利要求1所述的一种基于零空间的机械臂速度层轨迹规划方法，其特征在于,所述修正机械臂的避障方向为在原避障方向的基础上，叠加一个额外的方向向量，具体为：

机械臂移动的单位方向向量为障碍物的最近点到机械臂的单位方向向量为障碍物移动的单位方向向量为调整后的避障方向向量为在的基础上叠加方向向量其中为方向垂直于和所构成的平面的方向向量，即为方向垂直于和所构成的平面的方向向量，并且与的夹角小于90°。