0 0 1 0 0 0 1 0 0 0 1 0
0 0 1 0
0 0 0 1 r1 0 0 1
符号定义如下： Rot(x, θ 6 ):旋转变换，表示绕 x 轴旋转了 θ 6 Trans(m,n,p):平移变换，表示坐标轴分别沿 x、 y、z 轴平移了 m、n、p
图 1 机械臂实物图
图 1 机械臂结构图
在机械臂运动过程中，需要不断变换各个关节 的旋转角度，从而到达相应的位置点。设三维空间 中某点的坐标为 ( X , Y , Z ) ，对应机械臂 6 个关节 的旋转角度依次为 (θ 1 , θ 2 , θ 3 , θ 4 , θ 5 , θ 6 ) ，则机械 臂 运 动 模 型 即 ( X , Y , Z ) 和 (θ 1 , θ 2 , θ 3 , θ 4 , θ 5 , θ 6 ) 的对应关系。自定义机械臂的初始姿态，利用右手
6
上式中， R 为旋转变换矩阵， p 平移变换矩阵 。 由上可知，当已知 6 个关节的偏转角度后即可 以通过矩阵求得对应机械臂的位置和姿态。 2.2 机械臂运动学逆问题 机械臂运动学的逆问题即：已知机械臂的指尖 末端坐标，求此时机械臂各个关节的偏转角度。可 以通过求逆矩阵的方法求解，即有如下方程 ：
cosθ2 0 A2 = − sinθ2 0
0 sinθ2 0 1 0 0 0 cosθ2 0 0 0 1
0 0 1 0 0 0 1 0 0 0 1 0
0 0 r2 cosθ3 1 0 r2 sinθ3 0 1 0 0 0 1
0 1 0 0 cos θ4 − sinθ4 A4 = 0 sinθ4 cos θ4 0 0 0
0 1 0 0 0 0 1 0
0 0 0 θ4 1 0 r3 cos 0 1 r3 sinθ4 0 0 1
Ai :第 i 个关节的坐标系相对于前一个坐标系
nx n y = nz 0
A3 = Rot(z, θ 3 ) Trans( r2 c 3 , r2 s 3 ,0)
A4 = Rot(x, θ 4 ) Trans(0, r3 c 4 , r3 s 4 ,)
ox oy oz 0
ax ay az 0
px py = R pz 0 1
法则分别建立每个关节的坐标系[1]，如图 2 所示
A5 = Rot(x, θ 5 ) A6 = Rot(y, θ 6 ) Trans( r4 c 6 ,0, r4 s 6 ,)
展开可得：
cosθ1 − sinθ1 sinθ cosθ1 1 A1 = 0 0 0 0
图 2 各个关节坐标系的建立
指尖移动到空间点 X′，其中各个增量 ∆θ i 只能取到 -2, -1.9, -1.8, ┅，1.8, 1.9, 2 这 41 个离散值（即精 度为 0.1°，绝对值不超过 2°）。通过一系列的
− 180 ≤θ 1≤ 180 − 125 ≤θ ≤ 125 2 − 138 ≤θ 3≤ 138 st. − 270 ≤θ 4≤ 270 − 133.5 ≤θ ≤ 120 5 − 270 ≤θ 6≤ 270 ∆θ i ∈ {−2.0,−1.9,−1.8,...,1.8,1.9,2.0}
p 1
即
n x R = ny nz
ox oy oz
ax px a y ，p = py az pz
[2]
指令序列 P 1 , P2 , P 3 , Λ , Pn 可以将指尖依次到达位 置 X0，X1，┅，Xn，则称 X0，X1，┅，Xn 为从指尖 初始位置 X0 到达目标位置 Xn 的一条路径（运动轨 迹）。根据具体的目标和约束条件计算出合理、便 捷、有效的指令序列是机器人控制中的一个重要问 题。 3.1 模型的建立 求解机械臂指尖从初始位置到达目标位置的 指令序列问题可以归结为一个最优化问题： 目标：机械臂实际到达的目标点与给定的目标 点的偏差和需要对所有关节进行偏转调节的总次 数最小。 约束 1：机械臂每个关节偏转的最大角度范围 的限制。 约束 2：机械臂每个关节每次可转动的最大角 度限制。 设 T 为机械臂实际达到的目标点，O 为给定的 目标点， ∆θ i 表示第 i 个关节点在 T 点的转角和 O 点的转角的差值， 用 E 表示机械臂实际到达的目标 点与给定的目标点的偏差，用 F 表示需要对所有关 节进行偏转调节的总次数，则有： E=
联立运动学正问题的方程，可以求得各个关节 的偏转角度 θ i ，但是不唯一，结合关节的机械结构 约束， 可以得到各个关节变量的相应范围， 如下式：
− 180 ≤θ 1≤ 180 − 125 ≤θ ≤ 125 2 − 138 ≤θ 3≤ 138 − 270 ≤θ 4≤ 270 − 133.5 ≤θ 5≤ 120 − 270 ≤θ 6≤ 270
六自由度机械臂的运动学分析
柴 莹，王秀全
（华北电力大学计算机科学与技术学院，北京，102206）
THE KINEMATICS ANALYSIS OF 6-DOF MANIPULATOR
CHAI Ying,WANG Xiuquan （Department of Computer Science & Technology, North China Electric Power University,Beijing,102206）
正问题，第二个问题被称为运动学逆问题。在实际 应用中，机械臂每个关节偏转的最大角度有范围限 制，而每个关节每次可转动的角度也有限制，要使 机械臂末端可以由初始位置到达空间中指定的点 的位置要在这些约束和机械臂运动学的基础上找 出一条最优路径。本文以六自由度机械手臂为例对 其运动学做了分析。
2 机械臂运动学分析
[2]
T = A 1 A 2 A 3 A 4 A 5 A 6 -1 A 11 T = A 2 A 3 A 4 A 5 A 6 -1 -1 A 11 A 2 T = A 3 A 4 A 5 A 6 -1 -1 -1 A1 A 2 A 3 T = A 4 A 5 A 6 -1 -1 -1 A1 A -21 A 3 A4 T = A5A6 1 -1 -1 -1 -1 -1 A 11 A 2 A 3 A 4 A 5 T = A 6
结合约束条件可以求得唯一解。
∑ | ∆θ
i =1
i
|
则以上最优化问题可以描述为： min（E，F）
3 机械臂运动学分析的简单应用
假定机器人控制系统只能够接收改变各个关 节的姿态的关于连杆角度的增量指令
P = ∆Θ = (∆θ1 , ∆θ 2 , ∆θ 3 , ∆θ 4 , ∆θ 5 , ∆θ 6 ) ，使得
ABSTRACT: Along with the development of science, robotics development made a considerable progress and its application have infiltrated all walks of life. The robot arm and robot performance are closely related. This paper is designed for analyzing the manipulator kinematics problems using the example of 6-DOF manipulator. With the knowledge of positive kinematics problem and reverse kinematics problem and it analyzes the relation between manipulator end point and the coordinates of the corresponding joint and establishes a model to resolve a practical problem which is computing a instruction sequence from beginning point of manipulator end to the target point. This paper is a practical reference value for establishing manipulator kinematics model. KEY WORDS：Manipulator, Kinematics, Reverse Kinematics 摘要：随着科学的发展，机器人的研制取得长足的进步，它 的应用已经渗入到各行各业， 而机器人手臂的路径规划问题 和机器人的性能息息相关。 本文以一个六自由度的机械臂为 例分析了机械臂的运动学问题， 其中运用了运动学正问题与 逆问题的知识求解了机械臂末端所在点的坐标与各关节偏 转角度的对应关系； 本文还对一个实际问题， 求解机械臂指 尖丛初始位置到达目标位置的指令序列的问题， 建立了模型 并计算出结果。 本文对建立机械臂的运动学模型有实际参考 价值。 关键词：机械臂，运动学，逆运动学
的 变 换 矩 阵 ， 其 中 关 节 F,B,C,G,D,H 分 4 , A5 , A6
A: 机械臂指尖的坐标系相对于基坐标的变换 矩阵 坐标轴之间的变换由旋转和平移组成，各关节 的变换矩阵如下：
0 1 0 0 cos θ5 −sinθ5 A5 = 0 sin θ5 cos θ5 0 0 0
θ i :机械臂每一个关节对应的角度，其中关节
F,B,C,G,D,H 分别对应 θ 1 , θ 2 , θ 3 , θ 4 , θ 5 , θ 6 r1:AB 的距离 r2:BC 的距离 r3:CD 的距离 r4:DE 的距离
cosθ3 − sinθ3 A3 = sinθ3 cosθ3 0 0 0 0
0 0 0 1 0 0 0 1