CN106956260A - 一种多关节机械臂平面蛇形轨迹运动的逆运动学求解方法 - Google Patents

Abstract

本发明提供了一种多关节机械臂平面蛇形轨迹运动的逆运动学求解方法。该轨迹包含三种不同过程的轨迹，第一阶段为基座配合机械臂向前按照预定轨迹避开障碍到达指定位置；第二阶段为基座固定，机械臂在指定位置完成上下移动完成相应的工作；第三阶段为机械臂完成相应工作后，基座配合机械臂收回到最初位置。在各个阶段中，在给定机械臂末端位置和姿态的情况下，推导出该机械臂各个关节位置的几何表达式，再通过一系列化简求解的方法，求得该多关节机械臂按给定的轨迹运动的运动学逆解。本发明能够在给定机械臂各关节参数和所需运动轨迹的情况下迅速的求出运动学逆解，进而对机械臂的运动进行控制。

Description

一种多关节机械臂平面蛇形轨迹运动的逆运动学求解方法

技术领域

本发明提供的是一种多关节机械臂的类似蛇形运动轨迹运动过程的逆运动学求解方法。

背景技术

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

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

现在，已有多种对逆运动学求解的方法。Dubey提出利用拉格朗日乘子求逆解的方法，主要思路是尽力把关节速度控制在容许范围内。K.J.Wardron在运动学优化的基础上提出了一种基于优化几何结构的逆解。印度学者P.Kalra提出了基于遗传算法的逆运动学求解方法。Kim提出了用最优控制的必要条件来求冗余度机器人的逆解。上述方法的思路都是采用某种优化原则缩小逆解的范围，再通过数值方法求解，但是这样的方法计算过程较为复杂，耗时也较长，不能实现对机械臂实时的中快速控制，也不适用于本发明所针对的这种特殊的工作情况。

发明内容

本发明的目的在于提供了一种针对多关节机械臂平面蛇形轨迹运动的逆运动学求解方法，这种蛇形轨迹主要指的是机械臂先在基座配合下绕过各种障碍物到达工作位置，然后在工作位置附近完成工作，最后再在基座的配合下退回到最初位置这一系列运动所形成的轨迹。本发明在给定机械臂的初始姿态和目标轨迹、末端目标位置的条件下提出了一组高效、快速的求解出满足条件并且最为简便一组逆解。

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

(1)输入机械臂的关节参数：测量关节数量n，机械臂的长度、每个关节的最大转角、各关节间间距。

(2)确定机械臂运动的目标轨迹和目标位置：确定基座前进过程中机械臂的运动轨迹，基座固定过程中机械臂末端的目标位置和姿态。

(3)基座前进过程：

使机械臂各关节末端沿着所给定的轨迹y＝f(x)运动时，使第i关节跟随第i+1关节运动。

当基座向前运动距离x_s满足条件0<x_s<l时，只有第n节关节会有绕Z轴的旋转，其他关节仍然沿着X轴做平移运动。在这个过程中，θ₁＝θ₂＝…＝θ_n-1＝0。所以只需要求解t时刻的θ_n的值。而第n关节末端位置可以根据前面各关节的转角θ_i(i＝1,2,…,n-1)和各关节长度l求出：

将其代入轨迹方程y_n(θ_n)-f(x_n(θ_n))＝0中，利用数值方法二分法求解出θ_n的值。

当基座向前运动距离x_s满足条件(k-1)×l<x_s<k×l(k＝2,3,L,n)时，因为第k关节的运动过程是跟随第k+1关节的运动轨迹运动的，所以对θ_n-k+1,…,θ_n-1，有：

θ_j(t)＝θ_j+1(t-t_d)

其中，t_d可以由式求得。再同样通过利用数值方法二分法求解方程y_n(θ_n)-f(x_n(θ_n))＝0来求出θ_n的值，从而完成了整个基座前进过程的逆运动学求解。

(4)基座固定过程

当基座前进至指定工作空间后，基座固定，机械臂开始工作，这时需要针对基座固定时最末端关节的上下位移运动进行逆运动学求解时，为了使控制方便，在能到达指定位置的前提下，尽可能的转动较少的关节，即尽可能的只转动末端关节以及靠近末端的关节，且尽可能使关节转角不会太大。当为了运动到某一位置需要调整第k关节(k＝1,2,…,n-4)到第n关节的位置时，将第n-1、n-2、…、k+1关节看成一个关节，并保持θ_n-1、θ_n-2、…、θ_k+2的值不变，即：

