LTV Model of Two-Link Robot

This example shows how to obtain an LTV model of a two-link robot along a prescribed trajectory. The robot model and its corresponding dynamics are based on [1] and [2].

Nonlinear Model

This figure gives the schematics of the two-link planar robot.

The robot configuration is described by the joint angles ${\theta }_1$, ${\theta }_2$ and torques ${\tau }_1$, ${\tau }_2$ applied at the joints to control the position of the tip. The state vector is

$\mathit{x}\left(\mathit{t}\right)=\left[\begin{array}{c}{\theta }_1\\ {\theta }_2\\ {\dot{\theta }}_1\left(\mathit{t}\right)\\ {\dot{\theta }}_2\left(\mathit{t}\right)\end{array}\right]$.

The following equations describe the robot dynamics.

$$\left[\begin{array}{cc}\alpha +2\beta \cos \left(\theta {}_2\right)& \delta +\beta \cos \left(\theta {}_2\right)\\ \delta +\beta \cos \left(\theta {}_2\right)& \delta \end{array}\right]\left[\begin{array}{c}\ddot{{\theta }_1}\\ \ddot{{\theta }_2}\end{array}\right]+\left[\begin{array}{cc}-\beta \sin \left({\theta }_2\right)\dot{{\theta }_2}& \beta \sin \left({\theta }_2\right)\left(\dot{{\theta }_1}+\dot{{\theta }_2}\right)\\ \beta \sin \left({\theta }_2\right)\dot{{\theta }_1}& 0\end{array}\right]\left[\begin{array}{c}\dot{{\theta }_1}\\ \dot{{\theta }_2}\end{array}\right]=\left[\begin{array}{c}{\tau }_1\\ {\tau }_2\end{array}\right]$$

Here:

$\alpha ={\mathit{I}}_{\mathrm{z1}}+{\mathit{I}}_{\mathrm{z2}}+{\mathit{m}}_1{\mathit{r}}_1^2+{\mathit{m}}_2\left({\mathit{l}}_1^2+{\mathit{r}}_2^2\right)$

$\beta ={\mathit{m}}_2{\mathit{l}}_1{\mathit{r}}_2$

$\delta ={\mathit{I}}_{\mathrm{z2}}+{\mathit{m}}_2{\mathit{r}}_2^2$

This example uses the twoLinkInverseKinematics.m script to solve this inverse kinematics problem.

Define the physical parameters.

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

Desired Trajectory and Inverse Kinematics

The tip of the arm must follow a prescribed planar trajectory $\mathit{X}\left(\mathit{t}\right)$,$\mathit{Y}\left(\mathit{t}\right)$.

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')

Solve the inverse kinematics problem for joint angles, joint velocities, and torque inputs corresponding to the motion of the end point of link 2. Use joint angles to compute the trajectory of the tip of link 1.

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

Plot the joint angles and torques.

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;

Use a Simscape™ Multibody™ model of the two-link robot to simulate the robot response to the computed torques.

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

open_system('TwoLinkRobotSM')

Simulate the model, unpack the results, and verify that the tip follows the desired trajectory.

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

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

LTV Model

To build an LTV model of two-link robot along the desired trajectory, take linearization snapshots at 50 evenly distributed times between the start and end of the simulation. Use linearize to obtain linearized models at the corresponding operating conditions.

Specify linearization I/Os.

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

Take snapshot operating points.

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

Compute linearizations with offsets.

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

Use ssInterpolant to construct an LTV object from the array of linearized models and associated offsets. The resulting LTV model interpolates the linearized dynamics between the snapshot times tlin.

Gltv = ssInterpolant(G,info.Offsets);
Gltv.StateName

ans = 4x1 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'}

The states are ordered differently in the linearization. Use xperm to align the state order with the convention ${\theta }_1$, ${\theta }_2$, ${\dot{\theta }}_1$, ${\dot{\theta }}_2$.

Gltv = xperm(Gltv,[1 3 2 4]);
Gltv.StateName

ans = 4x1 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 Model Validation

Simulate the response of this LTV model for the same torque inputs and compare with the results from Simscape Multibody.

Simulate the LTV response to torque profiles.

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

Compare with Simscape results.

clf 
plot(tsim,xsim(:,1),'b',tref,xltv(:,1),'k--',...
   tsim,xsim(:,2),'r',tref,xltv(:,2),'k:','LineWidth',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')

Discretization

In embedded applications, you often want a discrete model that can be easily simulated. Using c2d, you can compute a Tustin discretization of Gltv and turn it into a discrete gridded LTV model.

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

Turn Gd into a gridded LTV model with 50 grid points.

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

The initial condition for the Tustin discretization is

${\mathit{z}}_0=\mathit{x}\left(0\right)-\frac{{\mathit{T}}_{\mathit{s}}}{2}\dot{\mathit{x}}\left(0\right)$.

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

Simulate and compare with Simscape results.

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')

References

[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.