Unit 06 notes: Cooperative path following and reconfiguration

Draft for instructor review - not approved for teaching, assessment, translation, or physical deployment.

Purpose, prerequisites, and outcomes

Two survey robots can stay close to their assigned curves while losing their intended relative progress. A moving target can also remain ahead of a robot by design. We therefore specify three objectives separately: convergence to a geometric path, convergence to a moving target, and regulation of team progress. None establishes collision safety.

Prerequisites are planar rotations, unicycle kinematics, derivatives and elementary Lyapunov analysis. The shared student reference is maintained at repository path common/control_foundations/en/notes.qmd; its stability and invariant-set sections supply the recap needed here. The geometry and local calculations below are derived explicitly. Marine guidance context is provided by (Fossen 2011, Ch. 10); the imported browser models are pedagogical constructions, not implementations of a verified marine-controller theorem.

After studying this unit, students should be able to:

distinguish a geometric path from a timed trajectory and a moving target;
construct a path frame, convert parameter rate to physical speed, and derive projection and moving-target error dynamics;
check a dimensionally consistent local heading-rate controller and a conditional Cartesian target tracker;
interpret supplied leader/reference offsets, communication validity, and distinct team-error metrics; and
state feasibility, safety and supervision conditions for a bounded reconfiguration case.

Path-frame definitions

Use a right-handed planar mathematical frame with x to the right, y upward and positive heading counterclockwise. Positions are in metres, time in seconds and angles in radians. These conventions describe a plane; calling it a marine NED frame without an explicit coordinate and angle mapping would change the sign interpretation.

A path is an oriented curve without a required time law. A trajectory prescribes position at a particular time. Write an arc-length-parameterized path as $\mathbf r(\ell)\in\mathbb R^2$ , where $\ell$ [m] measures signed forward progress from a declared origin. Assume the curve is at least $C^2$ and regular. Its unit tangent and chosen left normal are

$\mathbf t=\frac{d\mathbf r}{d\ell} =\begin{bmatrix}\cos\psi_p\\\sin\psi_p\end{bmatrix},\qquad \mathbf n=\begin{bmatrix}-\sin\psi_p\\\cos\psi_p\end{bmatrix},\qquad \mathbf R_p=[\mathbf t\ \mathbf n].$

Here $\psi_p$ is the tangent angle and $\mathbf R_p$ maps path-frame components to world-frame components. Therefore $\mathbf R_p^\top$ maps a world vector into tangent/normal coordinates. Signed curvature $\kappa$ [m $^{-1}$ ] satisfies

$\frac{d\mathbf t}{d\ell}=\kappa\mathbf n,\qquad \frac{d\mathbf n}{d\ell}=-\kappa\mathbf t,\qquad \frac{d\psi_p}{d\ell}=\kappa.$

Positive curvature turns toward the chosen left normal. By the chain rule, a target advancing with arc-length rate $\dot\ell$ has tangent angular rate

$\dot\psi_p=\kappa\dot\ell.$

Curvature measures turning per metre; curvature times physical path speed measures turning per second. A circle of radius $R=20$ m has $\kappa=1/R=0.05$ m $^{-1}$ under counterclockwise orientation. At $\dot\ell=2$ m/s, its frame turns at $0.1$ rad/s. A tangent change of $\pi/6$ rad covers $R\pi/6=10.472$ m. For a general path,

$\psi_p(\ell_2)-\psi_p(\ell_1)=\int_{\ell_1}^{\ell_2}\kappa(\ell)\,d\ell.$

Angle change alone cannot determine distance: a tight turn and a broad turn can have the same angle change. On a straight segment $\kappa=0$ , distance can increase while tangent angle stays constant. Inverting the angle to obtain distance requires a known curvature relation and an interval where that inversion is valid.

Arbitrary parameter versus arc length

Most implementations store an easier parameter than arc length. Let $\boldsymbol\rho(\xi)$ describe the same path, with $h(\xi)=\|\boldsymbol\rho'(\xi)\|>0$ . Then

$\frac{d\ell}{d\xi}=h(\xi),\qquad \dot\ell=h(\xi)\dot\xi,\qquad \dot\psi_p=\kappa(\xi)h(\xi)\dot\xi.$

The signed speed is $\dot\ell$ and the speed magnitude is $|\dot\ell|$ . For $\boldsymbol\rho(\xi)=[x(\xi),y(\xi)]^\top$ ,

