Хабрахабр

[Из песочницы] «Ручной» манипулятор

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

Рассмотрим обычный, скажем, 5-ти осевой манипулятор (кинематическая структурная схема изображена на рисунке ниже). Суть разработки такая. 3 координаты по трём осям XYZ и вращение вокруг двух из них. Его возможности при перемещении в пространстве широки: он способен совершать движения в 5-ти координатах, причём одновременно, т.е. В таком случае рабочий элемент манипулятора способен занять любое положение в рабочем пространстве и вместе с этим сохранить требуемую ориентацию рабочего органа (не считая поворот рабочего элемента вокруг собственной оси).

Данная конструкция относится к манипуляторам антропоморфного типа и поэтому способна, приближённо конечно, воспроизводить движение человеческой руки. Рассуждая над этим тезисом как раз пришла мысль, что интересно бы сделать так, чтобы манипулятор дистанционно мог копировать движения моей руки в реальном времени. Чтобы я просто одевал на руку сенсор, считывающий перемещения руки (как линейные, так и угловые), и эта механическая штука будет повторять их вслед за мной. Как манипулятор может выполнять функции перемещения и ориентирования рабочего органа, так и с помощью руки человек может перемещать и ориентировать свою ладонь, следовательно эти два процесса схожи и их можно представить как один общий процесс — захват движения руки манипулятором в реальном времени.

Раскатал губу

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

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

Разработка манипулятора
2. 1. Всё остальное Разработка устройства для отслеживания перемещения руки
3.

Вкратце о каждом из этапов:

  1. Здесь всё тривиально… В САПР проектируем конструкцию, выполняем чертежи деталей и изготавливаем, после собираем. Затем рассчитываем кинематику и пишем собственное программное обеспечение, которое скроет низкоуровневые обращения к приводам.
  2. На этом этапе определяемся с типом сенсоров, проектируем принципиальную схему, изготавливаем печатную плату, разрабатываем алгоритм определения положения в пространстве, пишем ПО.
  3. Собственно, ради чего были предыдущие этапы – заставить манипулятор слушаться хозяина. Здесь разрабатываю алгоритм конвертации координат в рабочее поле манипулятора.

1. Разработка манипулятора

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

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

Фотоотчёт сборки

ЗD–модель построена в САПР «Компас 3D». Материал деталей – фанера 4 мм и PLA для пластиковых деталей. Детали из фанеры вырезались на лазерном станке, детали из пластика печатались на 3d-принтере. В качестве приводов осей используются цифровые сервоприводы Dynamixel AX-12. Внешний вид манипулятора напоминает одного гнусного насекомого, от которого он получил прозвище «Комар».

Математическое описание кинематики

На этом этапе необходимо рассчитать прямую и обратную кинематику манипулятора. Прямая задача состоит в том, чтобы по известным значениям обобщённых координат (в данном случае углы поворота звеньев) определить положения звеньев относительно некоторой базовой системы координат. Обратная ей задача называется обратной или инверсной кинематикой, заключается в определении параметров обобщённых координат для достижения желаемой позиции и ориентации рабочего органа манипулятора.

Изобразим геометрическую схему манипулятора, из которой будут ясны интересующие нас геометрические соотношения. Начнём с расчёта обратной кинематики.

Здесь стоит пояснить, почему вектор $inline$\vec{r}$inline$ проведён до фланцевой точки, а не до конечной точки РО. Требуемую позицию изображаем радиус-вектором $inline$\vec$inline$. Этот вектор получается вычитанием из радиус-вектора $inline$\vec{R}$inline$, проведённого к конечное точки РО, вектора его ориентации $inline$\vec{v_0}$inline$, относительно БСК, т.е:
Поскольку ориентация рабочего органа нам известна заранее (я её сам определяю согласно некоторым требованиям), то необходимо, чтобы фланцевая точка оказалась в нужном месте, указанное как вектор $inline$\vec{r}$inline$.

