In multi-robot coordination, a consensus protocol is a local rule that drives robot states (positions, headings, velocities, etc.) toward agreement using only neighbor information. Let \( N \) robots be indexed by \( i \in \{1,\dots,N\} \), and let \( x_i(t) \in \mathbb{R}^m \) denote the state of robot \( i \). A consensus protocol is typically of the form

\[ \dot{x}_i(t) = \sum_{j \in \mathcal{N}_i} a_{ij}\bigl(x_j(t) - x_i(t)\bigr), \quad a_{ij} \ge 0, \]

where \( \mathcal{N}_i \) is the neighbor set of robot \( i \) in the communication graph and \( a_{ij} \) is a weight encoding interaction strength. Under suitable graph-connectivity conditions, all states converge to a common value (often the average of initial states).

Formation control extends consensus by asking robots to converge not to a single point but to a geometric pattern, such as a line, circle, or rigid polygon, while still relying on neighbor interactions. A typical idea is to perform consensus on offset-corrected states, so that relative positions encode the desired shape.

flowchart TD
  S["Start with initial states x_i(0)"] --> NBR["Each robot reads neighbor states x_j"]
  NBR --> CTRL["Compute local consensus term sum_j a_ij (x_j - x_i)"]
  CTRL --> DYN["Apply control to update x_i"]
  DYN --> CHK["Check convergence or formation error"]
  CHK -->|not small| NBR
  CHK -->|small| DONE["Consensus / formation achieved"]
        

In this lesson we focus on continuous-time and discrete-time linear consensus protocols on undirected and directed graphs, and show how simple algebra transforms consensus into formation control with prescribed offsets.

We model communication by a directed or undirected graph \( \mathcal{G} = (\mathcal{V}, \mathcal{E}) \), where \( \mathcal{V} = \{1,\dots,N\} \) and \( (j,i) \in \mathcal{E} \) means that robot \( i \) receives information from robot \( j \). The weighted adjacency matrix \( A = [a_{ij}] \in \mathbb{R}^{N \times N} \) is defined by

\[ a_{ij} = \begin{cases} > 0 & \text{if } (j,i) \in \mathcal{E}, \\ 0 & \text{otherwise}. \end{cases} \]

The (out-)degree of node \( i \) is \( d_i = \sum_{j=1}^N a_{ij} \). Collecting degrees into a diagonal matrix \( D = \mathrm{diag}(d_1,\dots,d_N) \), the graph Laplacian is

\[ L = D - A \in \mathbb{R}^{N \times N}. \]

For an undirected graph with symmetric adjacency \( A = A^\top \), the Laplacian is symmetric positive-semidefinite and satisfies \( L \mathbf{1} = \mathbf{0} \), where \( \mathbf{1} \) is the all-ones vector.

Let \( x_i(t) \in \mathbb{R} \) be a scalar state (e.g., heading) and stack states into \( \mathbf{x}(t) = [x_1(t),\dots,x_N(t)]^\top \). The consensus dynamics

\[ \dot{x}_i(t) = \sum_{j=1}^N a_{ij}\bigl(x_j(t) - x_i(t)\bigr) \]

can be written compactly as

\[ \dot{\mathbf{x}}(t) = -L \mathbf{x}(t). \]

This is a linear time-invariant (LTI) system whose dynamics are governed by the Laplacian spectrum. For an undirected, connected graph, eigenvalues satisfy \( 0 = \lambda_1 < \lambda_2 \le \dots \le \lambda_N \). The multiplicity of the zero eigenvalue equals the number of connected components. The eigenvector associated with \( \lambda_1 = 0 \) is \( \mathbf{1} \).

3.1 Invariance of the Average

Define the average state \( \bar{x}(t) = \frac{1}{N}\mathbf{1}^\top \mathbf{x}(t) \). Differentiating along trajectories of \( \dot{\mathbf{x}} = -L\mathbf{x} \) gives

\[ \frac{\mathrm{d}}{\mathrm{d}t}\bar{x}(t) = \frac{1}{N}\mathbf{1}^\top \dot{\mathbf{x}}(t) = -\frac{1}{N}\mathbf{1}^\top L \mathbf{x}(t). \]

For any Laplacian, \( \mathbf{1}^\top L = \mathbf{0}^\top \), hence

\[ \frac{\mathrm{d}}{\mathrm{d}t}\bar{x}(t) = 0 \quad \Rightarrow \quad \bar{x}(t) \equiv \bar{x}(0). \]

