Квадропод на MG90

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

Видео на Яндекс

Особенности:  4 ноги,  радиоуправление, легкий, оптимальная цена, без тупой ардуины.

ПОДРОБНЕЕ:

1. Робот с шестью ногами устойчив по определению, а потому не интересен. Мы выбрали более сложный в управлении 4-ногий вариант. Плюсы: дешевле, меньше сажает аккумулятор, меньше выпиливать деталей, более шустро двигает лапами.
2. Управляется по блютусу, проводов управления и питания нет. С ноута по радио идут только 5-байтные посылки управления. Весь расчет траектории, углов и импульсов управления сервами происходит в контроллере на борту.
3. Для снижения веса все детали сделали из текстолита, в них для легкости наделали дыр и окон. Нет лишних стоек и кронштейнов. Маленький легкий аккумулятор на 1,1 Ач – хватает на 40 минут. Литий-полимер, естессно.
4. СЕРВЫ: Машинки TowerPro MG90 не самые дешевые, но и не самые паршивые из того, что предлагает китаец. Заказывали на ибэе и возникло подозрение, что тайваньский камрад прислал более слабые машинки MG90S, переклеив наклейки. Синие SG90 с пластиковыми шестернями гудят и глючат в 50% случаев, не рекомендуем. КОНТРОЛЛЕР: Камень должен аппаратно поддерживать арихметику FloatPoint, никакая целочисленная AtMega не в состоянии успевать обсчитывать 12 сервомашинок. По цене/качеству выбрали плату STM32F3Discovery. Приятно удивила её хорошая защита от дурака.
5. Большинство деградантов-программистов начинают свой путь с некоего {censored} под названием «ардуина». Робот на ардуине получится довольно вяленький. Мы всё писали на Сях. Камень писали в KEILе, верх – в MS VS.

КОНСТРУКЦИЯ

Машинки китайского камрада имеют выход вала только на одну сторону. Если корневую машинку закрепить только с одной стороны, то собственный вес выльется в изгибающий момент и быстро ее умотает, поэтому мы решили заморочиться и приклеить с обратной стороны 10-миллиметровые стальные штыри соосно с валом. Корневые машинки закрепили между двумя обычными текстолитовыми платами. На нижней напаяны подшипники под штыри. На верхнем этаже располагается плата Дискавери (сидит на разъеме, без пайки), на нижнем – аккумулятор и два стабилизатора питания. Этажи скреплены пайкой длинными штырями (достаточно прочные и легкие, типа канцелярских скрепок), по ним же идет питалово на верхнюю плату. Габариты шасси выбирали исходя из размеров Дискавери. Длину колена выбрали 4 см, исходя из честных данных о сервах, приведенныхAriman`ом. Длина голени 7 см обеспечивает дорожный просвет порядка одного сантиметра. С более длинными ногами машинки не справятся. Детали (только плоские, сложногнутых нет) нарисованы в ватокаде, 3д-модель собрана там же. В фольге текстолита прорезали дорожки разводки питания и сигналов. Всё, что только можно, склеили дихлорэтаном. Финишная полная масса 367 грамм. Детали вырезали на станке ГФ-38 фрезами kyocera. Согласимся с Ariman`ом — за процессом фрезерования надо наблюдать лично, заказывать детальки у дяди за деньги нельзя — сделают фуфел. Без станка тоже бы получилось, но менее изящно и долго.

 

ПРОГРАММНАЯ ЧАСТЬ

В STM32F3Discovery есть достаточно многоканальных универсальных таймеров, что позволяет управлять 12ю (и более) машинками, совсем не прогружая проц. В камне аппаратно реализована функция вычисления квадратного корня (за что ему спасибо) всего за 13 тактов. Функция арксинуса из math.h оказалась адски тормозной, из-за чего пришлось писать своё разложение её в ряд Тейлора. В остальном использована школьная тригонометрия, в т.ч. теорема косинусов. Расчет происходит 50 раз в секунду, т.к. номинальный период подачи управления на машинки составляет 20мс. С помощью PLL проц выведен на максимальную производительность. Сервы управляются таймерами через одноимпульсный режим, до этого пробовали ШИМ — было хуже. Благодаря низкому расположению ЦТ и быстрому подниманию/опусканию лап робот не падает при подъеме двух лап одновременно. Изменение скорости, задний ход, поворот на ходу и разворот на месте реализованы за счет пересчета длины и направления шага. При сфазированном отталкивании передними ногами и сонаправленном движении задними – встает на дыбы. Модуль блютуса подключен к UARTу, все посылки содержат синхробайт и CRC. Скорость 9600. По TX-прерыванию вызывается обработчик пакетов, отдающий команды в основной цикл.

 

