Решатель обратной кинематики в C++

В этой статье я покажу вам, как подойти к решению обратной кинематики для манипулятора (промышленного робота). Демонстрация будет показана с использованием простых роботов двух типов: 2R в пространстве XY и 3R в пространстве XYZ. Реализация выполнена на C++. Исходный код вы найдете на моем Github.
Для матричных вычислений я использовал C++ библиотеку Eigen. Я верю, что понимание изображенных концепций (особенно представленных для 3R) даст вам прекрасную возможность решить инверсную кинематику для любого манипулятора.
Демонстрируемое решение основано на численном расчете. Наш метод (алгоритм) сводит решение к некоторой ошибке, которая завершает вызов функции (программы).
В следующей статье я не собираюсь исчерпывать проблему с преобразованиями Денавита-Хартенберга (параметры ЦТ), особенностями робота, оптимизацией (связанной с вычислением достижимых траекторий робота) и обходом препятствий .
Следующая статья должна рассматриваться как шаблон для вашего исследования и прототипирования.

Чтобы использовать библиотеку Eigen C++, посетите веб-сайт. Загрузите собственный репозиторий в свое любимое место на вашем компьютере и (для пользователей Linux) создайте мягкую ссылку.

git clone https://gitlab.com/libeigen/eigen.git
sudo ln -s /usr/include/eigen3/Eigen /usr/local/include/
/usr/include/eigen3/Eigen  -- location of your clone library

Введение

