We consider a swarm of \( N \) identical point robots moving in a 2D workspace. Time is discretized as \( k = 0,1,2,\dots \), and each robot \( i \) has position \( p_i(k) \in \mathbb{R}^2 \). The discrete-time single-integrator model is

\[ p_i(k+1) = p_i(k) + h\,u_i(k), \quad i = 1,\dots,N, \]

where \( h > 0 \) is the time step and \( u_i(k) \in \mathbb{R}^2 \) is the velocity command computed from local information.

In this lab you will:

• Formalize neighbor sets and interaction graphs using a disk sensing model.
• Implement a consensus-like aggregation policy with optional repulsion/dispersion.
• Study stability conditions and simple performance metrics (aggregation error, coverage).
• Implement the simulator in multiple languages and relate it to common robotics toolchains (ROS, MASON, Simulink, etc.).

We assume familiarity with basic graph Laplacians and consensus dynamics from Chapter 14, and with rigid-body kinematics and dynamics from the core robotics course.

Robots sense neighbors within a disk of radius \( R > 0 \). The neighbor set of agent \( i \) at time \( k \) is

\[ \mathcal{N}_i(k) = \left\{ j \in \{1,\dots,N\} \setminus \{i\} \,\middle|\, \left\| p_j(k) - p_i(k) \right\| \le R \right\}. \]

Define a (possibly time-varying) adjacency matrix \( A(k) = [a_{ij}(k)] \), where \( a_{ij}(k) \ge 0 \), \( a_{ij}(k) > 0 \) only if \( j \in \mathcal{N}_i(k) \) and \( a_{ii}(k) = 0 \). The standard graph Laplacian is

\[ L_{ij}(k) = \begin{cases} \displaystyle \sum_{\ell=1}^N a_{i\ell}(k) & \text{if } i = j,\\[0.7em] -a_{ij}(k) & \text{if } i \ne j. \end{cases} \]

We stack positions into a vector \( \mathbf{p}(k) = [p_1(k)^\top,\dots,p_N(k)^\top]^\top \in \mathbb{R}^{2N} \). A purely consensus-based local policy sets

\[ u_i(k) = \sum_{j \in \mathcal{N}_i(k)} a_{ij}(k)\bigl(p_j(k) - p_i(k)\bigr), \]

which in matrix form becomes

\[ \mathbf{p}(k+1) = \bigl(\mathbf{I} - h\,\mathbf{L}(k)\bigr)\mathbf{p}(k) \triangleq \mathbf{W}(k)\mathbf{p}(k). \]

To model more swarm-like behavior (aggregation with short-range repulsion), we extend the control:

\[ u_i(k) = w_{\text{att}} \sum_{j \in \mathcal{N}_i(k)} \bigl(p_j(k) - p_i(k)\bigr) + w_{\text{rep}} \sum_{j \in \mathcal{N}_i(k)} \phi\bigl(p_i(k)-p_j(k)\bigr), \]

where \( w_{\text{att}}, w_{\text{rep}} \ge 0 \) are tunable gains and \( \phi \) is a short-range repulsive function such as

\[ \phi(z) = \begin{cases} \displaystyle \frac{z}{\|z\|^3} & \text{if } \|z\| \le d_{\min},\\[0.6em] 0 & \text{otherwise,} \end{cases} \]

for some small distance \( d_{\min} > 0 \). The attraction term drives consensus, while the repulsion term prevents collisions and enables dispersion-like behavior.

To analyze stability, we temporarily ignore repulsion and time-variation and consider a fixed, undirected, connected interaction graph with Laplacian \( \mathbf{L} \) and update

\[ \mathbf{p}(k+1) = \mathbf{W}\mathbf{p}(k), \quad \mathbf{W} = \mathbf{I} - h\,\mathbf{L}. \]

3.1 Invariance of the swarm centroid

Define the centroid \( \bar{p}(k) \in \mathbb{R}^2 \) as

\[ \bar{p}(k) = \frac{1}{N}\sum_{i=1}^N p_i(k) = \frac{1}{N}\mathbf{1}^\top \mathbf{p}(k), \]

where \( \mathbf{1} \) is the all-ones vector in \( \mathbb{R}^N \). Using \( \mathbf{L}\mathbf{1} = \mathbf{0} \) and symmetry, we have \( \mathbf{1}^\top \mathbf{L} = \mathbf{0}^\top \) and thus \( \mathbf{1}^\top \mathbf{W} = \mathbf{1}^\top \). Therefore

