Сравнение риск-ориентированных методов планирования пути четвероногого робота на вероятностной карте проходимости

—

от автора

Аннотация

Рассматривается задача планирования пути четвероногого робота в недетерминированной среде, представленной вероятностной картой проходимости. Для шагающего робота кратчайший путь не всегда безопасен: уклон поверхности, неровности рельефа и высота препятствий могут привести к потере устойчивости. В работе предложена единая постановка задачи и методика сравнения риск-ориентированных планировщиков: базового алгоритма A* по кратчайшему пути, A* с вероятностно-взвешенной стоимостью, планирования с вероятностным ограничением и планирования по мере риска CVaR. Описан способ перевода уклона, шероховатости и высоты ступени в вероятность прохождения ячейки с учётом ограничений конкретной модели робота. Предложены сценарии на синтетических картах высот с разным уровнем неопределённости и метрики качества: длина маршрута, суммарный риск, время построения пути и доля успешных прохождений в симуляции. Методика позволяет определить, при каком уровне неопределённости карты кратчайший путь перестаёт быть приемлемым, и дать рекомендации по выбору метода и параметра риска.

Ключевые слова: четвероногий робот, планирование пути, вероятностная карта проходимости, риск-ориентированное планирование, CVaR, недетерминированная среда, ограничения устойчивости.

Классификационные коды: ГРНТИ 28.23.27; ASJC 2604, 2207; OECD 2.02.

Введение

Четвероногие роботы всё чаще применяются вне лабораторий: для инспекции промышленных объектов, работы в шахтах и на стройплощадках, мониторинга территорий и поисково-спасательных операций. Главное их преимущество перед колёсными платформами — способность двигаться по пересечённой местности, ступеням и рыхлому грунту [1, 2]. Однако это преимущество реализуется только тогда, когда система планирования учитывает реальные возможности робота и состояние поверхности.

Классические планировщики пути на сетке, например A* [3], ищут маршрут минимальной длины на бинарной карте «свободно/занято». Для шагающего робота такая модель среды слишком груба. Участок с уклоном, близким к предельному, формально проходим, но вероятность падения на нём велика. Кроме того, карта высот, построенная по данным лидара или стереокамеры, содержит ошибки измерения и локализации [4, 5], поэтому свойства каждой ячейки известны лишь с некоторой вероятностью. В результате кратчайший путь может проходить по самым опасным участкам.

Естественный способ учесть неопределённость — представить среду вероятностной картой проходимости, в которой каждой ячейке сопоставлена вероятность её успешного прохождения, и использовать риск-ориентированный критерий при поиске пути [6, 7]. В литературе предложено несколько семейств таких методов: минимизация ожидаемой стоимости, ограничения на вероятность отказа (chance constraints) [8], когерентные меры риска, прежде всего условная стоимость под риском CVaR [9, 10], а также графовые представления риска [11]. Эти методы разработаны для разных платформ и проверены в разных условиях, поэтому из опубликованных результатов трудно понять, какой из них предпочтителен для четвероногого робота и при каком уровне неопределённости карты.

Цель работы — сравнить риск-ориентированные методы планирования пути четвероногого робота на вероятностной карте проходимости в единой постановке. Новый алгоритм в статье не предлагается: вклад работы методологический и экспериментальный. Он состоит в следующем:

  • единая постановка задачи и стенд, в котором методы с разной трактовкой риска реализованы на одной вероятностной карте и в одном симуляторе;

  • модель перевода уклона, шероховатости и высоты ступени в вероятность прохождения ячейки с учётом допустимого угла наклона и высоты шага конкретной модели робота;

  • методика анализа зависимости качества планирования от уровня неопределённости карты;

  • схема получения практических рекомендаций по выбору метода и параметра риска (уровня \alpha для CVaR или порога вероятности) для разных типов местности.

Статья организована следующим образом. В разделе II проведён анализ существующих работ. В разделе III формально поставлена задача. В разделе IV описаны построение вероятностной карты, сравниваемые методы, экспериментальный стенд и метрики. Раздел V содержит выводы и план дальнейших исследований.

