发布时间 : 星期六 文章六自由度机械手运动控制更新完毕开始阅读7cdbd00e76c66137ee06192c
求解运动方程，即求得机械手各关节坐标，这对机械手的控制是至关重要的。根据
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，据此