两个收发器上的水下GPS

你好亲爱的habuchitel！

我们的水下GPS刚满三岁了。 在这段时间里，该系统变成了系列产品，我们将更多的系统和设备推向市场，但在这段时间里，我一直不遗余力地将水声技术带入大众，使普通爱好者，建模者， 城市疯狂市民科学等方向的代表都可以使用它。其他感兴趣。

今天，为了纪念我们的周年纪念日，我将用手指和手指详细解释如何仅使用两个收发器确定水下物体的地理位置：带有源代码，有趣的图片，图形和实验结果。

我请所有感兴趣的人在伏尔加格勒水库的干净温暖的水中乘皮划艇！



注意：本文中包含痕量的Matan！

对于那些不了解正在发生的事情，但又不介意调查的人们，我建议您熟悉我们之前有关水声和水下航行的出版物：
通过曝光进行水声视频传输的会话
每年从零开始的水下GPS
水下GPS：继续
蓝细菌对总裁言语功能的影响
水下航行：方向寻找-不要方向寻找，您注定要成功
还有akuzmin撰写的有关水声技术的出色评论文章

介绍性

在水下，无线电波不会延伸到任何明显的距离，并且只有借助声学，那里或多或少的体面的无线通信和导航才有可能实现。

导航系统只有两种工作类型：

• 超短基地台（USB，USBL）-基于确定信标信号的到达角度并测量距离（通过请求-响应方法或通过同步小时数）。 到达角使用天线阵列确定。 所需位置由角度和范围确定（直接测地任务）。 例如UKB系统，就是我们的ZIMA
• 远程（DB，LBL）-信号到达时间是在已知位置的几个接收器或几个发射器（我们的水下GPS都这样工作）测量的。 您可以使用“请求-响应”方法（测距仪方法）测量距离，也可以测量信号到达时间的差异（差分测距仪方法），例如GPS和GLONASS。 一个长的基础可以是浮动的（例如RedWAVE ）或底部。
• 短底座（KB，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是目标，B是基础。
所以我们可以在任何时候 $$$M（x，y，z）$ 写下一个表达式，该表达式说明这点如何适合我们的实验测量。 实际上，这将是从点M到相应基站的距离与测量距离之差的总和。 我们将平方每个这种差异。

$$

$$\ epsilon（x，y，z）= \ sum_ {n = 0} ^ N \左（\ sqrt {（x-x _ {{bn}}）^ 2 +（y-y _ {{bn}}）^ 2 +（z-z _ {{bn}}）^ 2}-d _ {{Tbn}} \对）$$

通常，涉及最小二乘法，理想点的坐标应理想地为函数ε的最小值。
如果我们要讨论的是差分测距仪方法，那么我们将在本文中完全不加区别地进行介绍，并且对于每个基站，我们都有一个无法测量的距离 $$$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-搜索从最低点的中点开始就触及了错误的最小值。 三要素基础

此处的搜索是使用Nelder-Mead算法 （也称为单纯形方法）实现的。 例如在Matlab中的fminsearch函数中使用它

它不需要导数的计算，非常直观，直观且易于实现。


图3-从最接近的基本元素开始时，搜索命中最小值。 三要素基础

有见识的人可能会因为没有人将基站挂在网上而感到愤怒，但是，首先，有时他们会这样做，其次，对于更直观的演示来说，这是夸张的。

“失败是完整的。 不可能发明任何东西“（C）A.和B. Strugatsky，“注定的城市”

你能在这里想出什么？

逻辑建议您可以使用两种全局优化方法之一- 模拟退火或类似方法。 但是，另一种（且效率更高的）方法是更充分地使用先验信息来选择搜索起点。

毕竟，我们可以肯定地知道，期望的点位于假设中心的圆圈上（假设的一小部分），这些圆圈的半径对应于测量范围！

而且我们还可以确定，在水声测量范围内的误差会随着该范围而增大：声音会非线性传播。

您可以尝试选择一个起点，以找到最接近的信标周围的最小值（从字面上看，在圆圈上），希望以最大的精度测量距其的距离，并以残差函数ε的相同值作为参考。

在这种情况下，候选点的坐标根据以下简单公式形成：

