Управление относительно поля: «вперёд» значит «вперёд»

Проблема, которая возникнет через десять секунд

Соберите робота с приводом mecanum, возьмите пульт и толкните стик вперёд. Робот уезжает от вас. Хорошо.

Теперь поверните его на 180° и снова толкните стик вперёд. Робот едет на вас, потому что «вперёд» означает «вперёд для робота», а робот теперь обращён к вам. Оператор автоматически мысленно поворачивает команду зеркально, ошибается, и робот разбивается.

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

Исправление — одна матрица поворота.

Две системы координат, одно преобразование

Стик можно интерпретировать двумя способами:

• В системе координат робота. Стик — команда в собственной системе робота, base_link, если говорить на языке ROS. Толкните вперёд — робот движется вдоль своего курса.
• В системе координат поля. Стик — команда в системе координат пола. Толкните вперёд — робот движется на север, куда бы ни смотрел его нос.

Привод понимает только систему робота — кинематика mecanum записана именно в ней. Поэтому управление относительно поля означает поворот команды оператора в систему робота до того, как её увидит кинематика.

Если курс робота равен θ (измеряется против часовой стрелки от оси x поля), поверните команду на −θ:

vx_robot = vx_field · cos(θ) + vy_field · sin(θ)
vy_robot = −vx_field · sin(θ) + vy_field · cos(θ)

Минус θ, а не θ. Неправильный знак даёт робота, который выглядит исправным, когда направлен вперёд, и едет точно в противоположную сторону после поворота на 90° — это десять мучительных минут.

Вращательная составляющая команды не преобразуется. ω означает одно и то же в обеих системах — вращение остаётся вращением, — поэтому значение передаётся напрямую.

Всё решение в коде

#include <math.h>

// stickX: forward on the stick, positive = away from the driver (field +x)
// stickY: left on the stick,    positive = to the driver's left (field +y)
// stickW: twist, positive = counter-clockwise
void driveFieldOriented(float stickX, float stickY, float stickW) {
  float theta = imuYawRadians() - headingOffset;   // see "the reset" below

  float c = cosf(theta);
  float s = sinf(theta);
  float vx =  stickX * c + stickY * s;
  float vy = -stickX * s + stickY * c;

  driveMecanum(vx, vy, stickW);   // the inverse kinematics, unchanged
}

Шесть строк, а это разница между роботом, которым можно управлять, и роботом, которым нельзя. Обратите внимание, что driveMecanum() ничего об этом не узнаёт: ориентация относительно поля — это преобразование команды, а не изменение привода.

Как получить θ

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

Рыскание от гироскопа получается интегрированием, поэтому смещение превращается в линейный дрейф:

// Integrate yaw rate, with the bias you measured while it sat still.
float rate = gyroZ() - gyroBiasZ;             // rad/s
if (fabsf(rate) < 0.004f) rate = 0.0f;        // deadband: kill the last of the creep
yaw += rate * dt;

Две детали важнее выбора датчика:

• Измеряйте смещение при каждой загрузке, пока робот неподвижен. Усредните 500 отсчётов за секунду-две до начала движения. Сырой MPU6050 без удаления смещения дрейфует на десятки градусов в минуту; с ним — на единицы.
• Мёртвая зона должна быть на скорости, а не на угле. Мёртвая зона на угле теряет медленные настоящие повороты. Мёртвая зона на скорости отбрасывает только шумовой пол, который как раз и интегрируется в дрейф.

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

Сброс, который обязателен

Ручной ноль необходим по двум причинам:

1. Дрейф. Через несколько минут представление робота о севере и представление оператора расходятся, и исправление становится проблемой.
2. Начальная ориентация. Ноль IMU находится там, куда робот смотрел при включении, а это редко совпадает с положением оператора.

Поэтому храните смещение и кнопку:

float headingOffset = 0.0f;

void resetFieldHeading() {          // bind to a button the driver can hit blind
  headingOffset = imuYawRadians();  // "whatever you are pointing at is now forward"
}

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

Ориентация относительно поля и для автономности

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

• «Переместиться на 300 mm к цели» — цель задана в координатах поля, значит, команда тоже.
• «Ехать боком, удерживая камеру в центре цели при её облёте» — облёт задан в системе поля, вращение — в системе робота, а управление относительно поля позволяет записать оба действия в одной строке.
• Всё, что находится после планировщика маршрута, который планирует по карте, а значит, в системе поля.

В ROS это делает преобразование odom → base_link, поэтому существует TF2 и поэтому cmd_vel явно определён в base_link. Управление относительно поля — самостоятельная реализация преобразования, которое иначе запросили бы у TF2.

Как это увидеть

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

Один и тот же стик, колёса и кинематика. Разница только в шести строках тригонометрии.

Диагностика

Далее

Ориентация относительно поля делает команду правильной. Замкнутое управление скоростью колёс заставляет колёса действительно её обеспечивать: на роботе с омни-приводом это единая система, потому что неодинаковое падение момента двигателей отклоняет робота от курса ещё до того, как IMU сможет вмешаться.