Страницы

воскресенье, 29 ноября 2015 г.

Решение задачи №1. Проехать заданное расстояние за определенное время

По просьбам подписчиков начинаем публикацию ответов на "Задачки по робототехнике Карандаша и Самоделкина". Сегодня мы рассмотрим решение задачи №1.


Казалось бы - что тут сложного? Дели расстояние на время и получай скорость. Если Вы попробуете подобрать мощность на моторах таким образом, чтобы робот проезжал заданное расстояние за определенное время то обнаружите, что и робот движется не с постоянной скоростью (ему нужно время на разгон) и поверхность пола/стола не идеальна и от заряда батареек зависит мощность, причем нелинейно. 

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

// пусть роботу нужно проехать 2 м за 35 секунд
R = 2000
T = 35
pi = 3.14
// Размер колеса робота, мм
W = 56
// Рассчитываем, сколько градусов всего нужно проехать, с учетом размера колес
S = R * 360 / (W * pi)
// Рассчитываем среднюю скорость в град/сек
С = S / T
// Мощность на моторах, с которой робот проезжает заданное расстояние за +/- приблизительное заданное время
P = 50
// начинаем движение
Моторы(P)
// Коэф-т ускорения
K = 2
ЦИКЛ
  {
  E1 = считываем показание энкодера моторов
  Ждать (1 сек)
  E2 = считываем показание энкодера моторов   
  // смотрим сколько проехал робот за секунду
  E = E2 - E1  
  // корректируем мощность на моторах
  ЕСЛИ (E > C) ТОГДА P = P - K
  ЕСЛИ (E < C) ТОГДА P = P + K
  Моторы(P)
  // корректируем среднюю скорость с учетом оставшегося расстояния, осталось проехать:
  S = R * 360 / (W * pi) - E2
  // обновление средней скорости
  C = S / (T - таймер)  
  }

Второй вариант решения, с использованием ПИД-регулятора:

// пусть роботу нужно проехать 2 м за 35 секунд
R = 2000
T = 35
pi = 3.14
// Размер колеса робота, мм
W = 56
// Рассчитываем, сколько мм он должен проезжать за 0.1 сек 
S = R / (T * 10)
// Теперь в градусах оборотов мотора c учетом размера колес
S = ((R / (T * 10)) * 360 / (W * pi)
// Мощность на моторах, с которой робот проезжает заданное расстояние за +/- приблизительное заданное время
P = 50

// Коэфт-ты ПИД регулятора
Kp = 0.1
Ki = 0.0001
Kd = 0.2

// начальное значение ошибки регулятора
E = 0

ЦИКЛ
  {
  // предыдущее значение ошибки
  Eold = E
  E1 = считываем показание энкодера моторов
  Ждать (0.1 сек)
  E2 = считываем показание энкодера моторов
  ?/ вычисляем ошибку - на сколько градусов 
  // быстрее или медленнее у робота получилось
  E = (E2-E1) - S
  // сумма всех ошибок - интеграл
  I = I + E
  // ПИД-регулятор
  V = (Kp * E) + (Ki * I) + (Kd * (E - Eold))
  // подаем мощность M на моторы 
  M = P - V
  Моторы(М)
  }


4 комментария:

  1. Здравствуйте!Извините за критику,но по-моему второе решение ,с ПИД регулятором ,не работоспособно!Возможно упущена какая-то деталь?
    Буду благодарен за ответ!

    ОтветитьУдалить
    Ответы
    1. Добрый день! Что конкретно Вас смутило в решении? Коэф-ты и стартовую мощность конечно же придется подобрать для реального робота. В момент старта будет рывок, т.к. стартовая ошибка будет очень большой из-за того, что первые 0,1 сек робот стоял.

      Удалить
  2. Смущает,что управляющее воздействие всегда близко к нулю,я ,ради интереса реализовал макет программы в LabView и не увидел никакого управляющего воздействия,интегральная составляющая ,только,изменяется и разница между первыми и вторым измерением оборотов)

    ОтветитьУдалить
    Ответы
    1. Андрей, все верно - регулятор лишь немного подстраивает скорость робота, а не задает ее с нуля. Поэтому важна "Мощность на моторах, с которой робот проезжает заданное расстояние за +/- приблизительное заданное время P = 50". В том числе воздействие действительно может местами быть равным 0, если P близка к требуемой. Возможно Вам стоит попробовать увеличить коэф-ты П,И,Д - они в тексте программы взяты условными по величине. С нуля такой регулятор скорость не установит - такое возможно только на очень длинном участке пути и небольших коэф-тах, потребуется значительное время для выхода регулятора на рабочий режим.

      Удалить