机械臂运动学.

机械臂运动学基础 1、机械臂的运动学模型

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

典型的机械臂由一些串行连接的关节和连杆组成。每个关节具有一个自由度,平移或旋转。对于具有n个关节的机械臂,关节的编号从1到n,有n +1个连杆,编号从0到n。连杆0是机械臂的基础,一般是固定的,连杆n上带有末端执行器。关节i连接连杆i和连杆i-1。一个连杆可以被视为一个刚体,确定与它相邻的两个关节的坐标轴之间的相对位置。一个连杆可以用两个参数描述,连杆长度和连杆扭转,这两个量定义了与它相关的两个坐标轴在空间的相对位置。而第一连杆和最后一个连杆的参数没有意义,一般选择为0。一个关节用两个参数描述,一是连杆的偏移,是指从一个连杆到下一个连杆沿的关节轴线的距离。二是关节角度,指一个关节相对于下一个关节轴的旋转角度。

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

●连杆长度a i沿着x i轴从z i-1和z i轴之间的距离; ●连杆扭转αi从z i-1轴到zi轴相对x i-1轴夹角;

●连杆偏移d i从坐标系i-1的原点沿着z i-1轴到x i轴的距离; ●关节角度θi x i-1轴和x i轴之间关于z i-1轴的夹角。

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

i i i

q d θ⎧=⎨⎩转动关节移动关节

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

1

cos sin cos sin sin cos sin cos cos cos sin sin 0sin cos 0 1i i i i i i i i i i i i i i i i i i

i a a A d θθαθαθθ θαθαθαα--⎡⎤⎢⎥-⎢ ⎥=⎢⎥ ⎢⎥⎣⎦

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

其中0

i T 表示坐标系i 相对于世界坐标系0的位置与姿态,简称位姿。 2、正向和反向运动学

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

