Подводный «GPS» на двух приемопередатчиках03.09.2018 08:17

Привет дорогой хабрачитатель!

Нашему подводному GPS на днях исполнилось три года. За это время система стала серийной, мы вывели на рынок еще несколько систем и устройств, но все это время меня не покидала навязчивая идея принести гидроакустику в широкие массы, сделать ее доступной простым любителям, моделистам, представителям такого направления, как городские сумасшедшие citizen science и прочим интересующимся.
И сегодня, в честь нашего скромного юбилея, я подробно и на пальцах объясню, как определять географическое положение подводного объекта при помощи всего двух приемопередатчиков: с исходным кодом, веселыми картинками, графиками и результатами экспериментов.
Всех заинтересованных прошу к нам на байдарку, в чистые и теплые воды Волгоградского Водохранилища!

ВНИМАНИЕ: статья содержит следовые количества матана!

Для тех, кто не понимает, что происходит, но не прочь вникнуть, предлагаю ознакомиться с нашими предыдущими публикациями по теме гидроакустики и подводной навигации:
Сеанс передачи видео звуком через воду с разоблачением
Подводный GPS с нуля за год
Подводный GPS: продолжение
К вопросу о влиянии цианобактерий на речевые функции президента
Навигация под водой: пеленгуй — не пеленгуй, обречен ты на успех
А также с великолепной обзорной статьей akuzmin про гидроакустику

Вводные

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

ультракороткобазисные (УКБ, USBL) — основанные на определении угла прихода сигнала маяка и измерении дистанции (либо методом запрос-ответ либо по синхронизированным часам). Угол прихода определяется при помощи антенной решетки. Искомое положение определяется по углу и дальности (прямая геодезическая задача). УКБ системой например, является наша ZIMA
длиннобазисные (ДБ, LBL) — где измеряются времена прихода сигнала либо на несколько приемников, либо от нескольких передатчиков (наш подводный GPS работает именно так) с известным положением. Можно или измерять дистанции по методу «запрос-ответ» (дальномерный способ) или измерять разности времен прихода сигнала (разностно-дальномерный способ), так например, работают GPS и GLONASS. Длинная база бывает плавучая (как наша RedWAVE) или донная.
Короткобазисные (КБ, SB) — по сути — те же длиннобазисные системы, но все опорные элементы базы располагаются на одном основании — к примеру, на судне.

Как я уже сказал, длинная база может работать по принципу «запрос-ответ», в этом случае требуется решить задачу о пересечении N сфер, или окружностей, если известна глубина искомого объекта.
А может база работать так же как GPS и GLONASS, такие системы еще называют гиперболическими, и вот почему: если искомый объект периодически излучает навигационный сигнал, а часы всех приемников (элементов базы) синхронизированы, то дистанцию напрямую измерить нельзя, а можно только узнать к какому из элементов базы источник ближе или дальше. То есть в наличии только разности дистанций. А как известно из школьного курса:

«Гипербола может быть определена как геометрическое место точек, абсолютная величина разности расстояний от которых до двух заданных точек, называемых фокусами, постоянна»

Другими словами, если подводный объект излучил сигнал, и мы приняли его на двух приемниках, часы которых синхронизированы, то мы можем утверждать, что наш объект лежит на гиперболоиде (или на гиперболе, если он вдруг передал нам свою глубину). Больше приемников — больше гипербол! Больше гипербол — выше точность определения местоположения нет дворца — нет дворца.
А если излучает не подводный объект, а элементы базы, то подводный объект, зная свою глубину, сам может определить свое местоположение, и более того, он в этом случае никому не мешает и таких объектов может быть сколько угодно — они все просто слушатели. Так устроены GPS/GLONASS и наш RedWAVE.
А я обещал навигацию по двум приемопередатчикам, один из которых — тот, чью позицию как раз и надо определить.

Давайте разбираться

