工业机器人-运动学

第3章机器人运动学3.1坐标变换3.2运动学方程习题2/5/2023第3章机器人运动学2/5/2023运动学研究的问题：

手在空间的运动与各个关节的运动之间的关系。正问题：已知关节运动，求手的运动。逆问题：已知手的运动，求关节运动。第3章机器人运动学2/5/2023数学模型：手的运动→位姿变化→位姿矩阵M

关节运动→参数变化→关节变量qi，i=1，…，n运动学方程：

M=f(qi)，i=1，…，n正问题：已知qi，求M。逆问题：已知M，求qi。第3章机器人运动学2/5/2023预备知识１、机器人位姿的表示２、机器人的坐标系

第3章机器人运动学2/5/2023１、机器人位姿的表示机器人的位姿主要是指机器人手部在空间的位置和姿态，有时也会用到其它各个活动杆件在空间的位置和姿态。

第3章机器人运动学2/5/2023１、机器人位姿的表示位置可以用一个3×1的位置矩阵来描述。ｐ(ｘ,ｙ,ｚ)ｚｙｘｏ第3章机器人运动学2/5/2023１、机器人位姿的表示姿态可以用坐标系三个坐标轴两两夹角的余弦值组成3×3的姿态矩阵来描述。

ｐ(ｘ,ｙ,ｚ)ｚｙｘｏｚhｙhｘhｏh第3章机器人运动学2/5/2023１、机器人位姿的表示例：右图所示两坐标系的姿态为：ｚ0ｙ0ｘ0ｏ0ｚ1ｙ1ｘ1ｏ1第3章机器人运动学2/5/20232、机器人的坐标系手部坐标系——参考机器人手部的坐标系，也称机器人位姿坐标系，它表示机器人手部在指定坐标系中的位置和姿态。机座坐标系——参考机器人机座的坐标系，它是机器人各活动杆件及手部的公共参考坐标系。杆件坐标系——参考机器人指定杆件的坐标系，它是在机器人每个活动杆件上固定的坐标系，随杆件的运动而运动。绝对坐标系——参考工作现场地面的坐标系，它是机器人所有构件的公共参考坐标系。第3章机器人运动学2/5/20232、机器人的坐标系手部坐标系{h}机座坐标系{0}

杆件坐标系{i}

i=1，…，n绝对坐标系{B}

3.1坐标变换1、直角坐标变换2、齐次坐标变换2/5/20233.1坐标变换1、直角坐标变换2/5/2023ｚiｙiｘiｏiｚjｙjｘjｏj坐标之间的变换关系：平移变换旋转变换（1）平移变换设坐标系{i}和坐标系{j}具有相同的姿态，但它俩的坐标原点不重合，若用矢量表示坐标系{i}和坐标系{j}原点之间的矢量，则坐标系{j}就可以看成是由坐标系{i}沿矢量平移变换而来的，所以称矢量为平移变换矩阵，它是一个3×1的矩阵，即：

3.1坐标变换1、直角坐标变换2/5/2023ｚiｙiｘiｏiｚjｙjｘjｏj3.1坐标变换1、直角坐标变换2/5/2023（1）平移变换

若空间有一点在坐标系{i}和坐标系{j}中分别用矢量和表示，则它们之间有以下关系：称上式为坐标平移方程。ｚiｙiｘiｏiｚjｙjｘjｏj（2）旋转变换设坐标系{i}和坐标系{j}的原点重合，但它俩的姿态不同，则坐标系{j}就可以看成是由坐标系{i}旋转变换而来的，旋转变换矩阵比较复杂，最简单的是绕一根坐标轴的旋转变换，下面以此来对旋转变换矩阵作以说明。3.1坐标变换1、直角坐标变换2/5/2023ｚiｙiｘiｏiｚjｙjｘjｏj（2）旋转变换绕z轴旋转θ角坐标系{i}和坐标系{j}的原点重合，坐标系{j}的坐标轴方向相对于坐标系{i}绕轴旋转了一个θ角。θ角的正负一般按右手法则确定，即由z轴的矢端看，逆时钟为正。3.1坐标变换1、直角坐标变换2/5/2023ｚiｙiｘiｏiｚjｙjｘjｏjθθ（2）旋转变换绕z轴旋转θ角若空间有一点p，则其在坐标系{i}和坐标系{j}中的坐标分量之间就有以下关系：

