Глава 2. Исполнительные механизмы образовательного комплекта

В составе образовательного комплекта «СТЕМ Мастерская» в качестве исполнительных механизмов выступают сервоприводы AR-S430-01 и XL431-T250-T (далее сервоприводы), представляющие собой модели промышленных автоматизированных приводов со встроенной системой управления. В таких сервоприводах в одном корпусе объединены двигатель постоянного тока, редуктор и системы управления. Применение данного типа сервоприводов позволяет разрабатывать модели манипуляционных роботов с различными типами кинематической схемы, обладающих высокой точностью и динамикой движения.

Двигатель постоянного тока

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

Рис. 2.1. Внешний вид типового ДПТ, применяемого в робототехнических наборах

Плюсами использования ДПТ в робототехнике, являются:

Резкий отклик при изменении рабочего напряжения. Другими словами, двигатель имеет очень быструю отзывчивость на получаемый сигнал управления (например, у двигателя внутреннего сгорания большой отклик). Что позволяет проектировать достаточно быстродействующие системы.
Большой рабочий диапазон по создаваемому моменту и частоте вращения.
Точность определения скорости и положения обусловлена только точностью установленных датчиков.
Инертность. Это значит, что при отключении питания двигатель продолжит какое-то время крутиться за счет инертности вала, ротора и т.д.
Низкий КПД.
Низкий развиваемый момент. Возможно устранение данной проблемы путем установки редуктора.
Установка редуктора (без которой чаще всего обойтись не получается) приводит к возникновению люфтов в сочленениях.

Для управления двигателем постоянного тока чаще всего используется специальная схема, так называемый «H-мост». C помощью данной схемы можно изменять как скорость вращения двигателя, так и направление вращения (включать «Реверс»). При такой схеме управления используются сигнальные импульсы от широтно-импульсного модулятора (ШИМ).

Принципиальная схема моста напоминает латинскую букву «Н», откуда и пошло название устройства. В качестве основного элемента «Н-моста» выступает 4 электронных ключа, расположенных попарно, слева и справа, а между ними включается нагрузка в виде ДПТ.

Рис. 2.2. Внешний вид типового ДПТ, применяемого в робототехнических наборах

На представленной на рис. 2.2. схеме видно, что переключатели должны включаться попарно и по диагонали для корректного протекания тока от источника. Когда включены ключи 1 и 4, электродвигатель вращается в одном направлении. Включение ключей 2 и 3 обеспечивает работу двигателя в противоположном направлении. На следующем рисунке (рис. 2.3) проиллюстрировано, что происходит, когда меняется положение ключей.

Рис. 2.3. Схема работы «H-моста». Красной стрелкой указано направление тока

Сервопривод

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

Сервопривод — это устройство, состоящее из двигателя (например, из ДПТ), встроенного редуктора и платы управления (рис. 2.4).

Рис. 2.4. Сервопривод в разобранном виде. Отчетливо видны ДПТ, плата управления, редуктор и выходной вал

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

Рис. 2.5. Схема устройства типового оптического энкодера

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

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

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

В состав образовательного робототехнического набора «СТЕМ Мастерская» входят высокопроизводительные сервоприводы, разработанные специально для применения в робототехнических системах. Сервоприводы AR-S430-01 и XL431-T250-T работают на протоколе Dynamixel. Уникальной особенностью сервоприводов является возможность их подключения к общей шине управления, до 255 штук к одному контроллеру управления (рис. 2.6). За счет высокой точности, скорости и крутящего момента данные сервоприводы могут быть использованы для создания подвижных механизмов повышенной сложности, таких как манипуляционные роботы, шагающие роботы, роботы-пауки, pan-tilt платформы и т.п.

Рис. 2.6 . Схема соединения и обмена данными между контроллером и Dynamixel

совместимыми устройства

Рис. 2.7. Линейка сервоприводов DYNAMIXEL. Самые популярные модели.

Рис. 2.8. Сервопривод AR-S430-01

В состав образовательного комплекта «СТЕМ Мастерская» входят сервоприводы AR-S430-01 и XL431-T250-T, работающие по протоколу Dynamixel (рис. 2.8).

В качестве интерфейса подключения сервоприводов используется TTL шина, со стандартными 3х пиновыми коннекторами molex. На каждом сервоприводе имеется два таких коннектора для организации сквозного последовательного соединения между цепочкой сервоприводов. Соединительный кабель может быть подключен к любому их этих двух разъемов (рис. 2.9).

Рис. 2.9. Схема распиновки портов сервопривода набора «СТЕМ Мастерская»

Внутреннее устройство памяти сервоприводов, протокол общения

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

Пакет Инструкций - пакет данных, содержащий команды управления сервоприводами, посылаемые внешним управляющим контроллером.
Пакет Состояний - пакет данных, содержащий ответы от сервопривода, посылаемые внешнему управляющему контроллеру.

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

