Определение координат робота по локальным координатам маяков

Недостатки метода триангуляции и аналогичных "точных" методов

Схема метода риангуляции

Классические подходы к определению глобальных координат обычно опираются на метод триангуляции или что-то в этом роде.

Метод триангуляции позволяет определить положение робота, решив треугольник (строго говоря, не один). А именно, зная расстояние между маяками-реперами (на рисунке обозначено как base), а так же имея измеренные расстояния до реперов (dist_1 и dist_2), мы можем определить положение робота. Как решать треугольники см. соответствующую статью в разделе "Планиметрия" обучающего сайта.

Одного треугольника не достаточно для точного определения положения робота, т.к. зная измеренные расстояния dist_1 и dist_2 и базу маяков base мы получаем две точки равновероятного положения робота. Вторую точку на рисунке можно найти, отразив треугольник относительно линии base.

На практике часто вместо достаточной для триангуляции и точной информации, мы имеем избыточную, но неточную, а поэтому противоречивую информацию. Например, мы знаем что расстояние между маяками 1 и 2 составляет 10м, но наши сенсоры выдали нам ответ, что расстояние до маяка 1 составляет 4 метра, а до маяка 2 расстояние 5 метров. В этом случае очевидно что мы имеем погрешность измерения расстояния до маяков минимум 0.5м. Если мы можем с хорошей точностью оценивать погрешность каждого известного нам параметра, то это в значительной степени может нам из массы неточной информации получить более-менее точные глобальные координаты, как это делается - читайте ниже.

Поиск глобальных координат робота через минимизацию функции "неправдоподобия"

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

Попробуем теперь понять, информация какого сорта может у нас оказаться:

• С погрешностью измеряемой в единицах угла:
  • Ориентация робота глобальная (компас, звезды); (измеренный угол - GAi, погрешность GAPi единиц угла);
  • Ориентация робота локальная (известные объекты на карте); (измеренный угол - Ai, погрешность APi единиц угла);

• С погрешностью измеряемой в единицах длины:
  • Расстояние от робота до известных объектов; (измеренное расстояние - Di, погрешность - DPi единиц длины)
  • Локальные координаты маяков относительно робота; (измеренные координаты - Xi,Yi, погрешность - ZPi единиц длины)
  • Глобальные координаты робота (GPS / энкодеры / другие готовые системы); (измеренные координаты - GXi,GYi, погрешность GZPi единиц длины)

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

Далее мы введем функцию "неправдопободия глобальных координат и направления робота" F(x,y,a) и вся работа будет сведена к минимизации этой функции. Наиболее простой и общепринятый подход к определиню такой функции - принять за неё сумму взвешенных квадратов отклонений (Wi*Di)^2 ожидаемого значения параметров при расположении робота (x,y,a) от наблюдаемого. Вес Wi каждого отклонения - 1/(погрешность соответствующего измерения параметра) для отклонений в единицах длины и K0/(погрешность соответствующего измерения параметра) для отклонений в единицах угла. При этом каждое Di есть функция 3 параметров - x,y,a - координат и направления робота.

То есть мы просто ищем наиболее правдоподобные координаты и направление робота, учитывая имеющуюся у нас информацию и сведения о точности этой информации.

Типовые компоненты Di

Здесь будут рассмотрены обычные компоненты Di:

• Если мы замерили расстояние Dist до объекта, чьи координаты известны (XOi,YOi), тогда i-я компонента функции "неправдоподобия" координат робота (X,Y,A) соответствующая этой информации будет считаться как Di=(X-XOi)^2+(Y-YOi)^2-Dist^2.

• Если мы замерили локальные координаты (XLOCi,YLOCi) относительно робота объекта, чьи глобальные координаты известны как (XOi,YOi), тогда i-я компонента функции "неправдоподобия" координат робота (X,Y,A) соответствующая этой информации будет считаться как Di=(X+XLOCi*cos(A)-YLOCi*sin(A)-XOi)^2+(Y+YLOCi*cos(A)+XLOCi*sin(A)-YOi)^2. (вроде не ошибся(?))

Поиск минимума полученной функции

Здесь мы напишем как можно искать минимум полученной функции.

Пример использования метода

Здесь будет пример использования.