θ_j＝θ_jo,(j＝n-1,n-2,…,k+2)

其中θ_jo表示各关节的初始角度。

所以，问题就简化为了对两关节的逆运动学求解问题。求解出来之后，再根据各关节的几何关节，就能解出θ_k、θ_k+1和θ_n的值。所以就得到了整个机械臂的逆运动学解。

(5)基座后退过程

在机械臂完成的指定的任务后，通常需要将机械臂收回到最初始的状态，这个过程是伴随着基座的后退运动完成的。机械臂初始的位置称为机械臂的初始轨迹。为了使机械臂在后退收回过程中避开障碍物，使机械臂各关节的末端在后退都过程中都始终保持在初始轨迹上运动。

当基座后退距离x_s满足条件0<x_s<l时，第一关节末端位置(x₁,y₁)应始终在第一关节在坐标系中的初始位置形成的轨迹方程y₁＝k₁x₁+b₁上，即：

y₀+l sinθ₁＝k₁(x₀+l cosθ₁)+b₁

取：

可以求得：

再用相同的方法，可以求得：

当基座后退距离x_s满足条件(k-1)×l<x_s<k×l(k＝2,3,L,n)时，因为第i关节是跟随第i+1关节的运动，所以有：

本发明的有益效果是：能够在已知各关节的初始姿态和目标轨迹和目标位置的条件下快速的求解出各关节转角的组合即运动学逆解，并选择出满足运动条件的解。能够直接适用于带有基座的多关节柔性机械臂的各种不同类型的运动。

附图说明

图1是多关节柔性机械臂模型示意图；

图2是基座前进运动阶段示意图(1)；

图3是基座前进运动阶段示意图(2)；

图4是基座固定阶段几何示意图(1)；

图5是基座固定阶段几何示意图(2)；

图6是matlab仿真过程图；

图7是matlab仿真过程图。

具体实施方式

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

(1)对于这种多关节机械臂平面蛇形轨迹运动的逆运动学求解，先要测量出该机械臂的关节长度，关节数量n，每个关节的最大转角，机械臂关节之间的间距，取机械臂的一个关节长度与该关节与前一关节间距和为l，并获取给定的机械臂运动轨迹。

(2)机械臂基座前进运动过程：

假设基座以速度v运动，机械臂初始位置如图1所示，需要通过调节关节转角使各个关节末端都处于给定轨迹上。根据基座位移x_s的不同可以把这一阶段分为n个阶段，其中p_i(x_i,y_i)表示第i节关节末端在直角坐标系中的位置。p₀(x₀,y₀)表示基座位置。

当0<x_s<l时，如图2所示，只有第n节关节会有绕Z轴的旋转。其他关节仍然沿着X轴做平移运动，在这个过程中，θ₁＝θ₂＝L＝θ_n-1＝0。所以只需要求解t时刻的θ_n的值。

由已知条件易得：

由已知条件求得前n-1节末端位置，即上式i＝n时：

由于θ₁θ₂Lθ_n-1的值都是已知的，所以只需求解θ_n的值。由于第n关节末端应保持在给定轨迹上运动，所以第n关节位置p_n应该满足方程：y_n＝f(x_n)。代入上式方程可以得到方程：

方程中只有一个未知数θ_n，所以可以利用二分法求解出θ_n的值。

令：G(θ_n)＝y_n(θ_n)-f(x_n(θ_n))＝0 (5)

当(k-1)×l<x_s<k×l(k＝2,3,…,n)时，如图3所示，第n,n-1，…，n-(k-1)节关节会有绕Z轴的旋转运动，其他关节仍然沿着X轴做平移运动，在这个过程中，θ₁＝θ₂＝…＝θ_n-k＝0。所以只需要求解t时刻的θ_n-k+1,…,θ_n的值。

因为在第1阶段运动过程中，第k关节的运动过程是跟随第k+1关节的运动轨迹运动的，所以对θ_n-k+1,…,θ_n-1，有：