Для удобства понимания все параметры разбиты на 2 группы, в зависимости от типа памяти, в которых хранятся эти значения. Значения параметров, хранящиеся в области RAM (ОЗУ), принимают значения по умолчанию при каждом сбросе питания. А значения параметров, хранящиеся в области EEPROM памяти (ПЗУ), сохраняют свои значения при выключении питания. Для чтения или записи данных пользователь должен указать правильный адрес в шестнадцатеричном виде, представляющий расположение данных в своей области памяти. Данные, доступные для чтения, обозначены в таблице как R, а доступные для чтения и записи как RW. Значения параметров по умолчанию приведены в отдельном столбце. Здесь надо понимать, что значениями по умолчанию для данных, хранящихся в EEPROM памяти являются заводские значения параметров, а для данных, хранящихся в RAM памяти – значения на момент включения питания сервопривода.

Управляющие данные поступают в сервопривод через линию Data (PIN3 на рис. 2.9, который можно назвать “информационным”). Общение происходит по последовательному каналу (полудуплексный UART) путем передачи пакетов данных. Пакеты бывают разного объема, одним из стандартов является объем в 16 бит.

Каждый пакет данных состоит из:

стартовой последовательности бит (некая последовательность, обозначающая старт нового пакета);
конечной последовательности бит;
битов информации.

Еще одной важной составляющей данного протокола является частота общения. Частота передатчика и приемника должна совпадать, иначе произойдет рассинхронизация, и все передаваемые данные потеряются. Передача информации в данном случае происходит по 1 линии, а передавать ее требуется в обе стороны. В противном случае, полученная система будет без обратной связи. То есть будет выполняться передача команды сервоприводу, но не будет получен ответ о результате исполнения этой команды. Для того, чтобы пересылать информацию в обе стороны по одной линии, используется следующий подход. Несколько пакетов данных идут в сторону сервопривода (обычно 4 или 5). После чего привод должен откликнуться о том, что принял эти данные, и по той же линии послать свой пакет с данными. Приемник, получая его, в свою очередь понимает, что может отослать новые пакеты. Такой же принцип используется при посылке информационных пакетов от сервопривода к микроконтроллеру.

Параметры, хранящиеся в ПЗУ сервопривода, представлены в следующей таблице:

*Add- ress*	*Size (Byte)*	*Data Name*	*Description*	*Access*	*Initial Value*
0	2	Model Number	Model Number	R	1061
6	1	Firmware Version	Firmware Version	R	-
7	1	ID	Dynamixel ID	RW	1
8	1	Baud Rate	Communication Speed	RW	1
9	1	Return Delay Time	Response Delay Time	RW	250
48	4	Max Position Limit	Maximum Position Limit	RW	0
52	4	Min Position Limit	Minimum Position Limit	RW	16383
31	1	Temperature Limit	Maximum Internal Temperature Limit	RW	70
34	2	Min Voltage Limit	Minimum Input Voltage Limit	RW	0
32	2	Max Voltage Limit	Maximum Input Voltage Limit	RW	140
38	2	Current Limit	Current Limit	RW	1023
68	1	Status Return Level	Select Types of Status Return	RW	2
65	1	LED	LED for Alarm	RW	36
63	1	Shutdown	Shutdown Error Information	RW	36

В первом столбце таблицы хранится порядковый адрес ячейки памяти, в котором находится данное значение. Во втором столбце количество байт, которое занимает данная переменная.

Представленные в таблице параметры:

Model number - модельный номер сервопривода, для модели - это 1061.

Firmware verison - версия прошивки привода.

ID – номер, по которому идет обращение к сервоприводу. Всего в одной сети может быть до 254 приводов. Очень важно выставить данный параметр корректно, если в одной сети будет несколько приводов с одинаковым ID, то произойдет ошибка.

Baud Rate – скорость, с которой осуществляется общение с приводом.

Обычно она составляет 1 000 000 бод.

Return Delay Time - задержка отклика (допустимая задержка при обращении к приводу).

Min Position Limit - предельный угол поворота при вращении по часовой стрелке. Стандартное значение параметра – 0.

Max Position Limit - предельный угол поворота при вращении против часовой стрелки. Стандартное значение параметра – 16384.

Temperature Limit - максимальная температура, при который привод не отключится. Стандартное значение параметра - 70 (в градусах Цельсия).

Min Voltage Limit - минимальное напряжение, при котором привод будет работать. Стандартное значение параметра - 0.

Max voltage Limit - максимальное напряжение, при котором привод будет работать. Стандартное значение параметра - 140.

Current Limit - максимальное усилие, которое может создать привод. Стандартное значение параметра, что соответствует 1,6 Н*м. Также это значение является максимальным возможным.

Status return level - способ, через который привод будет сигнализировать об ошибках.

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

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

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

