stewart平台的工作空间求解

stewart平台的工作空间的求解，使用matlab编写
参考书目：file:///C:/Users/giuyyt/Desktop/并联机器人机构学理论及控制.pdf
页码：p172、173等

总函数

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Stewart平台工作空间求解
%尝试两种方法：扫描方法和蒙特卡洛法
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
clc;clear;
tic
%% stewart平台的参数给定
%global参数

%%动平台相对静平台初始位置坐标
global xp yp zp;

xp = 0;
yp = 0;
zp = 5.5;
% zp = 90;

%%R：动平台外接圆半径；r：静平台外接圆半径
global R r; 

R = 1;
r = 3;


%%动平台铰点安装角
global upAngle0 upAngle1 upAngle2 upAngle3 upAngle4 upAngle5;

upAngle0 = 15;
upAngle1 = 105;
upAngle2 = 135;
upAngle3 = 225;
upAngle4 = 255;
upAngle5 = 345;

%并联机器人的限制范围，求工作空间时用

%电推杆最大长度与最小长度（绝对量）
global poleMaxLen poleMinLen;
poleMaxLen = 7.5;
poleMinLen = 4.5;
%虎克铰（下）最大转角和最小转角(°)
global hookeMaxAngle hookeMinAngle;
hookeMaxAngle = 45;
hookeMinAngle = -45;
%球铰（上）最大转角和最小转角(°)
global sphereMaxAngle sphereMinAngle;
sphereMaxAngle = 45;
sphereMinAngle = -45;
%电推杆半径（mm）
global poleRad;
poleRad = 0;


%%静平台铰点安装角
global downAngle0 downAngle1 downAngle2 downAngle3 downAngle4 downAngle5;


downAngle0 = 30;
downAngle1 = 90;
downAngle2 = 150;
downAngle3 = 210;
downAngle4 = 270;
downAngle5 = 330;

%运动范围限制
global xMin xMax;
global yMin yMax;
global zMin zMax;
global rollMin rollMax;
global pitchMin pitchMax;
global yawMin yawMax;


xMin = -10;xMax = 10;
yMin = -10;yMax = 10;
zMin = -10; zMax = 10;
% xMin = -40;xMax = 40;
% yMin = -40;yMax = 40;
% zMin = -40; zMax =40;
rollMin = -13.5; rollMax = 13.5;
pitchMin = -14.2; pitchMax = 14.2;
yawMin = -38; yawMax = 38;


%% 蒙特卡洛法
% expeNum = 10000;%随机取样次数
% acceptPointArray = [];
% totalPointArray = [];
% i = 0;
% 
% while(i<expeNum)
%      %取样,rand在0-1取样
%     x = (xMax-xMin)*rand+xMin;
%     y = (yMax-yMin)*rand+yMin;
%     z = (zMax-zMin)*rand+zMin;
%     
%     %姿态需要固定
%     roll = 0;
%     pitch = 0;
%     yaw = 0;
%     
%     totalPointArray = [
%             totalPointArray;
%             x,y,z
%             ];
%     
%     %判断是否可行
%     isSatisfy = isSatisfyTotal(x,y,z,roll,pitch,yaw);
%     
%     if(isSatisfy==1)
%         acceptPointArray = [
%             acceptPointArray;
%             x,y,z
%             ];
%         i = i +1;
%     else
%     end
% end


for m = 1:3
    %% 扫描法
    %%%参数设定
    i = 0;
    j = 0;
    k = 0;
    zNum = 400;
    gammaNum = 200;
    deltaRho = 0.01;

    %姿态需要固定
    roll = (m-1)*5;
    pitch = (m-1)*5;
    yaw = (m-1)*5;

    %%%实现
    deltaGamma = 2*pi/gammaNum;
    deltaZ = (zMax-zMin)/zNum;
    edgePointArray = [];%记录边缘上的点的数组

    for i = 1:zNum
        rhoTemp = 0;
        gammaTemp = 0;

        zTemp = zMin+deltaZ*i;
        for j = 1:gammaNum
            isSatisfyLast = 0;%记录上一次是否符合要求
            gammaTemp = gammaTemp+deltaGamma;
            k = 0;
            if(rhoTemp<0)
                rhoTemp = 0;
            end
            deltaRho = abs(deltaRho);

            while(1)
                %将极坐标转换成直角坐标
                [xTemp,yTemp] = polarCoo2CarsCoo(rhoTemp,gammaTemp);
                %判断是否可行
                isSatisfy = isSatisfyTotal(xTemp,yTemp,zTemp,roll,pitch,yaw);

                %第一次
                if(k==0)
                    isSatisfyLast = isSatisfy; 
                    %第一次在范围内，不断增大极径直到到达极限
                    if(isSatisfy == 1)
                        deltaRho = deltaRho*1; 
                    %第一次不在范围内，不断缩小极径直到到达极限    
                    else
                        deltaRho = deltaRho*(-1); 
                    end
                    rhoTemp = rhoTemp+deltaRho;

                %非第一次
                else
                    %若上一次状态与本次相同，继续变化极径；
                    if(isSatisfy==isSatisfyLast&&rhoTemp>0)
                        rhoTemp = rhoTemp+deltaRho;
                        isSatisfyLast = isSatisfy; 
                    %若上一次状态与本次不同，记录本次点，进入下一个gamma角
                    else
                        if(k>1)
                            edgePointArray = [edgePointArray;xTemp,yTemp,zTemp];
                        end
                        break;
                    end
                end
                k = k+1



            end
        end
    end



    %% 画图
    plot3(edgePointArray(:,1),edgePointArray(:,2),edgePointArray(:,3));
    hold on;
    view(0,0);
    toc