3.1坐标变换1、直角坐标变换2/5/2023ｚiｙiｘiｏiｚjｙjｘjｏjθ（2）旋转变换绕z轴旋转θ角若补齐所缺的有些项，再作适当变形，则有：

3.1坐标变换1、直角坐标变换2/5/2023（2）旋转变换绕z轴旋转θ角将上式写成矩阵的形式，则有：

3.1坐标变换1、直角坐标变换2/5/2023（2）旋转变换绕z轴旋转θ角

再将其写成矢量形式，则有：称上式为坐标旋转方程，式中：

——p点在坐标系{i}中的坐标列阵（矢量）；

——点在坐标系{j}中的坐标列阵（矢量）；

——坐标系{j}变换到坐标系{i}的旋转变换矩阵，也称为方向余弦矩阵。3.1坐标变换1、直角坐标变换2/5/2023（2）旋转变换

——旋转变换矩阵，也称为方向余弦矩阵，是一个3×3的矩阵，其中的每个元素就是坐标系{i}和坐标系{j}相应坐标轴夹角的余弦值，它表明坐标系{j}相对于坐标系{i}的姿态（方向）。3.1坐标变换1、直角坐标变换2/5/2023（2）旋转变换绕x轴旋转α角的旋转变换矩阵为：

3.1坐标变换1、直角坐标变换2/5/2023ｙiｚiｘiｏiｚjｙjｘjｏjαα（2）旋转变换绕y轴旋转β角的旋转变换矩阵为：

3.1坐标变换1、直角坐标变换2/5/2023ｘiｙiｚiｏiｚjｙjｘjｏjββ（2）旋转变换旋转变换矩阵的逆矩阵

旋转变换矩阵的逆矩阵既可以用线性代数的方法求出，也可以用逆向的坐标变换求出。以绕z轴旋转θ角为例，其逆向变换即为绕z轴旋转-θ角，则其旋转变换矩阵就为：

3.1坐标变换1、直角坐标变换2/5/2023（2）旋转变换旋转变换矩阵的逆矩阵

比较以下两式：

结论：

3.1坐标变换1、直角坐标变换2/5/2023（3）联合变换设坐标系{i}和坐标系{j}之间存在先平移变换，后旋转变换，则空间任一点在坐标系{i}和坐标系{j}中的矢量之间就有以下关系： 称上式为直角坐标系中的坐标联合变换方程。3.1坐标变换1、直角坐标变换2/5/2023（3）联合变换若坐标系{i}和坐标系{j}之间是先平移变换，后旋转变换，则上述关系是应如何变化？3.1坐标变换1、直角坐标变换2/5/2023例：已知坐标系{B}的初始位置与坐标系{A}重合，首先坐标系{B}沿坐标系{A}的x轴移动12个单位，并沿坐标系{A}的y轴移动6个单位，再绕坐标系{A}的z轴旋转30°，求平移变换矩阵和旋转变换矩阵。假设某点在坐标系{B}中的矢量为，求该点在坐标系{A}中的矢量。

3.1坐标变换1、直角坐标变换2/5/2023解：由题意可得平移变换矩阵和旋转变换矩阵分别为：，则：

3.1坐标变换1、直角坐标变换2/5/2023（1）齐次坐标的定义空间中任一点在直角坐标系中的三个坐标分量用表示，若有四个不同时为零的数与三个直角坐标分量之间存在以下关系：

则称是空间该点的齐次坐标。3.1坐标变换2、齐次坐标变换2/5/2023（1）齐次坐标的定义齐次坐标的性质Ⅰ.空间中的任一点都可用齐次坐标表示；Ⅱ.空间中的任一点的直角坐标是单值的，但其对应的齐次坐标是多值的；Ⅲ.k是比例坐标，它表示直角坐标值与对应的齐次坐标值之间的比例关系；Ⅳ.若比例坐标k=1，则空间任一点(x,y,z)的齐次坐标为(x,y,z)

，以后用到齐次坐标时，一律默认k=1

。

3.1坐标变换2、齐次坐标变换2/5/2023（2）齐次变换矩阵（D-H矩阵）若坐标系{j}是{i}先沿矢量平移，再绕z轴旋转θ角得到的，则空间任一点在坐标系{i}和坐标系{j}中的矢量和对应的变换矩阵之间就有，写成矩阵形式则为：3.1坐标变换2、齐次坐标变换2/5/2023（2）齐次变换矩阵（D-H矩阵）再用坐标分量等式表示，则有：

