Робот-тележка 2.0. Автономная навигация домашнего робота на базе ROS. Часть 1
Проект строился на базе достаточно известного в своих кругах другого проекта — linorobot (linorobot.org), при этом использовались доступные простому обывателю компоненты. Цели, которые были поставлены: добиться автономного перемещения робота в домашних условиях, используя low-cost компоненты, оценить производительность мини-пк для заявленных целей, настроить стек навигации для перемещения в узких пространствах хрущевок.
Железо.
Список компонентов в сухом остатке выглядит так:
— raspberry pi 3b — 2800 р.;
— lidar rplidar a1 — 7500 р.;
— силовые мосты l298n -2шт — 400р.;
— колесная пара с энкодерами типа А и В — 2000 р. (210 rpm — ссылка)
— teenzy 3.2 — 2100 р.;
— imu 9250 (либо 9150, либо 6050) — 240 р.;
— power bank на 10000mH — 1500 р.
— 3 аккумулятора 18650 и держатель -600 р.
— понижающий преобразователь dc-dc c 5V на 3.3V — 20р.
— кусок фанеры, паркета — ? руб.
Итого: 17160 р.
*Стоимость можно снизить, заменив teenzy на arduino mega 2560 (600 р.), а lidar заменив kinectом v. 1 (1000 р.): 
Однако необходимо учесть, что у первого kinectа минимальная область видимости (слепая зона) 0,5 м, что может негативно сказаться на навигации в стесненных условиях и он достоточно громоздкий. У лидара слепая зона поменьше 0,2 м. kinect v. 2 не подойдет к raspberry 3b (на ней нет usb 3.0). Arduino mega уступает teenzy размерами (и некоторыми характеристиками), кроме того, придется немного переработать код.
В целом «дизайн» выглядит так: 
Красоты тут нет, конечно, но не о ней идет речь.
Помимо самого робота, крайне желательно наличие внешнего ПК, на котором будут запускаться графические оболочки визуализации действий робота (rviz, gazebo).
Сборка робота.
Подробно расписана на сайте проекта — ссылка, поэтому остановимся на моментах, которые вызывают затруднения.
Для начала необходимо учесть, что энкодеры на колесной паре питаются 3.3V и при подаче 5V отлично выходят из строя.
Установочный скрипт linorobot успешно ставится на ubuntu 16.04, ubuntu 18.04. Версии ROS: ROS kinetic либо ROS melodic. На raspberry 3b придется предварительно создавать swap-файл.
Основной код, заливаемый в teenzy находится по пути:
roscd linorobot/teensy/firmware/lib/config
nano lino_base_config.h
И заливается в нее командой:
roscd linorobot/teensy/firmware
platformio run --target upload
Вышеуказанные связки команд будут достаточно часто использоваться, так что имеет смысл добавить их в aliases.
Из сложностей при сборке робота могут доставить хлопот правильное соотнесение подключенных контактов, идущих от l298n к teenzy. Не смотря на предельно ясную схему подключения, колеса могут вести себя иначе и придется экспериментировать, подбирая правильное соотнесение пинов в коде.
В моем случае получилось так:
/// ENCODER PINS
#define MOTOR1_ENCODER_A 14
#define MOTOR1_ENCODER_B 15
#define MOTOR2_ENCODER_A 11
#define MOTOR2_ENCODER_B 12
//MOTOR PINS
#ifdef USE_L298_DRIVER
#define MOTOR_DRIVER L298
#define MOTOR1_PWM 21
#define MOTOR1_IN_A 1
#define MOTOR1_IN_B 20
#define MOTOR2_PWM 5
#define MOTOR2_IN_A 8
#define MOTOR2_IN_B 6
Второй момент — прописать спецификацию колес робота:
//define your robot' specs here
#define MAX_RPM 210 // motor's maximum RPM
#define COUNTS_PER_REV 1365 // wheel encoder's no of ticks per rev
#define WHEEL_DIAMETER 0.065 // wheel's diameter in meters
#define PWM_BITS 8 // PWM Resolution of the microcontroller
#define LR_WHEELS_DISTANCE 0.235 // distance between left and right wheels
#define FR_WHEELS_DISTANCE 0.30 // distance between front and rear wheels. Ignore this if you're on 2WD/ACKERMANN
#define MAX_STEERING_ANGLE 0.415 // max steering angle. This only applies to Ackermann st
RPM можно найти в спецификации колес, а вот CPR (COUNTS_PER_REV) расчетная величина.
CPR = PPR x 4. PPR наших колес 341.
*PPR можно либо посмотреть в спецификации либо запуcтить код на роботе roslaunch linorobot minimal.launch и прокрутить колесо на 360 градусов. PPR будет равен числовому значению которое отобразится на экране после вращения колеса: 
*PPR = abs (RECENT_COUNT — INITIAL_COUNT).
Значит CPR = 1364. Также надо выставить WHEEL_DIAMETER и LR_WHEELS_DISTANCE в метрах. При чем LR_WHEELS_DISTANCE — это расстояние между центральными осями колес, а не от края до края. Остальные показатели можно игнорировать, если робот двухколесный, дифференциальный (не omni).
Третий момент, который может вызвать сложности — калибровка imu. Для этого, следуя инструкциям, необходимо фиксировать робота в определенных положениях. Подсказки будут в коде при выполнении скрипта rosrun imu_calib do_calib: 
, после завершения будет сформирован imu_calib.yaml с настройками.
Лучше калибровку выполнять, несмотря на то, что есть дефолтный файл с настройками imu.
*Кроме того, выяснилось, что калибровка imu может влиять на направления движения робота через teleop (влево вместо вправо и наоборот, при этом движение вперед-назад было верным). Ситуация не решалась перестановкой pinов в коде. Сам imu был закреплен и откалиброван вверхтормашками, т.к. закреплен с обратной стороны базы робота. Его (imu) дальнейший переворот без повторной калибровки решил проблему.
Четвертый момент — калибровка pid.
На сайте проекта, подробно описана процедура как реализовать регулировку — ссылка.
Однако новичку будет сложно разобраться что это и как с этим работать. В общих чертах pid регуляция нужна для того, чтобы робот двигался равномерно, без рывков/резких торможений. За это отвечают три параметра: p, i, d. Их значения содержатся в основном коде, заливаемом на teenzy:
#define K_P 2.0 // P constant angle
#define K_I 0.3 // I constant
#define K_D 0.1 // D constant
Можно поэкспериментировать с ними, запустив на роботе:
roslaunch linorobot minimal.launch
Далее на внешнем ПК в трех разных терминалах:
rosrun lino_pid pid_configure
rosrun teleop_twist_keyboard teleop_twist_keyboard.py
rqt
Двигая ползунки p, d, i в терминале rqt, а затем управляя роботом в терминале teleop_twist_keyboard, можно добиться необходимых результатов без необходимости загрузки каждый раз кода с новыми параметрами pid в teenzy: 
То есть все происходит «на лету» и в это время в прошивке teenzy могут находиться совершенно другие значения. *Можно не ориентироваться на графики, т.к. визуально и так будет понятно КАК едет робот.
Рекомендуют начинать с выставления значений p=1.0, d=0.1, i=0.1. Для нашего случая значения следующие (получено опытным путем) p=2.0, i=0.3, d=0.1.
Далее данные значения надо выставить в коде и залить в teenzy.
Также необходимо не забыть выставить в коде (DEBUG 0 вместо DEBUG 1):
#define DEBUG 0Одометрия.
Для ориентации робота и последующей навигации используются данные лидара, imu и данные с энкодеров колес. Если калибровка imu выполнена корректно, лидар корректно установлен, а значения энкодеров верны, то с одометрией проблем быть не должно.
Чтобы проверить корректность одометрии, надо немного поездить на роботе через teleop и посмотреть значения в топике odom, об этом написано на странице проекта — ссылка.
А также посмотреть в визуальную оболочку rviz: 
Одна клетка в rviz равна 1 м, поэтому желательно, чтобы робот проезжал этот метр как в визуальном редакторе, а также в натуре.
Кроме того, пройденное расстояние отображается в показателе w: 
Оно также должно стремиться к 1 м.
Это же касается и углов поворота робота: 
*В rviz и вживую робот должен поворачивать на одинаковые углы.
Одометрия с лидара определяется схожим образом — ссылка.Расстояние от лидара до ближайшей стены как вживую так и в rviz должно совпадать.
TF.
В ROS особое значение придается трасформациям — tf. В общих словах это понятие описывает соотнесение объектов относительно друг друга в пространстве. Чтобы объекты не болтались в воздухе, и программа понимала как они представлены друг относительно друга необходимо уделить внимание настройке tf.
В проекте все связи tf относительно друг друга уже настроены и необходимо только подправить показатели, например, указать как расположен лидар. Это необходимо, чтобы робот понимал, что препятствие возникло не перед ним непосредственно, а перед лидаром, который отстоит от центра робота на определенном расстоянии.
Зайдем в соответствующую директорию:
roscd linorobot/launch
nano bringup.launch
Сама база расположена на расстоянии 0.065 м от пола:
В laser.launch надо выставить расположение лидара относительно центра базы робота:
roscd linorobot/launch/include
nano laser.launch
В моем случае лидар смещен по оси x:-0.08, y:0.04 и «возвышается» над базой (ось z):0.08 м:
В rviz это расположение можно наблюдать визуально: 
Пока недалеко ушли от лидара, поправим его частоту, чтобы он выдавал больше точек (по умолчанию он дает 4k), это поможет при построении карты:
cd linorobot/launch/include/lidar
nano rplidar.launch
Добавим параметр:
и поменяем
Теперь 8к в нашем распоряжении: 
Физическое расположение лидара на роботе также имеет значение: 
Построение 2d карты помещения.
На сайте проекта предлагается построить карту помещения запустив в двух терминалах:
roslaunch linorobot bringup.launch
roslaunch linorobot slam.launch
Однако, практика показала, что slam, который идет по умолчанию с проектом отказывается работать при режиме работы лидара 8к: 
, а при режиме 4к карты получаются не слишком удачными, сильно шумят.
Поэтому лучше использовать другой slam — hector-slam.
Как его установить указано здесь.
После установки процедура построения карты будет та же, но вместо roslaunch linorobot slam.launch запустим roslaunch my_hector_mapping my_launch.launch
Карты получаются чище, но и их потом лучше доработать в графическом редакторе, удалив ненужное, тем более, если лидар сталкивался с зеркалами при построении карты: 
Не забываем сохранить карту помещения:
rosrun map_server map_saver -f my-map
*Карты лучше получаются в приятном полумраке, это позволит выжать максимум из бюджетного лидара.
Продолжение следует.
