From Chapters 1–3, AMR motion feasibility was primarily discussed through \( (x,y,\theta) \) evolution under nonholonomic constraints. In many AMRs, velocity commands are tracked by a low-level motor controller, and an outer autonomy stack treats \( (v,\omega) \) as directly commandable. This is a modeling choice, not a truth.

Kinematic unicycle model (outer-loop abstraction):

\[ \dot{x} = v\cos\theta,\qquad \dot{y} = v\sin\theta,\qquad \dot{\theta} = \omega. \]

A dynamic model becomes necessary when the autonomy layer must respect acceleration, torque, or force limits (or when the inner velocity loop is not “fast enough”). A minimal planar dynamic augmentation is:

\[ \dot{x} = v\cos\theta,\quad \dot{y} = v\sin\theta,\quad \dot{\theta} = \omega,\quad m\dot{v} = F_x - c_v v,\quad I_z\dot{\omega} = M_z - c_\omega \omega. \]

Here \( m \) is mass and \( I_z \) is yaw inertia; the terms \( c_v v \) and \( c_\omega\omega \) lump rolling/aerodynamic drag and yaw damping.

flowchart TD
  A["Start: choose motion model"] --> B["Are commanded accelerations small relative to limits?"]
  B -->|"yes"| C["Is inner velocity loop fast \n(bandwidth >> outer loop)?"]
  B -->|"no"| D["Use dynamics: \ninclude v_dot, w_dot, torque/force limits"]
  C -->|"yes"| E["Kinematic model is adequate \nfor planning + tracking"]
  C -->|"no"| D
  D --> F["Add actuator limits, acceleration bounds, \nand inertia effects"]
  E --> G["Validate with data: \ntracking error vs speed/turn rate"]
  F --> G
        

The rest of this lesson makes “fast” and “small” mathematically explicit, and derives acceleration feasibility constraints from actuator limits (without yet modeling slip).

A common AMR architecture is: autonomy outputs \( (v_c,\omega_c) \), a low-level controller tracks these setpoints, and the body pose integrates the achieved \( (v,\omega) \). A widely used first-order abstraction is:

\[ \dot{x} = v\cos\theta,\quad \dot{y}=v\sin\theta,\quad \dot{\theta}=\omega, \qquad \dot{v}=\frac{1}{\tau_v}(v_c - v),\quad \dot{\omega}=\frac{1}{\tau_\omega}(\omega_c - \omega), \]

where \( \tau_v \) and \( \tau_\omega \) are effective closed-loop time constants of the velocity/yaw-rate servos.

2.1 Proposition (Exact solution and tracking bound)

Assume \( v_c(t) \) is continuously differentiable and bounded. The linear ODE for \( v \) has the variation-of-constants solution:

\[ v(t)=e^{-t/\tau_v}v(0)+\frac{1}{\tau_v}\int_{0}^{t} e^{-(t-s)/\tau_v} v_c(s)\,ds. \]

Define the tracking error \( e_v(t)=v(t)-v_c(t) \). Differentiating yields:

\[ \dot{e}_v = -\frac{1}{\tau_v}e_v - \dot{v}_c(t). \]

Bound (Grönwall-type): for all \( t \ge 0 \),

\[ |e_v(t)| \le e^{-t/\tau_v}|e_v(0)| + \int_{0}^{t} e^{-(t-s)/\tau_v}|\dot{v}_c(s)|\,ds. \]

2.2 Theorem (Kinematic limit on finite horizons)

Let \( T \) be an outer-loop time horizon and define the “command rate” bound \( \|\dot{v}_c\|_\infty = \sup_{0\le s \le T}|\dot{v}_c(s)| \). Then:

\[ \sup_{0\le t \le T}|v(t)-v_c(t)| \le |e_v(0)| + \tau_v \|\dot{v}_c\|_\infty. \]

Proof: Use the bound in Section 2.1. Since \( \int_{0}^{t} e^{-(t-s)/\tau_v}ds \le \tau_v \), we obtain \( |e_v(t)| \le |e_v(0)| + \tau_v\|\dot{v}_c\|_\infty \). Taking the supremum over \( [0,T] \) proves the claim. The same argument holds for \( \omega \) with \( \tau_\omega \). ■

Interpretation: kinematics is accurate when \( \tau_v \|\dot{v}_c\|_\infty \) and \( \tau_\omega \|\dot{\omega}_c\|_\infty \) are small compared to the velocity magnitudes. Equivalently, define an outer-loop time scale \( T_o \) and the dimensionless ratio \( \varepsilon_v = \tau_v/T_o \). If \( \varepsilon_v \ll 1 \), then \( v \approx v_c \) and the kinematic model is a justified reduction.