Расстояние между двумя точками $A(x_a,y_a,z_a)$ и $B(x_b,y_b,z_b)$ в трехмерном пространстве определяется (мало ли, вдруг кто забыл) по простой формуле:

$d_{{AB}} = {\sqrt{(x_a - x_{b^{}})^2 + (y_a - y_b)^2 + (z_a - z_b)^2}}$

Пусть у нас есть N базовых станций. Из гуманных соображений предположим, что их координаты нам известны, и мы измерили расстояния $d_{bn}$ от каждой из базовых станций $B_n(x_{bn},y_{bn},z_{bn})$ до цели T. Для ясности: T — это target, a B — base.
Стало быть, мы можем для любой точки $M(x,y,z)$ записать выражение, которое показывает, насколько эта любая точка подходит под наши экспериментальные измерения. Фактически это будет сумма разностей между расстоянием от точки М до соответствующей базовой станции и измеренной дистанцией. Каждую эту разность возведем в квадрат.

$\epsilon (x, y, z) = \sum_{n=0}^N \left( \sqrt{(x - x_{{bn}})^2 + (y - y_{{bn}})^2 + (z - z_{{bn}})^2} - d_{{Tbn}} \right)$

В общем, дело идет к методу наименьших квадратов, а координаты искомой точки в идеале должны быть минимумом функции ε.
Если же речь идет о разностно-дальномерном способе, которого мы коснемся в этой статье совсем огульно, и у нас каждой базовой станции соответствует не измеренная дистанция $d_{bn}$ , а время прихода сигнала $t_{OA}$ , то в выражении функции ошибки будут не дистанции, а их разности. С той лишь разницей, что придется перебрать нужные пары базовых станций, например, для n-ой и n+1-ой станции:

$\sqrt{(x-x_{bn})^2+(y-y_{bn})^2+(z-z_{bn})^2}-\\ \sqrt{(x-x_{bn+1})^2+(y-y_{bn+1})^2+(z-z_{bn+1})^2}-\\v(t_{OAn}-t_{OAn+1})$

v — здесь означает скорость звука, которая, кстати, не постоянна, а сильно зависит от плотности среды, читай, от температуры, солености и давления. И если хочется сделать все по-взрослому, то ее нужно или непосредственно измерять, или рассчитывать, но об этом как-нибудь в другой раз.
Отметим, что глубина позиционируемых объектов в современных гидроакустических навигационных системах измеряется непосредственно и с достаточно высокой точностью (0.5–2 см), значит, задачу имеет смысл перевести в плоскую.
Можно было бы сообщить, что теперь вот это решаем любым методом оптимизации и на этом закончить. Но, во-первых, не все так просто, а во-вторых, не все так сложно.

Начнем со сложностей

Дело в том, что очень много зависит от расстановки базовых станций относительно друг друга (на это еще как-то можно влиять, например, административными методами =) и положения позиционируемого объекта, относительно элементов базы (на это влиять гораздо сложнее, в широком круге задач изначально положение позиционируемого объекта известно очень примерно и заранее выгодно установить базу почти невозможно).
Получить ситуацию как на рисунке 1 — как с добрым утром:

Рисунок 1 — Поверхность ошибок для базы их трех элементов. Позиционируемый объект вне базы. Наличие ложного минимума

Зелеными кружками обозначены положения базовых станций, а звездочкой — истинное положение позиционируемого объекта. Цвет точек показывает величину функции невязки (ε) в этой точки. Как говорил один политик, совершенно понятно, что поверхность имеет две впадины, и при поиске минимума очень легко попасть в ложный. Ситуация оказывается еще хуже: принимая во внимание масштаб, реальное положение объекта и среднюю скорость движения таких объектов можно сказать, что в таком положении он будет находится достаточно долго, форма поверхности ошибок будет сохраняться, и очень велика вероятность длительное время получать неверные данные о местоположении объекта.
На анимации ниже иллюстрируется подобная ситуация — и при старте поиска из средней точки между базовыми станциями, и при старте от ближней станции поиск ведет к ложному минимуму:

Рисунок 2 — Попадание поиска в ложный минимум при старте из средней точки базы. База из трех элементов

Поиск здесь реализован при помощи алгоритма Нелдера-Мида, или как его еще называют — метода симплекса. Именно он используется, например, в Matlab в функции fminsearch
Он не требует вычисления производных, очень нагляден, интуитивно понятен и прост в реализации.

Рисунок 3 — Попадание поиска в ложный минимум при старте из ближайшего элемента базы. База из трех элементов

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

«Поражение было полным. Придумать что-либо не представлялось возможным» © А. и Б. Стругацкие, «Град Обреченный»

Что же можно здесь придумать?

Логика подсказывает, что можно воспользоваться одним из двух методов глобальной оптимизации — Simulated Annealing или чем-то подобным. Однако, другой (и более продуктивный) подход заключается в более полном использовании априорной информации для выбора стартовой точки поиска.
Ведь мы же точно знаем, что искомая точка лежит (с долей допущения) на окружностях, в центрах которых находятся базовые станции, а радиусы этих окружностей соответствуют измеренным дальностям!
А еще мы уверены в том, ошибка измерения дальности в гидроакустике растет вместе с этой дальностью: звук распространяется нелинейно.
Можно взять и попробовать выбрать начальную точку поиска минимума вокруг (буквально, на окружности) ближайшего маяка, рассчитывая, что дальность до него измерена с максимальной точностью, и руководствуясь тем же самым значением функции невязки ε.
Координаты точек-кандидатов формируются в этом случае согласно следующим простым формулам:

$x=x_{bc}+d_{bc}cos\alpha\\ y= y_{bc}+d_{bc}sin\alpha$

$x_{bc}$ и $y_{bc}$ — координаты ближней (c — closest) базовой станции, $d_{bc}$ — дистанция от нее до позиционируемого объекта. $\alpha$ — аргумент (угол). Если теперь поискать минимум для $\epsilonε$ , варьируя $\alpha$ в диапазоне от 0 до 360 градусов с шагом, например, в 10 градусов, а потом поискать рядом с этим значением уменьшив шаг до 1 градуса и сузив область поиска до 20 (± 10) градусов, то можно очень хорошо определить первоначальное приближение, которое практически во всех случаях сразу бьет почти точно в цель!
На картинках ниже метод решения «в лоб» за 30 итераций доблестно попал в ложный минимум, что привело к абсолютно неверному решению, а метод с предварительной одномерной минимизацией почти сразу попал в точку и за 6 итераций приблизился к действительному положению с расхождением ~25 сантиметров при дистанции до ближайшей базовой станции ~400 метров.

Рисунок 4 — Попадание поиска в ложный минимум

Рисунок 5 — Нахождение верного решения при старте из точки, полученной предварительной одномерной минимизации

Рисунок 6 — Увеличенная область вокруг полученного решения. Желтый круг — полученное решение, зеленая звездочка — истинное положение объекта

В вышеприведенном примере уже первая итерация после предварительного выбора начальной позиции поиска дала решение в 20 метрах от реального положения (на поле ~600×600 метров).
На данном этапе мы разобрались с дальномерным способом, с разностно-дальномерным ситуация чуть сложнее: там нет таких четких ориентиров для выбора начальной точки (круг — хорошая фигура, потому что он замкнут, а гипербола — плохая, потому что уходит на бесконечность).
Вернемся к обещанному «подводному GPS» из двух приемопередатчиков. В каком-то смысле я конечно слукавил: полноценный подводный GPS как из нашей первой статьи не получится, но если нет денег купить правильный дивайс смириться с некоторыми ограничениями и особенно остановиться на определенном круге задач, то сделать кое-что все-таки можно.
Часто позиционируемый объект неподвижен (ну или хотя бы движется пренебрежимо медленно). В таких случаях вместо нескольких базовых станций можно действительно использовать одну, но перемещающуюся: такой подход называют VLBL или виртуальная длинная база. Если нам не требуется фиксировать трек движения подводного объекта, а просто определить его местоположение — то это то, что нужно.
Часто такая задача возникает при установке какого-либо донного оборудования путем свободного сброса с судна. При ощутимых глубинах и наличии течений реальное положение оборудования на дне может значительно отличаться от положения точки, где его сбросили. Этим оборудованием может быть и элемент донной навигационной базы: чтобы определить местоположение подводного объекта, сначала нужно определить местоположение подводного объекта.

