TPE 2

Conception et contrôle optimal de la tra jectoire d'un

drone avec évitement d'obstacles

proposition Dr. Kikmo

Résumé
Cet TPE propose la modélisation et la résolution d'un problème de contrôle optimal

de trajectoire pour un drone évoluant en 2D. L'ob jectif est de concevoir une stratégie de

pilotage garantissant l'atteinte d'une cible tout en évitant un obstacle modélisé par des

contraintes linéaires (formes linéaires). L'approche repose sur la formulation matricielle du

système dynamique, l'écriture du Lagrangien, la dérivation du problème dual et l'analyse des

multiplicateurs de Lagrange, interprétés comme forces virtuelles d'évitement. L'étudiant est

amené à construire les matrices associées, à résoudre numériquement le programme convexe

obtenu et à interpréter les conditions de KKT, ce qui assure une compréhension pratique et

rigoureuse du lien entre modélisation, optimisation et dualité.

1 Contexte et modèle

On considère un drone évoluant dans le plan 2D, dont l'état discret au pas k est :
 
pk
xk = ∈ R4 , pk = (xk , yk ) ∈ R2 , vk ∈ R2 ,
vk
où pk représente la position et vk la vitesse. La dynamique linéaire discrète (pas ∆t) s'exprime
par :
xk+1 = Axk + Buk , k = 0, . . . , N − 1,
avec ! !
I2 ∆t I2 1 2
2 ∆t I2
A= , B= ,
0 I2 ∆tI2
et uk ∈ R2 l'accélération appliquée (contrôles bornés). L'état initial x0 est connu et la position
cible est pgoal ∈ R2 .
Le drone doit éviter un ou plusieurs obstacles situés dans son environnement. Chaque obstacle,
de nature quelconque (arbre, bâtiment, poteau, zone interdite, autres drones, etc.), est modélisé
par un ensemble convexe de contraintes linéaires sur la position :
C (j) pk ≤ d(j) , j = 1, . . . , M,
où M est le nombre total d'obstacles et chaque ligne de C (j) ∈ Rmj ×2 dénit une face de
l'obstacle j , avec d(j) ∈ Rmj . Ces contraintes s'empilent ensuite pour former l'ensemble global
de contraintes d'évitement.
Les commandes sont, par exemple, limitées par :
(i)
∥uk ∥∞ ≤ umax ⇐⇒ −umax ≤ uk ≤ umax , i = 1, 2.
Le problème de contrôle optimal consiste à minimiser l'eort total et l'écart terminal :
N
X −1
min J= ∥uk ∥22 + ρ∥pN − pgoal ∥22 ,
u0 ,...,uN −1
k=0

1
tout en respectant les contraintes de dynamique, de commandes et d'évitement de tous les obs-
tacles.

2 Formulation matricielle compacte

Pour simplier la notation et faciliter la résolution numérique, on regroupe tous les états et
toutes les commandes sur l'horizon N :
   
x1 u0
 x2   u1 
X =  .  ∈ R4N , U =  .  ∈ R2N .
   
 ..   .. 
xN uN −1

Il existe des matrices A et B telles que la dynamique complète s'écrit :

X = Ax0 + BU.

Ces matrices accumulent les eets des matrices A et B sur tout l'horizon et seront explicitement
construites dans la partie numérique.
Le coût quadratique peut s'écrire sous la forme standard :
1
J(U ) = U ⊤ HU + f ⊤ U + const,
2
avec H ≻ 0 dénie positive, intégrant l'eort sur toutes les commandes et le terme terminal de
position.
Les contraintes se regroupent sous la forme matricielle :

