Skip to content

Глава 3. Угловой манипулятор ​

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

3.1. Подготовка к сборке ​

Подготовка к сборке манипулятора с угловой кинематикой выполняется аналогичным образом, как и подготовка к сборке паллетайзера, которая была рассмотрена в учебном пособии «СТЕМ Мастерская. Часть 1». Исключение заключается только в том, что в случае с паллетайзером использовалось 5 сервоприводов DYNAMIXEL AX-12A, а в текущем случае их необходимо подготовить 6 штук. После последовательной настройки ID сервоприводов необходимо установить во все привода гайки по приведенной на рисунке 3.1 схеме.

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

image144

Схема закладки гаек в сервопривода

image145

Пример закладки гаек в пазы сервопривода

Допускается не использовать некоторые из пазов (как это показано на схеме) и не вставлять в них гайки (например, на 5 и 6 приводах). Однако, в этом случае рекомендуется задействовать хотя бы по 6 пазов с каждой стороны.

После подготовки можно приступать к сборке.

3.2. Сборка манипулятора ​

Для сборки напечатанного на 3D принтере манипулятора с угловой кинематикой необходимы следующие комплектующие:

  • Комплект винтов М2х5
  • Комплект винтов М2х10 (или длиннее)
  • Комплект гаек М2
  • Комплект винтов М3х15
  • Комплект винтов М3х10
  • Комплект гаек М3
  • Комплект шайб М3
  • Комплект соединительных кабелей
  • Сервоприводы DYNAMIXEL AX-12A (6 шт)
  • Элементы конструкции

На рисунке 3.3 представлены фото всех использующихся элементов конструкции.

Во время сборки важно обращать внимание на следующее:

  1. Совмещенность одинарных рисок на корпусе и выходном валу приводов. Если риски не будут совмещены, то придется проводить пересборку на этапе программирования, так как манипулятор не сможет корректно отрабатывать некоторые действия и команды (Рис. 3.4).
  2. Длина винтов, используемых для сборки.

Винты, которые будут вворачиваться в гайки (которые ранее были вставлены в пазы), можно брать любой длины. Но для крепления конструкции к отверстиям в выходном валу приводов ОБЯЗАТЕЛЬНО использовать самые короткие винты – M2x5 (М2х6). В противном случае, винт пройдет насквозь, что может повредить корпус, а также приведет к заклиниванию выходного вала и к возможной поломке привода.

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

image146

image147

Все необходимые конструктивные элементы для сборки манипулятора

image151

Соотношение длины винта и толщины фланца сервопривода

image148

ОК НЕ ОК НЕ ОК

Совмещение рисок на корпусе и фланце сервопривода

Порядок сборки:

Сборку до пункта 16 необходимо проводить с помощью винтов и гаек М2.

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

image152

image149

image153

image150

image154

  1. Затем, нужно присоединить второй сервопривод (при помощи восьми винтов):
  2. Далее присоедините представленное на изображении ниже звено ко второму сервоприводу (при помощи четырех коротких винтов)

image156

image155

image157

image158

  1. Следующим шагом нужно присоединить третий сервопривод к предыдущему звену, как это продемонстрировано на рисунке ниже.

image160

image159

image161

image162

  1. Далее присоедините звено ко 3-му сервоприводу (при помощи четырех коротких винтов)
  2. А к этому звену – 4-ый сервопривод
  3. После чего соедините 5-ый сервопривод и представленное на изображении ниже звено. Следует обратить внимание, что в этом случае не используются 2 отверстия, однако, на качество соединения это не влияет.

image163

image164

image165

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

image167

image169

image168

image166

  1. А после этого, присоедините 6-ой сервопривод к этому звену
  2. Затем нужно присоединить к 6-му сервоприводу еще одно звено, представленное на изображении ниже
  3. Следующим шагом нужно собрать конструкцию схвата

image172

image170

image173

image171

  1. А затем, присоедините элементы схвата ко всей конструкции

image178

image176

image174

image175

image177

image179

  1. После чего все приводы нужно соединить последовательно при помощи соединительных кабелей

