Разбор мат-модели четырехногого робота-«паука»

Хотел бы разобрать код открытого проекта робота-«паука».

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

Первое знакомство и основные характеристики

Есть несколько версий данного робота.

В частности я наткнулся на одну из модификаций, ища что бы такого напечатать на свежекупленном 3D-принтере.

Основной репозиторий исходного проекта располагается на GitHub автора, но там нет некоторых деталей. В частности там нет кода, отвечающего за выравнивание сервоприводов перед затяжкой винтов.

Инструкции по сборке и первоначальной настройке можно найти по ссылке: http://www.instructables.com/id/DIY-Spider-RobotQuad-robot-Quadruped/ (есть проблемы с отображением страницы — CDN попали под блокировки, копия здесь)

Характеристики робота:

• 4 «ноги»

• Каждая «нога» состоит из трех сервоприводов типа SG90/MG90

• В качестве «мозга» — Arduino Nano(но можно использовать любую совместимую с Arduino плату)

В качестве дополнений существует версии кода с удаленным управлением по Bluetooth.

Так как сами сервоприводы в пике могут потреблять значительную мощность, то крайне желательно использовать мощный DC-DC преобразователь и контроллер заряда аккумуляторов.

Я же на свой страх и риск использую готовую плату, которая может и не выдержать такого надругательства.

Скелеты и кости

Можно сказать что робот симметричен относительно своей оси.

Поэтому можно рассмотреть только одну «лапу»:

• coxa — «сустав», вращает ногу вокруг оси Z, тем самым задает плоскость, в которой может перемещаться остальная часть

• femur — средняя «кость»

• tibia — опорная кость, в некоторых роботах отсутствует

Начальное положение костей относительно нейтральных позиций сервоприводов:

Разгребая код

Структурно код выглядит стандартно для большинства «скетчей» — масса глобальных переменных, функции setup и loop, и т.д.

Из нестандартных библиотек используется FlexiTimer2 и Servo

Блок переменных и констант требует дополнительных комментариев:

//define servos' ports/// 4 ноги по три сервы/// Тут ВНИМАНИЕ: каждая нога апределена в поряде: {femur, tibia, coxa}/// Что несколько противоречит ожидаемому {coxa, femur, tibia}/// И это нужно учитывать при подключении серв к платеconst int servo_pin[4][3] = { {2, 3, 4}, {5, 6, 7}, {8, 9, 10}, {11, 12, 13} };/* Size of the robot ---------------------------------------------------------*/// Длина femurconst float length_a = 55;// Длина tibiaconst float length_b = 77.5;// Длина coxaconst float length_c = 27.5;// ???const float length_side = 71;// Смещение низа корпуса от оси соединения coxa-femurconst float z_absolute = -28;/* Constants for movement ----------------------------------------------------*/const float z_default = -50, z_up = -30, z_boot = z_absolute;const float x_default = 62, x_offset = 0;const float y_start = 0, y_step = 40;const float y_default = x_default;/* variables for movement ----------------------------------------------------*/// Текущее РАССЧИТАННОЕ положениеvolatile float site_now[4][3];    //real-time coordinates of the end of each leg// Текущее заданное положениеvolatile float site_expect[4][3]; //expected coordinates of the end of each leg// дельты, на которые сервы смещаются каждый тикfloat temp_speed[4][3];   //each axis' speed, needs to be recalculated before each movement

Основные функции, отвечающие за передвижение робота:

• set_site — установка целевого положения опоры для конкретной «лапы», расчет скоростей

• wait_all_reach — ожидание достижения установленных точек — в цикле сравнивает текущее и заданное положение

• servo_service — наиболее интересная функция, отвечает за пересчет координат в углы поворота сервоприводов

В set_site есть баг: при отсутствии дельте перемещения равной нулю в скорости получается Infinity. Но «стреляет он только на платформах с аппаратной float-математики. На ATMega этого нет и используется программная реализация и вероятно она чем то отличается от поведения аппаратной.

Самое вкусное

Самое интересное находится в функции servo_service, вызываемой с частотой 50Hz

#define E_DELTA 0.01void servo_service(void){  sei();  static float alpha, beta, gamma;  // для каждой ноги  for (int i = 0; i < 4; i++)  {    // для каждй сервы    for (int j = 0; j < 3; j++)    {      // определить следующее положение      if (abs(site_now[i][j] - site_expect[i][j]) < (abs(temp_speed[i][j])+E_DELTA))        site_now[i][j] = site_expect[i][j];      else        site_now[i][j] += temp_speed[i][j];    }    // перевести декартовы координаты в углы поворота    cartesian_to_polar(alpha, beta, gamma, site_now[i][0], site_now[i][1], site_now[i][2]);    // установить углы серв, скорректировав их в зависимости от номера ноги    polar_to_servo(i, alpha, beta, gamma);  }  rest_counter++;}

А точнее в cartesian_to_polar:

void cartesian_to_polar(volatile float &alpha, volatile float &beta, volatile float &gamma, volatile float x, volatile float y, volatile float z){  //calculate w-z degree  float v, w;  w = (x >= 0 ? 1 : -1) * (sqrt(pow(x, 2) + pow(y, 2)));  v = w - length_c;  alpha = atan2(z, v) + acos((pow(length_a, 2) - pow(length_b, 2) + pow(v, 2) + pow(z, 2)) / 2 / length_a / sqrt(pow(v, 2) + pow(z, 2)));  beta = acos((pow(length_a, 2) + pow(length_b, 2) - pow(v, 2) - pow(z, 2)) / 2 / length_a / length_b);  //calculate x-y-z degree  gamma = (w >= 0) ? atan2(y, x) : atan2(-y, -x);  //trans degree pi->180  alpha = alpha / pi * 180;  beta = beta / pi * 180;  gamma = gamma / pi * 180;}

Комментарии и названия переменных мало что дают. Лично я убил несколько дней на разбор этого участка кода.

По шагам:

1. Рассчитывается длина вектора w (от центра координат до целевой точки в плоскости XY)

2. «Виртуально» перемещаемся в плоскость, задаваемую вектором w и осью Z.

3. На эту плоскость проецируется положение костей

4. Смещаем центр координат в соединение coxa-femur

5. После чего все сводится к теореме косинусов, часто используемой в инверсной кинематике

И становится понятно назначение переменных:

• alpha — угол между coxa и femur

• beta — угол между femur и tibia

• gamma — угол поворота coxa

P.S.:

Разобравшись, как что то работает можно изменять и/или создавать свое на похожих принципах. Если подумать, то у тех же роботов собак получается, что перемещение лап сводится к тому же треугольнику с тремя известными сторонами….

В разборе кода очень помогла статья «Инверсная кинематика в 2D»