$\kappa(\xi)=\frac{x'y''-y'x''}{(x'^2+y'^2)^{3/2}}.$

All derivatives in this formula are with respect to $\xi$ . Regularity excludes points where both derivatives vanish. It does not exclude zero curvature.

The imported multi-robot Version 4 uses horizontal station, called s in its code. We denote it $\xi$ to distinguish it from arc length. Its sine route is

$\boldsymbol\rho_i(\xi)= \begin{bmatrix}\xi\\ b_i+7\sin((\xi-4-a_i)/22)\end{bmatrix}\ \text{m},$

when station and the numerical constants 4, 7, 22 and $a_i,b_i$ are expressed in metres. Here $b_i$ is the lane offset and $a_i$ is a horizontal shift of the sine pattern. The spatial period is $44\pi\approx138.23$ m; 22 m is the sine’s length scale, not its full wavelength. With $f_i(\xi)$ denoting the second component,

$f_i'=\frac7{22}\cos((\xi-4-a_i)/22),\qquad h_i=\sqrt{1+f_i'^2},\qquad v_{\mathrm{VT},i}=|\dot\xi_i|h_i.$

Worked parameter check. At $\xi=4+a_i$ , $f_i'=7/22$ , so $h_i=1.04939967$ . A station rate of 1.5 m/s gives target path speed 1.57409951 m/s. Conversely, nominal path speed 1.55 m/s requires a free-running station rate $1.55/h_i=1.47703495$ m/s. Feedback, limits and reference tracking can change that rate, so the simulator’s nominal pace is not its realized target speed. A lane offset changes vertical position, a sine shift moves peaks horizontally, and a desired progress lag defines a separate team objective.

Closest-point projection and error dynamics

First choose the path point as the robot’s closest projection, within a neighborhood where that projection is unique. Its tangential position error is zero, so

$\mathbf p=\mathbf r(\ell)+e\mathbf n(\ell),$

where $e$ [m] is positive on the left side of the oriented curve. Differentiating gives

$\dot{\mathbf p}=(1-\kappa e)\dot\ell\,\mathbf t+\dot e\,\mathbf n.$

For the ideal unicycle

$\dot{\mathbf p}=v\begin{bmatrix}\cos\psi\\\sin\psi\end{bmatrix},\qquad \dot\psi=\omega,$

$v$ [m/s] is forward speed and $\omega$ [rad/s] is yaw rate. Define a continuous local heading-error coordinate $\tilde\psi=\psi-\psi_p(\ell)$ . Resolving velocity along $\mathbf t$ and $\mathbf n$ yields

$\dot e=v\sin\tilde\psi,\qquad \dot\ell=\frac{v\cos\tilde\psi}{1-\kappa e},\qquad \dot{\tilde\psi}=\omega-\kappa\dot\ell.$

The projection’s $\dot\ell$ is determined by robot motion and geometry. It is not an independently assignable target speed. A smooth path and $1-\kappa e>0$ avoid a local coordinate singularity, but do not alone prove global uniqueness of the nearest point. Nearby branches and self-intersections can make projection ambiguous. Piecewise-linear corners lack a well-defined tangent/curvature at the corner and require a declared switching or smoothing procedure.

Straight-path sign check. For $\mathbf r(\ell)=[\ell,0]^\top$ , $\kappa=0$ and $e=y$ . At $v=1$ m/s and $\tilde\psi=0.2$ rad, $\dot e=0.19866933$ m/s and $\dot\ell=0.98006658$ m/s. A positive heading error initially increases the positive cross-track error. For a counterclockwise circle, the normal points inward; the factor $1-e/R$ vanishes at the circle centre, where the closest point is not unique.

Stabilizing law and a local convergence claim

Let $L>0$ be a fixed error-normalization length [m] and $k_\psi>0$ a heading-error gain [s $^{-1}$ ]. Choose the dimensionless function

$V=\frac12\left(\frac eL\right)^2+\frac12\tilde\psi^2$

and define $\operatorname{sinc}(z)=\sin z/z$ with $\operatorname{sinc}(0)=1$ . Select

$\omega=\kappa\dot\ell-k_\psi\tilde\psi-\frac{ev}{L^2}\operatorname{sinc}(\tilde\psi).$

