Projet Matlab : Localisation d une automobile par GPS et capteurs proprioceptifs

Transcription

1 1 Projet Matlab : Localisation d une automobile par Par : PETIT Florian III) Etude du positionnement par GPS 1) Chargement du fichier maprixheim.mat % Chargement du fichier maprixheim.mat (Question 1) load maprixheim.mat; % Création de deux matrices : map_bd (bord droit du circuit) et map_bg (bord gauche du circuit) 2) Tracé du Circuit de Rixheim % Graph 1 : Le tracé non interpolé du circuit (bord droit et bord gauche) (Question 2) % Permet d'avoir plusieurs Graph sur la même figure plot(droit_abs,droit_ord,'.r','markersize',5); % Tracé du bord droit du circuit plot(gauche_abs,gauche_ord,'.g','markersize',5); % Tracé du bord gauche du circuit legend('',''); % Légende title('tracé du Circuit de Rixheim'); % Titre xlabel(''); ylabel(''); axis equal; % Normalisation du graphique Tracé du Circuit de Rixheim PETIT Florian lundi 15 octobre 27

2 2 3) Commentaire sur la courbe du tracé du circuit de Rixheim Après avoir tracé le circuit grâce aux données GPS et à l aide de MatLab, on peut observer quelques problèmes en particulier au début et a la fin Au début on peut observer certaines erreurs dues à la staticité de la voiture ainsi qu au démarrage des relevés GPS. Tracé du Circuit de Rixheim Tracé du Circuit de Rixheim A la fin cela peut être du aux arbres à proximité du circuit ce qui peut perturber le niveau du signal et rendre la localisation plus difficile ) Chargement du fichier datagps.mat % Chargement du fichier datagps.mat (Question 4) load datagps.mat; % Correspond à une acquisition GPS de la localisation du véhicule effectuée sur ce tracé 5) Création des vecteurs Tgps, Xgps, Ygps, Qgps et DataOk % Extraction des données du GPS de la localisation du véhicule (Question 5) Tgps=dataGps(:,1); % Extraction de la 1ere colonne de datagps Base de temps du GPS (en s) Xgps=dataGps(:,2); % Extraction de la 2eme colonne de datagps Abscisse de la position véhicule (en m) Ygps=dataGps(:,3); % Extraction de la 3eme colonne de datagps Ordonnée de la position véhicule (en m) Qgps=dataGps(:,4); % Extraction de la 4eme colonne de datagps Note de qualité du GPS DataOk=dataGps(:,5); % Extraction de la 5eme colonne de datagps Indicateur de réception d une nouvelle donnée GPS PETIT Florian lundi 15 octobre 27

3 3 6) Recherche de la fréquence d acquisition (1Hz) du capteur Si l'on observe la matrice : Tgps, on peut observer que le changement de valeur s'effectue tous les 5 relevés (sauf pour 3 ou 4 valeurs) Si l'on commence a la 4 ème valeur (Tgps = s) et que l'on termine a la 2386 ème valeur (Tgps = s) Soit environ 477 mesures différentes du GPS en s Soit une fréquence d'acquisition de 9,98 Hz (Valeur théorique de 1Hz) 7 et 8) Tracé de la trajectoire GPS non interpolée et interpolée du véhicule % Graph 2 : Le tracé de la trajectoire GPS non interpolée et interpolée du véhicule % Permet d'avoir plusieurs Graph sur la même figure plot(droit_abs,droit_ord,'.r','markersize',5); % Tracé du bord droit du circuit plot(gauche_abs,gauche_ord,'.g','markersize',5); % Tracé du bord gauche du circuit plot(xgps,ygps,'.','markersize',7); % Tracé de la localisation GPS du véhicule (non interpolée) plot(xgps,ygps,'k','linewidth',1); % Tracé de la localisation GPS du véhicule (interpolée) legend('','','localisation GPS du véhicule (non interpolée)','localisation GPS du véhicule (interpolée)'); % Légende title('tracé de la trajectoire GPS non interpolée et interpolée du véhicule'); % Titre xlabel(''); ylabel(''); axis equal; % Normalisation du graphique 1 Tracé de la trajectoire GPS non interpolée et interpolée du véhicule Localisation GPS du véhicule (non interpolée) Localisation GPS du véhicule (interpolée) PETIT Florian lundi 15 octobre 27

