EDA365电子论坛网

标题: 机器人问题 [打印本页]
作者: uaidenp    时间: 2020-10-14 13:28
标题: 机器人问题
下面的代码在2015b可以运行；
& v: W/ A+ K6 Xclear;, J, t/ M, Y; Q& H. S7 z
clc;2 s  x: G4 m2 O* }# |# r- M
startup_rvc;
: d' p$ Z+ F/ h. w7 A1 G' }n = 3;
: ^0 @0 H. d: h$ f* \& Jswitch n( J( \1 G4 G4 ]7 p
    case 13 p/ z7 [, w3 f4 l4 o& Z
    %建立机器人模型2 V$ G) @9 y; ^" H8 v1 h, u
%       theta    d        a        alpha     offset- l6 o0 Q3 F) ^" p0 W
L1=Link([0       0.4      0.025    pi/2      0     ]); %定义连杆的D-H参数
6 r) N  Q8 ]5 V! F" _L2=Link([pi/2    0        0.56     0         0     ]);
0 y5 j, K5 Z% Y% r& m; {* ~& WL3=Link([0       0        0.035    pi/2      0     ]);
* ~" Z* L, i9 T+ {/ dL4=Link([0       0.515    0        pi/2      0     ]);
0 o) e# {+ ~6 A" F: lL5=Link([pi      0        0        pi/2      0     ]);# x9 @2 v! I3 k1 T. v% W3 J4 Q# X
L6=Link([0       0.08     0        0         0     ]);& V5 h- c1 F5 @# V8 v2 _, o
robot=SerialLink([L1 L2 L3 L4 L5 L6],'name','manman'); %连接连杆，机器人取名manman
/ h+ t& b* |  L3 p% r1 R$ arobot.plot([0,pi/2,0,0,pi,0]);%输出机器人模型，后面的六个角为输出时的theta姿态
, ?* S1 v$ J* probot.display();
; S$ `+ M* ^$ ~1 r3 {teach(robot);! c2 g' t; o5 p( h+ r( g9 r

9 j' t9 y. n. h! X3 pcase 2. ?# I# a, P. k8 D
%建立机器人模型6 k* J+ r% ^" r# M& C
%       theta    d        a        alpha     offset% ~& ~6 ?1 h4 T0 P  n
L1=Link([0       0.4      0.025    pi/2      0     ]); %定义连杆的D-H参数
0 P' S3 n  I) ^: [7 w9 |3 QL2=Link([pi/2    0        0.56     0         0     ]);
# P; R: k+ m- A; H. UL3=Link([0       0        0.035    pi/2      0     ]);
" [& o# s9 o9 j0 fL4=Link([0       0.515    0        pi/2      0     ]);7 O3 H, j9 H+ ]. x4 I
L5=Link([pi      0        0        pi/2      0     ]);
, d1 W2 O6 F: L6 Y+ zL6=Link([0       0.08     0        0         0     ]);
4 ]0 s  p) D: s  p% a6 qrobot=SerialLink([L1 L2 L3 L4 L5 L6],'name','manman'); %连接连杆，机器人取名manman
3 M8 l' {- [, v, y# r1 Wtheta=[0,0,0,0,0,0];%指定的关节角8 Q. T# [: x  Y9 G+ w9 X5 n
p=robot.fkine(theta)%fkine正解函数，根据我们给定的关节角theta，求解出末端位姿p
7 e  ?) {5 B+ `& fq=robot.ikine(p)%ikine逆解函数，根据我们给定的末端位姿p，求解出关节角q- S( ~7 D  d/ z& }( Y
! B7 }" @% B, O
case 3- W$ H8 N9 ~- ~4 w' i8 i! _3 ?
%建立机器人模型
: x6 B$ h& w% n* u/ y%       theta    d        a        alpha     offset& t0 G: }2 ?0 o
L1=Link([0       0.4      0.025    pi/2      0     ]); %定义连杆的D-H参数+ b. @5 o- A  D  ]! N
L2=Link([pi/2    0        0.56     0         0     ]);
5 J4 r! d! @6 pL3=Link([0       0        0.035    pi/2      0     ]);
8 w, L, b8 i" D5 \1 eL4=Link([0       0.515    0        pi/2      0     ]);# L! y/ W/ o$ }5 i; q4 [$ R1 @- H
L5=Link([pi      0        0        pi/2      0     ]);" X3 l2 J6 t+ @8 J0 O8 h
L6=Link([0       0.08     0        0         0     ]);
& B6 Z- C* R& v7 f% Z6 mrobot=SerialLink([L1 L2 L3 L4 L5 L6],'name','manman'); %连接连杆，机器人取名manman
$ G, V  f$ W; V  V# W) i" L0 gT1=transl(0.5,0,0);%根据给定起始点，得到起始点位姿3 M7 k6 b# _3 |6 e
T2=transl(0,0.5,0);%根据给定终止点，得到终止点位姿
  [) \$ c( s  w6 l, _5 `
; o# c3 u6 \: q. Y9 hq1=robot.ikine(T1);%根据起始点位姿，得到起始点关节角
0 G0 B/ ?2 R' J% e0 o5 pq2=robot.ikine(T2);%根据终止点位姿，得到终止点关节角
. H3 f: N) X& y( D" t4 ][q ,qd, qdd]=jtraj(q1,q2,50); %五次多项式轨迹，得到关节角度，角速度，角加速度，50为采样点个数7 O8 j2 y2 ~6 C% r; a
grid on
2 B4 t& ?$ |  a" n9 |T=robot.fkine(q);%根据插值，得到末端执行器位姿
) S9 B1 _# `  u4 |nT=T.T; plot3(squeeze(nT(1,4,),squeeze(nT(2,4,),squeeze(nT(3,4,));%输出末端轨迹
: h. z$ O1 D: H# A& T0 h4 P, G0 Mhold on$ e4 o2 |1 [) }2 @
robot.plot(q);%动画演示
0 E, e. I; u! Zend
' G4 \. V$ H  x* K
; q( q: m/ J' N7 `8 p& P然后修改了下连杆的D-H参数，以及起始和终止点的参数。( c1 r* Z8 d/ f4 |# Y9 f
%按UR5的DH参数修改后，第一个点打孔的轨迹0 I" U8 q4 f( e' }5 |1 P  ?
%建立机器人模型# x3 g" ~2 `7 j1 y0 s
%       theta    d        a        alpha     offset
' J/ Y: g9 C9 w% eL1=Link([0       89.2     0        pi/2      0     ]); %定义连杆的D-H参数
& B% s. ?5 X+ ~! o8 cL2=Link([pi/2    0        -425     0         0     ]);
5 u( [! C) y7 m, {5 _! ]L3=Link([0       0        -392     0         0     ]);( Z9 N' T+ D1 g9 I6 e" v
L4=Link([0       109.3    0        pi/2      0     ]);
. I' _4 b) a* }& F" HL5=Link([pi      94.75    0        pi/2      0     ]);
5 [( x3 N. w) {  OL6=Link([0       82.5     0        0         0     ]);
% M: P+ g* R' S, {) F2 mrobot=SerialLink([L1 L2 L3 L4 L5 L6],'name','manman'); %连接连杆，机器人取名manman8 m9 q* R# m: e- n& M5 z% A
theta=[0,0,0,0,0,0];%指定的关节角! J7 y) s" Z& I: V2 W3 w( i) }5 c4 L
p=robot.fkine(theta)%fkine正解函数，根据我们给定的关节角theta，求解出末端位姿p4 d4 e/ x) j2 Z, `8 e& u
q=robot.ikine(p)%ikine逆解函数，根据我们给定的末端位姿p，求解出关节角q" {( w. [' W6 c
+ Q" i& c" `4 M1 ]2 W
% _. o7 w0 J, A* U
%建立机器人模型
6 r9 a) S2 @: V" ~" `3 Q%       theta    d        a        alpha     offset
5 J* j# M! U( e. kL1=Link([0       89.2     0        pi/2      0     ]); %定义连杆的D-H参数
) w- P5 A, L/ x9 M0 IL2=Link([pi/2    0        -425     0         0     ]);9 K, D$ M" e# X/ s( A" g  K  |
L3=Link([0       0        -392     0         0     ]);0 E% `* t2 N! d  _1 Z( c
L4=Link([0       109.3    0        pi/2      0     ]);
3 F! T3 g2 y; [' ~0 u* FL5=Link([pi      94.75    0        pi/2      0     ]);: B5 k9 R) _6 D" ^8 \: p% I$ e/ w
L6=Link([0       82.5     0        0         0     ]);9 l  ~# _8 \6 l0 j5 h& i
robot=SerialLink([L1 L2 L3 L4 L5 L6],'name','manman'); %连接连杆，机器人取名manman
& ]" ~, I" s$ f/ O+ d' _* \T1=transl(-33.8962,70.5806,-53.6681);%根据给定起始点，得到起始点位姿
8 r  T: H# {1 J& g/ y6 {* RT2=transl(-31.3206,68.8281,-55.4449);%根据给定终止点，得到终止点位姿% u& G- j2 O/ Q0 C# ~
q1=robot.ikine(T1);%根据起始点位姿，得到起始点关节角
) C1 E  p0 R; i0 Yq2=robot.ikine(T2);%根据终止点位姿，得到终止点关节角, Q+ a! M1 v& t' a0 Z0 O
[q ,qd, qdd]=jtraj(q1,q2,50); %五次多项式轨迹，得到关节角度，角速度，角加速度，50为采样点个数. y" R7 Q! q- h% U9 ^
grid on
! W: P6 B6 [) b6 A5 M5 NT=robot.fkine(q);%根据插值，得到末端执行器位姿
1 B1 r6 J  m1 _4 f: lnT=T.T; plot3(squeeze(nT(1,4,:)),squeeze(nT(2,4,:)),squeeze(nT(3,4,:)));%输出末端轨迹: z! U% x% M& d6 s/ h' r
hold on* i0 v7 Z' F5 ~; |2 K  T* Q- p
robot.plot(q);%动画演示6 P/ H; j" r* \. d- }% X

; F: [& _9 p1 j- ~9 n& F. ~# t$ }$ l2 ~1 [- a7 G1 \, a
机器人可以显示出来，但自动轨迹规划时出错。. X3 F* T$ y0 x
$ J8 q/ y! j# ~1 D
出现如下错误：“7 z# _- w5 z( b( @6 v
警告: ikine: rejected-step limit 100 exceeded (pose 1), final err 0.3757554 y* L0 H5 t' C3 d5 n/ W
> In SerialLink/ikine (line 244)
' q, g, X& H2 |' M4 D3 x3 P  In t (line 27)
4 K% C/ k, v' T! L警告: failed to converge: try a different initial value of joint coordinates
$ W' D1 ~, m, }) O& a> In SerialLink/ikine (line 273)) }$ m5 j; Z+ P6 p* Q0 y$ D
  In t (line 27)
+ }# n* t. c4 j警告: ikine: rejected-step limit 100 exceeded (pose 1), final err 0.40822: Z. @3 Z: ]' G- A5 v" _' I+ o4 }
> In SerialLink/ikine (line 244)0 w# X' n* y& t1 E, o/ N
  In t (line 28)
  n( z' i# u+ Q, V8 k7 Z警告: failed to converge: try a different initial value of joint coordinates
3 {( K( E& t& d$ W5 P> In SerialLink/ikine (line 273)& Z  q: W4 t, X8 `4 {- Z- d
  In t (line 28)
5 f& Z! \$ V: C& C( e) b: i7 `' H错误使用 SerialLink/fkine (line 84)
' H( K; c) e4 s) e- E1 Kq must have 6 columns
. O  r9 R9 w/ t1 \: B: ?# r3 I# U1 M- X& ]7 f6 z( ]/ B4 \& Z
出错 t (line 31)1 \9 j& M9 N8 t5 c
T=robot.fkine(q);%根据插值，得到末端执行器位姿”- i9 E+ p8 }2 F; s) ^2 ]( i

7 r6 ?& d9 Y0 Q! ?调试发现q1和q2都是空矩阵，导致q也是空矩阵。: k* J5 t6 z/ j. t8 S" Y
为何得不到那2点的关节角？
3 D7 A, c, z6 X; k7 ?3 l! w/ k3 N% k) Z

& D  L: s5 A5 Y1 b; x# f0 H# C5 ^/ t  \0 |/ T3 y
作者: nkkopd    时间: 2020-10-14 13:58
帮顶！！！！！！！
作者: 大小的小    时间: 2020-10-14 14:28
路径超过机器人工作空间了
作者: kekek    时间: 2020-10-14 14:40
请问一下，我是先robot.teach然后手动操作机械臂，再选取的两个坐标（杆件没有干涉，感觉完全可以达到的坐标），再ikine也显示超出工作空间，建模的工作空间到底多大啊
作者: llbnmo    时间: 2020-10-14 15:23
有可能是超出了工作空间，也有可能是迭代次数不够。
, }3 a8 [- a! K  i# R* `0 B/ T如果是迭代次数不够的话，可以试试增加迭代次数(默认是100)或者改变迭代时候的初始关节向量4 _! e! j5 }. i6 D/ F* _
robot.ikine(T,'rlimit',200,'q0',[pi/2,0,0,0,0,0]);%rlimit改变迭代次数，q0改变迭代初始关节向量



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