Навигация с использованием ArUco-маркеров

ArUco-маркеры — это популярная технология для позиционирования роботехнических систем с использованием компьютерного зрения.

Пример ArUco-маркеров:

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

Для быстрого генерирования маркеров для печати можно использовать онлайн-инструмент: http://chev.me/arucogen/.

aruco_pose

Модуль aruco_pose позволяет восстанавливать позицию коптера относительно карты ArUco-маркеров и сообщать ее полетному контролеру, используя механизм Vision Position Estimation.

При наличии источника положения коптера по маркерам появляется возможность производить точную автономную indoor-навигацию по позициям при помощи модуля simple_offboard.

Включение

Необходимо убедиться, что в launch-файле Клевера (~/catkin_ws/src/clever/clever/launch/clever.launch) включен запуск aruco_pose и камеры для компьютерного зрения:

<arg name="main_camera" default="true"/>

<arg name="aruco" default="true"/>

При изменении launch-файла необходимо перезапустить пакет clever:

sudo systemctl restart clever

Настройка карты ArUco-меток

В качестве карты меток можно использовать автоматически сгенерированный ArUco-board.

Настройка карты меток производится с помощью файла ~/catkin_ws/src/clever/clever/aruco.launch. Для использования AruCo-board введите его параметры:

<node pkg="nodelet" type="nodelet" name="aruco_pose" args="load aruco_pose/aruco_pose nodelet_manager">
    <param name="frame_id" value="aruco_map_raw"/>
    <!-- тип маркерного поля -->
    <param name="type" value="gridboard"/>

    <!-- количество маркеров по x -->
    <param name="markers_x" value="1"/>

    <!-- количество маркеров по y -->
    <param name="markers_y" value="6"/>

    <!-- ID маркера первого маркера (левого верхнего) -->
    <param name="first_marker" value="240"/>

    <!-- длина стороны маркера в метрах -->
    <param name="markers_side" value="0.3362"/>

    <!-- растояние между маркерами -->
    <param name="markers_sep" value="0.46"/>
</node>

Можно задать отдельно расстояние между маркерами по горизонтали и вертикали:

<!-- расстояние между маркерами по горизонтали -->
<param name="markers_sep_x" value="0.97"/>

<!-- расстояние между маркерами по вертикали -->
<param name="markers_sep_y" value="1.435"/>

Если используется карта с нестандартным порядком ID меток, то можно использовать параметр marker_ids:

<rosparam param="marker_ids">[5, 7, 9, 11, 13, 15]</rosparam>

Нумерация маркеров ведется с левого верхнего угла поля.

Для контроля карты, по которой в данный момент коптер осуществляет навигацию, можно просмотреть содержимое топика aruco_pose/map_image. Через браузер его можно просмотреть при помощи web_video_server по ссылке http://192.168.11.1:8080/snapshot?topic=/aruco_pose/map_image:

При полетах необходимо убедиться, что наклеенные на пол метки соответствуют карте.

В топике aruco_pose/debug (http://192.168.11.1:8080/snapshot?topic=/aruco_pose/debug) доступен текущий результат распознования меток:

TODO

Система координат

По соглашению, в маркерном поле используется стандартная система координат ENU:

x — вправо (условный "восток");
y — вперед (условный "север");
z — вверх.

Примечание: указанное выше определение приведено для ситуации, когда поле маркеров лежит на полу.

Таким образом, нулевой является левая нижня точка маркерного поля. Угол по рысканью считается равным 0, когда коптер смотрит направо (по оси x).

Настройка полетного контролера

Для правильной работы Vision Position Estimation необходимо (через QGroundControl) убедиться, что:

Для PixHawk: Установлена прошивка с LPE (local position estimator). Для PixHawk необходимо скачать прошивку px4fmu-v2_lpe.px4. Для PixRacer параметр SYS_MC_EST_GROUP должен быть установлен в local_position_estimator, attitude_estimator_q.
В параметре LPE_FUSION включены только флажки vision position, vision yaw, land detector. Итоговое значение 28.
Выключен компас: ATT_W_MAG = 0
Вес угла по рысканью по зрению: ATT_W_EXT_HDG = 0.5
Включена ориентация по Yaw по зрению: ATT_EXT_HDG_M = 2 MOCAP.
Настройки VPE: LPE_VIS_DELAY = 0 sec, LPE_VIS_XY = 0.1 m, LPE_VIS_Z = 0.15 m.
Рекомендуемые настройки land detector'а:
- COM_DISARM_LAND = 1 s
- LNDMC_ROT_MAX = 45 deg
- LNDMC_THR_RANGE = 0.5
- LNDMC_Z_VEL_MAX = 1 m/s

Для простоты настройки можно воспользоваться готовым файлом настроек для Clever 2 или для Clever 3 и вгрузить его в контроллер с помощью меню Tools - Load from file из раздела Parameters в QGroundControl.

Полет

При правильной настройке коптер начнет удерживать позицию по VPE (в режимах POSCTL или OFFBOARD) автоматически.

Для автономных полетов можно будет использовать функции navigate, set_position, set_velocity. Для полета в определенные координаты маркерного поля необходимо использовать фрейм aruco_map:

# Вначале необходимо взлететь, чтобы коптер увидел карту меток
# и появился фрейм aruco_map:
navigate(0, 0, 2, frame_id='fcu_horiz', speed=0.5, auto_arm=True) #  взлет на 2 метра

time.sleep(5)

# Полет в координату 2:2 маркерного поля, высота 2 метра
navigate(2, 2, 2, speed=1, frame_id='aruco_map', update_frame=True)  #  полет в координату 2:2, высота 3 метра

См. другие функции simple offboard.

Расположение маркеров на потолке

Образ версии >0.2.

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

Чтобы задавать карту маркеров в "перевернутой" системе координат, необходимо изменить параметр aruco_orientation в файле ~/catkin_ws/src/clever/clever/aruco.launch:

<param name="aruco_orientation" value="local_origin_upside_down"/>

При задании вышеуказанного параметра фрейм aruco_map также окажется "перевернутым". Таким образом, для полета на высоту 2 метра ниже потолка, аргумент z нужно устанавливать в 2:

navigate(x=1, y=2, z=1.1, speed=0.5, frame_id='aruco_map')

Навигация по ArUco-маркерам