CN112653992B - 一种无需时钟同步的移动编队相对自定位方法 - Google Patents

Abstract

本发明提供了一种无需时钟同步的移动编队相对自定位方法，其中，移动网络中的每个节点装配有惯性导航设备和深度计，惯性导航设备用于提供移动节点的惯导纬度、惯导经度、东向速度、北向速度参数；深度计用于提供移动节点的深度和垂直速度参数；该方法具体包括：所有节点通过收发KLV数据帧建立通信链路；所有节点通过应答通信，使用粒子滤波与轨迹预测相结合的方法对惯性导航设备的相对定位误差进行校正，从而实现自定位。本发明的方法在不需要时钟同步的前提下，利用水下移动编队中两两节点间的水声通信，结合多项式回归和粒子滤波方法，达到对节点自身惯导设备的相对定位误差进行校正的目的。

Description

一种无需时钟同步的移动编队相对自定位方法

技术领域

本发明属于水下无线传感器网络领域，特别涉及一种无需时钟同步的移动编队相对自定位方法。

背景技术

常见的水下移动节点定位导航技术主要包括四类：捷联惯性导航技术、水下声学定位与导航技术、地球物理及视觉导航技术及水下协同导航技术。其中，捷联惯性导航技术的导航误差随时间累积明显；水下声学定位与导航技术中的多普勒速度计程仪方法容易出现对地失锁现象，水声定位基阵方法需要声信标或其他辅助设备；地球物理及视觉导航技术需要对移动节点所在海域的地形地貌、重力或地磁信息有较为详尽的数据库支撑。水下协同导航技术是在利用惯性导航的数据基础上，通过水下移动节点间的相互应答通信实现相对自定位。目前，水下移动节点定位导航技术多采用以第一类为主、后三类为辅的方法。

常见的水下协同导航技术一般分为并行式和主从式两种。前者的移动编队中所有节点具有同等的地位和配置，因水声通信频带限制；后者一般需要一个定位精度较高的领航节点，和保持一定阵型的随从节点，由该领航节点定时广播自身位置及时间戳，其他节点根据广播信息进行测距以及自定位。因此目前的水下协同导航技术多以主从式为主。

发明内容

本发明针对水下移动节点惯性导航累计误差大的问题，提出了一种无需时钟同步的移动编队相对自定位方法，该方法利用水下移动编队网络通信，通过水下移动节点间少量的水声通信，使用粒子滤波与轨迹预测相结合，在不需要节点间时钟同步的前提下，对移动编队中的各个节点进行相对自定位，一定程度上减小了惯性导航误差。

为实现上述目的，本发明提出了一种无需时钟同步的移动编队相对自定位方法，其中，移动网络中的每个节点装配有惯性导航设备和深度计，惯性导航设备用于提供移动节点的惯导纬度、惯导经度、东向速度、北向速度参数；深度计用于提供移动节点的深度和垂直速度参数；该方法具体包括：

所有节点通过收发KLV数据帧建立通信链路；

所有节点通过应答通信，使用粒子滤波与轨迹预测相结合的方法对惯性导航设备的相对定位误差进行校正，从而实现自定位。

作为上述方法的一种改进，所述方法还包括：移动网络中的每个节点的初始化步骤，具体包括：

为移动网络的每个节点进行编号t，t为整数，t∈[0,T)，T为总节点数量；

设定KLV次数限制K、REQ次数限制Q、粒子数量L、粒子滤波迭代次数上限P、轨迹预测阶数上限G、速度变化上限S、最长REQ发射间隔R；

为每个节点建立空白链路存活表，设定链路存活时间上限C：

对于第t个节点，其链路存活表包含T项，其中的第i项，i∈[0,T)且i≠t，用于记录第i个节点与本节点间通信链路的剩余存活时间，第t项表示本节点自身循环链路，其始终为0表示不需要使用；空白链路存活表中的其余项初始时也都为0，表示不存在有效的通信链路；存活表刷新时刻fsh置为0；

为每个节点建立空白位置记录表、空白速度记录表和空白时间记录表：

对于第t个节点，其位置记录表包含T项，其中的第i项，i∈[0,T)，用于记录通过水声通信接收到的第i个节点的惯导经度、惯导纬度和深度；速度记录表包含T项，其中的第i项，i∈[0,T)，用于记录通过水声通信接收到的第i个节点的东向速度、北向速度和垂直速度；时间记录表包含T项，其中的第i项，i∈[0,T)，用于记录通过水声通信接收到的第i个节点上述位置和速度时由本地时钟给出的时间；

为每个节点建立空白轨迹偏移记录表：

对于第t个节点，其轨迹偏移记录表包含G项，其中的每一项均包含记录时间、本节点经度偏移值、本节点纬度偏移值；G至少为3；

为每个节点清零所有定时器和计数器，清空已设定标志位；

最后一个入水的节点在入水前设定发射KLV定时器的次数为K以及每一次发射KLV的时间，并将已设定标志位置为1；其首次发射KLV时间应不早于节点到达指定深度的时间，后续发射时间与前一次发射KLV时间间隔应不小于2×(T-1)×τ_KLV秒，其中τ_KLV不小于从一个节点开始发射KLV数据帧到另一个节点接收完毕该数据帧所需要的最长时间；

设各个节点到达指定深度的时间为其本地时钟的0时刻；所有节点将上次发射REQ时间reqt赋值为0，等待RPL帧序号wrpl赋值为-1，清空重发REQ标志位；所有节点的发射端进入等待状态，接收端进入等待接收数据状态。

作为上述方法的一种改进，所述所有节点通过收发KLV数据帧建立通信链路；具体包括：

最后一个入水的节点在发射KLV定时器触发时，建立空白KLV数据帧并进行赋值；赋值完毕后通过水声通信发射该KLV数据帧；发射完毕后帧序号计数器加1；如果帧序号计数器未超过KLV次数限制K，则该节点的发射端返回等待状态；如果帧序号计数器超过KLV次数限制K，则清零帧序号计数器，该节点的发射端进入空闲状态，开启查询定时器，每隔时间τ_CHK查询一次是否需要发射REQ数据帧；其它节点接收到最后一个入水的节点发射的KLV数据帧并进行处理；

所述空白KLV数据帧至少包含以下内容：帧类型、源节点、帧序号、惯导纬度、惯导经度、深度、东向速度、北向速度、垂直速度、校验和；其中，帧类型赋值为KLV，帧序号赋值为帧序号计数器当前值，源节点赋值为t，惯导纬度、惯导经度、东向速度、北向速度从惯性导航设备中实时读取，深度和垂直速度从深度计中实时读取，校验和赋值为上述内容的校验结果；记录东向速度、北向速度、垂直速度为报备速度；

每个节点发射KLV数据帧，接收其它节点的KLV数据帧并进行处理。

作为上述方法的一种改进，所述其它节点接收到最后一个入水的节点发射的KLV数据帧并进行处理，具体包括：

当一个节点接收到最后一个入水的节点发射的KLV数据帧时，进行数据校验：在校验不正确的情况下，丢弃该数据帧，不做任何处理，接收端返回等待接收数据状态；校验正确的情况下，则更新该节点的链路存活表：

读取源节点s、帧序号n、惯导纬度lati、惯导经度long、深度alti、东向速度vest、北向速度vnth、垂直速度vdpt；获取本地时钟当前值cur；将lati、long、alti依次记录到位置记录表的第s项中，将vest、vnth、vdpt依次记录到速度记录表的第s项中，时间记录表的第s项赋值为cur；遍历链路存活表，如果其中第i项的取值大于0，i∈[0,T)，则将其减去(cur-fsh)，如果减后不大于0，则将其赋值为0；将链路存活表中的第s项赋值为C；将fsh赋值为cur；然后该节点设定KLV发射次数与时间：

