工业机器人（10）-Matlab Robot Toolbox机械臂工作空间_matlab 蒙特卡罗法 机械臂工作空间

目录

01 数值法

双臂机器人工作空间求取

​​​

02 蒙特卡洛方法

---

Matlab Robot Toolbox使用教程请参考本系列文章：工业机器人（4）-- Matlab Robot Toolbox运动学正、逆解_Techblog of HaoWANG-CSDN博客

---

        机器人的工作空间是机器人在运转过程中,手部参考点在空间所能达到的点的集合。工作空间是一种重要的运动学指标，常用于衡量机器人活动范围，机器人无法到达处于工作空间以外的目标。机器人的工作空间的种类有三种，分别是全工作空间，可达工作空间和灵巧工作空间，本文中计算的是机器人给定所有位姿时，末端可到达目标点的集合，即全工作空间。

01 数值法

这里采用数值法进行机器人全工作空间的绘制，具体流程如下图所示。首先是设置各关节角度限制，然后以某一步距值对各关节分别进行累加并计算正解获得末端点位置，当各关节都到达最大限制角度后停止计算，最后对获得的所有末端点进行处理绘制出机器人的边界曲线，根据这些边界曲线可以绘制出代表机器人的工作空间的边界曲面。

双臂机器人工作空间求取

双臂机器人的运动学模型建立方法，请参考文章：1. 一些双臂的仿真结果，仅供娱乐 - 知乎做了几个demo，测试一下 https://zhuanlan.zhihu.com/p/264915274

2. Matlab双臂机器人建模仿真 - 知乎源码随后附上，稍安勿躁 实际上，如果就把双臂中的每个臂当做单个的机械臂进行规划，那么用matlab进行双臂建模就没有太大必要，只需要对每个单臂进行单独规划即可。但是，如果涉及到双臂之间的协同轨迹规划，如上…https://zhuanlan.zhihu.com/p/264911739

工业机器人（9）-- Matlab机器人工具箱之创建单臂/双臂机器人SDH/MDH方法_Techblog of HaoWANG-CSDN博客目录1. Matlab机器人工具箱2. 创建MDH单机械臂3. 创建MDH双臂机器人UR构型双臂如何进行轨迹仿真4. MDH-双臂机器人1. Matlab机器人工具箱官方网站Robotics Toolbox | Peter Corke下载，使用Matlab打开安装即可机械臂文档SerialLink2. 创建MDH单机械臂clear;clc;%建立机器人模型% theta d a a...https://blog.csdn.net/hhaowang/article/details/120215581?spm=1001.2014.3001.5502

%六轴机械臂工作空间计算
clc;
clear;
format short;%保留精度
 
%角度转换
du=pi/180;  %度
radian=180/pi; %弧度
 
%% 模型导入
%   theta |         d |         a |     alpha |     type|   offset |
L(1)=Link([0       -0.072        0.150        0      0     pi/2  ],'modified'); % 关节1这里的最后一个量偏置
L(2)=Link([0       0          0.022        pi/2      0    -pi/2  ],'modified');
L(3)=Link([0       0           0.285        0          0    -pi/2 ],'modified');
L(4)=Link([0       0.22        0.0035           -pi/2      0    0 ],'modified');
% L(5)=Link([0       0           0           -pi/2       0     ],'modified');
% L(6)=Link([0       0            0           pi/2      0     ],'modified');
%                  0.262
p560L=SerialLink(L,'name','LEFT');
p560L.tool=[0 -1 0 0;
            1 0 0 0;
            0 0 1 0 ;
            0 0 0 1;]; 
           
R(1)=Link([0       -0.072        -0.150        0      0     pi/2  ],'modified'); % 关节1这里的最后一个量偏置
R(2)=Link([0       0          0.022        pi/2      0    -pi/2  ],'modified');
R(3)=Link([0       0           0.285        0          0    -pi/2 ],'modified');
R(4)=Link([0       0.22        0.0035           -pi/2      0    0 ],'modified');
% R(5)=Link([0       0           0           -pi/2       0     ],'modified');
% R(6)=Link([0       0           0           pi/2      0     ],'modified');
%                  0.262
p560R=SerialLink(R,'name','RIGHT');
p560R.tool=[0 -1 0 0;
               1 0 0 0;
               0 0 1 0 ;
               0 0 0 1;]; 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%   platform
 
platform=SerialLink([0 0 0 0],'name','platform','modified');%虚拟腰部关节
platform.base=[1 0 0 0;
               0 1 0 0;
               0 0 1 0 ;
               0 0 0 1;]; %基座高度
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%   R
 
