Глава 5. SCARA-манипулятор
5.1. Обзор SCARA-манипулятора
SCARA-манипуляторы представляют собой механизмы с рычажной кинематикой, рабочий орган которых перемещается в одной плоскости за счет вращательного движения рычагов, на конце которых он расположен. Как правило, такие манипуляторы состоят из двух рычагов, соединенных в одной точке и двух независимых приводов, один из которых находится в точке соединения рычагов и вращает их друг относительно друга. Второй же привод находится в точке крепления рычага к основанию и вращает его относительно рабочей плоскости. Внешний вид такого манипулятора представлен на рисунке 5.1:
Типичный промышленный SCARA-манипулятор
5.2. Обратная задача кинематики SCARA-манипулятора
Для запуска манипулятора необходимо научиться решать обратную (инверсную) кинематическую задачу. Исходя из известной позиции, в которую требуется переместить манипулятор робота (например, схватить объект, который находится на конвейере в точке с координатами x, y) требуется определить величины углов, на которые необходимо повернуть двигатели, связанные с рычагами робота, чтобы установить его в правильное положение для захвата. Примеры положений звеньев манипулятора при достижении рабочим органом требуемого положения представлены на рисунке 5.2


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

Возможность положения звеньев манипулятора при достижения конечной точки
В данном случае, механизм обладает избыточными степенями подвижности – движение звеньев может осуществляться при неподвижном схвате, что позволяет преодолевать внешние препятствия или проводить работы в замкнутых объемах. Поэтому для неподвижности конечного звена со схватом необходимо ввести еще одну координату γ – это угол, под которым конечное звено должно подойти к предмету относительно положительного направления оси Х. Таким образом, неопределенность будет возникать только между первым и вторым звеном манипулятора. Эта проблема решается программным способом.
Для решения задачи введем несколько ключевых параметров, которые определяются геометрическими размерами робота: длины звеньев l1, l2, l3; угол подхода схвата к объекту относительно оси Х, γ.
Решение задачи осуществляется в программе MathCad. Зададим размеры звеньев:
Укажем координаты, в которые должен прийти манипулятор, и угол, под которым он будет располагаться:
После того, как необходимые данные введены, можем начать решение.

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


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





- Следующим шагом вычислим углы наклона проекции второго звена к оси Х:
Напомним, что данная функция определяется следующим образом:
Т.е. она учитывает принадлежность аргументов одному из четырех квадрантов.
- Определив разницу между углом наклона первого звена и второго, можно вычислить угол поворота второго звена относительно первого:
- Произведем расчёт для нахождения третьего угла поворота:
- Необходимо сделать преобразование координат, поскольку наша декартовая система отличается от системы координат робота. Это связано с тем, что 0° для сервоприводов находится в другом положении от нашего представления. Координатная плоскость робота представлена на рис. 5.5.
Таким образом, обратная задача кинематики была решена. Теперь необходимо разработать систему управления манипулятором с использованием управляющего микроконтроллера.




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

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).

Схема подключения сервоприводов и их 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.