$$display$$\vec{r}=\vec{R}-\vec{v_0}$$display$$

Рассмотрим переход фланцевой точки в нужное положение. Оно осуществляется поворотам звеньев a и b (их я изобразил в виде векторов) в шарнирах A, B и C. Начало базовой системы координат (БСК) поместим в точку шарнира B. Ось вращения шарнира A направлена вдоль оси Z, оси B и C направлены перпендикулярно Z.

Для решения обратной задачи кинематики я использовал геометрический подход в силу простоты конструкции манипулятора. Когда всё формальности соблюдены, перейдём к сути. Углы $inline$\theta$inline$, $inline$\gamma_1$inline$, $inline$\gamma_2$inline$ — углы поворота звеньев А, В и С соответственно.
Рассмотрим треугольник, ограниченный векторами $inline$\vec{r}$inline$, $inline$\vec{a}$inline$ и $inline$\vec{b}$inline$. Из геометрии видно, что вектор $inline$\vec{r}$inline$ равен сумме векторов звеньев $inline$\vec{a}$inline$ и $inline$\vec{b}$inline$. Пусть длины векторов будут равны:
Из этого треугольника по теореме косинусов найдём углы $inline$\alpha$inline$ и $inline$\beta$inline$.

$$display$$|\vec{r}|=r \quad |\vec{a}|=a \quad |\vec{b}|=b$$display$$

Запишем теорему косинусов относительно искомых углов:

$$display$$b^2=a^2+r^2-2ar\cos(\alpha) \\ r^2=a^2+b^2-2ab\cos(\beta)$$display$$

Выразим углы $inline$\alpha$inline$ и $inline$\beta$inline$:

$$display$$\alpha=\arccos\left(\frac{a^2+r^2-b^2}{2ar}\right) \quad \beta=\arccos\left(\frac{a^2+b^2-r^2}{2ab}\right) $$display$$

Из геометрической схемы видно, что:

$$display$$\theta=\arctan=\left( \frac{r_y}{r_x} \right) \quad \omega=\arcsin=\left( \frac{r_z}{r} \right)$$display$$

Тогда:

$$display$$\gamma_1=\omega+\alpha \quad \gamma_2=180^\circ-\beta$$display$$

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

$$display$$\theta=arctan\left( \frac{r_y}{r_x} \right)\\ \gamma_1=arcsin\left( \frac{r_z}{r} \right)+arccos\left( \frac{a^2+r^2-b^2}{2ab} \right)\\ \gamma_2=180^\circ-arccos\left( \frac{a^2+b^2-r^2}{2ar} \right)$$display$$

Чтобы это сделать, необходимо знать координаты вектора $inline$\vec{v}$inline$ относительно фланцевой точки, т.е. Теперь, когда переместили фланцевую точку в требуемое положение надо правильно сориентировать рабочий орган. координаты в базисе локальной системы координат (ЛСК) – $inline${E'}$inline$, начало которой расположено в фланцевой точке манипулятора.

$$display$$E'=\left[\vec{x}'\ \vec{y}'\ \vec{z}' \right]$$display$$

Вектор $inline$\vec{x}'$inline$ направлен по звену b, вектор $inline$\vec{y}'$inline$ – вдоль оси шарнирного соединения f1.
Для нахождения базиса $inline$E'$inline$ определим матрицу перехода от БСК (с базисом $inline$E_0$inline$) и ЛСК. Эта матрица получается путём комбинации поворотов в шарнирах A, B и C:

$$display$$E'=CE_0$$display$$

где

$$display$$C=R_CR_BR_A$$display$$

Т.к. матрица $inline$E_0$inline$ является единичной, тогда:

$$display$$E'=C=R_CR_BR_A$$display$$

Знающему читателю

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

Матрица $inline$C$inline$ вычисляется путём перемножения матриц поворота вокруг оси на угол.
Зададим функцию, которая вычисляет эту матрицу:

$$display$$f=axisAngle2rotMat(ось, угол)$$display$$

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

