Discussion :

[Pablooo_g]

Bonjour,
Pour un travail en classe préparatoire pour écoles d'ingénieures, je cherche à déterminer la position d'un solide grâce à une centrale inertielle (MPU6050 couplée à une carte Arduino et une carte SD).
Je sais bien que les centrales inertielles ne sont pas toujours efficaces à causes de certaines approximations (blocage de cardan, perte d'un degré de liberté, incertitude, fréquence d'échantillonnage, erreurs d'intégrations, …), mais le but étant de comparer différentes méthodes d'études expérimentales de la position, cette experience est tout de même intéressante.

Après avoir récolté les données dans un fichier csv test.txt, j'essaie, en utilisant les angles d'Euler (yaw, pitch, roll), de remonter jusqu'à la position de mon solide (à l'arrêt en position 0 à l'instant initiale) en python.
Je représente ensuite les différentes positions de mon solide au cours du temps dans un graph en 3 dimensions.
Cependant, la figure finale (ci-dessous) ne correspond pas à ce à quoi je m'attendais (la centrale inertielle est immobile mais la figure ne correspond pas au résultat attendu …).


Serait-il possible d'avoir un retour sur mon code python, et peut-être des hypothèses sur les sources possibles d'erreurs, notamment dans le traitement de mes données.

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
 
import numpy as np
import matplotlib.pyplot as plt
import math
 
# fonction pour importer facilement le fichier csv contenant les données
def import_csv(filename):
    file = open(filename, 'r')
    fichier = file.readlines()
    file.close()
    head = fichier[0].split(', ')
    head[6] = head[6].replace("\n", "")
    print(head)
    data = [[], [], []]
    for i in range(1, len(fichier) - 1):
        words = fichier[i].split(',')
        data[0].append(float(words[0]))
        data[1].append([float(words[1]), float(words[2]), float(words[3])])
        data[2].append([float(words[4]), float(words[5]), float(words[6].replace("\n", "0"))])
    return head, data
 
######################### Importation du fichier ################################
titre, donnees = import_csv('test.csv')
time = [0.001 * donnees[0][i] for i in range(len(donnees[0]))]
 
######################### Changement de base grace aux angles d'Euler ################################
 
lst_yaw = [math.radians(elem[0]) for elem in donnees[2]]
lst_pitch = [math.radians(elem[1]) for elem in donnees[2]]
lst_roll = [math.radians(elem[2]) for elem in donnees[2]]
 
R = [] #Liste des matrices de passage d'une base à une autre
for i in range(len(donnees[0])):
    yaw = lst_yaw[i]
    pitch = lst_pitch[i]
    roll = lst_roll[i]
    yawMatrix = np.array([[math.cos(yaw), -math.sin(yaw), 0],
                          [math.sin(yaw), math.cos(yaw), 0],
                          [0, 0, 1]])
    pitchMatrix = np.array([[math.cos(pitch), 0, math.sin(pitch)],
                            [0, 1, 0],
                            [-math.sin(pitch), 0, math.cos(pitch)]])
    rollMatrix = np.array([[1, 0, 0],
                           [0, math.cos(roll), -math.sin(roll)],
                           [0, math.sin(roll), math.cos(roll)]])
    R.append(np.dot(yawMatrix, np.dot(pitchMatrix, rollMatrix))) # Produit des 3 matrices de rotations
 
acc_B = donnees[1] #accélérations dans la base orientée selon le barycentre de la centrale
acc_O = [list(np.dot(R[i], acc_B[i])) for i in range(len(acc_B))] # accélération dans la base lié au barycentre de la centrale à l'instant initiale
 
x = [0.5 * acc_O[i][0] * ((time[i+1] - time[i])**2) for i in range(len(acc_O)-1)]  # Création du tableau de l'axe x
y = [0.5 * acc_O[i][1] * ((time[i+1] - time[i])**2) for i in range(len(acc_O)-1)]  # Création du tableau de l'axe y
z = [0.5 * acc_O[i][2] * ((time[i+1] - time[i])**2) for i in range(len(acc_O)-1)]  # Création du tableau de l'axe z
 
# Tracé du résultat en 3D
fig = plt.figure()
ax = fig.gca(projection='3d')  # Affichage en 3D
ax.scatter(x, y, z, label='Courbe', marker='o')  # Tracé des points 3D
plt.title("Points 3D")
ax.set_xlabel('X')
ax.set_ylabel('Y')
ax.set_zlabel('Z')
plt.tight_layout()
plt.show()

Merci d'avance pour vos avis/réponses

[robinechuca]

A premiere vu, le code semble correct bravo!

1) Pour le calcul de la position, tu utilises la formule x = 1/2 * a * t**2, (si on considère que tu pars à l'arret c'est correct). Mais dans ton implémentation, tu intègres avec la méthode des rectangles. En effet pour passer de la position t à t+dt tu ne considères que le point t, cela revient à dire que l'acceleration est constante sur ce segment. Pour plus de précision, tu peux considérer un acceleration "affine" ou même plus encore!

2) Les centrales inertielles sont très bruitées, si tu connais l'acceleration maximum raisonable de ton module, tu peux filtrer les fréquences qui sont au dessus.

3) En numpy, np.dot(A, B) s'écrit simplement A @ B

Voilou!