原理:利用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).qlimans =-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:numq1 = 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);
endfigure;
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);%正向運動學 求得TFive_dof.plot(q);%可顯示每次機械臂的實時位置%[x,y,z]=transl(T);%plot3(x,y,z,'B','markersize',1);P(i,:)=transl(T); %transl函數得出三維坐標點endplot3(P(:,1),P(:,2),P(:,3),'b.','markersize',1);%三維空間繪制%axis([-1.5 1.5 -1.5 -0.5 1.5]);hold on %保留機械臂grid ondaspect([1 1 1]);view([45 45]);Five_dof.plot([0 0 0 0 0]);