一类六自由度机械臂运动学逆解的快速简便求法

技术领域

本发明属于机械臂逆运动学领域，具体涉及一种一类六自由度机械臂运动学逆解角度估 计方法。

背景技术

机械臂逆运动学是已知机械臂末端的位置和姿态来计算各个关节转过的角度值，是正向 运动学问题的反过程，正向运动学问题相对简单且解唯一，而逆向运动学的求解则相对复杂， 可能出现多解或者无解的情况。通常，机械臂连杆的非零参数越多，达到某一特定目标的方 式也越多，这些连杆参数取决于机械臂的几何结构，因此机械臂的结构越复杂，逆运动学的 求解则越复杂，对于一个全部为旋转关节的六自由度机械臂来说，最多可能会出现16种解的 情况。针对多解的情况，将机械臂逆运动学的求解方法分为两大类：封闭解和数值解法。数 值解法迭代性质使得求解速度大大减慢，这不利于现代工业机械臂的实时控制，而封闭解则 是我们希望得到的，但是并不是任何六自由度机械臂都具有封闭解，只有满足一定条件的六 自由度机械臂才具有封闭解。

Pieper提出了一类具有3个相邻的轴相交于一点特点的六自由度机械臂，这类机械臂满 足具有封闭解的条件，而本发明提出的方法正是针对此类机械臂，即6个关节均为旋转关节、 且后面3个轴相交于一点的机械臂。工业机械臂大部分属于此类机械臂，因此对于这类机械 臂逆运动解的研究具有很大的意义。本发明提出的求解过程对此类机械臂逆运动学的求解具 有借鉴意义。逆运动解的求取速度和准确度会直接影响机械臂的实时控制，对于要执行复杂 任务的机械臂来说，逆运动解的求取速度和准确度将直接决定机械臂执行复杂任务的能力。

发明内容

本发明的目的是提出一种应用到几何结构不同但同属于一类六自由度机械臂上提高逆运 动解的求解速度的一类六自由度机械臂运动学逆解的快速简便求法。

本发明的目的是通过以下方案实现的：

（1）根据机械臂末端的位置(X,Y,Z)和姿态矩阵来确定后三个关节轴相交点O的坐 标(x,y,z)，即机械臂腕部点的位置：

通过测量得知后三个关节轴相交点O与机械臂末端的距离为L，而末端的姿态矩阵是 已知的，

${}_B{}^{0}R=\left(\begin{array}{ccc}o& p& q\\ r& s& t\\ u& v& w\end{array}\right)$

姿态矩阵中第一列元素o，r，u依次代表机械臂末端工具坐标系的X轴与机械臂基坐 标系的X轴，Y轴，Z轴夹角的余弦，第二列元素p，s，v依次代表机械臂末端工具坐标系 的Y轴与机械臂基坐标系X轴，Y轴，Z轴夹角的余弦，第三列元素q，t，w依次代表机械 臂末端工具坐标系的Y轴与机械臂基坐标系X轴，Y轴，Z轴夹角的余弦，其中，

x＝X+L·q

y＝Y+L·t，

z＝Z+L·w

确定机械臂腕部点O的坐标(x,y,z)；

（2）利用几何法求解出前三个关节轴转过的角度θ_i，i＝1,2,3：

在六个关节轴上建立坐标系，依次为1～6号坐标系，通过类六自由度机械臂的几何构造 解出前三个关节轴转过的角度θ_i(i＝1,2,3)：

l₃cos(θ₂+θ₃)-l₂sin(θ₂+θ₃)-l₁sinθ₂＝x/cosθ₁-d₁，

l₃sin(θ₂+θ₃)+l₂cos(θ₂+θ₃)+l₁cosθ₂＝z-d₂，

tanθ₁＝y/x，