4 9) Observation de la figure 2 Dans la courbe 1 et 2 et la ligne droite 1 et 2 du circuit, la localisation GPS du circuit semble correcte (on peut tout de même observer quelques points non alignés en ligne droite, mais cela doit surement venir de la précision du GPS utilisé) Projet MatLab : Localisation d'une automobile par Tracé de la trajectoire GPS non interpolée et interpolée du véhicule Localisation GPS du véhicule (non interpolée) Localisation GPS du véhicule (interpolée) 4 Tracé de la trajectoire GPS non interpolée et interpolée du véhicule Par contre on peut observer qu'après la courbe 2 ; le signal n'est plus cohérent, avec une courbe interpolée erronée de la position réelle du véhicule, cela est du a la présence de végétation sur cette partie du circuit. 1) Représentation de la qualité de la réception GPS % Graph 3 : Note de la qualité de réception du signal GPS (Question 1) % Permet d'avoir plusieurs Graph sur la même figure plot(tgps,qgps,'.g','markersize',5) % Tracé de l'indicateur de qualité de la réception du signal GPS legend('indicateur de qualité de la réception du signal GPS') % Légende title('tracé de l indicateur de qualité de réception du GPS (en fonction du temps)') % Titre xlabel('temps (en s)') ylabel('qualité du signal') axis([ 7 2]) % Imposition des axes Tracé de l indicateur de qualité de réception du GPS (en fonction du temps) Qualité du signal Indicateur de qualité de la réception du signal GPS Temps (en s) PETIT Florian lundi 15 octobre 27

5 5 11) Confirmation du résultat de la question 9 On peut observer que de Tgps=13,2s à Tgps=55,5s, on a un indicateur de qualité de la réception GPS égal à 18, ce qui montre bien notre premiére observation de la question 9, montre que la localisation GPS est correcte. De même après cela on a un indice de 2 3 ce qui montre que la réception est médiocre, et que la localisation est partielle (zone avec des arbres) IV) Etude du positionnement par données proprioceptives 12) Charger le fichier datacapt.mat % Chargement du fichier datacapt.mat (Question 12) load datacapt.mat; % Contient les mesures des capteurs proprioceptifs correspondantes au fichier GPS précédemment traité 13) Création des vecteurs Tcapt, Vt, Vl et Psip % Extraction des mesures des capteurs proprioceptifs correspondantes au fichier GPS précédemment traité Tcapt=dataCapt(:,1); % Extraction de la 1ere colonne de datacapt Base de temps des mesures capteurs (en s) Vt=dataCapt(:,2); % Extraction de la 2eme colonne de datacapt Vitesse transversale du véhicule (en m/s) Vl=dataCapt(:,3); % Extraction de la 3eme colonne de datacapt Vitesse longitudinale du véhicule (en m/s) Psip=dataCapt(:,4); % Extraction de la 4eme colonne de datacapt Vitesse de lacet du véhicule (en rad/s) 14) Valeur moyenne de l'écart de Psip par rapport à sur les 1 premiéres secondes : % Correction de la vitesse du lacet Psip (Question 14) Psip5=Psip(1:5,1); % Extraction des 5 premiéres valeurs de Psip (soit 1s) Moyenne_ecrat=mean(Psip5) % Valeur moyenne de l'écart par rapport à PsipReel=Psip-Moyenne_ecrat; % Psip réel (Compensé) Moyenne_ecrat =.258 rad/s 15) Tracé de Psip : le signal biaisé et le signal compensé % Graph 4 : Tracé de Psip : le signal biaisé et le signal compensé (Question 15) % Permet d'avoir plusieurs Graph sur la même figure plot(tcapt,psip,'.r') % Tracé du signal biaisé plot(tcapt,psipreel,'.b') % Tracé du signal Compensé legend('signal biaisé','signal compensé') % Légende title('tracé du signal biaisé et compensé') % Titre xlabel('temps (en s)') ylabel('vitesse de lacet (en rad/s)') PETIT Florian lundi 15 octobre 27

