メインコンテンツ

2 リンク ロボットの LTV モデル

R2026b

この例では、所定の軌跡に沿った 2 リンク ロボットの LTV モデルを取得する方法を示します。ロボット モデルおよび対応するダイナミクスは [1] および [2] を基にしています。

非線形モデル

次の図に 2 リンク平面ロボットの概略図を示します。

TLRobot.png

ロボットのコンフィギュレーションは、ジョイント角度 θ1 および θ2 と先端の位置を制御するジョイントに加えられるトルク τ1 および τ2 で記述されます。状態ベクトルは次のとおりです。

x(t)=[θ1θ2θ˙1(t)θ˙2(t)].

ロボット ダイナミクスは次の方程式で記述されます。

[α+2βcos(θ2)δ+βcos(θ2)δ+βcos(θ2)δ][θ1¨θ2¨]+[-βsin(θ2)θ2˙βsin(θ2)(θ1˙+θ2˙)βsin(θ2)θ1˙0][θ1˙θ2˙]=[τ1τ2]

ここで、以下となります。

α=Iz1+Iz2+m1r12+m2(l12+r22)

β=m2l1r2

δ=Iz2+m2r22

この例では、この逆運動学問題を twoLinkInverseKinematics.m スクリプトを使用して解きます。

物理パラメーターを定義します。

m1 = 3;         % Mass of link 1, kg
m2 = 2;         % Mass of link 2, kg
L1 = 0.3;       % Length of link 1, meters
L2 = 0.3;       % Length of link 2, meters

目的の軌跡と逆運動学

アームの先端は所定の平面軌跡 X(t),Y(t) に従わなければなりません。

T0 = 0;
Tf = 5;
tref = linspace(T0,Tf,400)';
load XYTrajectory x2ref y2ref
plot( x2ref,y2ref,"b",LineWidth=3,MarkerSize=10)
xlabel("x (m)")
ylabel("y (m)")
grid on
title("Target Trajectory")

Figure contains an axes object. The axes object with title Target Trajectory, xlabel x (m), ylabel y (m) contains an object of type line.

リンク 2 の終点の運動に対応するジョイント角度、ジョイント速度、およびトルク入力の逆運動学問題を解きます。ジョイント角度を使用してリンク 1 の先端の軌跡を計算します。

[x,tau] = twoLinkInverseKinematics(tref,x2ref,y2ref,m1,m2,L1,L2);
x1ref = L1*cos(x(:,1));
y1ref = L1*sin(x(:,1));

ジョイント角度とトルクをプロットします。

plot(tref,x(:,1),"b",tref,x(:,2),"r",LineWidth=2);
legend("\theta_1","\theta_2",Location="best")
ylabel("Angle (rad)")
xlabel("Time (s)")
grid on

Figure contains an axes object. The axes object with xlabel Time (s), ylabel Angle (rad) contains 2 objects of type line. These objects represent \theta_1, \theta_2.

plot(tref,tau(:,1),"b",tref,tau(:,2),"r",LineWidth=2);
legend("\tau_1","\tau_2",Location="best")
ylabel("Torque (Nm)")
xlabel("Time (s)")
grid on

Figure contains an axes object. The axes object with xlabel Time (s), ylabel Torque (Nm) contains 2 objects of type line. These objects represent \tau_1, \tau_2.

2 リンク ロボットの Simscape™ Multibody™ モデルを使用して、計算トルクに対するロボットの応答をシミュレートします。

tau1 = tau(:,1);
tau2 = tau(:,2);
xinit = x(1,:)';  % initial state

open_system("TwoLinkRobotSM")

TwoLinkRobotLPV_04.png

モデルをシミュレートし、結果を展開し、先端が目的の軌跡に従っていることを確認します。

out = sim("TwoLinkRobotSM","ReturnWorkspaceOutputs","on");
tsim = out.xsim.time;
xsim = out.xsim.signals.values;

plotTrajectory(xsim,tref,x1ref,y1ref,x2ref,y2ref,L1,L2)

Figure contains an axes object. The axes object with xlabel x (m), ylabel y (m) contains 8 objects of type line, text. One or more of the lines displays its values using only markers These objects represent desired, simulated.

LTV モデル

目的の軌跡に沿った 2 リンク ロボットの LTV モデルを作成するために、シミュレーションの開始から終了までの 50 個の等間隔な時間における線形化スナップショットを撮ります。linearize を使用して、対応する操作条件の線形化モデルを取得します。

線形化 I/O を指定します。

io = [...
   linio("TwoLinkRobotSM/tau1",1,"input");...
   linio("TwoLinkRobotSM/tau2",1,"input");...
   linio("TwoLinkRobotSM/Mux",1,"output")];

スナップショットの操作点を取得します。

tlin = linspace(T0,Tf,50);
op = findop("TwoLinkRobotSM",tlin);

オフセットをもつ線形化を計算します。

