Управление схватом

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

Дано: Длина двух звеньев манипулятора L1 = L2 = 8 см. Координаты положения узла схвата – точка В (13, 2.5), см. рисунок 18

Рисунок 18. Обратная задача кинематики. Схват выше крепления

Проведём прямую ОB, соединяющую начало координат O с заданной точкой В (x, y).

ОB^2 = x^2 + y^2

x = ОB*cos(q1)

y = ОB*sin(q1)

где q1 – угол между осью OX и прямой ОB, отсюда

q1 = arccos(x/ОB) илиq1 = arctg(y/x)

Пусть q2 – угол между прямой ОB и плечом L1 отсюда:

Q1 = q1 +q2

Для случая, когда схват манипулятора опускается ниже оси Х (см. Рисунок 19)

Q1 = q2 – q1

Найдем q2 при помощи теоремы косинусов, которая говорит:

Для плоского треугольника со сторонами a,b,c и углом alpha, противолежащим стороне a, справедливо соотношение:

a^2 = b^2 + c^2 - 2*b*c*cos(alpha)

Рисунок 19. Обратная задача кинематики. Схват ниже крепления

в нашем случае, по теореме косинусов:

L2^2 = B^2 + L1^2 - 2*ОB*L1*cos(q2)

=> q2 = arccos(L1^2 - L2^2 + OB^2 / 2*OB*L1)=>

q2 = arccos(L^2 - L^2 + OB^2 / 2*OB*L)=>

q2 = arcos(OB / 2L)

Q1 = q1 + q2 = arccos(x/B) +arccos(OB / 2L)

Т.к. ∆ OAB равносторонний, то угол q1 равен углу q2. По теореме о сумме углов треугольника третий угол равенq3= 180– 2 *q2.

Т.к. угол Q2 и q3 смежные, то их сумма равно 180, следовательно

Q2 = 2 * q2

Т.к. BC параллельна ось ОХ, то

Q3 = 180 – q2 + q1.

Для случая, когда схват манипулятора опускается ниже оси Х

Q3 = 180 – q2 -q1.

Таким образом, получаем углы для шести положений манипулятора:

Таблица 1

	Первая полка		Вторая полка		Третья полка
L	8.00	8.00	L	8.0000	8.0000		8.00	8.00
x	13.00	4.00	x	13.0000	4.0000		13.00	4.00
y	-2.50	-2.50	y	2.5000	2.5000		9.30	9.30
B^2=X^2+Y^2	175.25	22.25	B^2=X^2+Y^2	175.2500	22.2500		255.49	102.49
B	13.24	4.72	B	13.2382	4.7170		15.98	10.12
q1= arccos(x/B)	10.89	32.01	q1= arccos(x/B)	10.8855	32.0054		35.58	66.73
q2=arccos(B/2L)	34.17	72.85	q2=arccos(B/2L)	34.1687	72.8537		2.56	50.75
Q1=q2-q1	23.28	40.85	Q1=q1+q2	45.0542	104.8591		38.14	117.48
Q2=2*q2	68.34	145.71	Q2=2*q2	68.3374	145.7075		5.12	101.50
Q3= 180+q1-q2	134.95	75.14	Q3= 180-q1-q2	156.7168	139.1516		213.02	195.98

Для более плавного и равномерного перемещения из одного положения манипулятора в другое, переход выполняется за 20 шагов.