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

Материал из roboforum.ru Wiki
Перейти к: навигация, поиск

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

Имеем в плоскости 3 маяка с известными координатами (x1,y1), (x2,y2), (x3,y3) и замеренные расстояния до них (z1,z2,z3) известные с точностью до неизвестного одинакового отклонения (z). Требуется определить наиболее вероятные координаты робота (x,y) и соответственно одинаковое отклонение (z), зависящее от смещения часов робота относительно часов маяков.


Источник задачи - система локализации на базе (ультра)звуковых маяков, которые могут быть рассинхронизированы по часам с автономным роботом.

Решение численным методом Ньютона

Решенее предложил mandigit

Запишем уравнение для расстояния ri от i-го маяка до робота:

zi=sqrt((xi-x)^2+(yi-y)^2)+z

Продифференцировав это равенство получим:

dzi = ((xi-x)dx+(yi-y)dy)/sqrt((xi-x)^2+(yi-y)^2)+dz = ((xi-x)dx+(yi-y)dy)/(zi-z)+dz, умножив на (zi-z), получим:

(zi-z)dzi = (xi-x)dx + (yi-y)dy + (zi-z)dz

При известных dzi три таких равенства представляют собой систему линейных уравнений относительно неизвестных dx,dy,dz.

Примем d0=sqrt(dx^2+dy^2+dz^2), далее имея все эти выкладки запускаем следующий итерационный алгоритм.

  1. Инициализация x(0)=(x1+x2+x3)/3, y(0)=(y1+y2+y3)/3, z(0)=0;
  2. Шаг алгоритма №n (n>=1):
    1. Вычисляем zi' исходя из координат x(n-1), y(n-1), z(n-1);
    2. Находим их отклонение dzi=zi'-zi от реально измеренных zi;
    3. Решаем систему линейных уравнений и находим dx,dy,dz, добавляем их к x(n-1),y(n-1),z(n-1) и получаем x(n),y(n),z(n);
    4. Если d0 меньше некоторого epsilon, тогда останавливаемся и считаем найденные x(n),y(n),z(n) достаточно хорошим решением, которое невозможно уже значительно улучшить без серьезных времязатрат.

ВНИМАНИЕ! В решении есть проблема - непонятно почему для сходимости dz надо не добавлять, а отнимать от z(n-1).

ВНИМАНИЕ! Область сходимости данного метода до конца не исследована. Внутри треугольника маяков и в некоторой его окресности на практике метод сходится за 5-6 итераций с точность порядка 3-4% от диаметра треугольника маяков.

Решение аналитическим методом

Решение предложил boez.

Пусть r1, r2, r3 - расстояния до маяков, реальные.

Известные величины: r1', r2', r3' - расстояния с неким смещением, одинаковым.


расстояния:

  • r1 = r1'+d
  • r2 = r2'+d
  • r3 = r3'+d


Координаты маяков: (x1,y1),(x2,y2),(x3,y3)

Искомые величины: x,y,d - координаты и приведенное к расстоянию смещение во времени.


Уравнения окружностей (расстояния до маяков):

  • r1^2 = (x1-x)^2 + (y1-y)^2 (1)
  • r2^2 = (x2-x)^2 + (y2-y)^2 (2)
  • r3^2 = (x3-x)^2 + (y3-y)^2 (3)


Вычтем из первого уравнения второге и третье

  • (r1'-r2')(r1'+r2'+2*d) = (x1-x2)(x1+x2-2*x) + (y1-y2)(y1+y2-2*y) (4)
  • (r1'-r3')(r1'+r3'+2*d) = (x1-x3)(x1+x3-2*x) + (y1-y3)(y1+y3-2*y) (5)


А это уже линейная по x, y и d система. Объявив d известным, решаем ее и находим линейные же выражения x=x(d), y=y(d):

A1*x+B1*y=C1, где A1=2*(x1-x2), B1=2*(y1-y2), C1=(x1+x2)*(x1-x2)+(y1+y2)*(y1-y2)-(r1'-r2')*(r1'+r2'+2*d) = D1 + k1*d

A2*x+B2*y=C2, где A2=2*(x1-x3), B2=2*(y1-y3), C2=(x1+x3)*(x1-x3)+(y1+y3)*(y1-y3)-(r1'-r3')*(r1'+r3'+2*d) = D2 + k2*d


D=A1*B2-A2*B1=4*(x1-x2)*(y1-y3)-4*(x1-x3)*(y1-y2) - не ноль, т.к. маяки не на 1 прямой.


x(d)=(C1*B2-C2*B1)/(A1*B2-A2*B1) = ((D1+k1*d)*B2-(D2+k2*d)*B1)/(A1*B2-A2*B1) = P1+m1*d

y(d)=(A1*C2-A2*C1)/(A1*B2-A2*B1) = (A1*(D2+k2*d)-A2*(D1+k1*d))/(A1*B2-A2*B1) = P2+m2*d


Подставляем например в (1), получаем квадратное уравнение с одним неизвестным d:

(r1'+d)^2 = (x1-x)^2 + (y1-y)^2 = (x1-P1-m1*d)^2 + (y1-P2-m2*d)^2

d^2 + 2*r1'*d + r1'^2 = m1^2*d^2 - 2*(x1-P1)*m1*d + (x1-P1)^2 + m2^2*d^2 - 2*(y1-P2)*m2*d + (y1-P2)^2

(1 - m1^2 - m2^2)*d^2 + (2*r1'+2*(x1-P1)*m1+2*(y1-P2)*m2)*d + (r1'^2 - (x1-P1)^2 - (y1-P2)^2) = 0


a = 1 - m1^2 - m2^2

b = 2*r1'+2*(x1-P1)*m1+2*(y1-P2)*m2

c = r1'^2 - (x1-P1)^2 - (y1-P2)^2


корни d1,d2 = (-b +/- sqrt(b^2-4*a*c))/(2*a)


Квадратное уравнение имеет 2 корня. То есть, кроме реального решения, есть еще какое-то второе, тоже удовлетворяющее системе. Скорее всего это решение соответствующее отрицательным r1,r2,r3.


ВНИМАНИЕ! Размещены выкладки не проверявшиеся на практике.