Введение

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

Большинство аналитических подходов для решения обратной задачи кинематики являются достаточно затратными с точки зрения вычислительных процедур. Одним из альтернативных подходов является использование нейронных сетей. Входные данные. Рассмотрим трехзвенный манипулятор с параметрами, приведенными в таблице 1.

Таблица 1 - параметры DH для трехзвенного манипулятора

В среде MatLab, с использованием свободно распространяемого Robotics Toolbox строится компьютерная модель трехзвенного манипулятора. Ниже приведен фрагмент скрипта MatLab в котором присваиваем в массив ‘L’ значение параметров, A, Alfa, и D из таблицы 1. Для нашей модели - это постоянные значения и в процессе работы с манипулятором они не изменяются. Параметр Tetta мы присваиваем в переменную ‘initialPose_left’-начальное положение нашего манипулятора. 

function [L,initialPose_left,baseL] =model3z 
% Левая рука 
initialPose_left = deg2rad([0 0 0]); 
L(1) = Revolute('d', 0.2, 'alpha', pi/2, 'qlim', initialPose_left(1)+deg2rad([-90 +90]) ); 
L(2) = Revolute('d', 0, 'alpha', 0, 'a', 0.4, 'qlim', initialPose_left(2)+deg2rad([-20 +90]));   
L(3) = Revolute('d', 0, 'alpha', 0, 'a', 0.4, 'qlim', initialPose_left(2)+deg2rad([-90 +90])); 
% -178 +178 
baseL = [1 0 0 0;  
        0 1 0 0;  
        0 0 1 0;  
        0 0 0 1]; 

На рисунке 1 приведено графическое отображение выбранного (см. таблицу 1) начального положения трёхзвенного манипулятора. 

Изменяя значение углов Tetta, мы можем задавать произвольное начальное положение манипулятора. Для того чтобы манипулятор изначально имел другое положение необходимо изменить переменную initialPose_left. 

Входные данные

На входные данные нейронной сети подается положение и ориентация конечного узла манипулятора. Задача нейронной сети спрогнозировать конфигурацию соединения узлов манипулятора, которые максимально будут соответствовать заданному положению и ориентации в пространстве. 

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

Для обучающихся данных были созданы известные положения сервоприводов робота в случайном порядке. Полученные данные будем сохранять в виде вектора:

где q – это угол поворота сервопривода каждого сочленения. 

%Получаем минимальное и максимальное возможное положение сервоприводов 
t1_min = L(1).qlim(1); t1_max = L(1).qlim(2);  
t2_min = L(2).qlim(1); t2_max = L(2).qlim(2); 
t3_min = L(3).qlim(1); t3_max = L(3).qlim(2); 
% массив случайных положений сервоприводов 
t1 = t1_min + (t1_max-t1_min)*rand(N,1); 
t2 = t2_min + (t2_max-t2_min)*rand(N,1); 
t3 = t3_min + (t3_max-t3_min)*rand(N,1); 
Y = horzcat(t1, t2, t3);

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

Где:  x,y,z – координаты в пространстве конечной точки манипулятора. R –матрица поворота, представленная через углы Эйлера. 

% массив матриц преобразований 4x4 
T = zeros(4, 4, N); 
T(:, :, :) = leftArm.fkine(Y); % прямая задача, получаем координаты положения по углам сервопривода 
%R = tr2rpy(T(1:3, 1:3, :), 'arm'); % получаем тангаж рыскание и крен 
R = tr2eul(T(1:3, 1:3, :)); % получаем тангаж рыскание и крен 
Tx = reshape(T(1, 4, :), [N 1]); % все Х-ы 
Ty = reshape(T(2, 4, :), [N 1]); 
Tz = reshape(T(3, 4, :), [N 1]); 
% scatter3(Tx,Ty,Tz,'.','r'); 
X = horzcat(Tx, Ty, Tz, R); % массив координат 

Собранные данные будем подавать на вход нейронной сети. На Рисунке 3.2.1 показано начальное положение трехзвенного манипулятора, точками отмечено конечное положение манипулятора для 3000 случайно сгенерированных наборов значений углов соединения. 

Итого, был получен набор для обучения из 3000 данных. В качестве входящих значений для обучения нейронной сети мы имеем две матрицы, состоящие из набора векторов X и Y. 

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

Нейронная сеть

Для создания нейронной сети будет использоваться библиотека Keras для Python. В начале набор разбивается на тренировочные и тестовые данные.

