Multi-Path Length Minimization I've been thinking about path planning and am trying to make good heuristics for cases with multiple agents.

Suppose there are sets $S_i$ of coordinates in $\mathbb R^2$ or $\mathbb R^3$, each of the same size n, for each possible value of $i$ where $i \in [0, ... k]$.  

A path is defined as k line segments connecting a sequence of k+1 coordinates, made up of one coordinate from each set $S_i$ in consecutive order. I want to find n paths such that (a) no two paths have the same point for a given index i in the sequence, and b) the combined path length of all the paths is minimized. In other words, assign coordinates from each set without replacement to construct paths with the goal of making the total path length as small as possible.                                                                                                          

Right now i can do the minimization from some $i$ to $i+1$, but I am not sure if locally minimizing each step will yield a global minimum. I know I could brute force it, but that explodes really quickly.
You can solve this optimally in $O(kn^3)$ time by solving a sequence of $k$ Assignment problems.

Let $f(i)$ be the lowest sum of costs of any set of $n$ paths beginning at step $i$ and continuing to step $k$ (i.e., the end). We would therefore like to compute $f(0)$, as well as a set of paths realising it.

Assume we have already computed $f(i+1)$. (Clearly $f(k) = 0$, so we have a starting point.) Let $A_{i,i+1}$ be the solution to the Assignment problem (minimum-weight bipartite matching) in which one set of vertices are the points in $S_i$ and the other is the points in $S_{i+1}$. I claim that we can compute $f(i)$ as $A_{i,i+1} + f(i+1)$.

Suppose this claim is not correct: then for some $i$, there is a better solution with cost $B_i &lt; f(i)$, which can likewise be decomposed as $B_i = B_{i,i+1} + B_{i+1,k}$. In particular, let $i$ be the maximum possible $i$ for which $B_i &lt; f(i)$. Then at least one of $B_{i,i+1} &lt; A_{i,i+1}$ or $B_{i+1,k} &lt; f(i+1)$ must be true for our assumption $B_i &lt; f(i)$ to be true.

It cannot be that $B_{i,i+1} &lt; A_{i,i+1}$, since $A_{i,i+1}$ is the minimum possible cost by definition of the Assignment problem. So it must be that $B_{i+1,k} &lt; f(i+1)$. But this cannot be true either, since if it were then it would violate our assumption that $i$ is maximum possible. Thus supposing the existence of a solution with a better cost $B_i$ leads to a contradiction, so it cannot be true, meaning that $f(i)$ gives the optimal cost after all.

$f(0)$ gives you the cost of the optimal solution. To extract such a solution, just record the matchings produced by the Assignments problems you solved along the way.