$$display$$C_A=axisAngle2rotMat(E_0^{\langle3\rangle},\theta)\\ C_{BA}=R_BC_A=axisAngle2rotMat(C_A^{\langle2\rangle},\gamma_1)C_A\\ C_{CBA}=R_CC_{BA}=axisAngle2rotMat(C_{BA}^{\langle2\rangle},\gamma_1)C_{BA}$$display$$

В итоге получаем:

$$display$$C=C_{CBA} \quad E'=C$$display$$

Пусть вектор рабочего органа в БСК $inline$\vec{v_0}$inline$. Тогда справедливо равенство:

$$display$$\vec{v_0}=E' \vec{v}$$display$$

Отсюда выразим $inline$\vec{v}$inline$ – вектор рабочего органа в базисе $inline$E'$inline$, т.е. относительно фланцевой точки:

$$display$$\vec{v}=E'^T \vec{v_0}$$display$$

Теперь, зная вектор РО, можно рассчитать углы поворота ориентирующих осей f1 и f2

Чтобы переместить РО в заданную позицию, требуется совершить поворот на углы $inline$\varphi_1$inline$ и $inline$\varphi_2$inline$. Из рисунка видно, что

$$display$$\varphi_1=arctan\left( \frac{v_y}{v_z} \right) \quad \varphi_2=arcsin\left( \frac{\sqrt{v_y^2+v_z^2}}{V} \right)$$display$$

где $inline$V=|\vec{v}|=\sqrt{v_x^2+v_y^2+v_z^2}$inline$ — длина рабочего органа,
$inline$v_x,v_y,v_z$inline$ — координаты вектора $inline$\vec{v}$inline$.

Верхний уровень написан в Matlab в виде библиотеки и различными командами-методами, нижний – на микроконтроллере Atmega328. Программное обеспечение манипулятора разделено на два уровня, верхний и нижний. Его задача получить команду и в соответствии с ней установить приводы в нужное положение. Задача верхнего уровня состоит в том, чтобы сформулировать команды, отсылаемые на микроконтроллер. В таком виде система управления оказалось надёжной (пока что…) и удобной в использовании.

Почему Matlab?

Плюс в Matlab доступна практически любая «возжеланная» математическая операция, что доставляет огромное счастье.
Изюминка, как по мне, языка Matlab в том, что операции с матрицами там происходят как обычные алгебраические соотношения, и не надо городить вложенных циклов как это делается, например, в языке C.

Формулировка команды и отправка со стороны верхнего уровня происходит следующим образом: пользователь в командном окне Matlab вызывает функцию из библиотеки (например, функцию простого перемещения в точку), указывает необходимые аргументы (обычно это координаты и ориентация РО). После по изложенным выше математическим соображения, записанных в библиотеку, вычисляются углы поворота осей и при необходимости скорости вращения. После по простенькому протоколу собираем команду и шлём её на микроконтроллер нижнего уровня по последовательному COM – порту.

В её распоряжении есть следующие функции: Пару слов о написанной библиотеке.

  1. функции создания соединения по последовательному интерфейсу
  2. функции перемещения

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

    График

  • в точку, по вручную заданным углам поворота осей и скорости вращения

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

На видео продемонстрирована работа манипулятора.

2. Разработка устройства для отслеживания перемещения руки

Переходим к следующему этапу. Здесь потребуется спроектировать специальное устройство, которое может отслеживать в реальном времени положение в пространстве ладони. Также этот процесс носит название «захват движения». Для осуществления задуманного я решил использовать бесплатформенную инерциальную навигационную систему (БИНС) на основе трёх датчиков: гироскопа, акселлерометра и магнитометра. Связка этих датчиков называется IMU – сенсор (в переводе и есть то самое БИНС).

В качестве обрабатывающего контроллера я выбрал (точнее взял что было) микроконтроллер Atmega2560. Сперва я спроектировал печатную плату с необходимым мне функционалом, на которой я разместил эти три датчика. Это спаренные датчики трёхосные гироскоп и акселерометр MPU6050 и магнитометр HMC5883l. Сенсоры использовал распространённые и дешёвые на китайском рынке.