Without yet modeling slip, we can still derive a useful actuator-limited dynamic model. Let the two wheels apply torques \( \tau_R, \tau_L \) through wheel radius \( r_w \) and track width \( b \). Under ideal rolling, longitudinal wheel forces are approximately \( F_R \approx \tau_R/r_w \) and \( F_L \approx \tau_L/r_w \).

Resulting net body wrench:

\[ F_x = \frac{1}{r_w}(\tau_R + \tau_L),\qquad M_z = \frac{b}{2r_w}(\tau_R - \tau_L). \]

Substituting into the planar dynamics (Section 1) yields:

\[ \dot{v}=\frac{1}{m r_w}(\tau_R+\tau_L)-\frac{c_v}{m}v,\qquad \dot{\omega}=\frac{b}{2I_z r_w}(\tau_R-\tau_L)-\frac{c_\omega}{I_z}\omega. \]

3.1 Linear mapping and feasibility polytope

Define the input vector \( u=[\tau_R\;\; \tau_L]^\top \) and the “acceleration” vector (ignoring drag terms for the moment) \( a=[\dot{v}\;\; \dot{\omega}]^\top \). Then:

\[ a = A u,\qquad A=\begin{bmatrix} \frac{1}{m r_w} & \frac{1}{m r_w}\\[4pt] \frac{b}{2I_z r_w} & -\frac{b}{2I_z r_w} \end{bmatrix}. \]

If each wheel torque is bounded \( |\tau_R|\le \tau_{max} \), \( |\tau_L|\le \tau_{max} \), then the admissible set in torque space is a rectangle, and its image under the linear map \( A \) is a convex polygon (a parallelogram) in \( (\dot{v},\dot{\omega}) \).

Proposition (Convexity): the feasible acceleration set \( \mathcal{A}=\{Au: u \in [-\tau_{max},\tau_{max}]^2\} \) is convex. Proof: the torque box is convex and linear maps preserve convexity. ■

This is the first key reason dynamics matters: a planner/controller that requests \( (v,\omega) \) changes must respect that \( (\dot{v},\dot{\omega}) \) lies in \( \mathcal{A} \).

4.1 Stopping distance constraint (longitudinal)

Suppose an AMR must be able to brake to a stop before a hazard at distance \( d \). If the maximum achievable deceleration magnitude is \( a_{brk} \), then (constant decel idealization):

\[ 0 = v^2 - 2a_{brk}d \qquad →\qquad d_{stop} = \frac{v^2}{2a_{brk}}. \]

Therefore a purely kinematic planner that chooses speed solely from geometric curvature can violate safety if it ignores \( d_{stop} \).

4.2 Curvature vs lateral acceleration (turning at speed)

Even before slip is modeled, it is standard to enforce an operational lateral-acceleration envelope \( |a_{lat}|\le a_{lat,max} \) (often determined empirically). For a planar motion with curvature \( \kappa \) and speed \( v \), the centripetal requirement is:

\[ a_{lat} = v^2 \kappa \qquad →\qquad |v| \le \sqrt{\frac{a_{lat,max}}{|\kappa|}}. \]

4.3 Actuator-limited “command slew”

From Section 2, if the autonomy layer commands \( v_c(t) \) with large derivative \( |\dot{v}_c| \), then the inner loop produces lag of order \( \tau_v|\dot{v}_c| \). A practical sufficient condition for “near-kinematic” operation is:

\[ \tau_v \|\dot{v}_c\|_\infty \le \eta_v V,\qquad \tau_\omega \|\dot{\omega}_c\|_\infty \le \eta_\omega \Omega, \]

where \( V \) and \( \Omega \) are typical magnitudes and \( \eta_v,\eta_\omega \in (0,1) \) are tolerable relative lag levels (e.g., 5–10%).

For this chapter, we use a modeling ladder that stays consistent with the prerequisite sequence:

1. Kinematic: pose evolves with commanded \( (v,\omega) \).
2. Kinematic + actuator lag: first-order closed-loop dynamics for \( v,\omega \) (Section 2).
3. Rigid-body planar dynamics + actuator bounds: torque/force limited \( (\dot{v},\dot{\omega}) \) (Section 3).
4. Wheel–terrain interaction (slip, skid, deformation): deferred to Lesson 2.

This ladder is important because it prevents premature complexity while still capturing the dominant failure modes: saturation, stopping distance violations, and control lag.

The autonomy stack from Chapter 1 can be reinterpreted as a cascade of models. Dynamics matters whenever the layer that produces commands assumes a plant that is “more responsive” than reality.

