[发明专利]一种六轴串联工业机器人末端的姿态轨迹过渡规划方法

权利要求书

1.一种六轴串联工业机器人末端的姿态轨迹过渡规划方法，其特征在于，包括以下步骤：

S1、令i＝1，N为给定运动路径的总段数；

S2、在由姿态示教点Q_i和Q_i+1构成的球面线性插补轨迹和由姿态示教点Q_i+1和Q_i+2构成的球面线性插补轨迹之间进行平滑过渡；其中，利用三次四元数准均匀B样条曲线作为姿态轨迹过渡段；姿态示教点Q_i和Q_i+1分别为第i段姿态轨迹的起始姿态点和终止姿态点；姿态示教点Q_i+1和Q_i+2分别为第i+1段姿态轨迹的起始姿态点和终止姿态点；

S3、根据第i段姿态轨迹的姿态示教起始姿态点Q_i和终止姿态点Q_i+1求取第i段姿态轨迹的角位移长度θ_i和参数Q′_i(u)；参数Q′_i(u)的表达式如下：

S4、根据第i+1段姿态轨迹的姿态示教起始姿态点Q_i+1和终止姿态点Q_i+2求取第i+1段姿态轨迹的角位移长度θ_i+1和参数Q′_i+1(u)；参数Q′_i+1(u)的表达式如下：

S5、根据第i段姿态轨迹的角位移长度θ_i、第i+1段姿态轨迹的角位移长度θ_i+1和给定姿态过渡半径求取实际姿态过渡半径

S6、根据第i段姿态轨迹的角位移长度θ_i和实际姿态过渡半径求取第i段姿态轨迹过渡段的姿态过渡起点对应的参数值，再根据参数Q′_i(u)和导数的求取方式求取第i段姿态轨迹在姿态过渡起点处的姿态、一阶导数和二阶导数；

S7、根据第i+1段姿态轨迹的角位移长度θ_i+1和实际姿态过渡半径求取第i段姿态轨迹过渡段的姿态过渡终点对应的参数值，再根据参数Q′_i+1(u)和导数的求取方式求取第i+1段姿态轨迹在姿态过渡终点处的姿态、一阶导数和二阶导数；

S8、建立第i段姿态轨迹过渡段的参数表达式q_i(u)；

S9、根据两条姿态曲线在曲线拼接实现曲率连续需要满足的条件建立方程组，并求取姿态控制点的姿态后，代入第i段姿态轨迹过渡段的参数表达式q_i(u)；

S10、计算第i段姿态轨迹过渡段的角位移长度

S11、根据第i段姿态轨迹过渡段的角位移长度进行速度规划，获得角位移长度与时间的函数关系，从而根据插补点的时刻得到插补点在姿态轨迹过渡段上的相对角位移长度，接着根据插补点在姿态轨迹过渡段上的相对角位移长度反解出插补点对应的参数值，最后将插补点对应的参数值代入姿态轨迹过渡段的参数表达式获得插补点的姿态；

S12、令i＝i+1；判断i小于N-2是否成立，成立则执行步骤S2，不成立则执行步骤S14；

S13、通过运动学逆解求得六轴串联工业机器人各关节转动的角度以驱动六轴串联工业机器人转动，从而实现六轴串联工业机器人末端的姿态轨迹过渡。

2.如权利要求1所述的六轴串联工业机器人末端的姿态轨迹过渡规划方法，其特征在于，第i段姿态轨迹的角位移长度θ_i，如下：

θ_i＝arccos(Q_i,Q_i+1)

第i+1段姿态轨迹的角位移长度θ_i+1，如下：

θ_i+1＝arccos(Q_i+1,Q_i+2)。

3.如权利要求1所述的六轴串联工业机器人末端的姿态轨迹过渡规划方法，其特征在于，步骤S5还包括：

判断或是否成立，不成立则令成立则再判断θ_i＞θ_i+1是否成立，成立则令不成立则令