Я пытаюсь реализовать планирование «пространства убеждений» для робота, у которого камера является основным датчиком. Подобно SLAM, у робота есть карта 3D-точек, и он локализуется, выполняя 2D-3D-сопоставление с окружающей средой на каждом этапе. Для целей этого вопроса я предполагаю, что карта не меняется.

Как часть планирования пространства веры, я хочу спланировать пути для робота, которые берут его от начала до цели, но таким образом, чтобы точность его локализации всегда была максимальной. Следовательно, мне нужно было бы отобрать возможные состояния робота, фактически не перемещаясь туда, и наблюдения, которые робот сделал бы, если бы он был в тех состояниях, которые вместе (исправьте меня, если я ошибаюсь) формируют «веру» робота впоследствии кодируя его неопределенность локализации в этих точках. И тогда мой планировщик попытался бы соединить узлы, которые дают мне наименьшую неопределенность (ковариацию).

Так как моя неопределенность локализации для этого робота на основе камеры полностью зависит от таких вещей, как количество характерных точек, видимых из заданных местоположений, угол наклона робота и т. Д .: Мне нужна оценка того, насколько «плоха» моя локализация в определенной выборке. было бы, чтобы определить, должен ли я отказаться от него. Чтобы попасть туда, как мне определить модель измерения для этого, будет ли это модель измерения камеры или это будет что-то, связанное с положением робота? Как мне «угадать» мои измерения заранее и как вычислить ковариацию робота по этим предполагаемым измерениям?

РЕДАКТИРОВАТЬ: Основная ссылка для меня это идея быстрого изучения деревьев случайных убеждений , которая является расширением метода карт верования веры . Другой соответствующий документ использует RRBT для ограниченного планирования. В этой статье выборки состояний аналогичны обычным RRT, представленным как вершины в виде графа, но когда вершины должны быть соединены, алгоритм распространяет веру из текущей вершины в новую (функция PROPAGATE в разделе V из 1 ) и вот где я застрял: я не до конца понимаю, как я могу распространять убеждение по краю, фактически не пересекая его и не получая новые измерения, таким образом, новые ковариации из локализации. RRBT бумага говорит «уравнения прогнозирования ковариации и ожидания затрат реализованы в функции PROPAGATE»: но если используется только прогноз, как он узнает, скажем, достаточно ли объектов в будущем, которые могли бы повысить / снизить точность локализации?

Используйте локализацию только для подшипников для моделирования информативности камеры и моделирования измерений с нулевым шумом (например, без инноваций).

По ряду причин это фактически теоретически обоснованный способ оценки информативности пути.

Существует множество «безразмерных» метрик информативности , таких как информационная матрица Фишера . Все, что вам нужно, - это положение робота и местоположение ориентиров на карте, чтобы определить, сколько информации о положении робота будет получено путем измерения местоположений ориентира. (Или наоборот, инновация от измерений применяется как к цели, так и к роботу (это правильно SLAM?), Поэтому одинаковые метрики работают для обоих).

Я бы начал с датчика подшипника, поскольку это хорошая, хорошо принятая модель датчика зрения. Выясните «шум» на измерениях подшипников, допустив несколько пикселей ошибки при определении местоположения объектов в мире. Пусть состоянием системы будет положение робота плюс его неопределенность, а затем примерные пути (как вы предлагаете). Из каждой позиции в выбранном пути я бы пересчитал предсказанную неопределенность, используя FIM. Это не сложно сделать, просто предположим, что нет никаких ошибок в измерениях (т. Е. Не будет «новшеств» в убеждении робота, но вы все равно будете испытывать падение неопределенности, представленное уменьшающейся ковариацией в оценке положения робота. не обновлять местоположение или неопределенность ориентиров, просто чтобы упростить проблему.

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

Подведение итогов

1. $x$$\Sigma$
2. $\Sigma_i$
3. Пусть «стоимость» траектории будет некоторой выпуклой комбинацией прогресса в достижении цели и обратной ковариации (например, информационной матрицы )

Некоторые тонкости

Используйте наименьший вектор состояния, который имеет смысл. Если вы можете предположить, что робот может указывать камеру независимо от движения или имеет несколько камер, тогда игнорируйте ориентацию и просто отслеживайте положение. Я буду действовать только в 2D-позициях.

Вам придется вывести линеаризованную систему, но вы можете позаимствовать ее из приведенного выше тезиса. убедитесь, что вы не беспокоитесь о симулирующих измерениях (например, если вы выполняете только обновления EKF с помощью «симулированных измерений»), тогда предположите, что измерения соответствуют действительности и не имеют шума.

Если мы применяем идентичность матрицы Вудбери

$n$

$R$$\sum_{i=1}^n H_i^TR^{-1}H_i$

$H$$H$$nx2$$n$$n$$2$$R$$n\times n$$\sigma I_{n\times n}$$\sigma$

Что такое уравнение измерения? Это

$t$$r$

Разматывать рекурсию. Я бы поступил следующим образом:

1. Напишите алгоритм поиска пути, который находит свой путь без учета неопределенности.
2. $H$
3. $trace(H^TRH)$
4. Обратите внимание, что результат соответствует FIM траектории (упражнение предоставлено читателю), и вы правильно и теоретически правильно определили наиболее информативную траекторию.