Анализ существующих работ

Оценка проходимости и вероятностные карты

Стандартным представлением среды для шагающих роботов стала 2,5-мерная карта высот. Библиотека grid_map [12] задаёт многослойную регулярную сетку, в слоях которой хранятся высота, её дисперсия и производные признаки. В работе [4] предложено построение карты высот в системе координат робота с явным учётом неопределённости локализации: для каждой ячейки оценивается не только средняя высота, но и доверительный интервал. Именно такая карта служит входом для вероятностной оценки проходимости.

Проходимость ячейки обычно вычисляют по геометрическим признакам: уклону, шероховатости и высоте перепада [13]. Пороги этих признаков задаются из возможностей конкретного робота. Альтернативный подход — обучать оценку проходимости в симуляции [14] или на основе политики локомоции [15]. Обученные модели точнее описывают реальные возможности робота, но требуют больших объёмов данных и хуже интерпретируются. Для сравнения планировщиков в контролируемых условиях геометрическая модель предпочтительнее: её параметры явно связаны с характеристиками робота, а уровень неопределённости можно задавать непосредственно.

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

Наиболее простой способ учесть неопределённость — минимизировать ожидаемую стоимость пути. Такой критерий нейтрален к риску: редкие, но тяжёлые исходы (падение робота) усредняются и почти не влияют на решение. Поэтому в робототехнике применяют меры риска, чувствительные к «хвосту» распределения потерь. В работе [10] сформулированы аксиомы, которым должна удовлетворять мера риска для робота, и показано, что им отвечают когерентные меры, в том числе CVaR. Обзор [7] систематизирует применение мер хвостового риска в планировании, управлении и верификации.

Метод STEP [6] оценивает проходимость местности как случайную величину и строит путь с минимальным CVaR стоимости. Метод проверен на колёсных и шагающих роботах в рамках проекта DARPA SubT и показал, что учёт хвостового риска снижает число аварийных ситуаций без значительного удлинения пути. В работе [16] вместо CVaR использована энтропийная стоимость под риском (EVaR), которая даёт более консервативную оценку и допускает эффективное вычисление.

Альтернативный подход — планирование с вероятностным ограничением: путь должен быть кратчайшим при условии, что вероятность отказа не превышает заданного \delta [8]. Это интуитивно понятный параметр для пользователя, но точное решение такой задачи на графе требует поиска с ограничением на ресурс, а упрощение до порога на отдельную ячейку теряет гарантии для пути в целом.

Метод TRG-planner [11] строит разреженный граф риска прохождения: узлы размещаются в областях низкого риска, рёбра взвешиваются интегральным риском. Такой граф ускоряет повторное планирование в неструктурированной среде и был проверен на четвероногом роботе.

Планирование для шагающих роботов

Для шагающих роботов задача планирования обычно разбивается на глобальный уровень (маршрут по карте) и локальный (выбор опорных точек и походки). Система ArtPlanner [2] сочетает выборочный планировщик с обученной оценкой достижимости и успешно применялась в полевых испытаниях четвероногого робота ANYmal. В [13] проходимость оценивается по карте высот и используется планировщиком на основе RRT*. Подход R-LGP [17] учитывает достижимость при совместном планировании задач и движения для мобильного манипулятора. Локальные методы на основе обучения с подкреплением [18] хорошо реагируют на изменения среды, но не дают явной оценки риска глобального маршрута. Качество карты высот, в свою очередь, зависит от точности одометрии и замыкания цикла [19], и эта погрешность напрямую переходит в неопределённость проходимости.

Выводы из анализа

Существующие работы предлагают отдельные риск-ориентированные методы и проверяют их на собственных платформах, картах и метриках. Систематического сравнения методов с разной трактовкой риска в одинаковых условиях для четвероногого робота найдено не было. Не исследована и зависимость относительного качества методов от уровня неопределённости карты: неясно, при каком шуме имеет смысл отказываться от кратчайшего пути и какой параметр риска выбирать для разных типов местности. Эти вопросы и рассматриваются в настоящей работе.