Итак, как все это может быть устроено в реальном мире?

Наш экспериментальный сет-ап состоит из:

объекта, который мы будем прятать на дне водоема: наш кодовый модем RedGTR в автономном исполнении (это только звучит официально, на деле — модем, подключенный к герметичному батарейному боксу); Вот он на фото:

Рисунок 7 — Маяк-ответчик с батарейной канистрой
подвижной базовой станции на базе приманочной китайской лодочки из нашей другой статьи. У неё на борту, помимо родной начинки управления, установлен второй модем RedGTR, радиомодуль DORJI на 433 МГц с антенной, самодельный модуль GPS/GLONASS на базе quectel l76, собственная плата на базе STM32F429 (может быть легко заменена на NUCLEO-STM429 или STM32F4 Discovery, уверен, что и любая Arduino-подобная плата тоже справилась бы) которая просто играет роль маршрутизатора: с радиоканала по UART все идет в модем и наоборот, данные с GPS-модуля также сыпятся в радиоканал.
Так это выглядит в сборе:

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

Рисунок 9 — Фото тестового плавсредства. Вид в трюм
Все родные схемы остались почти без изменений, а все дополнительное оборудование питается от отдельного свинцового АКБ на 1.2 А*ч.
пульта оператора, в состав которого входит ноутбук с ПО и ответный радиомодуль, подключаемый через преобразователь к USB. ПО конечно несколько поспешное и годится только для демонстрации, но там уже реализован протокол взаимодействия с модемом, TOA-решатель, на основе того же самого метода Нелдера-Мида, выбор базовых точек, простая рисовалка географических позиций, а также возможность эмуляции GPS для передачи данных в какой-нибудь аналог Google Earth.

Рисунок 10 — «Пульт оператора». Тестовый прогон на столе.
Синие точки — положения лодки по GPS, зеленые — положения, в которых были сделаны измерения, лососевым выделены положения элементов виртуальной базы, красным — вычисленные положения маяка-ответчика.

Кто чем занят?

модем, который требуется найти, просто лежит на дне, ведет прием и отвечает на запросы;
модем, который на лодке, выполняет команды, пришедшие по UART — посылает запросы и принимает ответы, о чем сообщает по UART
плата с STM32F4 в лодке получает данные с GPS и модема по UART и перенаправляет их в радиомодуль по другому UARTу, а данные из радиомодуля она перенаправляет в модем; Тут есть небольшой нюанс, связанный с радиомодулем — он не полнодуплексный (прям как в гидроакустике =)), поэтому плата «придерживает» сообщение RMC от GPS-модуля и отсылает его только вместе с сообщением от модема;
пультовое ПО управляет всем действием, собирает данные о дистанциях до цели из разных географических положений, выбирает оптимальную базу и определяет местоположение цели;

Чуть более детально

Общение с модемами происходит по NMEA-подобному протоколу. Для нашей задачи используется всего три сообщения:

$PTNTE,targetAddr,requestedCmd,timeoutMs*hh

от пульта к модему. Передать удаленному абоненту по адресу targetAddr команду requestedCmd, таймаут ожидания ответа установить равным timeoutMs.
Команда может быть любой из утвержденного списка:

Команда	Код	Описание
CDS_CMD_PING	0	Запрос PING на который удаленный абонент отвечает PONG
CDS_CMD_PONG	1	Ответ на запрос PING
CDS_CMD_DPT	2	Команда удаленному абоненту на передачу его глубины
CDS_CMD_TMP	3	Команда удаленному абоненту на передачу его температуры
CDS_CMD_BAT	4	Команда удаленному абоненту на передачу напряжения его питания
CDS_CMD_USR_0	5	Пользовательская команда 1
CDS_CMD_USR_1	6	Пользовательская команда 2
… .	… .	… .
CDS_CMD_USR_34	39	Пользовательская команда 34

Модем поддерживает базовые функции: пинг, передачу глубины, температуры воды, заряда батареи и набор пользовательских кодовых команд.
На команду TNTE модем сразу отвечает о факте принятия команды или невозможности ее выполнить сообщением АСК:

$PTNT0,errCode

errCode — 0 если модем принял команду, в противном случае код ошибки
Если модем не дождался ответа удаленного абонента вовремя, он сообщает об этом сообщением REM_TIMEOUT, содержащим в поле параметров только адрес удаленного абонента:

$PTNTB,targetAddr

Если модем принял ответ вовремя, то он сообщает об этом командой REM_PONGEX, имеющей такой формат:

$PTNTD,requestedAddr,requestedCmd,receivedValue_decoded,snrd,dpl,pTime,[dst],[dpt],[tmp]

requestedAddr — адрес запрошенного абонента
requestedCmd — запрошенная команда
receivedValue_decoded — принятое значение
snrd — соотношение сигнал-шум на выходе приемника в дБ
dpl — доплеровский сдвиг в Гц
pTime — время распространения сигнала (в одну сторону) в секундах
dst — дистанция до абонента в метрах
dpt — собственная глубина модема в метрах
tmp — забортная температура воды в градусах Цельсия

Последние три параметра передаются, только если модем в исполнении со встроенным датчиком давления/температуры (наш вариант).
От модуля GPS нас интересует только сообщение RMC, откуда мы берем текущую географическую позицию (можно было бы использовать и GGA или GLL — последнее самое короткое).

Самое сложное — передачу данных через воду и измерение дистанции — выполняют модемы, определение географической позиции лодочки — GPS модуль, осталось самая малость — правильно воспользоваться этими данными.
Весь распарс и построение сообщений NMEA выполняются при помощи класса NMEAParser в библиотеке UCNLNMEA.
Пультовое ПО, работающее на ноутбуке, периодически посылает запросы REM_PINGEX абоненту по указанному в настройках приложения адресу, делает оно это либо по факту получения REM_TIMEOUT либо REM_PONGEX. Попутно фиксируются координаты лодочки и сохраняются данные о дистанции и глубине удаленного абонента вместе с координатами лодочки, в которых они были получены.
Здравый смысл (и опыт) подсказывает нам, что для определения позиции подводного объекта нужно 3–5 виртуальных базовых станций, и как видно из картинок выше, расположены они должны быть не абы как.
То есть формируется следующая задача: по мере поступления измерений необходимо определенным образом выбирать несколько из них в качестве элементов навигационной базы.

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

List Measurements.GetBase();

По мере поступления данных, в методе где парсится сообщение REM_PONGEX добавляем измерения и пытаемся построить навигационную базу и решить с помощью нее навигационную задачу. За это отвечает метод

GeoPoint3DWE LocateLBL_NLM(List base, GeoPoint3DWE prevLocation, double rErrorThreshold, out double stStageRErr, out int itCnt);

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

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

Для простоты, решение выполняется в метрах, для чего перед началом решения все координаты переводятся в локальную систему, с началом отсчета в средней точке навигационной базы. После решения результат обратно переводится в географические долготу и широту (долгота — это Х, а широта — Y). Для перевода градусов в метры и назад класс Navigation содержит методы GetDistance2DDeg и Meters2Deg.
Но, как известно, теория без практики мертва, и пора перейти к результатам натурных экспериментов.