Thus the consensus dynamics preserve the average of the initial conditions. Any limiting consensus value must equal this average.

3.2 Lyapunov Function and Asymptotic Consensus

For undirected connected graphs, we show that \( \mathbf{x}(t) \) converges to \( \bar{x}(0)\mathbf{1} \). Consider the quadratic Lyapunov function

\[ V(\mathbf{x}) = \tfrac{1}{2}\mathbf{x}^\top L \mathbf{x}. \]

Because \( L \) is symmetric positive semidefinite and \( L\mathbf{1} = \mathbf{0} \), we have \( V(\mathbf{x}) \ge 0 \) and \( V(\mathbf{x}) = 0 \) if and only if \( \mathbf{x} \) is a consensus vector \( c\mathbf{1} \). Differentiate along trajectories:

\[ \begin{aligned} \dot{V}(\mathbf{x}) &= \tfrac{1}{2}\dot{\mathbf{x}}^\top L \mathbf{x} + \tfrac{1}{2}\mathbf{x}^\top L \dot{\mathbf{x}} \\ &= \mathbf{x}^\top L \dot{\mathbf{x}} \quad \text{(since } L = L^\top\text{)} \\ &= \mathbf{x}^\top L(-L\mathbf{x}) = -\mathbf{x}^\top L^2 \mathbf{x}. \end{aligned} \]

Because \( L^2 \) is also symmetric positive semidefinite, \( \dot{V}(\mathbf{x}) \le 0 \). The set where \( \dot{V} = 0 \) equals \( \{\mathbf{x} \mid L^2\mathbf{x} = \mathbf{0}\} \), which coincides with \( \ker(L) = \mathrm{span}\{\mathbf{1}\} \) for a connected graph.

By LaSalle's invariance principle, trajectories converge to the largest invariant set contained in \( \ker(L) \), namely \( \mathbf{x}(t) \to c\mathbf{1} \) as \( t \to \infty \). Using average invariance, we get

\[ \mathbf{x}(t) \to \bar{x}(0)\mathbf{1}. \]

This shows that continuous-time consensus over an undirected connected graph is globally asymptotically stable to the average consensus.

In digital control or sampled-data systems, robots update state at discrete times \( k = 0,1,2,\dots \). A common discrete-time consensus protocol is

\[ x_i(k+1) = x_i(k) + \alpha \sum_{j=1}^N a_{ij}\bigl(x_j(k) - x_i(k)\bigr), \]

where \( \alpha > 0 \) is a scalar step-size (or “consensus gain”). In vector form,

\[ \mathbf{x}(k+1) = \Bigl(I - \alpha L\Bigr)\mathbf{x}(k) = P \mathbf{x}(k), \]

where \( P = I - \alpha L \) is the iteration matrix.

4.1 Spectral Condition for Asymptotic Consensus

For an undirected graph with Laplacian eigenvalues \( 0 = \lambda_1 < \lambda_2 \le \dots \le \lambda_N \), the eigenvalues of \( P \) are \( \mu_\ell = 1 - \alpha \lambda_\ell \). We require the iteration to be a contraction on the disagreement subspace, i.e., \( |\mu_\ell| < 1 \) for \( \ell \ge 2 \). This yields the condition

\[ -1 < 1 - \alpha \lambda_\ell < 1 \quad \forall \ell \ge 2. \]

The left inequality implies \( \alpha \lambda_\ell < 2 \) for all nonzero eigenvalues. The most restrictive constraint comes from \( \lambda_N \), so a sufficient condition is

\[ 0 < \alpha < \frac{2}{\lambda_N}. \]

Under this condition the iteration matrix has \( \rho(P - \tfrac{1}{N}\mathbf{1}\mathbf{1}^\top) < 1 \), where \( \rho(\cdot) \) denotes spectral radius, and the discrete-time consensus converges to the average of initial states.

Consider position vectors \( p_i(t) \in \mathbb{R}^d \) for each robot. A desired formation is often specified by relative offsets \( \delta_i \in \mathbb{R}^d \) such that the formation configuration can be written as

\[ p_i^\star = r + \delta_i, \]

where \( r \in \mathbb{R}^d \) is an arbitrary formation reference (global translation). The shape is encoded by differences \( \delta_j - \delta_i \).

5.1 Offset-Compensated Consensus

Define offset-corrected states \( q_i(t) = p_i(t) - \delta_i \). Suppose each robot applies the control law