МАШИНКИ

Для управления на машинку нужно постоянно подавать импульсы, иначе она отключается и не держит позицию. Даже если длительность импульса не меняется, всё равно его нужно постоянно подавать. В этом режиме машинка ведет себя как пружина – чем сильнее её поворачиваешь руками – тем сильнее она сопротивляется. Выключенная не сопротивляется вообще (но это не значит, что ее можно без толку крутить пальцами туда сюда обратно – уматается редуктор). Мы повторили эксперимент Ariman`а, поднимая машинкой пивную банку, результат как у него – поднимает около 700 грамм вместо китайских заявленных 2000 грамм. Нога в сборе может сопротивляться силе в 750 грамм (может и больше, не пробовали — начинают гнуться текстолитовые колени, да и машинка может сгореть), но относительно адекватно нога ведет себя при силе 400 грамм и меньше (да, сила обычно в ньютонах, переводите сами). Итого робот может держать на себе в пределе 750*4=3кг, это если конструкция не крякнет. Нести в движении может 400*2 (т.к. две ноги висят в воздухе)=800 грамм, минус 367 своей массы, итого 433 грамма. Чем сильнее грузишь – тем больше машинки греются. Жрут в сумме полтора ампера. Совет: первое, что нужно сделать с новыми машинками – разобрать и ответственно почистить от производственных фекалий. После сборки шасси все машинки нужно откалибровать, ибо товарищ СуньМуйВчай не гарантирует идентичность характеристик отдельных экземпляров; мы калибровали по двум точкам: 0 и 90 градусов. Вот такие проги, управляющая и тестирующе-калибровочная:

Детали и цены

Детали и цены (цены на весну 2014, Ибэй, Диалэкстрим, Промэлектроника):

• 12 сервомашинок MG90 +2…3 запасных + парочку дешевых синих на эксперименты — 12х185р
• Плата STM32F3Discovery или аналог, главное с FPU и доброй периферией — 507р
• Аккумулятор 1100 мАч — 329р
• 2 стабилизатора на 5 вольт — 2х101р
• Модуль блютус — 217р
• USB-UART переходник (для отладки) — 112р
• Автокраска в баллоне (цвет, как оказалось, на ходовые качества не влияет) – 130р
• 2 листа текстолита 20×30см — 2×150р
• Разъемчики, выключатель — 100р

ДОПОЛНИТЕЛЬНО

В основном делали мы вдвоём с Nolk`ом (г. Миасс) при участии Ивана и Михи (г. Златоуст). Делали не торопясь, по вечерам в течение 4 месяцев. Одному лучше за это не браться — не хватит энтузазизма и разнообразия идей реализации. Софт, который нужно знать и уметь: Автокад или СолидЭйдж (черетежи, 3д модель), Арткам (коды для ЧПУ станка), Маткад (статические расчеты), MS VS 2008 или Матлаб (прогоны матмодели), MS VS или Борляк (верхние проги), Кейл или Иар (нижние проги), ну и Эксэль и Калькулятор.

Апдэйт 12.07.14

Сегодня затестили управление с телефона. Опыта в андроиде мало, поэтому получилось простенько и кривовато, но работает. Развивать эту тему наверно не будем, так как прикупили джойстик — дальше будем управлять с него.
Вот коротенькое видео андроида: Жми сюда.

Некоторые нюансы реализации

