【Matlab】非对称3-SPR并联机器人工作空间求解_matlab求并联机构工作空间

非对称3-SPR并联机器人简介

【Matlab】非对称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');




1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217

实际效果

代码实际运行的结果↓↓↓↓↓↓

稍微调整一下colorbar，再减小步进长度，就可以放在论文里了~

除此之外呢，为了更直观一些，也可以只提取轮廓，按高度画图
我这边用CloudCompare提取的点云边界，然后在Matlab里面画图
教程有人看的话再写吧~