六自由度机械手运动控制 下载本文

求解运动方程,即求得机械手各关节坐标,这对机械手的控制是至关重要的。根据

T6我们知道机

器人的机械手要移动到什么地方,而且我们需要获得各关节的坐标值,以便进行这一移动。求解各关节的坐标,需要有直觉知识,这是将要遇到的一个最困难的问题。只已知机械手的姿态,没有一种算法能够求得解答。几何设置对于引导求解是必需的。 2.2.1 欧拉变换解 1、基本隐式方程的解 首先令

Euler (φ,θ,ψ)=T (2-22) 由式2-2知式中,

Euler( φ,θ,ψ)=Rot( z, φ)Rot (y, θ)Rot (z,ψ)

已知任一变换T,要求得φ,θ和ψ。也就是说,如果已知T矩阵各元的数值,那么其所对应的φ,

θ和ψ值是什么?

由式2-3和2-22,我们有下式

?nx??ny?nz?0 ?oxoyoz0axayaz0px??py??pz??1???s?s???c?c?c???s?s???s?c?c???s?c?0??c?c??s?s?c??s?s?s0?s?c?c?c0?c0??0c??s??0s??s?1 ? (2-23)

令矩阵方程两边各对应元素一一相等,可得16个方程式,其中有12个为隐式方程。我们将从这些隐式方程求得所需解答。在式(2-23)中,只有9个隐式方程,因为其平移坐标也是明显解。这些隐式方程如下

nx?c?c?c??s?s?ny?s?c?c??s?s? (2-24) (2-25)

nz??s?c? (2-26)

(2-27) (2-28)

ox??c?c?s??s?c?oy??s?c?s??c?c?oz?s?s? (2-29) (2-30) (2-31)

ax?c??say?s?s?az?c? (2-32)

2、用双变量反正切函数确定角度

可以试探地对φ,θ和ψ进行如下求解。据式2-32得

据式2-30和式2-33有 又据式2-26和2-33有

??arccos(az) (2-33)

??arccos(ax/s?) (2-34)

??arccos(?nz/s?) (2-35)

但是,这些解答是无用的,因为

(1)当由余弦函数求角度时,不仅此角度的符号是不确定的,而且所求角度的准确程度又与该角度本身有关,即cos(?)=cos(??)以及

dcos(?)/d?|90,180?0。

(2)在求解φ和ψ时,见式2-34和2-35,我们再次用到反余弦函数,而且除式的分母为sinθ。这样,当sinθ接近于0时,总会产生不准确。

(3)当θ=0°或θ=±180°时,式2-34和2-35没有定义。

因此,在求解时,总是采用双变量反正切函数atan2(令atan表示arctan)来确定角度。atan2提供二个自变量,即纵坐标y和横坐标x,见图2-5。当-π≤θ≤π,由atan2反求角度时,同时检查y和x的符号来确定其所在象限。这一函数也能检验什么时候x或y为0,并反求出正确的角度。atan2

的精确程度对其整个定义域都是一样的。 图2-5 反正切函数atan2 3、用显式方程求各角度

要求得方程式的解,采用另一种通常能够导致显式解答的方法。用未知逆变换依次左乘已知方程,对于欧拉变换有

?1Rot(z,?)T?Rot(y,?)Rot(z,?) (2-36)

?1?1Rot(y,?)Rot(z,?)?Rot(z,?) (2-37)

式2-36的左式为已知变换T和φ的函数,而右式各元素或者为0,或者为常数。令方程式的两边

应元素相等,对于式2-36即有

?c????s??0?0 ??f11(n)??f12(n)?f13(n)??00??c?00??010?001?f11(a)f12(a)f13(a)0s?0oap0??n??c?c??c?s?sxxxx?????noap?s?c00yyyy??????nzozazpz????s?c?s?s?c0?????00010001??? ? (2-38) f11(p)??f12(p)?f13(p)??1?

在计算此方程左式之前,我们用下列形式来表示乘积

f11(o)f12(o)f13(o)0其中,

f11?c?x?s?y,f12??s?x?c?y,f13?z而x,y和z为

f11,f12,f13的各相应分量,例如:

f12(a)??s?ax?c?ayf11(p)?c?px?s?py?f11(n)??f12(n)?f13(n)??0f11(o)f12(o)f13(o)0

于是,我们可把式2-38重写为

f11(a)f12(a)f13(a)0f11(p)??c?c???f12(p)??s???f13(p)??s?c???1??0?c?s?c?s?s?0s?0c?00??0?0??1? (2-39)

检查上式右式可见,

px,py和pz均为0。这是我们所期望的,因为欧拉变换不产生任何平移。此

外,位于第二行第三列的元素也为0。所以可得 上式两边分别加上

f12(a)?0,即

?s?ax?c?ay?0 (2-40)

s?ax,再除以

c?ay可得

tan??s?ay?c?ax

这样,即可以从反正切函数atan2得到 对式2-40两边分别加上

??atan2(ay,ax)?c?ax (2-41)

?c?ay,然后除以,可得

tan??s??ay?c??ax

这时可得式2-40的另一个解为

??atan2(?ay,?ax) (2-42)

式2-41与式2-42两解相差1800。 除非出现

ay和

ax同时为0的情况,我们总能得到式2-40的两个相差1800的解。当

ay和

ax均为

0时,角度?没有定义。这种情况是在机械手臂垂直向上或向下,且?和?两角又对应于同一旋转时出现的,参阅图2-2b。这种情况称退化(degeneracy)。这时,我们任取?=0。

求得?值之后,式2-39左式的所有元素也就随之确定。令左式元素与右边对应元素相等,可得:

s??f11(a)?c??f13(a),或

s??c?ax?s?ay,

c??ax。于是有

??atan2(c?ax?s?ay,ax) (2-43)

当正弦和余弦都确定时,角度?总是惟一确定的,而且不会出现前述角度?那种退化问题。 最后求解角度?。由式2-39有

s??f12(n),c??f12(o),或s???s?nx?c?ny,c???s?ax?c?oy从而得到

??atan2(?s?nx?c?ny,?s?ax?c?oy) (2-44)

概括地说,如果已知一个表示任意旋转的齐次变换,那么就能够确定其等价欧拉角

??atan2(ay,ax),????180

(2-45)

??atan2(c?ax?s?ay,ax)

??atan2(?s?nx?c?ny,?s?ax?c?oy)2.2.2 滚、仰、偏变换解

在分析欧拉变换时,已经知道,只有用显式方程才能求得确定的解答。所以在这里直接从显式方程来求解用滚动、俯仰和偏转表示的变换方程。式(2-4)和(2-5)给出了这些运动方程式。从式(2-4)得

Rot(z,?)?1T?Rot(y,?)Rot(x,?)

?f11(n)??f12(n)?f13(n)?0 ?式中,

f11(o)f12(o)f13(o)0f13

f11(a)f12(a)f13(a)0f11(p)??c???f12(p)??0?f13(p)???s???1??0f12(n)s?s?c?c?s?0s?c??s?c?c?00??0?0??1? (2-46)

f11,f12和的定义同前。令人与式(2-46)右式的对应元素相等,可得

?s?nx?c?ny?0从而得

??atan2(ny,nx) (2-47)

????180 (2-48) 又令式2-45中左右式中的(3,1)及(1,1)元素分别相等,有

?s??nx,

c??c?nx?s?ny于是得

??atan2(?nx,c?nx?s?ny) (2-49)

最后令第(2,3)和(2,2)对应元素分别相等,有

?s???s?ax?c?ayc???s?ox?c?oy,据此