In Chapter 3, Lesson 1 we modeled nonholonomic feasibility through rolling constraints and admissible velocities. This lesson adds a crucial geometric constraint: how sharply the robot can turn. A planned path is not feasible unless its curvature stays within what the platform can realize.

\( \kappa \): curvature (signed) of a planar path; \( R \): turning radius, related by \( R = 1/|\kappa| \) (where defined). Kinematic models typically relate curvature to control inputs: \( \kappa = \omega / v \) (unicycle) and \( \kappa = \tan\delta / L \) (bicycle/Ackermann).

flowchart TD
  P["Candidate path (x(s), y(s))"] --> K["Compute curvature kappa(s)"]
  K --> R["Turning radius R(s)=1/|kappa(s)|"]
  R --> C1["Geometry limits: \nkappa <= kappa_steer"]
  R --> C2["Dynamics/safety: \nv^2*|kappa| <= a_lat_max"]
  R --> C3["Actuation rate: \n|dkappa/dt| <= kappaDot_max"]
  C1 --> I["Intersect all constraints"]
  C2 --> I
  C3 --> I
  I --> F["Feasible? If yes: track; if no: replan / slow / smooth"]
        

In later lessons (Ch.3 L3–L5), you will use these bounds to characterize feasible path families and to perform fast feasibility checks. Here we focus on deriving the constraints and implementing the checks.

Consider a regular planar curve \( \gamma(t) = (x(t), y(t)) \) with nonzero speed \( \|\dot{\gamma}(t)\| \neq 0 \). Define the arc-length function \( s(t) \):

\[ s(t) = \int_{t_0}^{t} \|\dot{\gamma}(\tau)\|\, d\tau, \qquad \frac{ds}{dt} = \|\dot{\gamma}(t)\|. \]

The unit tangent is \( \mathbf{T}(t) = \dot{\gamma}(t)/\|\dot{\gamma}(t)\| \). In 2D, we can parameterize \( \mathbf{T} \) by a heading angle \( \theta \): \( \mathbf{T} = (\cos\theta, \sin\theta) \).

Proposition 1 (Geometric definition of curvature): If the curve is parameterized by arc length \( s \), then curvature satisfies \( \kappa(s) = d\theta/ds \).

Proof: Under arc-length parametrization, \( \|\gamma'(s)\| = 1 \), hence \( \mathbf{T}(s) = \gamma'(s) \). Using \( \mathbf{T}(s) = (\cos\theta(s), \sin\theta(s)) \),

\[ \frac{d\mathbf{T}}{ds} = \frac{d\theta}{ds}(-\sin\theta, \cos\theta) = \kappa(s)\,\mathbf{N}(s), \]

where \( \mathbf{N}(s) = (-\sin\theta, \cos\theta) \) is the unit normal. By definition, the signed scalar multiplying \( \mathbf{N} \) is the signed curvature; therefore \( \kappa(s)=d\theta/ds \). ■

For a general parameter \( t \), curvature can be computed directly from derivatives:

\[ \kappa(t) = \frac{\dot{x}(t)\ddot{y}(t) - \dot{y}(t)\ddot{x}(t)} {\left(\dot{x}(t)^2 + \dot{y}(t)^2\right)^{3/2}}. \]

This expression is invariant to reparameterization (as long as speed is nonzero), which is why curvature is a geometric property of the path.

Proposition 2 (Turning radius): If \( \kappa \neq 0 \) locally, the osculating circle radius is \( R = 1/|\kappa| \).

Proof sketch: For circular motion, heading changes at constant rate with arc length: \( d\theta/ds = 1/R \). Since the osculating circle matches the curve to second order, its radius is the reciprocal of curvature magnitude. ■

We now connect curvature to the control inputs used in standard AMR kinematic models. This is where a geometric path becomes a feasibility statement about inputs and bounds.

3.1 Unicycle model (velocity–turn-rate)

The planar unicycle kinematics (used widely as an abstraction for many mobile bases) are:

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

Proposition 3: If \( v \neq 0 \), the path curvature is \( \kappa = \omega/v \).

Proof: Using Proposition 1 with chain rule,

\[ \kappa = \frac{d\theta}{ds} = \frac{d\theta/dt}{ds/dt} = \frac{\omega}{\|\dot{\gamma}\|} = \frac{\omega}{|v|}. \]

For forward motion (\( v > 0 \) in the planning convention), this reduces to \( \kappa = \omega/v \). Turning radius is \( R = \frac{|v|}{|\omega|} \) when \( \omega \neq 0 \). ■

3.2 Differential drive (wheel speeds)

For wheel radius \( r_w \) and track width \( b \) (distance between wheel contact lines), with wheel angular speeds \( \omega_r, \omega_l \):

