poconoco: Попробуем собрать нестандартную конфигурацию — восьминогого 2DOF-робота. 2DOF-ноги намного проще программировать, к тому же у меня есть в запасе куча неиспользованных сервоприводов. А главное, можно будет назвать его в честь восьминогого коня бога Одина Слейпниром (всегда мечтал!).

У нашего Слейпнира с каждой стороны будет по четыре ноги с двумя шарнирами. Каждый шарнир — сервопривод, значит, восемь сервоприводов на сторону. Для простоты все восемь шарниров одной стороны коня будут вращаться в одной плоскости. Хотя это вовсе не обязательно. Более того, если ноги с одной стороны пустить немного «шахматкой», чтобы две соседние ноги не могли задеть друг друга, это будет даже лучше, позволит делать шире шаг и скакать галопом.

Аккуратное и функциональное, но далеко не самое дешевое решение — использовать нестандартную плату контроллера, оптимизированную для подключения сервоприводов в большом количестве. Мне подвернулась Dagu Spider Robot Controller — это тот же самый Arduino Mega, но на плате с заранее распаянными 3-пиновыми штырьковыми разъемами, куда сразу, без всяких шилдов, можно подключить те самые 48 сервоприводов. Идеальна для многоногих роботов на Arduino.

Управление

Управление у нас будет происходить по Bluetooth. Для этого есть различные аппаратные решения. Это и шилды, и отдельные платки с UART последовательным интерфейсом (как обычный ком-порт, только с уровнями сигналов 5 В). Мне самой практичной показалась именно маленькая платка с UART-интерфейсом. Подключается к соответствующим контактам UART/Serial порта Arduino. Отметим два нюанса: на Uno/Due/Nano и подобных всего один такой порт, и он же используется для прошивки через USB. Поэтому, возможно, потребуется отключать Bluetooth-модуль на время прошивки. А второй нюанс — не забывай, что RX-контакт модуля подключается к TX-контакту Arduino, а TX — к RX. Такие дела в UART.

Программирование Bluetooth не сложнее сервов, данные можно побайтово вычитывать, чем мы и будем пользоваться:

char cmd;
Serial.begin(9600);
if (Serial.available())
 cmd = Serial.read();

Если используется Arduino Mega и Bluetooth подключен ко второму порту, то вместо Serial пишется Serial1. Примечательно, что можно и не использовать Bluetooth, а управлять роботом прямо по USB. И в коде выше не изменится ничего! Это просто работа с последовательным портом, а висит ли там BT-передатчик или преобразователь USB Serial — нам неважно.

Алгоритм движения

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

Итак, что нам нужно делать для работы с 16 сервоприводами и выбранной походкой? Правильный ответ — читать про инверсную кинематику (ИК). Объем статьи не позволяет развернуть тему широко, но материалов в интернете предостаточно. Вкратце, ИК решает задачу нахождения необходимых управляющих сигналов для того, чтобы система в пространстве заняла нужное положение. Для ноги это значит, что по координатам точки, куда должна попасть стопа, следует определить углы сервоприводов, которые для этого нужно выставить. А управляя координатами стоп, можно управлять положением тела. У нас 2DOF-ноги, оси параллельны, поэтому стопа перемещается всегда в одной плоскости. Задача ИК в данном случае сводится к 2D-пространству, что сильно ее упрощает.

Пускай для каждой ноги локальным началом координат O будет вал верхнего серва, то есть бедра. И у нас есть координаты точки A, куда нужно попасть стопе. Тогда легко увидеть, что нужно решить задачу нахождения точек пересечения двух окружностей (см. схему ног одной стороны, там на самой правой ноге это проиллюстрировано). Найдя точку B пересечения окружностей (выбрав любую из них), несложно посчитать искомые углы, используя перевод из декартовых координат в полярные. В коде решение этой задачи выглядит так:

