In short...
I am solving a two point boundary value problem using B-spline. There are some numerical issues during solving a quadratic programming problem with bad conditioned hessian. The details are as follows. (I have included the example matlab files here for your testing.)
About B-spline
Firstly, the trajectory is expressed by B-spline (here we use 4th order uniform clamped B-spline) as: \begin{equation*} p(t)=\sum_{i=0}^N\tau_i B_i^4(t) \end{equation*} Here $t$ is the time, $p$ is position, $\tau_i$ is the B-spline control point and $B_i^4(t)$ is the 4th order B-spline base.
In order to have $p(0)=p_0$ and $p(T) = p_T$, we formulate a set of linear constraints as $$\begin{equation}\tag{1}\label{0} A\boldsymbol{\tau}=b \end{equation}$$ where$\boldsymbol{\tau}=[\tau_0, \tau_1,\dots,\tau_N]^T$.
In order to make the trajectory smooth, we want to minimize the integration of the square of its derivatives. In this case, we choose the 4th order derivative. That is we wish to minimize $$\begin{equation}\tag{2}\label{1} \int_{0}^{T}\frac{d^4 p(t)}{d t^4} dt \end{equation}$$ Notice, a $m$th order derivative of a $k$th order B-spline is a $k-m$th order B-spline, with changed control points, a.k.a: \begin{equation*} \frac{d^4 p(t)}{d t^4} = \sum_{i=0}^{N-4}\gamma_i B_i^{0}(t) \end{equation*} And we can show that to minimize Eq.\ref{1} is equivalent to minimize $$\begin{equation}\tag{3}\label{2} \boldsymbol{\gamma}^T\boldsymbol{\gamma} \end{equation}$$ where $\boldsymbol{\gamma}=[\gamma_0,\gamma_1,\dots,\gamma_{N-4}]^T$ And there is a linear relationship between $\boldsymbol{\tau}$ and $\boldsymbol{\gamma}$ as \begin{equation*} \boldsymbol{\gamma} = C\boldsymbol{\tau}, \, C\in\mathbb{R}^{(N-4)\times N} \end{equation*} Thus minimizing Eq.\ref{2} is to minimize $$\begin{equation}\tag{4}\label{g} \boldsymbol{\tau}^T C^T C \boldsymbol{\tau} \end{equation}$$
Problem
Combining Eq.\ref{0} and \ref{g}, the final optimization problem is $$\begin{equation}\tag{5}\label{p} \left\{\begin{matrix} min & \boldsymbol{\tau}^T C^T C \boldsymbol{\tau}\\ s.t. & A\boldsymbol{\tau}=b \\ & D\boldsymbol{\tau}<e & \mathrm{[optionally]} \end{matrix}\right. \end{equation}$$ with $\boldsymbol{\tau}=[\tau_0, \tau_1,\dots,\tau_N]^T$. We use off the shelf solvers, quadprog, CPLEX and Gurobi. The resulted quadratic programming problem can be solved nicely when $N<400$. But as $N$ grows bigger, we encounter numerical difficulties (see Fig.1 to 3). According to my analysis, the reason is as follows.
- $C^T C$ is singular.
- The non-zero Eigen values of $C^T C$ is ill conditioned, ranging from $1e-13$ to $1e3$.
Fig.1 N=20, horizontal axis is time, vertical is p
Fig.2 N=400, horizontal axis is time, vertical is p
Fig.3 N=800, horizontal axis is time, vertical is p
Things I have tried
To deal with this issue I have tried the following way to reformulate the problem. As a result, it works on some solver but not all of them.
I created a cascaded vector \begin{equation*} \boldsymbol{\theta}=\begin{bmatrix} \boldsymbol{\tau}\\ \boldsymbol{\gamma} \end{bmatrix} \end{equation*} and matrix $M^+$ and $M^-$ such that $\boldsymbol{\tau} = M^+ \boldsymbol{\theta}$ and $\boldsymbol{\gamma} = M^- \boldsymbol{\theta}$. From \ref{p}, we rewrite the problem as $$\begin{equation} \tag{6}\label{q} \left\{\begin{matrix} min & \boldsymbol{\theta}^T {M^-}^T M^- \boldsymbol{\theta}\\ s.t. & A M^+ \boldsymbol{\theta}=b \\ & (C M^+ - M^-) \boldsymbol{\theta} = 0 \\ & D M^+ \boldsymbol{\theta}<e & \mathrm{[optionally]} \end{matrix}\right. \end{equation}$$ Now, although ${M^-}^T M^-$ is still singular, at least its non-zero Eigen values are all 1. It seems to work well for quadprog(Matlab 2016a), but not so for Gurobi and CPLEX.
Question
- How can I improve the numerical stability for my original problem (Eq.\ref{p})?
- Is it possible to tune parameters in Gurobi and CPLEX to make them work well on the reformulated problem (Eq.\ref{q})?