Допустим, у нас есть 3R-манипулятор (робот с 3 шарнирами вращения. Мы решили, что наш робот (конечный эффектор — EE) должен следовать точно по определенной траектории в пространстве XYZ (математическое описание траектории известно).
Теперь нам нужно знать, как настроить двигатели или поворотные шарниры (я имею в виду угловые положения), чтобы гарантировать, что EE робота следует по пути. Обратите внимание, что нам нужно отрегулировать все шарниры одновременно. Эта проблема решается с помощью инверсной кинематики.

Для простых роботов, подобных представленным также в следующей статье 2R (в пространстве XY), вычисление как прямой кинематики FK, так и обратной кинематики является простым. Расчеты более сложных роботов, таких как 6R (6 шарниров), не являются прямыми.

Существует несколько методик, помогающих в расчете ИК такого робота. Во-первых, мы можем применить быстрый способ и вычислить IK аналитически (используя геометрические соотношения). Однако альтернативным способом использования является численный подход, который можно применять, если уравнения обратной кинематики не удовлетворяют аналитическим решениям.
Помимо того, что существует аналитическое решение, численные методы часто используются также для повышения точности этих решения.

Связь между прямой и инверсной кинематикой показана на рисунке ниже. Помните, что прямая кинематика определяет положение рабочего органа манипулятора (робота) или другой определенной точки робота в декартовом пространстве XYZ. Инверсная кинематика дает информацию о том, как «манипулировать» соединениями (двигателями) робота, чтобы с помощью EE (или другой точки робота) достичь определенного положения.

Манипулятор Якобиан представляет собой матрицу, которая отображает отношение между суставами манипулятора (шарнирное пространство) к скоростям рабочего органа манипулятора — EE (задачное пространство).

Для робота 2R якобиан и IK можно вывести следующим образом:

Полная реализация решателя обратной кинематики 2R XY была выполнена на C++. Исходный код на GitHub. Код компилируется с помощью библиотеки matplotlibrary (чтобы построить робота), поэтому вам нужно будет сначала загрузить заголовок и соответствующим образом скомпилировать свой код следующим образом:

Заголовочный файл (для библиотеки графиков) должен находиться в той же папке, что и ваш cpp.
Ваша программа может быть скомпилирована следующим образом,

//compile
g++ my_prog.cpp -o my_prog -I/usr/include/python3.8 -lpython3.8// //run
./my_prog//folder tree
├── my_prog
├── my_prog.cpp
├── matplotlibcpp.h

В этой статье для решения обратной кинематики (реализация C++ для 2R и 3R робота в моем репозитории Github) был использован метод Ньютона — Рафсона.

Манипулятор 2R был показан выше. 3R в пространстве XYZ можно представить следующим образом:

Чтобы понять развернутый метод, давайте рассмотрим рисунок ниже. xd — желаемая известная EEpose (положение робота/точка пути нашего робота). Эта точка может быть связана с неизвестной конфигурацией сустава манипулятора qd , а текущая поза ЭЭ xi , связанная с
текущим шарниром — конфигурацией qi такой, что xi = f (qi). Где f – функция прямой
кинематики манипулятора.

Используя изображенный алгоритм, мы можем решить ИК для любого вида манипулятора, имеющего обратный якобиан или псевдообратный.
Оба типа якобиана робота (матрицы) могут быть вычислены с использованием библиотеки Eigen ( см. на С++).

ИК-подход

Как я упоминал ранее, для решения IK мы можем использовать как аналитический метод, так и подход к решению (с определенным значением ошибки) с использованием численных методов; здесь я использовал метод Ньютона-Рафсона (см. рисунок ниже).

Методика решения ИК показана на следующем рисунке.

Развернутый алгоритм был реализован следующим образом

Помимо проблем с вычислением обратного или псевдообратного якобиана (оба метода доступны в Eigen, MathLab или Numpy), наиболее важной частью (которая должна выполняться вручную) в следующем методе является вычисление параметров DH.
Если вы не знаком с преобразованием DH, я очень рекомендую проверить захватывающий Youtube-канал Angela Sodemann.

Общий подход к созданию таблицы DH показан на рисунке ниже. Для каждого сустава вам нужно просто оценить углы поворота и расстояния (перемещения) между двумя кадрами N и N-1 (системы координат, в которых «работают» суставы).

Используя данные определения, таблицу DH для робота 3R XYZ можно определить следующим образом:

Выделив основной признак, мы можем резюмировать преобразование DH следующим образом (рассмотрите рисунок).

Полный список параметров DH обычно отображается в таблице DH. Где два первых дают информацию о вращениях, а последние два о смещении.
Для каждого соединения необходимо определиться с каркасом (координационная система OXYX). Количество кадров просто. Это номер робота (сустав + 1). Один добавляется, так как он указывает на базу робота.
Ось Z является осью вращения сустава (поворота) или линейного движения (для призматических суставов).

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

Матрица преобразования (которая переводит точку из кадра n в кадр n-1) может быть получена следующим образом.

ПРИМЕЧАНИЕ. Принимая во внимание приведенную выше таблицу DH (параметры DH), матрица преобразования каждого соединения является компьютерной. Пожалуйста, рассмотрите реализацию C++.

Матрица преобразования определяется следующим образом:

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

Как видите, матрица преобразования DH имеет размер 4x4 и состоит из «части», связанной с вращением (R), смещением (D) и перспективой (1).

Пожалуйста, рассмотрите, как это делается в моей реализации C++. Рекомендую также проверить следующую ссылку (от проф. Криса Хаузера), чтобы ознакомиться с деталями.

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

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

Для робота 3R мы используем 3 вращающихся шарнира, поэтому якобиан можно определить следующим образом:

Поскольку нас интересуют только линейные скорости в направлениях x, y и z, мы можем переписать наш якобиан в следующем виде (реализованном на C++).

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

MatrixXf applyAlgorithm()
{
//initial values
i_theta(0, 0) = M_PI / 6;
i_theta(1, 0) = M_PI / 9;
i_theta(2, 0) = M_PI / 10;
//calaculate initial values of forward kinematics
MatrixXf FWD = FWD_Kinematics(i_theta(0, 0), i_theta(1, 0), i_theta(2, 0));
// GOAL
Xd(0, 0) = 0.544895;
Xd(1, 0) = 0.943786;
Xd(2, 0) = 2.63099;
//consider "initial" error
e = Xd - FWD;
//solve IK numerically
while ((std::abs(e(0, 0)) > 0.00001) || (std::abs(e(1, 0)) > 0.00001) || (std::abs(e(2, 0)) > 0.00001))
{
MatrixXf invJ = computePseudoInverse(i_theta(0, 0), i_theta(1, 0), i_theta(2, 0));
//update
i_1_theta = i_theta + invJ * e;
//calulate new posion of EE
FWD = FWD_Kinematics(i_1_theta(0, 0), i_1_theta(1, 0), i_1_theta(2, 0));
//compute error to be evaluated
e = Xd - FWD;
i_theta = i_1_theta;
}
return i_theta;
}

Как тестировать

Оценка программы (для манипулятора 2R и 3R) может быть выполнена следующим образом. Сначала используйте прямую кинематику (установите положение соединения и проверьте положение EE : XYZ) и вызовите в main() только FWD_Kinematics(). Обратите внимание на вывод. Во-вторых, запустите полный решатель (программу) IK, установив цель (ваш ранее рассчитанный EE) и сравните результат, который вы вычислили, с начальным положением робота, использованным на первом этапе (совместное пространство).

Спасибо за чтение.

Решатель обратной кинематики в C++