\[ \dot{p}_i(t) = \sum_{j=1}^N a_{ij}\Bigl( \bigl(p_j(t) - \delta_j\bigr) - \bigl(p_i(t) - \delta_i\bigr)\Bigr) = \sum_{j=1}^N a_{ij}\bigl(q_j(t) - q_i(t)\bigr). \]

In terms of \( q_i \) this is exactly the standard consensus dynamics

\[ \dot{q}_i(t) = \sum_{j=1}^N a_{ij}\bigl(q_j(t) - q_i(t)\bigr). \]

Therefore, if the communication graph is connected, we get \( q_i(t) \to \bar{q}(0) \) for all \( i \). The corresponding robot positions converge to

\[ p_i(t) = q_i(t) + \delta_i \;\to\; \bar{q}(0) + \delta_i, \quad i = 1,\dots,N. \]

Thus all robots share the same formation reference \( r^\star = \bar{q}(0) \), while their offsets \( \delta_i \) encode the formation shape. The formation is rigidly translated but shape is preserved.

flowchart LR
  RREF["Common reference r (unknown)"] --> F2["Robot 2 target r + delta_2"]
  RREF --> F3["Robot 3 target r + delta_3"]
  RREF --> F4["Robot 4 target r + delta_4"]
  F2 --> NEI2["Robot 2 uses neighbors \nto drive q_2 to consensus"]
  F3 --> NEI3["Robot 3 uses neighbors \nto drive q_3 to consensus"]
  F4 --> NEI4["Robot 4 uses neighbors \nto drive q_4 to consensus"]
        

This offset-consensus viewpoint unifies point consensus and formation control: point consensus corresponds to the special case \( \delta_i = 0 \) for all robots.

We implement a simple one-dimensional discrete-time consensus protocol for \( N = 4 \) robots on a line, with a complete undirected graph and equal weights \( a_{ij} = 1 \) for \( i \neq j \). The update is

\[ x_i(k+1) = x_i(k) + \alpha \sum_{j \neq i} \bigl(x_j(k) - x_i(k)\bigr), \]

with step-size \( \alpha \) satisfying the spectral condition in Section 4. For a complete graph of size \( N \), one can show \( \lambda_N = N \) for the Laplacian, so \( 0 < \alpha < 2/N \) guarantees convergence.

6.1 Python Implementation (NumPy)

We use numpy for vector and matrix operations.

import numpy as np

# Number of agents
N = 4

# Complete graph Laplacian: L = N*I - 1*ones
L = N * np.eye(N) - np.ones((N, N))

# Step-size alpha satisfying 0 < alpha < 2 / lambda_max
alpha = 0.3  # for N = 4, 2 / lambda_max = 0.5

# Iteration matrix P
P = np.eye(N) - alpha * L

# Initial states (e.g., positions on a line)
x = np.array([0.0, 2.0, -1.0, 4.0])

def step_consensus(x):
    return P @ x

history = [x.copy()]
T = 30
for k in range(T):
    x = step_consensus(x)
    history.append(x.copy())

print("Final state:", x)
print("Average of initial states:", np.mean(history[0]))

# Example: plot trajectories (if matplotlib installed)
try:
    import matplotlib.pyplot as plt
    hist = np.array(history)
    for i in range(N):
        plt.plot(hist[:, i], label=f"agent {i+1}")
    plt.axhline(np.mean(history[0]), linestyle="--", label="average")
    plt.xlabel("k")
    plt.ylabel("x_i(k)")
    plt.legend()
    plt.show()
except ImportError:
    print("matplotlib not available; skipping plot.")
      

6.2 C++ Implementation (Eigen Library)

In C++, the Eigen library is widely used in robotics for linear algebra. Below is a simple consensus simulation.

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

int main() {
    const int N = 4;
    Eigen::MatrixXd L = N * Eigen::MatrixXd::Identity(N, N)
                      - Eigen::MatrixXd::Ones(N, N);

    double alpha = 0.3; // 0 < alpha < 0.5 for N = 4
    Eigen::MatrixXd P = Eigen::MatrixXd::Identity(N, N) - alpha * L;

    Eigen::VectorXd x(N);
    x << 0.0, 2.0, -1.0, 4.0;

    int T = 30;
    for (int k = 0; k < T; ++k) {
        x = P * x;
    }

    std::cout << "Final state: " << x.transpose() << std::endl;
    std::cout << "Average of initial states: "
              << (0.0 + 2.0 - 1.0 + 4.0) / 4.0
              << std::endl;

    return 0;
}
      

