Application Example

We consider a 2 - bar planar mechanism, with two bars of lengths l1 and l2, actuated with two motors doing an angle T1 and T2 from their initial pose T10 and T20, respectively. The position of the attachement point is [xA,yA] in the measurement frame.

Necessary Parameter Accuracy

Calculate Jq et Jxi

First, let write the kinematic model F as a function of the kinematic parameters:

F[xA_,yA_,l1_,l2_,T10_,T20_,T1_,T2_]:={{xA+l1*Cos[T1+T10]+l2*Cos[T1+T10+T2+T20]-xC},{yA+l1*Sin[T1+T10]+l2*Sin[T1+T10+T2+T20]-yC}};
MatrixForm[F[xA,yA,l1,l2,T10,T20,T1,T2]]

Differentiating F with respect to the actuator angles T1 and T2 leads to matrix JQ:

Jq[xA_,yA_,l1_,l2_,T10_,T20_,T1_,T2_]:={{D[F[xA,yA,l1,l2,T10,T20,T1,T2],T1][[1,1]],D[F[xA,yA,l1,l2,T10,T20,T1,T2],T2][[1,1]]},{D[F[xA,yA,l1,l2,T10,T20,T1,T2],T1][[2,1]],D[F[xA,yA,l1,l2,T10,T20,T1,T2],T2][[2,1]]}};
MatrixForm[Jq[xA,yA,l1,l2,T10,T20,T1,T2]]

JQ[l1_,l2_,T10_,T20_,T1_,T2_]:=(({{-l1 Sin[T1+T10]-l2 Sin[T1+T10+T2+T20], -l2 Sin[T1+T10+T2+T20]}, {l1 Cos[T1+T10]+l2 Cos[T1+T10+T2+T20], l2 Cos[T1+T10+T2+T20]}}));

Differentiating F with respect to the kinematic parameters [xA,yA,l1,l2,T10,T20] leads to matrix JXI :

Jxi[l1_,l2_,T10_,T20_,T1_,T2_]:={{D[F[xA,yA,l1,l2,T10,T20,T1,T2],xA][[1,1]],D[F[xA,yA,l1,l2,T10,T20,T1,T2],yA][[1,1]],D[F[xA,yA,l1,l2,T10,T20,T1,T2],l1][[1,1]],D[F[xA,yA,l1,l2,T10,T20,T1,T2],l2][[1,1]],D[F[xA,yA,l1,l2,T10,T20,T1,T2],T10][[1,1]],D[F[xA,yA,l1,l2,T10,T20,T1,T2],T20][[1,1]]},{D[F[xA,yA,l1,l2,T10,T20,T1,T2],xA][[2,1]],D[F[xA,yA,l1,l2,T10,T20,T1,T2],yA][[2,1]],D[F[xA,yA,l1,l2,T10,T20,T1,T2],l1][[2,1]],D[F[xA,yA,l1,l2,T10,T20,T1,T2],l2][[2,1]],D[F[xA,yA,l1,l2,T10,T20,T1,T2],T10][[2,1]],D[F[xA,yA,l1,l2,T10,T20,T1,T2],T20][[2,1]]}};
MatrixForm[Jxi[l1,l2,T10,T20,T1,T2]]

JXI[l1_,l2_,T10_,T20_,T1_,T2_]:=(({{1, 0, Cos[T1+T10], Cos[T1+T10+T2+T20], -l1 Sin[T1+T10]-l2 Sin[T1+T10+T2+T20], -l2 Sin[T1+T10+T2+T20]}, {0, 1, Sin[T1+T10], Sin[T1+T10+T2+T20], l1 Cos[T1+T10]+l2 Cos[T1+T10+T2+T20], l2 Cos[T1+T10+T2+T20]}}));

Calculate the necessary accuracy of the kinematic parameters

We made the assumption that all the distance parameters xA, yA, l1 and l2 must have the same accuracy DX, and that the orientation parameters T10 and T20 must have the same accuracy DT.

Let consider the accuracies of the actuator sensor dT1 and dT2 and the final desired accuracy dxC and dyC:

dT1=0.01*Pi/180;
dT2=dT1;
dxC=0.01;
dyC=dxC;

The right-hand side of condition (C1) can be written as:

Eq1[l1_,l2_,T10_,T20_,T1_,T2_]:={dxC,dyC}-Abs[JQ[l1,l2,T10,T20,T1,T2]].{dT1,dT2};
MatrixForm[Eq1[l1,l2,T10,T20,T1,T2]]

The left - hand side of condition (C1) can be written as :

Eq2[l1_,l2_,T10_,T20_,T1_,T2_,dX_,dT_]:=Abs[JXI[l1,l2,T10,T20,T1,T2]].{dX,dX,dX,dX,dT,dT};
MatrixForm[Eq2[l1,l2,T10,T20,T1,T2,dX,dT]]

Solve Eq1 and Eq2

For Eq1

Find the minimum values of Eq1, with the nominal values l1=1m, l2=0.5m, T10=T20=0:

Minimize[Eq1[1,0.5,0,0,T1,T2][[1]],{T1,T2}]
Eq1[1,0.5,0,0,Pi/2,0][[1]]

Minimize[Eq1[1,0.5,0,0,T1,T2][[2]],{T1,T2}]
Eq1[1,0.5,0,0,0,0][[2]]

For Eq2

Find the maximum values of Eq2, with the nominal values l1 = 1 m, l2 = 0.5 m, T10 = T20 = 0 :

Eq2[1,0.5,0,0,T1,T2,dX,dT][[1]]

Eq2[1,0.5,0,0,T1,T2,dX,dT][[2]]

It can be seen that Eq2 can be overestimated with 3*dX+2*dT

Solutions

Find the set of solutions for dX and dT:

Eq3[dX_,dT_]:=Eq1[1,0.5,0,0,0,0][[2]]-(3*dX+2*dT);
Eq3[dX,dT]

Plot the set of solutions:

Plot[Solve[Eq3[dX,dT]==0,dX][[1,1,2]],{dT,0,0.3*Pi/180}]

Choice of the numerical solutions

Let choose dT=0.002

Solve[Eq3[dX,0.002]==0,dX][[1,1,2]]

Then, dX = 0.001

dX=0.001;
dT=0.002;

Verification

Let plot Eq2-Eq1 over the workspace of the manipulator (T1 = T2 = [-90;90] degre). The solution has to be negative.

For dx :

Plot3D[Eq2[1,1,0,0,T1,T2,dX,dT][[1]]-Eq1[1,1,0,0,T1,T2][[1]],{T1,-Pi/2,Pi/2},{T2,-Pi/2,Pi/2}]

For dy :

Plot3D[Eq2[1,1,0,0,T1,T2,dX,dT][[2]]-Eq1[1,1,0,0,T1,T2][[2]],{T1,-Pi/2,Pi/2},{T2,-Pi/2,Pi/2}]

Maximal measurement inaccuracy

Write the identification function g

The identification function can be taken as the first row of F:

g[xC_,xA_,l1_,l2_,T10_,T20_,T1_,T2_]:=xA+l1*Cos[T1+T10]+l2*Cos[T1+T10+T2+T20]-xC;
g[xC,xA,l1,l2,T10,T20,T1,T2]

Write the matrices Gy, Gq and Gxi

Differenciating g with respect to xC yields to Gy:

Gy[xC_,xA_,l1_,l2_,T10_,T20_,T1_,T2_]:={D[g[xC,xA,l1,l2,T10,T20,T1,T2],xC]};
MatrixForm[Gy[xC,xA,l1,l2,T10,T20,T1,T2]]

GY[xC_,xA_,l1_,l2_,T10_,T20_,T1_,T2_]:={-1};

Differenciating g with respect to T1 and T2 yields to Gq:

Gq[xC_,xA_,l1_,l2_,T10_,T20_,T1_,T2_]:={{D[g[xC,xA,l1,l2,T10,T20,T1,T2],T1],D[g[xC,xA,l1,l2,T10,T20,T1,T2],T2]}};
MatrixForm[Gq[xC,xA,l1,l2,T10,T20,T1,T2]]

GQ[xC_,xA_,l1_,l2_,T10_,T20_,T1_,T2_]:=(({{-l1 Sin[T1+T10]-l2 Sin[T1+T10+T2+T20], -l2 Sin[T1+T10+T2+T20]}}));