Результат сборки представлен на изображении 3.5:

Следует обратить внимание, что из 1-го привода будет выходить один дополнительный кабель для последующего подключения манипулятора к плате STEM Board 2, а к 6-му приводу будет подключен только один кабель.

  1. После этого необходимо закрепить соединения конструкции, присоединив втулки к сторонам, противоположным выходному валу сервоприводов 2, 3 и 4, используя винты и шайбы М3.

image180

image182

Результат сборки углового манипулятора

image183

image181

image184

  1. И, наконец, необходимо присоединить основание ко всей конструкции

На этом сборка модели манипулятора с угловой кинематикой закончена.

image186

3.3. Расчеты ​

3.3.1. Максимальная масса груза ​

Для понимания возможностей манипулятора необходимо провести оценку его грузоподъемности. В рассматриваемой модели манипулятора применяются сервоприводы DYNAMIXEL AX-12A. Их технические характеристики известны. Зная максимальный момент, который может выдать такой сервопривод, а также длины элементов конструкции модели, можно сделать расчет максимальной массы нагрузки, которую сможет поднять манипулятор.

В документации на сервоприводы можно найти, что максимальный момент, при котором сервоприводы способны к плавному и точному перемещению нагрузки – 0,3 Н∙м, а максимальный момент, который могут удерживать сервоприводы – 1,5 Н∙м.

Используя эти данные, рассчитаем массу полезной нагрузки для 2-го сервопривода, поскольку он является самым нагруженным из всех сервоприводов манипулятора. Второму сервоприводу помимо массы груза также требуется перемещать массу всей конструкции манипулятора, которая расположена после него – то есть, 5 звеньев и 4 привода. Расчет будем проводить для самой нагруженной конфигурации манипулятора второго сервопривода: когда он выпрямлен, и вся конструкция, начиная со второго привода, находится в горизонтальном положении (Рис. 3.6):

В этом случае, момент нагрузки на второй сервопривод можно найти по формуле:

Значит, максимальный момент, с которым может справиться манипулятор, с учетом собственной конструкции, можно вычислить, приняв максимальной.

С помощью линейки можно примерно найти длины звеньев:

Также, используя весы, можно определить массы всех элементов конструкции:

image185

image187

Eqn2

Eqn5

Максимально нагруженное положение манипулятора

Eqn3

Eqn4

image190

Учитывая, что плечо груза можно принять равным длине манипулятора в выпрямленной конфигурации от 2 привода до краев захвата, то плечо груза

А значит, максимальная масса груза, которую может поднять манипулятор, равна

однако, нормальное оперирование таким грузом будет невозможно.

В реальности, сервопривод может уверенно работать с грузами, массой в 5 раз меньше, то есть, максимально допустимая масса груза

(однако, в этом вопросе стоит также учитывать качество печати пластиковой конструкции манипулятора, которой при определенных режимах плотности печати может не хватить жесткости для выдерживания такой нагрузки).

3.3.2. Обратная задачи кинематики ​

Рассмотрим решение обратной задачи кинематики (ОЗК) для манипулятора с угловой кинематикой. Для исключения избыточных случаев можно принять положение выходного звена манипулятора вертикальным, при этом, захват будет все время находиться в вертикальном положении. В этом случае, ОЗК будет решаться однозначно.

Манипулятор можно условно разделить на 4 линейных сегмента с соответствующими длинами . Причем, на первом и четвертом звеньях находятся сервоприводы, дающие вращательную степень свободы вдоль продольной оси звена (Рис. 3.7).

Зеленым цветом показаны звенья манипулятора. Крестиками отмечены продольные степени вращения.

При данных условиях (звено 1 имеет только продольную степень вращения, расположено вертикально, звено 4 расположено вертикально, а 5-ый сервопривод также дает вертикальную степень вращения) можно абстрагироваться от всех звеньев манипулятора, и рассматривать только 2 и 3 звенья, перенеся ось координат в точку стыковки 1 и 2 звеньев, получив в результате ранее рассмотренный случай задачи кинематики (Рис.3.9).

