Pomiar kątów za pomocą akcelerometra

Home | efekty | arduino | Pomiar kątów za pomocą akcelerometra

Pomiar kątów za pomocą akcelerometra

Tomasz Bartuś

2017-05-02

Wstęp

Na rynku znajduje się sporo tanich modułów integrujących różne urządzenia pomiarowe (np. akcelerometry, żyroskopy, grawimetry), które umożliwiają odczyt składowych wektorów przyspieszenia siły bezwładności, prędkości obrotowej, czy przyspieszenia siły ciężkości (g). Są one często wykorzystywane do stabilizacji lotu modeli latających (np. multikopterów) lub budowania robotów i pojazdów przemieszczających się w oparciu o jedną oś (np. Segway). Projekt przedstawia prosty układ umożliwiający pomiar kątów za pomocą akcelerometra (Fig. 1).

Lista części

platforma Arduino (tu Leonardo),
moduł GY-521 (akcelerometr i żyroskop MPU-6050),
płytka prototypowa,
przewody/mostki.

Hardware

Moduł GY-521 (Fig. 2) to dwa urządzenia: 3-osiowy czujnik przyspieszenia (akcelerometr) oraz 3-osiowy czujnik prędkości kątowej (żyroskop) zintegrowane w układzie MPU-6050. Akcelerometr umożliwia zmianę czułości pracy w zakresach: ±2g, ±4g, ±8g, ±16g. Zakresy pracy żyroskopu: 250°/s, 500°/s, 1000°/s, 2500°/s. Układ wyposarzono w 16-bitowy przetwornik analogowo-cyfrowy (ADC). Moduł może być zasilany napięciem 3,3-5,0V. Cechuje się niskim poborem prądu około 350uA. Układ do komunikacji wykorzystuje magistralę I²C.

Moduł MPU-6050 — Fig. 2. Moduł akcelerometru i żyroskopu GY-521

Schemat ideowy

Zasada działania

Po zmontowaniu układu z Fig. 3 i uzupełnieniu Arduino IDE o niezbędne biblioteki MPU6050 i I2Cdev można przystąpić do testów. W wyniku działania skryptu z przykładu (MPU6050raw), w monitorze portu szeregowego otrzymujemy 6 kolumn szybko zmieniających się liczb odzwierciedlających aktualne położenie akcelerometru i żyroskopu (Tab. 1). Wektory wypadkowe siły bezwładności i/lub siły grawitacji oraz siły rotacji są projektowane na trzy składowe układu kartezjańskiego X, Y i Z.

Accelarometer            Gyrometer
Ax	   Ay	   Az	  Gx	 Gy	  Gz
-6616	13880	-1380	 915	-68	 -49
-6624	13924	-1496	 909	-41	-136
-6680	13896	-1408	 917	-46	-148
-6636	13996	-1508	 913	-35	-111
-6668	13896	-1616	 902	-47	 -47
-6716	13944	-1496	 916	-43	 -40
-6616	13972	-1412	 879	-57	  -5
-6584	13884	-1536	 918	-47	 -25
-6920	14192	-1584	 892	-10	-164
-7792	15016	-1524	1022	-80	 -68
-6928	14244	-1484	 773   -112	 261
-6396	13764	-1416	 939	-72	 112
-6636	14020	-1596	 892	-43	-154
-6520	13836	-1524	 946	  4	-242
-6776	13916	-1552	 926	  8	-333
-6916	14008	-1568	 891	-64	 -54
-6592	13936	-1460	 866   -113	 216

