Балансирующий робот

[Mr.Kubikus]

Интересная статья о фильтре Калмана - "Фильтр Калмана — !cложно?"

[Mr.Kubikus]

Допиливать руками конечно хорошо, но все же хотел уточнить по поводу осей для колес.

В моем роботе используются оси 35063:

Ось 30 мм

Я посадил их на клей, так что теперь люфтов почти нет. Остался только небольшой в редукторе мотора.

[Eugene]

Нужна ваша помощь...
Углы вроде бы измеряются верно.
Даже GUI прицепил этот, графики строит красивые - http://www.x-firm.com/?page_id=210
А вот балансировать ну никак не хочет. ПИД регулятор уже весь измучил, но не могу подобрать коэффициенты.

Причем наверно из-за расположения платы пришлось добавить "минус"
У вас он тоже стоит, но вы другие функции используете, по ним еще больше вопросов.

Код: Выделить всё

int getGyroRate() {                                             // ARef=3.3V, Gyro sensitivity=2mV/(deg/sec)
  return int(sensorValue[GYR_Y] * -4.583333333);                 // in quid/sec:(1024/360)/1024 * 3.3/0.002)
}

Вот эти

Код: Выделить всё

float getGyroRate2() { 
 return float((sensorValue[GYR_Y] - gyro_offset) * gyro_scale); 
}
int getAccAngle2() { 
 return float((sensorValue[ACC_X]- acc_offset) * acc_scale); 
}

От куда вы берете эти оффсеты и масштабы?
У меня и без них вроде бы все верно, а если сделать аналогично вашей прошивке, то становится еще хуже.

Код: Выделить всё

float gyro_offset = 450;
float gyro_scale = 1.611;
float acc_offset = 500;
float acc_scale = 0.560;

[Mr.Kubikus]

У меня получился следующий алгоритм настройки регулятора:
1. Измерение угла работает нормально.
2. Привод работает нормально.
2. Установить Ki=0 и Kd=0.
3. Увеличивая Kp, добиться работы системы. При этом будут большие колебания относительно положения равновесия.
4. Установить Kp=90% от полученного значения. Увеличивая Kd, добиться уменьшения перерегулирования. Амплитуда колебаний должна уменьшиться.

Перед настройкой регулятора надо особенно внимательно проверить работу фильтра, который обрабатывает сигналы от гироскопа и акселерометров. Слишком низкая частота среза у фильтра может приводить к снижению качества регулирования САР.

Также будет полезно установить пару подстроечных резисторов на робота и подключить их к входам АЦП arduino. Подстроечники можно использовать для оперативного изменения Kp и Kd. Так будет проще настраивать регулятор.

[Eugene]

Попробую по такой схеме.

По поводу подстроечных регистров - можно обойтись без них, через GUI можно на лету менять значения коэффициентов ПИД-регулятора

Перед настройкой регулятора надо особенно внимательно проверить работу фильтра, который обрабатывает сигналы от гироскопа и акселерометров. Слишком низкая частота среза у фильтра может приводить к снижению качества регулирования САР.

Каким образом можно проверить? По графикам с виду всё верно выдает.

upd: Я кажется понял, почему робот не балансирует как надо.
У всей конструкции немного смещен центр масс. При этом робот пытается встать ровно в ноль градусов от вертикали.
Надо попробовать поменять SetPoint... завтра проверю

[Mr.Kubikus]

От куда вы берете эти оффсеты и масштабы?

Код: Выделить всё

    float gyro_offset = 450;
    float gyro_scale = 1.611;
    float acc_offset = 500;
    float acc_scale = 0.560;

Тут все просто - я вам записал не ту версию программы. Вот актуальная версия:

K_BotV1_zgva.zip

(3.89 КБ) 1487 скачиваний

За основу я взял KasBot V1.

Вам нужно будет внести исправления, чтобы использовать вашу схему размещения и подключения датчиков.
Также обратите внимание на функцию int Drive_Motor(int torque). В ней используются коэффициенты, учитывающие различия характеристик моторов. У меня получилось 0,95 и 1,0. У вас, наверное, будет по другому.

Код: Выделить всё

    analogWrite(PWM_R,torque * 0.95);
    analogWrite(PWM_L,torque * 1.00);

[Eugene]

Да в моей прошивке в принципе почти все тоже самое.
Моторы вроде бы одинаково работают, вокруг вертикальной оси робот не крутится.
ПИД-регулятор ну совсем не хочет как надо работать.
Попробовал ввести аналогичные вашим коэффициенты - 50 1 15... попробую чуть позже видео сделать.
Не знаю даже куда еще копать

[Mr.Kubikus]

Как дела? Удалось ли найти решение?

[Eugene]

Всем привет!

Да, в чем проблема нашел. Все дело было в колесах и крепеже колес к оси моторов.
Сами колеса проскальзывают на гладкой поверхности. Надо что-то на них надевать или клеить.
Система крепежа колеса к оси мотора очень ненадежная и имеет большой люфт. Колеса хотят ходуном.
Пример робота после небольшой доработки колес

Надел резинки и более жестко посадил пластиковые оси колес во втулку.

[Eugene]

А вот фотки того, что успел сделать за сегодня.
Чтобы уменьшить болтание колес, вместо пластиковой оси поставил винт.
В пластиковой втулке метчиком нарезал резьбу и вкрутил туда винт с колесом.
Раму сделал из оргстекла 4мм и метровой штанги с резьбой М5. Распилил ее на 4 части по 250мм
Лишнее еще надо отрезать, т.к. если робота делать на всю длину (250мм) моторы уже не справляются.

Модуль IMU прикрепил на бонках как можно ближе к оси колес.
Модуль расположен датчиками вверх.

Аккумулятор прилепил на двухсторонний мягкий скотч и дополнительно закрепил нейлоновыми стяжками.
Можно еще крест-накрест сделать. Отверстия имеются дополнительные.

Моторы планировал прикрепить 6ю винтами каждый, но немного не рассчитал. =)
Два винта помешали поставить контакты питания моторов.
Но и на 4х очень даже неплохо держится. Чутка правда криво получилось, надо бы устранить.

Видео будет попозже.