5. Построение траектории перемещения двухзвенного робота-манипулятора с помощью элементов символьной математики.

5.1. Решение уравнений в символьной форме.

Инструментарий символьной математики в среде MATLAB позволяет получать решения широкого класса задач в символьной форме. Остановимся на символьной форме решения алгебраических, трансцендентных и тригонометрических уравнений. В режиме прямых вычислений сначала необходимо ввести символьное выражение "syms x", затем ввести функцию "solve(‘E’)", обеспечивающую решение уравнения Е в символьной форме.

Пример 5.1.

Решим алгебраическое уравнение: Х + 5 = 0.

>>syms x %Объявление символьного выражения.

>>solve ('x + 5 = 0') %Решение уравнения в символьной форме.

ans =

-5

Пример 5.2.

Решим трансцендентное уравнение: .

>>symsx

>> solve ('exp(2*x) + 3*exp(x)=54')

ans =

[ log(-9)] %Получаем два решения

[ log(6)]

Далее вычисляем:

>>log(-9) %ln (-9)

ans =

2.1972 + 3.1416i %Комплексное число

>>log(6) %ln (6)

ans =

1.7918 %Действительное число

Из двух решений мы должны выбрать одно, выбираем:

x=1.7918 (действительное число).

Пример 5.3.

Решим систему уравнений:

>>s=solve('6*x+2*y=14',…

'3*x+7*y=31')

s =

x: [1x1 sym] %Одно решение для x.

y: [1x1 sym] %Одно решение для y.

>>s.x

ans =

1 %x=1

>>s.y

ans =

4 %y=4

Уравнения, содержащие период функции тригонометрического выражения, могут иметь бесконечное число решений. В таких случаях функция «solve» ограничивается решениями, расположенными вблизи нуля.

Пример 5.4

Решим уравнение: sin2x – cosx = 0.

>> solve ('sin(2*x)-cos(x)=0')

ans =

[ 1/2*pi] % 1,5708

[ -1/2*pi] % -1,5708

[ 1/6*pi] % 0,5236

[ 5/6*pi] % 2,6180

5.2. Позиционирование двухзвенного робота – манипулятора.

В настоящее время многие судовые краны для перегрузки контейнеров конструктивно выполнены по схеме двухзвенного робота- манипулятора (фирмы "KONE", "LIEBHERR"). Поэтому весьма актуальной является задача о точности наведения спредера и установки контейнера на грузовое место.

Рассмотрим решение задачи о позиционированиидрухзвенного робота-манипулятора, обеспечиваемом с помощью двух двигателей, расположенных в базовом узле и сочленении звеньев, путем поддержания требуемых значений углов Θ₁, Θ₂ в зависимости от положения "руки".

На рис.5.1 изображена кинематическая схема манипулятора, в которой заданы звенья манипулятора L₁ = 4, L₂ = 3 в метрах и координаты захвата груза x = 6, y = 2.

Рис.5.1 Кинематическая схема робота-манипулятора

Необходимо решить две задачи:

1.Вычислить углы Θ₁ и Θ₂ для заданных координат захвата.

2.Переместить захват манипулятора по прямой линии для случая, когда: х = 6 = const, и y изменяется в пределах 1… 3,6. Получить графики изменения углов Θ₁ и Θ₂ поворота приводов, в функции y= 1… 3,6.

Для решения первой задачи запишем систему уравнений:

(5.1)

В режиме прямых вычислений запишем решение уравнений (5.1) символьным методом:

>>S=solve('4*cos(tet1)+3*cos(tet1+tet2)=6',...

'4*sin(tet1)+3*sin(tet1+tet2)=2')

S =

tet1: [2x1 sym] %Два решения для Θ₁.

tet2: [2x1 sym] %Два решения для Θ₂.

>>double(S.tet1)*(180/pi) %Преобразование радиан в градусы.

ans =

-3.2981 % Θ₁, нижнее положение звена L₁.

40.1680 % Θ₁, верхнее положение звена L₁.

>>double(S.tet2)*(180/pi)

ans =

51.3178 % Θ₂, нижнее положение звена L₂.

-51.3178 % Θ₂, верхнее положение звена L₂.

Таким образом, имеем два решения:

первое решение Θ₁ = - 3,2981^о, Θ₂ = 51,3178^о (нижнее положение);

второе решение Θ₁ = 40,1680^о, Θ₂ = -51,3178^о (верхнее положение).

Для построения графика положений звеньев манипулятора в режиме прямых вычислений, составим программу:

>>xk=6; yk=2;

>> tet1=(pi/180)*40.1680;

>> tet10=(pi/180)*(-3.2981);

>> x=[0 4*cos(tet1) xk 4*cos(tet10) 0];

>> y=[0 4*sin(tet1) yk 4*sin(tet10) 0];

>>plot(x,y,x,y,'o'),grid