EU = b (contraintes d'égalité dynamiques explicites),

GU ≤ h (contraintes d'évitement de tous les obstacles et bornes sur uk ).

Pour chaque obstacle j = 1, . . . , M et chaque pas k = 1, . . . , N , les contraintes linéaires de

type
C (j) pk ≤ d(j)
se transforment en contraintes linéaires en fonction de U via la sélection de la position dans X :

C (j) Sk (Ax0 + BU ) ≤ d(j) =⇒ (C (j) Sk B)U ≤ d(j) − C (j) Sk Ax0 ,

où Sk est la matrice de sélection extrayant pk de X .

En empilant ces contraintes pour tous les obstacles et tous les pas, on obtient la matrice G
et le vecteur h globaux pour le problème d'optimisation :

GU ≤ h.

Ainsi, le problème de contrôle optimal du drone avec obstacles multiples se réduit à un

**programme quadratique convexe** :
1 ⊤
min U HU + f ⊤ U sous les contraintes EU = b, GU ≤ h,
U 2
prêt à être traité par des solveurs numériques.

2
3 Lagrangien et dualité

Le problème de contrôle optimal formulé en forme convexe standard s'écrit :

1 ⊤
min U HU + f ⊤ U
U 2
s.c. EU = b, GU ≤ h,
où EU = b encode la dynamique et GU ≤ h regroupe toutes les contraintes d'évitement des
obstacles et bornes sur les commandes.
On introduit le Lagrangien associé, avec ν pour les contraintes d'égalité et λ ≥ 0 pour les
contraintes d'inégalité :
1
L(U, λ, ν) = U ⊤ HU + f ⊤ U + λ⊤ (GU − h) + ν ⊤ (EU − b).
2
La fonction duale est dénie par :

g(λ, ν) = inf L(U, λ, ν).

U

La condition de stationnarité (dérivée de L par rapport à U ) donne :

HU + f + G⊤ λ + E ⊤ ν = 0.

Si H est dénie positive, la solution unique de cette minimisation est :

U ⋆ (λ, ν) = −H −1 (f + G⊤ λ + E ⊤ ν).

En remplaçant U ⋆ dans le Lagrangien, on obtient la fonction duale explicite :

1
g(λ, ν) = − (f + G⊤ λ + E ⊤ ν)⊤ H −1 (f + G⊤ λ + E ⊤ ν) − λ⊤ h − ν ⊤ b.
2
Le problème dual s'écrit alors :
max g(λ, ν).
λ≥0, ν

Interprétation physique : un multiplicateur λi > 0 associé à la i-ème face d'un obstacle

indique que cette contrainte est active à l'optimum. La trajectoire  pousse  sur cette face, et
la contrainte génère une force virtuelle inhibitrice.

4 Conditions KKT

Pour ce problème convexe, les conditions de Karush-Kuhn-Tucker (KKT) sont :

(a) Stationnarité : HU ⋆ + f + G⊤ λ⋆ + E ⊤ ν ⋆ = 0,
(b) Primal faisabilité : EU ⋆ = b, GU ⋆ ≤ h,
(c) Dual faisabilité : λ⋆ ≥ 0,
(d) Complémentarité : λ⋆i (Gi U ⋆ − hi ) = 0 pour tout i.
Ces conditions permettent d'identier quelles faces de quels obstacles sont actives et comment
la commande U ⋆ est ajustée pour éviter les collisions tout en minimisant le coût.

3
5 Cas numérique à résoudre

 ∆t = 0.5 s, horizon N = 8,
 État initial x0 = (p0 = (0, 0), v0 = (0, 0)),
 Position cible pgoal = (8, 4),
 Commande maximale umax = 1.5 m/s2 ,
 Obstacle : rectangle centré en (4, 2), demi-largeur 0.5 (axe x), demi-hauteur 0.75 (axe y ),
 Coecient terminal ρ = 100,
 Coût : norme L2 (Quadratic Program).

5.1 Matrice C et vecteur d pour le rectangle

Les quatre faces de l'obstacle sont représentées par :

x ≤ 4.5 =⇒ [1 0]p ≤ 4.5,
−x ≤ −3.5 =⇒ [−1 0]p ≤ −3.5,
y ≤ 2.75 =⇒ [0 1]p ≤ 2.75,
−y ≤ −1.25 =⇒ [0 − 1]p ≤ −1.25.

Pour chaque pas k, on obtient les contraintes en fonction de U :

Gk = CSk B, hk = d − CSk Ax0 ,

où Sk sélectionne la position pk dans X . En empilant tous les pas et tous les obstacles, on obtient
le G et h globaux.

5.2 Tâches à réaliser

(a) Construire numériquement les matrices A, B, H, f, G, h, E, b pour les paramètres donnés.

(b) Écrire explicitement le Lagrangien et la forme analytique du dual.
(c) Résoudre numériquement le primal (avec cvxpy + OSQP/ECOS) et récupérer les multiplica-
teurs λ⋆ .
(d) Identier les faces actives et instants correspondants.
(e) Vérier la condition de complémentarité.
(f) Tracer la trajectoire {pk }N
k=0 pour montrer l'évitement de l'obstacle.
(g) Sensibilité : augmenter les demi-dimensions du rectangle de 20%, recalculer la solution et
analyser l'impact sur la trajectoire et les multiplicateurs.

6 Points importants et recommandations

 Les blocs de A et B s'écrivent par accumulation des puissances de A et des contributions

de B sur l'horizon.
 La stationnarité donne U ⋆ = −H −1 (f + G⊤ λ + E ⊤ ν) ; insérer dans L fournit g(λ, ν)
(fonction concave).
 Les solveurs numériques retournent souvent les dual variables, ce qui permet d'interpréter
l'activation des contraintes.
 Pour la robustesse, on peut agrandir les obstacles (méthode d'ination) ou ajouter des
marges de sécurité.

4
7 Critères d'évaluation

L'évaluation de l'exercice repose sur quatre axes principaux, chacun précisant les livrables et
les attentes :
1. Construction matricielle et formulation des contraintes (25%) :
 Matrices A et B correctement construites.
 Contraintes GU ≤ h correctement formulées pour tous les obstacles et tous les pas
temporels.
2. Formulation du Lagrangien et du dual (25%) :
 Présentation précise du Lagrangien.
 Expression analytique du dual et interprétation claire des multiplicateurs de Lagrange.
3. Résolution numérique et vérication des conditions KKT (30%) :
 Résolution correcte du problème primal.
 Récupération et interprétation des multiplicateurs λ⋆ .
 Vérication de la complémentarité et tracés clairs de la trajectoire évitant les obs-
tacles.
4. Analyse de sensibilité et discussion (20%) :
 Étude de l'impact de l'ination des obstacles ou de variations des paramètres.
 Discussion sur la robustesse de la trajectoire et interprétation des résultats.