Цель в десяти сантиметрах, а робот предлагает двадцатиметровый объезд. Как за пивом за угол — через Магадан. На синтетической карте штатный ARM-планировщик построил свободный путь 12,75 см и путь через граф 20,10 м.
Я Лев Ларин, инженер Unibot. Мы подключаем Alfa Robotics SpeedyBot к своей платформе UNIBOT Fleet, сохраняя штатную навигацию робота-доставщика. Это шестая часть серии: выбор маршрута по полной стоимости.
Начало серии · ← Часть 5: Гражданин куст, уступите дорогу · Часть 7: К бабушке с пирожками — за край карты →
Два варианта поиска
На RK3588/AArch64 работают ROS1 и move_base. В штатном GlobalPlanner есть сеточный поиск Expander/Traceback и отдельный GraphSearch. Сетка costmap хранит свободные, дорогие и непроходимые клетки; граф задаёт дорожки, их узлы и направления.
Fleet: цель + профиль покрытия ↓ TerrainLayer + native costmap ↓ GlobalPlanner: [grid start→goal] или [grid→graph→grid] ↓ один план → move_base → штатное исполнение → шасси
В исследуемом режиме планировщик выбирал достижимый граф без сравнения с полным свободным путём. В контрпримере дорожка лежала в десяти метрах сбоку: короткий участок по графу требовал почти десятиметрового подъезда и такого же выезда.
Добавили построение двух законченных кандидатов родными алгоритмами: сеточный start→goal и grid→graph→grid. Второму начисляется вся цена:
цена подъезда + цена участка дорожки + цена выезда к цели
Одинаковая оценка обоих путей
Стоимость каждого сегмента:
J = Σ (neutral_cost + cell_cost[i]) × length_inside_cell[i] / resolution
neutral_cost — базовая цена расстояния из native Expander, length_inside_cell — длина внутри клетки. В коде координаты уже переведены в клетки, поэтому деление на resolution учтено в длине. Запрещённая клетка и выход за карту дают бесконечную цену.
Пример при resolution=5 см, neutral=50, cost асфальта 0 и газона 200: метр асфальта стоит 20×50=1000, газона — 20×250=5000. Четыре метра асфальта дешевле метра газона. Но 10 см газона стоят 500, а 20 м асфальта — 20 000. Ниже для реальной Z75 используется другая, фактическая цена travel_cost=240.
Суммировать цены waypoint нельзя: результат зависел бы от числа точек. В cost.c DDA проходит границы клеток и учитывает длину внутри каждой. Ядро интегрирования:
double next = next_x < next_y ? next_x : next_y; if (next > 1.) next = 1.; total += (next - previous) * length * cell_price; if (next == 1.) return total; previous = next;
next — доля отрезка до следующей границы, length — длина в клетках, cell_price=neutral+cost. Для hard obstacle возвращается infinity. На границе и в углу проверяем затронутые соседние клетки. native_route_cost включает также соединения реального старта с первым waypoint и последнего waypoint с целью.
Выбор кандидата
Оба плана строятся под штатными planner/costmap mutex и оцениваются на согласованной карте. Сокращённое правило из plan.c:
if (direct_cost < INFINITY && !(graph_cost <= direct_cost)) { select_native_grid(); } else if (graph_cost < INFINITY) { select_native_graph(); } else { return_no_plan(); }
select_* передаёт готовый native-вектор точек. При равной цене выбирается графовый вариант с ненулевым продвижением по графу; касание линии не считается. Мягкая стоимость участвует и в GraphSearch, с нормировкой через 1/neutral_cost.
Алгоритм выбирает лучший из двух готовых кандидатов с существующими точками подключения. Глобальный минимум среди всех входов и выходов на граф не ищется.
В исправленном контрпримере выбран сеточный путь 12,91 см вместо 20,10 м через граф. Это длины возвращённых цепочек; расстояние между заданными положениями — 10 см. Исполнение настоящего ARM-кода на ПК разобрано в предыдущей части.
Покрытие и препятствия
Высокая мягкая цена позволяет предпочесть дорожку, сохраняя возможность доехать до цели на газоне. Стены и непроходимые клетки остаются запретными.
В одной искусственной модели дорогим сделали только прямоугольник, и планировщик обошёл его по неразмеченной дешёвой поверхности. Когда стоимость нанесли на все альтернативы, предпочтительная дорожка стала общим участком путей. Геометрия графа не заменяет разметку покрытия.
Обнаружили также соединения со свободными концами, пересекавшие занятый угол клетки. Для них проверяем обход через свободный прямоугольный угол; обе стороны должны быть проходимы. На контрольной карте со стеной получился обход 2,461 м. Полностью перекрытая карта по-прежнему не даёт пути.
Исполнение и предпросмотр
Направления рёбер сохранены. Подъезд и выезд используют штатный объезд препятствий, выбранный строгий участок графа — строгое следование. Всё исполняется одной задачей.
Для move_type=1 точки с входящим графовым сегментом помечаем PoseStamped.header.seq=0x53425431, физический z остаётся нулём. Native dispatch читает метку, а горизонт текущего плана обрезается на границе политики. Повторяющиеся точки стыка не удерживают старый режим.
В первой версии предпросмотра режим запроса передавался через ROS GetPlan:
request.start.header.seq = 0x53425000 | move_type;
Это внутренний контракт нашего комплекта. Общий planner_method не переключается; режимы 0/2 сохраняют native obstacle avoidance. При подключении большого атласа предпросмотр перенесли в ArcPlanner, исполняющий поездку: следующая часть.
Построения на реальной карте
На ПК взяли снимок Z75 и полигон газона из Fleet. Размер costmap — 1176×1068 клеток, resolution около 0,05 м, origin (−23,65; −26,0) м. Перевод координат: mx=floor((x-origin_x)/resolution), аналогично для Y. Поверх сохранённых байтов нанесли travel_cost=240, сохранив hard obstacles и inflation.
Начало → цель |
Длина построенного пути |
|---|---|
Док → Площадка |
2,00 м |
Рядом → Площадка |
0,62 м |
Пруд → Площадка |
12,82 м |
Пруд → Скамейка |
15,24 м |
Пруд → координаты дока |
14,78 м |
Все пять путей выбрали сеточный вариант. Три маршрута от Пруда имели общее начало около 9,61 м рядом с линией графа: цена клеток там оказалась ниже точного прохождения графа.
Это планы на ПК, не следы выезда. На отдельной искусственной модели получились два сеточных и три графовых пути: граф участвует, когда выгоден по полной цене.
Проверка установленного комплекта
После локального этапа 11 октября установили обе библиотеки и проверили их загрузку работающим процессом. На неподвижном роботе выполнили девять построений к трём целям в разных режимах.
Первая поездка к Скамейке закончилась ошибкой привода. После физической проверки машины человеком испытания продолжили. Затем успешно прошли пять поездок: Площадка → Док → Пруд → Площадка → Док, задача по сценарию и возврат по расписанию.
Причину прежнего отказа привода эти результаты не устанавливают. Полная физическая проверка строгого следования и объезда на выбранном графовом участке в этих материалах осталась незавершённой.
Все 10 историй SpeedyBot
Робот доставщик дороже автомобиля, мокрая трава и открытый JDWP
За пивом за угол — через Магадан — вы здесь.