6 6.8.7 Tracé du signal biaisé et compensé Signal biaisé Signal compensé.6.5 Vitesse de lacet (en rad/s) Temps (en s) 16) Détermination de la localisation du véhicule à partir des mesures des capteurs embarqués % Valeurs initiales pour déterminer la localisation avec le positionnement par données proprioceptives (Question 16) x(1)= ; y(1)= ; w(1)=- 2.18; Tech=.2; % Période d échantillonnage de l acquisition (5Hz) % Détermination de la localisation du véhicule (x,y,w) à partir des mesures des capteurs embarqués (Question 16) for n = 1:length(Vt) % Pour n=1 à n=(taille de Vt) x(n+1) = x(n) + Tech * ( Vl(n)*cos(w(n)) - Vt(n)*sin(w(n))); % Calcul des x(n) (abscisse) y(n+1) = y(n) + Tech * ( Vl(n)*sin(w(n)) + Vt(n)*cos(w(n))); % Calcul des y(n) (ordonnée) w(n+1) = w(n) + Tech * PsipReel(n); % Calcul des w(n) end 17) Comparaison de la trajectoire GPS et de la trajectoire calculé à partir des mesures des capteurs embarqués % Graph 5 : Comparaison de la trajectoire GPS et de la trajectoire calculée à partir des mesures des capteurs embarqués (Question 17) % Permet d'avoir plusieurs Graph sur la même figure plot(droit_abs,droit_ord,'.r','markersize',5); % Tracé du bord droit du circuit plot(gauche_abs,gauche_ord,'.g','markersize',5); % Tracé du bord gauche du circuit plot(x,y,'.m','markersize',7); % Tracé de la trajectoire calculée à partir des mesures des capteurs embarqués plot(xgps,ygps,'.b','markersize',7); % Tracé de la localisation GPS du véhicule legend('','','localisation du véhicule à partir des mesures des capteurs enmbarqués','localisation GPS du véhicule') % Légende title('comparaison de la trajectoire GPS et de la trajectoire calculée à partir des mesures des capteurs embarqués') % Titre xlabel(''); ylabel(''); axis equal; % Normalisation du graphique PETIT Florian lundi 15 octobre 27

7 7 1 Comparaison de la trajectoire GPS et de la trajectoire calculée à partir des mesures des capteurs embarqués Localisation du véhicule à partir des mesures des capteurs enmbarqués localisation GPS du véhicule La méthode de positionnement par données proprioceptives permet d'avoir une courbe coohérente (Pas de divergence de points comme dans la derniére partie du positionnement par GPS, c'est à dire qu'on obtient une position même dans une zone avec des arbres). Par contre a cause de la précission des capteurs, on peut observer une dérive (Ligne droite 2) V) Fusion GPS capteurs proprioceptifs 18 à 22) Définition des constantes % Définition des constantes pour utiliser le filtre de Kalman (Question 18 à 22) Q=[.1,,;,.1,;,,.1*pi/18]; % Matrice Q des bruits du modèle (Question 18) Rgps=[.15^2,;,.15^2]; % Matrice des bruits de mesure du GPS (Question 19) Rpro=[8*1^(-7),,;,7.96*1^(-7),;,,4.17*1^(-6)]; % Matrice des bruits de mesure des capteurs proprioceptifs (Question 2) C=[1,,;,1,]; % Matrice d'observation reliant les mesures GPS au vecteur d'état X(x,y,w) (Question 21) P=[Rgps(1,1),,;,Rgps(2,2),;,,pi/3]; % Matrice d'incertitude (de covariance) de la prédiction (Question 22) I=[1,,;,1,;,,1]; % Matrice identité PETIT Florian lundi 15 octobre 27

