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

ededewa


6 H0 `: F; n8 x5 T. M# ?, d! P

& |6 R- O2 T; M( i+ p
7 L( I& Q! C3 d0 O
! M# e+ Z4 u( ]6 e+ u, c
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')
6 X2 t5 P1 |- W/ \' d' J: _L{4} =link([-pi/2    433     0       0       0   1 0 0 0 0 0 0 0 0 0 0 0],'sta')
# J- @( r# b: u+ c" k1 S3 qL{5} =link([pi/2     0       0        0        0   1 0 0 0 0 0 0 0 0 0 0 0],'sta')
' O+ N# E* k0 u; t% N! W" nL{6} =link([0        56      0        0        0   1 0 0 0 0 0 0 0 0 0 0 0],'sta')
3 P0 k2 {* K, F+ a( d0 mp560m=robot(L)
$ E4 e/ z4 t4 Q% X' Ac=[0 0 pi/2 0 0 0]%%%角度改为  [p i/2 0 pi/2 0 0 0]
T1=fkine(p560m,c)
0 ]! e& b, y8 n, U; J7 w. O4 xT2=T1-[0 0 0 500;0 0 0 -200;0 0 0 0;0 0 0 0]
t=0:0.056:10
T=ctraj(T1,T2,length(t))
" A9 Y* _% T3 l% dp=ikine(p560m,T)
' `2 [- M3 |# V( c" ]! r%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%添加的力矩控制
  q$ Q$ r( z# S/ F7 Vc2=p(length(t),: )%取出到达目标时的各个关节角度
[q qd qdd] =jtraj(c,c2,t)%得到关节空间规划轨迹
F=rne(p560m,q,qd,qdd)%得到控制各个关节的输出力矩
* r9 |- s$ N" ]%plot(F)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

" Z  k- _- Z1 x! ~for i=1:10:length(t)% 正常轨迹 i=1:length(t)
4 z) c' Z  c8 B! S; z) ox(i)=T(1,4,i)
5 P+ ~9 W9 K0 u- F; T3 r4 Py(i)=T(2,4,i)
8 Q' O" T% S' g. x# Z8 F' kz(i)=T(3,4,i)
# i# V% a/ w+ send
plot3(x,y,z)%画出运动轨迹
plot(p560m,p)
%plot(p)% 画出各个关节角的变量图
. j, E8 w# @  W; J- ^1 S0 |%drivebot(p560m)
$ D! p9 P9 I/ G+ e7 c6 ct1=t'
B=[t1 F]%关节驱动力矩输出
a1=p(:,1)
7 @. P4 V- O3 p- {a2=p(:,2)  
a3=p(:,3)
a4=p(:,4)
a5=p(:,5)  
" U7 ]; d; h2 pa6=p(:,6)
8 h$ O' y8 |! U0 d0 @%输出到 simulink的变量 [t1,a1]

7 G- c* f# k, {  b6 x8 c
) A) z5 ?1 a, L% w: v; ]

NingW

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