\[ v = \frac{r_w}{2}\left(\omega_r+\omega_l\right), \qquad \omega = \frac{r_w}{b}\left(\omega_r-\omega_l\right). \]

Therefore curvature is:

\[ \kappa = \frac{\omega}{v} = \frac{2}{b}\cdot \frac{\omega_r-\omega_l}{\omega_r+\omega_l}, \qquad (\omega_r+\omega_l \neq 0). \]

Solving for wheel speeds given \( (v,\kappa) \) (useful for controllers):

\[ \omega_r = \frac{v}{r_w}\left(1 + \frac{b}{2}\kappa\right), \qquad \omega_l = \frac{v}{r_w}\left(1 - \frac{b}{2}\kappa\right). \]

3.3 Bicycle/Ackermann model (steering angle)

The kinematic bicycle model (a standard abstraction for car-like robots) is:

\[ \dot{x} = v\cos\theta,\qquad \dot{y} = v\sin\theta,\qquad \dot{\theta} = \frac{v}{L}\tan\delta, \]

where \( L \) is wheelbase and \( \delta \) is the (virtual) front steering angle. Hence:

\[ \kappa = \frac{\dot{\theta}}{v} = \frac{1}{L}\tan\delta, \qquad R = \frac{1}{|\kappa|} = \frac{L}{|\tan\delta|}. \]

This explicit mapping makes curvature limits a direct consequence of steering angle and steering rate limits.

4.1 Steering angle bound implies curvature bound (car-like)

Suppose steering is bounded: \( |\delta| \le \delta_{\max} \). Then the maximum achievable curvature magnitude is:

\[ |\kappa| = \frac{|\tan\delta|}{L} \le \frac{\tan\delta_{\max}}{L} \triangleq \kappa_{\text{steer}}. \]

Equivalently, the minimum turning radius is:

\[ R \ge R_{\min} \triangleq \frac{1}{\kappa_{\text{steer}}} = \frac{L}{\tan\delta_{\max}}. \]

Proof: \( \tan(\cdot) \) is monotone on \( (-\pi/2,\pi/2) \), so bounding \( |\delta| \) bounds \( |\tan\delta| \); substitute into \( \kappa=\tan\delta/L \). ■

4.2 Wheel speed limits induce speed-dependent curvature limits (diff-drive)

If the motors saturate at \( |\omega_r| \le \omega_{\max} \) and \( |\omega_l| \le \omega_{\max} \), then not all combinations of \( (v,\kappa) \) are feasible. Using \( \omega_r = \frac{v}{r_w}\left(1 + \frac{b}{2}\kappa\right) \) and \( \omega_l = \frac{v}{r_w}\left(1 - \frac{b}{2}\kappa\right) \), assume forward motion \( v > 0 \) and define \( \alpha = \frac{\omega_{\max} r_w}{v} \). The saturation inequalities imply (sufficient condition):

\[ \left|1 \pm \frac{b}{2}\kappa\right| \le \alpha \quad \Rightarrow \quad |\kappa| \le \frac{2}{b}\left(\alpha - 1\right) = \frac{2}{b}\left(\frac{\omega_{\max} r_w}{v} - 1\right), \]

provided \( \alpha \ge 1 \) (equivalently \( v \le \omega_{\max} r_w \)). This shows a key AMR fact: for differential drive, “tight turns” can become impossible at higher commanded speeds due to wheel saturation.

4.3 From curvature bounds to turning-radius feasibility

If a path has curvature profile \( \kappa(s) \), then a necessary geometric feasibility condition for car-like steering is: \( \max_s |\kappa(s)| \le \kappa_{\text{steer}} \). If violated, you must smooth the path or increase turning radius (e.g., by re-planning).

Even if steering can realize the curvature, tracking at high speed can be unsafe or dynamically infeasible due to lateral acceleration (and tire friction). For a body following a path with curvature \( \kappa \) at speed \( v \), the normal (lateral) acceleration magnitude is:

\[ a_{\text{lat}} = v^2|\kappa| = \frac{v^2}{R}. \]

Proof (via Frenet decomposition): Let \( \mathbf{T} \) be the unit tangent and assume speed \( v \). Velocity is \( \dot{\gamma} = v\mathbf{T} \). Differentiate:

\[ \ddot{\gamma} = \dot{v}\mathbf{T} + v\dot{\mathbf{T}} = \dot{v}\mathbf{T} + v\left(\frac{d\mathbf{T}}{ds}\right)\frac{ds}{dt} = \dot{v}\mathbf{T} + v\left(\kappa\mathbf{N}\right)v = \dot{v}\mathbf{T} + v^2\kappa\mathbf{N}. \]

The normal component magnitude is \( v^2|\kappa| \). ■

If the platform or safety policy imposes \( a_{\text{lat}} \le a_{\text{lat,max}} \), then:

\[ v^2|\kappa| \le a_{\text{lat,max}} \quad \Rightarrow \quad v \le \sqrt{\frac{a_{\text{lat,max}}}{|\kappa|}}. \]

This is a speed planner’s key inequality: near high curvature regions, speed must drop (often aggressively) even if steering allows the geometry.

Real steering has finite rate: \( |\dot{\delta}| \le \dot{\delta}_{\max} \). Since \( \kappa = \tan\delta/L \), curvature cannot change arbitrarily fast.

\[ \dot{\kappa} = \frac{d}{dt}\left(\frac{\tan\delta}{L}\right) = \frac{1}{L}\sec^2\delta \; \dot{\delta}. \]

Therefore,

\[ |\dot{\kappa}| \le \frac{1}{L}\sec^2(\delta)\; \dot{\delta}_{\max} \le \frac{1}{L}\sec^2(\delta_{\max})\; \dot{\delta}_{\max}. \]

If you prefer curvature change per unit distance (useful for path parameter \( s \)), use \( \frac{d\kappa}{ds} = \frac{\dot{\kappa}}{ds/dt} = \frac{\dot{\kappa}}{v} \) (for \( v \neq 0 \)). This explains why “smooth curvature transitions” are crucial for car-like robots: discontinuities in curvature would imply infinite steering rate. (Continuous-curvature path families are introduced next lesson.)

In practice, planners often output a discrete polyline \( \{(x_i,y_i)\}_{i=1}^N \). A robust, local curvature estimator uses three consecutive points (circumcircle of the triangle). Let side lengths be \( a=\|p_2-p_1\| \), \( b=\|p_2-p_0\| \), \( c=\|p_1-p_0\| \), and signed triangle area \( A \). The circumradius satisfies \( R = abc/(4|A|) \), hence:

\[ \kappa \approx \frac{1}{R} = \frac{4A}{abc}. \]

Using the signed “double area” \( 2A = (p_1-p_0)_x (p_2-p_0)_y - (p_1-p_0)_y (p_2-p_0)_x \), we can compute a signed curvature estimate.

flowchart TD
  S0["Input: path points P[0..N-1], limits"] --> S1["For i=1..N-2: compute kappa[i] from (P[i-1],P[i],P[i+1])"]
  S1 --> S2["Compute R[i]=1/|kappa[i]| (if kappa!=0)"]
  S2 --> S3["Check: |kappa[i]| <= kappa_steer"]
  S3 --> S4["Given speed v[i]: check v[i]^2*|kappa[i]| <= a_lat_max"]
  S4 --> S5["Check steering-rate proxy: |dkappa/dt| <= kappaDot_max"]
  S5 --> OUT["Output: feasible mask + diagnostics"]
        

7.1 Python (NumPy) — curvature, limits, feasibility

Libraries: \( \)NumPy for vector math; optional Matplotlib for plots. The script includes unicycle/diff-drive/bicycle relationships and checks steering, lateral acceleration, and curvature-rate constraints.

File: Chapter3_Lesson2.py

#!/usr/bin/env python3
"""Chapter 3 - Lesson 2: Curvature and Turning Radius Limits

This script provides:
1) Continuous and discrete curvature computation for planar paths.
2) Turning-radius/curvature limits for:
   - Unicycle (v, omega) model
   - Differential-drive (wheel speeds)
   - Car-like (bicycle/Ackermann) model
3) Speed-dependent feasibility checks from lateral-acceleration bounds and steering-rate bounds.

Dependencies:
  - numpy
  - matplotlib (optional, for plotting)
"""

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


@dataclass(frozen=True)
class BicycleLimits:
    L: float                 # wheelbase [m]
    delta_max: float         # max steering angle magnitude [rad]
    delta_dot_max: float     # max steering rate magnitude [rad/s]


@dataclass(frozen=True)
class LateralAccelLimits:
    a_lat_max: float         # max lateral acceleration magnitude [m/s^2]


def curvature_parametric(x: np.ndarray, y: np.ndarray, t: np.ndarray) -> np.ndarray:
    """Compute signed curvature kappa(t) for a parametric curve (x(t), y(t)).
    Uses kappa = (x' y'' - y' x'') / ( (x'^2 + y'^2)^(3/2) )."""
    x1 = np.gradient(x, t)
    y1 = np.gradient(y, t)
    x2 = np.gradient(x1, t)
    y2 = np.gradient(y1, t)
    denom = (x1**2 + y1**2) ** 1.5
    eps = 1e-12
    kappa = (x1 * y2 - y1 * x2) / (denom + eps)
    return kappa


