Архитектура подсистемы управления движением

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

Общие требования

Основные варианты использования робота:

  1. Передвижение (свободная походка, ходьба, рысь) по внешней команде (интерфейс/джойстик).
  2. Осуществление различных действий: сесть, лечь, помахать копытом, эмоции и т.д. Выполнение инициируется командами, возможность запуска зависит от состояния робота.
  3. Синхронное управление в рамках означеныx действий разными элементами робота: ноги, голова, уши, хвост, глаза.
  4. Возможность быстрого включение новых действий.

На их основе выделяются следующий понятия:

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

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

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

Безударные (плавные) переключения. Желаемая поза робота при переключении задатчика может резко измениться, при попытке исполнения такого движения возникнет рывок/удар. Такое поведение нежелательно.

Общие требования к архитектуре

  1. Слабая связанность компонент (минимизация зависимостей меду модулями, исключение одинаковых настроек).
  2. Работа в реальном времени: OROCOS, ROS nodelet.
  3. Открытость (добавление новых модулей), интеграция с со стандартными средствами ROS tf, diagnostics.
  4. Централизированные средства настройки и мониторинга: dynamic_reconfigure, rqt, rviz, журналирование сообщений.

Специальные требования к архитектуре

  1. Наличие подсистемы переключения походок, исключение конфликтов ресурсов.
  2. Работа задатчиков в разных системах координат: декартовой, угловой.
  3. Т.к. решение задачи обратной кинематики потенциально неоднозначно, то для реализации ряда движений может потребоваться тесная интеграция с задатчиков и средств решения задачи обратной кинематики. Архитектура должна предоставлять средства разного уровня (простое решение по умолчанию, специализированные модули, настройки модуля ОК).

Общая схема

Ниже приведена общая схема СУ: arch-schema-9.png

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

Выделяются следующие подсистемы:

  1. Подсистема задатчиков движения. Походки/анимации и механизм переключения между ними (gait_*, animation_*, resource_arbiter).
  2. Агрегация желаемой позы и компенсация динамики робота. Агрегация и публикация желаемого состояния робота (agregator_*_ref). Расчет задающего воздействия для приводов (servo_inv_*, dynamics_inv) на основе модели робота.
  3. Исполнительная подсистема. Взаимодействие с аппаратной частью (herkulex_*, *_driver).
  4. Сенсорная подсистема. Обработка и публикация показания датчиков (agregator_real, odometery). Расчет внешних сил (servo_ident, dynamics_fwd).

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

В качестве основных переменных состояния робота выступают: позиция (q --- поза робота в угловой СК, s --- поза тела в АК), скорость (dq --- в угловой СК, w --- скорость платформы), список точек касания (c), силы реакции опоры (N), * моменты приводов (tau).

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

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

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

Для достижения последовательного исполнения компонент в течении цикла управления используется механизм синхронизации. Компоненты ON_TIMER исполняют свой код по получению внешнего сообщения от компонента-таймера на порт sync (порты и таймер в схеме не показаны). Компоненты ON_UPDATE --- по событию обновления входного порта. Если же компоненту типа ON_UPDATE также требуется следить за текущим циклом управления, то он снабжается портом sync_step, получающим сообщение от таймера в начале каждого цикла управления.

При работе нескольких задатчиков поза на выходе agregator_ref будет обновляться несколько раз за цикл управления.

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

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

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

Основные недостатки:

  1. Избыточные вычисления. При изменении выхода любого задатчика приводит к полному пересчета всех задающий воздействий (т.е. исполняются dynamics_inv, servo_inv, kinematics_dk, что включает в себя прямую кинематику и вычисление матриц динамической модели). При использовании "толстых" собщений, то будет не более 1-2 лишних пересчетав. Полностью исключить их можно только ожиданием выхода всех задатчиков, но такая схема менее надежна (вдруг сообщение не придет) и требует от агрегатора знания набора активных задатчиков. Возможный компромисс --- установка приоритетов задатчиков выше, чем у агрегатора.

  2. Относительно примитивный алгоритм выделения ресурсов. Подробнее см. в разделе Переключение задатчиков.

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

  4. Передача опорных коэффициентов SupportState выбивается из парадигмы "толстых" сообщений. Для устранения необходим переход к JointLimbState на выходе из задатчика. Это обсуждается позже.

Варианты использования общей схемы

