Управляем чем угодно откуда угодно

Управлялка на QT

Обсуждение клиентского ПО для доступа к виртурилке, управления и мониторинга

Re: Управлялка на QT

Сообщение phoenix367 » 21 мар 2014, 21:24

sanya555 писал(а):В общем большое спасибо phoenix367 за помощь, я ваш должник.


Смотрите, могу и счет выставить :mrgreen:

sanya555 писал(а):И вот сразу хочеться спросить где почитать про реализацию 3D модели положения (тоесть как вы реализовали ваш паралелипипед в программе)?

Ох... тут в двух словах и не расскажешь. Делается так:
1. Считываются показания датчиков акселерометра и гироскопа (можно еще магнитометр прикрутить)
2. Из полученных данных вычитается некоторая трендовая составляющая (зависит от температуры, самого датчика, фазы луны и т.д.)
3. Обработанные показания подаются в алгоритм IMU Data Fusing. Я использовал готовые реализации Мэджвика и Махони.
4. На выходе получаем некоторый кватернион.
5. Преобразуем кватернион в углы Эйлера
6. Прореживаем данные, чтобы не забивать канал и отправляем их через широковещательный сокет в сеть
7. На клиентской стороне считываем сообщение и с помощью средств OpenGL рисуем прямоугольник с заданым наклоном

Вот приблизительная схема работы за вычетом некоторых несущественных в данном случае деталей. Более подробно можете почитать вот здесь http://www.olliw.eu/2013/imu-data-fusing/ и далее по приведенным в статье ссылкам.
phoenix367
 
Сообщения: 193
Зарегистрирован: 09 сен 2013, 00:19

Re: Управлялка на QT

Сообщение sanya555 » 24 мар 2014, 17:27

Спасибо, почитаю разберусь.
Я где то читал что вы netbeans настроили для написания программ под саму виртурилку. Так как я в линухе новичок, не могли бы вы пошагово описать весь процесс настройки?
sanya555
 
Сообщения: 74
Зарегистрирован: 10 фев 2014, 13:20

Re: Управлялка на QT

Сообщение phoenix367 » 25 мар 2014, 14:51

sanya555 писал(а):Спасибо, почитаю разберусь.
Я где то читал что вы netbeans настроили для написания программ под саму виртурилку. Так как я в линухе новичок, не могли бы вы пошагово описать весь процесс настройки?

Кое что здесь описано http://forum.virt2real.ru/viewtopic.php?f=44&t=13323&start=10#p16925
А вообще ставите Codesourcery, создаете в нетбинс новую конфигурацию тулзов, добавляете туда компиляторы от Codesourcery, выбираете созданную конфигурацию в настройках проекта и пишете программы как вы обычно это делаете. Потом заливаете то, что скомпилировалось на виртурилку и запускаете/отлаживаете.
phoenix367
 
Сообщения: 193
Зарегистрирован: 09 сен 2013, 00:19

Re: Управлялка на QT

Сообщение sanya555 » 26 мар 2014, 13:58

А какие вы датчики гироскопов и акселерометров используете?
sanya555
 
Сообщения: 74
Зарегистрирован: 10 фев 2014, 13:20

Re: Управлялка на QT

Сообщение phoenix367 » 26 мар 2014, 15:43

ITG3200 - гироскоп
ADXL345 - акселерометр

все как у всех
phoenix367
 
Сообщения: 193
Зарегистрирован: 09 сен 2013, 00:19

Re: Управлялка на QT

Сообщение phoenix367 » 31 мар 2014, 22:30

Добавил роботу немного автономности. Теперь он умеет поворачиваться из стороны в сторону на заданный угол. Пока не слишком точно, но это дело поправимое. Видео процесса можно посмотреть тут

http://youtu.be/LZxWYqjQc54