Dane wyświetlane są w formie "surowej". Liczby odzwierciedlają poziomy napięć czujników przetworzone przez przetwornik ADC na wartości bezwymiarowe. W związku z wyposarzeniem chipa MPU-6050 w 16-bitowy przetwornik ADC, zakres zmieności każdego z sześciu wskazań (Ax, Ay, Az, Gx, Gy i Gz) wynosi [-32768, +32767] (2¹⁶). Rzeczywiste wartości przyspieszenia i rotacji urządzenia zależą od ustawień akceleratora i żyroskopu (zapisywanych w odpowiednich rejestrach obu urządzeń, zob. str.: 14 (GYRO_CONFIG) i 15 (ACCEL_CONFIG) dokumentacji MPU-6000 and MPU-6050 Register Map and Descriptions. Czułości akcelerometra może wynosić: ± 2, 4, 8 lub 16g, a zakresy pracy żyroskopu: ± 250, 500, 1000 lub 2000 [°/s]. Wyświetlane dane, za pomocą odpowiednich wzorów przeliczeniowych można przetworzyć na wartości wyrażone we właściwych jednostkach przyspieszenia [m/s²] (akcelerometr) i prędkości obrotowej [°/s] (żyroskop). Wzory przeliczeniowe dostępne są w Accelerometer & Gyro Tutorial. Domyślne ustawienia chipu MPU-6050 w klasie biblioteki I2Cdevlib to: ± 2g dla akcelerometru i ± 250 [°/s] dla żyroskopu. Jeśli urządzenie jest perfekcyjnie skalibrowane i pozostaje w stanie spoczynku, wtedy: wskazania skladowych siły grawitacji oddziałującej na akcelerometr w osiach X/Y powinny wskazywać wartość 0 zaś rejestracja siły działajacej wzdłuż osi Z powinna wskazywać wartość 1g (przy czułości +2g będzie to wartość +16384). W stanie spoczynku prędkości rotacji wokół osi X/Y/Z powinny wskazywać wartosc 0. W rzeczywistości wartości wskazywane przez akcelerometr dla osi X i Y nigdy nie będą równe dokładnie 0. Dzieje się tak z powodu trudności z idealnym spoziomowaniem urządzenia, a także w związku z szumami/błędami. Podobnie wartości wskazywane przez żyroskop dla roracji wokół trzech osi, z tych samych powodów nigdy nie będą wynosiły dokładnie 0. Więcej na temat zasady działania akcelerometru i żyroskopu w Accelerometer & Gyro Tutorial.

Dane generowane przez akcelerometr można wykorzystać do obliczania kątów nachylenia urządzenia wokół osi X (tzw. przechył, ang. roll), wokół osi Y (tzw. pochylenie, ang. pitch) oraz wokół osi Z (tzw. kierunek azymutalny, ang. yaw). Formuły konwertujące wartości akcelerometra na wartości kątów roll (ρ) i pitch (φ) prezentuje Fig. 4.

Kąt nachylenia urządzenia względem osi X, Y i Z — Fig. 4. Kąt nachylenia urządzenia względem osi X (ρ), Y (φ) i Z

W praktyce programistycznej, do obliczenia wartości obu kątów wykorzystywana jest funkcja:

double atan2(double a, double b)

obliczająca arcus tangens ilorazu wartości a/b przekazywanych jako argumenty, która jest dostępna za pośrednictwem biblioteki Math.h. Funkcja zwraca odpowiedni kąt wyrażony w radianach. W celu przeliczenia radianów na wartości kąta wyrażone w stopniach, obliczone wartości kątów są dzielone przez wyrażenie (pi/180).

Kod

Poniższy kod podaje uśrednione wartości pomiarów kątów


#include "Wire.h"
#include "Math.h"
#include "I2Cdev.h"
#include "MPU6050.h"

MPU6050 accelerometer;

const float pi = 3.141592; 
const int sample_no = 100; // no of samples for aproximation
int16_t ax, ay, az;
float x, y, z;
int  sample;
float _angle_x, angle_x, _angle_y, angle_y;
long ax_sum, ay_sum, az_sum;

void setup() {
    Wire.begin();
    Serial.begin(38400);
    accelerometer.initialize();
    if (accelerometer.testConnection());
    Serial.println("MPU 6050 connection OK...");  
}

void loop() {
    accelerometer.getAcceleration(&ax, &ay, &az);
    ax_sum = ax_sum + ax;
    ay_sum = ay_sum + ay;
    az_sum = az_sum + az; 
    sample++;
      
    if (sample == sample_no)
    {
     // mean values
     x = ax_sum/sample_no;
     y = ay_sum/sample_no;
     z = az_sum/sample_no;
     
     // Calculate of roll and pitch in deg
     angle_x = atan2(x, sqrt(square(y) + square(z)))/(pi/180);
     angle_y = atan2(y, sqrt(square(x) + square(z)))/(pi/180);
     
     // Reset values for next aproximation   
     sample=0;
     ax_sum = 0;
     ay_sum = 0;
     az_sum = 0;
        
    Serial.print(angle_x);
    Serial.print("\t"); // \t = tablator 
    Serial.println(angle_y);
    }
}

Tomasz Bartuś