其中l₁代表2号坐标系原点与3号坐标系原点之间的距离，l₂代表3号坐标系原点与4 号坐标系原点之间的距离，l₃代表4号坐标系与机械臂腕部点O之间的距离，d₁代表1号坐 标系z轴与基坐标系z轴之间的距离，d₂代表1号坐标系原点与基坐标系原点在基坐标系z轴 方向上的距离；根据已经求得的前三个关节轴转过的角度和机械臂末端的姿态矩阵求出 机械臂的X-Y-Z欧拉角变换矩阵；

（3）通过欧拉角变换矩阵求得后三个关节轴转过的角度θ_i(i＝4,5,6)：

由旋转矩阵的变换关系得：

${}_E{}^{0}R={}_4{}^{0}R{|}_{{\theta }_4=0}·{}_6{}^{4}R{|}_{{\theta }_4={\theta }_5={\theta }_6=0}·{}_E{}^{6}R_{\mathrm{xyz}},$

其中，为6号坐标系相对于基坐标系的旋转矩阵，由机械臂前三个关节角度所 决定的，为4号坐标系到6号坐标系的变换，后三个关节转角都为0，为X-Y-Z 欧拉角的变换矩阵：

${}_E{}^{6}R_{\mathrm{xyz}}={}_6{}^{4}R{|}_{{\theta }_4={\theta }_5={\theta }_6=0}^{-1}·{}_4{}^{0}R{|}_{{\theta }_4=0}^{-1}·{}_E{}^{0}R$

其中为一个恒定的矩阵，由已经求得的前三个关节轴转过的角度θ_i(i＝1,2,3) 和${}_6{}^{4}R{|}_{{\theta }_4={\theta }_5={\theta }_6=0}^{-1}$可确定${}_6{}^{4}R{|}_{{\theta }_4={\theta }_5={\theta }_6=0}^{-1}·{}_4{}^{0}R{|}_{{\theta }_4=0}^{-1},$

根据末端的姿态矩阵和${}_6{}^{4}R{|}_{{\theta }_4={\theta }_5={\theta }_6=0}^{-1}·{}_4{}^{0}R{|}_{{\theta }_4=0}^{-1}$计算出

X-Y-Z欧拉角变换矩阵为：

$R_{\mathrm{XYZ}}({\theta }_4,{\theta }_5,{\theta }_6)=\left(\begin{array}{ccc}c{\theta }_{5}c{\theta }_6& -c{\theta }_{5}s{\theta }_6& s{\theta }_5\\ s{\theta }_{4}s{\theta }_{5}c{\theta }_6+c{\theta }_{4}s{\theta }_6& -s{\theta }_{4}s{\theta }_{5}s{\theta }_6+c{\theta }_{4}c{\theta }_6& -s{\theta }_{4}c{\theta }_5\\ -c{\theta }_{4}s{\theta }_{5}c{\theta }_6+s{\theta }_{4}s{\theta }_6& c{\theta }_{4}s{\theta }_{5}s{\theta }_6+s{\theta }_{4}c{\theta }_6& c{\theta }_{4}c{\theta }_5\end{array}\right),$

如$R_{\mathrm{XYZ}}({\theta }_4,{\theta }_5,{\theta }_6)=\left(\begin{array}{ccc}{r}_{11}& r_{12}& r_{13}\\ r_{21}& r_{22}& r_{23}\\ r_{31}& r_{32}& r_{33}\end{array}\right),$

若cosθ₅≠0，

${\theta }_5={\tan }^{-1}(r_{13}/\sqrt{r_{11}^2+r_{12}^2}),$

θ₄＝tan^-1((-r₂₃/cosθ₅)/(r₃₃/cosθ₅))，

θ₆＝tan^-1((-r₁₂/cosθ₅)/(r₁₁/cosθ₅))，

最后得到机械臂后三个关节轴转过的角度θ_i(i＝4,5,6)。

本发明的有益效果在于：该方法提出的思路可以应用到几何结构不同但同属于此类的六 自由度机械臂上。该方法大大简化了机械臂逆运动解的求解过程，提高了逆运动解的求解速 度，能够满足工业机械臂实时控制中对于快速性和准确性的要求。