Фото готовой платы

Алгоритм расчёта положения в пространстве достаточно прост: вначале определяем ориентацию в виде базиса локальной системы координат (ЛСК), которая связана с датчиками, а в конечном итоге с ладонью руки.

Базисы НСК и ЛСК состоят вектор-столбцов её орт:
Ориентацию определяем относительно начальной системы координат (НСК), что по сути является начальным положением устройства в момент включения питания.

$$display$$E_0=\left[ \vec{X}\ \vec{Y}\ \vec{Z} \right]=\left[ \begin{array}{cccc}1&0&0\\0&1&0\\0&0&1 \end{array} \right]\\ E'=\left[ \vec{x}'\ \vec{y}'\ \vec{z}' \right]$$display$$

Они связаны соотношением:

$$display$$E'=CE_0$$display$$

Где $inline$C$inline$ – матрица перехода, она же матрица вращения в пространстве. Базис НСК будем считать единичной матрицей. Следовательно

$$display$$E'=C$$display$$

.
После, вектор ускорения $inline$\vec{A}$inline$, который известен относительно ЛСК, необходимо определить в НСК. Это делается путём умножения вектора-столбца ускорения на базис ЛСК:

$$display$$\vec{A}=E'\vec{A}'$$display$$

Затем полученный вектор ускорения два раза интегрируем по времени и получаем расстояние, т.е. координаты:

$$display$$\vec{r}=\iint\vec{A}dt$$display$$

Первые попытки реализации этого алгоритма были весьма наивны… Наивность была в том, что я посчитал возможным только по данным гироскопа, используя дискретное интегрирование, получить матрицу поворота вокруг оси. Осью в данном случае можно рассматривать как псевдовектор угловой скорости, компоненты которого (проекции на оси) и есть выходные данные с датчика гироскопа. А угол в свою очередь получается интегрированием модуля вектора. Первоначальный задор и энтузиазм пропал, когда я увидел чудовищный по величине, так называемый «дрейф нуля», вызванный накапливающейся ошибкой из-за погрешности дискретного интегрирования и собственного шума датчика. Ещё тогда у меня появились мысли, что по данным акселерометра, который в состоянии покоя показывает вектор, обратный вектору ускорения свободного падения (далее в тексте вектор гравитации), можно делать корректировку матрицы, но и здесь появляется неопределённость, связанная с тем, что корректировка возможно только с точностью до поворота угла вокруг оси Z. Корректировка будет полноценна в том случае, если будет возможным восстановить матрицу поворота альтернативным способом, т.е. без применения данных гироскопа.

Особо интересующимся

Почему нельзя по акселерометру восстановить матрицу поворота? Особо интересующимся поясню. Как нам известно, вектор гравитации направлен всегда к центру планеты, а, следовательно, перпендикулярен поверхности Земли. Точнее недостаточно данных. Отсюда делаем вывод, что показания акселерометра в состоянии покоя (или при равномерном движении) говорят нам о положении оси $inline$\vec{z}'$inline$ ЛСК, относительно НСК. Поскольку мы связываем начальную систему координат, например, со столом, так что плоскость XY параллельная поверхности стола, который в свою очередь параллелен (с некоторой условностью) полу, а пол в свою очередь параллелен плоскости основания здания, которая обычно является касательной плоскостью к поверхности Земли. вращение вокруг оси $inline$\vec{z}'$inline$ не изменят саму ось $inline$\vec{z}'$inline$, но изменяет две остальные оси $inline$\vec{x}'$inline$ и $inline$\vec{y}'$inline$. На этом полезность этих данных исчерпывается, т.к. Следовательно показания акселерометра не изменятся и тогда положение будет не определено.

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