如果已设定标志位为1，则不做任何处理，该节点的接收端返回等待接收数据状态；如果已设定标志位为空，则将帧序号计数器赋值为n，设定KLV发射次数为(K-n)并设定每一次发射KLV的时间，并将已设定标志位置为1；本节点首次发射KLV时间为mod((t-s-1+T),T)×τ_KLV秒之后，后续发射时间与前一次发射KLV时间间隔应不小于2×(T-1)×τ_KLV秒；设定完毕后，该节点的接收端返回等待接收数据状态。

作为上述方法的一种改进，所述每个节点发射KLV数据帧，接收其它节点的KLV数据帧并进行处理；具体包括：

任一节点发射KLV数据帧：

该节点在发射KLV定时器触发时，建立空白KLV数据帧；赋值完毕后发射该KLV数据帧；发射完毕后帧序号计数器加1；如果帧序号计数器未超过KLV次数限制K，则返回等待状态，等待下次发射KLV定时器触发；如果帧序号计数器超过KLV次数限制K，则清零帧序号计数器，该节点的发射端进入空闲状态，开启查询定时器，每隔时间τ_CHK查询一次是否需要发射REQ数据帧；

任一节点接收KLV数据帧：

该节点在收到KLV数据帧时先进行数据校验；在校验不正确的情况下，丢弃该数据帧，不做任何处理，接收端返回等待接收数据状态；校验正确的情况下，则根据KLV数据帧更新链路存活表：

读取源节点s、帧序号n、惯导纬度lati、惯导经度long、深度alti、东向速度vest、北向速度vnth、垂直速度vdpt；获取本地时钟当前值cur；将lati、long、alti依次记录到位置记录表的第s项中，将vest、vnth、vdpt依次记录到速度记录表的第s项中，时间记录表的第s项赋值为cur；遍历链路存活表，如果其中第i项的取值大于0，i∈[0,T)，则将其减去(cur-fsh)，如果减后不大于0，则将其赋值为0；将链路存活表中的第s项赋值为C；将fsh赋值为cur；然后设定KLV发射次数与时间：

如果已设定标志位为1，则不做任何处理，该节点的接收端返回等待接收数据状态；如果已设定标志位为空，则将帧序号计数器赋值为n，设定KLV发射次数为(K-n)以及每一次发射KLV的时间，并将已设定标志位置为1；首次发射KLV时间为mod((t-s-1+T),T)×τ_KLV秒之后，后续发射时间与前一次发射KLV时间间隔不小于2×(T-1)×τ_KLV秒；设定完毕后，该节点的接收端返回等待接收数据状态。

作为上述方法的一种改进，所述所有节点通过应答通信，使用粒子滤波与轨迹预测相结合的方法对惯性导航设备的相对定位误差进行校正，从而实现自定位，具体包括：

每个节点在查询定时器触发时，当满足一定的条件下，生成并发射REQ数据帧；

每个节点接收REQ数据帧并进行处理，生成和发射RPL数据帧；

每个节点在等待RPL定时器触发时进行处理；

每个节点接收RPL数据帧，使用粒子滤波与轨迹预测相结合的方法对惯性导航设备的相对定位误差进行校正，从而实现自定位。

作为上述方法的一种改进，所述每个节点在查询定时器触发时，当满足一定的条件下，生成并发射REQ数据帧；具体包括：

任一节点的查询定时器触发时，当存在以下五种情况之一时，则该节点的发射端生成并发射REQ数据帧；否则发射端返回空闲状态；五种情况分别为：

情况一：惯导设备的东向速度、北向速度和深度计的垂直速度与报备速度中的对应记录数值的差的模值超过S时；

情况二：链路存活表中所有项均为0时；

情况三：链路存活表中仅有一项不为0，记该项序号为i，i∈[0,T)且i≠t，通过位置记录表的第i项、速度记录表的第i项、时间记录表的第i项和当前时间估算其与本节点的距离已经大于水声通信有效作用距离的0.8倍时；

情况四：本节点当前时间cur与reqt的差已经超过R时；

情况五：重发REQ标志位为1时；

该节点的发射端生成并发射REQ数据帧的过程为：

建立空白REQ数据帧；空白REQ数据帧至少包含以下内容：帧类型、源节点、帧序号、惯导纬度、惯导经度、深度、东向速度、北向速度、垂直速度、校验和；其中，帧类型赋值为REQ，帧序号赋值为帧序号计数器当前值，源节点赋值为t，惯导纬度、惯导经度、东向速度、北向速度从惯性导航设备中实时读取，深度和垂直速度从深度计中实时读取，校验和赋值为上述内容的校验结果；记录东向速度、北向速度、垂直速度为报备速度；赋值完毕后通过水声通信发射该REQ数据帧；发射完毕后，reqt赋值为本地时钟的当前时间，wrpl记为帧序号计数器当前值，帧序号计数器加1；停止查询定时器，开启等待RPL定时器，定时时长不小于τ_REQ+T×τ_RPL秒，其中τ_REQ不小于REQ数据帧从一个节点开始发射到另一个节点接收完毕所需要的最长时间，τ_RPL不小于RPL数据帧从一个节点开始发射到另一个节点接收完毕所需要的最长时间。

作为上述方法的一种改进，所述每个节点接收REQ数据帧并进行处理，生成和发射RPL数据帧；具体包括：

任一节点在接收到REQ数据帧时先进行数据校验；在校验不正确的情况下，丢弃该数据帧，不做任何处理，该节点的接收端返回等待接收数据状态；校验正确的情况下，如果此时已设定标志位仍为空，则将已设定标志位置为1，更新链路存活表：

读取源节点s、帧序号n、惯导纬度lati、惯导经度long、深度alti、东向速度vest、北向速度vnth、垂直速度vdpt；获取本节点当前时钟cur；将lati、long、alti依次记录到位置记录表的第s项中，将vest、vnth、vdpt依次记录到速度记录表的第s项中，时间记录表的第s项赋值为cur；遍历链路存活表，如果其中第i项的取值大于0，i∈[0,T)，则将其减去(cur-fsh)，如果减后不大于0，则将其赋值为0；将fsh赋值为cur；将链路存活表中的第s项赋值为C；然后建立空白RPL数据帧：

空白RPL数据帧至少包含以下内容：帧类型、源节点、目的节点、帧序号、回复延迟、惯导纬度、惯导经度、深度、东向速度、北向速度、垂直速度和校验和；其中，帧类型赋值为RPL，源节点赋值为t，目的节点赋值为s，帧序号赋值为n，回复延迟赋值为(t+1)×τ_RPL，惯导纬度、惯导经度、东向速度、北向速度从惯性导航设备中实时读取，深度和垂直速度从深度计中实时读取，校验和赋值为上述内容的校验结果；

赋值完毕后延迟(t+1)×τ_RPL秒再通过水声通信发射该RPL数据帧；记录东向速度、北向速度、垂直速度为报备速度；该节点的接收端返回等待接收数据状态。

作为上述方法的一种改进，所述每个节点在等待RPL定时器触发时进行处理；具体包括：

停止等待RPL定时器，开启查询定时器；如果帧序号计数器取值不大于Q，则将重发REQ标志位置为1，发射端返回空闲状态；如果帧序号计数器取值大于Q，则清空重发REQ标志位，清零帧序号计数器，发射端返回空闲状态。

作为上述方法的一种改进，所述每个节点接收RPL数据帧，使用粒子滤波与轨迹预测相结合的方法对惯性导航设备的相对定位误差进行校正，从而实现自定位，具体包括：

任一节点在收到RPL数据帧时先进行数据校验；在校验不正确的情况下，丢弃该数据帧，不做任何处理，接收端返回等待接收数据状态；校验正确的情况下，更新链路存活表：

读取源节点s、目的节点d、回复延迟τ、帧序号n、惯导纬度lati、惯导经度long、深度alti、东向速度vest、北向速度vnth、垂直速度vdpt；获取本地时钟当前值cur；将lati、long、alti依次记录到位置记录表的第s项中，将vest、vnth、vdpt依次记录到速度记录表的第s项中，时间记录表的第s项赋值为cur；遍历链路存活表，如果其中第i项的取值大于0，i∈[0,T)，则将其减去(cur-fsh)，如果减后不大于0，则将其赋值为0；将链路存活表中的第s项赋值为C；将fsh赋值为cur；

如果d不等于t或者n不等于wrpl，则丢弃该RPL数据帧，不做任何处理；否则，清空重发REQ标志位，清零帧序号计数器，取消等待RPL定时器，开启查询定时器；将粒子滤波迭代次数p赋值为0；粒子滤波坐标经度初始值JI赋值为惯导经度JS，粒子滤波坐标纬度初始值WI赋值为惯导纬度WS；然后计算参考点2的纬度W2、经度J2：

W2＝W1–(vnth×τ–NS×(cur–reqt))/110945.8

J2＝J1–(vest×τ–ES×(cur–reqt))/11319.5/cos(W2×π/180)

其中，W1为参考点1的纬度；J1为参考点1的经度；NS为本节点惯导北向速度、ES为本节点惯导东向速度、π为圆周率；参考距离r0赋值为参考点1和参考点2间的距离；按照下式计算测量距离dis：

dis＝c×(cur–req–τ–PKL_REQ–PKL_RPL)

其中，c为水中声速、PKL_REQ为REQ数据帧时间长度、PKL_RPL为RPL数据帧时间长度；

利用多项式回归方法，进行轨迹预测：

根据轨迹偏移记录表估计出本节点当前的经度偏移阶数JO及经度偏移率JF_m、纬度偏移率WO及纬度偏移阶数WF_n，m∈[0,JO)、n∈[0,WO)；其中，JO为拟合经度偏移值时所使用的多项式阶数，JF_m为本节点经度偏移值随时间变化的第m阶多项式系数，WO为拟合纬度偏移值时所使用的多项式阶数，WF_n为本节点的纬度偏移值随时间变化的第n阶多项式系数；如果轨迹偏移记录表为空或者多项式回归时出现零除现象，则本节点当前的经度偏移率、纬度偏移率均为0；

使用粒子滤波的方法，进行相对位置矫正；

计算第l，l∈[0,L)个粒子(JE_l,WE_l)；根据JO、JF_m、当前时间cur和JI计算出第l个粒子的经度JE_l；根据WO、WF_n、当前时间cur和WI计算出第l个粒子的纬度WE_l，l∈[0,L)；

JE_l＝JI+α_l×(Σ_m JF_m×cur^m+δ_l)/4^p,m∈[0,JO)

WE_l＝WI+β_l×(Σ_n WF_n×curⁿ+ε_l)/4^p,n∈[0,WO)

其中，α_l、β_l均为绝对值不大于2的随机实数、δ_l、ε_l均为绝对值不大于0.001的随机实数；

然后，量测距离r1_l赋值为粒子(JE_l,WE_l)与参考点1的间距，量测距离r2_l赋值为粒子(JE_l,WE_l)与参考点2的间距；当r1_l与r2_l差的绝对值不小于r0时，粒子(JE_l,WE_l)的wei_l赋值为0；否则计算权重wei_l，其计算原则为：wei_l≥0且r1_l+r2_l与dis差的绝对值越小wei_l越大；根据wei_l对L个粒子进行重采样，重采样原则为：在粒子数量保持L不变的前提下，wei_l越大的粒子的后代粒子越多，wei_l越小的粒子的后代粒子越少甚至被抛弃；JI赋值为重采样后得到的所有L个JE_l’的均值，WI赋值为重采样后得到所有L个的WE_l’的均值；将p加1；如果此时p未超过P，则重复本步骤继续使用粒子滤波进行相对位置矫正；如果此时p超过P，则结束粒子滤波；

更新轨迹偏移记录表：

逐次递增的将轨迹偏移记录表的第g+1项元素值对应的赋值给第g项元素值，其中g∈[0,G-1)；轨迹偏移记录表的第G-1项元素值中记录时间赋值为本节点当前时间cur、本节点经度偏移值赋值为JS-JI、本节点纬度偏移值赋值为WS-WI；再将JS赋值为JI，WS赋值为WI。

本发明的优势在于：

本发明的方法在不需要时钟同步的前提下，利用水下移动编队中两两节点间的水声通信，结合多项式回归和粒子滤波方法，达到对节点自身惯导设备的相对定位误差进行校正的目的。

附图说明

图1为本发明的无需时钟同步的移动编队相对自定位方法的示意图；

图2为本发明的粒子滤波方法示意图；

图3为OPNET仿真中的节点模型；

图4为软件仿真中采用的2节点直线运动轨迹图；

图5为软件仿真中采用的3节点直线运动轨迹图；

图6为软件仿真中采用的2节点圆周运动轨迹图；

图7为软件仿真中采用的3节点圆周运动轨迹图；

图8(a)为2节点中第一个节点的圆周运动时校正情况对比图(0.15海里/小时)；

图8(b)为2节点中第二个节点的圆周运动时校正情况对比图(0.15海里/小时)；

图9(a)为3节点中第一个节点的直线运动时校正情况对比图(0.25海里/小时)；

图9(b)为3节点中第二个节点的直线运动时校正情况对比图(0.25海里/小时)；

图9(c)为3节点中第三个节点的直线运动时校正情况对比图(0.25海里/小时)。

具体实施方式

下面结合附图和具体实施例对本发明的技术方案进行详细的说明。

本发明提出了一种无需时钟同步的移动编队相对自定位方法，是在移动编队运动期间，通过水声网络通信，使用粒子滤波与轨迹预测相结合的方法，在不需要进行节点间时钟同步的前提下对移动编队中的各个节点进行相对自定位，

如图1所示，本发明的方法包括以下步骤：

步骤1)初始化

1-1)、设定网络包含的总节点数量T及本节点地址t

本节点地址t为整数，t∈[0,T)，且应保证不同节点的地址t在网络中是唯一的；

初始化完毕后，t与T均不能修改；

1-2)、设定KLV次数限制K、REQ次数限制Q、粒子数量L、粒子滤波迭代次数上限P、轨迹预测阶数上限G、速度变化上限S、最长REQ发射间隔R

初始化完毕后，K、Q、L、P、G、S和R不能修改；

1-3)、建立空白链路存活表，设定链路存活时间上限C

链路存活表包含T项，其中的第i项，i∈[0,T)且i≠t，用于记录第i个节点与本节点间通信链路的剩余存活时间，第t项表示本节点自身循环链路，其始终为0表示不需要使用；空白链路存活表中的其余项初始时也都为0，表示不存在有效的通信链路；存活表刷新时刻fsh置为0；

初始化完毕后，C不能修改；

1-4)、建立空白位置记录表、空白速度记录表和空白时间记录表

位置记录表包含T项，其中的第i项，i∈[0,T)，用于记录通过水声通信接收到的第i个节点的惯导经度、惯导纬度和深度；速度记录表包含T项，其中的第i项，i∈[0,T)，用于记录通过水声通信接收到的第i个节点的东向速度、北向速度和垂直速度；时间记录表包含T项，其中的第i项，i∈[0,T)，用于记录通过水声通信接收到的第i个节点上述位置和速度时由本地时钟给出的时间；

1-5)、建立空白轨迹偏移记录表

轨迹偏移记录表包含G项，其中的每一项均包含记录时间、本节点经度偏移值、本节点纬度偏移值；G至少为3且初始化后不能修改；

1-6)、每个节点装配有惯性导航设备和深度计

惯性导航设备应提供移动节点的惯导纬度、惯导经度、东向速度、北向速度参数；深度计应提供移动节点的深度和垂直速度参数；每个节点入水之前获取其GPS真实值，包括经度、纬度，并将GPS真实值作为该节点惯性导航设备的经度、纬度初始值；深度初始值为0，东向速度、北向速度、垂直速度的初始值也均为0；

1-7)、清零所有定时器和计数器，清空已设定标志位

清零后，最后一个入水的节点在入水前设定发射KLV定时器的次数为K以及每一次发射KLV的时间，并将已设定标志位置为1；其首次发射KLV时间应不早于节点到达指定深度的时间，后续发射时间与前一次发射KLV时间间隔应不小于2×(T-1)×τ_KLV秒，其中τ_KLV不小于从一个节点开始发射KLV数据帧到另一个节点接收完毕该数据帧所需要的最长时间；为方便表述，设各个节点到达指定深度的时间为其本地时钟的0时刻；所有节点将上次发射REQ时间reqt赋值为0，等待RPL帧序号wrpl赋值为-1，清空重发REQ标志位；所有节点的发射端进入等待状态，接收端进入等待接收数据状态；

步骤2)通过收发KLV数据帧建立通信链路

2-1)、最后一个入水的节点发射KLV

最后一个节点在发射KLV定时器触发时，建立空白KLV数据帧；空白KLV数据帧至少包含以下内容：帧类型、源节点、帧序号、惯导纬度、惯导经度、深度、东向速度、北向速度、垂直速度、校验和；其中，帧类型赋值为KLV，帧序号赋值为帧序号计数器当前值，源节点赋值为t，惯导纬度、惯导经度、东向速度、北向速度从惯性导航设备中实时读取，深度和垂直速度从深度计中实时读取，校验和赋值为上述内容的校验结果；记录东向速度、北向速度、垂直速度为报备速度；赋值完毕后通过水声通信发射该KLV数据帧；发射完毕后帧序号计数器加1；如果帧序号计数器未超过KLV次数限制K，则发射端返回等待状态；如果帧序号计数器超过KLV次数限制K，则清零帧序号计数器，发射端进入空闲状态，开启查询定时器，每隔时间τ_CHK查询一次是否需要发射REQ数据帧；

2-2)、其余节点处理接收到的KLV数据帧

a)进行数据校验

在校验不正确的情况下，丢弃该数据帧，不做任何处理，接收端返回等待接收数据状态；校验正确的情况下，转入步骤b)；

b)更新链路存活表

读取源节点s、帧序号n、惯导纬度lati、惯导经度long、深度alti、东向速度vest、北向速度vnth、垂直速度vdpt；获取本地时钟当前值cur；将lati、long、alti依次记录到位置记录表的第s项中，将vest、vnth、vdpt依次记录到速度记录表的第s项中，时间记录表的第s项赋值为cur；遍历链路存活表，如果其中第i项的取值大于0，i∈[0,T)，则将其减去(cur-fsh)，如果减后不大于0，则将其赋值为0；将链路存活表中的第s项赋值为C；将fsh赋值为cur；转入步骤c)；

c)设定KLV发射次数与时间

如果已设定标志位为1，则不做任何处理，接收端返回等待接收数据状态；如果已设定标志位为空，则将帧序号计数器赋值为n，设定KLV发射次数为(K-n)并设定每一次发射KLV的时间，并将已设定标志位置为1；首次发射KLV时间为mod((t-s-1+T),T)×τ_KLV秒之后，后续发射时间与前一次发射KLV时间间隔应不小于2×(T-1)×τ_KLV秒；设定完毕后，接收端返回等待接收数据状态；

2-3)、所有节点轮流收发KLV数据帧

a)任意节点发射KLV数据帧

任意节点在发射KLV定时器触发时，建立空白KLV数据帧；空白KLV数据帧至少包含以下内容：帧类型、源节点、帧序号、惯导纬度、惯导经度、深度、东向速度、北向速度、垂直速度、校验和；其中，帧类型赋值为KLV，帧序号赋值为帧序号计数器当前值，源节点赋值为t，惯导纬度、惯导经度、东向速度、北向速度从惯性导航设备中实时读取，深度和垂直速度从深度计中实时读取，校验和赋值为上述内容的校验结果；记录东向速度、北向速度、垂直速度为报备速度；赋值完毕后通过水声通信发射该KLV数据帧；发射完毕后帧序号计数器加1；如果帧序号计数器未超过KLV次数限制K，则返回等待状态，等待下次发射KLV定时器触发；如果帧序号计数器超过KLV次数限制K，则清零帧序号计数器，发射端进入空闲状态，开启查询定时器，每隔时间τ_CHK查询一次是否需要发射REQ数据帧；

b)任意节点处理接收到的KLV数据帧

任意节点在收到KLV数据帧时先进行数据校验；在校验不正确的情况下，丢弃该数据帧，不做任何处理，接收端返回等待接收数据状态；校验正确的情况下，则转入步骤c)；

c)根据KLV数据帧更新链路存活表

读取源节点s、帧序号n、惯导纬度lati、惯导经度long、深度alti、东向速度vest、北向速度vnth、垂直速度vdpt；获取本地时钟当前值cur；将lati、long、alti依次记录到位置记录表的第s项中，将vest、vnth、vdpt依次记录到速度记录表的第s项中，时间记录表的第s项赋值为cur；遍历链路存活表，如果其中第i项的取值大于0，i∈[0,T)，则将其减去(cur-fsh)，如果减后不大于0，则将其赋值为0；将链路存活表中的第s项赋值为C；将fsh赋值为cur；转入步骤d)；

d)设定KLV发射次数与时间

如果已设定标志位为1，则不做任何处理，接收端返回等待接收数据状态；如果已设定标志位为空，则将帧序号计数器赋值为n，设定KLV发射次数为(K-n)以及每一次发射KLV的时间，并将已设定标志位置为1；首次发射KLV时间为mod((t-s-1+T),T)×τ_KLV秒之后，后续发射时间与前一次发射KLV时间间隔不小于2×(T-1)×τ_KLV秒；设定完毕后，接收端返回等待接收数据状态；

步骤3)所有节点通过应答通信进行相对自定位

3-1)、任意节点在查询定时器触发时的处理

存在以下五种情况之一的，则发射端生成并发射REQ数据帧；否则发射端返回空闲状态；

惯导设备的东向速度、北向速度和深度计的垂直速度与报备速度中的对应记录数值的差的模值超过S时；

链路存活表中所有项均为0时；

链路存活表中仅有一项不为0，记该项序号为i，i∈[0,T)且i≠t，通过位置记录表的第i项、速度记录表的第i项、时间记录表的第i项和当前时间估算其与本节点的距离已经大于水声通信有效作用距离的0.8倍时；

本节点当前时间cur与reqt的差已经超过R时；

重发REQ标志位为1时；

所述发射端生成并发射REQ数据帧，具体包括：

建立空白REQ数据帧；空白REQ数据帧至少包含以下内容：帧类型、源节点、帧序号、惯导纬度、惯导经度、深度、东向速度、北向速度、垂直速度、校验和；其中，帧类型赋值为REQ，帧序号赋值为帧序号计数器当前值，源节点赋值为t，惯导纬度、惯导经度、东向速度、北向速度从惯性导航设备中实时读取，深度和垂直速度从深度计中实时读取，校验和赋值为上述内容的校验结果；记录东向速度、北向速度、垂直速度为报备速度；赋值完毕后通过水声通信发射该REQ数据帧；发射完毕后，reqt赋值为本地时钟的当前时间，wrpl记为帧序号计数器当前值，帧序号计数器加1；停止查询定时器，开启等待RPL定时器，定时时长不小于τ_REQ+T×τ_RPL秒，其中τ_REQ不小于REQ数据帧从一个节点开始发射到另一个节点接收完毕所需要的最长时间，τ_RPL不小于RPL数据帧从一个节点开始发射到另一个节点接收完毕所需要的最长时间；

2)、任意节点接收到REQ数据帧时的处理

a)校验REQ数据帧

在接收到REQ数据帧时先进行数据校验；在校验不正确的情况下，丢弃该数据帧，不做任何处理，接收端返回等待接收数据状态；校验正确的情况下，如果此时已设定标志位仍为空，则将已设定标志位置为1，转入步骤b)；

a)更新链路存活表

读取源节点s、帧序号n、惯导纬度lati、惯导经度long、深度alti、东向速度vest、北向速度vnth、垂直速度vdpt；获取本节点当前时钟cur；将lati、long、alti依次记录到位置记录表的第s项中，将vest、vnth、vdpt依次记录到速度记录表的第s项中，时间记录表的第s项赋值为cur；遍历链路存活表，如果其中第i项的取值大于0，i∈[0,T)，则将其减去(cur-fsh)，如果减后不大于0，则将其赋值为0；将fsh赋值为cur；将链路存活表中的第s项赋值为C；转入步骤c)；

c)回复RPL数据帧

建立空白RPL数据帧；空白RPL数据帧至少包含以下内容：帧类型、源节点、目的节点、帧序号、回复延迟、惯导纬度、惯导经度、深度、东向速度、北向速度、垂直速度、校验和；其中，帧类型赋值为RPL，源节点赋值为t，目的节点赋值为s，帧序号赋值为n，回复延迟赋值为(t+1)×τ_RPL，惯导纬度、惯导经度、东向速度、北向速度从惯性导航设备中实时读取，深度和垂直速度从深度计中实时读取，校验和赋值为上述内容的校验结果；赋值完毕后延迟(t+1)×τ_RPL秒再通过水声通信发射该RPL数据帧；记录东向速度、北向速度、垂直速度为报备速度；接收端返回等待接收数据状态；

3)、任意节点在等待RPL定时器触发时的处理

停止等待RPL定时器，开启查询定时器；如果帧序号计数器取值不大于Q，则将重发REQ标志位置为1，发射端返回空闲状态；如果帧序号计数器取值大于Q，则清空重发REQ标志位，清零帧序号计数器，发射端返回空闲状态；

4)、任意节点在接收到RPL数据帧时的处理

a)校验RPL数据帧

在收到RPL数据帧时先进行数据校验；在校验不正确的情况下，丢弃该数据帧，不做任何处理，接收端返回等待接收数据状态；校验正确的情况下，转入步骤b)；

b)更新链路存活表

读取源节点s、目的节点d、回复延迟τ、帧序号n、惯导纬度lati、惯导经度long、深度alti、东向速度vest、北向速度vnth、垂直速度vdpt；获取本地时钟当前值cur；将lati、long、alti依次记录到位置记录表的第s项中，将vest、vnth、vdpt依次记录到速度记录表的第s项中，时间记录表的第s项赋值为cur；遍历链路存活表，如果其中第i项的取值大于0，i∈[0,T)，则将其减去(cur-fsh)，如果减后不大于0，则将其赋值为0；将链路存活表中的第s项赋值为C；将fsh赋值为cur；

c)如果d不等于t或者n不等于wrpl，则丢弃该RPL数据帧，不做任何处理；否则，清空重发REQ标志位，清零帧序号计数器，取消等待RPL定时器，开启查询定时器；将粒子滤波迭代次数p赋值为0；粒子滤波坐标经度初始值JI赋值为惯导经度JS，粒子滤波坐标纬度初始值WI赋值为惯导纬度WS；转入步骤d)；

d)参考点1的纬度W1赋值为lati、经度J1赋值为long；按下式计算参考点2的纬度W2、经度J2：

W2＝W1–(vnth×τ–NS×(cur–reqt))/110945.8

J2＝J1–(vest×τ–ES×(cur–reqt))/11319.5/cos(W2×π/180)

其中，NS为本节点惯导北向速度、ES为本节点惯导东向速度、π为圆周率；参考距离r0赋值为参考点1和参考点2间的距离；按照下式计算测量距离dis：

dis＝c×(cur–req–τ–PKL_REQ–PKL_RPL)

其中，c为水中声速、PKL_REQ为REQ数据帧时间长度、PKL_RPL为RPL数据帧时间长度；转入步骤e)；

e)利用多项式回归方法，进行轨迹预测

根据轨迹偏移记录表估计出本节点当前的经度偏移阶数JO及经度偏移率JF_m、纬度偏移率WO及纬度偏移阶数WF_n，m∈[0,JO)、n∈[0,WO)；其中，JO为拟合经度偏移值时所使用的多项式阶数，JF_m为本节点经度偏移值随时间变化的第m阶多项式系数，WO为拟合纬度偏移值时所使用的多项式阶数，WF_n为本节点纬度偏移值随时间变化的第n阶多项式系数；以经度偏移率为例，确定JO的原则为：从2开始逐阶增加多项式阶数直到G-1，令JO等于使得估计出的经度偏移值最小的多项式阶数，JF_m也取为多项式阶数为JO时对应项的系数；另外，考虑到深度计读取的深度值相对误差较小，暂不对深度值进行多项式回归预测；如果轨迹偏移记录表为空或者多项式回归时出现零除现象，则本节点当前的经度偏移率、纬度偏移率均为0；转入步骤f)；

f)使用粒子滤波的方法，进行相对位置矫正

粒子滤波方法的原理示意图为图2；

计算第l，l∈[0,L)个粒子(JE_l,WE_l)；根据JO、JF_m、当前时间cur和JI计算出第l个粒子的经度JE_l；根据WO、WF_n、当前时间cur和WI计算出第l个粒子的纬度WE_l，l∈[0,L)；

JE_l＝JI+α_l×(Σ_m JF_m×cur^m+δ_l)/4^p,m∈[0,JO)

WE_l＝WI+β_l×(Σ_n WF_n×curⁿ+ε_l)/4^p,n∈[0,WO)

其中，α_l、β_l均为绝对值不大于2的随机实数、δ_l、ε_l均为绝对值不大于0.001的随机实数；

然后，量测距离r1_l赋值为粒子(JE_l,WE_l)与参考点1的间距，量测距离r2_l赋值为粒子(JE_l,WE_l)与参考点2的间距；当r1_l与r2_l差的绝对值不小于r0时，粒子(JE_l,WE_l)的wei_l赋值为0；否则计算权重wei_l，其计算原则为：wei_l≥0且r1_l+r2_l与dis差的绝对值越小wei_l越大；根据wei_l对L个粒子进行重采样，重采样原则为：在粒子数量保持L不变的前提下，wei_l越大的粒子的后代粒子越多，wei_l越小的粒子的后代粒子越少甚至被抛弃；JI赋值为重采样后得到的JE_l’的均值，WI赋值为重采样后得到的WE_l’的均值；将p加1；如果此时p未超过P，则重复本步骤继续使用粒子滤波进行相对位置矫正；如果此时p超过P，则结束粒子滤波，转入步骤g)；

g)更新轨迹偏移记录表

逐次递增的将轨迹偏移记录表的第g+1项元素值对应的赋值给第g项元素值，其中g∈[0,G-1)；轨迹偏移记录表的第G-1项元素值中记录时间赋值为本节点当前时间cur、本节点经度偏移值赋值为JS-JI、本节点纬度偏移值赋值为WS-WI；再将JS赋值为JI，WS赋值为WI；

步骤4)所有节点重复步骤3)，直至电量耗尽或者外因终止时结束。

本方法利用OPNET软件，针对条件比较恶劣的水声传感器网络进行仿真，并设计半双工的水声通信方式。

一、半双工水声通信设计

OPNET中，将无线链路分为14个管道模型阶段进行建模。需要在节点模型中设定发射机与接收机的中心频率、带宽、调制方式、传输速率、纠错编码。OPNET中默认无线链路采用全双工方式，而目前大多数水声通信只能采用半双工方式。

1、管道模型阶段

仿真中，将水声通信参数设置为：BPSK调制、半双工方式、中心频率6kHz、带宽4kHz、通信速率1kbps、有效距离5km。具体到水声通信中，还需要注意的管道模型阶段有如下几点：

(1)第6阶段中，水下声波传播速度为1500m/s，与默认电磁波传播速度不同，需要修改。

(2)第8阶段需要根据水声传播衰减模型重新编写代码计算接收功率。本方法的仿真中采用了经典的Marsh-Schulkin模型。

(3)实际水声通信中，一旦多个接收信号之间发生碰撞，这些接收信号都无法正确接收。因此在第9阶段的噪声串扰时，放大了信号间噪声串扰的影响。

(4)第10阶段中应计算海洋背景噪声级，仿真中采用经典的经验公式将其分为四类进行计算。

(5)通过调整发射功率等参数，使得第11阶段在没有碰撞时的误码率在10^－3左右，而发生碰撞时误码率在10^－1量级。

2、半双工设计

前已叙及，OPNET中使用的无线链路均为全双工通信方式，不论同一节点的发射机与接收机是否设置为相同中心频率以及带宽。因此，需要自行设定半双工通信方式。

OPNET仿真中设计的节点模型如图3所示。图3中Source为数据来源，可以视为传感器。在Router中实现上述的方法。Transmitter为发射换能器。Receiver为接收水听器。实线表示数据帧流向。虚线为统计中断线，一根为接收信号上升沿触发中断，标志接收信号起始，另一根为接收信号下降沿触发中断，标志接收信号结束。当发射换能器处于发射状态时，忽略接收信号的上升沿及下降沿触发中断，并丢弃所有接收到的数据帧。反之，设置水听器接收状态标志，将待发射信号延迟到下降沿触发中断之后再发送。

需要补充的是：在无误码的情况下，数据帧成功接收时的流中断(由Receiver指向Router的数据帧流向线产生)与接收信号下降沿中断是同时产生的，取其一即可。但在因为误码而丢弃数据帧的情况下，不会产生流中断，但还是会产生下降沿中断。因此必须使用两条统计中短线来设置和清空接收水听器的接收状态标志位。

二、节点运动轨迹及误差设计

对于节点匀速直线运动或者匀速折线运动的情况，可以直接使用OPNET中对于移动节点轨迹定义命令，并在节点属性中添加相应的轨迹文件即可。但是对于节点曲线运动或者变速运动的情况，使用轨迹文件便不方便。

此时可以使用op_ima_obj_pos_get()和op_ima_obj_pos_set_geocentric()函数来获取和设置节点坐标。首先，设计好节点的真实运动轨迹函数，并使用op_ima_obj_pos_get()和op_ima_obj_pos_set_geocentric()函数设定节点的真实位置，而且这个真实位置在仿真过程中，节点自身是无法获悉的。然后根据惯性导航设备的误差模型以及深度计的误差模型给真实位置添加误差，并将其结果作为惯性导航设备及深度计的输出结果。根据文献记载，不同条件下惯性导航设备精度差别较大，仅有极为精准的惯性导航设备精度在恒温条件下才能达到0.1海里/小时。

三、仿真设置及流程

以2节点的场景为例，节点运动轨迹与仿真设置及流程无关，因此可以不考虑运动轨迹。KLV次数限制K和REQ次数限制Q均设置为3，粒子数量L设置为1024，粒子滤波迭代次数上线P设置为5，轨迹预测结束上限G设置为4，速度变化上限S设置为0.1，最长REQ发射时间间隔R设置为1800秒，链路存活时间上限C设置为600秒。1号节点先布放，0号节点后布放。在仿真中选用的惯性导航误差模型数值上限为0.25海里/小时和0.15海里/小时两种，节点运动轨迹为直线和圆周两种，仿真时长为16小时。

设0号节点到达指定深度的时间为0时刻，并设定在0.3秒、16.8秒和33.4秒时发送KLV数据帧。0.3秒时，0号节点发射KLV数据帧。1号节点接收到该数据帧并校验正确后，设定在5.2秒、21.8秒和38.3秒时发送KLV数据帧。两个节点分别在设定时间发送KLV数据帧后，进入空闲状态。此时0号节点链路存活表的第1项和1号节点链路存活表的第0项均约为600。

约640秒时，1号节点因检测到链路存活表中第0项不大于0开始发射REQ数据帧。约644秒时，0号节点因检测到链路存活表中第1项不大于0开始发射REQ数据帧，并因此错过了接收1号节点的REQ数据帧。约655秒时，1号节点回复RPL数据帧。约660秒时，0号节点收到校验正确的RPL数据帧，完成相对自定位。约941秒时，1号节点因等待RPL定时器触发，重发REQ数据帧。约949秒时，0号节点回复RPL数据帧。约954秒时，1号节点收到校验正确的RPL数据帧，完成相对自定位。

后续仿真时间内，两个节点轮流发送REQ和RPL直至仿真结束。

三、仿真结果分析

在图4-图7所示几种典型的场景下进行软件仿真，并对比分析了节点在不同情况下的定位误差。校正前后均方根误差(海里/小时)如表1所示。由表可见，对于两节点情况，本方法平均可以将惯导误差降低40％以上；而对于三节点情况，本方法平均可以将惯导误差降低约18％。其原因在于：本方法更适用于于不同移动节点的惯性导航设备漂移方向不同的情况，而对于不同移动节点的惯导设备漂移方向相同时，惯导误差校正能力一般。具体来说，当A节点的惯导设备与B节点的惯导设备漂移方向相反时，本方法可以较好的同时校正A、B两个节点惯导误差；当A节点的惯导设备与B节点的惯导设备漂移方向相同时，本发明的方法对A、B两个节点惯导误差的校正情况则非常有限。因此，在使用本方法前，如果能够对两个节点的惯导设备进行测量，将漂移方向相反的两个节点配成一对使用，将会较好的提升惯导误差校正能力。而当三个节点两两互相可以通信时，即使事先进行了配对，也会存在两个漂移方向相同的节点互相通信校正的情况，降低本方法的惯导误差校正能力。此时应该切断两个漂移方向相同的节点之间的通信链路，就可以提升本方法的惯导误差校正能力。2节点圆周运动在导航经度0.15海里/小时情况下的校正前后对比见图8(a)和图8(b)，3节点直线运动在导航经度0.25海里/小时情况下的校正前后对比见图9(a)、图9(b)和图9(c)。

表1校正前后均方根误差对照表

最后所应说明的是，以上实施例仅用以说明本发明的技术方案而非限制。尽管参照实施例对本发明进行了详细说明，本领域的普通技术人员应当理解，对本发明的技术方案进行修改或者等同替换，都不脱离本发明技术方案的精神和范围，其均应涵盖在本发明的权利要求范围当中。

Claims (9)

1.一种无需时钟同步的移动编队相对自定位方法，其中，移动网络中的每个节点装配有惯性导航设备和深度计，惯性导航设备用于提供移动节点的惯导纬度、惯导经度、东向速度、北向速度参数；深度计用于提供移动节点的深度和垂直速度参数；该方法具体包括：

所有节点通过收发KLV数据帧建立通信链路；

所有节点通过应答通信，使用粒子滤波与轨迹预测相结合的方法对惯性导航设备的相对定位误差进行校正，从而实现自定位；

所述所有节点通过收发KLV数据帧建立通信链路；具体包括：

最后一个入水的节点在发射KLV定时器触发时，建立空白KLV数据帧并进行赋值；赋值完毕后通过水声通信发射该KLV数据帧；发射完毕后帧序号计数器加1；如果帧序号计数器未超过KLV次数限制K，则该节点的发射端返回等待状态；如果帧序号计数器超过KLV次数限制K，则清零帧序号计数器，该节点的发射端进入空闲状态，开启查询定时器，每隔时间τ_CHK查询一次是否需要发射REQ数据帧；其它节点接收到最后一个入水的节点发射的KLV数据帧并进行处理；

所述空白KLV数据帧至少包含以下内容：帧类型、源节点、帧序号、惯导纬度、惯导经度、深度、东向速度、北向速度、垂直速度、校验和；其中，帧类型赋值为KLV，帧序号赋值为帧序号计数器当前值，源节点赋值为t，惯导纬度、惯导经度、东向速度、北向速度从惯性导航设备中实时读取，深度和垂直速度从深度计中实时读取，校验和赋值为上述内容的校验结果；记录东向速度、北向速度、垂直速度为报备速度；

每个节点发射KLV数据帧，接收其它节点的KLV数据帧并进行处理。

2.根据权利要求1所述的方法，其特征在于，所述方法还包括：移动网络中的每个节点的初始化步骤，具体包括：

为移动网络的每个节点进行编号t，t为整数，t∈[0,T)，T为总节点数量；

设定KLV次数限制K、REQ次数限制Q、粒子数量L、粒子滤波迭代次数上限P、轨迹预测阶数上限G、速度变化上限S、最长REQ发射间隔R；

为每个节点建立空白链路存活表，设定链路存活时间上限C：

对于第t个节点，其链路存活表包含T项，其中的第i项，i∈[0,T)且i≠t，用于记录第i个节点与本节点间通信链路的剩余存活时间，第t项表示本节点自身循环链路，其始终为0表示不需要使用；空白链路存活表中的其余项初始时也都为0，表示不存在有效的通信链路；存活表刷新时刻fsh置为0；

为每个节点建立空白位置记录表、空白速度记录表和空白时间记录表：

对于第t个节点，其位置记录表包含T项，其中的第i项，i∈[0,T)，用于记录通过水声通信接收到的第i个节点的惯导经度、惯导纬度和深度；速度记录表包含T项，其中的第i项，i∈[0,T)，用于记录通过水声通信接收到的第i个节点的东向速度、北向速度和垂直速度；时间记录表包含T项，其中的第i项，i∈[0,T)，用于记录通过水声通信接收到的第i个节点上述位置和速度时由本地时钟给出的时间；

为每个节点建立空白轨迹偏移记录表：

对于第t个节点，其轨迹偏移记录表包含G项，其中的每一项均包含记录时间、本节点经度偏移值、本节点纬度偏移值；G至少为3；

为每个节点清零所有定时器和计数器，清空已设定标志位；

最后一个入水的节点在入水前设定发射KLV定时器的次数为K以及每一次发射KLV的时间，并将已设定标志位置为1；其首次发射KLV时间应不早于节点到达指定深度的时间，后续发射时间与前一次发射KLV时间间隔应不小于2×(T-1)×τ_KLV秒，其中τ_KLV不小于从一个节点开始发射KLV数据帧到另一个节点接收完毕该数据帧所需要的最长时间；

设各个节点到达指定深度的时间为其本地时钟的0时刻；所有节点将上次发射REQ时间reqt赋值为0，等待RPL帧序号wrpl赋值为-1，清空重发REQ标志位；所有节点的发射端进入等待状态，接收端进入等待接收数据状态。

3.根据权利要求1所述的方法，其特征在于，所述其它节点接收到最后一个入水的节点发射的KLV数据帧并进行处理，具体包括：

当一个节点接收到最后一个入水的节点发射的KLV数据帧时，进行数据校验：在校验不正确的情况下，丢弃该数据帧，不做任何处理，接收端返回等待接收数据状态；校验正确的情况下，则更新该节点的链路存活表：

读取源节点s、帧序号n、惯导纬度lati、惯导经度long、深度alti、东向速度vest、北向速度vnth、垂直速度vdpt；获取本地时钟当前值cur；将lati、long、alti依次记录到位置记录表的第s项中，将vest、vnth、vdpt依次记录到速度记录表的第s项中，时间记录表的第s项赋值为cur；遍历链路存活表，如果其中第i项的取值大于0，i∈[0,T)，则将其减去(cur-fsh)，如果减后不大于0，则将其赋值为0；将链路存活表中的第s项赋值为C；将fsh赋值为cur；然后该节点设定KLV发射次数与时间：

如果已设定标志位为1，则不做任何处理，该节点的接收端返回等待接收数据状态；如果已设定标志位为空，则将帧序号计数器赋值为n，设定KLV发射次数为(K-n)并设定每一次发射KLV的时间，并将已设定标志位置为1；本节点首次发射KLV时间为mod((t-s-1+T),T)×τ_KLV秒之后，后续发射时间与前一次发射KLV时间间隔应不小于2×(T-1)×τ_KLV秒；设定完毕后，该节点的接收端返回等待接收数据状态。

4.根据权利要求3所述的方法，其特征在于，所述每个节点发射KLV数据帧，接收其它节点的KLV数据帧并进行处理；具体包括：

任一节点发射KLV数据帧：

该节点在发射KLV定时器触发时，建立空白KLV数据帧；赋值完毕后发射该KLV数据帧；发射完毕后帧序号计数器加1；如果帧序号计数器未超过KLV次数限制K，则返回等待状态，等待下次发射KLV定时器触发；如果帧序号计数器超过KLV次数限制K，则清零帧序号计数器，该节点的发射端进入空闲状态，开启查询定时器，每隔时间τ_CHK查询一次是否需要发射REQ数据帧；

任一节点接收KLV数据帧：

该节点在收到KLV数据帧时先进行数据校验；在校验不正确的情况下，丢弃该数据帧，不做任何处理，接收端返回等待接收数据状态；校验正确的情况下，则根据KLV数据帧更新链路存活表：

读取源节点s、帧序号n、惯导纬度lati、惯导经度long、深度alti、东向速度vest、北向速度vnth、垂直速度vdpt；获取本地时钟当前值cur；将lati、long、alti依次记录到位置记录表的第s项中，将vest、vnth、vdpt依次记录到速度记录表的第s项中，时间记录表的第s项赋值为cur；遍历链路存活表，如果其中第i项的取值大于0，i∈[0,T)，则将其减去(cur-fsh)，如果减后不大于0，则将其赋值为0；将链路存活表中的第s项赋值为C；将fsh赋值为cur；然后设定KLV发射次数与时间：

如果已设定标志位为1，则不做任何处理，该节点的接收端返回等待接收数据状态；如果已设定标志位为空，则将帧序号计数器赋值为n，设定KLV发射次数为(K-n)以及每一次发射KLV的时间，并将已设定标志位置为1；首次发射KLV时间为mod((t-s-1+T),T)×τ_KLV秒之后，后续发射时间与前一次发射KLV时间间隔不小于2×(T-1)×τ_KLV秒；设定完毕后，该节点的接收端返回等待接收数据状态。

5.根据权利要求4所述的方法，其特征在于，所述所有节点通过应答通信，使用粒子滤波与轨迹预测相结合的方法对惯性导航设备的相对定位误差进行校正，从而实现自定位，具体包括：

每个节点在查询定时器触发时，当满足一定的条件下，生成并发射REQ数据帧；

每个节点接收REQ数据帧并进行处理，生成和发射RPL数据帧；

每个节点在等待RPL定时器触发时进行处理；

每个节点接收RPL数据帧，使用粒子滤波与轨迹预测相结合的方法对惯性导航设备的相对定位误差进行校正，从而实现自定位。

6.根据权利要求5所述的方法，其特征在于，所述每个节点在查询定时器触发时，当满足一定的条件下，生成并发射REQ数据帧；具体包括：

任一节点的查询定时器触发时，当存在以下五种情况之一时，则该节点的发射端生成并发射REQ数据帧；否则发射端返回空闲状态；五种情况分别为：

情况一：惯导设备的东向速度、北向速度和深度计的垂直速度与报备速度中的对应记录数值的差的模值超过S时；

情况二：链路存活表中所有项均为0时；

情况三：链路存活表中仅有一项不为0，记该项序号为i，i∈[0,T)且i≠t，通过位置记录表的第i项、速度记录表的第i项、时间记录表的第i项和当前时间估算其与本节点的距离已经大于水声通信有效作用距离的0.8倍时；

情况四：本节点当前时间cur与reqt的差已经超过R时；

情况五：重发REQ标志位为1时；

该节点的发射端生成并发射REQ数据帧的过程为：

