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

%这里的每个连杆的数据是和下一个坐标系之间的关系，也就是求得的是机械手末端的坐标系  ( ?; E9 ?- Q. G  r, _; v9 L. h. b, 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是连杆质量
3 j4 x/ q5 _4 b$ V' x% r3 h1 ZL{2} =link([0       149      0       432    0       0.0000001 0 0 0 0 0 0 0 0 0 0 0],'sta')3 T, E- w7 G: ]$ p7 R8 W! m
L{3} =link([pi/2    0        pi/2     -20    0       0.0000001 0 0 0 0 0 0 0 0 0 0 0],'sta')7 E0 f" B5 Z  j2 {; `5 L" B
L{4} =link([-pi/2    433     0        0      0       0.0000001 0 0 0 0 0 0 0 0 0 0 0],'sta'), d3 B, ^9 e" K2 a/ P: R# E
L{5} =link([pi/2     0       0        0          0      0.0000001 0 0 0 0 0 0 0 0 0 0 0],'sta')1 w- g5 x, h! _6 s
L{6} =link([0        56      0        0          0      0.000004 0 0 0 0 0 0 0 0 0 0 0],'sta')+ P3 j: i' u; w% s7 j# K3 H
p560m=robot(L);

c=[0 0 pi/2 0 0 0];( ~9 n/ [% Y; d  P% A
T1=fkine(p560m,c);) |8 [1 s: ?' B; J; ^$ l
T2=T1-[0 0 0 500;0 0 0 -200;0 0 0 0;0 0 0 0];8 C% ?! A2 l. C8 F5 Z& Q6 l
t=0:0.056:10;
) ?, P9 x- V% }7 t& lT=ctraj(T1,T2,length(t));+ w9 u% V# o  ^! ]' G
p=ikine(p560m,T);; k  @* |7 l! k" b6 w, O+ U. o
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%添加的求得转矩' x9 e% M9 T8 s* i1 [! E3 ?. h
c2=p(length(t),: );   %取出到达目标时的各个关节角度0 u! _/ H- o, `" a9 W2 p; _
[q qd qdd] =jtraj(c,c2,t);   %得到关节空间规划轨迹6 G7 H, v2 @" i5 z7 Q& Y
F=rne(p560m,q,qd,qdd);       %得到控制各个关节的输出力矩;! r0 \  P4 R$ p/ D1 F  s
%plot(F)

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%添加的求得控制输入电压
0 P+ |% _1 r7 K" y/ ], vU1=220/sqrt(3)%线电压
3 T6 U/ ?* ^/ I. d9 u9 x" ]N=3%电源相数
  f+ k  q) D2 v  cP=2%极对数
, w3 h2 X2 k8 n0 t: {$ j& ]FN=50%电源频率  R7 `& a7 k) D
R1=0.095%定子电阻6 }$ M: P& q5 d! t5 W
X1=0.68%定子漏电感3 P+ l7 P+ M5 N" k! @# g% n* z
X2=0.672%转子漏电感" y! }0 m# F* q9 N5 B# W7 j
sd=2*pi*FN
+ b1 Z( x7 r* v1 p1 Sns=60*FN/P%同步转速

R2=0.1%%转子电阻; N  i, @1 u) l4 y, S" e6 d
s=0.8%转差率
9 e- y# `. V3 _$ Cfor  i=1:length(t)5 L$ K0 y) u  b$ C  f
    for j=1:6
7 ~9 D/ B; I3 |# A. K  I+ u        U2=N*P*R2/s/sd/[(R1+R2/s)^2+(X1+X2)^2];%三相异步电机转矩公式
2 o; h) ~; `% y        U(i,j)=F(i,j)/U2*sqrt(3);
2 |% q# V' ]- G! B8 a    end
/ B0 X5 S7 @  V# p/ q4 A- x   
* P0 b5 j5 e6 Tend
+ D1 Q" ?  \* x5 Fplot(U)2 D3 Y: @& x  H& N+ Y4 v% W
ylabel('电机输入电压')

% for m=1:5  %%%转子电阻对转矩的影响
0 X' r) W( A! J%       if m==1
9 N' c! b& `' k9 D# e8 v* L* g%           R2=0.1) i! g8 M: T% W) c& O; q1 e+ M
%       elseif m==2: w  b- R1 y2 |$ t% a( x. i  u
%           R2=0.24 {! U# G& U0 s) A0 s6 c+ n' t
%       elseif m==33 m% l$ d* c+ N5 a2 |& R; x
%           R2=0.5
& @4 h% L7 Z' ~/ K! J. Q5 c- _%       elseif m==4
& I8 a# d4 w/ I4 |%           R2=1.0
$ ^/ `  M) i/ w8 i; C2 \# ~%       else R2=1.5 end
) v: m* l. o- W- N: g% M%      8 m0 z: A, [- ~- n# S" W
%      
8 C4 C; W: q, C6 J: l( I. N%  for  n=1:400# K9 l) ~2 u% I1 Z% v- G' j
%      s=n/400 2 j) h; Y" @- g8 j* h8 W* w! }! ]
%     t=N*P*U1*R2/s/sd/[(R1+R2/s)^2+(X1+X2)^2]
$ W  k& C# T  V0 b, B& }( X- b0 p# e%      plot(s,t)  %转差率与转矩的关系
: a+ x5 q. G1 b; t4 [! P6 e# d6 ?%      hold on
, q2 N) T1 T, J% \9 X* }( W* t%  end
: w+ Q- F" S9 h" F0 Z8 @%    hold on: {$ G( t/ t- V# k2 c) }
% end/ b+ o. a9 t1 @. p3 @3 Y* N
% xlabel('转差率')5 N; `2 v$ g$ X8 J% D& N2 l
% ylabel('电机转矩')

$ b5 G) E. j* j9 a
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%# |: G( k! V! w) i2 @
for i=1:10:length(t)% 正常轨迹 i=1:length(t)! ?  C( f6 i% i' n0 K' [3 D
x(i)=T(1,4,i): v2 E' X3 Y  h" ?/ }
y(i)=T(2,4,i)5 P) }3 Z3 i1 ~9 q8 [: `
z(i)=T(3,4,i)
" u1 L& D0 t- G# T6 Vend
8 F' V1 g5 b4 z%plot3(x,y,z)%画出运动轨迹
) G! W9 V7 L* ]& S%plot(p560m,p)! {0 W) ]8 s- d2 n% m: h% m: K
%plot(p)% 画出各个关节角的变量图4 H( z3 G: P/ R
%drivebot(p560m)

t1=t'* a; `' e; Q( F
B1=[t1 F]%电机输出力矩
8 W( Y' M7 h8 M2 A4 T% AB2=[t1 U]%电机控制电压: G$ O2 m  \/ F# C; l8 n' @8 A
a1=p(:,1)' @3 _/ ^" q0 _! Q1 X# j
a2=p(:,2)  5 _* w+ [6 x) w/ Z: a  f' }) y+ \; b
a3=p(:,3)
$ v! b2 o; T/ o/ {, `1 Ra4=p(:,4)+ ]  M: w. H- X9 H$ H: b
a5=p(:,5)  , |$ `( f# T* D; g% r
a6=p(:,6)  n3 j1 w" [) B$ T1 U4 v
%输出到 simulink的变量 [t1,a1]
% n4 a& h$ _# ]! l$ `2 h1 I%plot(F)