flowchart TD
  P["Planner: path + desired speed profile"] --> C["Command generator: v_c(t), w_c(t)"]
  C --> V["Velocity servo (finite bandwidth)"]
  V --> A["Actuators (torque limits)"]
  A --> R["Robot body: v,w -> pose"]
  R --> S["Sensors"]
  S --> FB["Feedback to estimation/control"]
  A --> D["Disturbances (slope, drag, payload)"]
  D --> R
        

In this lesson, the emphasis is not on designing the full controller, but on identifying when the planner must incorporate dynamic feasibility (acceleration/torque/stop constraints) rather than assuming instantaneous tracking of kinematic commands.

The following implementations simulate two models side-by-side: (i) ideal kinematic unicycle driven by \( (v_c,\omega_c) \), and (ii) a torque-limited dynamic model for a differential-drive base producing \( (v,\omega) \).

7.1 Python (NumPy, Matplotlib)

File: Chapter4_Lesson1.py

"""
Chapter4_Lesson1.py
Autonomous Mobile Robots - Chapter 4 (Mobile Robot Dynamics Applied)
Lesson 1: When Dynamics Matter for AMR
"""
import numpy as np
import matplotlib.pyplot as plt

def rk4_step(f, t, x, dt, u):
    k1 = f(t, x, u)
    k2 = f(t + 0.5*dt, x + 0.5*dt*k1, u)
    k3 = f(t + 0.5*dt, x + 0.5*dt*k2, u)
    k4 = f(t + dt, x + dt*k3, u)
    return x + (dt/6.0)*(k1 + 2*k2 + 2*k3 + k4)

def wrap_pi(a):
    return (a + np.pi) % (2*np.pi) - np.pi

def sat(x, lo, hi):
    return np.minimum(np.maximum(x, lo), hi)

def kin_unicycle_rhs(t, x, u):
    px, py, th = x
    v_cmd, w_cmd = u
    return np.array([v_cmd*np.cos(th), v_cmd*np.sin(th), w_cmd], dtype=float)

def dyn_diffdrive_rhs_factory(params):
    m   = params["m"]
    Iz  = params["Iz"]
    rw  = params["rw"]
    b   = params["b"]
    bv  = params["bv"]
    bw  = params["bw"]
    tau_max = params["tau_max"]
    kv = params["kv"]
    kw = params["kw"]

    def rhs(t, x, u):
        px, py, th, v, w = x
        v_ref, w_ref = u

        tau_sum  = kv*(v_ref - v)
        tau_diff = kw*(w_ref - w)

        tau_R = 0.5*(tau_sum + tau_diff)
        tau_L = 0.5*(tau_sum - tau_diff)

        tau_R = float(sat(tau_R, -tau_max, tau_max))
        tau_L = float(sat(tau_L, -tau_max, tau_max))

        Fx = (tau_R + tau_L)/rw
        Mz = (b/(2.0*rw))*(tau_R - tau_L)

        v_dot = (Fx - bv*v)/m
        w_dot = (Mz - bw*w)/Iz

        return np.array([v*np.cos(th), v*np.sin(th), w, v_dot, w_dot], dtype=float)

    return rhs

def cmd_profile(t):
    if t <= 5.0:
        v_cmd = 0.2 + 0.18*t
        w_cmd = 0.6
    else:
        v_cmd = 1.1
        w_cmd = 1.5
    return np.array([v_cmd, w_cmd], dtype=float)