Постановка задачи

Среда задаётся регулярной сеткой размера W \times H с шагом \Delta. Для каждой ячейки i известна оценка высоты \hat z_i и её стандартное отклонение \sigma_i, характеризующее неопределённость карты. Из карты высот вычисляется вероятность прохождения p_i \in [0, 1] — вероятность того, что робот пересечёт ячейку без потери устойчивости. Способ её вычисления описан в подразделе IV-A.

Карта рассматривается как граф G = (V, E), где вершины — ячейки, а рёбра соединяют соседние ячейки в 8-связной окрестности. Длина ребра \ell_{ij} равна \Delta или \sqrt{2}\Delta. Путь \pi = (v_0, v_1, \dots, v_n) соединяет начальную вершину v_0 = s с целевой v_n = g. Его длина

L(\pi) = \sum_{k=1}^{n} \ell_{v_{k-1} v_k}.

Будем считать события прохождения разных ячеек независимыми. Тогда вероятность успешного прохождения пути и его риск равны

P(\pi) = \prod_{k=0}^{n} p_{v_k}, \qquad R(\pi) = 1 - P(\pi).

Предположение о независимости упрощает модель: соседние ячейки на самом деле коррелированы, так как ошибка карты имеет пространственную структуру. Влияние этого упрощения оценивается в эксперименте сравнением R(\pi) с наблюдаемой долей неудачных прохождений.

Планировщик \mathcal{A} по карте, вершинам s, g и параметру риска \theta строит путь \pi = \mathcal{A}(M, s, g; \theta). Требуется для набора планировщиков \{\mathcal{A}_1, \dots, \mathcal{A}_K\} и набора карт с разным уровнем неопределённости \sigma_n оценить:

  • длину пути L(\pi);

  • суммарный риск R(\pi);

  • время построения пути T;

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

Исследовательские вопросы работы формулируются так:

  1. при каком уровне неопределённости \sigma_n кратчайший путь перестаёт обеспечивать приемлемую долю успешных прохождений;

  2. какой метод даёт лучший компромисс между длиной пути и риском на разных типах местности;

  3. как выбирать параметр риска (\alpha для CVaR, порог вероятности \delta или вес \lambda) в зависимости от типа местности и уровня неопределённости.

Описание методов

Вероятностная карта с учётом устойчивости

Для каждой ячейки по окрестности радиуса r вычисляются три признака: уклон \varphi_i — угол между нормалью к аппроксимирующей плоскости и вертикалью; шероховатость \rho_i — среднеквадратичное отклонение высот от этой плоскости; высота ступени h_i — максимальный перепад высот между соседними ячейками. Пороги признаков определяются возможностями робота: допустимым углом наклона \varphi_{\max}, высотой шага h_{\max} и допустимой шероховатостью \rho_{\max}. Для модели Unitree Go2 их значения берутся из технических характеристик и уточняются в симуляции.

Признаки вычисляются по неточной карте и потому сами являются случайными величинами. Будем считать их нормально распределёнными со средними \hat\varphi_i, \hat\rho_i, \hat h_i и стандартными отклонениями \sigma^{\varphi}_i, \sigma^{\rho}_i, \sigma^{h}_i, полученными распространением неопределённости высот \sigma_i. Тогда вероятность того, что признак не превысит порог, равна \Phi((x_{\max} - \hat x_i)/\sigma^{x}_i), где \Phi — функция стандартного нормального распределения. Вероятность прохождения ячейки:

p_i = \Phi\!(\frac{\varphi_{\max} - \hat\varphi_i}{\sigma^{\varphi}_i})        \Phi\!(\frac{h_{\max} - \hat h_i}{\sigma^{h}_i})        \Phi\!(\frac{\rho_{\max} - \hat\rho_i}{\sigma^{\rho}_i}).