\[ \bar{p}(k+1) = \frac{1}{N}\mathbf{1}^\top \mathbf{p}(k+1) = \frac{1}{N}\mathbf{1}^\top \mathbf{W}\mathbf{p}(k) = \frac{1}{N}\mathbf{1}^\top \mathbf{p}(k) = \bar{p}(k). \]

Hence the centroid is invariant under the consensus dynamics: the swarm aggregates around its initial centroid.

3.2 Step-size constraint for stability

Let the eigenvalues of \( \mathbf{L} \) be \( 0 = \lambda_1 \le \lambda_2 \le \dots \le \lambda_N \). The eigenvalues of \( \mathbf{W} \) are then \( \mu_i = 1 - h\lambda_i \). For consensus, we require \( |\mu_1| = 1 \) (to preserve the centroid) and \( |\mu_i| < 1 \) for \( i \ge 2 \) so that all disagreement modes decay. For \( i \ge 2 \):

\[ |\mu_i| = |1 - h\lambda_i| < 1 \iff -1 < 1 - h\lambda_i < 1. \]

From \( -1 < 1 - h\lambda_i \) we obtain \( -2 < -h\lambda_i \) and hence \( h\lambda_i < 2 \). From \( 1 - h\lambda_i < 1 \) we obtain \( -h\lambda_i < 0 \) and thus \( h\lambda_i > 0 \). Combining gives the familiar condition

\[ 0 < h < \frac{2}{\lambda_i}, \quad i \ge 2. \]

Enforcing this for all \( i \ge 2 \) yields

\[ 0 < h < \frac{2}{\lambda_{\max}(\mathbf{L})}, \]

which guarantees that all disagreement modes shrink and the swarm positions converge to a consensus configuration with centroid equal to \( \bar{p}(0) \).

In the simulator, you will choose \( h \) small enough to ensure numerical stability and visually smooth trajectories.

All implementations in this lab follow the same abstract simulation loop:

flowchart TD
  S["Start lab"] --> P["Set N, dt, R, weights"]
  P --> I["Initialize agent positions"]
  I --> L["For each time step"]
  L --> NBR["Compute neighbors for each agent"]
  NBR --> CTRL["Evaluate local control law"]
  CTRL --> UPD["Update positions and apply saturation"]
  UPD --> LOG["Log data for plots"]
  LOG --> CHK["Check stop condition"]
  CHK -->|continue| L
  CHK -->|stop| END["Analyze metrics and visualize"]
        

For a swarm of moderate size (e.g., \( N \le 200 \)), a naive neighbor search with quadratic complexity in \( N \) is adequate. More advanced spatial indexing (grids, k-d trees) can be added as an extension.

Python is convenient for rapid prototyping and visualization. A common robotics stack would combine this kind of swarm simulator with ROS 2 (rclpy) and a simulator such as Gazebo or Webots, but here we keep a minimal dependency set: numpy for computation and matplotlib for plotting.

import numpy as np
import matplotlib.pyplot as plt

def initialize_positions(N, box_size=10.0, seed=0):
    rng = np.random.default_rng(seed)
    # Positions: shape (N, 2)
    return box_size * (rng.random((N, 2)) - 0.5)

def compute_neighbors(positions, R):
    N = positions.shape[0]
    neighbors = [[] for _ in range(N)]
    R2 = R * R
    for i in range(N):
        for j in range(i + 1, N):
            d = positions[j] - positions[i]
            dist2 = np.dot(d, d)
            if dist2 <= R2:
                neighbors[i].append(j)
                neighbors[j].append(i)
    return neighbors

def step_swarm(positions, h, R, w_att, w_rep, d_min, v_max):
    N = positions.shape[0]
    neighbors = compute_neighbors(positions, R)
    vel = np.zeros_like(positions)

    for i in range(N):
        p_i = positions[i]
        att = np.zeros(2)
        rep = np.zeros(2)
        for j in neighbors[i]:
            p_j = positions[j]
            diff = p_j - p_i
            att += diff
            # Short-range repulsion
            dist2 = np.dot(diff, diff)
            if dist2 > 1e-12:
                dist = np.sqrt(dist2)
                if dist <= d_min:
                    rep += -diff / (dist**3)
        u_i = w_att * att + w_rep * rep
        vel[i] = u_i

    # Saturate velocities
    speeds = np.linalg.norm(vel, axis=1)
    mask = speeds > v_max
    vel[mask] *= (v_max / speeds[mask])[:, None]

    # Euler integration
    new_positions = positions + h * vel
    return new_positions