附图说明

图1是在此类六自由度机械臂各个关节轴上所建立的坐标系的示意图；

图2是关于本发明中提到的一类六自由度机械臂的运动学逆解的求解流程图。

具体实施方式

下面将结合附图举例对本发明做更详细的描述：

根据机械臂末端的位置和姿态矩阵求取出机械臂后三个关节轴相交点，即机械臂腕部的 位置，根据机械臂腕部的位置坐标和机械臂的几何结构求出机械臂前三个关节轴转过的角度， 再根据求得的前三个关节角与机械臂末端的姿态矩阵求出机械臂的X-Y-Z欧拉角变换矩阵， 通过该矩阵求出后三个关节角。

该方法提出的求解思路可以应用到几何结构不同但同属于此类的六自由度机械臂上。该 方法大大简化了机械臂逆运动解的求解过程，提高了逆运动解的求解速度，能够满足工业机 械臂实时控制中对于快速性和准确性的要求。

对于该问题的其他发明大多数都是利用几何法或者代数法单独实现的，而本发明提出的 方法将几何法和欧拉角变换法相结合应用于机械臂逆运动解的求解中。理论上本发明提出的 方法是没有误差的，这能够保证该类六自由度机械臂运动学逆解的求解精度，而且该发明提 出的求解过程相比于单纯应用几何法或者代数法要简单许多，这能够保证该类六自由度机械 臂运动学逆解的求解速度。有少部分发明提出了将几何法与代数法联合应用求解机械臂的逆 运动解，但与本发明中提出的求解过程并不相同，本发明中提出的求解过程比其他同类发明 中提出的求解过程更加简便，而且适用范围更加广阔。

根据图1可知六自由度机械臂上各个关节轴的坐标系位置，本算法首先根据机械臂末端 的位置(X,Y,Z)和姿态矩阵求出机械臂的腕部点O的坐标，即图1中5号坐标系原点的坐 标(x,y,z)，如图1所示，点O的坐标与机械臂末端的位置(X,Y,Z)有如下的关系：

x＝X+L·q

y＝Y+L·t

z＝Z+L·w

L为点O与6号坐标系原点的距离，q，t，w对应着姿态矩阵第三列的元素。

点O的坐标(x,y,z)确定之后，可以根据机械臂的几何结构得到点O的坐标与机械臂前三 个关节角θ_i(i＝1,2,3)的关系，它们之间的关系可用如下方程表示：

l₃cos(θ₂+θ₃)-l₂sin(θ₂+θ₃)-l₁sinθ₂＝x/cosθ₁-d₁

l₃sin(θ₂+θ₃)+l₂cos(θ₂+θ₃)+l₁cosθ₂＝z-d₂

tanθ₁＝y/x

其中d₁代表1号坐标系z轴与基坐标系z轴之间的距离，d₂代表1号坐标系原点与基坐 标系原点在基坐标系z轴方向上的距离，这些距离可以通过测量得知，l₁，l₂，l₃在图1中均 有表示，由此可通过以上方程组求得机械臂前三个关节轴转过的角度θ_i(i＝1,2,3)。

由旋转矩阵的变换关系可得：

${}_E{}^{0}R={}_4{}^{0}R{|}_{{\theta }_4=0}·{}_6{}^{4}R{|}_{{\theta }_4={\theta }_5={\theta }_6=0}·{}_E{}^{6}R_{\mathrm{xyz}}$

其中，为6号坐标系相对于基坐标系的旋转矩阵，由机械臂前三个关节角度所 决定的，为4号坐标系到6号坐标系的变换，该变换的前提是后三个关节转角都 为0°，为X-Y-Z欧拉角变换矩阵。由上述的变换关系可得：