pR=SerialLink([platform,p560R],'name','R'); % 单独右臂模型，加装底座
pR.display();
pL=SerialLink([platform,p560L],'name','L'); % 单独左臂模型，加装底座
pL.display();
 
%% 求取工作空间
     %左臂关节角限位
    l_q1_s=-90; l_q1_end=40;
    l_q2_s=0;    l_q2_end=110;
    l_q3_s=0;  l_q3_end=112;
    l_q4_s=-90; l_q4_end=90;
 
    
    %右臂关节角限位
    r_q1_s=-40; r_q1_end=90;
    r_q2_s=0; r_q2_end=110;
    r_q3_s=0;  r_q3_end=112;
    r_q4_s=-90; r_q4_end=90;
 
    
    %设置step
    %step越大，点越稀疏，运行时间越快
    step=2;%计算步距
    step1= (l_q1_end -l_q1_s) / (0.5*step);
    step2= (l_q2_end -l_q2_s) / (2*step);
    step3= (l_q3_end -l_q3_s) / (2*step);
    
    step4= (r_q1_end -r_q1_s) / (0.5*step);
    step5= (r_q2_end -r_q2_s) / (2*step);
    step6= (r_q3_end -r_q3_s) / (2*step);
    
    %计算工作空间
    tic;%tic1
    i=1;
    j=1;
%     left arm 
    T_l = zeros(3,1);
    T_l_x = zeros(1,step1*step2*step3);
    T_l_y = zeros(1,step1*step2*step3);
    T_l_z = zeros(1,step1*step2*step3);  
%     right arm
    T_r = zeros(3,1);
    T_r_x = zeros(1,step4*step5*step6);
    T_r_y = zeros(1,step4*step5*step6);
    T_r_z = zeros(1,step4*step5*step6); 
    % left
    for  l_q1=l_q1_s:step:l_q1_end
        for  l_q2=l_q2_s:step:l_q2_end
              for  l_q3=l_q3_s:step:l_q3_end
%                   T_l=pL.fkine([0 l_q1*du l_q2*du l_q3*du 0]);%正向运动学仿真函数
                  T_l=pL.fkine([0 0 l_q2*du l_q3*du 0]);%正向运动学仿真函数
                  T_l_x(1,i) = T_l.t(1); 
                  T_l_y(1,i) = T_l.t(2); 
                  T_l_z(1,i) = T_l.t(3); 
                  i=i+1;
             end
        end 
    end
   
% right
    for  r_q1=r_q1_s:step:r_q1_end
        for  r_q2=r_q2_s:step:r_q2_end
              for  r_q3=r_q3_s:step:r_q3_end
                  T_r=pR.fkine([0 r_q1*du r_q2*du r_q3*du 0]);%正向运动学仿真函数
%                   T_r=pL.fkine([0 0 r_q2*du r_q3*du 0]);%正向运动学仿真函数
                  T_r_x(1,j) = T_r.t(1); 
                  T_r_y(1,j) = T_r.t(2); 
                  T_r_z(1,j) = T_r.t(3); 
                  j=j+1;
             end
        end 
    end
    
    disp(['循环运行时间：',num2str(toc)]); 
    t1=clock;
     
%% 绘制工作空间
 
figure('name','4轴双机械臂工作空间')
hold on
plotopt = {
    'noraise', 'nowrist', 'nojaxes', 'delay',0};
pL.plot([0 0 pi/4 pi/9 0 ], plotopt{:});
plot3(T_l_x,T_l_y,T_l_z,'r.','MarkerSize',3);
 
pR.plot([0 0 0 0 0 ], plotopt{:});
% plot3(T_r_x,T_r_y,T_r_z,'B*','MarkerSize',3);
 
disp(['绘制工作空间运行时间：',num2str(etime(clock,t1))]);  
 
%获取X,Y,Z空间坐标范围
Point_range=[min(T_l_x) max(T_l_x) min(T_l_y) max(T_l_y) min(T_l_z) max(T_l_z)]
hold off

---

02 蒙特卡洛方法

蒙特卡罗方法（英语：Monte Carlo method），也称统计模拟方法，是1940年代中期由于科学技术的发展和电子计算机的发明，而提出的一种以概率统计理论为指导的数值计算方法。是指使用随机数（或更常见的伪随机数）来解决很多计算问题的方法。

通常蒙特卡罗方法可以粗略地分成两类：一类是所求解的问题本身具有内在的随机性，借助计算机的运算能力可以直接模拟这种随机的过程。例如在核物理研究中，分析中子在反应堆中的传输过程。中子与原子核作用受到量子力学规律的制约，人们只能知道它们相互作用发生的概率，却无法准确获得中子与原子核作用时的位置以及裂变产生的新中子的行进速率和方向。科学家依据其概率进行随机抽样得到裂变位置、速度和方向，这样模拟大量中子的行为后，经过统计就能获得中子传输的范围，作为反应堆设计的依据。

