5 Зміна підходу до розрахунку орієнтації в просторі (в процесі)

План уроку

Опис проблеми кутів Ейлера
Альтернатива кутам Ейлера – кватерніони
Опис основних властивостей кватерніонів
Комплексні числа
Гіперкомплексні числа

Матеріали

Роборука
Система керування роборукою

Результат уроку

Результатом даного уроку має бути:

Розуміння проблематики кутів Ейлера
Розуміння доцільності застосування кватерніонів в розрахунках
Розуміння основних властивостей кватерніонів
Розуміння механізмів роботи комплексних та гіперкомплексних чисел

Дана стаття знаходиться на етапі редакції та не опублікована повністю

Урок

В чому проблема використання кутів Ейлера? – так звані замки Ейлера.
Якщо простіше – ми можемо це зрозуміти переглянувши наступні анімації:

на першій анімації видно, що плече нашої руки може обертатись як мінімум в трьох степенях вільності:
[анімація з опущеною рукою]

якщо ми змінимо положення нашої руки, то побачимо, як дві осі починають виконувати один і той же обертальний рух:
[анімація з піднятою рукою в бік]

Це може не здаватись проблемою, для людей. Ми можемо змінити положення руки і виконати ті рухи, які нам потрібні.

Але в математиці все працює складніше. Наш мозок вміє уникати складних ситуацій, і вміє з них виходити на рівні підсвідомості.

При розробці складних механізмів з декількома степенями вільності рано чи пізно ми зустрінемо таку проблему.

Фізики називають цей процес складанням рамок.

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

Тому для відображення і вираховування нашої позиції в просторі прийнято уникати такого інструменту як кути Ейлера. Натомість найчастіше знаходять своє застосування кватерніони та матриці обертання, або як їх ще називають “матриці напрямних косинусів”.

Швидше за все з такими поняттями в школі чи в коледжі ви не стикались.

В межах даного курсу ми розглянемо тільки кватерніони, так як операції з ними простіші, їх менше і вони за допомогою чотирьох чисел відображають нашу орієнтацію в просторі уникаючи проблеми кутів Ейлера. Єдине, що варто зауважити, кватерніони складніші для розуміння через неочевидність механізмів їх роботи.

Старатимусь поступово і просто пояснити їх суть.

Як це працює в порівнянні з кутами Ейлера?
Для обертання обʼєкта в просторі за допомогою кутів Ейлера нам треба здійснити три дії: обертання навколо кожної осі окремо.

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

Здійснюючи менше рухів, виконуючи менше операцій ми добиваємось того ж результату.

Якщо звернути увагу на корінь цього слова, “кватер” та від опису вище, стане зрозуміло, кватерніон – це вектор, що складається не зі звичних для нас трьох координат, а з чотирьох. Один із них – кут (насправді все трохи складніше, але для кращого розуміння в межах цієї статті подано таке спрощене пояснення).

vector_angle = [angle, x, y, z]

Здавалось би все просто і тут зовсім нічого складного. Але, якщо розглядати дану задачу з таким підходом, то виникає питання, а як знаходити нову орієнтацію? Які математичні операції дозволять знайти нову позицію в просторі використовуючи кутову швидкість? Цим питанням математики задавались давно.
Одним із них був Вільям Гамільтон. У 1848 році він довів, що 4 цифр може бути достатньо для повного відображення орієнтації обʼєкта уникаючи проблеми замикання рамок.

Назвав він це кватерніонами та представив наступним чином:

Кватерніон складається з чотирьох компонентів
Перший компонент кватерніона – не просто кут обертання навколо вектора, а косинус половини цього кута:
w = cos(angle / 2)
Наступні три компоненти – не просто вектор, навколо якого здійснюється обертання …

Дана стаття знаходиться на етапі редакції та не опублікована повністю

#include <Arduino.h>
#include <esp_now.h>
#include <WiFi.h>
#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>

#include "quaternion.h"

using namespace imu;

#define CHANNEL 1

esp_now_peer_info_t slave;
Adafruit_MPU6050 mpu;
sensors_event_t a, g, temp;

Quaternion orientation, qDeviation;
Vector<3> eulerAngles;

float lastSendTime = 0;
double Bias[3];
int data[2];

double deltaX, deltaY, deltaZ;
double angleX, angleY, angleZ;

void OnDataSent(const uint8_t *mac_addr, esp_now_send_status_t status);
void ScanForSlave();
int RadToDeg(double radVal);
void LEDBlinking(int ledPin, int numOfBlinks, int blinkDuration, bool counting);

