Discussion :

[Sunviv]

Bonjour,

Je suis un étudiant en stage dans un bureau d'étude. Je fais partie d'un projet assez compliqué, calculé la position et la vitesse d'une balle de volley durant un match.
Pour le réaliser, j'utilise un accéléromètre tri-axiale, un gyroscope tri-axial et un magnétomètre lui aussi tri-axiale.

Je commence donc par obtenir les angles de cardans puis le calcule de la vitesse et de la position déroule par la suite.

Mais je bloque pour le cap.
En effet, j'arrive à peu près a avoir un bon tangage et un bon roulis, mais impossible de déterminer un bon cap .

En faisant un test (90° tangage, 90° roulis, 90° cap), j'obtiens ces résultats :

Pièce jointe 293856

Voici mon code : (Je récupère les données du capteur via le SSh de la raspberry et par le biais d'un serveur MQTT mosquitto)

Code :

Sélectionner tout - Visualiser dans une fenêtre à part

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
###    Code Terre_Plate    ###
 
 
# Importation des modules necessaires :
import sys
from math import sin, cos, atan, pi, sqrt, atan2
import numpy as np
import matplotlib.pyplot as plt
import struct  #struct.unpack('motif_du_format_a_convertir', donnee) convertit C -> type Python
import paho.mqtt.client as mqtt    #le as '' cree un alias (racourci)
import base64  #contient les classes d'encodage et de decodage
import time
 
 
### Initialisation pour le calcul de l'Attitude
weight = 0.98
t_ech = 0.010
 
def qmul(Qb, Qc):
    ''' Methode permettant de multiplier deux quaternions '''
    Qa = Qc[0] * Qb
    Qa[0] = Qa[0] - Qb[1]*Qc[1] - Qb[2]*Qc[2] - Qb[3]*Qc[3]
    Qa[1] = Qa[1] + Qb[0]*Qc[1] + Qb[2]*Qc[3] - Qb[3]*Qc[2]
    Qa[2] = Qa[2] + Qb[0]*Qc[2] + Qb[3]*Qc[1] - Qb[1]*Qc[3]
    Qa[3] = Qa[3] + Qb[0]*Qc[3] + Qb[1]*Qc[2] - Qb[2]*Qc[1]
    return Qa
 
f = np.loadtxt('/home/alex/Documents/Alex projet Fusion/Données Phillipe/donnee_capteur_dynamique.csv',
               delimiter=',', usecols=(0, 1, 2, 3, 4, 5, 6, 7, 8))
 
MAccX = f[:, 0]  *9.78
MAccY = f[:, 1]   *9.78
MAccZ = f[:, 2]  *9.78
MGyroX = f[:, 3]*(2*pi/360)
MGyroY = f[:, 4]*(2*pi/360)
MGyroZ = f[:, 5]*(2*pi/360)
MMagX = f[:, 6] #-0.14716
MMagY = f[:, 7] #+ 0.12514
MMagZ = f[:, 8] #-0.08250
 
MGyroX_l = []
MGyroY_l = []
MGyroZ_l = []
MAccX_l = []
MAccY_l = []
MAccZ_l = []
 
long_senseurs = np.size(MAccX)
 
for i in range(100):
    MGyroX_l.append(MGyroX[i])
    MGyroY_l.append(MGyroY[i])
    MGyroZ_l.append(MGyroZ[i])
    MAccX_l.append(MAccX[i])
    MAccY_l.append(MAccY[i])
    MAccZ_l.append(MAccZ[i])    
 
MGyroX_m = np.mean(MGyroX_l)
MGyroY_m = np.mean(MGyroY_l)
MGyroZ_m = np.mean(MGyroZ_l)
MAccX_m = np.mean(MAccX_l)
MAccY_m = np.mean(MAccY_l)
MAccZ_m = np.mean(MAccZ_l)
 
# Formatage des donnees senseurs
for i in range(long_senseurs):
    MGyroX[i] = MGyroX[i] - MGyroX_m
    MGyroY[i] = MGyroY[i] - MGyroY_m
    MGyroZ[i] = MGyroZ[i] - MGyroY_m
    #MAccX[i] = (MAccX[i] - MAccX_m)/4
    #MAccY[i] = (MAccY[i] - MAccY_m)/4
    #MAccZ[i] = (MAccZ[i] - MAccZ_m)/4 +9.78
 
# Fenetre de temps
t_ech = 0.010
tps = np.arange(t_ech, long_senseurs*t_ech, t_ech)
nbre_pas = np.size(tps)
weight = 0.94
 
Pos = np.array([0, 0, 0])
Pos = Pos[:, np.newaxis]
Position = np.zeros((nbre_pas, 3))
 
Attitude_Gyr = np.zeros((3, 1))
Attitude_Fused = np.zeros((3, 1))
Attitude = np.zeros((nbre_pas, 3))
 
 
# Vitesse earth dans repere navigation
Vit = np.array([0, 0, 0])
Vit = Vit[:, np.newaxis]
Vitesse = np.zeros((nbre_pas, 3))
 
 
# Increments acceleration
incr_f_b = np.array([0, 0, 9.78])
incr_f_b = incr_f_b[:, np.newaxis]
# Increments gyro
incr_w_nb_b = np.array([0, 0, 0])
incr_w_nb_b = incr_w_nb_b[:, np.newaxis]
 
# Matrice de passage de body a navigation
C_b_n = np.array([[1, 0, 0], [0, 1, 0], [0, 0, 1]])
# Vecteur rotation body par rapport plateforme dans body
w_nb_b = np.array([0, 0, 0])
w_nb_b = w_nb_b[:, np.newaxis]
 
 
 
Tangage = []
Roulis = []
Cap = []
Pos_x = []
Pos_y = []
Pos_z = []
Vit_x = []
Vit_y = []
Vit_z = []
 
 
 
#############################################################
#Boucle principale d'iteration d'integration de la navigation
#############################################################
for k in range(nbre_pas):
 
    vect_incr_body = np.array([MAccX[k], MAccY[k], MAccZ[k]])
    Vecteur_Gravite = np.array([0, 0, -9.78])
    Vecteur_Gravite = Vecteur_Gravite[:, np.newaxis]
    Acc_gama = atan(MAccY[k]/sqrt(MAccX[k]**2 + MAccZ[k]**2))
    Acc_theta = atan(MAccX[k] / sqrt(MAccY[k]**2 + MAccZ[k]**2 ))
 
 
    #xh = MMagX[k]*cos(Acc_theta) + MMagY[k]*sin(Acc_gama) -MMagZ[k]*cos(Acc_theta)*sin(Acc_gama)
    #yh = MMagY[k]*cos(Acc_gama) + MMagZ[k]*sin(Acc_gama)
    #Acc_psi = atan(-yh / xh) 
 
    variation = 0.52 * (pi/180)  
    xh = MMagX[k]*cos(Acc_gama) + MMagY[k]*sin(Acc_gama)*sin(Acc_gama) + MMagZ[k]*cos(Acc_gama)*sin(Acc_theta)
    yh = MMagY[k]*cos(Acc_theta) - MMagZ[k]*sin(Acc_theta)
    Acc_psi = atan(-yh / xh) + variation
 
    #Acc_psi = atan(-MMagY[k] / MMagX [k]) #4
 
 
    Attitude_Acc = np.array([[Acc_gama], [Acc_theta], [Acc_psi]])
 
    Attitude_Fused[0] = weight*( Attitude_Fused[0] + MGyroX[k]*t_ech) + (1-weight)*(Attitude_Acc[0])
    Attitude_Fused[1] =  weight*( Attitude_Fused[1] + MGyroY[k]*t_ech) + (1-weight)*(Attitude_Acc[1])
    Attitude_Fused[2] =  weight*( Attitude_Fused[2] + MGyroZ[k]*t_ech) + (1-weight)*(Attitude_Acc[2])
 
    # Tangage rotation autour de x_aile_droite
    Attitude[0] = Attitude_Fused[0]
    g = Attitude_Fused[0] * (180/pi)
    # Roulis rotation autour de y_devant
    Attitude[1] = Attitude_Fused[1]
    t = Attitude_Fused[1] * (180/pi)
    # Cap rotation autour de z_up
    Attitude[2] = Attitude_Fused[2]
    p = (Attitude_Fused[2]) * (180/pi)
 
 
    #print(k)
    #print("Tangage = " + str(g[0]) + "  Roulis = " + str(t[0]) + "  Cap = " + str(p[0]) )    
 
 
    # Passage en quaternions
    q1 = sin(g/2)*sin(t/2)*sin(p/2) + cos(g/2)*cos(t/2)*cos(p/2)
    q2 = sin(g/2)*cos(t/2)*cos(p/2) - cos(g/2)*sin(t/2)*sin(p/2)
    q3 = sin(g/2)*cos(t/2)*sin(p/2) + cos(g/2)*sin(t/2)*cos(p/2)
    q4 = -sin(g/2)*sin(t/2)*cos(p/2) + cos(g/2)*cos(t/2)*sin(p/2)
    quat_q = np.array([q1, q2, q3, q4])
    quat_q_conj = np.array([q1, -q2, -q3, -q4])
 
    a = np.array([0.])
    quat_incr_body = np.concatenate((a, vect_incr_body), axis=0)
    quat_interm = qmul(quat_incr_body, quat_q_conj)
    quat_incr_navig = qmul(quat_q, quat_interm)
    vect_incr_navig = quat_incr_navig[1:4]
    vect_incr_navig = vect_incr_navig[:, np.newaxis]
    Vit = Vit + (vect_incr_navig + Vecteur_Gravite) * t_ech
    Vitesse[0][0] = Vit[0][0]
    Vitesse[0][1] = Vit[1][0]
    Vitesse[0][2] = Vit[2][0]
 
    # integration de la position
    Pos = Pos + Vit * t_ech
    Position[0][0] = Pos[0][0]
    Position[0][1] = Pos[1][0]
    Position[0][2] = Pos[2][0]
 
    # Affichage valeur:
    Tangage.append(g[0])
    Roulis.append(t[0])
    Cap.append(p[0])
    Pos_x.append(Position[0][0])
    Pos_y.append(Position[0][1])
    Pos_z.append(Position[0][2])
    Vit_x.append(Vitesse[0][0])
    Vit_y.append(Vitesse[0][1])
    Vit_z.append(Vitesse[0][2])
 
 
plt.clf()
 
plt.subplot(3, 3, 1)
plt.plot(tps, Pos_x, color='red')
plt.xlabel('Position_x', labelpad=1)
plt.ylabel('Temps')
plt.grid(True)
plt.subplot(3, 3, 2)
plt.plot(tps, Pos_y, color='red')
plt.xlabel('Position_y', labelpad=-2)
plt.grid(True)      
 
plt.subplot(3, 3, 3)
plt.plot(tps, Pos_z, color='red')
plt.xlabel('Position_z', labelpad=1)
plt.grid(True)
plt.subplot(3, 3, 4)
plt.plot(tps, Vit_x, color='blue')
plt.xlabel('Vitesse_x', labelpad=1)
plt.ylabel('Temps', labelpad=15)
plt.grid(True)
plt.subplot(3, 3, 5)
plt.plot(tps, Vit_y, color='blue')
plt.xlabel('Vitesse_y', labelpad=1)
plt.grid(True)
plt.subplot(3, 3, 6)
plt.plot(tps, Vit_z, color='blue')
plt.xlabel('Vitesse_z', labelpad=1)
plt.grid(True)
plt.subplot(3, 3, 7)
plt.plot(tps, Tangage, color='green')
plt.xlabel('Tangage', labelpad=1)
plt.ylabel('Temps', labelpad=15)
plt.grid(True)
plt.subplot(3, 3, 8)
plt.plot(tps, Roulis, color='green')
plt.xlabel('Roulis', labelpad=1)
plt.grid(True)
plt.subplot(3, 3, 9)
plt.plot(tps, Cap, color='green')
plt.xlabel('Cap', labelpad=1)
plt.grid(True)
 
plt.show()

La je sèche vraiment ! pourquoi je n'arrive pas à avoir des résultats qui collent avec mon expérience.

Merci

[tbc92]

Tu dis que tes résultats ne collent pas avec l'expérience. Bon, si tu le dis, tu dois avoir raison.
Mais concrètement, pour des gens qui débarquent et qui lisent ton message, j'imagine que le problème est sur les courbes de la pièce jointe. Tu montres les courbes obtenues, mais tu pensais obtenir quoi comme courbes. Qu'est-ce qui te fais penser qu'il y a une erreur ?

Ton message a été lu 30 fois ... et personne n'a formulé un début de réponse ! Tout le monde est resté perplexe.

Par ailleurs, je ne suis pas sûr que le forum mathématique soit le plus adapté pour poster un code de 200 lignes. Je ne suis pas sûr d'ailleurs qu'il y ait un forum adapté pour débugger un code de 200 lignes. Mais avant de déplacer la discussion dans un autre forum, on va essayer de voir un peu plus clair.

[Sunviv]

Oui je vais essayer d'être plus clair.

Comme je l'ai dis, j'ai testé de faire un angle de 90° sur les axes X,Y,Z correspondant respectivement aux angles de cardans nommé Tangage, Roulis et Cap.

Comme on peut le voir sur les 3 courbes du bas, les courbes de tangage et de roulis ont bien fait un angle de 90°, mais ce n'est pas le cas du Cap malheureusement.


Le problème se situe dans les équations d'attitude.
Attitude_Fused correspondant à une matrice englobant les 3 angles cherchés.

Pour la calculer, on utilise un filtre complémentaire pour fusionner les données des capteurs accéléro et gyro.


J'ai poster mon message dans la catégorie Mathématique car on utilise beaucoup de math dans la navigation Inertielle, sinon je ne savais pas où le poser.

[Jipété]

Ton message a été lu 30 fois ... et personne n'a formulé un début de réponse ! Tout le monde est resté perplexe.

Y a de quoi : orthographe aléatoire, français approximatif, et cerise sur le gâteau,

Comme on peut le voir sur les 3 courbes du bas,

voilà ce qu'on gagne à cliquer sur le lien :

[tbc92]

Je vais poser des questions "stupides".

Les mots tangages / roulis / Cap, je les connais plus ou moins dans le domaine de la marine. Juste besoin d'une toute petite remise à niveau.
Sur un bateau, si je place des capteurs en haut du mat, au pied du mat, à l'avant du bateau et sur la droite du bateau, je vais pouvoir mesurer la position de ces capteurs, et calculer position / vitesse / tangage/roulis / cap.

L'avantage du bateau, c'est que le point qui est à l'avant du bateau, il reste globalement à l'avant du bateau ( le bateau ne se déplace pas en crabe ni en marche arrière) et le point en haut du mat, il reste globalement plus haut que le point en bas du mat.

Mais sur un ballon, si je place 4 capteurs sur mon ballon et que je mesure les déplacements de mes 4 capteurs dans l'espace, il se peut que le capteur 1 soit à l'avant du ballon au début du trajet, et à l'arrière du ballon à un autre moment, et même sur la droite ou la gauche à un autre moment. Comment définis-tu les notions de roulis / Cap / tangage dans ces conditions ?

[Sunviv]

jipété sur mon ordinateur je vois bien ma pièce jointe, bizarre :



tbc92, je ne comprends pas vraiment ton exemple avec la montgolfière. Si on prend l'exemple d'une moto, on définit bien trois axes, Mx vers l'avant de la moto (lié au roulis), My vers la droite (lié au tangage) et Mz vers le haut (lié au cap).


Ce site te l'explique très bien :
https://sites.google.com/site/moiryr...ude-de-la-moto

[anapurna]

salut,

si j'ai bien compris tu arrive à extraire les différents paramètres de vols tels que tangage roulis et le lacets mais pas le cap

alors que pour moi le cap est l’élément le plus simple à déterminer le cap correspond à la différence du point centrale de ton ballon entre le temps n et n+1
tous les autre paramètre pour un ballon ne sont possible qu'avec des information de périphérie

[Sunviv]

Alors tout d'abord le lacet est un autre nom du cap, c'est la même chose.

Le mien ne suit pas le mouvement que je fais, c'est là qu'est mon problème. Au lieu de monter à 90° il reste inchangé.

J'aimerais savoir si le cap que l'on calcul avec un algorithme de navigation doit indiquer le cap géographique ou le cap magnétique. Si c'est le cap magnétique, qu'il ne monte pas à ce seuil de 90° est normal.

[anapurna]

salut

le lacet ce calcul comme les autres mouvement circulaire

Le premier angle de rotation dans la séquence lacet, tangage et roulis.
Le lacet mesure la rotation autour de l'axe Z du système de coordonnées pivotant par rapport au système de coordonnées global.

peut être que l'exemple du satellite pourra te mettre sur la voie

[Sunviv]

Pour le calcul du lacet je me suis fié a ce code :


https://gist.github.com/timtrueman/322555

[anapurna]

salut

dans l'exemple que tu nous donne je pense qu'ils ont simplement considérer le cap/lacet comme négligeable et donc
tes résultats sont totalement faussé

tu as plusieurs possibilité
soit tu le calcul de façon matriciel voir angle d'euler

soit par les distances
pour un acceleromettre utilise les fonction ci-dessous et dis moi si ce n'est pas mieux
angle_x = (atan2(x, sqrt(y*y + z*z)) *(180/PI)); //
angle_y = (atan2(y, sqrt(x*x + z*z)) *(180/PI)); //
angle_z = (atan2(sqrt(x*x + y*y), z) *(180/PI)); //


et voici un liens fort interessant sur ses fameux calcul

[Sunviv]

Merci pour l'aide,


Voici les résultats après avoir fait les modifs :




Le cap varie quand il ne devrait pas ! Il fait tout l'inverse de ce qu'il devrait faire ! Et quand il devrait monté à 90° il reste approximativement constant.

Mais en tout cas il y a moins de bruit et ça m'a l'air un peu mieux qu'avant merci.

[wendyls]

Il est possible que le magnétomètre soit perturbé par les matériaux aux alentours (aimant, fer ...)

Par ailleurs, es tu sûr de la configuration de ton magnétomètre ?

[anapurna]

salut

tu as remarqué que les pic du cap apparaisse au même moment que les deux autres combiné
il faut peut être filtrer certaine données

petit question c'est un acceleromettre ou un magnétomètre ?
je pense qu'il doit y avoir des différences a prendre en compte vu que l'un des deux dépend de la force du champs magnétique terrestre

[Sunviv]

Salut,


C'est un accéléromètre, mais j'ai aussi un magnétomètre à disposition (que je n'utilise pas).


Le problème c'est que quand le cap doit bouger ( à 115 secondes ), il ne fait pas grand chose ...

[anapurna]

salut

essai cette formule tenant compte des deux premier angles

Code :

Sélectionner tout - Visualiser dans une fenêtre à part

1
2
3
4
5
6
7
8
9
 
 
  norm := Sqrt((y*y)+(x*x)+(z*z))
  Y1 :=  (y*y)/norm
  X1 :=  (x*x)/norm
  Z1 :=  (z*z)/norm
  cap=atan2( (-Y1*cos(tangage)   + Z1*sin(tangage) ) , 
                     X1*cos(Roulis) + Z1*sin(Roulis)*sin(tangage)
                  + Z1*sin(Roulis)*cos(tangage)) *180/PI;

[Sunviv]

Les résultats sont très bruités cette fois.

Juste avec l'accéléromètre on à ça :



et en utilisant un filtre complémentaire qui combine les données de l'accéléro et du gyro on obtient ça :




Ce qui à en effet réduit le bruitage causé par l’accéléromètre sans pour autant améliorer les valeurs.

J'ai testé beaucoup d'équation différente qui devait donner le cap, mais à chaque fois elle ne fonctionne pas. Je pense que beaucoup doivent être dans le même cas que moi.

[anapurna]

salut

en cherchant un peu sur le net j'ai trouvé cours sur l'accélerometre
a priori il parle effectivement de correction

[Sunviv]

Salut

Dans cette exemple ils n'ont pas réussi non plus à calculer l'angle de lacet.

[Sunviv]

Salut,


Aujourd'hui je bachote sur une autre méthode de calcul, le filtre de Kalman.

J'utilise un filtre de Kalman Étendu pour linéariser mes termes non linéaire, et mon objectif est de déterminer le biais du gyro avec ce filtre.

Je comprends le principe mais l'appliquer à mon sujet et une autre chose ...

Je trouve très peu d'exemple sur Internet, seul celui de ferdinand piette était intéressant (http://www.ferdinandpiette.com/blog/...pe-et-exemple/) mais elle limite beaucoup trop son sujet.

Avez vous des exemples de calcul d'attitude avec un tel filtre ?