Copra

Cette librairie a été développée pour résoudre le problème du contrôle prédictif. Elle résout des systèmes discrets en utilisant un QP. La librairie a été conçue de manière à être performante et facile à utiliser. De plus, des bindings Python permettent son utilisation dans un environnment Python.

C’est ma première grosse librairie ! Dans cet artcile, je vais décrire en quoi un mpc est utile, pourquoi l’utiliser et comment.

Un MPC, c’est quoi ?

MPC est l’acronyme de Model Predictive Control ou en français Modèle de Contrôle Prédictif. C’est utile pour trouver une solution dans un horizon de temps donné, cela en minimisant une fonction de coût. La librairie ne peut que résoudre des systèmes linéaires invariants dans le temps i.e. le système ne change pas de comportement dans le temps.

De manière plus simple, un mpc est fait pour rechercher une trajectoire qui mène à un point désiré ou encore de rechercher le contrôle à appliquer pour une trajectoire donnée.

Systèmes continus

Habituellement, tout commence par un beau système linéaire. Comme il est difficile de résoudre le problème analytiquement et rapidement, il faut discrétiser le système qui devient alors résoluble.

Regardons tout d’abord quels types de système peuvent convenir. Tout système de la forme

\[\mathbf{\dot{s}} = A_c\mathbf{s} + B_c\mathbf{u} + \mathbf{d}_c\]

convient. $\mathbf{s}$ est le vecteur d’état du système et $\mathbf{u}$ le vecteur de contrôle. Après discrétisation d’un pas de temps $T$, le système devient :

\[\mathbf{s}_{k+1} = A\mathbf{s}_k + B\mathbf{u}_k + \mathbf{d}\]

où $A$, $B$ et $\mathbf{d}$ sont respectivement la matrice d’état, la matrice de contrôle et le biais. Pour comprendre comment discrétiser son système rien de mieux qu’une page Wikipédia

Comment calculer le futur ?

Pour expliquer comment le mpc trouve une solution, il faut d’abord trouver la récurrence des équation ci-dessus. Aux premières étapes, on a

\[\begin{eqnarray} \mathbf{x}_1 & = & A\mathbf{x}_0 + B\mathbf{u}_0 + \mathbf{d} \\ \mathbf{x}_2 & = & A\mathbf{x}_1 + B\mathbf{u}_1 + \mathbf{d} \\ & = & A(A\mathbf{x}_0 + B\mathbf{u}_0 + \mathbf{d}) + B\mathbf{u}_1 + \mathbf{d} \\ & = & A^2\mathbf{x}_0 + [AB\ B][\mathbf{u}^T_0\ \mathbf{u}^T_1]^T + A\mathbf{d} + \mathbf{d} \\ \mathbf{x}_3 & = & A\mathbf{x}_2 + B\mathbf{u}_2 + \mathbf{d} \\ & = & A(A^2\mathbf{x}_0 + [AB\ B][\mathbf{u}^T_0\ \mathbf{u}^T_1]^T + A\mathbf{d} + \mathbf{d}) + B\mathbf{u}_1 + \mathbf{d} \\ & = & A^3\mathbf{x}_0 + [A^2B\ AB\ B][\mathbf{u}^T_0\ \mathbf{u}^T_1\ \mathbf{u}^T_2]^T + A^2\mathbf{d} + A\mathbf{d} + \mathbf{d} \end{eqnarray}.\]

Ainsi, on voit assez facilement (on passera la démonstration ici) qu’à la $N$-ième étape on a:

\[\mathbf{X}_N = \Phi\mathbf{x}_0 + \Psi\mathbf{U}_{N-1} + \mathbf{\xi}\]

avec

\[\Phi = \left[ \begin{array}{c} I \\ A \\ A^2 \\ \vdots \\ A^N \end{array} \right], \ \Psi = \left[ \begin{array}{cccc} 0 & 0 & \cdots & 0 \\ B & 0 & \cdots & 0 \\ AB & B & \ddots & \vdots \\ \vdots & \vdots & \ddots & 0 \\ A^{N-1}B & A^{N-2}B & \cdots & B \end{array} \right], \ \mathbf{\xi} = \left[ \begin{array}{c} \mathbf{0} \\ \mathbf{d} \\ A\mathbf{d} + \mathbf{d} \\ \vdots \\ \sum_{i=0}^{N-1} A^{i}\mathbf{d} \end{array} \right]\]