end
%标出（0，0）点
scatter3(0,0,0);






% surf(X,Y,Z),view(90,0)
% title('侧视图')
% subplot(2,2,3)
% surf(X,Y,Z),view(0,0)
% title('正视图')
% subplot(2,2,4)
% surf(X,Y,Z),view(0,90)
% title('俯视图')

分函数：

将极坐标转换为直角坐标

157行的函数->polarCoo2CarsCoo:

function [xOut,yOut] = polarCoo2CarsCoo(rhoIn,gammaIn)
%polarCoo2CarsCoo 将极坐标转换为直角坐标

xOut = rhoIn*cos(gammaIn);
yOut = rhoIn*sin(gammaIn);

end

输入并联机器人的平动量和姿态，输出其是否能到达

159行的函数->isSatisfyTotal:

function [isSatisfy] = isSatisfyTotal(x,y,z,roll,pitch,yaw)
%isSatisfyTotal 输入并联机器人的平动量和姿态，输出其是否能到达

%电推杆最大长度与最小长度（绝对量）
global poleMaxLen poleMinLen;
%虎克铰（下）最大转角和最小转角
global hookeMaxAngle hookeMinAngle;
%球铰（上）最大转角和最小转角
global sphereMaxAngle sphereMinAngle;
%电推杆半径
global poleRad;


[lenArray,vectorArray,brArray,BrArray] = stewartInverseKinematicFunction(x,y,z,roll,pitch,yaw);

%电推杆长度
lenSatisfy = isLenSatisfy(lenArray,poleMinLen,poleMaxLen);
%虎克铰角度
hookeAngleSatisfy = isHookeAngleSatisfy(vectorArray,hookeMinAngle,hookeMaxAngle);
% hookeAngleSatisfy  = 1;
%球铰角度
sphereAngleSatisfy = isSphereAngleSatisfy(vectorArray,sphereMinAngle,sphereMaxAngle,roll,pitch,yaw);
% sphereAngleSatisfy = 1;
%干涉
interferenceSatisfy = isInterferenceSatisfy(lenArray,vectorArray,brArray,BrArray,poleRad);
% interferenceSatisfy = 1;

isSatisfy = lenSatisfy*hookeAngleSatisfy*sphereAngleSatisfy*interferenceSatisfy;






end

stewart平台运动学反解

function [lenArray,vectorArray,brArray,BrArray] = stewartInverseKinematicFunction(x,y,z,roll,pitch,yaw)
%stewartInverseKinematicFunction 求解stewart平台逆解
%输入6个自由度上的位移量
%输出6个杆的杆长和对应的杆向量（静平台指向动平台）

%-----------------------stewart平台本体参数-------------------------------------
global xp yp zp;
global R r;
global upAngle0 upAngle1 upAngle2 upAngle3 upAngle4 upAngle5;
global downAngle0 downAngle1 downAngle2 downAngle3 downAngle4 downAngle5;

%-------------------------动平台的位恣-------------------------------------

P = [ x+xp; y+yp; z+zp ];%动平台圆心点相对静平台的坐标

%--------------------------平台的基本尺寸----------------------------------
%% 方法1：输入角度
%----------动平台的6个铰点，在动平台坐标系中的位置矢量---------------------
bR1 = [ R*cosd( upAngle0 ); R*sind( upAngle0 );0 ];
bR2 = [ R*cosd( upAngle1 ); R*sind( upAngle1 ); 0 ];
bR3 = [ R*cosd( upAngle2 ); R*sind( upAngle2 ); 0 ];
bR4 = [ R*cosd( upAngle3 ); R*sind( upAngle3 ); 0 ];
bR5 = [ R*cosd( upAngle4 ); R*sind( upAngle4 ); 0 ];
bR6 = [ R*cosd( upAngle5 ); R*sind( upAngle5 ); 0 ];


