CC BY
113
llO
УДК 519.687.5: 519.687.7: 004.896
П. В. Ложкин, О. В. Толстель
СОЗДАНИЕ ПРОГРАММНО-АППАРАТНОГО КОМПЛЕКСА, ОБЕСПЕЧИВАЮЩЕГО ДИНАМИЧЕСКОЕ РАВНОВЕСИЕ И ДВИЖЕНИЕ РТК AR-600
Описаны особенности архитектуры и построения РТК AR-600. Рассмотрены основные задачи динамика двуногой ходьбы и методы их решений. Представлены расчеты необходимых углов при передвижении и время их выполнения. Также обсуждается вопрос создания фильтра Кальмана для системы аксилерометр-гироскоп-РТК.
Features of architecture and creation of robotic system AR-600 are described. The main objectives of loudspeaker of biped walking and methods of their decisions are considered. Calculations of necessary corners at movement and time of their performance are presented. As the question of creation of the filter of Kalman for system aksilerometr-giroskop-robotic system is considered.
Ключевые слова: микроконтроллер, РТК, робот, фильтр Калмана, двуногое хождение, динамическая стабилизация, равновесие, центр тяжести, датчик ускорения, AR-600.
Key words: microcontroller, robotic system, robot, Kalman's filter, biped circulation, dynamic stabilization, balance, center of gravity, sensor of acceleration, AR-600.
1. Методы решения проблемы динамической стабилизации
Проанализируем проблему динамической стабилизации двуногих шагающих устройств. Имеется немного методов ее решения.
1. Динамика хождения «по Белецкому». Учитывается центр масс робототехнического комплекса (РТК), все его углы, но не учитывается внешнее воздействие.
2. Решение задачи динамики с помощью самообучающейся искусственной нейронной сети или при помощи обучения с учителем с помощью костюма управления. Небольших результатов в этом направлении достигли японские разработчики.
3. Динамика хождения «по Белецкому», а также отслеживания центра масс при помощи шестиосного датчика гироскопа-акселерометра.
Ниже рассмотрен третий вариант решения поставленной задачи, при этом возникает проблема фильтрации данных с датчика ускорения-гироскопа. Для ее решения применим наиболее распространенный фильтр Калмана, адаптированный под рассматриваемую систему.
Для решения задачи динамической стабилизации необходимы.
1. Контроллеры, управляющие драйверами двигателей:
— микропрограммы каждого контроллера;
— обратная связь с энкодеров.
2. Шестиосный датчик ускорения-гироскоп.
3. Контроллер с фильтром Калмана (поскольку датчик ускорения, установленный на роботе, не обладает функцией фильтрации).
© Ложкин П. В., Толстель О. В., 2013
Вестник Балтийского федерального университета им. И. Канта. 2013. Вып. 4. С. 110—117.
2. Кинематическая схема РТК.
Используемые микроконтроллеры и двигатели
Кинематическая схема робота AR-600, а также его размеры представлены на рисунке 1. В голеностопах, торсе и плечах установлены двигатели Maxon RE-35 (90 Вт), в колене и бедре — двигатели Maxon RE-40 (150 Вт), в локтевых суставах — двигатели RE-25 (20 Вт). К этим двигателям рекомендованы инкрементные энкодеры HEDS-5540 и абсолютные — AEAT-6012. Подобное расположение двигателей и их технические характеристики позволяют роботу двигаться вперед со скоростью до 2,7 км/ ч, при этом, если у робота в руках находятся предметы (до 2,7 кг), он двигается со скоростью до 1,5 км/ч.
Скорость передвижения назад — до 1,5 км/ч. За счет двигателей таза робот разворачивается на месте с углом поворота в 30 градусов. Также робот шагает на 30 см в сторону.
Моторные драйверы работают под управлением 32-разрядного микроконтроллера ARM на базе ядра Cortex™-M3 с 128 Кб Flash памяти STM32F103RB. Системная плата робота будет работать на 32-разрядном ARM микроконтроллере на базе ядра Cortex™-M3 с 256 Кб Flash памяти STM32F107RC.
Разработка ПО ведется в среде CooCox IDE, основанной на Eclipse. Имеет все для работы с ARM, позволяет докачивать библиотеки. Умеет работать с отладчиками. Для эмуляции работы прошивки микроконтроллера используется программы Proteus 7 Profession.
3. Датчик ускорения. Фильтр Калмана
На робот AR-600 поставлен датчик ADIS16400 от фирмы Analog Device — полнофункциональная инерциальная измерительная подсистема, включающая трехосевые гироскоп, акселерометр и магнетометр [1]. Рассмотрены принципы работ датчика ускорения. На рисунке 2 представлены различные случаи действия на трехосный датчик ускорения.
Сигнал с акселерометра (рис. 2) является теоретическим. Реальный же сигнал будет иметь достаточно много шумов. Так как из-за различных факторов сигнал с датчиков ускорения и гироскопов измеряется с ошибкой, то выделяется полезный сигнал и тем самым повышается точность измерений. В этом случае используется фильтр Калмана, который после получения первых измерений в состоянии достаточно качественно отфильтровать полученные значения матриц.
Вес:
Рис. 1. Кинематическая схема робота AR-600
lll
112
Рис. 2. Данные с трехосного датчика ускорения
Результат фильтрации сигнала с датчиков — это его сглаживание. Мгновенные значения отфильтрованного сигнала будут ближе к его фактическим значениям, но интеграл от сглаженного сигнала может расходиться быстрей, чем интеграл от исходного зашумленного сигнала. Априорная информация о движении объекта в алгоритме будет выражаться в матрице наблюдаемости и в фундаментальной матрице.
Рассмотрим алгоритм работы фильтра Калмана применительно к данной системе.
Определение модели процесса. Для того чтобы применить фильтр, необходимо определить матрицы переменных, определяющих динамику системы и измерений Б, В и Н:
где Б — описывает динамику системы. В нашем случае — это коэффициент, определяющий последующее расположение РТК за время дискретизации (время между шагами алгоритма);
В — определяет применение управляющего воздействия. Это есть информация о движении робота, данные с гироскопа;
Н — отношение между измерениями и состоянием системы.
Определение сглаживающих свойств.
И — ошибка измерения. Определяется испытанием акселерометра и выявлением погрешности его измерений.
Q — задание шума процесса. Этот параметр подбирается для обеспечения требуемого уровня фильтрации (ковариация шума).
1. Предсказание состояния системы: х_ = Бхк_1 + Вик_х, где ик_1 — управляющее воздействие в прошлый момент времени; х_ — предсказание системы в текущий момент времени; хк_1 — предсказание системы в прошлый момент времени.
2. Предсказание ошибки ковариации: Рк = БРк_1БГ + Q, где Рк~ — предсказание ошибки; Рк_1 — ошибка в прошлый момент времени.
3. Вычисление усиления Калмана: Кк = Р_ Нг (НРкНг + И)_1.
4. Обновление оценки с учетом измерений: хк: хк = х_ + Кк (хк _ Нхк).
5. Обновление ошибки ковариации: Рк = (I _ КкН)Рк, где I — матрица идентичности.
Сигнал по одной из осей акселерометра представлен на рисунках 3—4 тонкой линией, а толстой — фильтрованный сигнал (по оси Ох — время, по оси Оу — усилие).
113
Рис. 3. Сигнал с датчика ускорения при его равномерном передвижении
Рис. 4. Сигнал с датчика ускорения при внешнем воздействии на него 4. Ритмика и кинематика движения
В зависимости от конкретной проблемы целесообразно рассматривать модель большей или меньшей сложности. Но всегда будут фигурировать следующие факторы [2].
1. Аппарат имеет массу, то есть в поле силы тяжести на него действует сила собственного веса.
2. Аппарат управляем, то есть в соединительных шарнирах его звеньев действуют или могут действовать моменты сил управления.
3. Передвижение аппарата по поверхности сопровождается наличием сил реакции опоры.
Два шага робота представлено схематично в виде 18 этапов на рисунке 5, при этом точкой в каждом этапе в проекции по оси Ох отмечено изменение центра масс робота.
Произведем расчет необходимых углов при прямолинейном движении робота. Для простоты будем считать, что поверхность гладкая и
114
находится под углом 90° к прямому расположению РТК. Для точного позиционирования ноги при постановке на поверхность планируется использовать силомоментные датчики.
Рис. 5. 18 этапов движения РТК
1-й основной период.
Рассмотрим схему постановки ноги на 6-м этапе (рис. 6).
Основной расчет сводится к нахождению угла альфа, первоначального сгиба ног. Для этого рассматривается величина опоры (в нашем случае Б), при постановке второй ноги необходимо учитывать пересечение по оси Оу опоры правой и левой ног на некоторую величину дельта для правильного перемещения центра масс на следующем шаге. Рассчитаем необходимые углы.
д2
1. По теореме косинусов у = агссоБ(1---)•
Рис. 6. Постановка ноги после 1-го шага
2.
Р = агссоБ
По
1 -
(5-А):
212
той
.2 Л
же
формуле
3. И вычисляем а = 90° -
у+Р
4. При поднятии ноги на величину
А
up : аир
= arccos(cos а---------—).
2l
5. При постановке ноги по а п = arccos
-(s -А)2
/2 lAup
l cos а----------
2
Рассмотрим 3-й этап передвижения робота. На этом этапе необходимо найти угол | для сгиба ног для дальнейшего удерживания проекции центра масс на ось Ох на центр одной опоры (во время стояния на
2l
одной ноге). Ширину торса робота примем за Ь. Тогда ^ = arccos—.
Ь
2-й основной период.
Теперь рассмотрим, как робот перемещает центр масс на вторую ногу. На рисунке 7 показана схема ног РТК при смене опорной ноги.
Угол а такой же, как и на предыдущем этапе. Находим
s-А-l sin а
Р = arccos-
l
Рис. 7. Перемещение центра масс на неопорную ногу
Из прямоугольных треугольников находим угол у сгиба коленного сустава неопорной ноги: у = 90 + Р + а.
А теперь рассмотрим 8-й этап, на котором РТК перемещает свой центр масс с одной ноги на другую путем бокового перемещения: это будет тот же самый угол |, как выше, только со знаком минус.
Следующие этапы передвижения считаются повторяющимися. Их углы строятся по этим двум основным этапам. Также необходимо будет учитывать изменения центра масс при хождении РТК с поднятыми руками (как в стороны, так и вперед). Система верхнего уровня управления РТК должна передавать в главный контроллер изменения нагрузки на руку и ее положение (для корректировки центра масс на некоторую величину 3).
Рассмотрим движение двигателей правой ноги. На рисунке 8 приводятся графики изменения положения суставов. Очень хорошо видно, как синхронно работают оба вращающих двигатели, что было описано выше математически.
Также график показывает, что коленный сустав проходит путь в два раза больший, чем шагающие суставы бедра и лодыжки. Именно поэтому на коленном двигателе стоит редуктор, в два раза меньший редуктора на бедре и лодыжке.
2
115
116
Сустав бедра шагающий
Сустав бедра вращающий Сустав лодыжки шагающий
Коленный сустав
Сустав лодыжки вращающий
Тазобедренный сустав
Рис. 8. График движения суставов правой ноги
Список литературы
1. Ложкин П. В., Павляшик К. В., Прокопович П. А. и др. Некоторые вопросы ло-комоций, манипуляций и возникновения сознания антропоморфного мобильного робота // Гибридные и синергетические интеллектуальные системы: теория и практика : материалы 1-го международного симпозиума. Калининград, 2012.
2. Белецкий В. В. Двуногая ходьба: модельные задачи динамики и управления. М., 1984.
Об авторах
Павел Владимирович Ложкин — ведущий инженер, Банк России, Калининград.
E-mail: [email protected]
Олег Владимирович Толстель — канд. техн. наук, доц., Балтийский федеральный университет им. И. Канта, Калининград.
E-mail: [email protected]
About authors
Pavel Lozhkin — chief engineer of Central Bank of Russia, Kaliningrad.
E-mail: [email protected]
Dr Oleg Tolstel' — ass. prof., I. Kant Baltic Federal University, Kaliningrad.
E-mail: [email protected]
