Создание ультразвуковой локальной системы позиционирования

Ультразвуковая локальная система позиционирования (УЗ ЛСП) — система позиционирования для роботов, являющаяся близким аналогом спутниковых систем навигации (ССН) как в аппаратной, так и в программно-алгоритмической части.

Принцип работы

Основным компонентом спутниковых систем позиционирования являются космические аппараты, которые имеют единую высоту и период вращения вокруг Земли.

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

Зная эту информацию, а также время задержки между отправкой сигнала со спутника и его приемом на пользовательском устройстве, оно способно найти расстояние от себя до спутника просто умножив время задержки на скорость радиоволны (~300 000 000 м/с). Зная расстояния от себя до нескольких спутников пользовательское устройство способно вычислить собственное положение с помощью алгоритма трилатерации.

Организация аналогичной системы в помещении не приведет к удовлетворительным результатам из-за малого расстояния между передатчиком и приемником, которое во много раз меньше расстояния между космическим спутником и приемником на Земле, а также, из-за огромной скорости радиоволны, что делает невозможным замер задержки между отправкой и получением.

Следовательно, в помещении уместнее использовать звук, так как он имеет меньшую скорость (~340 м/с).

Ниже рассмотрена инвертированная система, то есть передатчики установлены не на спутниках, а на клиентском устройстве. Это сделано для того, чтобы повысить частоту работы системы в три раза не меняя аппаратное обеспечение (все приёмники/передатчики используют одинаковую частоту — 40 кГц).

Алгоритм определения положения

Основные положения и допущения

Рисунок 1.

Для определения положения робота используется алгоритм трилатерации.

Местоположение Приёмника О будет являться точкой начала координат (0; 0), а Приёмники А и Б расположены взаимно перпендикулярно (см. Рисунок 1).

Расстояние от Приёмника О до Приёмника А (обозначим как i) и от Приёмника О до Приёмника Б (обозначим как j) известно.

Геометрическое представление

С геометрической точки зрения задача трилатерации сводится к нахождению точки пересечения трех или четырех сфер, координаты центра которых известны (ими являются Приёмники О, А, Б), а радиусом которых является расстояние от центра каждой из сфер до робота.

Если известно расстояние от робота до одного приёмника, то эта информация позволяет говорить, что робот находится где-то на поверхности этой сферы (см. Рисунок 2.1).

Если известны расстояния до двух приёмников, то это сужает область местонахождения робота до пространства, образуемого в месте пересечения двух сфер (см. Рисунок 2.1).

Расстояния до трех приемников позволяет узнать две точки, в одной из которых находится робот — это точки пересечения трех сфер (см. Рисунок 2.2).

А если известны расстояния до четырех приемников, то можно определить единственную точку пересечения четырех сфер, которая и будет являться точкой местонахождения робота (см. Рисунок 2.3).

Таким образом, применение системы из 3-х приемников уместно только в том случае, когда робот находится с ними в одной плоскости. В ситуациях, когда предполагается полёт робота — требуется применение системы из 4-х приемников.

• 
  Рисунок 2.1.
• 
  Рисунок 2.2.
• 
  Рисунок 2.3.

Вычисления

Рисунок 3.

Запишем систему, состоящую из уравнений трех сфер:

$r_{1}^{2}=x^{2}+y^{2}+z^{2}$

$r_{2}^{2}=(x-i)^{2}+y^{2}+z^{2}$

$r_{3}^{2}=x^{2}+(y-j)^{2}+z^{2}$

Где

• $r_{1}$, $r_{2}$, $r_{3}$ — расстояния от объекта (робота) до приёмников О, А, Б;
• $x$, $y$, $z$ — координаты робота;
• $i$ — смещение приёмника А относительно приёмника О по оси x;
• $j$ — смещение приёмника Б относительно приёмника О по оси y.

Имеем три неизвестных, выразим их одно через другое:

$x={\frac {r_{1}^{2}-r_{2}^{2}+i^{2}}{2i}}$

$y={\frac {r_{1}^{2}-r_{3}^{2}+j^{2}}{2j}}$

$z=\pm {\sqrt {r_{1}^{2}-x^{2}-y^{2}}}$

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

Значение координаты z выражается как корень из числа и может иметь 0, 1 или 2 решения.

Расчет скорости звука

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