def run_simulation(N=50, steps=500, h=0.05, R=2.0,
                   w_att=0.2, w_rep=0.05, d_min=0.5, v_max=0.5):
    positions = initialize_positions(N)
    traj = [positions.copy()]

    for k in range(steps):
        positions = step_swarm(positions, h, R, w_att, w_rep, d_min, v_max)
        traj.append(positions.copy())
    return np.array(traj)  # shape (steps+1, N, 2)

def plot_trajectories(traj, stride=10):
    steps, N, _ = traj.shape
    plt.figure()
    for i in range(N):
        plt.plot(traj[::stride, i, 0], traj[::stride, i, 1], alpha=0.6)
        plt.scatter(traj[-1, i, 0], traj[-1, i, 1], s=10)
    plt.xlabel("x")
    plt.ylabel("y")
    plt.title("Swarm trajectories (subsampled)")
    plt.axis("equal")
    plt.grid(True)
    plt.show()

if __name__ == "__main__":
    traj = run_simulation()
    plot_trajectories(traj)
      

You are encouraged to experiment with different values of R, w_att, and w_rep to observe transitions between tight aggregation and more dispersed configurations.

C++ is the dominant language in many robotics stacks (e.g., ROS 2 rclcpp, Gazebo, ARGoS). The following snippet implements the same swarm core without any external robotics framework, suitable for later embedding in a ROS node or a physics-based simulator.

#include <vector>
#include <cmath>
#include <iostream>

struct Vec2 {
    double x{0.0}, y{0.0};
    Vec2() = default;
    Vec2(double x_, double y_) : x(x_), y(y_) {}
    Vec2& operator+=(const Vec2& o) { x += o.x; y += o.y; return *this; }
    Vec2 operator+(const Vec2& o) const { return Vec2{x + o.x, y + o.y}; }
    Vec2 operator-(const Vec2& o) const { return Vec2{x - o.x, y - o.y}; }
    Vec2 operator*(double s) const { return Vec2{x * s, y * s}; }
};

double norm2(const Vec2& v) { return v.x * v.x + v.y * v.y; }

int main() {
    const int N = 50;
    const double h = 0.05;
    const double R = 2.0;
    const double w_att = 0.2;
    const double w_rep = 0.05;
    const double d_min = 0.5;
    const double v_max = 0.5;
    const int steps = 500;

    std::vector<Vec2> pos(N);
    std::vector<Vec2> vel(N);

    // Simple initialization: agents on a circle
    for (int i = 0; i < N; ++i) {
        double theta = 2.0 * M_PI * static_cast<double>(i) / N;
        pos[i] = Vec2{5.0 * std::cos(theta), 5.0 * std::sin(theta)};
    }

    for (int k = 0; k < steps; ++k) {
        // Zero velocities
        for (int i = 0; i < N; ++i) {
            vel[i] = Vec2{0.0, 0.0};
        }

        // Compute local controls
        for (int i = 0; i < N; ++i) {
            Vec2 p_i = pos[i];
            Vec2 att{0.0, 0.0};
            Vec2 rep{0.0, 0.0};
            for (int j = 0; j < N; ++j) {
                if (j == i) continue;
                Vec2 diff = Vec2{pos[j].x - p_i.x, pos[j].y - p_i.y};
                double d2 = norm2(diff);
                if (d2 <= R * R) {
                    att += diff;
                    if (d2 > 1e-12) {
                        double d = std::sqrt(d2);
                        if (d <= d_min) {
                            // Short-range repulsion
                            double scale = -1.0 / (d * d * d);
                            rep += diff * scale;
                        }
                    }
                }
            }
            Vec2 u_i = att * w_att + rep * w_rep;
            vel[i] = u_i;
        }

        // Saturate velocities and integrate
        for (int i = 0; i < N; ++i) {
            double speed2 = norm2(vel[i]);
            if (speed2 > v_max * v_max) {
                double speed = std::sqrt(speed2);
                double scale = v_max / speed;
                vel[i] = vel[i] * scale;
            }
            pos[i].x += h * vel[i].x;
            pos[i].y += h * vel[i].y;
        }
    }

    // Output final positions (for plotting elsewhere)
    for (int i = 0; i < N; ++i) {
        std::cout << pos[i].x << " " << pos[i].y << "\n";
    }

    return 0;
}
      