X_train, X_test, y_train, y_test = train_test_split(data_x, data_y, test_size=0.2,
random_state=42) 

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

def base_model():
 model = Sequential()
 model.add(Dense(32,input_dim=6,activation='relu'))
 model.add(Dense(64,activation='relu'))
 model.add(Dense(128,activation='relu'))
 model.add(Dense(32,activation='relu'))
 model.add(Dense(3, init='normal'))
 model.compile(loss='mean_absolute_error', optimizer = 'adam', metrics=['accuracy'])
 model.summary()
 return model

Трехзвенный манипулятор имеет небольшую степень свободы, для решения этой задачи может справиться небольшая нейронная сеть. Рациональней использовать нейронную сеть с пятью слоями. Можно построить такую же сеть и на одном слое, но количество нейронов во внутреннем слое сильно возрастет, что приведет к замедлению обучения. Поэтому вариант сети с большим количеством, скрытых слоев, может оказаться гораздо экономней. Теперь обучим новую сеть нуля в течение пятиста эпох используя оболочку KerasRegressor, которая встроенная в Keras.

clf = KerasRegressor(build_fn=base_model, epochs=500, batch_size=20,verbose=2)
clf.fit(X_train,y_train) 

После обучения сети ее можно использовать для прогнозирования.

res = clf.predict(X_test) 

Модель показала на тестовом наборе 99% точности, что является хорошим результатом. 

Результат обучения

На Рисунке 3 линией отмечено движение робота, рассчитанное обратной кинематикой, точками отмечены положение робота, найденное нейронной сетью. Как видно траектории частично совпали. В ходе численного моделирования была проведена серия экспериментов, различающихся в зависимости от выборки и количества эпох обучения. Многочисленные эксперименты показали высокую эффективность предложенного метода моделирования нейронной сети для обратной кинематической задачи. Основная проблема нейронных сетей - нехватка памяти. Также в алгоритм адаптивных нейронных сетей, в которых нет памяти, нет возможность вспомнить предыдущие ситуации, чтобы улучшить свою работу, когда снова столкнулся с такой же ситуацией.

%% Проверка решений нейронной сети, движение по кругу 
M=[-178:10:178]; % массив изменений первого узла от -178 градусов до +178 градусов с шагом 10 
M_size=length(M); 
first_q=zeros(M_size, 3); 
T33 = zeros(4, 4, M_size); 
T34 = zeros(4, 4, M_size); 
first_q(:,1)=[deg2rad(M)]; % подготавливаем q 
T33(:, :, :) = leftArm.fkine(first_q);% получаем координаты куда должен прийти робот, круговое движение 
R = tr2rpy(T33(1:3, 1:3, :), 'arm'); % получаем тангаж рыскание и крен 
Tx = reshape(T33(1, 4, :), [M_size 1]); % все Х-ы 
Ty = reshape(T33(2, 4, :), [M_size 1]); 
Tz = reshape(T33(3, 4, :), [M_size 1]); 
plot3(Tx,Ty,Tz) 
axis([-1 1 -1 1 -1 1]);hold on;grid on; 

XX = horzcat(Tx, Ty, Tz, R); % массив координат в формате для нейронной сети 
T34(:, :, :) = leftArm.fkine(q_new); % по результатам решений нейронной сети, получаем координаты 
Tx2 = reshape(T34(1, 4, :), [M_size 1]); % все Х-ы 
Ty2 = reshape(T34(2, 4, :), [M_size 1]); 
Tz2 = reshape(T34(3, 4, :), [M_size 1]); 
plot3(Tx2,Ty2,Tz2,'.') 
axis([-1 1 -1 1 -1 1]) 

Заключение

В ходе выполнения работы была решена обратная задача кинематики с помощью нейросетевого подхода. Задача была решена для трехзвенного манипулятора. С увеличением числа звеньев, задача резко усложняется из-за возможности возникновения множества решений. С этой целью, в дальнейшем нейронная сеть будет усовершенствована. Входные данные для обучения нейронной сети можно решать несколькими способами. Как с использованием подхода «Programming by demonstration», так и с помощью решения прямой задачи кинематики. В настоящей работе решение прямой задачи кинематики реализовывалось с помощью пакета Matlab и компьютерной модели манипулятора, построенной на основе подхода Денавита – Хартенберга.

Исходники: GitHub