% 
% ----------静平台的6个铰点，在静平台坐标系中的位置矢量---------------------
Br1 = [ r*cosd( downAngle0 ) ;r*sind( downAngle0 );0 ];
Br2 = [ r*cosd( downAngle1 ) ; r*sind( downAngle1 );0 ];
Br3 = [ r*cosd( downAngle2 ) ; r*sind( downAngle2 );0 ];
Br4 = [ r*cosd( downAngle3 ) ; r*sind( downAngle3 );0 ];
Br5 = [ r*cosd( downAngle4 ) ; r*sind( downAngle4 );0 ];
Br6 = [ r*cosd( downAngle5 ) ; r*sind( downAngle5 );0 ];


%% 方法2：直接输入坐标
% ----------动平台的6个铰点，在动平台坐标系中的位置矢量---------------------
% bR1 = [ 13.39; 15.81;0 ];
% bR2 = [ -13.39; 15.81; 0 ];
% bR3 = [-20.39; 3.69; 0 ];
% bR4 = [-7; -19.5; 0 ];
% bR5 = [7;-19.5; 0 ];
% bR6 = [20.39;3.69; 0 ];
% 
% 
% 
% % ----------静平台的6个铰点，在静平台坐标系中的位置矢量---------------------
% Br1 = [10.7;25.025;0 ];
% Br2 = [-10.7; 25.025;0 ];
% Br3 = [-27;-3.23;0 ];
% Br4 = [-16.3;-21.77;0 ];
% Br5 = [16.3;-21.77;0 ];
% Br6 = [27;-3.23;0 ];

%% 其它
%相对固定轴，RPY的XYZ
%相当于欧拉角中的ZYX，绕动轴转动
TransM = rotz(yaw) * roty(pitch) * rotx(roll); % XYZ旋转矩阵


%----------动平台的6个铰点，在静平台坐标系中的位置矢量---------------------
br1 = TransM * bR1 + P;
br2 = TransM * bR2 + P;
br3 = TransM * bR3 + P;
br4 = TransM * bR4 + P;
br5 = TransM * bR5 + P;
br6 = TransM * bR6 + P;

%--动平台的6个铰点位置矢量，减去，静平台的6个铰点位置矢量，得到每个杆长矢量
L1 = br1 - Br1;
L2 = br2 - Br2;
L3 = br3 - Br3;
L4 = br4 - Br4;
L5 = br5 - Br5;
L6 = br6 - Br6;

%-----------求模，得到每个杆的杆长-----------------------------------------
LenL1 = norm(L1);
LenL2 = norm(L2);
LenL3 = norm(L3);
LenL4 = norm(L4);
LenL5 = norm(L5);
LenL6 = norm(L6);

L1 = L1/LenL1;
L2 = L2/LenL2;
L3 = L3/LenL3;
L4 = L4/LenL4;
L5 = L5/LenL5;
L6 = L6/LenL6;

lenArray = [LenL1;LenL2;LenL3;LenL4;LenL5;LenL6];
vectorArray = [
    L1,L2,L3,L4,L5,L6];

brArray = [br1,br2,br3,br4,br5,br6];
BrArray = [Br1,Br2,Br3,Br4,Br5,Br6];






end

判断电推杆长度是否符合要求

function [isSatisfy] = isLenSatisfy(lenArray,minLen,maxLen)
%isLenSatisfy 输入长度数组，判断电机长度是否达标（6*1）
%1达标，0不达标
ArrayNum = size(lenArray,1);

i = 0;
isSatisfy = 1;

for i = 1:ArrayNum
    temp = lenArray(i);
    if(temp<=maxLen&&temp>=minLen)
        isSatisfy = isSatisfy*1; 
    else
        isSatisfy = 0;
    end
    
end


end

判断虎克铰的角度是否符合要求

function [isSatisfy] = isHookeAngleSatisfy(vectorArray,minHookeAngle,maxHookeAngle)
%isHookeAngleSatisfy 输入向量数组，虎克铰（下面的铰）最小角和最大角
%1达标，0不达标

%静平台姿态：总是(0,0,1)
staticVector = [0;0;1];


ArrayNum = size(vectorArray,2);

i = 0;
isSatisfy = 1;

