一种多关节机械臂空间函数轨迹运动的逆运动学求解方法

摘要

本发明提供的是一种多关节机械臂空间函数轨迹运动的逆运动学求解方法。解决了多关节机械臂在按照既定轨迹函数运动的过程中，操作空间与关节空间以及关节空间与驱动空间的映射关系。在给定空间轨迹函数之后，本发明的能够输出各个时刻的各个关节的姿态（具体为各个关节的末端位置）以及每根驱动绳的绳长。在解决操作空间与关节空间的映射关系时，采用数值方法中的二分法对该关系进行数值求解。在解决关节空间与驱动空间的映射关系时，可以算法求出各个关节坐标系关于基坐标系的转换矩阵，从而得到各个驱动绳的绳长。

说明书

技术领域

本发明提供的是一种多关节机械臂空间函数轨迹运动的逆运动学求解方法。

背景技术

现在的机械臂一般多为刚性结构，具有5-7个自由度，手臂本体的灵活度较差，无法满足空间狭小、复杂等环境下的作业要求。而本发明所适用的多关节机械手，其自由度多、姿态灵活多变，能够满足上述工况作业要求。

本发明适用的情况主要是在一些较为复杂、障碍物较多的环境中，需要使机械臂在基座的配合下按照给定轨迹绕过障碍物运动到指定位置，再在指定空间的中完成所需要的工作(如对管道的切割)。而对这种输入轨迹函数的运动还没有专门的逆运动学求解方式，本发明就针对这种运动情况提出了一种求逆运动学解的方式。

发明内容

本发明的目的在于提供了一种给定空间轨迹函数多关节机械臂逆运动学求解方法。操作方式为在提前给定空间轨迹的情况下，机械臂通过此种方法在基座的配合下进行运动，以达到规定空间轨迹曲线的形态，到达既定的工作位置。

本发明在给定机械臂的空间轨迹函数的条件下，给出各个时刻各个机械臂的位置以及牵引绳的长度。

本发明技术方案的具体内容如下：

(1)输入机械臂的总关节数n，每节关节长度L，运动时间t(t为自变量)，空间轨迹函数。

(2)解决操作空间与关节空间的映射关系，即给定空间轨迹函数后，输出每个时刻的机械臂姿态信息(每个关节的末端位置)。

(3)解决关节空间与驱动空间的映射关系，在求解出每个关节的末端姿态之后，只了解到该时刻的机械臂空间姿态模样，本发明还提供了一套算法求解出驱动空间即对应驱动绳的长度。

本发明的有益效果是：提出一种轨迹操控的多关节机械臂空间运动算法，能够在已知机械臂的基础信息(关节个数，各关节长度等信息)的情况下，得到整个运动过程各个时刻的机械臂姿态信息以及驱动绳长，从理论上给出了一类多关节机械臂的轨迹函数运动的运动学分析。

附图说明

图1是多关节机械臂的外观以及运动示意图；

图2是二分法算法框图；

图3是两关节之间转动副的结构示意图；

图4是matlab算法演示图。

具体实施方式

下面根据附图对本发明的求解过程做更详细的描述。

假设该多关节机械臂的总关节数为n，从电机往外依次为第n节，第n-1节…第1节，每节关节长度为L，运动时间为t。本发明从两种运动模式探究三维运动情况下，各关节的运动情况。示意如图1。

(1)操作空间与关节空间

由于空间曲线方程有多种表达形式，为了描述方便选取下述方程组来描述空间轨迹：

其中x、y、z分别代表空间曲线轨迹的三维坐标。在上述表达方式中，给出x与z对自变量y的函数表达式，可以很容易地得出轨迹的一个很重要的性质，即对于y坐标的单值性，同一个y值只会对应轨迹当中的一个点。由于机械臂沿y轴推进，所以在这种轨迹下机械臂不能在y方向上转向，即不能返回运动，这也非常符合对于机械臂的运动要求。

与此同时还需注意另一个运动特性。机械臂按照既定轨迹运动时，后一关节总是在重复前一关节在L/v时间前的动作，同时可以理解为除了第1个关节的之后所有关节总是在重复第1个关节的动作。假设第i节的首端的三维坐标分别为xi(t)、yi(t)、zi(t)，(此处应注意，为了简化描述，简化模型为第i节首端与第i+1节尾端为同一位置，实际上此处的位置坐标应该为两节关节中间转动副的中心点坐标)那么将存在以下关系：

