可到达空间的判定很重要关系到机器人能否到此处完成任务,是否有解。其方法是产生随机的N个点,构成。也可使用几何法自己判断。
1、MATLAB中蒙特卡洛法得出结果图:
matlab代码
clear;
clc;
%建立机器人模型
q1_lim = [-180,180];q1_lim = deg2rad(q1_lim);
q2_lim = [-90,90];q2_lim = deg2rad(q2_lim);
q3_lim = [-90,90];q3_lim = deg2rad(q3_lim);%限制每个关节的运动角度,转弧度制
% theta d a alpha offset
L1=Link([0 20 0 pi/2 0 ]);L1.qlim = q1_lim;%定义连杆的D-H参数
L2=Link([0 0 50 0 0 ]);L2.qlim = q2_lim;
L3=Link([0 0 40 0 0 ]);L3.qlim = q3_lim;
%L4=Link([0 0 0.2 0 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','S725'); %连接连杆,机器人取名manman
robot=SerialLink([L1 L2 L3],'name','S725');
N=3000; %随机次数
theta1=q1_lim(1)+diff(q1_lim)*rand(N,1); %关节1限制
theta2=q2_lim(1)+diff(q2_lim)*rand(N,1); %关节2限制
theta3=q3_lim(1)+diff(q3_lim)*rand(N,1); %关节3限制
%theta4=q4_lim(1)+diff(q4_lim)*rand(N,1); %关节4限制
pos = {1,N};
% 正运动学求解
for i=1:N
pos{i}=robot.fkine([theta1(i) theta2(i) theta3(i)]);
end
% 绘图
figure
robot.plot([0 0 0],'jointdiam',1)
axis equal;
hold on;
for i=1:N
plot3(pos{i}.t(1),pos{i}.t(2),pos{i}.t(3),'r.');
hold on;
end
grid off
view(20,30)
2、取值法表示机器人可达空间
matlab代码如下所示:
%%取值法,机器人可达空间可视化
close all;
clear;
clc;
L1 = Link('d', 5, 'a', 5, 'alpha', -pi/2,'offset',0); %Link 类函数;offset建立初始的偏转角
L2 = Link('d', 0, 'a', 20, 'alpha', 0, 'offset', 0);
L3 = Link('d', 0, 'a', 5, 'alpha', -pi/2,'offset',0);
L4 = Link('d', 20, 'a', 0, 'alpha', pi/2,'offset',0);
L5 = Link('d', 0, 'a', 0, 'alpha', -pi/2,'offset',0);
L6 = Link('d', 10, 'a', 0, 'alpha', 0, 'offset',0);
robot=SerialLink([L1,L2,L3,L4,L5,L6],'name','S725'); %SerialLink 类函数
ks = pi/180;
L1.qlim = [-pi,pi];%利用qlim设置每个关节的旋转角度范围
L2.qlim = [-100*ks,pi/3];
L3.qlim = [-100*ks,pi/3];
L4.qlim = [-pi,pi];
L5.qlim = [-100*ks,60*ks];
L6.qlim = [-pi,pi];
%平均各关节的步长5^6,设定5步即可,太大需要时间太长
setps_1 = (pi+pi)/5;
setps_2 = (pi/3+100*ks)/5;
setps_3 = (pi/3+100*ks)/5;
setps_4 = (pi+pi)/5;
setps_5 = (pi/3+100*ks)/5;
setps_6 = (pi+pi)/5;
tic;%启动秒表计时器
i = 1;
for q1=-pi:setps_1:pi
for q2=-100*ks:setps_2:pi/3
for q3=-100*ks:setps_3:pi/3
for q4=-pi:setps_4:pi
for q5=-100*ks:setps_5:pi/3
for q6=-pi:setps_6:pi
T_1 = robot.fkine([q1,q2,q3,q4,q5,q6]);
T_x(1,i) = T_1.t(1,1);
T_y(1,i) = T_1.t(2,1);
T_z(1,i) = T_1.t(3,1);
i=i+1;
end
end
end
end
end
end
disp(['用时为:',num2str(toc)]);
clock;%关闭计时器
%绘制工作空间
figure('name','六轴机械臂工作空间')
hold on
plotopt = {'noraise', 'nowrist', 'nojaxes', 'delay',0};
robot.plot([0 0 0 0 0 0], plotopt{:});
plot3(T_x,T_y,T_z,'r.','MarkerSize',3);
本文借鉴:作者https://blog.csdn.net/gyxx1998/article/details/105323608?spm=1001.2014.3001.5501