Eqn7

Eqn8

Eqn9

Eqn10

Eqn11

Eqn12

image191

Схема для решения обратной задачи кинематики

image192

Схема разбиения манипулятора на звенья

Eqn13

image193

Следует обратить внимание на наличие вспомогательных отрезов d и k.

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

После этого можно получить уравнения для решения ОЗК:

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

Eqn15

Eqn14

3.4. Программирование ​

Для программирования манипулятора с угловой кинематикой используется то же самое программное обеспечение и такой же набор аппаратных средств, которые использовались для программирования манипулятора с плоско-параллельной кинематикой, рассматриваемого в учебном пособии «СТЕМ Мастерская. Часть 1»

3.4.1. Чтение позиций сервоприводов ​

Для чтения позиций всех сервоприводов манипулятора с угловой кинематикой можно использовать следующий пример:

cpp
#include <Dynamixel2Arduino.h>  // подключение библиотеки Dynamixel
#define DXL_SERIAL
#define DEBUG_SERIAL Serial  // Установка константы, отвечающей за последовательный порт, подключаемый к компьютеру
const uint8_t DXL_DIR_PIN = 22; // инициализация переменной, отвечающей за номер пина, подключенного к информационному пину сервоприводов манипулятора
const float DXL_PROTOCOL_VERSION = 1.0;   // инициализация переменной, отвечающей за протокол передачи данных от OpenCM9.04 к сервоприводам
Dynamixel2Arduino dxl(DXL_SERIAL, DXL_DIR_PIN); // инициализация указателя на команды из библиотеки Dynamixel
#define jointN 6           // инициализация константы, обозначающей количество сервоприводов (и, соответственно, элементов массива)
int pos=0;                 // инициализация переменной pos
int i=0;                   // инициализация переменной i
int buf[jointN+1];         // инициализация одномерного массива, его размер задается 6+1=7, так как в программировании нумерация элементов начинается с 0, но нулевой элемент мы использовать не будем, в связи с чем начнем с первого
void setup() {

DEBUG_SERIAL.begin(57600); // установка скорости обмена данными по последовательному порту компьютера
dxl.begin(1000000);        // установка скорости обмена данными по последовательному порту манипулятора
dxl.setPortProtocolVersion(DXL_PROTOCOL_VERSION);   // выбор протокола обмена данными
for (i=1; i<jointN+1; i++)   // цикл с изменением i от 1 до 6 с шагом 1
{
dxl.setOperatingMode(i, OP_POSITION);  // установка режима работы сервопривода с номером i в качестве шарнира
}
}
void loop() {
for (i=1; i<=jointN; i++) // цикл с изменением i от 1 до 6 с шагом 1
{
pos = dxl.getPresentPosition(i);  // получение текущей позиции привода с номером i и запись в переменную pos
buf[i]=pos;  // запись переменной значения pos в i-тый элемент массива buf
//Serial.println(buf[i]);
dxl.torqueOff(i);   // отключение блокировки привода с номером i
}
Serial.print(«{0, «); //вывод в монитор порта текста «{0, «
for (i=1; i<=jointN; i++) // цикл с изменением i от 1 до 6 с шагом 1
{
Serial.print(buf[i]); // вывод в монитор порта значения ячейки массива с номером i
Serial.print(«, «); // вывод в монитор порта текста «, «
}
Serial.print(«}, «); // вывод в монитор порта текста «}, «
Serial.println(‘ ‘); // вывод в монитор порта текста « « и перенос курсора на следующую строку
delay(1000);          // задержка, пауза в 1 секунду
}

Данные после считывания будут выводиться на экран (в монитор порта) в заданном формате.

3.4.2. Воспроизведение записанных позиций ​

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

