机器人技术第3讲

§2-8杆件坐标系的建立1手部姿态和位置的表示手部姿态可用3×3矩阵表示：图中，定义为接近矢量，定义为姿态矢量，定义为法向矢量

手部的位置可以用从基准参考系原点指向手部中心的矢量来表示

2024/1/311§2-8杆件坐标系的建立手部的位姿可以用4X4矩阵表示

2确定杆系的D－H法

机器人运动学的重点是研究手部的位姿和运动，而手部位姿是与机器人各杆件的尺寸、运动副类型及杆间的相互关系直接相关连的，因此要研究手部相对于机座的几何关系，必须先分析两相邻杆件的相互关系，为此要先确定杆件坐标系。2024/1/312§2-8杆件坐标系的建立任何一个连杆，如图中的连杆n，两端有关节n和n＋l，该连杆可以用两个量来描述，一个是两个关节轴线沿公垂线的距离an，称作连杆长度，另一个是在垂直于an的平面内两个轴线的夹角，称之为连杆扭角。这两个参数为连杆本身的参数。

2确定杆系的D－H法2024/1/313§2-8杆件坐标系的建立考虑杆n与其相邻连杆n－1的关系，它们通过关节n相连，其相对位置可用两个参数dn和来确定，其中dn是沿关节n的轴线两个公垂线的距离，是垂直于关节n轴线的平面内两个公垂线的夹角。这是表达相邻杆件相互关系的两个参数。2确定杆系的D－H法2024/1/314§2-8杆件坐标系的建立建立杆件坐标系按Denavit－Hartenberg的方法，n系的坐标原点设在关节n的轴线和关节n＋1的轴线的公垂线与关节n＋l的轴线相交之处，n系的Z轴与关节n＋1的轴线重合，X与上述公垂线重合，且方向从关节n指向关节n＋l。当关节是转动关节时，成为关节变量，若关节为移动关节，dn成为关节变量。当n－1系的Xn－1轴与n系的X轴平行且方向相同时，定义。简言之，按D－H法确定杆件坐标系，可取坐标系n的Z轴与关节n＋l的轴线重合，

X轴取为相邻Z轴的公垂线，Y轴则按右手系确定。

2024/1/315§2-8杆件坐标系的建立3．杆件坐标系之间的变换矩阵在用D－H法建立了各杆件坐标系后，n-1系与n系间的变换关系可以用坐标系的平移、旋转来实现。考虑从n-1系到n系的变换，可先令n-1系绕Zn－1轴旋转角，再沿Zn－1轴平移dn，然后沿Xn轴移动an，最后绕Xn轴旋转角，使n-1系与n系重合。用变换矩阵表示，则有2024/1/316§2-8杆件坐标系的建立

（2－40）在很多机器人设计时，或90度，dn＝0或an＝0，从而可以简化矩阵计算和控制2024/1/317§2-9正向运动学正向运动学主要解决机器人运动方程的建立及手部位姿的求解问题。

机器人机构可以认为是一系列杆件由关节连接起来，我们把描述一个杆件与下一个杆件之间关系的齐次变换阵记为A阵，A1描述第一个杆系相对于固定系的位姿，A2描述第二个杆系相对于第一个杆系的位姿，而第二个杆系相对于固定系的位姿可用A1A2表示，令其等于T2，即T2＝A1A2，第三个杆系对固定系有T3＝A1A2A3，如此类推，对六杆机器人，有T6＝A1A2A3A4A5A6，这里T6表示了手部的位姿，而方程（2－41）表示了从固定系到手部的各坐标系之间的变换矩阵与手部位姿的关系，我们称之为机器人的运动方程

2024/1/318§2-9正向运动学

(2-41)

(2-42)2024/1/319§2-9正向运动学1斯坦福机器人的运动方程斯坦福机器人是6自由度RRPRRR型机器人，其外形如下图所示，首先建立各杆件坐标系，注意各系Z轴沿转动轴线的方向，X轴沿公垂线方向，n系的原点设在n轴n＋1轴公垂线与n＋1轴交点处，O系的位置可任选，各杆参数见表2－1，各杆系见图2－17。现在根据各杆系的关系写出A阵。