Differenciating g with respect to the kinematic parameters xA, yA, l1, l2, T10 and T20 yields to Gxi:

Gxi[xC_,xA_,l1_,l2_,T10_,T20_,T1_,T2_]:={{D[g[xC,xA,l1,l2,T10,T20,T1,T2],xA],D[g[xC,xA,l1,l2,T10,T20,T1,T2],yA],D[g[xC,xA,l1,l2,T10,T20,T1,T2],l1],D[g[xC,xA,l1,l2,T10,T20,T1,T2],l2],D[g[xC,xA,l1,l2,T10,T20,T1,T2],T10],D[g[xC,xA,l1,l2,T10,T20,T1,T2],T20]}};
MatrixForm[Gxi[xC,xA,l1,l2,T10,T20,T1,T2]]

GXi[xC_,xA_,l1_,l2_,T10_,T20_,T1_,T2_]:=(({{1, 0, Cos[T1+T10], Cos[T1+T10+T2+T20], -l1 Sin[T1+T10]-l2 Sin[T1+T10+T2+T20], -l2 Sin[T1+T10+T2+T20]}}));

Numerical values

Define the nominal values

xA=0.1;
yA=0;
l1=1;
l2=0.5;
T10=0;
T20=0;

dT1=0.01*Pi/180;
dT2=dT1;
dxC=0.01;
dyC=dxC;

dX=0.001;
dT=0.002;

Conditions on the measurement device accuracy

Condition (C3) yields to :

Abs[GY[xC,xA,l1,l2,T10,T20,T1,T2]].DY+Abs[GQ[xC,xA,l1,l2,T10,T20,T1,T2]].{dT1,dT2}-Abs[GXi[xC,xA,l1,l2,T10,T20,T1,T2].{dX,dX,dX,dX,dT,dT}]

We need EqY < 0, with:

EqY[xC_,xA_,l1_,l2_,T10_,T20_,T1_,T2_,DY_]:=0.00017453292519943296*Abs[-Sin[T1]-0.5*Sin[T1+T2]]-Abs[0.001+0.001*Cos[T1]+0.001*Cos[T1+T2]+0.002*(-Sin[T1]-0.5*Sin[T1+T2])-0.001*Sin[T1+T2]]+0.00008726646259971648*Abs[Sin[T1+T2]]+DY;

At the initial configuration T1=T2=0, we have:

EqY[xC,xA,l1,l2,T10,T20,0,0,DY]

We solve EqY == 0 for DY, the measurement device accuracy, over the entire workspace:

Plot3D[{NSolve[EqY[xC,xA,l1,l2,T10,T20,T1,T2,DY]==0,{DY}][[1,1,2]],0},{T1,-Pi/2,Pi/2},{T2,-Pi/2,Pi/2}]

Let plot the regions for which EqY == 0 for different values of DY:

a=75*Pi/180;
b=0*Pi/180;
ContourPlot[{T2==b,T2==-a,T1==-a,T1==b,T2==-2*T1+Pi/2,NSolve[EqY[xC,xA,l1,l2,T10,T20,T1,T2,DY]==0,{DY}][[1,1,2]]==0.005,NSolve[EqY[xC,xA,l1,l2,T10,T20,T1,T2,DY]==0,{DY}][[1,1,2]]==0.0045,NSolve[EqY[xC,xA,l1,l2,T10,T20,T1,T2,DY]==0,{DY}][[1,1,2]]==0.004,NSolve[EqY[xC,xA,l1,l2,T10,T20,T1,T2,DY]==0,{DY}][[1,1,2]]==0.0035,NSolve[EqY[xC,xA,l1,l2,T10,T20,T1,T2,DY]==0,{DY}][[1,1,2]]==0.003,NSolve[EqY[xC,xA,l1,l2,T10,T20,T1,T2,DY]==0,{DY}][[1,1,2]]==0.0025,NSolve[EqY[xC,xA,l1,l2,T10,T20,T1,T2,DY]==0,{DY}][[1,1,2]]==0.002,NSolve[EqY[xC,xA,l1,l2,T10,T20,T1,T2,DY]==0,{DY}][[1,1,2]]==0.0015,NSolve[EqY[xC,xA,l1,l2,T10,T20,T1,T2,DY]==0,{DY}][[1,1,2]]==0.001},{T1,-Pi/2,10*Pi/180},{T2,-Pi/2,10*Pi/180}]

Choice of the measurement accuracy and the measurement workspace

Analyzing the above plot, we can see that choosing a measurement accuracy DY=3mm and a measurement workspace for T1 and T2 in [-75;0] degres seems to satisfy (C3).
Let do a verification:

Plot3D[{EqY[xC,xA,l1,l2,T10,T20,T1,T2,0.003],0},{T1,-a,b},{T2,-a,b}]

Analyze of T2 == -2*T1 + Pi/2

From the above plots, it seems that an interesting region is when T2 == -2*T1 + Pi/2. In this case, DY has to be almost equal to zero to satisfy EqY==0.
Let visualize the concerned configurations of the 2-bar mechanism:

Sol[T1_,T2_]:={xA+l1*Cos[T1+T10]+l2*Cos[T1+T10+T2+T20],yA+l1*Sin[T1+T10]+l2*Sin[T1+T10+T2+T20]};

Point1[T1_,T2_]:={xA+l1*Cos[T1+T10],yA+l1*Sin[T1+T10]};

Show[Graphics[{Line[{{xA,yA},Point1[0*Pi/20,-0*Pi/10+Pi/2]}],Line[{{xA,yA},Point1[1*Pi/20,-1*Pi/10+Pi/2]}],Line[{{xA,yA},Point1[2*Pi/20,-2*Pi/10+Pi/2]}],Line[{{xA,yA},Point1[3*Pi/20,-3*Pi/10+Pi/2]}],Line[{{xA,yA},Point1[4*Pi/20,-4*Pi/10+Pi/2]}],Line[{{xA,yA},Point1[5*Pi/20,-5*Pi/10+Pi/2]}],Line[{{xA,yA},Point1[6*Pi/20,-6*Pi/10+Pi/2]}],Line[{{xA,yA},Point1[7*Pi/20,-7*Pi/10+Pi/2]}],Line[{{xA,yA},Point1[8*Pi/20,-8*Pi/10+Pi/2]}],Line[{{xA,yA},Point1[9*Pi/20,-9*Pi/10+Pi/2]}],Line[{{xA,yA},Point1[10*Pi/20,-10*Pi/10+Pi/2]}],Line[{Point1[0*Pi/20,-0*Pi/10+Pi/2],Sol[0*Pi/20,-0*Pi/10+Pi/2]}],Line[{Point1[1*Pi/20,-1*Pi/10+Pi/2],Sol[1*Pi/20,-1*Pi/10+Pi/2]}],Line[{Point1[2*Pi/20,-2*Pi/10+Pi/2],Sol[2*Pi/20,-2*Pi/10+Pi/2]}],Line[{Point1[3*Pi/20,-3*Pi/10+Pi/2],Sol[3*Pi/20,-3*Pi/10+Pi/2]}],Line[{Point1[4*Pi/20,-4*Pi/10+Pi/2],Sol[4*Pi/20,-4*Pi/10+Pi/2]}],Line[{Point1[5*Pi/20,-5*Pi/10+Pi/2],Sol[5*Pi/20,-5*Pi/10+Pi/2]}],Line[{Point1[6*Pi/20,-6*Pi/10+Pi/2],Sol[6*Pi/20,-6*Pi/10+Pi/2]}],Line[{Point1[7*Pi/20,-7*Pi/10+Pi/2],Sol[7*Pi/20,-7*Pi/10+Pi/2]}],Line[{Point1[8*Pi/20,-8*Pi/10+Pi/2],Sol[8*Pi/20,-8*Pi/10+Pi/2]}],Line[{Point1[9*Pi/20,-9*Pi/10+Pi/2],Sol[9*Pi/20,-9*Pi/10+Pi/2]}],Line[{Point1[10*Pi/20,-10*Pi/10+Pi/2],Sol[10*Pi/20,-10*Pi/10+Pi/2]}]}],ParametricPlot[{Sol[T,-2*T+Pi/2][[1]],Sol[T,-2*T+Pi/2][[2]]},{T,0,Pi/2},AspectRatio->1,PlotStyle->Directive[Red,Thick]]]