matlab编写6-SPS并联机器人工作空间的程序

%% Section 0
clear all;
clc;
close all;

xa=[92.58 132.58 40 -40 -132.58 -92.58];
ya=[99.64 30.36 -130 -130 30.36 99.64];
za=23.1*ones（16）;
xb=[30 78.22 48.22 -48.22 -78.22 -30];
yb=[73 -10.52 -62.48 -62.48 -10.52 73];
zb=-37.1*ones（16）;
roumin=454.5*ones（16）;
roumax=504.5*ones（16）;

u=xa-xb;%圆心位置x
v=ya-yb;%圆心位置y
w=za-zb;%球心位置z

zmin=min（w）;%工作空间最小高度
zmax=max（w）+max（roumax）;%工作空间最大高度

jd=1e-9;%设置判断重合的精度
ratio=10;%离散圆弧的点数
ratio_z=16;%离散过z轴的截面
frame=1;

%% Section 1
%等z值绘图
%———————————————————————————————————————————————————————————————————————
ARCset_local=[];%初始化弧表
s_local=0;%初始化弧表数量
AREA=[];

figure（1）;

for z=zmin:2:zmax
    %display（[‘z = ‘num2str（z）‘ initializing...‘]）;
    CIRset=[];%初始化圆集合
    flgset=[];%初始化内外边界标识集合1为外-1为内
    Flagset=[];%初始化标识，用于弧表
    %display（‘Generating circle sets...‘）;
    cirsets;%生成圆表，得到同一z坐标下的圆表CIRset
    ARCset=[];%初始化弧集合
    %display（‘Generating arc sets...‘）;
    arcsets;%生成交集边界弧得到同一z坐标下的弧表ARCset
    %将同一z坐标下交集边界弧集ARCset放入总弧表ARCset_local中
    if（~isempty（ARCset））
        s_ztemp=size（ARCset2）;
        ARCset_local（:s_local+1:s_local+s_ztemp）=[ARCset;z*ones（1s_ztemp）];
        AREA=[AREA;calarea（ARCsetFlagset）z];
        s_local=s_local+s_ztemp;
        %-----------------------
        %display（‘Generating point sets...‘）;
        %PTset=[];
        for i=1:s_ztemp
            t_temp=linspace（ARCset（4i）ARCset（5i）ratio）;
            r_temp=ARCset（3i）;
            x_temp=ARCset（1i）+r_temp*cos（t_temp）;
            y_temp=ARCset（2i）+r_temp*sin（t_temp）;
            %PTset（:ratio*（i-1）+1:ratio*i）=[x_temp;y_temp;z*ones（1ratio）];
            plot3（x_tempy_tempz*ones（1ratio））;hold on;axis equal;grid on;%axis vis3d;            
        end        
        %text（00num2str（AREA（1）））;
        pause（0.1）;
        display（[‘Local generation done. Area for z=‘num2str（z）‘ is ‘num2str（calarea（ARCsetFlagset））]）;
        %-----------------------
    else
        display（[‘z