EDA365电子论坛网

标题: #技术风云榜#matlab机器人p560 由关节角求驱动力矩仿真代码 [打印本页]

作者: ededewa    时间: 2020-11-20 15:36
标题: #技术风云榜#matlab机器人p560 由关节角求驱动力矩仿真代码

; i: G6 [; m. m- L4 S
# L' ]4 I8 x; G5 ?" E
 $ m7 z% G& A$ a1 D9 @4 c. C

* F; ]; q8 c* }7 }1 i, w9 V9 j3 C1 ~! o; g0 e

% }# X9 b' D" X$ k8 p9 i, V" |

1 n# i4 u2 h1 X7 \L{1} =link([-pi/2    0       pi/2    0         0  1 0 0 0 0 0 0 0 0 0 0 0],'sta')% 1是连杆质量  `! G3 \# m2 U3 c, ^; @2 R8 R
L{2} =link([0       149      0       432     0  1 0 0 0 0 0 0 0 0 0 0 0],'sta')6 _* d- x/ Y0 s( \# h" r& v
L{3} =link([pi/2    0        pi/2     -20    0   1 0 0 0 0 0 0 0 0 0 0 0],'sta')
, g/ E: r+ l. q  ?4 v. @5 a. t4 EL{4} =link([-pi/2    433     0       0       0   1 0 0 0 0 0 0 0 0 0 0 0],'sta')
! k6 h/ Z/ D: l, y, r5 f2 \6 bL{5} =link([pi/2     0       0        0        0   1 0 0 0 0 0 0 0 0 0 0 0],'sta')! C/ r* d5 _5 P( f& C
L{6} =link([0        56      0        0        0   1 0 0 0 0 0 0 0 0 0 0 0],'sta')  E4 c. o6 B. M! ~
p560m=robot(L)
- F+ P2 Y' R6 ~) `; c5 t; Uc=[0 0 pi/2 0 0 0]%%%角度改为  [p i/2 0 pi/2 0 0 0]
8 P) U$ V3 _+ ?+ r& A/ |T1=fkine(p560m,c)) K$ N1 t4 }# B* v" [8 D4 [
T2=T1-[0 0 0 500;0 0 0 -200;0 0 0 0;0 0 0 0]/ o7 r. ~( e$ ^6 n6 [- Z( I
t=0:0.056:10
$ a& x& n7 ~1 M& E# B$ xT=ctraj(T1,T2,length(t))- _  b& k% X. v$ B
p=ikine(p560m,T)/ i- p- o5 ]# Q+ K
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%添加的力矩控制
# e- ]( I; I- u- Pc2=p(length(t),: )%取出到达目标时的各个关节角度6 s' ^" A! @1 U: m4 }, D; {
[q qd qdd] =jtraj(c,c2,t)%得到关节空间规划轨迹7 ^) R, O; K* h
F=rne(p560m,q,qd,qdd)%得到控制各个关节的输出力矩
& Y4 `/ V( M# H7 M3 J( y  R%plot(F)# g! B. C8 u4 B& ?& J
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%0 G; {5 B5 K- ^) O2 b
" r1 i4 T' o% t: _- L' C" [0 e! u
for i=1:10:length(t)% 正常轨迹 i=1:length(t)) ]0 r7 r9 B1 Z' K+ Q9 e0 ]
x(i)=T(1,4,i)6 t* i  I1 }* }5 M
y(i)=T(2,4,i)
9 n) d* E. E' z* o/ f0 nz(i)=T(3,4,i)
5 {" W. X  \' O+ cend
! {: u. x& K  c1 E2 _plot3(x,y,z)%画出运动轨迹
+ C% C$ k5 ]6 V; `0 Yplot(p560m,p)6 b9 ~. h6 m; l% J0 s1 r
%plot(p)% 画出各个关节角的变量图
! r7 `8 C4 L! Z* l$ K( q% T%drivebot(p560m)
1 j' R) e$ I- w7 G# B- r" P2 Zt1=t', n  k6 z  P/ M& C) n  b* Y
B=[t1 F]%关节驱动力矩输出5 T; s( d* E. A  y7 Y' h
a1=p(:,1)) W1 x% ], G2 E- U+ x* [
a2=p(:,2)  
7 {# O1 n# |# g) S9 c% _5 b8 |" C( aa3=p(:,3)  y5 M7 N" I7 K1 y
a4=p(:,4)) [' j2 |: {0 B# t' }% J4 ]
a5=p(:,5)  9 Q' ]$ D7 |2 M# B6 y4 S" Y2 |/ K
a6=p(:,6)' Z+ E+ E2 \% ]3 h$ ?% Q4 }
%输出到 simulink的变量 [t1,a1]% R; n% h# Q' ]
7 M; S% c8 C4 |' i! {* t

1 s  }  J7 _2 x
作者: NingW    时间: 2020-11-20 16:33
matlab机器人p560 由关节角求驱动力矩仿真代码




欢迎光临 EDA365电子论坛网 (https://bbs.eda365.com/) Powered by Discuz! X3.2