Obtenir un angle à partir de l'accélération et de la vitesse angulaire

Piloter un aspirateur autonome ou une tondeuse baladeuse... Pourquoi pas avec un Raspberry Pi ?

Modérateur : Francois

Répondre
Pinhapple
Raspinaute
Messages : 125
Enregistré le : jeu. 23 févr. 2017 15:53
Localisation : Rouen

Obtenir un angle à partir de l'accélération et de la vitesse angulaire

Message par Pinhapple » lun. 27 mars 2017 15:46

Bonjour,

Dans le cadre d'un projet de mesure d'accélération, de rotation, et d'orientation, j'utilise un shield Sense HAT sur un RPi 3. Le Sense HAT possède un gyroscope/accéléromètre LSM9DS1, c'est ce dernier que j'utilise pour faire mes mesures. A ce jour, j'arrive à lire en C++ les valeurs d'accélération (accélération de la pesanteur, mesurée en g) et de rotation (vitesse angulaire, mesurée en degrés par seconde), autour de chacun des trois axes du RPi.

J'en arrive maintenant à mon problème : je dois également récupérer les valeurs d'orientation (angle par rapport à un référentiel fixe, mesuré en degrés). Après avoir fait pas mal de recherche, c'est le "filtre complémentaire" qui revient le plus souvent, et à propos du quel on peut trouver un grand nombre d'articles. Ce filtre est un calcul qui permet de combiner les valeurs d'accélération et de rotation d'un instant t pour obtenir les valeurs d'orientation à ce même instant t.

Je me suis donc basé sur cet article, concernant Arduino mais que j'ai adapté à mes besoins, pour obtenir le code suivant (du Python 3 pour tester, j'adapterai en C++ lorsque le problème sera résolu) :

Code : Tout sélectionner

from sense_hat import SenseHat
import time
import math

sh = SenseHat()

angle = 0.0

while (True):

	acc = sh.get_accelerometer_raw() # Lecture des valeurs brutes d'accélération (en [i]g[/i]).
	rot = sh.get_gyroscope_raw() # Lecture des valeurs brutes de rotation (en radians par seconde).
	
	ax = acc['x']
	ay = acc['y']
	az = acc['z']

	gx = rot['x']
	gy = rot['y']
	gz = rot['z']

	angle = 0.98 * (angle + gy * 0.01 * 57.296) + 0.02 * math.atan2(ax, az) * 57.296 # 57,296 car conversion en degrés (angle * 180 / pi).
	
	print("angle = ") + str(angle)) # Affichage de l'angle d'inclinaison selon l'axe Y.
	
	time.sleep(0.01)
Je démarre le programme pour le tester avec le RPi immobile, et j'obtiens des valeurs cohérentes bien que lentes à se mettre à jour (une dizaine de secondes pour afficher de 0 à 90° alors "qu'en vrai", je l'ai penché immédiatement"). Cela devrait convenir à mes besoins, puisque les angles à mesurer ne devraient pas varier autant.

Pour la suite, je bloque un peu pour le calcul des angles selon les deux axes restants, X et Z.

Pour X, je dirais que je dois utiliser la formule suivante :

Code : Tout sélectionner

angle = 0.98 * (angle + gx * 0.01 * 57.296) + 0.02 * math.atan2(ay, az) * 57.296
Quelqu'un pourrait confirmer ?

En revanche, je n'ai aucune idée de comment adapter la formule pour obtenir l'angle selon l'axe Z vertical, et il me semble avoir lu que cette méthode ne permettait pas d'obtenir l'angle selon l'axe Z (évidemment, impossible de retrouver le lien...). Du coup, pourriez-vous me donner un coup de main pour cet axe ?

L'API officielle Python du Sense HAT propose une méthode get_orientation_degrees() qui est exactement ce dont j'ai besoin, mais qui utilise la bibliothèque RTIMULib que je n'utilise pas, la méthode getIMUData() pour être exact, mais je ne sais pas ce qu'elle fait car je ne trouve pas de fichier la décrivant, même sur le GitHub. D'un autre côté, le GitHub du Sense HAT indique que la biblio RTIMU est une custom version, donc qui n'a peut-être rien à voir.Idéalement, il me faudrait un équivalent de get_orientation_degrees() en C++...

Vous l'aurez compris : je suis paumé.

Je suis également preneur si vous avez des suggestions ! :)
  • RPi 3 + LibreELEC / RPi 3 + RetroPie / RPi B+ + Sense HAT ou Framboisedorf ou module caméra
  • Arduino Mega, Uno, Nano
  • Freescale FRDM KL25Z

Avatar du membre
Flachy Joe
Messages : 88
Enregistré le : mar. 20 sept. 2016 22:30

Re: Obtenir un angle à partir de l'accélération et de la vitesse angulaire

