In applied AMR design, we often need a model that:

• captures acceleration limits and actuator lag missing in kinematics,
• remains simple enough for real-time prediction and tuning,
• degrades gracefully when slip is small (slip modeling was introduced in Lesson 2).

We will use a planar pose \( (p_x, p_y, \theta) \) and body-frame velocities \( (v, \omega) \), giving the canonical “second-order unicycle” state \( \mathbf{x} = [p_x,\,p_y,\,\theta,\,v,\,\omega]^\top \). Two input parameterizations appear throughout AMR software stacks:

• Force/moment inputs: \( \mathbf{u} = [F,\,M]^\top \) (virtual inputs derived from wheel torques).
• Commanded velocities with lag: \( \mathbf{u} = [v_\text{cmd},\,\omega_\text{cmd}]^\top \), common when higher layers output desired twists.

flowchart TD
  A["Choose model purpose"] --> B["Pick state: (px, py, theta, v, w)"]
  B --> C["Pick inputs: (F, M) or (v_cmd, w_cmd)"]
  C --> D["Derive body dynamics + constraints"]
  D --> E["Compose with kinematics"]
  E --> F["Discretize for software (dt)"]
  F --> G["Simulate + tune parameters"]
        

Throughout, assume parameters satisfy \( m > 0 \) and \( I_z > 0 \) (mass and yaw inertia), and time constants satisfy \( T_v > 0 \), \( T_\omega > 0 \).

Let body-frame translational velocities be \( (v_x, v_y) \) and yaw rate be \( \omega \). Define body-frame force inputs \( (F_x, F_y) \) and yaw moment \( M_z \). Newton–Euler equations in the body frame can be written as:

\[ \begin{aligned} m\big(\dot v_x - \omega v_y\big) &= F_x,\\ m\big(\dot v_y + \omega v_x\big) &= F_y,\\ I_z \dot \omega &= M_z. \end{aligned} \]

For many wheeled bases on moderate terrain, a practical “no lateral velocity” rolling approximation is: \( v_y = 0 \). This is a nonholonomic constraint on velocities (introduced in Chapter 3), and the corresponding lateral force \( F_y \) becomes a constraint reaction, not an independent actuator.

2.1 Eliminating the Unknown Constraint Force

Enforce \( v_y = 0 \) for all time. Then \( \dot v_y = 0 \), and the second Newton equation yields:

\[ 0 + \omega v_x = \frac{F_y}{m} \quad \Longrightarrow \quad F_y = m\,\omega\,v_x. \]

Substituting the constraint into the first and third equations, the reduced dynamics become:

\[ m\dot v_x = F_x, \qquad I_z \dot \omega = M_z. \]

In other words, the constraint “absorbs” the lateral coupling into the unmodeled reaction \( F_y \), while the longitudinal and yaw channels stay low-order.

2.2 Kinematic Reconstruction of World-Frame Motion

Let \( \mathbf{p} = [p_x,\,p_y]^\top \). With \( v_y=0 \), the body-to-world mapping is:

\[ \dot{\mathbf{p}} = \begin{bmatrix} \cos\theta & -\sin\theta\\ \sin\theta & \cos\theta \end{bmatrix} \begin{bmatrix} v_x\\ 0 \end{bmatrix} = \begin{bmatrix} v_x\cos\theta\\ v_x\sin\theta \end{bmatrix}, \qquad \dot\theta = \omega. \]

Defining \( v \equiv v_x \), the state-space model (force/moment input) is:

\[ \begin{aligned} \dot p_x &= v\cos\theta,\\ \dot p_y &= v\sin\theta,\\ \dot \theta &= \omega,\\ \dot v &= \frac{1}{m}F,\\ \dot \omega &= \frac{1}{I_z}M. \end{aligned} \]

2.3 Energy and Dissipation (Stability Insight)

Add viscous losses \( c_v v \) and \( c_\omega \omega \) to represent drivetrain drag. Then \( m\dot v = F - c_v v \), \( I_z\dot\omega = M - c_\omega\omega \). Define kinetic energy storage:

\[ E(v,\omega) = \tfrac{1}{2}m v^2 + \tfrac{1}{2}I_z \omega^2. \]

Taking the derivative and substituting the dynamics gives the standard passivity balance:

\[ \dot E = m v\dot v + I_z\omega\dot\omega = v(F - c_v v) + \omega(M - c_\omega\omega) = vF + \omega M - c_v v^2 - c_\omega\omega^2. \]

