CN108356820B - 一种多关节机械臂手动操控的逆运动学求解方法 - Google Patents

Abstract

本发明提供的是一种多关节机械臂手动操控的逆运动学求解方法。在给定空间轨迹函数之后，本发明能够输出各个时刻的各个关节的姿态以及每根驱动绳的绳长。在解决操作空间与关节空间的映射关系时，采用数值方法中的二分法对该关系进行数值求解。在解决关节空间与驱动空间的映射关系时，可以求出各个关节坐标系关于基坐标系的转换矩阵，从而得到各个驱动绳的绳长。本发明能够在已知机械臂的基础信息的情况下，得到整个运动过程各个时刻的机械臂姿态信息以及驱动绳长，给出了一类多关节机械臂的实时操控运动的运动学分析方法。

Description

一种多关节机械臂手动操控的逆运动学求解方法

技术领域

本发明提供的是一种多关节机械臂手动操控的逆运动学求解方法

背景技术

现在的机械臂一般多为刚性结构，具有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-l)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节，通过arduino与电脑matlab建立串口通信，将摇杆的数据通过arduino传送至matlab控制机械臂的走向。

操作模式采用预轨迹的运动方式，即预定机械臂之后运动的直线轨迹，利用上一种的运动模式求解方法求解出各个节的位置关系，摇杆传回的数据改变的是机械臂之后运动的直线轨迹。假设预轨迹的方向向量为(V_x,V_y,V_z)，则轨迹方程为：

转换成前述中的规定形式为：

其中x₁、y₁、z₁与之前叙述的相同，皆为当前时刻第1节首端的坐标。

总而言之，即利用摇杆确定预轨迹的方向向量之后，剩下的求解方法步骤与前述相同。接下来描述如何利用摇杆确定预轨迹的方向。

首先需要明确一点的是，摇杆的操作坐标系不应该为基坐标系，因为在机械臂运动一段时间之后，第1节运动指向与基坐标系偏差大，人为控制变得不便利，所以应该将摇杆的操作坐标系放置在第1节的坐标系上，这样较为符合人为操作的习惯。

先分析第1节首端已进入y>0区域，第2节首端还未进入y>0区域，则第2节的坐标系与基坐标系重合，第1节的坐标系与该节的指向有关。两节之间的转动副如图3，该转动副具有两个自由度，即绕两垂直轴旋转的自由度。由于第2节首端还未进入y>0区域，因此轴2与基坐标系的z轴平行。假设基坐标系的单位正交基向量为

第一节的坐标系为

则该转动副的运动可以理解为先绕着轴2旋转α角度，再绕着轴1旋转β角度。该过程可以描述为以下等式：

即化简为：

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

x₁-x₂ y₁-y₂ z₁-z₂]/norm

其中norm为该向量的模。

则存在以下关系：

则可得以下等式：

则转换矩阵M可得(另描述单位方向向量为[V_x1 V_y1 V_z1]＝[x₁-x₂ y₁-y₂ z₁-z₂]/norm)：

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

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

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

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

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

摇杆通过串口传递给matlab的数据是摇杆偏离原点的横纵坐标(X，Z)，是一组二维的数据坐标，它表示的是在第1节关节坐标系下，预轨迹下一刻要偏离(改变)的方向，值得注意的是，在这里的描述中，并没有描述为“第1节关节下一刻要偏离的方向”，这是因为该方法如上述一般，实时控制的是预轨迹直线的方向(即(V_x,V_y,V_z))，预轨迹指向和第1关节实际上并非同一概念，第1节一直在靠近预轨迹的指向，如果预轨迹一段时间不变化(即不操纵摇杆)那么第1节的指向终将和预轨迹指向重合。定义一个类似速度的参数Velocity，当摇杆偏离原点的横纵坐标为(X，Z)时，下一时刻的预轨迹方向向量(V_x‘,V_y’,V_z‘)：

其中[V_xV_yV_z]为前一时刻预轨迹方向向量，由上式可以看出，参数Velocity的大小影响着预轨迹改变的速度大小，该参数可以人为调节。

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

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

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

)：

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

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

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

Claims (1)

1.一种多关节机械臂手动操控的逆运动学求解方法，其特征在于：假设该多关节机械臂的总关节数为n，从电机往外依次为第n节，第n-1节…第1节，每节关节长度为L，其特征在于：求解操作空间与关节空间的映射关系时，定义三维空间中的多关节机械臂的空间轨迹为y坐标的单值函数；那么只需关注第1个关节的运动情况，并且记录第1节的首端的三维坐标随时间变化关系即可得到后续所有关节的运动情况；在已知前一时刻的姿态信息的情况下，利用二分法对运动过程进行数值求解，从而求出整个运动过程的操作空间与关节空间的映射关系；

其中第1节关节首端应满足以下方程：

其中x、y、z分别代表空间曲线轨迹的三维坐标，下标代表关节数；

将上述三元方程的前两方程带入第三个方程得：

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

采用二分法来求出该方程的数值解，得到有解区间为y₂<y₁<＝y₂+L，在这个有解区间内有且只有一个解；

求解关节空间与驱动空间的映射关系时，已知每个关节的姿态，即每个关节的末端位置，先求出每个关节坐标系相对于基坐标系的转换矩阵，再利用每个关节的坐标系通过空间两点距离公式求解出相邻关节之间对应驱动绳的绳长，即解决关节空间与驱动空间的映射关系，具体是：

摇杆通过串口传递给matlab的数据是摇杆偏离原点的横纵坐标(X，Z)，是一组二维的数据坐标，它表示的是在第1节关节坐标系下，预轨迹下一刻要偏离的方向，定义一个类似速度的参数Velocity，当摇杆偏离原点的横纵坐标为(X，Z)时，下一时刻的预轨迹方向向量(Vx‘,Vy’,Vz‘)：

其中[V_xV_yV_z]为前一时刻预轨迹方向向量，由上式可以看出，参数Velocity的大小影响着预轨迹改变的速度大小；

假设基坐标系的单位正交基向量为

第一节的坐标系为

则转动副的运动可以理解为先绕着第二根轴旋转α角度，再绕着第一根轴旋转β角度，所述第二根轴与第一根轴正交；该过程描述为以下等式：

假设第2节的正交坐标基为

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

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

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

Images