另一种类型是所求解问题可以转化为某种随机分布的特征数，比如随机事件出现的概率，或者随机变量的期望值。通过随机抽样的方法，以随机事件出现的频率估计其概率，或者以抽样的数字特征估算随机变量的数字特征，并将其作为问题的解。这种方法多用于求解复杂的多维积分问题。

假设我们要计算一个不规则图形的面积，那么图形的不规则程度和分析性计算（比如，积分）的复杂程度是成正比的。蒙特卡罗方法基于这样的想法：假设你有一袋豆子，把豆子均匀地朝这个图形上撒，然后数这个图形之中有多少颗豆子，这个豆子的数目就是图形的面积。当你的豆子越小，撒的越多的时候，结果就越精确。借助计算机程序可以生成大量均匀分布坐标点，然后统计出图形内的点数，通过它们占总点数的比例和坐标点生成范围的面积就可以求出图形面积。

使用蒙特卡罗方法进行分子模拟计算是按照以下步骤进行的：

1. 使用随机数生成器产生一个随机的分子构型。
2. 对此分子构型的其中粒子坐标做无规则的改变，产生一个新的分子构型。
3. 计算新的分子构型的能量。
4. 比较新的分子构型与改变前的分子构型的能量变化，判断是否接受该构型。
   • 若新的分子构型能量低于原分子构型的能量，则接受新的构型，使用这个构型重复再做下一次迭代。
   • 若新的分子构型能量高于原分子构型的能量，则计算玻尔兹曼因子，并产生一个随机数。
     • 若这个随机数大于所计算出的玻尔兹曼因子，则放弃这个构型，重新计算。
     • 若这个随机数小于所计算出的玻尔兹曼因子，则接受这个构型，使用这个构型重复再做下一次迭代。
5. 如此进行迭代计算，直至最后搜索出低于所给能量条件的分子构型结束

        使用蒙特卡罗法计算工作空间相较于数值化，优势在于用时较短，以100000个点为例，用时在2到3分钟内（根据关节运动范围和关节数不同而不同）
劣势在于蒙特卡罗方法只能趋近于结果而不能完全得到真实的结果，但经过测试，只要数据的数量足够大，也能获得较为准确的结果。

---

蒙特卡罗法一般实现步骤

1. 用蒙特卡罗方法模拟某一过程时，需要产生各种概率分布的随机变量。
2. 用统计方法把模型的数字特征估计出来，从而得到实际问题的数值解。

蒙特卡罗法工作空间求解步骤

1. 产生各关节的随机变量,随机产生一组机械臂的关节空间向量；
2. 计算运动学正解，由关节空间向末端的工作空间（笛卡尔坐标系）映射；
3. 绘制工作空间分布图。

clc;
clear;
 
%% 准备
    %保留精度
    format short;
    
    %角度转换
    du=pi/180;  %度
    radian=180/pi; %弧度
    
 
    %% 模型导入
%   theta |         d |         a |     alpha |     type|   offset |
L(1)=Link([0       -0.072        0.150        0      0     pi/2  ],'modified'); % 关节1这里的最后一个量偏置
L(2)=Link([0       0          0.022        pi/2      0    -pi/2  ],'modified');
L(3)=Link([0       0           0.285        0          0    -pi/2 ],'modified');
L(4)=Link([0       0.22        0.0035           -pi/2      0    0 ],'modified');
% L(5)=Link([0       0           0           -pi/2       0     ],'modified');
% L(6)=Link([0       0            0           pi/2      0     ],'modified');
%                  0.262
p560L=SerialLink(L,'name','LEFT');
p560L.tool=[0 -1 0 0;
            1 0 0 0;
            0 0 1 0 ;
            0 0 0 1;]; 
           
R(1)=Link([0       -0.072        -0.150        0      0     pi/2  ],'modified'); % 关节1这里的最后一个量偏置
R(2)=Link([0       0          0.022        pi/2      0    -pi/2  ],'modified');
R(3)=Link([0       0           0.285        0          0    -pi/2 ],'modified');
R(4)=Link([0       0.22        0.0035           -pi/2      0    0 ],'modified');
% R(5)=Link([0       0           0           -pi/2       0     ],'modified');
% R(6)=Link([0       0           0           pi/2      0     ],'modified');
%                  0.262
p560R=SerialLink(R,'name','RIGHT');
p560R.tool=[0 -1 0 0;
               1 0 0 0;
               0 0 1 0 ;
               0 0 0 1;]; 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%   platform
 