2024/1/31102024/1/3111§2-5正向运动学2024/1/3112§2-9正向运动学1系与0系为旋转关节，见图a

(2-43)2系与1系为旋转关节，杆长为d2，见图b

(2-44)2024/1/3113§2-9正向运动学3系与2系为移动关节，移动行程为d3，见图c

(2-45)4系与3系为旋转关节，见图d

(2-46)2024/1/3114§2-9正向运动学5系与4系为旋转关节，见图e

(2-47)6系与5系为旋转关节，见图f

(2-48)A全部建立，要知道非相邻杆件之间的关系，就用相应的A阵连乘即可2024/1/3115§2-9正向运动学以上方程均为和d的函数，当和d给出后，即可计算出手部的位置和方向这些值就是手部位姿的解。这个求解过程就是正向求解或直接求解

（2-49）2024/1/3116§2-9正向运动学肘状机器人的运动方程6R肘状机器人的外形如图所示，按D—H法建立杆系，可以直接写出各A阵2024/1/3117§2-9正向运动学

(2-50)

(2-51)2024/1/3118§2-9正向运动学

(2-52)

(2-53)2024/1/3119§2-9正向运动学

(2-54)

(2-55)2024/1/3120§2-9正向运动学

反映手部与固定系关系的运动方程为：

(2-56)

以上从固定系出发，通过杆件尺寸及相互位置关系，逐一确定各杆件位姿，最后求取手部位姿，这就是机器人的正向运动学求解2024/1/3121§2-10反向运动学

已知、d求出手部位姿，这一求解过程比较容易，将各变量代入运动方程即可得出。在机器人控制和轨迹规划中，问题正好相反，即已知手部要达到的空间位姿的情况下，如何求关节变量，以驱动各关节的马达，使手部的位姿得到满足，这就是逆运动学问题，也称间接位置求解问题。

1欧拉变换的解

已知欧拉变换方程：其反向求解就是已知T中的各元素，求出

(2-57)

以上方程最简单的办法就是两边展开，对应元素相等，即可求出欧拉角2024/1/3122§2-10反向运动学

(2-58)

(2-59)

(2-60)

(2-61)

(2-62)

(2-63)2024/1/3123§2-10反向运动学

(2-64)

(2-65)

(2-66)

(2-67)

(2-68)

(2-69)

(2-70)在机器人运动学中，不用反余弦而用反正切求关节角，因其可唯一确定

2024/1/3124§2-10反向运动学对欧拉方程左乘

(2-71)上式左边为和T的函数，只要找到右边等于0或常数的元素，令与左边对应项相等，即可得出相应的解，对上式展开

(2-72)2024/1/3125§2-10反向运动学取相应项相等，有

(2-73)

(2-74)当ax、ay皆为零时，手部在x、y方向无分量，为向上或向下状态，此时令

(2-75)2024/1/3126§2-10反向运动学

(2-76)

(2-77)

(2-78)

(2-79)

(2-80)

(2-81)2024/1/3127§2-10反向运动学

2RPY变换的解RPY变换方程为：

(2-82)

(2-83)

(2-84)2024/1/3128§2-10反向运动学

2RPY变换的解

(2-85)

(2-86)

(2-87)2024/1/3129§2-10反向运动学

3Sph变换的解

对球坐标变换，有方程

(2-88)

(2-89)

(2-90)2024/1/3130§2-10反向运动学

3Sph变换的解

(2-91)

(2-92)2024/1/3131§2-10反向运动学

为了求r，对（2－90）左乘展开后可得r

(2-93)

这样就将、、r全部求出

2024/1/3132§2-10反向运动学斯坦福机器人的反向求解

斯坦福机器人的运动方程为：

(2-94)已知手部位姿T6及各杆件的结构参数a，d，求

(2-95)2024/1/3133§2-10反向运动学斯坦福机器人的反向求解

(2-96)2024/1/3134§2-10反向运动学斯坦福机器人的反向求解

(2-97)（2－97）变为2024/1/3135§2-10反向运动学斯坦