def main():
    dt = 0.002
    T  = 10.0
    ts = np.arange(0.0, T + dt, dt)

    xk = np.array([0.0, 0.0, 0.0], dtype=float)

    params = dict(m=30.0, Iz=1.2, rw=0.10, b=0.50, bv=6.0, bw=0.25, tau_max=3.0, kv=8.0, kw=1.2)
    xd = np.array([0.0, 0.0, 0.0, 0.0, 0.0], dtype=float)
    rhs_dyn = dyn_diffdrive_rhs_factory(params)

    Xk = np.zeros((ts.size, 3))
    Xd = np.zeros((ts.size, 5))
    U  = np.zeros((ts.size, 2))

    for i, t in enumerate(ts):
        u = cmd_profile(t)
        U[i, :] = u
        Xk[i, :] = xk
        Xd[i, :] = xd
        xk = rk4_step(kin_unicycle_rhs, t, xk, dt, u); xk[2] = wrap_pi(xk[2])
        xd = rk4_step(rhs_dyn, t, xd, dt, u);         xd[2] = wrap_pi(xd[2])

    plt.figure()
    plt.plot(Xk[:, 0], Xk[:, 1], label="Kinematic (ideal v,w)")
    plt.plot(Xd[:, 0], Xd[:, 1], label="Dynamic (torque-limited)")
    plt.axis("equal"); plt.grid(True); plt.legend()
    plt.xlabel("x [m]"); plt.ylabel("y [m]")
    plt.title("Trajectory: kinematic vs dynamic")

    plt.figure()
    plt.plot(ts, U[:, 0], label="v_cmd")
    plt.plot(ts, Xd[:, 3], label="v (dynamic)")
    plt.grid(True); plt.legend()
    plt.xlabel("t [s]"); plt.ylabel("v [m/s]")
    plt.title("Linear speed tracking")

    plt.figure()
    plt.plot(ts, U[:, 1], label="w_cmd")
    plt.plot(ts, Xd[:, 4], label="w (dynamic)")
    plt.grid(True); plt.legend()
    plt.xlabel("t [s]"); plt.ylabel("w [rad/s]")
    plt.title("Yaw-rate tracking")

    plt.show()

if __name__ == "__main__":
    main()
      

Relevant Python robotics libraries: NumPy/SciPy for simulation; SymPy for symbolic checks; ROS 2 (rclpy) for integration; physics simulators such as PyBullet for plant realism (optional).

7.2 C++ (RK4, CSV output)

File: Chapter4_Lesson1.cpp

/*
Chapter4_Lesson1.cpp
Autonomous Mobile Robots - Chapter 4 (Mobile Robot Dynamics Applied)
Lesson 1: When Dynamics Matter for AMR
*/
#include <iostream>
#include <array>
#include <cmath>
#include <algorithm>

static inline double wrap_pi(double a) {
    const double pi = 3.14159265358979323846;
    a = std::fmod(a + pi, 2.0*pi);
    if (a < 0.0) a += 2.0*pi;
    return a - pi;
}

static inline double sat(double x, double lo, double hi) {
    return std::min(std::max(x, lo), hi);
}

struct Params {
    double m   = 30.0;
    double Iz  = 1.2;
    double rw  = 0.10;
    double b   = 0.50;
    double bv  = 6.0;
    double bw  = 0.25;
    double tau_max = 3.0;
    double kv  = 8.0;
    double kw  = 1.2;
};

static std::array<double,2> cmd_profile(double t) {
    double v_cmd, w_cmd;
    if (t <= 5.0) { v_cmd = 0.2 + 0.18*t; w_cmd = 0.6; }
    else         { v_cmd = 1.1;          w_cmd = 1.5; }
    return {v_cmd, w_cmd};
}

static std::array<double,3> kin_rhs(const std::array<double,3>& x, const std::array<double,2>& u) {
    const double th = x[2];
    const double v = u[0], w = u[1];
    return { v*std::cos(th), v*std::sin(th), w };
}

static std::array<double,5> dyn_rhs(const std::array<double,5>& x, const std::array<double,2>& u, const Params& p) {
    const double th = x[2], v = x[3], w = x[4];
    const double v_ref = u[0], w_ref = u[1];

    const double tau_sum  = p.kv*(v_ref - v);
    const double tau_diff = p.kw*(w_ref - w);

    double tau_R = 0.5*(tau_sum + tau_diff);
    double tau_L = 0.5*(tau_sum - tau_diff);

    tau_R = sat(tau_R, -p.tau_max, p.tau_max);
    tau_L = sat(tau_L, -p.tau_max, p.tau_max);

    const double Fx = (tau_R + tau_L)/p.rw;
    const double Mz = (p.b/(2.0*p.rw))*(tau_R - tau_L);

    const double v_dot = (Fx - p.bv*v)/p.m;
    const double w_dot = (Mz - p.bw*w)/p.Iz;

    return { v*std::cos(th), v*std::sin(th), w, v_dot, w_dot };
}

template <size_t N, typename RHS, typename U>
static std::array<double,N> rk4_step(RHS rhs, double dt, const std::array<double,N>& x, const U& u) {
    auto k1 = rhs(x, u);
    std::array<double,N> x2;
    for (size_t i=0;i<N;i++) x2[i] = x[i] + 0.5*dt*k1[i];
    auto k2 = rhs(x2, u);
    std::array<double,N> x3;
    for (size_t i=0;i<N;i++) x3[i] = x[i] + 0.5*dt*k2[i];
    auto k3 = rhs(x3, u);
    std::array<double,N> x4;
    for (size_t i=0;i<N;i++) x4[i] = x[i] + dt*k3[i];
    auto k4 = rhs(x4, u);

    std::array<double,N> xn;
    for (size_t i=0;i<N;i++) xn[i] = x[i] + (dt/6.0)*(k1[i] + 2.0*k2[i] + 2.0*k3[i] + k4[i]);
    return xn;
}

int main() {
    const double dt = 0.002, T = 10.0;
    const int steps = static_cast<int>(T/dt) + 1;

    std::array<double,3> xk = {0.0, 0.0, 0.0};
    std::array<double,5> xd = {0.0, 0.0, 0.0, 0.0, 0.0};
    Params p;

    std::cout << "t,xk,yk,thk,xd,yd,thd,vd,wd,v_cmd,w_cmd\n";
    for (int i=0;i<steps;i++) {
        const double t = i*dt;
        auto u = cmd_profile(t);

        if (i % 250 == 0) {
            std::cout << t << "," << xk[0] << "," << xk[1] << "," << xk[2] << ","
                      << xd[0] << "," << xd[1] << "," << xd[2] << "," << xd[3] << "," << xd[4] << ","
                      << u[0] << "," << u[1] << "\n";
        }

        xk = rk4_step<3>([](const std::array<double,3>& x, const std::array<double,2>& u){ return kin_rhs(x,u); }, dt, xk, u);
        xk[2] = wrap_pi(xk[2]);

        xd = rk4_step<5>([&p](const std::array<double,5>& x, const std::array<double,2>& u){ return dyn_rhs(x,u,p); }, dt, xd, u);
        xd[2] = wrap_pi(xd[2]);
    }
    return 0;
}
      

Relevant C++ robotics libraries: Eigen for linear algebra; ROS 2 (rclcpp) for system integration; common simulators (Gazebo/Ignition) for validation (optional).

7.3 Java (plain Java, CSV output)

File: Chapter4_Lesson1.java

/*
Chapter4_Lesson1.java
Autonomous Mobile Robots - Chapter 4 (Mobile Robot Dynamics Applied)
Lesson 1: When Dynamics Matter for AMR
*/
import java.io.PrintStream;

public class Chapter4_Lesson1 {

    static class Params {
        double m = 30.0;
        double Iz = 1.2;
        double rw = 0.10;
        double b  = 0.50;
        double bv = 6.0;
        double bw = 0.25;
        double tauMax = 3.0;
        double kv = 8.0;
        double kw = 1.2;
    }

    static double wrapPi(double a) {
        double pi = Math.PI;
        a = (a + pi) % (2.0*pi);
        if (a < 0.0) a += 2.0*pi;
        return a - pi;
    }

    static double sat(double x, double lo, double hi) {
        return Math.min(Math.max(x, lo), hi);
    }

    static double[] cmdProfile(double t) {
        double vCmd, wCmd;
        if (t <= 5.0) { vCmd = 0.2 + 0.18*t; wCmd = 0.6; }
        else         { vCmd = 1.1;          wCmd = 1.5; }
        return new double[]{vCmd, wCmd};
    }

    static double[] kinRhs(double[] x, double[] u) {
        double th = x[2];
        double v = u[0], w = u[1];
        return new double[]{ v*Math.cos(th), v*Math.sin(th), w };
    }

    static double[] dynRhs(double[] x, double[] u, Params p) {
        double th = x[2], v = x[3], w = x[4];
        double vRef = u[0], wRef = u[1];

        double tauSum  = p.kv*(vRef - v);
        double tauDiff = p.kw*(wRef - w);

        double tauR = 0.5*(tauSum + tauDiff);
        double tauL = 0.5*(tauSum - tauDiff);

        tauR = sat(tauR, -p.tauMax, p.tauMax);
        tauL = sat(tauL, -p.tauMax, p.tauMax);

        double Fx = (tauR + tauL)/p.rw;
        double Mz = (p.b/(2.0*p.rw))*(tauR - tauL);

        double vDot = (Fx - p.bv*v)/p.m;
        double wDot = (Mz - p.bw*w)/p.Iz;

        return new double[]{ v*Math.cos(th), v*Math.sin(th), w, vDot, wDot };
    }

    interface RhsFunction { double[] eval(double t, double[] x, double[] u); }