def curvature_three_points(p0: np.ndarray, p1: np.ndarray, p2: np.ndarray) -> float:
    """Signed curvature from three points in R^2 using the circumcircle formula.

    For triangle (p0, p1, p2):
      kappa = 4 * A / (a*b*c)
    where A is signed area, a=|p1-p2|, b=|p0-p2|, c=|p0-p1|.
    """
    p0 = np.asarray(p0, dtype=float).reshape(2)
    p1 = np.asarray(p1, dtype=float).reshape(2)
    p2 = np.asarray(p2, dtype=float).reshape(2)

    v01 = p1 - p0
    v02 = p2 - p0
    v12 = p2 - p1

    a = np.linalg.norm(v12)
    b = np.linalg.norm(v02)
    c = np.linalg.norm(v01)

    area2 = v01[0] * v02[1] - v01[1] * v02[0]  # = 2*A_signed
    denom = a * b * c
    if denom < 1e-12:
        return 0.0
    return 2.0 * area2 / denom  # since kappa = 4*A/(abc) and area2 = 2*A


def discrete_curvature_polyline(P: np.ndarray) -> np.ndarray:
    """Compute curvature along a polyline P (N x 2) using 3-point formula at interior points."""
    P = np.asarray(P, dtype=float)
    N = P.shape[0]
    kappa = np.zeros(N)
    for i in range(1, N - 1):
        kappa[i] = curvature_three_points(P[i - 1], P[i], P[i + 1])
    return kappa


def bicycle_delta_from_kappa(kappa: np.ndarray, L: float) -> np.ndarray:
    return np.arctan(L * kappa)


def bicycle_kappa_max(lim: BicycleLimits) -> float:
    return np.tan(lim.delta_max) / lim.L


def v_max_from_lateral_accel(kappa: np.ndarray, lat: LateralAccelLimits) -> np.ndarray:
    kabs = np.abs(kappa)
    vmax = np.full_like(kabs, np.inf, dtype=float)
    mask = kabs > 1e-12
    vmax[mask] = np.sqrt(lat.a_lat_max / kabs[mask])
    return vmax


def kappa_dot_max_from_delta_dot(lim: BicycleLimits, delta: float) -> float:
    sec2 = 1.0 / (np.cos(delta) ** 2)
    return (sec2 / lim.L) * lim.delta_dot_max


def check_bicycle_feasibility(
    kappa: np.ndarray,
    v: np.ndarray,
    lim: BicycleLimits,
    lat: Optional[LateralAccelLimits] = None,
) -> Tuple[np.ndarray, dict]:
    kappa = np.asarray(kappa, dtype=float)
    v = np.asarray(v, dtype=float)
    if kappa.shape != v.shape:
        raise ValueError("kappa and v must have the same shape")

    delta = bicycle_delta_from_kappa(kappa, lim.L)
    steer_ok = np.abs(delta) <= lim.delta_max + 1e-12

    lat_ok = np.ones_like(steer_ok, dtype=bool)
    a_lat = None
    if lat is not None:
        a_lat = (v ** 2) * np.abs(kappa)
        lat_ok = a_lat <= lat.a_lat_max + 1e-12

    N = len(kappa)
    dt = 1.0 / max(N - 1, 1)
    kappa_dot = np.gradient(kappa, dt)
    kappa_dot_max = np.array([kappa_dot_max_from_delta_dot(lim, float(d)) for d in delta])
    rate_ok = np.abs(kappa_dot) <= kappa_dot_max + 1e-12

    feasible = steer_ok & lat_ok & rate_ok
    diagnostics = {
        "delta": delta,
        "steer_ok": steer_ok,
        "a_lat": a_lat,
        "lat_ok": lat_ok,
        "kappa_dot": kappa_dot,
        "kappa_dot_max": kappa_dot_max,
        "rate_ok": rate_ok,
        "kappa_max_steer": bicycle_kappa_max(lim),
    }
    return feasible, diagnostics


def diff_drive_wheel_speeds(v: float, kappa: float, r_w: float, b: float) -> Tuple[float, float]:
    wr = (v / r_w) * (1.0 + 0.5 * b * kappa)
    wl = (v / r_w) * (1.0 - 0.5 * b * kappa)
    return wr, wl


def example_demo() -> None:
    t = np.linspace(0.0, 10.0, 400)
    x = t
    y = 2.0 * np.sin(0.6 * t)

    kappa = curvature_parametric(x, y, t)
    v = np.full_like(kappa, 1.5)  # m/s

    lim = BicycleLimits(L=0.35, delta_max=np.deg2rad(30.0), delta_dot_max=np.deg2rad(60.0))
    lat = LateralAccelLimits(a_lat_max=2.5)

    feasible, diag = check_bicycle_feasibility(kappa, v, lim, lat)

    print("Steering-limited |kappa|max =", diag["kappa_max_steer"])
    print("Feasible fraction =", feasible.mean())

    vmax_lat = v_max_from_lateral_accel(kappa, lat)
    finite = vmax_lat[np.isfinite(vmax_lat)]
    if finite.size > 0:
        print("Median lateral-accel vmax =", float(np.median(finite)))

    i = len(kappa) // 2
    r_w, b = 0.08, 0.32
    wr, wl = diff_drive_wheel_speeds(v=float(v[i]), kappa=float(kappa[i]), r_w=r_w, b=b)
    print(f"Diff-drive wheel speeds at mid path: wr={wr:.3f} rad/s, wl={wl:.3f} rad/s")


