Model podwójnego wahadła (manipulatora planarnego 2R)

Wektor konfiguracji: q=((((Subscript[θ, 1]),(Subscript[θ, 2])))^T) - położenie  kątowe przegubów 
Wektor sterowań:  u=((((Subscript[u, 1]),(Subscript[u, 2])))^T)- momenty przyłożone w przegubach
Równania dynamiki: Q q''+B=u  (bardziej szczegółowo: Q q''+C q'+D=u )
Równania kinematyki:   y=k(q)

(* wekory *)
vq[t_]={θ1[t],θ2[t]};
vu[t_]={u1[t],u2[t]};

(* parametry wahadła *)
params={g->9.81,(Subscript[m, 1])->1,(Subscript[m, 2])->1,(Subscript[l, 1])-> 1,(Subscript[l, 2])->1};

(* położenia przegubów - kinematyka *)
vk1[t_]={(Subscript[l, 1])Sin[vq[t][[1]]],-(Subscript[l, 1])Cos[vq[t][[1]]]};
vk2[t_]={(Subscript[l, 1])Sin[vq[t][[1]]]+(Subscript[l, 2])Sin[vq[t][[2]]],-(Subscript[l, 1])Cos[vq[t][[1]]]-(Subscript[l, 2])Cos[vq[t][[2]]]};

(* elementy składowe równań dynamiki *)
mQ={{((Subscript[m, 1])+(Subscript[m, 2]))((Subscript[l, 1])^2),(Subscript[m, 2])(Subscript[l, 1])(Subscript[l, 2])Cos[vq[t][[1]]-vq[t][[2]]]},
	{(Subscript[m, 2])(Subscript[l, 1])(Subscript[l, 2])Cos[vq[t][[1]]-vq[t][[2]]],(Subscript[m, 2])((Subscript[l, 1])^2)}};
mB={{(Subscript[m, 2])(Subscript[l, 1])(Subscript[l, 2])(((vq'[t][[2]]))^2)Sin[vq[t][[1]]-vq[t][[2]]]+g((Subscript[m, 1])+(Subscript[m, 2]))(Subscript[l, 1])Sin[vq[t][[1]]]},
	{-(Subscript[m, 2])(Subscript[l, 1])(Subscript[l, 2])(((vq'[t][[1]]))^2)Sin[vq[t][[1]]-vq[t][[2]]]+g (Subscript[m, 2])(Subscript[l, 2])Sin[vq[t][[2]]]}};

(* równania dynamiki *)
eqns=mQ.vq''[t]+mB==vu[t];
(* z tarciem *)
 (* mR=DiagonalMatrix[{.1,.1}];
eqns=mQ.vq''[t]+mB+mR.vq'[t]==vu[t]; *)

Map[MatrixForm,eqns,1]   (* wyświetlmy równania *)

Symulacja zachowania układu

(* warunki początkowe i sterowania *)
ics={θ1[0]==Pi/2,θ2[0]==0,θ1'[0]==0,θ2'[0]==0};
contr={u1[t]->0,u2[t]->0};
(* rozwiązanie *)
tmax=100;
sol=NDSolve[{eqns,ics}/.params/.contr,Head/@vq[t],{t,0,tmax}]//First;
Plot[Evaluate[vq[t]/.sol],{t,0,tmax},PlotLegends->{"(Subscript[θ, 1])","(Subscript[θ, 2])"},PlotRange->All]

(* i animacja *)
pendul[t_]:={{Line[{{0,0},vk1[t],vk2[t]}],PointSize[.025],{Red,Point[vk1[t]]},{Blue, Point[vk2[t]]}}/.sol/.params}
trace[t_]:={Green,Line[Map[Function[Evaluate[vk2[#]/.params/.sol]],Range[0,t,0.025]]]}
Animate[Graphics[{pendul[t],trace[t]},PlotRange->{{-2,2},{-2,2}}, Axes->True,Ticks->False,ImageSize->500],{t,0,tmax,.025},AnimationRate->1/2, AnimationRunning->False]

Sterownik (metoda odsprzęgania wejściowo-wyjściowego - pełna linearyzacja)

Nowy wektor sterowań:  v=((((Subscript[v, 1]),(Subscript[v, 2])))^T)- przyspieszenia końca wahadła
Zlinearyzowane równania dynamiki:  y''=v  (mieliśmy: Q q''+C q'+D=u )

(* nowe sterowania, jakobian wahadła, odsprzężenie *)
vv[t_]={v1[t],v2[t]};
mJ[t_]=D[vk2[t],{vq[t]}];
mD=mJ[t].Inverse[mQ];
mP=mJ'[t].vq'[t]-mJ[t].Inverse[mQ].mB;
lineqns=(mQ.vq''[t]+mB==-Inverse[mD].mP+Inverse[mD].vv[t]);
(* mieliśmy eqns = mQ.vq''[t]+mB==vu[t] *)

Symulacja układu zlinearyzowanego

(* warunki początkowe *)
ics={θ1[0]==Pi/2,θ2[0]==0,θ1'[0]==0.,θ2'[0]==0.};

(* najpierw bez sterowań - potem jakieś proste *)
newcontr={v1[t] -> 0,v2[t] -> 0}; 
(* newcontr={v1[t] -> -0.1 Sin[t],v2[t] -> 0.1 Cos[t]}; *)

(* rozwiązanie *)
tmax=20;
sol=NDSolve[{lineqns,ics}/.params/.newcontr,Head/@vq[t],{t,0,tmax}]//First;
Plot[Evaluate[vq[t]/.sol],{t,0,tmax},PlotLegends->{"(Subscript[θ, 1])","(Subscript[θ, 2])"}]

(* i animacja *)
Animate[Graphics[{pendul[t],trace[t]},PlotRange->{{-2,2},{-2,2}}, Axes->True,Ticks->False,ImageSize->500],{t,0,tmax,.025},AnimationRate->1/2, AnimationRunning->False]

(* sterowania u wyliczane przez moduł linearyzacji *)
vuComp[t_]=mQ.vq''[t]+mB/.params;
Plot[Evaluate[vuComp[t]/.sol],{t,0,tmax},PlotLegends->{"(Subscript[u, 1])","(Subscript[u, 2])"}]

Zamknijmy pętlę sprzężenia zwrotnego

(* powtórzone warunki początkowe - dla wygody symulacji *)
ics={θ1[0]==Pi/2,θ2[0]==0,θ1'[0]==0,θ2'[0]==0};

(* trajektoria zadana *)
vyd[t_]={0.3+0.3Cos[t],0.3+0.3 Sin[t]};

(* sterownik PD *)
K1={{1,0},{0,1}};      (* macierz wzmocnień *)
K0={{3,0},{0,3}};

pdscheme=vyd''[t]-K1.(vk2'[t]-vyd'[t])-K0.(vk2[t]-vyd[t]);
loop=Apply[Rule,{vv[t],pdscheme}//Transpose,1];

(* rozwiązanie *)
tmax=20;
sol=NDSolve[{lineqns,ics}/.loop/.params,Head/@vq[t],{t,0,tmax}]//First;
Plot[Evaluate[vq[t]/.sol],{t,0,tmax},PlotLegends->{"(Subscript[θ, 1])","(Subscript[θ, 2])"}]

(* and animate *)
destraj[t_]:={Blue,Line[Map[Function[Evaluate[vyd[#] /.sol]],Range[0,t,0.025]]]};
Animate[Graphics[{pendul[t],destraj[t],trace[t]},PlotRange->{{-2,2},{-2,2}}, Axes->True,Ticks->False,ImageSize->500],{t,0,tmax,.025},AnimationRate->1/2, AnimationRunning->False]

(* błędy sterowania *)
verr[t_]=Norm[vyd[t]-vk2[t]/.params];
Plot[Evaluate[verr[t]/.sol],{t,0,tmax},PlotLegends->{"e"},PlotRange->All]

(* sterowania u wyliczane przez sterownik *)
vuComp[t_]=mQ.vq''[t]+mB/.params;
Plot[Evaluate[vuComp[t]/.sol],{t,0,tmax},PlotLegends->{"(Subscript[u, 1])","(Subscript[u, 2])"}]