$c={\sqrt {x*R*T}}$

Где

• $x$ — показатель адиабаты;
• $R$ — газовая постоянная (Дж/кг·К);
• $T$ — температура газа (К).

Аппаратная часть

Рисунок 4.

Рисунок 5.

Аппаратные средства системы состоит из двух частей: клиентской и серверной. Клиентской стороной является устройство с ультразвуковым передатчиком, устанавливающееся на робота, а серверной — три однотипных устройства с ультразвуковыми приёмниками.

Клиентская сторона

Клиентское устройство системы включает в себя:

• плату Arduino Mega 2560;
• плату расширения RS-485 Shield реализующую интерфейс RS-485 (для синхронизации с серверной стороной и обмена информацией с ней);
• ультразвуковой дальномер HC-SR04 с замотанным изолентой ультразвуковым приёмником, но работающим ультразвуковым передатчиком.

Серверная сторона

Каждое устройство серверной стороны системы включает в себя:

• плату Arduino Uno;
• ультразвуковой дальномер HC-SR04 с замотанным изолентой ультразвуковым передатчиком, но работающим ультразвуковым приёмником;

Устройство-координатор (Приёмник О) помимо вышеперечисленных компонентов имеет в своём составе:

• аналоговый термометр TMP36;
• плату расширения RS-485 Shield реализующую интерфейс RS-485 (для синхронизации с клиентским устройством и обмена информацией с ним).

Корпус (рабочее название — Табуретка), к которому крепятся элементы серверной стороны, выполнен в виде трех взаимно перпендикулярных брусков, длинной по 0.5 м каждый.

Для удобства эксплуатации на платы установлены винтовые клеммы (Screwshield).

Для обмена данными между платами серверной стороны (расстояние от соответствующего Приёмника до клиентского устройства) используется шина I2C. Схема подключений см. Рисунок 5.

Все платы серверной стороны имеют общую землю (GND) и питание (5В), что позволяет запустить всю систему просто подключив питание к любой плате. Потребляемый ток — 0.2 А.

Программная часть

Алгоритм работы

В сжатом виде алгоритм работы системы выглядит следующим образом:

1. Определение температуры
2. Расчет скорости звука
3. Отправка синхросигналов
4. Получение времени задержки Приёмника О
5. Отправка синхросигналов
6. Получение времени задержки Приёмников А, Б
7. Расчет расстояний до робота
8. Расчет координат робота

ПО системы

В конце статьи приведены ссылки на программный код всех элементов системы.

По причине того, что использование платы расширения RS-485 приводит к невозможности одновременной передачи данных по USB, в систему введен еще один микроконтроллер, который получает координаты x;y;z от Приёмника О по I2C и передает их через USB на ПК.

Анализ качества

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

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

Таких точек было взято три: A (0.5; 0.5), B (1; 1), C (1.2; 1.2) и в каждой из них робот пребывал примерно полторы минуты — за это время он получал около 1000 значений своего положения.

Для наглядного отображения полученных результатов было реализовано программное обеспечение, получающее массивы координат по оси x и оси y, а затем выводящее их на координатную плоскость в виде красных точек.

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

Результаты экспериментов с положением роботов A, B, C см. Рисунки 6.1, 6.2, 6.3 соответственно.

Как можно заметить, в каждом из экспериментов имело место отклонение усреднённой координаты от реальной. Причем, во всех случаях отклонение было в меньшую сторону.

• 
  Рисунок 6.1.
• 
  Рисунок 6.2.
• 
  Рисунок 6.3.

Анализ влияния каждого из источников ошибки

Всего было выявлено три источника ошибки, которые можно разбить на две группы, описанные ниже.

Накапливающаяся ошибка — растущая по мере удаления робота от Приёмников:

• Неточное определение скорости звука;
• Неточное определение расстояния между приемниками.

Статическая ошибка — постоянна в любом месте рабочей области системы:

• Рассинхронизация.

Ниже произведена оценка влияния каждого из источников ошибки на точность определения координат.

Неточное определение скорости звука

Как было сказано ранее, температура воздуха влияет на скорость звука. Используемый термометр имеет погрешность до ±3 градуса Цельсия.