et le vecteur d’état total $\mathbf{X}$ et le vecteur de contrôle total $\mathbf{U}$ sont

\[\mathbf{X} = \left[ \begin{array}{c} \mathbf{x}_0 \\ \mathbf{x}_1 \\ \vdots \\ \mathbf{x}_N \end{array} \right], \ \mathbf{U} = \left[ \begin{array}{c} \mathbf{u}_0 \\ \mathbf{u}_1 \\ \vdots \\ \mathbf{u}_{N-1} \end{array} \right].\]

Finalement, considérant une valeur désirée/une trajectoire désirée $\mathbf{X}_T$ on calcule sur l’horizon de temps choisi et considérant une fonction de coût.

La librairie de plus près

La compréhension des matrices ci-dessus est importante car elle permet de mieux cerner le fonctionnement de la librairie. Son développement a été fait avec les outills de c++14. Hormis la librairie c++ standard, j’ai utilisé eigen pour la gestion des matrices. Enfin, et pour la suite, on appelle matrice-pas (resp. vecteur-pas) une matrice (resp. un vecteur) de la dimension d’un pas de temps et matrice-total (resp. vecteur-total) une matrice (resp. un vecteur) de la dimension de tous les pas de temps.

Le système

Le système est géré dans le fichier PreviewSystem.h. Ce fichier défini la totalité sus sytème and implémente les matrices $\Phi$, $\Psi$ et le vector $\mathbf{\xi}$. Il est préférable de créer un shared pointer de l’objet.

auto ps = std::make_shared<copra::PreviewSystem>();
// Initialise le système
ps->system(A, B, d, x_0, nrStep);
// Initialise le système dans le constructeur
auto ps = std::make_shared<copra::PreviewSystem>(A, B, d, x_0, nrStep);

Le mpc

Le mpc est implémenté dans le fichier LMPC.h. Créer une instance du mpc est simple comme bonjour.

copra::LMPC controller(ps);
// Construit le système et le résout
controller.solve();

Il est possible de (ré)initialiser le système utilisé par le mpc.

copra::LMPC controller();
// (Ré)initialise le mpc avec un nouveau système
controller.initializeController(ps);

Pour récupérer les résultats, vous avez juste besoin de demander :)

Eigen::VectorXd trajectory = controller.trajectory();
Eigen::VectorXd control = controller.control();

Les contraintes

La librairie fourni plusieurs types de contrainte. Toutes les contraintes peuvent être instanciées en utilisant des matrices-pas et des matrices-total. Nous verrons aussi qu’il est possible de faire un mixe des deux. Les contraintes ont aussi besoin d’être englobé dans un shared_ptr pour être envoyé dans le mpc

Contrainte de trajectoire

On distingue deux types de contraintes de trajectoire: La contrainte $E\mathbf{X} \leq \mathbf{f}$ ou $E\mathbf{X} = \mathbf{f}$ et la contrainte de trajectoire limite $\underline{\mathbf{X}} \leq \mathbf{X} \leq \overline{\mathbf{X}}$. On peut remarquer qu’il est possible de transformer la seconde pour faire la première.

// Créer une contrainte d'inégalité
// E est une matrice-pas ou matrice-total, f est vecteur-pas ou vecteur-total.
auto ineqTrajConstr = std::make_shared<copra::TrajectoryConstraint>(E, f);
// Créer une contrainte d'égalité
auto eqTrajConstr = std::make_shared<copra::TrajectoryConstraint>(E, f, false);
// Créer une contrainte de limite
// lower et upper sont des vecteurs-pas ou vecteurs-total
auto boundTrajConstr = std::make_shared<copra::TrajectoryBoundConstraint>(lower, upper);

Remarque: pour assigner des bornes infinies, il est possible d’inclure la libraire stl <limits> et utiliser la fonction std::numeric_limits<double>::max(). Côté Python, il faut utiliser la fonction float('Inf').

Contraintes de contrôle