6.3 Java Implementation

In Java, one may use linear algebra libraries such as EJML or Apache Commons Math, but here we show a minimal implementation with plain arrays.

public class ConsensusSimulation {
    public static void main(String[] args) {
        int N = 4;
        double alpha = 0.3;

        // Build Laplacian for complete graph
        double[][] L = new double[N][N];
        for (int i = 0; i < N; i++) {
            for (int j = 0; j < N; j++) {
                if (i == j) {
                    L[i][j] = N - 1; // degree = N - 1
                } else {
                    L[i][j] = -1.0;
                }
            }
        }

        // Initial states
        double[] x = new double[] {0.0, 2.0, -1.0, 4.0};
        int T = 30;

        for (int k = 0; k < T; k++) {
            double[] xNext = new double[N];
            for (int i = 0; i < N; i++) {
                double sum = 0.0;
                for (int j = 0; j < N; j++) {
                    sum += L[i][j] * x[j];
                }
                xNext[i] = x[i] - alpha * sum;
            }
            x = xNext;
        }

        System.out.println("Final state:");
        for (int i = 0; i < N; i++) {
            System.out.print(x[i] + " ");
        }
        System.out.println();

        double avg0 = (0.0 + 2.0 - 1.0 + 4.0) / 4.0;
        System.out.println("Average of initial states: " + avg0);
    }
}
      

6.4 MATLAB / Simulink Implementation

In MATLAB, matrix operations make consensus updates concise. Simulink can then be used to realize block-diagram implementations for each agent.

N = 4;
L = N * eye(N) - ones(N);  % Laplacian for complete graph
alpha = 0.3;
P = eye(N) - alpha * L;

x = [0; 2; -1; 4];  % initial state
T = 30;
history = zeros(N, T + 1);
history(:, 1) = x;

for k = 1:T
    x = P * x;
    history(:, k + 1) = x;
end

disp('Final state:');
disp(x);
disp('Average of initial states:');
disp(mean(history(:, 1)));

% Plot trajectories
figure;
plot(0:T, history);
xlabel('k');
ylabel('x_i(k)');
legend('agent 1', 'agent 2', 'agent 3', 'agent 4');
grid on;
      

A Simulink realization uses, for each agent, a Sum block forming sum_j a_ij (x_j - x_i), followed by a discrete-time integrator implementing x_i(k+1) = x_i(k) + alpha * input. The communication graph is encoded by gain blocks or a shared From/Goto structure broadcasting each x_i.

6.5 Wolfram Mathematica Implementation

Mathematica's list-processing capabilities yield compact consensus simulations.

(* Number of agents *)
N = 4;

(* Laplacian for complete graph *)
L = N IdentityMatrix[N] - ConstantArray[1, {N, N}];

alpha = 0.3;
P = IdentityMatrix[N] - alpha L;

x0 = {0.0, 2.0, -1.0, 4.0};

consensusStep[x_] := P . x;

T = 30;
traj = NestList[consensusStep, x0, T];

Print["Final state: ", Last[traj]];
Print["Average of initial states: ", Mean[x0]];

ListLinePlot[Transpose[traj],
  PlotLegends -> {"agent 1", "agent 2", "agent 3", "agent 4"},
  AxesLabel -> {"k", "x_i(k)"},
  GridLines -> Automatic
]
      

Problem 1 (Laplacian Eigenvalues for a Path Graph): Consider three robots with scalar states connected in a line \( 1 - 2 - 3 \) with equal edge weights \( a_{12} = a_{21} = a_{23} = a_{32} = 1 \) and no direct edge between robots 1 and 3. Construct the Laplacian \( L \) and compute its eigenvalues.

Solution: The degree vector is

\[ d_1 = 1, \quad d_2 = 2, \quad d_3 = 1, \]

and the adjacency matrix is \( A = \begin{bmatrix}0 & 1 & 0 \\ 1 & 0 & 1 \\ 0 & 1 & 0\end{bmatrix} \). Therefore

\[ L = D - A = \begin{bmatrix} 1 & -1 & 0 \\ -1 & 2 & -1 \\ 0 & -1 & 1 \end{bmatrix}. \]

Computing the characteristic polynomial \( \det(L - \lambda I) \) yields eigenvalues \( \lambda_1 = 0 \), \( \lambda_2 = 1 \), and \( \lambda_3 = 3 \). The single zero eigenvalue reflects that the graph is connected.