    static double[] rk4Step(RhsFunction f, double t, double[] x, double dt, double[] u) {
        double[] k1 = f.eval(t, x, u);
        double[] x2 = new double[x.length];
        for (int i=0;i<x.length;i++) x2[i] = x[i] + 0.5*dt*k1[i];

        double[] k2 = f.eval(t + 0.5*dt, x2, u);
        double[] x3 = new double[x.length];
        for (int i=0;i<x.length;i++) x3[i] = x[i] + 0.5*dt*k2[i];

        double[] k3 = f.eval(t + 0.5*dt, x3, u);
        double[] x4 = new double[x.length];
        for (int i=0;i<x.length;i++) x4[i] = x[i] + dt*k3[i];

        double[] k4 = f.eval(t + dt, x4, u);

        double[] xn = new double[x.length];
        for (int i=0;i<x.length;i++) xn[i] = x[i] + (dt/6.0)*(k1[i] + 2*k2[i] + 2*k3[i] + k4[i]);
        return xn;
    }

    public static void main(String[] args) {
        double dt = 0.002, T = 10.0;
        int steps = (int)(T/dt) + 1;

        double[] xk = new double[]{0.0, 0.0, 0.0};
        double[] xd = new double[]{0.0, 0.0, 0.0, 0.0, 0.0};
        Params p = new Params();

        PrintStream out = System.out;
        out.println("t,xk,yk,thk,xd,yd,thd,vd,wd,v_cmd,w_cmd");

        for (int i=0;i<steps;i++) {
            double t = i*dt;
            double[] u = cmdProfile(t);

            if (i % 250 == 0) {
                out.printf(java.util.Locale.US,
                    "%.6f,%.6f,%.6f,%.6f,%.6f,%.6f,%.6f,%.6f,%.6f,%.6f,%.6f%n",
                    t, xk[0], xk[1], xk[2], xd[0], xd[1], xd[2], xd[3], xd[4], u[0], u[1]
                );
            }

            xk = rk4Step((tt, xx, uu) -> kinRhs(xx, uu), t, xk, dt, u);
            xk[2] = wrapPi(xk[2]);

            xd = rk4Step((tt, xx, uu) -> dynRhs(xx, uu, p), t, xd, dt, u);
            xd[2] = wrapPi(xd[2]);
        }
    }
}
      

Relevant Java robotics libraries: EJML (linear algebra), Apache Commons Math (numerics); ROS integrations exist but are ecosystem-dependent—Java is commonly used at the application layer (UIs, fleet tools).

7.4 MATLAB / Simulink (ODE + programmatic Simulink model option)

File: Chapter4_Lesson1.m

% Chapter4_Lesson1.m
% Autonomous Mobile Robots - Chapter 4 (Mobile Robot Dynamics Applied)
% Lesson 1: When Dynamics Matter for AMR
clear; clc;

dt = 0.002;
T  = 10.0;
ts = 0:dt:T;

p.m = 30.0; p.Iz = 1.2; p.rw = 0.10; p.b = 0.50;
p.bv = 6.0; p.bw = 0.25; p.tau_max = 3.0; p.kv = 8.0; p.kw = 1.2;

xk = [0;0;0];
xd = [0;0;0;0;0];

Xk = zeros(3, numel(ts));
Xd = zeros(5, numel(ts));
U  = zeros(2, numel(ts));

for i = 1:numel(ts)
    t = ts(i);
    u = cmd_profile(t);
    U(:,i) = u;

    Xk(:,i) = xk;
    Xd(:,i) = xd;

    xk = rk4_step(@(tt,xx) kin_rhs(tt, xx, u), t, xk, dt);
    xk(3) = wrap_pi(xk(3));

    xd = rk4_step(@(tt,xx) dyn_rhs(tt, xx, u, p), t, xd, dt);
    xd(3) = wrap_pi(xd(3));
end

figure; plot(Xk(1,:), Xk(2,:), 'LineWidth', 1.2); hold on;
plot(Xd(1,:), Xd(2,:), 'LineWidth', 1.2);
axis equal; grid on;
xlabel('x [m]'); ylabel('y [m]');
title('Trajectory: kinematic vs dynamic');
legend('Kinematic (ideal v,w)', 'Dynamic (torque-limited)');

% Local functions
function u = cmd_profile(t)
    if t <= 5.0
        v_cmd = 0.2 + 0.18*t; w_cmd = 0.6;
    else
        v_cmd = 1.1; w_cmd = 1.5;
    end
    u = [v_cmd; w_cmd];
end

function dx = kin_rhs(~, x, u)
    th = x(3); v = u(1); w = u(2);
    dx = [v*cos(th); v*sin(th); w];
end