После неудачных попыток ответить на это вопрошание самостоятельно я полез в интернет, где нашёл нужную мне информацию. Теперь возникает закономерный вопрос – «как это сделать?». Один из популярных методов – это так называемый фильтр Маджвика (Sebastian O. Задача определения ориентации в пространстве по данным трёх измерений (угловая скорость, вектор гравитации и вектор магнитного поля) встаёт также при проектировании любительских летательных аппаратов (например коптеры), поэтому эта задача не раз решалась различными методами. Madgwick). H. Пока углублялся в изучении статьи у меня всё чаще и чаще возникала мысль о том, чтобы всё-таки попробовать написать свой алгоритм фильтра определения положения, учитывая, что к этому моменту мой уровень познаний в этой области заметно повысился. Прочитав оригинальную статью и не поняв англицкого, я обратился к замечательному переводу (спасибо автору за проделанный труд). И я его «изобрёл». Хотя бы ради интереса «изобрести велосипед»! Ниже привожу свои рассуждения.

Напомню, задача алгоритма – вычисление ориентации объекта и компенсация дрейфа нуля гироскопа, учитывая данный пары акселерометра и магнитометра. В алгоритме используются показания все трёх датчиков. он удобен в плане построения и оптимизация алгоритма и требует меньше математических операций для его расчёта в отличие от матриц. В качестве математического инструмента, описывающего положение, используется кватернион, т.к. Кватернион вращения в пространстве выглядит следующим образом:

$$display$$q=\left[ q_1\ q_2\ q_3\ q_4 \right]$$display$$

Зная ось вращения, описанное нормированным вектором $inline$\vec{v}=\left[ v_x\ v_y\ v_z \right]$inline$, и угол $inline$\varphi$inline$ (откуда они берутся сказано выше) можно рассчитать кватернион:

$$display$$q_{v,\varphi}=\left[ \cos\left( \frac{\varphi}{2} \right)\ \ v_x\sin\left( \frac{\varphi}{2} \right)\ \ v_y\sin\left( \frac{\varphi}{2} \right)\ \ v_z\sin\left( \frac{\varphi}{2} \right) \right]\quad(*)$$display$$

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

$$display$$q_n^G=q_{n-1}q_{v,d\varphi}$$display$$

Здесь $inline$q_n^G$inline$ – кватернион в данный момент времени. Индекс G сверху говорит о том, что это значение относится к измеренному относительно показаний гироскопа, т.е. угловой скорости; $inline$q_{n-1}$inline$ – значение кватерниона в предыдущий момент времени; $inline$q_{v,d\varphi}$inline$ –изменение положение за один шаг измерений, можно сказать дискретное изменение положения на угол $inline$d\varphi$inline$ за $inline$dt$inline$ – период дискретизации, выраженное кватернионом вращения (*).

Конкретно, рассматриваем вектор гравитации и вектор индукции магнитного поля Земли, которые, как это было сказано выше, статичны относительно Земли и связанной с ней НСК. Следующий этап вычислений – нахождение матрицы поворота исходя из данных пары датчиков акселерометра и магнитометра. Здесь тезис такой: зная значения гравитации и индукции в базисе ЛСК и НСК можно рассчитать матрицу перехода (поворота) от НСК к ЛСК и кватернион вращения в пространстве.

Его можно принять таким:
Обозначим через $inline$\vec{G}$inline$ нормированный начальный вектор гравитации.

$$display$$\vec{G}={\left[ 0\ 0\ 1 \right]}^T$$display$$

Через $inline$\vec{H}$inline$ обозначаем нормированный начальный вектор магнитной индукции:

$$display$$\vec{H}={\left[ H_x\ H_y\ H_z \right]}^T$$display$$

Также требуется третий вектор $inline$\vec{K}$inline$, который связан с этими двумя. Он получается векторным умножением $inline$\vec{H}$inline$ на $inline$\vec{G}$inline$:

$$display$$\vec{K}={\left[ K_x\ K_y\ K_z \right]}^T= \vec{H}\times\vec{G}={\left[ H_y\ -H_x\ 0 \right]}^T$$display$$

