Алгоритмы компенсации динамики робота

Данный документ содержит концепт системы управления движением с учетом динамики. Он базируется на системе управления NimbRo-Op (в части придания упрогости звеньев) [Compliant Robot Behavior using Servo Actuator Models identified by Iterative Learning Control] и курсе лекций в части решения обратной задачи динамики (расчета усилий приводов по желаемой скорости).

Задачи решаемые подсистемой

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

  2. Обеспечить упругость (compliance) за счет приводов. Это означает, что конечности робота ведут себя как немного подпружиненные. Это позволяет решать множество проблем с естественностью походки (неточная постановка ног), снизить энергопотребление, сделать движения робота менее жесткими.

  3. Идентификация неизвестных параметров приводов (модели силы трения) без разборки робота.

  4. Детектирование внешних сил, действующих на робота. В частности ударов о препятствие, постановка ноги, внешнее силовое воздействие.

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

Система управления движением

arch-schema-9.png

Система первого этапа

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

Основная идея

Формируемая на выходе агрегатора agregator_ref желаемая поза (q (JointState) --- углы) транслируется в команды привода (g (ServoGoal) --- пара: целевая позиция и время ее достижения) модулем обращения приводов servo_inv_lead каждый период дискретизации.

Особенности

  1. Требуется задавать только желаемую позу в угловой СК.

  2. servo_inv_lead берет упреждение при вычислении целевой позиции приводов на 1-2 период дискретизации, чтобы обеспечить плавное движение в случае потери пакета.

  3. Коэффициент ПИД-регуляторов приводов должны быть достаточно велики, что обеспечить точность исполнения команд. В результате упругость в суставах отсутствует.

Система второго этапа

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

Необходимые условия реализации

  1. Наличие достоверно динамической модели робота в URDF формате для: решения задачи обращения

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

  3. Требуется знать модели приводов и силы трения. Эту задачу предлагается решать методом идентификации. При идентификации необходимо подавать тестовую последовательность и опрашивать состояние приводов.

  4. Определение внешних сил возможно только при опросе приводов. При этом требуется знать позиции, скорости и ускорения.

Основная идея

Агрегатор agregator_ref формирует полную желаемую позу робота q, dq в угловой СК (JointState), также он агрегирует информацию о контактах от задатчиков. Компонент прямой кинематики kinematics_fwd рассчитывает положения концов ног и головы и их скорости. Далее модуль одометрии odometry_legs, либо используя информацию о опорных ногах полученную от агрегатора, либо определяя предполагаемый список контактов, рассчитывает позу s и скорость w платформы (тела) робота.

Полная поза робота (q, dq, s, w) поступает на вход модуля обратной динамики dynamics_inv, который по динамической модели робота [Robot Dynamics Lecture Notes, формула (3.99)] рассчитывают моменты приводов tau и силы реакции опоры N в точках контакта. Для расчетов динамики придется вычислять ускорения, численно дифференцируюя скорость. Динамическая модель робота загружается из URDF.

Компонент обращения приводов servo_inv_7param по желаемым позициям q, скоростям dq и рассчетным усилиям приводов tau определяют команды для приводов, опираясь на его модель и модель силы трения. Здесь используется модель из [Compliant Robot Behavior using Servo Actuator Models identified by Iterative Learning Control]. Модели приводов загружаются сообщениями servo_model (ServoModel) либо идентификатором, либо из файла.

Компонент идентификации и расчета ошибки предсказания servo_ident способен по внешней команде проводить идентификацию заданной группы приводов. На вход получает измеренную позицию q measured и скорость dq measured (JointState), расчетные моменты tau (JointState). Он передает сообщение ServoModel компоненту servo_inv_7param. Возможна загрузка и сохранение моделей из параметров. Также компонент рассчитывает расхождение tau_e между расчетным моментом по задающему воздействию (tau) и по модели привода. Это расхождение является оценкой внешних сил, действующих на конечность.

Компонент dynamics_fwd пересчитывает моменты приводов в значения эквивалентных внешних сил, приложенных к конечности или иному элементу робота.

Правильный порядок исполнения обеспечивается: 1. назначением Event портов "по последнему", 2. таймером, 3. приоритетами.

Особенности

  1. Не ясно, следует ли включать стопу в расчеты по динамической модели. Если стопы всегда параллельно опорной поверхности и робот находится в состоянии равновесия, то моменты должны быть близки к нулевым. Однако это условие выполнено не всегда.

  2. На первых этапах удобно обойтись без идентификатора servo_ident и загружать модель в xml файла методом сериализации OROCOS в параметр servo_inv_7param.

  3. Скорость и ускорение платформы необходимы для расчета сил в системе со подвижной платформой.

  4. Модуль одометрии предлагается выполнить как часть для агрегатора. Это решает отчасти проблему синхронизации вычислений.

  5. Алгоритм одометрии прост (не очень: он определяет 2-3-4 опорных ноги и полагает, что они касаются земли. Возможно получение информации о касании от модуля походки или иного источника.

  6. В указанной схеме Якобиан надо вычислять несколько раз в разных модулях. Проблема может быть решена за счет сервиса прямой кинематики (?), либо можно публиковать якобиан на уровне агрегатора. На текущий момент предлагается оставить как есть.

Система третьего этапа

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

Необходимые условия реализации

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

  2. Требуются динамическая модель (URDF), модели приводов (идентификация или сохраненные).

  3. Для целей идентификации и оценки внешних сил требуется измерять позиции, скорости и ускорения приводов.

  4. Возможности управления движением конечностей, управляемых в угловой СК ограничены зонами сингулярности якобиана. Надежно ими можно управлять только в зоне "шаговых движений", сложные жесты делать затруднительно, для этого надо использовать угловые координаты.

Основная идея

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

Компонент agregator_dynamics_ref получает на вход от задатчика-стабилизатора походки и других задатчиков желаемое ускорение платформы dw, ускорение свободных конечностей dw/ddq (в абсолютной СК или угловой) и список конечностей в состоянии контакта.

Для 3-х векторов du (ускорение платформы и ускорения приводов), tau (моменты приводов), N внешние силы в точках контакта система управления движением решает оптимизационную задачу последовательно достигая следующих целей:

  1. выполнение уравнений динамики,
  2. выполнение ограничений на скорость для точек контакта,
  3. ограничения на моменты приводов,
  4. условие положительности сил нормального давления, условие непроскальзывания (невыход силы реакции за конус силы трения),
  5. условие на траекторию точки нулевого момента,
  6. обеспечение заданного ускорения платформы,
  7. обеспечение заданного ускорения свободных конечностей .

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

На выходе этого компонента получается задающее воздействие для приводов c их моментами (q, dq, tau) и расчетная траектория движения платформы (s, w). Тем самым он полностью заменяет агрегатор желаемой позы.

Особенности

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

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

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

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

  5. Описанная система не является стабилизатором, это feedforward control, как и система второго этапа) только с дополнительными ограничениями.