function dx = dyn_rhs(~, x, u, p)
    th = x(3); v = x(4); w = x(5);
    v_ref = u(1); w_ref = u(2);

    tau_sum  = p.kv*(v_ref - v);
    tau_diff = p.kw*(w_ref - w);

    tau_R = 0.5*(tau_sum + tau_diff);
    tau_L = 0.5*(tau_sum - tau_diff);

    tau_R = sat(tau_R, -p.tau_max, p.tau_max);
    tau_L = sat(tau_L, -p.tau_max, p.tau_max);

    Fx = (tau_R + tau_L)/p.rw;
    Mz = (p.b/(2*p.rw))*(tau_R - tau_L);

    v_dot = (Fx - p.bv*v)/p.m;
    w_dot = (Mz - p.bw*w)/p.Iz;

    dx = [v*cos(th); v*sin(th); w; v_dot; w_dot];
end

function xn = rk4_step(f, t, x, dt)
    k1 = f(t, x);
    k2 = f(t + 0.5*dt, x + 0.5*dt*k1);
    k3 = f(t + 0.5*dt, x + 0.5*dt*k2);
    k4 = f(t + dt, x + dt*k3);
    xn = x + (dt/6)*(k1 + 2*k2 + 2*k3 + k4);
end

function a = wrap_pi(a)
    a = mod(a + pi, 2*pi) - pi;
end

function y = sat(x, lo, hi)
    y = min(max(x, lo), hi);
end
      

Relevant MATLAB/Simulink robotics tooling: Robotics System Toolbox for ROS integration and mobile robot utilities; Simulink for real-time and controller prototyping (the provided script optionally builds a simple model programmatically).

7.5 Wolfram Mathematica (symbolic + NDSolve)

File: Chapter4_Lesson1.nb

(* Chapter4_Lesson1.nb
   Autonomous Mobile Robots - Chapter 4 (Mobile Robot Dynamics Applied)
   Lesson 1: When Dynamics Matter for AMR
*)
Notebook[{
  Cell["Chapter 4 - Lesson 1: When Dynamics Matter for AMR", "Title"],
  Cell["Symbolic dynamics", "Section"],
  Cell[BoxData @ ToBoxes @ HoldForm[
    Clear[m, Iz, rw, b, bv, bw, tauR, tauL, v, w];
    eq1 = m*D[v[t], t] == (tauR[t] + tauL[t])/rw - bv*v[t];
    eq2 = Iz*D[w[t], t] == (b/(2*rw))*(tauR[t] - tauL[t]) - bw*w[t];
    {eq1, eq2}
  ], "Input"],
  Cell["Simulation with bounded torques", "Section"],
  Cell[BoxData @ ToBoxes @ HoldForm[
    params = <|"m" -> 30, "Iz" -> 1.2, "rw" -> 0.10, "b" -> 0.50, "bv" -> 6.0, "bw" -> 0.25,
              "tauMax" -> 3.0, "kv" -> 8.0, "kw" -> 1.2|>;

    cmdV[t_] := Piecewise[{ {0.2 + 0.18*t, t <= 5} }, 1.1];
    cmdW[t_] := Piecewise[{ {0.6, t <= 5} }, 1.5];

    tauSum[t_, v_] := params["kv"]*(cmdV[t] - v);
    tauDiff[t_, w_] := params["kw"]*(cmdW[t] - w);

    tauRfun[t_, v_, w_] := Clip[0.5*(tauSum[t, v] + tauDiff[t, w]), {-params["tauMax"], params["tauMax"]}];
    tauLfun[t_, v_, w_] := Clip[0.5*(tauSum[t, v] - tauDiff[t, w]), {-params["tauMax"], params["tauMax"]}];

    eqKin = {
      px'[t] == cmdV[t]*Cos[th[t]],
      py'[t] == cmdV[t]*Sin[th[t]],
      th'[t] == cmdW[t],
      px[0] == 0, py[0] == 0, th[0] == 0
    };

    eqDyn = {
      pxd'[t] == vd[t]*Cos[thd[t]],
      pyd'[t] == vd[t]*Sin[thd[t]],
      thd'[t] == wd[t],
      vd'[t] == ((tauRfun[t, vd[t], wd[t]] + tauLfun[t, vd[t], wd[t]])/params["rw"] - params["bv"]*vd[t])/params["m"],
      wd'[t] == ((params["b"]/(2*params["rw"]))*(tauRfun[t, vd[t], wd[t]] - tauLfun[t, vd[t], wd[t]]) - params["bw"]*wd[t])/params["Iz"],
      pxd[0] == 0, pyd[0] == 0, thd[0] == 0, vd[0] == 0, wd[0] == 0
    };

    solKin = NDSolveValue[eqKin, {px, py, th}, {t, 0, 10}];
    solDyn = NDSolveValue[eqDyn, {pxd, pyd, thd, vd, wd}, {t, 0, 10}];

    ParametricPlot[
      { {solKin[[1]][t], solKin[[2]][t]},
        {solDyn[[1]][t], solDyn[[2]][t]} },
      {t, 0, 10},
      PlotLegends -> {"Kinematic", "Dynamic"},
      AxesLabel -> {"x", "y"},
      PlotRange -> All
    ]
  ], "Input"]
}]
      

