Міряємо кут нахилу за допомогою акселерометру

2021-08-19

В попередніх частинах нашої розповіді про аналоговий акселерометр ADXL335 ми навчилися позбавлятися шумів на його виходах - фільтрувати сигнал, а також шукати початок відліку сигналу - визначати нульові покази. Якщо ми встановимо ADXL на якийсь об'єкт і не будемо його повертати, то акселерометр буде міряти силу гравітації, яка діє на об'єкт. Вектор цієї сили завжди направлений вниз - в напрямку центру Землі. При горизонтальному положенні плати акселерометру цей вектор співпадає з віссю Z. Якщо ми повернемо ADXL на деякий кут ? навколо вісі Y, то на виходах акселерометру ми отримаємо напруги, які пропорційні величинам проекцій цієї сили на вісі просторової системи координат. На малюнку ці проекції вектора сили гравітації F позначимо Fx та Fz.

Для пошуку кута повороту ? розглянемо плаский варіант, обмежившись площиною Z0X:

Легко побачити, що вектор F та його проекції Fx та Fz утворюють прямокутний трикутник.

Катет a трикутнику відповідає довжині вектору Fx, катет b - довжині вектору Fz , а сила гравітації F співпадає з гіпотенузою С трикутника.

Очевидь, що ? легко знаходиться зі шкільного співвідношення:

Таким чином, для пошуку кута відхилення об'єкту від горизонтального положення відносно вісі Y потрібно виконати наступний алгоритм:

1. Виміряти напруги на відповідних виходах акселерометру при горизонтальному положенні об'єкту - Uxo та Uzo. Визначити нуль.

2. Визначити величину напруг Ux та Uz при нахиленні об'єкту.

3. Знайти величину відхилення напруг від "горизонтального" стану (від нуля).

?Ux = Ux - Uxo

?Uz = Uz - Uzo

4. Знаючи чутливості акселерометру по вісям Sx=300 мВ, знайти величину проекцій вектору на вісі координат (значення сили гравітації по вісям):

Gx = ?Ux / Sx

Gz = ?Uz / Sz

5. Обчислити кут:

a=arctg(Gx/Gz)

Почнемо реалізацію алгоритму. Зберемо схему.

Тепер візьмемо програму з попередньої частини статті (Шукаємо нуль в акселерометрі) та визначимо нульові покази акселерометра по вісям Х та Z. Для цього покладемо контролер з акселерометром горизонтально, так щоб вісь X була паралельна площині столу.

Для зручності я використав саморобний шилд, зібраний на базі Arduino Prototype Shield (код товару ASC111). Відкриваємо монітор порта та дочекавшись сталих значень, запишемо число Ux з точністю 5 знаків після коми. Ставимо контроллер так, щоб вісь Z була паралельна площині столу.

Перезапускаємо скетч і запишемо значення "нулю" Uz. В результаті я отримав такі числа:

Uxo = 1.51366

Uzo = 1.56282

УВАГА! У вас, скоріш за все, будуть інші значення. Тепер можна записати в Arduino програму визначення куту нахилу об'єкту відносно вісі Y. Не забуваємо замінити зірочки (*) на свої значення "нулів".

#include <Math.h> // підключаємо математичну бібліотеку

//Оголошення змінних

float ADC_ref=5.0; //максимальне значення напруги у АЦП

float analog_resolution=1024.0; //роздільна здатність АЦП

float zero_x =*.*****; //значення нулів

float zero_z = *.*****;

float sensitivity_x = 0.3; //чутливості по вісях

float sensitivity_z = 0.3;

unsigned int value_x; //значення зчитані з датчика

unsigned int value_z;

float gx; //проекції вектору гравітації

float gz;

float angle_y; //кут нахилу

void setup() {

       Serial.begin(9600);

}

void loop() {

       value_x = analogRead(A0); // читаємо дані з акселерометру

       value_z = analogRead(A2);

// обчислюємо проекції вектору гравітації на вісі X та Z

       gx = (value_x/analog_resolution*ADC_ref - zero_x)/sensitivity_x;

       gz = (value_z/analog_resolution*ADC_ref - zero_z)/sensitivity_z;

       angle_y =atan2(gx, gz)*RAD_TO_DEG; //розраховуємо кут нахилу

//виводимо кут нахилу

       Serial.print(" Ydeg = "); Serial.println(angle_y);

       delay(100);

}

Нахиляємо акселерометр по вісі Y та дивимось на результати у моніторі порта.

Якщо вам достатньо точності в 1 градус, тоді фільтр в останній програмі не потрібен. В іншому випадку ви можете додати його самостійно, але пристрій стане доволі інерційним, на точне вимірювання кута йому знадобиться деякий час, затримка буде пов'язана з інтегральною природою фільтру.

ВІТАЮ! Ви побудували простий інклінометр. На цьому наше невеличке дослідження закінчено.