第三章机器人运动学
合集下载
第三章机器人运动学
第三章机器人运动学机器人运动学是研究机器人如何在二维或三维空间中进行运动的学科。
它涉及到机器人的轨迹规划、运动控制和路径规划等重要内容。
本章将介绍机器人运动学的基本概念和常用模型，帮助读者全面了解机器人的运动规律和控制原理。
1. 机器人运动学的基本概念机器人运动学是研究机器人位置和姿态变化的学科，包括正运动学和逆运动学两个方面。
正运动学研究机器人的末端执行器的位置和姿态如何由关节变量确定；逆运动学则研究机器人如何通过末端执行器的位置和姿态来确定关节变量的值。
机器人的运动学建模一般采用DH（Denavit-Hartenberg）参数表示方法。
DH 参数是由Denavit和Hartenberg提出的一种机器人坐标系的选择和旋转轴的确定方法。
通过定义一系列关节坐标系，建立起机器人的坐标系链，并确定各个关节的旋转轴和约定的方向，可以方便地描述机器人的运动学特性。
2. 机器人正运动学机器人正运动学是研究机器人末端执行器位置和姿态如何由关节变量确定的问题。
在机器人的正运动学中，常用的方法有几何法和代数法。
2.1 几何法几何法是一种较为直观的方法，通过对机器人各个关节坐标系的位置和旋转进行推导，得到机器人末端执行器的位置和姿态。
几何法适用于无约束和无外力干扰的情况，可以简单快速地推导出机器人的正运动学方程。
2.2 代数法代数法是一种基于运动学链的代数运算的方法，通过DH参数建立起机器人的坐标系链，并通过矩阵运算推导出机器人的正运动学方程。
代数法在机器人正运动学的推导和计算过程中更具有普适性和灵活性。
3. 机器人逆运动学机器人逆运动学是研究机器人如何通过末端执行器的位置和姿态来确定关节变量的值的问题。
机器人逆运动学在机器人运动规划和路径控制中起到重要的作用。
机器人逆运动学的求解一般采用迭代方法，通过迭代计算来逼近解析解，实现对机器人关节变量的求解。
逆运动学的求解过程中可能会出现奇异点和多解的情况，需要通过约束条件和优化方法来处理。
第3章 机器人运动
3 齐次坐标变换 3.1齐次坐标变换 3.1齐次坐标变换 假设机器人手部拿一个钻头在 工件上实施钻孔作业，已知钻 头中心P点相对于手腕中心的 位置，求P点相对于基座的位 置。
x i o
zb kb yb jb o, ib xb P
z
k
j
y
分别在基座和手部设置为固定坐标系和动坐标系， 如图所示。
P点 相对于固定坐标系
1 4 0 −3 0 7 0 1
T中第一列的三个元素（0，1，0）T表示活动坐标系的u轴与 固定坐标系三个坐标轴之间的投影，故u轴平行于y轴；T中第 二列的三个元素（0，0，1）T表示活动坐标系的v轴与固定坐 标系三个坐标轴之间的投影，故v轴平行于z轴；T中第三列的 三个元素（1，0，0）T表示活动坐标系的w轴与固定坐标系三 个坐标轴之间的投影，故轴w平行于x轴；T中第四列的三个元 素（4，-3，7）T表示活动坐标系的原点与固定坐标系原点之 间的距离。
b
3.3.2 举例 ⋅ i i
z kb k o, xb i o xi y j y j
1 0 0 R = 0 1 0 0 0 1
所以
x0 X 0 = y0 z0
0 0 1 0 0 1 0 0
1 0 A = Trans( x0 , y0 , z0 ) = 0 0
上面所述的坐标变换每步都是相对于固定坐标系进行的，也可以 相对于动坐标系进行变换： 坐标系 {o , : u , v, w} 初始与固定坐标系 {o:x, y, z} 相重合，首先相对于固定坐标系平移
4i − 3 j + 7 k ；然后绕活动系的v轴旋转900；最后绕w轴旋转900。
变换的几何表示如图所示。这是合成变换矩阵为
机器人学第3章 机器人运动学
(3.46)
如果已知一个表示任意旋转的齐次变换，那么就能够 确定其等价欧拉角。
3.2 机械手运动方程的求解
21
3.2.2 滚、仰、偏变换解
直接从显式方程来求解用滚动、俯仰和偏转表示的变 换方程。 RPY变换各角如下：
atan2(n y , n x ) 180 atan2(n z , cn x sn y ) atan2( sa x ca y , so x co y )
0
T6 0T1 (1 )1T2 (2 )2T3 (3 )3T4 (4 )4T5 (5 )5T6 (6 )
3.1 机器人运动方向的表示
5
3.1.1 运动姿态和方向角
用横滚、俯仰和偏转角表示运动姿态 另一种常用的旋转集合是横滚（roll）、俯仰（pitch） 和偏转（yaw）。
图3.3 用横滚、俯仰和偏转表示机械手运动姿态
3.1 机器人运动方向的表示 6
3.1.1 运动姿态和方向角
对于旋转次序，规定：
1
(3.16)
3.1 机器人运动方向的表示
15
3.1.3 连杆变换矩阵及其乘积
如果机械手与参考坐标系的相对关系是由变换 Z 来 表示的，而且机械手与其端部工具的关系由变换 E 表示，那么此工具端部对参考坐标系的位置和方向 可由变换 X 表示如下：
可求得：
X ZT6 E
T6 Z 1 XE 1
(3.52)
3.2 机械手运动方程的求解
22
3.2.3 球面变换解
把求解滚、仰和偏变换方程的技术用于球面坐标表示 的运动方程。 球面变换的解为：
atan2( p y , p x ), 180 atan2(cp x sp y , p z )
第三章_机器人运动学
举例（example）
• 一个差动驱动机器人（针对图3.3所示机器人） 将滚动约束和滑动约束方程联合起来可得到式：
J1 ( s ) J C ( ) R( ) I 2 1 s 0
由于小脚轮无动力，并可在任何方向自由运动，因此可忽略第三个接触点。 其余两个轮不可操纵，因此 J1 ( s ) 和 C1 ( s ) 分别简化为
• 瞬时转动中心 ICR （instantaneous center of rotation） 在任何给定时刻，轮子必定沿着半径为 R的某个圆瞬时的运动，使得那个圆的中心 处在零运动直线上，该中心称为瞬时转动 中心。它可以位于沿零运动直线的任何地 方。
•
要使机器人运动存在一个单独的解，必须有 一个单独的ICR，即所有的零运动直线在一个单 独点相交。 • ICR的几何特性显示了机器人的活动性是机 器人运动上的独立约束数目的函数而不是轮子数 目的函数。 • 独立的滑动约束的数目可用 C1 (s ) 的秩来描述
.
.
.
.
（1）
• 其次，计算在YR 方向的贡献
由于没有一个轮子可以提供侧向运动， 所以沿YR 方向的速度总是零。 • 最后，计算旋转角速度分量。可独立的计 算各轮的贡献，且只要简单相加即可。 . .
r 1 r 2 1 2 2l 2l
（2）
ห้องสมุดไป่ตู้
• 联合式（1）和式（2）得到差动驱动机器人的 运动学模型如式（3）所示：
x I y
• 为了根据分量的移动描述机器人的移动， 需要将全局参考架下的移动映射到局部参 考框架下的运动。该运动可由正交旋转矩 阵来完成：
举例（example）
机器人学基础_第3章_机器人运动学
机械手的运动姿态往往由 一个绕轴x ，y 和 z 的旋转 序列来规定。这种转角的 序列，称为欧拉（Euler） 角。 欧拉角: 用一个绕 z 轴 旋转ф角，再绕新的 y 轴 y’旋转θ角，最后绕新的 z 轴z’’旋转ψ角来描述任 图3.2 欧拉角的定义 何可能的姿态。 欧拉变换Euler可由连乘三个旋转矩阵来求得，即 Euler (φ ,θ ,ψ ) = Rot ( z , φ ) Rot ( y,θ ) Rot ( z ,ψ ) （3.3）
Kinematics treats motion without regard to the forces that cause it. Within the science of kinematics one studies the position, velocity, acceleration, and all higher order derivatives of the position variables (with respect to time or any other variable). 从几何学 几何学的观点来处 几何学 理手指位置 手指位置P与关节变量 关节变量 手指位置 L1, L2, θ1 和 θ 2的关系称为 运动学(Kinematics)。 运动学
(3.9)
3.1 Representation of Kinematics Equation of Manipulator
17
3.1 Representation of Kinetic Equation of Robot Manipulator
3.1 Representation of Kinematics Equation of Manipulator
12
3.1.1 Kinetic Pose and Oriented Angle Roll, Pitch, Yaw to represent motion pose
Kinematics treats motion without regard to the forces that cause it. Within the science of kinematics one studies the position, velocity, acceleration, and all higher order derivatives of the position variables (with respect to time or any other variable). 从几何学 几何学的观点来处 几何学 理手指位置 手指位置P与关节变量 关节变量 手指位置 L1, L2, θ1 和 θ 2的关系称为 运动学(Kinematics)。 运动学
(3.9)
3.1 Representation of Kinematics Equation of Manipulator
17
3.1 Representation of Kinetic Equation of Robot Manipulator
3.1 Representation of Kinematics Equation of Manipulator
12
3.1.1 Kinetic Pose and Oriented Angle Roll, Pitch, Yaw to represent motion pose
机器人技术基础课件第三章-机器人运动学精选全文完整版
03T 01T12T 23T
如此类推，对于六连杆机器人，有下列矩阵：
06T 01T 12T 23T 34T 45T 56T
3.2 3.2 机械手运动学方程
26
0 6
T
3.1.4 连杆变换矩阵及其乘积
06T 01T12T 23T 34T 45T 56T
机器人运动学方程
此式右边表示了从固定参考系到手部坐标系的各连杆
一个六连杆机械手可具有六个自由度，每个连杆含 有一个自由度，并能在其运动范围内任意定位与定向。 其中三个自由度用于规定位置，而另外三个自由度用 来规定姿态。
8
3.1.1 连杆坐标系
机械手的运动方向
机器人手部的位置和姿态也可以
用固连于手部的坐标系{B}的位姿
来表示
关节轴为ZB， ZB轴的单位方向 矢量α称为接近矢量，指向朝外。
(1) 坐标系｛i-1｝绕xi-1轴转角αi-1，使Zi－1与Zi平行，算子为Rot(x, αi-1) ； (2) 沿Xi-1轴平移ai-1，使Zi－1和Zi共线， 算子为Trans(ai-1,0,0)； (3)绕Zi轴转角θi; 使得使Xi－1与Xi平行, 算子为Rot(z,θi)；
(4) 沿Zi轴平移di。使得i-1系和i系重合, 算子为Trans(0,0,di)。
3.2.1 机器人正运动学方程
连杆 i 1
2
3
连杆长 度ai-1
0
a0
a1
连杆偏距 di 0
0
d2
连杆扭角 αi-1 00
00
-900
关节角 θi
θ1(00) θ2(00) θ3(00)
3.2.1 机器人正运动学方程
该3自由度机器人的运动学方程为：
如此类推，对于六连杆机器人，有下列矩阵：
06T 01T 12T 23T 34T 45T 56T
3.2 3.2 机械手运动学方程
26
0 6
T
3.1.4 连杆变换矩阵及其乘积
06T 01T12T 23T 34T 45T 56T
机器人运动学方程
此式右边表示了从固定参考系到手部坐标系的各连杆
一个六连杆机械手可具有六个自由度，每个连杆含 有一个自由度，并能在其运动范围内任意定位与定向。 其中三个自由度用于规定位置，而另外三个自由度用 来规定姿态。
8
3.1.1 连杆坐标系
机械手的运动方向
机器人手部的位置和姿态也可以
用固连于手部的坐标系{B}的位姿
来表示
关节轴为ZB， ZB轴的单位方向 矢量α称为接近矢量，指向朝外。
(1) 坐标系｛i-1｝绕xi-1轴转角αi-1，使Zi－1与Zi平行，算子为Rot(x, αi-1) ； (2) 沿Xi-1轴平移ai-1，使Zi－1和Zi共线， 算子为Trans(ai-1,0,0)； (3)绕Zi轴转角θi; 使得使Xi－1与Xi平行, 算子为Rot(z,θi)；
(4) 沿Zi轴平移di。使得i-1系和i系重合, 算子为Trans(0,0,di)。
3.2.1 机器人正运动学方程
连杆 i 1
2
3
连杆长 度ai-1
0
a0
a1
连杆偏距 di 0
0
d2
连杆扭角 αi-1 00
00
-900
关节角 θi
θ1(00) θ2(00) θ3(00)
3.2.1 机器人正运动学方程
该3自由度机器人的运动学方程为：
机器人运动学
R3
Z
三个平移自由度 T1, T2, T3
三个旋转自由度 R1, R2, R3
T3
T1
T2
Y R2
X
2019/3/31
R1
2.2 刚体位姿描述
方位描述
第三章
机器人运动学
利用固定于物体的坐标系描述方位 (orientation)。方位又称为姿 态 (pose)。
在刚体 B上设置直角坐标系 {B} ，利用与 {B} 的坐标轴平行 的三个单位矢量表示B的姿态。
A
p R ( x , ) p
B
zB
zA
Bp
P
yB
{A}
1 0 R ( x , ) 0 c 0 s
c R ( y , ) 0 s 0 s 1 0 , 0 c
0 s c
s c 0 0 0 1
2019/3/31
i A iB A jB r11 r12
第三章
机器人运动学
2.2 刚体位姿描述
位置与姿态的表示 相对于参考坐标系{A}，坐标系{B}的原点位置和坐标轴的 方位可以由位置矢量和旋转矩阵描述。刚体B在参考坐标 系{A}中的位姿利用坐标系{B}描述。
{ B}
当表示位置时 当表示方位时
zA
iB
jB
A
kA 坐标系{B}的三个单位主矢量在坐标系{A}中的描述：
pBo
kB
yA
{ A iB , A jB , A k B }
坐标系{B}相对于坐标系{A}的姿态描述：
A B
O
R { iB , jB , k B }
A A A
第三章机器人运动学
Axis i+1
αi
3.2.3连杆附加坐标系的规定
（4）建立连杆坐标系的步骤
确定关节轴，并画出轴的延长线。 找出关节轴i和i+1的公垂线或交点，作为坐标系i的原点。 规定Zi的指向是沿着第i个关节轴。 规定Xi轴得指向是沿着轴i和i+1的公垂线的方向，如果关节轴 i和i+1相交，则Xi轴垂直于关节轴i和i+1所在的平面。 Yi 轴的方向由右手定则确定。 当第一个关节变量为0时，规定坐标系{0}和{1} 重合，对于坐 标系{N}，尽量选择坐标系使得连杆参数为0.
3.2.3连杆附加坐标系的规定
为了描述每个连杆和相邻连杆之间的相对位置关系,需要在每个连杆 上定义一个固连坐标系. （1）连杆中的中间连杆 规定: 坐标系{i}的Z轴称为Zi,与 关节轴i重合; 坐标系{i}的原点位于公垂 线ai与关节轴i的交点处. Xi轴沿ai方向由关节i指向 关节i+1 (若: ai =0,则Xi垂直于Zi和Zi+1所 在的平面;按照右手定则绕Xi轴的 转角定义为αi ,由于Xi轴的符号 有两种,则转角的符号也有两种.) Yi轴由右手定则确定
3.2.5 PUMA560运动学方程
（2）连续连杆变换 定义了连杆坐标系和相应得连杆参数，就能建立运动学 方程，坐标系{N}相对于坐标系{0}的变换矩阵为：
0 N
0
T T T T
0 1 1 2 2 3
N 1 N
T
变换矩阵 NT 是关于n个关节变量的函数，这些变量可以通 过放置在关节上的传感器测得，则机器人末端连杆再基坐标系 （笛卡尔坐标系）中的位置和姿态就能描述出来。
2) joint angle 关节角 描述两个相邻连杆绕公共轴线旋 转的夹角θi. 当i为转动关节时,关节角为一变量.
αi
3.2.3连杆附加坐标系的规定
（4）建立连杆坐标系的步骤
确定关节轴，并画出轴的延长线。 找出关节轴i和i+1的公垂线或交点，作为坐标系i的原点。 规定Zi的指向是沿着第i个关节轴。 规定Xi轴得指向是沿着轴i和i+1的公垂线的方向，如果关节轴 i和i+1相交，则Xi轴垂直于关节轴i和i+1所在的平面。 Yi 轴的方向由右手定则确定。 当第一个关节变量为0时，规定坐标系{0}和{1} 重合，对于坐 标系{N}，尽量选择坐标系使得连杆参数为0.
3.2.3连杆附加坐标系的规定
为了描述每个连杆和相邻连杆之间的相对位置关系,需要在每个连杆 上定义一个固连坐标系. （1）连杆中的中间连杆 规定: 坐标系{i}的Z轴称为Zi,与 关节轴i重合; 坐标系{i}的原点位于公垂 线ai与关节轴i的交点处. Xi轴沿ai方向由关节i指向 关节i+1 (若: ai =0,则Xi垂直于Zi和Zi+1所 在的平面;按照右手定则绕Xi轴的 转角定义为αi ,由于Xi轴的符号 有两种,则转角的符号也有两种.) Yi轴由右手定则确定
3.2.5 PUMA560运动学方程
（2）连续连杆变换 定义了连杆坐标系和相应得连杆参数，就能建立运动学 方程，坐标系{N}相对于坐标系{0}的变换矩阵为：
0 N
0
T T T T
0 1 1 2 2 3
N 1 N
T
变换矩阵 NT 是关于n个关节变量的函数，这些变量可以通 过放置在关节上的传感器测得，则机器人末端连杆再基坐标系 （笛卡尔坐标系）中的位置和姿态就能描述出来。
2) joint angle 关节角 描述两个相邻连杆绕公共轴线旋 转的夹角θi. 当i为转动关节时,关节角为一变量.
- 1、下载文档前请自行甄别文档内容的完整性，平台不提供额外的编辑、内容补充、找答案等附加服务。
- 2、"仅部分预览"的文档,不可在线预览部分如存在完整性等问题,可反馈申请退款(可完整预览的文档不适用该条件!)。
- 3、如文档侵犯您的权益，请联系客服反馈,我们会尽快为您处理(人工客服工作时间：9:00-18:30)。
坐标系原点沿x i轴的偏移距离(是z i-1轴和zi两轴间的最小距离） i ：绕x i轴（右手规则）由z i-1轴转向zi轴的偏角。 对于转动关节，di、ai、αi是关节参数，θi是关节变量。 移动关节的关节参数是θi、 ai、αi， di是关节变量。
3.1.3 建立i坐标系和i-1 坐标系的齐次变换矩阵
连杆i 1 2 3 4 5 6
θi
θ1(90°) θ2(0°)
θ3(-90°) θ4(0°)
θ5(0°) θ6(0°)
αi-1
ai-1
di
0°
0
0
-90°
0
d2
0°
a2
0
-90°
a3
d4
90°
0
0
-90°
0
0
c1 s1 0 0
A1
s1 0
c1 0
0 0 1 0
0
0
0 1
(3)将坐标系Oi-1xi-1yi-1zi-1沿xi轴平移 距离ai ，使两坐标系的原点重合； (4)将坐标系Oi-1xi-1yi-1zi-1绕xi轴转αi 角，使两坐标系完全重合。
i坐标系和i—l坐标系的 齐次变换矩阵i-1Ai可以根据矩 阵的合成规则得到，i-1Ai称为 相邻坐标系i和i—1的D-H变换 矩阵。即
zi-1与zi的公垂线为xi-1, zi与zi+1的公垂线为xi轴,
αi-1:为zi-1与zi的交错角；
αi:绕x i轴（右手规则）由z i轴转向zi+1轴的偏角；
ai-1：从第i-1坐标系原点到x i-1轴和z i的交点沿x i-1 轴的偏移距离
ai：从第i坐标系原点到x i轴和z i+1的交点沿x i轴的 偏移距离
[例]确定下图所示机器人的位置和姿态 解：用D—H法建立坐标系转换矩阵，首先 列出各连轩及关节参数，如下表所示。
斯坦福机器人的连杆及关节参数表
斯坦福机器人及其坐标系图
将表中的参数分别代入i坐标系和i—l坐标系的齐次变换矩阵i-1Ai
c osi
i1 Ai
s
in
i
0
0
cosi sin i cosi cosi
确定和建立每个坐标系应根据 下面3条规则:
转动关节连杆四参数示意图
①z i-1轴沿着第i关节的运动轴； ②x i轴垂直于z i-1轴和z i轴并指向 离开z i-1轴的方向 ③y i轴按右手坐标系的要求建立。
按照这些规则，第0号坐标系在机座上的位置和方向可任选，只要z0轴沿 着第i关节运动轴。第n坐标系可放在手的任何部位、只要xn轴与zn-1轴垂直。
sin i 0
sin i sin i sin i cosi
cosi 0
ai cosi
ai
sin
i
di 1
可得如下变换矩阵：
c1 0 s1 0
A1
s1 0
0 1
c1 0
0 0
0
0
0 1
c2 0 s2 0
A2
s2 0
c5 s5 0 0
A5
0
s5 0
0 c5 0
1 0 0 0 0 1
式中 ：ci=cosi si=sini
c6 s6 0 0
A6
0
0s6
0 c6
0
1
0
0 0
0 1
§ 3.2 机器人运动学正问题 机器人运动学正问题是已知机器人各关节、各连杆 参数及各关节变量，求机器人手端坐标在基础坐标中的 位置和姿态。
cos i
sin i cos i1
si
n
i
si 0
n
i 1
sin i
cos i cos i1 cos i sin i1
0
0
sin i1 cos i1
0
a i 1
d
i
si
n
i
1
di cos i1
1
例 建立右图所示机器人相邻坐标 系间的转换矩阵
s1c2 c1s2
s1s2 c1c2
l2
(s1c2
c1s2
)
l1s1
s12
c12
l2 s12
l1s1
0
0
1
0 0
1
式中，c12=cos(1+ 2)， s12=sin(1+ 2)
容易验证上式的正确性，即：末端位置为[ 姿态为1+ 2 ；
T2= A1 A2
同理，若A3表示第三个连杆相对于第二个连杆的位置和姿态，则有
T3= A1 A2 A3
称这些A矩阵的乘积为T矩阵，其前置上标若为0，则可省略。对于六 连杆机械手，有下列T矩阵
T6= A1 A2 A3 A4 A5 A6
手爪坐标系
机械手的运动方向
原点由矢量p表示。
图3.1 矢量n,o,a和p
c2 s2 l2c2
A2
s2
c2
l2
s2
0 0 1
c1 s1 l1c1c2 s2 l2c2
T A1A2 s1
c1
l1s1
s2
c2
l2
s2
0 0 1 0 0 1
c1c2 s1s2 c1s2 s1c2 l2 (c1c2 s1s2 ) l1c1 c12 s12 l2c12 l1c1
c2 s2 0 0
A2
0
0s2
0 c2
0
1 0 0
d
2
0
1
c3 s3 0 a2
A3
s3
0
c3 0
0
0
1 0
0
0
0
1
c4 s4 0 a3
A4
0
0s4
0 c4
0
1 0 0
d
4
0
1
解：建立的坐标系如右图，这是二维坐 标系(在三维空间中，各坐标系的z轴垂 直于纸面)，其相邻坐标系的变换矩阵 为
c1 s1 01 0 l1 c1 s1 l1c1
A1 R T z, x,l1 s1
c1
00 1 0 s1
c1
l1s1
0 0 10 0 1 0 0 1
0
0
0
1
六连杆机械手的T矩阵（ T6 ）可由指定其16个元素的 数值来决定。在这16个元素中，只有12个元素具有实际 含义。
3.1.1 Denavt-Hartenberg(D-H)表示法
机械手由一串用转动或平移关节连
接的刚体(杆件)组成。每一对关节–杆件
构成一个自由度。杆件的编号由手臂的 固定基座开始，固定基座可看成杆件0， Ai
两杆间的距离di：关节轴上两个法线的距离
夹角θi：关节轴上两个法线的夹角
Ai-1
Ai
ai
ai1
di
i
ai
i
3.1.1 Denavt-Hartenberg(D-H)表示法
为 描 述 相 邻 杆 件 间 平 移 和 转 动 的 关 系 。 Denavt 和 Hartenberg (1955)提出了一种为关节链中的每一杆件建立 附体坐标系的矩阵方法。D-H方法是为每个关节处的杆件坐 标系建立4 4齐次变换矩阵，表示它与前一杆件坐标系的关 系。这样逐次变换，用“手部坐标”表示的末端执行器可被 变换并用机座坐标表示。
固联坐标系后置（zi位于 i+1关节轴上），变换公式
i1Ai Rot(zi1,i )Trans(0,0, di )Trans(ai ,0,0)Rot(xi ,i )
cosi sini 0 01 0 0 0 1 0 0 ai 1 0
0 0
A i1 i
sin
机器人机械手上坐标系的配置取决于机械手连杆连接的类型。有两种连接—— 转动关节和棱柱联轴节。现在来考虑棱柱联轴节（平动关节）的情况。下图示
出其特征参数 , d和 。
棱柱关节连杆四参数示意图 9
3.1.2 几何参数定义
3.1.2 几何参数定义
根据上述对杆件参数及坐标 系的定义，描述串联机器人相邻 坐标系之间的关节关系可归结如 下4个参数： θi：绕z i-1轴（右手规则）由x i-1轴向x i轴的关节角； di：从第i—1坐标系的原点到z i-1轴和x i轴的交点沿z i-1轴的距离； ai：从z i-1和x i轴的交点到第i
i
0
0
c os i 0 0
0
00
1
0
0 0
100 Nhomakorabea0
c os i
sin i
0
1 0
00 10
0 0
1 0
di 1
0 0
0 0
1 0
0 0
1
0
sin i 0
c os i 0
0 1
cosi
s
in
θi :绕z i轴由x i-1轴向x i轴的关节角；
di：从x i-1 轴和z i轴的交点到第i坐标系的原点沿z i 轴的距离
固联坐标系前置（zi位于i关节轴上），变换公式
3.1.3 建立i坐标系和i-1 坐标系的齐次变换矩阵
连杆i 1 2 3 4 5 6
θi
θ1(90°) θ2(0°)
θ3(-90°) θ4(0°)
θ5(0°) θ6(0°)
αi-1
ai-1
di
0°
0
0
-90°
0
d2
0°
a2
0
-90°
a3
d4
90°
0
0
-90°
0
0
c1 s1 0 0
A1
s1 0
c1 0
0 0 1 0
0
0
0 1
(3)将坐标系Oi-1xi-1yi-1zi-1沿xi轴平移 距离ai ，使两坐标系的原点重合； (4)将坐标系Oi-1xi-1yi-1zi-1绕xi轴转αi 角，使两坐标系完全重合。
i坐标系和i—l坐标系的 齐次变换矩阵i-1Ai可以根据矩 阵的合成规则得到，i-1Ai称为 相邻坐标系i和i—1的D-H变换 矩阵。即
zi-1与zi的公垂线为xi-1, zi与zi+1的公垂线为xi轴,
αi-1:为zi-1与zi的交错角；
αi:绕x i轴（右手规则）由z i轴转向zi+1轴的偏角；
ai-1：从第i-1坐标系原点到x i-1轴和z i的交点沿x i-1 轴的偏移距离
ai：从第i坐标系原点到x i轴和z i+1的交点沿x i轴的 偏移距离
[例]确定下图所示机器人的位置和姿态 解：用D—H法建立坐标系转换矩阵，首先 列出各连轩及关节参数，如下表所示。
斯坦福机器人的连杆及关节参数表
斯坦福机器人及其坐标系图
将表中的参数分别代入i坐标系和i—l坐标系的齐次变换矩阵i-1Ai
c osi
i1 Ai
s
in
i
0
0
cosi sin i cosi cosi
确定和建立每个坐标系应根据 下面3条规则:
转动关节连杆四参数示意图
①z i-1轴沿着第i关节的运动轴； ②x i轴垂直于z i-1轴和z i轴并指向 离开z i-1轴的方向 ③y i轴按右手坐标系的要求建立。
按照这些规则，第0号坐标系在机座上的位置和方向可任选，只要z0轴沿 着第i关节运动轴。第n坐标系可放在手的任何部位、只要xn轴与zn-1轴垂直。
sin i 0
sin i sin i sin i cosi
cosi 0
ai cosi
ai
sin
i
di 1
可得如下变换矩阵：
c1 0 s1 0
A1
s1 0
0 1
c1 0
0 0
0
0
0 1
c2 0 s2 0
A2
s2 0
c5 s5 0 0
A5
0
s5 0
0 c5 0
1 0 0 0 0 1
式中 ：ci=cosi si=sini
c6 s6 0 0
A6
0
0s6
0 c6
0
1
0
0 0
0 1
§ 3.2 机器人运动学正问题 机器人运动学正问题是已知机器人各关节、各连杆 参数及各关节变量，求机器人手端坐标在基础坐标中的 位置和姿态。
cos i
sin i cos i1
si
n
i
si 0
n
i 1
sin i
cos i cos i1 cos i sin i1
0
0
sin i1 cos i1
0
a i 1
d
i
si
n
i
1
di cos i1
1
例 建立右图所示机器人相邻坐标 系间的转换矩阵
s1c2 c1s2
s1s2 c1c2
l2
(s1c2
c1s2
)
l1s1
s12
c12
l2 s12
l1s1
0
0
1
0 0
1
式中，c12=cos(1+ 2)， s12=sin(1+ 2)
容易验证上式的正确性，即：末端位置为[ 姿态为1+ 2 ；
T2= A1 A2
同理，若A3表示第三个连杆相对于第二个连杆的位置和姿态，则有
T3= A1 A2 A3
称这些A矩阵的乘积为T矩阵，其前置上标若为0，则可省略。对于六 连杆机械手，有下列T矩阵
T6= A1 A2 A3 A4 A5 A6
手爪坐标系
机械手的运动方向
原点由矢量p表示。
图3.1 矢量n,o,a和p
c2 s2 l2c2
A2
s2
c2
l2
s2
0 0 1
c1 s1 l1c1c2 s2 l2c2
T A1A2 s1
c1
l1s1
s2
c2
l2
s2
0 0 1 0 0 1
c1c2 s1s2 c1s2 s1c2 l2 (c1c2 s1s2 ) l1c1 c12 s12 l2c12 l1c1
c2 s2 0 0
A2
0
0s2
0 c2
0
1 0 0
d
2
0
1
c3 s3 0 a2
A3
s3
0
c3 0
0
0
1 0
0
0
0
1
c4 s4 0 a3
A4
0
0s4
0 c4
0
1 0 0
d
4
0
1
解：建立的坐标系如右图，这是二维坐 标系(在三维空间中，各坐标系的z轴垂 直于纸面)，其相邻坐标系的变换矩阵 为
c1 s1 01 0 l1 c1 s1 l1c1
A1 R T z, x,l1 s1
c1
00 1 0 s1
c1
l1s1
0 0 10 0 1 0 0 1
0
0
0
1
六连杆机械手的T矩阵（ T6 ）可由指定其16个元素的 数值来决定。在这16个元素中，只有12个元素具有实际 含义。
3.1.1 Denavt-Hartenberg(D-H)表示法
机械手由一串用转动或平移关节连
接的刚体(杆件)组成。每一对关节–杆件
构成一个自由度。杆件的编号由手臂的 固定基座开始，固定基座可看成杆件0， Ai
两杆间的距离di：关节轴上两个法线的距离
夹角θi：关节轴上两个法线的夹角
Ai-1
Ai
ai
ai1
di
i
ai
i
3.1.1 Denavt-Hartenberg(D-H)表示法
为 描 述 相 邻 杆 件 间 平 移 和 转 动 的 关 系 。 Denavt 和 Hartenberg (1955)提出了一种为关节链中的每一杆件建立 附体坐标系的矩阵方法。D-H方法是为每个关节处的杆件坐 标系建立4 4齐次变换矩阵，表示它与前一杆件坐标系的关 系。这样逐次变换，用“手部坐标”表示的末端执行器可被 变换并用机座坐标表示。
固联坐标系后置（zi位于 i+1关节轴上），变换公式
i1Ai Rot(zi1,i )Trans(0,0, di )Trans(ai ,0,0)Rot(xi ,i )
cosi sini 0 01 0 0 0 1 0 0 ai 1 0
0 0
A i1 i
sin
机器人机械手上坐标系的配置取决于机械手连杆连接的类型。有两种连接—— 转动关节和棱柱联轴节。现在来考虑棱柱联轴节（平动关节）的情况。下图示
出其特征参数 , d和 。
棱柱关节连杆四参数示意图 9
3.1.2 几何参数定义
3.1.2 几何参数定义
根据上述对杆件参数及坐标 系的定义，描述串联机器人相邻 坐标系之间的关节关系可归结如 下4个参数： θi：绕z i-1轴（右手规则）由x i-1轴向x i轴的关节角； di：从第i—1坐标系的原点到z i-1轴和x i轴的交点沿z i-1轴的距离； ai：从z i-1和x i轴的交点到第i
i
0
0
c os i 0 0
0
00
1
0
0 0
100 Nhomakorabea0
c os i
sin i
0
1 0
00 10
0 0
1 0
di 1
0 0
0 0
1 0
0 0
1
0
sin i 0
c os i 0
0 1
cosi
s
in
θi :绕z i轴由x i-1轴向x i轴的关节角；
di：从x i-1 轴和z i轴的交点到第i坐标系的原点沿z i 轴的距离
固联坐标系前置（zi位于i关节轴上），变换公式