# Shortest Path and Minimum Curvature Path - implementation

Let's say we are given a race track, which may be described as a closed curve of given width (it may differ along the curve). My task is to implement an algorithm which finds two kinds of trajectories for the car. The first one is the shortest path, i.e. such closed curve contained in the race track that enables the car to drive the shortest distance per lap, the speed is irrelevant. The second one is minimum curvature path where the car can achieve higher speeds in the corners, the length is irrelevant.

The following picture shows the two kinds of paths for an example race track:
http://i.stack.imgur.com/or51S.png

I found a paper (F. Braghin et al. / Computers and Structures 86 (2008) 1503-1516) that describes the two problems in such way that they can be presented as quadratic programming problems. The problem is that I don't have sufficient knowledge to convert the input data to the appropriate matrices, needed in the quadratic equation. I can only understand the transformations up to some moment.

Here are the descriptions of the algorithms, found in the mentioned paper:
http://i.imgur.com/2EeGQKp.png

Could someone please help me and explain the transformations done in the equation (6) and (7) for the SP problem and virtually all equations for the MCP problem? The format of representation of all the coordinates, depicted in Fig. 2 in the document, is perfectly clear for me. I realize that some equations are just the intermediate steps to achieve the goal, so maybe there's an easy, algorithmic way to explain a newbie like me how I should transform the given coordinates to get the matrices for the quadratic programming solver.

-
Anyway, in what form your racetrack is given? If this is a list of straight edges, then you could construct the visibility graph and then use Dijkstra's (or some other kind of A*). For the second problem you could on bisector of each corner put a circle of desired curvature and binary-search for global radius that keeps the racetrack open. – dtldarek Jul 15 '13 at 16:23
In my situation, I don't have any specified input format. The format proposed in the paper seems quite convenient for me, so first I'll give these algos a go. Thanks for the suggestions about graph algos, I thought of Dijkstra and similar, too. I'll probably use them if I don't succeed in implementing the algos I'm asking about. – Piotr Krzemiński Jul 15 '13 at 17:00

# Equation 6

By (5) you have formulas for the displacement in $x$ and in $y$ direction. These are simply numbers. The squared length is the sum of the squares of these displacements, by Pythagoras. That squared length is then written as a function of a two-element parameter vector $\alpha_i$. It is quadratic in $\alpha$, so it can be split into a matrix which gets multiplied by $\alpha_i$ from both sides, a vector which gets multiplied by $\alpha_i$ in what is essentially a dot product, and a constant part which does not depend on $\alpha_i$ at all. This separation is what (6) does.

Let's have a closer look. Remember that the form of your $x$ displacement was this:

$$\Delta P_{x,i} = \begin{pmatrix}\Delta x_{i+1}\\-\Delta x_i\end{pmatrix}^{\mathrm T}\alpha_i + \Delta x_{i,0} = \alpha_i^{\mathrm T}\begin{pmatrix}\Delta x_{i+1}\\-\Delta x_i\end{pmatrix} + \Delta x_{i,0}$$

Lets introduce some abbreviations for the reoccurring vectors here:

$$\beta_i := \begin{pmatrix}\Delta x_{i+1}\\-\Delta x_i\end{pmatrix} \\ \gamma_i := \begin{pmatrix}\Delta y_{i+1}\\-\Delta y_i\end{pmatrix}$$

Now you can write

$$\Delta P_{x,i} = \beta_i^{\mathrm T}\alpha_i + \Delta x_{i,0} \\ \Delta P_{y,i} = \gamma_i^{\mathrm T}\alpha_i + \Delta y_{i,0}$$

and with these you can compute the squared length as the sum of the squared displacements in the two coordinate directions like this:

\begin{align*} \Delta P_{x,i}^2 + \Delta P_{y,i}^2 &= \left(\beta_i^{\mathrm T}\alpha_i + \Delta x_{i,0}\right)^2 + \left(\gamma_i^{\mathrm T}\alpha_i + \Delta y_{i,0}\right)^2 \\&= \left(\beta_i^{\mathrm T}\alpha_i\right)^2 + 2\left(\beta_i^{\mathrm T}\alpha_i\right)\Delta x_{i,0} + \Delta x_{i,0}^2 + \left(\gamma_i^{\mathrm T}\alpha_i\right)^2 + 2\left(\gamma_i^{\mathrm T}\alpha_i\right)\Delta y_{i,0} + \Delta y_{i,0}^2 \\&= \alpha_i^{\mathrm T}\beta_i\beta_i^{\mathrm T}\alpha_i + 2\Delta x_{i,0}\beta_i^{\mathrm T}\alpha_i + \Delta x_{i,0}^2 + \alpha_i^{\mathrm T}\gamma_i\gamma_i^{\mathrm T}\alpha_i + 2\Delta y_{i,0}\gamma_i^{\mathrm T}\alpha_i + \Delta y_{i,0}^2 \\&= \alpha_i^{\mathrm T}\left( \beta_i\beta_i^{\mathrm T} + \gamma_i\gamma_i^{\mathrm T} \right)\alpha_i + \left( 2\Delta x_{i,0}\beta_i^{\mathrm T} + 2\Delta y_{i,0}\gamma_i^{\mathrm T} \right)\alpha_i + \left( \Delta x_{i,0}^2 + \Delta y_{i,0}^2 \right) \end{align*}

So here you see that $H_{S,i}$ is the matrix

$$H_{S,i}:=\beta_i\beta_i^{\mathrm T} + \gamma_i\gamma_i^{\mathrm T}$$

and $B_{S,i}$ is the row vector

$$B_{S,i}:= 2\Delta x_{i,0}\beta_i^{\mathrm T} + 2\Delta y_{i,0}\gamma_i^{\mathrm T}$$

while the constant term is the last parenthesis and irrelevant for the subsequent optimization.

# Equation 7

Based on this, (7) expresses $\alpha_i$ as the matrix-times-vector product

$$\alpha_i = E_i\alpha = \begin{pmatrix}\alpha_{i+1}\\\alpha_i\end{pmatrix}$$

The matrix $E_i$ here simply has two rows and $n$ columns. The first row has a $1$ in column $i+1$ and the second a $1$ in column $i$. This matrix $E_i$ can be combined with the matrices of the previous step in order to obtain an expression in $\alpha$, without the need to select elements based on $i$. With this formulation, the sum of products with matrices can be turned into a product with sums of matrices, using

\begin{align*} H_S &:= \sum_{i=1}^n E_i^{\mathrm T}H_{S,i}E_i \\ B_S &:= \sum_{i=1}^n B_{S,i}E_i \end{align*}

# Minimum curvature

Explaining pretty much every equation in the MCP case is beyond the scope of this post. The above should give you an idea about the kind of transformations required. If you need more help, please ask more specific questions separately.

-
I've just successfully implemented the Shortest Path algorithm, thank you for your time, your explanation is perfectly clear for me! In fact, I didn't understand how one can transpose a number, e.g. $\Delta P_{x,i}$. I guess it's like a 1x1 matrix? – Piotr Krzemiński Jul 16 '13 at 22:19
@PiotrKrzemiński: exactly, the numbers are treated like $1\times1$ matrices there, the result of multiplying a $1\times2$ matrix (a row vector) by a $2\times1$ matrix (the column vector $\alpha_i$). The transposition doesn't change anything for a $1\times1$ matrix, but the rule that that transposition can be moved inside the matrix multiplication still holds. – MvG Jul 17 '13 at 5:23
I see, thanks once again! – Piotr Krzemiński Jul 17 '13 at 13:48