float A = -2 * x;
float B = -2 * y;
float C = sqr(x) + sqr(y) + 
sqr(hipLength) - sqr(shinLength);
float X0 = -A * C / (sqr(A) + sqr(B));
float Y0 = -B * C / (sqr(A) + sqr(B));
float D = sqrt( sqr(hipLength) - 
(sqr(C) / (sqr(A) + sqr(B))) );
float mult = sqrt( sqr(D) / (sqr(A) + 
sqr(B)) );
float ax, ay, bx, by;
ax = X0 + B * mult;
bx = X0 - B * mult;
ay = Y0 - A * mult;
by = Y0 + A * mult;
// или bx для другой точки пересечения
float jointLocalX = ax;
// или by для другой точки пересечения
float jointLocalY = ay;
float hipPrimaryAngle = 
polarAngle(jointLocalX, jointLocalY);
float hipAngle = hipPrimaryAngle - 
hipStartAngle;
float shinPrimaryAngle = polarAngle
(x - jointLocalX, y - jointLocalY);
float shinAngle = (shinPrimaryAngle - 
hipAngle) - shinStartAngle;

где x и y — координаты точки, куда нужно дотянуться стопой; hipStartAngle — угол, на который повернуто «бедро» изначально (при среднем положении серва), аналогично — shinStartAngle. Кстати, в этих расчетах углы, очевидно, в радианах, а в объекты Servo их передавать нужно уже в градусах. Полный работоспособный код прошивки, включающий этот кусочек, выложен на GitHub, см. ссылку в конце статьи. Это кусок ИК, но кроме него нужно еще немного довольно простого кода, чтобы использовать эту ИК на всех ногах (см. функции legsReachTo(), legWrite()). Также необходим будет код, который собственно реализует походку — движение одной группы ног «назад» (чтобы робот двигался вперед), в то время как другая группа ног приподнимается и переставляется вперед для следующего шага, см. функцию stepForward(). Она делает один шаг с заданными параметрами. Этими параметрами, кстати, можно сделать и шаг назад, несмотря на название функции. Если эту функцию вызывать в цикле, то робот будет шагать вперед.

Теперь получение команд и их интерпретация. Добавим в программу состояние:

enum State
{
 STOP,
 FORWARD,
 BACKWARD,
 FORWARD_RIGHT,
 FORWARD_LEFT
};

И в главном цикле исполнения loop() будем смотреть на текущее состояние (переменная state) и дергать stepForward(), если движемся вперед (с поворотом или без), и опять же stepForward(), но с отрицательным аргументом xamp, если надо двигаться назад. Повороты при этом будут обрабатываться в legWrite(), и для поворота направо ноги с правой стороны будут стоять на месте (пока левые гребут). Вот такой вот конь-танк. Брутально, зато очень просто и работает. Плавный поворот можно сделать только с 3DOF-ногами, пример этого можно увидеть в репозитории buggybug.

switch (state)
{
 case FORWARD:
 case FORWARD_RIGHT:
 case FORWARD_LEFT:
 stepForward(h, dh, xamp, 
 xshift); break;
 case BACKWARD:
 stepForward(h, dh, - xamp, 
 xshift); break;
}

В остальных случаях — стоим. Далее здесь же, в loop(), будем вычитывать из последовательного порта команды и изменять state по ним:

char command;
while (Serial1.available())
command = Serial1.read();
switch (command)
{
 case 'w':
 state = FORWARD; break;
 case 's':
 state = BACKWARD; break;
 case 'd':
 state = FORWARD_RIGHT; break;
 case 'a':
 state = FORWARD_LEFT; break;
 default:
 state = STOP;
}

На этом основные моменты прошивки закончились, остальное — всякая мелочевка. Хотя есть еще один, пожалуй, важный момент — возможность точной подстройки сервов. Даже при самой аккуратной сборке, если всем сервам подать команду повернуться на 90°, все равно некоторые из них получатся чуть со сбитым углом. Потому нужна возможность его подстраивать. Как у меня это сделано, можно посмотреть в методах hipsWrite() и shinsWrite() и собственно в массивах тонких настроек hipsTune[] и shinsTune[].

Сборка

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

