В центре блок-схемы
находится основная подпрограмма, которая содержит в себе все режимы управления
манипулятором. При управлении поворотами звеньев робота решается прямая задача
для определения координат рабочей точки, при переключении на режим управления
координатами, эти координаты используются чтобы продолжить двигаться роботу с
того же места, при этом решается обратная задача, чтобы определить углы
поворотов звеньев. Данные об углах звеньев обрабатываются и посылаются в COM порт в виде строки. При программном управлении в командах
программы считываются координаты точки, далее по этим координатам решается
обратная задача и определяются углы поворотов звеньев, к которым робот
стремится повернуть свои звенья, чтобы достичь заданной точки.
7. Тестирование
работы манипулятора
В ходе тестирования работы манипулятора проверяются стабильность работы компьютерной программы и её связь с платой Arduino, наличие соединения сервоприводов с Arduino и подключение внешнего питания, работа манипулятора согласно заданной программы движения.
Сначала подключаются сервоприводы к Arduino согласно схеме на рисунке 6.1, далее Arduino подключается по COM порту к компьютеру. При подачи питания на сервоприводы манипулятор становится в начальное положение.
При запуске компьютерной программы управления манипулятором, она сразу определяет подключенный COM порт: выводится сообщение «COM порт подключен!». Для проверки правильности подключения сервоприводов к Arduino в ручном режиме управления проверяется каждое звено по отдельности, т.е. при нажатии кнопки должен отреагировать соответствующий сервопривод. Все сервоприводы подключены верно. Для проверки стабильности программы внезапно отключаем COM порт Arduino от компьютера, программа вывела сообщение «COM порт не подключен!», робот остался в том же положении в каком и был и никаких действий самостоятельно не производит. После обратного подключения COM порта нажимаю кнопку «Поиск COM портов» и программа тут же находит этот COM порт и подключается к нему, теперь возможно дальнейшее управление манипулятором. При ручном управлении манипулятором по осям координат, рабочая точка передвигается строго вдоль оси, по которой её передвигаю. Все звенья работают синхронно, т.к. для каждого нового движения просчитывается обратная задача кинематики.
При движении манипулятора заметны некоторые небольшие резкие рывки сервоприводов что приводит к колебанию всей конструкции манипулятора. Это происходит из за того, что сервоприводы работают не с достаточно высокой точностью и на малые углы поворота (например в 1 градус) могут не отреагировать. При этом программа Arduino поворачивает сервоприводы с шагом в 0.01 градуса, т.е. с программной точки зрения движение должно происходить плавно.
Далее выполняется проверка
программного управления. Подвожу в ручную положение рабочей точки манипулятора
в нужную позицию и нажимаю кнопку «Добавить положение», данная позиция
сохраняется. Таким образом создаётся вся программа целиком и добавляю команды
задержки в некоторых местах. Нажимаю кнопку «Запуск», манипулятор начинает
движение проходя через все запрограммированные точки и ожидает заданное
количество времени на командах задержки, при движении манипулятора есть
возможность регулировать скорость его движения в реальном времени.
Заключение
В ходе выполнения работы была построена кинематическая схема трёхстепенного манипулятора, в которой использовался параллельный механизм для того чтобы схват манипулятора оставался параллельно основанию. Работа данной конструкции была смоделирована на динамической модели манипулятора где подтвердилась её работоспособность.
Также была построена 3D модель манипулятора, с учётом всех длин и размеров деталей, для определения её конструктивных ограничений, а именно диапазонов углов поворотов звеньев. Эти углы определили форму рабочей зоны, максимальный радиус которой получился 570 мм от центра основания робота. Рабочая зона строилась при помощи динамической модели манипулятора, в которой манипулятор прошёлся через все крайние положения рабочей зоны.
В динамической модели были определены зависимости различных велечин таких как: сила, момент, угловая скорость и угловое ускорение, которые действовали на оси поворотов звеньев и основания. Также было смоделировано как вращающий момент, который развивают реальные сервоприводы, действовал на движение звеньев манипулятора. Этот же момент использовался для определения грузоподъёмности манипулятора, которая составила 150 грамм при максимально вытянутой руке манипулятора.
На основе спроектированной трёхмерной модели были изготовлены детали для схвата, основания и ещё некоторые элементы манипулятора при помощи 3D печати из пластика и металлические детали звеньев, которые создавались вручную.
Была разработана система управления манипулятором на основе платы Arduino Uno, которая получает сигналы управления с компьютера. Компьютерная программа позволяет управлять манипулятором в различных режимах как вручную - управление каждым звеном манипулятора по отдельности или передвижением рабочей точки по осям координат, так и управление в автоматическом режиме, когда программируются различные позиции, по которым должен пройти манипулятор.
Проверка работоспособности манипулятора показала, что робот отрабатывает точно все движения, которые ему задаются в программе. При этом были заметны некоторые небольшие рывки при движении конструкции манипулятора, это происходит из-за особенностей сервоприводов. Тестирование работы манипулятора позволило определить его следующие параметры: повторяемость, которая в среднем составляла 2 мм; реальная грузоподъёмность, равная от 50 до 100 грамм в зависимости от длины плеча, что является меньше чем при моделировании, т.к. реальная масса робота оказалась немного больше ожидаемой; рабочая зона, которая получилась такая же как и при моделировании.
Так же были получены новые знания и навыки построения 3D моделей деталей, работы с 3D принтером, сборки манипулятора, разработки и тестировании его компьютерной динамической модели, разработки системы управления и программировании на языке C#.
Разработанный в данной работе,
манипулятор может эффективно использоваться в качестве лабораторного
оборудования для изучения задач кинематики и динамики роботов.
Список литературы
1. Промышленные роботы и манипуляторы [электронный ресурс]. URL: http://cncnc.ru/documentation/theory_of_mechanismus_and_machines/lect_19.htm (дата обращения: 09.06.2015).
2. Клюев С.А. Компьютерное моделирование: Учебно-методическое пособие. - М.: Волжский политехнический институт, 2009. - 89 с.
3. Arduino [электронный ресурс]. URL: https://www.arduino.cc/ (дата обращения: 09.06.2015).
. Промышленные роботы манипуляторы [электронный ресурс]. URL: http://robo24.ru/promyshlennye-roboty (дата обращения: 09.06.2015).
. Манипуляционные роботы [электронный ресурс]. URL: http://refleader.ru/jgeyfsrnarna.html (дата обращения: 10.06.2015).
. Лекции по УРиРТС [электронный ресурс]. URL: http://www.studfiles.ru/dir/cat39/subj1292/file13007/view135824/page2.html\ (дата обращения: 10.06.2015).
. Схиртладзе А.Г., Выходец В.И. Оборудование машиностроительных предприятий. - М.: РПК «Политехник», 2005. - 92 с.
. Компьютерное моделирование [электронный ресурс]. URL: http://www.inf1.info/book/export/html/215 (дата обращения: 8.03.2015).
. Гонсалес Р., Фу К., Ли К. Робототехника. - М.: Москва «Мир», 1989. - 620 с.
10. Adams - MSC Software [электронный ресурс]. URL: http://www.mscsoftware.com/product/adams (дата обращения: 10.06.2015).
11. Universal Mechanism - моделирование динамики механических систем [электронный ресурс]. URL: http://www.umlab.ru/pages/index.php? id=1 (дата обращения: 10.06.2015).
. NX: Siemens PLM Software [электронный ресурс]. URL: http://www.plm.automation.siemens.com/ru_ru/products/nx/ (дата обращения: 10.06.2015).
. CATIA - Dassault Systèmes [электронный ресурс]. URL: http://www.3ds.com/ru/produkty-i-uslugi/catia/ (дата обращения: 10.06.2015).
. SolidWorks-Russia [электронный ресурс]. URL: http://www.solidworks.ru/ (дата обращения: 10.06.2015).
. Пособие по ADAMS [электронный ресурс]. URL: http://mmm_samgu.ssu.samara.ru/polyakov/adams/Adams_pos_new.pdf (дата обращения: 8.03.2015).
16. Fanuc M-410iB Series [электронный ресурс]. URL: http://www.fanucrobotics.com/cmsmedia/datasheets/M-410iB % 20Series_15.pdf (дата обращения: 11.06.2015).
. Описание сервопривода DF15RMG [электронный ресурс]. Режим доступа: http://www.electronshik.ru/item/df15rmg-tilt-kit-20kg-1114314 (дата обращения: 12.06.2015).
. Описание сервопривода RDS3115 [электронный ресурс]. Режим доступа: http://www.aliexpress.com/snapshot/6494586405.html? orderId=65771461539714 (дата обращения: 12.06.2015).
. Описание сервопривода SR430 [электронный ресурс]. Режим доступа: http://robot-kit.ru/print_product_info.php/products_id/438 (дата обращения: 12.06.2015).
. Описание Google SketchUp 8 [электронный ресурс]. Режим доступа: http://reviewsoft.ru/Windows/Audio-Video/Audio-Recorders/google-sketchup/ (дата обращения: 12.06.2015).
21. Visual
Studio - Microsoft Developer Tools [электронный ресурс]. Режим доступа: https://www.visualstudio.com/ru-ru/visual-studio-homepage-vs.aspx (дата обращения:
13.06.2015).
Приложения
Приложение 1
Код Matlab для вычисления математической динамической модели манипулятора
L1 = 0.2;= 0.2;= pi/4; dQ1 = 0; ddQ1 = 0;= pi/4; dQ2 = pi/8; ddQ2 = pi/6;= 0.21;= 0.18;= 9.80665;
= cos(Q1); S1 = sin(Q1);= cos(Q1); S2 = sin(Q1);= cos (Q1+Q2); S12 = sin (Q1+Q2);
_0 = [C1 S1 0 L1*C1C1 0 L1*S1
0 1 0
0 0 1];_1 = [C2 - S2 0 L2*C2C2 0 L2*S2
0 1 0
0 0 1];_0 = A1_0 * A2_1;= [0 -1 0 0
0 0 0
0 0 0
0 0 0];= MQ * A1_0;= MQ * A2_0;= A1_0 * MQ * A2_1;= [1/3*m1*L1^2 0 0 -1/2*m1*L1
0 0 0
0 0 0
/2*m1*L1 0 0 m1];= [1/3*m2*L2^2 0 0 -1/2*m2*L2
0 0 0
0 0 0
/2*m2*L2 0 0 m2];
= 1/3*m1*L1^2 + 4/3*m2*L2^2 + m2*C2*L2^2;= 1/3*m2*L2^2 + 1/2*m2*L2^2*C2;= 1/3*m2*L2^2;= -1/2*m2*S2*L2^2*dQ2^2 - m2*S2*L2^2*dQ1*dQ2;= 1/2*m2*S2*L2^2*dQ1^2;= 1/2*m1*g*L1*C1 + 1/2*m2*g*L2*C12 + m2*g*L1*C1;= 1/2*m2*g*L2*C12;= [D11 D12D22];= [ddQ1];= [h1];= [c1];
= D*ddQ + h + c;
Приложение 2
Код программы
Arduino для управления сервоприводами манипулятора
#include «Servo.h»
// Обьекты Servoservo0;servo1;servo2;servo3; s=»»;
int i;q0, nq0 = 94; // начальное положене угла серво0q1, nq1 = 144; // начальное положене угла серво1q2, nq2 = 180; // начальное положене угла серво2q3, nq3 = 50; // начальное положене угла серво3
float p0, p1, p2, p3;
setup()
{.begin(9600);.attach(6); // серво1 подключен к пин 6
servo1.attach(8); // серво1 подключен к пин 8.attach(10); // серво2 подключен к пин 10.attach(12); // серво3 подключен к пин 12= nq0;= nq1;= nq2;= nq3;.write(q0); // Повернуть серво0 на q0 градусов.write(q1); // Повернуть серво1 на q1 градусов.write(q2); // Повернуть серво2 на q2 градусов.write(q3); // Повернуть серво3 на q3 градусов
p0=q0;=q1;=q2;=q3;
}
loop()
{
while (Serial.available())
{ // считывание данных с COM порта
char c = Serial.read(); // читаем символ+= c; // добавляем к строке(c == '!') // если считали символ, строка закончилась
{=0;=0;=0;=0;=0;(s[i]!= ' ')
{=q0*10;=q0+(int (s[i]) - 48);++;
}++;(s[i]!= ' ')
{=q1*10;=q1+(int (s[i]) - 48);++;
}++;(s[i]!= ' ')
{=q2*10;=q2+(int (s[i]) - 48);++;
}++;(s[i]!= '!')
{=q3*10;=q3+(int (s[i]) - 48);++;
}
s = «»;
}
}
// повороты сервоприводов
if (p0<q0)
{
p0=p0+0.01;
servo0.write(p0); // Повернуть серво0 на p0 градусов
}(p0>q0)
{=p0-0.01;.write(p0); // Повернуть серво0 на p0 градусов
}(p1<q1)
{=p1+0.01;.write(p1); // Повернуть серво1 на p1 градусов
}(p1>q1)
{=p1-0.01;.write(p1); // Повернуть серво1 на p1 градусов
}(p2<q2)
{=p2+0.01;.write(p2); // Повернуть серво2 на p2 градусов
}(p2>q2)
{=p2-0.01;.write(p2); // Повернуть серво2 на p2 градусов
}(p3<q3)
{=p3+0.01;.write(p3); // Повернуть серво3 на p3 градусов
}(p3>q3)
{=p3-0.01;.write(p3); // Повернуть серво2 на p2 градусов
}(0.01);
}
Приложение 3
Код компьютерной
программы на языке C# для управления манипулятором
using System;System. Collections. Generic;System. ComponentModel;System. Data;System. Drawing;System. Linq;System. Text;System. Threading. Tasks;System. Windows. Forms;System.IO. Ports; // подключение пространства имен с классом SerialPortSystem.IO;
RobotProgram
{partial class Form1: Form
{
double q0 = 0, q1 = 69, q2 = 38, q3 = 50; // значения начальных углов поворотов звеньевs0 = 1, s1 = 1, s2 = 1, s3 = 1; // скорости поворотов звеньев (изменять не нужно)
// длины звеньев, ммL0 = 90; // основаниеL1 = 200; // 1 ое звеноL2 = 200; // 2 ое звеноL3 = 170; // схват
// переменные координат и углов звеньев
double X, Y, Z, Xm, Ym, Zm, qq0, qq1, qq2;
// флаги наджатия кнопок управленияf0U = false, f0D = false,U = false, f1D = false,U = false, f2D = false,U = false, f3D = false;
flag = true;flag1 = false;start_flag = false;gotovo = true;f_time = false;time1 = 0;str_pos = «»;str_pos_1 = «»;str1 = «»;str_x = «»;str_y = «»;str_z = «»;str_s = «»;str_t = «»;xk, yk, zk, sk, tk;
k =
57.295779513;q3r;stroka = «»;
SerialPort Port1;Form1 ()
{();
}
void Form1_Load (object sender, EventArgs e)
{[] myPort; // создаем массив строк= System.IO. Ports. SerialPort. GetPortNames(); // в массив помещаем доступные порты
comboBox1. Items. AddRange(myPort); // теперь этот массив заносим в список(comboBox)
if (comboBox1. Items. Count!= 0) comboBox1. SelectedIndex = 0; // если есть порты выбираем 1 ый
}
void Form1_FormClosing (object sender, FormClosingEventArgs e)
{(comboBox1. Items. Count!= 0) // если есть доступный COM порт
{(Port1. IsOpen!= false)
{. Close(); // при закрытии программы закрываем порт (если он открыт)
}
}
}
private void button1_Click (object sender, EventArgs e) {}
void button2_Click
(object sender, EventArgs e) {}
//// управление роботом ////
//// ручное управление //// void timer1_Tick (object sender, EventArgs e)
{(angles. Checked) // если выбрано управление углами
{. Text = «Q0»;. Text = «Q1»;
label3. Text = «Q2»;
// определение значений скоростей регулировки углов
//s0 = Q0TB. Value; // скорость движения манипулятора
s0 = 1;= s0;= s0;= 1;
// настройка скорости((q3r * k + s1) > 0)
{= (int) Math. Abs (Math. Round (q3r * k));= s1;(s1 == 0)
{= 1;= 1;
}
}
// управление углами(f0U)
{= q0 + s0; // увеличение q0(q0 > 85) q0 = 85;
}(f0D)
{= q0 - s0; // уменьшение q0(q0 < -85) q0 = -85;
}T. Text = q0. ToString(); // вывод значения угла q0
(f1U)
{= q1 + s1; // увеличение q1(q1 > 102) q1 = 102;
}(f1D && ((q3r * k) < 0))
{= q1 - s1; // уменьшение q1(q1 < -42) q1 = -42;
}T. Text = q1. ToString(); // вывод значения угла q1
if (f2U)
{
q2 = q2 + s2; // увеличение q2
if (q2 > 48) q2 = 48;
}
if (f2D && ((q3r * k) < 0))
{
q2 = q2 - s2; // уменьшение q2
if (q2 < -90) q2 = -90;
}T. Text = q2. ToString(); // вывод значения угла q2
if (f3U)
{
q3 = q3 + s3; // увеличение q3
if (q3 > 90) q3 = 90;
}(f3D)
{= q3 - s3; // уменьшение q3(q3 < 0) q3 = 0;
}T. Text = q3. ToString(); // вывод значения угла q3
// корректировка q1 и q2((q3r * k) > 0)
{(q1 < 0) q1 = q1 + s0;(q2 < 0) q2 = q2 + s0;
}
_zadacha(); // решение прямой задачи
// присвоение координат
X = Xm;= Ym;= Zm;
}(coordinates. Checked) // если выбрано управление координатами
{
label1. Text = «X»;. Text = «Y»;
label3. Text = «Z»;
// скорость движения манипулятора= 1;= 1;
// настройка скорости((q3r * k + s1) > 0)
{= (int) Math. Abs (Math. Round (q3r * k));(s0 == 0) s0 = 1;
}
// управление координатами
if (f0U &&! ((flag == false) && (X > 0)))
{= X + s0; // увеличение X
}(f0D)
{= X - s0; // уменьшение X
}T. Text = X. ToString(); // вывод
значения X
if (f1U &&! ((flag == false) && (Y > 0)))
{= Y + s0; // увеличение Y
}
if (f1D &&! ((flag == false) && (Y < 0)))
{= Y - s0; // уменьшение Y
}
Q1T. Text = Y. ToString(); // вывод значения Y
(f2U && ((q3r * k) < 0) &&! ((flag == false) && (Z > 0)))
{= Z + s0; // увеличение Z
}(f2D &&! ((flag == false) && (Z < 0)))
{= Z - s0; // уменьшение Z
}T. Text = Z. ToString(); // вывод значения угла Z
// управление схватом(f3U)
{= q3 + s3; // разжимать схват
if (q3 > 90) q3 = 90;
}(f3D)
{= q3 - s3; // сжимать схват(q3 < 0) q3 = 0;
}T. Text = q3. ToString(); // вывод значения угла q3
obratnaya_zadacha(); // решение обратной задачи
if (double. IsNaN(qq0) || double. IsNaN(qq1) || double. IsNaN(qq2))
{ // если при решении обратной задачи значение угла будет NaN, значит это уже недосягаемая зона
label9. ForeColor = Color. Red;. Text = «Достигнут край рабочей зоны!»;= false; // ставим флаг, чтобы робот дальше не двигался
}
{= qq0;= qq1;= qq2;= true;
}(); // корректировка углов поворотов звеньев
}
// вывод различных значений на экран. Text = «X =» + Math. Round(X).ToString();. Text = «Y =» + Math. Round(Y).ToString();. Text = «Z =» + Math. Round(Z).ToString();
. Text = «Q0 =» + q0. ToString();. Text = «Q1 =» + q1. ToString();. Text = «Q2 =» + q2. ToString();. Text = «Схват =» + q3. ToString();