if __name__ == "__main__":
    example_demo()
      

Exercise file: Chapter3_Lesson2_Ex1.py

#!/usr/bin/env python3
"""Chapter 3 - Lesson 2 (Exercise 1): Find the maximum constant speed that satisfies
both steering curvature limits and lateral-acceleration limits on a given path."""

from __future__ import annotations
import numpy as np
from Chapter3_Lesson2 import (
    discrete_curvature_polyline,
    BicycleLimits,
    LateralAccelLimits,
    bicycle_kappa_max,
    v_max_from_lateral_accel,
)


def main() -> None:
    s = np.linspace(0.0, 12.0, 401)
    x = s
    y = 1.2 * np.sin(0.7 * s) + 0.2 * np.sin(2.2 * s)
    P = np.vstack([x, y]).T

    kappa = discrete_curvature_polyline(P)

    lim = BicycleLimits(L=0.35, delta_max=np.deg2rad(28.0), delta_dot_max=np.deg2rad(60.0))
    lat = LateralAccelLimits(a_lat_max=2.0)

    kappa_max = bicycle_kappa_max(lim)
    if np.any(np.abs(kappa) > kappa_max + 1e-12):
        print("Path violates steering curvature limit at some samples.")
        print("Consider smoothing path or increasing turning radius.")
    else:
        print("Path satisfies steering curvature bound.")

    vmax_lat = v_max_from_lateral_accel(kappa, lat)
    finite = vmax_lat[np.isfinite(vmax_lat)]
    vmax = float(np.min(finite)) if finite.size > 0 else float("inf")
    print(f"Maximum constant speed from lateral acceleration limit: v_max = {vmax:.3f} m/s")


if __name__ == "__main__":
    main()
      

7.2 C++ (Eigen) — discrete curvature and steering feasibility

Libraries: Eigen (matrix/vector math) is common in robotics; in ROS/ROS2 stacks it is ubiquitous for geometry and estimation code.

File: Chapter3_Lesson2.cpp

/*
Chapter 3 - Lesson 2: Curvature and Turning Radius Limits (C++)

Build note:
  This example uses Eigen (header-only).

Example compilation:
  g++ -O2 -std=c++17 Chapter3_Lesson2.cpp -I /usr/include/eigen3 -o Chapter3_Lesson2
*/

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

using Vec2 = Eigen::Vector2d;

static double signed_curvature_three_points(const Vec2& p0, const Vec2& p1, const Vec2& p2) {
    Vec2 v01 = p1 - p0;
    Vec2 v02 = p2 - p0;
    Vec2 v12 = p2 - p1;

    double a = v12.norm();
    double b = v02.norm();
    double c = v01.norm();

    double area2 = v01.x() * v02.y() - v01.y() * v02.x(); // = 2*A_signed
    double denom = a * b * c;
    if (denom < 1e-12) return 0.0;
    return 2.0 * area2 / denom; // kappa = 4*A/(abc), area2 = 2*A
}

static std::vector<double> curvature_polyline(const std::vector<Vec2>& P) {
    const int N = static_cast<int>(P.size());
    std::vector<double> kappa(N, 0.0);
    for (int i = 1; i < N - 1; ++i) {
        kappa[i] = signed_curvature_three_points(P[i-1], P[i], P[i+1]);
    }
    return kappa;
}

static double kappa_max_steer(double L, double delta_max_rad) {
    return std::tan(delta_max_rad) / L;
}

int main() {
    const int N = 401;
    std::vector<Vec2> P;
    P.reserve(N);
    for (int i = 0; i < N; ++i) {
        double s = 12.0 * static_cast<double>(i) / static_cast<double>(N - 1);
        double x = s;
        double y = 1.2 * std::sin(0.7 * s) + 0.2 * std::sin(2.2 * s);
        P.emplace_back(x, y);
    }

    auto kappa = curvature_polyline(P);

    double L = 0.35;
    double delta_max = 28.0 * M_PI / 180.0;
    double kappa_max = kappa_max_steer(L, delta_max);

    bool ok = true;
    double kappa_peak = 0.0;
    for (double ki : kappa) {
        kappa_peak = std::max(kappa_peak, std::abs(ki));
        if (std::abs(ki) > kappa_max + 1e-12) ok = false;
    }

    std::cout << "kappa_max_steer = " << kappa_max << " 1/m\n";
    std::cout << "peak |kappa|    = " << kappa_peak << " 1/m\n";
    std::cout << "steering-feasible? " << (ok ? "YES" : "NO") << "\n";
    return 0;
}
      