Each term in $\omega$ has units s $^{-1}$ , with angles expressed in radians. Taking $L=1$ m reproduces the previous numerical coefficients when lengths are entered in metres; $L$ is a normalization scale, not a lookahead distance. Without it, adding squared metres to squared heading error and using $ev$ as an angular rate hides a dimensional error.

For constant $v>0$ , exact continuous-time actuation and no saturation,

$\begin{aligned} \dot V &=\frac{e}{L^2}\dot e+\tilde\psi\dot{\tilde\psi}\\ &=\frac{ev}{L^2}\sin\tilde\psi+ \tilde\psi\left[-k_\psi\tilde\psi-\frac{ev}{L^2}\operatorname{sinc}(\tilde\psi)\right]\\ &=-k_\psi\tilde\psi^2\le0. \end{aligned}$

A nonpositive derivative does not by itself establish attraction. On $\dot V=0$ we have $\tilde\psi=0$ , but there

$\dot{\tilde\psi}=-ev/L^2.$

Consequently only $e=\tilde\psi=0$ remains invariant. Apply LaSalle to the autonomous error dynamics on a compact positively invariant sublevel of $V$ contained inside a neighborhood with valid unique projection, $1-\kappa e>0$ and a continuous local heading-error coordinate. Such a neighborhood and sufficient heading-rate authority are assumptions of this local claim. The along-path coordinate itself need not be compact. If an error sublevel lies within the valid domain and commands remain feasible there, nonincrease of $V$ keeps its errors in that sublevel. Merely knowing $v(t)>0$ for a varying speed does not establish the constant-speed argument used here.

For $e=0.8$ m, $L=2$ m, $v=1.5$ m/s, $k_\psi=0.7$ s $^{-1}$ , $\tilde\psi=0.3$ rad and a straight path, the requested yaw rate is $-0.50552021$ rad/s and $\dot V=-0.063$ s $^{-1}$ . A yaw-limited robot may be unable to execute that request, so its actual $\dot V$ need not equal the ideal expression.

The committed scripts/generate_path_following.py plot uses a different, bounded heading choice on a straight path. It checks signs and simple path convergence; it does not numerically validate this heading-rate law.

An independently evolving virtual target

Now choose a reference point $\mathbf r(\ell(t))$ whose progress is an independent state, rather than the closest projection. Define the vehicle-minus-target error in the moving path frame:

$\begin{bmatrix}e_x\\e_y\end{bmatrix} =\mathbf R_p^\top[\mathbf p-\mathbf r(\ell)].$

Both components can be nonzero. Using $\dot{\mathbf R}_p=\mathbf R_p\mathbf S\kappa\dot\ell$ , where $\mathbf S=\begin{bmatrix}0&-1\\1&0\end{bmatrix}$ , gives

$\begin{aligned} \dot e_x&=v\cos\tilde\psi-\dot\ell+\kappa\dot\ell e_y,\\ \dot e_y&=v\sin\tilde\psi-\kappa\dot\ell e_x,\\ \dot{\tilde\psi}&=\omega-\kappa\dot\ell. \end{aligned}$

The terms involving curvature arise because the error frame rotates. If $\ell$ is supplied by an independent target law, use these two position-error equations. If the point is the closest projection, impose $e_x=\dot e_x=0$ and solve the first equation for $\dot\ell$ to recover the projection model above. These constructions cannot be interchanged. For an arbitrary station $\xi$ , replace $\dot\ell$ with $h(\xi)\dot\xi$ everywhere.

Cartesian tracking with target-velocity feedforward

A simple fully actuated reference calculation uses the opposite error convention,

$\boldsymbol\epsilon=\boldsymbol\rho(\xi)-\mathbf p,\qquad \mathbf v_{\mathrm{cmd}}=\boldsymbol\rho'(\xi)\dot\xi+\mathbf K\boldsymbol\epsilon,$

where $\boldsymbol\epsilon$ is in world coordinates and $\mathbf K$ is a constant symmetric positive-definite gain matrix [s $^{-1}$ ]. If the vehicle executes $\dot{\mathbf p}=\mathbf v_{\mathrm{cmd}}$ exactly, then

$\dot{\boldsymbol\epsilon}=-\mathbf K\boldsymbol\epsilon,\qquad \|\boldsymbol\epsilon(t)\|\le e^{-\lambda_{\min}(\mathbf K)t}\|\boldsymbol\epsilon(0)\|.$