3.1坐标变换2、齐次坐标变换2/5/2023（2）齐次变换矩阵（D-H矩阵）引入齐次坐标，补齐所缺各项，再适当变形，则有：

3.1坐标变换2、齐次坐标变换2/5/2023（2）齐次变换矩阵（D-H矩阵）再将其写成矩阵形式则有：

3.1坐标变换2、齐次坐标变换2/5/2023（2）齐次变换矩阵（D-H矩阵）由此可得联合变换的齐次坐标方程为：

式中，——齐次坐标变换矩阵，它是一个4×4的矩阵。3.1坐标变换2、齐次坐标变换2/5/2023（2）齐次变换矩阵（D-H矩阵）①齐次坐标变换矩阵的意义若将齐次坐标变换矩阵分块，则有：意义：左上角的3×3矩阵是两个坐标系之间的旋转变换矩阵，它描述了姿态关系；右上角的3×1矩阵是两个坐标系之间的平移变换矩阵，它描述了位置关系，所以齐次坐标变换矩阵又称为位姿矩阵。

3.1坐标变换2、齐次坐标变换2/5/2023（2）齐次变换矩阵（D-H矩阵）①齐次坐标变换矩阵的意义齐次变换矩阵的通式为：

式中，

——{j}的原点在{i}中的坐标分量；

——{j}的x轴对{i}的三个方向余弦；

——{j}的y轴对{i}的三个方向余弦；

——{j}的z轴对{i}的三个方向余弦。3.1坐标变换2、齐次坐标变换2/5/2023（2）齐次变换矩阵（D-H矩阵）②单独的平移或旋转齐次坐标变换矩阵

平移变换的齐次矩阵为：3.1坐标变换2、齐次坐标变换2/5/2023（2）齐次变换矩阵（D-H矩阵）②单独的平移或旋转齐次坐标变换矩阵

旋转变换的齐次矩阵为：3.1坐标变换2、齐次坐标变换2/5/2023（2）齐次变换矩阵（D-H矩阵）②单独的平移或旋转齐次坐标变换矩阵

同理可得：3.1坐标变换2、齐次坐标变换2/5/2023（2）齐次变换矩阵（D-H矩阵）③联合变换与单步齐次变换矩阵的关系观察以下三个齐次变换矩阵的关系：

3.1坐标变换2、齐次坐标变换2/5/2023（2）齐次变换矩阵（D-H矩阵）③联合变换与单步齐次变换矩阵的关系经观察可得：

3.1坐标变换2、齐次坐标变换2/5/2023（2）齐次变换矩阵（D-H矩阵）

③联合变换与单步齐次变换矩阵的关系任何一个齐次坐标变换矩阵均可分解为一个平移变换矩阵与一个旋转变换矩阵的乘积，即：

3.1坐标变换2、齐次坐标变换2/5/2023（2）齐次变换矩阵（D-H矩阵）

③联合变换与单步齐次矩阵的关系

当空间有任意多个坐标系时，若已知相邻坐标系之间的齐次坐标变换矩阵，则由坐标变换原理可知：

由此可知，建立机器人的坐标系，可以通过齐次坐标变换，将机器人手部在空间的位置和姿态用齐次坐标变换矩阵描述出来，从而建立机器人的运动学方程。

3.1坐标变换2、齐次坐标变换2/5/2023{0}{i-1}{i}{n}（2）齐次变换矩阵（D-H矩阵）

④相对变换两个坐标系之间总的齐次坐标变换矩阵等于每次单独变换的齐次坐标变换矩阵的乘积，而相对变换则决定这些矩阵相乘的顺序，称其为左乘和右乘原则：Ⅰ.若坐标系之间的变换是始终相对于原来的参考坐标系，则齐次坐标变换矩阵左乘；Ⅱ.若坐标系之间的变换是相对于当前新的坐标系，则齐次坐标变换矩阵右乘。

3.1坐标变换2、齐次坐标变换2/5/2023（2）齐次变换矩阵（D-H矩阵）④相对变换例：已知坐标系{B}是绕坐标系{A}的zA轴旋转90°，再绕{A}的xA轴旋转90°，最后沿矢量平移得到的，求坐标系{A}与坐标系{B}之间的齐次坐标变换矩阵。

