К бабушке с пирожками — за край карты

Условная бабушка живёт за краем карты. Для доставки через лес приходится расширять территорию, а цельный растр быстро расходует память. Мы оставили детализацию 5 см и разделили хранение карты и рабочее окно навигации.

Я Лев Ларин, инженер Unibot. Мы подключаем Alfa Robotics SpeedyBot к своей платформе UNIBOT Fleet, сохраняя штатную навигацию робота-доставщика. Это седьмая часть серии.

Начало серии · ← Часть 6: За пивом за угол — через Магадан · Часть 8: Парк, я тебя отрендерил →

Сколько стоит цельный растр

Для территории 2×2 км при клетке 5 см:

2000 м / 0,05 м = 40 000 клеток по стороне
40 000 × 40 000 = 1 600 000 000 клеток

Один байтовый слой занимает 1,6 ГБ, примерно 1,49 ГиБ. RGBA-представление потребовало бы 6,4 ГБ. Это только массивы одного прямоугольника, без копий, других слоёв и 3D-данных. Для узких обследованных дорожек большая часть прямоугольника была бы пустой.

Тайлы на диске

tools/native_map_atlas.py использует Python, NumPy, Pillow и PyYAML. Байтовые растры хранятся PNG-тайлами 512×512 клеток, по 25,6×25,6 м. Метаданные задают общую геометрию; штатные PCD/BlockMap остаются у native SLAM. Пример файла: atlas/tiles/map/-1_0.png.

Из мировой координаты получаем клетку, номер тайла и локальный индекс:

gx = math.floor(x / resolution)
gy = math.floor(y / resolution)
tx, lx = divmod(gx, 512)
ty, ly = divmod(gy, 512)

Для отрицательных дробных координат нужен floor, а не усечение int. При x=-0,05 м и resolution=0,05 клетка равна −1, тайл — −1, локальная колонка — 511. NumPy индексируется как [ly,lx]; при записи PNG переворачиваем Y, поскольку строки изображения идут сверху вниз.

Кеш Tiles хранит восемь декодированных тайлов, вытесняя их по LRU через OrderedDict. Полезные массивы занимают 2 МиБ для одноканальных 512² и 8 МиБ для RGBA. В рабочее окно копируются только пересекающиеся части; общий массив на всю территорию не создаётся.

Рабочее окно

Ядро окна — 200×200 м, нахлёст — по 25 м с каждой стороны. Итого 250×250 м, или 5000² клеток: 25 млн байт на слой. Настройки:

resolution = 0.05
tile_cells = 512
core_cells = 4000   # 200 м
halo_cells = 500    # 25 м с каждой стороны

Номер ядра для (x,y) — (floor(x/200),floor(y/200)). В клетках 4000+2×500=5000, меньше native-предела 8192. Соседние окна имеют 50 м общего участка. Тайл 512² — единица хранения, ядро 4000² — единица работы навигации.

Территория остаётся одной картой с общими координатами стен, зон и объектов. Ограничивается рабочий растр; родное облако и блоки SLAM вместе с ним не обрезаются. Память трёхмерной локализации этим способом не ограничена.

Продолжение пути после смены окна

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

В split_road_access позу P проецируем на доступный сегмент A→B:

dx, dy = bx - ax, by - ay
t = max(0.0, min(1.0,
    ((px - ax)*dx + (py - ay)*dy) / (dx*dx + dy*dy)))
qx, qy = ax + t*dx, ay + t*dy

Нулевой сегмент пропускается. Проверяем видимость P→Q относительно стен и разбиваем дорогу в Q. Проходимость проверяется до выбора ближайшего соединения.

В рабочей интеграции территория имеет одно native-имя. Поездка остаётся одной задачей с прежними целью, yaw и действиями; планировщик запрашивает новый растр при смене ядра. Новая задача и повторная локализация для этого не создаются. Выбор grid/graph описан в предыдущей части.

Обновление карты внутри SDK

