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

ロボットのコンフィギュレーションは、ジョイント角度 および と先端の位置を制御するジョイントに加えられるトルク および で記述されます。状態ベクトルは次のとおりです。
.
ロボット ダイナミクスは次の方程式で記述されます。
ここで、以下となります。
この例では、この逆運動学問題を 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
目的の軌跡と逆運動学
アームの先端は所定の平面軌跡 , に従わなければなりません。
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")

リンク 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

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

2 リンク ロボットの Simscape™ Multibody™ モデルを使用して、計算トルクに対するロボットの応答をシミュレートします。
tau1 = tau(:,1); tau2 = tau(:,2); xinit = x(1,:)'; % initial state open_system("TwoLinkRobotSM")

モデルをシミュレートし、結果を展開し、先端が目的の軌跡に従っていることを確認します。
out = sim("TwoLinkRobotSM","ReturnWorkspaceOutputs","on"); tsim = out.xsim.time; xsim = out.xsim.signals.values; plotTrajectory(xsim,tref,x1ref,y1ref,x2ref,y2ref,L1,L2)

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 を使用して、状態の順序を 、、、 の順に合わせて並べ替えます。
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")

離散化
組み込みアプリケーションでは、簡単にシミュレートできる離散モデルが必要なことがよくあります。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 離散化の初期条件は次のとおりです。
.
[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")

参考文献
[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.
参考
ltvss | ssInterpolant | sample | ndgrid | c2d | linearize (Simulink Control Design)