Message par Flachy Joe » lun. 27 mars 2017 22:00

Salut,
Pinhapple a écrit :il me semble avoir lu que cette méthode ne permettait pas d'obtenir l'angle selon l'axe Z
effectivement, puisque la gravité est justement dirigée suivant z et que c'est ça qui est utilisé pour déterminer les angles autour de x et y.
Pour obtenir l'angle de lacet, il faut utiliser une boussole. (magnétomètre) ou intégrer les valeurs de rotation.

Bonnes bidouilles !

Avatar du membre
Flachy Joe
Messages : 88
Enregistré le : mar. 20 sept. 2016 22:30

Re: Obtenir un angle à partir de l'accélération et de la vitesse angulaire

Message par Flachy Joe » lun. 27 mars 2017 22:15

En fait c'est simplissime avec la bibliothèque SenseHat, les filtrages sont déjà fait :

Code : Tout sélectionner

from sense_hat import SenseHat

sense = SenseHat()

while True:
    orientation = sense.get_orientation()
    pitch = orientation['pitch']
    roll = orientation['roll']
    yaw = orientation['yaw']
    print("pitch={0}, roll={1}, yaw={2}".format(pitch,yaw,roll))

Pinhapple
Raspinaute
Messages : 125
Enregistré le : jeu. 23 févr. 2017 15:53
Localisation : Rouen

Re: Obtenir un angle à partir de l'accélération et de la vitesse angulaire

Message par Pinhapple » mar. 28 mars 2017 08:22

Flachy Joe a écrit :En fait c'est simplissime avec la bibliothèque SenseHat, les filtrages sont déjà fait
J'ai oublié de préciser que je n'utilise pas l'API Python du Sense HAT, car les délais de lecture sont trop lents.

En revanche, tu viens de me sauver ma journée avec cette suggestion d'utiliser le magnétomètre, merci mille fois ! :D
  • RPi 3 + LibreELEC / RPi 3 + RetroPie / RPi B+ + Sense HAT ou Framboisedorf ou module caméra
  • Arduino Mega, Uno, Nano
  • Freescale FRDM KL25Z

Avatar du membre
Flachy Joe
Messages : 88
Enregistré le : mar. 20 sept. 2016 22:30

Re: Obtenir un angle à partir de l'accélération et de la vitesse angulaire

Message par Flachy Joe » mar. 28 mars 2017 08:57

Les délais de lectures sont long mais les résultats sont précis, moins tu utiliseras de données et pire sera la précision...

Pinhapple
Raspinaute
Messages : 125
Enregistré le : jeu. 23 févr. 2017 15:53
Localisation : Rouen

Re: Obtenir un angle à partir de l'accélération et de la vitesse angulaire

Message par Pinhapple » mar. 28 mars 2017 09:11

Flachy Joe a écrit :Les délais de lectures sont long mais les résultats sont précis, moins tu utiliseras de données et pire sera la précision...
En C++, j'arrive à lire plus de 1 000 * 6 valeurs par seconde (accélération sur 3 axes et rotation sur 3 axes) en I2C, et les résultats sont cohérents.

Je travaille sur le magnétomètre là, si quelqu'un a une idée de comment obtenir l'orientation en degrés à partir des valeurs en gauss du magnétomètre, je suis preneur !

EDIT : Je suis tombé sur un tuto pas mal, mais je n'obtiens pas du tout les mêmes valeurs brutes que l'auteur, et quand je convertis ces valeurs en direction avec la formule fournie, je n'obtiens que des valeurs entre environ -20 et 30. :?

Mon code utilisé :

Code : Tout sélectionner

#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include <time.h>
#include <wiringPi.h>
#include <wiringPiI2C.h>

#define MAG_ADD 0x1c

float getHeading(float magRawX, float magRawY)
{

	return 180 * atan2(magRawY, magRawX) / M_PI;

}

int main(void)
{

	int fd;

	short mx, my, mz;

	wiringPiSetup();

	fd = wiringPiI2CSetup(MAG_ADD);

	wiringPiI2CWriteReg8(fd, 0x20, 0b11011000);
	wiringPiI2CWriteReg8(fd, 0x21, 0b00000000);
	wiringPiI2CWriteReg8(fd, 0x22, 0b00000000);

	while (1)
	{

		mx = wiringPiI2CReadReg8(fd, 0x29) << 8 | wiringPiI2CReadReg8(fd, 0x28);
		my = wiringPiI2CReadReg8(fd, 0x2b) << 8 | wiringPiI2CReadReg8(fd, 0x2a);
		mz = wiringPiI2CReadReg8(fd, 0x2d) << 8 | wiringPiI2CReadReg8(fd, 0x2c);

		printf("mx = %d | my = %d | mz = %d\n", mx, my, mz);

		//printf("Heading : %.2f.\n", getHeading(mx, my));

		delay(100);

	}

	return 0;

}
Je me demande s'il n'y a pas une histoire de conversion de valeurs brutes vers réelles dans tout ça...
  • RPi 3 + LibreELEC / RPi 3 + RetroPie / RPi B+ + Sense HAT ou Framboisedorf ou module caméra
  • Arduino Mega, Uno, Nano
  • Freescale FRDM KL25Z

