Моделирование движения плоскостного манипулятора
Построение графиков положения, скорости и ускорения звеньев манипулятора в обобщенной системе координат. Визуализация движения робота в декартовой системе координат. Планирование траектории в обобщенных координатах методом сплайн-интерполяции Лагранжа.
| Рубрика | Экономико-математическое моделирование |
| Вид | курсовая работа |
| Язык | русский |
| Дата добавления | 30.09.2013 |
| Размер файла | 745,8 K |
Отправить свою хорошую работу в базу знаний просто. Используйте форму, расположенную ниже
Студенты, аспиранты, молодые ученые, использующие базу знаний в своей учебе и работе, будут вам очень благодарны.
Размещено на http://www.allbest.ru/
Размещено на http://www.allbest.ru/
Министерство образования РБ
БЕЛОРУССКИЙ НАЦИОНАЛЬНЫЙ ТЕХНИЧЕСКИЙ УНИВЕРСИТЕТ
Кафедра "Робототехнические системы"
ПОЯСНИТЕЛЬНАЯ ЗАПИСКА
к курсовой работе по дисциплине:
Математическое обеспечение ПР
НА ТЕМУ:
«МОДЕЛИРОВАНИЕ ДВИЖЕНИЯ ПЛОСКОСТНОГО МАНИПУЛЯТОРА»
Исполнитель Ходжаев Т. А.
Руководитель Околов А.Р.
Минск 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 |
|
|
0 |
20 |
60 |
90 |
||
|
0 |
0 |
0 |
0 |
||
|
0 |
0 |
0 |
0 |
L1=6, L2=3,
где t - время прохождения промежуточных опорных точек;
x и y - положение схвата в опорные моменты времени;
v - скорость сочленений (звеньев) в опорные моменты времени;
, , - углы Эйлера, задающие ориентацию схвата.
2. РЕШЕНИЕ ЗАДАЧИ В ОБЩЕМ ВИДЕ
При планировании траектории движения манипулятора в системе обобщенных координат необходимо:
По заданным значениям углов Эйлера , , и положениям схвата (xcx , ycx) в начальной, конечной и промежуточных точках составить матрицы однородных преобразований Tн, Тпр, Тк;
Для начального Tн, конечного Тк и промежуточных Тпр положений рабочего органа манипулятора определить соответствующие значения трех обобщенных координат i (Qнi, Qпрi, Qкi), решив обратную задачу кинематики ;
В соответствии с заданным видом интерполяции построить зависимости, характеризующие положения, скорости и ускорения каждой обобщенной координаты 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 |
d |
Q |
||
|
1 |
0 |
0 |
Q1 |
||
|
2 |
0 |
0 |
Q2 |
||
|
3 |
0 |
0 |
0 |
Q3 |
Матрица, описывающая положение третьего звена (схвата) относительно базовой системы координат, выглядит следующим образом
,
Где с123=cos(Q1+ Q2+ Q3);
s123=sin(Q1+ Q2+ Q3);
c1=cos(Q1);
c12=cos(Q1+ Q2);
s1=sin(Q1);
s12=sin(Q1+ Q2).
Для рассматриваемого манипулятора матрицу можно записать в виде
=
гдес=cos;
s=sin;
Приравняем выражения (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 - Решение ПЗК для манипулятора.
1.-первое звено; 2.-второе звено; 3.-третье звено (схват);
Декартовы координаты точек A(x1,y1) и B(x2,y2) относительно центра базовой системы координат O(0,0) вычисляются с помощью геометрических зависимостей (рисунок 3.3):
x1=l1cosQ1 ; x2=x1+l2cos(Q1+Q2) ;
y1=l1sinQ1 ; y2=y1+l2sin(Q1+Q2) .
Таким образом, определили все необходимые для написания программы и моделирования движения манипулятора формулы.
4. МОДЕЛИРУЮЩАЯ ПРОГРАММА
4.1 Описание программы
Моделирующая программа написана на языке ООП Visual C# 4.0[7] и выполняет следующие операции:
-решение обратной задачи кинематики для заданных контрольных точек (начальной, конечной и промежуточных);
-планирование сглаженной траектории в обобщенных координатах методом интерполяции полиномом Лагранжа;
-решение прямой задачи кинематики в каждой точке геометрическим методом;
-визуализацию движения трехзвенного манипулятора;
-построение графиков зависимостей обобщенных координат q1, q2, q3, а также их скоростей и ускорений от времени.
Программа построена по блочно-модульному типу с использованием многократно вызываемых методов и событий. Моделирующая программа включает в себя следующие основные методы и события:
- TimeIntervals - метод создает массив значений времени, в соответствии со временем прохождения каждого участка (всего 80 элементов);
- Result4x4 - метод, по заданным значениям углов Эйлера , , и положениям схвата () в начальной, конечной и промежуточных точках составляются матрицы однородных преобразований;
- 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;
using System.ComponentModel;
using System.Data;
using System.Drawing;
using System.Drawing.Drawing2D;
using System.Linq;
using System.Text;
using System.Windows.Forms;
namespace курсачМОПР
{
public partial class Form1 : Form
{
public Form1()
{
InitializeComponent();
}
private void Form1_Load(object sender, EventArgs e)
{
}
//Массив для координат X, Y, взятых из условия
double[,] 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;
double l2 = 3.0;
int p1 = 20;
int p2 = 20;
int p3 = 20;
//Углы psi для разных t
double[] psi = { 0, 20.0, 60.0, 90.0 };
double[] speed = { 0, 30.0, 50.0, 0};
double[] time;
//Массив времени t
double[] t = { 0, 1.0, 2.0, 3.0 };
int nx, ny;
#region Методы
//Разбить все время выполнения на одинаковые интервалы
public void TimeIntervals()
{
time = new double[p1 + p2 + p3];
time[0] = t[0];
time[p1 - 1] = t[1];
time[p1 + p2 - 1] = t[2];
time[p1 + p2 + p3 - 1] = t[3];
for (int i = 1; i < p1 - 1; i++)
{
time[i] = time[i - 1] + (t[1] - t[0]) / p1;
}
for (int i = p1; i < p1 + p2 - 1; i++)
{
time[i] = time[i - 1] + (t[2] - t[1]) / p2;
}
for (int i = p1 + p2; i < p1 + p2 + p3 - 1; i++)
{
time[i] = time[i - 1] + (t[3] - t[2]) / p3;
}
}
//Создание матрицы однородных преобразований
public double[,] Result4x4(double psi, double x, double y)
{
int fi = 0;
int eta = 0;
psi = psi * Math.PI / 180;
double[,] arrresult = new double[4, 4];
arrresult[0, 0] = Math.Cos(fi) * Math.Cos(eta);
arrresult[0, 1] = Math.Cos(fi) * Math.Sin(eta) * Math.Sin(psi) - Math.Sin(fi) * Math.Cos(psi);
arrresult[0, 2] = Math.Cos(fi) * Math.Sin(eta) * Math.Cos(psi) + Math.Sin(fi) * Math.Sin(psi);
arrresult[1, 0] = Math.Sin(fi) * Math.Cos(eta);
arrresult[1, 1] = Math.Sin(fi) * Math.Sin(eta) * Math.Sin(psi) - Math.Cos(fi) * Math.Cos(psi);
arrresult[1, 2] = Math.Sin(fi) * Math.Sin(eta) * Math.Cos(psi) - Math.Cos(fi) * Math.Sin(psi);
arrresult[2, 0] = -Math.Sin(eta);
arrresult[2, 1] = Math.Cos(eta) * Math.Sin(psi);
arrresult[2, 2] = Math.Cos(eta) * Math.Cos(psi);
arrresult[0, 3] = x;
arrresult[1, 3] = y;
arrresult[2, 3] = 0;
arrresult[3, 0] = 0;
arrresult[3, 1] = 0;
arrresult[3, 2] = 0;
arrresult[3, 3] = 1;
return arrresult;
}
//Обратная задача кинематики(найти углы Q1, Q2, Q3 в градусах)
public double[,] OZK()
{
double c2, s2;
double[,] qr = new double[3, realXY.GetLength(1)];
for (int i = 0; i < realXY.GetLength(1); i++)
{
c2 = (realXY[0, i] * realXY[0, i] + realXY[1, i] * realXY[1, i] - l1 * l1 - l2 * l2) / (2 * l1 * l2);
s2 = Math.Sqrt(1 - c2 * c2);
if (c2 > 0) qr[1, i] = 180 / Math.PI * Math.Atan(s2 / c2);
if (c2 < 0) qr[1, i] = 180 + 180 / Math.PI * Math.Atan(s2 / c2);
qr[0, i] = 180 / Math.PI * Math.Atan(realXY[1, i] / realXY[0, i]) - 180 / (Math.PI) * Math.Atan((l2 * s2) / (l1 + l2 * c2));
qr[2, i] = psi[i] - qr[0, i] - qr[1, i];
}
return qr;
}
//Сплайн интерполяция
public double[,] Spline(double[,] arr)
{
TimeIntervals();
double[,] splineQR = new double[3, p1 + p2 + p3];
double tau, tm, k0, k1, k2, k3;
double[,] mass = OZK();
int min = 0;
int max = 80;
for (int j = 1; j < 4; j++)
{
if (j == 1) { min = 0; max = p1; }
if (j == 2) { min = p1-1; max = p1+p2; }
if (j == 3) { min = p1+p2-1; max = p1+p2+p3; }
for (int i = min; i < max; i++)
{
for (int k = 0; k < 3; k++)
{
tau = time[i] - t[j - 1];
tm = t[j] - t[j - 1];
k0 = mass[k, j - 1];
k1 = speed[j - 1];
k2 = (3 * mass[k, j] - 3 * mass[k, j - 1] - 2 * speed[j - 1] * tm - speed[j] * tm) / (tm * tm);
k3 = (2 * mass[k, j - 1] - 2 * mass[k, j] + speed[j] * tm + speed[j - 1] * tm) / (tm * tm * tm);
splineQR[k, i] = k3 * tau * tau * tau + k2 * tau * tau + k1 * tau + k0;
}
}
}
return splineQR;
}
//Нахождение координат скорости
public double[,] SplineSpeed(double[,] arr)
{
TimeIntervals();
double[,] splineQR = new double[3, p1 + p2 + p3];
double tau, tm, k0, k1, k2, k3;
double[,] mass = OZK();
int min = 0;
int max = 80;
for (int j = 1; j < 4; j++)
{
if (j == 1) { min = 0; max = p1; }
if (j == 2) { min = p1 - 1; max = p1 + p2; }
if (j == 3) { min = p1 + p2 - 1; max = p1 + p2 + p3; }
for (int i = min; i < max; i++)
{
for (int k = 0; k < 3; k++)
{
tau = time[i] - t[j - 1];
tm = t[j] - t[j - 1];
k0 = mass[k, j - 1];
k1 = speed[j - 1];
k2 = (3 * mass[k, j] - 3 * mass[k, j - 1] - 2 * speed[j - 1] * tm - speed[j] * tm) / (tm * tm);
k3 = (2 * mass[k, j - 1] - 2 * mass[k, j] + speed[j] * tm + speed[j - 1] * tm) / (tm * tm * tm);
splineQR[k, i] = 3*k3 * tau * tau + 2*k2 * tau + k1;
}
}
}
return splineQR;
}
//Нахождение координат ускорения
public double[,] SplineAcceleration(double[,] arr)
{
TimeIntervals();
double[,] splineQR = new double[3, p1 + p2 + p3];
double tau, tm, k0, k1, k2, k3;
double[,] mass = OZK();
int min = 0;
int max = 80;
for (int j = 1; j < 4; j++)
{
if (j == 1) { min = 0; max = p1; }
if (j == 2) { min = p1 - 1; max = p1 + p2; }
if (j == 3) { min = p1 + p2 - 1; max = p1 + p2 + p3; }
for (int i = min; i < max; i++)
{
for (int k = 0; k < 3; k++)
{
tau = time[i] - t[j - 1];
tm = t[j] - t[j - 1];
k0 = mass[k, j - 1];
k1 = speed[j - 1];
k2 = (3 * mass[k, j] - 3 * mass[k, j - 1] - 2 * speed[j - 1] * tm - speed[j] * tm) / (tm * tm);
k3 = (2 * mass[k, j - 1] - 2 * mass[k, j] + speed[j] * tm + speed[j - 1] * tm) / (tm * tm * tm);
splineQR[k, i] = 6*k3 * tau + 2*k2;
}
}
}
return splineQR;
}
//Прямая задача кинематики(найти координаты точек манипулятора)
public double[,] PZK(double[,] arr)
{
double[,] pzkXY = new double[8, p1 + p2 + p3];
//Перевод градусов в радианы
for (int i = 0; i < 3; i++)
{
for (int j = 0; j < p1+p2+p3; j++)
{
arr[i, j] = arr[i, j] * Math.PI / 180;
}
}
for (int i = 0; i < p1 + p2 + p3; i++)
{
//Координаты точки 1
pzkXY[0, i] = l1 * Math.Cos(arr[0, i]);
pzkXY[1, i] = l1 * Math.Sin(arr[0, i]);
//Координаты точки 2
pzkXY[2, i] = l1 * Math.Cos(arr[0, i]) + l2 * Math.Cos(arr[0, i] + arr[1, i]);
pzkXY[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]);
pzkXY[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);
pzkXY[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);
}
return pzkXY;
}
//Примеры вызова PZK
//double[,] mass = PZK(Spline(OZK()));
#endregion
#region Рисование
public void DrawDimension(Graphics g, Pen blackpen, SolidBrush brush)
{
Font font = new Font("Arial", 9);
string x, y;
int ii = 0;
int j = 19;
for(int k = 0; k < 3; k++) {
g.DrawLine(blackpen, new Point(ii + 25 + nx, 270), new Point(ii + 25 + nx, 280));
x = time[j].ToString();
g.DrawString(x, font, brush, ii + 25 + nx - 5, 290.0F);
ii = ii + nx;
j = j + 20;
}
ii = 275;
for (int k = 0; k < 6; k++)
{
g.DrawLine(blackpen, new Point(20, ii - ny), new Point(30, ii - ny));
y = (k + 1).ToString();
g.DrawString(y, font, brush, 5, ii - ny - 5);
ii = ii - ny;
}
ii = 275;
for (int k = 0; k < 6; k++)
{
g.DrawLine(blackpen, new Point(20, ii + ny), new Point(30, ii + ny));
y = (- k - 1).ToString();
g.DrawString(y, font, brush, 5, ii + ny - 5);
ii = ii + ny;
}
x = "0";
g.DrawString(x, font, brush, 5, 270.0F);
font.Dispose();
}
private void DrawManipulator(object sender, EventArgs e)
{
nx = 35;
ny = 35;
double[,] promD = PZK(Spline(OZK()));
int[,] prom = new int[promD.GetLength(0), promD.GetLength(1)];
for (int i = 0; i < promD.GetLength(1); i++) {
prom[0, i] = (int)(promD[0, i] * nx) + 300;
prom[1, i] = 275 - (int)(promD[1, i] * ny);
prom[2, i] = (int)(promD[2, i] * nx) + 300;
prom[3, i] = 275 - (int)(promD[3, i] * ny);
prom[4, i] = (int)(promD[4, i] * nx) + 300;
prom[5, i] = 275 - (int)(promD[5, i] * ny);
prom[6, i] = (int)(promD[6, i] * nx) + 300;
prom[7, i] = 275 - (int)(promD[7, i] * ny);
}
using (Graphics g = this.CreateGraphics())
{
Pen blackpen = new Pen(Color.Black, 5);
Pen bluepen = new Pen(Color.Blue, 1);
Pen bp = new Pen(Color.Black, 1);
Pen red = new Pen(Color.Red, 2);
Font font = new Font("Arial", 14);
Font f = new Font("Arial", 8);
SolidBrush brush = new SolidBrush(Color.Black);
string osx = "X";
string osy = "Y";
string op;
for (int i = 0; i < p1+p2+p3; i++)
{
g.Clear(Color.White);
//Начертить звенья
g.DrawLine(blackpen, new Point(300, 275), new Point(prom[0, i], prom[1, i]));
g.DrawLine(blackpen, new Point(prom[0, i], prom[1, i]), new Point(prom[2, i], prom[3, i]));
g.DrawLine(bluepen, new Point(prom[2, i], prom[3, i]), new Point(prom[4, i], prom[5, i]));
g.DrawLine(bluepen, new Point(prom[2, i], prom[3, i]), new Point(prom[6, i], prom[7, i]));
//Начертить оси
g.DrawLine(bp, new Point(300, 25), new Point(300, 475));
g.DrawLine(bp, new Point(25, 275), new Point(575, 275));
//Стрелки
g.DrawLine(bp, new Point(300, 25), new Point(288, 50));
g.DrawLine(bp, new Point(300, 25), new Point(312, 50));
g.DrawLine(bp, new Point(575, 275), new Point(550, 263));
g.DrawLine(bp, new Point(575, 275), new Point(550, 287));
//Подпись
g.DrawString(osx, font, brush, 580.0F, 275.0F);
g.DrawString(osy, font, brush, 305.0F, 10.0F);
//Подписать доп. оси
g.DrawString(osx, f, brush, prom[4, i] + 5, prom[5, i] - 5);
g.DrawString(osy, f, brush, prom[6, i] + 5, prom[7, i] - 5);
//Обозначить пересечения звеньев
g.DrawEllipse(blackpen, prom[2, i], prom[3, i], 5.0F, 5.0F);
g.DrawEllipse(blackpen, 300, 275, 5.0F, 5.0F);
g.DrawEllipse(blackpen, prom[0, i], prom[1, i], 5.0F, 5.0F);
//Нарисовать схват
g.DrawLine(blackpen, new Point(prom[2, i], prom[3, i]), new Point(prom[2, i] - 10, prom[3, i] - 10));
g.DrawLine(blackpen, new Point(prom[2, i], prom[3, i]), new Point(prom[2, i] + 10, prom[3, i] - 10));
//Подписать оси X
for (int ii = 300 + nx; ii <= 575; ) {
g.DrawLine(bp, new Point(ii, 270), new Point(ii, 280));
op = ((ii - 300)/nx).ToString();
g.DrawString(op, f, brush, ii - 5, 290);
ii += nx;
}
for (int ii = 300 - nx; ii >= 25; )
{
g.DrawLine(bp, new Point(ii, 270), new Point(ii, 280));
op = ((ii - 300) / nx).ToString();
g.DrawString(op, f, brush, ii - 5, 290);
ii -= nx;
}
//Подписать оси Y
for (int ii = 275 - ny; ii >= 25; )
{
g.DrawLine(bp, new Point(295, ii), new Point(305, ii));
op = (Math.Abs(ii - 275) / ny).ToString();
g.DrawString(op, f, brush, 280, ii - 5);
ii -= ny;
}
for (int ii = 275 + ny; ii <= 475; )
{
g.DrawLine(bp, new Point(295, ii), new Point(305, ii));
op = ((- ii + 275) / ny).ToString();
g.DrawString(op, f, brush, 280, ii - 5);
ii += ny;
}
op = "0";
g.DrawString(op, f, brush, 280, 260);
System.Threading.Thread.Sleep(30);
}
//Траектория движения и опорные точки
for (int i = 0; i < p1 + p2 + p3; i++ )
{
g.DrawEllipse(red, prom[2, i], prom[3, i], 2.0F, 2.0F);
}
g.DrawEllipse(blackpen, prom[2, 0], prom[3, 0], 6.0F, 6.0F);
g.DrawEllipse(blackpen, prom[2, p1 - 1], prom[3, p1 - 1], 6.0F, 6.0F);
g.DrawEllipse(blackpen, prom[2, p1 + p2 - 1], prom[3, p1 + p2 - 1], 6.0F, 6.0F);
g.DrawEllipse(blackpen, prom[2, p1 + p2 + p3 - 1], prom[3, p1 + p2 + p3 - 1], 6.0F, 6.0F);
red.Dispose();
bp.Dispose();
blackpen.Dispose();
bluepen.Dispose();
font.Dispose();
brush.Dispose();
}
}
private void DrawPosition(object sender, EventArgs e)
{
nx = 165;
ny = 80;
double[,] promD = Spline(OZK());
int[,] prom = new int[promD.GetLength(0) * 2, promD.GetLength(1)];
for (int i = 0; i < promD.GetLength(1); i++)
{
prom[0, i] = 25 + (int)(time[i] * nx);
prom[1, i] = 275 - (int)(promD[0, i] * (Math.PI / 180) * ny);
prom[2, i] = 25 + (int)(time[i] * nx);
prom[3, i] = 275 - (int)(promD[1, i] * (Math.PI / 180) * ny);
prom[4, i] = 25 + (int)(time[i] * nx);
prom[5, i] = 275 - (int)(promD[2, i] * (Math.PI / 180) * ny);
}
using (Graphics g = this.CreateGraphics()) {
Pen blackpen = new Pen(Color.Black, 1);
Pen redpen = new Pen(Color.Red, 2);
Pen bluepen = new Pen(Color.Blue, 2);
Pen greenpen = new Pen(Color.Green, 2);
Font font = new Font("Arial", 14);
SolidBrush brush = new SolidBrush(Color.Black);
string osx = "t,с";
string osy = "Q(t),рад";
string one = "Звено 1";
string two = "Звено 2";
string three = "Звено 3";
g.Clear(Color.White);
g.DrawLine(blackpen, new Point(25, 25), new Point(25, 475));
g.DrawLine(blackpen, new Point(25, 275), new Point(575, 275));
g.DrawLine(blackpen, new Point(25, 25), new Point(13, 50));
g.DrawLine(blackpen, new Point(25, 25), new Point(37, 50));
g.DrawLine(blackpen, new Point(575, 275), new Point(550, 263));
g.DrawLine(blackpen, new Point(575, 275), new Point(550, 287));
g.DrawString(osx, font, brush, 580.0F, 275.0F);
g.DrawString(osy, font, brush, 15.0F, 0.0F);
for (int i = 0; i < promD.GetLength(1) - 1; i++) {
g.DrawLine(redpen, new Point(prom[0, i], prom[1, i]), new Point(prom[0, i + 1], prom[1, i + 1]));
}
for (int i = 0; i < promD.GetLength(1) - 1; i++)
{
g.DrawLine(bluepen, new Point(prom[2, i], prom[3, i]), new Point(prom[2, i + 1], prom[3, i + 1]));
}
for (int i = 0; i < promD.GetLength(1) - 1; i++)
{
g.DrawLine(greenpen, new Point(prom[4, i], prom[5, i]), new Point(prom[4, i + 1], prom[5, i + 1]));
}
g.DrawString(one, font, brush, 95.0F, 10.0F);
g.DrawLine(redpen, new Point(175, 20), new Point(205, 20));
g.DrawString(two, font, brush, 245.0F, 10.0F);
g.DrawLine(bluepen, new Point(325, 20), new Point(355, 20));
g.DrawString(three, font, brush, 395.0F, 10.0F);
g.DrawLine(greenpen, new Point(475, 20), new Point(505, 20));
DrawDimension(g, blackpen, brush);
redpen.Dispose();
blackpen.Dispose();
bluepen.Dispose();
greenpen.Dispose();
font.Dispose();
brush.Dispose();
}
}
private void DrawSpeed(object sender, EventArgs e) {
nx = 165;
ny = 80;
TimeIntervals();
double[,] promD = SplineSpeed(OZK());
int[,] prom = new int[promD.GetLength(0) * 2, promD.GetLength(1)];
for (int i = 0; i < promD.GetLength(1); i++)
{
prom[0, i] = 25 + (int)(time[i] * nx);
prom[1, i] = 275 - (int)(promD[0, i] * (Math.PI / 180) * ny);
prom[2, i] = 25 + (int)(time[i] * nx);
prom[3, i] = 275 - (int)(promD[1, i] * (Math.PI / 180) * ny);
prom[4, i] = 25 + (int)(time[i] * nx);
prom[5, i] = 275 - (int)(promD[2, i] * (Math.PI / 180) * ny);
}
using (Graphics g = this.CreateGraphics())
{
Pen blackpen = new Pen(Color.Black, 1);
Pen redpen = new Pen(Color.Red, 2);
Pen bluepen = new Pen(Color.Blue, 2);
Pen greenpen = new Pen(Color.Green, 2);
Font font = new Font("Arial", 14);
SolidBrush brush = new SolidBrush(Color.Black);
string osx = "t,с";
string osy = "Q'(t),рад";
string one = "Звено 1";
string two = "Звено 2";
string three = "Звено 3";
g.Clear(Color.White);
g.DrawLine(blackpen, new Point(25, 25), new Point(25, 475));
g.DrawLine(blackpen, new Point(25, 275), new Point(575, 275));
g.DrawLine(blackpen, new Point(25, 25), new Point(13, 50));
g.DrawLine(blackpen, new Point(25, 25), new Point(37, 50));
g.DrawLine(blackpen, new Point(575, 275), new Point(550, 263));
g.DrawLine(blackpen, new Point(575, 275), new Point(550, 287));
g.DrawString(osx, font, brush, 580.0F, 275.0F);
g.DrawString(osy, font, brush, 15.0F, 0.0F);
for (int i = 0; i < promD.GetLength(1) - 1; i++)
{
g.DrawLine(redpen, new Point(prom[0, i], prom[1, i]), new Point(prom[0, i + 1], prom[1, i + 1]));
}
for (int i = 0; i < promD.GetLength(1) - 1; i++)
{
g.DrawLine(bluepen, new Point(prom[2, i], prom[3, i]), new Point(prom[2, i + 1], prom[3, i + 1]));
}
for (int i = 0; i < promD.GetLength(1) - 1; i++)
{
g.DrawLine(greenpen, new Point(prom[4, i], prom[5, i]), new Point(prom[4, i + 1], prom[5, i + 1]));
}
g.DrawString(one, font, brush, 95.0F, 10.0F);
g.DrawLine(redpen, new Point(175, 20), new Point(205, 20));
g.DrawString(two, font, brush, 245.0F, 10.0F);
g.DrawLine(bluepen, new Point(325, 20), new Point(355, 20));
g.DrawString(three, font, brush, 395.0F, 10.0F);
g.DrawLine(greenpen, new Point(475, 20), new Point(505, 20));
DrawDimension(g, blackpen, brush);
redpen.Dispose();
blackpen.Dispose();
bluepen.Dispose();
greenpen.Dispose();
font.Dispose();
brush.Dispose();
}
}
private void DrawAcceleration(object sender, EventArgs e)
{
nx = 165;
ny = 60;
TimeIntervals();
double[,] promD = SplineAcceleration(OZK());
int[,] prom = new int[promD.GetLength(0) * 2, promD.GetLength(1)];
for (int i = 0; i < promD.GetLength(1); i++)
{
prom[0, i] = 25 + (int)(time[i] * nx);
prom[1, i] = 275 - (int)(promD[0, i] * (Math.PI / 180) * ny);
prom[2, i] = 25 + (int)(time[i] * nx);
prom[3, i] = 275 - (int)(promD[1, i] * (Math.PI / 180) * ny);
prom[4, i] = 25 + (int)(time[i] * nx);
prom[5, i] = 275 - (int)(promD[2, i] * (Math.PI / 180) * ny);
}
using (Graphics g = this.CreateGraphics())
{
Pen blackpen = new Pen(Color.Black, 1);
Pen redpen = new Pen(Color.Red, 2);
Pen bluepen = new Pen(Color.Blue, 2);
Pen greenpen = new Pen(Color.Green, 2);
Font font = new Font("Arial", 14);
SolidBrush brush = new SolidBrush(Color.Black);
string osx = "t,с";
string osy = "Q''(t),рад";
string one = "Звено 1";
string two = "Звено 2";
string three = "Звено 3";
g.Clear(Color.White);
g.DrawLine(blackpen, new Point(25, 25), new Point(25, 475));
g.DrawLine(blackpen, new Point(25, 275), new Point(575, 275));
g.DrawLine(blackpen, new Point(25, 25), new Point(13, 50));
g.DrawLine(blackpen, new Point(25, 25), new Point(37, 50));
g.DrawLine(blackpen, new Point(575, 275), new Point(550, 263));
g.DrawLine(blackpen, new Point(575, 275), new Point(550, 287));
g.DrawString(osx, font, brush, 580.0F, 275.0F);
g.DrawString(osy, font, brush, 15.0F, 0.0F);
for (int i = 0; i < promD.GetLength(1) - 1; i++)
{
g.DrawLine(redpen, new Point(prom[0, i], prom[1, i]), new Point(prom[0, i + 1], prom[1, i + 1]));
}
for (int i = 0; i < promD.GetLength(1) - 1; i++)
{
g.DrawLine(bluepen, new Point(prom[2, i], prom[3, i]), new Point(prom[2, i + 1], prom[3, i + 1]));
}
for (int i = 0; i < promD.GetLength(1) - 1; i++)
{
g.DrawLine(greenpen, new Point(prom[4, i], prom[5, i]), new Point(prom[4, i + 1], prom[5, i + 1]));
}
g.DrawString(one, font, brush, 95.0F, 10.0F);
g.DrawLine(redpen, new Point(175, 20), new Point(205, 20));
g.DrawString(two, font, brush, 245.0F, 10.0F);
g.DrawLine(bluepen, new Point(325, 20), new Point(355, 20));
g.DrawString(three, font, brush, 395.0F, 10.0F);
g.DrawLine(greenpen, new Point(475, 20), new Point(505, 20));
DrawDimension(g, blackpen, brush);
redpen.Dispose();
blackpen.Dispose();
bluepen.Dispose();
greenpen.Dispose();
font.Dispose();
brush.Dispose();
}
}
#endregion
#region Кнопки
private void buttonDecart_Click(object sender, EventArgs e)
{
DrawManipulator(sender, e);
}
private void buttonObobs_Click(object sender, EventArgs e)
{
DrawPosition(sender, e);
}
private void buttonSkor_Click(object sender, EventArgs e)
{
DrawSpeed(sender, e);
}
private void buttonUskor_Click(object sender, EventArgs e)
{
DrawAcceleration(sender, e);
}
#endregion
}
}
траектория координата декартовый манипулятор
5. РЕЗУЛЬТАТЫ ВЫПОЛНЕНИЯ ПРОГРАММЫ
Результаты выполнения программы представлены на рисунках .5.1 - 5.5. На рис.5.1 показан экран программы после планирования траектории с изображенным манипулятором, траекторией движения, промежуточными значениями и исходными данными.
Рисунок 5.1- Визуализация движения манипулятора в системе декартовых координат (начальное положение)
Рисунок 5.2-Визуализация движения манипулятора в системе декартовых координат (конечное положение).
Рисунок 5.3 - Графики обобщенных координат звеньев.
Рисунок 5.4-Графики скоростей звеньев
Рисунок 5.5- Графики ускорений звеньев
ЛИТЕРАТУРА
1. К. Фу, Р. Гонсалес, К. Ли. Робототехника. - М.: Мир, 1989. - 624 с.
2. М. Шахинпур. Курс робототехники. - М.: Мир, 1990.- 527 с.
3. Механика промышленных роботов. Под ред. К.В. Фролова,
Е.И. Воробьева. Кн. 1. Кинематика и динамика. - М.: Высшая школа, 1988. - 304 с.
4. Г. Корн, Т. Корн. Справочник по математике. - М.: Мир,
1974. - 832 с.
5. Программное обеспечение робототехнических систем. Курс лекций.
6. Программное обеспечение робототехнических систем. Лабораторный практикум.
7. Шилдт Г. С# 4.0: полное руководство. : Пер. с англ. -- М. : ООО "И.Д. Вильяме", 2011. -- 1056 с.: ил. -- Парал. тит. англ.
Размещено на Allbest.ru
Подобные документы
Проведение регрессионного анализа опытных данных в среде Excel. Построение графиков полиномиальной зависимости и обобщенной функции желательности Харрингтона. Определение дисперсии коэффициентов регрессии. Оценка частных откликов по шкале желательности.
контрольная работа [375,6 K], добавлен 21.01.2014Характеристика направлений перевозок и флота. Расчет нормативов работы судов на схемах движения. Составление математической модели задачи. Нахождение оптимального плана работы флота и оптимальных схем движения судов, построение симплекс таблицы.
курсовая работа [1,4 M], добавлен 24.10.2012Концентрация на складах, как одна их основных составляющих перемещения материальных потоков в логистической цепи. Методы определения координат оптимального расположения двух складов минеральной воды в городе Липецке. Итерационный алгоритм поиска складов.
курсовая работа [2,9 M], добавлен 18.12.2010Сведения о методе скользящей средней, коэффициенте линейной парной корреляции, регрессионном анализе. Построение графиков изменения значений показателей по данным варианта. Обработка динамических рядов методом скользящей средней и построение графиков.
курсовая работа [614,4 K], добавлен 08.06.2012Порядок и разновидности соединений звеньев, их характеристика и отличительные черты. Амплитудно-частотные характеристики при различных соединениях, порядок их расчета и анализа. Методика и этапы моделирования последовательного соединения звеньев.
лабораторная работа [44,0 K], добавлен 26.04.2009Построение графиков сечений заданных поверхностей с помощью экспериментальных данных, полученных при моделировании электропотенциального поля в проводящей среде эквипотенциальных поверхностей. Построение графика распределения разностей потенциалов.
контрольная работа [160,0 K], добавлен 18.11.2013Описание моделей деятельности предприятия для выбора траектории устойчивого развития. Анализ ООО "У Каравая"; прогнозирование и планирование его развития и обеспечения конкурентоспособности на основе информационных моделей. Создание систем менеджмента.
курсовая работа [922,5 K], добавлен 26.07.2015Построение и изучение математической модели случайного стационарного эргодического процесса с вероятностными характеристиками: ожидание и дисперсия. Построение графиков динамики изменения эмпирических данных и гистограмм распределения для всех выборок.
курсовая работа [217,2 K], добавлен 18.03.2012Построение адаптивной мультипликативной модели Хольта-Уинтерса с учетом сезонного фактора. Коммерческий расчет экспоненциально скользящей средней цены с использованием интервала сглаживания. Построение графиков фактических, расчетных и прогнозных данных.
контрольная работа [626,5 K], добавлен 28.04.2011Составление схем моделирования методом последовательного (непосредственного) интегрирования, методом вспомогательной переменной и методом канонической формы. Модель в пространстве состояний в форме простых сомножителей. Моделирование нелинейных систем.
курсовая работа [1,1 M], добавлен 23.12.2013