cpp
#include <Dynamixel2Arduino.h>
#define DXL_SERIAL
#define DEBUG_SERIAL Serial  // Установка константы, отвечающей за последовательный порт, подключаемый к компьютеру
const uint8_t DXL_DIR_PIN = 22; // OpenCM9.04 EXP Board’s DIR PIN. (To use the DXL port on the OpenCM 9.04 board, you must use 28 for DIR PIN.)
const float DXL_PROTOCOL_VERSION = 1.0; // инициализация переменной, отвечающей за протокол передачи данных от OpenCM9.04 к сервоприводам
Dynamixel2Arduino dxl(DXL_SERIAL, DXL_DIR_PIN); // инициализация указателя на команды из библиотеки Dynamixel
#define jointN 6 // задание количества ненулевых столбцов массива
#define pages 3 // задание количества ненулевых строк массива
int i=0;                    // задание переменной-счетчика i
int j=1;                    // задание переменной-счетчика j
int buf[pages+1][jointN+1]={  // инициализация двумерного массива, его размер задается: 3+1=4 на 6+1=7, 4 на 7, так как в программировании нумерация элементов начинается с 0, но нулевой элемент мы использовать не будем, в связи с чем начнем с первого
{0,0,  0,  0,  0,  0,  0},
{0, 507, 492, 531, 498, 521, 626, },
{0, 506, 443, 403, 385, 607, 626, },
{0, 508, 532, 627, 613, 412, 626, },
};
void setup() {
DEBUG_SERIAL.begin(57600); // установка скорости обмена данными по последовательному порту компьютера
dxl.begin(1000000);       // установка скорости обмена данными по последовательному порту манипулятора
dxl.setPortProtocolVersion(DXL_PROTOCOL_VERSION); // выбор протокола обмена данными
for (i=1; i<jointN+1; i++)   // цикл с изменением i от 1 до 6 с шагом 1
{
dxl.setOperatingMode(i, OP_POSITION); // установка режима работы привода с номером i в качестве шарнира
}
}
void loop() {
for (j=1;j<=pages;j++)  // цикл с изменением j от 1 до 6 с шагом 1
{
for(i=1;i<=jointN;i++) // цикл с изменением i от 1 до 6 с шагом 1
{
dxl.setGoalVelocity(i, 50);         // задание целевой скорости 50 сервоприводу с номером i
dxl.setGoalPosition(i, buf[j][i]);  // задание целевого положения, получаемого из элемента ([j],[i]) сервоприводу с номером i
}
delay(5000);
}
}

Обратите внимание, в этой программе задается двумерный массив, в котором записаны положения сервоприводов. Построчно записаны различные положения манипулятора, а по столбцам – отдельные положения каждого из сервоприводов. Поэтому массив имеет 6 столбцов и переменное количество строк, которое зависит от количества воспроизводимых положений (на самом деле, число строк и столбцов на 1 больше, так как есть неиспользуемые нулевые строка и столбец; они задаются нулевыми).

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

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

Считывание и вывод на экран позиций (используется программа для чтения позиций).

Затем человек вручную задает некоторую позицию манипулятора (поворачивая манипулятор рукой и придерживая его при этом). В монитор порта через каждую секунду выводится текущая конфигурация манипулятора. Когда значения построчно начинают повторяться, то есть манипулятор вручную установлен в целевую позицию, в мониторе порта необходимо выключить автопрокрутку, а затем, выделив курсором одну из нужных строчек (обозначающих целевую позицию), скопировать эту строку (используя связку горячих клавиш Ctrl+C). Далее эту строку необходимо будет вставить в программу для воспроизведения позиций.

Эти действия нужно повторять до тех пор, пока все позиции для воспроизведения не будут записаны.

Воспроизведение позиций.

Когда все позиции считаны и записаны в массив, необходимо убедиться, что программа работает корректно. Это можно сделать, проведя компиляцию. Также необходимо следить за тем, чтобы переменная, отвечающая за количество строк в массиве, была задана верно – под именно текущее количество строк (а оно зависит от пользователя).

Таким образом, предварительно «научив» манипулятор переносить какой-либо предмет (последовательно задавая все положения манипулятора, необходимые для переноса предмета: начальное, наведение на предмет, подход к предмету, схват, перенос, опускание предмета, отпускание предмета), можно добиться того, что манипулятор будет воспроизводить позиции, которым он ранее научился.