$$

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

$$$x_ {bc}$ 和 $$$y_ {bc}$ -最近（c-最近）基站的坐标， $$$d_ {bc}$ -从它到已定位对象的距离。 $$$\ alpha$ -参数（角度）。 如果您现在为 $$$\epsilonε$ 变化的 $$$\ alpha$ 在0到360度的范围内，以10度为步长，然后通过将步长减小到1度并将搜索范围缩小到20（±10）度，在此值旁边进行搜索，可以很好地确定初始近似值在所有情况下，它几乎都能立即击中目标！

在下面的图片中，经过30次迭代的“正面”求解方法英勇地降到了一个错误的最小值，这导致了绝对错误的求解，并且采用一维最小化的方法几乎立即达到了目的，并且在6次迭代中，实际位置接近25厘米，相差25厘米。到最近的基站的距离是〜400米。


图4-搜索达到错误的最小值


图5-从初步一维最小化获得的点开始寻找正确的解决方案


图6-所得解决方案周围的放大区域。 黄色圆圈是收到的解决方案，绿色星形是对象的真实位置

在以上示例中，在初步选择初始搜索位置之后的第一次迭代给出了距实际位置20米的解（在现场〜600x600米）。
在这一阶段，我们想出了测距仪的方法，差距测距仪的情况要复杂一些：没有选择起点的明确指导（圆圈是一个好图形，因为它是闭合的，而双曲线则是坏的，因为它达到了无穷大）。
回到承诺的两个收发器的“水下GPS”。 从某种意义上说，我当然很狡猾：作为完善的水下GPS，从我们的第一篇文章中 是不可能的 ，但是，如果您没有钱购买合适的设备，忍受某些限制，尤其是专注于某些任务，那么您仍然可以做一些事情。

经常放置的物体是静止不动的（或至少缓慢移动得微不足道）。 在这种情况下，您实际上可以使用一个基站，而不是几个基站，但可以移动：这种方法称为VLBL或虚拟长基站。 如果我们不需要固定水下物体的运动轨迹，而只需确定其位置，那么这就是我们所需要的。

当通过从船上自由排放安装任何底部设备时，通常会出现此问题。 在有明显的深度和电流的情况下，设备底部的实际位置可能与倾倒点的位置明显不同。 该设备也可能是底部导航基础的元素：要确定水下物体的位置，必须首先确定水下物体的位置。

那么如何在现实世界中安排所有这些呢？

我们的实验设置包括：

• 我们将藏在容器底部的对象：我们的RedGTR代码调制解调器 （独立版本）（实际上，它听起来像是官方的-调制解调器连接到密封的电池盒）； 在照片中：
  
  图7-带电池罐的响应者信标
  
• 一个基于中国诱饵船的移动基站。 除了本地控制填充之外，她还拥有第二台RedGTR调制解调器，带有天线的433 MHz DORJI无线电模块，基于quectel l76的自制GPS / GLONASS模块，基于STM32F429的自己的主板（可以很容易地用NUCLEO-STM429或STM32F4 Discovery替换）。我敢肯定，任何类似Arduino的电路板也能做到这一点），它只是充当路由器的角色：一切都通过UART从无线电通道通过调制解调器进入调制解调器，反之亦然，GPS模块的数据也将流到无线电通道。
  
  因此，它看起来像一个程序集：
  
  
  图8-测试船只组件的照片
  
  这次，我们固定了一种将尾杆与调制解调器连接的横梁，自由牵引的导航员的经验揭示了这种方案的恶性，无论是在可控性方面，还是在设备和船体相对位置的变化方面。
  
  因此，它处于保留状态：
  
  
  图9-测试飞行器的照片。 货舱视图
  
  所有本机电路几乎保持不变，所有其他设备均由单独的铅酸电池供电，电流为1.2 A * h。
  
• 操作员控制台，其中包括一台带软件的笔记本电脑和一个通过USB连接到转换器的响应无线电模块。 该软件当然有点草率，仅适合演示，但是已经基于相同的Nelder-Mead方法，基于基点的选择，地理位置的简单绘制以及模拟GPS进行数据传输的能力，已经实现了与调制解调器交互的协议，TOA求解器。 Google Earth的类似物。
  
  
  图10-“操作员控制台”。 在表上测试运行。
  
  蓝色点表示船只的GPS位置，绿色点表示进行测量的位置，虚拟基本元素的位置由鲑鱼突出显示，并且响应信标的计算位置以红色突出显示。
  