7.3 Java — discrete curvature and steering feasibility

Libraries: in larger robotics Java codebases, EJML is common for matrix math; here we stay dependency-free.

File: Chapter3_Lesson2.java

/*
Chapter 3 - Lesson 2: Curvature and Turning Radius Limits (Java)
*/

import java.util.ArrayList;
import java.util.List;

public class Chapter3_Lesson2 {

    static class Vec2 {
        final double x;
        final double y;
        Vec2(double x, double y) { this.x = x; this.y = y; }
        Vec2 minus(Vec2 o) { return new Vec2(this.x - o.x, this.y - o.y); }
        double norm() { return Math.sqrt(x*x + y*y); }
    }

    static double signedCurvatureThreePoints(Vec2 p0, Vec2 p1, Vec2 p2) {
        Vec2 v01 = p1.minus(p0);
        Vec2 v02 = p2.minus(p0);
        Vec2 v12 = p2.minus(p1);

        double a = v12.norm();
        double b = v02.norm();
        double c = v01.norm();

        double area2 = v01.x * v02.y - v01.y * v02.x; // = 2*A_signed
        double denom = a * b * c;
        if (denom < 1e-12) return 0.0;
        return 2.0 * area2 / denom; // kappa = 4*A/(abc), area2 = 2*A
    }

    static double[] curvaturePolyline(List<Vec2> P) {
        int N = P.size();
        double[] kappa = new double[N];
        for (int i = 1; i < N - 1; i++) {
            kappa[i] = signedCurvatureThreePoints(P.get(i-1), P.get(i), P.get(i+1));
        }
        return kappa;
    }

    static double kappaMaxSteer(double L, double deltaMaxRad) {
        return Math.tan(deltaMaxRad) / L;
    }

    public static void main(String[] args) {
        int N = 401;
        List<Vec2> P = new ArrayList<>(N);
        for (int i = 0; i < N; i++) {
            double s = 12.0 * ((double)i) / ((double)(N - 1));
            double x = s;
            double y = 1.2 * Math.sin(0.7 * s) + 0.2 * Math.sin(2.2 * s);
            P.add(new Vec2(x, y));
        }

        double[] kappa = curvaturePolyline(P);

        double L = 0.35;
        double deltaMax = Math.toRadians(28.0);
        double kappaMax = kappaMaxSteer(L, deltaMax);

        boolean ok = true;
        double kappaPeak = 0.0;
        for (double ki : kappa) {
            kappaPeak = Math.max(kappaPeak, Math.abs(ki));
            if (Math.abs(ki) > kappaMax + 1e-12) ok = false;
        }

        System.out.println("kappa_max_steer = " + kappaMax + " 1/m");
        System.out.println("peak |kappa|    = " + kappaPeak + " 1/m");
        System.out.println("steering-feasible? " + (ok ? "YES" : "NO"));
    }
}
      

7.4 MATLAB/Simulink — curvature, steering, lateral-accel limits + model builder

Tooling: MATLAB is common for offline analysis; Simulink is valuable when you want a block-diagram realization of the bicycle kinematics. Robotics System Toolbox integrates with common robotics workflows, but this example runs without requiring specialized blocks.

File: Chapter3_Lesson2.m

% Chapter 3 - Lesson 2: Curvature and Turning Radius Limits (MATLAB/Simulink)

clear; clc;

%% Parameters
L = 0.35;                      % wheelbase [m]
delta_max = deg2rad(28);        % steering angle limit [rad]
a_lat_max = 2.0;                % lateral acceleration limit [m/s^2]

%% Example path (students may replace)
N = 401;
s = linspace(0, 12, N);
x = s;
y = 1.2*sin(0.7*s) + 0.2*sin(2.2*s);
P = [x(:), y(:)];

%% Discrete curvature using 3-point circumcircle formula
kappa = zeros(N,1);
for i = 2:N-1
    p0 = P(i-1,:);
    p1 = P(i,:);
    p2 = P(i+1,:);

    v01 = p1 - p0;
    v02 = p2 - p0;
    v12 = p2 - p1;

    a = norm(v12);
    b = norm(v02);
    c = norm(v01);

    area2 = v01(1)*v02(2) - v01(2)*v02(1); % = 2*A_signed
    denom = a*b*c;

    if denom < 1e-12
        kappa(i) = 0;
    else
        kappa(i) = 2*area2/denom; % kappa = 4*A/(abc), area2 = 2*A
    end
end

%% Bicycle steering mapping and limits
delta = atan(L*kappa);
kappa_max_steer = tan(delta_max)/L;