Примеров работы с блютусом под эклипс в сети достаточно. В том числе готовых/толковых на русском.
Подключаться проще всего по ИД (без траты времени на поиск устройств в радиусе доступности, и без риска подключиться к другому девайсу). ИД модуля (у нас модуль HC-06 c ID= «20:13:11:01:11:28» ) надо заранее узнать посредством меню блютуса в телефоне. Дальше нужно подключится к устройству с UUID= «00001101-0000-1000-8000-00805F9B34FB». Это UUID последовательного порта. Настраивать его не надо. Работает на 9600 бод.
Сначала сделали управление на кнопках, но так как у нас скорость на роботе регулируется плавно, то решено было перейти на тачскрин. В качестве тачскрина в layout`е выступает элемент TextView1 в однопальцевом режиме, он же и отладочные цифры выводит. У него особенность: работает он по setOnTouchListener, но если не объявить setOnClickListener, то реагирует только на первое касание касание, а на движение и отпускание — нет.Так выглядит в кострукторе и в эмуляторе:

Как оказалось тачскрин забивает буфер последовательного порта из-за того что генерирует нотификацию слишком часто. Пришлось вводить исскуственное замедление sleep(100). С кнопками такого не случалось.
Перед запуском Эклипса рекомендуем сохранить/закрыть остальные проги. Одновременно с майнкрафтом не запустится вовсе. Под Win7 эклипс глючил по страшному. Перешли на WinХР. Отлаживаться лучше не в эмуляторе (тормозянка), а прямо в телефоне — поставить у него в настройках галку «Отладка по USB».

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

Нога состоит из трёх звеньев, получается четыре узловых точки. Для решения прямой задачи (знаем углы машинок — надо найти координаты четвёртой точки относительно первой) в общем случае требуется троекратная замена базиса XYZ от точки к точке, а это перемножение матриц 3х3, элементами которых являются cos и sin. Это прямая задача. А решить надо ОБРАТНУЮ задачу (знаем координаты — а надо рассчитать углы). Как решить эту задачу в общем случае мы не знаем до сих пор. Единственный вариант, который мы придумали, это многократное решение прямой задачи с подгонкой углов под заданные координаты с заданной погрешностью. Но. Спасло то, что задачка решилась разложением на несколько плоских задач, для решения которых достаточно школьной тригонометрии. Потребовалась только одна замена базиса (и та не в 3D, а в 2D). Всю векторную картину разбили на треугольники, для расчета углов которых потребовались арккосинус, арксинус и арктангенс. Так как эти функции взаимновычисляемые (с помощью т.Пифагора, т.е. с вычислением квадратного корня) то решили использовать только арксинус. Его разложение в ряд Тейлора/Маклорена самое простое из приведенных в педивикии. Хотя можно было бы использовать простую кусочно-линейную или кусочно-параболическую аппроксимацию с расчетом по методу наименьших квадратов. (Что в итоге привело бы к той же формуле, что и ряд Тейлора.) Этот метод расчета (конкретно этот, без хитрых оптимизаций и применения хакерских алгоритмов типа Герцеля с FixedPoint-арифметикой) никакая атмега не протянет (поначалу тоже хотели её использовать), завязнет на первом же делении или вычислении корня или синуса, а их десятки, а машинок двенадцать, а цикл 1/50 секунды. Если всё же использовать атмегу, то расчет углов нужно делать на PC, а роботу слать по блютусу финишные значения (24 байта получается), либо заранее просчитать все фазы движения на все случаи жизни и занести в таблицу, по которой шагать.

Цели, планы

Главная цель в общем-то достигнута: робот ходит, чуть ли не танцует. Пока, наверно возьмём таймаут до осени, чтоб мозг отдохнул. Там как раз мини-камеры придут, будем ставить. Сейчас по-быстрому наверно перенесём управляющую прогу на андроид, чтоб управлять не с ноута, а с телефона с тачскрином. Мысли по дальнейшему применению (если руки дойдут) такие: 1) Поставить на него сонар на сервомашинку и сканировать вокруг, отправлять данные на комп, тот будет строить карту местности (как в играх) и соответственно направлять робота дальше; 2) Т.к. аккумулятор небольшой — есть смысл научить его самозаряжаться, т.е. приделать к плинтусу два контакта, и чтоб он сам к ним коннектился. Находить будет или по ультразвуку или по инфрасвету или 3) Можно научить ходить по нарисованной на полу белой линии, для чего нужно анализировать только одну строку из кадра с бортовой камеры, смотрящей вперед и вниз; 4) Если завести ноги под себя, как кошка/собака то можно проходить более узкие коридоры нежели при раскинутых лапах. По сути это уже другой метод перемещения, другая математика. 

 

Автор: Nolk (Skype: nolk777)

Расскажи друзьям: