Моделирование движения плоскостного манипулятора
Министерство образования РБ
БЕЛОРУССКИЙ НАЦИОНАЛЬНЫЙ ТЕХНИЧЕСКИЙ УНИВЕРСИТЕТ
Кафедра "Робототехнические системы"
ПОЯСНИТЕЛЬНАЯ
ЗАПИСКА
к курсовой
работе по дисциплине:
Математическое
обеспечение ПР
НА ТЕМУ:
«МОДЕЛИРОВАНИЕ
ДВИЖЕНИЯ ПЛОСКОСТНОГО МАНИПУЛЯТОРА»
Исполнитель
Ходжаев Т. А.
Руководитель
Околов А.Р.
Минск 2013г.
АННОТАЦИЯ
В данной работе отображены вопросы планирования траектории для
плоскостного трехзвенного манипулятора, в частности планирование траектории в
обобщенных координатах методом сплайн-интерполяции. Результатом выполнения
работы является построение графиков положения, скорости и ускорения звеньев
манипулятора в обобщенной системе координат и визуализация движения робота в
декартовой системе координат.
ВВЕДЕНИЕ
Опираясь на законы кинематики и динамики манипулятора, можно так
управлять приводами сочленений, чтобы манипулятор двигался вдоль некоторой
траектории, обеспечивающей выполнение поставленной задачи. Перед началом
движения манипулятора важно знать: во-первых, существуют на его пути какие-то
препятствия, и, во-вторых, накладываются ли какие-то ограничения на характер
его траектории.
Кривую, вдоль которой схват манипулятора движется из начальной точки в
конечную, называют его траекторией. Задача состоит в разработке математического
аппарата для выбора и описания желаемого движения манипулятора между начальной
и конечной точками траектории. Как правило, траектория, соединяющая начальное и
конечное положение схвата, не единственна. Возможно, например, движение схвата
вдоль прямой, соединяющей начальную и конечную точки (прямолинейная
траектория), а также вдоль некоторой гладкой кривой, удовлетворяющей ряду
ограничений на положение и ориентацию схвата на начальном, конечном и
промежуточных участках траектории (сглаженная траектория). При этом не смотря
на то, что виды траектории будут различными между опорными точками, тем не
менее в опорных точках положения будут одинаковыми, равными заданным значениям.
Кроме того, что могут различаться виды траекторий, способы получения
траекторий также могут быть различными. Различают способы планирования в
декартовых координатах (ДК) и обобщенных координатах (ОК).
Первый состоит в том, что исследователь задает точный набор ограничений
для звеньев (заданные значения положения, скорости, ускорения и непрерывность
их) в некоторых узловых точках траектории. Планировщик траектории выбирает
функцию, проходящую через узловые точки и удовлетворяющую заданным
ограничениям, таким образом формирую все промежуточные положения звеньев. После
этого решается прямая задача кинематики( ПЗК), т.е. по полученным значениям
положений сочленений (звеньев) определяется положение схвата относительно
базовой системы координат во всех точках траектории в ДК.
Второй подход состоит в том, что исследователь задает желаемую траекторию
манипулятора в виде некоторой аналитически описываемой функции в декартовых
координатах. Планировщик после этого решает обратную задачу кинематики (ОЗК),
т.е. по заданному положение схвата относительно базовой системы координат
определяются положения сочленений (звеньев) в ОК, используемые для управления
приводами движения.
Известно, что отработка траектории манипулятора производится в
пространстве обобщенных координат. Поэтому, если планирование траектории
осуществляется в декартовых координатах, то для ее отработки необходимо решать
обратную задачу кинематики о положении манипулятора. Если такое преобразование
необходимо выполнять в реальном масштабе времени, то оно является серьезной
вычислительной нагрузкой для системы управления. Кроме того законы движения
звеньев манипулятора будут далеки от идеальных, возможны обрывы и скачки
траектории в ОК. Преимуществом планирования траектории в пространстве
декартовых координат является наглядность и предсказуемость траектории движения
схвата манипулятора, так как непосредственно планируется траектория,
отрабатываемая схватом.
Преимуществом планирования траектории в пространстве обобщенных координат
является:
·
планирование
непосредственно траектории, которую должны отрабатывать приводы звеньев манипулятора;
·
планирование
траектории требует небольшого количества вычислений;
·
траекторию легче
планировать, так как отсутствует понятие ориентации.
Недостатком планирования в ОК является то, что итоговая траектория,
получаемая после решения ПЗК, является непредсказуемой и может сильно
отличаться от желаемой.
Сейчас в микропроцессорных системах управления промышленных роботов
наиболее часто применяются линейная и круговая интерполяция в декартовой
системе координат и линейная интерполяция в пространстве обобщенных координат.
При линейной интерполяции в обобщенных координатах манипулятор совершает
движение за минимально необходимое время за счет минимально необходимых
перемещений звеньев. Закон движения звеньев трапецеидальный для скорости. При
линейной интерполяции в декартовых координатах схват манипулятора движется по
прямой линии, ориентация его также меняется по линейному закону. В последнее
время все большее применение находит сплайн-интерполяция. Сущность ее
заключается в представлении траектории между узловыми точками кривой,
описываемой степенными полиномами третьей, четвертой или пятой степени. При
этом коэффициенты полинома выбираются так, чтобы обеспечить отработку
ограничений, накладываемых на траекторию.
1.
ФОРМУЛИРОВКА ЗАДАЧИ
Целью
задания является моделирование движения манипулятора, приведенного на рисунке 1
в обобщенной системе координат (СК) методом сплайн интерполяции траектории и
графическое отображение его движения на дисплее ПК.
Значения
для задания варианта №1 и подварианта11 приведены в таблице 1.1, исходные
данные - в таблице 1.2 соответственно.
Рисунок 1 - Кинематическая схема манипулятора
Таблица1.1 - Вариант задания
|
Система координат
|
Вид интерполяции
|
|
обобщенная
|
декартовая
|
линейная
|
круговая
|
со сглаживанием отрезков
|
Лагранжа
|
сплайн
|
|
+
|
-
|
-
|
-
|
-
|
-
|
+
|
Таблица
1.2 - Исходные данные
|
t
|
t0
|
t1
|
t2
|
t3
|
|
x
|
5
|
5,8
|
5,4
|
4,3
|
|
y
|
-5,6
|
-4,2
|
-2
|
-1,2
|
|
v
|
0
|
30
|
50
|
0
|
|
y
|
0
|
20
|
60
|
90
|
|
q
|
0
|
0
|
0
|
0
|
|
j
|
0
|
0
|
0
|
0
|
L1=6, L2=3,
где t - время прохождения промежуточных
опорных точек;
x и y - положение схвата в опорные моменты
времени;
v -
скорость сочленений (звеньев) в опорные моменты времени;
y, q, j -
углы Эйлера, задающие ориентацию схвата.
2. РЕШЕНИЕ
ЗАДАЧИ В ОБЩЕМ ВИДЕ
При планировании траектории движения манипулятора в системе обобщенных
координат необходимо:
По
заданным значениям углов Эйлера j, q, y и положениям схвата
(xcx , ycx) в начальной, конечной и промежуточных точках
составить матрицы однородных преобразований 
Tн, Тпр, Тк;
1. Для начального Tн, конечного Тк и промежуточных Тпр
положений рабочего органа манипулятора определить соответствующие значения трех
обобщенных координат i (Qнi, Qпрi, Qкi), решив обратную
задачу кинематики ;
2. В соответствии с заданным видом
интерполяции построить зависимости, характеризующие положения, скорости и
ускорения каждой обобщенной координаты i в текущие моменты времени t Qi(t), используя
найденные ранее точки (Qнi, Qпрi, Qкi), в качестве опорных. При построении графиков скорости и
ускорения использовать метод приближенного дифференцирования графика положения,
скорости.
Написать программу визуализации движения манипулятора по сформированной
траектории. Программа должна определять текущие значения матриц однородных
преобразований Ai(t), описывающих в ДК положения и
ориентацию звеньев i в текущие
моменты времени t, по найденным
ранее значениям обобщенных координат Qi(t) (решение ПЗК). Используя значения
данных матриц должна строиться проволочная модель робота. В результате
циклического выполнения программы на экране дисплея должна отображаться
движущаяся проволочная модель манипулятора.
3. МАТЕМАТИЧЕСКОЕ РЕШЕНИЕ ЗАДАЧИ
Первый этап решения - составление матриц однородных преобразований для
начальной, конечной и промежуточных точек.
Матрица однородного преобразования, описывающая положение и ориентацию
схвата в базовой системе координат Qi(t), имеет вид
Подставляя значения углов Эйлера и координат схвата в формулу (3.1),
получаем матрицы однородных преобразований для начальной, конечной и
промежуточных точек траектории
, 
,
, 
.
Второй
этап - решение обратной задачи кинематики для каждого звена в начальной,
конечной и промежуточных точках. Решим ее с точки зрения алгебраического
подхода.
Добавляем
к манипулятору еще одну степень подвижности (поворот схват на угол Q3).
Кинематические параметры полученного трехзвенного манипулятора приведены в
таблице 3.1.
Таблица
3.1 - Кинематические параметры манипулятора
|
I
|
a
|
a
|
d
|
Q
|
|
1
|
0
|
 0Q1