fprintf('kappa_max_steer = %.4f 1/m\n', kappa_max_steer);
fprintf('Peak |kappa|    = %.4f 1/m\n', max(abs(kappa)));
fprintf('Steering-feasible? %s\n', string(all(abs(kappa) <= kappa_max_steer + 1e-12)));

%% Lateral acceleration speed limits: v <= sqrt(a_lat_max/|kappa|)
vmax = inf(N,1);
mask = abs(kappa) > 1e-12;
vmax(mask) = sqrt(a_lat_max ./ abs(kappa(mask)));

%% Visualizations
figure; plot(x,y,'LineWidth',1.5); axis equal; grid on;
title('Path (x,y)'); xlabel('x [m]'); ylabel('y [m]');

figure; plot(s,kappa,'LineWidth',1.5); grid on;
title('Curvature kappa(s)'); xlabel('s (index)'); ylabel('kappa [1/m]');

figure; plot(s,vmax,'LineWidth',1.5); grid on;
title('Speed upper bound from lateral acceleration'); xlabel('s (index)'); ylabel('v_{max} [m/s]');

%% Optional: Build a simple Simulink bicycle kinematics model
% modelName = 'Chapter3_Lesson2_BicycleModel';
% buildSimulinkBicycleModel(modelName, L);
% open_system(modelName);

function buildSimulinkBicycleModel(modelName, L)
    if bdIsLoaded(modelName)
        close_system(modelName, 0);
    end
    new_system(modelName);
    open_system(modelName);

    add_block('simulink/Sources/In1', [modelName '/v']);
    add_block('simulink/Sources/In1', [modelName '/delta']);
    add_block('simulink/Math Operations/Trigonometric Function', [modelName '/cos'], 'Operator', 'cos');
    add_block('simulink/Math Operations/Trigonometric Function', [modelName '/sin'], 'Operator', 'sin');
    add_block('simulink/Math Operations/Trigonometric Function', [modelName '/tan'], 'Operator', 'tan');

    add_block('simulink/Math Operations/Product', [modelName '/vcos']);
    add_block('simulink/Math Operations/Product', [modelName '/vsin']);
    add_block('simulink/Math Operations/Product', [modelName '/v_tan']);
    add_block('simulink/Math Operations/Gain', [modelName '/1_over_L'], 'Gain', num2str(1/L));

    add_block('simulink/Continuous/Integrator', [modelName '/Int_x']);
    add_block('simulink/Continuous/Integrator', [modelName '/Int_y']);
    add_block('simulink/Continuous/Integrator', [modelName '/Int_theta']);

    add_block('simulink/Sinks/Out1', [modelName '/x']);
    add_block('simulink/Sinks/Out1', [modelName '/y']);
    add_block('simulink/Sinks/Out1', [modelName '/theta']);

    add_line(modelName, 'delta/1', 'tan/1');
    add_line(modelName, 'tan/1', 'v_tan/2');
    add_line(modelName, 'v/1', 'v_tan/1');
    add_line(modelName, 'v_tan/1', '1_over_L/1');
    add_line(modelName, '1_over_L/1', 'Int_theta/1');
    add_line(modelName, 'Int_theta/1', 'cos/1');
    add_line(modelName, 'Int_theta/1', 'sin/1');
    add_line(modelName, 'v/1', 'vcos/1');
    add_line(modelName, 'cos/1', 'vcos/2');
    add_line(modelName, 'v/1', 'vsin/1');
    add_line(modelName, 'sin/1', 'vsin/2');
    add_line(modelName, 'vcos/1', 'Int_x/1');
    add_line(modelName, 'vsin/1', 'Int_y/1');

    add_line(modelName, 'Int_x/1', 'x/1');
    add_line(modelName, 'Int_y/1', 'y/1');
    add_line(modelName, 'Int_theta/1', 'theta/1');

    set_param(modelName, 'StopTime', '10');
    save_system(modelName);
end
      

7.5 Wolfram Mathematica — curvature and constraints

File: Chapter3_Lesson2.nb

(* Chapter 3 - Lesson 2: Curvature and Turning Radius Limits (Wolfram Mathematica) *)

ClearAll["Global`*"];

L = 0.35;
deltaMax = 28 Degree;
aLatMax = 2.0;

N = 401;
s = Subdivide[0, 12, N - 1];
x[s_] := s;
y[s_] := 1.2 Sin[0.7 s] + 0.2 Sin[2.2 s];

P = Table[{x[si], y[si]}, {si, s}];

signedCurvature3[p0_, p1_, p2_] := Module[{v01, v02, v12, a, b, c, area2, denom},
  v01 = p1 - p0; v02 = p2 - p0; v12 = p2 - p1;
  a = Norm[v12]; b = Norm[v02]; c = Norm[v01];
  area2 = v01[[1]] v02[[2]] - v01[[2]] v02[[1]]; (* = 2*A_signed *)
  denom = a b c;
  If[denom < 10^-12, 0.0, 2.0*area2/denom]
];