Target-velocity feedforward cancels motion of the reference. Proportional position feedback then removes the remaining position error. For $\mathbf K=0.4\mathbf I$ s $^{-1}$ and initial target error $[3,4]^\top$ m, the ideal norm after 5 s is $5e^{-2}=0.67667642$ m.

A unicycle-like simulator instead asks for heading $\operatorname{atan2}(v_{\mathrm{cmd},y},v_{\mathrm{cmd},x})$ and speed $\|\mathbf v_{\mathrm{cmd}}\|$ , then applies speed/yaw limits. If the realized velocity differs by $\mathbf d_v=\dot{\mathbf p}-\mathbf v_{\mathrm{cmd}}$ , the actual equation is

$\dot{\boldsymbol\epsilon}=-\mathbf K\boldsymbol\epsilon-\mathbf d_v.$

Thus exact exponential convergence is a conditional ideal calculation. Heading transients, propulsion reduction, current and saturation can create tracking error. The zero-command direction also requires an implementation convention. This construction does not prove stability of an underactuated hydrodynamic marine vehicle.

The early imported simulator kept an intended preview gap of 9.5 m; tracking that gap is a different objective from driving target distance to zero. The recovered V4 offers both modes. In this unit use Track actual virtual target, and keep preview only as a contrast between specifications.

Multi-vehicle path offsets and supplied reference

A transparent in-scope architecture supplies timestamped arc-length reference $\ell_r(t)$ and fixed offsets $\Delta_i$ [m]. Define

$e_{\ell,i}=\ell_i-\ell_r-\Delta_i.$

Negative $\Delta_i$ means behind the reference. Under the ideal reduced interface $\dot\ell_i=u_{\ell,i}$ , an unsaturated command

$u_{\ell,i}=\dot\ell_r-k_\ell e_{\ell,i},\qquad k_\ell>0$

gives $\dot e_{\ell,i}=-k_\ell e_{\ell,i}$ . This is a supplied-reference tracking calculation: each vehicle needs the reference, its own progress, the fixed offset and a feasible progress input. A physical path follower must convert the progress request into admissible speed/heading commands. Clipping that request, stale reference data or a failed local tracker changes the scalar error equation. A target being synchronized does not imply the physical robot has reached it.

For $\ell_r=30$ m, desired follower offset $\Delta_i=-8$ m and actual progress $\ell_i=19$ m, the progress error is $-3$ m: the follower is behind its assigned 22 m station. With reference rate 1.2 m/s and $k_\ell=0.2$ s $^{-1}$ the ideal request is 1.8 m/s. If authority is limited to 1.5 m/s, use the limited request and report the changed error dynamics.

The browser V4 uses station $\xi$ rather than arc length and offers Leader broadcast (one-way). For this course use only that supplied-reference/leader subset. On different route shapes equal station does not mean equal travelled distance, a rigid Euclidean formation, aligned peaks or collision-free motion. On a sine wave, changing $a_i$ moves peaks even when station lags stay zero.

Every reference packet needs the path ID/version, progress definition, units, frame, timestamp/clock basis, derivative, sequence and validity. When age exceeds a declared threshold, stop trying to close progress error using that stale reference. Enter a specified slow/hold/loiter/surface/abort policy appropriate to the platform, while preserving only locally valid behavior. A toy communication switch supplies no validated operational timeout.

Simulator metrics, faults, and a reproducible comparison

The maintained browser collection is at repository root html/, with multirobot.html for the recovered V4 and los.html for the dedicated LOS model. Use the portal to launch them. Paths are given here as repository locations because the root portal and rendered course notes have different publication roots.

Define metrics before accepting a plot label. For $N\ge2$ robots, desired positive station lag $d_i$ behind robot 1, actual x coordinate $x_i$ and target station $\xi_i$ , V4 calculates

$E_x=\sqrt{\frac1{N-1}\sum_{i=2}^N(x_i-x_1+d_i)^2},\qquad E_\xi=\sqrt{\frac1{N-1}\sum_{i=2}^N(\xi_i-\xi_1+d_i)^2},$

$E_{\mathrm{target}}=\sqrt{\frac1N\sum_{i=1}^N \|\boldsymbol\rho_i(\xi_i)-\mathbf p_i\|^2}.$

