机器人大作业
一． 利用前置法建立D-H 坐标系如下图所示：
照上图由题意列写连杆参数表如下表所示：
4
其中初始位置时：θ1=
i -1
i
π
2
，θ2=θ3=θ4=θ5=θT =0。
2 计算各
T
-s θi c θi ⋅c ∂i -1c θi ⋅s ∂i -1
0-s ∂i -1c ∂i -1
⎤⎥
-d i ⋅s ∂i -1⎥
， ⎥
d i ⋅c ∂i -1
⎥⎥1⎦
⎡c θi
⎢
⎢s θi ⋅c ∂i -1i -1
T =由所学知道i
⎢
s θ⋅s ∂i -1⎢i ⎢0⎣
αi -1
将上表中的各个值代入可得：
⎡c 1
⎢
0⎢s 1T =1⎢
0⎢⎢⎣0⎡c 3⎢2⎢0T =3⎢-s
⎢3⎢⎣0⎡c 5⎢
4⎢0T =5⎢-s
⎢5⎢⎣0
-s 1c 100-s 30-c 30-s 50-c [1**********]100
⎡c 20⎤
⎥⎢0⎥1⎢0T =⎥2⎢s 0.5
⎥⎢2
⎢1⎥⎦⎣0⎡c 40⎤
⎥⎢0.5⎥3⎢0T =4⎢s 0⎥
⎥⎢4
⎢1⎥⎦⎣00⎤
⎥0.5⎥0⎥⎥1⎥⎦
⎡c T
⎢5⎢0T =T ⎢s
⎢T ⎢⎣0
-s 20c 20-s 40c 40-s T 0c T 0
0-1000-1000-100
⎤
⎥-0.5⎥0⎥
⎥1⎥⎦00
⎤
⎥-0.5⎥0⎥
⎥1⎥⎦⎤
⎥-0.5⎥0⎥
⎥1⎥⎦0
4 任意设定各关节变量，计算T 。
T
0T
T =
1
T ∙
12
T ∙
23
T ∙
34
T ∙
45
T ∙
5T
T
⎡r 11
⎢⎢r 21=⎢⎢r 31⎢0⎣
r 12r 22r 320
r 13r 23r 330
p x ⎤⎥p y ⎥⎥p z ⎥1⎥⎦
其中：
r 11=c 1c 2c 3c 4c 5c T -s 1s 3c 4c 5c T +c 1s 4c 5c T -s 2s 4c 5c T -c 1c 2s 3s 5c T -s 1c 3s 5c T +s 1s 3s 4s T
-c 1c 2c 3s 4s T +c 1c 4s T -s 2c 4s T
r 21=s 1c 2c 3c 4c 5c T +c 1s 3c 4c 5c T -s 1s 2s 3c 5c T +c 1c 3s 5c T -s 1c 2s 3s 5c T -s 1c 2c 3s 4s T
-c 1s 3s 4s T -s 1s 2c 4s T
r 31=s 2c 3c 4c 5c T +c 2s 4c 5c T -s 2s 3s 5c T -s 2c 3s 4s T +c 2c 4s T
r 12=-c 1c 2c 3c 4c 5s T +s 1s 3c 4c 5s T -c 1s 4c 5s T +s 2s 4c 5s T +c 1c 2s 3s 5s T +s 1c 3s 5s T +
s 1s 3s 4c T -c 1c 2c 3s 4c T +c 1c 4c T -s 2c 4c T
r 22=-s 1c 2c 3c 4c 5s T -c 1s 3c 4c 5s T +s 1s 2s 4c 5s T -c 1c 3s 5s T +s 1c 2s 3s 5s T -s 1c 2c 3s 4c T -
c 1s 3s 4c T -s 1s 2c 4c T
r 32=-s 2c 3c 4c 5s T -c 2s 4c 5s T +s 2s 3s 5s T -s 2c 3s 4c T +c 2c 4c T
r 13=c 1c 2c 3c 4s 5-s 1s 3c 4s 5+c 1s 4s 5-s 2s 4s 5+c 1c 2s 3c 5+s 1c 3c 5r 23=s 1c 2c 3c 4s 5+c 1s 3c 4s 5-s 1s 2s 4s 5+s 1c 2s 3c 5-c 1c 3c 5 r 33=s 2c 3c 4s 5+c 2s 4s 5+s 2s 3c 5
p x =0.5(c1c 2c 3c 4s 5-s 1s 3c 4s 5-c 1s 2s 4s 5+c 1c 2s 3c 5+s 1c 3c 5+s 1s 3s 4-
c 1c 2c 3s 4-c 1s 2c 4+c 1c 2s 3+s 1c 3-c 1s 2+s 1)
p y =0.5(s1c 2c 3c 4s 5+c 1s 3c 4c 5-s 1s 2s 4s 5+s 1c 2s 3c 5-c 1c 3c 5+s 1c 2s 3-c 1c 3-
s 1s 2-c 1-s 1c 2c 3s 4-c 1s 3s 4-s 1s 2s 4)
p z =0.5(s2c 3c 4s 5+c 2s 4s 5+s 2s 3c 5+s 2s 3+c 2+1-s 2c 3s 4+c 2c 4)
令θ1=θ5=
0T
π
2
, θ2=θ3=θ4=θT =0；代入以上各式并计算T T 得：
23
T =
1
T ∙2T ∙[1**********]0-10000-[1**********]
1
T ∙0
34
T ∙
45
T ∙
5T
T 0-100
⎤⎡1⎥⎢-0.5⎥0
∙⎢⎢00⎥⎥⎢1⎦⎣00⎡10⎤⎥⎢0.5⎥0
∙⎢⎢00⎥⎥⎢1⎦⎣0
00-100010
0-1000100
⎤
⎥0.5⎥
∙0⎥
⎥1⎦0⎤⎥-0.5⎥0⎥
⎥1⎦0
⎡⎢=⎢⎢⎢⎣⎤⎡1⎥⎢0⎥0
∙⎢⎢00.5⎥⎥⎢1⎦⎣0⎤⎡0⎥⎢-0.5⎥0
∙⎢⎢-10⎥⎥⎢1⎦⎣00⎤
⎥0.5⎥1.5⎥
⎥1⎦1
0010-1000
⎡1⎢⎢0⎢0⎢⎣0⎡⎢=⎢⎢⎢⎣
-1000
0100
5 根据上面得到的
0T
T
，利用Paul 反变换法求解各关节变量。
令θT =0，求θ1、θ2、θ3、θ4、θ5。 由0T T =
01
T ∙2T ∙1
1
2
12
T ∙3
3
3
T ∙4
4
4
T ∙5
5T
T 得到：
T
-1
∙T T =2T ∙3T ∙4T ∙5T ∙T T
05
0010
0-100
0⎤
⎥-0.5
⎥由此得到： 0⎥⎥1⎦
⎡⎢0
其中T T =⎢
⎢⎢⎣
-1000
00100010
0100s 1c 100
1⎤⎡1⎥⎢0.505⎥, T =⎢
T
⎢01.5⎥
⎥⎢1⎦⎣0c 1+0.5s 1⎤
⎥
0.5c 1-s 1
⎥ ⎥1
⎥1⎦
⎡-c 1
⎢s 0-10⎢1T ∙T =1T
⎢0⎢⎣0
' ⎡r 11⎢' r 2112345⎢T ∙3T ∙4T ∙5T ∙T T ='
2
⎢r 31⎢⎣0
r 12r 22r 320
' '
'
r 13r 23r 330
' '
'
p x ⎤' ⎥p y ⎥
'
p z ⎥⎥1⎦
'
上式中：
r 11=c 2c 3c 4c 5-s 2s 4c 5-c 2s 3s 5r 12=-c 2c 3s 4-s 2c 4r 21=s 3c 4c 5+c 3s 5
' ' '
' ' '
r 22=-s 3s 4
r 31=s 2c 3c 4c 5+c 2s 4c 5-s 2s 3s 5r 32=-s 2c 3s 4+c 2c 4r 13=c 2c 3c 4s 5-s 2s 4s 5+c 2s 3c 5r 23=s 3c 4s 5-c 3c 5
r 33=s 2c 3c 4s 5+c 2s 4s 5+s 2s 3c 5
' ' '
p x =0.5(c2s 3-s 2-c 2c 3s 4-s 2c 4+c 2c 3c 4s 5-s 2s 4s 5+c 2s 3c 5) p y =0.5(-s 3s 4-c 3-1+s 3c 4s 5-c 3c 5)
p z =0.5(s2s 3+c 2-s 2c 3s 4+c 2c 4+s 2c 3c 4s 5+c 2s 4s 5+s 2s 3c 5)
' '
'
由对应项相等可得：
r 22=-s 3s 4=0，令θ3=0（即c 3=1、s 3=0）代入r 12、r 32
'
'
'
并由对应式相等
得到：
c 2s 4+s 2c 4=0c 2c 4-s 2s 4=1
··① ··②
'
将②式代入r 11得到：
·······③ c 5=-c 1·
' 由r 21=s 3c 4c 5+c 3s 5=s 1得到：
·········④ s 5=s 1·
将③式代入p ' y =0.5(-s 3s 4-c 3-1+s 3c 4s 5-c 3c 5) =0.5c 1-s 1可得：
s 1=1又因为s 5=s 1=1所以θ1=θ5=
π
2
，
将①、②式代入
p z =0.5(s2s 3+c 2-s 2c 3s 4+c 2c 4+s 2c 3c 4s 5+c 2s 4s 5+s 2s 3c 5) =1可得：
'
c 2=1所以θ2=0，代入②式可得：θ4=0
所以θ1=θ5=
π
2
, θ2=θ3=θ4=0。
6 当θ
i
=sin(π t /4（) 0≤t ≤1) 时利用机器人的位移方程求解其末端
轨迹（x,y,z ）；
由题意可令0P =(x y z 1)，T P =(0001)则
T
T
P =
0T
T ∙
T
P ，计算可得：
x =p x
=0.5(c1c 2c 3c 4s 5-s 1s 3c 4s 5-c 1s 2s 4s 5+c 1c 2s 3c 5+s 1c 3c 5+s 1s 3s 4-c 1c 2c 3s 4-c 1s 2c 4+c 1c 2s 3+s 1c 3-c 1s 2+s 1)
y =p y
=0.5(s1c 2c 3c 4s 5+c 1s 3c 4c 5-s 1s 2s 4s 5+s 1c 2s 3c 5-c 1c 3c 5+s 1c 2s 3-c 1c 3- s 1s 2-c 1-s 1c 2c 3s 4-c 1s 3s 4-s 1s 2s 4)
z =p z
=0.5(s2c 3c 4s 5+c 2s 4s 5+s 2s 3c 5+s 2s 3+c 2+1-s 2c 3s 4+c 2c 4)
因为θ1=θ2=θ3=θ4=θ5=sin (πt 4)其中0≤t ≤1，令θn =θ1则上式简化为：
x =0.5c n s n +s n +3c n s n +2s n c n +s n
(
432
)
4
3
2
y =0.5c n s n -c n +c n s n -c n -c n -s n -s n -s n z =0.5s n +1∙c n +c n +1
(
323222
)
(
2
)(
2
)
7 利用ADAMS 建模，并测量机器人末端轨迹
7.1 首先利用Matlab 编程画出其末端轨迹，程序如下： t=0:0.01:1;
th=sin(pi*t/4); a=sin(th); b=cos(th);
x=0.5*(b.*b.*b.*b.*a-2*a.*a.*a.*b+a.*a.*a+b.*b.*a+a);
y=0.5*(b.*b.*b.*a.*a-b.*b.*b+b.*b.*a.*a-b.*b-b-a.*a.*a.*a-a.*a.*a-a.*a);
z=0.5*(a.*a.*b.*b+b.*b+b.*a.*a+b+a.*a+1); plot3(x,y,z);
xlabel('X'),ylabel('Y'),zlabel('Z'); title('末端轨迹图'); grid on;
所得结果如下图所示：
7.2 利用ProE 仿真满足条件的轨迹如下图所示：
7.3 各轴位移对比如下所示： ① X方向的对比：
② Y方向的对比：
③ Z方向的对比：
图中可以看出，matlab 的计算结果与仿真吻合
8 当
=[0, 2*π*0.15*sin(2*π*t) , 2*π*0.15*cos(2*π*t)]T X
0≤t ≤1，m/s，
并任意设定关节初始位形，求解机器人关节轨迹并用ADAMS 模型进行仿真。
已知X 求θ，由所学可得：θ=
∙
∙
∙
J
+
∙
∙X
。
⎡∂x ⎢∂θ⎢1⎢∂y
其中J =⎢
⎢∂θ1⎢∂z ⎢⎣∂θ1
∂x ∂y ∂z
∂x ∂y ∂z
∂x ∂y ∂z
∂θ2∂θ3∂θ4∂θ2∂θ3∂θ4∂θ2∂θ3∂θ4
∂x ⎤
⎥∂θ5
⎥⎡x x x x x ⎤
12345
∂y ⎥⎢⎥
⎥=⎢y 1y 2y 3y 4y 5⎥∂θ5⎥
⎢⎣z 1z 2z 3z 4z 5⎥⎦⎥∂z ⎥∂θ5⎦
8.1 利用MATLAB 计算机器人各个关节轨迹 设各关节初始位形为(
π
2, -
π
3
,0,
π
2
,0) 。
编写第一个M 文件，文件名为zhaolei.m functiondS=zhaolei( t,th ) th1=th(1); th2=th(2); th3=th(3); th4=th(4); th5=th(5);
x1=(-sin(th1)*cos(th2)*cos(th3)*cos(th4)*sin(th5)-
cos(th1)*sin(th3)*cos(th4)*cos(th5)-sin(th1)* sin(th4)*sin(th5)-sin(th1)*cos(th2)*sin(th3)* cos(th5)+cos(th1)*cos(th3)*cos(th5)+cos(th1)* sin(th3)*sin(th4)+sin(th1)*cos(th2)*cos(th3)* sin(th4)-sin(th1)*cos(th4)-sin(th1)*cos(th2)* sin(th3)+cos(th1)*cos(th3)-sin(th1)+cos(th1))/2; x2=(-cos(th1)*sin(th2)*cos(th3)*cos(th4)*sin(th5)-
cos(th2)*sin(th4)*sin(th5)-cos(th1)*sin(th2)* sin(th3)*cos(th5)+cos(th1)*sin(th2)*cos(th3)* sin(th4)-cos(th2)*cos(th4)-cos(th1)*sin(th2)* sin(th3)-cos(th2))/2;
x3=(-cos(th1)*cos(th2)*sin(th3)*cos(th4)*sin(th5)-
sin(th1)*cos(th3)*cos(th4)*sin(th5)+cos(th1)* cos(th2)*cos(th3)*cos(th5)-sin(th1)*sin(th3)* cos(th5)+sin(th1)*cos(th3)*sin(th4)+cos(th1)* cos(th2)*sin(th3)*sin(th4)+cos(th1)*cos(th2)* cos(th3)-sin(th1)*sin(th3))/2;
x4=(-cos(th1)*cos(th2)*cos(th3)*sin(th4)*sin(th5)+
sin(th1)*sin(th3)*sin(th4)*sin(th5)+cos(th1)* cos(th4)*sin(th5)-sin(th2)*cos(th4)*sin(th5)+ sin(th1)*sin(th3)*cos(th4)-cos(th1)*cos(th2)* cos(th3)*cos(th4)-cos(th1)*sin(th4)+sin(th2)* sin(th4))/2;
x5=(cos(th1)*cos(th2)*cos(th3)*cos(th4)*cos(th5)-
sin(th1)*sin(th3)*cos(th4)*cos(th5)+cos(th1)*
sin(th4)*cos(th5)-sin(th2)*sin(th4)*cos(th5)-
cos(th1)*cos(th2)*sin(th3)*sin(th5)-sin(th1)*
cos(th3)*sin(th5))/2;
y1=(cos(th1)*cos(th2)*cos(th3)*cos(th4)*sin(th5)-
sin(th1)*sin(th3)*cos(th4)*sin(th5)-cos(th1)*
sin(th2)*sin(th4)*sin(th5)+cos(th1)*cos(th2)*
sin(th3)*cos(th5)+sin(th1)*cos(th3)*cos(th5)+
cos(th1)*cos(th2)*sin(th3)+sin(th1)*cos(th3)-
cos(th1)*sin(th2)+sin(th1)-cos(th1)*cos(th2)*
cos(th3)*sin(th4)+sin(th1)*sin(th3)*sin(th4)-
cos(th1)*sin(th2)*sin(th4))/2;
y2=(-sin(th1)*sin(th2)*cos(th3)*cos(th4)*sin(th5)-
sin(th1)*cos(th2)*sin(th4)*sin(th5)-sin(th1)*
sin(th2)*sin(th3)*cos(th5)-sin(th1)*sin(th2)*
sin(th3)-sin(th1)*cos(th2)+sin(th1)*sin(th2)*
cos(th3)*sin(th4)-sin(th1)*cos(th2)*sin(th4))/2;
y3=(-sin(th1)*cos(th2)*sin(th3)*cos(th4)*sin(th5)+
cos(th1)*cos(th3)*cos(th4)*sin(th5)+sin(th1)*
cos(th2)*cos(th3)*cos(th5)+cos(th1)*sin(th3)*
cos(th5)+sin(th1)*cos(th2)*cos(th3)+cos(th1)*
sin(th3)+sin(th1)*cos(th2)*sin(th3)*sin(th4)-
cos(th1)*cos(th3)*sin(th4))/2;
y4=(-sin(th1)*cos(th2)*cos(th3)*sin(th4)*sin(th5)-
cos(th1)*sin(th3)*sin(th4)*sin(th5)-sin(th1)*
sin(th2)*cos(th4)*sin(th5)-sin(th1)*cos(th2)*
cos(th3)*cos(th4)-cos(th1)*sin(th3)*cos(th4)-
sin(th1)*sin(th2)*cos(th4))/2;
y5=(sin(th1)*cos(th2)*cos(th3)*cos(th4)*cos(th5)+
cos(th1)*sin(th3)*cos(th4)*cos(th5)-sin(th1)*
sin(th2)*sin(th4)*cos(th5)-sin(th1)*cos(th2)*
sin(th3)*sin(th5)+cos(th1)*cos(th3)*sin(th5))/2;
z1=0;
z2=(cos(th2)*cos(th3)*cos(th4)*sin(th5)-sin(th2)*
sin(th4)*sin(th5)+cos(th2)*sin(th3)*cos(th5)+
cos(th2)*sin(th3)-sin(th2)-cos(th2)*cos(th3)*
sin(th4)-sin(th2)*cos(th4))/2;
z3=(-sin(th2)*sin(th3)*cos(th4)*sin(th5)+sin(th2)*
cos(th3)*cos(th5)+sin(th2)*cos(th3)+sin(th2)*
sin(th3)*sin(th4))/2;
z4=(-sin(th2)*cos(th3)*sin(th4)*sin(th5)+cos(th2)*
cos(th4)*sin(th5)-sin(th2)*cos(th3)*cos(th4)-
cos(th2)*sin(th4))/2;
z5=(sin(th2)*cos(th3)*cos(th4)*cos(th5)+cos(th2)*
sin(th4)*cos(th5)-sin(th2)*sin(th3)*sin(th5))/2;
j=[x1,x2,x3,x4,x5;y1,y2,y3,y4,y5;z1,z2,z3,z4,z5];
j_1=pinv(j);
dx=[0;2*pi*0.15*sin(2*pi*t);2*pi*0.15*cos(2*pi*t)];
dS=j_1*dx;
end
求解各关节角θi 的程序（m 文件名为fanqiuth.m ）如下：
[tth]=ode45('zhaolei',[0:0.01:1],[ pi/2;-pi/3;0;
pi/2;0]);
plot(t,th(:,1),'--b.',t,th(:,2),'-gx',t,th(:,3),':r*',t,th(:,4),'-.mx',t,th(:,5),'--k*');
xlabel('Time');title('关节轨迹曲线');
由MATLAB 画出的关节轨迹曲线如下图所示：
其中蓝色曲线表示θ1，绿色曲线表示θ2，红色曲线表示θ3，
品红色曲线表示θ4，黑色曲线表示θ5。
8.2 利用上面反求的各θ用Matlab 计算出末端轨迹
编写一个M 文件，命名为yuan.m ，程序如下：
syms th1 th2 th3 th4 th5
th1=th(:,1);th2=th(:,2);th3=th(:,3);th4=th(:,4);th5=th(:,5);
px=(cos(th1).*cos(th2).*cos(th3).*cos(th4).*sin(th5)-sin(th1).*sin
(th3).*cos(th4).*sin(th5)+cos(th1).*sin(th4).*sin(th5)-sin(th2).*sin(th4).*sin(th5)+cos(th1).*cos(th2).*sin(th3).*cos(th5)+sin(th1).*cos(th3).*cos(th5)+sin(th1).*sin(th3).*sin(th4)-cos(th1).*cos(th2).*cos(th3).*sin(th4)+cos(th1).*cos(th4)-sin(th2).*cos(th4)+cos(th1).*cos(th2).*sin(th3)+sin(th1).*cos(th3)+cos(th1)+sin(th1)-sin(th2))./2;
py=(sin(th1).*cos(th2).*cos(th3).*cos(th4).*sin(th5)+cos(th1).*sin
(th3).*cos(th4).*cos(th5)-sin(th1).*sin(th2).*sin(th4).*sin(th5)+sin(th1).*cos(th2).*sin(th3).*cos(th5)-cos(th1).*cos(th3).*cos(th5)+sin(th1).*cos(th2).*sin(th3)-cos(th1).*cos(th3)-sin(th1).*sin(th2)-cos(th1)-sin(th1).*cos(th2).*cos(th3).*sin(th4)-cos(th1).*sin(th3).*sin(th4)-sin(th1).*sin(th2).*sin(th4))./2; pz=(sin(th2).*cos(th3).*cos(th4).*sin(th5)+cos(th2).*sin(th4).*sin
(th5)+sin(th2).*sin(th3).*cos(th5)+sin(th2).*sin(th3)+cos(th2)+1-sin(th2).*cos(th3).*sin(th4)+cos(th2).*cos(th4))./2;
画出满足所求 的末端轨迹
t=0:0.01:1;
jiaoweiyi
plot3(px,py,pz);
axis equal
所得轨迹如下图所示：
8.3 利用ProE 仿真所得轨迹，如下图所示：