Основой будет прямоугольная рама. Конечности — 4-сантиметровые полосочки. Стоит также запастись множеством маленьких болтиков, гаечек. Режем уголок на нужные кусочки, вырезаем пазы для сервов, сверлим дырочки для крепежных болтов и шурупов. Конструкцию лучше раз показать, чем описывать. Размеры могут быть любые, роботы должны быть разнообразны. Но помни: чем длиннее ноги, тем больший рычаг придется толкать сервоприводу и тем больше будет на него нагрузка. Вплоть до невозможности провернуться и даже поломки. Но 4–5 см — без проблем.

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

В комплекте с каждым сервом, как правило, поставляется пара-тройка шурупов и набор насадок, которые можно закрепить шурупом на валу для различных применений. Нам больше всего подойдет одиночный «рог» (или horn), который позволяет прикрепить к серву планку. Так, к одной планке крепятся оси двух сервов, и планка становится «бедром». При этом один серв крепится на теле, а другой становится частью голени. К нему стоит прикрутить еще планку, просто чтобы удлинить или сделать конечность поинтересней. Немного кропотливой работы — и платформа готова (удобные наборы отверток, ключей, пинцеты, кусачки и прочее сильно ускоряют дело).

#include <Servo.h> 
#include <math.h>

#define sqr(x) ((x) * (x))

Servo hips[8];  // Бёдра
Servo shins[8]; // Голени

int hipsTune[8];
int shinsTune[8];

const float hipLength  = 32;
const float shinLength = 40;
const float hipStartAngle = - M_PI / 2.0;
const float shinStartAngle = - 3.0 * M_PI / 4.0;

enum State
{
  STOP,
  FORWARD,
  BACKWARD,
  FORWARD_RIGHT,
  FORWARD_LEFT
};

State state = STOP;

void hipsWrite(int idx, int val)
{
  hips[idx].write(val + hipsTune[idx]);
}

void shinsWrite(int idx, int val)
{
  shins[idx].write(val + shinsTune[idx]);
}

void attachLegs()
{
  hips [0].attach(30); hips [1].attach(45);
  shins[0].attach(31); shins[1].attach(44);
  hips [2].attach(32); hips [3].attach(43);
  shins[2].attach(33); shins[3].attach(42);
  hips [4].attach(34); hips [5].attach(41);
  shins[4].attach(35); shins[5].attach(40);
  hips [6].attach(29); hips [7].attach(46);
  shins[6].attach(28); shins[7].attach(47);
  
  hipsTune[0] = 4; hipsTune[1] = 12;
  hipsTune[2] = 10; hipsTune[3] = 0;
  hipsTune[4] = 17; hipsTune[5] = 0;
  hipsTune[6] = 4; hipsTune[7] = 0;

  shinsTune[0] = 0; shinsTune[1] = -3;
  shinsTune[2] = 20; shinsTune[3] = 7;
  shinsTune[4] = 5; shinsTune[5] = 12;
  shinsTune[6] = 0; shinsTune[7] = 7;
}

void detachLegs()
{
  for (int i = 0; i < 8; ++i)
  {
    hips[i].detach();
    shins[i].detach();
  }
}

void resetLegs()
{
  int d = 20;
  
  for (int i = 0; i < 8; i++)
  {
    hipsWrite(i, 90);
    shinsWrite(i, 90 + ((i % 2) ? d : -d));
  }
}

float polarAngle(float x, float y)
{
  if (x > 0)
    return atan(y / x);

  if (x < 0 && y >= 0)
    return atan(y / x) + M_PI;

  if (x < 0 && y < 0)
    return atan(y / x) - M_PI;

  // x = 0
  if (y > 0)
    return M_PI * 0.5;

  if (y < 0)
    return M_PI * -0.5;

  // y = 0
  return 0;
}

void legWrite(float hipAngleRad, float shinAngleRad, int legIndex)
{
  if (state == FORWARD_RIGHT && (legIndex % 2) == 1)
      return;
  
  if (state == FORWARD_LEFT && (legIndex % 2) != 1)
      return;
  
  float hipAngle = hipAngleRad * 180.0 / M_PI + 90;
  float shinAngle = shinAngleRad * 180.0 / M_PI + 90;

  hipsWrite(legIndex, ! (legIndex % 2) ? hipAngle  : (180 - hipAngle));
  shinsWrite(legIndex, ! (legIndex % 2) ? (180 - shinAngle) : shinAngle);
}