This core can be integrated into a ROS 2 node publishing agent poses as geometry_msgs::msg::PoseArray, enabling visualization in RViz or coupling to other nodes.

Java is often used for agent-based simulations using frameworks such as MASON or Repast. The following skeleton illustrates a pure Java implementation that could serve as the core step function in such frameworks.

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

class Agent {
    double x, y;
    double vx, vy;

    Agent(double x, double y) {
        this.x = x;
        this.y = y;
        this.vx = 0.0;
        this.vy = 0.0;
    }
}

public class SwarmSim {

    static List<Agent> initializeAgents(int N, double boxSize, long seed) {
        Random rng = new Random(seed);
        List<Agent> agents = new ArrayList<>(N);
        for (int i = 0; i < N; i++) {
            double x = boxSize * (rng.nextDouble() - 0.5);
            double y = boxSize * (rng.nextDouble() - 0.5);
            agents.add(new Agent(x, y));
        }
        return agents;
    }

    static void step(List<Agent> agents, double h, double R,
                     double wAtt, double wRep, double dMin, double vMax) {
        int N = agents.size();
        double R2 = R * R;

        // Reset velocities
        for (Agent a : agents) {
            a.vx = 0.0;
            a.vy = 0.0;
        }

        // Compute controls
        for (int i = 0; i < N; i++) {
            Agent ai = agents.get(i);
            double attX = 0.0, attY = 0.0;
            double repX = 0.0, repY = 0.0;

            for (int j = 0; j < N; j++) {
                if (j == i) continue;
                Agent aj = agents.get(j);
                double dx = aj.x - ai.x;
                double dy = aj.y - ai.y;
                double d2 = dx * dx + dy * dy;
                if (d2 <= R2) {
                    attX += dx;
                    attY += dy;
                    if (d2 > 1e-12) {
                        double d = Math.sqrt(d2);
                        if (d <= dMin) {
                            double scale = -1.0 / (d * d * d);
                            repX += dx * scale;
                            repY += dy * scale;
                        }
                    }
                }
            }
            ai.vx = wAtt * attX + wRep * repX;
            ai.vy = wAtt * attY + wRep * repY;
        }

        // Saturate and integrate
        for (Agent a : agents) {
            double speed2 = a.vx * a.vx + a.vy * a.vy;
            if (speed2 > vMax * vMax) {
                double speed = Math.sqrt(speed2);
                double scale = vMax / speed;
                a.vx *= scale;
                a.vy *= scale;
            }
            a.x += h * a.vx;
            a.y += h * a.vy;
        }
    }

    public static void main(String[] args) {
        int N = 50;
        double h = 0.05;
        double R = 2.0;
        double wAtt = 0.2;
        double wRep = 0.05;
        double dMin = 0.5;
        double vMax = 0.5;
        int steps = 500;

        List<Agent> agents = initializeAgents(N, 10.0, 0L);
        for (int k = 0; k < steps; k++) {
            step(agents, h, R, wAtt, wRep, dMin, vMax);
        }

        // Print final positions
        for (Agent a : agents) {
            System.out.println(a.x + " " + a.y);
        }
    }
}
      

A Java-based multi-agent framework would typically manage scheduling and visualization; your task here is to ensure the local policy is correctly implemented.

MATLAB is widely used in control and robotics (Robotics System Toolbox, Navigation Toolbox). The core swarm dynamics can be implemented as a MATLAB script, and then wrapped in a Simulink model using a MATLAB Function block to evaluate the local policy.

