Matlab机器人工具箱使用4 蒙特卡洛法绘制工作区间

发布于:2025-09-10 ⋅ 阅读:(13) ⋅ 点赞:(0)

原理:利用rand随机数,给各个关节设置随机关节变量,通过正运动学得到末端位姿变换矩阵,然后利用变换矩阵2三维坐标标记出末端坐标,迭代多次就可以构成点云。

教程视频:【MATLAB机器人工具箱10.4 机械臂仿真教学(未完结)】 https://www.bilibili.com/video/BV1q44y1x7WC/?p=5&share_source=copy_web&vd_source=2c56c6a2645587b49d62e5b12b253dca

【【Matlab机器人工具箱】+【运动学】+5、蒙特卡洛法的工作域分析】 https://www.bilibili.com/video/BV15V411v72f/?share_source=copy_web&vd_source=2c56c6a2645587b49d62e5b12b253dca

1 设置关节限制

首先要设置关节限制,不是每个关节都是正负180大回旋的。我们修改L(x).qlim来设定关节角度范围,也可以直接在L(x)定义时增加这个参数。

clear;
clc;

L(1) = Link('revolute', 'd', 0.216, 'a', 0, 'alpha', pi/2);
L(2) = Link('revolute', 'd', 0, 'a', 0.5, 'alpha', 0, 'offset', pi/2);
L(3) = Link('revolute', 'd', 0, 'a', 0, 'alpha', 0, 'offset', sqrt(0.145^2+0.42746^2), 'alpha', 0, 'offset', atan(427.46/145));
L(4) = Link('revolute', 'd', 0, 'a', 0, 'alpha', pi/2, 'offset', atan(427.46/145));
L(5) = Link('revolute', 'd', 0, 'a', 0.258, 'alpha', 0);

Five_dof = SerialLink(L,'name','5-dof');
Five_dof.base = transl(0,0,0.28);

L(1).qlim = [-150, 150]/180*pi;
L(2).qlim = [-100, 90]/180*pi;
L(3).qlim = [-90, 90]/180*pi;
L(4).qlim = [-100, 100]/180*pi;
L(5).qlim = [-180, 180]/180*pi;

% subplot(1,2,1)
% plot(Five_dof,[0 0 0 0 0],'tilesize',0.1,'workspace',[-1 1 -1 1 -0.2 2])

Five_dof.teach

可以用下式获取每个关节的限制范围:

>> L(1).qlim

ans =

   -2.6180    2.6180

2 生成随机关节变量

第二步是生成随机数的处理,如下图,在[m,n]生成随机数的方式就是m+rand*(m-n),对应关节变量就是q=q_min+rand*(q_max-q_min)

num = 30000;
P = zeros(num, 3);

for i=1:num
    q1 = L(1).qlim(1) + rand * (L(1).qlim(2) - L(1).qlim(1));
    q2 = L(2).qlim(1) + rand * (L(2).qlim(2) - L(2).qlim(1));
    q3 = L(3).qlim(1) + rand * (L(3).qlim(2) - L(3).qlim(1));
    q4 = L(4).qlim(1) + rand * (L(4).qlim(2) - L(4).qlim(1));
    q5 = L(5).qlim(1) + rand * (L(5).qlim(2) - L(5).qlim(1));

    q = [q1 q2 q3 q4 q5];

    T = Five_dof.fkine(q);

    P(i, :) = transl(T);
end

figure;
plot3(P(:,1), P(:,2), P(:,3), 'b.', 'markersize', 1);
xlabel('X-axis');
ylabel('Y-axis');
zlabel('Z-axis');
title('Workspace of 5-DOF Robot');
grid on;
axis equal;

设置30000次迭代(30000个散点构成工作空间轮廓)
预设存储点数组,大小为num,存储三维坐标,因此第二参为3.
每次迭代都会随机生成5个关节的关节变量,然后计算T变换矩阵
然后利用transl将变换矩阵变为三维空间的点,存入预定义数组P中(P(i,:) 表示矩阵 P 的第 i 行的所有列。换句话说,P(i,:) 是一个 1x3 的行向量,用于存储第 i 个样本的末端执行器位置。)
最后plot3将P中每一个点的坐标绘制出来,并保持grid on;意思是叠加。(同理,P(:,1) ,P(:,2),P(:,3)表示所有行的第一列、第二列、第三列),把三位坐标分别取出来绘图。

完整代码

% 工作空间范围可视化

L(1)=Link('revolute','d',0.216,'a',0,'alpha',pi/2);

L(2)=Link('revolute','d',0,'a',0.5,'alpha',0,'offset',pi/2);

L(3)=Link('revolute','d',0,'a',sqrt(0.145^2+0.42746^2),'alpha',0,'offset',-atan(427.46/145));

L(4)=Link('revolute','d',0,'a',0,'alpha',pi/2,'offset',atan(427.46/145));

L(5)=Link('revolute','d',0.258,'a',0,'alpha',0);

Five_dof=SerialLink(L,'name','五轴机器人');

Five_dof.base=transl(0,0,0.28);

%Five_dof.teach;

L(1).qlim=[-150,150]/180*pi; %qlim弧度限制   工作空间范围可视化

L(2).qlim=[-100,90]/180*pi;

L(3).qlim=[-90,90]/180*pi;

L(4).qlim=[-100,100]/180*pi;

L(5).qlim=[-180,180]/180*pi;

num=300; %随机次数300次

P=zeros(num,3) %零矩阵(num*3行列)

for i=1:num

    %最小关节角度+rand*(最大关节角度减去最小关节角度)

    q1=L(1).qlim(1)+rand*(L(1).qlim(2)-L(1).qlim(1)) 

    q2=L(2).qlim(1)+rand*(L(2).qlim(2)-L(2).qlim(1))

    q3=L(3).qlim(1)+rand*(L(3).qlim(2)-L(3).qlim(1))

    q4=L(4).qlim(1)+rand*(L(4).qlim(2)-L(4).qlim(1))

    q5=L(5).qlim(1)+rand*(L(5).qlim(2)-L(5).qlim(1))

    q=[q1 q2 q3 q4 q5];

    T=Five_dof.fkine(q);%正向运动学 求得T

    Five_dof.plot(q);%可显示每次机械臂的实时位置

    %[x,y,z]=transl(T);

    %plot3(x,y,z,'B','markersize',1);

    P(i,:)=transl(T); %transl函数得出三维坐标点

end

plot3(P(:,1),P(:,2),P(:,3),'b.','markersize',1);%三维空间绘制

%axis([-1.5 1.5 -1.5 -0.5 1.5]);

hold on %保留机械臂

grid on

daspect([1 1 1]);

view([45 45]);

Five_dof.plot([0 0 0 0 0]);

300次效果展示


网站公告

今日签到

点亮在社区的每一天
去签到