16 ноября 2015      227      Комментарии к записи Робот-сегвей на ТРИК отключены

Робот-сегвей на ТРИК

В.А. Егоров, Д.Г. Копосов

Стабилизация перевернутого маятника на примере
робота-сегвея, управляемого контроллером «ТРИК»

Стабилизация твердых тел в пространстве, когда точка приложения силы не совпадает с его центром масс, представляет собой важную научную и практическую задачу. Примером движения таких систем могут служить человеческая ходьба, полет ракеты, движение баржи, толкаемой буксиром, и т.д. В каждом из подобных случаев существует момент сил, стремящихся вывести систему из положения неустойчивого равновесия, и для возвращения в положение равновесия нужно создать момент сил, возвращающий ее в это положение. Изучению данной проблемы посвящено большое количество исследований. Без них было бы невозможно создание в недавнем времени человекоподобных роботов, способных ходить, бегать, танцевать, подниматься или спускаться по лестнице [1].

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

Итак, перевернутый маятник представляет собой модель нестабильной системы управления, сам маятник закреплен сверху на тележке, которая может перемещаться вправо и влево в горизонтальной плоскости, причем это перемещение является управляемым. Задача управления состоит в стабилизации маятника в вертикальном положении на возможно более продолжительное время. Задача стабилизации перевернутого маятника — это проблема хорошо известна в сфере автоматики [2] и часто используется для демонстрации работы различных регуляторов.

Задача разработать без использования высшей математики модель регулятора для стабилизации перевернутого маятника на примере собранного робота-сегвея была поставлена и реализована учащимся МБОУ Гимназия №24 (одним из авторов этой статьи).

Сегвей — одноосный двухколесный робот, который, перемещаясь в двух противоположных направлениях, способен удерживать свое «тело» в вертикальном положении.

p1

Рис.1. Робот-сегвей на основе контроллера ТРИК

В качестве управляющего контроллера робота (рис.1) была выбрана разработка Санкт-Петербургского государственного университета — контроллер «ТРИК», который предназначен для управления средствами передвижения, беспилотными летательными аппаратами, встраиваемыми устройствами и киберфизическими системами.

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

Для того чтобы осуществить эту стабилизацию, контроллер должен:

  • определить углы наклона робота относительно поверхности земли;
  • вычислить отклонение от требуемого положения;
  • подать управляющие сигналы на моторы для компенсации отклонения и приведения робота в требуемое положение.

Для получения данных об углах наклона робота и вычисления отклонения от заданного положения были использованы гироскоп и акселерометр.

3-х осевой гироскоп L3G4200D — устройство, способное реагировать на изменение углов ориентации тела, и выдавать достаточно плавный сигнал. Частота опроса гироскопа 95 Гц. Диапазон значений угловой скорости: ±2000 градусов в секунду (dps). Гироскоп очень чувствителен к небольшим отклонениям, что позволяет быстро корректировать управляющие воздействия, чтобы сохранить равновесие. Гироскоп выдает целое число (обозначим его G_data), которое можно перевести в угловую скорость (G_data*0.07). Проинтегрировав угловую скорость за минимальный промежуток времени (dt), мы получим угловое перемещение (angle_g), которое нам и нужно. Формула примет вид:

angle_g = angle_g + G_data * 0.07*dt

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

3-х осевой гироскоп акселерометр mma845x (диапазон значений: ±2g) не такой быстрый и чувствительный к малым отклонениям, но по его показаниям всегда можно определить, как объект расположен по отношению к горизонту. Формула расчета угла отклонения (angle_a) по данным акселерометра (A_data по соответствующим осям) имеет следующий вид:

angle_a = Math.atan(A_data[Z] / A_data[X]) * 180.0 / pi

Совместное использование акселерометра и гироскопа позволяет добиться более точного определения текущего угла отклонения от вертикального положения (angle). Сделать такое объединение позволяет комплементарный фильтр [3], работа которого определяется следующим выражением:

angle = (1-K)*(angle_g) + K*angle_a,

где K — коэффициент фильтра, который может принимать значение в диапазоне 0,01–0,05.

Итоговая величина угла наклона представляет собой сумму интегрированного значения гироскопа и мгновенного значения акселерометра. Коэффициент фильтра подбирался экспериментальным образом (с шагом 0.005). Оптимальное значение для собранного робота — 0.025. Данный фильтр является упрощенным вариантом фильтра Калмана [4].

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

ПИД-регулятор — устройство в цепи обратной связи, используемое в системах автоматического управления для формирования управляющего сигнала. Он формирует управляющий сигнал (U), являющийся суммой трех слагаемых:

U = Up+Ui+Ud ,

первое из которых (Up) пропорционально входному сигналу, второе (Ui) — интеграл входного сигнала, третье (Ud) — производная входного сигнала. Назначение ПИД-регулятора — поддержание заданного значения угла наклона, называемого уставкой (ustavka), с помощью изменения мощности моторов. Разность (Err) — невязкой или рассогласованием.

Err = angle – ustavka

Выходной сигнал регулятора определяется тремя слагаемыми:

Up = Kp*Err

Ui = Ui + Ki*Err*dt

Ud = Kd * (Err — Err_last)

