第三章-工业机器人运动学-3逆运动学课件

第三章工业机器人的运动学-3第三章工业机器人的运动学-31

主要内容

数学基础——齐次坐标变换机器人运动学方程的建立（正运动学）

机器人逆运动学分析（逆运动学）

主要内容数学基础——齐次坐标变换2三、逆运动学方程

（InverseKinematicEquations）3.1引言3.2逆运动学方程的解3.3斯坦福机械手的逆运动学解3.4欧拉变换的逆运动学解3.5RPY变换的逆运动学解3.6球坐标变换的逆运动学解3.7本章小结

三、逆运动学方程

（InverseKinematic33.1引言(Introduction)所谓逆运动学方程的解，就是已知机械手直角坐标空间的位姿（pose）T6，求出各节变量θn

ordn

。

T6=A1A2A3A4A5A6（3.1）逆运动学方程解的步骤如下：（1）根据机械手关节坐标设置确定AnAn为关节坐标的齐次坐标变换，由关节变量和参数确定。关节变量和参数有：

an－连杆长度; αn－连杆扭转角;

dn－相邻两连杆的距离; θn－相邻两连杆的夹角。 对于旋转关节θn为关节变量，而对于滑动关节dn为关节变量。其余为连杆参数，由机械手的几何尺寸和组合形态决定。3.1引言(Introduction)4（2）

根据任务确定机械手的位姿T6

T6为机械手末端在直角坐标系（参考坐标或基坐标）中的位姿，由任务确定，即式（2.37）给出的表达式T6=Z-1XE-1确定。它是由三个平移分量构成的平移矢量P（确定空间位置）和三个旋转矢量n，o，a（确定姿态）组成的齐次变换矩阵描述。（3）由T6和An（n＝1，2，…，6）和式（4.1）求出相应的关节变量θn或dn。

（2）根据任务确定机械手的位姿T653.2逆运动学方程的解（Solvinginversekinematicequations）根据式（3.1）T6=A1A2A3A4A5A6分别用An（n＝1，2，…，5）的逆左乘式（3.1）有A1-1T6=1T6(1T6=A2

A3A4A5A6)（3.2）A2-1A1-1T6=2T6(2T6=A3A4A5A6)（3.3）A3-1A2-1A1-1T6=3T6(3T6=A4A5A6)（3.4）A4-1A3-1A2-1A1-1T6=4T6(4T6=A5A6

)（3.5）A5-1

A4-1A3-1A2-1A1-1T6=5T6(5T6=A6)（3.6）根据上述五个矩阵方程对应元素相等，可得到若干个可解的代数方程，便可求出关节变量θn或dn。3.2逆运动学方程的解（Solvinginverse6

3.3斯坦福机械手的逆运动学解

（InversesolutionofStanfordmanipulator）在第三章我们推导出StanfordManipulator的运动方程和各关节齐次变换式。下面应用式（3.2）～（3.6）进行求解：3.3斯坦福机械手的逆运动学解7这里

f11=C1x＋S1y

(3.10)

f12=-z

(3.11)

f13=-S1x＋C1y

(3.12)其中

x=[nxoxaxpx]T，y=[nyoyaypy]T，z=[nzozazpz]T由前节得到的斯坦福机械手运动学方程式（2.48）为C2(C4C5C6

-S4S6)-S2S5C6-C2(C4C5S6+S4C6)+S2S5S6

S2(C4C5C6-S4S6)+C2S5C6-S2(C4C5S6+S4C6)-C2S5S6

1T6=S4C5C6+C4C6-S4C5S6+C4C6

00

C2C4S5+S2C5S2d3S2C4S5-C2C5-C2d3S4S5d2（3.13）01这里8比较式（3.9）和式（3.13）矩阵中的第三行第四列元素相等得到f13（p）=d2（3.14）或-S1

px＋C1py=d2（3.15）令px

=rcosΦ

（3.16）

py

=rsinΦ

（3.17）其中（3.18）（3.19）将式（3.16）和式（3.17）代入式（3.15）有

sinΦconθ1－conΦsinθ1

＝d2/r

（0<d2/r≤1）（3.20）由式（3.20）可得

sin(Φ－θ1)＝d2/r

（0<Φ－θ1<）（3.21）

con(Φ－θ1)＝（3.22）这里±号表示机械手是右肩结构（＋）还是左肩结构（－）。比较式（3.9）和式（3.13）矩阵中的第三行第四列元素相等9由式（3.21）、（3.22）和（3.18）可得到第一个关节变量θ1的值

（3.23）根据同样的方法，利用式（3.9）和式（3.13）矩阵元素相等建立的相关的方程组，可得到其它各关节变量如下：