3.1坐标变换2、齐次坐标变换2/5/2023（2）齐次变换矩阵（D-H矩阵）④相对变换解：由于变换始终是相对于原来的参考坐标系，所以满足左乘原则，即有：

3.1坐标变换2、齐次坐标变换2/5/2023（2）齐次变换矩阵（D-H矩阵）④相对变换解：若例中的变换是相对于每次变换后新的当前坐标系，其就满足右乘原则，即有：

3.1坐标变换2、齐次坐标变换2/5/2023（2）齐次变换矩阵（D-H矩阵）⑤逆变换已知{i}通过先平移，后旋转变成{j}，则变换矩阵为：3.1坐标变换2、齐次坐标变换2/5/2023ｚiｙiｘiｏiｚjｙjｘjｏj（2）齐次变换矩阵（D-H矩阵）⑤逆变换逆变换时：变换顺序颠倒；先平移，后旋转→先旋转，后平移。变换参数取反。旋转（θ）

→（

-θ），平移（px，py，pz）

→（-px，-py，-pz）。3.1坐标变换2、齐次坐标变换2/5/2023（2）齐次变换矩阵（D-H矩阵）⑤逆变换则{j}到{i}的变换矩阵为：3.1坐标变换2、齐次坐标变换2/5/2023ｚiｙiｘiｏiｚjｙjｘjｏj（2）齐次变换矩阵（D-H矩阵）⑤逆变换3.1坐标变换2、齐次坐标变换2/5/2023（2）齐次变换矩阵（D-H矩阵）⑤逆变换3.1坐标变换2、齐次坐标变换2/5/2023（2）齐次变换矩阵（D-H矩阵）⑤逆变换若齐次坐标变换矩阵为：则：

3.1坐标变换2、齐次坐标变换2/5/2023（2）齐次变换矩阵（D-H矩阵）⑤逆变换若齐次坐标变换矩阵为：3.1坐标变换2、齐次坐标变换2/5/20233.2运动学方程的建立1、运动学方程建立步骤（1）建立坐标系（2）确定参数（3）相邻杆件的位姿矩阵（4）建立方程2、运动学方程的解2/5/20233.2运动学方程的建立1、运动学方程建立步骤2/5/2023运动学方程的模型：

M=f(qi)，i=1，…，nM——机器人手在空间的位姿

qi——机器人各个关节变量3.2运动学方程的建立1、运动学方程建立步骤2/5/2023（1）建立坐标系

①机座坐标系{0}②杆件坐标系{i}i=1，2，…，n③手部坐标系{h}3.2运动学方程的建立1、运动学方程建立步骤2/5/2023（1）建立坐标系

①机座坐标系{0}

建立原则：z轴垂直，x轴水平，方向指向手部所在平面。x0z0o03.2运动学方程的建立1、运动学方程建立步骤2/5/2023（1）建立坐标系②杆件坐标系{i}，i=1，2，…，n

建立原则：

z轴与关节轴线重合，x轴与两关节轴线的距离重合，方向指向下一个杆件。杆件坐标系有两种：

第一种：z轴与i+1关节轴线重合；

第二种：z轴与i关节轴线重合。3.2运动学方程的建立1、运动学方程建立步骤2/5/2023（1）建立坐标系②杆件坐标系{i}

第一种坐标系：

z轴与i+1关节轴线重合。x0z0o00123关节1关节2关节3z2x2o2x1y1o1z3x3o3x0z0o00123关节1关节2关节3x2y2o2z3x3o3x1z1o13.2运动学方程的建立1、运动学方程建立步骤2/5/2023（1）建立坐标系②杆件坐标系{i}

第二种坐标系：

z轴与i关节轴线重合。3.2运动学方程的建立1、运动学方程建立步骤2/5/2023（1）建立坐标系③手部坐标系{h}

在第一种杆件坐标系下，{h}与{n}坐标系重合。x0z0o00123关节1关节2关节3z2x2o2x1y1o1Z3hx3ho3h3.2运动学方程的建立1、运动学方程建立步骤2/5/2023（1）建立坐标系③手部坐标系{h}

在第二种杆件坐标系下，{h}与{n}坐标系的方向保持一致。ohx0z0o00123关节1关节2关节3x2y2o2z3x3o3x1z1o1Zhxh3.2运动学方程的建立1、运动学方程建立步骤2/5/2023（2）确定参数①杆件几何参数（不变）