*Address*	*Size (Byte)*	*Data Name*	*Description*	*Access*	*Initial Value*
64	1	Torque Enable	Motor Torque	On/Off	RW
65	1	LED	Status LED	on/Off	RW
116	4	Goal Position	Target Position		RW
104	4	Goal Velocity	Target Speed		RW
132	4	Present Position	Present Position		R
128	4	Present Velocity	Present Velocity		R
126	2	Present Current	Present Current		R
144	2	Present Voltage	Present Voltage		R
146	1	Present Temperature	Present Temperature		R

Представленные в таблице параметры:

Torque enable – параметр, отвечающий за запуск двигателя. Значение параметра «0» - включить усилие, «1» - выключить.

LED - параметр, отвечающий за включение светодиода. Значение параметра «0» - включить светодиод, «1» - выключить.

Goal position - “целевая позиция” сервопривода. То есть это угловая позиция, в которую приводу нужно переместиться. При включении сервопривода данная ячейка памяти пустая. При ее заполнении привод сразу начнет вращаться, чтобы достичь заданной позиции. Параметр может принимать значения от 0 до 16384.

Goal Velocity - “целевая скорость” сервопривода. Параметр работает по аналогии с предыдущим пунктом. Может принимать значения так же от 0 до 16384.

Torque Limit - параметр, дублирующий аналогичный параметр из ПЗУ.

Present position - текущая позиция сервопривода. Present Velocity - текущая скорость сервопривода. Present Current - текущая нагрузка на сервоприводе. Present voltage - текущее напряжение на сервоприводе.

Present temperature - текущая температура на сервоприводе.

Устройство системы управления

Как было сказано ранее, данный сервопривод может функционировать в двух режимах работы:

По заданной скорости - путем задания сервоприводу скорости, которой необходимо придерживаться.

По заданному положению – путем задания сервоприводу положения, которое нужно удерживать.

Рис. 2.10. Блок-схема типичной системы управления сервоприводом

Типичная блок-схема системы управления сервоприводом представлена на рисунке рис. 2.10. В ней на вход подается значение требуемого положения или скорости. Затем ДПТ начинает движение, которое фиксирует энкодер. С помощью энкодера вычисляется отклонение от заданного положения или скорости. Это отклонение попадает на регулятор (П, ПД, ПИД или другой), который, в свою очередь, формирует управляющий сигнал, снова идущий на сервопривод. Данный рабочий цикл все время выполняется в собственной системе управления сервопривода. Поэтому, как только мы отправляем ему необходимую скорость или положение, сервопривод сразу приводится в движение.

Спроецируем этот подход на сервопривод. Рассмотрим работу энкодера на данном сервоприводе, который обладает 16384 отсчетами на 360 градусов (рис. 2.11).

Рис. 2.11. Определение угла поворота у сервопривода

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

Регуляторы, используемые в сервоприводах AR-S430-01

Коэффициенты ПИД используются в режиме «Контроля Позиции» и

«Расширенном Режиме Контроля Позиции». Коэффициент P управления позицией внутреннего контроллера сокращен до KPP, а коэффициент таблицы управления сокращен до KPP(TBL).

Далее приведена блок-схема, описывающая контроллер позиции в режиме «Контроля Позиции» и «Расширенном Режиме Контроля Позиции». Когда инструкция от пользователя принимается сервоприводом, выполняются следующие шаги до приводки рога.

Инструкция от пользователя передается через шину DYNAMIXEL, затем регистрируется в Goal Position(116) .
Goal Position(116) преобразуется в желаемую траекторию позиции и желаемую траекторию скорости с помощью Profile Velocity(112) и Profile Acceleration(108).
Желаемая траектория позиции и желаемая траектория скорости сохраняются соответственно в Position Trajectory(140) и Velocity Trajectory(136).
Предварительный усилитель и контроллер ПИД вычисляют выход ШИМ для двигателя на основе желаемых траекторий.
Goal PWM(100) устанавливает ограничение на рассчитанный выход ШИМ и определяет окончательное значение ШИМ.
Окончательное значение ШИМ применяется к двигателю через инвертор, и приводится в действие ключём сервопривода.


	Контрол- лер	Уравнения преобразования	Диапазон	Описание
Коэффициент D управления позицией (80)	KPD	KPD = KPD(TBL) / 16	0 ~ 16,383	Коэффициент D
Коэффициент I управления позицией (82)	KPI	KPI = KPI(TBL) / 65,536	0 ~ 16,383	Коэффициент I
Коэффициент P управления позицией (84)	KPP	KPP = KPP(TBL) / 128	0 ~ 16,383	Коэффициент P
Коэффициент 2-го предварительного усиления (88)	KFF2nd	KFF2nd(TBL) / 4	0 ~ 16,383	Коэффициент ускорения пред- варительного усиления
Коэффициент 1-го предварительного усиления (90)	KFF1st	KFF1st(TBL) / 4	0 ~ 16,383	Коэффициент скорости пред- варительного усиления

Результаты сохраняются в Present Position(132), Present Velocity(128), Present PWM(124) и Present Current(126).

