article │nmpc-cgmres.md

Commande prédictive non linéaire avec C/GMRES

Commande optimale d’un robot à entraînement différentiel : le hamiltonien, la méthode de continuation et un solveur GMRES, en Python.

Sokheng Din28 oct. 2025~9 min#control#optimisation#python#robotics

Théorie du contrôle optimal dans les systèmes de contrôle

Le contrôle optimal est une branche de la théorie du contrôle appliquée aux systèmes de contrôle. Cette théorie consiste à résoudre un problème d'optimisation d'un système dynamique afin de trouver la solution optimale pour ce système.

Qu'est-ce que le contrôle prédictif par modèle (MPC) ?

Le contrôle prédictif par modèle (MPC) est une méthode de contrôle optimal qui utilise des algorithmes d'optimisation pour minimiser une fonction de coût associée à une entrée désirée et obtenir ainsi l'entrée de contrôle optimale sur des horizons finis ou infinis. Récemment, le MPC a été largement adopté par les chercheurs en systèmes de contrôle pour sa rapidité, son efficacité et sa robustesse dans les systèmes complexes.

Équation de Hamilton-Jacobi-Bellman (HJBE)

L'équation HJBE est une équation aux dérivées partielles non linéaire qui exprime les conditions nécessaires à l'optimalité en minimisant la fonction de perte du système.

On définit l'hamiltonien :

\mathcal{H}(x,u,\lambda,\mu) = \sum_{k=0}^{N-1} (x_k - x_{k,\mathrm{ref}})^\top Q (x_k - x_{k,\mathrm{ref}}) + (u_k - u_{k,\mathrm{ref}})^\top R (u_k - u_{k,\mathrm{ref}}) + \phi(x) + \lambda^\top f(x,u) + \mu^\top C(x,u).

Où :

$N$ — nombre d'horizons de prédiction (nœuds de tir).
$x_k$ — état au nœud $k$ .
$u_k$ — entrée au nœud $k$ . - $Q, R$ — Matrices de pondération (généralement diagonales).
$f(x,u)$ — dynamique de transition d'état.
$C(x,u)$ — fonctions de contrainte d'égalité/inégalité.
$\lambda$ — vecteur co-état.
$\mu$ — multiplicateurs de Lagrange pour les contraintes.

La commande prédictive non linéaire (MPC) admet plusieurs méthodes de résolution ; nous nous concentrons ici sur la méthode de continuation/GMRES proposée par le professeur Toshiyuki Ohtsuka.

Supposons que nous souhaitions obtenir le vecteur solution à l'instant d'échantillonnage $t$ :

$U(t) = [u_0^{*}, \mu_0^{*}, \dots, u_{N-1}^{*}, \mu_{N-1}^{*}]$

Nous définissons les équations d'Euler-Lagrange discrétisées (ELE). Pour résoudre itérativement le système non linéaire résultant, nous utilisons la méthode de Newton sur $F(x,u) = 0$

où $F$ représente la stationnarité. Conditions à l'horizon :

F(x, u) = \begin{bmatrix} \frac{\partial \mathcal{H}}{\partial u}(x_0^*, u_0^*, \lambda_1^*, \mu_0^*) \\ C(x_0^*, u_0^*) \\ \vdots \\ \frac{\partial \mathcal{H}}{\partial u}(x_{N-1}^*, u_{N-1}^*, \lambda_N^*, \mu_{N-1}^*) \\ C(x_{N-1}^*, u_{N-1}^*) \end{bmatrix} = 0

La résolution directe de ce système est coûteuse ; la continuation réduit la complexité, puis nous appliquons la GMRES.

Continuation

