机器人正逆运动学 课件
合集下载
第三章机器人运动学PPT课件
（2）矩阵与数相乘：该数与矩阵各元素相乘。
（3）矩阵与矩阵相乘： （4） 矩阵的转置：把矩阵的行换成同序数的列，记为
7. 矩阵的逆（逆矩阵） 8. 分块矩阵：分块后的矩阵与普通矩阵的运算相同。
9. 正交矩阵：如果
，则A为正交矩阵。它满足：
如果
是正交矩阵，则
行列式和矩阵的区别：矩阵是按一定方式排成的数表；行列式是 一个数。
三、矢量的点积（内乘积或标量积）
其中θ是a和b两矢量间的夹角，如图3-2所示。 令b=i （i为b方向上的单位矢量），则
图3-2标量积
换句话说：一个矢量在另一个矢量上的投影等于该矢量与另一矢量 方向上单位矢量的点积。
再令a=j （j 为a方向上的单位矢量），则
即两矢量方向上单位矢量的点乘等于两矢量夹角的余弦。
二、坐标旋转
图3-6 坐标旋转
如图3-6，{B}与{A}有共同的坐标原点，但方位不同。令
和
分别是{A}和{B}中的单位主矢量，点P 在两
坐标系中各坐标轴上的坐标分量分别为：
和
所以有 利用点乘的性质和上式共同求解得
将
代入上面三式中并写成矩阵形式得
上式简写为： 此式称为坐标旋转方程。其中旋转矩阵 表示了坐标系{B}相 对于{A}的方位，正好与刚体姿态的描述相同。同理也可得
和 APCO APBO
进而有
例3.2 已知坐标系{B}初始位姿与{A}重合，首先{B}相对{A}的zA轴
转30°，再沿{A}的xA轴移动10个单位，并沿{A}的
,求 。
解：
zB zA
OB OA
xA30oxB
yB 30o
yA
zA zB
OA
(10,5,0)
xA
（3）矩阵与矩阵相乘： （4） 矩阵的转置：把矩阵的行换成同序数的列，记为
7. 矩阵的逆（逆矩阵） 8. 分块矩阵：分块后的矩阵与普通矩阵的运算相同。
9. 正交矩阵：如果
，则A为正交矩阵。它满足：
如果
是正交矩阵，则
行列式和矩阵的区别：矩阵是按一定方式排成的数表；行列式是 一个数。
三、矢量的点积（内乘积或标量积）
其中θ是a和b两矢量间的夹角，如图3-2所示。 令b=i （i为b方向上的单位矢量），则
图3-2标量积
换句话说：一个矢量在另一个矢量上的投影等于该矢量与另一矢量 方向上单位矢量的点积。
再令a=j （j 为a方向上的单位矢量），则
即两矢量方向上单位矢量的点乘等于两矢量夹角的余弦。
二、坐标旋转
图3-6 坐标旋转
如图3-6，{B}与{A}有共同的坐标原点，但方位不同。令
和
分别是{A}和{B}中的单位主矢量，点P 在两
坐标系中各坐标轴上的坐标分量分别为：
和
所以有 利用点乘的性质和上式共同求解得
将
代入上面三式中并写成矩阵形式得
上式简写为： 此式称为坐标旋转方程。其中旋转矩阵 表示了坐标系{B}相 对于{A}的方位，正好与刚体姿态的描述相同。同理也可得
和 APCO APBO
进而有
例3.2 已知坐标系{B}初始位姿与{A}重合，首先{B}相对{A}的zA轴
转30°，再沿{A}的xA轴移动10个单位，并沿{A}的
,求 。
解：
zB zA
OB OA
xA30oxB
yB 30o
yA
zA zB
OA
(10,5,0)
xA
ppt机器人正逆运动学解析
将上面两个方程两边平方相加，并利用和差化积公式得到
S2 S23 C2C23 cos3
于是有：
C3
(
pxC1
py S1
C234a4 )2 ( pz 2a2a3
S234a4 )2
a22
a32
已知 S3 1 C32
于是可得到：
3
arctan
S3 C3
依次类推，分别在方程2.19两边左乘A1~A4的逆，可得到
O4
x2
z5
y5
x4
O5
y4
z2
y2
关节3
A1 连杆2
O2 坐标系2
x5
o3 , o4 , o5重合 d4 d5 0
关节2 O1
z1
坐标系1
y1 连杆1
x1
d2
关节1 坐标系0
ai—沿 xi 轴， zi-1 轴与 xi 轴交点到Oi 的距离 αi — 绕 xi 轴，由 zi-1 转向zi di — 沿 zi-1 轴，zi-1 轴和 xi 交点至Oi –1 坐标
系原点的距离 θi — 绕 zi-1 轴，由 xi-1转向 xi
连杆0
z0 y0
d1 x0
O0
解：
例2、PUMA560运动学方程（六个自由度，全部是旋转关节） 关节变量都是θ
θ2
θ1
θ3
θ5
θ4 θ6
PUMA560机器人的连杆及关节编号
A1
O1 O0
A2
为右手坐标系，Yi轴：按右手定则
C234a4 ) S234a4 )
进而可得：
4 234 2 3
工业机器人技术基础4.5.1工业机器人的正运动学计算ppt课件
杆4
X5 Y6
杆5
X6
根据D-H表示法，确定了相邻 连杆坐标系{i}与{i-1}的齐次变 换矩阵，最后根据正向运动学 方程，写出机器人的总变换矩
阵
60T 01T •21T •32T •43T •54T •65T 01T (q1)•21T (q2 )•32T (q3 )•43T (q4 )•54T (q5 )•65T (q6 )
2 50 ( 2
6 4
2 ) 25 4
6 75 2
2 2
8
解法2
T Rot(zA,1)Trans(l1,0,0)Rot(zB ,2 )Trans(l2,0,0)
cos 45 sin 45 0 01 0 0 100cos(30 ) sin(30 ) 0 01 0 0 50
sin
45
cos 45
0 00 1 0
0
sin(30
)
cos(30 )
0 00 1 0
0
0
0 1 00 0 1 0 0
0
1 00 0 1 0
0
0
0 10 0 0
1
0
0
0
1
0
0
0
1
6 4
2 4
6 4
2 4
0
0
2 6 44 2 6 44
0
0
0
25
6 125 2
2
2
0
25
6 75 2
2 2
1
0
0
6 0
R
6 0
P
0
0
0
1
6
正向运动学计算-例题
• 某机器人有2个关节，分OA别OB位 于 ， 点，O机C 械手中心为 点。这三个点分别为三个坐 标系的原点，调整机器人各 关节l1 使100得,l2 末50端,1 操 45作,2器 最30终到 达指定位置（未沿Z轴发生平 移），其中
机器人技术基础课件第三章-机器人运动学精选全文完整版
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自由度机器人的运动学方程为：
第四章机器人学逆运动学方程ppt课件
这里 其中
f11 = C1 x＋S1 y
f12 = - z f13 = - S1 x＋C1 y
(4.10) (4.11) (4.12)
x =[ nx ox ax px ]T， y =[ ny oy ay py ]T， z =[ nz oz az pz ]T 由第三章得到的斯坦福机械手运动学方程式（3.48）为
同样比较式（4.48）等号两边矩阵的第2行第1列和第2行第2列元素可知
sin f12 (n)
（4.64）
cos f12 (o)
（4.65）
或
由此可得
sin sin nx cos ny cos sin ox cos oy
tan
1
sin sin
n o
x x
cos ny cos oy
d3 S2 C1 px S1 py C2 pz
（4.24） （4.25）
4
tan 1
C2
S1ax C1ay C1ax S1ay S2az
（4.26）
5
tan 1
C4
C2
C1ax
S1ay S2az S2 C1ax S1ay
S4 S1ax C2az
C1ay
由式（4.36）和式（4.43）可解出Ψ角
cos1 nz
sin
（4.43） （4.44） （4.45）
这里需要指出的是，在我们采用式（4.43）~式（4.45） 来计算θ、φ、Ψ时都是采用反余弦函数，而且式（4.43）和 式（4.45）的分母为sinθ，这会带来如下问题：
1）由于绝对值相同的正负角度的余弦相等，如cosθ＝ cos（-θ），因此不能确定反余弦的结果是在那个象限；
由于机械手各关节变量的相互耦合，后面计算的关节变量与前 面的关节变量有关，因此当前面关节变量的计算结果发生变化 时，后面关节变量计算的结果也会发生变化，所以逆运动方程 的解不是唯一的，我们应该根据机械手的组合形态和各关节的 运动范围，经过多次反覆计算，从中选择一组合理解。由此可 见，求解机械手的逆运动方程是一个十分复杂的过程。
电机拖动技术基础第三章机器人的运动学PPT课件
第三章 机器人的运动学
►3.1 刚体的位姿描述 ►3.2 坐标变换 ►3.3 齐次坐标和齐次变换 ►3.4 变换方程和欧拉角 ►3.5 机器人运动学的正问题和逆问题
3.1 刚体的位姿描述
一、位姿的定义
刚体参考点的位置（坐标系的位置）和刚体的姿态统称为刚体的位姿。
(为描述机器人本身的各个连杆之间.机器人和环境之间的运动关系,将
n
n o a
手爪的方位由旋转矩阵R规定。
R n
o
a
手爪的位置由位置矢量 p
规定。
代表手p 爪坐标系的原点。
则手爪的位姿可由四个矢量
来 来描述。
noa p
记为:
T n o a p
3.2 坐标变换
定义：由于空间中任意点P在不同坐标系中的描述不同，所以需要 研究从一个坐标系的描述到另一个坐标系的描述之间的变换关，通 常称为坐标变换。
{S}代表工作站(操作台)坐标系(工作站框)
{G}代表目标坐标系(目标框) 它们之间的位姿关系用相应的齐次变换来描述。图3-6 机器人与环境坐标系
B S
T描述工作站框{S}相对于基座{B}的位姿，
S G
T描述目标框{G}相对于工作站{S}的位姿。
对物体进行操作时(搬运或装配机器人)，工具框{T}相对目标框{G} 的位姿 直接GT T 影响操作效果。 是机GT T器人控制和轨迹规划的对象。
=
相对于固定坐标系运动 相对于活动坐标系运动
2.变换过程的可逆性
齐次坐标变换过程是可逆的. 若有 ，则逆变换
。
所以有 I44BATABT A B0R BP 1AO BA0R AP 1BO
A BR0BAR
A BRAPB1OBPAO
►3.1 刚体的位姿描述 ►3.2 坐标变换 ►3.3 齐次坐标和齐次变换 ►3.4 变换方程和欧拉角 ►3.5 机器人运动学的正问题和逆问题
3.1 刚体的位姿描述
一、位姿的定义
刚体参考点的位置（坐标系的位置）和刚体的姿态统称为刚体的位姿。
(为描述机器人本身的各个连杆之间.机器人和环境之间的运动关系,将
n
n o a
手爪的方位由旋转矩阵R规定。
R n
o
a
手爪的位置由位置矢量 p
规定。
代表手p 爪坐标系的原点。
则手爪的位姿可由四个矢量
来 来描述。
noa p
记为:
T n o a p
3.2 坐标变换
定义：由于空间中任意点P在不同坐标系中的描述不同，所以需要 研究从一个坐标系的描述到另一个坐标系的描述之间的变换关，通 常称为坐标变换。
{S}代表工作站(操作台)坐标系(工作站框)
{G}代表目标坐标系(目标框) 它们之间的位姿关系用相应的齐次变换来描述。图3-6 机器人与环境坐标系
B S
T描述工作站框{S}相对于基座{B}的位姿，
S G
T描述目标框{G}相对于工作站{S}的位姿。
对物体进行操作时(搬运或装配机器人)，工具框{T}相对目标框{G} 的位姿 直接GT T 影响操作效果。 是机GT T器人控制和轨迹规划的对象。
=
相对于固定坐标系运动 相对于活动坐标系运动
2.变换过程的可逆性
齐次坐标变换过程是可逆的. 若有 ，则逆变换
。
所以有 I44BATABT A B0R BP 1AO BA0R AP 1BO
A BR0BAR
A BRAPB1OBPAO
机器人学导论--ppt课件可编辑全文
关节变量
ppt课件
2
1.2 描述：位置、姿态和坐标系
位置描述
一旦建立坐标系，就能用一
个3*1的位置矢量对世界坐标 系中的任何点进行定位。因 为在世界坐标系中经常还要 定义许多坐标系，因此在位 置矢量上附加一信息，标明 是在哪一坐标系中被定义的。
例如：AP表示矢量P在A坐标系中的表示。
BP 表示矢量P在B坐标系中的表示。
c os90
c os120 c os30 c os90
XB XA
X
B
YA
X B Z A
c os90 c os90 cos0
]
YB X A YB YA YB Z A
ZB XA
ZB
YA
ZB Z A
ppt课件
5
坐标系的变换
完整描述上图中操作手位姿所需的信息为位置和姿态。机器人学中
在从多重解中选择解时，应根据具体情况，在避免碰撞的前 提下通常按“最短行程”准则来选择。同时还应当兼顾“多 移动小关节，少移动大关节”的原则。
ppt课件
23
4 PUMA560机器人运动学反解-反变换法
❖ 由于z4 , z5, z6 交于一点W,点W在基础坐标系中的位置仅与 1,2,3
有关。据此，可先解出 1,2,3 ，再分离出 4 ,5,6 ，并逐
PUMA560变换矩阵
ppt课件
21
将各个连杆变换矩阵相乘便得到PUMA560手臂变换矩阵
06T 01T (1)21T (2 )23T (3 )34T (4 )45T (5 )56T (6 )
什么是机器人运动学正解？ 什么是机器人运动学反解？
ppt课件
22
操作臂运动学反解的方法可以分为两类：封闭解和数值解、 在进行反解时总是力求得到封闭解。因为封闭解的计算速度 快，效率高，便于实时控制。而数值法不具有些特点为。 操作臂的运动学反解封闭解可通过两种途径得到：代数解和 几何解。 一般而言，非零连杆参数越多，到达某一目标的方式也越多， 即运动学反解的数目也越多。
机器人运动学正解逆解课件
机器人力控制
在机器人力控制中，需要知道每个关节的角度变化来调整 机器人的姿态和力矩。逆解可以用于求解每个关节的角度 变化，从而调整机器人的姿态和力矩。
机器人定位
在机器人定位中，需要知道每个关节的角度变化来调整机 器人的位置和姿态。逆解可以用于求解每个关节的角度变 化，从而调整机器人的位置和姿态。
04
实现复杂运动轨迹
利用运动学正解与逆解，可以规划出 复杂的运动轨迹，满足各种应用需求 。
02
机器人运动学正解
正解的基本概念
正解是指机器人末端执行器从某一初 始位置和姿态到达目标位置和姿态所 需经过的关节角度值。
正解是机器人运动学中的基本问题， 是实现机器人精确控制和自主导航的 基础。
正解的求解方法
逆解的求解方法
01
代数法
通过建立机器人关节角度与目标点坐标之间的方程组，利用数学软件求
解方程组得到关节角度。这种方法适用于简单的机器人结构，但对于复
杂机器人结构求解过程可能较为繁琐。
02
数值法
通过迭代或搜索的方法，不断逼近目标点坐标，最终得到满足要求的关
节角度。这种方法适用于复杂机器人结构，但求解时间较长且可能存在
机器人运动学正解逆解课件
目 录
• 机器人运动学概述 • 机器人运动学正解 • 机器人运动学逆解 • 机器人运动学正逆解的对比与联系 • 机器人运动学正逆解的实例分析
01
机器人运动学概述
定义与分类
定义
机器人运动学是研究机器人末端 执行器位姿与关节变量之间的关 系的学科。
分类
根据机器人的结构和运动特性， 可以分为串联机器人和并联机器 人。
局部最优解。
03
解析法
通过几何学和代数学的方法，直接求解关节角度与目标点坐标之间的关
在机器人力控制中，需要知道每个关节的角度变化来调整 机器人的姿态和力矩。逆解可以用于求解每个关节的角度 变化，从而调整机器人的姿态和力矩。
机器人定位
在机器人定位中，需要知道每个关节的角度变化来调整机 器人的位置和姿态。逆解可以用于求解每个关节的角度变 化，从而调整机器人的位置和姿态。
04
实现复杂运动轨迹
利用运动学正解与逆解，可以规划出 复杂的运动轨迹，满足各种应用需求 。
02
机器人运动学正解
正解的基本概念
正解是指机器人末端执行器从某一初 始位置和姿态到达目标位置和姿态所 需经过的关节角度值。
正解是机器人运动学中的基本问题， 是实现机器人精确控制和自主导航的 基础。
正解的求解方法
逆解的求解方法
01
代数法
通过建立机器人关节角度与目标点坐标之间的方程组，利用数学软件求
解方程组得到关节角度。这种方法适用于简单的机器人结构，但对于复
杂机器人结构求解过程可能较为繁琐。
02
数值法
通过迭代或搜索的方法，不断逼近目标点坐标，最终得到满足要求的关
节角度。这种方法适用于复杂机器人结构，但求解时间较长且可能存在
机器人运动学正解逆解课件
目 录
• 机器人运动学概述 • 机器人运动学正解 • 机器人运动学逆解 • 机器人运动学正逆解的对比与联系 • 机器人运动学正逆解的实例分析
01
机器人运动学概述
定义与分类
定义
机器人运动学是研究机器人末端 执行器位姿与关节变量之间的关 系的学科。
分类
根据机器人的结构和运动特性， 可以分为串联机器人和并联机器 人。
局部最优解。
03
解析法
通过几何学和代数学的方法，直接求解关节角度与目标点坐标之间的关
- 1、下载文档前请自行甄别文档内容的完整性，平台不提供额外的编辑、内容补充、找答案等附加服务。
- 2、"仅部分预览"的文档,不可在线预览部分如存在完整性等问题,可反馈申请退款(可完整预览的文档不适用该条件!)。
- 3、如文档侵犯您的权益，请联系客服反馈,我们会尽快为您处理(人工客服工作时间：9:00-18:30)。
§1.4 机器人正向运动学
工业机器人的正向运动学是指已知各关节的类型、相邻 关节之间的尺寸和相邻关节相对运动量的大小时，如何确 定工业机器人末端操作器在固定坐标系中的位姿。
主要包括以下内容： 1) 相对杆件的坐标系的确定； 2) 建立各连杆的模型矩阵A； 3) 正运动学算法；
D-H表示法
学习目标：1. 理解D-H法原理 2. 学会用D-H法对机器人建模
C234a4 ) S234a4 )
进而可得：
4 234 2 3
再 根 据 对 应 项 元 素 相 等， 可 以 得 到
S5 C23（4 C1ax S1a y ) S234az
C5 C1a y S1ax
5
arctan C234 (C1ax S1a x
#
d
a
1
1
0
0
90
2
2
0
a2
0
3
3
0
a3
0
4
4
0
a4
-90
5
5
0
0
90
6
6
0
0
0
nTn1 An1 Rot(z,n1 ) Trans (0,0, dn1 ) Trans (an1 ,0,0) Rot ( x,n1 )
C n1
An1
1 0
0
0
0
1
C3 S3 0 C3a3
A3
S3
0
C3 0
0
S
3a3
1 0
0
0
0
1
C4 0 S4 C4a4
A4
S4
0
0 1
C4 0
S4a4
0
0
0
0
1
C5 0 S5 0
A5
S5
0
0 1
C5 0
S n1
0
0
Sn1C n1 C n1C n1
S n1
0
S n1 S n1 Cn1S n1
C n1
0
an1C n1
a
n1
S
n1
dn1
1
#
d
a
1
1
0
0
90
2
2
0
a2
0
3
3
0
a3
0
4
4
0
a4
-90
5
5
0
O4
x2
z5
y5
x4
O5
y4
z2
y2
关节3
A1 连杆2
O2 坐标系2
x5
o3 , o4 , o5重合 d4 d5 0
关节2 O1
z1
坐标系1
y1 连杆1
x1
d2
关节1 坐标系0
ai—沿 xi 轴， zi-1 轴与 xi 轴交点到Oi 的距离 αi — 绕 xi 轴，由 zi-1 转向zi di — 沿 zi-1 轴，zi-1 轴和 xi 交点至Oi –1 坐标
0
90
C1 0 S1 0
A1
S1
0
0 1
C1 0
0 0
0
0
0
1
6
6
0
0
0
第四步：将参数代入A矩阵，可得到
C1 0 S1 0
A1
S1
0
0 1
C1 0
0 0
0
0
0
1
C2 S2 0 C2a2
A2
S2
0
C2 0
0
S
2a2
• 为右手坐标系 • 原点Oi： Ai与Ai+1关节轴线的交点
A6
y6
z6
A5
连杆5
• zi轴：与Ai+1关节轴重合，指向任意
x6
O6
关节6
关节5 坐标系4
• xi轴： Zi和Zi-1构成的面的法线 • yi轴：按右手定则
坐标系5
d6 z4
A4 z3
关节4 坐标系3
x3
连杆4
y3
O3
连杆3
A3
d3 A2
C234 (C1nx S1ny ) S234nz
C1ny S1nx S234 (C1nx S1ny )
C
234nz
0
C234 (C1ox S1oy )
S234oz C1oy S1ox
S234 (C1ox S1oy )
C 234oz 0
坐标系的确定
1.第一个关节指定为关节 n,第二个关节为n+1,其余 关节以此类推。
2.Z轴确定规则：如果关 节是旋转的，Z轴位于按 右手规则旋转的方向， 转角 为关节变量。如 果关节是滑动的，Z轴为 沿直线运动的方向，连 杆长度d为关节变量。关 节n处Z轴下标为n-1。
3.X轴确定规则 情况1：两关节Z轴既不平行也不相交 取两Z轴公垂线方向作为X轴方向，命名规则同Z轴。
S1a y
)
接下来再一次利用式
pxC1 p y S1 C234a4 C23a3 C2a2
pz S234a4 S23a3 S2a2
由于C12=C1C2-S1S2以及S12=S1C2+C1S2，最后得到：
2
arctan (C3a3 (C3a3
a2 )( pz S234a4 ) S3a3 ( pxC1 py S1 a2 )( pxC1 py S1 C234a4 ) S3a3 ( pz
axC1 a y S1 az
ax S1 a yC1 0
C234C5C6 S234 S6
S234C5C
6Hale Waihona Puke C234 S6
S5C6
0
C234C5C6 S234C6 S234C5C6 C234C6
S5 S6 0
PxC1 Py S1
pz
Px
S1
an α n 0 -900
a2
0
a3 -900
0 900
0 -900
00
例3
对下图所示简单机器人，根据D-H法，建立必要坐标系及 参数表。
第一步：根据D-H法建立坐标系的规则建立坐标系
第二步：将做好的坐标系简化为我们熟悉的线图形式
第三步：根据建立好的坐标系，确定各参数，并写 入D-H参数表
将上面两个方程两边平方相加，并利用和差化积公式得到
S2 S23 C2C23 cos3
于是有：
C3
(
pxC1
py S1
C234a4 )2 ( pz 2a2a3
S234a4 )2
a22
a32
已知 S3 1 C32
于是可得到：
3
arctan
S3 C3
依次类推，分别在方程2.19两边左乘A1~A4的逆，可得到
C234 (C1ax
S1a y ) S234ax C1a y S1a x S234 (C 1ax S1a y )
C 234a z 0
C234 (C1 px S1 py ) S234 pz
C34a2 C4a3 a4
0
S234 (C1Px
S1 Py
S1(C234a4
C23a3 C2a2 S234a4 S23a3
) S2a2
1
求逆运动学方程的解
依次用 A1左1 乘上面两个矩阵，得到：
nxC1 ny S1
nz
nx
S1
nyC1
0
oxC1 oy S1 oZ
ox S1 oyC1 0
情况2：两关节Z轴平行 此时，两Z轴之间有无数条公垂线，可挑选与前一关节的公垂线共线的 一条公垂线。 情况3：两关节Z轴相交
取两条Z轴的叉积方向作为X轴。 4.Y轴确定原则
取X轴、Z轴叉积方向作为Y轴方向。（右手）
5.变量选择原则
用θn+1角表示Xn到Xn+1绕Zn轴的旋转角;dn+1表示从Xn到Xn+1沿Zn测量
0 0
0
0
0
1
C6 S6 0 0
A6
S
6
0
C6 0
0 0 1 0
0
0 0 1
第5步 求出总变化矩阵
RTH A1 A2 A3 A4 A5 A6
C1(C234C5C6 S234 S6 ) S1S5C6
S1(C234C5C6 C1S5 S6
)
C 234
pz
S34a2 S4a3 1
C5C6 C5S6 S5 0
S5C
6
S5S6
C5
0
S6
C6
0 0
0
0
0 1
234
arctan(
az C1ax
S1a y
工业机器人的正向运动学是指已知各关节的类型、相邻 关节之间的尺寸和相邻关节相对运动量的大小时，如何确 定工业机器人末端操作器在固定坐标系中的位姿。
主要包括以下内容： 1) 相对杆件的坐标系的确定； 2) 建立各连杆的模型矩阵A； 3) 正运动学算法；
D-H表示法
学习目标：1. 理解D-H法原理 2. 学会用D-H法对机器人建模
C234a4 ) S234a4 )
进而可得：
4 234 2 3
再 根 据 对 应 项 元 素 相 等， 可 以 得 到
S5 C23（4 C1ax S1a y ) S234az
C5 C1a y S1ax
5
arctan C234 (C1ax S1a x
#
d
a
1
1
0
0
90
2
2
0
a2
0
3
3
0
a3
0
4
4
0
a4
-90
5
5
0
0
90
6
6
0
0
0
nTn1 An1 Rot(z,n1 ) Trans (0,0, dn1 ) Trans (an1 ,0,0) Rot ( x,n1 )
C n1
An1
1 0
0
0
0
1
C3 S3 0 C3a3
A3
S3
0
C3 0
0
S
3a3
1 0
0
0
0
1
C4 0 S4 C4a4
A4
S4
0
0 1
C4 0
S4a4
0
0
0
0
1
C5 0 S5 0
A5
S5
0
0 1
C5 0
S n1
0
0
Sn1C n1 C n1C n1
S n1
0
S n1 S n1 Cn1S n1
C n1
0
an1C n1
a
n1
S
n1
dn1
1
#
d
a
1
1
0
0
90
2
2
0
a2
0
3
3
0
a3
0
4
4
0
a4
-90
5
5
0
O4
x2
z5
y5
x4
O5
y4
z2
y2
关节3
A1 连杆2
O2 坐标系2
x5
o3 , o4 , o5重合 d4 d5 0
关节2 O1
z1
坐标系1
y1 连杆1
x1
d2
关节1 坐标系0
ai—沿 xi 轴， zi-1 轴与 xi 轴交点到Oi 的距离 αi — 绕 xi 轴，由 zi-1 转向zi di — 沿 zi-1 轴，zi-1 轴和 xi 交点至Oi –1 坐标
0
90
C1 0 S1 0
A1
S1
0
0 1
C1 0
0 0
0
0
0
1
6
6
0
0
0
第四步：将参数代入A矩阵，可得到
C1 0 S1 0
A1
S1
0
0 1
C1 0
0 0
0
0
0
1
C2 S2 0 C2a2
A2
S2
0
C2 0
0
S
2a2
• 为右手坐标系 • 原点Oi： Ai与Ai+1关节轴线的交点
A6
y6
z6
A5
连杆5
• zi轴：与Ai+1关节轴重合，指向任意
x6
O6
关节6
关节5 坐标系4
• xi轴： Zi和Zi-1构成的面的法线 • yi轴：按右手定则
坐标系5
d6 z4
A4 z3
关节4 坐标系3
x3
连杆4
y3
O3
连杆3
A3
d3 A2
C234 (C1nx S1ny ) S234nz
C1ny S1nx S234 (C1nx S1ny )
C
234nz
0
C234 (C1ox S1oy )
S234oz C1oy S1ox
S234 (C1ox S1oy )
C 234oz 0
坐标系的确定
1.第一个关节指定为关节 n,第二个关节为n+1,其余 关节以此类推。
2.Z轴确定规则：如果关 节是旋转的，Z轴位于按 右手规则旋转的方向， 转角 为关节变量。如 果关节是滑动的，Z轴为 沿直线运动的方向，连 杆长度d为关节变量。关 节n处Z轴下标为n-1。
3.X轴确定规则 情况1：两关节Z轴既不平行也不相交 取两Z轴公垂线方向作为X轴方向，命名规则同Z轴。
S1a y
)
接下来再一次利用式
pxC1 p y S1 C234a4 C23a3 C2a2
pz S234a4 S23a3 S2a2
由于C12=C1C2-S1S2以及S12=S1C2+C1S2，最后得到：
2
arctan (C3a3 (C3a3
a2 )( pz S234a4 ) S3a3 ( pxC1 py S1 a2 )( pxC1 py S1 C234a4 ) S3a3 ( pz
axC1 a y S1 az
ax S1 a yC1 0
C234C5C6 S234 S6
S234C5C
6Hale Waihona Puke C234 S6
S5C6
0
C234C5C6 S234C6 S234C5C6 C234C6
S5 S6 0
PxC1 Py S1
pz
Px
S1
an α n 0 -900
a2
0
a3 -900
0 900
0 -900
00
例3
对下图所示简单机器人，根据D-H法，建立必要坐标系及 参数表。
第一步：根据D-H法建立坐标系的规则建立坐标系
第二步：将做好的坐标系简化为我们熟悉的线图形式
第三步：根据建立好的坐标系，确定各参数，并写 入D-H参数表
将上面两个方程两边平方相加，并利用和差化积公式得到
S2 S23 C2C23 cos3
于是有：
C3
(
pxC1
py S1
C234a4 )2 ( pz 2a2a3
S234a4 )2
a22
a32
已知 S3 1 C32
于是可得到：
3
arctan
S3 C3
依次类推，分别在方程2.19两边左乘A1~A4的逆，可得到
C234 (C1ax
S1a y ) S234ax C1a y S1a x S234 (C 1ax S1a y )
C 234a z 0
C234 (C1 px S1 py ) S234 pz
C34a2 C4a3 a4
0
S234 (C1Px
S1 Py
S1(C234a4
C23a3 C2a2 S234a4 S23a3
) S2a2
1
求逆运动学方程的解
依次用 A1左1 乘上面两个矩阵，得到：
nxC1 ny S1
nz
nx
S1
nyC1
0
oxC1 oy S1 oZ
ox S1 oyC1 0
情况2：两关节Z轴平行 此时，两Z轴之间有无数条公垂线，可挑选与前一关节的公垂线共线的 一条公垂线。 情况3：两关节Z轴相交
取两条Z轴的叉积方向作为X轴。 4.Y轴确定原则
取X轴、Z轴叉积方向作为Y轴方向。（右手）
5.变量选择原则
用θn+1角表示Xn到Xn+1绕Zn轴的旋转角;dn+1表示从Xn到Xn+1沿Zn测量
0 0
0
0
0
1
C6 S6 0 0
A6
S
6
0
C6 0
0 0 1 0
0
0 0 1
第5步 求出总变化矩阵
RTH A1 A2 A3 A4 A5 A6
C1(C234C5C6 S234 S6 ) S1S5C6
S1(C234C5C6 C1S5 S6
)
C 234
pz
S34a2 S4a3 1
C5C6 C5S6 S5 0
S5C
6
S5S6
C5
0
S6
C6
0 0
0
0
0 1
234
arctan(
az C1ax
S1a y