Proof (Lyapunov argument): If the inputs are zero (\(F=0\), \(M=0\)) and \( c_v > 0 \), \( c_\omega > 0 \), then \( \dot E = -c_v v^2 - c_\omega\omega^2 \le 0 \), so energy is non-increasing. The largest invariant set where \( \dot E = 0 \) is \( v=0,\,\omega=0 \). Therefore, by LaSalle’s invariance principle, \( (v(t),\omega(t)) → (0,0) \).

For a differential-drive base (Chapter 2 kinematics), let wheel radius be \( R \) and half-wheelbase be \( b \) (track width is \( 2b \)). Let wheel torques be \( T_R \) and \( T_L \). Under the no-lateral-velocity approximation and negligible wheel inertia, the longitudinal traction force is approximately the sum of wheel longitudinal forces:

\[ F \approx \frac{T_R}{R} + \frac{T_L}{R} = \frac{T_R + T_L}{R}, \qquad M \approx b\left(\frac{T_R}{R} - \frac{T_L}{R}\right) = \frac{b}{R}(T_R - T_L). \]

If you include viscous damping in the body channels (as in Section 2.3), the full reduced dynamics become:

\[ \begin{aligned} \dot v &= \frac{1}{m}\left(\frac{T_R + T_L}{R} - c_v v\right),\\ \dot \omega &= \frac{1}{I_z}\left(\frac{b}{R}(T_R - T_L) - c_\omega \omega\right). \end{aligned} \]

flowchart TD
  TR["Right wheel torque TR"] --> MAP["Map to body inputs"]
  TL["Left wheel torque TL"] --> MAP
  MAP --> FM["F = (TR+TL)/R,  M = b(TR-TL)/R"]
  FM --> V["Velocity dynamics: v_dot, w_dot"]
  V --> KIN["Kinematics: x_dot = v cos(theta), y_dot = v sin(theta), theta_dot = w"]
  KIN --> INT["Integrator / simulator"]
        

This mapping is intentionally simple: it assumes the wheel/ground contact enforces rolling without substantial longitudinal slip. When longitudinal slip is significant, Lesson 2’s traction–slip relations should replace the algebraic force mapping.

Many AMR stacks output desired body velocities \( (v_\text{cmd},\omega_\text{cmd}) \) from planners or tracking controllers. A simple and widely used model is a first-order actuator lag:

\[ \dot v = \frac{1}{T_v}\big(v_\text{cmd} - v\big), \qquad \dot \omega = \frac{1}{T_\omega}\big(\omega_\text{cmd} - \omega\big), \qquad T_v, T_\omega > 0. \]

Combining with kinematics yields the “dynamic unicycle with lag”:

\[ \begin{aligned} \dot p_x &= v\cos\theta,\\ \dot p_y &= v\sin\theta,\\ \dot \theta &= \omega,\\ \dot v &= \frac{1}{T_v}\big(v_\text{cmd} - v\big),\\ \dot \omega &= \frac{1}{T_\omega}\big(\omega_\text{cmd} - \omega\big). \end{aligned} \]

Why this model is useful: it captures acceleration limits and bandwidth without requiring explicit motor electrical parameters. The time constants can be fitted from logged step responses: if \( v_\text{cmd} \) steps from 0 to \( \bar v \), then \( v(t) = \bar v(1 - e^{-t/T_v}) \).

4.1 Exponential Stability of the Velocity Subsystem

For constant commands, define errors \( e_v = v - v_\text{cmd} \), \( e_\omega = \omega - \omega_\text{cmd} \). Then:

\[ \dot e_v = -\frac{1}{T_v} e_v, \qquad \dot e_\omega = -\frac{1}{T_\omega} e_\omega. \]

Proof: Choose \( V = \tfrac{1}{2}e_v^2 + \tfrac{1}{2}e_\omega^2 \). Then \( \dot V = -\tfrac{1}{T_v}e_v^2 - \tfrac{1}{T_\omega}e_\omega^2 \le 0 \). Hence \( (e_v(t),e_\omega(t)) → (0,0) \) exponentially with rate at least \( \min\{1/T_v,\,1/T_\omega\} \).

For real-time prediction, we discretize at a sample time \( \Delta t > 0 \). For piecewise-constant commands on each interval, the first-order lag admits an exact discrete update. Consider the scalar subsystem:

\[ \dot z = \frac{1}{T}(z_\text{cmd} - z), \qquad T > 0. \]

Over one sample, with constant \( z_\text{cmd} \), the solution is:

\[ z((k+1)\Delta t) = e^{-\Delta t/T} z(k\Delta t) + \big(1 - e^{-\Delta t/T}\big) z_\text{cmd}(k). \]

Thus the implementable update is:

\[ z_{k+1} ← a z_k + (1-a) z_{\text{cmd},k}, \qquad a = e^{-\Delta t/T}. \]

The pose update typically uses numerical integration because the rotation \( \theta(t) \) couples into \( \dot p_x, \dot p_y \). Forward Euler is common in embedded firmware, while Runge–Kutta is common in simulation.

5.1 Linearization (Local Model for Analysis)

Linearize the lag-unicycle about a nominal trajectory with state \( \bar{\mathbf{x}}(t) \) and inputs \( \bar{\mathbf{u}}(t) \). The Jacobian for the kinematic part is:

\[ \frac{\partial}{\partial \theta} \begin{bmatrix} v\cos\theta\\ v\sin\theta \end{bmatrix} = \begin{bmatrix} -v\sin\theta\\ v\cos\theta \end{bmatrix}, \qquad \frac{\partial}{\partial v} \begin{bmatrix} v\cos\theta\\ v\sin\theta \end{bmatrix} = \begin{bmatrix} \cos\theta\\ \sin\theta \end{bmatrix}. \]

This local model is useful for sanity-checking prediction sensitivity and for designing observers later (Chapters 6–7).

The following implementations simulate the lag-unicycle model and (in Python) also include an optional torque-input differential-drive model. These are intentionally minimal and designed to be extended in later labs.

6.1 Python (NumPy + SciPy)

Robotics-relevant ecosystem notes: numpy/scipy for numerics and ODEs, matplotlib for visualization, sympy for symbolic Jacobians, and (later in the course) ROS 2 Python nodes via rclpy to connect models to real sensors/actuators.

File: Chapter4_Lesson3.py

# Chapter4_Lesson3.py
# Simple dynamic models for a ground robot: (i) velocity-lag unicycle, (ii) torque-input differential drive (optional).
# Dependencies: numpy, scipy, matplotlib (optional for plots)

from __future__ import annotations
from dataclasses import dataclass
from typing import Callable, Tuple
import numpy as np

try:
    from scipy.integrate import solve_ivp
except ImportError as e:
    raise ImportError("This script requires scipy. Install via: pip install scipy") from e


@dataclass(frozen=True)
class LagUnicycleParams:
    T_v: float = 0.25   # [s] longitudinal velocity time constant
    T_w: float = 0.20   # [s] yaw-rate time constant


def lag_unicycle_ode(t: float, x: np.ndarray,
                     v_ref: Callable[[float], float],
                     w_ref: Callable[[float], float],
                     p: LagUnicycleParams) -> np.ndarray:
    """State x = [px, py, theta, v, w]."""
    px, py, th, v, w = x
    dx = np.zeros_like(x)
    dx[0] = v * np.cos(th)
    dx[1] = v * np.sin(th)
    dx[2] = w
    dx[3] = (v_ref(t) - v) / p.T_v
    dx[4] = (w_ref(t) - w) / p.T_w
    return dx


def exact_first_order_discretization(z: float, z_ref: float, T: float, dt: float) -> float:
    """Exact discrete-time update for \dot z = (z_ref - z)/T with piecewise-constant z_ref on [k dt,(k+1) dt)."""
    a = np.exp(-dt / T)
    return a * z + (1.0 - a) * z_ref


# ---- Optional: torque-input differential-drive (very simplified) ----
@dataclass(frozen=True)
class DiffDriveTorqueParams:
    m: float = 25.0     # [kg]
    Iz: float = 1.2     # [kg m^2]
    R: float = 0.10     # [m] wheel radius
    b: float = 0.22     # [m] half wheelbase
    c_v: float = 3.5    # [N s/m] viscous longitudinal drag
    c_w: float = 0.8    # [N m s/rad] viscous yaw drag


def diffdrive_torque_ode(t: float, x: np.ndarray,
                         TR: Callable[[float], float],
                         TL: Callable[[float], float],
                         p: DiffDriveTorqueParams) -> np.ndarray:
    """State x = [px, py, theta, v, w]. Inputs are wheel torques TR, TL [N m]."""
    px, py, th, v, w = x
    # Map wheel torques to body force and yaw moment (no lateral slip; see lesson notes)
    F = (TR(t) + TL(t)) / p.R
    M = p.b * (TR(t) - TL(t)) / p.R

    dx = np.zeros_like(x)
    dx[0] = v * np.cos(th)
    dx[1] = v * np.sin(th)
    dx[2] = w
    dx[3] = (F - p.c_v * v) / p.m
    dx[4] = (M - p.c_w * w) / p.Iz
    return dx


def simulate_lag_unicycle(t0: float, tf: float, x0: np.ndarray,
                          v_ref: Callable[[float], float],
                          w_ref: Callable[[float], float],
                          p: LagUnicycleParams,
                          dt_out: float = 0.01) -> Tuple[np.ndarray, np.ndarray]:
    t_eval = np.arange(t0, tf + dt_out, dt_out)
    sol = solve_ivp(lambda t, x: lag_unicycle_ode(t, x, v_ref, w_ref, p),
                    t_span=(t0, tf), y0=x0, t_eval=t_eval, rtol=1e-8, atol=1e-10)
    if not sol.success:
        raise RuntimeError(sol.message)
    return sol.t, sol.y.T


def main() -> None:
    # Step commands: accelerate to 1.0 m/s, turn at 0.7 rad/s after 2 seconds
    def v_ref(t: float) -> float:
        return 1.0 if t >= 0.5 else 0.0

    def w_ref(t: float) -> float:
        return 0.7 if t >= 2.0 else 0.0

    p = LagUnicycleParams(T_v=0.30, T_w=0.25)
    x0 = np.array([0.0, 0.0, 0.0, 0.0, 0.0])

    t, X = simulate_lag_unicycle(0.0, 8.0, x0, v_ref, w_ref, p, dt_out=0.01)

    # Print final state
    px, py, th, v, w = X[-1]
    print(f"Final pose: px={px:.3f} m, py={py:.3f} m, theta={th:.3f} rad")
    print(f"Final body velocities: v={v:.3f} m/s, w={w:.3f} rad/s")

    # Optional plot
    try:
        import matplotlib.pyplot as plt
        plt.figure()
        plt.plot(X[:, 0], X[:, 1])
        plt.xlabel("px [m]")
        plt.ylabel("py [m]")
        plt.title("Trajectory (lag-unicycle)")
        plt.axis("equal")

        plt.figure()
        plt.plot(t, X[:, 3], label="v")
        plt.plot(t, X[:, 4], label="w")
        plt.xlabel("t [s]")
        plt.ylabel("v, w")
        plt.legend()
        plt.title("Body velocities")
        plt.show()
    except Exception:
        pass


if __name__ == "__main__":
    main()
      

6.2 C++ (Eigen + RK4)

Robotics-relevant ecosystem notes: linear algebra with Eigen, ODE integration with Boost odeint (optional), and later integration into ROS 2 with rclcpp.

File: Chapter4_Lesson3.cpp

// Chapter4_Lesson3.cpp
// Simple dynamic model for a ground robot: velocity-lag unicycle with RK4 integration.
// Dependencies (recommended): Eigen (header-only). If Eigen is unavailable, replace VectorXd with std::array.
// Build example (Linux):
//   g++ -O2 -std=c++17 Chapter4_Lesson3.cpp -I /usr/include/eigen3 -o amr_dyn

#include <iostream>
#include <cmath>
#include <Eigen/Dense>

struct Params {
  double T_v = 0.30;
  double T_w = 0.25;
};

static double v_ref(double t) { return (t >= 0.5) ? 1.0 : 0.0; }
static double w_ref(double t) { return (t >= 2.0) ? 0.7 : 0.0; }

Eigen::VectorXd f(double t, const Eigen::VectorXd& x, const Params& p) {
  // x = [px, py, theta, v, w]
  Eigen::VectorXd dx(5);
  const double px = x(0), py = x(1), th = x(2), v = x(3), w = x(4);
  dx(0) = v * std::cos(th);
  dx(1) = v * std::sin(th);
  dx(2) = w;
  dx(3) = (v_ref(t) - v) / p.T_v;
  dx(4) = (w_ref(t) - w) / p.T_w;
  return dx;
}

Eigen::VectorXd rk4_step(double t, double h, const Eigen::VectorXd& x, const Params& p) {
  const auto k1 = f(t, x, p);
  const auto k2 = f(t + 0.5*h, x + 0.5*h*k1, p);
  const auto k3 = f(t + 0.5*h, x + 0.5*h*k2, p);
  const auto k4 = f(t + h, x + h*k3, p);
  return x + (h/6.0)*(k1 + 2.0*k2 + 2.0*k3 + k4);
}

int main() {
  Params p;
  Eigen::VectorXd x(5);
  x << 0.0, 0.0, 0.0, 0.0, 0.0;

  const double t0 = 0.0, tf = 8.0, h = 0.001;
  double t = t0;

  while (t < tf) {
    x = rk4_step(t, h, x, p);
    t += h;
  }

  std::cout << "Final pose: px=" << x(0) << " py=" << x(1)
            << " theta=" << x(2) << "\n";
  std::cout << "Final body velocities: v=" << x(3) << " w=" << x(4) << "\n";
  return 0;
}
      

6.3 Java (pure Java RK4; optional EJML)

Robotics-relevant ecosystem notes: matrix algebra via EJML (optional), numerical utilities via Apache Commons Math (optional), and ROS integration via ROSJava (where applicable).

File: Chapter4_Lesson3.java

// Chapter4_Lesson3.java
// Simple dynamic model for a ground robot: velocity-lag unicycle with RK4 integration.
// Pure-Java implementation (no external dependencies). If you prefer a robotics math library,
// you can replace the small vector ops with EJML (org.ejml) or Apache Commons Math.

public class Chapter4_Lesson3 {

    static class Params {
        double T_v = 0.30;
        double T_w = 0.25;
    }

    static double vRef(double t) { return (t >= 0.5) ? 1.0 : 0.0; }
    static double wRef(double t) { return (t >= 2.0) ? 0.7 : 0.0; }

    // x = [px, py, theta, v, w]
    static double[] f(double t, double[] x, Params p) {
        double px = x[0], py = x[1], th = x[2], v = x[3], w = x[4];
        return new double[] {
            v * Math.cos(th),
            v * Math.sin(th),
            w,
            (vRef(t) - v) / p.T_v,
            (wRef(t) - w) / p.T_w
        };
    }

    static double[] add(double[] a, double[] b, double s) {
        double[] out = new double[a.length];
        for (int i = 0; i < a.length; i++) out[i] = a[i] + s * b[i];
        return out;
    }

    static double[] rk4Step(double t, double h, double[] x, Params p) {
        double[] k1 = f(t, x, p);
        double[] k2 = f(t + 0.5*h, add(x, k1, 0.5*h), p);
        double[] k3 = f(t + 0.5*h, add(x, k2, 0.5*h), p);
        double[] k4 = f(t + h, add(x, k3, h), p);
        double[] out = new double[x.length];
        for (int i = 0; i < x.length; i++) {
            out[i] = x[i] + (h/6.0)*(k1[i] + 2.0*k2[i] + 2.0*k3[i] + k4[i]);
        }
        return out;
    }

    public static void main(String[] args) {
        Params p = new Params();
        double[] x = new double[] {0,0,0,0,0};

        double t0 = 0.0, tf = 8.0, h = 0.001;
        double t = t0;
        while (t < tf) {
            x = rk4Step(t, h, x, p);
            t += h;
        }

        System.out.printf("Final pose: px=%.3f py=%.3f theta=%.3f%n", x[0], x[1], x[2]);
        System.out.printf("Final body velocities: v=%.3f w=%.3f%n", x[3], x[4]);
    }
}
      

6.4 MATLAB / Simulink

Robotics-relevant ecosystem notes: rapid prototyping with ODE solvers, parameter sweeps, and block-diagram simulation in Simulink. If you have Robotics System Toolbox, you can later connect to ROS and log real robot data.

File: Chapter4_Lesson3.m

% Chapter4_Lesson3.m
% Simple dynamic model for a ground robot: velocity-lag unicycle.
% Includes: (1) ODE45 simulation, (2) exact discrete-time update for first-order lag,
% (3) programmatic Simulink model builder (optional).
%
% Requires: MATLAB base. For the Simulink model builder, Simulink must be installed.

clear; clc;

% Parameters
T_v = 0.30;   % [s]
T_w = 0.25;   % [s]

% Step commands
v_ref = @(t) (t >= 0.5) * 1.0;
w_ref = @(t) (t >= 2.0) * 0.7;

% ODE definition: x = [px; py; theta; v; w]
f = @(t,x) [
  x(4)*cos(x(3));
  x(4)*sin(x(3));
  x(5);
  (v_ref(t) - x(4))/T_v;
  (w_ref(t) - x(5))/T_w
];

tspan = [0 8];
x0 = [0;0;0;0;0];

opts = odeset('RelTol',1e-8,'AbsTol',1e-10);
[t,X] = ode45(f, tspan, x0, opts);

fprintf('Final pose: px=%.3f m, py=%.3f m, theta=%.3f rad\n', X(end,1), X(end,2), X(end,3));
fprintf('Final body velocities: v=%.3f m/s, w=%.3f rad/s\n', X(end,4), X(end,5));

% Plot (optional)
figure; plot(X(:,1), X(:,2), 'LineWidth', 1.5); axis equal; grid on;
xlabel('px [m]'); ylabel('py [m]'); title('Trajectory (lag-unicycle)');

figure; plot(t, X(:,4), 'LineWidth', 1.5); hold on; plot(t, X(:,5), 'LineWidth', 1.5);
grid on; xlabel('t [s]'); ylabel('v, w'); legend('v','w'); title('Body velocities');

% Exact discrete-time update for zdot = (z_ref - z)/T with sample time dt
exactUpdate = @(z, zref, T, dt) exp(-dt/T)*z + (1 - exp(-dt/T))*zref;

% Demonstration of discrete update for v only
dt = 0.05;
v = 0;
for k = 1:round(8/dt)
    tk = (k-1)*dt;
    v = exactUpdate(v, v_ref(tk), T_v, dt);
end
fprintf('Discrete-time v at t=8s (piecewise-constant): %.3f\n', v);

% --- Optional: build a Simulink model programmatically ---
% The model implements:
%   vdot = (v_ref - v)/T_v,  wdot = (w_ref - w)/T_w
%   xdot = v cos(theta), ydot = v sin(theta), thetadot = w

buildSimulink = false;  % set true to build model
if buildSimulink
    mdl = 'Chapter4_Lesson3_Simulink';
    if bdIsLoaded(mdl), close_system(mdl, 0); end
    new_system(mdl); open_system(mdl);

    % Add blocks (Integrator, Trig, Product, Sum, Gain, Step)
    add_block('simulink/Sources/Step', [mdl '/v_ref'], 'Position',[50 60 80 90], ...
              'Time','0.5','Before','0','After','1');
    add_block('simulink/Sources/Step', [mdl '/w_ref'], 'Position',[50 140 80 170], ...
              'Time','2','Before','0','After','0.7');

    add_block('simulink/Math Operations/Sum', [mdl '/Sum_v'], 'Inputs','+-', 'Position',[140 60 160 90]);
    add_block('simulink/Math Operations/Gain', [mdl '/Gain_v'], 'Gain',sprintf('1/%g',T_v), 'Position',[190 60 240 90]);
    add_block('simulink/Continuous/Integrator', [mdl '/Int_v'], 'Position',[270 60 300 90]);

    add_block('simulink/Math Operations/Sum', [mdl '/Sum_w'], 'Inputs','+-', 'Position',[140 140 160 170]);
    add_block('simulink/Math Operations/Gain', [mdl '/Gain_w'], 'Gain',sprintf('1/%g',T_w), 'Position',[190 140 240 170]);
    add_block('simulink/Continuous/Integrator', [mdl '/Int_w'], 'Position',[270 140 300 170]);

    add_block('simulink/Continuous/Integrator', [mdl '/Int_theta'], 'Position',[360 140 390 170]);

    add_block('simulink/Math Operations/Trigonometric Function', [mdl '/cos'], 'Operator','cos', 'Position',[420 60 460 90]);
    add_block('simulink/Math Operations/Trigonometric Function', [mdl '/sin'], 'Operator','sin', 'Position',[420 100 460 130]);

    add_block('simulink/Math Operations/Product', [mdl '/Prod_xdot'], 'Position',[500 60 530 90]);
    add_block('simulink/Math Operations/Product', [mdl '/Prod_ydot'], 'Position',[500 100 530 130]);

    add_block('simulink/Continuous/Integrator', [mdl '/Int_x'], 'Position',[560 60 590 90]);
    add_block('simulink/Continuous/Integrator', [mdl '/Int_y'], 'Position',[560 100 590 130]);

    % Wiring
    add_line(mdl,'v_ref/1','Sum_v/1');
    add_line(mdl,'Int_v/1','Sum_v/2');
    add_line(mdl,'Sum_v/1','Gain_v/1');
    add_line(mdl,'Gain_v/1','Int_v/1');

    add_line(mdl,'w_ref/1','Sum_w/1');
    add_line(mdl,'Int_w/1','Sum_w/2');
    add_line(mdl,'Sum_w/1','Gain_w/1');
    add_line(mdl,'Gain_w/1','Int_w/1');

    add_line(mdl,'Int_w/1','Int_theta/1');

    add_line(mdl,'Int_theta/1','cos/1');
    add_line(mdl,'Int_theta/1','sin/1');

    add_line(mdl,'Int_v/1','Prod_xdot/1');
    add_line(mdl,'cos/1','Prod_xdot/2');
    add_line(mdl,'Int_v/1','Prod_ydot/1');
    add_line(mdl,'sin/1','Prod_ydot/2');

    add_line(mdl,'Prod_xdot/1','Int_x/1');
    add_line(mdl,'Prod_ydot/1','Int_y/1');

    set_param(mdl,'StopTime','8');
    save_system(mdl);
    fprintf('Built and saved Simulink model: %s.slx\n', mdl);
end
      

6.5 Wolfram Mathematica

Mathematica is especially useful for symbolic manipulation (deriving Jacobians and checking stability) and high-accuracy ODE solving.

File: Chapter4_Lesson3.nb

(* Chapter4_Lesson3.nb *)
Notebook[{
  Cell["Chapter 4 - Lesson 3: Simple Dynamic Models for Ground Robots", "Title"],
  Cell["Velocity-lag unicycle model simulated with NDSolve.", "Text"],
  Cell[BoxData@ToBoxes@Module[
    {Tv=0.30, Tw=0.25, vRef, wRef, sol, x, y, th, v, w, t},
    vRef[t_] := If[t >= 0.5, 1.0, 0.0];
    wRef[t_] := If[t >= 2.0, 0.7, 0.0];

    sol = NDSolve[
      {
        x'[t] == v[t] Cos[th[t]],
        y'[t] == v[t] Sin[th[t]],
        th'[t] == w[t],
        v'[t] == (vRef[t] - v[t])/Tv,
        w'[t] == (wRef[t] - w[t])/Tw,
        x[0] == 0, y[0] == 0, th[0] == 0, v[0] == 0, w[0] == 0
      },
      {x,y,th,v,w}, {t,0,8},
      AccuracyGoal->12, PrecisionGoal->12, MaxStepFraction->1/2000
    ];

    Print["Final pose (x,y,theta): ",
      {x[8], y[8], th[8]} /. sol[[1]] // N];
    Print["Final body velocities (v,w): ",
      {v[8], w[8]} /. sol[[1]] // N];

    ParametricPlot[
      Evaluate[{x[t], y[t]} /. sol],
      {t,0,8}, AxesLabel->{"x","y"}, PlotLabel->"Trajectory (lag-unicycle)", AspectRatio->1
    ]
  ]], "Input"]
}];
      

Problem 1 (Constraint elimination): Starting from the body-frame Newton–Euler model \( m(\dot v_x - \omega v_y)=F_x \), \( m(\dot v_y + \omega v_x)=F_y \), \( I_z \dot\omega = M_z \), impose the rolling approximation \( v_y=0 \). Derive (i) the reduced dynamics for \( v_x \) and \( \omega \), and (ii) the required constraint force \( F_y \).

Solution:

Since \( v_y=0 \) for all time, we also have \( \dot v_y=0 \). The second Newton equation gives:

\[ m(0 + \omega v_x) = F_y \quad \Longrightarrow \quad F_y = m\omega v_x. \]

Substituting \( v_y=0 \) into the first and third equations yields:

\[ m\dot v_x = F_x, \qquad I_z\dot\omega = M_z. \]

Problem 2 (Energy dissipation): Consider the damped reduced dynamics \( m\dot v = F - c_v v \), \( I_z\dot\omega = M - c_\omega\omega \), with \( c_v > 0 \), \( c_\omega > 0 \). Show that the storage \( E = \tfrac{1}{2}mv^2 + \tfrac{1}{2}I_z\omega^2 \) satisfies \( \dot E \le vF + \omega M \) and conclude stability for \(F=0\), \(M=0\).

Solution:

\[ \dot E = mv\dot v + I_z\omega\dot\omega = v(F - c_v v) + \omega(M - c_\omega\omega) = vF + \omega M - c_v v^2 - c_\omega\omega^2 \le vF + \omega M. \]

With \(F=0\), \(M=0\), we obtain \( \dot E = -c_v v^2 - c_\omega\omega^2 \le 0 \), implying \( (v,\omega) \) is stable and converges to \( (0,0) \) (LaSalle’s invariance principle).

Problem 3 (Exact discretization of first-order lag): For \( \dot z = \tfrac{1}{T}(z_\text{cmd} - z) \), assume \( z_\text{cmd} \) is constant on \( [k\Delta t,(k+1)\Delta t) \). Derive \( z_{k+1} \) as a function of \( z_k \), \( z_{\text{cmd},k} \), \(T\), and \(\Delta t\).

Solution:

Rewrite as \( \dot z + \tfrac{1}{T}z = \tfrac{1}{T} z_\text{cmd} \). Multiply by the integrating factor \( e^{t/T} \) and integrate over one sample:

\[ \frac{d}{dt}\big(e^{t/T}z(t)\big) = \frac{1}{T} e^{t/T} z_\text{cmd} \quad \Longrightarrow \quad z_{k+1} = e^{-\Delta t/T} z_k + \big(1 - e^{-\Delta t/T}\big) z_{\text{cmd},k}. \]

Problem 4 (Local linearization around straight motion): For the lag-unicycle model, linearize about a straight-line operating point with constant \( \bar\omega=0 \), constant \( \bar v > 0 \), and heading \( \bar\theta \). Provide the Jacobian entries for the pose subsystem \( (p_x,p_y,\theta) \) with respect to \( (\theta,v,\omega) \).

Solution:

The pose dynamics are \( \dot p_x = v\cos\theta \), \( \dot p_y = v\sin\theta \), \( \dot\theta=\omega \). Hence:

\[ \begin{aligned} \frac{\partial \dot p_x}{\partial \theta} &= -v\sin\theta, &\quad \frac{\partial \dot p_x}{\partial v} &= \cos\theta, &\quad \frac{\partial \dot p_x}{\partial \omega} &= 0,\\ \frac{\partial \dot p_y}{\partial \theta} &= v\cos\theta, &\quad \frac{\partial \dot p_y}{\partial v} &= \sin\theta, &\quad \frac{\partial \dot p_y}{\partial \omega} &= 0,\\ \frac{\partial \dot\theta}{\partial \theta} &= 0, &\quad \frac{\partial \dot\theta}{\partial v} &= 0, &\quad \frac{\partial \dot\theta}{\partial \omega} &= 1. \end{aligned} \]

Evaluate at the operating point by substituting \( v=\bar v \), \( \theta=\bar\theta \).

Problem 5 (Stopping distance under lag dynamics): Suppose the command is set to zero at \( t=0 \) and the longitudinal lag dynamics are \( \dot v = -\tfrac{1}{T_v}v \) with \( v(0)=v_0 \), \( T_v > 0 \). Assuming heading is constant during braking, compute the distance traveled until speed drops to \( \varepsilon \), where \( 0 < \varepsilon < v_0 \).

Solution:

The velocity is \( v(t)=v_0 e^{-t/T_v} \). The time to reach \( v(t_\varepsilon)=\varepsilon \) is:

\[ \varepsilon = v_0 e^{-t_\varepsilon/T_v} \quad \Longrightarrow \quad t_\varepsilon = T_v\ln\left(\frac{v_0}{\varepsilon}\right). \]

The traveled distance is \( s = \int_0^{t_\varepsilon} v(t)\,dt \):

\[ s = \int_0^{t_\varepsilon} v_0 e^{-t/T_v} dt = v_0 T_v \left(1 - e^{-t_\varepsilon/T_v}\right) = v_0 T_v\left(1 - \frac{\varepsilon}{v_0}\right) = T_v(v_0 - \varepsilon). \]

We derived compact planar dynamic models for wheeled ground robots by combining rigid-body Newton–Euler equations with the no-lateral-velocity rolling constraint. This produced low-order longitudinal and yaw dynamics that are easy to tune and simulate. We then connected wheel torques to body inputs for differential drive, introduced the commanded-velocity lag model common in AMR software, and presented an exact discrete-time update for the lag states. These models are the primary “workhorse” dynamics used for prediction and tuning before adding richer contact effects.