Робот-тележка 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 р.):
ecvbv42ipnldzd5vhureodavsqi.jpeg
Однако необходимо учесть, что у первого kinectа минимальная область видимости (слепая зона) 0,5 м, что может негативно сказаться на навигации в стесненных условиях и он достоточно громоздкий. У лидара слепая зона поменьше 0,2 м. kinect v. 2 не подойдет к raspberry 3b (на ней нет usb 3.0). Arduino mega уступает teenzy размерами (и некоторыми характеристиками), кроме того, придется немного переработать код.
В целом «дизайн» выглядит так:
ourux9icibrxsh4ruai61e3wkx4.jpeg

ygbh0hlvz8juq6qi4e9tarrhznq.jpeg

Красоты тут нет, конечно, но не о ней идет речь.
Помимо самого робота, крайне желательно наличие внешнего ПК, на котором будут запускаться графические оболочки визуализации действий робота (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 будет равен числовому значению которое отобразится на экране после вращения колеса:
be7zksecqhlyuyv-e_dkxqdyrwe.png
*PPR = abs (RECENT_COUNT — INITIAL_COUNT).
Значит CPR = 1364. Также надо выставить WHEEL_DIAMETER и LR_WHEELS_DISTANCE в метрах. При чем LR_WHEELS_DISTANCE — это расстояние между центральными осями колес, а не от края до края. Остальные показатели можно игнорировать, если робот двухколесный, дифференциальный (не omni).

Третий момент, который может вызвать сложности — калибровка imu. Для этого, следуя инструкциям, необходимо фиксировать робота в определенных положениях. Подсказки будут в коде при выполнении скрипта rosrun imu_calib do_calib:
ggfj9eg71ilp0baj7rvshjl-fmw.png
, после завершения будет сформирован 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:
qr9mefipf7mqz9vwqqyqkktkeug.png
То есть все происходит «на лету» и в это время в прошивке 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:
rrtdsilkdlggf3ifjhk1uhl8bz0.png
Одна клетка в rviz равна 1 м, поэтому желательно, чтобы робот проезжал этот метр как в визуальном редакторе, а также в натуре.
Кроме того, пройденное расстояние отображается в показателе w:
i1ojl6vg5jyugymjwgk-5cfsrlm.png Оно также должно стремиться к 1 м.
Это же касается и углов поворота робота:
l0ufbvhsko99na50frablnrvsc4.png
*В 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 это расположение можно наблюдать визуально:
1vrzgjmhx-gf-jar7jxfbhcovte.png

Пока недалеко ушли от лидара, поправим его частоту, чтобы он выдавал больше точек (по умолчанию он дает 4k), это поможет при построении карты:

cd linorobot/launch/include/lidar
nano rplidar.launch


Добавим параметр:



и поменяем



Теперь 8к в нашем распоряжении:
zfu0fqd4d7nwmuv50am1qhwqxbm.png
Физическое расположение лидара на роботе также имеет значение:
46mwvqq3kj7nxpskvoadm7xe738.jpeg

Построение 2d карты помещения.


На сайте проекта предлагается построить карту помещения запустив в двух терминалах:

roslaunch linorobot bringup.launch
roslaunch linorobot slam.launch


Однако, практика показала, что slam, который идет по умолчанию с проектом отказывается работать при режиме работы лидара 8к:
zexaa5f_1byvgcvvvkvcr7cowbi.png
, а при режиме 4к карты получаются не слишком удачными, сильно шумят.
Поэтому лучше использовать другой slam — hector-slam.
Как его установить указано здесь.
После установки процедура построения карты будет та же, но вместо roslaunch linorobot slam.launch запустим roslaunch my_hector_mapping my_launch.launch

Карты получаются чище, но и их потом лучше доработать в графическом редакторе, удалив ненужное, тем более, если лидар сталкивался с зеркалами при построении карты:
cnzojvfwpkwgldoo3mylfwwh-yg.jpeg
Не забываем сохранить карту помещения:

rosrun map_server map_saver -f my-map


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

Продолжение следует.

© Habrahabr.ru