Определение координат робота по расстояниям до маяков, измеренным с одинаковым отклонением — различия между версиями
=DeaD= (обсуждение | вклад) (→Решение аналитическим методом) |
Delphi (обсуждение | вклад) (→Решение аналитическим методом) |
||
(не показано 6 промежуточных версий 3 участников) | |||
Строка 1: | Строка 1: | ||
== Постановка задачи == | == Постановка задачи == | ||
− | Имеем в плоскости 3 маяка с известными координатами (x1,y1), (x2,y2), (x3,y3) и замеренные расстояния до них (z1,z2,z3) известные с точностью до неизвестного одинакового отклонения (z). | + | Имеем в плоскости 3 маяка с известными координатами <math>(x1,y1), (x2,y2), (x3,y3)</math> и замеренные расстояния до них <math>(z1,z2,z3)</math> известные с точностью до неизвестного одинакового отклонения <math>(z)</math>. |
− | Требуется определить наиболее вероятные координаты робота (x,y) и соответственно одинаковое отклонение (z), зависящее от смещения часов робота относительно часов маяков. | + | Требуется определить наиболее вероятные координаты робота <math>(x,y)</math> и соответственно одинаковое отклонение <math>(z)</math>, зависящее от смещения часов робота относительно часов маяков. |
Строка 9: | Строка 9: | ||
Решенее предложил mandigit | Решенее предложил mandigit | ||
− | Запишем уравнение для расстояния ri от i-го маяка до робота: | + | Запишем уравнение для расстояния <math>~ri</math> от i-го маяка до робота: |
− | zi=sqrt | + | <math>zi=\sqrt{(xi-x)^2+(yi-y)^2}+z</math> |
Продифференцировав это равенство получим: | Продифференцировав это равенство получим: | ||
− | dzi = | + | <math>dzi = \frac{(xi-x)dx+(yi-y)dy}{\sqrt{(xi-x)^2+(yi-y)^2}}+dz = \frac{(xi-x)dx+(yi-y)dy}{(zi-z)}+dz</math>, умножив на <math>~(zi-z)</math>, получим: |
− | (zi-z)dzi = (xi-x)dx + (yi-y)dy + (zi-z)dz | + | <math>~(zi-z)dzi = (xi-x)dx + (yi-y)dy + (zi-z)dz</math> |
− | При известных dzi три таких равенства представляют собой систему линейных уравнений относительно неизвестных dx,dy,dz. | + | При известных <math>~dzi</math> три таких равенства представляют собой систему линейных уравнений относительно неизвестных <math>~dx,dy,dz</math>. |
− | Примем d0=sqrt | + | Примем <math>d0=\sqrt{dx^2+dy^2+dz^2}</math>, далее имея все эти выкладки запускаем следующий итерационный алгоритм. |
− | # Инициализация x(0)= | + | # Инициализация <math>x(0)=\frac{x1+x2+x3}{3}</math>, <math>y(0)=\frac{y1+y2+y3}{3}</math>, <math>~z(0)=0</math>; |
# Шаг алгоритма №n (n>=1): | # Шаг алгоритма №n (n>=1): | ||
− | ## Вычисляем zi' исходя из координат x(n-1), y(n-1), z(n-1); | + | ## Вычисляем <math>~zi'</math> исходя из координат <math>~x(n-1), y(n-1), z(n-1)</math>; |
− | ## Находим их отклонение dzi=zi'-zi от реально измеренных zi; | + | ## Находим их отклонение <math>~dzi=zi'-zi</math> от реально измеренных <math>~zi</math>; |
− | ## Решаем систему линейных уравнений и находим dx,dy,dz, добавляем их к x(n-1),y(n-1),z(n-1) и получаем x(n),y(n),z(n); | + | ## Решаем систему линейных уравнений и находим <math>~dx,dy,dz</math>, добавляем их к <math>~x(n-1),y(n-1),z(n-1)</math> и получаем <math>~x(n),y(n),z(n)</math>; |
− | ## Если d0 меньше некоторого epsilon, тогда останавливаемся и считаем найденные x(n),y(n),z(n) достаточно хорошим решением, которое невозможно уже значительно улучшить без серьезных времязатрат. | + | ## Если <math>~d0</math> меньше некоторого <math>~epsilon</math>, тогда останавливаемся и считаем найденные <math>~x(n),y(n),z(n)</math> достаточно хорошим решением, которое невозможно уже значительно улучшить без серьезных времязатрат. |
'''ВНИМАНИЕ!''' В решении есть проблема - непонятно почему для сходимости dz надо не добавлять, а отнимать от z(n-1). | '''ВНИМАНИЕ!''' В решении есть проблема - непонятно почему для сходимости dz надо не добавлять, а отнимать от z(n-1). | ||
+ | |||
+ | '''ВНИМАНИЕ!''' Область сходимости данного метода до конца не исследована. Внутри треугольника маяков и в некоторой его окресности на практике метод сходится за 5-6 итераций с точность порядка 3-4% от диаметра треугольника маяков. | ||
== Решение аналитическим методом == | == Решение аналитическим методом == | ||
Решение предложил boez. | Решение предложил boez. | ||
− | Пусть r1, r2, r3 - расстояния до маяков, реальные. | + | Пусть <math>~r1, r2, r3</math> - расстояния до маяков, реальные. |
+ | |||
+ | Известные величины: <math>~r1', r2', r3'</math> - расстояния с неким смещением, одинаковым. | ||
− | |||
расстояния: | расстояния: | ||
− | r1 = r1'+d | + | * <math>~r1 = r1'+d</math> |
− | r2 = r2'+d | + | * <math>~r2 = r2'+d</math> |
− | r3 = r3'+d | + | * <math>~r3 = r3'+d</math> |
+ | |||
− | Координаты маяков: (x1,y1),(x2,y2),(x3,y3) | + | Координаты маяков: <math>~(x1,y1),(x2,y2),(x3,y3)</math> |
+ | |||
+ | Искомые величины: <math>~x,y,d</math> - координаты и приведенное к расстоянию смещение во времени. | ||
− | |||
Уравнения окружностей (расстояния до маяков): | Уравнения окружностей (расстояния до маяков): | ||
+ | * <math>~r1^2 = (x1-x)^2 + (y1-y)^2</math> (1) | ||
+ | * <math>~r2^2 = (x2-x)^2 + (y2-y)^2</math> (2) | ||
+ | * <math>~r3^2 = (x3-x)^2 + (y3-y)^2</math> (3) | ||
− | |||
− | |||
− | |||
Вычтем из первого уравнения второге и третье | Вычтем из первого уравнения второге и третье | ||
+ | * <math>~(r1'-r2')(r1'+r2'+2 \cdot d) = (x1-x2)(x1+x2-2 \cdot x) + (y1-y2)(y1+y2-2 \cdot y)</math> (4) | ||
+ | * <math>~(r1'-r3')(r1'+r3'+2 \cdot d) = (x1-x3)(x1+x3-2 \cdot x) + (y1-y3)(y1+y3-2 \cdot y)</math> (5) | ||
− | |||
− | |||
− | А это уже линейная по x, y и d система. Объявив d известным, решаем ее и находим линейные же выражения x=x(d), y=y(d) | + | А это уже линейная по x, y и d система. Объявив d известным, решаем ее и находим линейные же выражения <math>~x=x(d), y=y(d)</math>: |
− | A1 | + | <math>~A1 \cdot x+B1 \cdot y=C1</math>, где <math>~A1=2 \cdot (x1-x2)</math>, <math>~B1=2 \cdot (y1-y2)</math>, <math>~C1=(x1+x2) \cdot (x1-x2)+(y1+y2) \cdot (y1-y2)-(r1'-r2') \cdot (r1'+r2'+2 \cdot d) = D1 + k1 \cdot d</math> |
− | |||
− | + | <math>~A2 \cdot x+B2 \cdot y=C2</math>, где <math>~A2=2 \cdot (x1-x3)</math>, <math>~B2=2 \cdot (y1-y3)</math>, <math>~C2=(x1+x3) \cdot (x1-x3)+(y1+y3) \cdot (y1-y3)-(r1'-r3') \cdot (r1'+r3'+2 \cdot d) = D2 + k2 \cdot d</math> | |
− | x(d)= | + | |
− | y(d)= | + | <math>~D=A1 \cdot B2-A2 \cdot B1=4 \cdot (x1-x2) \cdot (y1-y3)-4 \cdot (x1-x3) \cdot (y1-y2)</math> - не ноль, т.к. маяки не на 1 прямой. |
+ | |||
+ | |||
+ | <math>~x(d)=\frac{C1 \cdot B2-C2 \cdot B1}{A1 \cdot B2-A2 \cdot B1} = \frac{(D1+k1 \cdot d) \cdot B2-(D2+k2 \cdot d) \cdot B1}{A1 \cdot B2-A2 \cdot B1} = P1+m1 \cdot d</math> | ||
+ | |||
+ | <math>~y(d)=\frac{A1 \cdot C2-A2 \cdot C1}{A1 \cdot B2-A2 \cdot B1} = \frac{A1 \cdot (D2+k2 \cdot d)-A2 \cdot (D1+k1 \cdot d)}{A1 \cdot B2-A2 \cdot B1} = P2+m2 \cdot d</math> | ||
Подставляем например в (1), получаем квадратное уравнение с одним неизвестным d: | Подставляем например в (1), получаем квадратное уравнение с одним неизвестным d: | ||
− | (r1'+d)^2 = (x1-x)^2 + (y1-y)^2 = (x1-P1-m1 | + | <math>~(r1'+d)^2 = (x1-x)^2 + (y1-y)^2 = (x1-P1-m1 \cdot d)^2 + (y1-P2-m2 \cdot d)^2</math> |
+ | |||
+ | <math>~d^2 + 2 \cdot r1' \cdot d + r1'^2 = m1^2 \cdot d^2 - 2 \cdot (x1-P1) \cdot m1 \cdot d + (x1-P1)^2 + m2^2 \cdot d^2 - 2 \cdot (y1-P2) \cdot m2 \cdot d + (y1-P2)^2</math> | ||
+ | |||
+ | <math>~(1 - m1^2 - m2^2) \cdot d^2 + (2 \cdot r1'+2 \cdot (x1-P1) \cdot m1+2 \cdot (y1-P2) \cdot m2) \cdot d + (r1'^2 - (x1-P1)^2 - (y1-P2)^2) = 0</math> | ||
+ | |||
+ | |||
+ | <math>~a = 1 - m1^2 - m2^2</math> | ||
+ | |||
+ | <math>~b = 2 \cdot r1'+2 \cdot (x1-P1) \cdot m1+2 \cdot (y1-P2) \cdot m2</math> | ||
− | + | <math>~c = r1'^2 - (x1-P1)^2 - (y1-P2)^2</math> | |
− | |||
− | + | корни <math>~d1,d2 = \frac{-b \pm \sqrt{b^2-4 \cdot a \cdot c}}{2 \cdot a}</math> | |
− | |||
− | c | ||
− | |||
− | Квадратное уравнение имеет 2 корня. То есть, кроме реального решения, есть еще какое-то второе, тоже удовлетворяющее системе. | + | Квадратное уравнение имеет 2 корня. То есть, кроме реального решения, есть еще какое-то второе, тоже удовлетворяющее системе. Скорее всего это решение соответствующее отрицательным r1,r2,r3. |
− | + | Данный метод был проверен на практике и дает неудовлетворительный результат, так как измерения в реальности не могут быть проведены с одинаковым отклонением. Метод расходится на практике, хотя при искусственном одинаковым отклонении действительно получается адекватный результат. Маяковая система навигации, определяющая положение роботов по 3м маякам с известными координатами. Полностью автономная, с промышленным протоколом.Описание системы вы можете найти здесь http://robot-develop.org/archives/484 Эта система применялась для навигации мобильного автономного робота проекта Евробот 2011. Здесь был использован искусственный аналитический метод, когда для каждой пары окружностей - множества точек одинакового удаления решалась задача пересечения, в случае отсутствия решения выбиралась точка, равноудаленная от обеих окружностей. Далее находились такие пары точек, чья дисперсия была наименьшей, их координаты усреднялись и на выходе получался довольно адекватный результат определения положения робота в пространстве. |
Текущая версия на 21:55, 1 марта 2011
Постановка задачи
Имеем в плоскости 3 маяка с известными координатами <math>(x1,y1), (x2,y2), (x3,y3)</math> и замеренные расстояния до них <math>(z1,z2,z3)</math> известные с точностью до неизвестного одинакового отклонения <math>(z)</math>. Требуется определить наиболее вероятные координаты робота <math>(x,y)</math> и соответственно одинаковое отклонение <math>(z)</math>, зависящее от смещения часов робота относительно часов маяков.
Источник задачи - система локализации на базе (ультра)звуковых маяков, которые могут быть рассинхронизированы по часам с автономным роботом.
Решение численным методом Ньютона
Решенее предложил mandigit
Запишем уравнение для расстояния <math>~ri</math> от i-го маяка до робота:
<math>zi=\sqrt{(xi-x)^2+(yi-y)^2}+z</math>
Продифференцировав это равенство получим:
<math>dzi = \frac{(xi-x)dx+(yi-y)dy}{\sqrt{(xi-x)^2+(yi-y)^2}}+dz = \frac{(xi-x)dx+(yi-y)dy}{(zi-z)}+dz</math>, умножив на <math>~(zi-z)</math>, получим:
<math>~(zi-z)dzi = (xi-x)dx + (yi-y)dy + (zi-z)dz</math>
При известных <math>~dzi</math> три таких равенства представляют собой систему линейных уравнений относительно неизвестных <math>~dx,dy,dz</math>.
Примем <math>d0=\sqrt{dx^2+dy^2+dz^2}</math>, далее имея все эти выкладки запускаем следующий итерационный алгоритм.
- Инициализация <math>x(0)=\frac{x1+x2+x3}{3}</math>, <math>y(0)=\frac{y1+y2+y3}{3}</math>, <math>~z(0)=0</math>;
- Шаг алгоритма №n (n>=1):
- Вычисляем <math>~zi'</math> исходя из координат <math>~x(n-1), y(n-1), z(n-1)</math>;
- Находим их отклонение <math>~dzi=zi'-zi</math> от реально измеренных <math>~zi</math>;
- Решаем систему линейных уравнений и находим <math>~dx,dy,dz</math>, добавляем их к <math>~x(n-1),y(n-1),z(n-1)</math> и получаем <math>~x(n),y(n),z(n)</math>;
- Если <math>~d0</math> меньше некоторого <math>~epsilon</math>, тогда останавливаемся и считаем найденные <math>~x(n),y(n),z(n)</math> достаточно хорошим решением, которое невозможно уже значительно улучшить без серьезных времязатрат.
ВНИМАНИЕ! В решении есть проблема - непонятно почему для сходимости dz надо не добавлять, а отнимать от z(n-1).
ВНИМАНИЕ! Область сходимости данного метода до конца не исследована. Внутри треугольника маяков и в некоторой его окресности на практике метод сходится за 5-6 итераций с точность порядка 3-4% от диаметра треугольника маяков.
Решение аналитическим методом
Решение предложил boez.
Пусть <math>~r1, r2, r3</math> - расстояния до маяков, реальные.
Известные величины: <math>~r1', r2', r3'</math> - расстояния с неким смещением, одинаковым.
расстояния:
- <math>~r1 = r1'+d</math>
- <math>~r2 = r2'+d</math>
- <math>~r3 = r3'+d</math>
Координаты маяков: <math>~(x1,y1),(x2,y2),(x3,y3)</math>
Искомые величины: <math>~x,y,d</math> - координаты и приведенное к расстоянию смещение во времени.
Уравнения окружностей (расстояния до маяков):
- <math>~r1^2 = (x1-x)^2 + (y1-y)^2</math> (1)
- <math>~r2^2 = (x2-x)^2 + (y2-y)^2</math> (2)
- <math>~r3^2 = (x3-x)^2 + (y3-y)^2</math> (3)
Вычтем из первого уравнения второге и третье
- <math>~(r1'-r2')(r1'+r2'+2 \cdot d) = (x1-x2)(x1+x2-2 \cdot x) + (y1-y2)(y1+y2-2 \cdot y)</math> (4)
- <math>~(r1'-r3')(r1'+r3'+2 \cdot d) = (x1-x3)(x1+x3-2 \cdot x) + (y1-y3)(y1+y3-2 \cdot y)</math> (5)
А это уже линейная по x, y и d система. Объявив d известным, решаем ее и находим линейные же выражения <math>~x=x(d), y=y(d)</math>:
<math>~A1 \cdot x+B1 \cdot y=C1</math>, где <math>~A1=2 \cdot (x1-x2)</math>, <math>~B1=2 \cdot (y1-y2)</math>, <math>~C1=(x1+x2) \cdot (x1-x2)+(y1+y2) \cdot (y1-y2)-(r1'-r2') \cdot (r1'+r2'+2 \cdot d) = D1 + k1 \cdot d</math>
<math>~A2 \cdot x+B2 \cdot y=C2</math>, где <math>~A2=2 \cdot (x1-x3)</math>, <math>~B2=2 \cdot (y1-y3)</math>, <math>~C2=(x1+x3) \cdot (x1-x3)+(y1+y3) \cdot (y1-y3)-(r1'-r3') \cdot (r1'+r3'+2 \cdot d) = D2 + k2 \cdot d</math>
<math>~D=A1 \cdot B2-A2 \cdot B1=4 \cdot (x1-x2) \cdot (y1-y3)-4 \cdot (x1-x3) \cdot (y1-y2)</math> - не ноль, т.к. маяки не на 1 прямой.
<math>~x(d)=\frac{C1 \cdot B2-C2 \cdot B1}{A1 \cdot B2-A2 \cdot B1} = \frac{(D1+k1 \cdot d) \cdot B2-(D2+k2 \cdot d) \cdot B1}{A1 \cdot B2-A2 \cdot B1} = P1+m1 \cdot d</math>
<math>~y(d)=\frac{A1 \cdot C2-A2 \cdot C1}{A1 \cdot B2-A2 \cdot B1} = \frac{A1 \cdot (D2+k2 \cdot d)-A2 \cdot (D1+k1 \cdot d)}{A1 \cdot B2-A2 \cdot B1} = P2+m2 \cdot d</math>
Подставляем например в (1), получаем квадратное уравнение с одним неизвестным d:
<math>~(r1'+d)^2 = (x1-x)^2 + (y1-y)^2 = (x1-P1-m1 \cdot d)^2 + (y1-P2-m2 \cdot d)^2</math>
<math>~d^2 + 2 \cdot r1' \cdot d + r1'^2 = m1^2 \cdot d^2 - 2 \cdot (x1-P1) \cdot m1 \cdot d + (x1-P1)^2 + m2^2 \cdot d^2 - 2 \cdot (y1-P2) \cdot m2 \cdot d + (y1-P2)^2</math>
<math>~(1 - m1^2 - m2^2) \cdot d^2 + (2 \cdot r1'+2 \cdot (x1-P1) \cdot m1+2 \cdot (y1-P2) \cdot m2) \cdot d + (r1'^2 - (x1-P1)^2 - (y1-P2)^2) = 0</math>
<math>~a = 1 - m1^2 - m2^2</math>
<math>~b = 2 \cdot r1'+2 \cdot (x1-P1) \cdot m1+2 \cdot (y1-P2) \cdot m2</math>
<math>~c = r1'^2 - (x1-P1)^2 - (y1-P2)^2</math>
корни <math>~d1,d2 = \frac{-b \pm \sqrt{b^2-4 \cdot a \cdot c}}{2 \cdot a}</math>
Квадратное уравнение имеет 2 корня. То есть, кроме реального решения, есть еще какое-то второе, тоже удовлетворяющее системе. Скорее всего это решение соответствующее отрицательным r1,r2,r3.
Данный метод был проверен на практике и дает неудовлетворительный результат, так как измерения в реальности не могут быть проведены с одинаковым отклонением. Метод расходится на практике, хотя при искусственном одинаковым отклонении действительно получается адекватный результат. Маяковая система навигации, определяющая положение роботов по 3м маякам с известными координатами. Полностью автономная, с промышленным протоколом.Описание системы вы можете найти здесь http://robot-develop.org/archives/484 Эта система применялась для навигации мобильного автономного робота проекта Евробот 2011. Здесь был использован искусственный аналитический метод, когда для каждой пары окружностей - множества точек одинакового удаления решалась задача пересечения, в случае отсутствия решения выбиралась точка, равноудаленная от обеих окружностей. Далее находились такие пары точек, чья дисперсия была наименьшей, их координаты усреднялись и на выходе получался довольно адекватный результат определения положения робота в пространстве.