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

%这里的每个连杆的数据是和下一个坐标系之间的关系，也就是求得的是机械手末端的坐标系  % S8 Y% v4 c7 S- X! k1 Q6 G4 o
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是连杆质量
6 _$ E& O2 `& B2 e- H0 \' uL{2} =link([0       149      0       432    0       0.0000001 0 0 0 0 0 0 0 0 0 0 0],'sta')- v3 j2 M$ k. M% ^5 Y7 ~
L{3} =link([pi/2    0        pi/2     -20    0       0.0000001 0 0 0 0 0 0 0 0 0 0 0],'sta')
4 \) p1 {6 V- t5 j, E6 |L{4} =link([-pi/2    433     0        0      0       0.0000001 0 0 0 0 0 0 0 0 0 0 0],'sta')
+ O7 _/ b  ~$ S: ^$ R5 [' AL{5} =link([pi/2     0       0        0          0      0.0000001 0 0 0 0 0 0 0 0 0 0 0],'sta')  t0 h7 c* ]% S" {) B: F* f
L{6} =link([0        56      0        0          0      0.000004 0 0 0 0 0 0 0 0 0 0 0],'sta')
3 x6 j: a5 K; K$ a$ s  Y) Dp560m=robot(L);

c=[0 0 pi/2 0 0 0];
* _, o; O0 Q9 Z% b$ pT1=fkine(p560m,c);
4 c9 Z9 ]7 g8 i7 GT2=T1-[0 0 0 500;0 0 0 -200;0 0 0 0;0 0 0 0];
2 M4 O; A6 h$ y+ ^8 f7 s& bt=0:0.056:10;# H, J  i8 w+ T+ N
T=ctraj(T1,T2,length(t));
* u# r$ ]( C1 f! d' bp=ikine(p560m,T);& V; o3 C5 f. E$ N# @: N) l
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%添加的求得转矩
& L% [6 q" {1 G8 i# O6 Lc2=p(length(t),: );   %取出到达目标时的各个关节角度
3 n3 t4 `& ?. G, i[q qd qdd] =jtraj(c,c2,t);   %得到关节空间规划轨迹
" T/ J+ |6 K) G! F4 [3 q2 BF=rne(p560m,q,qd,qdd);       %得到控制各个关节的输出力矩;
/ T& ~) J8 V5 z' C( U5 K/ l%plot(F)

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%添加的求得控制输入电压
% w# U- L! _' O. bU1=220/sqrt(3)%线电压
+ ~! D& B3 y. z( v" |. V% ^N=3%电源相数
1 h2 S8 x; e9 v& p0 G6 iP=2%极对数( v, j; `' ^/ n& c1 m
FN=50%电源频率
% H$ V' L, A# Q2 N) ^) @$ sR1=0.095%定子电阻
5 G0 \2 _- k. T5 R% h! o$ CX1=0.68%定子漏电感
6 N  X$ m' v# t4 r. YX2=0.672%转子漏电感
) h" C+ k- O8 W) k" g6 `9 Asd=2*pi*FN5 ~- I+ f; W2 J( N2 h
ns=60*FN/P%同步转速

R2=0.1%%转子电阻
2 G5 ^, M) w7 M( D& K% t; Rs=0.8%转差率
" P' F' W2 L: h$ t1 sfor  i=1:length(t)
9 ?# s& I' V" O/ y    for j=1:6
2 W8 Z$ X# u# g( {) P- Z        U2=N*P*R2/s/sd/[(R1+R2/s)^2+(X1+X2)^2];%三相异步电机转矩公式, j) N# }- y0 ?! y0 b
        U(i,j)=F(i,j)/U2*sqrt(3);
7 [% D6 H3 s8 ]    end% s, o9 ~5 d+ J: h
    % I1 R. L% u; X& U2 ]
end
" ?0 ^4 L/ S6 l, u; |0 w; Y9 hplot(U), B" P0 |) V* V# X7 g/ e
ylabel('电机输入电压')

% for m=1:5  %%%转子电阻对转矩的影响
( L- J  c# n4 g, e- p4 ~%       if m==15 Y" H  E3 d; }1 U  C3 b7 q
%           R2=0.14 q( }# \  R+ F7 J2 z
%       elseif m==2
! h5 s- l( ~1 T0 @) p+ }%           R2=0.2
, T+ k: k: o: |! V%       elseif m==39 j" D& J5 j$ P% h& q; j
%           R2=0.5
( i; _" t: @% S0 h( Y%       elseif m==4% ^6 K  X2 ?" d1 V. ^
%           R2=1.0
$ i) T+ L5 I8 b) l' d0 j0 Q5 ]%       else R2=1.5 end
% X* P4 y/ ^# \3 m5 z; c8 r0 e) F%      
, G4 j/ \' x9 n% `%      
" Y4 H8 }" V3 ?/ k6 F%  for  n=1:400
. W* E, a  V+ ~/ u7 ~- z) X; v* y%      s=n/400
; g# v, [- ?' {; {%     t=N*P*U1*R2/s/sd/[(R1+R2/s)^2+(X1+X2)^2]+ z3 ^# r4 D& U6 o# W& `
%      plot(s,t)  %转差率与转矩的关系' E) `9 s# q5 Y1 j
%      hold on+ k' c3 B$ V0 G: g
%  end
7 v- |( D, k3 `, ?3 g3 q$ a- {%    hold on
& F- W& m: s2 v% end
6 F1 I6 s( z: B6 m/ Y4 W% xlabel('转差率')
& x' ]! i' r" a2 O% ylabel('电机转矩')

1 W9 w. }$ h/ g- w) \0 t/ ~%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
5 Q# j  g4 @) F& q) e# [for i=1:10:length(t)% 正常轨迹 i=1:length(t)
( G" \4 P( q2 N, `0 N- [x(i)=T(1,4,i)
/ C: j) Q6 l* K" Yy(i)=T(2,4,i)4 u: S. O1 Y; d9 d0 G
z(i)=T(3,4,i)
' D7 p  R) T  D7 iend
* |( p4 N) a$ S( T0 [%plot3(x,y,z)%画出运动轨迹
- W% _1 @  s& s1 o: X/ s( {%plot(p560m,p)
' a/ b* V7 ]6 p! R0 m# U%plot(p)% 画出各个关节角的变量图7 }! O3 n- S" n: ]$ `
%drivebot(p560m)

t1=t'- b1 J0 W/ b2 A# ^* _
B1=[t1 F]%电机输出力矩1 }9 P# l3 u$ c* W
B2=[t1 U]%电机控制电压: ]7 P  I5 j5 V8 C; I
a1=p(:,1)
8 e  |5 W9 u% |3 t1 ^a2=p(:,2)  2 t, h7 F  q8 E; _( X: L( M
a3=p(:,3)* ?2 n1 j& s( C8 ?. i7 Y3 G9 S$ s
a4=p(:,4)
/ }# m; U# U0 a4 E. c1 d+ n- ?a5=p(:,5)  
2 n1 @/ n9 N$ sa6=p(:,6)
% I: \& a; Z1 B8 j+ j2 u6 J8 C# P/ U%输出到 simulink的变量 [t1,a1]" H6 _  x) \5 z  n; y5 k- z
%plot(F)