void legsReachTo(float x, float y, int legGroup)
{
  // Решаем задачу инверсной кинематики на плоскости, в которой надо найти 2 угла
  // Мы знаем координаты точки крепления бедра (0;0), и координаты точки, куда хотим 
  // дотянуться - (x;y). Задача сводится к задаче поиска точки пересечения двух окружностей
  float A = -2 * x;
  float B = -2 * y;
  float C = sqr(x) + sqr(y) + sqr(hipLength) - sqr(shinLength);
  float X0 = -A * C / (sqr(A) + sqr(B));
  float Y0 = -B * C / (sqr(A) + sqr(B));
  float D = sqrt( sqr(hipLength) - (sqr(C) / (sqr(A) + sqr(B))) );
  float mult = sqrt( sqr(D) / (sqr(A) + sqr(B)) );
  float ax, ay, bx, by;
  ax = X0 + B * mult;
  bx = X0 - B * mult;
  ay = Y0 - A * mult;
  by = Y0 + A * mult;

  float jointLocalX = ax; // или bx для другой точки пересечения
  float jointLocalY = ay; // или by для другой точки пересечения

  float hipPrimaryAngle  = polarAngle(jointLocalX, jointLocalY);
  float hipAngle = hipPrimaryAngle - hipStartAngle;
  
  float shinPrimaryAngle = polarAngle(x - jointLocalX, y - jointLocalY);
  float shinAngle = (shinPrimaryAngle - hipAngle) - shinStartAngle;

  if (legGroup % 2 == 0)
  {
    legWrite(hipAngle, shinAngle, 0);
    legWrite(hipAngle, shinAngle, 3);
    legWrite(hipAngle, shinAngle, 4);
    legWrite(hipAngle, shinAngle, 7);
  }
  else
  {
    legWrite(hipAngle, shinAngle, 1);
    legWrite(hipAngle, shinAngle, 2);
    legWrite(hipAngle, shinAngle, 5);
    legWrite(hipAngle, shinAngle, 6);
  }
}

void stepForward(float height, float deltaHeight, float xamp, float xshift)
{
  attachLegs();

  for (float i = 0; i < 200; i += 2.5)
  {
    float dx1 = + 10.0 - (float) i / 10;
    float dx2 = - 10.0 + (float) i / 10;

    float dhNormal = abs(dx1) / 10.0;
    float dh = deltaHeight * dhNormal;

    legsReachTo(dx1 * xamp + xshift, - height, 0);
    legsReachTo(dx2 * xamp + xshift, - (height - deltaHeight + dh), 1);

    delay(1);
  }

  for (float i = 0; i < 200; i += 2.5)
  {
    float dx1 = + 10.0 - (float) i / 10;
    float dx2 = - 10.0 + (float) i / 10;

    float dhNormal = abs(dx1) / 10.0;
    float dh = deltaHeight * dhNormal;

    legsReachTo(dx1 * xamp + xshift, - height, 1);
    legsReachTo(dx2 * xamp + xshift, - (height - deltaHeight + dh), 0);

    delay(1);
  }
  
  detachLegs();
}

void setup() 
{ 
  Serial1.begin(9600);

  attachLegs();
  resetLegs();
  delay(300);
  detachLegs();
} 

void loop() 
{
  float xamp = 1.5;
  float xshift = 5;
  
  float h = 66;
  float dh = 16;

  switch (state)
  {
    case FORWARD:
    case FORWARD_RIGHT:
    case FORWARD_LEFT:
      stepForward(h, dh, xamp, xshift); break;
    case BACKWARD:
      stepForward(h, dh, - xamp, xshift); break;
  }

  char command;
  while (Serial1.available())
    command = Serial1.read();
  switch (command)
  {
    case 'w':
      state = FORWARD; break;
    case 's':
      state = BACKWARD; break;
    case 'd':
      state = FORWARD_RIGHT; break;
    case 'a':
      state = FORWARD_LEFT; break;
    default:
      state = STOP;
  }
}

xakep.ru@