|
|
|
|
2
|
0
|
 0Q2
|
|
|
|
3
|
0
|
0
|
0
|
Q3
|
Матрица, описывающая положение третьего звена (схвата) относительно
базовой системы координат, выглядит следующим образом
,
Где
с123=cos(Q1+ Q2+ Q3);
s123=sin(Q1+ Q2+ Q3);=cos(Q1);=cos(Q1+ Q2);=sin(Q1);12=sin(Q1+ Q2).
Для
рассматриваемого манипулятора матрицу 
можно
записать в виде
= 
гдесy=cosy;
sy=siny;
Приравняем
выражения (3.2) и (3.3) и получим четыре нелинейных уравнения, которые
необходимо решить относительно Q1, Q2, Q3.
Возведя
в квадрат, левые и правые части двух последних уравнений системы (3.4) и
складывая их, получим:
,
где
с2=cosQ2.
Решая
уравнение (2.5) относительно с2, получим
.
Для
существования решения правая часть уравнения (3.6) должна лежать в пределах
[-1…+1]. Если условие выполнено, то находим
.
Выбор
знака у sinQ2 соответствует одному из возможных решений. Принимаем
положение манипулятора “локоть вниз”, для которого sinQ2 в данном
случае принимает положительные значения. При определении угла Q2
воспользуемся функцией ATAN2, которая обеспечивает выбор всех решений и выбор
квадранта.
Отсюда
Q2 будет равно
.
Зная
Q2, воспользуемся двумя последними уравнениями системы
(3.4) для определения угла Q1. Перепишем их в следующем виде
Где
;
.
Для
решения уравнений системы (3.9) выполним замену переменных:
; 
,
тогда
; 
.
Теперь
систему уравнений (3.9) можно записать следующим образом:
Откуда находим
Используя функцию ATAN2, имеем
Из
первых двух уравнений системы (3.4) находим
.
Подставляя
исходные данные в выражения (3.6) - (3.13) и проведя соответствующий расчет,
находим значения обобщенных координат для начальной, конечной и промежуточных
точек
; 
; 
;
.
Третий
этап решения - планирование траектории движения манипулятора в обобщенных
координатах с учетом сплайн-интерполяции, сущность которой заключается в
представлении траектории между узловыми точками с помощью кривой, описываемой
степенными полиномами третьей, четвертой или пятой степени вида
При
этом коэффициенты полиномов выбираются так, чтобы обеспечить непрерывность
функций положения, скорости и ускорения при переходе с одного сегмента на
другой. Последовательность степенных функций, сцепленных друг с другом,
представляют желаемую траекторию, которая обеспечивает плавное движение
манипулятора (рисунок 3.1).
Рисунок
2 - Общий вид траектории при сплайн-интерполяции
Порядок
полинома, выбранного для представления желаемой траектории, зависит от
спецификации манипуляционной задачи. Когда заданы положения и скорость в
начальные и конечные моменты времени, то этим четырем условиям удовлетворяет
полином третьей степени с четырьмя неизвестными параметрами. Если в эти моменты
времени также задано ускорение, тогда необходимо удовлетворение 6 условий и,
соответственно, необходим полином 5 степени с 6 неизвестными параметрами.
Рисунок
3.- I-ый участок траектории при сплайн-интерполяции
При
выполнении сплайн-интерполяции траектории рассмотрим i-ый участок
траектории (рисунок 3.2), который описывается следующим полиномом:
,
где
Четвертый этап решения - поточечное преобразование спланированной в
обобщенной системе координат траектории в систему декартовых координат путем
решения прямой задачи кинематики. Прямую задачу будем решать геометрическим
методом.
Рисунок 4 - Решение ПЗК для манипулятора.
.-первое звено; 2.-второе звено; 3.-третье звено (схват);
Декартовы координаты точек A(x1,y1) и B(x2,y2) относительно центра базовой системы координат O(0,0) вычисляются с помощью
геометрических зависимостей (рисунок 3.3):
x1=l1cosQ1 ; x2=x1+l2cos(Q1+Q2) ;=l1sinQ1 ; y2=y1+l2sin(Q1+Q2)
.
Таким образом, определили все необходимые для написания программы и
моделирования движения манипулятора формулы.
4.
МОДЕЛИРУЮЩАЯ ПРОГРАММА
4.1 Описание программы
Моделирующая программа написана на языке ООП Visual C# 4.0[7] и выполняет следующие операции:
решение обратной задачи кинематики для заданных контрольных точек
(начальной, конечной и промежуточных);
планирование сглаженной траектории в обобщенных координатах методом
интерполяции полиномом Лагранжа;
решение прямой задачи кинематики в каждой точке геометрическим методом;
визуализацию движения трехзвенного манипулятора;
-построение графиков зависимостей обобщенных координат q1, q2, q3, а
также их скоростей и ускорений от времени.
Программа построена по блочно-модульному типу с использованием
многократно вызываемых методов и событий. Моделирующая программа включает в
себя следующие основные методы и события:
- TimeIntervals - метод создает массив значений
времени, в соответствии со временем прохождения каждого участка (всего 80
элементов);
-
Result4x4 - метод, по заданным значениям углов Эйлера j, q, y и положениям схвата (
) в
начальной, конечной и промежуточных точках составляются матрицы однородных
преобразований;
OZK - метод для
решения обратной задачи кинематики, с точки зрения алгебраического подхода, для
каждого звена в начальной, конечной и промежуточных точках;
Spline - метод
для реализации сплайн интерполяции;
SplineSpeed -
метод для реализации сплайн интерполяции (нахождения скорости - первая
производная полинома по времени);
SplineAcceleration
- метод для реализации сплайн интерполяции (нахождения ускорения - вторая
производная полинома по времени);
PZK - метод для
решения прямой задачи кинематики в каждой точке геометрическим методом;
DrawDimension
-графический метод, производит разметку осей графиков с обобщенными
координатами в соответствии с масштабом графиков;
DrawManipulator
- графический метод осуществляет визуализацию движения трехзвенного манипулятора
в декартовых координатах;
DrawPosition -
графический метод осуществляет построение графиков положения всех звеньев в
обобщенных координатах;
DrawSpeed -
графический метод осуществляет построение графиков скорости всех звеньев в
обобщенных координатах;
DrawAcceleration
- графический метод осуществляет построение графиков ускорений всех звеньев в
обобщенных координатах;
buttonDecart_Click
- обработчик события Click, запускает метод DrawManipulator;
buttonObobs_Click
- обработчик события Click, запускает метод DrawPosition;
buttonSkor_Click
- обработчик события Click, запускает метод DrawSpeed;
buttonUskor_Click
- обработчик события Click, запускает метод DrawAcceleration;
Для хранения в памяти значений обобщенных координат, их скоростей и
ускорений используются массивы типа double[,]. Размер массива определен заранее, исходя из шага квантования dt=0.05 с и общего времени по заданию T=3 c (всего 60 элементов).
Построение графических объектов (манипулятора, графиков зависимостей)
осуществляется в режиме 600х500 точек.
4.2 Текст моделирующей программы
using System;
using
System.Collections.Generic;System.ComponentModel;System.Data;System.Drawing;System.Drawing.Drawing2D;System.Linq;System.Text;System.Windows.Forms;
курсачМОПР
{partial class Form1 : Form
{
Form1()
{();
}
void Form1_Load(object sender, EventArgs e)
{
}
//Массив для координат X, Y, взятых из условия[,] realXY = new double[2,
4] {
{ 5.0, 5.8, 5.4, 4.3 },
{ -5.6, -4.2, -2.0, -1.2 }
};
//Длины суставов манипулятора
double l1 = 6.0;l2 = 3.0;
p1 = 20;
int p2 = 20;p3 = 20;
//Углы psi для разных t
double[] psi = { 0, 20.0, 60.0, 90.0 };
[] speed = { 0, 30.0, 50.0, 0};
[] time;
//Массив времени t[] t = { 0, 1.0, 2.0, 3.0 };
nx, ny;
#region Методы
//Разбить все время выполнения на одинаковые интервалы
public void TimeIntervals()
{= new double[p1 + p2 + p3];[0] = t[0];[p1 - 1] = t[1];[p1 +
p2 - 1] = t[2];[p1 + p2 + p3 - 1] = t[3];
(int i = 1; i < p1 - 1; i++)
{[i] = time[i - 1] + (t[1] - t[0]) / p1;
}(int i = p1; i < p1 + p2 - 1; i++)
{[i] = time[i - 1] + (t[2] - t[1]) / p2;
}(int i = p1 + p2; i < p1 + p2 + p3 - 1; i++)
{[i] = time[i - 1] + (t[3] - t[2]) / p3;
}
}
//Создание матрицы
однородных преобразованийdouble[,] Result4x4(double psi, double x, double y)
{fi = 0;eta = 0;= psi * Math.PI / 180;
[,] arrresult = new double[4, 4];[0, 0] = Math.Cos(fi) *
Math.Cos(eta);[0, 1] = Math.Cos(fi) * Math.Sin(eta) * Math.Sin(psi) -
Math.Sin(fi) * Math.Cos(psi);[0, 2] = Math.Cos(fi) * Math.Sin(eta) *
Math.Cos(psi) + Math.Sin(fi) * Math.Sin(psi);[1, 0] = Math.Sin(fi) *
Math.Cos(eta);[1, 1] = Math.Sin(fi) * Math.Sin(eta) * Math.Sin(psi) -
Math.Cos(fi) * Math.Cos(psi);[1, 2] = Math.Sin(fi) * Math.Sin(eta) *
Math.Cos(psi) - Math.Cos(fi) * Math.Sin(psi);[2, 0] = -Math.Sin(eta);[2, 1] =
Math.Cos(eta) * Math.Sin(psi);[2, 2] = Math.Cos(eta) * Math.Cos(psi);
[0, 3] = x;[1, 3] = y;[2, 3] = 0;[3, 0] = 0;[3, 1] = 0;[3, 2]
= 0;[3, 3] = 1;
arrresult;
}
//Обратная задача кинематики(найти углы Q1, Q2, Q3 в градусах)
public double[,] OZK()
{
c2, s2;[,] qr = new double[3, realXY.GetLength(1)];
(int i = 0; i < realXY.GetLength(1); i++)
{
= (realXY[0, i] * realXY[0, i] + realXY[1, i] * realXY[1, i]
- l1 * l1 - l2 * l2) / (2 * l1 * l2);= Math.Sqrt(1 - c2 * c2);(c2 > 0) qr[1,
i] = 180 / Math.PI * Math.Atan(s2 / c2);(c2 < 0) qr[1, i] = 180 + 180 /
Math.PI * Math.Atan(s2 / c2);[0, i] = 180 / Math.PI * Math.Atan(realXY[1, i] /
realXY[0, i]) - 180 / (Math.PI) * Math.Atan((l2 * s2) / (l1 + l2 * c2));[2, i]
= psi[i] - qr[0, i] - qr[1, i];
}qr;
}
//Сплайн интерполяцияdouble[,] Spline(double[,] arr)
{();
[,] splineQR = new double[3, p1 + p2 + p3];tau, tm, k0, k1,
k2, k3;
[,] mass = OZK();
min = 0;max = 80;(int j = 1; j < 4; j++)
{(j == 1) { min = 0; max = p1; }(j == 2) { min = p1-1; max =
p1+p2; }(j == 3) { min = p1+p2-1; max = p1+p2+p3; }
(int i = min; i < max; i++)
{
(int k = 0; k < 3; k++)
{= time[i] - t[j - 1];= t[j] - t[j - 1];= mass[k, j - 1];=
speed[j - 1];= (3 * mass[k, j] - 3 * mass[k, j - 1] - 2 * speed[j - 1] * tm -
speed[j] * tm) / (tm * tm);= (2 * mass[k, j - 1] - 2 * mass[k, j] + speed[j] *
tm + speed[j - 1] * tm) / (tm * tm * tm);
[k, i] = k3 * tau * tau * tau + k2 * tau * tau + k1 * tau +
k0;
}
}
}
splineQR;
}
//Нахождение координат скоростиdouble[,]
SplineSpeed(double[,] arr)
{();[,] splineQR = new double[3, p1 + p2 + p3];tau, tm, k0,
k1, k2, k3;
[,] mass = OZK();
min = 0;max = 80;
(int j = 1; j < 4; j++)
{(j == 1) { min = 0; max = p1; }(j == 2) { min = p1 - 1; max
= p1 + p2; }(j == 3) { min = p1 + p2 - 1; max = p1 + p2 + p3; }
(int i = min; i < max; i++)
{
(int k = 0; k < 3; k++)
{= time[i] - t[j - 1];= t[j] - t[j - 1];= mass[k, j - 1];=
speed[j - 1];= (3 * mass[k, j] - 3 * mass[k, j - 1] - 2 * speed[j - 1] * tm -
speed[j] * tm) / (tm * tm);= (2 * mass[k, j - 1] - 2 * mass[k, j] + speed[j] *
tm + speed[j - 1] * tm) / (tm * tm * tm);[k, i] = 3*k3 * tau * tau + 2*k2 * tau
+ k1;
}
}
}
splineQR;
}
//Нахождение координат ускоренияdouble[,]
SplineAcceleration(double[,] arr)
{();
[,] splineQR = new double[3, p1 + p2 + p3];tau, tm, k0, k1,
k2, k3;
[,] mass = OZK();
min = 0;max = 80;
(int j = 1; j < 4; j++)
{(j == 1) { min = 0; max = p1; }(j == 2) { min = p1 - 1; max
= p1 + p2; }(j == 3) { min = p1 + p2 - 1; max = p1 + p2 + p3; }
(int i = min; i < max; i++)
{
(int k = 0; k < 3; k++)
{= time[i] - t[j - 1];= t[j] - t[j - 1];= mass[k, j - 1];=
speed[j - 1];= (3 * mass[k, j] - 3 * mass[k, j - 1] - 2 * speed[j - 1] * tm -
speed[j] * tm) / (tm * tm);= (2 * mass[k, j - 1] - 2 * mass[k, j] + speed[j] *
tm + speed[j - 1] * tm) / (tm * tm * tm);
[k, i] = 6*k3 * tau + 2*k2;
}
}
}
splineQR;
}
//Прямая задача кинематики(найти координаты точек манипулятора)
public double[,] PZK(double[,] arr)
{[,] pzkXY = new double[8, p1 + p2 + p3];
//Перевод градусов в радианы(int i = 0; i < 3; i++)
{(int j = 0; j < p1+p2+p3; j++)
{[i, j] = arr[i, j] * Math.PI / 180;
}
}
(int i = 0; i < p1 + p2 + p3; i++)
{
//Координаты точки 1[0, i] = l1 * Math.Cos(arr[0, i]);[1, i] = l1 *
Math.Sin(arr[0, i]);
//Координаты точки 2[2, i] = l1 * Math.Cos(arr[0, i]) + l2 * Math.Cos(arr[0, i]
+ arr[1, i]);[3, i] = l1 * Math.Sin(arr[0, i]) + l2 * Math.Sin(arr[0, i] +
arr[1, i]);
//Нужны для построения оси Y точки 2
pzkXY[4, i] = l1 * Math.Cos(arr[0, i]) + l2 * Math.Cos(arr[0,
i] + arr[1, i]) + Math.Sin(arr[2, i]);[5, i] = l1 * Math.Sin(arr[0, i]) + l2 *
Math.Sin(arr[0, i] + arr[1, i]) + Math.Cos(arr[2, i]);
//Нужны для построения оси X точки 2
pzkXY[6, i] = l1 * Math.Cos(arr[0, i]) + l2 * Math.Cos(arr[0,
i] + arr[1, i]) + Math.Sin(Math.PI + arr[2, i] - 90 * Math.PI / 180);[7, i] =
l1 * Math.Sin(arr[0, i]) + l2 * Math.Sin(arr[0, i] + arr[1, i]) +
Math.Cos(Math.PI + arr[2, i] - 90 * Math.PI / 180);
}pzkXY;
}
//Примеры вызова PZK
//double[,] mass = PZK(Spline(OZK()));
#endregion
#region Рисованиеvoid DrawDimension(Graphics g, Pen blackpen, SolidBrush brush)
{font = new Font("Arial", 9);x, y;
ii = 0;j = 19;
(int k = 0; k < 3; k++) {.DrawLine(blackpen, new Point(ii
+ 25 + nx, 270), new Point(ii + 25 + nx, 280));
= time[j].ToString();.DrawString(x, font, brush, ii + 25 + nx
- 5, 290.0F);
= ii + nx;= j + 20;
}
= 275;(int k = 0; k < 6; k++)
{.DrawLine(blackpen, new Point(20, ii - ny), new Point(30, ii
- ny));
= (k + 1).ToString();.DrawString(y, font, brush, 5, ii - ny -
5);
= ii - ny;
}= 275;(int k = 0; k < 6; k++)
{.DrawLine(blackpen, new Point(20, ii + ny), new Point(30, ii
+ ny));
= (- k - 1).ToString();.DrawString(y, font, brush, 5, ii + ny
- 5);
= ii + ny;
}
= "0";.DrawString(x, font, brush, 5, 270.0F);
.Dispose();
}
void DrawManipulator(object sender, EventArgs e)
{= 35;= 35;
[,] promD = PZK(Spline(OZK()));
[,] prom = new int[promD.GetLength(0),
promD.GetLength(1)];(int i = 0; i < promD.GetLength(1); i++) {[0, i] =
(int)(promD[0, i] * nx) + 300;[1, i] = 275 - (int)(promD[1, i] * ny);
[2, i] = (int)(promD[2, i] * nx) + 300;[3, i] = 275 -
(int)(promD[3, i] * ny);[4, i] = (int)(promD[4, i] * nx) + 300;[5, i] = 275 -
(int)(promD[5, i] * ny);
[6, i] = (int)(promD[6, i] * nx) + 300;[7, i] = 275 -
(int)(promD[7, i] * ny);
}
(Graphics g = this.CreateGraphics())
{blackpen = new Pen(Color.Black, 5);bluepen = new
Pen(Color.Blue, 1);bp = new Pen(Color.Black, 1);red = new Pen(Color.Red,
2);font = new Font("Arial", 14);f = new Font("Arial",
8);brush = new SolidBrush(Color.Black);
osx = "X";osy = "Y";op;
{.Clear(Color.White);
//Начертить звенья.DrawLine(blackpen, new Point(300, 275), new Point(prom[0,
i], prom[1, i]));.DrawLine(blackpen, new Point(prom[0, i], prom[1, i]), new
Point(prom[2, i], prom[3, i]));.DrawLine(bluepen, new Point(prom[2, i], prom[3,
i]), new Point(prom[4, i], prom[5, i]));.DrawLine(bluepen, new Point(prom[2,
i], prom[3, i]), new Point(prom[6, i], prom[7, i]));
//Начертить оси.DrawLine(bp, new Point(300, 25), new Point(300,
475));.DrawLine(bp, new Point(25, 275), new Point(575, 275));
//Стрелки.DrawLine(bp, new
Point(300, 25), new Point(288, 50));.DrawLine(bp, new Point(300, 25), new
Point(312, 50));.DrawLine(bp, new Point(575, 275), new Point(550,
263));.DrawLine(bp, new Point(575, 275), new Point(550, 287));
//Подпись.DrawString(osx, font,
brush, 580.0F, 275.0F);.DrawString(osy, font, brush, 305.0F, 10.0F);
//Подписать доп. оси.DrawString(osx, f, brush,
prom[4, i] + 5, prom[5, i] - 5);.DrawString(osy, f, brush, prom[6, i] + 5,
prom[7, i] - 5);
//Обозначить пересечения звеньев.DrawEllipse(blackpen,
prom[2, i], prom[3, i], 5.0F, 5.0F);.DrawEllipse(blackpen, 300, 275, 5.0F,
5.0F);.DrawEllipse(blackpen, prom[0, i], prom[1, i], 5.0F, 5.0F);
//Нарисовать схват.DrawLine(blackpen, new Point(prom[2, i], prom[3, i]), new
Point(prom[2, i] - 10, prom[3, i] - 10));.DrawLine(blackpen, new Point(prom[2,
i], prom[3, i]), new Point(prom[2, i] + 10, prom[3, i] - 10));
//Подписать оси X(int ii = 300 + nx; ii <= 575; ) {
g.DrawLine(bp, new Point(ii, 270), new Point(ii, 280));= ((ii
- 300)/nx).ToString();.DrawString(op, f, brush, ii - 5, 290);+= nx;
}(int ii = 300 - nx; ii >= 25; )
{.DrawLine(bp, new Point(ii, 270), new Point(ii, 280));= ((ii
- 300) / nx).ToString();.DrawString(op, f, brush, ii - 5, 290);-= nx;
}
//Подписать оси Y(int ii = 275 - ny; ii >= 25; )
{.DrawLine(bp, new Point(295, ii), new Point(305, ii));=
(Math.Abs(ii - 275) / ny).ToString();.DrawString(op, f, brush, 280, ii - 5);-=
ny;
}(int ii = 275 + ny; ii <= 475; )
{.DrawLine(bp, new Point(295, ii), new Point(305, ii));= ((-
ii + 275) / ny).ToString();.DrawString(op, f, brush, 280, ii - 5);+= ny;
}= "0";.DrawString(op, f, brush, 280, 260);
System.Threading.Thread.Sleep(30);
}
//Траектория движения и опорные точки
for (int i = 0; i < p1 + p2 + p3; i++ )
{.DrawEllipse(red, prom[2, i], prom[3, i], 2.0F, 2.0F);
}.DrawEllipse(blackpen, prom[2, 0], prom[3, 0], 6.0F,
6.0F);.DrawEllipse(blackpen, prom[2, p1 - 1], prom[3, p1 - 1], 6.0F,
6.0F);.DrawEllipse(blackpen, prom[2, p1 + p2 - 1], prom[3, p1 + p2 - 1], 6.0F,
6.0F);.DrawEllipse(blackpen, prom[2, p1 + p2 + p3 - 1], prom[3, p1 + p2 + p3 -
1], 6.0F, 6.0F);
.Dispose();.Dispose();.Dispose();.Dispose();.Dispose();.Dispose();
}
}
void DrawPosition(object sender, EventArgs e)
{= 165;= 80;
[,] promD = Spline(OZK());
[,] prom = new int[promD.GetLength(0) * 2,
promD.GetLength(1)];(int i = 0; i < promD.GetLength(1); i++)
{[0, i] = 25 + (int)(time[i] * nx);[1, i] = 275 -
(int)(promD[0, i] * (Math.PI / 180) * ny);
[2, i] = 25 + (int)(time[i] * nx);[3, i] = 275 - (int)(promD[1,
i] * (Math.PI / 180) * ny);
[4, i] = 25 + (int)(time[i] * nx);[5, i] = 275 -
(int)(promD[2, i] * (Math.PI / 180) * ny);
}
(Graphics g = this.CreateGraphics()) {
blackpen = new Pen(Color.Black, 1);redpen = new
Pen(Color.Red, 2);bluepen = new Pen(Color.Blue, 2);greenpen = new
Pen(Color.Green, 2);font = new Font("Arial", 14);brush = new
SolidBrush(Color.Black);
osx = "t,с";osy = "Q(t),рад";one
= "Звено 1";two = "Звено 2";three = "Звено 3";
.Clear(Color.White);
.DrawLine(blackpen, new Point(25, 25), new Point(25,
475));.DrawLine(blackpen, new Point(25, 275), new Point(575, 275));
.DrawLine(blackpen, new Point(25, 25), new Point(13,
50));.DrawLine(blackpen, new Point(25, 25), new Point(37,
50));.DrawLine(blackpen, new Point(575, 275), new Point(550, 263));.DrawLine(blackpen,
new Point(575, 275), new Point(550, 287));
.DrawString(osx, font, brush, 580.0F,
275.0F);.DrawString(osy, font, brush, 15.0F, 0.0F);
(int i = 0; i < promD.GetLength(1) - 1; i++)
{.DrawLine(redpen, new Point(prom[0, i], prom[1, i]), new Point(prom[0, i + 1],
prom[1, i + 1]));
}(int i = 0; i < promD.GetLength(1) - 1; i++)
{.DrawLine(bluepen, new Point(prom[2, i], prom[3, i]), new
Point(prom[2, i + 1], prom[3, i + 1]));
}(int i = 0; i < promD.GetLength(1) - 1; i++)
{.DrawLine(greenpen, new Point(prom[4, i], prom[5, i]), new
Point(prom[4, i + 1], prom[5, i + 1]));
}
.DrawString(one, font, brush, 95.0F, 10.0F);.DrawLine(redpen,
new Point(175, 20), new Point(205, 20));
.DrawString(two, font, brush, 245.0F,
10.0F);.DrawLine(bluepen, new Point(325, 20), new Point(355, 20));
.DrawString(three, font, brush, 395.0F,
10.0F);.DrawLine(greenpen, new Point(475, 20), new Point(505, 20));
(g, blackpen, brush);
.Dispose();.Dispose();.Dispose();.Dispose();.Dispose();.Dispose();
}
}
void DrawSpeed(object sender, EventArgs e) {
= 165;= 80;
();
[,] promD = SplineSpeed(OZK());[,] prom = new
int[promD.GetLength(0) * 2, promD.GetLength(1)];(int i = 0; i <
promD.GetLength(1); i++)
{[0, i] = 25 + (int)(time[i] * nx);[1, i] = 275 - (int)(promD[0,
i] * (Math.PI / 180) * ny);
[2, i] = 25 + (int)(time[i] * nx);[3, i] = 275 -
(int)(promD[1, i] * (Math.PI / 180) * ny);
[4, i] = 25 + (int)(time[i] * nx);[5, i] = 275 -
(int)(promD[2, i] * (Math.PI / 180) * ny);
}
(Graphics g = this.CreateGraphics())
{
blackpen = new Pen(Color.Black, 1);redpen = new
Pen(Color.Red, 2);bluepen = new Pen(Color.Blue, 2);greenpen = new
Pen(Color.Green, 2);font = new Font("Arial", 14);brush = new
SolidBrush(Color.Black);
osx = "t,с";osy = "Q'(t),рад";one
= "Звено 1";two = "Звено 2";three = "Звено 3";
.Clear(Color.White);.DrawLine(blackpen, new Point(25, 25),
new Point(25, 475));.DrawLine(blackpen, new Point(25, 275), new Point(575,
275));
.DrawLine(blackpen, new Point(25, 25), new Point(13,
50));.DrawLine(blackpen, new Point(25, 25), new Point(37,
50));.DrawLine(blackpen, new Point(575, 275), new Point(550,
263));.DrawLine(blackpen, new Point(575, 275), new Point(550, 287));
.DrawString(osx, font, brush, 580.0F,
275.0F);.DrawString(osy, font, brush, 15.0F, 0.0F);
(int i = 0; i < promD.GetLength(1) - 1; i++)
{.DrawLine(redpen, new Point(prom[0, i], prom[1, i]), new
Point(prom[0, i + 1], prom[1, i + 1]));
}(int i = 0; i < promD.GetLength(1) - 1; i++)
{.DrawLine(bluepen, new Point(prom[2, i], prom[3, i]), new
Point(prom[2, i + 1], prom[3, i + 1]));
}(int i = 0; i < promD.GetLength(1) - 1; i++)
{.DrawLine(greenpen, new Point(prom[4, i], prom[5, i]), new
Point(prom[4, i + 1], prom[5, i + 1]));
}
.DrawString(one, font, brush, 95.0F, 10.0F);.DrawLine(redpen,
new Point(175, 20), new Point(205, 20));.DrawString(two, font, brush, 245.0F,
10.0F);.DrawLine(bluepen, new Point(325, 20), new Point(355, 20));
.DrawString(three, font, brush, 395.0F,
10.0F);.DrawLine(greenpen, new Point(475, 20), new Point(505, 20));
(g, blackpen, brush);
.Dispose();.Dispose();.Dispose();.Dispose();.Dispose();.Dispose();
}
}
void DrawAcceleration(object sender, EventArgs e)
{
= 165;= 60;
();
[,] promD = SplineAcceleration(OZK());[,] prom = new
int[promD.GetLength(0) * 2, promD.GetLength(1)];(int i = 0; i <
promD.GetLength(1); i++)
{[0, i] = 25 + (int)(time[i] * nx);[1, i] = 275 -
(int)(promD[0, i] * (Math.PI / 180) * ny);
[2, i] = 25 + (int)(time[i] * nx);[3, i] = 275 -
(int)(promD[1, i] * (Math.PI / 180) * ny);
[4, i] = 25 + (int)(time[i] * nx);[5, i] = 275 -
(int)(promD[2, i] * (Math.PI / 180) * ny);
}
(Graphics g = this.CreateGraphics())
{
blackpen = new Pen(Color.Black, 1);redpen = new
Pen(Color.Red, 2);bluepen = new Pen(Color.Blue, 2);greenpen = new
Pen(Color.Green, 2);font = new Font("Arial", 14);brush = new
SolidBrush(Color.Black);
osx = "t,с";osy = "Q''(t),рад";one = "Звено 1";two = "Звено 2";three = "Звено 3";
.Clear(Color.White);
.DrawLine(blackpen, new Point(25, 25), new Point(25,
475));.DrawLine(blackpen, new Point(25, 275), new Point(575, 275));.DrawLine(blackpen,
new Point(25, 25), new Point(13, 50));.DrawLine(blackpen, new Point(25, 25),
new Point(37, 50));.DrawLine(blackpen, new Point(575, 275), new Point(550,
263));.DrawLine(blackpen, new Point(575, 275), new Point(550, 287));
.DrawString(osx, font, brush, 580.0F,
275.0F);.DrawString(osy, font, brush, 15.0F, 0.0F);
(int i = 0; i < promD.GetLength(1) - 1; i++)
{.DrawLine(redpen, new Point(prom[0, i], prom[1, i]), new
Point(prom[0, i + 1], prom[1, i + 1]));
}(int i = 0; i < promD.GetLength(1) - 1; i++)
{.DrawLine(bluepen, new Point(prom[2, i], prom[3, i]), new
Point(prom[2, i + 1], prom[3, i + 1]));
}(int i = 0; i < promD.GetLength(1) - 1; i++)
{.DrawLine(greenpen, new Point(prom[4, i], prom[5, i]), new
Point(prom[4, i + 1], prom[5, i + 1]));
}
.DrawString(one, font, brush, 95.0F, 10.0F);.DrawLine(redpen,
new Point(175, 20), new Point(205, 20));
.DrawString(two, font, brush, 245.0F,
10.0F);.DrawLine(bluepen, new Point(325, 20), new Point(355,
20));.DrawString(three, font, brush, 395.0F, 10.0F);.DrawLine(greenpen, new
Point(475, 20), new Point(505, 20));
(g, blackpen, brush);
.Dispose();.Dispose();.Dispose();.Dispose();.Dispose();.Dispose();
}
}
#endregion
#region Кнопки
void buttonDecart_Click(object sender, EventArgs e)
{(sender, e);
}
void buttonObobs_Click(object sender, EventArgs e)
{(sender, e);
}
void buttonSkor_Click(object sender, EventArgs e)
{(sender, e);
}
void buttonUskor_Click(object sender, EventArgs e)
{(sender, e);
}
#endregion
}
}
траектория координата
декартовый манипулятор
5.
РЕЗУЛЬТАТЫ ВЫПОЛНЕНИЯ ПРОГРАММЫ
Результаты выполнения программы представлены на рисунках .5.1 - 5.5. На
рис.5.1 показан экран программы после планирования траектории с изображенным
манипулятором, траекторией движения, промежуточными значениями и исходными
данными.
Рисунок 5.1- Визуализация движения манипулятора в системе декартовых
координат (начальное положение)
Рисунок 5.2-Визуализация движения манипулятора в системе декартовых координат
(конечное положение).
Рисунок 5.3 - Графики обобщенных координат звеньев.
Рисунок 5.4-Графики скоростей звеньев
Рисунок 5.5- Графики ускорений звеньев
ЛИТЕРАТУРА
1. К. Фу, Р.
Гонсалес, К. Ли. Робототехника. - М.: Мир, 1989. - 624 с.
. М.
Шахинпур. Курс робототехники. - М.: Мир, 1990.- 527 с.
. Механика
промышленных роботов. Под ред. К.В. Фролова,
Е.И.
Воробьева. Кн. 1. Кинематика и динамика. - М.: Высшая школа, 1988. - 304 с.
. Г. Корн, Т.
Корн. Справочник по математике. - М.: Мир,
. - 832 с.
. Программное
обеспечение робототехнических систем. Курс лекций.
. Программное
обеспечение робототехнических систем. Лабораторный практикум.
. Шилдт Г. С#
4.0: полное руководство. : Пер. с англ. - М. : ООО "И.Д. Вильяме",
2011. - 1056 с.: ил. - Парал. тит. англ.