Goal: compute trajectories for a small team of pursuit agents that drive a swarm of evading targets to a goal region (the origin), where the targets follow nonlinear repulsive dynamics and cannot be controlled directly — only nudged through the pursuers' positions.
Consider a group of autonomous pursuit agents (shepherds) tasked with driving a swarm of evading targets (sheep) toward a goal region. Targets don't follow commands — they simply flee from nearby pursuers via short-range repulsive forces.
The challenge is cooperative: pursuers must coordinate their positions so that the net repulsive field experienced by the swarm points toward the origin. Poor coordination scatters the targets; good coordination funnels them inward.
We solve this as an optimal control problem using iterative LQR (iLQR), linearizing the nonlinear repulsive dynamics at each step and computing locally optimal pursuit trajectories that successfully contain and guide the swarm.
Targets are repelled from pursuers by an inverse-square force, giving nonlinear dynamics $x_{t+1} = F(x_t, u_t)$ with
At each step we linearize $F$ around the nominal trajectory $(\bar{x}_t, \bar{u}_t)$, taking Jacobians $A_t = \partial F/\partial x$ and $B_t = \partial F/\partial u$, and solve a quadratic LQR problem on the resulting local linear system. The optimal cost-to-go $V_t(x) = x^\top P_t x$ is found by sweeping the discrete-time Riccati equation backward:
The feedback law $u_t^\star = -K_t x_t$ is read off from $P_{t+1}$. Iterating—re-linearize, re-solve, roll forward—converges to a locally optimal trajectory that corrals the targets to the origin.
Two pursuit agents corral two targets to the origin. The iLQR controller computes efficient trajectories that coordinate the pursuers without scattering the targets.