一种解耦型六自由度工业机器人的运动控制方法

摘要

本发明公开了一种解耦型六自由度工业机器人的运动控制方法，该方法包括：（a）根据机器人所需实现的位姿，通过D-H模型法获得末端执行机构相对于基坐标系的位姿矩阵；（b）将机器人避开奇异形位时所能实现的正常位姿定义为不同的关节特性属性，并设定机器人实现所需的位姿时的关节特征属性组合；（c）根据位姿矩阵以及设定的关节特征属性组合及其取值条件，通过反变换法分别求得关于机器人各个关节变量的唯一解；（d）根据所求得的解，执行对六自由度工业机器人的关节运动，相应完成整体运动控制过程。通过本发明，具备可预知过奇异点路径、算法简单、反解速度快以及能较好地确定唯一解等优点，并能很好地应用于实际的工业机器人运动控制。

主权项

1.一种解耦型六自由度工业机器人的运动控制方法，该六自由度工业机器人包括底座、腰部回转部件、大臂、小臂、手腕部件和末端执行机构，并通过第一关节完成机器人腰部的回转运动，通过第二、第三关节分别执行大臂和小臂的俯仰动作，以及通过第四至第六关节共同执行末端执行机构的位姿操作，所述方法包括下列步骤：（a）根据对机器人末端执行机构所需实现的位置和姿态，通过D-H模型法获得末端执行机构相对于基坐标系的位姿矩阵${}_6{}^{0}T={}_1{}^{0}T{}_2{}^{1}T{}_3{}^{2}T{}_4{}^{3}T{}_5{}^{4}T{}_6{}^{5}T=\left[\begin{array}{cccc}{n}_x& o_x& a_x& p_x\\ n_y& o_y& a_y& p_y\\ n_z& o_z& a_z& p_z\\ 0& 0& 0& 1\end{array}\right]---\left(1\right)$该位姿矩阵中各个元素分别如以下表达式组（2）所示，其中c₁~c₆、s₁~s₆分别依次表示机器人各个关节变量θ₁~θ₆的余弦值和正弦值，c₂₃、s₂₃分别表示关节变量θ₂与θ₃之和的余弦值和正弦值，a₁~a₃分别表示机器人第一、第二和第三关节各自与其相邻的下一关节之间的轴线距离，d₄表示第四关节与末端执行机构之间的距离：第三关节轴线和第四、五关节轴线交点之间的距离：$\left.\begin{array}{c}{n}_x=c_1[c_{23}(c_4{c}_5{c}_6-s_4{s}_6)-s_{23}{s}_5{c}_6]+s_1(s_4{c}_5{c}_6+c_4{s}_6)\\ n_y=s_1[c_{23}(c_4{c}_5{c}_6-s_4{s}_6)-s_{23}{s}_5{c}_6]-c_1(s_4{c}_5{c}_6+c_4{s}_6)\\ n_z=s_{23}(c_4{c}_5{c}_6-s_4{s}_6)+c_{23}{s}_5{c}_6\\ o_x=c_1[c_{23}(-c_4{c}_5{c}_6-s_4{c}_6)+s_{23}{s}_5{s}_6]+s_1(c_4{c}_6-s_4{c}_5{s}_6)\\ o_y=s_1[c_{23}(-c_4{c}_5{c}_6-s_4{c}_6)+s_{23}{s}_5{s}_6]-c_1(c_4{c}_6-s_4{c}_5{s}_6)\\ o_z=s_{23}(-c_4{c}_5{c}_6-s_4{c}_6)-c_{23}{s}_5{s}_6\\ a_x=c_1(c_{23}{c}_4{s}_5+s_{23}{c}_5)+s_1{s}_4{s}_5\\ a_y=s_1(c_{23}{c}_4{s}_5+s_{23}{c}_5)-c_1{s}_4{s}_5\\ a_z=s_{23}{c}_4{s}_5-c_{23}{c}_5\\ p_x=c_1(a_1+a_2{c}_2+a_3{c}_{23}+d_4{s}_{23})\\ p_y=s_1(a_1+a_2{c}_2+a_3{c}_{23}+d_4{s}_{23})\\ p_z=a_2{s}_2+a_3{s}_{23}-d_4{c}_{23}\end{array}\right\}---\left(2\right)$（b）将机器人避开奇异形位时所能实现的正常位姿定义为不同的关节特性属性cfg1、cfg2和cfg3，并设定机器人末端执行机构实现所需的位姿时的关节特征属性组合，其中所述关节特性属性所代表的意义和取值条件分别如下：cfg1表示当机器人手腕部件的中心点分别处于第一参考平面左右两侧时的状态，并且满足$\mathrm{cfg}1=\left\{\begin{array}{c}1,c_1{p}_x+s_1{p}_y\ge 0\\ 0,c_1{p}_x+s_1{p}_y<0\end{array}\right.,$所述第一参考平面是以机器人第一关节的转动轴线与第二关节的轴线平行线两者所构成的平面；cfg2表示机器人手腕部件的中心点分别处于第二参考平面左右两侧时的状态，并且满足$\mathrm{cfg}2=\left\{\begin{array}{c}1,\tan {\theta }_3\ge d_4/a_3\\ 0,\tan {\theta }_3<d_4/a_3\end{array}\right.,{\theta }_3\in (-\pi ,\pi ),$所述第二参考平面是以机器人大臂和第三关节的轴线所构成的平面；cfg3表示机器人手腕部件的中心点分别处于第三参考平面左右两侧时的状态，并且满足$\mathrm{cfg}3=\left\{\begin{array}{c}1,{\theta }_5<0\\ 0,{\theta }_5\ge 0\end{array}\right.,{\theta }_3\in (-2\pi ,2\pi ),$所述第三参考平面是以机器人小臂和第五关节的轴向所构成的平面；（c）根据步骤（a）所获得位姿矩阵、以及步骤（b）所设定的关节特征属性组合及其取值条件，通过反变换法分别求得关于机器人各个关节变量θ₁~θ₆的唯一解；（d）根据所求得的各个关节变量的解，执行对六自由度工业机器人的关节运动，相应完成所需位姿的运动控制过程。