>>axis([-0.5 6.5 -0.5 3]);

>>xlabel('X(мм)'),ylabel(' Y(мм) ')

Рис.5.2 График положений звеньев манипулятора

Для получения графиков изменения углов поворота манипулятора, в функции ''y'', запишем программу в символьной форме в режиме прямых вычислений:

>> S=solve('4*cos(tet1)+3*cos(tet1+tet2)=6',...

'4*sin(tet1)+3*sin(tet1+tet2)=y','tet1','tet2');

>>yt=1:0.1:3.6; %Вектор изменения y.

>>v=S.tet1; %Два решения для Θ₁: v(1), v(2).

>>w=S.tet2; %Два решения для Θ₂: w(1), w(2).

>>tetr1=subs(v(1),'y',yt); %Одновременная выдача нескольких

>>tetr2=subs(v(2),'y',yt); %параметров в зависимости от yдля

>> tetr3=subs(w(1),'y',yt); %решений v(1), v(2), w(1), w(2).

>> tetr4=subs(w(2),'y',yt);

>>tet1r=(180/pi)*double([tetr1;tetr2]);%Преобразование израдиан в градусы %решений v(1), v(2).

>>tet2r=(180/pi)*double([tetr3;tetr4]);%Преобразование израдиан в градусы %решений w(1), w(2).

>>subplot(2,1,1) %Изображение 2-х рядов 1 колонки в %верхней части графика.

>>plot(yt,tet1r,2,-3.2981,'*',2,40.1680,'o'),grid

>>xlabel('y (m)'),ylabel(' Teta1(degrees) ')

>>subplot(2,1,2) %Изображение 2-х рядов 1 колонки в %нижней части графика.

>>plot(yt,tet2r,2,-51.3178,'o',2,51.3178,'*'),grid

>>xlabel(' y (m)'),ylabel(' Teta2 (degrees) ')

Рис.5.3 Графики изменения углов поворота манипулятора, в функции y

На основании выше изложенного, для исходных данных:

составим два файла pt6.m и pt7.m. В файле pt6.m составлена программа для вычисления и построения углов Θ₁ и Θ₂ . В файле pt7.m составлена программа для построения графиков изменения углов поворота звеньев манипулятора, как функции yt = 1000:20:4860.

Лабораторная работа №6. "Определение углов поворота и построение положений звеньев робота - манипулятора с помощью элементов символьной математики"

% Robotic system.

% File pt6.m

S=solve('3500*cos(tet1)+2800*cos(tet1+tet2)=4000',...

'3500*sin(tet1)+2800*sin(tet1+tet2)=3000')

double(S.tet1)*(180/pi) % Angles in degrees

double(S.tet2)*(180/pi)

xk=4000;

yk=3000; %Вариант 1.

%yk=1000; %Вариант 2.

%уk=4860; %Вариант 3.

tet1=(pi/180)*69.6996;tet10=(pi/180)*4.0402;

x=[0 3500*cos(tet1) xk 3500*cos(tet10) 0];

y=[0 3500*sin(tet1) yk 3500*sin(tet10) 0];

plot(x,y,x,y,'o'),grid

axis([0 4500 0 3500]);

xlabel('X(мм)'),ylabel(' Y(мм) ')

Решение варианта 1:

S =

tet1: [2x1 sym]

tet2: [2x1 sym]

ans =

4.0402

69.6996

ans =

75.4923

-75.4923

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

Лабораторная работа №7. "Построение графиков положений звеньев робота –манипулятора при постоянной координате ''х'' и изменяющейся координате ''y'' захвата груза"

% Roboticsystem.

% Solutions for motor angles in terms of variable y.

% File pt7.m

S=solve('3500*cos(tet1)+2800*cos(tet1+tet2)=4000',...

'3500*sin(tet1)+2800*sin(tet1+tet2)=y','tet1','tet2');

yt=1000:20:4860;

v=S.tet1;

w=S.tet2;

tetr1=subs(v(1),'y',yt);

tetr2=subs(v(2),'y',yt);

tetr3=subs(w(1),'y',yt);

tetr4=subs(w(2),'y',yt);

tet1r=(180/pi)*double([tetr1;tetr2]);

tet2r=(180/pi)*double([tetr3;tetr4]);

subplot(2,1,1)

plot(yt,tet1r,3000,4.0402,'x',3000,69.6996,'o'),grid

xlabel('y (millimetres)'),ylabel(' Teta1(degrees) ')

subplot(2,1,2)

plot(yt,tet2r,3000,-75.4923,'o',3000,75.4923,'x'),grid

xlabel(' y (millimetres)'),ylabel(' Teta2 (degrees) ')

Рис.5.5 Графики изменения углов поворота звеньев робота–манипулятора при постоянной координате ''х'' и изменяющейсякоординате ''y''.