L’idée est quasiment la même qu’au dessus, il y a deux types: la contrainte de contrôle $G\mathbf{U} \leq \mathbf{f}$ ou $G\mathbf{U} = \mathbf{f}$ et la contrainte de contrôle limite $\underline{\mathbf{U}} \leq \mathbf{U} \leq \overline{\mathbf{U}}$.

// Créer une contrainte d'inégalité
// G est une matrice-pas ou matrice-total, f est vecteur-pas ou vecteur-total.
auto ineqControlConstr = std::make_shared<copra::ControlConstraint>(E, f);
// Créer une contrainte d'égalité
auto eqControlConstr = std::make_shared<copra::ControlConstraint>(E, f, false);
// Créer une contrainte de limite
// lower et upper sont des vecteurs-pas ou vecteurs-total
auto boundControlConstr = std::make_shared<copra::ControlBoundConstraint>(lower, upper);

Contrainte mixte

La contrainte mixte vous permet d’ajouter une contrainte mélant trajectoire et contrôle. Elle s’écrite $E\mathbf{X} + G\mathbf{U} \leq \mathbf{f}$ ou $E\mathbf{X} + G\mathbf{U} = \mathbf{f}$. Comme d’hab’:

// Créer une contrainte d'inégalité
// E est une matrice-pas ou matrice-total, G est une matrice-pas ou matrice-total, f est vecteur-pas ou vecteur-total.
auto ineqMixedConstr = std::make_shared<copra::MixedConstraint>(E, G, f);
// Créer une contrainte d'égalité
auto eqMixedConstr = std::make_shared<copra::MixedConstraint>(E, G, f, false);

Ajout et mis-à-jour des contraintes

La seule chose à faire et de laisser le mpc prendre connaissance des contraintes. Il mettra automatiquement à jour les contraintes au moment de la résolution du problème.

// Ajout d'un contrainte de contrôle d'inégalité
controller.addConstraint(ineqControlConstr);

Fonction coût

Comme pour les contraintes, la librairie fourni plusieurs types de fonctions coût. Toutes les fonctions coût (sauf une) peuvent être créer en utilisant des matrices-pas ou matrices-total. Elles ont aussi besoin d’être englobées dans un shared pointer.

Coût en valeur finale

La fonction coût en valeur finale minimise $||M\mathbf{x}_N + \mathbf{p}||$. $M$ doit être une matrice-pas et $p$ doit être un vecteur-pas.

// Créer un coût en valeur finale
auto targetCost = std::make_shared<copra::TargetCost>(M, p);

Coût en trajectoire

La fonction coût en trajectoire minimise $||M\mathbf{X} + \mathbf{p}||$.

// Créer un coût en trajcetoire
// M est une matrice-pas ou matrice-total, p est un vecteur-pas ou vecteur-total.
auto trajectoryCost = std::make_shared<copra::TrajectoryCost>(M, p);

Coût en Contrôle

La fonction coût en contrôle minimise $||N\mathbf{U} + \mathbf{p}||$.

// Créer un coût en contrôle
// M est une matrice-pas ou matrice-total, p est un vecteur-pas ou vecteur-total.
auto ControlCost = std::make_shared<copra::ControlCost>(N, p);

Coût mixte

Le coût mixte minimise $||M\mathbf{X} + N\mathbf{U} + \mathbf{p}||$.

// Créer un coût mixte
// M est une matrice-pas ou matrice-total, N est une matrice-pas ou matrice-total, p est un vecteur-pas ou vecteur-total.
auto mixedCost = std::make_shared<copra::MixedCost>(M, N, p);

Ajout et mis-à-jour des fonctions coût

La seule chose à faire et de laisser le mpc prendre connaissance des fonctions coût. Il mettra automatiquement à jour les coûts au moment de la résolution du problème.

// Ajout d'une fonction coût valeur finale
controller.addCost(targetCost);

Spécificité des fonctions coûts et des contraintes

Il y a plusieurs outils d’aide pour les fonctions coût et les contraintes.

Suppression

