工业机器人（10）-Matlab Robot Toolbox机械臂工作空间
Posted Techblog of HaoWANG
tags:
篇首语：本文由小常识网(cha138.com)小编为大家整理，主要介绍了工业机器人（10）-Matlab Robot Toolbox机械臂工作空间相关的知识，希望对你有一定的参考价值。
目录
Matlab Robot Toolbox使用教程请参考本系列文章：工业机器人（4）-- Matlab Robot Toolbox运动学正、逆解_Techblog of HaoWANG-CSDN博客
机器人的工作空间是机器人在运转过程中,手部参考点在空间所能达到的点的集合。工作空间是一种重要的运动学指标，常用于衡量机器人活动范围，机器人无法到达处于工作空间以外的目标。机器人的工作空间的种类有三种，分别是全工作空间，可达工作空间和灵巧工作空间，本文中计算的是机器人给定所有位姿时，末端可到达目标点的集合，即全工作空间。
01 数值法
这里采用数值法进行机器人全工作空间的绘制，具体流程如下图所示。首先是设置各关节角度限制，然后以某一步距值对各关节分别进行累加并计算正解获得末端点位置，当各关节都到达最大限制角度后停止计算，最后对获得的所有末端点进行处理绘制出机器人的边界曲线，根据这些边界曲线可以绘制出代表机器人的工作空间的边界曲面。
双臂机器人工作空间求取
双臂机器人的运动学模型建立方法，请参考文章：1. 一些双臂的仿真结果，仅供娱乐 - 知乎做了几个demo，测试一下 
https://zhuanlan.zhihu.com/p/264915274
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年代中期由于科学技术的发展和电子计算机的发明，而提出的一种以概率统计理论为指导的数值计算方法。是指使用随机数（或更常见的伪随机数）来解决很多计算问题的方法。
通常蒙特卡罗方法可以粗略地分成两类：一类是所求解的问题本身具有内在的随机性，借助计算机的运算能力可以直接模拟这种随机的过程。例如在核物理研究中，分析中子在反应堆中的传输过程。中子与原子核作用受到量子力学规律的制约，人们只能知道它们相互作用发生的概率，却无法准确获得中子与原子核作用时的位置以及裂变产生的新中子的行进速率和方向。科学家依据其概率进行随机抽样得到裂变位置、速度和方向，这样模拟大量中子的行为后，经过统计就能获得中子传输的范围，作为反应堆设计的依据。
另一种类型是所求解问题可以转化为某种随机分布的特征数，比如随机事件出现的概率，或者随机变量的期望值。通过随机抽样的方法，以随机事件出现的频率估计其概率，或者以抽样的数字特征估算随机变量的数字特征，并将其作为问题的解。这种方法多用于求解复杂的多维积分问题。
假设我们要计算一个不规则图形的面积，那么图形的不规则程度和分析性计算（比如，积分）的复杂程度是成正比的。蒙特卡罗方法基于这样的想法：假设你有一袋豆子，把豆子均匀地朝这个图形上撒，然后数这个图形之中有多少颗豆子，这个豆子的数目就是图形的面积。当你的豆子越小，撒的越多的时候，结果就越精确。借助计算机程序可以生成大量均匀分布坐标点，然后统计出图形内的点数，通过它们占总点数的比例和坐标点生成范围的面积就可以求出图形面积。
使用蒙特卡罗方法进行分子模拟计算是按照以下步骤进行的：
使用蒙特卡罗法计算工作空间相较于数值化，优势在于用时较短，以100000个点为例，用时在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)];
以上是关于工业机器人（10）-Matlab Robot Toolbox机械臂工作空间的主要内容，如果未能解决你的问题，请参考以下文章
matlab TR = FKINE(ROBOT, Q)，Q具体是啥意思，不是已经有个robot对象了吗，为啥还需要参数Q
ROS机器人编程：原理与应用（A Systematic Approach to Learning Robot Programming with ROS 中文译版）
ROS2机器人编程简述新书推荐-A Concise Introduction to Robot Programming with ROS2
ROS2机器人编程简述新书推荐-A Concise Introduction to Robot Programming with ROS2