function swarm_lab_matlab()
    N     = 50;
    h     = 0.05;
    R     = 2.0;
    wAtt  = 0.2;
    wRep  = 0.05;
    dMin  = 0.5;
    vMax  = 0.5;
    steps = 500;

    % Initialize positions uniformly in a square
    rng(0);
    boxSize = 10.0;
    pos = boxSize * (rand(N, 2) - 0.5);

    traj = zeros(steps + 1, N, 2);
    traj(1, :, :) = pos;

    for k = 1:steps
        pos = step_swarm(pos, h, R, wAtt, wRep, dMin, vMax);
        traj(k + 1, :, :) = pos;
    end

    % Simple trajectory plot
    figure; hold on; grid on; axis equal;
    for i = 1:N
        xy = squeeze(traj(:, i, :));
        plot(xy(:, 1), xy(:, 2));
    end
    title('Swarm trajectories (MATLAB)');
    xlabel('x'); ylabel('y');
end

function pos_new = step_swarm(pos, h, R, wAtt, wRep, dMin, vMax)
    N = size(pos, 1);
    vel = zeros(N, 2);
    R2 = R^2;

    for i = 1:N
        p_i = pos(i, :);
        att = [0.0, 0.0];
        rep = [0.0, 0.0];
        for j = 1:N
            if j == i, continue; end
            diff = pos(j, :) - p_i;
            d2 = diff * diff';
            if d2 <= R2
                att = att + diff;
                if d2 > 1e-12
                    d = sqrt(d2);
                    if d <= dMin
                        rep = rep - diff / (d^3);
                    end
                end
            end
        end
        vel(i, :) = wAtt * att + wRep * rep;
    end

    % Saturate velocities
    speeds = sqrt(sum(vel.^2, 2));
    mask = speeds > vMax;
    vel(mask, :) = vel(mask, :) .* (vMax ./ speeds(mask));

    pos_new = pos + h * vel;
end
      

In Simulink, you can:

• Maintain positions in a state vector using a Unit Delay block.
• Use a MATLAB Function block implementing step_swarm to compute the next positions.
• Feed outputs to a Scope or custom visualization (e.g. using Simulink 3D Animation).

Wolfram Mathematica is well suited to concise vectorized implementations and symbolic experimentation (e.g., analyzing small Laplacians). Below is a simple discrete-time swarm implementation.

ClearAll[initializePositions, computeNeighbors, stepSwarm, runSwarm];

initializePositions[N_Integer, boxSize_: 10., seed_: 0] := Module[{pts},
  SeedRandom[seed];
  pts = boxSize (RandomReal[{-0.5, 0.5}, {N, 2}]);
  pts
];

computeNeighbors[pts_, R_] := Module[{N, neighbors, R2, i, j, d2},
  N = Length[pts];
  neighbors = Table[{}, {N}];
  R2 = R^2;
  For[i = 1, i <= N, i++,
    For[j = i + 1, j <= N, j++,
      d2 = Norm[pts[[j]] - pts[[i]]]^2;
      If[d2 <= R2,
        neighbors[[i]] = Append[neighbors[[i]], j];
        neighbors[[j]] = Append[neighbors[[j]], i];
      ];
    ];
  ];
  neighbors
];

stepSwarm[pts_, h_, R_, wAtt_, wRep_, dMin_, vMax_] := Module[
  {N, neighbors, vel, R2, i, j, pI, att, rep, diff, d2, d, speed},
  N = Length[pts];
  neighbors = computeNeighbors[pts, R];
  vel = ConstantArray[{0., 0.}, N];
  R2 = R^2;

  For[i = 1, i <= N, i++,
    pI = pts[[i]];
    att = {0., 0.};
    rep = {0., 0.};
    Do[
      diff = pts[[j]] - pI;
      d2 = diff.diff;
      If[d2 <= R2,
        att += diff;
        If[d2 > 1.*^-12,
          d = Sqrt[d2];
          If[d <= dMin,
            rep += -diff/(d^3);
          ];
        ];
      ];
      ,
      {j, neighbors[[i]]}
    ];
    vel[[i]] = wAtt att + wRep rep;
  ];

  (* Saturation *)
  vel = vel /. v_List :> Module[{speed = Norm[v]},
      If[speed > vMax, vMax v/speed, v]
    ];

  pts + h vel
];

runSwarm[N_: 50, steps_: 300, h_: 0.05, R_: 2., wAtt_: 0.2,
         wRep_: 0.05, dMin_: 0.5, vMax_: 0.5] :=
 Module[{pts, traj},
  pts = initializePositions[N];
  traj = {pts};
  Do[
    pts = stepSwarm[pts, h, R, wAtt, wRep, dMin, vMax];
    traj = Append[traj, pts];
    ,
    {steps}
  ];
  traj
];

