6PSS并联结构的运动学正逆解推导

原理推导

式中：

最终，得到定平台顶点在定坐标系中的Z坐标的表达式：

运动学逆解

clc;
clear;
syms A_flex R r z1 z2 z3 z4 z5 z6 Px Py Pz alpha beta gamma L
% A_fix为定平台顶点连线的角度的一半；
% A_flex为动平台顶点连线的角度的一半；
% R为定平台半径，r为动平台半径；
% z1 z2 z3 z4 z5 z6 为定平台顶点在定坐标系中的Z坐标；
% Px Py Pz为动坐标系原点在定坐标系中的向量；
% alpha beta gamma为动坐标系绕XYZ轴的旋转角度；
% L为杆长；

A1=%%%;
A2=%%%;
A3=%%%;
A4=%%%;
A5=%%%;
A6=%%%;

b1=%%%;
b2=%%%;
b3=%%%;
b4=%%%;
b5=%%%;
b6=%%%;

P=[Px;Py;Pz];

Rot_M=[cos(beta)*cos(gamma),                                    -cos(beta)*sin(gamma),                                    sin(beta);
   sin(alpha)*sin(beta)*cos(gamma)+cos(alpha)*sin(gamma),   -sin(alpha)*sin(beta)*sin(gamma)+cos(alpha)*cos(gamma),   -sin(alpha)*cos(beta);
   -cos(alpha)*sin(beta)*cos(gamma)+sin(alpha)*sin(gamma),  cos(alpha)*sin(beta)*sin(gamma)+sin(alpha)*cos(gamma),    cos(alpha)*cos(beta)];

B1=Rot_M*b1+P;
B2=Rot_M*b2+P;
B3=Rot_M*b3+P;
B4=Rot_M*b4+P;
B5=Rot_M*b5+P;
B6=Rot_M*b6+P;

z1=-sqrt((L^2-(A1(1)-B1(1))^2-(A1(2)-B1(2))^2))+B1(3);
z2=-sqrt((L^2-(A2(1)-B2(1))^2-(A2(2)-B2(2))^2))+B2(3);
z3=-sqrt((L^2-(A3(1)-B3(1))^2-(A3(2)-B3(2))^2))+B3(3);
z4=-sqrt((L^2-(A4(1)-B4(1))^2-(A4(2)-B4(2))^2))+B4(3);
z5=-sqrt((L^2-(A5(1)-B5(1))^2-(A5(2)-B5(2))^2))+B5(3);
z6=-sqrt((L^2-(A6(1)-B6(1))^2-(A6(2)-B6(2))^2))+B6(3);

Ai和bi的确定需根据具体的机构进行计算。

运动学正解

运动学正解的原理是牛顿迭代解算6个非线性方程组。

和运动学逆解的结果基本完全一致，因为正逆解就是相互推导的过程。

function df=dfun(x)
    f=fun(x);
    df=[diff(f,'x1');diff(f,'x2');diff(f,'x3');diff(f,'x4');diff(f,'x5');diff(f,'x6')]; %雅克比矩阵
end

clear
clc
format;
x0  = [0 0 0 0 0 0];  % 迭代初始值
eps = 0.001;  % 定位精度要求
for i = 1:10
    f  = double(subs(fun(x0) ,{'x1' 'x2' 'x3' 'x4' 'x5' 'x6'},{x0(1) x0(2) x0(3) x0(4) x0(5) x0(6)}));
    df = double(subs(dfun(x0),{'x1' 'x2' 'x3' 'x4' 'x5' 'x6'},{x0(1) x0(2) x0(3) x0(4) x0(5) x0(6)}));  %雅克比矩阵
    x = x0 - f/df;
    if(abs(x-x0) < eps)
        break;
    end
    x0 = x; % 更新迭代结果
end
disp('定位坐标：');
x
disp('迭代次数：');
i

function f=fun(x,z1,z2,z3,z4,z5,z6)
	syms x1 x2 x3 x4 x5 x6
	
	A_flex=***;
    R=***;
    r=***;
    L=***;
    
    A1=[R;0;z1];
    A2=[R*cos(pi/3);R*sin(pi/3);z2];
    A3=[-R*cos(pi/3);R*sin(pi/3);z3];
    A4=[-R;0;z4];
    A5=[-R*cos(pi/3);-R*sin(pi/3);z5];
    A6=[R*cos(pi/3);-R*sin(pi/3);z6];

    b1=[r*cos(pi/6-A_flex);r*sin(pi/6-A_flex);0];
    b2=[r*cos(pi/6+A_flex);r*sin(pi/6+A_flex);0];
    b3=[-r*cos(pi/6+A_flex);r*sin(pi/6+A_flex);0];
    b4=[-r*cos(pi/6-A_flex);r*sin(pi/6-A_flex);0];
    b5=[-r*sin(A_flex);-r*cos(A_flex);0];
    b6=[r*sin(A_flex);-r*cos(A_flex);0];

    P=[x1;x2;x3+**];

    Rot_M=[cos(x5)*cos(x6),                                    -cos(x5)*sin(x6),              sin(x5);
       sin(x4)*sin(x5)*cos(x6)+cos(x4)*sin(x6),   -sin(x4)*sin(x5)*sin(x6)+cos(x4)*cos(x6),   -sin(x4)*cos(x5);
       -cos(x4)*sin(x5)*cos(x6)+sin(x4)*sin(x6),  cos(x4)*sin(x5)*sin(x6)+sin(x4)*cos(x6),    cos(x4)*cos(x5)];

    B1=Rot_M*b1+P;
    B2=Rot_M*b2+P;
    B3=Rot_M*b3+P;
    B4=Rot_M*b4+P;
    B5=Rot_M*b5+P;
    B6=Rot_M*b6+P;
    
    f1 = -sqrt(L^2-(A1(1)-B1(1))^2-(A1(2)-B1(2))^2)+B1(3)-z1;
    f2 = -sqrt(L^2-(A2(1)-B2(1))^2-(A2(2)-B2(2))^2)+B2(3)-z2;
    f3 = -sqrt(L^2-(A3(1)-B3(1))^2-(A3(2)-B3(2))^2)+B3(3)-z3;
    f4 = -sqrt(L^2-(A4(1)-B4(1))^2-(A4(2)-B4(2))^2)+B4(3)-z4;
    f5 = -sqrt(L^2-(A5(1)-B5(1))^2-(A5(2)-B5(2))^2)+B5(3)-z5;
    f6 = -sqrt(L^2-(A6(1)-B6(1))^2-(A6(2)-B6(2))^2)+B6(3)-z6;
    f=[f1 f2 f3 f4 f5 f6];
end