I、杆件长度li：两关节轴线的距离。

II、杆件扭角αi：两关节轴线的夹角。iliαi3.2运动学方程的建立1、运动学方程建立步骤2/5/2023（2）确定参数②关节运动参数

I、关节平移量di：相邻杆件的长度在关节轴线上的距离。

II、关节回转量θi：相邻杆件的长度在关节轴线上的夹角。ili-1i-1liθi关节idi3.2运动学方程的建立1、运动学方程建立步骤2/5/2023（2）确定参数②关节运动参数关节变量：

di——平移关节；

θi——回转关节。ili-1i-1liθi关节idi3.2运动学方程的建立1、运动学方程建立步骤2/5/2023（3）相邻杆件位姿矩阵①第一种坐标系建立坐标系{i-1}、{i}，试分析{i-1}→{i}的变换过程。ii-1关节iXi-1Zi-1Oi-1XiZiOi3.2运动学方程的建立1、运动学方程建立步骤2/5/2023（3）相邻杆件位姿矩阵①第一种坐标系

I、{i-1}→{i}变换过程

a、Trans（0，0，di）；

b、Rot（z，θi）；c、Trans（li，0，0）；

d、Rot（x，αi）。ii-1liθi关节idiXi-1Zi-1Oi-1XiZiOiαi3.2运动学方程的建立1、运动学方程建立步骤2/5/2023（3）相邻杆件位姿矩阵①第一种坐标系

II、单步齐次变换矩阵3.2运动学方程的建立1、运动学方程建立步骤2/5/2023（3）相邻杆件位姿矩阵①第一种坐标系

III、相邻杆件的位姿矩阵3.2运动学方程的建立1、运动学方程建立步骤2/5/2023（3）相邻杆件位姿矩阵①第一种坐标系

III、相邻杆件的位姿矩阵3.2运动学方程的建立1、运动学方程建立步骤2/5/2023（3）相邻杆件位姿矩阵①第一种坐标系

注意：特例！！！3.2运动学方程的建立1、运动学方程建立步骤2/5/2023（3）相邻杆件位姿矩阵②第二种坐标系建立坐标系{i-1}、{i}，试分析{i-1}→{i}的变换过程。ii-1关节iXi-1Zi-1Oi-1XiZiOi3.2运动学方程的建立1、运动学方程建立步骤2/5/2023（3）相邻杆件位姿矩阵②第二种坐标系

I、{i-1}→{i}变换过程

a、Trans（li-1，0，0）；

b、Rot（x，αi-1）；c、Trans（0，0，di）；

d、Rot（z，θi）。ili-1i-1θi关节idiXi-1Zi-1Oi-1XiZiOiαi-13.2运动学方程的建立1、运动学方程建立步骤2/5/2023（3）相邻杆件位姿矩阵②第二种坐标系

II、单步齐次变换矩阵3.2运动学方程的建立1、运动学方程建立步骤2/5/2023（3）相邻杆件位姿矩阵②第二种坐标系

III、相邻杆件的位姿矩阵3.2运动学方程的建立1、运动学方程建立步骤2/5/2023（3）相邻杆件位姿矩阵②第二种坐标系

III、相邻杆件的位姿矩阵3.2运动学方程的建立1、运动学方程建立步骤2/5/2023（4）建立方程3.2运动学方程的建立1、运动学方程建立步骤2/5/2023例：已知三自由度平面关节机器人如图所示，设机器人杆件1、2、3的长度为l1，l2，l3。建立机器人的运动学方程。

l1l3l23.2运动学方程的建立1、运动学方程建立步骤2/5/2023解：（1）建立坐标系(第一种)a、机座坐标系｛0｝

b、杆件坐标系｛i｝

c、手部坐标系｛h｝（与末端杆件坐标系｛n｝重合）