Оживим теорию практической проверкой

По старой традиции, испытания проводим в устье реки Пичуга. Там удобное место, есть глубины почти до 30 метров, и достаточно сложная акватория.
Маяк-ответчик закреплен на специальном черенке «стенде» (в кадр не попали пенопластовые балберки и якорь, которые помогают всей конструкции держаться вертикально):

Рисунок 11 — «Донная станция» маяк-ответчик.

Установку маяка сняли на видео, немного не рассчитали со светом и забыли взять с собой фонарь, так что момент контакта с грунтом остался сокрытым во тьме.
Пока собирались на испытания дали лодочке имя:

Рисунок 12 — «Палыч» в естественной среде

На транец «Палычу» нейлоновыми стяжками закреплен кусок от удочки, на котором и располагается модем RedGTR (чтобы не болтался и не всплывал).
Эксперимент состоял в том, что маяк-ответчик устанавливается на дно, сверху от него торчит только небольшой поплавок, «Палыч» при этом, управляемый с берега, ходит по акватории большими кругами, пультовое ПО периодически запрашивает у ответчика что-нибудь, например его глубину. Все данные записываются в журнал, а на экране все отображается согласно реальному положению вещей. Вот так:

Рисунок 13 — Скриншот пультового ПО с траекторией лодки и вычисленными положениями маяка-ответчика (красным показано лучшее вычисленное положение)

Как видно из рисунка 13, маяк располагался на глубине 13.2 м, а температура воды на этой глубине составила 24.1 °С.

Рисунок 14 — Импорт полученных треков в GoogleEarth

Получение хорошего результата в данном случае заняло от силы минут 15 плавания.
Во втором эксперименте маяк был установлен на глубине 16.5 метров, где ожидаемо несколько холоднее — всего 22.6 °С.

Рисунок 15 — результат определения местоположения маяка-ответчика во втором эксперименте на экране пультового ПО

Правильной оценкой точности в данном случае мы не занимались ввиду острой нехватки времени (на самом деле данные потешные испытания стали причиной некоторых трений среди руководства, ибо отрывали от работы), а удовольствовались тем, что когда судно выходило к поплавку (этот момент можно видеть на первом треке), то в плане его позиция полностью совпадала с лучшим решением, а дистанция совпадала с глубиной (с разностью глубин ответчика и модема, установленного на судне). Точки, в которых производился сброс маяков, фиксировались, и расхождение между вычисленным положением и точками сброса находится в районе 2–3 метров, что дополнительно подтверждает работоспособность системы.

В общем и целом, можно констатировать что данный proof-of-concept успешно выполнен!
Мы получили истинное наслаждение и солнечные ожоги, будем рады ответить на вопросы и выслушать критику!

Послесловие

Грустный факт состоит в том, что применяемые в данном эксперименте модемы хоть и очень дешевые по меркам мирового рынка средств гидроакустической связи, все же не очень чтобы доступны любителям.
Чтобы не быть голословным, приведу ссылку на отличную работу мисс Бенсон, в которой на странице 54 по PDF или 36 по нумерации документа есть интересная табличка, с ценами на некоторые гидроакустические модемы по состоянию на 2010 год. Такие дела. (В таблице есть небольшая ошибка — дальность связи Range для разрабатываемого ею модема указана в метрах, в то время как в заголовке колонки стоит km).
Хорошая же новость состоит в том, что мы внезапно разработали модемы, которые могут почти все тоже самое (чуть поменьше по размерам (по факту — самые маленькие в мире, а до них самые маленькие были тоже наши — из этой статьи), меньше дальность связи, ниже скорость передачи), и по нашим оценкам стоят уже вполне подъемно для зажиточных энтузиастов. На данный момент электроника и прошивка полностью готовы, а мы допиливаем корпусное решение. По окончанию разработки обязательно опубликуем статью с результатами экспериментальной проверки.