locomotion_controller

Last updated 7 months ago

locomotion_controller

Данный модуль является контроллером шагания робота. В качестве алгоритма стабилизации корпуса он использует метод ZMP Preview Control.

На вход алгоритма поступают желаемые скорости движения, на основе которых блок Foot Planner строит желаемые точки опоры робота, а также выдает информацию о том, в какой фазе нога должна находиться в каждый момент времени: опоры или переноса. Если нога должна находиться в фазе переноса, то модуль Swing Trajectory Generater формирует траекторию на основе текущего положения ступни, высоты шага, частоты шага и точки, в которую нужно поставить ногу. Если нога должна находиться в фазе опоры, то модуль ZMP Trajectory Generator формирует задающую траекторию движения точки ZMP (Zero-Moment Point), которая поступает на блок ZMP Preview Control. Этот модуль, используя модель линейного обратного маятника генерирует задающую траекторию для ЦМ (центра масс) корпуса робота. Далее, благодаря модулю Body Moving Control координаты ЦМ преобразуются в координаты ступней модуля. Выходы модулей Swing Trajectory Generator и Body Moving Control передаются на мультиплексирующий модуль Mux, который выдает желаемые координаты ступней каждой ноги робота в зависимости от того, в какой фазе должна находиться конкретная нога. В конце блок решения обратной задачи кинематики IK Solver преобразует координаты в желаемые углы в сочленениях и отправляет их в аппаратную часть по протоколу LCM.

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

как у млекопитающих, обозначается буквой "m"

X-образная, обозначается буквой "х"

О-обращная, обозначается буквой "о"

LCM API

Канал SERVO_CMD: предназначен для отправки задающих параметров сервоприводов. Формат сообщения имеет следующий вид:

struct servo_cmd_msg
{
    float timestamp; // временная метка

    float position[12];  // угловое положение [рад]
    float velocity[12];  // угловая скорость [рад/сек]
    float torque[12];  // момент [Н м]
    float kp[12];  // пропорциональный коэффициент
    float kd[12];  // дифференциальный коэффициент
}

Канал SERVO_STATE: предназначен для получения состояния сервоприводов. Формат сообщения имеет следующий вид:

struct servo_state_msg
{
    float timestamp; // временная метка

    float position[12];  // угловое положение [рад]
    float velocity[12];  // угловая скорость [рад/сек]
    float torque[12];  // момент [Н м]
}

ROS API

Параметры

Файл параметров располагается в папке config

general
- frequency (по умолчанию: 300) - частота, на которой работает модуль
- contact_flags - если True, то показывает находится ли в данный момент нога в фазе опоры
simulation - параметры для симуляции
- kp - пропорциональный коэффициент для ПД-регулятора привода
- kd - дифференциальный коэффициент для ПД-регулятора привода
- yaw_kp - пропорциональный коэффициент для П-регулятора, стабилизирующего курс робота
- kinematic_scheme - выбор кинематической схемы ног робота. Доступные варианты: "m", "x", "o". То, как будет выглядеть робот в стойке, показано на рисунках выше.
- robot_height - высота робота в стойке
- cog_x_offset - смещение центра тяжести корпуса робота по оси X
- cof_z_offset - смещение центра тяжести корпуса робота по оси Y
- ef_init_x - изначальное положение стопы робота по оси X
- ef_init_y - изначальное положение стопы робота по оси Y
- stride_frequency - частота шага робота во время ходьбы
- preview_horizon - на сколько времени вперед алгоритм прогнозирует будущее состояние робота
hardware
- kp - пропорциональный коэффициент для ПД-регулятора привода
- kd - дифференциальный коэффициент для ПД-регулятора привода
- yaw_kp - пропорциональный коэффициент для П-регулятора, стабилизирующего курс робота
- kinematic_scheme - выбор кинематической схемы ног робота. Доступные варианты: "m", "x", "o". То, как будет выглядеть робот в стойке, показано на рисунках выше.
- robot_height - высота робота в стойке
- cog_x_offset - смещение центра тяжести корпуса робота по оси X
- cof_z_offset - смещение центра тяжести корпуса робота по оси Y
- ef_init_x - изначальное положение стопы робота по оси X
- ef_init_y - изначальное положение стопы робота по оси Y
- stride_frequency - частота шага робота во время ходьбы
- preview_horizon - на сколько времени вперед алгоритм прогнозирует будущее состояние робота
topics - названия ros-топиков и lcm-каналов, с которыми работает пакет
- ros_servo_cmd_topic (по умолчанию: "joint_cmd") - топик, по которому передаются команды на приводы
- lcm_servo_cmd_channel (по умолчанию: "SERVO_CMD") - название LCM-канала, по которому передаются команды на приводы
- lcm_servo_state_channel (по умолчанию: "SERVO_STATE") - название LCM-канала, по которому передается состояние приводов
- ros_imu_topic (по умолчанию: "imu/data") - название топика для получения данных с IMU
- ros_cmd_vel_topic (по умолчанию: "cmd_vel") - топик для получения желаемой скорости
- ros_cmd_pose_topic (по умолчанию: "cmd_pose") - топик для получения желаемого положения и ориентации корпуса робота
- ros_cmd_ef_pose_topic (по умолчанию: "ef_position/command") - топик для получения желаемого положения стоп робота
- ros_cmd_joint_pos_topic (по умолчанию: "joint_group_position_controller/command") - топик для получения желаемых значений для сервоприводов
- ros_state_contact_flags_topic (по умолчанию: "foot_contacts") - топик для выдачи информации о том, находится ли нога в фазе опоры
- ros_state_ef_pose_topic (по умолчанию: "ef_position/states") - топик для выдачи текущего положения стоп робота

Входящие топики

Исходящие топики

Previousjoy_converter Nextlog_writer

Last updated 7 months ago