Теперь формируется матрица $inline$M_0$inline$, которая характеризует начальной положение, т.е. НСК, поскольку в начальный момент времени ЛСК совпадает с НСК. Матрица получается из компонентов векторов $inline$\vec{H}$inline$, $inline$\vec{G}$inline$ и $inline$\vec{K}$inline$:

$$display$$M_0=\left[ \vec{H}\ \vec{G}\ \vec{K} \right]=\left[ \begin{array}{cccc} H_x&0&H_y\\H_y&0&-H_x\\H_z&1&0 \end{array} \right]$$display$$

Аналогично, но уже на каждой итерации цикла, создаётся подобная матрица $inline$M'=\left[ \vec{h}\ \vec{g}\ \vec{k} \right]$inline$, где $inline$\vec{h}$inline$ – текущий нормированный вектор магнитной индукции, $inline$\vec{g}$inline$ – текущий нормированный вектор гравитации, $inline$\vec{k} =\vec{h}\times\vec{g}$inline$. Эта матрица характеризует положение ЛСК, т.к. эти вектора известны в системе координат датчиков, которая совпадает с ЛСК. Тогда, зная обе матрицы, можно записать такое уравнение:

$$display$$E'M'=E_0M_0$$display$$

Оно связывает две матрицы, найденные при разных обстоятельствах. Учитывая выражение $inline$E'=CE_0$inline$ и то, что базис $inline$E_0$inline$ – единичная матрица, получим:

$$display$$CM'=M_0$$display$$

Отсюда выразим матрицу перехода $inline$C$inline$:

$$display$$C={\left( M_0^{-1} \right)}^TM'^T$$display$$

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

После, когда найдена матрица перехода, преобразуем её в кватернион $inline$q_n^A$inline$ (алгоритм конвертации из матрицы в кватернион вещь публичная и приводить здесь его не буду).

И для совмещения результатов я применил простенький комплементарный фильтр с коэффициентом $inline$F$inline$, который берётся из диапазона от 0 до 1:
В итоге нам известны два кватерниона, найденные независимо друг от друга.

$$display$$q_n=Fq_n^G+\left( 1-F \right)q_n^A$$display$$

В результате получаем кватернион, в котором находится актуальная информация о положении в пространстве устройства с модулем БИНС.

Свой спортивный интерес я удовлетворил, и теперь можно обратиться к готовым решениям, более качественным. «Велосипед» поехал… Алгоритм работал, но в некоторых положениях вёл себя несколько не адекватно, что в принципе не мешало, но и не давало повода думать о нём положительно. В архиве были исходный коды на разных языках, в том числе и на языке Matlab. Тогда я вновь вернулся к статье Себастьяна Маджвика и решил использовать его алгоритм, благо, что этот замечательный человек опубликовал все исходники на сайте проекта. Микроконтроллер используется только для опрашивания датчиков и отсылку данных на ПК (похожая ситуация и с манипулятором). Этот факт повлиял на моё решение отказать от идеи делать все вычисления на микроконтроллере (тем паче что он 8-ми битный) и писать программу уже на компьютере в среде Matlab. В таком случае основной программный код, который подвергается изменениям и отладке, базируется в среде Matlab, что весьма удобно в процессе работы.

Что дальше по плану? Хорошо, ориентацию, заданную кватернионом, мы получили. Что ж, здесь лучше просто констатировать, что невозможно с нужной точностью по акселерометру определить координаты. А далее следует нахождение линейных координат объекта по данным акселерометра. Конечно, были попытки решить эту задачу, но в режиме реального времени находить координаты не получалось. Даже хотя бы находить изменение положения, которое было бы адекватным! Потому, что двойное дискретное интегрирование зашумленных показаний датчика приводила только к полёту в стратосферу, причём с бешеной скоростью, но никак не к положительному результату. Почему? Поэтому обозначенный в начале пункта алгоритм я обрезал до момента нахождения ориентации, и на этом остановился.