ArcPlanner обращается к типизированному ROS-сервису AtlasPlan существующего native adapter. Контракт:

geometry_msgs/PoseStamped start
geometry_msgs/PoseStamped goal
int8 method
nav_msgs/MapMetaData window
---
nav_msgs/Path plan
nav_msgs/MapMetaData window
bool negate
float64 occupied_thresh
float64 free_thresh
uint8[] raster
string error

В запросе передаётся геометрия фактической costmap. При смене ядра ответ содержит новый bounded PGM. Родной load_map::loadMapFromFile читает его со своими порогами и публикует /nav_map; существующий StaticLayer принимает растр под тем же именем карты. PGM передаётся локально на роботе, не через мобильную сеть. checkoutMap не требуется.

Ближний участок рассчитывает штатный GlobalPlanner на live costmap. Дальний использует world-reference обследованных дорог или авторских треков и продолжает путь к той же цели. Для свободного режима 0 чужой трек не подставляется.

Vendor GetPlan вызывал невиртуальную перегрузку с tolerance, обходя wrapper. Предпросмотр Fleet перевели на /speedybot/native_make_plan, чтобы он проходил через тот же ArcPlanner, что и исполнение.

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

Сохранение досъёмки

В фотослое обновляем наблюдаемые пиксели: записываем RGB и alpha 255. Прозрачные участки новой съёмки сохраняют прежнее покрытие:

pixels[local[:, 1], local[:, 0], :3] = rgb[selected]
pixels[local[:, 1], local[:, 0], 3] = 255

Для лидарного растра применяется np.minimum в байтовом формате native-карты. К фотослою эта операция не относится.

Изменённый PNG записывается во временный файл и переименовывается. Облако, atlas, мировая разметка и рабочий растр собираются в candidate-каталог и публикуются вместе. Атлас не восстанавливается из последнего PGM: рабочее окно — производное представление.

Досъёмка продолжается из локализованной точки той же карты; возврат на док для начала или окончания прохода не требуется.

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

Синтетический стенд

Территория 2×2 км с чередующимися проходами между стенами дала путь 19,8 км через сто окон. Работал настоящий код импорта, экспорта, хранения и маршрутизации; измерения и ответы SDK о продвижении задавал стенд.

Это два отдельных процесса на ПК, не потребление SLAM на бортовом RK3588. В первом опыте проверили 3962 заданных положения. Физической поездки по этой территории не было.

Проверки на роботе

После переноса Z75 и Z77 сравнили на роботе и во Fleet все 59 базовых тайлов: четыре у Z75 и 55 у Z77. Координаты, размеры и декодированные пиксели совпали.

При реальном сохранении Z77 после возобновления съёмки сохранились 69 018 прежних занятых клеток и пользовательские объекты десяти карт. Граница Z77 выросла влево на 13 клеток, 65 см. Этот сбор был неподвижным.

На установленной ветке прошли поездка к Площадке примерно на 0,85 м от старта и штатный возврат на док. Новая досъёмка владельца 18 октября также сохранилась как native-карта. Исправления передачи фотослоя и обработки поз проверили пересчётом сохранённого прохода на установленном комплекте; следующий полный живой цикл оставался отдельной проверкой.

Километровый пробег, точность SLAM на такой дистанции и физический переход границы окна эти результаты не подтверждают. Подтверждены разреженное хранение, сохранность перенесённых данных и короткая поездка установленного комплекта.

Все 10 историй SpeedyBot

1. Робот доставщик дороже автомобиля, мокрая трава и открытый JDWP

2. Робот в тумане: лошадь оставить, туман удалить

3. 25 FPS с видеокамеры по 3G. Модем, извини

4. CRISPR для ARM: чиним ДНК робота

5. Гражданин куст, уступите дорогу

6. За пивом за угол — через Магадан

7. К бабушке с пирожками — за край карты — вы здесь.

8. Парк, я тебя отрендерил

9. Джойстик отпустили. Робот не в курсе

10. Мечтают ли роботы о свободных ядрах?