linOpt = linearizeOptions(StoreOffsets=true);
[G,~,info] = linearize("TwoLinkRobotSM",io,op,linOpt);
G.u = "tau";
G.y = "x";
G.SamplingGrid = struct("Time",tlin);

ssInterpolant を使用して、線形化モデルと関連オフセットの配列から LTV オブジェクトを作成します。結果の LTV モデルは、線形化されたダイナミクスをスナップショット時間 tlin の間に内挿します。

Gltv = ssInterpolant(G,info.Offsets);
Gltv.StateName
ans = 4×1 cell
    {'TwoLinkRobotSM.Subsystem.Revolute_Joint1.Rz.q'}
    {'TwoLinkRobotSM.Subsystem.Revolute_Joint1.Rz.w'}
    {'TwoLinkRobotSM.Subsystem.Revolute_Joint2.Rz.q'}
    {'TwoLinkRobotSM.Subsystem.Revolute_Joint2.Rz.w'}

線形化では状態が異なる順序になります。xperm を使用して、状態の順序を θ1、θ2、θ˙1、θ˙2 の順に合わせて並べ替えます。

Gltv = xperm(Gltv,[1 3 2 4]);
Gltv.StateName
ans = 4×1 cell
    {'TwoLinkRobotSM.Subsystem.Revolute_Joint1.Rz.q'}
    {'TwoLinkRobotSM.Subsystem.Revolute_Joint2.Rz.q'}
    {'TwoLinkRobotSM.Subsystem.Revolute_Joint1.Rz.w'}
    {'TwoLinkRobotSM.Subsystem.Revolute_Joint2.Rz.w'}

LTV モデルの検証

この LTV モデルの同じトルク入力に対する応答をシミュレートし、Simscape Multibody の結果と比較します。

トルク プロファイルに対する LTV の応答をシミュレートします。

xltv = lsim(Gltv,[tau1 tau2],tref,xinit);

Simscape の結果と比較します。

clf 
plot(tsim,xsim(:,1),"b",tref,xltv(:,1),"k--",...
   tsim,xsim(:,2),"r",tref,xltv(:,2),"k:",LineWidt=2);
xlim([0 5])
legend("\theta_1 (Simscape)","\theta_1 (LTV)",...
   "\theta_2 (Simscape)","\theta_2 (LTV)",Location="best")
title("Simulation with Continuous LTV Model")

Figure contains an axes object. The axes object with title Simulation with Continuous LTV Model contains 4 objects of type line. These objects represent \theta_1 (Simscape), \theta_1 (LTV), \theta_2 (Simscape), \theta_2 (LTV).

離散化

組み込みアプリケーションでは、簡単にシミュレートできる離散モデルが必要なことがよくあります。c2d を使用して、Gltv の Tustin 離散化を計算し、それをグリッド付き離散 LTV モデルにすることができます。

Ts = 0.05;
Gd = c2d(Gltv,Ts,"tustin");

Gd を 50 個のグリッド点をもつグリッド付き LTV モデルにします。

k = round(linspace(0,Tf/Ts,50));
Gd = ssInterpolant(Gd,struct("Time",k));

Tustin 離散化の初期条件は次のとおりです。

z0=x(0)-Ts2x˙(0).

[A,B,~,~,~,dx0,x0,u0] = Gltv.DataFunction(0);
dxinit = dx0+A*(xinit-x0)+B*(tau(1,:)'-u0);
zinit = xinit-Ts/2*dxinit;

シミュレートして Simscape の結果と比較します。

t = T0:Ts:Tf;
taud = interp1(tref,[tau1 tau2],t);
xd = lsim(Gd,taud,t,zinit);

clf 
plot(tsim,xsim(:,1),"b",t,xd(:,1),"k--",...
   tsim,xsim(:,2),"r",t,xd(:,2),"k:",LineWidth=2);
xlim([0 5])
legend("\theta_1 (Simscape)","\theta_1 (discrete LTV)",...
   "\theta_2 (Simscape)","\theta_2 (discrete LTV)",Location="best")
title("Simulation with Discretized LTV Model")

Figure contains an axes object. The axes object with title Simulation with Discretized LTV Model contains 4 objects of type line. These objects represent \theta_1 (Simscape), \theta_1 (discrete LTV), \theta_2 (Simscape), \theta_2 (discrete LTV).

参考文献

[1] Murray, Richard M., Zexiang Li, and Shankar Sastry. A Mathematical Introduction to Robotic Manipulation. Boca Raton: CRC Press, 1994.

[2] Seiler, Peter, Robert M. Moore, Chris Meissen, Murat Arcak, and Andrew Packard. “Finite Horizon Robustness Analysis of LTV Systems Using Integral Quadratic Constraints.” Automatica 100 (February 2019): 135–43. https://doi.org/10.1016/j.automatica.2018.11.009.

参考

| | | | | (Simulink Control Design)

トピック