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

ededewa

1 k! r, u! i: F3 l; y, f



4 y6 m* c- v' I; ]
( O* k  N' X3 F: h& I2 g

5 w4 U& k2 a1 Y# B8 b$ M, Q/ b
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')
3 C& |" Z! X+ J; Y& t  d& wL{3} =link([pi/2    0        pi/2     -20    0   1 0 0 0 0 0 0 0 0 0 0 0],'sta')
L{4} =link([-pi/2    433     0       0       0   1 0 0 0 0 0 0 0 0 0 0 0],'sta')
6 s# O! V0 q& S% Z" H0 ~, E% z& jL{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)
; M) T- M8 h! j5 K) Y0 ?0 o& G9 M9 tc=[0 0 pi/2 0 0 0]%%%角度改为  [p i/2 0 pi/2 0 0 0]
T1=fkine(p560m,c)
% E" J5 y4 @3 D& |8 P  O& R( Z$ y2 UT2=T1-[0 0 0 500;0 0 0 -200;0 0 0 0;0 0 0 0]
' f- G! x0 h2 h$ ^3 Bt=0:0.056:10
# b6 P- K& s$ kT=ctraj(T1,T2,length(t))
p=ikine(p560m,T)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%添加的力矩控制
  s( q2 O# `4 f% `, v0 J8 Hc2=p(length(t),: )%取出到达目标时的各个关节角度
[q qd qdd] =jtraj(c,c2,t)%得到关节空间规划轨迹
7 W9 [# l0 d: j/ s* }0 s! mF=rne(p560m,q,qd,qdd)%得到控制各个关节的输出力矩
1 F8 \0 g( T3 ?%plot(F)
  X) |! W+ a- ^! k' t%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

, U# }5 U% h6 H4 O* E( v5 x; _for i=1:10:length(t)% 正常轨迹 i=1:length(t)
1 I: l% o+ F% V# [" y. Sx(i)=T(1,4,i)
y(i)=T(2,4,i)
z(i)=T(3,4,i)
( @* O/ `1 P1 a$ d# b" Mend
plot3(x,y,z)%画出运动轨迹
9 z4 Z& M5 _  z, ^9 x. _plot(p560m,p)
%plot(p)% 画出各个关节角的变量图
; y" ~$ w# V+ G8 q%drivebot(p560m)
t1=t'
B=[t1 F]%关节驱动力矩输出
' b: n/ P" Y# F# E$ Ka1=p(:,1)
" h; y4 u6 v+ v+ M# ia2=p(:,2)  
/ M9 P! d$ I( l8 ta3=p(:,3)
a4=p(:,4)
4 O" E* e3 `7 O  h% P3 V) S( ^5 c0 xa5=p(:,5)  
a6=p(:,6)
& |$ a9 ]& V* L7 l; u' \%输出到 simulink的变量 [t1,a1]
$ g* x' b: a' N$ B! b! f+ T# D; r
0 _! m' a( i, o, `& s5 _
, H1 c% r# B' |* z3 D

NingW

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