Soit (\zeta, Soit α, h ∈ ℝ. Définissons la dynamique de continuation de F :

\frac{\partial F}{\partial u}\,\dot{u}(t) ​​= -\zeta F(x,u,t) - \frac{\partial F}{\partial x}(x,u,t)\,\dot{x} - \frac{\partial F}{\partial t}(x,u,t).

Définissons les approximations par différences finies :

\begin{aligned} \text{right} &= F\!\big(x(t) + h\dot{x}(t),\, u(t) + h\dot{u}(t),\, t + h\big) - F\!\big(x(t) + h\dot{x}(t),\, u(t),\, t + h\big), \\ \text{left} &= -\zeta\, F(x(t), u(t), t) - \frac{F\!\big(x(t) + h\dot{x}(t),\, u(t),\, t + h\big) - F(x(t), u(t), t)}{h}. \end{aligned}

Méthode des résidus minimaux généralisée (GMRES)

La GMRES est une méthode itérative pour la résolution de grands systèmes linéaires creux $A x = b$ . Le processus d'Arnoldi construit une base de Krylov orthonormée $V_m$ et une matrice de Hessenberg supérieure $H_m$ , puis on résout un problème de moindres carrés pour minimiser le résidu.

Schéma :

Choisir $x_0$ , calculer $r_0 = b - A x_0$ , puis définir $v_1 = \dfrac{r_0}{\|r_0\|}$ .
Pour $k = 0, \dots, m-1$ : appliquer la méthode d'Arnoldi pour obtenir $H_m$ et $V_m$ .
Résoudre $y = \arg\min_y \|\, \|r_0\| e_1 - H_m y \,\|$ .
Mettre à jour $x_m = x_0 + V_m y$ . Pour un modèle à entraînement différentiel avec état $x = [x,\, y,\, \phi]^\top$ et dynamique $\dot{x} = f(x,u)$ , on peut définir :

$f(x,u) = [\,u_1 \cos\phi,\; u_1 \sin\phi,\; u_2\,]^\top$
contraintes $C(x,u)$ (par exemple, contraintes de boîte sur $u_1, u_2$ )
pénalité terminale $\phi(x) = (x_N - x_{N,\mathrm{ref}})^\top Q (x_N - x_{N,\mathrm{ref}})$

Alors,

\frac{\partial \mathcal{H}}{\partial x} = \begin{bmatrix} (x-x_{\mathrm{ref}})q_1 \\[2pt] (y-y_{\mathrm{ref}})q_2 \\[2pt] (\phi-\phi_{\mathrm{ref}})q_3 - \lambda_1 u_1 \sin\phi + \lambda_2 u_1 \cos\phi \end{bmatrix}, \qquad \frac{\partial \mathcal{H}}{\partial u} = \begin{bmatrix} r_1(u_1-u_{1,\mathrm{ref}}) + \lambda_1\cos\phi + \lambda_2\sin\phi \\[2pt] r_2(u_2-u_{2,\mathrm{ref}}) + \lambda_3 \end{bmatrix}.

Implémentation en Python

Il est conseillé de commencer par les bases, comme la création d'une classe pour la représentation du modèle. Cependant, pour l'instant, je me contenterai d'implémenter CGMRES, ayant déjà réalisé de nombreuses démonstrations de création de classes pour les modèles.

Créons une classe pour l'équation de Bellman-Jacobien de Hamilton, que nous avons déjà dérivée de l'équation précédente.

code

class HJBEquation:

def __init__(self, q1, q2, q3, r1, r2, u_max, u_min):

self.q1 = q1

self.q2 = q2

self.q3 = q3

self.r1 = r1

self.r2 = r2

self.u1_min = u_min[0]

self.u2_min = u_min[0]

self.u1_max = u_max[0]

self.u2_max = u_max[1]

def dhdx(self,x, y, yaw, x_r, y_r, yaw_r, u1, lam1, lam2):

dhdx1 = (x-x_r)*self.q1

dhdx2 = (y-y_r)*self.q2

dhdx3 = (yaw-yaw_r)*self.q3-u1*lam1*np.sin(yaw)+u1*lam2*np.cos(yaw)

retourner dhdx1, dhdx2, dhdx3

def dhdu(self, yaw, u1, u2, u1_r, u2_r, lam1, lam2, lam3, mu1, mu2):

dhdu1 = self.r1*(u1-u1_r)+lam1*np.cos(yaw)+lam2*np.sin(yaw)+2*mu1*u1*(u1+(self.u1_max+self.u1_min)/2)

dhdu2 = self.r2*(u2-u2_r)+lam3+2*mu2*(u2+(self.u2_max+self.u2_min)/2)

return dhdu1, dhdu2

def dphidx(self, x, y, yaw, x_r, y_r, yaw_r):

dphidx1 = self.q1*(x-x_r)

dphidx2 = self.q2*(y-y_r)

dphidx3 = self.q3*(yaw-yaw_r)

return dphidx1, dphidx2, dphidx3

``

Je vous suggère de bien comprendre les équations mathématiques que je tente d'expliquer ci-dessus. C'est facile à implémenter en Python si vous en comprenez déjà l'expression. J'ai maintenant créé une autre classe pour NMPC avec CGMRES.

```python
class NMPCCGMRES:

def __init__(self):

## Paramètres NMPC

self.x_dim = 3

self.u_dim = 2

self.c_dim = 2

self.pred_horizons = 10

### Paramètres de continuation

self.ht = 1e-5

self.zeta = 1/self.ht

self.alpha = 0.5

self.tf = 1.0

### Matrice

self.q1 = 1

self.q2 = 6

self.q3 = 1

self.r1 = 1

self.r2 = 0.1

### Contrainte

self.u_min = [ 0, 0]

self.u_max = [ 2, 1.57]

self.u_dummy = [0.01, 0.01]

### Initialisation de la matrice Solution

self.dU = np.zeros((self.u_dim, self.pred_horizons))

### Point de référence

self.X_r = [3, 2, 0.0]

self.U_r = [0, 0]

Ces valeurs servent uniquement à l'ajustement et à l'initialisation des paramètres. Concentrons-nous maintenant sur la méthode CGMRES, comme indiqué dans l'équation (1) ci-dessus. Notons que :

code

X = np.zeros((x_dim, pred_horizons+1)) ## État pour N-prédiction+1
U = np.zeros((u_dim, pred_horizons)) ## Contrôle d'entrée pour N-prédiction
Lambda = np.zeros((x_dim, pred_horizons+1)) ## Vecteur de co-état pour N-prédiction+1
dt = 0.01 ## Temps d'échantillonnage

On peut alors appliquer l'équation (1) comme suit :

code

def calc_state(self, X, U, dt):

for i in range(self.pred_horizons):

x_dot, y_dot, yaw_dot = self.robot_model.forward_kinematic(X[2, i], U[0, i], U[1, i])

X[0, i+1] = X[0, i] + x_dot * dt

X[1, i+1] = X[1, i] + y_dot * dt

X[2, i+1] = X[2, i] + yaw_dot * dt

return X

Ensuite, nous calculons la costate en sens inverse.

code

def calc_costate(self, X, U, Lambda, dt):

x_N, y_N, yaw_N = self.hjb_equation.dphidx(X[0, -1], X[1, -1], X[2, -1],

self.X_r[0], self.X_r[1], self.X_r[2])

Lambda[0, -1] = x_N

Lambda[1, -1] = y_N

Lambda[2, -1] = yaw_N

for i in reversed(range(1, self.pred_horizons)):

dhdx1, dhdx2, dhdx3 = self.hjb_equation.dhdx(

X[0, i], X[1, i], X[2, i],

self.X_r[0], self.X_r[1], self.X_r[2],

U[0, i], Lambda[0, i+1], Lambda[1, i+1]

)

Lambda[0, i] = Lambda[0, i+1] + dhdx1 * dt

Lambda[1, i] = Lambda[1, i+1] + dhdx2 * dt

Lambda[2, i] = Lambda[2, i+1] + dhdx3 * dt

return Lambda

Et enfin, calcul de la fonction F de Newton

code


def calc_f(self, X, U, Lambda, Mu, dt):

X_new = self.calc_state(X, U, dt)

Lambda_new = self.calc_costate(X_new, U, Lambda, dt)

F = np.zeros((self.u_dim, self.pred_horizons))

for i in range(self.pred_horizons):

dhdu1, dhdu2 = self.hjb_equation.dhdu(X_new[2, i], U[0, i], U[1, i],

self.U_r[0], self.U_r[1],

Lambda_new[0, i+1], Lambda_new[1, i+1], Lambda_new[2, i+1],

Mu[0, i], Mu[1, i])

F[0, i] = dhdu1

F[1, i] = dhdu2

F = F.T.ravel().reshape(-1, 1)

return F

Introduisons maintenant la méthode Continuation pour la fonction F. Vous pouvez consulter la section Continuation.

code

# Horizons des pas de temps
dt = self.tf * (1-np.exp(-self.alpha*time)) / self.pred_horizons
# F(x,u,t+h)
F = self.calc_f(X, U, Lambda, Mu, dt)
# F(x+hdx,u,t+h)

x_dot, y_dot, yaw_dot = self.robot_model.forward_kinematic(X[2, 0], U[0, 0], U[1, 0])
X[:, 0] = X[:, 0] + np.array([x_dot, y_dot, yaw_dot]) * self.ht
Fxt = self.calc_f(X, U, Lambda, Mu, dt)
# F(x+hdx,u+hdx,t+h)
Fuxt = self.calc_f(X, U + self.dU*self.ht, Lambda, Mu , dt)

# Définition de l'équation du second membre
gauche = (Fuxt - Fxt) / self.ht
droite = -self.zeta * F - (Fxt - F)/self.ht

Voici l'étape cruciale : nous devons implémenter le solveur GMRES pour notre fonction F afin d'obtenir le vecteur solution.

code


# Définition du second membrede l'équation
gauche = (Fuxt - Fxt) / self.ht
droite = -self.zeta * F - (Fxt - F)/self.ht

# Définition des itérations
m = (self.u_dim ) * self.pred_horizons
# Définition de r0
r0 = droite - gauche
# print(r0)

# GMRES
Vm = np.zeros((m, m+1))
Vm[:, 0:1] = r0 / np.linalg.norm(r0)
# print("Vm", Vm[:, 0])
Hm = np.zeros((m+1, m))
# print(Vm[:, 0])

for i in range(m):

Fuxt = self.calc_f(

X, U + Vm[:, i:i+1].reshape(self.u_dim, self.pred_horizons, ordre='F') * self.ht,

Lambda, Mu,

dt

)

Av = (Fuxt - Fxt) / self.ht

pour k dans range(i+1):

Hm[k, i] = np.dot(Av.T, Vm[:, k:k+1])

temp_vec = np.zeros((m, 1))

pour k dans range(i+1):

temp_vec = temp_vec + np.dot(Hm[k, i], Vm[:, k:k+1])

v_hat = Av - temp_vec

Hm[i+1, i] = np.linalg.norm(v_hat)

Vm[:,i+1:i+2] = v_hat/Hm[i+1, i]

e = np.zeros((m+1, 1))
e[0, 0] = 1.0

gm = np.linalg.norm(r0) * e
# print(gm)

UTMat, gm = self.ToUTMat(Hm, gm, m)

min_y = np.zeros((m, 1))

for i in range(m):

min_y[i][0] = (gm[i, 0] - np.dot(UTMat[i:i+1 ,:] ,min_y))/UTMat[i, i]

dU_new = self.dU + np.dot(Vm[:, 0:m], min_y).reshape(self.u_dim, self.pred_horizons, order='F')

self.dU = dU_new

U = U + self.dU * self.ht

Vous pouvez consulter mon code dans la fonction solve_nmpc.

Expérimentation en Python

Robot : entraînement différentiel
État initial : $(x_0 = [0, 0, 0])$
Entrée initiale : $(u_0 = [0, 0])$

Animation :

Animation du robot à entraînement différentiel sur une trajectoire optimale

Graphiques de simulation :

Toutes les données obtenues après la simulation

Conclusion

La commande prédictive non linéaire (NMPC) avec CGMRES offre un calcul numérique très rapide grâce à l'algorithme GMRES avec rotations de Givens. Comparé à d'autres solveurs NMPC, CGMRES peut être sensible au réglage des paramètres et présenter un comportement numérique instable s'il n'est pas paramétré avec soin. Les variantes à tirs multiples sont souvent plus robustes numériquement que celles à tir unique.

Références

article │nmpc-cgmres.md

Commande prédictive non linéaire avec C/GMRES

Commande optimale d’un robot à entraînement différentiel : le hamiltonien, la méthode de continuation et un solveur GMRES, en Python.

Sokheng Din28 oct. 2025~9 min#control#optimisation#python#robotics

Théorie du contrôle optimal dans les systèmes de contrôle

Qu'est-ce que le contrôle prédictif par modèle (MPC) ?

Équation de Hamilton-Jacobi-Bellman (HJBE)

L'équation HJBE est une équation aux dérivées partielles non linéaire qui exprime les conditions nécessaires à l'optimalité en minimisant la fonction de perte du système.

On définit l'hamiltonien :

\mathcal{H}(x,u,\lambda,\mu) = \sum_{k=0}^{N-1} (x_k - x_{k,\mathrm{ref}})^\top Q (x_k - x_{k,\mathrm{ref}}) + (u_k - u_{k,\mathrm{ref}})^\top R (u_k - u_{k,\mathrm{ref}}) + \phi(x) + \lambda^\top f(x,u) + \mu^\top C(x,u).

Où :

$N$ — nombre d'horizons de prédiction (nœuds de tir).
$x_k$ — état au nœud $k$ .
$u_k$ — entrée au nœud $k$ . - $Q, R$ — Matrices de pondération (généralement diagonales).
$f(x,u)$ — dynamique de transition d'état.
$C(x,u)$ — fonctions de contrainte d'égalité/inégalité.
$\lambda$ — vecteur co-état.
$\mu$ — multiplicateurs de Lagrange pour les contraintes.

La commande prédictive non linéaire (MPC) admet plusieurs méthodes de résolution ; nous nous concentrons ici sur la méthode de continuation/GMRES proposée par le professeur Toshiyuki Ohtsuka.

Supposons que nous souhaitions obtenir le vecteur solution à l'instant d'échantillonnage $t$ :

$U(t) = [u_0^{*}, \mu_0^{*}, \dots, u_{N-1}^{*}, \mu_{N-1}^{*}]$

Nous définissons les équations d'Euler-Lagrange discrétisées (ELE). Pour résoudre itérativement le système non linéaire résultant, nous utilisons la méthode de Newton sur $F(x,u) = 0$

où $F$ représente la stationnarité. Conditions à l'horizon :

F(x, u) = \begin{bmatrix} \frac{\partial \mathcal{H}}{\partial u}(x_0^*, u_0^*, \lambda_1^*, \mu_0^*) \\ C(x_0^*, u_0^*) \\ \vdots \\ \frac{\partial \mathcal{H}}{\partial u}(x_{N-1}^*, u_{N-1}^*, \lambda_N^*, \mu_{N-1}^*) \\ C(x_{N-1}^*, u_{N-1}^*) \end{bmatrix} = 0

La résolution directe de ce système est coûteuse ; la continuation réduit la complexité, puis nous appliquons la GMRES.

Continuation

Soit (\zeta, Soit α, h ∈ ℝ. Définissons la dynamique de continuation de F :

\frac{\partial F}{\partial u}\,\dot{u}(t) ​​= -\zeta F(x,u,t) - \frac{\partial F}{\partial x}(x,u,t)\,\dot{x} - \frac{\partial F}{\partial t}(x,u,t).

Définissons les approximations par différences finies :

\begin{aligned} \text{right} &= F\!\big(x(t) + h\dot{x}(t),\, u(t) + h\dot{u}(t),\, t + h\big) - F\!\big(x(t) + h\dot{x}(t),\, u(t),\, t + h\big), \\ \text{left} &= -\zeta\, F(x(t), u(t), t) - \frac{F\!\big(x(t) + h\dot{x}(t),\, u(t),\, t + h\big) - F(x(t), u(t), t)}{h}. \end{aligned}

Méthode des résidus minimaux généralisée (GMRES)

Schéma :

Choisir $x_0$ , calculer $r_0 = b - A x_0$ , puis définir $v_1 = \dfrac{r_0}{\|r_0\|}$ .
Pour $k = 0, \dots, m-1$ : appliquer la méthode d'Arnoldi pour obtenir $H_m$ et $V_m$ .
Résoudre $y = \arg\min_y \|\, \|r_0\| e_1 - H_m y \,\|$ .
Mettre à jour $x_m = x_0 + V_m y$ . Pour un modèle à entraînement différentiel avec état $x = [x,\, y,\, \phi]^\top$ et dynamique $\dot{x} = f(x,u)$ , on peut définir :

$f(x,u) = [\,u_1 \cos\phi,\; u_1 \sin\phi,\; u_2\,]^\top$
contraintes $C(x,u)$ (par exemple, contraintes de boîte sur $u_1, u_2$ )
pénalité terminale $\phi(x) = (x_N - x_{N,\mathrm{ref}})^\top Q (x_N - x_{N,\mathrm{ref}})$

Alors,

\frac{\partial \mathcal{H}}{\partial x} = \begin{bmatrix} (x-x_{\mathrm{ref}})q_1 \\[2pt] (y-y_{\mathrm{ref}})q_2 \\[2pt] (\phi-\phi_{\mathrm{ref}})q_3 - \lambda_1 u_1 \sin\phi + \lambda_2 u_1 \cos\phi \end{bmatrix}, \qquad \frac{\partial \mathcal{H}}{\partial u} = \begin{bmatrix} r_1(u_1-u_{1,\mathrm{ref}}) + \lambda_1\cos\phi + \lambda_2\sin\phi \\[2pt] r_2(u_2-u_{2,\mathrm{ref}}) + \lambda_3 \end{bmatrix}.

Implémentation en Python

Créons une classe pour l'équation de Bellman-Jacobien de Hamilton, que nous avons déjà dérivée de l'équation précédente.

code

class HJBEquation:

def __init__(self, q1, q2, q3, r1, r2, u_max, u_min):

self.q1 = q1

self.q2 = q2

self.q3 = q3

self.r1 = r1

self.r2 = r2

self.u1_min = u_min[0]

self.u2_min = u_min[0]

self.u1_max = u_max[0]

self.u2_max = u_max[1]

def dhdx(self,x, y, yaw, x_r, y_r, yaw_r, u1, lam1, lam2):

dhdx1 = (x-x_r)*self.q1

dhdx2 = (y-y_r)*self.q2

dhdx3 = (yaw-yaw_r)*self.q3-u1*lam1*np.sin(yaw)+u1*lam2*np.cos(yaw)

retourner dhdx1, dhdx2, dhdx3

def dhdu(self, yaw, u1, u2, u1_r, u2_r, lam1, lam2, lam3, mu1, mu2):

dhdu1 = self.r1*(u1-u1_r)+lam1*np.cos(yaw)+lam2*np.sin(yaw)+2*mu1*u1*(u1+(self.u1_max+self.u1_min)/2)

dhdu2 = self.r2*(u2-u2_r)+lam3+2*mu2*(u2+(self.u2_max+self.u2_min)/2)

return dhdu1, dhdu2

def dphidx(self, x, y, yaw, x_r, y_r, yaw_r):

dphidx1 = self.q1*(x-x_r)

dphidx2 = self.q2*(y-y_r)

dphidx3 = self.q3*(yaw-yaw_r)

return dphidx1, dphidx2, dphidx3

``

Je vous suggère de bien comprendre les équations mathématiques que je tente d'expliquer ci-dessus. C'est facile à implémenter en Python si vous en comprenez déjà l'expression. J'ai maintenant créé une autre classe pour NMPC avec CGMRES.

```python
class NMPCCGMRES:

def __init__(self):

## Paramètres NMPC

self.x_dim = 3

self.u_dim = 2

self.c_dim = 2

self.pred_horizons = 10

### Paramètres de continuation

self.ht = 1e-5

self.zeta = 1/self.ht

self.alpha = 0.5

self.tf = 1.0

### Matrice

self.q1 = 1

self.q2 = 6

self.q3 = 1

self.r1 = 1

self.r2 = 0.1

### Contrainte

self.u_min = [ 0, 0]

self.u_max = [ 2, 1.57]

self.u_dummy = [0.01, 0.01]

### Initialisation de la matrice Solution

self.dU = np.zeros((self.u_dim, self.pred_horizons))

### Point de référence

self.X_r = [3, 2, 0.0]

self.U_r = [0, 0]

Ces valeurs servent uniquement à l'ajustement et à l'initialisation des paramètres. Concentrons-nous maintenant sur la méthode CGMRES, comme indiqué dans l'équation (1) ci-dessus. Notons que :

code

X = np.zeros((x_dim, pred_horizons+1)) ## État pour N-prédiction+1
U = np.zeros((u_dim, pred_horizons)) ## Contrôle d'entrée pour N-prédiction
Lambda = np.zeros((x_dim, pred_horizons+1)) ## Vecteur de co-état pour N-prédiction+1
dt = 0.01 ## Temps d'échantillonnage

On peut alors appliquer l'équation (1) comme suit :

code

def calc_state(self, X, U, dt):

for i in range(self.pred_horizons):

x_dot, y_dot, yaw_dot = self.robot_model.forward_kinematic(X[2, i], U[0, i], U[1, i])

X[0, i+1] = X[0, i] + x_dot * dt

X[1, i+1] = X[1, i] + y_dot * dt

X[2, i+1] = X[2, i] + yaw_dot * dt

return X

Ensuite, nous calculons la costate en sens inverse.

code

def calc_costate(self, X, U, Lambda, dt):

x_N, y_N, yaw_N = self.hjb_equation.dphidx(X[0, -1], X[1, -1], X[2, -1],

self.X_r[0], self.X_r[1], self.X_r[2])

Lambda[0, -1] = x_N

Lambda[1, -1] = y_N

Lambda[2, -1] = yaw_N

for i in reversed(range(1, self.pred_horizons)):

dhdx1, dhdx2, dhdx3 = self.hjb_equation.dhdx(

X[0, i], X[1, i], X[2, i],

self.X_r[0], self.X_r[1], self.X_r[2],

U[0, i], Lambda[0, i+1], Lambda[1, i+1]

)

Lambda[0, i] = Lambda[0, i+1] + dhdx1 * dt

Lambda[1, i] = Lambda[1, i+1] + dhdx2 * dt

Lambda[2, i] = Lambda[2, i+1] + dhdx3 * dt

return Lambda

Et enfin, calcul de la fonction F de Newton

code


def calc_f(self, X, U, Lambda, Mu, dt):

X_new = self.calc_state(X, U, dt)

Lambda_new = self.calc_costate(X_new, U, Lambda, dt)

F = np.zeros((self.u_dim, self.pred_horizons))

for i in range(self.pred_horizons):

dhdu1, dhdu2 = self.hjb_equation.dhdu(X_new[2, i], U[0, i], U[1, i],

self.U_r[0], self.U_r[1],

Lambda_new[0, i+1], Lambda_new[1, i+1], Lambda_new[2, i+1],

Mu[0, i], Mu[1, i])

F[0, i] = dhdu1

F[1, i] = dhdu2

F = F.T.ravel().reshape(-1, 1)

return F

Introduisons maintenant la méthode Continuation pour la fonction F. Vous pouvez consulter la section Continuation.

code

# Horizons des pas de temps
dt = self.tf * (1-np.exp(-self.alpha*time)) / self.pred_horizons
# F(x,u,t+h)
F = self.calc_f(X, U, Lambda, Mu, dt)
# F(x+hdx,u,t+h)

x_dot, y_dot, yaw_dot = self.robot_model.forward_kinematic(X[2, 0], U[0, 0], U[1, 0])
X[:, 0] = X[:, 0] + np.array([x_dot, y_dot, yaw_dot]) * self.ht
Fxt = self.calc_f(X, U, Lambda, Mu, dt)
# F(x+hdx,u+hdx,t+h)
Fuxt = self.calc_f(X, U + self.dU*self.ht, Lambda, Mu , dt)

# Définition de l'équation du second membre
gauche = (Fuxt - Fxt) / self.ht
droite = -self.zeta * F - (Fxt - F)/self.ht

Voici l'étape cruciale : nous devons implémenter le solveur GMRES pour notre fonction F afin d'obtenir le vecteur solution.

code


# Définition du second membrede l'équation
gauche = (Fuxt - Fxt) / self.ht
droite = -self.zeta * F - (Fxt - F)/self.ht

# Définition des itérations
m = (self.u_dim ) * self.pred_horizons
# Définition de r0
r0 = droite - gauche
# print(r0)

# GMRES
Vm = np.zeros((m, m+1))
Vm[:, 0:1] = r0 / np.linalg.norm(r0)
# print("Vm", Vm[:, 0])
Hm = np.zeros((m+1, m))
# print(Vm[:, 0])

for i in range(m):

Fuxt = self.calc_f(

X, U + Vm[:, i:i+1].reshape(self.u_dim, self.pred_horizons, ordre='F') * self.ht,

Lambda, Mu,

dt

)

Av = (Fuxt - Fxt) / self.ht

pour k dans range(i+1):

Hm[k, i] = np.dot(Av.T, Vm[:, k:k+1])

temp_vec = np.zeros((m, 1))

pour k dans range(i+1):

temp_vec = temp_vec + np.dot(Hm[k, i], Vm[:, k:k+1])

v_hat = Av - temp_vec

Hm[i+1, i] = np.linalg.norm(v_hat)

Vm[:,i+1:i+2] = v_hat/Hm[i+1, i]

e = np.zeros((m+1, 1))
e[0, 0] = 1.0

gm = np.linalg.norm(r0) * e
# print(gm)

UTMat, gm = self.ToUTMat(Hm, gm, m)

min_y = np.zeros((m, 1))

for i in range(m):

min_y[i][0] = (gm[i, 0] - np.dot(UTMat[i:i+1 ,:] ,min_y))/UTMat[i, i]

dU_new = self.dU + np.dot(Vm[:, 0:m], min_y).reshape(self.u_dim, self.pred_horizons, order='F')

self.dU = dU_new

U = U + self.dU * self.ht

Vous pouvez consulter mon code dans la fonction solve_nmpc.

Expérimentation en Python

Robot : entraînement différentiel
État initial : $(x_0 = [0, 0, 0])$
Entrée initiale : $(u_0 = [0, 0])$

Animation :

Animation du robot à entraînement différentiel sur une trajectoire optimale

Graphiques de simulation :

Toutes les données obtenues après la simulation