Skip to content

Глава 5. SCARA-манипулятор ​

5.1. Обзор SCARA-манипулятора ​

SCARA-манипуляторы представляют собой механизмы с рычажной кинематикой, рабочий орган которых перемещается в одной плоскости за счет вращательного движения рычагов, на конце которых он расположен. Как правило, такие манипуляторы состоят из двух рычагов, соединенных в одной точке и двух независимых приводов, один из которых находится в точке соединения рычагов и вращает их друг относительно друга. Второй же привод находится в точке крепления рычага к основанию и вращает его относительно рабочей плоскости. Внешний вид такого манипулятора представлен на рисунке 5.1:

Типичный промышленный SCARA-манипулятор

5.2. Обратная задача кинематики SCARA-манипулятора ​

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

image216

image217

Формализуем задачу: неподвижное основание и его движущиеся рычаги можно представить в виде отрезков, соединенных между собой шарнирами. Углы поворота рычагов робота обозначим как Θ1, Θ2 и Θ3, а координаты точек конца каждого рычага Х0, Х1, Х2 и Y0, Y1, Y2, где в реальности будут закреплены привода. Конечные координаты Х3 и Y3 нам известны. Это точка, в которую манипулятор должен прийти. Первый угол поворота рассматриваем от положительного направления оси Х до рычага. Последующие углы рассматриваются от предыдущего звена до рассматриваемого. Положительный угол поворота направлен против часовой стрелки.

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

Варианты положений рабочего органа манипулятора

image218

Возможность положения звеньев манипулятора при достижения конечной точки

В данном случае, механизм обладает избыточными степенями подвижности – движение звеньев может осуществляться при неподвижном схвате, что позволяет преодолевать внешние препятствия или проводить работы в замкнутых объемах. Поэтому для неподвижности конечного звена со схватом необходимо ввести еще одну координату γ – это угол, под которым конечное звено должно подойти к предмету относительно положительного направления оси Х. Таким образом, неопределенность будет возникать только между первым и вторым звеном манипулятора. Эта проблема решается программным способом.

Для решения задачи введем несколько ключевых параметров, которые определяются геометрическими размерами робота: длины звеньев l1, l2, l3; угол подхода схвата к объекту относительно оси Х, γ.

Решение задачи осуществляется в программе MathCad. Зададим размеры звеньев:

Укажем координаты, в которые должен прийти манипулятор, и угол, под которым он будет располагаться:

После того, как необходимые данные введены, можем начать решение.

Презентация1

Варианты положений рабочего органа манипулятора

Eqn35

Eqn34

  1. Определим координаты конца второго звена:
  2. Найдем координаты конца первого звена. Для этого составим систему уравнений двух окружностей. Центр координат первой окружности находится в начале системы координат (0,0), а её радиус равен длине первого звена - l1. Центр второй окружности находится в координатах конца второго звена (Х2, Y2), а её радиус равен l2. Найдем координаты пересечения этих двух окружностей. В результате получим квадратное уравнение и два корня. Приведем решение в программе MathCad:

В данном случае решение одинаковое, так как D = 0, а пересечение окружностей осуществляется лишь в одной точке.

  1. Теперь можно найти угол поворота первого звена для двух вариантов:
  2. Найдем координаты Y конца первого звена двух вариантов:
  3. Далее рассчитаем длину проекции второго звена на оси:

Eqn37

Eqn38

Eqn36

Eqn39

Eqn40

  1. Следующим шагом вычислим углы наклона проекции второго звена к оси Х:

Напомним, что данная функция определяется следующим образом:

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

  1. Определив разницу между углом наклона первого звена и второго, можно вычислить угол поворота второго звена относительно первого:
  2. Произведем расчёт для нахождения третьего угла поворота:
  3. Необходимо сделать преобразование координат, поскольку наша декартовая система отличается от системы координат робота. Это связано с тем, что 0° для сервоприводов находится в другом положении от нашего представления. Координатная плоскость робота представлена на рис. 5.5.

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

Eqn42

Eqn41

Eqn43

Eqn44

Координатная плоскость робота

Презентация2