谁在忙什么？

• 您要查找的调制解调器位于最下方，接收并响应请求；
• 船上的调制解调器执行通过UART到达的命令-发送请求和接收响应，如UART所报告
• 船上装有STM32F4的板通过UART接收来自GPS和调制解调器的数据，并通过另一个UART将其重定向到无线电模块，然后将数据从无线电模块重定向到调制解调器。 与无线电模块有关的细微差别-它不是全双工的（就像在水声=中一样），因此，该板“保留”了来自GPS模块的RMC消息，并仅将其与来自调制解调器的消息一起发送；
• 远程控制软件控制整个动作，从不同地理位置收集到目标的距离数据，选择最佳基准并确定目标的位置；

更详细一点

通过类似NMEA的协议与调制解调器进行通信。 对于我们的任务，仅使用三个消息：

$PTNTE,targetAddr,requestedCmd,timeoutMs*hh

从远程到调制解调器。 将requestedCmd Cmd命令发送到位于targetAddr的远程订户，将响应超时设置为timeoutMs。

该命令可以是任何批准的列表：

团队	代号	内容描述
CDS_CMD_PING	0	远程用户应答PONG的PING请求
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

调制解调器支持以下基本功能：ping，传输深度，水温，电池电量和一组用户代码命令。

调制解调器立即回答TNTE命令有关接受命令的事实或无法使用ACK消息执行该命令：

$PTNT0,errCode

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接收机输出端的信噪比，以dB为单位
dpl以赫兹为单位的多普勒频移
pTime信号传播时间（单向），以秒为单位
dst到订户的距离，以米为单位
dpt调制解调器自身的深度（以米为单位）
tmp舷外水温，以摄氏度为单位

仅当调制解调器设计为集成压力/温度传感器（我们的选件）时，才传输最后三个参数。

从GPS模块，我们只对RMC消息感兴趣，我们从哪里获得当前的地理位置（我们可以使用GGA或GLL-最后一个最短的）。

最困难的事情-通过水传输数据并测量距离-由调制解调器完成，确定船的地理位置-GPS模块，只剩下一点点-才能正确使用此数据。

NMEA消息的所有解析和构建都是使用UCNLNMEA库中的NMEAParser类执行的。

便携式计算机上运行的控制台软件会在应用程序设置中指定的地址处定期向订阅者发送REM_PINGEX请求，它会在收到REM_TIMEOUT或REM_PONGEX时执行此REM_PONGEX 。 沿途记录了船的坐标，并存储了与远程订户的距离和深度有关的数据以及接收它们的船的坐标。

常识（和经验）告诉我们，为了确定水下物体的位置，需要3-5个虚拟基站，从上图可以看出，无论如何都不应放置它们。

即，形成以下任务：当接收到测量值时，必须以某种方式选择其中几个作为导航基础的元素。

我提出以下选择：选择这样的测量，以便它们形成最凸的图形，该图形在定位对象的估计位置周围描述。

为此，请确定一个点（在初始阶段，当尚不清楚所定位的对象在哪里时，所有测量的中间点都将出现，然后将其替换为所需对象位置的第一近似值），然后从中计算出方位角（向北的方向与向北的方向之间的角度）点）。 现在，我们将选择一些最均匀覆盖角度范围的结果。 在远程控制中，这是通过以下方法完成的

 List<Measurement> Measurements.GetBase(); 

当数据到达时，在REM_PONGEX REM_PONGEX消息的方法中， REM_PONGEX添加度量并尝试构建导航库并解决导航问题。 该方法对此负责。

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

基座本身被传递给它-我们选择用来解决导航问题的一组测量值，先前的位置（如果存在，则还将目标物体的深度也传递到那里），径向误差阈值和两个诊断输出参数-解决方案第一阶段的径向误差（根据一维优化）和迭代次数，然后在第二阶段获得解决方案。