上述关系式的提出大大地简化了之后的求解计算过程，即只需关注第1个关节的运动情况即可，并且记录第1节的首端的三维坐标随时间变化关系即可得到后续所有关节的运动情况。

假设在t时刻，第k节首端已经开始沿着轨迹运动，第k+1节首端还未进入轨迹区还处于直线区。对于k<i<＝n，其关节首端坐标为以下关系：

x_i＝0

y_i＝-(i-1)L+vt

z_i＝0

对于1<i<＝k，其关节首端坐标为以下关系：

对与第1节关节首端应满足以下方程：

将上述三元方程(其中x2、y2、z2为在前述已求得的第2节关节首端三维坐标)的前两方程带入第三个方程得：

g(y₁)＝(x_f(y₁)-x₂)²+(y₁-y₂)²+(z_f(y₁)-z₂)²-L²＝0

采用二分法来求出该方程的数值解，显然有解区间为y2<y1<＝y2+L，且由于前面所定义的轨迹方程的描述方法所限制，在这个有解区间内有且只有一个解。具体算法框图描述如图2。其中E为运算精度，当E足够小时认为即求出上述方程的数值解。

(2)关节空间与驱动空间

先分析第1节首端已进入y>0区域，第2节首端还未进入y>0区域，则第2节的坐标系与基坐标系重合，第1节的坐标系与该节的指向有关。两节之间的转动副如图3，该转动副具有两个自由度，即绕两垂直轴旋转的自由度。由于第2节首端还未进入y>0区域，因此轴2与基坐标系的z轴平行。假设基坐标系的单位正交基向量为第一节的坐标系为则该转动副的运动可以理解为先绕着轴2旋转α角度，再绕着轴1旋转β角度。该过程可以描述为以下等式：

在第1节坐标系中，以第1节指向为该坐标系的y轴。在基坐标系上，第1节的单位方向向量为：

[x₁-x₂>1-y₂>1-z₂]/norm

其中norm为该向量的模。

则存在以下关系：

则可得以下等式：

则转换矩阵M可得(另描述单位方向向量为[Vx1 Vy1 Vz1]＝[x₁-x₂>1-y₂>1-z₂]/norm)：

相似地，由于轨迹运动的特殊性，后一关节总是在重复前一关节在L/v时间前的动作，所以对于某个时刻t来说，假设Mi表示第i节关节的坐标系的转换矩阵，则有以下等下：

因此，只需要记录每个时刻第1节关节坐标系的转换矩阵，即可的得到每个时刻其他关系坐标系的转换矩阵。

只探究前两节关节坐标系，由上式可得：

由于要记录每个时刻的第1节关节坐标系的转换矩阵，所以还需求出此刻的M1。由前述方法可得：

求解出上述线性方程组即可求解出M1矩阵。

在每两节的转动副中，并不是通过电机驱动改变两节的相对角度，而是通过连接两关节的绳来调节两个关节的相对位置。在前述中，算法解决了各个时刻的每个关节的指向以及位置，现在阐述一种解决关节空间与驱动空间的算法，即算出每个时刻对应绳的长度。

在每两个关节之中存在着许多用于绳穿过的孔洞，且两个关节的孔洞一一对应，计算绳长的关键在于计算相应孔洞之间的距离。其中假设一圈孔洞距离关节中心轴的距离为R，待计算的绳长所处位置与相应坐标系x轴夹角为θ(显然可知，在k+1节的坐标系中夹角为θ的绳与在k节的坐标系中夹角为θ的绳为同一根绳)，另外需要指出的是由于机械设计的特殊性，第k+1节的首端截面中心与第k节的尾端截面中心距离始终相等，设为D。在前述当中，在每个时刻都能够得到每个节的坐标系的转换矩阵Mi，即可以得到每两个节的相对转角α与β。相似地，由于轨迹运动的特殊性，只需算出每个时刻的第1节与第2节之间的绳长即可。

在第2节坐标系中，将坐标系的原点设在第2节的首端截面的中心，夹角为θ的绳所在孔洞坐标为(假设第2节的正交坐标基为)：

位于第1节尾端的对应孔洞坐标为：

只需求出上述两坐标的直线距离即求得之间的绳长。

上述算法matlab模拟演示如图4。