01112(n n n T A A A K q -==

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

总的机械臂变换0

n T 一般简写为T n ,对6个自由度的机械臂简写为T 6。对于任意的机械臂, 无论其它有多少个关节,具有什么结构,正向运动学解都是可以得到的。 在机械臂的路径规划中,用到的是反向运动学的解10

(n q K T -=,它给出了特定的末端位姿对应的机械臂的关节角度。一般来说,反向运动学的解不是唯一的,对具有某种结构的机械臂,封闭解可能不存在。

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

(1三个相邻关节轴交于一点 (2三个相邻关节轴相互平行

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

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

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

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

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

一般而言,非零连杆参数越多,到达某一目标的方式也越多,即运动学逆解的数目也越多。在从多重解中选择解时,应根据具体情况,在避免碰撞的前提下通常按“最短行程”准则来选择。同时还应当兼顾“多移动小关节,少移动大关节”的原则。

n个自由度的机械臂的末端位姿由n个关节变量所决定,这n个关节变量统称为n维关节

矢量,记为q 。所有的关节矢量构成的空间称为关节空间。机械臂末端的位姿用6个变量描述,3个平移(x,y,z和3个旋转(ωx , ωy , ωz ,记x=(x,y,z, ωx , ωy , ωz ,x 是机械臂末端在基坐标空间中的坐标,所有的矢量x 构成的空间称为操作空间或作业定向空间。工作空间是操作臂的末端能够到达的空间范围,即末端能够到达的目标点集合。值得指出的是,工作空间应该严格地区分为两类:

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

(2 可达(工作空间 指机械臂末端至少在一个方位上能够到达的目标点集合。 机械臂各关节驱动器的位置组成的矢量称为驱动矢量s ,由这些矢量构成的空间称为驱动空间。

3、Jacobian 矩阵

机械臂的Jacobian 矩阵表示机械臂的操作空间与关节空间之间速度的线性映射关系,对于

一个n 轴的机械臂,机械臂末端在基坐标系中的速度是x

Jq = 其中x 是6个元素的向量。对于6个关节机械臂Jacobian 矩阵是方阵,如果它是可逆的,则可以由机械臂的末端速度求出各个关节的速度。Jacobian 矩阵在机械臂的奇异位姿上是不可逆的。在实际应用中,当机械臂的末端位置接近奇异位置时,Jacobian 矩阵是病态的,可能导致关节速度不能正确地得到。

上式解决的是正速度问题,即已知q 和q

求末端执行器的速度x 。对于逆速度解问题,由上驱动空间 关节空间 工作空间 正向运动学 运动学逆解

式可以得到速度逆解公式为1 q

J x -= ,注意到此时需要求雅可比矩阵的逆,由线性方程组理论知上式对任意的x ,q 都有解的必要条件是雅可比矩阵的秩rank(J=6,这意味着机械臂的自由度数n ≥6。

这也说明了具有冗余自由度的机械臂,在末端位姿固定的条件下,能使关节在一个较大的关节空间的子空间中运动,有效地避开障碍或奇异位姿,并把关节位移在允许范围内,从而具有更大的运动灵活性。

雅可比矩阵可以看成是从关节空间到操作空间运动速度的传动比,同时也可用来表示两空间之间力的传递关系。对于冗余自由度机械臂,其雅可比矩阵是长方矩阵,因J 满秩且方程个数少于未知数个数,所以有无穷多个解,这时,一般是求其中的最小范数解,或采用加权最

小范数解也就是说使T q

Dq 最小的解,其中D 是对称正定加权矩阵。此时的解是使机械臂在能量消耗最小的情况下的解。

这时,逆速度问题便转为:求q 满足1 q J x -= 且使12 T

L q Dq = 最小。实际上等同于求性能指标L 在约束条件1 q

J x -= 下的极值。应用Lagrange 乘子法,以上极值为题的解是111(T T q D J JD J x ---= ,当D =I 时,雅可比矩阵是1(T T J J JJ +-=,称为雅可比矩阵的伪逆。

下面通过一个两自由度的平面机械臂说明雅可比矩阵的特性,根据右图中的几何关系容易求得

1121211121211212111212c c cos(,cos( s s sin(,sin(

x l l c c y l l s s θθθθθθ=+==+⎧⎨ =+==+⎩两边微分后写成矩阵形式 12121

2x x d dx d dy y y θθθθθθ∂∂⎡⎤ ⎢⎥∂∂⎡⎤

⎡⎤⎢⎥=⎢⎥⎢⎥∂∂⎢⎥⎣⎦ ⎣⎦⎢⎥∂∂⎣⎦ 即 11212

2121112122122s s s c c c l l l d dx l l l d dy θθ---⎡⎤⎡⎤

⎡⎤=⎢⎥⎢⎥⎢⎥+⎣⎦⎣⎦⎣⎦

简写成 dx=Jd θ,式中J 就称为机械臂的雅可比(Jacobian 矩阵,它由函数x ,y 的偏微分组成,反映了关节微小位移d θ与机械臂末端微小运动dx 之间的关系。 将两边同除以dt dt 得到:dx/dt=Jd θ/dt,

因此机械臂的雅可比矩阵也可以看做是操作空间中的速度与关

节空间中速度的线性变换。dx/dt 称为末端在操作空间中的广义速度,简称操作速度,d θ/dt 为关节速度。可以看出,雅可比矩阵的每一列表示其它关节不动而某一关节以单位速度运动产生的末端速度。

由1121221211212212s s s c c c l l l J l l l ---⎡⎤ =⎢ ⎥+⎣⎦

可以看出,J 阵的值随末端位置的不同而不同,即θ1和θ2的

改变会导致J 的变化。对于关节空间的某些位姿,机械臂的雅可比矩阵的秩减少,这些位姿

称为机械臂的奇异位姿。上例机械臂雅可比矩阵的行列式为:122det(sin(J l l θ=,当θ2=0°

或θ2=180°时,机械臂的雅可比行列式为0,矩阵的秩为1,这时机械臂处于奇异位姿。机械臂在操作空间的自由度将减少。

如果机械臂的雅可比J 是满秩的方阵,相应的关节速度即可求出,即1 J x θ

-= ,上例平面2R 机械臂的逆雅可比矩阵212212

1

1121211212122 1 l c l s J

l c l c l s l s l l s -⎡⎤= ⎢⎥----⎣⎦

,显然,当θ2趋于0°(或

180°时,机械臂接近奇异位姿,相应的关节速度将趋于无穷大。

为了补偿机器人末端执行器位姿与目标物体之间的误差,以及解决两个不同坐标系之间的微位移关系问题,需要讨论机器人连杆在作微小运动时的位姿变化。

假设一变换的元素是某个变量的函数,对该变换的微分就是该变换矩阵各元素对该变量的偏导数所组成的变换矩阵乘以该变量的微分。例如给定变换T 为:

1112

131421222324313233344142 43

44t t t t t t t t T t t t t t t t t ⎡⎤⎢⎥⎢⎥= ⎢⎥⎢ ⎥⎣⎦

若它的元素是变量x 的函数,则变换T 的微分为:

13111214232122243132333443414244t t t t x x x x t t t t x x x x dT dx t t t t x x x x t t t t x x x x ∂∂∂∂⎡⎤⎢⎥∂∂∂∂⎢⎥∂∂∂∂⎢⎥⎢⎥∂∂∂∂=⎢

⎥∂∂∂∂⎢⎥⎢⎥∂∂∂∂⎢⎥ ∂∂∂∂⎢⎥∂∂∂∂⎣⎦

下面讨论机械臂的微分运动,设机械臂某一连杆相对于基坐标系的位姿为T ,经过微运动后该连杆相对基坐标系的位姿变为T+dT ,若这个微运动是相对于基坐标系(静系进行的(左乘,总可以用微小的平移和旋转来表示,即

(,,(,x y z T dT Trans d d d Rot k d T θ+= 所以有

44(,,(,x y z dT Trans d d d Rot k d I T θ⨯⎡⎤=-⎣⎦

根据齐次变换的对称性,若微运动是相对某个连杆坐标系i (动系进行的(右乘,则T+dT 可以表示为

(,,(,x y z T dT T Trans d d d Rot k d θ+=⋅ 所以有

44(,,(,x y z dT T Trans d d d Rot k d I θ⨯⎡⎤=-⎣⎦ 令44(,,(,x y z Trans d d d Rot k d I θ⨯∆=- 为微分算子,则相对基系有dT=Δ0T ,相对i 系

有dT=T Δi 。这里Δ的下标不同是由于微运动相对不同坐标系进行的。在机械臂运动学中微分变换分为微分平移和微分旋转两类。

微分平移变换与一般平移变换一样,其变换矩阵为:

1000 10(,,0010

001dx dy Trans dx dy dz dz ⎡⎤⎢⎥⎢ ⎥=⎢⎥ ⎢⎥ ⎣⎦

由于微分旋转时θ→0 ,所以sin θ→d θ,cos θ→1将它们代入旋转变换通式中得微分旋转表达式:

1010(,100 1z y z

x y x k d k d k d k d Rot k d k d k d θθθθθθθ-⎡⎤⎢⎥-⎢⎥=⎢⎥-⎢⎥⎣⎦ 于是得到微分算子44(,,(,x y z Trans d d d Rot k d I θ⨯∆=- ,即 0000 0z y z

x y x k d k d dx k d k d dy k d k d dz θθθθθθ-⎡⎤⎢⎥-⎢⎥∆=⎢⎥-⎢⎥⎣⎦ 微分旋转与有限旋转相比,有一些特殊的性质,下面分别说明。

(1微分旋转的无序性,当θ→0 时,有sin θ→d θ,cos θ→1.若令δx=d θx ,δy=d θy ,δz=d θz ,则绕三个坐标轴的微分旋转矩阵分别为

10 00010(,010 00 01x

Rot x x x δδδ⎡⎤⎢⎥-⎢ ⎥ =⎢⎥⎢ ⎥ ⎣⎦1 000100(,0100

001y Rot y y y δδδ⎡⎤⎢⎥⎢ ⎥=⎢⎥-⎢⎥⎣⎦ 100100(,00 100001z z Rot z z δδδ-⎡⎤ ⎢⎥⎢ ⎥=⎢⎥ ⎢ ⎥⎣⎦

略去2次项,得到

1

0010010010(,(,101000010001y y x y x

x Rot x x Rot y y y x y x δδδδδδδδδδδδ⎡⎤⎡⎤ ⎢⎥⎢⎥--⎢ ⎥⎢

⎥∴==⎢⎥⎢⎥--⎢ ⎥⎢⎥⎣⎦⎣⎦ 1 0100010010(,(,10100 001000 1x y y y x

x Rot y y Rot x x y x y x δδδδδδδδδδδδ⎡⎤⎡⎤⎢⎥⎢⎥--⎢ ⎥⎢

⎥==⎢⎥⎢⎥--⎢ ⎥⎢⎥⎣⎦⎣⎦

两者结果相同,可见这里左乘与右乘等效。结论:微分旋转其结果与转动次序无关,这是与有限转动(一般旋转的一个重要区别。

(2微分旋转的可加性,考虑两个微分旋转复合后的效果

1 01 0(,(,(,100 00

1z y z x Rot x x Rot y y Rot z z y x δδδδδδδδδ-⎡⎤ ⎢⎥-⎢ ⎥ =⎢⎥ -⎢ ⎥⎣⎦

若Rot (δx ,δy ,δz 和Rot (δx ’,δy’,δz ’ 表示两个不同的微分旋转,则两次连续转动的结果为:

1(''0'1('

0(,,(',','(''100001z z y y z z x x Rot x y z Rot x y z y y x x δδδδδδδδδδδδδδδδδδ-++⎡ ⎤ ⎢⎥+-+⎢ ⎥=⎢⎥-++⎢ ⎥⎣⎦

上式表明:任意两个微分旋转的结果为绕每个轴转动的元素的代数和,即微分旋转是可加的。 由等效转轴和等效转角与(,(,(,Rot x x Rot y y Rot z z δδδ等效,有

(,(,(,(,Rot k d Rot x x Rot y y Rot z z θδδδ= 1010101010100 100 1z y z x y x k d k d z

y k d k d z x k d k d y x θθδδθθδδθθδδ--⎡⎤⎡⎤ ⎢⎥⎢⎥--⎢⎥⎢ ⎥ =⎢⎥⎢⎥ --⎢⎥⎢⎥⎣⎦⎣⎦

所以有kxd θ=δx , kyd θ=δy , kzd θ=δz ,将它们代入Δ得 00 00 0z y x z x y y x

z d d d δδδδδδ-⎡⎤⎢⎥-⎢⎥∆=⎢⎥-⎢⎥⎣⎦

可见,微分变换由两个部分组成δ微分转动矢量,d 微分平移矢量,合称为微分运动矢量,

可表示为(,,,,,T x y z x y z D d d d δδδ= 例:已知一个坐标系0 01101

00501000001A ⎡⎤⎢⎥⎢ ⎥=⎢⎥ ⎢⎥ ⎣⎦

,相对固定系的微分平移矢量d=[1 0 0.5],微分旋转矢量δ=[0 0.1 0 ],求微分变换dA 。

00 00.110 000000.1000.50 00 00 0z y x z x

y y x

z d d d δδδδδδ-⎡⎤⎡⎤⎢⎥⎢⎥-⎢⎥⎢⎥∆==⎢⎥⎢⎥--⎢⎥⎢⎥⎣⎦⎣⎦ 000.1

10011000.1010

000100500000.1000.50100000.10.50 000010000dA A ⎡⎤⎡⎤⎡⎤ ⎢⎥⎢⎥⎢⎥⎢

⎥⎢⎥⎢⎥∴=∆==⎢⎥⎢⎥⎢⎥ ---⎢⎥⎢⎥⎢⎥ ⎣⎦⎣⎦⎣⎦

下面讨论两坐标系之间的微分关系,设第一个坐标系为i 系,第二个坐标系为j 系不失一般性,假定j 系就是固定的0系。

00 1x

x x x y y y y i z

z z z n o a p n o a p T n o a p ⎡⎤ ⎢⎥⎢⎥=⎢⎥⎢⎥⎣⎦ 因为 00 00 00

0z y dx z x dy y x dz δδδδδδ-⎡⎤⎢⎥-⎢ ⎥ ∆=⎢⎥ -⎢ ⎥⎣⎦,0 00 0i i i i i i i i i i z y dx z

x dy y x dz δδδδδδ-⎡⎤⎢⎥-⎢ ⎥∆=⎢⎥-⎢⎥⎣⎦ 所以0 0i i i T T ∆=∆,0 1

0i i i T T -∆=∆,整理得到

((((((ix ix iy iy iz iz d n p d n d o p d o d a p d a δδδδδδδδδ =⨯+==⨯+==⨯+= 000000(((((((((0000000

x y z x y

z i x y z x y z i x y z x y

z i x y z i x y z i x y

z i n n n p n p n p n dx dx o o o p o p o p o dy dy a a a p a p a p a dz dz n n n x x o o o y y a a a z z δδδδδδ⨯⨯⨯⎡⎤⎡⎤⎡⎤

⎢⎥⎢⎥⎢⎥⨯⨯⨯⎢⎥⎢⎥⎢⎥ ⎢⎥⨯⨯⨯⎢⎥⎢⎥

=⎢⎥⎢⎥⎢⎥⎢⎥⎢⎥⎢⎥⎢⎥⎢⎥⎢⎥ ⎢⎥⎢⎥⎢⎥⎢⎥⎢

⎥⎢⎥⎣⎦⎣⎦⎣⎦ 对于任何三维矢量 p =[p x , p y , p z ],其反对称矩阵s(p 定义为: (00z y z x y x

p p s p p p p p ⎡⎤-⎢⎥=-⎢⎥⎢⎥-⎣ ⎦ 记 x x x i y

y y z z

z n o a R n o a n o a ⎡⎤⎢⎥=⎢⎥⎢⎥⎣⎦ 上式简写成 000 00(0 T T i i i i T

i i d R R s p R δ⎡⎤-⎡⎤=⎢⎥⎢⎥⎣⎦⎣⎦

类似地,任意两坐标系{A}和{B}之间广义速度的坐标变换为: (0B B A B A A A BO B B A A R RS P V V R ωω⎡⎤⎡⎤⎡⎤-=⎢⎥⎢⎥⎢⎥⎣⎦⎣⎦⎣⎦,(0A A

B A B B B AO A

A B B R RS P V V R ωω⎡⎤⎡⎤⎡⎤ -=⎢⎥⎢⎥⎢⎥⎣⎦⎣⎦ ⎣⎦ 例:已知一个坐标系0 01101

00501000001A ⎡⎤⎢⎥⎢ ⎥=⎢⎥

⎢⎥ ⎣⎦

,相对固定系的微分平移矢量d=[1 0 0.5],微分旋转矢量δ=[0 0.1 0 ],求A 系中等价的微分平移矢量d A 和微分旋转矢量δA 。 解:将d=[1 0 0.5] 和δ=[0 0.1 0 ]代入

((((((ix ix iy iy iz iz d n p d n d o p d o d a p d a δδδδδδδδδ =⨯+==⨯+==⨯+= 得到[] []0 0 5.10.100T T

A A d δ==。 4、机械臂轨迹规划

机械臂的轨迹规划可以在关节空间也可以在笛卡尔空间中进行,或者说机械臂轨迹规划是指在关节空间或者笛卡尔空间中研究机械臂轨迹生成方法。简言之,机械臂轨迹规划是运动学逆解的实际应用,它描述了机械臂在空间中的运动路线 。 在知道末端位姿的前提下,通过运动学逆解得到各个关节在相应时刻的转动量或者平移量,合理的规划指的是规划出的 角位移曲线、角速度曲线以及角加速度曲线,可以有效地减少了机械臂在运动过程中的冲击和振动,使机械臂的工作寿命得以延长。

械臂可以分为点到点作业(Point-to-Point Motion 和连续路径作业(Continuous-Path Motion 。点到点的运动指的是机械臂在运动过程中,只要求在某些点上有准确的位置和姿态,相邻的点不做要求。连续运动要求机械臂严格的沿特定的曲线运动。

机械臂的关节角位移变化率比较小,能够有效地防止了机械臂工作时的振动和冲击。机械臂关节角速度和角加速度变化均平顺连续,从而有效避免了机械部件的磨损,能够保证整个机械臂系统的长期、稳定的运行,满足机械臂的工作要求。

5、robotics工具箱中的相关函数

link 建立一个连杆对象,例如对于本次竞赛的机械臂,根据连杆参数得到 L{1}=link([pi/2 0 0 120 0 0]; L{2}=link([pi/2 0 0 0 0 0]; L{3}=link([-pi/2 0 0 140.8 0 pi]; L{4}=link([-pi/2 71.8 0 0 0 pi/2 ]; L{5}=link([+pi/2 71.8 0 0 0 pi]; L{6}=link([-pi/2 0 0 0 0 pi/2]; L{7}=link([0 0 0 129.6 0 0]; robot 建立一个机械臂对象 R= robot(L

noname (7 axis, RRRRRRR

grav = [0.00 0.00 9.81] standard D&H parameters alpha A theta D R/P 1.5708 0 120 R (std 1.5708. 0 0 R (std

-1.5708 0 140.8 R (std -1.5708 71.8 0 R (std 1.5708 71.8 0 R (std -1.5708 0 0 R (std 0 0 129.6 R (std

drivebot 用滑块控制的机械臂图形 drivebot(R,ones(1,7*pi plot 机械臂的图形显示 plot(R,[pi/2 pi/2 0 0 0 0 0] fkine 串联机械臂正向运动学计算 tr =fkine (ROBOT, Q

ROBOT表示机械臂对象,Q机械臂关节坐标值。 tr =fkine (R, [0 0 0 pi/2 0 0 0] tr =

0.0000 -0.0000 1.0000 129.6000 -0.0000 1.0000 0.0000 -0.0000 -1.0000 -0.0000 0.0000 -20.8000 0 0 0 1.0000

ikine串联机械臂逆向运动学计算

q = ikine(ROBOT, T q = ikine(ROBOT, T, Q q = ikine(ROBOT, T, Q, M 输入变量

ROBOT表示机械臂对象,T机械臂末端变换矩阵。

输出变量q机械臂关节的角度(单位是弧度,一般来说逆运动学的解不是唯一的,取决于初始值Q,缺省时是0向量。如果机械臂的自由度(DOF小于6,由于解空间的维数大于机械臂的自由度,这时需要第4个输入量M来确定笛卡尔坐标(手腕对应的坐标系中的哪些量在求解中被忽略。M中有6个元素,分别表示沿着x,y,z方向的平移和相对于x轴,y轴, z轴的旋转,值是0(忽略或1。非零元素的个数应该等于机械臂的自由度。例如,对典型的有5个自由度的机械臂,一般是忽略相对手腕坐标的转动,这时M = [1 1 1 1 1 0]。另外一种用法是

qt = ikine(ROBOT, TG qt = ikine (ROBOT, TG, Q qt = ikine (ROBOT, TG, Q, M 输入变量

ROBOT表示机械臂对象,TG是4x4xN机械臂末端变换矩阵。 输出变量

qt是一组(N个TG对应的关节坐标。一行对应一个输入变换,每一步的初始值取上一步的值。求解使用机械臂Jacobian矩阵的伪逆,这是数值求解方法,对于特定机械臂逆运动学解(如果可能应该尽量使用解析解。但是这种方法可以得到奇异点上的解,零空间中的关节角度可以任取。

q=ikine(R,tr q =

0.0000 0.0000 0.0000 0.7854 -0.0000 -0.7854 0.0000

注意:对于机械臂末端的一个位置与姿态,逆运动学计算不是唯一的,验证 tr=fkine(R,q tr =

0.0000 -0.0000 1.0000 129.6000 -0.0000 1.0000 0.0000 -0.0000 -1.0000 -0.0000 0.0000 -20.8000 0 0 0 1.0000 transl 计算平移变换 tr= transl (X, Y, Z

返回机械臂末端坐标X, Y, Z对应的齐次表换矩阵 tr=transl(129.6,0,20.8 tr =

1.0000 0 0 129.6000 0 1.0000 0 0 0 0 1.0000 20.8000 0 0 0 1.0000

[X Y Z]' = transl(T

返回齐次表换表示中的平移值,作为一个3元素的列向量 xyz=transl(tr' xyz =

129.6000 0 20.8000

ctraj 计算工作空间中两点T0,T1之间的轨迹 tc= ctraj(T0, T1, N tc = ctraj(T0, T1, R

返回从T0到T1笛卡尔坐标系的轨迹TC N表示轨迹中的点数。在第1中情况下,轨迹中的点在T0到T1中等距离分配。在第2中情况下,向量R给出轨迹中每个点的距离,R 中的元素取值为[0 1]。一个轨迹是4x4xN 矩阵,最后一个下标表示点索引。旋转插值使用四元球形线性插值。

tr0=fkine(R,[0 0 0 0 0 0 0] tr0 =

1.0000 -0.0000 -0.0000 -0.0000 0.0000 1.0000 0.0000 0.0000 -0.0000 -0.0000 1.0000 108.8000 0 0 0 1.0000

tr1=fkine(R,[pi/4 pi/6 0 pi/3 0 0 0] tr1 =

0.6124 -0.7071 0.3536 95.6008 0.6124 0.7071 0.3536 95.6008 -0.5000 -0.0000 0.8660 110.3005 0 0 0 1.0000 tc(:,:,1 =

1.0000 0 0 -0.0000 0 1.0000 0 0.0000 0 0 1.0000 108.8000 0 0 0 1.0000 tc(:,:,2 =

0.76 -0.3822 0.2198 47.8004 0.3571 0.9226 0.1458 47.8004 -0.2585 -0.0523 0.96 109.5503 0 0 0 1.0000 tc(:,:,3 =

0.6124 -0.7071 0.3536 95.6008 0.6124 0.7071 0.3536 95.6008 -0.5000 -0.0000 0.8660 110.3005 0 0 0 1.0000

transl(tc ans =

-0.0000 0.0000 108.8000 47.8004 47.8004 109.5503 95.6008 95.6008 110.3005

jtraj 计算关节中两点Q0,Q1之间的轨迹 [Q QD QDD] = jtraj(Q0, Q1, N

[Q QD QDD] = jtraj (Q0, Q1, N, QD0, QD1 [Q QD QDD] = jtraj (Q0, Q1, T

[Q QD QDD] = jtraj (Q0, Q1, T, QD0, QD1

轨迹中的点数是N,或者是一个时间向量T。插值使用7次多项式,边界速度由QD0, QD1指定,缺省时边界速度和加速度为0。

q0=[pi pi pi pi pi pi pi]; q1=[pi pi/2 0 0 0 pi/2 0]; tr0=fkine(R,[pi pi pi pi pi pi pi]; tr1=fkine(R,[pi pi/2 0 0 0 pi/2 0]; [QT,QD,QDD]=jtraj(q0,q1,30; figure

subplot(2,2,1,plot(R,QT

subplot(2,2,2,plot(QT,grid on,

legend('q1','q2','q3','q4','q5','q6','q7','Location', 'NorthWest' subplot(2,2,3,plot(QD,grid on subplot(2,2,4,plot(QDD,grid on %注意:其中有一些曲线重合

jacob0 计算机械臂在基坐标系中Jacobian 矩阵 J = jacob0(ROBOT, Q

tr2jac 计算机械臂在基坐标系中Jacobian 矩阵 J = TR2JAC(T

diff2tr 微分表示转换为齐次变换 tr = diff2tr(D

返回表示微分平移与旋转的齐次变换矩阵,矩阵中包含一个反对称的旋转子矩阵。tr2diff 转换为齐次变换转换为微分表示

D =tr2diff(T D = tr2diff(T1, T2

第一种形式将齐次表换矩阵表示转换为6-元素向量微分表示。

第二种形式返回6-元素向量,表示从T1 到T2的在基坐标系中需要的微分移动。J = jacob0(R, q1

% Jacobian and differential motion demonstration

% A differential motion can be represented by a 6-element vector with elements % [dx dy dz drx dry drz]

% where the first 3 elements are a differential translation, and the last 3 % are a differential rotation. When dealing with infinitisimal rotations, % the order becomes unimportant. The differential motion could be written % in terms of compounded transforms

% transl(dx,dy,dz * trotx(drx * troty(dry * trotz(drz % but a more direct approach is to use the function diff2tr( D = [.1 .2 0 -.2 .1 .1]' diff2tr(D T=fkine(R,q1

% then the differential motion in the second frame would be given by DT = tr2jac(T * D; DQ= pinv(J * DT;

vel = [1 0 0 0 0 0]'; % translational motion in the X direction qvel = pinv(J * vel; ans =

-0.0000 0.0000 -0.0000 0.0039 -0.0000 -0.0039 0.0000

%这是计算工作空间轨迹和求逆解的另外一种方法。但是,如果Jacobian矩阵奇异时%会失效。如果机械臂的自由度大于6,即是冗余的,采用Jacobian矩阵伪逆计算,或% 对Jacobian矩阵进行奇异值分解。

附录 rpy 角与euler 角 (1rpy 角

rpy 角是描述船舶航行时的姿态的一种方法,将船的行驶方向作为Z 轴,则绕Z 轴旋转称为滚动(Roll 角α,将绕Y 轴(与海面平行方向的旋转称为俯仰(Pitch 角β,取X 轴与海面垂直方向,将绕X 轴的旋转称为偏转(Yaw 角γ。机械臂末端的定义类似,故习惯上称为rpy 角。

描述运动坐标系的规则是:首先使运动坐标系的初始方位与固定坐标系重合,将运动坐标系绕固定坐标系X 轴转动γ,再将运动坐标系绕固定坐标系Y 轴转动β,最后将运动坐标系绕固定坐标系Z 轴转动α。因为三次转动都是相对固定坐标系的,所以相应的旋转矩阵

(,,(,(,(,

0000100000010000001000000 010 0100

1rpy rot z rot y rot x c s c s s c c s s c s c γβααβγα αβ βααγγβ β

γγ=-⎡⎤⎡⎤⎡⎤⎢⎥⎢⎥⎢⎥-⎢⎥⎢⎥⎢

⎥=⎢⎥⎢⎥⎢⎥-⎢⎥⎢⎥⎢ ⎥⎣⎦⎣⎦⎣⎦ 将三个矩阵相乘得到 00(,,00

1c c c s s s c c s c s c s s s c c s s c rpy s c s c c αβαβγαγαβγαβ αβγαγ

αβγγβαβφγβγ-⎡⎤⎢⎥-⎢ ⎥=⎢⎥-⎢⎥⎣⎦

它表示绕固定坐标系的三个轴依次旋转得到的旋转矩阵,称为绕固定轴XYZ 旋转的rpy 方法。下面讨论逆问题:从给定的旋转矩阵得到绕固定轴XYZ 旋转的rpy 角γβα。令

00(,,00 1x x x y y y z z z n o a n

o a rpy n o a γβα⎡⎤⎢⎥⎢⎥=⎢⎥⎢⎥⎣⎦

上式中有3个未知数,9个方程,其中6个不,因此可利用其中3个解出未知数。

22

cos(x y n n β=+,如果cos(β不为零,则可以得到 22

a tan 2(,,a tan 2(,,a tan 2(,z x y x x y z n n n o n a a βαγ=-+== (2绕运动系ZYX 转动的euler 角

描述运动坐标系的规则是:运动坐标系的初始方位与参考坐标系重合,首先将运动坐标系绕Z 轴转动α,再将运动坐标系绕Y 轴转动β,最后将运动坐标系绕X 轴转动γ。这种描述方法中各次的转动都是相对运动系的,而不是相对固定坐标系的。相应的旋转矩阵为

(,,(,(,(,

0000100000010000001000000 010 0100

1euler rot x rot y rot z c s c s s c c s s c s c αβγαβγα αβ βααγγβ β

γγ=-⎡⎤⎡⎤⎡⎤⎢⎥⎢⎥⎢⎥-⎢⎥⎢⎥⎢ ⎥=⎢⎥⎢⎥⎢⎥-⎢⎥⎢⎥⎢ ⎥⎣⎦⎣⎦⎣⎦ 将三个矩阵相乘得到 00(,,00

1c c c s s s c c s c s c s s s c c s s c euler s c s c c αβαβγαγαβγαβ

αβγαγ

αβγαβγββγβγ-⎡⎤⎢⎥-⎢ ⎥=⎢⎥-⎢⎥⎣⎦

结果与绕固定轴XYZ 旋转相同,这是因为绕固定轴旋转的顺序与绕运动轴旋转的顺序相反,且旋转角度对应相等。因此,用ZYX euler 角与XYZ rpy 角的描述方法是等价的。 另外一种常用的euler 角方法是ZYZ 方法,首先使运动坐标系与参考坐标系重合,将运动坐标系绕Z 轴转动α,再将运动坐标系绕Y 轴转动β,最后将运动坐标系绕Z 轴转动γ。

(,,(,(,(,

0000000001000000100000100 010 010

01euler rot z rot y rot z c s c s c s s c s c s c αβγαβγα αβ βγγααγγβ β

=--⎡⎤⎡⎤⎡⎤

⎢⎥⎢⎥⎢⎥⎢⎥⎢⎥⎢⎥=⎢⎥⎢⎥⎢⎥ -⎢⎥⎢⎥⎢⎥⎣⎦⎣⎦⎣⎦ 将三个矩阵相乘得到

00(,,000

1c c c s s c s s s c c s s c c c s s s s c c s s euler s c s s c αβγαγαβγαγαβαβγαγ αβγαγ

αβαβγβγβγβ---⎡⎤ ⎢⎥+-+⎢ ⎥=⎢⎥-⎢ ⎥⎣ ⎦

如果sin(β不为零,ZYZ 方式euler 角逆解是 22

a tan 2(,,a tan 2(,,a tan 2(,x y z z Z y x a a a o n a a βαγ=+==-