Рис. 2.12. Блок-схема работы сервопривода в режиме контроля позиции

ПРИМЕЧАНИЕ:
В случае режима управления ШИМ, как контроллер ПИД, так и предварительный усилитель деактивируются, пока значение [Goal PWM(100)] непосредственно управляет двигателем через инвертор. Таким образом, пользователи могут напрямую управлять подачей напряжения на сервопривод.
K - это коэффициент против нарастания, который не может быть изменен пользователями.

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

Предварительный усилитель и контроллер ПИД рассчитывают желаемый ток на основе желаемой траектории.
[Goal Current(102)] устанавливает окончательный желаемый ток, устанавливая ограничение на рассчитанный желаемый ток.
Контроллер тока рассчитывает выход ШИМ для двигателя на основе окончательного желаемого тока.
[Goal PWM(100)] устанавливает предел на вычисленный ШИМ-выход и определяет окончательное значение ШИМ.
Окончательное значение ШИМ применяется к мотору через Инвертор, и сервопривод приводится в движение.
Результаты сохраняются в [Present Position(132)] , [Present Velocity(128)], [Present PWM(124)] и [Present Current(126)].

Рис. 2.13. Блок-схема работы сервопривода в режиме контроля ШИМ

Регуляторы

Регулятор – одно из ключевых понятий робототехники и автоматизированной системы управления в целом. Само понятие «Регулятор» пришло из теории автоматического управления. Эта наука занимается исследованием систем автоматического управления, а такие системы встречаются на каждом шагу: автопилоты, система регулирования температуры в кондиционерах, системы управления температурой в химическом производстве, регуляторы положения поглощающих стержней в атомном реакторе, системы наведения торпед и ракет — всё это примеры систем автоматического управления.

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

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

Рис. 2.14. Иллюстрация применения регулятора на примере задачи следования роботом по линии

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

если не на черной поверхности -> поворачивай направо иначе -> поворачивай налево

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

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

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

Пропорционально - дифференциальный (ПД) - представляет собой сочетание пропорционального и дифференциального регуляторов. ПД-регулятор оказывает воздействие на объект регулирования пропорционально отклонению регулируемой величины и производной от величины отклонения, т. е. ее скорости изменения. Такой регулятор применяется на объектах, где регулируемая величина имеет свойство быстро отклоняться от заданного значения и необходима реакция на скорость ее отклонения. Регулятор обеспечивает высокое быстродействие, но точность, по-прежнему, остается на достаточно низком уровне.

Пропорционально - интегрально - дифференциальный (ПИД) - регулятор обеспечивает очень высокую точность и быстродействие, но является достаточно сложным в настройке. ПИД-регулятор воздействует на объект регулирования пропорционально отклонению регулируемой величины, интегралу от этого отклонения и скорости изменения регулируемой величины. При скачкообразном изменении регулируемой величины идеальный ПИД-регулятор в начальный момент времени оказывает мгновенное бесконечно большое воздействие на объект регулирования; затем величина воздействия мгновенно падает до значения, определяемого пропорциональной частью регулятора, после чего постепенно начинает оказывать свое влияние интегральная (астатическая) часть регулятора.

Еще одним базовым понятием из систем автоматического управления является отклонение или ошибка. Управляемая нами система имеет некоторое идеальное состояние. В случае с примером следования роботом по линии, это состояние, когда робот находится четко на границе черного и белого. С помощью датчика можно измерить отклонение от данного состояния. Например, условимся принять следующие показания ИК-датчика отслеживающего линию робота:

«0» — черный цвет (линия);

«100» — белый цвет (поле).

В данном случае задача сведется к нахождению системы в идеальном состоянии быть на границе между черным и белым цветами. Тогда идеальное состояние будет рассчитано как: (100+0)/2=50

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

Left_speed = speed error

В то же время на правый мотор подадим скорость, равную: Right_speed = speed +error

Таким образом, если робот находится на черном цвете, показания датчика равны 0, ошибка равна 50 и скорость правого мотора будет больше скорости левого мотора. Следовательно, робот будет поворачивать направо и т. д.

В этом примере был реализован простой пропорциональный регулятор. Осталось лишь добавить одно дополнение — коэффициент. Так как показания датчика и скорости двигателя могут иногда сильно разниться в числовом значении (например, иметь разные порядки), необходимо вводить поправочный коэффициент ошибки. Например, показания датчика имеют диапазон [0-1000], а скорость двигателя задается в диапазоне [0- 10]. Тогда ошибка будет равна:

\(error = kP*(S-50)\)

где kP - поправочный коэффициент пропорциональной составляющей регулятора. Данный коэффициент чаще всего приходится подбирать эмпирическим путем.

Теперь рассмотрим ПИД регулятор. Здесь интегральная составляющая — это интеграл нашего отклонения или ошибки. А дифференциальная составляющая — это производная от ошибки.

Исходя из определения производной:

Пусть функция f — это отклонение (error), а x — это время. Тогда функция f(x) — показывает зависимость отклонения от времени. А производная этой функции — разница ошибки в момент времени x1 и x0, где x1 = x0+deltaX. По определению, x1 > x0, то есть f(x1) — это значение ошибки в следующий момент времени. Мы не можем знать состояние системы в будущем, но можем попытаться его предсказать, используя данные о состоянии в текущий момент времени (вместо будущего) и в предыдущий момент времени (вместо настоящего). Таким образом, получим:

\(x1 = x0-deltaX.\)

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

\(D = kD*(error-errorOld)\)

Где D — дифференциальная составляющая управляющего сигнала, kD

— поправочный коэффициент, error — ошибка, а errorOld — ошибка за предыдущую итерацию цикла управления.

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

Рис. 2.15. Геометрический смысл производной

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

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

Программный вариант этого метода выглядит следующим образом: I = kI*error+I

Где I — интегральная составляющая управляющего сигнала, kI — её поправочный коэффициент.

В результате, при использовании ПИД-регулятора, на колеса робота будет подаваться следующая скорость:

\(Left_speed = speed — error — D — I Right_speed = speed + error + D + I\)

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

Dynamixel Workbench

Для разработки внешней системы управления сервоприводами используется инструментарий Dynamixel Workbench. Данный инструментарий существует под различные среды разработки. Применительно к данному набору и входящему в его состав контроллеру OpenCM, такой средой разработки является Arduino IDE с установленной поддержкой входящего в состав набора контроллера – OpenCM. Для добавления данной поддержки в среду разработки необходимо выполнить следующие шаги:

Зайти в меню «Файл» – «Настройки», а затем в строку «Дополнительные ссылки для Менеджера плат» добавить следующую строку (рис. 2.16):

https://raw.githubusercontent.com/ROBOTIS-GIT/OpenCM9.04/master/ arduino/opencm_release/package_opencm9.04_index.json

Рис. 2.16. Добавление ссылки для менеджера плат

После чего нажать кнопку ОК.

Зайти в меню «Инструменты» – «Платы» – «Менеджер плат» и в строке поиска ввести «OpenCM». В результате должен появится пакет «OpenCM9.04 by ROBOTIS» (рис. 2.17).

Рис. 2.17. Пакет поддержки платы OpenCM9.04 в менеджере плат

В выпадающем списке версий необходимо выбрать требуемую версию пакета и нажать кнопку «Установка». После чего начнется загрузка необходимых пакетов и их последующая установка. Этот процесс может занять несколько минут. После установки пакета появится сообщение об успешной установке, после чего среда разработки будет готова к работе с платой OpenCM. Для дальнейшей работы необходимо выбрать данный тип платы в меню «Инструменты» – «Плата» и указать номер порта, на котором плата установилась при подключении ее к компьютеру. После этого можно работать с инструментарием DynamixelWorkbench.

Первое, что необходимо выполнить при работе с любым Dynamixel- совместимым устройством – узнать ID устройства, скорость передачи данных, на которую он настроен, и убедиться, что в цепи сервоприводов нет устройств с одинаковыми ID. В противном случае, система будет работать некорректно.

Фактически, инструментарий состоит из класса DynamixelWorkbench, который необходимо использовать для создания объекта класса и затем использовать методы данного объекта. В качестве примера создадим объект и назовем его dxl_wb:

DynamixelWorkbench dxl_wb;

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

bool init(const char* device_name = «/dev/ttyUSB0», uint32_t baud_rate = 57600, const char **log = NULL);

Первый параметр, который передается в функцию это название устройства, которое используется. Второй параметр частота общения. Третий параметр лог строка, данный параметр можно ситуативно передавать в любую функцию библиотеки для отслеживания ошибок. Например, если устройство не инициализируется, то можно вывести на экран содержимое log и посмотреть, в чем причина. Все функции библиотеки имеют тип «bool», так как возвращают логическое значение: «True», если функция выполнилась успешно, и «False», если выполнить функцию не удалось. Пример блока инициализации порта Dynamixel для OpenCm выглядит следующим образом:

const char *log; // Создаем log переменную

bool result; // Создаем переменную результата функции

Serial.begin(57600); // инициализируем последовательный порт для вывода информации на экран.

result = dxl_wb.init(“3”, uint32_t(57600), &log); // здесь просходит инициализация 3го порта на OpenCM (3й порт физически выведен на плату расширения STEM Board) на скорости 57600 бод

if result == True{ Serial,println(“its ok”)

}

else{ Serial.println(log)

}

Далее можно проверить переменную result и, если ее значение равно

False, вывести log на экран с помощью функции (Serial.println).

После подключения сервоприводов необходимо проверить, инициализировались ли сервопривода в сети. Для этого можно воспользоваться функцией ping. Данная функция отправляет на сервопривод пробный пакет данных, если сервопривод его принимает, то сразу же отвечает своим пакетом данных, и функция возвращает значение True. Если же по какой-то причине информация до сервопривода не дошла, то функция возвращает False.