5.3. Устройство SCARA-манипулятора ​

Инструкция по сборке робота, а также исходные файлы конструктивных элементов под лазерную резку находятся в свободном доступе и доступны для скачивания с официального сайта appliedrobotics.ru из раздела «Учебные материалы». Между каждым звеном установим три сервопривода. В качестве управляющего контроллера используется Arduino-подобная аппаратная платформа OpenCM, установленная на плате расширения 485 Expansion Board (также возможно использование платы STEM Board). Питание обеспечивается с помощью либо стандартного LiPo аккумулятора, выдающего номинальное напряжение в 11,1 В, либо через сетевой адаптер питания. Соединение сервоприводов рекомендуется выполнить последовательно и подключить получившуюся цепь к плате расширения 485 Expansion Board как на рис. 5.6.

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

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

Для уточнения и установки ID сервоприводов можно использовать стандартные примеры из Arduino IDE с установленной поддержкой платы OpenCM9.04. Для этого необходимо открыть два примера под названием: “o_Find_Dynamixel” и “c_ID_Change”. (Файл => Примеры => OpenCM9.04 => DynamixelWorkbench).

Screenshot_9

Схема подключения сервоприводов и их ID

С помощью примера “o_Find_Dynamixel” можно узнать текущее ID сервопривода следующим образом: сервопривод подключается к плате, подключается питание и загружается пример на плату. После чего в Мониторе порта можно увидеть информацию о текущем значении ID, которое необходимо запомнить.

После этого нужно открыть второй пример - “c_ID_Change”. В оглавлении, при необходимости, нужно изменить эти строки:

#define DXL_ID 1 // 1 – это текущее значение ID сервопривода

#define NEW_DXL_ID 2 // 2 – это значение, на которое требуется поменять ID

Помимо этого, в обоих случаях необходимо поменять скорость “общения” платы со входами Dynamixel

#define BAUDRATE 57600 // изменяем значение на 1 000 000

Таким образом, устанавливаются необходимые ID для сервоприводов.

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

5.4. Разработка управляющей программы ​

В модели данного робота в качестве управляющего контроллера используется Arduino-подобный контроллер OpenCM. Для работы с ним используется среда разработки Arduino IDE.

Для работы с Dynamixel подключаем библиотеку и инициализируем переменные:

cpp
#include <DynamixelWorkbench.h>
DynamixelWorkbench dxl_wb;

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

cpp
#define Device_Name  “1” – подключение к порту на плате OpenCM9.04
#define Device_Name “2”  – подключение через USB переходник или через Bluetooth
#define Device_Name  “3”  – подключение к плате расширения OpenCM 485 Expansion Board или STEM Board

В рассматриваемом примере требуется выбрать 3 вариант

Указываем скорость обмена данными через порт подключения

cpp
#define BAUDRATE  1000000

Далее необходимо указать ID сервоприводов, которыми необходимо управлять. Все это делается в коде программы до функции setup() при помощи следующих команд:

cpp
#define DXL_ID1    1
#define DXL_ID2    2
#define DXL_ID3    3
#define DXL_ID4    4    // Объявление ID сервоприводов манипулятора (1-4)

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

cpp
float l1 = 92;
float l2 = 66;
float l3 = 106;
float theta1, theta2, theta3;
float m, n, k, t;
float x11;

Функция setup()

Зададим переменные model_number и присвоим им ID сервоприводов. После этого производим подключение всех сервоприводов манипулятора.

Реализуем функцию инициализации:

cpp
dxl_wb.init(DEVICE_NAME, BAUDRATE, &log);
Serial.begin(1000000);
const char *log ;
uint16_t model_number1 = 1;
uint16_t model_number2 = 2;
uint16_t model_number3 = 3;
uint16_t model_number4 = 4;
dxl_wb.ping(DXL_ID1, &model_number1, &log);
dxl_wb.ping(DXL_ID2, &model_number2, &log);
dxl_wb.ping(DXL_ID3, &model_number3, &log);
dxl_wb.ping(DXL_ID4, &model_number4, &log);

Настроим режим работы сервоприводов для манипулятора

