机器人运动学
相关主题
- 1、下载文档前请自行甄别文档内容的完整性，平台不提供额外的编辑、内容补充、找答案等附加服务。
- 2、"仅部分预览"的文档,不可在线预览部分如存在完整性等问题,可反馈申请退款(可完整预览的文档不适用该条件!)。
- 3、如文档侵犯您的权益，请联系客服反馈,我们会尽快为您处理(人工客服工作时间：9:00-18:30)。
第6、7讲 机器人位置运动学
Kinematics of Robotics
机器人正向运动学（运动学正解）
已知所有连杆长度和关节角度，计算机器人手的位姿
机器人逆向运动学（运动学逆解）
已知机器人手的位姿，计算所有连杆长度和关节角度
机器人运动学分析步骤和内容
一、机器人连杆参数及其D-H坐标变换
(连杆参数/连杆坐标系及D-H连杆变换)
i i i i i
c i s i 0 0
s i c i c i c i s i 0
s i s i c i s i c i 0
ai c i ai s i di 1
（三）移动连杆坐标系及连杆的D-H坐标变换
移动连杆坐标系的建立
（二）转动连杆坐标系及连杆的D-H坐标变换
转动连杆坐标系的建立
• 坐标轴Zi：与i+1关节的轴线重合； • 坐标轴Xi：沿连杆i两关节轴线的公垂线,指向i+1关节； • 坐标轴Yi：按右手直角坐标系法则确定；
• 坐标原点Oi：
（1）当关节i轴线和关节i+1轴线相交时，取交点； （2）当关节i轴线和关节i+1轴线异面时，取两轴线的公垂 线与关节i+1轴线的交点； （3）当关节i轴线和关节i+1轴线平行时，取关节i+1轴线与 关节i+2轴线的公垂线与关节i+1轴线的交点；
二、机器人运动学方程
（一）运动学方程
• 机械手可以看成由一系列关节连接起来的连杆组构成。
• 给每一个连杆在关节处设置一个连杆坐标系，该连杆坐标系 随关节运动而运动。
二、 机器人运动学方程
1、A矩阵和T矩阵
• 用A矩阵描述连杆坐标系间相对平移和旋转 的齐次变换。 • A1表示第一连杆对基坐标的位姿， A2表示 第二连杆对第一连杆位姿…… • 则第二连杆对基坐标的位姿为 T2 A1 A2 • 手爪相对于基座的位姿
二、机器人运动学方程
(运动学方程/典型机器人运动学方程)
三、机器人逆运动学
(机器人运动学逆解有关问题/典型臂运动学逆解)
一、机器人连杆参数及其D-H坐标变换
在驱动装置带动下，连杆将绕或沿关节轴线， 相对于前一临近连杆转动或移动。
（一）连杆参数
（一）连杆参数
• 连杆的尺寸参数
连杆长度ai：两个关节轴线i和i+1 沿共垂线的距离； 连杆扭角αi ：两个关节轴线i和i+1的夹角；
0 s3 0 c3 1 0 0 0
a3c3 a3 s3 d3 1
0 a 2 c2 0 a2 s2 1 d2 0 1 s4 0 c4 0 0 0 0 1
c5 s A5 5 0 0
0 s5 0 c5 1 0 0 0
Байду номын сангаас
（1）绕Zi-1轴转θi ；Rot(Zi-1,θi)
（2）沿Zi-1轴移动di ；Trans(Zi-1,di) （3）沿Xi轴移动ai ；Trans(Xi,ai)
（4）绕Xi轴转αi ；Rot(Xi,αi)
• 由于以上变换都是相对于动坐标系的，根据“由左向右”的原则可求 出变换矩阵： A Rot( z , )Trans(0,0, d )Trans(a ,0,0) Rot( x, )
• PUMA560六自由度机械手
c1 0 s1 s 0 c1 1 A1 0 1 0 0 0 0
0 0 0 1
c2 s A2 2 0 0
c4 s A4 4 0 0
s2 c2 0 0
0 0 1 0
c3 s A3 3 0 0
T6 A1 A2 A3 A4 A5 A6
注意前后 顺序
二、 机器人运动学方程
2、手爪位姿的表示
位置矢量P：两手指连线的中点（手爪坐标系的原点）； 接近矢量a：夹持器进入物体的方向（手爪坐标系的Z轴）； 方向矢量o：指尖互相指向（手爪坐标系的Y轴）； 法线矢量n：垂直手掌面的方向（手爪坐标系的X轴）；
移动连杆坐标系的规定：
• 坐标轴Zi：与i+1关节的轴线重合； • 坐标轴Xi：沿移动关节i轴线与关节i+1轴线的公垂线,指向i+1关节； • 坐标轴Yi：按右手直角坐标系法则确定； • 坐标原点Oi： （1）当关节i轴线和关节i+1轴线相交时，取交点； （2）当关节i轴线和关节i+1轴线异面时，取两轴线的公垂线与关节i轴 线的交点； （3）当关节i轴线和关节i+1轴线平行时，取关节i+1轴线与关节i+2轴线 的公垂线与关节i+1轴线的交点；
n oa oo 1 aa 1 oa 1
nx n T T6 y nz 0
ox oy oz 0
ax ay az 0
px py pz 1
二、 机器人运动学方程
3、机器人运动学方程 • 由手爪相对于基座的两种位姿表示，可得：
nx n T T6 y nz 0
球面（极）坐标臂（RRP）
z3
三个连杆长度分别为： d1、d2、d3 ，其中d3是 变量
参数 θi 连杆
θ2 z1 x2
x3 z2
di d1 d2 d3
ai 0 0 0
αi -90º 90º 0
1 2 3
θ1 θ2 0
x1
z0
θ1 x0 y0
A1 Rot( Z , θ 1)Trans(0,0, d 1) Rot( x, π / 2) A2 Rot( Z , θ )Trans(0,0, d 2) Rot( x,π / 2) A3 Trans(0,0, d 3) T A1 A2 A3 c1c2 s1 c1s2 d 3c1s2 d 2 s1 s c c s s d s s d c 1 2 1 1 2 3 1 2 2 1 s2 0 c2 d 3c2 d1 0 0 1 0
• 存在性：对于给定的位姿，至少存在一组 关节变量来产生希望的机器人位姿；如果 给定机械手位置在工作空间外，则解不存 在。
（一）机器人运动学逆解有关问题
• 唯一性：对于给定的位姿，仅有一组关节变量来产生希望的机器人位 姿。对于机器人，可能出现多解。 • 机器人运动学逆解的数目取决于关节数目、连杆参数和关节变量的活 动范围。一般，非零连杆参数越多，运动学逆解数目越多（多至16 个）。 • 如何从多重解中选择出其中的一组？应根据具体情况而定，在避免碰 撞的前提下，通常按最短行程的准则来择优，使每个关节的移动量为 最小。 • 由于工业机器人前面三个连杆的尺寸较大，后面三个较小，故应加权 处理，遵循多移动小关节、少移动大关节的原则。
移动连杆坐标系的建立
移动连杆前的相邻连杆坐标系的规定：
• 坐标轴Zi-1：过原点Oi且平行于移动关节i的轴线； • 坐标轴Xi-1：沿移动关节i-1轴线与Zi-1轴线的公垂 线,指向Zi-1轴线； • 坐标轴Yi-1：按右手直角坐标系法则确定；
• 坐标原点Oi-1：关节轴线i-1和Zi-1轴的公垂线与Zi-1 轴的交点；
ox oy oz 0
ax ay az 0
px py ＝A1 A2 A3 A4 A5 A6 pz 1
• 方程左边是手爪相对基座的位置和姿态，方程右边是各连 杆A矩阵的乘积（是n个关节变量的函数），上式称为机器 人的运动学方程。
典型机器人运动学方程
• 圆柱坐标臂（PRP）
参数 θi 连杆
0 0 0 1
c6 s A6 6 0 0
s6 c6 0 0
0 0 1 0
0 0 0 1
机器人末端位置和姿态为：T A1 A2 A3 A4 A5 A6
该机械手末端的位置方程如下：
Px c1[d 6(c23c4 s5 s 23c5) d 4 s 23 a2c2] s1(d 6 s 4 s5 d 2) Py s1[d 6(c23c4 s5 s 23c5) d 4 s 23 a2c2] c1(d 6 s 4 s5 d 2) Pz d 6(c 23c5 s 23c4 s5) d 4c 23 a 2 s 2
三、机器人逆运动学
nx n T T6 y nz 0 ox oy oz 0 ax ay az 0 px py ＝A1 A2 A3 A4 A5 A6 pz 1
• 1)问题:已知手部位姿,求各关节位置 • 2)意义:是机械手控制的关键
（一）机器人运动学逆解有关问题
• 相邻连杆的关系参数
连杆偏置di ： Xi-1沿Zi-1至Xi的距离，沿Zi-1正向时为正； 关节转角θi ：Xi-1绕Zi-1转至Xi的转角，符号根据右手定则 确定；
转动连杆坐标系的D-H变换
• 转动连杆的D-H参数为θi、ai、αi 、di，其中关节变量是θi 。这四 个参数确定了连杆i相对于连杆i-1的位姿，即D-H坐标变换矩阵Ai。 • 坐标系{i-1}经过下面四次有序的相对变换可得到坐标系{i}：
移动连杆坐标系的建立
• 首连杆0：基座坐标系{0}是固定不动的；Z0
轴取关节1的轴线，O0的设置任意，通常与 O1重合； • 末连杆n：工具坐标系{n}固定在机器人的 终端，由于连杆n的终端不再有关节，约定 坐标系{n}与{n-1}平行；
再看移动连杆参数的含义
• 由于移动连杆的OiZi轴线平行于移动关节轴 线移动， OiZi在空间的位置是变化的，因而 ai参数无意义。连杆i的长度在坐标系{i-1} 中考虑， 故参数ai=0 。原点Oi的零位与Oi-1 重合，此时移动连杆的变量di=0 。
di d1 0 d3
ai 0 a2 0
αi 0 -90º 0
1 2 3
0 θ2 0
A1 Trans(0,0, d 1) A2 Rot( Z , θ )Trans( x, a 2) Rot( x, π / 2) A3 Trans(0,0, d 3) T A1 A2 A3
c2 s 2 0 0 0 s2 0 c2 1 0 0 0 d 3 s 2 a 2 c2 d 3c2 a2 s2 d1 1
转动连杆坐标系的建立
• 首连杆0：基座坐标系{0}是固定不动的；Z0
轴取关节1的轴线，O0的设置任意，通常与 O1重合； • 末连杆n：工具坐标系{n}固定在机器人的 终端，由于连杆n的终端不再有关节，约定 坐标系{n}与{n-1}平行；
再看转动连杆参数的含义
• 连杆的尺寸参数
连杆长度ai：Zi和Zi-1沿Xi的距离，总为正；； 连杆扭角αi ：Zi-1绕Xi转至Zi的转角，符号根据右手定则确 定；
• 相邻连杆的关系参数
连杆偏置di ：沿关节i轴线方向，两个共垂线之间的距离；
关节转角θi ：垂直于关节轴线的平面内，两个共垂线之 间的夹角；
关节变量
• 旋转关节：
关节转角θi是关节变量，连杆长度ai、连杆 扭角αi 、连杆偏置di 是固定不变的；
• 移动关节：
连杆偏置di是关节变量，连杆长度ai 、连杆 扭角αi 、关节转角 θi是固定不变的；
转动坐标臂（RRR）
Z0
X1 X0 Z1 Z2 Z3 X2 X3
参数 θi 连杆
di 0
ai 0
αi 90º
1
θ1
2
3
θ2
θ3
0
0
a2
a3
0
0
A1 Rot( Z , θ 1) Rot( x,π / 2) A2 Rot( Z , θ )Trans(a 2,0,0) A3 Rot( Z , θ 3)Trans(a 3,0,0) T A1 A2 A3
移动连杆坐标系的D-H变换
• 移动连杆的D-H参数为θi、ai、αi 、di，其中关 节变量是di 。用与求转动连杆坐标系相同的方法 可求出移动连杆的D-H变换矩阵：
Ai Rot( z , i )Trans(0,0, d i )Trans(ai ,0,0) Rot( x, i ) c i s i 0 0 s i c i c i c i s i 0 s i s i c i s i c i 0 0 0 di 1
Kinematics of Robotics
机器人正向运动学（运动学正解）
已知所有连杆长度和关节角度，计算机器人手的位姿
机器人逆向运动学（运动学逆解）
已知机器人手的位姿，计算所有连杆长度和关节角度
机器人运动学分析步骤和内容
一、机器人连杆参数及其D-H坐标变换
(连杆参数/连杆坐标系及D-H连杆变换)
i i i i i
c i s i 0 0
s i c i c i c i s i 0
s i s i c i s i c i 0
ai c i ai s i di 1
（三）移动连杆坐标系及连杆的D-H坐标变换
移动连杆坐标系的建立
（二）转动连杆坐标系及连杆的D-H坐标变换
转动连杆坐标系的建立
• 坐标轴Zi：与i+1关节的轴线重合； • 坐标轴Xi：沿连杆i两关节轴线的公垂线,指向i+1关节； • 坐标轴Yi：按右手直角坐标系法则确定；
• 坐标原点Oi：
（1）当关节i轴线和关节i+1轴线相交时，取交点； （2）当关节i轴线和关节i+1轴线异面时，取两轴线的公垂 线与关节i+1轴线的交点； （3）当关节i轴线和关节i+1轴线平行时，取关节i+1轴线与 关节i+2轴线的公垂线与关节i+1轴线的交点；
二、机器人运动学方程
（一）运动学方程
• 机械手可以看成由一系列关节连接起来的连杆组构成。
• 给每一个连杆在关节处设置一个连杆坐标系，该连杆坐标系 随关节运动而运动。
二、 机器人运动学方程
1、A矩阵和T矩阵
• 用A矩阵描述连杆坐标系间相对平移和旋转 的齐次变换。 • A1表示第一连杆对基坐标的位姿， A2表示 第二连杆对第一连杆位姿…… • 则第二连杆对基坐标的位姿为 T2 A1 A2 • 手爪相对于基座的位姿
二、机器人运动学方程
(运动学方程/典型机器人运动学方程)
三、机器人逆运动学
(机器人运动学逆解有关问题/典型臂运动学逆解)
一、机器人连杆参数及其D-H坐标变换
在驱动装置带动下，连杆将绕或沿关节轴线， 相对于前一临近连杆转动或移动。
（一）连杆参数
（一）连杆参数
• 连杆的尺寸参数
连杆长度ai：两个关节轴线i和i+1 沿共垂线的距离； 连杆扭角αi ：两个关节轴线i和i+1的夹角；
0 s3 0 c3 1 0 0 0
a3c3 a3 s3 d3 1
0 a 2 c2 0 a2 s2 1 d2 0 1 s4 0 c4 0 0 0 0 1
c5 s A5 5 0 0
0 s5 0 c5 1 0 0 0
Байду номын сангаас
（1）绕Zi-1轴转θi ；Rot(Zi-1,θi)
（2）沿Zi-1轴移动di ；Trans(Zi-1,di) （3）沿Xi轴移动ai ；Trans(Xi,ai)
（4）绕Xi轴转αi ；Rot(Xi,αi)
• 由于以上变换都是相对于动坐标系的，根据“由左向右”的原则可求 出变换矩阵： A Rot( z , )Trans(0,0, d )Trans(a ,0,0) Rot( x, )
• PUMA560六自由度机械手
c1 0 s1 s 0 c1 1 A1 0 1 0 0 0 0
0 0 0 1
c2 s A2 2 0 0
c4 s A4 4 0 0
s2 c2 0 0
0 0 1 0
c3 s A3 3 0 0
T6 A1 A2 A3 A4 A5 A6
注意前后 顺序
二、 机器人运动学方程
2、手爪位姿的表示
位置矢量P：两手指连线的中点（手爪坐标系的原点）； 接近矢量a：夹持器进入物体的方向（手爪坐标系的Z轴）； 方向矢量o：指尖互相指向（手爪坐标系的Y轴）； 法线矢量n：垂直手掌面的方向（手爪坐标系的X轴）；
移动连杆坐标系的规定：
• 坐标轴Zi：与i+1关节的轴线重合； • 坐标轴Xi：沿移动关节i轴线与关节i+1轴线的公垂线,指向i+1关节； • 坐标轴Yi：按右手直角坐标系法则确定； • 坐标原点Oi： （1）当关节i轴线和关节i+1轴线相交时，取交点； （2）当关节i轴线和关节i+1轴线异面时，取两轴线的公垂线与关节i轴 线的交点； （3）当关节i轴线和关节i+1轴线平行时，取关节i+1轴线与关节i+2轴线 的公垂线与关节i+1轴线的交点；
n oa oo 1 aa 1 oa 1
nx n T T6 y nz 0
ox oy oz 0
ax ay az 0
px py pz 1
二、 机器人运动学方程
3、机器人运动学方程 • 由手爪相对于基座的两种位姿表示，可得：
nx n T T6 y nz 0
球面（极）坐标臂（RRP）
z3
三个连杆长度分别为： d1、d2、d3 ，其中d3是 变量
参数 θi 连杆
θ2 z1 x2
x3 z2
di d1 d2 d3
ai 0 0 0
αi -90º 90º 0
1 2 3
θ1 θ2 0
x1
z0
θ1 x0 y0
A1 Rot( Z , θ 1)Trans(0,0, d 1) Rot( x, π / 2) A2 Rot( Z , θ )Trans(0,0, d 2) Rot( x,π / 2) A3 Trans(0,0, d 3) T A1 A2 A3 c1c2 s1 c1s2 d 3c1s2 d 2 s1 s c c s s d s s d c 1 2 1 1 2 3 1 2 2 1 s2 0 c2 d 3c2 d1 0 0 1 0
• 存在性：对于给定的位姿，至少存在一组 关节变量来产生希望的机器人位姿；如果 给定机械手位置在工作空间外，则解不存 在。
（一）机器人运动学逆解有关问题
• 唯一性：对于给定的位姿，仅有一组关节变量来产生希望的机器人位 姿。对于机器人，可能出现多解。 • 机器人运动学逆解的数目取决于关节数目、连杆参数和关节变量的活 动范围。一般，非零连杆参数越多，运动学逆解数目越多（多至16 个）。 • 如何从多重解中选择出其中的一组？应根据具体情况而定，在避免碰 撞的前提下，通常按最短行程的准则来择优，使每个关节的移动量为 最小。 • 由于工业机器人前面三个连杆的尺寸较大，后面三个较小，故应加权 处理，遵循多移动小关节、少移动大关节的原则。
移动连杆坐标系的建立
移动连杆前的相邻连杆坐标系的规定：
• 坐标轴Zi-1：过原点Oi且平行于移动关节i的轴线； • 坐标轴Xi-1：沿移动关节i-1轴线与Zi-1轴线的公垂 线,指向Zi-1轴线； • 坐标轴Yi-1：按右手直角坐标系法则确定；
• 坐标原点Oi-1：关节轴线i-1和Zi-1轴的公垂线与Zi-1 轴的交点；
ox oy oz 0
ax ay az 0
px py ＝A1 A2 A3 A4 A5 A6 pz 1
• 方程左边是手爪相对基座的位置和姿态，方程右边是各连 杆A矩阵的乘积（是n个关节变量的函数），上式称为机器 人的运动学方程。
典型机器人运动学方程
• 圆柱坐标臂（PRP）
参数 θi 连杆
0 0 0 1
c6 s A6 6 0 0
s6 c6 0 0
0 0 1 0
0 0 0 1
机器人末端位置和姿态为：T A1 A2 A3 A4 A5 A6
该机械手末端的位置方程如下：
Px c1[d 6(c23c4 s5 s 23c5) d 4 s 23 a2c2] s1(d 6 s 4 s5 d 2) Py s1[d 6(c23c4 s5 s 23c5) d 4 s 23 a2c2] c1(d 6 s 4 s5 d 2) Pz d 6(c 23c5 s 23c4 s5) d 4c 23 a 2 s 2
三、机器人逆运动学
nx n T T6 y nz 0 ox oy oz 0 ax ay az 0 px py ＝A1 A2 A3 A4 A5 A6 pz 1
• 1)问题:已知手部位姿,求各关节位置 • 2)意义:是机械手控制的关键
（一）机器人运动学逆解有关问题
• 相邻连杆的关系参数
连杆偏置di ： Xi-1沿Zi-1至Xi的距离，沿Zi-1正向时为正； 关节转角θi ：Xi-1绕Zi-1转至Xi的转角，符号根据右手定则 确定；
转动连杆坐标系的D-H变换
• 转动连杆的D-H参数为θi、ai、αi 、di，其中关节变量是θi 。这四 个参数确定了连杆i相对于连杆i-1的位姿，即D-H坐标变换矩阵Ai。 • 坐标系{i-1}经过下面四次有序的相对变换可得到坐标系{i}：
移动连杆坐标系的建立
• 首连杆0：基座坐标系{0}是固定不动的；Z0
轴取关节1的轴线，O0的设置任意，通常与 O1重合； • 末连杆n：工具坐标系{n}固定在机器人的 终端，由于连杆n的终端不再有关节，约定 坐标系{n}与{n-1}平行；
再看移动连杆参数的含义
• 由于移动连杆的OiZi轴线平行于移动关节轴 线移动， OiZi在空间的位置是变化的，因而 ai参数无意义。连杆i的长度在坐标系{i-1} 中考虑， 故参数ai=0 。原点Oi的零位与Oi-1 重合，此时移动连杆的变量di=0 。
di d1 0 d3
ai 0 a2 0
αi 0 -90º 0
1 2 3
0 θ2 0
A1 Trans(0,0, d 1) A2 Rot( Z , θ )Trans( x, a 2) Rot( x, π / 2) A3 Trans(0,0, d 3) T A1 A2 A3
c2 s 2 0 0 0 s2 0 c2 1 0 0 0 d 3 s 2 a 2 c2 d 3c2 a2 s2 d1 1
转动连杆坐标系的建立
• 首连杆0：基座坐标系{0}是固定不动的；Z0
轴取关节1的轴线，O0的设置任意，通常与 O1重合； • 末连杆n：工具坐标系{n}固定在机器人的 终端，由于连杆n的终端不再有关节，约定 坐标系{n}与{n-1}平行；
再看转动连杆参数的含义
• 连杆的尺寸参数
连杆长度ai：Zi和Zi-1沿Xi的距离，总为正；； 连杆扭角αi ：Zi-1绕Xi转至Zi的转角，符号根据右手定则确 定；
• 相邻连杆的关系参数
连杆偏置di ：沿关节i轴线方向，两个共垂线之间的距离；
关节转角θi ：垂直于关节轴线的平面内，两个共垂线之 间的夹角；
关节变量
• 旋转关节：
关节转角θi是关节变量，连杆长度ai、连杆 扭角αi 、连杆偏置di 是固定不变的；
• 移动关节：
连杆偏置di是关节变量，连杆长度ai 、连杆 扭角αi 、关节转角 θi是固定不变的；
转动坐标臂（RRR）
Z0
X1 X0 Z1 Z2 Z3 X2 X3
参数 θi 连杆
di 0
ai 0
αi 90º
1
θ1
2
3
θ2
θ3
0
0
a2
a3
0
0
A1 Rot( Z , θ 1) Rot( x,π / 2) A2 Rot( Z , θ )Trans(a 2,0,0) A3 Rot( Z , θ 3)Trans(a 3,0,0) T A1 A2 A3
移动连杆坐标系的D-H变换
• 移动连杆的D-H参数为θi、ai、αi 、di，其中关 节变量是di 。用与求转动连杆坐标系相同的方法 可求出移动连杆的D-H变换矩阵：
Ai Rot( z , i )Trans(0,0, d i )Trans(ai ,0,0) Rot( x, i ) c i s i 0 0 s i c i c i c i s i 0 s i s i c i s i c i 0 0 0 di 1