3. Математическое решение задачи

Первый этап решения – составление матриц однородных преобразований для начальной, конечной и промежуточных точек.

Матрица однородного преобразования, описывающая положение и ориентацию схвата в базовой системе координат Q_i(t), имеет вид:

(2.1)

.

Подставляя значения углов Эйлера и координат схвата в формулу (2.1), получаем матрицы однородных преобразований для начальной, конечной и промежуточных точек траектории:

,

,

,

.

Второй этап – решение обратной задачи кинематики для каждого звена в начальной, конечной и промежуточных точках. Решим ее с точки зрения алгебраического подхода.

Добавляем к манипулятору еще одну степень подвижности (поворот схвата на угол Q₃). Кинематические параметры полученного трехзвенного манипулятора приведены в таблице.2.1.

Таблица.2.1. Кинематические параметры манипулятора

I		a	d	Q
1	0		0	Q1
2	0		0	Q2
3	0	0	0	Q3

Матрица, описывающая положение третьего звена (схвата) относительно базовой системы координат, выглядит следующим образом:

(2.2)

,

где с123=cos(Q₁+ Q₂+ Q₃);

s123=sin(Q₁+ Q₂+ Q₃);

c1=cos(Q₁);

c12=cos(Q₁+ Q₂);

s1=sin(Q₁);

s12=sin(Q₁+ Q₂).

Для рассматриваемого манипулятора матрицу можно записать в виде:

(2.3)

= ,

где с=cos;

s=sin;

Приравняем выражения (2.2) и (2.3) и получим четыре нелинейных уравнения, которые необходимо решить относительно Q₁, Q₂, Q₃.

(2.4)

В

(2.5)

озведя в квадрат, левые и правые части двух последних уравнений истемы (2.4) и складывая их, получим:

.

где с2=cosQ₂.

Решая уравнение (2.5) относительно с2, получим

(2.6)

.

Для существования решения правая часть уравнения (2.6) должна лежать в пределах [-1…+1]. Если условие выполнено, то находим

(2.7)

.

Выбор знака у sinQ₂ соответствует одному из возможных решений. Принимаем положение манипулятора “локоть вниз”, для которого sinQ₂ в данном случае принимает положительные значения. При определении угла Q₂ воспользуемся функцией ATAN2, которая обеспечивает выбор всех решений и выбор квадранта.

О

(2.8)

тсюда Q₂ будет равно

.

Зная Q₂, воспользуемся двумя последними уравнениями системы (2.4) для определения угла Q₁. Перепишем их в следующем виде:

(2.9)

где ;

.

Для решения уравнений системы (2.9) выполним замену переменных:

; ,

тогда

; .

Теперь систему уравнений (2.9) можно записать следующим образом:

(2.10)

Откуда находим

(2.11)

Используя функцию ATAN2, имеем

(2.12)

Из первых двух уравнений системы (2.4) находим

(2.13)

.

Подставляя исходные данные в выражения (2.6) – (2.13) и проведя соответствующий расчет, находим значения обобщенных координат для начальной, конечной и промежуточных точек:

; ; ; .

Третий этап решения – планирование траектории движения манипулятора в обобщенных координатах с учетом интерполяции полиномом Лагранжа, сущность которой заключается в том, что теоретически можно провести непрерывную и гладкую кривую точно через последовательность узловых точек, используя формулу , интерполяции Лагранжа:

где - значения аргумента в опорных точках траектории.

- значения функции в этих точках.

В этом случае мы получаем высокую плавность траектории, без каких либо скачков скорости и ускорения (см. рис.2.1), однако при этом методе степень полинома растет пропорционально числу точек, что увеличивает время расчетов и колебательность.

Рис. 2.1.Вид интер­поляции по формуле Лагранжа

Кроме того, желаемую траекторию между узловыми точками очень трудно предугадать. По вышеперечисленным причинам метод интер­поляции по формуле Лагранжа не нашел практического применения.

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

Рисунок 2.2.Решение ПЗК для манипулятора.

1.-первое звено; 2.-второе звено; 3.-третье звено (схват);

Декартовы координаты точек A(x₁,y₁) и B(x₂,y₂) относительно центра базовой системы координат O(0,0) вычисляются с помощью геометрических зависимостей (рис.2.3):

x₁=l₁cosQ₁ ; x₂=x₁+l₂cos(Q₁+Q₂) ;

y₁=l₁sinQ₁ ; y₂=y₁+l₂sin(Q₁+Q₂) .

Таким образом, определили все необходимые для написания программы и моделирования движения манипулятора формулы.