Допустим, замеры проводились при температуре 20 градусов — тогда скорость звука равна 343.6 м/с. Пусть термометр показал информацию на 3 градуса ниже — 17 градусов, при которых скорость звука равна 341.7 м/с.

Разница в скорости звука в этой ситуации составляет 1.9 м/с. При координате B (1; 1) расстояние от Приёмника О до робота равняется примерно 1.4142 м — при верно определенной скорости звука, звуковая волна проходит это расстояние за 4.1158 мс.

Умножим это время на скорость звука, определенную с ошибкой и получим значение 1.4063 м, то есть в рассматриваемой ситуации отклонение, причиной которого является неправильно определенная температура воздуха, составляет 0.0079 м.

Неточное определение расстояния между приемниками

Рисунок 7.

Рассмотрим следующую возможную причину возникновения ошибки — неправильное определение расстояния между приёмниками. Допустим, при замере расстояния между ними была допущена ошибка в 0.02 м и вместо 0.45 м это значение было определено как 0.47 м (см. Рисунок 7).

Воспользуемся формулой трилатерации, упомянутой ранее:

$x={\frac {r_{1}^{2}-r_{2}^{2}+i^{2}}{2i}}$

При верно определенном расстоянии между приёмниками:

$x={\frac {1.5^{2}-1.25^{2}+0.45^{2}}{2*0.45}}=0.9888$

При неверно определенном расстоянии между приёмниками:

$x={\frac {1.5^{2}-1.25^{2}+0.45^{2}}{2*0.47}}=0.9663$

Таким образом, ошибка, вызванная этой причиной, в рассматриваемой ситуации составляет 0.0225 м. Как и ошибка, вызванная неточным определением температуры, она имеет свойство накапливаться — то есть с увеличением расстояния от приемников до робота растёт и ошибка.

Рассинхронизация

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

Если настройка синхронизации не была осуществлена достаточно точно, то полученная из-за этого ошибка будет неизменна и на расстоянии в 0.1 м, и на расстоянии в 4 м, благодаря этому её легко можно вычесть и тем самым улучшить результат.

Устранение ошибки

Чтобы устранить отклонение усредненного определенного значения от реального значения положения был использован метод коррекции, основанный на статистическом подходе.

Для этого были проведены эксперименты с определением координат для еще двух точек: D(1.7; 1.7), E(2.8; 2.8). Данные, полученные в ходе экспериментов с пятью точками, были усреднены и сведены в таблицу с реальными значениями робота в текущем эксперименте, после чего была вычислена ошибка для каждой оси.

Среднее значение (ось X)	Среднее значение (ось Y)	Реальное значение	Значение ошибки (ось X)	Значение ошибки (ось Y)
0,441304348	0,482391304	0.5	0,058695652	0,017608696
0,911305115	0,886719577	1	0,088694885	0,113280423
1,084007682	1,050550576	1.2	0,115992318	0,149449424
1,529365942	1,46611413	1.7	0,170634058	0,23388587
2,504018801	2,410763807	2.8	0,295981199	0,389236193

Рисунок 8.1.

Рисунок 8.2.

По полученным данным были построены графики зависимости значения ошибки от реальных координат по оси X (см. Рисунок 8.1) и оси Y (см. Рисунок 8.2), которые показывают, что накапливающаяся ошибка линейна, а значит может быть рассчитана и скорректирована в получаемых результатах.

Величина достоверности аппроксимации ($R^{2}$) для обеих осей близка к единице. Различие значений ошибки и, как следствие, уравнений аппроксимирующей прямой объясняется тем, что расстояние между Приёмником О и Приёмником А отличается от расстояния между Приёмником О и Приёмником Б и равно 0.455 и 0.425 м соответственно.

Опираясь на полученные результаты была применена корректировка координат с помощью следующей формулы для оси X:

$x_{r}=x+(0.1064*x-0.0073)$

Где

• $x$ — полученная координата;
• $x_{r}$ — координата после коррекции.

И для оси Y:

$y_{r}=y+(0.1599*y-0.0495)$

Где

• $y$ — полученная координата;
• $y_{r}$ — координата после коррекции.

В результате применения такой коррекции к координатам трёх проведённых ранее опытов были получены удовлетворительные результаты (см. Рисунки 9.1, 9.2, 9.3).

• 
  Рисунок 9.1.
• 
  Рисунок 9.2.