θ_j(t)＝θ_j+1(t-t_d) (6)

其中t_d可以由式求得。

利用式(6)求解出θ₁、θ₂、…、θ_n-1后，再通过用二分法求解式(5)来求解出θ_n的值。

(3)机械臂基座固定运动过程

假设机械手末端初始位置为p_io(x_io,y_io),(i＝1,2,…,n)，基座位置为p₀(x₀,y₀)，各关节初始角度为θ_io(i＝1,2,…,n)。末端目标位置为p_n(x_n,y_n)。为了使机械手更平稳的工作，在机械手上下平移运动时，需要保持最后一节与水平方向即X轴平行，即

在上下平移运动过程中，尽量在满足所需要求的情况下使各关节转角尽量的小，从而更方便进行后续的操作。同时，尽量进行较少的调整，即在满足所需要求的情况下尽可能转动较少的靠近末端的关节。

当只需调整第n-2到第n关节的位置时，由于需要保持最后一节水平，所以第n-1关节末端位置为：

由于只需要移动第n-2，n-1，n三个关节，所以有：

因为由上述条件可得：

所以接下来要求得该过程运动学逆解的话，只需要求解θ_n-2和θ_n-1就行了。

由已知条件，可以得到方程：

将两式平方相加，再利用三角和公式：

可以得到：

(x_n-1-x_n-3)²+(y_n-1-y_n-3)²＝2l²+2l²cosθ_n-1 (12)

由上式求解cosθ_n-1，得到：

上式有解的条件是右边的值必须在-1到1之间。如果上式解满足条件，则有：

最后可以求得：

上式求得的解是有正解和负解的，这里先将两组解的结果都求解出来后再对两组解进行取舍。取θ_12…(n-2)＝θ₁+θ₂+…+θ_n-2，式(10)就可以改写为：

令：

可以将式(16)写成：

由式(17)可以求得：

所以：

由式(14)可知，求得的θ_n-2和θ_n-1有两组解，所以需要选择优解。在能到达指定位置的情况下，希望各个关节的变动越小越好，所以选取与θ_(n-2)o的值更接近的一个解作为要选择的解。

当需要调整第n-3到第n关节的位置时：

在一些情况下，如果只调整的n-2到第n个关节的位置，由于每个关节转角都有一定的限制，即有最大转角θ_max，不能到达指定位置，此时需要调整更多关节的位置才能满足要求。

当只调整第n-2、n-1、n关节不能到达指定位置时，多数是因为θ_n-2或θ_n的值到达了上限或者下限。此时，各个关节末端初始位置为p_io(x_io,y_io),(i＝1,2,3,4,5)，基座位置为p₀(x₀,y₀)，各关节初始角度为θ_io(i＝1,2,3,4,5)。末端目标位置为p_n(x_n,y_n)。仍有：

同时将第n-2关节和第n-1关节看成一个关节，保持θ_n-1的值不变，即：

θ_n-1＝θ_(n-1)o (23)

关节长度l_(n-2)(n-1)为：

这样同理，由于要满足式(9)，这样只需要求解θ_n-2、θ_n-1的值。问题就可以转化为：

其中，θ_12…(n-3)＝θ₁+θ₂+…+θ_n-3，θ′_(n-2)(n-1)为第n-3关节末端与第n-1关节末端连线与第n-3关节的转角。用公式(10)到(19)的方法，可以求解出θ_12…(n-3)和θ′_(n-2)(n-1)的值。

所以由上式可求得：

由图4所示关系可以求得θ_n-2的值：

再由式(1-9)可以求得：

需要调整第k(i＝1,2,…,n-4)到第n关节的位置时：

当调整第n-1节关节不能到达指定位置时，需要调节更多关节。各个关节末端位置为p_io(x_io,y_io),(i＝1,2,…,n)，基座位置为p₀(x₀,y₀)，目标位置为各关节初始角度为θ_io(i＝1,2,…,n)。末端目标位置为p_n(x_n,y_n)。

已知：

θ_j＝θ_jo,(j＝1,2,…,k-1) (28)

将第n-1、n-2、…、k+1关节看成一个关节，保持θ_n-1、θ_n-2、…、θ_k+2的值不变，即：