Problem 2 (Average Invariance Revisited): For the continuous-time consensus system \( \dot{\mathbf{x}} = -L\mathbf{x} \) with symmetric Laplacian, show directly that \( \sum_{i=1}^N x_i(t) \) is constant in time.

Solution: We compute the derivative of the sum of states:

\[ \frac{\mathrm{d}}{\mathrm{d}t} \sum_{i=1}^N x_i(t) = \sum_{i=1}^N \dot{x}_i(t) = \sum_{i=1}^N \sum_{j=1}^N a_{ij}\bigl(x_j(t) - x_i(t)\bigr). \]

Split into two sums and swap indices in the first term:

\[ \begin{aligned} \sum_{i,j} a_{ij} x_j &= \sum_{j} x_j \sum_i a_{ij}, \\ \sum_{i,j} a_{ij} x_i &= \sum_{i} x_i \sum_j a_{ij}. \end{aligned} \]

For symmetric weights \( a_{ij} = a_{ji} \), \( \sum_i a_{ij} = \sum_j a_{ij} = d_j \) is the degree of node \( j \). Hence the two sums cancel and the total derivative is zero:

\[ \frac{\mathrm{d}}{\mathrm{d}t} \sum_{i=1}^N x_i(t) = 0. \]

Thus the sum, and hence the average, is invariant for all time.

Problem 3 (Step-Size Bound for Discrete Consensus): For the three-node path graph of Problem 1, consider the discrete-time consensus iteration \( \mathbf{x}(k+1) = (I - \alpha L)\mathbf{x}(k) \). Determine a range of \( \alpha \) that guarantees asymptotic consensus.

Solution: The Laplacian eigenvalues are \( 0, 1, 3 \), so the nonzero eigenvalues are \( \lambda_2 = 1 \) and \( \lambda_3 = 3 \). The discrete-time convergence condition from Section 4 is \( 0 < \alpha < 2/\lambda_{\max} \), so \( 0 < \alpha < \frac{2}{3} \).

Any \( \alpha \) in this interval yields consensus to the average of initial states.

Problem 4 (Formation via Offset Consensus): Consider four planar robots with positions \( p_i(t) \in \mathbb{R}^2 \) connected by a complete graph with equal weights. Let the desired formation be a square of side length \( s > 0 \) centered at an arbitrary reference position \( r \in \mathbb{R}^2 \). Define offsets

\[ \begin{aligned} \delta_1 &= (-s/2,\; -s/2)^\top, \\ \delta_2 &= ( s/2,\; -s/2)^\top, \\ \delta_3 &= ( s/2,\; s/2)^\top, \\ \delta_4 &= (-s/2,\; s/2)^\top. \end{aligned} \]

Show that the offset-consensus control law from Section 5 makes the robots converge to a translated copy of this square.

Solution: Define

\[ q_i(t) = p_i(t) - \delta_i. \]

Under the control law \( \dot{p}_i = \sum_j a_{ij}\bigl(q_j - q_i\bigr) \), the dynamics for \( q_i \) satisfy

\[ \dot{q}_i(t) = \sum_j a_{ij}\bigl(q_j(t) - q_i(t)\bigr), \]

which is standard consensus. For a complete graph, the corresponding Laplacian is connected, so \( q_i(t) \to \bar{q}(0) \) as \( t \to \infty \). Therefore

\[ p_i(t) = q_i(t) + \delta_i \;\to\; \bar{q}(0) + \delta_i. \]

This is an exact copy of the desired square formation translated by \( r^\star = \bar{q}(0) \). Different initial conditions lead to different translations, but the shape is preserved.

In this lesson we modeled multi-robot consensus using graph Laplacians, derived continuous-time dynamics \( \dot{\mathbf{x}} = -L\mathbf{x} \) and discrete-time dynamics \( \mathbf{x}(k+1) = (I - \alpha L)\mathbf{x}(k) \), and analyzed convergence using spectral and Lyapunov tools. We showed that, for connected undirected graphs, continuous-time consensus converges to the average of initial states and that discrete-time consensus requires step-sizes bounded by the largest Laplacian eigenvalue. Formation control was formulated as consensus on offset-corrected coordinates, yielding rigid formation shapes modulo global translation. Multi-language simulations illustrated how these concepts are implemented in practice and prepare the ground for more advanced topics such as distributed task allocation and collision avoidance in subsequent lessons.