cpp
dxl_wb.jointMode(DXL_ID1, 50, 0, &log);
dxl_wb.jointMode(DXL_ID2, 50, 0, &log);
dxl_wb.jointMode(DXL_ID3, 50, 0, &log);
dxl_wb.jointMode(DXL_ID4, 50, 0, &log);

Где значение 50 является скоростью, а 0 – ускорение.

Теперь рассмотрим действие каждой функции отдельно.

Функция find_angle(float x, float y, float a)

В данной функции происходит решение обратной задачи. Входные данные:

x – координата схвата по оси Х

y - координата схвата по оси Y

a – угол относительно оси Х, под которым необходимо, чтобы схват подошел к объекту

cpp
{
  float x2 = x - l3*cos(a);  // Нахождение координат конца второго звена
  float y2 = y - l3*sin(a);
  float A = x2*x2 + l1*l1 + y2*y2 - l2*l2; // Введем параметр
  float D = (4*x2*A)*(4*x2*A) - 4*(-4*y2*y2 - 4*x2*x2)*(-(A*A) + 4*l1*l1*y2*y2); // Найдем дискриминант
  if(D < 0) // Проверка дискриминанта
  {
     Serial.print(«Error: D < 0»); // Если значение дискриминанта меньше нуля, манипулятор занимает базовое положение
     theta1 = 0;
     theta2 = 0;
     theta3 = 0;
  }

else
  {
  x11 = (-4*x2*A - sqrt(D))/(2*(-4*y2*y2-4*x2*x2));  // Нахождение корней уравнения
  float x12 = (-4*x2*A + sqrt(D))/(2*(-4*y2*y2-4*x2*x2));
  float fi11 = acos(x11/l1); // Нахождение первого угла первого варианта
  fi11 = 1.57 - fi11; // Преобразование первого угла первого варианта
  theta1 = fi11;
  if(abs(fi11) > 1.66) // Проверка на угол (сможет ли повернуться звено на заданный угол)
  {
  float fi12 = acos(x12/l1); // Первый корень не подошел, находим угол поворота второго варианта
  theta1 = 1.57 - fi12; // преобразование первого угла второго варианта
if(abs(fi12) > 1.66) // Если второй угол так же не подходит, манипулятор займет базовое положение
    {
       Serial.print(«Error: wrong destination «);
          theta1 = 0;
          theta2 = 0;
          theta3 = 0;
    }
else
{
    float y12 = sqrt(l1*l1 - x12*x12); // Расчет угла поворота для второго и третьего угла
    float x22 = x2 - x12;
    float y22 = y2 - y12;
    float t22 = atan2(y22,x22);
    float fi22 = t22 - theta1;
    if(fi22 < -4.62  ) // Решение может выдать поворот в обратную сторону, для этого найдем разницу от 360 градусов
  {
    fi22 = 6.28+fi22;
  }
    float fi32 = a - theta1 - fi22;
    if(fi32 < -4.62 )
  {
    fi32 = 6.28+fi32;
  }
   theta2 = -fi22; // преобразование второго угла второго варианта
   theta3 = -fi32; // преобразование второго угла второго варианта
 }
}
  еlse // Если первый корень подходит, расчет углов поворота второго и третьего звена
  {
  float y11 = sqrt(l1*l1 - x11*x11);
  float x21 = x2 - x11;
  float y21 = y2 - y11;
  float t21 = atan2(y21,x21);
  float fi21 = t21 - theta1;
   if(fi21 < -4.62  )
  {
    fi21 = 6.28+fi21;
  }
  float fi31 = a - theta1 - fi21;
  if(fi31 < -4.62 )
  {
    fi31 = 6.28+fi31;
  }
  fi21 = -fi21; // преобразование второго угла первого варианта
   theta2 = fi21;
  fi31 = -fi31; // преобразование третьего угла первого варианта
  theta3 = fi31;
}
  }
  if(abs(theta1) > 1,66 || abs(theta2) > 1,66 || abs(theta3) > 1,66)
// Проверка, чтобы не сломать физическую модель манипулятора.
// Угол поворота каждого звена не может превышать 95 градусов в каждую сторону. // Иначе манипулятор приходит в базовое положение
  {
    Serial.print(«Error: wrong destination. Angle bigger than 95 «);
          theta1 = 0;
          theta2 = 0;
          theta3 = 0;
  }
}