bool ping(uint8_t id, const char **log = NULL);

Как можно заметить, на вход функции поступает ID устройства, которое необходимо “пингануть”, а также указатель на переменную для логирования. После проверки сервоприводов необходимо указать им один из двух режимов работы:

Режим шарнира- сервопривод может вращаться только в ограничен-

ном диапазоне углов (от 0 до 360 градусов), также данные ограничения можно задать в памяти сервопривода.

Режим колеса - сервопривод может вращаться без ограничений Для этого используются следующие методы:

bool jointMode(uint8_t id, int32_t velocity = 0, int32_t acceleration = 0, const char **log = NULL);

Данный метод инициализирует сервопривод в качестве шарнира. Первый принимаемый аргумент - ID сервопривода, второй – скорость, с которой сервоприводу разрешается двигаться, третий - максимальное ускорение, которое сервоприводу разрешается развивать.

bool wheelMode(uint8_t id, int32_t acceleration = 0, const char log = NULL);

Метод wheelMode инициализирует сервопривод в качестве колеса. Первым принимается параметр - ID сервопривода, вторым - максимальное ускорение, которое сервоприводу разрешается развивать.

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

bool writeRegister(uint8_t id, uint16_t address, uint16_t length, uint8_t* data, const char log = NULL);

Первый входной параметр - ID сервопривода, затем адрес регистра, который необходимо записать и длина регистра (в байтах). После этого данные, которые необходимо записать, и последний параметр - log, в который запишется, успешно ли прошла запись информации в регистр.

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

uint8_t id = 1 
uint8_t CWcm = ... 
uint8_t CCWcm = ... 
uint8_t CWcs = ... 
uint8_t CCWcs = ... 
uint8_t Punch = ...

dxl_wb.writeRegister(dxl_id, uint16_t(26), uint16_t(1), &CWcm,&log); 
dxl_wb.writeRegister(dxl_id[iter], uint16_t(27), uint16_t(1), &CCWcm,&log);
&log);

dxl_wb.writeRegister(dxl_id[iter], uint16_t(28), uint16_t(1), &CWcs,&log);


&log);

