非对称3-SPR并联机器人简介
工作空间求解原理
使用的是数值搜索算法，有的也叫做拟球坐标搜索法。
暂不做讲解，这里提供两篇知网论文以供参考：
[1]高峰,黄玉美,史文浩,等.3-RPS并联机构工作空间分析的球坐标搜索法[J].西安理工大学学报,2001(03):239-242.DOI:10.19322/j.cnki.issn.1006-4710.2001.03.004.
[2]石岩,郝巧红,许国贤,等.一种平面对称3-SPR并联机构平台尺寸和工作空间的关系分析[J].机械设计与制造,2012(09):37-39.DOI:10.19356/j.cnki.1001-3997.2012.09.014.
Matlab源码
代码说明： 源码写的比较简单，也没有优化，估计电脑配置差一些可能就要死机了。原因是搜索出来大概有几百万个点，用matlab的scatter会容易死机
解决办法1： 可以将搜索出来的点保存到文件里，然后用点云专用的处理软件查看（比如CloudCompare）
解决办法2： 调低代码里搜索的步进长度
%***********************************************************%
% ** Name: 非对称3-SPR并联机构拟球坐标搜索法工作空间绘制 ** %
% ** Author: zia ** %
% ** Date: 2022-10-09 ** %
%***********************************************************%
clc;
clear;
%% 基本尺寸参数设置
R_A = 78.603; % 动平台转动副外接圆半径
r1 = 600; % 球铰1距离原点的长度
r2 = 600; % 球铰2距离原点的长度
r3 = 600; % 球铰3距离原点的长度
%% 约束条件设置
L_min = 1100; % 移动副最小距离约束
L_max = 1800; % 移动副最大距离约束
R_Ang_min = -75; % 转动副最小转动角度约束
R_Ang_max = 75; % 转动副最大转动角度约束
P_Ang_Z_max = 45; % 球铰和Z轴的最大夹角
%% 搜索参数设置
Z_min = 600; % 动平台Z轴搜索起始高度
Z_max = 2500; % 动平台Z轴搜索中止高度
Z_Add = 20; % 动平台Z轴搜索步进角
Phi_min = -50; % Phi角起始搜索角度
Phi_max = 50; % Phi角终止搜索角度
Phi_Add = 0.2; % Phi角步进角
Theta_min = -50; % Theta角起始搜索角度
Theta_max = 50; % Theta角终止搜索角度
Theta_Add = 0.2; % Theta角步进角
% 中心移动副参数设置
% 当没有中心移动副时，根据实际情况设置P_Mid_min为末端参考点距离动平台中心的距离，将P_Mid_max和P_Mid_Add设置为0
P_Mid_min = 0; % 中心移动副的最小伸长量
P_Mid_max = 0; % 中心移动副的最大伸长量
P_Mid_Add = 0; % 中心移动副的伸长量步距
%% 全局参数：不需设置
SpacePointCount = 0; % 工作空间有效点计数
ForCount = 0; % 循环次数计数，用来显示进度条
ForMaxCount = floor((Z_max-Z_min)/Z_Add+1)*floor((Phi_max-Phi_min+1)/Phi_Add)*floor((Theta_max-Theta_min+1)/Theta_Add);% 总循环次数
% % % WorkspaceInitNum = (Phi_max-Phi_min/Phi_Add)*(Theta_max-Theta_min/Theta_Add)*20;%;*(P_Mid/P_Mid_Add); % 工作空间矩阵分配空间
Workspace = zeros(9000000,3); % 工作空间矩阵初始化
Workspace(:,3) = Z_max;
P_Ang = zeros(1,3);
time1 = clock;
for Z_o = Z_min:Z_Add:Z_max % 遍历的Z平面
for Phi = Phi_min:Phi_Add:Phi_max % 遍历的Phi值
localpoint = zeros(1,3);
for Theta = Theta_min:Theta_Add:Theta_max % 遍历的Theta值
% 循环计数
ForCount = ForCount+1;
% 计算Phi
Delta = atand((3*(r2-r1)*(cosd(Theta)-cosd(Phi))+3*sqrt(3)*(r1+r2)*sind(Phi)*sind(Theta))/(4*sqrt(3)*r3*cosd(Theta)+3*(r2-r1)*sind(Phi)*sind(Theta)+sqrt(3)*(r1+r2)*(3*cosd(Phi)+cosd(Theta))));
% RPY旋转矩阵(定轴旋转)
ux = cosd(Theta)*cosd(Delta); uy = cosd(Theta)*sind(Delta); uz = -sind(Theta);
vx = sind(Phi)*sind(Theta)*cosd(Delta)-cosd(Phi)*sind(Delta);
vy = sind(Phi)*sind(Theta)*sind(Delta)+cosd(Phi)*cosd(Delta);
vz = sind(Phi)*cosd(Theta);
wx = cosd(Phi)*sind(Theta)*cosd(Delta)+sind(Phi)*sind(Delta);
wy = cosd(Phi)*sind(Theta)*sind(Delta)-sind(Phi)*cosd(Delta);
wz = cosd(Phi)*cosd(Theta);
R(1,1) = ux; R(1,2)=vx; R(1,3)=wx;
R(2,1) = uy; R(2,2)=vy; R(2,3)=wy;
R(3,1) = uz; R(3,2)=vz; R(3,3)=wz;
% 计算X_o和Y_o
O_A(1,1) = (6*(uz*vy-vz*uy)*Z_o-(3*(ux-vy)+sqrt(3)*(3*vx-uy))*uy*r2+2*(sqrt(3)*uy+3*vy)*uy*r3)/(6*(vx*uy-ux*vy));
O_A(2,1) = (6*(uz*vx-vz*ux)*Z_o-(3*(ux-vy)+sqrt(3)*(3*vx-uy))*ux*r2+2*(sqrt(3)*ux+3*vx)*uy*r3)/(6*(ux*vy-vx*uy));
O_A(3,1) = Z_o;
% 计算球铰点坐标
B1 = [r1*cosd(30) r1*sind(30) 0]';
B2 = [r2*cosd(150) r2*sind(150) 0]';
B3 = [r3*cosd(270) r3*sind(270) 0]';
% 计算旋转副点坐标
A1 = R * [R_A*cosd(30) R_A*sind(30) 0]'+ O_A;
A2 = R * [R_A*cosd(150) R_A*sind(150) 0]'+ O_A;
A3 = R * [R_A*cosd(270) R_A*sind(270) 0]'+ O_A;
% 计算各移动副方向向量
P1 = A1 - B1; P1_len = norm(P1);
P2 = A2 - B2; P2_len = norm(P2);
P3 = A3 - B3; P3_len = norm(P3);
P1_Ang = acosd(dot(P1,A1-(A2+A3)/2)/(norm(P1)*norm(A1-(A2+A3)/2)))-90;
P2_Ang = acosd(dot(P2,A2-(A1+A3)/2)/(norm(P1)*norm(A2-(A1+A3)/2)))-90;
P3_Ang = acosd(dot(P3,A3-(A1+A2)/2)/(norm(P1)*norm(A3-(A1+A2)/2)))-90;
P1_Ang_Z = acosd(dot(P1,[0 0 1]')/norm(P1));
P2_Ang_Z = acosd(dot(P2,[0 0 1]')/norm(P2));
P3_Ang_Z = acosd(dot(P3,[0 0 1]')/norm(P3));
if(O_A(1,1)>10000||O_A(1,1)<-10000||O_A(2,1)>10000||O_A(2,1)<-10000)
continue;
end
% 稳定性约束判断
% if sqrt(O_A(1,1)*O_A(1,1)+O_A(2,1)*O_A(2,1))>r1
% continue;
% end
% 移动副约束判断
P_len = [P1_len P2_len P3_len];
if(min(P_len)<L_min||max(P_len)>L_max)
continue;
end
% 转动副约束判断
P_Ang = [P1_Ang P2_Ang P3_Ang];
if(min(P_Ang)<R_Ang_min||max(P_Ang)>R_Ang_max)
continue;
end
% 球铰副约束判断
P_Ang_Z = [P1_Ang_Z P2_Ang_Z P3_Ang_Z];
if(min(P_Ang_Z)>P_Ang_Z_max)
continue;
end
% 满足所有约束，保存到工作空间
if(P_Mid_Add == 0)
Z_CenterPushRodAdd = P_Mid_min * [0 0 1]';
Z_CenterPushRodAdd = R * Z_CenterPushRodAdd;
S_A = O_A + Z_CenterPushRodAdd;
SpacePointCount = SpacePointCount+1;
Workspace(SpacePointCount, 1) = S_A(1,1);
Workspace(SpacePointCount, 2) = S_A(2,1);
Workspace(SpacePointCount, 3) = S_A(3,1);
% 满足所有约束，但还需要步进中心移动副
else
for j = P_Mid_min:P_Mid_Add:P_Mid_max
Z_CenterPushRodAdd = j * [0 0 1]';
Z_CenterPushRodAdd = R * Z_CenterPushRodAdd;
S_A = O_A+Z_CenterPushRodAdd;
SpacePointCount = SpacePointCount+1;
Workspace(SpacePointCount, 1) = S_A(1,1);
Workspace(SpacePointCount, 2) = S_A(2,1);
Workspace(SpacePointCount, 3) = S_A(3,1);
end
end
% Workspace(SpacePointCount, 4) = Theta;
% Workspace(SpacePointCount, 5) = Delta;
% Workspace(SpacePointCount, 6) = Phi;
end
time2 = clock;
fprintf('\r\r\r 计算进度：%0.3f%% \r 运行时间：%0.3fs\r 剩余时间：%0.3fs\r 工作空间点数：%d\r',ForCount/ForMaxCount*100,etime(time2,time1),etime(time2,time1)/(ForCount/ForMaxCount)-etime(time2,time1),SpacePointCount);
end
end
%% 保存工作空间点云到文件
% path = 'D:\Files-Matlab\Graduate Design\picture\';
% T = clock;
% dirName = [num2str(r1),'+',num2str(r2),'+',num2str(r3),' ',num2str(T(4)),'_',num2str(T(5)),'_',num2str(T(6))];
% mkdir (path,dirName);
% if(P_Mid ==0)% 中心杆伸长量为0时计算工作空间的切面
% Z_flag = 0;
% Z_begin = 1;
% for i = 1:1:size(Workspace,1)
% if(Workspace(i,3) > (Z_flag-1) && Workspace(i,3) < (Z_flag+1))
% continue;
% end
% Z_flag = Workspace(i,3);
% if(i == 1)
% continue;
% end
% scatter(Workspace(Z_begin:i,1),Workspace(Z_begin:i,2),50,'.','k');
% axis equal;
% axis([-1500,1500,-1500,1500]);
% Z_begin = i+1;
% picPath = [path,dirName,'\'];
% saveas(gcf,[picPath,'pic',num2str(Workspace(i,3))],'png');
% end
% end
% fprintf('计算完成，开始保存工作空间点云\r');
% % % fid=fopen([path,dirName,'\','workspace.txt'],'wt');
% [m,n]=size(Workspace);
% for i=1:1:m
% for j=1:1:n
% if j==n
% fprintf(fid,'%g\n',Workspace(i,j));
% else
% fprintf(fid,'%g\t',Workspace(i,j));
% end
% end
% end
% fclose(fid);
%% 绘制工作空间点云
fprintf('保存完成，开始绘图\r');
scatter3(Workspace(:,1),Workspace(:,2),Workspace(:,3),4,Workspace(:,3),'fill','MarkerFaceAlpha',1);
axis equal;
set(gcf,'Units','centimeters','Position',[6 6 12 8]);
colormap (parula);
axis([-1500,1500,-1500,1500,1000,2000]);
% axis([-1500,1500,-1500,1500,500,2000]);
set(gca,'xtick',-1500:500:1500);
set(gca,'ytick',-1500:500:1500);
set(gca,'ztick',1000:500:2000);
% set(gca,'xtick',-1500:1000:1500);
% set(gca,'ytick',-1500:1000:1500);
% set(gca,'ztick',500:500:2000);
xlabel('x','FontSize',12);
ylabel('y','FontSize',12);
zlabel('z','FontSize',12);
view(-35,30);
colorbar;
% cb = colorbar;
% cb.Ticks = [900:100:1800];caxis([900 1800])
% cb.Layout.Tile = 'east';
% picPath = [path,dirName,'\'];
% saveas(gcf,[picPath,'pic0'],'png');
实际效果
代码实际运行的结果↓↓↓↓↓↓
稍微调整一下colorbar，再减小步进长度，就可以放在论文里了~
除此之外呢，为了更直观一些，也可以只提取轮廓，按高度画图
我这边用CloudCompare提取的点云边界，然后在Matlab里面画图
教程有人看的话再写吧~
1788
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
