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

Приветствуем вас, уважаемые коллеги!

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

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

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

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

 

Вводные

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

На сегодняшний день существует три основных работающих типа навигационных систем:

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

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

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

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

А мы обещали навигацию по двум приемопередатчикам, один из которых – тот, чью позицию как раз и надо определить.

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

Расстояние между двумя точками A(xa,ya,za) и B(xb,yb,zb) в трехмерном пространстве определяется по простой формуле:

Пусть у нас есть N базовых станций. Предположим, что их координаты нам известны, и мы измерили расстояния dbn от каждой из базовых станций Bn(xbn,ybn,zbn) до цели T. Для ясности: T – это target, a B – base.
Стало быть, мы можем для любой точки M(x,y,z) записать выражение, которое показывает, насколько эта любая точка подходит под наши экспериментальные измерения. Фактически это будет сумма разностей между расстоянием от точки М до соответствующей базовой станции и измеренной дистанцией. Каждую эту разность возведем в квадрат:

В общем, дело идет к методу наименьших квадратов, а координаты искомой точки в идеале должны быть минимумом функции ε.

Если же речь идет о разностно-дальномерном способе, которого мы коснемся в этой статье, и у нас каждой базовой станции соответствует не измеренная дистанция dbn, а время прихода сигнала tOA, то в выражении функции ошибки будут не дистанции, а их разности. С той лишь разницей, что придется перебрать нужные пары базовых станций, например, для n-ой и n+1-ой станции:

v – здесь означает скорость звука, которая, как известно, не постоянна, а сильно зависит от плотности среды, читай, от температуры, солености и давления. И если хочется рассчитать все точно, то ее нужно или непосредственно измерять, или рассчитывать (об этом будет отдельная статья).

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

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

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

На практике мы скорее всего получим вот такую ситуацию:

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

На анимации ниже иллюстрируется подобная ситуация – и при старте поиска из средней точки между базовыми станциями, и при старте от ближней станции поиск ведет к ложному минимуму:

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

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

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

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

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

Логика подсказывает, что можно воспользоваться одним из двух методов глобальной оптимизации – Simulated Annealing или чем-то подобным. Однако другой (и более продуктивный) подход заключается в более полном использовании априорной информации для выбора стартовой точки поиска.

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

А еще мы уверены в том, ошибка измерения дальности в гидроакустике растет вместе с этой дальностью: звук распространяется нелинейно.

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

Координаты точек-кандидатов формируются в этом случае согласно следующим простым формулам:

xbc и ybc – координаты ближней (c – closest) базовой станции, dbc – дистанция от нее до позиционируемого объекта. a – аргумент (угол). Если теперь поискать минимум для ε, варьируя a в диапазоне от 0 до 360 градусов с шагом, например, в 10 градусов, а потом поискать рядом с этим значением уменьшив шаг до 1 градуса и сузив область поиска до 20 (+/- 10) градусов, то можно очень хорошо определить первоначальное приближение, которое практически во всех случаях сразу бьет почти точно в цель!

На картинках ниже метод решения «в лоб» за 30 итераций доблестно попал в ложный минимум, что привело к абсолютно неверному решению, а метод с предварительной одномерной минимизацией почти сразу попал в точку и за 6 итераций приблизился к действительному положению с расхождением ~25 сантиметров при дистанции до ближайшей базовой станции ~400 метров.

В вышеприведенном примере уже первая итерация после предварительного выбора начальной позиции поиска дала решение в 20 метрах от реального положения (на поле ~600×600 метров).

На данном этапе мы разобрались с дальномерным способом. С разностно-дальномерным ситуация чуть сложнее: там нет таких четких ориентиров для выбора начальной точки (круг – хорошая фигура, потому что он замкнут, а гипербола – плохая, потому что уходит на бесконечность).

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

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

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

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

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

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

Так это выглядит в сборе:

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

Так все выглядит в трюме:

Все родные схемы остались почти без изменений, а все дополнительное оборудование питается от отдельного свинцового АКБ на 1.2 А*ч.

• пульта оператора, в состав которого входит ноутбук с ПО и ответный радиомодуль, подключаемый через преобразователь к USB. Программное обеспечение писалось, что называется, на коленке, и годится только для демонстрации, но в нём уже реализован протокол взаимодействия с модемом, TOA-решатель, на основе того же самого метода Нелдера-Мида, выбор базовых точек, методы отображения географических позиций, а также возможность эмуляции GPS для передачи данных в Google Earth или аналоги.

Синие точки — положения лодки по 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, откуда мы берем текущую географическую позицию.

Самое сложное – передачу данных через воду и измерение дистанции – выполняют модемы, определение географической позиции лодочки – 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 метров, и достаточно сложная акватория.
Маяк-ответчик закреплен на специальном «стенде» (в кадр не попали пенопластовые балберки и якорь, которые помогают всей конструкции держаться вертикально):

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

На транец «Палычу» нейлоновыми стяжками закреплен кусок удочки, на котором и располагается модем RedGTR (чтобы не болтался и не всплывал).

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

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

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

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

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

Послесловие

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

---