（3.24）（3.25）（3.26）（3.27）（3.28）由式（3.21）、（3.22）和（3.18）可得到第一个关节10注意：在求解关节变量过程中如出现反正切函数的分子和分母太小，则计算结果误差会很大，此时应重新选择矩阵元素建立新的方程组再进行计算，直到获得满意的结果为止。同样，如果计算结果超出了机械手关节的运动范围，也要重新计算，直到符合机械手关节的运动范围。由于机械手各关节变量的相互耦合，后面计算的关节变量与前面的关节变量有关，因此当前面关节变量的计算结果发生变化时，后面关节变量计算的结果也会发生变化，所以逆运动方程的解不是唯一的，我们应该根据机械手的组合形态和各关节的运动范围，经过多次反覆计算，从中选择一组合理解。由此可见，求解机械手的逆运动方程是一个十分复杂的过程。注意：113.4欧拉变换的逆运动学解

（InversesolutionofEulerAngles

）由前节知欧拉变换为Euler(ø,θ,ψ)＝Rot(z,ø)Rot(y,θ)Rot(z,ψ)（3.29）我们用T来表示欧拉变换的结果，即T＝Euler(ø,θ,ψ)（3.30）或T＝Rot(z,ø)Rot(y,θ)Rot(z,ψ)（3.31）其中

（3.32）3.4欧拉变换的逆运动学解

（Inversesol12（3.33）第三章-工业机器人运动学-3逆运动学ppt课件13比较式（3.32）和式（3.33）有（3.34）（3.35）（3.36）（3.37）（3.38）（3.39）（3.40）（3.41）（3.42）比较式（3.32）和式（3.33）有14由式（3.42）可解出θ角（3.43）由式（3.40）和式（3.43）可解出φ角（3.44）由式（3.36）和式（3.43）可解出Ψ角（3.45）由式（3.42）可解出θ角15

这里需要指出的是，在我们采用式（3.43）~式（3.45）来计算θ、φ、Ψ时都是采用反余弦函数，而且式（3.43）和式（3.45）的分母为sinθ，这会带来如下问题：1）由于绝对值相同的正负角度的余弦相等，如cosθ＝cos（-θ），因此不能确定反余弦的结果是在那个象限；2）当sinθ接近于0时，由式（3.43）和式（3.45）所求出的角度φ和Ψ是不精确的；3）当θ＝0或±180º时，式（3.43）和式（3.45）无数值解。为此，我们必须寻求更为合理的求解方法。由三角函数的知识我们知道，反正切函数θ＝tan－1（x/y）所在的象限空间可由自变量的分子和分母的符号确定（如图3.1所示），因此如果我们得到欧拉角的正切表达式，就不难确定欧拉角所在的象限。为此，我们采用前节的方法，用Rot(z,ø)－1左乘式（3.31）有Rot－1(z,ø)T＝Rot(y,θ)Rot(z,ψ)（3.46）yx＋y＋y－x＋y－xx－y＋x－图3.1正切函数所在象限θ这里需要指出的是，在我们采用式16即（3.47）将上式写成如下形式（3.48）式中（3.49）（3.50）（3.51）同样，上面三个式子中的x、y、z分别表示n、o、a、p矢量的各个分量，如（3.52）即17比较式（3.48）等号两边矩阵的第2行第3列元素可知（3.63）即（3.54）由此可得到（3.55）或（3.56）结果得到（3.57）或（3.58）比较式（3.48）等号两边矩阵的第2行第3列元素可知18上述结果相差180º，可根据实际系统的组合形态从中选择一个合理解。如果ay和ax都为0，则式（3.57）和式（3.58）无定义，这是一种退化现象，此时φ值可任意设置，如φ＝0。由于角φ已求出，比较式（3.48）等号两边矩阵第1行第3列和第3行第3列元素相等有（3.59）（3.60）或（3.61）（3.62）由此可得（3.63）上述结果相差180º，可根据实际系19同样比较式（3.48）等号两边矩阵的第2行第1列和第2行第2列元素可知（3.64）（3.65）或（3.66）（3.67）由此可得（3.68）至此，我们求出了欧拉变换的逆运动学解。同样比较式（3.48）等号两边矩阵的第2行第1列和第2行第2203.5RPY变换的逆运动学解（InversesolutionofRPY）第三章介绍的摇摆、俯仰和偏转(RPY)变换的表达式如下T=RPY(ø,θ,ψ)＝Rot(z,ø)Rot(y,θ)Rot(x,ψ)（3.69）用Rot－1(z,ø)左乘上式得到Rot－1(z,ø)T＝Rot(y,θ)Rot(x,ψ)（3.70）将上式写成式（3.48）的形式（3.71）式中（3.72）（3.73）（3.74）3.5RPY变换的逆运动学解（Inversesolut21由式（3.71）等号两边矩阵的第2行第1列元素相等有（3.75）由此得到（3.76）或（3.77）角φ已求出，根据式（3.71）等号两边矩阵的第3行第1列和第1行第1列元素相等有（3.78）（3.79）由此可得（3.80）由式（3.71）等号两边矩阵的第2行第1列元素相等有22进一步比较式（3.71）等号两边矩阵元素，由第2行第3列和第2行第2列元素相等有（3.81）（3.82）由此可得（3.83）

至此，我们求出了RPY的逆运动学解。进一步比较式（3.71）等号两边矩233.6球坐标变换的逆运动学解

（InversesolutionofSphericalCoordinates

）前节介绍的球坐标变换的表达式如下T=Sph(α,β,