// ===========================================
// КОД ПИСАТИ ТУТ       \/
// Функція розрахунку аддитивної похибки
void calcBiases()
{
    // g.gyro.x - Кутова швидкість навкого осі X
    // g.gyro.y - Кутова швидкість навкого осі Y
    // g.gyro.z - Кутова швидкість навкого осі Z
    // deltaX - Значення аддитивної похибки по осі X
    // deltaY - Значення аддитивної похибки по осі Y
    // deltaZ - Значення аддитивної похибки по осі Z
    int loopInt = 850;
    for (int i = 0; i < loopInt; i++)
    {
        mpu.getEvent(&a, &g, &temp);
        deltaX += g.gyro.x;
        deltaY += g.gyro.y;
        deltaZ += g.gyro.z;
    }
    deltaX /= loopInt;
    deltaY /= loopInt;
    deltaZ /= loopInt;
}

// Функція розрахунку кутів
void calcOrient()
{
    // angleX - Розрахований кут X
    // angleY - Розрахований кут Y
    // angleZ - Розрахований кут Z

}

void setup()
{
    Serial.begin(115200);
    WiFi.mode(WIFI_STA);
    esp_now_init();
    esp_now_register_send_cb(OnDataSent);
    ScanForSlave();
    esp_now_add_peer(&slave);
    pinMode(LED_BUILTIN, OUTPUT);

    if (!mpu.begin())
    {
        Serial.println("Failed to find MPU6050 chip");
        while (1)
        {
            delay(10);
        }
    }

    mpu.setAccelerometerRange(MPU6050_RANGE_16_G);
    mpu.setGyroRange(MPU6050_RANGE_2000_DEG);
    mpu.setFilterBandwidth(MPU6050_BAND_260_HZ);
    delay(100);

    Serial.println("Calibrate MPU6050 strats in...");
    LEDBlinking(LED_BUILTIN, 5, 500, 1);

    digitalWrite(LED_BUILTIN, HIGH);

    calcBiases();

    digitalWrite(LED_BUILTIN, LOW);
}

void loop()
{
    if ((millis() - lastSendTime) >= 10)
    {
        calcOrient();

        int eueueX = RadToDeg(eulerAngles.x());
        int eueueZ = RadToDeg(eulerAngles.z());

        if (eueueX >= 0 && eueueX <= 180)
            data[1] = eueueX;
        if (eueueZ >= 0 && eueueZ <= 180)
            data[0] = eueueZ;

        esp_now_send(slave.peer_addr, (uint8_t *)data, sizeof(data));
        lastSendTime = millis();
    }
}

void ScanForSlave()
{
    int8_t scanResults = WiFi.scanNetworks();

    for (int i = 0; i < scanResults; ++i)
    {
        String SSID = WiFi.SSID(i);
        String BSSIDstr = WiFi.BSSIDstr(i);

        if (SSID.indexOf("RXNO") == 0)
        {

            int mac[6];
            if (6 == sscanf(BSSIDstr.c_str(), "%x:%x:%x:%x:%x:%x", &mac[0], &mac[1], &mac[2], &mac[3], &mac[4], &mac[5]))
                for (int ii = 0; ii < 6; ++ii)
                    slave.peer_addr[ii] = (uint8_t)mac[ii];

            slave.channel = CHANNEL;
            slave.encrypt = 0;
            break;
        }
    }
}

/** callback when data is sent from Master to Slave **/
void OnDataSent(const uint8_t *mac_addr, esp_now_send_status_t status)
{
    for (int i = 0; i < 2; i++)
    {
        Serial.print(data[i]);
        Serial.print("\t");
    }
    Serial.println("");
}

int RadToDeg(double radVal)
{
    return int((radVal * (180.0 / M_PI)));
}

void LEDBlinking(int ledPin, int numOfBlinks, int blinkDuration, bool counting)
{
    for (int i = 1; i <= numOfBlinks; i++)
    {
        if (counting)
        {
            Serial.print(i);
            Serial.print("\t");
        }
        digitalWrite(ledPin, HIGH);
        delay(blinkDuration / 2);
        digitalWrite(ledPin, LOW);
        delay(blinkDuration / 2);
    }
}

ГОТОВИЙ КОД:

#include <Arduino.h>
#include <esp_now.h>
#include <WiFi.h>
#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>

#include "quaternion.h"

using namespace imu;

#define CHANNEL 1

esp_now_peer_info_t slave;
Adafruit_MPU6050 mpu;
sensors_event_t a, g, temp;

Quaternion orientation, qDeviation;
Vector<3> eulerAngles;

float lastSendTime = 0;
double Bias[3];
int data[2];

double deltaX, deltaY, deltaZ;
double angleX, angleY, angleZ;