如果我们满意的解决方案是由于径向误差，那么我们将使用该解决方案作为构建基础的参考点。

因此，在我们累之前，我们将获得使我们满意的解决方案。

顾名思义， LocateLBL_NLM方法使用Nelder-Mead方法解决了该问题，如果没有先前的解决方案，则会预先执行一维优化。 实际上，您可能并不局限于先前的结果，而是不断地进行一维优化-策略可能有所不同，并取决于特定条件。

为简单起见，解决方案以米为单位进行执行，为此，在解决方案开始之前，所有坐标都被传输到本地系统，其原点位于导航基础的中点。 求解后，结果转换回地理经度和纬度（经度为X，纬度为Y）。 为了将度数转换为米，然后返回，Navigation类包含GetDistance2DDeg和Meters2Deg方法。

但是，正如您所知，没有实践的理论已经死了，是时候继续进行现场实验的结果了。

让我们通过实践验证来重振理论

根据古老的传统，我们在Pichuga河口处进行测试。 这是一个方便的地方，深度可达近30米，水域相当复杂。

被告灯塔安装在特殊的柄“架子”上（框架中未包括泡沫泡沫阳台和可帮助整个结构保持直立的锚）：


图11-“底部站”信标响应器

灯塔的安装是通过视频拍摄的，他们没有一点光就被计算了下来，却忘了带上手电筒，所以与地面的接触时刻隐藏在黑暗中。

当他们要进行测试时，他们给船起了个名字：


图12-自然环境中的“ Palych”

在“ Palychu”横梁上，尼龙扎带从鱼竿上固定了一块，RedGTR调制解调器位于该鱼竿上（以免悬挂和弹出）。

实验包含以下事实：被告信标安装在底部，只有一个小浮标从其中伸出，而从海岸控制的“ Palych”则在水域中大步走动，遥控器会定期向被告询问一些东西，例如深度。 所有数据都写入日志，并且根据事物的真实状态将所有内容显示在屏幕上。 像这样：


图13-远程控制软件的屏幕快照，其中包括船的轨迹和响应信标的计算位置（最佳计算位置以红色显示）

从图13可以看出，灯塔位于13.2 m的深度，该深度的水温为24.1°C。


图14-将收到的曲目导入GoogleEarth

在这种情况下，要获得良好的效果需要花费大约15分钟的时间。
在第二个实验中，将灯塔安装在16.5米的深度处，预计会更冷-仅22.6°C。


图15-在远程控制软件的屏幕上确定第二个实验中响应者信标位置的结果

在这种情况下，由于时间紧缺，我们无法正确地评估精度（实际上，这些有趣的测试由于不堪重负而在管理人员之间造成了一些摩擦，因为它们被从工作中撕裂了），并且对船只驶向浮标时感到满意（这一刻可以在第一时间看到） 轨道 ），则其位置与最佳解决方案完全重合，距离与深度重合（响应器与安装在船上的调制解调器之间的深度之差）。 信标被重置的点是固定的，计算位置与排放点之间的差异在2-3米范围内，这进一步确认了系统的可操作性。

总的来说，我们可以说这个概念证明已经成功完成了！
我们获得了真正的喜悦和晒伤，我们将很乐意回答问题并听取批评！

后记

可悲的事实是，尽管按照世界水声通信市场的标准，该实验中使用的调制解调器虽然很便宜，但对于业余爱好者来说仍然不是很方便。

为了避免毫无根据，我将链接到本森小姐的出色著作 ，在PDF的第54页或文档编号的36页上，有一个有趣的图版，标明了截至2010年某些声纳调制解调器的价格。 这样的事情。 （表中有一个小错误-她正在开发的调制解调器的范围通信范围以米为单位，而列的标题为km）。

好消息是，我们突然开发了几乎可以做所有事情的调制解调器（体积更小（实际上，这是世界上最小的，而在它们之前最小的也是我们的-根据本文），通信范围更短了，较低的传输速度），并且根据我们的估算，对于富有的发烧友来说，它们已经令人振奋。 目前，电子设备和固件已完全准备就绪，我们正在完成机箱解决方案。 在开发结束时，我们将发布带有实验验证结果的文章。

注意！ 意见调查：