Il y a deux façons de retirer une contrainte/un coût du mpc. Une façon est de laisser faire le mpc. Dans sa fonction solve(), le mpc vérifie si oui ou non la contrainte/le coût a besoin d’être supprimé(e). Une contrainte/un coût est supprimé(e) si l’utilisateur (vous) a détruit son shared pointer de la dite contrainte/le dit coût. Si la shared pointer est gardé en vie, la contrainte/le coût reste dans le mpc. Une autre possibilité est d’appelée la fonction removeCost()/removeConstraint().

La fonction autoSpan

La fonction autoSpan() est présente dans toutes les contraintes et coûts. La méthode doit être appelée si vous avez créé une contrainte/un coût qui mèle des matrices/vecteurs-pas avec des matrices/vecteurs-total. Par exemple, si vous construisait une contrainte de contrôle avec une matrice-pas constante $G$ et vecteur-total $\mathbf{f}$, vous pouvez passer ces deux variables dans la contrainte et vous devez ensuite appeler la fonction autoSpan.

Changer de QP solveur.

Suivant votre compilation, 4 QP sont implémentés. Il est possible de changer de QP n’importe quand. À cet instant les QP disponibles sont:

QuadProg (Par défaut)
QLD
Gurobi (besoin d’une licence)
LSSOL (besoin d’une licence commerciale)
OSQP

Pour changer de QP:

// Créer une instance de QP avec solveur différent
copra::LMPC controller(copra::SolverFlag::QLD)
// Changer le QP n'importe quand
controller.selectQPSolver(copra::SolverFlag::GUROBIDense)

Si les options par défaut des solveurs ne sont pas suffisantes, il possible de donner au contrôleur votre propre solveur.

// Créer le QP
std::unique_ptr<Eigen::QLD> solver = solverFactory(copra::SolverFlag::QLD);
// Faire les modifications désiéres
solver->SI_feasibilityTolerance(1e-8);
// ...
// L'envoyer au mpc
controller.useSolver(std::move(solver));

Utilisateurs Python

Les utilisateurs Python peuvent utiliser la librairie de la même manière que les utilisateurs c++. Il y a quelques différences par contre. Pour passer des arguments du type de Eigen aux fonctions, des convertisseurs ont été créés avec pygen-convert. Cela permet à l’utilisateur d’exploiter directement les numpy array ou des listes Python. Il faut cependant s’assurer que le type de l’objet Python est bien du float (qui est aussi du float64 dans numpy). Par exemple, [1, 2, 3] est une liste de int et [1., 2., 3.] est une liste de float. Enfin, pour respecter la norme PEP8, les fonctions ont été renommées en miniscule avec underscores comme séparateur.

Améliorer les performances et les temps de calculs

Tout d’abord, il est possible de mesurer le temps de résolution et le temps de construction du problème avec les fonctions solveTime() et solveAndBuildTime().

Mettre à jour le système soi-même

L’initialisation du système permet l’allocation de mémoire des matrices ($\Phi$, $\Psi$, etc…). Normalment, quand la fonction solve() du mpc est appelée, le système est mis à jour. Dans le cas où vous pouvez (voulez) mettre à jour le système vous-même, vous pouvez juste appeler la fonction updateSystem(). La fonction solve() ne cherchera pas à mettre à jour le système si c’est déjà fait.

Remarque, vous avez le contrôle total du système. Il est possible de modifier les matrices appartenant au système si vous avez des matrices spécifiques. N’oubliez pas de (ré)initialiser le mpc si vous avez modifié la dimension du problème.

Donner des rvalue aux contraintes et aux coûts

Les constructeurs des contraintes/des coûts et les fonctions poids utilisent des références universelles pour permettre des move-semantic. Ceci n’est pas disponible pour les utilisateurs Python.

Privilégier les matrices-pas et les vecteurs-pas

La libraire vous laisse utilser les matrices-pas/vector-pas et matrices-total/vector-total. Lorsqu’uniquement les matrices-pas et vecteurs-pas sont donné(e)s, et vu que le problème est creux, le calcule ira plus vite.

Les matrices creuses seront utilisées pour améliorer les performances pour les release futures

Examples

Vous pouvez un exemple c++ ici et un exemple python ici

Pymanoid

Pymanoid est une librairie développée par Stéphane Caron qui permet l’utilisation du mpc pour la plannification de trajectoire et autre.