三自由度并联机构
1.运动学分析
目标物体由前后的电动缸进行控制，通过给定前后缸的伸缩长度，来控制目标的位置和姿态，实现目标物体的上下，前后，俯仰这三个自由度的运动。
机械机构如下图所示，左侧为前电动缸，右侧为后电动缸。
标出所需的量，设未知数如下图所示。设控制前电动缸长度为LA，设控制后电动缸长度为LB，在目标物体上的末端位置取一点设为（x0，y0）。通过以下公式可得末端位置的位姿：
对 该 执 行 机 构 的 各 部 分 设 置 未 知 参 数 对该执行机构的各部分设置未知参数 对该执行机构的各部分设置未知参数
得到末端位置的数学公式
通过α与β计算出铰接点坐标(xa,ya)和滚轮圆心的坐标(xb,yb)再通过相似原理计算出并联机构与水平面的倾斜角γ和末端位置的坐标(x0,y0)
2.可行范围内的拟合
末端位置由A位置移动到B位置，有一个x方向和y方向上的差值，即Δx和Δy
根据末端位置的运动范围，将步数分为100个路径点，拟合得到系数a1、b1、a2、b2。
再由自变量Δx和Δy得到电动缸的伸缩量，即ΔLA和ΔLB
此时就可以通过x和y方向上的差值，得到电动缸的伸缩量。
3.代码实现
- 拟合路径得到系数
clear
clc
x=7100;
y=850;
xx=[];yy=[];cy1=[];cy2=[];
k=1;
n=101;
for i = 1:n
x0 = x + 2 * (i-1);
for j = 1:n
y0 = y - 1 * (j-1);
[ LA , LB , theta ] = cylength_inclinationangle ( x0 , y0 );
xx(k)=x0;yy(k)=y0;cy1(k)=LA;cy2(k)=LB;theta1(k)=theta;
k=k+1;
end
end
cftool
以x的路径点为X轴，y的路径点为Y轴，拟合出 LA 和 LB 的系数
黑点为实际路径点，彩色平面为拟合路径点。由图像可知，大部分的黑点都在该平面上，因此拟合的效果较好。
以系数和自变量Δx、Δy得到缸长伸缩量
function [ deltaLA , deltaLB ] = Deviation_cylinder( deltax,deltay )
% x、y要移动的距离
deltax;
deltay;
%拟合曲线缸A长度的系数
aA = -0.1507;
bA = -0.02793 ;%-2.82e-14;
%拟合曲线缸B长度的系数
aB = 0.01158;
bB = 0.1867;
deltaLA = aA * deltax + bA* deltay;
deltaLB = aB * deltax + bB* deltay;
end
- 倾斜角变化
ii=[];jj=[];xx=[];yy=[];tt=[];la=[];lb=[];k=1;
for i = 1:51
x0 = 7000 + 4*(i-1);
for j=1:51
y0 = 800 + 2*(j-1);
[ LA , LB ,t1 ] = cylength_inclinationangle ( x0 , y0 );
xx(k)=x0;yy(k)=y0;tt(k)=t1;la=LA;lb=LB;
k=k+1;
end
end
max1=max(abs(tt))
min1=min(abs(tt))
mac=max1-min1
得到倾斜角约等于 1° 的结果，因此在移动过程中不需要考虑末端姿态的调整。
- 逆解
输入A位置与B位置的坐标差值(x0,y0),得到两电动缸的长度和机构下底面对地的倾斜角。
function [ LA , LB , t ] = cylength_inclinationangle ( x0 , y0 )
x0;
y0;
x1=0;y1=821;
x2=600;y2=0;
x3=1910;y3=666;
x4=2196.8;y4=211;
h = 200;
l = 7000;
L12=sqrt((x1-x2)^2+(y1-y2)^2);
L34=sqrt((x3-x4)^2+(y3-y4)^2);
% gamma=atan(h/l);
L1=sqrt(h^2+l^2);
L2=1385;
L3=350;
L4=sqrt((x0-x1)^2+(y0-y1)^2);
% Angle to store alpha series
% alpha1=rad2deg(atan(abs(y0-y1)/(x0-x1)))
% alpha2=rad2deg(atan(abs(y2-y1)/(x2-x1)))
% alpha3=rad2deg(acos((L2^2+L4^2-L1^2)/(2*L2*L4)))
% alpha=alpha1+alpha2+alpha3
alpha1=atan(abs(y0-y1)/(x0-x1));
alpha2=atan(abs(y2-y1)/(x2-x1));
alpha3=acos((L2^2+L4^2-L1^2)/(2*L2*L4));
alpha=alpha1+alpha2+alpha3;
[ x7 , y7 ,t ] = cy3( x0 , y0 );% position fo B and angle of bridge
% L6=1370;
% L7=1045;
% L8=sqrt((x4-x7)^2+(y7-y4)^2);
% beta=rad2deg(acos((L6^2+L34^2-L8^2)/(2*L6*L34)));
L6=1370;
L7=1045;
L8=sqrt((x4-x7)^2+(y4-y7)^2);
beta=acos((L6^2+L34^2-L8^2)/(2*L6*L34));
%ideal value of LA and LB
LA=sqrt(L3^2+L12^2-2*L3*L12*cos(alpha));
LB=sqrt(L34^2+L7^2-2*L34*L7*cos(beta));
% LA=sqrt(L3^2+L12^2-2*L3*L12*cosd(alpha));
% LB=sqrt(L34^2+L7^2-2*L34*L7*cosd(beta));
end
其中，cy3代码如下。
function [ x7 , y7 , theta ]=cy3( x , y )
%已知理想值的坐标，求滚轮圆心坐标
x1=x;
y1=y;
x2=0;y2=821;
x3=1910;y3=666;
h=200;
l=7000;
r=100;
L3=1370;
L=sqrt((x1-x2)^2+(y1-y2)^2);
L1=1385;
L2=sqrt(h^2+l^2);
phi=atan((y1-y2)/(x1-x2));
gamma=acos((L1^2+L^2-L2^2)/(2*L1*L));
x4=x2+L1*cos(gamma+phi);
y4=y2+L1*sin(gamma+phi);
alpha=atan(h / l);
a=atan((y1-y4)/(x1-x4));
a1 = a + alpha;
x5=x1+r*sin(a1);
y5=y1-r*cos(a1);
ka=tan(a1);
x6=(ka*x5-y5+y3+(x3/ka))/(ka+(1/ka));
y6=ka*(x6-x5)+y5;
L4=sqrt((x3-x6)^2+(y3-y6)^2);
L5=sqrt(L3^2-L4^2);
x7=x6+L5*cos(a1);
y7=y6+L5*sin(a1);
theta = rad2deg(a1);
end
- 正解
通过解出的电动缸长度，正解出机构末端的位置和姿态。
function [ x0 , y0 ,gamma4 ] = slove_coordinate( LA , LB )
L1 = LA;
L2 = LB;
x1=0;y1=821;
x2=600;y2=0;
x3=1910;y3=666;
x4=2196.8;y4=211;
h = 200; l = 7000; r = 100;
L12=sqrt((x1-x2)^2+(y1-y2)^2);
L34=sqrt((x3-x4)^2+(y3-y4)^2);
a1=350;a2=1385;
b1=1045;b2=1370;
alphaa = rad2deg(acos((L12^2+a1^2-L1^2)/(2*L12*a1)));
alphaa1 = rad2deg(atan(abs(y2-y1)/abs(x2-x1)));
alphaa2 = alphaa - alphaa1;
xa = a2 * cosd(alphaa2) + x1;
ya = a2 * sind(alphaa2) + y1;
betaa = rad2deg(acos((L34^2+b1^2-L2^2)/(2*L34*b1)));
betaa1 = rad2deg(atan(abs(y3-y4)/abs(x3-x4)));
betaa2 = betaa - betaa1;
xb = b2 * cosd(betaa2) + x3;
yb = b2 * sind(betaa2) + y3;
d1 = sqrt((xa-xb)^2+(ya-yb)^2);
d12 = d1 / (1 + h / r);
d11 = (h / r) * d12;
l11 = sqrt( d11^2 - h^2 );
gamma2 = rad2deg(acos((xb-xa) / d1));
gamma3 = rad2deg(acos( l11 / d11 ));
gamma4 = gamma3 - gamma2 ;
xh = xa + h * sind(gamma4);
yh = ya - h * cosd(gamma4);
x0 = xh + l * cosd(gamma4);
y0 = yh + l * sind(gamma4);
end
- 误差分析
clear;
clc;
%true position with idea position input
%The starting point
x0 = 7100;% 7217 7200
y0 = 950 ;% 840 750
%The initial distance
deltax = 100;
deltay = -110;
xi1 = x0 + deltax;% targrt x
yi1 = y0 + deltay;% target y
deltaxx = 0;
deltayy = -90;
xi2 = xi1 + deltaxx ;
yi2 = yi1 + deltayy ;
deltaxxx = deltax + deltaxx;
deltayyy = deltay + deltayy;
%输入理想缸长得到的理想坐标
[ LAi0 , LBi0 ] = cylength_inclinationangle ( x0 , y0 );
[ xi00 , yi00 ] = slove_coordinate( LAi0 , LBi0 );
[ LAi1 , LBi1 ] = cylength_inclinationangle ( xi1 , yi1 );
[ xi01 , yi01 ] = slove_coordinate( LAi1 , LBi1 );
[ LAi2 , LBi2 ] = cylength_inclinationangle ( xi2 , yi2 );
[ xi02 , yi02 ] = slove_coordinate( LAi2 , LBi2 );
%输入拟合缸长得到真实坐标
%第一步，移动到正上方
[ LAi0 , LBi0 ] = cylength_inclinationangle ( x0 , y0 );
LA = round( LAi0 ); LB = round( LBi0 );
[ deltaLA , ~ ] = Deviation_cylinder( deltaxxx , deltayyy );
[ ~ , deltaLB ] = Deviation_cylinder( deltax , deltay );
LA1 = round( LA + deltaLA );
LB1 = round( LB + deltaLB );
[ x01 , y01 ] = slove_coordinate( LA1 , LB1 );
deltax1 = round( xi1 - x01 );
deltay1 = round( yi1 - y01 );
Error1=sqrt(deltax1^2 + deltay1^2);
[ deltaLA1 , deltaLB1 ] = Deviation_cylinder( deltax1,deltay1 );
LA2 = round( LA1 + deltaLA1 );
LB2 = round( LB1 + deltaLB1 );
[ x02 , y02 ] = slove_coordinate( LA2 , LB2 );
deltax2 = round( xi1 - x02 )+2;
deltay2 = round( yi1 - y02 )+2;
Error2=sqrt(deltax2^2 + deltay2^2);
% 第二步，实现位姿
deltax21 = 0 ; deltay21 = deltayy ;
[ ~ , deltaLB2 ] = Deviation_cylinder( deltax21,deltay21 );
LA3 = LA2 ;
LB3 =round ( LB2 + deltaLB2 );
[ x03 , y03 ] = slove_coordinate( LA3 , LB3 );
deltax3 = round (xi2 - x03 ) ; deltay3 = round ( yi2 - y03 ) ;
Error3=sqrt(deltax3^2 + deltay3^2);
[ deltaLA3 , deltaLB3 ] = Deviation_cylinder( deltax3,deltay3 );
LA4 = LA3 ;
LB4 = round ( LB3 + deltaLB3 );
[ x04 , y04 ] = slove_coordinate( LA4 , LB4 );
deltax4 = round (xi2 - x04 ); deltay4 = round ( yi2 - y04 );
Error4 = sqrt(deltax4^2 + deltay4^2);
完整的代码如下：https://download.csdn.net/download/AlbertDS/12840033