traj = runSwarm[];
ListAnimate[
  ListPlot[traj[[k]], PlotRange -> { {-6, 6}, {-6, 6} }, 
    AxesLabel -> {"x", "y"}, AspectRatio -> 1] & /@ Range[Length[traj]]
]
      

Mathematica makes it easy to manipulate the Laplacian of small interaction graphs symbolically, which is helpful for verifying the consensus properties discussed earlier.

To move beyond qualitative visualization, we introduce quantitative metrics that measure aggregation and coverage.

10.1 Aggregation error

The aggregation error (or disagreement energy) at time \( k \) is

\[ J_{\text{agg}}(k) = \frac{1}{N}\sum_{i=1}^N \left\|p_i(k) - \bar{p}(k)\right\|^2, \quad \bar{p}(k) = \frac{1}{N}\sum_{i=1}^N p_i(k). \]

For pure consensus with a connected fixed graph and a stable step size, \( J_{\text{agg}}(k) \) is nonincreasing and converges to zero as \( k \to \infty \).

10.2 Coverage measure

A crude coverage metric is the area of the convex hull of agent positions:

\[ J_{\text{cov}}(k) = \operatorname{area} \bigl(\operatorname{conv}\{p_1(k),\dots,p_N(k)\}\bigr). \]

Aggregation corresponds to small \( J_{\text{cov}} \), while dispersion seeks larger \( J_{\text{cov}} \).

10.3 Experiment sketch

• Fix \( h \) and \( R \). Vary \( w_{\text{att}} \) from small to large, with \( w_{\text{rep}} = 0 \), and record \( J_{\text{agg}}(k) \) over time.
• Fix \( w_{\text{att}} \) and introduce \( w_{\text{rep}} > 0 \). Study the trade-off between convergence of \( J_{\text{agg}} \) and increase of \( J_{\text{cov}} \).
• Increase \( R \) gradually to approach a complete graph, and compare convergence rates numerically to the spectral gap \( \lambda_2(\mathbf{L}) \).

Problem 1 (Centroid invariance for row-stochastic weights). Consider the consensus dynamics \( \mathbf{p}(k+1) = \mathbf{W}\mathbf{p}(k) \) where \( \mathbf{W} \) is an \( N \times N \) matrix with nonnegative entries and \( \mathbf{W}\mathbf{1} = \mathbf{1} \). Show that the centroid \( \bar{p}(k) = \frac{1}{N}\mathbf{1}^\top\mathbf{p}(k) \) is invariant.

Solution.

\[ \bar{p}(k+1) = \frac{1}{N}\mathbf{1}^\top \mathbf{p}(k+1) = \frac{1}{N}\mathbf{1}^\top \mathbf{W}\mathbf{p}(k). \]

Using \( \mathbf{W}\mathbf{1} = \mathbf{1} \) and nonnegativity, \( \mathbf{1}^\top\mathbf{W} = \mathbf{1}^\top \) (each row sums to one). Hence

\[ \bar{p}(k+1) = \frac{1}{N}\mathbf{1}^\top\mathbf{p}(k) = \bar{p}(k). \]

Thus averaging is preserved for any row-stochastic weight matrix, including those arising from symmetric Laplacians via \( \mathbf{W} = \mathbf{I} - h\mathbf{L} \).

Problem 2 (Step-size condition for consensus dynamics). For a connected, undirected graph with Laplacian \( \mathbf{L} \), consider \( \mathbf{p}(k+1) = (\mathbf{I} - h\mathbf{L})\mathbf{p}(k) \). Derive the condition on \( h \) such that all eigenvalues except one lie strictly inside the unit circle.

Solution.

Let the eigenvalues of \( \mathbf{L} \) be \( 0 = \lambda_1 \le \lambda_2 \le \dots \le \lambda_N \). The eigenvalues of \( \mathbf{I} - h\mathbf{L} \) are \( \mu_i = 1 - h\lambda_i \).

\[ |\mu_i| = |1 - h\lambda_i| < 1 \iff -1 < 1 - h\lambda_i < 1. \]

As in Section 3, manipulation of inequalities yields \( 0 < h\lambda_i < 2 \) for all \( i \ge 2 \), i.e.

\[ 0 < h < \frac{2}{\lambda_i}, \quad i \ge 2. \]