guillaume9344
Raspinaute
Messages : 629
Enregistré le : mar. 6 janv. 2015 19:44
Localisation : finistere

Re: Obtenir un angle à partir de l'accélération et de la vitesse angulaire

Message par guillaume9344 » mar. 28 mars 2017 12:02

J ai vue il y quelques temps un composant qui intègre les fonctions accélération pour donner les angles:
BNO055.
rpi b+ ,osmc, motioneyes
rpi 2 raspbian , server minecraft 24h/24 , utilisation gpio
orange pi pc debian ,utilisation gpio, motion cam

Pinhapple
Raspinaute
Messages : 125
Enregistré le : jeu. 23 févr. 2017 15:53
Localisation : Rouen

Re: Obtenir un angle à partir de l'accélération et de la vitesse angulaire

Message par Pinhapple » mar. 28 mars 2017 15:44

guillaume9344 a écrit :J ai vue il y quelques temps un composant qui intègre les fonctions accélération pour donner les angles: BNO055.
Éventuellement en dernier recours, vu que je peux à priori calculer ça à partir de valeurs existantes. Merci ;)

En décortiquant un exemple de la bibliothèque LSM9DS1 SparkFun pour Arduino, j'ai trouvé un autre moyen de calculer l'orientation X et Y, plus rapide qu'avec le filtre complémentaire : angleX = atan2(-accX, sqrt(accY * accY + accZ * accZ)) et angleY = atan2(accY, accZ), avec accX/Y/Z l'accélération en valeurs brutes sur ces trois axes. J'ai fait des tests, c'est plutôt précis, donc ça me va.

Il ne me reste "plus qu'à" calculer angleZ, soit la position par rapport au nord magnétique. Dans le même exemple, il y a une méthode pour le calculer (code Arduino, mais l'algo est le même en C++) :

Code : Tout sélectionner

float heading;

if (my == 0)
	heading = (mx < 0) ? PI : 0;
else
	heading = atan2(mx, my);
  
if (heading > PI)
	heading -= (2 * PI);
else if(heading < -PI)
	heading += (2 * PI);
else if (heading < 0)
	heading += 2 * PI;
Ce qui permet d'obtenir angleZ en radians. Ça a l'air de marcher nickel chez SparkFun, mais chez moi j'obtiens, après conversion en degrés, des valeurs entre 78 et 107°, alors que je fais faire un tour complet à plat au RPi. :?
Auriez-vous une idée de ce qui ne va pas ?

Mon code C++ est le suivant :

Code : Tout sélectionner

#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include <time.h>
#include <LSM9DS1.h>

#define IMU_ADD 0x6a
#define MAG_ADD 0x1c

int main(void)
{

	LSM9DS1 imu(IMU_MODE_I2C, IMU_ADD, MAG_ADD);

	imu.begin();
	imu.calibrate();

	float pitch, roll, heading;

	while (1)
	{

		imu.readMag();
		imu.readAccel();
		
		pitch = atan2(-imu.ax, sqrt(imu.ay * imu.ay + imu.az * imu.az));
		roll = atan2(imu.ay, imu.az);

		if (imu.my == 0)
		{

			if (imu.mx < 0) // Je n'aime pas trop les ternaires, donc je l'ai découpée.
			{

				heading = M_PI;

			}

			else
			{

				heading = 0;

			}

		}

		else
		{

			heading = atan2(imu.mx, imu.my);

		}

		if (heading > M_PI)
		{

			heading -= 2 * M_PI;

		}

		else if (heading < -M_PI)
		{

			heading += 2 * M_PI;

		}

		else if (heading < 0)
		{

			heading += 2 * M_PI;

		}
		
		pitch *= 180.0 / M_PI;
		roll  *= 180.0 / M_PI;
		heading *= 180.0 / M_PI;

		printf("pitch : %.2f | roll : %.2f | heading : %.2f\n", pitch, roll, heading);

	}

	return 0;

}
EDIT : J'ai fait autrement en calculant l'angle en intégrant la vitesse angulaire en fonction du temps écoulé, et ça fonctionne. Ça me débloque, mais j'ai quand même toujours ce problème d'utilisation du magnétomètre.
  • RPi 3 + LibreELEC / RPi 3 + RetroPie / RPi B+ + Sense HAT ou Framboisedorf ou module caméra
  • Arduino Mega, Uno, Nano
  • Freescale FRDM KL25Z

Répondre

Retourner vers « Robots and Co »