[发明专利]在机器人运动规划中避免运动学奇异点的方法及装置

权利要求书

1.一种在机器人运动规划中避免运动学奇异点的方法，所述机器人是球形腕工业机器人，每个所述球形腕由该工业机器人的转轴轴线相交于一点的三个相邻旋转关节构成，其特征在于，该方法包括：

建立所述工业机器人的运动学模型(S1)；

根据所述运动学模型求出预规划路径的路径点的运动学反解并根据所求出的运动学反解建立所述工业机器人的关节空间的运动学轨迹(S2)；

判断所述运动学轨迹是否经过球形腕的运动学奇异点(S3)；

当判定所述运动学轨迹不经过球形腕的任何运动学奇异点时，执行所述预规划路径(S4)；

当判定所述运动学轨迹经过所述球形腕运动学奇异点时，则将所述预规划路径的起始点的角度进行微小偏转而得到新的规划路径(S5)，然后

根据所述运动学模型求出新的规划路径的路径点的运动学反解，并根据所求出的运动学反解建立所述工业机器人的关节空间的新的运动学轨迹(S6)；

判断所述新的运动学轨迹是否经过球形腕的运动学奇异点(S7)；

当判定新的运动学轨迹仍经过球形腕的运动学奇异点时，放弃执行上述预规划路径(S8)；

当判定新的运动学轨迹不经过球形腕的任何运动学奇异点时，执行所述新的规划路径(S9)。

2.根据权利要求1所述的在机器人运动规划中避免运动学奇异点的方法，其特征在于，判断所述运动学轨迹是否经过球形腕的运动学奇异点(S3)的步骤包括：

计算出所述预规划路径中每个路径点的运动学反解中每个关节的角度与上一个路径点的运动学反解中对应的关节的角度之差，由此得出该关节在运动到所述每个路径点时的转动速度，并进而判断该转动速度是否超过一个预定的速度阈值；

若该关节的所述转动速度超过所述速度阈值，则判断球形腕的沿从该工业机器人基座至终端执行器的方向看的第二个关节的关节角度的绝对值是否小于零点阈值，若该关节角度的绝对值小于零点阈值，则判定该预规划路径经过球形腕运动学奇异点。

3.根据权利要求1所述的在机器人运动规划中避免运动学奇异点的方法，其特征在于，在建立所述工业机器人的运动学模型时，根据所述工业机器人的预规划路径中的起始点终端执行器姿态矩阵(T₁)和结束点终端执行器姿态矩阵(T₂)，通过插值方法计算预规划路径中的路径点。

4.根据权利要求3所述的在机器人运动规划中避免运动学奇异点的方法，其特征在于，在执行所述插值方法时，

首先将所述起始点终端执行器姿态矩阵(T₁)分解为起始点终端执行器姿态矩阵平移部分(p₁)和起始点终端执行器姿态矩阵旋转部分(q₁)，将所述结束点终端执行器姿态矩阵(T₂)分解为结束点终端执行器姿态矩阵平移部分(p₂)和结束点终端执行器姿态矩阵旋转部分(q₂)；

然后，分别对所述起始点终端执行器姿态矩阵平移部分(p₁)和所述结束点终端执行器姿态矩阵平移部分(p₂)进行线性插值而得到所述预规划路径中的路径点的终端执行器姿态矩阵平移部分(p_n)，对所述起始点终端执行器姿态矩阵旋转部分(q₁)和所述结束点终端执行器姿态矩阵旋转部分(q₂)进行线性插值而得到所述预规划路径中的路径点的终端执行器姿态矩阵旋转部分(q_n)；

合成所述预规划路径中的路径点的终端执行器姿态矩阵平移部分(p_n)和所述预规划路径中的路径点的终端执行器姿态矩阵旋转部分(q_n)而得到所述预规划路径中的各路径点的终端执行器姿态矩阵(T_n)。

5.根据权利要求4所述的在机器人运动规划中避免运动学奇异点的方法，其特征在于，

在执行所述新的规划路径(S9)的步骤之后，使用新的规划路径代替所述预规划路径(S10)。