Несколько комментариев к видео. Итак, открыта программа управления роботом и форма программирования. Пока можно запрограммировать только одну команду rotate, которая поворачивает робот на заданный угол в градусах. Соответственно набор команд
Код: Выделить всё
rotate 20
rotate 0
rotate 43
rotate -20


поворачивает робота против часовой стрелки на угол в 20, 0, 40 и -23 градуса. В окне лога можно наблюдать процесс выполнения команд. В данном случае все они выполнены успешно. Стоит отметить, что PID регулятор, управляющий поворотом реализован непосредственно на стороне виртурилки, а управляющая программа на хосте лишь отправляет команды (на данный момент по одной, но можно и все скопом отправить). Информация об угле поворота считывается с IMU.

Следующим этапом необходимо будет научить робота ездить вперед/назад на заданное расстояние. Тут все гораздо сложнее, поскольку, как показало вскрытие, при работе двигателей сигнал от акселерометра забит шумом вусмерть. Соответственно алгоритмы одометрии на основе IMU выдают полнейшую ерунду. Выходом могло бы стать подвешивание платы с датчиками на резинках + пассивное гашение вибраций двигателей, но что-то как-то лень пока этим колхозом заниматься :roll: Поэтому пока остановился на том, что буду делать оптическую одометрию. Должно работать неплохо.
phoenix367
 
Сообщения: 193
Зарегистрирован: 09 сен 2013, 00:19

Re: Управлялка на QT

Сообщение phoenix367 » 07 апр 2014, 23:19

Итак, допилил PID контроллер управления поворотом и добавил еще команд управления. Теперь робот может ехать вперед и назад заданный период времени в секундах, а также находится в режиме ожидания. По ссылке ролик, демонстрирующий управление тележкой по заданной программе http://youtu.be/EGS-dQA1O9Q.

Одной из сложностей настройки PID контроллера было определение границ и устранение влияния т.н. "мертвой зоны", когда сигнал управления на двигатели подается, а они не крутятся из-за силы трения. Если кому будет интересно, то могу описать, как это сделать с помощью System identification toolbox из среды MATLAB.
phoenix367
 
Сообщения: 193
Зарегистрирован: 09 сен 2013, 00:19

Re: Управлялка на QT

Сообщение sanya555 » 14 апр 2014, 15:04

И снова здраствуйте. В общем я начал изучать алгоритм обработки данных с гироскопа и акселерометра. До конца я еще не дочитал предложенною вами статью но, забежав вперед, увидел реализацию обработки на С Меджвика и Махони. Вопрос состоит в следующем: как вы вычисляете трендовые значение с датчиков. И второй - каким образом кватернион вы преобразовываете в углы Эйлера?
sanya555
 
Сообщения: 74
Зарегистрирован: 10 фев 2014, 13:20

Re: Управлялка на QT

Сообщение phoenix367 » 15 апр 2014, 00:16

sanya555 писал(а):И снова здраствуйте. В общем я начал изучать алгоритм обработки данных с гироскопа и акселерометра. До конца я еще не дочитал предложенною вами статью но, забежав вперед, увидел реализацию обработки на С Меджвика и Махони. Вопрос состоит в следующем: как вы вычисляете трендовые значение с датчиков. И второй - каким образом кватернион вы преобразовываете в углы Эйлера?


Трендовые значения с датчиков вычисляются очень просто. Введем сначала понятие времени калибровки. Это период, в течении которого вычисляются значения смещений показаний датчиков. У меня длительность данного периода составляет 500 циклов. С учетом того, что длительность каждого цикла измерения положения составляет 10 миллисекунд этого достаточно для довольно точного вычисления начальных смещений. Теперь рассмотрим по отдельности рассмотрим вычисление трендов для гироскопа и акселерометра.