void OnDataSent(const uint8_t *mac_addr, esp_now_send_status_t status);
void ScanForSlave();
int RadToDeg(double radVal);
void LEDBlinking(int ledPin, int numOfBlinks, int blinkDuration, bool counting);

// ===========================================
// КОД ПИСАТИ ТУТ       \/
// Функція розрахунку аддитивної похибки
void calcBiases()
{
    // g.gyro.x - Кутова швидкість навкого осі X
    // g.gyro.y - Кутова швидкість навкого осі Y
    // g.gyro.z - Кутова швидкість навкого осі Z
    // deltaX - Значення аддитивної похибки по осі X
    // deltaY - Значення аддитивної похибки по осі Y
    // deltaZ - Значення аддитивної похибки по осі Z
    int loopInt = 850;
    for (int i = 0; i < loopInt; i++)
    {
        mpu.getEvent(&a, &g, &temp);
        deltaX += g.gyro.x;
        deltaY += g.gyro.y;
        deltaZ += g.gyro.z;
    }
    deltaX /= loopInt;
    deltaY /= loopInt;
    deltaZ /= loopInt;
}

// Функція розрахунку кутів
void calcOrient()
{
    // angleX - Розрахований кут X
    // angleY - Розрахований кут Y
    // angleZ - Розрахований кут Z
    mpu.getEvent(&a, &g, &temp);

    Quaternion qW(0, (g.gyro.x - deltaX), (g.gyro.y - deltaY), (g.gyro.z - deltaZ));

    qDeviation = orientation * qW * 0.5;

    orientation = orientation + qDeviation * 0.01;
    orientation.normalize();

    eulerAngles = orientation.toEuler();
}

void setup()
{
    Serial.begin(115200);
    WiFi.mode(WIFI_STA);
    esp_now_init();
    esp_now_register_send_cb(OnDataSent);
    ScanForSlave();
    esp_now_add_peer(&slave);
    pinMode(LED_BUILTIN, OUTPUT);

    if (!mpu.begin())
    {
        Serial.println("Failed to find MPU6050 chip");
        while (1)
        {
            delay(10);
        }
    }

    mpu.setAccelerometerRange(MPU6050_RANGE_16_G);
    mpu.setGyroRange(MPU6050_RANGE_2000_DEG);
    mpu.setFilterBandwidth(MPU6050_BAND_260_HZ);
    delay(100);

    Serial.println("Calibrate MPU6050 strats in...");
    LEDBlinking(LED_BUILTIN, 5, 500, 1);

    digitalWrite(LED_BUILTIN, HIGH);

    calcBiases();

    digitalWrite(LED_BUILTIN, LOW);
}

void loop()
{
    if ((millis() - lastSendTime) >= 10)
    {
        calcOrient();

        int eueueX = RadToDeg(eulerAngles.x());
        int eueueZ = RadToDeg(eulerAngles.z());

        if (eueueX >= 0 && eueueX <= 180)
            data[1] = eueueX;
        if (eueueZ >= 0 && eueueZ <= 180)
            data[0] = eueueZ;

        esp_now_send(slave.peer_addr, (uint8_t *)data, sizeof(data));
        lastSendTime = millis();
    }
}

void ScanForSlave()
{
    int8_t scanResults = WiFi.scanNetworks();

    for (int i = 0; i < scanResults; ++i)
    {
        String SSID = WiFi.SSID(i);
        String BSSIDstr = WiFi.BSSIDstr(i);

        if (SSID.indexOf("RXNO") == 0)
        {

            int mac[6];
            if (6 == sscanf(BSSIDstr.c_str(), "%x:%x:%x:%x:%x:%x", &mac[0], &mac[1], &mac[2], &mac[3], &mac[4], &mac[5]))
                for (int ii = 0; ii < 6; ++ii)
                    slave.peer_addr[ii] = (uint8_t)mac[ii];

            slave.channel = CHANNEL;
            slave.encrypt = 0;
            break;
        }
    }
}

/** callback when data is sent from Master to Slave **/
void OnDataSent(const uint8_t *mac_addr, esp_now_send_status_t status)
{
    for (int i = 0; i < 2; i++)
    {
        Serial.print(data[i]);
        Serial.print("\t");
    }
    Serial.println("");
}

int RadToDeg(double radVal)
{
    return int((radVal * (180.0 / M_PI)));
}

void LEDBlinking(int ledPin, int numOfBlinks, int blinkDuration, bool counting)
{
    for (int i = 1; i <= numOfBlinks; i++)
    {
        if (counting)
        {
            Serial.print(i);
            Serial.print("\t");
        }
        digitalWrite(ledPin, HIGH);
        delay(blinkDuration / 2);
        digitalWrite(ledPin, LOW);
        delay(blinkDuration / 2);
    }
}