dxl_wb.writeRegister(dxl_id[iter], uint16_t(29), uint16_t(1), &CCWcs, dxl_wb.writeRegister(dxl_id[iter], uint16_t(49), uint16_t(1), &punch,

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

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

Считывать информацию из конкретного регистра можно с помощью функции:

bool readRegister(uint8_t id, uint16_t address, uint16_t length, uint32_t - data, const char **log = NULL);

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

Также важным аспектом данной библиотеки являются syncWriteHandler-ы. Данные объекты позволяют одновременно записывать информацию на несколько устройств. При большом количестве сервоприводов в сети, запись данных на каждый сервопривод по отдельности может занимать много времени (данные должны дойти до сервопривода, потом должен вернуться ответ и т.д). Поэтому существует режим “синхронной записи” данных на все сервопривода в сети. Чтобы им воспользоваться, сначала необходимо создать объект обработчика синхронной записи:

bool addSyncWriteHandler(uint8_t id, const char *item_name, const char - *log = NULL);

Первый параметр - это ID “главного сервопривода”, от которого данные будут расходиться по остальным сервоприводам. Второй параметр

это имя регистра, в который мы хотим синхронно посылать данные, например, goal_position.

Чтобы синхронно отправить данные необходимо отправить их с помощью функции:

bool syncWrite(uint8_t index, int32_t *data, const char **log = NULL);

Первый элемент, посылаемый в функцию - это “индекс” обработчика синхронной записи. Индекс равен ID “главного” сервопривода, указанно-

го при создании обработчика. Второй элемент - это массив данных, которые необходимо записать, и последний - log. Разберем пример синхронной отправки позиции на сервопривода:

Пусть в нашей системе 3 сервопривода, для начала создадим массив ID сервоприводов:

uint8_t dxl_id[3];

Затем создадим массив, содержащий необходимые позиции: int32_t goal_position[3];

Инициализируем порт и создаем объект, отвечающий за синхронную запись:

dxl_wb.addSyncWriteHandler(dxl_id[0], «Goal_Position»);

Заполним массив goal_position и отправим его на сервопривода: goal_position[0] = 0;

goal_position[1] = 0;

goal_position[2] = 0; dxl_wb.syncWrite(dxl_id[0], goal_position);

После этого сервопривода с ID 1,2 и 3 займут нулевое положение.

Аналогично можно добавлять объекты синхронного считывания (только для версии протокола Dynamixel 2.0):

bool addSyncReadHandler(uint8_t id, const char *item_name, const cha - *log = NULL);

Также библиотека DynamixelWorkbench включает в себя множество методов для прямого обращения к конкретному полю ПЗУ и ОЗУ сервопривода. Данные методы автоматически подбирают нужные ячейки памяти для любой модели сервопривода и передавать в них адрес ячейки памяти не нужно. Рассмотрим несколько основных методов.

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

bool torqueOn(uint8_t id, const char **log = NULL); bool torqueOff(uint8_t id, const char **log = NULL);

Здесь первый передаваемый параметр - это ID сервопривода, второй

log строка.

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

bool goalPosition(uint8_t id, int32_t value, const char **log = NULL); bool goalVelocity(uint8_t id, int32_t value, const char **log = NULL);

В данном случае первый передаваемый параметр - ID, второй параметр - абсолютное значение скорости или позиции в системе управления сервопривода (для модели минимальное значение равно 0, максимальное - 16384).

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

bool getPresentPositionData(uint8_t id, int32_t* data, const char **log = NULL);

bool getPresentVelocityData(uint8_t id, int32_t* data, const char **log = NULL);

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

int32_t velocity_data; dxl_wb.getPresentVelocityData(ID, &velocity_data);

Также существуют методы, позволяющие управлять сервоприводом, используя систему измерения СИ. То есть положение вала будет задаваться в РАДИАНАХ, а скорость вала в РАДИАНАХ В СЕКУНДУ.

bool goalPosition(uint8_t id, float radian, const char **log = NULL); bool goalVelocity(uint8_t id, float velocity, const char **log = NULL);

Ключевое отличие в том, что здесь второй принимаемый параметр (то есть положение или скорость) имеет тип float, а не int32_t, и именно поэтому воспринимается как радианы или радианы в секунду.

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

bool getRadian(uint8_t id, float* radian, const char **log = NULL); bool getVelocity(uint8_t id, float* velocity, const char **log = NULL);

Типовые примеры по работе с библиотекой DynamixelWorkbench с помощью контроллеров OpenCM и OpenCR можно найти в Arduino IDE (после установки необходимых для платы библиотек) во вкладке «Файлы» -> «Примеры» -> «OpenCM/OpenCR» -> «DynamixelWorkbench».

Использование Dynamixel Wizard 2.0

Для проверки работоспособности, исправности, а также для настройки сервопривода можно воспользоваться утилитой Dynamixel Wizard 2.0. Данное приложение можно установить на любую операционную систему. Интерфейс этого приложение показан на рис. 2.18. С помощью данного приложения удобно изменять стандартные настройки сервопривода, такие как: ID, BAUDRATE и т.д. Также Dynamixel Wizard 2.0 позволяет обновить прошивку сервопривода.

Рис. 2.18. Окно программы DYNAMIXEL Wizard 2.0

Базовые манипуляции с сервоприводом

Для подключения сервоприводов к компьютеру необходимо воспользоваться соответствующим преобразователем интерфейсов (адаптером). Таких адаптеров существует 3 вида: usb2dynamixel, U2D2, USB- DXL-AR. На рис. 2.19 представлена схема подключения сервоприводов к компьютеру с использованием адаптера USB-DXL-AR.

Рис. 2.19. Схема подключения сервоприводов DYNAMIXEL к компьютеру

Рис. 2.20. Преобразователь USB-DXL-AR

Стоит учесть, что USB-DXL-AR не подает питание на сервопривод, поэтому питание должен обеспечивать внешний источник питания. Будьте осторожны, так как рекомендуемое напряжение для каждого сервопривода может варьироваться в зависимости от модели. Для обеспечения сервоприводов необходимым питанием рекомендуется использовать преобразователь DXL-PWR или SMPS2Dynamixel (рис. 2.21)

Рис. 2.21. Преобразователь DXL-PWR и SMPS2Dynamixel

Подключение возможно только к соответствующему разъему от сетевого адаптера и к сервомотору DYNAMIXEL, тем самым, передавая последнему питание от сети. На преобразователе DXL-PWR установлены четыре 3-пиновых (2 стандарта MOLEX 2 стандарта JST) разъема для подключения сервомоторов DYNAMIXEL серии АХ и четыре 4-пиновых разъема (2 стандарта MOLEX 2 стандарта JST) для серий DX/RX. Линии питания и данных объединены, что позволяет использовать преобразователь в качестве шины расширения сервомоторов.

После соединения всех компонентов и подключения к компьютеру, можно перейти к поиску устройств в приложении. Для этого нажмите кнопку «Scan» в интерфейсе программы Dynamixel Wizard 2.0. После чего появится окно с выбором настроек протокола и подключения (рис. 2.22).

В данном окне необходимо выбрать порт, к которому подключен преобразователь интерфейсов USB-DXL-AR. После этого нужно выбрать остальные настройки поиска, такие как: протокол общения (первого или второго типа), скорость, на которой будет происходить поиск, и ID приводов, которые необходимо найти. Если же о приводах ничего не известно, то лучше отметить все параметры для поиска. Из-за этого поиск займет больше времени, но шансов инициализировать привод с первого раза станет больше.

Найденные сервоприводы высвечиваются в колонке слева (рис. 2.23) и группируются по моделям, учитывая их ID, Baudrate и т.д

Рис. 2.22. Окно настроек подключения

Рис. 2.23. Процесс поиска подключенных сервоприводов

Рис. 2.24. Окно настроек сервопривода

Клавиша «Factory Reset», расположенная в правом верхнем углу (рис. 2.24), обнулит все настройки сервопривода (ID и Baudrate не будут изменены). Клавиша «Reboot» перезагрузит сервопривод. Ползунок «Torque» включает или выключает момент на валу сервопривода. Ползунок «LED» вкключает и выключает встроенный в сервопривод светодиод.

С помощью данного приложения также можно в онлайн режиме управлять скоростью и позицией сервопривода. Для этого необходимо убедиться, что ползунок «Torque» стоит во включенном положении. Затем в управляющей таблице сервопривода требуется выбрать элемент «Goal Position». После этого в правом нижнем углу программы появится интерфейс управления позицией. Управление возможно двумя способами: вращением красной стрелки или записью числового значения позиции напрямую через ползунки снизу интерфейса. При выставлении новой целевой позиции, сервопривод будет сразу занимать необходимую позицию (рис. 2.25). При этом индикаторы T («Transmit» – передача данных) и R («Receive» – прием данных), расположенные в нижнем левом углу экрана, показывают, происходит ли в данный момент обмен данными между сервоприводами и компьютером, а также указывают направление этого обмена данными.

Рис. 2.25. Управление положением сервопривода

Построение графиков

Для визуального анализа характеристик работы сервоприводов в приложении Dynamixel Wizard 2.0 имеется функционал визуализации различных параметров в виде построения графиков. Для использования данного функционала необходимо зайти в меню «Tools» > «Options» (рис. 2.26)

Рис. 2.26. Меню инструментов

Далее требуется выбрать пункт «Graph» в левом меню выпавшей панели и нажать на кнопку “+” в правом нижнем углу для добавления параметра, график которого необходимо построить (рис. 2.27).

Следующим шагом необходимо выбрать требуемый для визуализации параметр, масштаб и цвет графика. Также, нажав галочку “Use Multi axis”, на графике можно добавить еще одну ось (рис. 2.28).

Рис. 2.27. Окно настроек визуализации

Параметр «Interval» - это интервал между принятием и отсылкой пакетов. Значение этого параметра рекомендуется оставить равным 20 мс. Для плавной скорости построения графика рекомендуется нажать на галочку “Read only selected items for Speed”. Нажмите на кнопку «ОК», чтобы сохранить изменения.

Для вывода графика на экран необходимо запустить соответствующую утилиту. Для этого требуется выбрать в окне меню View кнопку Graph (рис. 2.29).

Для старта процесса построения графика нужно нажать кнопку

«Start» (рис. 2.30). После начала построения можно изменять положение фланца сервопривода путем работы с регистром «Goal Position», как это было показано ранее.

Рис. 2.28. Окно выбора параметра

Рис. 2.29. Открытие окна с построением графиков

Рис. 2.30. Построение графиков текущего положения и требуемого положения фланца сервопривода выполнять настройку вывода можно с помощью следующих параметров:

Start/Stop : начать/остановить построение графика.

Reset : вернуть масштаб в начальную позицию.

Capture : сохранить график, как картинку.

Save : сохранить график в CSV формате.

Zoom In : выделите зону, чтобы приблизить ее.

Shift + Select Area : увеличение масштаба (Zoom In) по оси Х.

Ctrl + Select Area : увеличение масштаба (Zoom In) по оси Y. Использовать колесо мышки для приближения/отдаления.

Сочетание клавиш Ctrl + «Колесо мыши» изменяет длину оси X от 0 до 10 секунд.

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

Отправка пакета данных

Помимо визуализации параметров работы сервопривода приложение Dynamixel Wizard 2.0 позволяет подключаться к системе управления сервоприводом и обмениваться данными на низком уровне – путем формирования командной последовательности и ее анализа. Для этого

Рис. 2.31. Окно вызова утилиты Packet

В данном окне нужно настроить следующие параметры: порт, к которому подключены сервоприводы, и скорость обмена данными. После чего нажать кнопку «Open», чтобы инициализировать порт общения (Рис. 2.32).

Рис. 2.32. Инициализация порта общения в Packet

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

Рис. 2.33. Выбор протокола обмена данными и инструкции

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

Рис. 2.34. Формирование пакета инструкций

Записанный пакет можно увидеть в строке «Packet Details» в нижней части окна отправки пакета (рис. 2.35)

Рис. 2.35. Сформированный пакет данных

Чтобы отправить пакет данных, используйте кнопку «Send». Если отправка прошла успешно, в левой части окна отправки и приема сообщений появится сообщение от сервопривода об успешном приеме пакета (рис. 2.36).

Рис. 2.36. Отправка пакета данных