Problem 1 (Kinematic limit bound): Consider the first-order velocity servo \( \dot{v}=\frac{1}{\tau_v}(v_c-v) \) on \( t\in[0,T] \) with continuously differentiable \( v_c(t) \). Prove that \( \sup_{0\le t\le T}|v(t)-v_c(t)| \le |v(0)-v_c(0)| + \tau_v\|\dot{v}_c\|_\infty \).

Solution: Define \( e_v=v-v_c \). Then \( \dot{e}_v=-(1/\tau_v)e_v-\dot{v}_c(t) \). Solve using variation of constants:

\[ e_v(t)=e^{-t/\tau_v}e_v(0)-\int_0^t e^{-(t-s)/\tau_v}\dot{v}_c(s)\,ds. \]

Taking absolute values and using \( \int_0^t e^{-(t-s)/\tau_v}ds \le \tau_v \) yields the bound. ■

Problem 2 (Maximum linear acceleration under torque limits): Using the simplified differential-drive mapping (ignore drag), show that if \( |\tau_R|\le \tau_{max} \) and \( |\tau_L|\le \tau_{max} \), then the maximum achievable forward acceleration satisfies \( \dot{v}_{max}=\frac{2\tau_{max}}{m r_w} \).

Solution: From Section 3, \( \dot{v}=\frac{1}{m r_w}(\tau_R+\tau_L) \). The sum is maximized by choosing both at their upper bounds: \( \tau_R=\tau_L=\tau_{max} \), so

\[ \dot{v}_{max}=\frac{1}{m r_w}(2\tau_{max}). \]

Similarly, the most negative acceleration is \( -\dot{v}_{max} \). ■

Problem 3 (Stopping-distance constraint): An AMR travels at speed \( v \) and must stop within clearance distance \( d \). Given maximum braking deceleration magnitude \( a_{brk} \), derive the speed constraint on \( v \).

Solution: Using constant deceleration, \( d_{stop}=\frac{v^2}{2a_{brk}} \). Requiring \( d_{stop}\le d \) implies:

\[ \frac{v^2}{2a_{brk}} \le d \qquad →\qquad |v| \le \sqrt{2a_{brk}d}. \]

Problem 4 (Curvature-limited speed envelope): Assume an operational lateral acceleration bound \( |a_{lat}|\le a_{lat,max} \). For a planned curvature \( \kappa \), derive the maximum speed \( v_{max}(\kappa) \).

Solution: Using \( a_{lat}=v^2\kappa \) and enforcing \( |v^2\kappa|\le a_{lat,max} \) gives:

\[ v_{max}(\kappa)=\sqrt{\frac{a_{lat,max}}{|\kappa|}}. \]

This constraint is dynamic (speed depends on curvature), and is not captured by pure nonholonomic kinematics alone. ■

Problem 5 (Stability of a torque-based velocity servo): Consider the simplified longitudinal dynamics \( m\dot{v}=\frac{1}{r_w}(\tau_R+\tau_L) - c_v v \). Suppose a controller sets \( \tau_R=\tau_L=\frac{r_w}{2}k(v_c-v) \) (ignore saturation). Prove exponential convergence of \( v \) to constant \( v_c \).

Solution: Substituting: \( m\dot{v}=k(v_c-v)-c_v v = k v_c -(k+c_v)v \). Define error \( e=v-v_c \). Then \( \dot{e}= -\frac{k+c_v}{m}e \), so

\[ e(t)=e(0)\exp\!\left(-\frac{k+c_v}{m}t\right), \]

which is exponential stability. The time constant is \( \tau=\frac{m}{k+c_v} \), connecting directly to the “fast inner loop” condition from Section 2. ■

Dynamics matters for AMR autonomy when (i) velocity servos are not fast relative to outer-loop command variation, (ii) actuator torque/force bounds constrain achievable accelerations, and (iii) safety envelopes (stopping distance, lateral acceleration vs curvature) must be enforced. We derived a torque-to-acceleration map for differential drive and proved a finite-horizon kinematic limit bound in terms of \( \tau_v \) and \( \|\dot{v}_c\|_\infty \). Lesson 2 extends this foundation to slip/skid and terrain interaction models.