kappa = ConstantArray[0.0, N];
Do[
  kappa[[i]] = signedCurvature3[P[[i - 1]], P[[i]], P[[i + 1]]],
  {i, 2, N - 1}
];

kappaMaxSteer = Tan[deltaMax]/L;
Print["kappa_max_steer = ", N[kappaMaxSteer], " 1/m"];
Print["peak |kappa|    = ", N[Max[Abs[kappa]]], " 1/m"];
Print["steering-feasible? ", If[Max[Abs[kappa]] <= kappaMaxSteer, "YES", "NO"]];

vMax = Table[If[Abs[kappa[[i]]] < 10^-12, Infinity, Sqrt[aLatMax/Abs[kappa[[i]]]]], {i, 1, N}];

ListLinePlot[Transpose[{s, kappa}], PlotRange -> All, AxesLabel -> {"s", "kappa"}, PlotLabel -> "Curvature kappa(s)"]
ListLinePlot[Transpose[{s, vMax}], PlotRange -> All, AxesLabel -> {"s", "vMax"}, PlotLabel -> "Speed limit from lateral acceleration"]
      

Problem 1 (Unicycle curvature): Starting from \( \dot{x}=v\cos\theta \), \( \dot{y}=v\sin\theta \), \( \dot{\theta}=\omega \), prove that the geometric curvature of the traced path is \( \kappa=\omega/|v| \) for \( v\neq 0 \).

Solution: The path speed is \( \|\dot{\gamma}\| = \sqrt{\dot{x}^2+\dot{y}^2}=|v| \). Arc-length rate is \( ds/dt=|v| \). Therefore:

\[ \kappa = \frac{d\theta}{ds} = \frac{d\theta/dt}{ds/dt} = \frac{\omega}{|v|}. \]

Problem 2 (Bicycle steering limit): For a bicycle model \( \kappa=\tan\delta/L \) with \( |\delta|\le\delta_{\max} \), derive \( R_{\min} \).

Solution: Since \( |\kappa|\le\tan\delta_{\max}/L \), and \( R=1/|\kappa| \),

\[ R \ge \frac{1}{\tan\delta_{\max}/L} = \frac{L}{\tan\delta_{\max}} \triangleq R_{\min}. \]

Problem 3 (Lateral acceleration speed bound): Suppose \( a_{\text{lat}} \le a_{\text{lat,max}} \). For a path segment of constant curvature magnitude \( |\kappa| \), find the maximum constant speed.

Solution: With \( a_{\text{lat}} = v^2|\kappa| \), enforce the bound:

\[ v^2|\kappa| \le a_{\text{lat,max}} \quad \Rightarrow \quad v \le \sqrt{\frac{a_{\text{lat,max}}}{|\kappa|}}. \]

Problem 4 (Diff-drive feasibility under wheel saturation): Assume \( |\omega_r|\le\omega_{\max} \), \( |\omega_l|\le\omega_{\max} \), and forward motion \( v>0 \). Using \( \omega_r = \frac{v}{r_w}(1 + \frac{b}{2}\kappa) \) and \( \omega_l = \frac{v}{r_w}(1 - \frac{b}{2}\kappa) \), derive a sufficient bound on \( |\kappa| \) in terms of \( v \).

Solution: Require \( \left|1 \pm \frac{b}{2}\kappa\right| \le \alpha \), where \( \alpha=\omega_{\max}r_w/v \). If \( \alpha < 1 \), no solution exists with both wheels within bounds (speed too high). Otherwise, sufficient is

\[ \left|\frac{b}{2}\kappa\right| \le \alpha - 1 \quad \Rightarrow \quad |\kappa| \le \frac{2}{b}\left(\frac{\omega_{\max}r_w}{v} - 1\right). \]

Problem 5 (Three-point curvature estimator): Show that for three non-collinear points forming a triangle with side lengths \( a,b,c \) and area \( A \), the curvature of the circumcircle is \( \kappa = 4A/(abc) \).

Solution: The circumradius is a classical identity: \( R = abc/(4A) \). Since circle curvature is \( 1/R \), we get:

\[ \kappa = \frac{1}{R} = \frac{4A}{abc}. \]

We formalized curvature as the geometric quantity that determines “how hard the robot must turn,” and derived: (i) curvature formulas for planar curves, (ii) curvature-input mappings for unicycle, differential-drive, and bicycle/Ackermann kinematics, (iii) explicit curvature/turning-radius bounds from steering and wheel saturation, and (iv) speed-dependent feasibility constraints from lateral acceleration and steering-rate limits. These results are the foundation for the feasible path families introduced in Chapter 3, Lesson 3.