for i = 1:ArrayNum
    temp = vectorArray(:,i);
    hookeAngleTemp = acosd(dot(temp,staticVector)); 
    
    if(hookeAngleTemp<=maxHookeAngle&&hookeAngleTemp>=minHookeAngle)
        isSatisfy = isSatisfy*1;
    else
        isSatisfy = 0;
    end
    
end


end

判断球铰的角度是否符合要求

function [isSatisfy] = isSphereAngleSatisfy(vectorArray,minSphereAngle,maxSphereAngle,roll,pitch,yaw)
%isSphereAngleSatisfy 输入向量数组，球铰（上面的铰）最小角和最大角，rpy角，输出是否符合球铰角度要求
%1达标，0不达标


ArrayNum = size(vectorArray,2);
%相对固定轴，RPY的XYZ
%相当于欧拉角中的ZYX
TransM = rotz(yaw) * roty(pitch) * rotx(roll); % XYZ旋转矩阵

%z轴在静平台坐标系中的方向矢量
%机器人学蔡自兴p30
axisZ = TransM(:,3);

%球铰的向量方向：取负号
sphereAxis = -axisZ;

i = 0;
isSatisfy = 1;


for i = 1:ArrayNum
    temp = -vectorArray(:,i);%注意要取负号
    sphereAngleTemp = acosd(dot(temp,sphereAxis)); 
    
    if(sphereAngleTemp<=maxSphereAngle&&sphereAngleTemp>=minSphereAngle)
        isSatisfy = isSatisfy*1;
    else
        isSatisfy = 0;
    end
    
end









end

判断杆是否干涉

function [isSatisfy] = isInterferenceSatisfy(lenArray,vectorArray,brArray,BrArray,poleRad)
%isInterferenceSatisfy 判断连杆之间是否干涉
%输入连杆的直径，长度和方向,以及动静平台的铰点在静坐标系下的坐标；
%是为1，否为0

ArrayNum = size(lenArray,1);

i = 0;
isSatisfy = 1;


for i = 1:ArrayNum
    %判断相邻两杆之间的最短距离
    if(i==ArrayNum)
        brArrayTwo = [brArray(:,i),brArray(:,1)];
        BrArrayTwo = [BrArray(:,i),BrArray(:,1)];
        vectorArrayTwo = [vectorArray(:,i),vectorArray(:,1)];
        lenArrayTwo = [lenArray(i),lenArray(1)];
    else
        brArrayTwo = [brArray(:,i),brArray(:,i+1)];
        BrArrayTwo = [BrArray(:,i),BrArray(:,i+1)];
        vectorArrayTwo = [vectorArray(:,i),vectorArray(:,i+1)];
        lenArrayTwo = [lenArray(i),lenArray(i+1)];
    end
    a = 0;
    minDis = brokenLineIntersaction(brArrayTwo,BrArrayTwo,vectorArrayTwo,lenArrayTwo);
    %将最短距离和杆的半径进行对比
    if(minDis>2*poleRad)
        isSatisfy = isSatisfy*1; 
    else
        isSatisfy = 0;
    end
    
end




end

求空间中两条线段之间最短距离

function [dis] = brokenLineIntersaction(brArrayTwo,BrArrayTwo,vectorArrayTwo,lenArrayTwo)

%brokenLineIntersaction 
%输入两条线段的起始点（两个列向量），方向向量（两个列向量），长度;以数组形式输入
%输出两者的最短距离
%分成平行、异面、相交三种情形，每种情形再分类讨论。
epsilon = 1e-2;

dis = 0;
%求公法线向量
ni = cross(vectorArrayTwo(:,1),vectorArrayTwo(:,2));
%若平行
if(norm(ni)<epsilon)
    pLine = brArrayTwo(:,2)-brArrayTwo(:,1);
    dis =  norm(cross(pLine,vectorArrayTwo(:,1)));
    return;
end

ni = ni/norm(ni);

%异面
%最短距离
%求出公垂线和两者的交点
[m,n] = linePerpCalFun(BrArrayTwo(:,1),brArrayTwo(:,1),BrArrayTwo(:,2),brArrayTwo(:,2));
%判断交点是否分别在两者上
if(m<=1&&m>=0&&n<=1&&n>=0)
    %是，则距离就是垂线长度
    deltaTemp = abs(dot(ni,brArrayTwo(:,2)-brArrayTwo(:,1)));