${}_E{}^{6}R_{\mathrm{xyz}}={}_6{}^{4}R{|}_{{\theta }_4={\theta }_5={\theta }_6=0}^{-1}·{}_4{}^{0}R{|}_{{\theta }_4=0}^{-1}·{}_E{}^{0}R$

其中为一个恒定的矩阵，取决于图1中4号坐标系与6号坐标系的相对旋转 关系。由图1可得：

${}_6{}^{4}R{|}_{{\theta }_4={\theta }_5={\theta }_6=0}^{-1}=\left(\begin{array}{ccc}0& 0& 1\\ 0& -1& 0\\ 1& 0& 0\end{array}\right)$

由已经求得的前三个关节轴转过的角度θ_i(i＝1,2,3)和可确定 ${}_6{}^{4}R{|}_{{\theta }_4={\theta }_5={\theta }_6=0}^{-1}·{}_4{}^{0}R{|}_{{\theta }_4=0}^{-1}.$

${}_6{}^{4}R{|}_{{\theta }_4={\theta }_5={\theta }_6=0}^{-1}·{}_4{}^{0}R{|}_{{\theta }_4=0}^1=\left(\begin{array}{ccc}\cos \left({\theta }_1\right)·\cos ({\theta }_2+{\theta }_3)& \sin \left({\theta }_1\right)·\cos ({\theta }_2+{\theta }_3)& \sin ({\theta }_2+{\theta }_3)\\ -\sin {\theta }_1& \cos {\theta }_1& 0\\ -\cos \left({\theta }_1\right)·\sin ({\theta }_2+{\theta }_3)& -\sin \left({\theta }_1\right)·\sin ({\theta }_2+{\theta }_3)& \cos ({\theta }_2+{\theta }_3)\end{array}\right)$

根据已知的末端姿态矩阵和可以计算出而是由后三个 关节角度θ_i(i＝4,5,6)决定的。该矩阵是X-Y-Z欧拉角变换矩阵，该矩阵是6号坐标系经过三 个旋转得来，分别对应着4、5、6号坐标系对应的关节轴的旋转，这三个轴的旋转可以看成 依次绕着6号坐标系的x轴、y轴和z轴的旋转，X-Y-Z欧拉角变换矩阵为：

$R_{\mathrm{XYZ}}({\theta }_4,{\theta }_5,{\theta }_6)=\left(\begin{array}{ccc}c{\theta }_{5}c{\theta }_6& -c{\theta }_{5}s{\theta }_6& s{\theta }_5\\ s{\theta }_{4}s{\theta }_{5}c{\theta }_6+c{\theta }_{4}s{\theta }_6& -s{\theta }_{4}s{\theta }_{5}s{\theta }_6+c{\theta }_{4}c{\theta }_6& -s{\theta }_{4}c{\theta }_5\\ -c{\theta }_{4}s{\theta }_{5}c{\theta }_6+s{\theta }_{4}s{\theta }_6& c{\theta }_{4}s{\theta }_{5}s{\theta }_6+s{\theta }_{4}c{\theta }_6& c{\theta }_{4}c{\theta }_5\end{array}\right)$

如$R_{\mathrm{XYZ}}({\theta }_4,{\theta }_5,{\theta }_6)=\left(\begin{array}{ccc}{r}_{11}& r_{12}& r_{13}\\ r_{21}& r_{22}& r_{23}\\ r_{31}& r_{32}& r_{33}\end{array}\right)$

若cosθ₅≠0，可得到

${\theta }_5={\tan }^{-1}(r_{13}/\sqrt{r_{11}^2+r_{12}^2})$

θ₄＝tan^-1((-r₂₃/cosθ₅)/(r₃₃/cosθ₅))

θ₆＝tan^-1((-r₁₂/cosθ₅)/(r₁₁/cosθ₅))

通过上述过程可求出机械臂后三个关节轴转过的角度θ_i(i＝4,5,6)。

本发明最后给出了此类六自由度机械臂运动学逆解的求解流程，如图2所示。