l1l3l2x0y0y1x1y2x2y3hx3h3.2运动学方程的建立1、运动学方程建立步骤2/5/2023解：（2）确定参数l1l3l2x0y0y1x1y2x2y3hx3hidiθiliαiqi10θ1l10θ120θ2l20θ230θ3l30θ3θ3θ2θ13.2运动学方程的建立1、运动学方程建立步骤2/5/2023解：（3）相邻杆件位姿矩阵l1l3l2x0y0y1x1y2x2y3hx3hθ3θ2θ13.2运动学方程的建立1、运动学方程建立步骤2/5/2023解：（3）相邻杆件位姿矩阵l1l3l2x0y0y1x1y2x2y3hx3hθ3θ2θ13.2运动学方程的建立1、运动学方程建立步骤2/5/2023解：（3）相邻杆件位姿矩阵l1l3l2x0y0y1x1y2x2y3hx3hθ3θ2θ13.2运动学方程的建立1、运动学方程建立步骤2/5/2023解：（4）建立方程将相邻杆件位姿矩阵依次相乘，则有：

3.2运动学方程的建立1、运动学方程建立步骤2/5/2023解：（4）建立方程若用矩阵形式表示，则为：

3.2运动学方程的建立1、运动学方程建立步骤2/5/2023解：（4）建立方程若用方程组形式表示，则为：

3.2运动学方程的建立1、运动学方程建立步骤2/5/2023解：（1）建立坐标系(第二种)a、机座坐标系｛0｝

b、杆件坐标系｛i｝

c、手部坐标系｛h｝（与末端杆件坐标系｛n｝方向一致）

l1l3l2x0y0y1x1y2x2y3x3yhxh3.2运动学方程的建立1、运动学方程建立步骤2/5/2023解：（2）确定参数ili-1αi-1diθiqi1

000θ1θ12l100θ2θ23l200θ3θ3l1l3l2x0y0y1x1y2x2y3x3yhxhθ3θ2θ13.2运动学方程的建立1、运动学方程建立步骤2/5/2023解：（3）相邻杆件位姿矩阵l1l3l2x0y0y1x1y2x2y3x3yhxhθ3θ2θ13.2运动学方程的建立1、运动学方程建立步骤2/5/2023解：（3）相邻杆件位姿矩阵l1l3l2x0y0y1x1y2x2y3x3yhxhθ3θ2θ13.2运动学方程的建立1、运动学方程建立步骤2/5/2023解：（3）相邻杆件位姿矩阵l1l3l2x0y0y1x1y2x2y3x3yhxhθ3θ2θ13.2运动学方程的建立1、运动学方程建立步骤2/5/2023解：（3）相邻杆件位姿矩阵l1l3l2x0y0y1x1y2x2y3x3yhxhθ3θ2θ13.2运动学方程的建立1、运动学方程建立步骤2/5/2023解：（4）建立方程将相邻杆件位姿矩阵依次相乘，则有：

3.2运动学方程的建立1、运动学方程建立步骤2/5/2023解：（4）建立方程若用矩阵形式表示，则为：

3.2运动学方程的建立1、运动学方程建立步骤2/5/2023解：（4）建立方程若用方程组形式表示，则为：

3.2运动学方程的建立2、运动学方程的解2/5/2023运动学方程的模型：

M0h=f(qi)，i=1，…，n正问题：已知关节变量qi的值，求手在空间的位姿M0h。逆问题：已知手在空间的位姿M0h，求关节变量qi的值。3.2运动学方程的建立2、运动学方程的解2/5/2023（1）运动学方程的正解正问题：已知关节变量qi的值，求手在空间的位姿M0h。正解特征：唯一性。用处：检验、校准机器人。3.2运动学方程的建立2、运动学方程的解2/5/2023（2）运动学方程的逆解逆问题：已知手在空间的位姿M0h，求关节变量qi的值。逆解特征分三种情况：多解、唯一解、无解。多解的选择原则：最近原则。计算方法：递推逆变换法，即3.2运动学方程的建立2、运动学方程的解2/5/2023例：已知四轴平面关节SCARA机器人如图所示，试计算：（1）机器人的运动学方程；（2）当关节变量取qi=[30°，-60°，－120，90°]T时，机器人手部的位置和姿态；（3）机器人运动学逆解的数学表达式。8004003002003.2运动学方程的建立2、运动学方程的解2/5/2023解：（1）运动学方程a、建立坐标系（第一种）机座坐标系｛0｝杆件坐标系｛i｝手部坐标系｛h｝800400300200x0z0x1z1x2z2x3z3x4hz4h3.2运动学方程的建立2、运动学方程的解2/5/2023解：（1）运动学方程b、确定参数idiθiliαiqi1800θ14000θ120θ2300