θ_j＝θ_jo,(j＝n-1,n-2,…,k+2) (29)

关节长度l_{(k+1)…(n-1)}为：

同样θ_n可以由式(9)求出，所以需要求解的是θ_k、θ_k+1两个角。这个问题可以转化为：

其中，θ′_{(k+1)…(n-1)}为第k关节末端与第n-1关节末端连线与第k关节的夹角。使用上述的方法，可以求解出θ_12…k和θ′_{(k+1)…(n-1)}的值。则有：

假设θ′_{(k+1)-(k+1)…(n-1)}为第k关节末端与第n-1关节末端连线与第k+1关节的夹角，l_{(k+2)…(n-1)}为第k+2关节末端到第n-1关节末端的距离，其值为：

则有：

由图5所示，θ′_{(k+1)-(k+1)…(n-1)}、θ′_{(k+1)…(n-1)}、θ_k+1三角度几何关系可知：

θ_k+1＝θ′_{(k+1)…(n-1)}-θ′_{(k+1)-(k+1)…(n-1)}或θ_k+1＝θ′_{(k+1)…(n-1)}+θ′_{(k+1)-(k+1)…(n-1)}(35)

由于θ₂产生了两个解，所以分别将两个解代入求出第n关节的末端位置p′_n(x′_n,y′_n)与目标位置p_n(x_n,y_n)比较，得到所需的θ_i+1的值。

再由式(9)可以求得：θ_n＝-(θ₁+θ₂+…+θ_n-1)

(4)机械臂基座后退过程：

在机械手完成相应工作后，需要返回初始位置。在进行这一阶段的控制时，机械手各关节末端初始位置为p_io(x_io,y_io),(i＝1,2,…,n)，基座位置为p₀(x_0o,y_0o)，各关节初始角度为θ_io(i＝1,2,…,n)，各关节末端之间的连线构成机械手的初始轨迹。由已知位置求出机械手的初始轨迹：

y＝k_ix+b_i(x_(i-1)o≤x<x_io),(x＝1,2,…n) (39)

其中，

b_i＝y_i-k_ix_i,(i＝1,2,…,n) (41)

在返回过程中，基座以速度v(t)沿Y轴负方向后退，运动过程中始终保持各关节末端在机械手初始轨迹上运动。现在根据基座位移x_s将该过程分为n个不同的阶段，其中，在回收运动过程中，基座位置为：

各关节末端位置由式(2)可得：

当0<x_s<l时，各关节末端始终在初始轨迹上，对第1关节末端位置有：

y₁＝k₁x₁+b₁ (43)

将(2)代入上式，得：

y₀+l sinθ₁＝k₁(x₀+l cosθ₁)+b₁ (44)

因为该方程不便于直接求解，用单一变量u对上式进行以下变换：

对形式为c sinθ+d cosθ＝e的方程，将式(45)代入，且两边同时乘以1+u²，化简得：

(c+e)u²-2du+(e-c)＝0 (46)

求解得：

因此：

将式(44)化为c sinθ+d cosθ＝e的形式，即：

l sinθ₁-k₁l cosθ₁＝k₁x₀+b₁-y₀ (49)

取：

所以根据(48)有：

再将求得的两个解中选取出符合条件的一个解。

同理，对第i关节末端位置有：

y_i＝k_ix_i+b_i,(i＝2,3,…,n) (52)

即：

移项可得：

取：

所以根据式(48)有：

再从得到的两个解中选取出符合条件的解。

当(k-1)×l<x_s<k×l(k＝2,3,…,n)时，回收过程是跟随前一段过程运动的过程，所以有：

其中t_d可以由式求得。于是便求解出了整个收回过程中各关节转角θ的值。

最后，对上述的过程进行了matlab仿真，验证了该逆运动学求解方法的正确性。验证时取v＝1，l＝0.3，f(x)＝0.3sin(4x)，θ_max＝π/3，e＝10^-6，n＝5，初始时θ_i＝0(i＝1,2,3,4,5)，第5关节末端坐标x₅＝0,y₅＝0，即图1所示。t＝2s时，机械臂处于向前运动过程，如图6所示。t＝5s时，机械臂处于基座固定运动阶段，如图7所示。