else
    %否，距离要根据端点到另一条线段的距离来算
    %a点和线段cd距离；b点和线段cd距离
    %c点和线段ab距离；d点和线段ab距离
    %四者取最小者
   %点br1与线段br2,Br2的距离
   dis1 = disPointLineSeg(brArrayTwo(:,1),brArrayTwo(:,2),BrArrayTwo(:,2));
   %点Br1与线段br2,Br2的距离
   dis2 = disPointLineSeg(BrArrayTwo(:,1),brArrayTwo(:,2),BrArrayTwo(:,2));
   %点br2与线段br1,Br1的距离
   dis3 = disPointLineSeg(brArrayTwo(:,2),brArrayTwo(:,1),BrArrayTwo(:,1));
   %点Br2与线段br1,Br1的距离
   dis4 = disPointLineSeg(BrArrayTwo(:,2),brArrayTwo(:,1),BrArrayTwo(:,1));
   %最短
   deltaTemp = min([dis1,dis2,dis3,dis4]);
end


if(deltaTemp<epsilon)
    %若最短距离为0,则在同一平面内
    %a点和线段cd距离；b点和线段cd距离
    %c点和线段ab距离；d点和线段ab距离
    %四者取最小者
   %点br1与线段br2,Br2的距离
   dis1 = disPointLineSeg(brArrayTwo(:,1),brArrayTwo(:,2),BrArrayTwo(:,2));
   %点Br1与线段br2,Br2的距离
   dis2 = disPointLineSeg(BrArrayTwo(:,1),brArrayTwo(:,2),BrArrayTwo(:,2));
   %点br2与线段br1,Br1的距离
   dis3 = disPointLineSeg(brArrayTwo(:,2),brArrayTwo(:,1),BrArrayTwo(:,1));
   %点Br2与线段br1,Br1的距离
   dis4 = disPointLineSeg(BrArrayTwo(:,2),brArrayTwo(:,1),BrArrayTwo(:,1));
   %最短
   dis = min([dis1,dis2,dis3,dis4]);
else
    dis = deltaTemp;
end



end

空间中点到线段最短距离

function [minDis] = disPointLineSeg(pointCoo,linePoint1Coo,linePoint2Coo)
%disPointLineSeg 输入三维点的坐标，三维线段的坐标（即始末两点的坐标），均为列向量形式
% 输出最短距离
%https://blog.csdn.net/Mr_HCW/article/details/82816490
x0 = pointCoo(1);
y0 = pointCoo(2);
z0 = pointCoo(3);

x1 = linePoint1Coo(1);
y1 = linePoint1Coo(2);
z1 = linePoint1Coo(3);

x2 = linePoint2Coo(1);
y2 = linePoint2Coo(2);
z2 = linePoint2Coo(3);

%求垂足
a = (x0 - x1) * (x2 - x1) + (y0 - y1) * (y2 - y1) + (z0 - z1) * (z2 - z1);
b = (x2 - x1) * (x2 - x1) + (y2 - y1) * (y2 - y1) + (z2 - z1) * (z2 - z1);
k = a/b;

xN = k * (x2 - x1) + x1;
yN = k * (y2 - y1) + y1;
zN = k * (z2 - z1) + z1;

pointDropFeet = [xN;yN;zN];

%分析三种情况
%linePoint1:A;
%linePoint2:B;
%pointCoo:P;
%pointDropFeet:C;
vectorAP = pointCoo-linePoint1Coo;
vectorAB = linePoint2Coo-linePoint1Coo;
vectorBP = pointCoo-linePoint2Coo; 
vectorCP = pointCoo-pointDropFeet; 

r = dot(vectorAP,vectorAB)/norm(vectorAB)^2;

if(r<=0)%r<=0
    minDis = norm(vectorAP);
elseif(r>=1)%r>=1
    minDis = norm(vectorBP);
else%其它
    minDis = norm(vectorCP);
end





end

求异面两条直线的公垂线和这两条直线的交点的位置

function [m,n] = linePerpCalFun(p1Vector,p2Vector,p3Vector,p4Vector)
%linePerpCalFun 输入异面的两条直线（p1p2和p3p4），点向量均为列向量
% 输出它们的公垂线交点在线段上的位置m,n
%https://blog.csdn.net/weixin_40332490/article/details/89133859

d1Vector = p2Vector-p1Vector;
d2Vector = p4Vector-p3Vector;

a = dot(d1Vector,d2Vector);
b = dot(d1Vector,d1Vector);
c = dot(d2Vector,d2Vector);
d = dot(p3Vector-p1Vector,d1Vector);
e = dot(p3Vector-p1Vector,d2Vector);

epsilon = 1e-2;

if(a<epsilon)%两直线垂直
    m = d/b;
    n = -e/c;
else
    m = (a*e-c*d)/(a*a-b*c);
    n = (b/a) * m - (d/a);
end



end