机器人问题

uaidenp · 发表于 2020-10-14 13:28

EDA365欢迎您登录！

您需要登录才可以下载或查看，没有帐号？注册

x

下面的代码在2015b可以运行；
clear;
clc;
startup_rvc;
n = 3;
switch n
case 1
%建立机器人模型
%    theta d       a       alpha    offset
L1=Link([0    0.4    0.025 pi/2    0    ]); %定义连杆的D-H参数
L2=Link([pi/2 0       0.56    0       0    ]);
L3=Link([0    0       0.035 pi/2    0    ]);
L4=Link([0    0.515 0       pi/2    0    ]);
L5=Link([pi    0       0       pi/2    0    ]);
L6=Link([0    0.08    0       0       0    ]);
robot=SerialLink([L1 L2 L3 L4 L5 L6],'name','manman'); %连接连杆，机器人取名manman
robot.plot([0,pi/2,0,0,pi,0]);%输出机器人模型，后面的六个角为输出时的theta姿态
robot.display();
teach(robot);

case 2
%建立机器人模型
%    theta d       a       alpha    offset
L1=Link([0    0.4    0.025 pi/2    0    ]); %定义连杆的D-H参数
L2=Link([pi/2 0       0.56    0       0    ]);
L3=Link([0    0       0.035 pi/2    0    ]);
L4=Link([0    0.515 0       pi/2    0    ]);
L5=Link([pi    0       0       pi/2    0    ]);
L6=Link([0    0.08    0       0       0    ]);
robot=SerialLink([L1 L2 L3 L4 L5 L6],'name','manman'); %连接连杆，机器人取名manman
theta=[0,0,0,0,0,0];%指定的关节角
p=robot.fkine(theta)%fkine正解函数，根据我们给定的关节角theta，求解出末端位姿p
q=robot.ikine(p)%ikine逆解函数，根据我们给定的末端位姿p，求解出关节角q

case 3
%建立机器人模型
%    theta d       a       alpha    offset
L1=Link([0    0.4    0.025 pi/2    0    ]); %定义连杆的D-H参数
L2=Link([pi/2 0       0.56    0       0    ]);
L3=Link([0    0       0.035 pi/2    0    ]);
L4=Link([0    0.515 0       pi/2    0    ]);
L5=Link([pi    0       0       pi/2    0    ]);
L6=Link([0    0.08    0       0       0    ]);
robot=SerialLink([L1 L2 L3 L4 L5 L6],'name','manman'); %连接连杆，机器人取名manman
T1=transl(0.5,0,0);%根据给定起始点，得到起始点位姿
T2=transl(0,0.5,0);%根据给定终止点，得到终止点位姿

q1=robot.ikine(T1);%根据起始点位姿，得到起始点关节角
q2=robot.ikine(T2);%根据终止点位姿，得到终止点关节角
[q ,qd, qdd]=jtraj(q1,q2,50); %五次多项式轨迹，得到关节角度，角速度，角加速度，50为采样点个数
grid on
T=robot.fkine(q);%根据插值，得到末端执行器位姿
nT=T.T; plot3(squeeze(nT(1,4,

),squeeze(nT(2,4,

),squeeze(nT(3,4,

));%输出末端轨迹
hold on
robot.plot(q);%动画演示
end

然后修改了下连杆的D-H参数，以及起始和终止点的参数。
%按UR5的DH参数修改后，第一个点打孔的轨迹
%建立机器人模型
%    theta d       a       alpha    offset
L1=Link([0    89.2    0       pi/2    0    ]); %定义连杆的D-H参数
L2=Link([pi/2 0       -425    0       0    ]);
L3=Link([0    0       -392    0       0    ]);
L4=Link([0    109.3 0       pi/2    0    ]);
L5=Link([pi    94.75 0       pi/2    0    ]);
L6=Link([0    82.5    0       0       0    ]);
robot=SerialLink([L1 L2 L3 L4 L5 L6],'name','manman'); %连接连杆，机器人取名manman
theta=[0,0,0,0,0,0];%指定的关节角
p=robot.fkine(theta)%fkine正解函数，根据我们给定的关节角theta，求解出末端位姿p
q=robot.ikine(p)%ikine逆解函数，根据我们给定的末端位姿p，求解出关节角q

%建立机器人模型
%    theta d       a       alpha    offset
L1=Link([0    89.2    0       pi/2    0    ]); %定义连杆的D-H参数
L2=Link([pi/2 0       -425    0       0    ]);
L3=Link([0    0       -392    0       0    ]);
L4=Link([0    109.3 0       pi/2    0    ]);
L5=Link([pi    94.75 0       pi/2    0    ]);
L6=Link([0    82.5    0       0       0    ]);
robot=SerialLink([L1 L2 L3 L4 L5 L6],'name','manman'); %连接连杆，机器人取名manman
T1=transl(-33.8962,70.5806,-53.6681);%根据给定起始点，得到起始点位姿
T2=transl(-31.3206,68.8281,-55.4449);%根据给定终止点，得到终止点位姿
q1=robot.ikine(T1);%根据起始点位姿，得到起始点关节角
q2=robot.ikine(T2);%根据终止点位姿，得到终止点关节角
[q ,qd, qdd]=jtraj(q1,q2,50); %五次多项式轨迹，得到关节角度，角速度，角加速度，50为采样点个数
grid on
T=robot.fkine(q);%根据插值，得到末端执行器位姿
nT=T.T; plot3(squeeze(nT(1,4,:)),squeeze(nT(2,4,:)),squeeze(nT(3,4,:)));%输出末端轨迹
hold on
robot.plot(q);%动画演示

机器人可以显示出来，但自动轨迹规划时出错。

出现如下错误：“
警告: ikine: rejected-step limit 100 exceeded (pose 1), final err 0.375755
> In SerialLink/ikine (line 244)
  In t (line 27)
警告: failed to converge: try a different initial value of joint coordinates
> In SerialLink/ikine (line 273)
  In t (line 27)
警告: ikine: rejected-step limit 100 exceeded (pose 1), final err 0.40822
> In SerialLink/ikine (line 244)
  In t (line 28)
警告: failed to converge: try a different initial value of joint coordinates
> In SerialLink/ikine (line 273)
  In t (line 28)
错误使用 SerialLink/fkine (line 84)
q must have 6 columns

出错 t (line 31)
T=robot.fkine(q);%根据插值，得到末端执行器位姿”

调试发现q1和q2都是空矩阵，导致q也是空矩阵。
为何得不到那2点的关节角？