475 lines
37 KiB
TeX
475 lines
37 KiB
TeX
\chapter{Réalisation du projet}
|
||
\section{Prise en main de ROS et du drone présent à la smartroom}
|
||
Pour la prise en main de ROS, nous avons suivi les étapes du tutoriel de M. \bsc{Frezza-Buet}. Cela nous a permis de comprendre comment fonctionne ROS et de pouvoir l'utiliser de manière basique et simple.
|
||
|
||
Pour installer et utiliser ROS, ainsi que notre projet, on pourra se référer à l'annexe \ref{chap:installation_projet}.
|
||
|
||
\section{Réduction du bruit}
|
||
\subsection{Problème et solution liés au bruit causé par le calcul de la position du drone par rapport à l'image}
|
||
Nous avons constaté qu'une forte incertitude existait lors du calcul de la position du drone par rapport au panneau. En effet, celle-ci, même si le drone était immobile, bougeait beaucoup et faisait des "sauts". Il s'agissait d'un bruit de type de Poisson, difficile, voire impossible, à traiter en automatique pour obtenir un asservissement satisfaisant. En effet, le drone, avec ce bruit, y aurait été asservi, rendant impossible l'obtention d'une position fixe. Notre drone aurait fait de grands écarts par rapport à la position attendue.
|
||
|
||
Afin de régler ce problème, l'idée proposée a été de modifier l'algorithme en place. Ce dernier calcule la position des trois carrés bleus dans l'image renvoyée par le drone par les position H, R et L. Cette détermination se faisait par gngt. Nous avons donc développé un nouveau script Python qui utilise la labellisation \cite{scipy} afin de déterminer la position des panneaux.
|
||
|
||
|
||
Dans cette partie, nous ne nous intéresserons qu'au fichier
|
||
\verb|find_targets.py|. Il permet, à partir d'un seuil RGB défini, de trouver les parties "bleues" de l'image\footnote{On pourrait cependant chercher d'autres couleurs en changeant les valeurs des seuils.}. Une fois ces positions trouvées, on ne garde que les trois plus grandes parties et on en calcule le centre de masse. Les résultats obtenus sont plutôt satisfaisants.
|
||
\subsection{Détails des fonctions exposées par \mintinline{bash}|find_targets.py|}
|
||
Le module contient deux fonctions.
|
||
\subsubsection{\mintinline{python}|find_targets|}
|
||
|
||
Cette fonction prend en argument l'image dont on souhaite extraire la cible, des valeurs de seuil bleu, rouge et vert minimales et maximales, un booléen \verb|return_slices| qui indique s'il faut, ou non, retourner les slices localisant les limites de la cible et un booléen \verb|return_binary| qui indique s'il faut, ou non, retourne l'image binaire. Ce dernier sera utile pour régler des seuils lorsque, pas exemple, la couleur des cibles est modifiée.
|
||
|
||
La fonction retourne les coordonnées des centres de masse des trois carrés de la cible sous la forme d'un tuple \verb|(H,L,R)| avec \verb|H| le point le plus haut, \verb|L| le point le plus à gauche et \verb|R| le point le plus à droite ainsi que les slices correspondant aux zones bleues le cas échéant\footnote{Cela permet de dessiner les rectangles, ce qui est utile lorsqu'on souhaite procéder à des vérifications}.
|
||
|
||
La première étape de l'algorithme consiste à détecter tous les points répondant aux conditions de seuil définis dans les arguments de la fonction. Lorsqu'un pixel de l'image remplit les conditions, un 1 est placé sur ce pixel, sinon on y place un 0. On construit ainsi une matrice de 0 et de 1 correspondant aux points satisfaisant les critères de seuil.
|
||
|
||
On définit ensuite une structure de labellisation. Pour cela, on utilise la matrice suivante :
|
||
$$
|
||
\begin{bmatrix}
|
||
0 & 1 & 0 \\
|
||
1 & 1 & 1 \\
|
||
0 & 1 & 0
|
||
\end{bmatrix}
|
||
$$
|
||
Cette matrice permet de demander à l'algorithme de labellisation de considérer que deux pixels A et B sont dans la même surface si et seulement si le pixel B se trouve à gauche, à droite, au-dessus ou en-dessous du pixel B. L'algorithme numérote ensuite ces surfaces de manière unique. Nous récupérons ainsi l'image labellisée ainsi que le nombre de zones détectées.
|
||
|
||
Dans le cas où moins de trois zones sont trouvées, on considère que la détection s'est mal déroulée et une erreur est levée.
|
||
|
||
On récupère ensuite les zones détectées sous la forme d'une liste de 3-uplets \verb|(a[0],a[1],i)| où \verb|a[0]| et \verb|a[1]| sont les slices des zones (elles délimitent le parallélépipède le plus petit contenant la zone) et \verb|i| est le numéro de la zone.
|
||
|
||
On trie ensuite les zones par aire et on ne garde que les trois zones ayant les aires les plus grandes. On suppose ici qu'aucune autre surface bleue importante, qui pourrait être confondue avec les cibles, ne sera présente sur notre image.
|
||
|
||
Nous déterminons ensuite les centres de masse des zones trouvées grâce à \verb|center_of_mass|.
|
||
|
||
Enfin, on trie les centre de masse selon l'axe vertical et on définit \verb|H| comme le point le plus haut. On trie les centres de masse restants selon l'axe horizontal et on définit \verb|L| par le point le plus à gauche et \verb|R| comme le point le plus à droite. On retrouve ainsi les trois centres de masse de la cible. On retourne ces derniers, avec leurs slices le cas échéant.
|
||
\subsubsection{\mintinline{python}|normalize_coordinates|}
|
||
Cette fonction prend en argument un point, la largeur et la hauteur d'une image et retourne des coordonnées \verb|(x,y)| normalisés. Cette fonction nous permet d'adapter le format des coordonnées renvoyés au code qui existait déjà. Il s'agit donc d'un changement de repère : on place l'origine au centre de l'image, on inverse l'axe y\footnote{Dans une image classique, l'axe y est orienté "vers le bas"} et on normalise les positions de manière à ce que la plus grande dimension (hauteur ou largeur) de l'image vaille 1.
|
||
|
||
En posant $w$ la largeur, $h$ la hauteur et $j = max(w, h)$, cela revient à calculer les nouvelles coordonnées $(x', y')$ par la transformation affine de l'équation \ref{eq:normalisation}.
|
||
\begin{equation}
|
||
(x',y') = (x,y)\cdot\begin{pmatrix}
|
||
\frac{1}{j} & 0 \\
|
||
0 & -\frac{1}{j}
|
||
\end{pmatrix}+ \left(-\frac{w}{2j}, \frac{h}{2j}\right) \label{eq:normalisation}
|
||
\end{equation}
|
||
\subsection{Conclusion et résultats}
|
||
Nous obtenons désormais une position bien plus stable du drone. Évidemment, le bruit n'a pas été totalement supprimé, mais il ne provoque plus de "sauts" dans la position et reste, en moyenne, autour de la position du drone. Le bruit restant est probablement causé par la qualité de l'image renvoyée par le drone et par le seuil que nous avons défini. Des résultats de l'algorithme sont donnés figure \ref{result}. Les positions des cibles seront ensuite publiées grâce au nœud \mintinline{bash}|target_publisher.py| dans un format compréhensible des nœuds de triangularisation.
|
||
|
||
Ces résultats ont été obtenu à l'aide du script \verb|test_find_targets.py| donné en annexe \ref{chap:test_find_targets}.
|
||
|
||
\begin{figure}
|
||
\centering
|
||
\includegraphics[width=\linewidth]{images/realisation/target.png}
|
||
\caption{Résultat de l'algorithme}
|
||
\label{result}
|
||
\end{figure}
|
||
|
||
\FloatBarrier
|
||
\section{Premier asservissement sommaire}
|
||
|
||
|
||
Le premier asservissement qui a été réalisé n'utilisait qu'une seule boucle et asservissait la position du drone. On se référera à la modélisation mécanique réalisée précédemment.
|
||
|
||
Pour ce premier asservissement sommaire, nous avons estimé la courbe de la réponse fréquentielle de notre drone afin d'en déduire le type de correcteur à utiliser.
|
||
|
||
Nous savons que selon l'axe z, le drone répond seulement en vitesse. On peut approximer sa réponse fréquentielle par une pente nulle en basse fréquence, puis une pente \verb|[-1]| en haute fréquence. Afin d'asservir le drone, nous aurons donc besoin d'un correcteur de type proportionnel-intégral
|
||
\begin{equation}
|
||
C(p)=K_p + \frac{K_i}{p}
|
||
\end{equation}
|
||
Concernant les axes x et y, nous savons que le drone ne peut être correctement asservi en vitesse. En effet, le réglage de la vitesse et de l'accélération du drone se fait selon un angle d'inclinaison : le drone accélère en permanence s'il conserve le même angle ce qui ne permet pas le maintien d'une vitesse constante. La réponse fréquentielle selon ses axes est donc représentée par une pente de \verb|[-1]| dans les basses fréquence et une pente de \verb|[-2]| dans les hautes fréquences. Afin de corriger ces axes, nous allons donc utiliser une correction de type Proportionnelle-Dérivée :
|
||
\begin{equation}
|
||
C(p)=K_p+K_d p
|
||
\end{equation}
|
||
|
||
\subsection{Script \mintinline{bash}|triangle_control.py|}
|
||
|
||
Ce script reprend, en partie, le script provenant du projet initial qu'il est possible de retrouver sur le Gitlab de M. \bsc{Frezza-Buet} \cite{gitlab_frezza}. Nous l'avons adapté à notre projet et en avons repris les éléments principaux. Il utilise le fichier de configuration (Voir Annexe \ref{chap:configuration}) \verb|triangle_control.cfg| qui a également été repris, en partie, du projet initial.
|
||
La classe \verb|TriangleControl| implémente 6 méthodes :
|
||
\begin{description}
|
||
\item[\mintinline{python}|on_reconf|] Cette méthode permet, à partir du fichier de configuration, de définir les valeurs à donner aux différents paramètres des 4 PID, ainsi que les valeurs limites de l'accélération et de la vitesse du drone, l'angle de la caméra, la largeur et la longueur de la cible et la distance à laquelle on souhaite que le drone se trouve par rapport à la cible;
|
||
\item[\mintinline{python}|clear_controls|] : remet les erreurs à 0;
|
||
\item[\mintinline{python}|saturate_twist|] : limite la vitesse selon tous les axes;
|
||
\item[\mintinline{python}|on_comp|] : méthode qui n'a pas été modifié et qui permet de définir la position des trois rectangles bleus de la cible;
|
||
\item[\mintinline{python}|triangle|] : prend en paramètre les trois points de la cible et calcule la consigne de sortie à l'aide de la bibliothèque SimplePID de Python.
|
||
\end{description}
|
||
|
||
\subsection{Conclusion sur l'utilisation d'une simple boucle}
|
||
|
||
La régulation simple donne des résultats satisfaisants. Cependant quelques problèmes subsistent. Tout d'abord celui du dépassement. En effet la régulation étant linéaire, le dépassement est proportionnel à la consigne. Or si le drone se trouve à grande distance de la cible, le dépassement est tel que cette dernière finit par se trouver hors champ de la caméra, provoquant un comportement chaotique du drone. Une solution pour cela serait de saturer la vitesse du drone afin de limiter l'inertie, et donc le dépassement. D'autre part le système reste "assez lent", dans le sens où il ne donne pas vraiment le sentiment d'un asservissement rigide à la cible. Enfin, il serait intéressant de rendre notre code plus modulaire afin de pouvoir facilement changer la régulation dans un fichier \verb|.launch| de ROS. Afin de régler les deux premiers problèmes, nous choisissons d'implémenter une double régulation en vitesse pour laquelle nous développons une bibliothèque pour effectuer de l'automatique simple sous ROS
|
||
|
||
\section{Développement de la bibliothèque d'automatique sous ROS}
|
||
|
||
Nous avons développer une bibliothèque permettant de créer d'implémenter des PID sous ROS et de les régler dynamiquement par un protocole expérimental simple. Afin d'implémenter les correcteurs, nous avons créé des nœuds ROS que nous avons ensuite connectés dans des fichiers \verb|.launch|.
|
||
|
||
L'idée est de créer plusieurs classes de nœuds dans un script Python dont les paramètres seront obtenus à l'aide de fichiers \verb|.cfg|.
|
||
|
||
\subsection{Mesure de la vitesse}
|
||
|
||
Afin d'implémenter la partie dérivée de nos PID, il faut pouvoir calculer la vitesse du drone.
|
||
|
||
La mesure de vitesse doit se faire en dérivant la mesure de position. Cependant le bruit présent sur cette mesure empêche d'utiliser une dérivation naïve: en effet, on aurait alors de brusques sauts sur la valeur mesurée dus aux hautes fréquences dans le spectre du bruit. Afin d'obtenir une valeur lissée de la vitesse, on se propose d'utiliser un filtre de \bsc{Savitzky–Golay}.
|
||
|
||
\subsubsection{Principe}
|
||
|
||
Un filtre de \bsc{Savitzky–Golay} \cite{sav_gol} est un filtre à réponse impulsionnelle finie, qui correspond à une approximation locale du signal par un polynôme de degré faible. Ainsi, un filtre moyenneur est un filtre de \bsc{Savitzky–Golay} simple. Afin d'obtenir la réponse indicielle $[b_0, \dots b_n ]$ du filtre, on utilise la méthode des moindres carrés. En posant $((x_i,y_i))_{i\in\intervalleEntier{1}{n}}$ les $n$ points de la fenêtre, et en écrivant le polynôme d'approximation $a_0 + a_1 X + \dots + a_k X^k$, Cela revient donc à déterminer le jeu de coefficients $(a_0,\dots, a_k)$ qui minimise la grandeur de l'équation \ref{eq:moindrecarres_raw}.
|
||
|
||
\begin{equation}
|
||
\left \|
|
||
\begin{pmatrix}
|
||
y_1 \\
|
||
y_2 \\
|
||
\vdots \\
|
||
y_n
|
||
\end{pmatrix}
|
||
-
|
||
\begin{pmatrix}
|
||
1 & x_1 & x_1 ^2 & \cdots & x_1 ^ k \\
|
||
1 & x_2 & x_2 ^2 & \cdots & x_2 ^ k \\
|
||
\vdots & \vdots & \vdots & & \vdots \\
|
||
1 & x_n & x_n ^2 & \cdots & x_n ^ k
|
||
\end{pmatrix}
|
||
\cdot
|
||
\begin{pmatrix}
|
||
a_0 \\
|
||
a_1 \\
|
||
\vdots \\
|
||
a_k
|
||
\end{pmatrix}
|
||
\right \| ^2
|
||
\label{eq:moindrecarres_raw}
|
||
\end{equation}
|
||
|
||
En supposant de plus que la fenêtre de points $((x_i,y_i))_{i\in\intervalleEntier{1}{k}}$ que l'on souhaite lisser est centrée en 0 (on peut toujours se ramener à ce cas par changement de variable), telle que les $x_i$ soient espacés régulièrement d'un pas $h$, et $k$ impair, on peut s'affranchir des valeurs prises par $(x_i)$ en calculant les $(a'_0,\dots, a'_k)$ qui minimisent
|
||
\begin{eqnarray}
|
||
L((y_k),(a_k)) &=&
|
||
\left \|
|
||
\underbrace{
|
||
\begin{pmatrix}
|
||
y_1 \\
|
||
y_2 \\
|
||
\vdots \\
|
||
y_n
|
||
\end{pmatrix}}_{=Y}
|
||
-
|
||
\underbrace{
|
||
\begin{pmatrix}
|
||
1 & -\floor{\frac{n}{2}} & \left(-\floor{\frac{n}{2}}\right) ^2 & \cdots & \left(-\floor{\frac{n}{2}}\right) ^ k \\
|
||
1 & \left(-\floor{\frac{n-1}{2}}\right) & \left(-\floor{\frac{n-1}{2}}\right) ^2 & \cdots & \left(-\floor{\frac{n-1}{2}}\right) ^ k \\
|
||
\vdots & \vdots & \vdots & \vdots & \vdots \\
|
||
1 & -1 & \left(-1\right) ^2 & \cdots & \left(-1\right) ^ k \\
|
||
1 & 0 & 0 & \cdots & 0 \\
|
||
1 & \left(1\right) & \left(1\right) ^2 & \cdots & \left(1\right) ^ k \\
|
||
\vdots & \vdots & \vdots & \vdots & \vdots \\
|
||
1 & \left(\floor{\frac{n-1}{2}}\right) & \left(\floor{\frac{n-1}{2}}\right) ^2 & \cdots & \left(\floor{\frac{n-1}{2}}\right) ^ k \\
|
||
1 & \left(\floor{\frac{n}{2}}\right) & \left(\floor{\frac{n}{2}}\right) ^2 & \cdots & \left(\floor{\frac{n}{2}}\right) ^ k
|
||
\end{pmatrix}}_{=M}
|
||
\cdot
|
||
\underbrace{
|
||
\begin{pmatrix}
|
||
a'_0 \\
|
||
a'_1 \\
|
||
\vdots \\
|
||
a'_k
|
||
\end{pmatrix}}_{=A}
|
||
\right \| ^2 \\
|
||
&=& \left(
|
||
Y
|
||
- M \cdot A
|
||
\right)^\intercal \cdot
|
||
\left(
|
||
Y
|
||
- M \cdot A
|
||
\right)
|
||
\label{eq:moindrecarres}
|
||
\end{eqnarray}
|
||
On a donc :
|
||
\begin{eqnarray}
|
||
\frac{\partial L}{\partial (A)}(A) &=& -2 \cdot M^\intercal \cdot (Y - M \cdot A)
|
||
\end{eqnarray}
|
||
Et finalement, en cherchant le point où cette dérivée s'annule :
|
||
\begin{eqnarray}
|
||
A = \underbrace{(M^\intercal\cdot M)^{-1} \cdot M^\intercal}_{=B} \cdot Y
|
||
\end{eqnarray}
|
||
|
||
On peut ainsi obtenir facilement les valeurs "lissées" en chaque point $i$ : en effet, la valeur de la fonction en un point est donné par le coefficient $a_0$, celle de sa dérivée par $\frac{a_1}{h}$ etc. Plus formellement, pour approximer la dérivée d'ordre $d$, on peut convoluer le signal par le filtre RIF $[b_1, \dots b_n ]$, avec
|
||
\begin{equation}
|
||
\forall i \in \intervalleEntier{0}{n}, b_i = B_{d,n-i}
|
||
\end{equation}
|
||
|
||
Dans le cas de notre projet, on ne s'intéresse qu'a la vitesse (donc $d=1$). Il faut maintenant déterminer la taille du filtre ($n$) et le degré $k$ du polynôme d'approximation. En effet on souhaite minimiser $n$ car la mesure de vitesse induit un retard de $\frac{n}{2} \times h$.
|
||
|
||
\subsubsection{Protocole expérimental}
|
||
|
||
Afin de déterminer le filtre que nous utiliserons, nous choisissons d'en tester plusieurs sur le même relevé de position. Pour relever la position, on se contente de tenir le drone allumé face à la cible puis de marcher dans sa direction. On se référera à l'annexe \ref{chap:export} pour la méthode employée pour exporter les données depuis ROS vers un fichier \verb|output.txt|. On peut ensuite traiter les données avec le script Julia \verb|test_filter.jl| (annexe \ref{chap:test_filter}).
|
||
|
||
|
||
\subsubsection{Choix du filtre}
|
||
Les résultats obtenus sont donnés figures \ref{fig:savgol_quad}, \ref{fig:savgol_cub} et \ref{fig:savgol_big_quad}. On remarque que la vitesse est mieux lissée lorsqu'un filtre quadratique est utilisé. En effet, plus le degré du polynôme est élevé, plus il sera proche du signal mesuré et donc lissera moins bien.
|
||
|
||
Sans surprise, le fait d'augmenter la taille du filtre permet de mieux lisser la vitesse. Toutefois, pour éviter de causer un retard trop important dû à l'attente des points pour calculer la dérivée, il sera nécessaire de trouver un compromis entre signal lissé et retard inhérent au filtre, ceci afin d'augmenter la stabilité du système.
|
||
|
||
Après étude des courbes obtenues, nous avons choisis de commencer les expérimentations d'asservissement avec un filtre quadratique de 19 points, ce qui semble assurer un bien meilleur lissage. Cependant il faut prendre en compte le retard induit par la taille de fenêtre.
|
||
|
||
Lors de l'implémentation du filtre, il faut également s'assurer que la fréquence d'échantillonage du signal d'entrée correspond à celle pour lequel le filtre est calculé\footnote{Nos expérimentations ont montré la très mauvaise stabilité du filtre dans les cas où la fréquence d'échantillonage n'est pas constante.}.
|
||
|
||
\begin{figure}[h!]
|
||
\centering
|
||
\includegraphics[width=\linewidth]{images/realisation/mesure_vitesse_quadratique.eps}
|
||
\caption{Performance des filtres quadratiques}
|
||
\label{fig:savgol_quad}
|
||
\end{figure}
|
||
\begin{figure}[h!]
|
||
\centering
|
||
\includegraphics[width=\linewidth]{images/realisation/mesure_vitesse_cubique.eps}
|
||
\caption{Performance des filtres cubiques}
|
||
\label{fig:savgol_cub}
|
||
\end{figure}
|
||
\begin{figure}[h!]
|
||
\centering
|
||
\includegraphics[width=\linewidth]{images/realisation/mesure_vitesse_big_quadra.eps}
|
||
\caption{Performance des filtres quadratiques pour des nombres de points élevés}
|
||
\label{fig:savgol_big_quad}
|
||
\end{figure}
|
||
\FloatBarrier
|
||
|
||
\subsection{Création de fichiers de configuration}
|
||
Les fichiers de configuration vont nous permettre de régler, à l'aide d'une interface graphique, les paramètres de nos correcteurs dans la double boucle. Pour une explication plus détaillée du fonctionnement des fichiers de configuration, on se réfèrera à l'annexe \ref{chap:configuration}.
|
||
|
||
Nous avons créé les fichiers de configurations suivants :
|
||
\begin{description}
|
||
\item[\mintinline{python}|DerivativeNode.cfg|] : permet de choisir le gain de la partie dérivée du correcteur et le filtre de Savitzky-Golay adapté (ordre et taille du polynôme à utiliser);
|
||
\item[\mintinline{python}|IntegralNode.cfg|] : permet de choisir le gain de la partie intégrale du correcteur et les valeurs minimale et maximale de l'intégrale;
|
||
\item[\mintinline{python}|ProportionalNode.cfg|] : permet de choisir le gain de la partie proportionnelle du correcteur;
|
||
\item[\mintinline{python}|InputNode.cfg|] : contient la valeur d'une entrée;
|
||
\item[\mintinline{python}|SaturateNode.cfg|] : permet de choisir les valeurs de saturation (max et min);
|
||
\item[\mintinline{python}|RateNode.cfg|] : permet de forcer une fréquence d'échantillonage constante.
|
||
\end{description}
|
||
\subsection{Création des classes de nœuds dans un script Python}
|
||
L'idée du script est de créer une classe de n\oe ud \verb|ControlNode| qui se spécialise en d'autres n\oe uds utiles pour notre PID. Chacun des noeuds héritant de \verb|ControlNode| prendra alors une ou plusieures entrées afin de générer une sortie. On peut également leur appliquer un reset afin de mettre la sortie à 0.
|
||
|
||
Dans la classe \verb|ControlNode|, on définit un Publisher \verb|output_topic| qui publie sur le topic "output" ainsi qu'un Subscriber qui s'inscrit au topic "reset".
|
||
|
||
Le diagramme d'héritage de notre script est donné figure \ref{fig:diag_pid}, page \pageref{fig:diag_pid}.
|
||
|
||
\begin{landscape}
|
||
\begin{figure}[h!]
|
||
\centering
|
||
\includegraphics[width=\linewidth]{images/realisation/uml.png}
|
||
\caption{Diagramme UML}
|
||
\label{fig:diag_pid}
|
||
\end{figure}
|
||
\end{landscape}
|
||
|
||
Les méthodes de la classe mère sont les suivantes :
|
||
\begin{description}
|
||
\item[\mintinline{python}|on_reset(self, value)|] : la valeur de l'output est remise à 0. Certaines classes filles comme \verb|DerivativeNode| surchargent cette méthode pour remettre d'autres valeurs à 0;
|
||
\item[\mintinline{python}|on_reconf(self, config, level)|] : permet de récupérer la configuration, réalise également un appel à \mintinline{python}|on_reset|;
|
||
\item[\mintinline{python}|on_compute(self, value)|] : publie sur le topic "output" et met à jour la date du dernier calcul pour la limitation de fréquence;
|
||
\item[\mintinline{python}|check_time(self, delta_time=0.0)|] : permet de vérifier qu'on ne réalise pas de calculs trop souvent et donc de manière irrégulière\footnote{En effet, comme il est dit dans la section portant sur le filtre dérivateur, ce dernier nécessite un pas d'échantillonage constant.}. On vérifie que le temps passé depuis le dernier calcul est supérieur au \verb|refresh_time| que l'on s'est imposé.
|
||
\end{description}
|
||
|
||
On définit une classe fille \verb|InputControlNode| qui hérite de \verb|ControlNode|. Ceci a été l'occasion d'expérimenter un effet de bord intéressant de l'héritage en environnement multi-threadé. En effet, \verb|ControlNode| lance le Subscriber sur \verb|on_reset| dès l'appel à la méthode \verb|__init__| de la classe mère, mais c'est la méthode \verb|on_reset| de la classe fille qui est appelée, méthode qui n'est possiblement pas encore correctement initialisée, menant à des erreurs d'exécutions. Cette classe fille nous permet de rajouter un Subscriber qui récupère un input d'un topic "input".
|
||
|
||
Toutes les classes filles de \verb|InputControlNode| s'inscriront donc à un topic pour récupérer une entrée et publieront sur un topic "output". Les classes héritant de \verb|InputControlNode| sont :
|
||
\begin{description}
|
||
\item[\mintinline{python}|ProportionalNode|] : partie proportionnelle du correcteur, permet de définir la valeur du gain et de publier l'output sur un topic avec \verb|on_compute| après vérification du temps;
|
||
\item[\mintinline{python}|SaturateNode|] : permet d'imposer une valeur maximale et minimale à l'output. La classe \verb|IntegralNode| en hérite. Cette classe permet de définir la valeur du gain de l'intégrale et de publier l'output sur un topic (output qui sera donc saturé). Ceci permet d'éviter des instabilités dûes à l'action intégrale. L'intégration est réalisée avec la méthode des rectangles\footnote{Il serait intéressant d'étudier l'effet de méthodes d'intégration plus performantes, notamment sur la stabilité.};
|
||
\item[\mintinline{python}|DerivativeNode|] : partie dérivée du correcteur, permet de définir la valeur du gain, la façon de calculer la valeur de la dériver et de publier l'output de la correction dérivée sur un output. La valeur de la dérivée est calculée à l'aide d'un filtre de \bsc{Savitzky-Golay} dont on récupère le paramètre dans la configuration;
|
||
\item[\mintinline{python}|DifferenciateNode|] : permet de faire la différence entre une valeur d'entrée et une valeur mesurée;
|
||
\item[\mintinline{python}|InputNode|] : permet de définir un nœud publiant une sortie;
|
||
\item[\mintinline{python}|RateNode|] : permet de forcer une fréquence d'échantillonage fixe sur la sortie.
|
||
\end{description}
|
||
|
||
Enfin, \verb|SumNode| hérite de \verb|ControlNode|. Il va permettre de faire la somme des différentes parties du correcteur PID. Nous ne le faisons pas hériter de \verb|InputControlNode| car il ne prendra pas toujours une seule entrée. La classe prend donc en paramètre un nombre de topics auxquels elle s'inscrit et qui seront les entrées de la somme.
|
||
|
||
Ces classes permettront la création de nos nœuds ROS et à terme devraient permettre l'implémentation de la double boucle d'asservissement.
|
||
|
||
Le script que nous avons écrit prend en paramètre le type de nœud que l'utilisateur souhaite créer (\verb|sum|, \verb|differenciate|, \verb|input|, \verb|saturate|, \verb|derivative|, \verb|integral|, \verb|rate| ou \verb|proportional|) afin de pouvoir aisément lancer beaucoup de nœuds en les organisant dans un fichier \verb|.launch|
|
||
|
||
\subsection{Fichier \mintinline{bash}|control.launch|}
|
||
|
||
|
||
Puisque dans la suite nous aurons régulièrement à créer des régulateurs PID, nous décidons d'ajouter à notre bibliothèque un fichier \verb|launch| permettant d'en inclure un facilemetn. Ce fichier utilise 8 paramètres : \verb|input|, \verb|output|, \verb|measure|, \verb|reset|, \verb|param_P|, \verb|param_I|, \verb|param_D| et \verb|param_input|. Les quatre premiers paramètres permettent de régler les topics sur lesquels le block doit publier ou s'inscrire, tandis que les quatre derniers donnent les chemin relatif vers les fichiers de configuration des différents nœuds par rapport au répertoire \verb|params| de notre projet.
|
||
|
||
Le bloc est composé des noeuds suivants : une entrée, une différence qui entre l'entrée et la mesure, les parties proportionnelle, intégrale et dérivée du PID qui font leurs calculs et les publie sur des topics de sorties et une somme sur ces sorties et publie le résultat sur le topic \verb|output|.
|
||
|
||
On crée les nœuds de la manière suivante :
|
||
\begin{minted}{xml}
|
||
<node name="nome name" pkg="package" type="control_compute.py" args="name of the class">
|
||
<remap from="name of the argument in control_compute.py" to="new name" />
|
||
...
|
||
</node>
|
||
\end{minted}
|
||
|
||
Le remap permet de relier les noeuds entre eux. Par exemple, la sortie de la différence est remapé sur un epsilon et les entrées du PID sont remapées sur epsilon. La sortie de la somme devient donc l'entrée du PID.
|
||
|
||
Durant les phases de test, nous avons rencontré des problèmes lors du remap, notamment lors de l'utilisation du noeud de somme. Nous avons donc décidé de simplifier notre diagramme d'héritage en ne faisant pas hériter \verb|SumNode| de \verb|InputControlNode| ce qui nous permettait de définir plusieurs inputs et de régler le problème de remap. Nous ne savons toutefois pas pourquoi le problème existait au préalable.
|
||
|
||
Le fichier \verb|control.lauch| permet ainsi de générer le graphe de nœuds de la figure \ref{fig:control_block}.
|
||
|
||
\begin{figure}[!h]
|
||
\centering
|
||
\includegraphics[width=\linewidth]{images/realisation/control_launch.png}
|
||
\caption{Nœuds ROS du block control}
|
||
\label{fig:control_block}
|
||
\end{figure}
|
||
|
||
\section{Nœuds divers réalisés pour mettre en place l'asservissement}
|
||
|
||
Afin d'interfacer notre bibliothèque avec les drones BeBop, divers nœuds ont étés réalisés ou adaptés du projet existant de M. \bsc{Frezza}.
|
||
|
||
\subsection{Script \mintinline{bash}|safe_drone_teleop.py|}
|
||
|
||
Ce script a été presque entièrement repris du projet initial, à l'exception de l'ajout d'une publication sur le topic \verb|reset_pid| lors du passage en mode automatique afin de déclencher la remise à zéro des régulateurs, par exemple pour désaturer les actions intégrales. Il s'agit donc de l'interface de contrôle principale des drones.
|
||
|
||
\subsection{Script \mintinline{bash}|triangle.py|}
|
||
|
||
Ce script a pour mission de calculer la position du drone à partir de la position normalisée des cibles dans l'image. Il publie ensuite les positions au format \verb|Float64| de ROS, que les nœuds de notre bibliothèque comprennent.
|
||
|
||
\subsection{Script \mintinline{bash}{twist_controls.py}}
|
||
|
||
Ce script est destiné à lire la sortie des différents contrôleurs afin de générer un objet \verb|Twist| qui sera envoyé à \mintinline{bash}|safe_drone_teleop.py| pour être interprété comme la consigne à donner au drone.
|
||
|
||
\section{Asservissement simple boucle avec la bibliothèque}
|
||
\subsection{Méthode de \bsc{Ziegler-Nichols}}
|
||
Afin de déterminer les paramètres des correcteurs PID (axes $z$ et angle $z$), nous avons utilisé la méthode de \bsc{Ziegler-Nichols} \cite{zieg_nic_premier}. Pour réguler les boucles selon les quatre axes, nous allons générer des oscillations (qui sont faciles à voir) en augmentant progressivement le gain proportionnel. Une fois des oscillations d'amplitude et de fréquence constantes obtenues, nous allons pouvoir calculer le régulateur à utiliser en fonction de $K_u$ le gain proportionnel obtenu et $T_u$ la fréquence des oscillations.
|
||
\begin{center}
|
||
\begin{tabular}{|c|c|}
|
||
\hline
|
||
Type de contrôle & Paramètres de réglage \\
|
||
\hline
|
||
P.I.D & $K_p=0.6 K_u$ $K_i=\frac{T_u}{2}$ $K_d=\frac{T_u}{8}$ \\
|
||
\hline
|
||
P.I.D peu de dépassement & $K_p=0.33 K_u$ $K_i=\frac{T_u}{2}$ $K_d=\frac{T_u}{3}$ \\
|
||
\hline
|
||
P.I.D aucun dépassement & $K_p=0.2 K_u$ $K_i=\frac{T_u}{2}$ $K_d=\frac{T_u}{3}$ \\
|
||
\hline
|
||
\end{tabular}
|
||
\end{center}
|
||
\subsection{Méthode empirique}
|
||
Pour les deux autres axes ($x$ et $y$), nous avons utilisé une méthode empirique de réglage. Tous les autres gains étant par ailleurs nuls, on augmente progressivement le gain D\footnote{Pour rappel, cela revient à augmenter le gain P du PI équivalent du fait du comportement double intégrateur du système.}. Lorsque l'on atteint la limite du comportement oscillant du système, on augmente alors le gain D\footnote{Pour les mêmes raisons que précédemment, cela revient à augmenter le I du PI équivalent.}.
|
||
|
||
\subsection{Protocole de réglage}
|
||
|
||
Afin de faciliter le réglage du drone, il convient de régler les différents axes dans le "bon" ordre. Pour appliquer la méthode de \bsc{Ziegler-Nichols}, on pourra filmer les oscillations du drone (par exemple avec un téléphone portable) afin de mesurer la période des oscillations \textit{a posteriori}\footnote{Il existe de nombreux logiciels de relevé de position à partir d'une vidéo, par exemple avimeca.}.
|
||
|
||
Nous conseillons donc de procéder au réglage du drone dans cet ordre :
|
||
\begin{enumerate}
|
||
\item Axe $z$;
|
||
\item Lacet (angle selon $z$);
|
||
\item Axe $y$;
|
||
\item Axe $x$.
|
||
\end{enumerate}
|
||
|
||
Cependant on notera que l'asservissement des différents axes n'est pas totalement indépendant. En particulier, nous avons pu nous rendre compte que le réglage de l'axe $x$ a tendance à instabiliser le lacet. De plus, il est difficile de régler correctement l'axe $y$ si l'axe $x$ est instable, et \textit{vice-versa}. On procédera donc à un réglage par itérations successives. Enfin on notera que du fait de la mesure de l'erreur, il est nécessaire de donner des gains négatifs pour les axes $x$ et de lacet.
|
||
|
||
\subsection{Fichier \mintinline{bash}{launch}}
|
||
|
||
Nous avons créé un fichier \verb|simple_loop.launch| qui permet de lancer la simple boucle tout en chargeant les paramètres de drone que nous avons déterminé. Ce fichier fait usage du fichier \verb|control.launch| de notre bibliothèque.
|
||
|
||
\section{Asservissement double boucle avec la bibliothèque}
|
||
|
||
Afin d'améliorer les performances, on souhaite intégrer une boucle de régulation interne sur la vitesse. En outre, cette boucle devrait nous permettre d'ajouter une saturation en vitesse limitant le dépassement lorsque le drone arrive depuis une grande distance vers la cible. On modélise la nouvelle régulation avec la figure \ref{fig:double_boucle} (page \pageref{fig:double_boucle}).
|
||
|
||
\begin{figure}[h!]
|
||
\centering
|
||
\begin{tikzpicture}[auto, node distance=2cm,>=latex']
|
||
% We start by placing the blocks
|
||
\node [input, name=input] {};
|
||
\node [sum, right of=input] (sum) {};
|
||
\node [block, right of=sum] (controller) {$C_p(p)$};
|
||
\node [sum, right of=controller, node distance=3cm] (sum_speed) {};
|
||
\node [block, right of=sum_speed] (controller_speed) {$C_v(p)$};
|
||
\node [block, right of=controller_speed,
|
||
node distance=3cm] (system) {Drone};
|
||
\node [block, right of=system,
|
||
node distance=3cm] (integrate) {$\frac{1}{p}$};
|
||
\node [block, below of=controller_speed] (measurements_speed) {Mesure};
|
||
% We draw an edge between the controller and system block to
|
||
% calculate the coordinate u. We need it to place the measurement block.
|
||
\draw [->] (controller) -- node[pos=0.99] {$+$}
|
||
node [near end] {$u$} (sum_speed);
|
||
\node [output, right of=system, node distance=5cm] (output) {};
|
||
\node [block, below of=measurements_speed] (measurements) {Mesure};
|
||
|
||
% Once the nodes are placed, connecting them is easy.
|
||
\draw [draw,->] (input) -- node {consigne +} (sum);
|
||
\draw [->] (sum) -- node {$\epsilon_p$} (controller);
|
||
\draw [->] (sum_speed) -- node {$\epsilon_v$} (controller_speed);
|
||
\draw [->] (controller_speed) -- node [name=x] {} (system);
|
||
\draw [->] (system) -- node [name=v] {} (integrate);
|
||
\draw [->] (measurements_speed) -| node[pos=0.99] {$-$}
|
||
node [near end] {mesure de la vitesse} (sum_speed);
|
||
\draw [->] (integrate) -- node [name=y] {position }(output);
|
||
\draw [->] (v) |- (measurements_speed);
|
||
\draw [->] (y) |- (measurements);
|
||
\draw [->] (measurements) -| node[pos=0.99] {$-$}
|
||
node [near end] {} (sum);
|
||
\end{tikzpicture}
|
||
\caption{Modélisation simple de l'asservissement}
|
||
\label{fig:double_boucle}
|
||
\end{figure}
|
||
|
||
\subsection{Réglage de la double boucle}
|
||
\subsubsection{Méthode}
|
||
Afin de régler la double boucle, on utilisera la méthode de \bsc{Ziegler-Nichols}. Cette méthode permet de déterminer les paramètres du PID à implémenter de deux façons possibles.
|
||
\begin{itemize}
|
||
\item On peut générer des oscillations d'amplitudes et de fréquence constante sur la mesure à régler en augmentant doucement le gain proportionnel. Avec les valeurs du gain $K_u$ et de la période des oscillations $T_u$, on calcule les paramètres du PID de la façon suivante : $K_i=2/T_u,$ $K_p=0.6K_u$, $K_d=T_u/8$;
|
||
\item On enregistre la réponse du système non régulé à un échelon et on en déduit la valeur des paramètres par analyse de cette réponse comme donné sur la \ref{fig:ziegnic}.
|
||
\begin{figure}[h!]
|
||
\centering
|
||
\includegraphics[width=\linewidth]{zieg_nic.jpg}
|
||
\caption{Méthode de Ziegler-Nichols pour déterminer les paramètres d'un PID \cite{zieg_nic}}
|
||
\label{fig:ziegnic}
|
||
\end{figure}
|
||
\end{itemize}
|
||
\subsubsection{Réglage de la boucle interne}
|
||
Nous allons commencer par régler la boucle interne, soit la boucle asservissant la vitesse. Pour cela, nous allons utiliser la réponse du système non régulé à un échelon afin de trouver les paramètres du PID interne.
|
||
|
||
Pour cela, non envoie un échelon en inclinaison à notre drone (il s'agit donc d'une accélération) car nous ne pouvons pas directement lui donner une commande en vitesse, et on enregistre la réponse indicielle en boucle ouverte. Pour la calculer, nous avons utilisé un filtre de \bsc{Savitsky-Golay} quadratique de 19 points (avoir beaucoup de retard dans les calculs ne pose, ici, aucun problème, car nous ne cherchons pas à avoir une réponse rapide, mais une réponse la plus lissée possible pour déterminer au plus juste les paramètres découlant de la méthode de \bsc{Ziegler-Nichols}.
|
||
|
||
Afin de déterminer la réponse indicielle de la vitesse en boucle ouverte, nous avions besoin d'un grand espace. Nous avons donc déterminé cette réponse dans le gymnase en modifiant la couleur des cibles (jaunes) pour que le drone ne soit pas perturbé par le sol qui est bleu.
|
||
|
||
Cependant nos essais n'ont pas été très concluants pour plusieurs raisons. Tout d'abord parce que le drone a tendance à perdre la cible lorsqu'il est à grande distance, et à cause des changements de luminosité. Cependant un réglage à proximité de la cible n'est pas envisageable car le drone met quelques secondes à atteindre sa vitesse maximale : il dépasse donc la cible. La régulation en double boucle n'est donc pas fonctionnelle à l'heure de la cloture de ce rapport.
|
||
|
||
|
||
\subsection{Création du fichier launch et connection des noeuds}
|
||
|
||
Pour réaliser la double boucle, on crée un fichier \verb|launch| dédié.
|
||
|
||
On crée des noeuds afin de récupérer la position du drone
|
||
\begin{minted}{xml}
|
||
<node name="targets" pkg="detect_targets" type="target_publisher.py">
|
||
</node>
|
||
|
||
<node name="triangle" pkg="detect_targets" type="triangle_control.py" output="screen">
|
||
<remap from="component_centers" to="targets"/>
|
||
</node>
|
||
\end{minted}
|
||
|
||
On organise grâce à des groupes :
|
||
\begin{minted}{xml}
|
||
<group ns="name">
|
||
...
|
||
</group>
|
||
\end{minted}
|
||
|
||
À l'intérieur de ces groupes, on récupère les noeuds crées avec \verb|control.launch| en incluant le fichier :
|
||
\begin{minted}{xml}
|
||
<include file="$(find detect_targets)/launch/control.launch" ns="name of the loop">
|
||
\end{minted}
|
||
|
||
Et on donne les bonnes valeurs aux arguments grâce à \begin{minted}{xml}
|
||
<arg name="name of the argument in control.launch" value="value of the argument" />
|
||
\end{minted}
|
||
|
||
Pour les boucles en x et en y, on crée deux boucles, tandis que pour les boucles en z, on en crée qu'une seule (car on ne régulera pas cet axe avec une double boucle, ce dernier n'étant pas soumis aux problèmes de lenteur et de dépassement étant donné les faibles déplacements sur cet axe).
|
||
|
||
Il "suffit" ensuite de bien relier les entrées et les sorties des noeuds. Nous obtenu le résultat de nos noeuds \ref{fig:noeuds}.
|
||
\begin{figure}[h!]
|
||
\centering
|
||
\includegraphics[width=\linewidth]{noeuds.png}
|
||
\caption{Nœuds ROS pour la double boucle}
|
||
\label{fig:noeuds}
|
||
\end{figure}
|