platform=SerialLink([0 0 0 0],'name','platform','modified');%虚拟腰部关节
platform.base=[1 0 0 0;
               0 1 0 0;
               0 0 1 0 ;
               0 0 0 1;]; %基座高度
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%   R
 
pR=SerialLink([platform,p560R],'name','R'); % 单独右臂模型，加装底座
pR.display();
pL=SerialLink([platform,p560L],'name','L'); % 单独左臂模型，加装底座
pL.display();
 
%% 参数
    %关节角限位
    q1_s=-180; q1_end=180;
    q2_s=0;    q2_end=90;
    q3_s=-90;  q3_end=90;
    q4_s=-180; q4_end=180;
    q5_s=-90;  q5_end=90;
    q6_s=0;    q6_end=360;
    
     %左臂关节角限位
    l_q1_s=-90; l_q1_end=40;
    l_q2_s=-10;    l_q2_end=100;
    l_q3_s=0;  l_q3_end=112;
    l_q4_s=-90; l_q4_end=90;
 
    
    %右臂关节角限位
    r_q1_s=-40; r_q1_end=90;
    r_q2_s=-10; r_q2_end=90;
    r_q3_s=0;  r_q3_end=112;
    r_q4_s=-90; r_q4_end=90;
    
    %计算点数
    num=100000;
 
%% 求取工作空间
    %设置轴关节随机分布,轴6不对工作范围产生影响，设置为0
    q0_rand = rand(num,1)*0;
    q1_rand = l_q1_s + rand(num,1)*(l_q1_end - l_q1_s);%rand产生num行1列，在0~1之间的随机数
%     q1_rand = rand(num,1)*0;
    q2_rand = l_q2_s + rand(num,1)*(l_q2_end - l_q2_s);
    q3_rand = l_q2_s + rand(num,1)*(l_q3_end - l_q3_s);
    q4_rand = rand(num,1)*0;
    
    q = [q0_rand q1_rand q2_rand q3_rand q4_rand]*du;
    
    
    %right arm
    q0_rand_r = rand(num,1)*0;
    q1_rand_r = r_q1_s + rand(num,1)*(r_q1_end - r_q1_s);%rand产生num行1列，在0~1之间的随机数
%     q1_rand = rand(num,1)*0;
    q2_rand_r = r_q2_s + rand(num,1)*(r_q2_end - r_q2_s);
    q3_rand_r = r_q2_s + rand(num,1)*(r_q3_end - r_q3_s);
    q4_rand_r = rand(num,1)*0;
    
    q_ = [q0_rand_r q1_rand_r q2_rand_r q3_rand_r q4_rand_r]*du;
    
    
    %正运动学计算工作空间
    tic;
    T_cell = cell(num,1);
    T_cell_r = cell(num,1);
    for i=1:1:num
        
        [T_cell{i}]=pL.fkine(q(i,:));%正向运动学仿真函数
        [T_cell_r{i}]=pR.fkine(q_(i,:));%正向运动学仿真函数
        
    end
%     [T_cell{:,1}]=p560.fkine(q);%正向运动学仿真函数
    disp(['运行时间：',num2str(toc)]);
 
 %% 分析结果
    %绘制工作空间
    t1=clock;
    figure('name','机械臂工作空间')
    hold on
    plotopt = {
    'noraise', 'nowrist', 'nojaxes', 'delay',0};
    pL.plot([0 0 0 0 0], plotopt{:});
    pR.plot([0 0 0 0 0], plotopt{:});
     figure_x=zeros(num,1);
     figure_y=zeros(num,1);
     figure_z=zeros(num,1);
     for cout=1:1:num
         figure_x(cout,1)=T_cell{cout}.t(1);
         figure_y(cout,1)=T_cell{cout}.t(2);
         figure_z(cout,1)=T_cell{cout}.t(3);
     end
     plot3(figure_x,figure_y,figure_z,'r*','MarkerSize',3);
     
     figure_x_r=zeros(num,1);
     figure_y_r=zeros(num,1);
     figure_z_r=zeros(num,1);
     for cout=1:1:num
         figure_x_r(cout,1)=T_cell_r{cout}.t(1);
         figure_y_r(cout,1)=T_cell_r{cout}.t(2);
         figure_z_r(cout,1)=T_cell_r{cout}.t(3);
     end
     plot3(figure_x_r,figure_y_r,figure_z_r,'b.','MarkerSize',3);
     
     
     hold off
     disp(['绘制工作空间运行时间：',num2str(etime(clock,t1))]);  
     
     %获取X,Y,Z空间坐标范围
     Point_range=[min(figure_x) max(figure_x) min(figure_y) max(figure_y) min(figure_z) max(figure_z)];