$E_x$ is actual x-station alignment RMS, not complete physical formation error; $E_\xi$ is target-station alignment RMS; $E_{\mathrm{target}}$ measures local moving-target tracking. In the archived import, “Physical formation RMS” and CSV physical_RMS_m refer to $E_x$ . The maintained copy names the columns vehicle_x_station_RMS_m, target_x_station_RMS_m and vehicle_to_target_RMS_m to make these definitions explicit. The code’s mean path-error approximation is

$\bar e_{\mathrm{approx}}=\frac1N\sum_i \frac{|y_i-f_i(x_i)|}{\sqrt{1+f_i'(x_i)^2}}.$

This is a local tangent-distance approximation for a graph path, rather than an exact closest-point distance on a general curve. The logged minimum separation is

$d_{\min}=\min_{i<j}\|\mathbf p_i-\mathbf p_j\|.$

It measures point-centre distance; no hull radii are included. The maintained CSV names the path approximation approx_cross_track_absolute_mean_m and separation min_vehicle_point_separation_m. The archive’s older min_hull_separation_m name is misleading and must not be read as a hull-clearance measurement. Two robots with x errors $-2$ m and $+1$ m relative to their prescribed slots give $E_x=\sqrt{5/2}=1.58113883$ m, regardless of their lateral error. A small $E_x$ can therefore coexist with poor geometry or collision risk.

V4’s fault slider multiplies achieved speed on the selected robot after its scripted event time (18 s in this version). It is a speed multiplier, not an electrical-power, thruster or hydrodynamic fault model. The optional outage at 58 s disables the reference-coupling contribution while retaining local target following; it does not simulate individual delayed packets, clocks, jitter or a physical radio/acoustic channel. No collision avoidance, environmental current or hull dynamics is included.

For an ungraded 12-minute comparison, select three robots, the mixed-path preset, Leader broadcast (one-way) and Track actual virtual target. Record nominal speed 1.55 m/s, each pace multiplier, path choices, shifts, lane offsets, lags, gain, initial state, fault settings and simulator version. First disable faults and compare independent target following with supplied-leader operation. Then repeat matched runs with the same selected robot and speed multiplier fault; finally test the reference switch separately. Reset between comparisons. Export CSV and describe $E_x$ , $E_\xi$ , $E_{\mathrm{target}}$ , actual/target speeds and minimum centre separation together. Predict which error each change should affect before running it. Do not use the preserved transcript’s numerical outcomes as checked answers for a different version or scenario.

The dedicated LOS simulator uses closest-path geometry and a tangent lookahead construction. V4’s “LOS” baseline instead aims at the route point at horizontal station $x_i+9.5$ m. Those geometries and plant abstractions differ: a cross-simulator comparison cannot isolate a guidance algorithm unless the relevant conditions are matched.

Bounded grid reconfiguration: optional extension

The optional hybrid case uses fixed role-to-cell mappings. A discrete supervisor permits a move only when the destination and swept region are clear, localization is valid, and timeout/deadlock checks pass. The continuous controller then realizes the motion. This is a safety-focused abstraction of the legacy grid material; it does not claim deadlock freedom without a complete proof. The 120-minute plan prioritizes the path/target/reference distinction; grid reconfiguration remains an optional appendix or follow-up activity.

Assumptions, failure cases, and summary

Check smoothness and regularity, projection uniqueness, heading branch, scale and units, turn/speed authority, sampling, pose error, reference age and degraded mode. Test straight and curved paths, ambiguous projection, speed zero/reversal, infeasible targets, saturation and reference outage. Log requested and executed inputs beside errors. Collision safety requires its own geometry, sensing/delay/actuation conditions and safe-set argument; decreasing a tracking function is insufficient.

Path following regulates geometry; moving-target tracking adds a specified reference evolution; shared progress adds a team objective. The equations, parameter definitions, validity domain and measurement definitions must accompany every claim. Core marine background is (Fossen 2011, Ch. 10). All detailed equations and numerical examples above are derived here; imported conversations identify teaching ideas and model history. ../source_map.yml records provenance and limitations. English remains a technical-review draft; Croatian synchronization awaits English approval.

References

Fossen, Thor I. 2011. Handbook of Marine Craft Hydrodynamics and Motion Control. 1st ed. John Wiley & Sons. https://doi.org/10.1002/9781119994138.