原理推导
式中:
最终,得到定平台顶点在定坐标系中的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