Algorithms/Solvers for Hard Constrained Non-Linear Optimization Problems - Model Predictive Control Example

Question

I have an autonomous robotic swarm path planning/control problem where a set of "leader" robots have predefined (nontrivial) dynamics in the control set, and "follower" robots are subject to optimization in their control. $u_i = (u_{l_i}^T \ u_{f_i}^T)^T$ is a vector containing the predefined control efforts of the leaders $u_{l_i}^T \in \mathbb{R}^{n_l}$ and the optimizable efforts of the followers $u_{f_i}^T \in \mathbb{R}^{n_f}$, for each time step $i \in \{1,...,t\}$. Then $u_i \in R^N, N = n_l+n_f$. The control efforts $u_i$ may be stacked in

$$u = \begin{pmatrix}u_1\\ ... \\ u_t \end{pmatrix} \in R^{N*t}$$

The same process can be done for $u_l$ and $u_f$.

This is an optimization problem of the form $$min_{u_f} \ \ ||u||^2 \\ s.t. \ G(x_i)u_i \geq 0 \\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \dot{x}_i = u_i \\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \forall i \in \{1,...,t\} $$ where $G(x_i) \in \mathbb{R}^{NxN}$ is a very sensitive non-linear matrix function of the vector state $x_i$. This dynamical system has a single integrator dynamics $\dot{x}_i = u_i$ which was discretized using a simple forward Euler technique. This rendered examples with a few hundred optimization variables $u_{f}$. Also the examples explored seem to always require a good initial guess (that respects the constraint in all discretized steps), regardless of the selected solver, and can not be relaxed in tolerance.

I have tried fmincon solver in MATLAB with all its algorithms. The optimization fails, because the initial guess is "shrunk" with little regard for the constraint.

I could use fminsearch and CMA-ES by including the constraints inside the cost function (returning NaN or a high value). With this I could improve the initial guess, but it still seems far from an optimal configuration.

Is there some algorithm/solver worth exploring for this type of problem? Is there some way of exploiting the "consecutive choice" nature of the control vector $u_i$?

Analytical derivatives of G(x) are not available. No success using log or barrier methods

Answer 1

Let $n_l$ be the number of leaders and $n_f$ the number of followers so that $u_l\in \mathbb{R}^{n_l},u_f\in \mathbb{R}^{n_f}$. The problem is

$$ \min_{u_f}\Vert u\Vert^2 \text{ s.t. } G(x)u\geq 0 $$

with $u=\begin{pmatrix}u_l\\u_f\end{pmatrix}$ and $G(x)\in \mathbb{R}^{N \times N}$, $N=n_l+n_f$ for every $x\in \mathbb{R}^{N}$. We can rewrite the inequality term

$$ G(x)u = \begin{pmatrix}G_{11}(x)&G_{12}(x)\\G_{21}(x)&G_{22}(x)\end{pmatrix}\begin{pmatrix}u_l\\u_f\end{pmatrix} $$

Now use the fact that $u_l$ is given so you get:

$$ \begin{align}\tag{1} A_1 u_f &\leq b_1\\ A_2 u_f &\leq b_2\\ \end{align} $$ with $$ \begin{align} A_1&=-G_{12}(x) \\ A_2&=-G_{22}(x) \\ b_1&=G_{11}(x)u_l \\ b_2&=G_{21}(x)u_l \\ \end{align} $$

You can stack $(1)$ together to get

$$ A u_f \leq b $$

with

$$ A=\begin{pmatrix}A_1\\A_2\end{pmatrix},b=\begin{pmatrix}b_1\\b_2\end{pmatrix} $$

Now consider the cost which is

$$ \Vert u\Vert^2 = u_f^Tu_f + u_l^Tu_l $$

Since $u_l$ is given, it cannot be changed by the optimization. So it is just a constant offset of the objective and can be omitted for the minimization. So you end up with

$$ \begin{align} \tag{2} \min_{u_f} \frac{1}{2}u_f^T Q u_f\\ \text{s.t. } A u_f \leq b \end{align} $$

This is just a quadratic program in $u_f$ with $A$ and $B$ as defined above, $Q = 2I$ and $I$ the identity matrix of matching size.

So in every timestep you can insert $x$ into the matrices to get a quadratic program that you can then solve to get the optimal $u_f$ (assuming the quadratic program is feasible). In Matlab you could for example use the quadprog function for this.