机器人运动学

1、机械臂的运动学模型

机械臂运动学研究的是机械臂运动,而不考虑产生运动的力。运动学研究机械臂的位置,速度和加速度。机械臂的运动学的研究涉及到的几何和基于时间的内容,特别是各个关节彼此之间的关系以及随时间变化规律。

典型的机械臂由一些串行连接的关节和连杆组成。每个关节具有一个自由度,平移或旋转。对于具有n个关节的机械臂,关节的编号从1到n,有n +1个连杆,编号从0到n。连杆0是机械臂的基础,一般是固定的,连杆n上带有末端执行器。关节i连接连杆i和连杆i-1。

一个连杆可以被视为一个刚体,确定与它相邻的两个关节的坐标轴之间的相对位置。一个连杆可以用两个参数描述,连杆长度和连杆扭转,这两个量定义了与它相关的两个坐标轴在空间的相对位置。而第一连杆和最后一个连杆的参数没有意义,一般选择为0。一个关节用两个参数描述,一是连杆的偏移,是指从一个连杆到下一个连杆沿的关节轴线的距离。二是关节角度,指一个关节相对于下一个关节轴的旋转角度。

为了便于描述的每一个关节的位置,我们在每一个关节设置一个坐标系, 对于一个关节链,Denavit和Hartenberg提出了一种用矩阵表示各个关节之间关系的系统方法。对于转动关节i,规定它的转动平行于坐标轴zi-1,坐标轴xi-1对准从zi-1到zi的法线方向,如果zi-1与zi相交,则xi-1取zi−1 ×zi的方向。连杆,关节参数概括如下:

  1. 连杆长度ai  沿着xi轴从zi-1和zi轴之间的距离;
  2. 连杆扭转αi  从zi-1轴到zi轴相对xi-1轴夹角;
  3. 连杆偏移di 从坐标系i-1的原点沿着zi-1轴到xi轴的距离;
  4. 关节角度θi xi-1轴和xi轴之间关于zi-1轴的夹角。

对于一个转动关节θi是关节变量,di是常数。而移动关节di是可变的,θi是恒定的。为了统一,表示为

 

运用Denavit-Hartenberg(DH)方法,可以将相邻的两个坐标系之间的变换关系表示为一个4x4的齐次变换矩阵

 

上式表示出了坐标系i相对于坐标系i-1的关系。即

其中 表示坐标系i相对于世界坐标系0的位置与姿态,简称位姿。

(1)创建单关节连杆

 输入

>> L = Link([0 10 5 pi/6])

输出

L =

 theta=q, d=10, a=5, alpha=0.5236, offset=0 (R,stdDH)

也可以表达为

>> L = Link([0 10 5 pi/6 0])

>> L = Link([0 10 5 pi/6],'revolute')

 输入

>> L = Link([0 10 5 pi/6 1])

输出

L =

 theta=0, d=q, a=5, alpha=0.5236, offset=0 (P,stdDH)

也可以表达为

>> L = Link([0 10 5 pi/6],'prismatic')

(2)创建多关节连杆

我们以UR5机械臂为例,建立机械臂的运动学模型,UR5机械臂一共有6个转动关节,因此可以被看做6R机械臂,其D-H参数如下

 

输入

>> L(1) = Link([0 0.089549 0 pi/2]);

L(2)  = Link([0 0 0.425 0]);

L(3)  = Link([0 0 0.39225 0]);

L(4)  = Link([0 0.10915 0 pi/2]);

L(5)  = Link([0 0.09465 0 -pi/2]);

L(6)  = Link([0 0.0823 0 0]);

L

输出

L =

 theta=q1, d=0.08955, a=0, alpha=1.571, offset=0 (R,stdDH)

 theta=q2, d=0, a=0.425, alpha=0, offset=0 (R,stdDH)

 theta=q3, d=0, a= 0.3922, alpha=0, offset=0 (R,stdDH)

 theta=q4, d=0.1092, a=0, alpha=1.571, offset=0 (R,stdDH)

 theta=q5, d=0.09465, a=0, alpha= -1.571, offset=0 (R,stdDH)

 theta=q6, d=0.0823, a=0, alpha=0, offset=0 (R,stdDH)

使用SerialLink可以把连杆联系起来形成机械臂

输入

>> UR5_link=SerialLink(L,'name','sixlink')

输出

 

其中的stdDH表示UR5机械臂是在标准D-H参数下建立的模型,grav为重力加速度,通常会默认为在z轴上,为9.8N/kg。

2、正运动学