建立空白REQ数据帧；空白REQ数据帧至少包含以下内容：帧类型、源节点、帧序号、惯导纬度、惯导经度、深度、东向速度、北向速度、垂直速度、校验和；其中，帧类型赋值为REQ，帧序号赋值为帧序号计数器当前值，源节点赋值为t，惯导纬度、惯导经度、东向速度、北向速度从惯性导航设备中实时读取，深度和垂直速度从深度计中实时读取，校验和赋值为上述内容的校验结果；记录东向速度、北向速度、垂直速度为报备速度；赋值完毕后通过水声通信发射该REQ数据帧；发射完毕后，reqt赋值为本地时钟的当前时间，wrpl记为帧序号计数器当前值，帧序号计数器加1；停止查询定时器，开启等待RPL定时器，定时时长不小于τ_REQ+T×τ_RPL秒，其中τ_REQ不小于REQ数据帧从一个节点开始发射到另一个节点接收完毕所需要的最长时间，τ_RPL不小于RPL数据帧从一个节点开始发射到另一个节点接收完毕所需要的最长时间。

7.根据权利要求6所述的方法，其特征在于，所述每个节点接收REQ数据帧并进行处理，生成和发射RPL数据帧；具体包括：

任一节点在接收到REQ数据帧时先进行数据校验；在校验不正确的情况下，丢弃该数据帧，不做任何处理，该节点的接收端返回等待接收数据状态；校验正确的情况下，如果此时已设定标志位仍为空，则将已设定标志位置为1，更新链路存活表：

读取源节点s、帧序号n、惯导纬度lati、惯导经度long、深度alti、东向速度vest、北向速度vnth、垂直速度vdpt；获取本节点当前时钟cur；将lati、long、alti依次记录到位置记录表的第s项中，将vest、vnth、vdpt依次记录到速度记录表的第s项中，时间记录表的第s项赋值为cur；遍历链路存活表，如果其中第i项的取值大于0，i∈[0,T)，则将其减去(cur-fsh)，如果减后不大于0，则将其赋值为0；将fsh赋值为cur；将链路存活表中的第s项赋值为C；然后建立空白RPL数据帧：

空白RPL数据帧至少包含以下内容：帧类型、源节点、目的节点、帧序号、回复延迟、惯导纬度、惯导经度、深度、东向速度、北向速度、垂直速度和校验和；其中，帧类型赋值为RPL，源节点赋值为t，目的节点赋值为s，帧序号赋值为n，回复延迟赋值为(t+1)×τ_RPL，惯导纬度、惯导经度、东向速度、北向速度从惯性导航设备中实时读取，深度和垂直速度从深度计中实时读取，校验和赋值为上述内容的校验结果；

赋值完毕后延迟(t+1)×τ_RPL秒再通过水声通信发射该RPL数据帧；记录东向速度、北向速度、垂直速度为报备速度；该节点的接收端返回等待接收数据状态。

8.根据权利要求7所述的方法，其特征在于，所述每个节点在等待RPL定时器触发时进行处理；具体包括：

停止等待RPL定时器，开启查询定时器；如果帧序号计数器取值不大于Q，则将重发REQ标志位置为1，发射端返回空闲状态；如果帧序号计数器取值大于Q，则清空重发REQ标志位，清零帧序号计数器，发射端返回空闲状态。

9.根据权利要求8所述的方法，其特征在于，所述每个节点接收RPL数据帧，使用粒子滤波与轨迹预测相结合的方法对惯性导航设备的相对定位误差进行校正，从而实现自定位，具体包括：

任一节点在收到RPL数据帧时先进行数据校验；在校验不正确的情况下，丢弃该数据帧，不做任何处理，接收端返回等待接收数据状态；校验正确的情况下，更新链路存活表：

读取源节点s、目的节点d、回复延迟τ、帧序号n、惯导纬度lati、惯导经度long、深度alti、东向速度vest、北向速度vnth、垂直速度vdpt；获取本地时钟当前值cur；将lati、long、alti依次记录到位置记录表的第s项中，将vest、vnth、vdpt依次记录到速度记录表的第s项中，时间记录表的第s项赋值为cur；遍历链路存活表，如果其中第i项的取值大于0，i∈[0,T)，则将其减去(cur-fsh)，如果减后不大于0，则将其赋值为0；将链路存活表中的第s项赋值为C；将fsh赋值为cur；

如果d不等于t或者n不等于wrpl，则丢弃该RPL数据帧，不做任何处理；否则，清空重发REQ标志位，清零帧序号计数器，取消等待RPL定时器，开启查询定时器；将粒子滤波迭代次数p赋值为0；粒子滤波坐标经度初始值JI赋值为惯导经度JS，粒子滤波坐标纬度初始值WI赋值为惯导纬度WS；然后计算参考点2的纬度W2、经度J2：

W2＝W1–(vnth×τ–NS×(cur–reqt))/110945.8

J2＝J1–(vest×τ–ES×(cur–reqt))/11319.5/cos(W2×π/180)

其中，W1为参考点1的纬度；J1为参考点1的经度；NS为本节点惯导北向速度、ES为本节点惯导东向速度、π为圆周率；参考距离r0赋值为参考点1和参考点2间的距离；按照下式计算测量距离dis：

dis＝c×(cur–req–τ–PKL_REQ–PKL_RPL)

其中，c为水中声速、PKL_REQ为REQ数据帧时间长度、PKL_RPL为RPL数据帧时间长度；

利用多项式回归方法，进行轨迹预测：

根据轨迹偏移记录表估计出本节点当前的经度偏移阶数JO及经度偏移率JF_m、纬度偏移率WO及纬度偏移阶数WF_n，m∈[0,JO)、n∈[0,WO)；其中，JO为拟合经度偏移值时所使用的多项式阶数，JF_m为本节点经度偏移值随时间变化的第m阶多项式系数，WO为拟合纬度偏移值时所使用的多项式阶数，WF_n为本节点的纬度偏移值随时间变化的第n阶多项式系数；如果轨迹偏移记录表为空或者多项式回归时出现零除现象，则本节点当前的经度偏移率、纬度偏移率均为0；

使用粒子滤波的方法，进行相对位置矫正；

计算第l，l∈[0,L)个粒子(JE_l,WE_l)；根据JO、JF_m、当前时间cur和JI计算出第l个粒子的经度JE_l；根据WO、WF_n、当前时间cur和WI计算出第l个粒子的纬度WE_l，l∈[0,L)；

JE_l＝JI+α_l×(Σ_mJF_m×cur^m+δ_l)/4^p,m∈[0,JO)

WE_l＝WI+β_l×(Σ_nWF_n×curⁿ+ε_l)/4^p,n∈[0,WO)

其中，α_l、β_l均为绝对值不大于2的随机实数、δ_l、ε_l均为绝对值不大于0.001的随机实数；

然后，量测距离r1_l赋值为粒子(JE_l,WE_l)与参考点1的间距，量测距离r2_l赋值为粒子(JE_l,WE_l)与参考点2的间距；当r1_l与r2_l差的绝对值不小于r0时，粒子(JE_l,WE_l)的wei_l赋值为0；否则计算权重wei_l，其计算原则为：wei_l≥0且r1_l+r2_l与dis差的绝对值越小wei_l越大；根据wei_l对L个粒子进行重采样，重采样原则为：在粒子数量保持L不变的前提下，wei_l越大的粒子的后代粒子越多，wei_l越小的粒子的后代粒子越少甚至被抛弃；JI赋值为重采样后得到的所有L个JE_l’的均值，WI赋值为重采样后得到所有L个的WE_l’的均值；将p加1；如果此时p未超过P，则重复本步骤继续使用粒子滤波进行相对位置矫正；如果此时p超过P，则结束粒子滤波；

更新轨迹偏移记录表：

逐次递增的将轨迹偏移记录表的第g+1项元素值对应的赋值给第g项元素值，其中g∈[0,G-1)；轨迹偏移记录表的第G-1项元素值中记录时间赋值为本节点当前时间cur、本节点经度偏移值赋值为JS-JI、本节点纬度偏移值赋值为WS-WI；再将JS赋值为JI，WS赋值为WI。

Images