#技术风云榜#matlab机器人p560 由驱动转矩求得三相异步电机控制电压仿真

%这里的每个连杆的数据是和下一个坐标系之间的关系，也就是求得的是机械手末端的坐标系  - [9 h3 n! U2 q  [
L{1} =link([-pi/2    0       pi/2    0        0       0.00001 0 0 0 0 0 0 0 0 0 0 0],'sta')% 0.00001是连杆质量, q6 b8 v1 o/ p+ y% G0 ?; ?1 z
L{2} =link([0       149      0       432    0       0.0000001 0 0 0 0 0 0 0 0 0 0 0],'sta')
( U' g: Z3 n! c( cL{3} =link([pi/2    0        pi/2     -20    0       0.0000001 0 0 0 0 0 0 0 0 0 0 0],'sta')
7 A, }' ], g, g9 D9 \0 ZL{4} =link([-pi/2    433     0        0      0       0.0000001 0 0 0 0 0 0 0 0 0 0 0],'sta')  [$ K5 j. b4 w& B! S
L{5} =link([pi/2     0       0        0          0      0.0000001 0 0 0 0 0 0 0 0 0 0 0],'sta'), A: W( V8 q/ R; z  ?. L
L{6} =link([0        56      0        0          0      0.000004 0 0 0 0 0 0 0 0 0 0 0],'sta')
' I0 z8 P2 R/ ^4 np560m=robot(L);

c=[0 0 pi/2 0 0 0];
, x, f. q' ^4 dT1=fkine(p560m,c);6 p# Y+ h* y9 y* `
T2=T1-[0 0 0 500;0 0 0 -200;0 0 0 0;0 0 0 0];# t: d7 Y0 t5 {7 ?. C
t=0:0.056:10;1 M# l" M6 |4 e& k, [' X
T=ctraj(T1,T2,length(t));6 ^& L( p- h9 |& _
p=ikine(p560m,T);- I3 |! ~: @0 I2 p& K; n/ Q: h
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%添加的求得转矩3 a1 G' C$ J/ t- K
c2=p(length(t),: );   %取出到达目标时的各个关节角度
; t" R. d3 T* [* Y" j! G9 ~: b[q qd qdd] =jtraj(c,c2,t);   %得到关节空间规划轨迹3 S/ Q# J. O9 e+ o- G
F=rne(p560m,q,qd,qdd);       %得到控制各个关节的输出力矩;& \' f9 h( h4 m4 g
%plot(F)

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%添加的求得控制输入电压& R) p  K  b' \  T  L
U1=220/sqrt(3)%线电压/ q+ F" C% e1 }# ]4 a6 W  Q1 s2 F0 v
N=3%电源相数
3 ~2 r* U: [6 y. x- _* s) hP=2%极对数
* i# ?7 u: [9 J" U/ xFN=50%电源频率
+ C! [- ^2 F0 \' v+ t) W* [0 tR1=0.095%定子电阻
" l* g6 a+ R* z" r* Y2 mX1=0.68%定子漏电感8 l  m3 L: K$ |1 O+ v( D
X2=0.672%转子漏电感
3 f; N* E% X: x& Psd=2*pi*FN# n# ]2 M$ N7 [0 c" L5 b
ns=60*FN/P%同步转速

R2=0.1%%转子电阻
# O" J. F0 a) z! d' {s=0.8%转差率
  l5 f. S4 ^/ ffor  i=1:length(t)
* j0 l) @6 m9 q+ G& n( g' z" I    for j=1:6$ y5 w" i$ }% e
        U2=N*P*R2/s/sd/[(R1+R2/s)^2+(X1+X2)^2];%三相异步电机转矩公式$ i8 W' z3 ^. S
        U(i,j)=F(i,j)/U2*sqrt(3);3 X" J" i2 S+ D3 T. a% y
    end
/ m  W' a2 }7 g* a    : _! R' I+ f: [1 P7 I/ m& T% ~2 ^
end
1 J- j* k9 P) U$ x: vplot(U)
: k" F9 v$ a7 _' e9 L) fylabel('电机输入电压')

% for m=1:5  %%%转子电阻对转矩的影响
* E& U, i. b2 T8 u# q) ^%       if m==1
) D* d5 D4 \( g$ B1 {* q" U. C%           R2=0.1, l' B; @/ i$ a- D  Y
%       elseif m==2
7 n) B! E* t- e  m6 K. ~. `%           R2=0.2
! _( A1 h# s3 A" ]/ D+ g; \5 o5 v%       elseif m==3% x% {/ T; `1 V' x$ q$ q. v" W
%           R2=0.5
8 m, i4 e# p% Y8 A% k7 a%       elseif m==4
4 ]4 {8 I! q& u, l: M' z%           R2=1.0
  Y' ]0 O% ^4 e% @. o. k%       else R2=1.5 end4 p4 ?5 V- P* D9 s! E' F8 Q# I
%      4 n3 U$ C: b2 U# C4 g
%       * V3 P2 d0 g% N2 u
%  for  n=1:4000 c/ J7 R0 Z3 _# L: H/ H
%      s=n/400
2 d/ N6 g( L4 U! Q4 J%     t=N*P*U1*R2/s/sd/[(R1+R2/s)^2+(X1+X2)^2]
# T# y9 F; f$ S: e/ q. p%      plot(s,t)  %转差率与转矩的关系
: e. m8 a, [1 @% H/ G( L%      hold on7 x0 G( J: H# _0 \) j% ?. t3 H
%  end
6 }1 _$ `9 z9 v) b% W( T, o%    hold on; ]$ P$ C, l# T! |, q
% end
7 B7 ^9 Y9 Y. m: ~3 g; M1 t7 X6 x6 X% xlabel('转差率')) z: ]4 u. R( U! J" l  U. e, M
% ylabel('电机转矩')

5 D! {; g/ Q  p. A( J%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
. l/ a$ H2 ?/ q1 x7 e5 vfor i=1:10:length(t)% 正常轨迹 i=1:length(t), [6 I4 T2 }$ B. N7 Y, e
x(i)=T(1,4,i)4 S5 \2 \3 @9 p  F7 g2 m
y(i)=T(2,4,i)
1 \" z- ]7 k6 j8 u. Q- E9 Hz(i)=T(3,4,i)
: Q0 W' j* Q* ^7 G; uend
( [8 B0 b5 m& A" x- r+ x9 E" _: a%plot3(x,y,z)%画出运动轨迹
6 C2 {, A$ Z8 q! g. O" x%plot(p560m,p)
# X# p- ]+ F/ O/ ~9 l%plot(p)% 画出各个关节角的变量图% c$ P1 ?2 {4 }+ q6 e$ Z
%drivebot(p560m)

t1=t'0 W2 [: L7 p0 B7 \" @* G
B1=[t1 F]%电机输出力矩
9 l! a) e7 b- `/ Q! B1 N1 {B2=[t1 U]%电机控制电压/ I6 v  Z1 j- r7 Y6 ?( t
a1=p(:,1)
) K1 S0 q& @8 h+ c3 y7 ba2=p(:,2)  
3 `! L2 t- P+ ^a3=p(:,3)
* x& b: O1 O, \6 Q$ w# ka4=p(:,4): ~3 ~5 v( R6 j7 L
a5=p(:,5)  ) T, P: y# ]) L
a6=p(:,6)9 }" u# n5 ]# z
%输出到 simulink的变量 [t1,a1]
' {* N- ?! U9 [* U%plot(F)