8 8 23) Utilisation du Filtre de Kalman Xest(2)=Xgps(2); % Enregistrement des premières valeurs initiales Yest(2)=Xgps(2); % Phase de Prédiction du filtre for n=2:length(vt) % Pour n=2 à n=(taille de Vt) A=[1,,-Tech*(Vt(n-1)*cos(w(n-1))+Vl(n-1)*sin(w(n-1))),1,Tech*(Vl(n-1)*cos(w(n-1))-Vt(n-1)*sin(w(n-1))),,1]; B=[Tech*cos(w(n-1)),-Tech*sin(w(n-1)), Tech*sin(w(n-1)),Tech*cos(w(n-1)),,,Tech]; % Calcul de A % Calcul de B % Prédiction des états du modèle d évolution X Xest(n) = Xest(n-1) + Tech * ( Vl(n-1)*cos(w(n-1)) - Vt(n-1)*sin(w(n-1))); Yest(n) = Yest(n-1) + Tech * ( Vl(n-1)*sin(w(n-1)) + Vt(n-1)*cos(w(n-1))); X=[Xest(n),Yest(n),w(n)]'; % Etat caractérisant la localisation du véhicule P=A*P*A'+B*Rpro*B'+Q; % Calcul de la matrice de covariance de la prédiction % Phase de correction if DataOk(n)~= DataOk(n-1) % Si nouvelle localisation GPS if Qgps(n)==18 % Si Note de qualité = 18 K=P*(C')*inv([C*P*(C')+Rgps]); % Calcul du gain de Kalman mesure_gps=[xgps(n);ygps(n)]; % Définition d'un vecteur des mesures GPS X=X+K*(mesure_gps-C*X); % Correction de la prédiction des états P=(I-K*C)*P; % Correction de la matrice de covariance Xest(n)=Xgps(n);% Enregistrement dans Xest(n) de la valeur réelle (GPS qualité 18) Yest(n)=Ygps(n);% Enregistrement dans Yest(n) de la valeur réelle (GPS qualité 18) end end end % Graph 6 : Localisation du véhicule avec le filtre de Kalman (GPS+capteurs) (Question 24) % Permet d'avoir plusieurs Graph sur la même figure plot(droit_abs,droit_ord,'.r','markersize',5); % Tracé du bord droit du circuit plot(gauche_abs,gauche_ord,'.g','markersize',5); % Tracé du bord gauche du circuit plot(xest,yest,'*k','markersize',3) % Tracé de la localisation du véhicule (GPS+capteurs) legend('','','trajectoire de la voiture');% Legende title('localisation du véhicule avec le filtre de Kalman (GPS+capteurs)'); % Titre xlabel(''); ylabel(''); axis equal; % Normalisation du graphique Pour utilise la fonction (Fichier : fusion.m) Il sufit d écrire : fusion(a,b) dans la table de commande Avec a = datagps et b = datacapt PETIT Florian lundi 15 octobre 27

9 9 24) Localisation du véhicule à l aide du filtre de Kalman (GPS+Capteurs) Exécution de la fonction : fusion(datagps,datacapt) Localisation du véhicule avec le filtre de Kalman (GPS+capteurs) Trajectoire de la voiture ) On observe que jusqu'au virage 2, la trajectoire est sensiblement là même que celle du GPS, on peut toutefois remarquer que sur le zoom, on a un décalage toutes les 5 mesures (due a la prise de valeurs du GPS et de la correction des autres mesures Après ce virage 2, on observe que la courbe a la même forme que la courbe avec les capteurs Localisation du véhicule avec le filtre de Kalman (GPS+capteurs) Trajectoire de la voiture PETIT Florian lundi 15 octobre 27

10 1 26) On peut donc en conclure qu en utilisant la position obtenue par DR et celle obtenue grâce au GPS avec le filtre de Kalman, on arrive a une localisation de 1% avec une précision accru, avec l utilisation du GPS quand la reception est bonne, et l utilisation des capteurs de la voiture quand la réception est mauvaise Graphique de Conclusion GPS uniquement Capteurs Uniquement GPS+capteurs Localisation du véhicule Liste des Figures lors de l exécution de la fonction : fusion Figure 1 : Tracé du Circuit de Rixheim Figure 2 : Le tracé de la trajectoire GPS non interpolée et interpolée du véhicule Figure 3 : Tracé de l'indicateur de qualité de réception du GPS (en fonction du temps) Figure 4 : Tracé de Psip : le signal biaisé et le signal compensé Figure 5 : Comparaison de la trajectoire GPS et de la trajectoire calculée à partir des mesures des capteurs embarqués Figure 6 : Localisation du véhicule avec le filtre de Kalman (GPS+capteurs) Figure 7 : Conclusion Projet réalisé avec : MatLab 7.3. (R26b) PETIT Florian lundi 15 octobre 27