Функция set_pos()

В данной функции осуществляется задание угла поворота вала двигателя на манипуляторе

cpp
void set_pos( float p1, float p2,  float p3, float p3, int del) // Поворот задается в радианах, del – задержка
{
  dxl_wb.goalPosition(DXL_ID1,p1);
  dxl_wb.goalPosition(DXL_ID2,p2);
  dxl_wb.goalPosition(DXL_ID3,p3);
dxl_wb.goalPosition(DXL_ID4,p4);
  delay(del);
}

Функция loop()

Рассмотрим функцию loop(), которая представляет собой бесконечный цикл. В качестве примера зададим перемещение манипулятора в координаты х = 264; у = 0; а = 0;

cpp
void loop()
{
find_angle(264, 0, 0 ); // В этих координатах манипулятор будет параллелен и будет пересекать ось Х
set_pos(theta1, theta2, theta3, 0, 700); // Поворот валов приводов на вычисленный угол – достижение заданных координат
}

Также можно корректировать положения робота в реальном времени. С помощью консоли пользователь вручную может вводить координаты, которые будет занимать робот. Отредактируем функцию loop():

cpp
if (Serial.available() > 0 ) // Функция, проверяющая наличие символов, введенных в консоль
{
  m = Serial.parseInt(); // Присваивание глобальной переменной первое целочисленное значение – координату Х
  n = Serial.parseInt(); // Координата Y
  k = Serial.parseInt(); // Координата угла прихода схвата к объекту
  t = Serial.parseInt(); // Угол поворота схвата по оси Z
Serial.print(«X3 = « ); // Вывод координат, введённых пользователем
Serial.println(m);
Serial.print(«Y3 = « );
Serial.println(n);
Serial.print(«Alfa = « );
Serial.println(k);
Serial.print(«Angle fof gripping device = « );
Serial.println(t);
find_angle(m, n, k ); // Расчет углов поворота для введенных координат
Serial.print(«Angle #1: « ); // Вывод угла поворота первого двигателя
Serial.println(theta1);
Serial.print(«Angle #2: « );
Serial.println(theta2);
Serial.print(«Angle #3: « );
Serial.println(theta3);
set_pos(theta1, theta2, theta3, t, 700); ); // Поворот на вычисленный угол
}
}

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

В результате, код управляющей программы будет выглядеть следующим образом:

cpp
#include <DynamixelWorkbench.h>
#if defined(__OPENCM904__)
#define DEVICE_NAME «3» //Dynamixel on Serial3(USART3)  <-OpenCM 485EXP
#elif defined(__OPENCR__)
#define DEVICE_NAME «»
#endif
#define BAUDRATE  1000000 // Скорость передачи данных
#define DXL_ID1    1 //адрес сервопривода DYNAMIXEL
#define DXL_ID2    2
#define DXL_ID3    3
#define DXL_ID4    4
DynamixelWorkbench dxl_wb;
float theta1, theta2, theta3;
float m, n, k, t;
float l1 = 92;
float l2 = 66;
float l3 = 106;
float x11;
void set_pos( float p1 , float p2, float p3, float p4, int del)
// Функция для задания угла поворота вала двигателя на манипулятора в радианах
{
  dxl_wb.goalPosition(DXL_ID1,p1);
  dxl_wb.goalPosition(DXL_ID2,p2);
  dxl_wb.goalPosition(DXL_ID3,p3);
  dxl_wb.goalPosition(DXL_ID4,p4);
  delay(del);
}
void find_angle(float x, float y, float a)
{
  float x2 = x - l3*cos(a);
  float y2 = y - l3*sin(a);
  float A = x2*x2 + l1*l1 + y2*y2 - l2*l2;
  float D = (4*x2*A)*(4*x2*A) - 4*(-4*y2*y2 - 4*x2*x2)*(-(A*A) + 4*l1*l1*y2*y2);
  if(D < 0)
  {
     Serial.print(«Error: D < 0»);
     theta1 = 0;
     theta2 = 0;
     theta3 = 0;
  }
   else
  {
  x11 = (-4*x2*A - sqrt(D))/(2*(-4*y2*y2-4*x2*x2));
  float x12 = (-4*x2*A + sqrt(D))/(2*(-4*y2*y2-4*x2*x2));
  float fi11 = acos(x11/l1);
  fi11 = 1.57 - fi11; // преобразование первого угла первого варианта
  theta1 = fi11;
  if(abs(fi11) > 1.66)
  {
  float fi12 = acos(x12/l1);
  theta1 = 1.57 - fi12; // преобразование первого угла второго варианта
   if(abs(fi12) > 1.66)
    {
       Serial.print(«Error: wrong destination «);
          theta1 = 0;
          theta2 = 0;
          theta3 = 0;
    }

  else
{
    float y12 = sqrt(l1*l1 - x12*x12);
    float x22 = x2 - x12;
    float y22 = y2 - y12;
    float t22 = atan2(y22,x22);
    float fi22 = t22 - theta1;
    if(fi22 < -4.62  )
  {
    fi22 = 6.28+fi22;
  }
    float fi32 = a - theta1 - fi22;
    if(fi32 < -4.62 )
  {
    fi32 = 6.28+fi32;
  }
   theta2 = -fi22; // преобразование второго угла второго варианта
   theta3 = -fi32; // преобразование второго угла второго варианта
 }
  }
  else
  {
  float y11 = sqrt(l1*l1 - x11*x11);
  float x21 = x2 - x11;
  float y21 = y2 - y11;
  float t21 = atan2(y21,x21);
  float fi21 = t21 - theta1;
   if(fi21 < -4.62  )
  {
    fi21 = 6.28+fi21;
  }
  float fi31 = a - theta1 - fi21;
  if(fi31 < -4.62 )
  {
    fi31 = 6.28+fi31;
  }
  fi21 = -fi21; // преобразование второго угла первого варианта
   theta2 = fi21;
  fi31 = -fi31; // преобразование третьего угла первого варианта
  theta3 = fi31;
}
  }
  if(abs(theta1) > 1,66 || abs(theta2) > 1,66 || abs(theta3) > 1,66)
  {
    Serial.print(«Error: wrong destination. Angle bigger than 95 «);
          theta1 = 0;
          theta2 = 0;
          theta3 = 0;
  }
}
void setup() {
  const char *log ;
  Serial.begin(57600);

uint16_t model_number1 = 1;
uint16_t model_number2 = 2;
uint16_t model_number3 = 3;
uint16_t model_number4 = 4;
dxl_wb.init(DEVICE_NAME, BAUDRATE, &log);
dxl_wb.ping(DXL_ID1, &model_number1, &log);
dxl_wb.ping(DXL_ID2, &model_number2, &log);
dxl_wb.ping(DXL_ID3, &model_number3, &log);
dxl_wb.ping(DXL_ID4, &model_number4, &log);
dxl_wb.jointMode(DXL_ID1, 50, 0, &log);
dxl_wb.jointMode(DXL_ID2, 50, 0, &log);
dxl_wb.jointMode(DXL_ID3, 50, 0, &log);
dxl_wb.jointMode(DXL_ID4, 50, 0, &log);
}
void loop() {
 if (Serial.available() > 0 ) {
  m = Serial.parseInt();
  n = Serial.parseInt();
  k = Serial.parseInt();
  t = Serial.parseInt();
Serial.print(«X3 = « );
Serial.println(m);
Serial.print(«Y3 = « );
Serial.println(n);
Serial.print(«Alfa = « );
Serial.println(k);
Serial.print(«Angle fof gripping device = « );
Serial.println(t);
find_angle(m, n,k );
Serial.print(«Angle #1: « );
Serial.println(theta1);
Serial.print(«Angle #2: « );
Serial.println(theta2);
Serial.print(«Angle #3: « );
Serial.println(theta3);
set_pos(theta1, theta2, theta3, t, 700);
}
}

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