3. Всё остальное

Итак, вот я подобрался к заключительному этапу проектирования, вдоволь наигравшись с манипулятором и навертевшись IMU-сенсором. Работая над двумя предыдущими этапами я уже чётко представлял, как всё будет работать и по какому алгоритму. Поэтому с этим этапом я расправился достаточно быстро. Поскольку известна только ориентация ладони, то работаем непосредственно с рабочим органом, который требуется соответственно ориентировать. РО может принять любое направление, если оно не нарушает рабочую зону (вращаться на все 360 не получиться).

Так вот, этот вектор и нужно определить исходя из ориентации ладони руки. В пункте про разработку манипулятора вектор РО задавался вручную, и уже после находились углы поворота приводов. Базис состоит из вектор-столбцов его орт, следовательно получаем три вектора $inline$\vec{x}'$inline$, $inline$\vec{y}'$inline$ и $inline$\vec{z}'$inline$. Ориентацию можно выразить базисом $inline$E'$inline$, найдя его, конвертировав кватернион в матрицу. вектор $inline$\vec{v_0}$inline$ в начале сонаправлен с осью X БСК и НСК (начальное положение модуля БИНС располагаем так, чтобы НСК совпадала БСК манипулятора). Будет удобным взять за ориентацию РО вектор $inline$\vec{x}'$inline$, поскольку я предполагал такое начальное положение манипулятора, когда все звенья и РО расположены в плоскости XZ базовой системы координат, и РО в этот момент расположен горизонтально, т.е. После нормируем его по длине РО и получаем требуемый вектор относительно базовой системы координат манипулятора. В таком случае первый столбец матрицы $inline$E'$inline$ приравниваем вектору $inline$\vec{v_0}$inline$. 1. Затем следуем последовательности расчёта обобщённых координат из п.

Второй режим вызывает больше интереса, т.к. Стоит заметить, что манипулятор, при изменении ориентации РО, способен выполнить перемещение в двух режимах: первый – когда фланцевая точка неподвижна, второй – когда неподвижна конечная точка РО, но соблюдается его ориентация. Впрочем, переход от первого режима ко второму происходит с добавлением всего лишь одной операции вычитания векторов для нахождения вектора $inline$\vec{r}$inline$(см. даже в отсутствии линейного перемещения всё равно задействованы все оси вращения для перемещения фланцевой точки, одновременно соблюдая ориентацию РО. 1). п.

В общих чертах структура программы следующая: Все выше описанные операции также программировались в среде Matlab.

  1. Калибровка при запуске. Устанавливаем всё хозяйство в исходное положение и не смея прикасаться калибруем модуль БИНС по некоторому количеству сырых данных с него. Калибровка необходима, поскольку нужно убрать начальное смещение, которое выдавал фильтр (я не стал разбираться почему, и так работало прекрасно).
  2. Начало скрипта. Инициализируем все библиотеки и переменные, задаёмся начальным положение фланцевой или конечной точки РО.
  3. Запуск цикла. На каждой итерации опрашиваем датчики, находим кватернион, переводим его в матрицу, которая есть базис $inline$E'$inline$, он же базис, в котором расположен РО. Первый столбец принимаем за ориентацию РО и находим все обобщенные координаты, используя математические соображения из п. 1.
  4. Зная обобщенные координаты в прошлый момент времени и в настоящий, рассчитываем угловые скорости вращения осей.
  5. Отправляем пакет данных на микроконтроллер манипулятора.

Получилось конечно корявенько, но если приглянуться, то можно увидеть положительный результат исследования. Эта работа, безусловно, не претендует на законченную и требует ещё много усилий для доработки, но только в том случае, если этому можно найти применение. Но конкретное применение я ещё не нашёл… Может кто подскажет?

Показать больше

Похожие публикации

Добавить комментарий

Ваш адрес email не будет опубликован. Обязательные поля помечены *

Кнопка «Наверх»