3自由度パラレル機構
1. 運動学的解析
対象物を前後の電動シリンダで制御し、前後シリンダの伸縮長の設定により対象物の和を制御することで、対象物の上下、前後、ピッチの３自由度の動きを実現する位置。姿态
機械機構は下図のとおりで、左側が前電動シリンダ、右側が後電動シリンダです。
以下の図に示すように、必要な数量にラベルを付け、未知のものを想定します。制御前の電動シリンダの長さを L A、制御後の電動シリンダの長さを L Bとし、対象物の終了位置の点を とします（x0，y0）。終了位置の姿勢は以下の式で求められます。
アクチュエータの各部に不明なパラメータを設定する アクチュエータの各部に不明なパラメータを設定するアクチュエータの部品に未知のパラメータを設定する_
終了位置を取得するための数式
ヒンジ点の座標(xa,ya)とローラー中心の座標をα と β で計算(xb,yb)し、パラレル機構と水平面との傾斜角 γ と終端位置の座標を相似原理により計算します。(x0,y0)
2. 実現可能な範囲でフィッティング
終了位置をA位置に移動するB位置と、x 方向と y 方向に差が生じ、ΔxとΔyになります。
終端位置の可動範囲に応じてステップ数を１００個の経路点に分割し、フィッティングにより係数ａ１、ｂ１、ａ２、ｂ２を求める。
次に、電動シリンダーの伸縮は独立変数ΔxとΔy 、つまりΔL AとΔL Bから得られます。
このとき電動シリンダーの伸縮は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 軸として取得し、 L Aと L Bの係数を適合させます。
黒い点は実際のパス ポイントであり、色付きの平面は適合されたパス ポイントです。画像から、ほとんどの黒い点がこの平面上にあるため、フィッティング効果が優れていることがわかります。
係数と独立変数Δ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位置、2本の電動シリンダーの長さと機構の下底の地面に対する傾斜角度を求めます。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