Указанная архитектура довольно гибка. Она позволяет допускает много различных вариантов использования:

  1. Подсистема переключения модулей может быть использована для "вытесняющего" захвата любых разделяемых ресурсов робота (legs, head, tail, eyes). С другой стороны принципиально возможно не включать в нее eyes и tail и делать совершенно отдельную подсистему. Однако тогда потребуются дополнительные усилия по синхронизации задатчиков.

  2. Компоненты походки (gait_*) и анимации (animation_*) могут работать как в угловой, так и в декартовой СК. Модули обратной кинематики могут быть использованы разными способами:

    • Вывод желаемой позиции в декартовой СК в порт. Для перехода угловой используется стандартный компонент обратной кинематики (kinemaics_inv).

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

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

  3. Обработка полной позы робота на выходе agregator_ref может быть организована разными способами: непосредственная передача на приводы, через обращение генератора траектории приводов (servo_inv), с учетом динамики робота (компонент dynamics_inv).

  4. Использования имен приводов в ServoGoal и внешнего тактирования позволяет разбить одну сеть Herkulex на две. Это позволит ускорить обмен.

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

Альтернативные решения

К текущей схеме можно предложить две модификации.

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

    Замечания: Полезны метки заголовка seq, чтобы точно отнести к конкретному периоду пришедшее сообщение. Следует предусмотреть разные алгоритмы интегрирования и дифференцирования.

  2. Использование сообщения JointLimbState при передаче данных от задатчиками к агрегатору, агрегатор публикует позу в JointLimbState и JointState одновременно. Здесь положительные эффекты и не так очевидны. Особенности такой реализации представлены в виде списка:

    • agregator_ref способен работать в терминах ресурсов, возможна реализация более сложной системы переключения задающих воздействий. Подробнее см. Переключение задатчиков .
    • Некоторым алгоритмам (обратной и прямой кинематики, динамики из KDL) проще работать сразу с JntArray, а использование JointState подразумевает необходимость преобразования JntArray.
    • В состав JointLimbState и CartesianState (MultiDOFJointState) можно сразу внести опорные коэффициенты, тогда необходимость в дополнительных средствах их передачи отпадает.
    • Усложняется agregator_ref, он должен обладать полной информацией по именованию звеньев и их отношениях в кинематической цепочки. В базовой реализации это не нужно: она просто получает сообщений JointState, сохраняет их и посылает снова.
    • Усложняется процедура инициализации agregator_ref начальной позой. Требуется либо отдельный порт, либо операция, либо преобразование JointState (показания датчиков) в JointLimbState.
    • Усложняются задатчики, работающие с JointState: траектория из файла, траектория из rviz.

Из базового варианта приведенные решения исключены в целях упрощения.

Ошибочные ситуации

Механизмы извещения о ошибках:

  1. Текстовой журнал (Logger OROCOS,ROS_ERROR, ROS_INFO...). Различаются уровни debug, info, warning, error.

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

  2. Сообщения giagnostic_msg.

    Дополнение к основному механизму, использование обязательно когда требуется известить верхний уровень о ошибке. В принципе, безопасно дублировать ими базовый механизм. Однако требует добавления во все компоненты порта diagnostics.

  3. Перевод компонент OROCOS в состояние Error, FatalError

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

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

Уровни ошибочных ситуаций:

  1. Предупреждение (warning) --- подозрительная ситуация, не влияющая на работу компонента существенно (неправильное имя, лишняя конечность и т.п.)
  2. Ошибка (error) --- ошибочная ситуация, текущая операция должна быть прервана/отменена, но работоспособность сохранена (попытка решить задачу кинематики для лишней конечности, принципиальная невозможность решить задачу обратной кинематики, привод не отозвался на запрос).
  3. Фатальная ошибка (fatal, critical) --- ошибочная ситуация, требующая остановки компонента и делающее его неработоспособным до вмешательства пользователя или до перезагрузки (повторяющиеся ошибки ввода-вывода).

Уровни журналирования:

  1. Информация (info) --- важные события в жизни компонент.
  2. Отладка (debug) --- отладка вне контекста РВ.
  3. Отладка РВ (realtime) --- отладка контекста РВ.

Deployer сохраняет свой журнал в orocos.log, по умолчанию уровень info.

Использование ROS и OROCOS

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

Для тактирования ON_TIMER предлагается использовать внешний компонент-таймер (на схеме не показан). Можно использовать стандартный таймер OCL::TimerComponent, но лучше на его базе вделать свой, выдающий номер периода дискретизации, который помещается в поле seq заголовка сообщений. Эта информация может быть очень полезна при численном интегрировании, дифференцировании и отладке.