对于一个n-轴刚性连接的机械臂,正向运动学的解给出的是最后一个连杆坐标系的位置和姿态。重复利用上式,得到

 

机械臂末端位姿在笛卡尔坐标系中有6个自由度,3个平移,3个旋转。所以,一般来说具有6个自由度的机械臂可以使末端实现任意的位姿。

总的机械臂变换 一般简写为Tn,对6个自由度的机械臂简写为T6。对于任意的机械臂,无论其它有多少个关节,具有什么结构,正向运动学解都是可以得到的。

三个两两相互垂直的XYZ轴构成欧几里得空间,存在六个自由度:沿XYZ平移的三个自由度,绕XYZ旋转的三个自由度。在欧几里得空间中任意线性变换都可以通过这六个自由度完成。

Denavit-Hartenberg提出的D-H参数模型能满足机器人学中的最小线性表示约定,用4个参数就能描述坐标变换:绕X轴平移距离a;绕X轴旋转角度alpha;绕Z轴平移距离d;绕Z轴旋转角度theta。

(1)标准D-H(Standard D-H)参数描述

 

以KUKA puma560机械臂为例

其标准D-H参数表如下

 

在MATLAB命令行窗口输入

clear;

clc;

%建立puma 560机器人模型

%       theta    d           a        alpha     offset

%Standard D-H

SL(1)=Link([0 0 0 pi/2        0     ],'standard');

SL(2)=Link([0 0 0.4318 0           0     ],'standard');

SL(3)=Link([0 0.15 0.0203 -pi/2        0     ],'standard');

SL(4)=Link([0 0.4308 0 pi/2        0     ],'standard');

SL(5)=Link([0 0 0 -pi/2        0     ],'standard');

SL(6)=Link([0 0 0 0           0     ],'standard');

L1=SerialLink(SL,'name','puma560-std');

figure(1),teach(L1)

输出

 

下面我们使用机器人工具箱和根据运动学公式两种方法,基于标准D-H参数的方法求解运动学正解

1)主函数

clear;

clc;

%建立puma 560机器人模型

%       theta    d           a        alpha     offset

%Standard D-H

SL(1)=Link([0 0 0 pi/2        0     ],'standard');

SL(2)=Link([0 0 0.4318 0           0     ],'standard');

SL(3)=Link([0 0.15 0.0203 -pi/2        0     ],'standard');

SL(4)=Link([0 0.4308 0 pi/2        0     ],'standard');

SL(5)=Link([0 0 0 -pi/2        0     ],'standard');

SL(6)=Link([0 0 0 0           0     ],'standard');

L1=SerialLink(SL,'name','puma560-std');

Sta_T06=L1.fkine([0,0,pi/2,0,0,pi/2]) %工具箱正解函数

Edite_Sta_T06=function_stafkine(0,0,pi/2,0,0,pi/2)  %编写正解函数

2)根据标准D-H参数下,运动学公式建立的功能主函数(function)

function [T06]= function_stafkine (theta1,theta2,theta3,theta4,theta5,theta6)

SDH=[theta1   0       0   pi/2;

     theta2   0       0.4318    0;

     theta3   0.15       0.0203   -pi/2;

     theta4   0.4308   0        pi/2;

     theta5   0       0       -pi/2;

     theta6   0      0        0];

 T01=

[cos(SDH(1,1)) -sin(SDH(1,1))*cos(SDH(1,4)) sin(SDH(1,1))*sin(SDH(1,4))  SDH(1,3)*cos(SDH(1,1));

    sin(SDH(1,1))  cos(SDH(1,1))*cos(SDH(1,4))  -cos(SDH(1,1))*sin(SDH(1,4))    SDH(1,3)*sin(SDH(1,1));

      0               sin(SDH(1,4))                 cos(SDH(1,4))                  SDH(1,2);

      0               0                             0                              1];

 T12=[cos(SDH(2,1))  -sin(SDH(2,1))*cos(SDH(2,4))   sin(SDH(2,1))*sin(SDH(2,4))    SDH(2,3)*cos(SDH(2,1));

      sin(SDH(2,1))   cos(SDH(2,1))*cos(SDH(2,4))  -cos(SDH(2,1))*sin(SDH(2,4))    SDH(2,3)*sin(SDH(2,1));

      0               sin(SDH(2,4))                 cos(SDH(2,4))                  SDH(2,2);

      0               0                             0                              1];

 T23=[cos(SDH(3,1))  -sin(SDH(3,1))*cos(SDH(3,4))   sin(SDH(3,1))*sin(SDH(3,4))    SDH(3,3)*cos(SDH(3,1));

      sin(SDH(3,1))   cos(SDH(3,1))*cos(SDH(3,4))  -cos(SDH(3,1))*sin(SDH(3,4))    SDH(3,3)*sin(SDH(3,1));

      0               sin(SDH(3,4))                 cos(SDH(3,4))                  SDH(3,2);

      0               0                             0                              1];

 T34=[cos(SDH(4,1))  -sin(SDH(4,1))*cos(SDH(4,4))   sin(SDH(4,1))*sin(SDH(4,4))    SDH(4,3)*cos(SDH(4,1));

      sin(SDH(4,1))   cos(SDH(4,1))*cos(SDH(4,4))  -cos(SDH(4,1))*sin(SDH(4,4))    SDH(4,3)*sin(SDH(4,1));

      0               sin(SDH(4,4))                 cos(SDH(4,4))                  SDH(4,2);

      0               0                             0                              1];

 T45=[cos(SDH(5,1))  -sin(SDH(5,1))*cos(SDH(5,4))   sin(SDH(5,1))*sin(SDH(5,4))    SDH(5,3)*cos(SDH(5,1));

      sin(SDH(5,1))   cos(SDH(5,1))*cos(SDH(5,4))  -cos(SDH(5,1))*sin(SDH(5,4))    SDH(5,3)*sin(SDH(5,1));

      0               sin(SDH(5,4))                 cos(SDH(5,4))                  SDH(5,2);

      0               0                             0                              1];

 T56=[cos(SDH(6,1))  -sin(SDH(6,1))*cos(SDH(6,4))   sin(SDH(6,1))*sin(SDH(6,4))    SDH(6,3)*cos(SDH(6,1));

      sin(SDH(6,1))  cos(SDH(6,1))*cos(SDH(6,4)) -cos(SDH(6,1))*sin(SDH(6,4))    SDH(6,3)*sin(SDH(6,1)); 0       sin(SDH(6,4))   cos(SDH(6,4))   SDH(6,2);

      0    0   0         1];

T06=T01*T12*T23*T34*T45*T56;

输出结果为

Sta_T06 =

   -0.0000   -0.0000   -1.0000    0.0010

    1.0000         0   -0.0000   -0.1500

         0   -1.0000    0.0000    0.0203

         0         0         0    1.0000

Edite_Sta_T06 =

   -0.0000   -0.0000   -1.0000    0.0010

    1.0000         0   -0.0000   -0.1500

         0   -1.0000    0.0000    0.0203

         0         0         0    1.0000

(2)改进D-H(Modified D-H)参数描述

 

Puma560机器人其改进D-H参数表如下

 

在MATLAB命令行窗口输入

clear;

clc;

%建立puma 560机器人模型

%       theta    d           a        alpha     offset

%Modified D-H

ML(1)=Link([0      0          0           0         0     ],'modified');

ML(2)=Link([0   0.2435        0      -pi/2       0     ],'modified');

ML(3)=Link([0  -0.0934        0.4318       0          0     ],'modified');

ML(4)=Link([0    0.4331       -0.0203      pi/2       0     ],'modified');

ML(5)=Link([0      0          0           -pi/2       0     ],'modified');

ML(6)=Link([0       0           0          pi/2       0     ],'modified');

L2=SerialLink(ML,'name','puma560-mod');

figure(2),teach(L2)

输出

 

可以看出末端位置,不完全一致,因为改进DH并没有建到工具坐标系,再乘一个纯平移的变换即可。

将平移变化添加到改进D-H模型中,即输入

clear;

clc;

%建立puma 560机器人模型

%       theta    d           a        alpha     offset

%Modified D-H

ML(1)=Link([0      0          0           0         0     ],'modified');

ML(2)=Link([0   0.2435        0      -pi/2       0     ],'modified');

ML(3)=Link([0  -0.0934        0.4318       0          0     ],'modified');

ML(4)=Link([0    0.4331       -0.0203      pi/2       0     ],'modified');

ML(5)=Link([0      0          0           -pi/2       0     ],'modified');

ML(6)=Link([0    -0.02      0.0406          pi/2       0     ],'modified');

L2=SerialLink(ML,'name','puma560-mod');

figure(2),teach(L2)

输出

 

下面我们使用机器人工具箱和根据运动学公式两种方法,基于改进D-H参数的方法求解运动学正解

1)主函数

clear;

clc;

%建立puma 560机器人模型

%       theta    d           a        alpha     offset

%Modified D-H

ML(1)=Link([0      0          0           0         0     ],'modified');

ML(2)=Link([0   0.2435        0      -pi/2       0     ],'modified');

ML(3)=Link([0  -0.0934        0.4318       0          0     ],'modified');

ML(4)=Link([0    0.4331       -0.0203      pi/2       0     ],'modified');

ML(5)=Link([0      0          0           -pi/2       0     ],'modified');

ML(6)=Link([0    -0.02      0.0406          pi/2       0     ],'modified');

L2=SerialLink(ML,'name','puma560-mod');

Mod_T06=L2.fkine([0,0,pi/2,0,0,pi/2]) %工具箱正解函数

Edite_Mod_T06=function_modfkine(0,0,pi/2,0,0,pi/2)  %编写正解函数

2)根据改进D-H参数下,运动学公式建立的功能主函数(function)

function [T06]=function_modfkine(theta1,theta2,theta3,theta4,theta5,theta6)

MDH=[theta1   0       0         0;

     theta2   0       0.180    -pi/2;

     theta3   0       0.600     0;

     theta4   0.630   0.130    -pi/2;

     theta5   0       0         pi/2;

     theta6   0       0        -pi/2];

T01=[cos(MDH(1,1))                 -sin(MDH(1,1))                 0               MDH(1,3);

     cos(MDH(1,4))*sin(MDH(1,1))    cos(MDH(1,4))*cos(MDH(1,1))  -sin(MDH(1,4))  -MDH(1,2)*sin(MDH(1,4));

     sin(MDH(1,4))*sin(MDH(1,1))    sin(MDH(1,4))*cos(MDH(1,1))   cos(MDH(1,4))   MDH(1,2)*cos(MDH(1,4));

     0                              0                             0               1];

T12=[cos(MDH(2,1))                 -sin(MDH(2,1))                 0               MDH(2,3);

     cos(MDH(2,4))*sin(MDH(2,1))    cos(MDH(2,4))*cos(MDH(2,1))  -sin(MDH(2,4))  -MDH(2,2)*sin(MDH(2,4));

     sin(MDH(2,4))*sin(MDH(2,1))    sin(MDH(2,4))*cos(MDH(2,1))   cos(MDH(2,4))   MDH(2,2)*cos(MDH(2,4));

     0                              0                             0               1];

T23=[cos(MDH(3,1))                 -sin(MDH(3,1))                 0               MDH(3,3);

     cos(MDH(3,4))*sin(MDH(3,1))    cos(MDH(3,4))*cos(MDH(3,1))  -sin(MDH(3,4))  -MDH(3,2)*sin(MDH(3,4));

     sin(MDH(3,4))*sin(MDH(3,1))    sin(MDH(3,4))*cos(MDH(3,1))   cos(MDH(3,4))   MDH(3,2)*cos(MDH(3,4));

     0                              0                             0               1];

T34=[cos(MDH(4,1))                 -sin(MDH(4,1))                 0               MDH(4,3);

     cos(MDH(4,4))*sin(MDH(4,1))    cos(MDH(4,4))*cos(MDH(4,1))  -sin(MDH(4,4))  -MDH(4,2)*sin(MDH(4,4));

     sin(MDH(4,4))*sin(MDH(4,1))    sin(MDH(4,4))*cos(MDH(4,1))   cos(MDH(4,4))   MDH(4,2)*cos(MDH(4,4));

     0                              0                             0               1];

T45=[cos(MDH(5,1))                 -sin(MDH(5,1))                 0               MDH(5,3);

     cos(MDH(5,4))*sin(MDH(5,1))    cos(MDH(5,4))*cos(MDH(5,1))  -sin(MDH(5,4))  -MDH(5,2)*sin(MDH(5,4));

     sin(MDH(5,4))*sin(MDH(5,1))    sin(MDH(5,4))*cos(MDH(5,1))   cos(MDH(5,4))   MDH(5,2)*cos(MDH(5,4));

     0                              0                             0               1];

T56=[cos(MDH(6,1))                 -sin(MDH(6,1))                 0               MDH(6,3);

     cos(MDH(6,4))*sin(MDH(6,1))    cos(MDH(6,4))*cos(MDH(6,1))  -sin(MDH(6,4))  -MDH(6,2)*sin(MDH(6,4));

     sin(MDH(6,4))*sin(MDH(6,1))    sin(MDH(6,4))*cos(MDH(6,1))   cos(MDH(6,4))   MDH(6,2)*cos(MDH(6,4));

     0                              0                             0               1];

T06=T01*T12*T23*T34*T45*T56;

输出

Mod_T06 =

   -0.0000   -0.0000    1.0000    0.8449

    1.0000         0    0.0000    0.1501

         0    1.0000    0.0000   -0.0203

         0         0         0    1.0000

Edite_Mod_T06 =

   -0.0000   -0.0000    1.0000    0.8649

    1.0000         0    0.0000    0.1501

         0    1.0000    0.0000   -0.0203

         0         0         0    1.0000

3、逆运动学

在机械臂的路径规划中,用到的是逆运动学的解 ,它给出了特定的末端位姿对应的机械臂的关节角度。一般来说,逆运动学的解不是唯一的,对具有某种结构的机械臂,封闭解可能不存在。

  对于6自由度的机器人而言,运动学逆解非常复杂,一般没有封闭解。只有在某些特殊情况下才可能得到封闭解。不过,大多数工业机器人都满足封闭解的两个充分条件之一(Pieper准则)

(1)三个相邻关节轴交于一点

(2)三个相邻关节轴相互平行

如果机械臂多于6个关节,称关节为冗余的,这时解是存在多解。如果对于机械臂某个特别的位姿,解不存在,称这个位姿为奇异位姿。机械臂的奇异性可能是由于机械臂中某些坐标轴的重合,或位置不能达到引起的。

机械臂的奇异位姿分为两类:

(1)边界奇异位姿,当机械臂的关节全部展开或折起时,使得末端处于操作空间的边界或边界附近,雅克比矩阵奇异,机械臂的运动受到物理结构的约束,这时机械臂的奇异位姿称为边界奇异位姿。

(2)内部奇异位姿,两个或两个以上的关节轴线重合时,机械臂各个关节的运动相互抵消,不产生操作运动,这时机械臂的奇异位姿称为内部奇异位姿。

机械臂运动学逆解的方法可以分为两类:封闭解和数值解、在进行逆解时总是力求得到封闭解。因为封闭解的计算速度快,效率高,便于实时控制。而数值解法不具有这些特点。机械臂运动学的封闭逆解可通过两种途径得到:代数法和几何法。

一般而言,非零连杆参数越多,到达某一目标的方式也越多,即运动学逆解的数目也越多。

在从多重解中选择解时,应根据具体情况,在避免碰撞的前提下通常按“最短行程”准则来选择。同时还应当兼顾“多移动小关节,少移动大关节”的原则。n个自由度的机械臂的末端位姿由n个关节变量所决定,这n个关节变量统称为n维关节矢量,记为q。所有的关节矢量构成的空间称为关节空间。机械臂末端的位姿用6个变量描述,3个平移(x,y,z)和3个旋转(wx, wy, wz),记x=(x,y,z, wx, wy, wz),x是机械臂末端在基坐标空间中的坐标,所有的矢量x构成的空间称为操作空间或作业定向空间。工作空间是操作臂的末端能够到达的空间范围,即末端能够到达的目标点集合。值得指出的是,工作空间应该严格地区分为两类:

(1) 灵活(工作)空间  指机械臂末端能够以任意方位到达的目标点集合。因此,在灵活空间的每个点上,手爪的指向可任意规定。

(2) 可达(工作)空间  指机械臂末端至少在一个方位上能够到达的目标点集合。

机械臂各关节驱动器的位置组成的矢量称为驱动矢量s,由这些矢量构成的空间称为驱动空间

以puma560机器人为例,求解机器人运动学逆解

clear;

clc;

%建立puma 560机器人模型

%       theta    d           a        alpha     offset

%Standard D-H

SL(1)=Link([0 0 0 pi/2        0     ],'standard');

SL(2)=Link([0 0 0.4318 0           0     ],'standard');

SL(3)=Link([0 0.15 0.0203 -pi/2        0     ],'standard');

SL(4)=Link([0 0.4308 0 pi/2        0     ],'standard');

SL(5)=Link([0 0 0 -pi/2        0     ],'standard');

SL(6)=Link([0 0 0 0           0     ],'standard');

L1=SerialLink(SL,'name','puma560-std');

P=L1.fkine([0,0,pi/2,0,0,pi/2])    %工具箱正解函数

Q=L1.ikine(P)  %ikine逆解函数,根据我们给定的末端位姿P,求解出关节角Q

输出

P =

   -0.0000   -0.0000   -1.0000    0.0010

    1.0000         0   -0.0000   -0.1500

         0   -1.0000    0.0000    0.0203

         0         0         0    1.0000

Q =

0.0000    3.0432    1.6650   -0.4734   -2.7786    1.3967

  • 2
    点赞
  • 28
    收藏
    觉得还不错? 一键收藏
  • 0
    评论

“相关推荐”对你有帮助么?

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值