При \sigma \to 0 выражение (3) переходит в бинарную карту проходимости. С ростом неопределённости вероятность ячеек вблизи порога стремится к 0{,}5, а ячейки, далёкие от порога, остаются надёжно проходимыми или непроходимыми. Ячейки, занятые препятствиями, получают p_i = 0. Карта хранится в формате grid_map [12] как отдельный слой.

Сравниваемые методы

Все методы реализуются на основе одного поиска A* [3] на графе G с евклидовой эвристикой и отличаются только стоимостью ребра или правилом отбрасывания вершин. Это исключает влияние деталей реализации на сравнение времени и качества.

1) Базовый A по кратчайшему пути.* Карта бинаризуется: ячейка считается свободной при p_i \ge 0{,}5. Стоимость ребра равна его длине \ell_{ij}. Метод игнорирует риск и служит нижней границей длины пути.

2) A с вероятностно-взвешенной стоимостью.* Логарифмируя (2), получаем -\ln P(\pi) = \sum_k (-\ln p_{v_k}). Риск становится аддитивным, и его можно включить в стоимость ребра:

c_{ij} = \ell_{ij} (1 - \lambda \frac{\ln p_j}{\Delta}),

где \lambda \ge 0 — вес риска. Нормировка на \Delta делает стоимость независимой от разрешения карты. При \lambda = 0 метод совпадает с базовым A*, при больших \lambda ищется путь с максимальной вероятностью успеха.

3) Планирование с вероятностным ограничением. Требуется найти кратчайший путь при условии R(\pi) \le \delta, то есть \sum_k (-\ln p_{v_k}) \le -\ln(1-\delta). Рассматриваются два варианта. В упрощённом варианте из графа удаляются ячейки с p_i < p_{\mathrm{th}}, после чего ищется кратчайший путь; ограничение на путь в целом при этом не гарантируется. В точном варианте используется поиск с ограничением на ресурс: в состояние A* добавляется накопленный риск, а пути, нарушающие ограничение, отбрасываются. Сравнение вариантов показывает, сколько теряется при простом пороге на ячейку.

4) Планирование по мере риска CVaR. Следуя [6], риск ячейки описывается случайной величиной потерь \xi_i, связанной с запасом устойчивости. Для нормального распределения с параметрами \mu_i, s_i условная стоимость под риском на уровне \alpha вычисляется в замкнутой форме [9]:

\mathrm{CVaR}_\alpha(\xi_i) = \mu_i + s_i \frac{\phi(\Phi^{-1}(\alpha))}{1 - \alpha},

где \phi — плотность стандартного нормального распределения. Стоимость ребра

c_{ij} = \ell_{ij} (1 + \lambda  \mathrm{CVaR}_\alpha(\xi_j)).

Параметр \alpha \in [0, 1) задаёт отношение к риску: при \alpha = 0 получается ожидаемая стоимость, при \alpha \to 1 — оценка по худшему случаю. Поскольку CVaR — когерентная мера риска и обладает субаддитивностью, сумма \mathrm{CVaR} по ячейкам является верхней оценкой CVaR суммарных потерь пути, то есть критерий (6) консервативен.

Основные свойства методов сведены в таблице ниже.

Метод

Параметр

Трактовка риска

A* (кратчайший)

—

не учитывается

A* с весом риска

\lambda

вероятность отказа пути

Порог на ячейку

p_{\mathrm{th}}

отказ отдельной ячейки

Ограничение на путь

\delta

отказ пути \le \delta

CVaR

\alpha, \lambda

хвост распределения потерь

Экспериментальный стенд

Схема стенда приведена ниже. Сравнение проводится на синтетических картах высот размером от 100 \times 100 до 200 \times 200 ячеек. Рельеф задаётся шумом Перлина [20], к которому добавляются склоны, ступени и препятствия. Используются четыре типа местности: ровная с препятствиями, холмистая, ступенчатая и смешанная. Для каждой карты истинного рельефа строятся наблюдаемые карты с аддитивным шумом высоты уровня \sigma_n, например \sigma_n \in \{0{,}01;\ 0{,}03;\ 0{,}05;\ 0{,}10\} м.

Схема экспериментального стенда: генерация рельефа (шум Перлина, склоны, ступени, препятствия) → наблюдаемая карта высот с шумом \sigma_n → вероятностная карта проходимости → планировщики A*, взвешивание риска, вероятностное ограничение и CVaR → симуляция Unitree Go2 в ROS 2 / Gazebo или Isaac Sim → метрики L, R, T, S.

Симуляция выполняется в ROS 2 с Gazebo или Isaac Sim на модели четвероногого робота Unitree Go2. Робот проходит построенный путь с помощью штатного локального контроллера по истинному рельефу. Прохождение считается неудачным при падении робота, застревании дольше заданного времени или отклонении от пути больше допустимого. Для каждой пары «карта — метод» выполняется серия запусков со случайными начальной и целевой точками.

Для проверки на реальном рельефе используется открытый набор данных RELLIS-3D [21]. Он записан на колёсной платформе, но содержит облака точек реальной пересечённой местности. По ним строятся карты высот, к которым применяются те же планировщики; оценка ограничивается метриками L, R и T, так как прохождение моделируется только в симуляции.

Метрики и обработка результатов

Для каждого запуска фиксируются длина пути L по (1), риск R по (2), вычисленный по истинной карте, время построения T и признак успешного прохождения. Доля успешных прохождений S оценивается по серии запусков с доверительным интервалом Уилсона. Чтобы сравнивать методы на картах разного размера, длина нормируется на длину кратчайшего пути на истинной карте.

Компромисс между длиной и риском анализируется по фронту Парето в координатах (L, R), который строится перебором параметра каждого метода (\lambda, p_{\mathrm{th}}, \delta, \alpha). Зависимость S(\sigma_n) для базового A* позволяет найти уровень неопределённости, начиная с которого кратчайший путь становится неприемлемым, например при S < 0{,}9. Рекомендации по выбору метода и параметра формируются отдельно для каждого типа местности как точка фронта Парето с минимальной длиной при заданной требуемой доле успешных прохождений.

Заключение

В работе поставлена задача сравнения риск-ориентированных методов планирования пути четвероногого робота на вероятностной карте проходимости. Предложена модель, переводящая уклон, шероховатость и высоту ступени с учётом неопределённости карты в вероятность прохождения ячейки по ограничениям конкретного робота. Четыре семейства методов — кратчайший путь, взвешивание по вероятности отказа, вероятностное ограничение и CVaR — сведены к единой реализации на основе A*, что делает сравнение корректным. Описаны стенд на синтетических картах с управляемым уровнем шума, симуляция на модели Unitree Go2 и метрики качества.

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

Литература

  1. Miki T., Lee J., Hwangbo J., Wellhausen L., Koltun V., Hutter M. Learning robust perceptive locomotion for quadrupedal robots in the wild. Science Robotics, vol. 7, no. 62, eabk2822, 2022. DOI: 10.1126/scirobotics.abk2822.

  2. Wellhausen L., Hutter M. ArtPlanner: Robust legged robot navigation in the field. Field Robotics, vol. 3, pp. 413–434, 2023.

  3. Hart P. E., Nilsson N. J., Raphael B. A formal basis for the heuristic determination of minimum cost paths. IEEE Transactions on Systems Science and Cybernetics, vol. 4, no. 2, pp. 100–107, 1968.

  4. Fankhauser P., Bloesch M., Hutter M. Probabilistic terrain mapping for mobile robots with uncertain localization. IEEE Robotics and Automation Letters, vol. 3, no. 4, pp. 3019–3026, 2018.

  5. Thrun S., Burgard W., Fox D. Probabilistic Robotics. MIT Press, Cambridge, 2005.

  6. Fan D. D., Otsu K., Kubo Y., Dixit A., Burdick J., Agha-Mohammadi A. STEP: Stochastic traversability evaluation and planning for risk-aware off-road navigation. Robotics: Science and Systems (RSS), 2021. arXiv:2103.02828.

  7. Akella P., Dixit A., Ahmadi M., Lindemann L., Chapman M. P., Pappas G. J., Ames A. D., Burdick J. W. Risk-aware robotics: Tail risk measures in planning, control, and verification. arXiv:2403.18972, 2024.

  8. Blackmore L., Ono M., Williams B. C. Chance-constrained optimal path planning with obstacles. IEEE Transactions on Robotics, vol. 27, no. 6, pp. 1080–1094, 2011.

  9. Rockafellar R. T., Uryasev S. Optimization of conditional value-at-risk. Journal of Risk, vol. 2, no. 3, pp. 21–41, 2000.

  10. Majumdar A., Pavone M. How should a robot assess risk? Towards an axiomatic theory of risk in robotics. In: Robotics Research, Springer Proceedings in Advanced Robotics, vol. 10, pp. 75–84, 2020.

  11. Lee D., Nahrendra I. M. A., Oh M., Yu B., Myung H. TRG-planner: Traversal risk graph-based path planning in unstructured environments for safe and efficient navigation. IEEE Robotics and Automation Letters, 2025. arXiv:2501.01806.

  12. Fankhauser P., Hutter M. A universal grid map library: Implementation and use case for rough terrain navigation. In: Robot Operating System (ROS): The Complete Reference (Volume 1), Springer, pp. 99–120, 2016.

  13. Wermelinger M., Fankhauser P., Diethelm R., Krüsi P., Siegwart R., Hutter M. Navigation planning for legged robots in challenging terrain. 2016 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), pp. 1184–1189, 2016.

  14. Chavez-Garcia R. O., Guzzi J., Gambardella L. M., Giusti A. Learning ground traversability from simulations. IEEE Robotics and Automation Letters, vol. 3, no. 3, pp. 1695–1702, 2018.

  15. Frey J., Hoeller D., Khattak S., Hutter M. Locomotion policy guided traversability learning using volumetric representations of complex environments. 2022 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), 2022.

  16. Dixit A., Ahmadi M., Burdick J. W. Risk-sensitive motion planning using entropic value-at-risk. 2021 European Control Conference (ECC), 2021.

  17. Ly K. T., Semenov V., Risiglione M., Merkt W., Havoutis I. R-LGP: A reachability-guided logic-geometric programming framework for optimal task and motion planning on mobile manipulators. 2024 IEEE International Conference on Robotics and Automation (ICRA), Yokohama, pp. 14917–14923, 2024. DOI: 10.1109/ICRA57147.2024.10611389.

  18. Киселев С. Д., Алтунян А. В. Локальные методы планирования траекторий на основе обучения с подкреплением. Искусственный интеллект в промышленных, коммерческих, медицинских и финансовых приложениях, 2025: сб. ст. науч.-техн. семинара. Вып. 3 / под ред. А. Р. Ефимова. М.: НИТУ «МИСИС», 2025. С. 84–89.

  19. Епифанов К. А. Исследование алгоритмов замыкания цикла в лидарной одометрии. Искусственный интеллект в промышленных, коммерческих, медицинских и финансовых приложениях, 2025: сб. ст. науч.-техн. семинара. Вып. 3 / под ред. А. Р. Ефимова. М.: НИТУ «МИСИС», 2025. С. 57–61.

  20. Perlin K. Improving noise. ACM Transactions on Graphics, vol. 21, no. 3, pp. 681–682, 2002.

  21. Jiang P., Osteen P., Wigness M., Saripalli S. RELLIS-3D dataset: Data, benchmarks and analysis. 2021 IEEE International Conference on Robotics and Automation (ICRA), pp. 1110–1116, 2021.

ссылка на оригинал статьи https://habr.com/ru/articles/1110108/