#技术风云榜#matlab机器人p560 由关节角求驱动力矩仿真代码

ededewa

. n7 [) G# b( g



# A1 N1 \) L& O% U9 a5 W% m



+ {' @4 \9 V+ ?; Y1 I. @L{1} =link([-pi/2    0       pi/2    0         0  1 0 0 0 0 0 0 0 0 0 0 0],'sta')% 1是连杆质量
L{2} =link([0       149      0       432     0  1 0 0 0 0 0 0 0 0 0 0 0],'sta')
L{3} =link([pi/2    0        pi/2     -20    0   1 0 0 0 0 0 0 0 0 0 0 0],'sta')
' c" V5 f) y. e7 OL{4} =link([-pi/2    433     0       0       0   1 0 0 0 0 0 0 0 0 0 0 0],'sta')
( r# R/ Y3 T. A' f) iL{5} =link([pi/2     0       0        0        0   1 0 0 0 0 0 0 0 0 0 0 0],'sta')
L{6} =link([0        56      0        0        0   1 0 0 0 0 0 0 0 0 0 0 0],'sta')
p560m=robot(L)
, C+ q1 b  w' Y& H0 {' U9 e/ yc=[0 0 pi/2 0 0 0]%%%角度改为  [p i/2 0 pi/2 0 0 0]
/ ^/ C) C, A$ ], Y; R( ZT1=fkine(p560m,c)
T2=T1-[0 0 0 500;0 0 0 -200;0 0 0 0;0 0 0 0]
8 j) ~/ r& u/ E8 B1 {0 tt=0:0.056:10
' M) b' q; U  U7 I! N' o  YT=ctraj(T1,T2,length(t))
6 X+ e* p' Z" B7 ~% p7 Z: Xp=ikine(p560m,T)
& I4 \* \. R+ |' l* `  k/ `  q" W5 h3 d%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%添加的力矩控制
0 q8 S4 `! C+ {$ @0 |c2=p(length(t),: )%取出到达目标时的各个关节角度
1 [6 G2 n7 W2 z. S7 S* b& e& q/ Z[q qd qdd] =jtraj(c,c2,t)%得到关节空间规划轨迹
F=rne(p560m,q,qd,qdd)%得到控制各个关节的输出力矩
%plot(F)
8 ]' u( i- A$ G%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
- L) ~  H; S) A% h& s" j* T
for i=1:10:length(t)% 正常轨迹 i=1:length(t)
! D3 b$ m5 `8 w' Rx(i)=T(1,4,i)
5 _( R# |1 g/ h' ?; F2 By(i)=T(2,4,i)
z(i)=T(3,4,i)
! n3 z- h% [) n2 X2 e+ M0 a' ?; B% @end
plot3(x,y,z)%画出运动轨迹
0 E( ^/ Q# L3 r6 l8 {! C/ G: L/ Y0 p' eplot(p560m,p)
9 N6 B% h4 r, x%plot(p)% 画出各个关节角的变量图
%drivebot(p560m)
t1=t'
% `; S, ]: [! Z/ B+ ?0 c! \B=[t1 F]%关节驱动力矩输出
a1=p(:,1)
7 t. E4 F" g, _# L8 C( ma2=p(:,2)  
a3=p(:,3)
! }4 j+ V0 B2 g  q  }/ p+ r) ja4=p(:,4)
& O. J- U) v+ l4 _5 da5=p(:,5)  
, b0 T; z4 z7 F0 q- n9 t! ^/ @a6=p(:,6)
6 V+ M; V& p7 S* A%输出到 simulink的变量 [t1,a1]

5 Y, p* X% l7 e5 I9 e/ P  \# U
% y. h4 v) O6 _, G8 n

NingW

matlab机器人p560 由关节角求驱动力矩仿真代码