I am currently working on an algorithm to determine both the shortest and minimum curvature path over a racetrack. The paper by Braghin et al. perfectly describes a solution to my problem, especially since one can eventually take the weighted average over the shortest path and minimum curvature path to find an optimal path for a given situation.
In short, the system tries to find the values for $\alpha_i$ for which the summed curvature of the path is minimal. The path is a series of points that are allowed to travel along the normal of the track, between the edges. The value of $\alpha_i$ indicates the ratio of the distance from the inner edge to the total track width at point $i$ and thus runs from 0 to 1.
Currently, I have succesfully implemented the shortest path finding algorithm. However, the minimum curvature path is not converging to a sensible solution, see figure. In this figure the green path is the shortest path, while the blue path should be the minimum curvature path.
Looking at the two different [H] matrices and the {B} vectors, the entries for the minimum curvature path are a factor 10^5 smaller than the entries for the shortest path. As in the paper is described that we can use a weighted average of these matrices and vectors to determine alternatives paths along the track, I would assume that the order of magnitude of the entries should be similar.
In the paper it is described that the [H] matrix for the minimum curvature path is calculated by:
$H = ([dx]^T[D]^T[D][dx])$
As H should be n x n matrix, I would expect both $[dx]$ and $[D]$ to be n x n matrices as well. As the value $dx$ is simply the difference in the $x$-coordinate of the two edges of the track, I expect the matrix $[dx]$ to be a diagonal matrix with the values $dx_i$ on the diagonal.
In the paper they say $\left(\frac{d^2\bar{x}(t)}{dt^2}\vert_{t = 0}\right) = [D]\bar{x}$. As all the points $x_i$ are approximated using cubic splines, taking the second derivative at t = 0 will simply result in two times the coefficient of the quadratic term of the cubic spline $i$, $2 \cdot c_{i, x}$. I expect the matrix [D] to be diagonal as well, where the vaules on the diagonal are given by $2\frac{c_{i,x}}{x_i}$. We need to devide by $x_i$ as [D]$\bar{x} = 2\bar{c_x}$, where $\bar{c_x}$ is a vector with all $c_{i, x}$. I determine the $c_{i, x}$ by using SciPy's CubicSpline interpolation.
Finding [H] and {B} for both the $x$ and $y$-coordinates of the track's centerline we can construct the full [H] and {B}. Due to the additive nature of the formula for the curvature of path, I expect that we can simply add the two matrices and the two vectors to find the full [H] and {B}.
However, like I said, with this method I am not able to find the minimum curvature path. Instead it seems that the algorithm finds its minimum in one of the two extremes (the edges of the track).
I think that I determine the [D] matrix in a wrong way and thus the curvature of the path. Hpefully, one of you can help me tackle this problem! Thank you in advance!
I have looked at the answers of this, this and this question. However, sadly they have not helped me solving my problem.
After taking some steps back, I figured it out. In the paper, they did not use the exact second derivative of the coordinates in $t$-space. However they used the linear approximation of the second derivative, which in this case equates to: $\frac{d^2x_i}{dt^2} \approx x_{i-1} - 2x_i + x_{i+1}$. This results in a tridiagonal [D] matrix with -2 on the diagonal and 1's on the secondary diagonals. See the attached figure for the current results of the shortest path (green) and minimum curvature path (blue).