• 
  Рисунок 9.3.

Устранение разброса координат

Помимо ошибки определения координаты существует еще одна проблема: значительный разброс координат вокруг точки реального местонахождения робота.

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

Разброс координат является следствием неточного определения расстояния от одного из Приёмников до робота. Если расстояние между Приёмниками О и А, а также Приёмниками О и Б небольшое, например, 0.45 м, то ошибка определения расстояния в 0.01 м приводит к значительному отклонению вычисленного значения координаты робота от реальной.

Воспользуемся формулой трилатерации, упомянутой ранее:

$x={\frac {r_{1}^{2}-r_{2}^{2}+i^{2}}{2i}}$

При верно определенном расстоянии между Приёмниками и роботом (Рисунок 10.1):

$x={\frac {1.5^{2}-1.25^{2}+0.45^{2}}{2*0.45}}=0.9888$

При ошибке в 0.01 м при определении расстояния от Приёмника О и от Приёмника А (Рисунок 10.2):

$x={\frac {1.49^{2}-1.26^{2}+0.45^{2}}{2*0.45}}=0.9277$

Как можно видеть, отклонение вычисленной координаты от реальной составило 0.06 м.

• 
  Рисунок 10.1.
• 
  Рисунок 10.2.

Единственным способом устранения этого недостатка является увеличение расстояния между Приёмниками — при его увеличении в 4 раза, то есть до 2 м, область разброса уменьшится более чем в 4 раза. Проведем расчеты:

При верно определенном расстоянии между Приёмниками и роботом (Рисунок 10.3):

$x={\frac {1.5^{2}-1.25^{2}+2^{2}}{2*2}}=1.1718$

При ошибке в 0.01 м при определении расстояния от Приёмника О и от Приёмника А (Рисунок 10.4):

$x={\frac {1.49^{2}-1.26^{2}+2^{2}}{2*2}}=1.1581$

Как можно видеть, отклонение вычисленной координаты от реальной составило 0.01 м.

• 
  Рисунок 10.3.
• 
  Рисунок 10.4.

ТТХ полученной системы

Площадь рабочей области

Используемые ультразвуковые дальномеры имеют максимальную заявленную дальность 4 метра в режиме дальномера, то есть звуковая волна, генерируемая ими, способна преодолеть 4 метра до объекта, отразиться от него и вернуться обратно. То есть суммарное максимальное проходимое звуковой волной расстояние составляет 8 метров.

Рабочая область системы ограничена, в первую очередь, дальностью работы Приёмника О, так как он удален от робота больше двух других. Проведем расчеты:

$a^{2}+a^{2}=8^{2}$

$2a^{2}=64$

$a=5.6569$ метров.

Таким образом, рабочая область системы равна

$5.6569*5.6569=32$ кв.м.

Точность

Отклонение среднего значения составляет: ±0.05 м.

Частота работы

Частота получения координат: до 8-10 Гц.

Литература

• Sabir Shrestha, Samuel Kusi, Deep Shrestha Ultrasonic Positioning System(англ.). — Columbia University, 2010. — С. 3-5.
• Русаков Д.А. 6.22. Ультразвуковая система позиционирования // Актуальные вопросы развития систем и средств воздушно-космической обороны. Сборник докладов Пятой научно-технической конференции молодых ученых и специалистов / Под общей редакцией кандидата технических наук Н.Э. Ненартовича. — Москва: ОАО «ГСКБ «Алмаз-Антей»», 2014. — С. 677-683. — 744 с. — 300 экз.
• Русаков Д.А. Ультразвуковая локальная система позиционирования // Современные технологии в задачах управления, автоматики и обработки информации: Труды XXIV Международной научно-технической конференции, (14-20 сентября 2015 г., Алушта). — Москва: Издательский дом МЭИ, 2015. — С. 272-273. — 300 с. — ISBN 978-5-383-00950-5
• Jan D Bjerknes, Wenguo Liu, Alan FT Winfield, Chris Melhuish, Coldharbour Lane Low cost ultrasonic positioning system for mobile robots(англ.) // Proceeding of Towards Autonomous Robotic Systems : журнал. — 2007. — С. 107-114.

Ссылки

• Исходный код

Участники

--Daemon2010 (обсуждение) 14:21, 30 июня 2015 (UTC)