Гироскоп:
- во время процедуры калибровки копим данные с осей x, y и z, а в конце вычисляем вектор средних значений. Этот вектор и будет вектором смещений, которые будут вычитаться из показаний гироскопа. Тут однако вылезает проблема, связанная с долговременным дрейфом осей. Т.е. величины смещений могут меняться с течением времени. У меня данная проблема решается с помощью т.н. порогового фильтра. Суть его состоит в том, что мы устанавливаем некое пороговое значение скорости поворота в градусах по каждой оси. В том случае, если скорость поворота по заданной оси меньше этого порога, то происходит обновление соответствующей величины смещения с определенным весом. Если же скорость поворота выше порога, то дальше на обработку отправляется эта скорость за вычетом величины смещения. Это позволяет устранить дрейф угла по оси z с учетом величины шума датчика в состоянии покоя. К недостаткам подхода стоит отнести нечувствительность получаемой системы к вращениям с угловой скоростью ниже порогового значения фильтра. По идее от этого можно избавиться с помощью добавления показаний магнитометра, но у меня пока не получилось его прикрутить так, чтобы система выдавала приемлемые результаты. А код порогового фильтра выгладит как-то так:

Код: Выделить всё
float AHRSProcessor::filterGyroValue(float rawGyroValue, float& offset)
{
    float gyroAngle = (rawGyroValue - offset) / GYROSCOPE_SENSITIVITY;
   
    if (fabs(gyroAngle) < gyroThreshold && gyroAngle != 0.0f)
    {
        offset = 0.9f * offset + 0.1f * rawGyroValue;
        gyroAngle = 0.0f;
    }
   
    return gyroAngle;
}


Соответственно получение показаний гироскопа за вычетом смещения выглядит как-то так:

Код: Выделить всё
    float gyroX = filterGyroValue(data.rawGyroX, gyroOffsetX);
    float gyroY = filterGyroValue(data.rawGyroY, gyroOffsetY);
    float gyroZ = filterGyroValue(data.rawGyroZ, gyroOffsetZ);


Как видите, все довольно просто. При этом система нормально работает в движении и самокалибруется в состоянии покоя. Заметного дрейфа осей нет в течение десятков минут и даже часов. Мне нравится полученный результат.

Акселерометр:
- имеет некоторые небольшие смещения осей, которые мало меняются со временем. Кроме того, наличие таких смещений не приводит к заметному временному дрейфу осей. Поэтому здесь применим другой подход. Во время процедуры калибровки будем подавать на вход алгоритма IMU данные с осей акселерометра, а показания гироскопа не будем учитывать (например, занулим полученные от него значения). По окончании процедуры калибровки на выходе IMU будем иметь кватернион, определяющий мнимый наклон нашего акселерометра. Предполагая, что по время калибровки он был установлен горизонтально нам необходимо будет каждый раз поворачивать систему координат, связанную с кватернионом IMU в обратном направлении, которое определяется полученным ранее кватернионом смещений акселерометра. У меня в коде это выглядит как-то так:

Код: Выделить всё
    QuaternionProd(&qRaw, &offsetQ, &q);

..............................................................
void AHRSProcessor::setOffsetQuaternion(const QUATERNION& q)
{
    QuaternionConj(&q, &offsetQ);
}



Вообще посмотрите реализацию работы с IMU в моем проекте https://github.com/phoenix367/v2rpins_sdk/blob/master/cherokey-robot/commserver/src/AHRSProcessor.cpp, думаю многое станет понятнее.

По поводу преобразования кватерниона в углы Эйлера... Здесь нет однозначного ответа, поскольку все зависит от того, с какой системой координат связан этот кватернион: xyz, zyx, xzy и т.д. У Мэджвика используется clockwise система координат xyz, как удалось установить опытным путем. Возможно про это написано у него в диссертации, но мне пока ее лень читать :) Я портировал код преобразования кватерниона в углы Эйлера из библиотеки Мэджвика. Получилось как-то так:

Код: Выделить всё
bool QuaternionNormalize(const QUATERNION* qin, QUATERNION* qout)
{
    float recipNorm;
   
    if (!qin || !qout)
    {
        return false;
    }

   recipNorm = invSqrt(qin->q0 * qin->q0 + qin->q1 * qin->q1 +
            qin->q2 * qin->q2 + qin->q3 * qin->q3);
    qout->q0 = qin->q0 * recipNorm;
    qout->q1 = qin->q1 * recipNorm;
    qout->q2 = qin->q2 * recipNorm;
    qout->q3 = qin->q3 * recipNorm;

    return true;
}

bool Quaternion2Euler(const QUATERNION* q, float *phi, float* theta,
        float* psi)
{
    QUATERNION qin;
   
    if (!q)
    {
        return false;
    }

    QuaternionNormalize(q, &qin);

    float R_1_1 = 2.0f * qin.q0 * qin.q0 - 1.0f + 2.0f * qin.q1 * qin.q1;
    float R_2_1 = 2.0f * (qin.q1 * qin.q2 - qin.q0 * qin.q3);
    float R_3_1 = 2.0f * (qin.q1 * qin.q3 + qin.q0 * qin.q2);
    float R_3_2 = 2.0f * (qin.q2 * qin.q3 - qin.q0 * qin.q1);
    float R_3_3 = 2.0f * qin.q0 * qin.q0 - 1.0f + 2.0f * qin.q3 * qin.q3;

    if (phi)
    {
        *phi = atan2(R_3_2, R_3_3);
    }
   
    if (theta)
    {
        *theta = -atan(R_3_1 / sqrt(1.0f - R_3_1 * R_3_1));
    }
   
    if (psi)
    {
        *psi = atan2(R_2_1, R_1_1);
    }

    return true;
}


А вообще про кватернионы почитайте первую главу книжки Фурмана "Комплекснозначные и гиперкомплексные системы в задачах обработки многомерных сигналов". Там все разжевано с примерами и картинками. И даже на русском.

Кроме того, можете еще посмотреть исходники функций quat2angle и angle2quat из пакета Aerospace Toolbox, входящего в MATLAB. Правда учтите, что там anticlockwise кватернионы, а соответственно формулы преобразования будут несколько отличаться от мэджвиковских.
phoenix367
 
Сообщения: 193
Зарегистрирован: 09 сен 2013, 00:19

Re: Управлялка на QT

Сообщение sanya555 » 24 апр 2014, 19:12

Добрый день.
В общем пока дальше все изучаю алгоритм фильтрации. Просматривал ваш код и пока мне не ясно для чего нужны следующие функции
Код: Выделить всё
bool QuaternionProd(const QUATERNION* q_a, const QUATERNION* q_b, QUATERNION* q_out)

и
Код: Выделить всё
bool QuaternionConj(const QUATERNION* q_in, QUATERNION* q_out)

которые находятся в файле madgwik_ahrs.c
И еще вопрос, в функции
Код: Выделить всё
void AHRSProcessor::updateState(const IMUSensorsData& data, QUATERNION& q)

вот этот кусок кода
Код: Выделить всё
processedData.rawGyroX = gyroX * M_PI / 180;
    processedData.rawGyroY = gyroY * M_PI / 180;
    processedData.rawGyroZ = gyroZ * M_PI / 180;

для чего именно это делается?

И мне еще вот что интересно:
когда устанавливаем значение calibrated = true то в стуктуру заносятся данные
Код: Выделить всё
            gyroState.offsetX += x;
            gyroState.offsetY += y;
            gyroState.offsetZ += z;

Вот значит вопрос, где они дальше обрабатываются? Просто интересно увидеть код калибровки.
sanya555
 
Сообщения: 74
Зарегистрирован: 10 фев 2014, 13:20

Пред.След.

Вернуться в Софт на управляющих устройствах (iOS, Android, Win/Mac/Linux)

Кто сейчас на конференции

Сейчас этот форум просматривают: нет зарегистрированных пользователей и гости: 1

cron