Определение местоположения множества векторов во множестве точек?

Добрый день всем. Тема больше математическая, все равно.

В общем, есть некоторое пространство точек. К примеру, карта предметов на полу — на полу лежит штук 20 кубиков, известно точное расположение каждого из них.

В некоторой точке (x, y) этой комнаты находится наблюдатель (Н). Наблюдатель не знает своих координат в комнате. Наблюдатель знает свой угол поворота относительно оси север-юг.

Наблюдатель видит 5 кубиков, измеряет точное расстояние до каждого из них. Таким образом, наблюдатель получает набор векторов от себя до каждого увиденного кубика, знает длину каждого вектора.


Вопрос: Как наблюдателю определить свои координаты (X, Y), пользуясь известными данными о положении ВСЕХ кубиков комнаты, имея набор векторов? При этом наблюдатель не знает, какие кубики он видит (не знает номера кубиков, т.е. он не может точно определить, какой вектор на какой кубик указывает).


UPD1: Ниже привел картинку, иллюстрирующую ситуацию. Кубики — это известные точки, D₁-D_x — измеренные расстояния от наблюдателя (Н) до точек. Angle₁-Angle_x — углы поворота вектора относительно оси Север-Юг.

UPD2: Теоретическая задача — нахождение местоположения Н (наблюдателя) при известных D_x, Angle_x, известных координатах каждой точки.

Момент1: Наблюдатель не знает, до каких именно точек он измерил расстояние (не знает их глобальные координаты), ему известно только их расположение относительно себя (локальные координаты в координатной системе с наблюдателем в качестве центра.

Момент2: Все измерения проводятся в двумерной системе координат (в трехмерной мат.затраты будут слишком большие).


Картинка:



Практическая задача — нахождение местоположения робота в помещении. Известна карта помещения (координаты всех точек препятствий на высоте расположения датчиков робота), получены результаты измерений (лазерным датчиком линии или путем нескольких точечных измерений обычным сенсором с изменением угла поворота сенсора), надо определить свое положение. Задача осложняется тем, что точек в карте помещения (даже с учетом низкого разрешения, т.е. 1 точка на 10см, например) будет очень много, простой перебор всех вариантов займет некислое время.


Какой вариант возникал у меня:

1) Вычисление всех возможных относительных векторов от каждой точки до каждой точки карты (с учетом расстояния, т.е. вычисляем вектора только для точек, расстояния между которыми не больше метра). Назовем их глобальными векторами.

2) Вычисление всех возможных векторов между измеренными точками (мы знаем их координаты в локальной системе координат, можем вычислить относительные вектора. Назовем их измеренными векторами.

3) Перебор измеренных векторов.

3.1) Находится любой совпадающий глобальный вектор для измеренного вектора (совпадают направление и длина), фиксируется.

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

3.2.1) Если глобальный вектор найден, то он фиксируется, переходим к шагу 3.2

3.2.2) Если глобальный вектор НЕ найден, результаты сбрасываются, переходим к шагу 3.1.

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

3.4) Ну а вычисление точки расположения наблюдателя, когда известны координаты измеренных точек — это уже простая геометрия.