机器人学-第3章_机器人运动学

{i-1} {R}，对应变换Rot(x, i-1 )；
2. 沿 XR轴平移 ai-1 {R} {Q}，对应变换Trans(ai-1 ,0,0)；
4. 沿 ZP 轴平移 di {P} {i}，对应变换Trans(0 ,0, di)
3. 绕 ZQ轴旋转 qi 角 {Q} {P}，对应变换Rot(z, qi )
2
三轮全向移动机器人运动学
双轮移动机器人运动中最大的问题是不 能横向移动，在实际应用中灵活性比较差。
全向移动轮是一种新的轮式移动机构，在 大轮的边缘上布置若干小轮，使得机器人的移 动方向不再限定于大轮所在的平面方向。
xoy是机器人坐标系，机器人的运动速度用 vx、vy和w表示，三个全向轮的角速度分别用w1 、w2和w3表示，v1、v2和v3分别表示三个全向轮 轮心处的线速度。假设全向轮的半径为R，距运 动机构中心的距离为L，则速度间关系为：
构参数。如果机器人6个关节均为转动关节，18个固定参数可以用6组（ai-1, i-
1, di）表示。
空间机械臂坐标系选择
为了获得机械臂末端执行器在3维空间的位置和姿态，需要在每个连杆上 定义与连杆固连的坐标系来描述相邻连杆之间的位置关系。
根据固连坐标系所在连杆的编号对固连坐标系命名，如在固连在连杆i上 的固连坐标系称为坐标系{i}。
若ai =0，两Z轴相交，则选Xi垂于Zi和Zi+1 ，坐标系{i}的选择不是唯一的。
9
轴i θi
轴 i-1
连杆坐标系中连杆参数确定
θi-1
连杆 i-1
DH参数按以下方法确定：
Zi
ai =沿Xi轴，从Zi移动到Zi+1的距离；
Yi
i =绕Xi轴，从Zi旋转到Zi+1的角度；
di =沿Zi轴，从Xi-1移动到Xi的距离；
R R
vy wL vx sin
vy
cos
w
L
场地坐标系下的速度Vx、Vy和W 与机器人 坐标系下机器人速度之间的变换关系如下：
Vx cosq
VWy
sinq 0
sin q cosq
0
0 vx
0 1
wvy
vx cosq
wvy
sin 0
q
sinq cosq
0
0 Vx
0 1
o
X
由（3-1）式可得运动学约束条件，x&sinq y&cosq 0 平面轮式移动机器人
是所谓的“非完整约束”。物理含义是，机器人不能沿轮轴线方向横移。
设轮距为D，轮半径为r，两轮独立驱动时轮子转速wL，wR 则
v
r 2
wR
wL
,
w
r D
wR
wL
（3-2）
1
v
r 2
wR
wL
,
w
r D
wR
wL
q2
A
两连杆平面旋转关节机械臂，其结构由连
q1
x
杆长度L1，L2和关节角q1，q2确定。
平面机械臂
表示关节位置的变量q1，q2称为关节变量。
旋转关节变量用关节角q表示，而移动关节变量用移动距离d表示。
机械手末端位置与关节角之间的关系为
x L1c1 L2c12 y L1s1 L2s12
其中 c1=cosq1，c12=cos(q1+q2)， s1=sinq1，s12=sin(q1+q2)。
v1 w1R vx sin vy cos wL v2 w2R vy wL v3 w3R vx sin vy cos wL
（3-5）
全向移动轮
三轮全向移动机构
3
w1R vx sin vy cos wL
三个全www132向 轮R1的角ssini0速n度与ccoo机ss1器人LLL速wvvxy度 之（间3关-6系）：ww32
因此，可以得到 q2 = -
q1+β和β都可计算，因此q1也是可以计算的。
tan q1 y / x , tan L2 sinq2 /(L1 L2 cosq2)
平面机械臂简图
因此，q1 atan y / x atan y / x atanL2 sinq2 /(L1 L2 cosq2)
8
轴i
θi
轴 i-1
连杆 i
固连在基座上的坐标系称为坐标系
θi-1
连杆 i-1
{0}。该坐标系在机械臂运动过程中保
持固定，因此在研究机械臂运动学问
题时一般把坐标系{0}选为参考系，用
来描述其它连杆坐标系的位置。 ai
原则上参考系{0}可以任意设定，
di
但为了简化描述，通常设定Z0轴沿关 节轴1的方向，并且关节1的关节变量
Z2
a2=L2 2=0 q2=90o d2=-L1
轴i θi
连杆 i-1
Zi Yi
Yi-1
di
Xi ai
ai-1
qi
Zi-1
Xi-1 i-1
坐标系{i}选择示意图
L1
L2
11
q3
L3
L2
例3-1 如图3-9所示的平面三连杆机械臂，因为三个 关节均为旋转关节，称为RRR（或3R）机构。请在 该机构上建立连杆坐标系并写出DH参数。
qi =绕Zi轴，从Xi-1旋转到Xi的角度；
以上4个参数中，ai表示连杆长度只能取
Zi-1
非负值；而其余3参数可以为正，也可以为负。
Yi-1
di
ai-1
Xi-1 i-1
Xi ai qi
建立连杆坐标系的步骤
1. 找出各关节轴，并标出轴的延长线。步骤2-5 坐标系{i}选择示意图
仅考虑两个相邻关节轴（i 和 i+1）和坐标系{i}。
相邻连杆间坐标变换公式
建立 {P}、{Q}和{R}3个中间坐标系， 其中{i}和{i-1}是固定在连杆 i 和 i-1 上的固 连坐标系，如图3-13所示。
连杆 i-1 Zi
ZP
Xi ai
di ZQ XQ
ZR
qi
Zi-1
Xi-1XR ai-1
XP
i-1
1. 绕 Xi-1 轴旋转 i-1角
图3-13中间坐标系选择示意图
杆为连杆n。为了使机械臂末端执行器可以在3维空间达到任意的位置
和姿态，机械臂至少需要6个关节，因此，典型的工业机械臂一般都具
有6个关节。
6
轴i
θi
用一条空间直线表示关节的转轴
轴 i-1
连杆 i
（平移轴），连杆i 的运动可以用 转轴i 和连杆i相对连杆i-1的转动
θi-1
连杆 i-1
角度qi 来描述。下面给出几个连
第3章 机器人运动学
运动学研究物体的位姿、速度和加速度之间的关系。
本章将介绍双轮移动机器人、三轮全向移动机器人和关节式机械臂的运 动学问题。
双轮移动机器人运动学
Y
v
（x, y, q）表示双轮机器人位姿，v 表示机器人前
q
进速度，表示机器人转动速度w，则
xy&&
v v
cosq sin q
q& w
w
（3-1）
采用矢量表示为 r = f(q )
式中f 表示矢量函数，r =[x,y]T，q=[q1, q2]T。 从关节变量q 求手爪位置 r 称为正运动学，
反之，从手爪位置r求关节变量q 称为逆运动学。
5
逆运动学公式：
△OAB中 可以根据余弦定理确定
arccos [L12 L22 (x2 y2)] / 2L1L2
2. 找出关节轴i和i+1之间的公垂线或两个轴的交点，以两个轴的交 点或公垂线与关节轴i的交点为坐标系{i}的原点。
10
3. 规定Zi沿关节轴i的方向。
轴 i-1
4. 规定Xi沿公垂线指向关节轴i+1，若两个 θi-1
轴相交，规定Xi垂直于两轴所在的平面。
5. 按右手定则确定Yi轴。
6. 当第一个关节变量为0时坐标
系{1}与坐标系{0}重合。
对于坐标系{n}，原点位置可以在关节轴
上任意选取， Xn的方向也是任意的。但在选 择时应尽量使更多的连杆参数为0。
(a) Z1
Y1
X2 Z2
X1
a1=0 1=-90o d1=0
Y2
a2=L2 2=0 q2=-90o d2=L1
(b)
Z1
X2
Y2
Y1
X1
a1=0 1=90o d1=0
q2 L1
定义参考坐标系{0}，它固定在基座上，当第一
个关节变量（q1）为0时坐标系{1}与坐标系{0}重合
，因此建立参考坐标系{0}如图所示，Z0轴与关节1 的轴线重合且垂直于机械臂所在平面。
q1
平面3R机械臂
由于机械臂位于一个平面上，因此所有Z轴相互平
X3
行，且连杆偏距d和连杆转角均为0。该机械臂的DH
逆运动学的解一般不唯一，显然图中机械臂关于OB轴对称的位置也 是逆运动学问题的一个解。
空间机械臂连杆描述
机械臂可以看成一系列刚体通过关节连接而成的链式运动机构。 一般把这些刚体称为连杆，通过关节将相邻的连杆连接起来。旋转关 节和移动关节是机械臂设计中经常采用的单自由度关节。
称基座为连杆0。第一个可移动连杆为连杆1，机械臂的最末端连
因为所有变换都是相对于动坐标系的，所 以坐标系{i}和{i-1}之间的变换矩阵为：
i i
1T
= Rot(x,i-1)Trans(ai-1,0,0)Rot(z,qi)Trans(0,0,di)
13
其中各独立变换矩阵如下：
Rot(x,i-1)Trans(ai-1,0,0)Rot(z,qi)Trans(0,0,di)
关节角，两相邻连杆绕公共轴线旋转的角度称为关节角。
7
机器人的每个连杆都可以用以上四个参数描述，其中连杆长度和连杆转 角描述连杆本身，连杆偏距和关节角描述连杆之间的连接关系。
对于转动关节，qi为关节变量，其它三个参数是常数；对于移动关节，di
为关节变量，其它三个参数是常数。
这种用连杆参数描述机构运动学关系的规则称为（Devanit-Hartenberg） DH方法，连杆参数称为DH参数。 对于一个6关节机器人，需要18个参数就可以完全描述机械臂固定的运动学结
动距离分别为lR = rR和lL = rL，
机器人移动距离
l=(lR+lL)/2
方位角变化
q =(lR-lL)/D。
第n步机器人位姿可以按下面公式更新：
qn qn1 q
xn
xn1
l
cos qn1
q
/
2
yn
yn1
l
sin qn1
q
/
2
若已知机器人的初始位姿，根据该递推公式可以确定任意时刻机器
人位姿，比较简单，但因积累误差大，所以长时间不可靠。
sq
0
cqi
0
0 0 1 0
0
0 0 1
连杆间的通用变换公式：
cqi
sqi
对于任意的n连杆 机械臂，只要给出各 连杆的DH参数，即可
ii Βιβλιοθήκη Baidu T
ssqqii
ci 1 si 1
0
cqi ci 1 cqi si1
0
以计算机械臂末端在 固定坐标系{0}下表示
0nT 01T 21T L
T n1 n
0
1 0
0 0
Rot(
x, i 1 )
0 0
0
ci1 si1
0
si1 ci1
0
0 0 1
1 0 0 ai1
Trans(ai 1 ,
0,
0)
0 0
1 0
0 1
0 0
0 0 0 1
1 0 0 0
Trans(0,
0,
di
)
0 0
0
1 0 0
0 1 0
0
di 1
cqi sqi 0 0
Rot(z,qi
)
杆参数的定义：
连杆长度，连杆两端关节轴线间 公垂线的长度
连杆转角，过关节轴i-1做垂直 于公垂线的平面，在该平面内 做过垂足且平行于关节轴i 的直 线。该直线与关节轴i-1的夹角 定义为连杆转角。
连杆转角只在 两个关节轴为 空间异面直线 的情况有意义
ai
di qi
ai-1
i-1 连杆描述
连杆偏距，关节轴i与相邻关节转轴(i-1和i+1) 公垂线间距离称为连杆偏距
VWy
图3-4全向移动机构在场 地坐标系中的位置
w1 ww32
1 R
sin
0
sin
cos
1
cos
L cosq L sinq
L 0
sin q cosq
0
0 Vx
0 1
VWy
可以非常方便地通过控制电机的转速来控制机器人在场地坐标
系下的移动和转动速度。
4
平面机械臂运动学
y
L1
B L2
qi ai-1
为0时参考系{0}与坐标系{1}重合。
因此，当关节1为转动关节时a0=0，0=0， d1=0；当关节1为移动关节时a0=0，0=0，q1 =0。
i-1
坐标系{i}选择示意图
对于中间连杆i，坐标系{i}的Zi轴与关节轴i重合，坐标系{i}的原点位于 公垂线ai与关节轴i的交点处。Xi沿ai方向由关节i指向关节i+1，并按照右手 系规则确定Yi，ai按右手定则绕Xi转角定义。
si 1 ci 1
给定期望的机器人前进速度v，转动速度w，则可以确定机器人的两轮转速为
wR (v Dw / 2) / r, wL (v Dw / 2) / r
（3-3）
因此，可以非常方便地通过控制电机的转速来控制机器人移动和转动速度。
机器人位置估计
已知初始位姿为（x0, y0, q0），两轮转角增量为L和 R，则两轮移
参数如表3-1所示。
表3-1 3R机械臂DH参数
Y3
Y2
X2
Y0
i
i-1
ai-1
di
qi
Y1
X1
1
0
0
0
q1
X0
2
0
L1
0
q2
连杆坐标系布局
3
0
L2
0
q3
12
轴i θi
空间机械臂运动学
轴 i-1
θi-1
本节将导出相邻连杆间坐标系变换的一 般形式，然后将这些独立的变换联系起来 求出连杆n相对连杆0的位置和姿态。