机器人学基础
实验报告
中南大学机电工程学院机械电子工程系
2016年10月
一、实验目的
1．了解四自由度机械臂的开链结构；
2．掌握机械臂运动关节之间的坐标变换原理； 3．学会机器人运动方程的正反解方法。
二、实验原理
本实验以SCARA 四自由度机械臂为例研究机器人的运动学问题.机器人运动学问题包括运动学方程的表示,运动学方程的正解、反解等,这些是研究机器人动力学和机器人控制的重要基础,也是开放式机器人系统轨迹规划的重要基础。
机械臂杆件链的最末端是机器人工作的末端执行器(或者机械手)，末端执行器的位姿是机器人运动学研究的目标，对于位姿的描述常有两种方法：关节坐标空间法和直角坐标空间法。
关节坐标空间：
末端执行器的位姿直接由各个关节的坐标来确定，所有关节变量构成一个关节矢量，关节矢量构成的空间称为关节坐标空间。
图1-1是GRB400机械臂的关节坐标空间的定义。
因为关节坐标是机器人运动控制直接可以操纵的，因此这种描述对于运动控制是非常直接的。
直角坐标空间：
机器人末端的位臵和方位也可用所在的直角坐标空间的坐标及方位角来描述，当描述机器人的操作任务时，对于使用者来讲采用直角坐标更为直观和方便（如图1-2）。
当机器人末端执行器的关节坐标给定时，求解其在直角坐标系中的坐标就是
正向运动学求解（运动学正解）问题；反之，当末端执行器在直角坐标系中的坐
图1-1 机器人的关节坐标空间 图1-2 机器人的直角坐标空间法
标给定时求出对应的关节坐标就是机器人运动学逆解（运动学反解）问题。
运动学反解问题相对难度较大，但在机器人控制中占有重要的地位。
机器人逆运动学求解问题包括解的存在性、唯一性及解法三个问题。
存在性：至少存在一组关节变量来产生期望的末端执行器位姿，如果给定末端执行器位臵在工作空间外，则解不存在。
唯一性：对于给定的位姿，仅有一组关节变量来产生希望的机器人位姿。
机器人运动学逆解的数目决定于关节数目、连杆参数和关节变量的活动范围。
通常按照最短行程的准则来选择最优解，尽量使每个关节的移动量最小。
解法：逆运动学的解法有封闭解法和数值解法两种。
在末端位姿已知的情况下，封闭解法可以给出每个关节变量的数学函数表达式；数值解法则使用递推算法给出关节变量的具体数值，速度快、效率高，便于实时控制。
下面介绍D-H 变化方法求解运动学问题。
建立坐标系如下图所示
连杆坐标系{i }相对于{ i −1 }的变换矩阵可以按照下式计算出，其中连杆坐标系D-H 参数为由表1-1给出。
齐坐标变换矩阵为：
其中描述连杆i 本身的特征；和描述连杆i−1与i 之间的联系。
对于旋转关节，仅是关节变量，其它三个参数固定不变；对于移动关节，仅是关节变量，其它三个参数不变。
表1-1 连杆参数表
其中连杆长l 1=200mm ，l 2=200mm ，机器人基坐标系为O-X 0Y 0Z 0。
根据上面的坐标变换公式，各个关节的位姿矩阵如下：
⎥⎥⎥⎥⎦
⎤⎢⎢⎢⎢⎣⎡--=10
00
cos sin 00sin cos cos cos sin 0sin sin cos sin cos 33
3
3
33
333333323
d T αααθαθθ
αθαθθ
运动学正解：各连杆变换矩阵相乘，可得到机器人末端执行器的位姿方程（正运动学模型）为：
其中：z 轴为手指接近物体的方向，称接近矢量 a （approach ）；y 轴为两手指的连线方向，称方位矢量o （orientation ）；x 轴称法向矢量n （normal ），由右手法则确定，n=o*a 。
p 为手爪坐标系原点在基坐标系中的位臵矢量。
运动学逆解：通常可用未知的连杆逆变换右乘上式：
令两式对应元素分别相等即可解出。
其中
22
12
222212y
x y
x p
p l p p l l M +++-=
将上式回代，可得，
11221222)arctan
sin()arctan
(cos arctan
l p p p p p p p p y
x
y x y
x
y x -++++=θθθ
令第二行第四个元素对应相等，可得：z 3d p = 令第四行第三个元素对应相等，可得：
所以，
注意：关节运动范围：
θ1 0-180°
θ2 0-100°
d3 ±40mm
θ4 ±170°
三、实验步骤
（本实验由老师演示）
步骤1．检查实验系统各部分的信号连接线、电源是否插好，完成后打开伺服驱动系统的电源开关。
步骤2．运行GRBserver程序，出现以下程序界面。
图1-6 机器人示教程序界面
步骤3．按下“打开控制器”按钮，按下“伺服上电”按钮。
步骤4．清理周围环境，避免机械臂运动时打到周围的人或物。
检查末端执行器上的电线连接，避免第四个关节运动时电线缠绕而被拉断。
步骤5．按下“自动回零”按钮，机械臂自动回零。
步骤6．选择“关节空间”或“直角坐标空间”，选择“运动步长”，选择“运动速度倍率”为合适值。
一般刚开始时尽量选择较小的值，以使运动速度不致太快。
步骤7．在“示教操作”区按下相应关节按钮，观察机械臂的运动情况。
此时可以按下“记录”按钮，以便以后重复该次运动。
步骤8．重复步骤7，演示各种运动及功能。
四、运动学正反解MATLAB程序
正解程序：
function T=ydzj(n1,n2,m3,n4,a1,a2)
% n1,n2,m3,n4为四个关节变量θ1,θ2,d3,θ4;a1，a2分别为杆长l1,l2
syms w1w2d3w4l1l2;
cw1=cos(w1);cw2=cos(w2);cw4=cos(w4);
sw1=sin(w1);sw2=sin(w2);sw4=sin(w4);
A1=[cw1,-sw1,0,l1*cw1;sw1,cw1,0,l1*sw1;0,0,1,0;0,0,0,1];
A2=[cw2,-sw2,0,l2*cw2;sw2,cw2,0,l2*sw2;0,0,1,0;0,0,0,1];
A3=[1,0,0,0;0,1,0,0;0,0,1,d3;0,0,0,1];
A4=[cw4,-sw4,0,0;sw4,cw4,0,0;0,0,1,0;0,0,0,1];
A=A1*A2*A3*A4;
T=subs(A,{w1,w2,d3,w4,l1,l2},{n1,n2,m3,n4,a1,a2});
%T矩阵的第四列为直角坐标—————————————————————————————————————————反解程序：
function X=ydfj(A,a1,a2)
%A为齐次坐标变换矩阵，a1，a2分别为杆长l1,l2
l1=a1; l2=a2;
n1=A(1,1);n2=A(2,1);n3=A(3,1);
o1=A(1,2);o2=A(2,2);o3=A(3,2);
a1=A(1,3);a2=A(2,3);a3=A(3,3);
p1=A(1,4);p2=A(2,4);p3=A(3,4);
M=(l1^2-l2^2+p1^2+p2^2)/(2*l1*sqrt(p1^2+p2^2));
m1(1)=atan(M/sqrt(1-M^2))-atan(p1/p2);%
m1(2)=atan(-M/sqrt(1-M^2))-atan(p1/p2);
m2=atan(sqrt(p1^2+p2^2)*cos(m1+atan(p1/p2))./(sqrt(p1^2+p2^2)*sin(m1+atan(p1/p2 ))-l1));
t3=[p3,p3];
m4=-m2+asin(n2*cos(m1)-n1*sin(m1));
X=[m1;m2;t3;m4];
% X为反解的两组解，取符合关节坐标范围的解
五、实验数据
六、实验结果分析与讨论。