Claims (1)

1.一种多关节机械臂平面蛇形轨迹运动的逆运动学求解方法，其特征在于该方法具体是：

输入机械臂的关节参数：测量关节数量n，机械臂的长度、每个关节的最大转角和各关节间间距；

确定机械臂运动的目标轨迹和目标位置：确定基座前进过程中机械臂的运动轨迹，基座固定过程中机械臂末端的目标位置和姿态；

基座前进过程：

机械臂各关节末端沿着所给定的轨迹y＝f(x)运动时，使第i关节跟随第i+1关节运动；

当基座向前运动距离x_s满足条件0<x_s<l时，只有第n节关节会有绕Z轴的旋转，其他关节仍然沿着X轴做平移运动；在这个过程中，θ₁＝θ₂＝…＝θ_n-1＝0；求解t时刻的θ_n的值；第n关节末端位置根据前面各关节的转角θ_i和各关节长度l求出，i＝1,2,…,n-1：

$\left\{\begin{array}{c}{x}_n=x_{n-1}+lcos\left(\underset{j=1}{\overset{n}{\mathrm{\&Sigma;}}}{\mathrm{\&theta;}}_j\right)\\ y_n=y_{n-1}+lsin\left(\underset{j=1}{\overset{n}{\mathrm{\&Sigma;}}}{\mathrm{\&theta;}}_j\right)\end{array}\right.$

将其代入轨迹方程y_n(θ_n)-f(x_n(θ_n))＝0中，利用数值方法二分法求解出θ_n的值；

当基座向前运动距离x_s满足条件(k-1)×l<x_s<k×l时，k＝2,3,…,n，对θ_n-k+1,…,θ_n-1，有：θ_j(t)＝θ_j+1(t-t_d)

其中，t_d由式求得，v(t)表示基座的运动速度；再同样通过利用数值方法二分法求解方程y_n(θ_n)-f(x_n(θ_n))＝0来求出θ_n的值，从而完成了整个基座前进过程的逆运动学求解；

基座固定过程：

当基座前进至指定工作空间后，基座固定，机械臂开始工作，这时需要针对基座固定时最末端关节的上下位移运动进行逆运动学求解，当为了运动到某一位置需要调整第k关节到第n关节的位置时，将第n-1、n-2、…、k+1关节看成一个关节，并保持θ_n-1、θ_n-2、…、θ_k+2的值不变，即：θ_j＝θ_jo，其中θ_jo表示各关节的初始角度；

问题就简化为了对两关节的逆运动学求解问题；求解出来之后，再根据各关节的几何关节，就能解出θ_k、θ_k+1和θ_n的值；就得到了整个机械臂的逆运动学解；

基座后退过程

当基座后退距离x_s满足条件0<x_s<l时，第一关节末端位置(x₁,y₁)应始终在第一关节在坐标系中的初始位置形成的轨迹方程y₁＝k₁x₁+b₁上，即：y₀+lsinθ₁＝k₁(x₀+lcosθ₁)+b₁

取：

$\left\{\begin{array}{c}{c}_1=l\\ d_1=-k_{1}l\\ e_1=k_1{x}_0+b_1-y_0\end{array}\right.$

求得：

${\mathrm{\&theta;}}_1=2{\tan }^{-1}\left(\frac{d_1\&PlusMinus;\sqrt{{d_1}^2+{c_1}^2-{e_1}^2}}{c_1+e_1}\right)$

再用相同的方式，求得：

${\mathrm{\&theta;}}_i=2{\tan }^{-1}\left(\frac{d_i\&PlusMinus;\sqrt{{d_i}^2+{c_i}^2-{e_i}^2}}{c_i+e_i}\right)-\underset{j=1}{\overset{i-1}{\&Sigma;}}{\mathrm{\&theta;}}_j$

当基座后退距离x_s满足条件(k-1)×l<x_s<k×l时，因为第i关节是跟随第i+1关节的运动，所以有：

${\mathrm{\&theta;}}_j\left(t\right)=\left\{\begin{array}{cc}{\mathrm{\&theta;}}_{j-1}(t-t_d),& (j=k,k+1,...,n)\\ 0,& (j=1,...,k-2,k-1)\end{array}\right..$