где Кp, Кi, Кd — коэффициенты усиления пропорциональной, интегральной и дифференциальной составляющих регулятора соответственно.

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

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

  • увеличение Кp приводит к увеличению быстродействия, но снижению устойчивости системы;
  • увеличение Кd также приводит к увеличению быстродействия;
  • дифференциальная составляющая устраняет колебания, возникающие при использовании только пропорциональной составляющей;
  • интегральная составляющая устраняет остаточное рассогласование системы при настроенных пропорциональной и дифференциальной составляющих.

В результате удалось добиться приемлемой стабилизации робота в вертикальном положении (рис.2).

p2

Рис.2. Робот, стабилизированный в вертикальном положении

Фрагмент основной части программы, стабилизирующей робота в вертикальном положении:

var pi = 3.1415926500000002;

var ustavka = 0;  //уставка

var angle_g = 0;  //угол отклонения по гироскопу

var angle_a = 0;  //угол отклонения по акселерометру

var angle = 0;    //результирующий угол наклона

var last_time = Date.now();   //предыдущий момент времени      

var current_time= Date.now(); //текущий момент времени

var dt=0;    //дельта по времени

var Up=0;    //пропорциональная составляющая

var Ui = 0;  //интегральная составляющая

var Ud=0;    //дифференциальная составляющая

var Err = 0; //ошибка

var Err_last = 0; //предыдущее значение ошибки

var Kp = 12.0;    //коэффициент пропорциональной составляющей

var Ki = 5;       //коэффициент интегральной составляющей

var Kd = -12.0;   //коэффициент дифферециальной составляющей

var A_data;       //массив данных с акселерометра (3 элемента)

var G_data;       //массив данных с гироскопа (3 элемента)

var G_correction = 5; //компенсирующая константа

var K=0.045;          //коэффициент комплементарного фильтра

while (true) {

  current_time= Date.now();    //записываем текущее время

  //читаем текущие показания гироскопа и скселерометра

  G_data = brick.gyroscope().read();

  A_data = brick.accelerometer().read();  

  dt = (current_time — last_time)/1000.0; //приращение времени

  //расчет угла отклонения по данным гироскопа

  angle_g = angle_g + (G_data[0] +G_correction)* 0.07*dt;     

  //вычисляем угол от акселерометра

  angle_a = Math.atan(A_data[2] / A_data[0]) * 180.0 / pi;  

  //комплементирующий фильтр

  angle = (1 — K)*(angle_g) + K*angle_a ; 

  Err = angle —   ustavka; //вычисление ошибки (отклонения)

  Up = Kp*Err;    //пропорциональная составляющая

  Ui = Ui + Ki*Err*dt;   //интегральная составляющая

  Ud = Kd * (Err  — Err_last);   //дифференциальная составляющая

  U = Up+Ui+Ud;    //управляющее воздействие ПИД-регулятора

  brick.motor(M3).setPower(U);  

  //задаем управляющим воздействием мощность моторов

  brick.motor(M4).setPower(U);  

  Err_last = Err ;   //запоминаем предыдущую ошибку

  //сброс времени для следующих вычислений

  last_time = current_time;  

}

 

В качестве языка программирования был выбран язык Qt Script — интерпретируемый объектно-ориентированный язык, реализация стандарта ECMAScript. Его можно рассматривать как диалект JavaScript. Подробное описание языка можно найти, например, на сайте: http://javascript.ru/ecma. При программировании на контроллере ТРИК используется библиотека, обеспечивающая взаимодействие скрипта с аппаратным обеспечением.

Изучение подобных систем трудно переоценить, так как система управления перевернутым маятником — довольно распространенная задача во многих отраслях промышленности, а требования к алгоритмам управления постоянно повышаются. Любая ракета представляет собой обратный маятник, так как ее двигатель расположен ниже центра тяжести, и подобных систем довольно много не только в космонавтике. Одна из основных сложностей реализации подобных систем управления заключается в отличии реальной системы от ее математической модели из-за всевозможных неучтенных в математической модели факторов, воздействующих на систему: внешние механические воздействия, неточности изготовления механических узлов и прочие случайные отклонения физических величин от их средних значений.

 

Библиографический список

  1. Порецкий А.C. Методы стабилизации одиночного и многозвенного перевернутого маятника // Физика и прогресс: Тезисы докладов молодежной научной конференции (2006). — CПб: Издательство СПбГУ, 2006. С.230-235 [электронный ресурс].
  2. Ким Д.П. Теория автоматического управления Т. 1, Т. 2. — М.: ФИЗМАТЛИТ, 2003. 288 с.
  3. Федоров Д.С. Разработка системы стабилизации угла отклонения балансирующего робота // Автоматика и программная инженерия. 2015, №2 (12). — Новосибирск. Издательство НГТУ. С.16-34.
  4. Использование фильтра Калмана в задаче определения ориентации тела, подвешенного на струне: Руководство по лабораторной работе / Сост.: Д.С. Иванов, М.Ю. Овчинников, С.С. Ткачев — М.: МФТИ, 2008. — 29 с.
Любимые цитаты
«Науки делятся на естественные, неестественные и противоестественные» (Ландау Лев Давидович)
Популярные записи
Архивы
Амперка. Модули
РобоНорд-2017
Олимпиада «Будущий инженер»
Календарь записей
Сентябрь 2017
Пн Вт Ср Чт Пт Сб Вс
« Авг    
 123
45678910
11121314151617
18192021222324
252627282930  
Пять последних постов
Фотографии

© 2017 Начала инженерного образования в школе · 163051, Архангельск, ул. Тимме, 22/3. МБОУ Гимназия № 24