Enforcing this for the worst case \( \lambda_N \) leads to \( 0 < h < 2/\lambda_{\max}(\mathbf{L}) \). Under this condition, \( \mu_1 = 1 \) (centroid mode) and \( |\mu_i| < 1 \) for \( i \ge 2 \), so consensus is reached.

Problem 3 (Aggregation error as a Lyapunov function). Consider the aggregation error \( J_{\text{agg}}(k) \) defined in Section 10 for the linear consensus dynamics with a fixed connected graph and admissible \( h \). Show that \( J_{\text{agg}}(k+1) \le J_{\text{agg}}(k) \).

Solution (sketch).

Let \( \tilde{\mathbf{p}}(k) = \mathbf{p}(k) - \mathbf{1}\bar{p}(k) \) be the disagreement vector. Since the centroid is invariant, \( \tilde{\mathbf{p}}(k+1) = \mathbf{W}\tilde{\mathbf{p}}(k) \). The aggregation error can be written as

\[ J_{\text{agg}}(k) = \frac{1}{N}\left\|\tilde{\mathbf{p}}(k)\right\|^2. \]

Using spectral decomposition of \( \mathbf{W} \) on the disagreement subspace (orthogonal to \( \mathbf{1} \)) with eigenvalues \( \mu_i \) satisfying \( |\mu_i| < 1 \), we obtain

\[ \left\|\tilde{\mathbf{p}}(k+1)\right\|^2 = \left\|\mathbf{W}\tilde{\mathbf{p}}(k)\right\|^2 \le \rho\bigl(\mathbf{W}^\top\mathbf{W}\bigr) \left\|\tilde{\mathbf{p}}(k)\right\|^2, \]

where \( \rho(\cdot) \) denotes spectral radius and \( \rho(\mathbf{W}^\top\mathbf{W}) < 1 \). Hence \( J_{\text{agg}}(k+1) \le J_{\text{agg}}(k) \), establishing Lyapunov monotonicity.

Problem 4 (Computational complexity of neighbor search). Suppose you simulate a swarm with \( N \) agents using a naive double loop to compute \( \mathcal{N}_i(k) \). What is the computational complexity per time step? How would a uniform grid with cell size on the order of \( R \) change this complexity (qualitatively)?

Solution.

A naive neighbor search checks all \( \binom{N}{2} \) pairs, so the complexity per time step is \( \mathcal{O}(N^2) \). With a uniform grid of cell size comparable to \( R \), each agent only needs to check agents in its own cell and adjacent cells. Under mild spatial uniformity assumptions, the average number of neighbors per cell is bounded, yielding expected complexity close to \( \mathcal{O}(N) \) per time step.

Problem 5 (Design flow for local policy parameters). You wish to design a swarm that aggregates quickly with mild short-range repulsion. Sketch a design-flow for choosing \( R \), \( h \), \( w_{\text{att}} \), and \( w_{\text{rep}} \).

Solution (flow chart).

flowchart TD
  OBJ["Select objective: fast aggregation"] --> RSEL["Choose R to ensure graph connectivity"]
  RSEL --> HSEL["Pick dt small enough for stability bound"]
  HSEL --> WATT["Increase attraction weight until convergence time is acceptable"]
  WATT --> WREP["Add small repulsion weight to avoid collisions"]
  WREP --> METR["Monitor aggregation error and coverage"]
  METR --> ADJ["Adjust parameters if collisions or slow convergence observed"]
        

This qualitative design flow complements the mathematical constraints \( 0 < h < 2/\lambda_{\max}(\mathbf{L}) \) and encourages systematic tuning rather than ad hoc parameter choices.

In this lab you implemented a swarm simulator with simple local interaction policies and connected it to consensus theory from multi-robot coordination. You derived a stability condition on the discrete-time step size, showed invariance of the swarm centroid, and introduced aggregation and coverage metrics. On the implementation side, you built multi-language realizations of the same dynamical model, preparing you to integrate swarm behaviors into realistic robotics middleware (ROS 2, Simulink, agent-based simulators, and Wolfram environments).

These techniques serve as a practical complement to the theoretical swarm behaviors discussed in earlier lessons of this chapter and form a basis for exploring more sophisticated local policies, probabilistic effects, and robustness in large-scale robot swarms.