一种无需时钟同步的水下移动节点惯导误差校正方法与流程

1.本发明属于水声网络领域，特别涉及一种无需时钟同步的水下移动节点惯导误差校正方法。


背景技术：

2.常见的水下移动节点定位导航技术主要包括四类：捷联惯导技术、水下声学定位与导航技术、地球物理及视觉导航技术及水下协同导航技术。其中，捷联惯导技术的导航误差随时间累积明显；水下声学定位与导航技术中的多普勒速度计程仪方法容易出现对地失锁现象，水声定位基阵方法需要声信标或其他辅助设备；地球物理及视觉导航技术需要对移动节点所在海域的地形地貌、重力或地磁信息有较为详尽的数据库支撑；水下协同导航技术则主要应用于水下移动编队各个节点间的相对自定位。四类方法使用场景不同，具有不同的优势。
3.即使水下移动节点使用极高精度的组合导航设备，经过长时间水下航行，也会产生定位偏差，此时可以通过水下声学定位导航技术对惯导误差进行矫正。目前，大多数水下声学定位导航技术依赖水下节点间的时间同步。也有学者使用卡尔曼滤波在时钟异步的条件下实现了移动节点导航，但是考虑到卡尔曼滤波在非高斯、非线性条件下的局限性，我们采用虚拟平移结合粒子滤波的方法。另外，多数水下移动节点导航定位技术均忽略了移动节点从发射定位请求信号到接收定位回复信号期间的位移，这在水下移动节点运动速度稍快时，会造成较大误差。所以本方法考虑到了移动节点的位移。


技术实现要素：

4.本发明方法针对水下移动节点惯导累计误差随时间增大的问题，提出了一种无需时钟同步的水下移动节点惯导误差矫正方法，该方法利用水声网络与移动节点间的应答通信，在考虑到应答通信信号往返距离不相等的情况下，使用虚拟平移结合粒子滤波的方法，在不需要节点间时钟同步的条件下对移动节点的进行绝对自定位，一定程度上减小了惯导误差。
5.为实现上述目的，本发明提供了一种无需时钟同步的水下移动节点惯导误差校正方法，所述方法包括：利用水声网络中固定节点与水下移动节点间的应答通信，基于应答通信信号往返距离不相等的条件下，使用虚拟平移结合粒子滤波的方法，在不需要节点间时钟同步的条件下对移动节点的惯导设备的定位误差进行校正。
6.作为上述方法的一种改进，所述方法具体包括：
7.步骤1)为每个水下移动节点上建立位移记录表、gps记录表和距离记录表；位移记录表包含t 1组，t为固定节点的个数，其中前t组用于记录固定节点在经度、纬度、深度三个维度上的虚拟位移，最后一组用于记录该移动节点的惯导设备给出的从发射req到等待rpl超时过程中，产生的经度位移nor_dis、纬度位移est_dis和深度位移alt_dis；gps记录表用于记录每个固定节点的经度、纬度和深度；距离记录表包含t项，其中的第i项用于记录该移
动节点与固定节点i的往返测距结果；
8.步骤2)建立req数据帧，记录由该移动节点的本地时钟给出的req数据帧发射开始时刻reqt并发射req数据帧；发射完毕后，开启等待rpl定时器，等待rpl定时器应不少于t
×
delayslot秒，delayslot为延时时间片，应不小于节点最大通信距离/水中声速 最长数据帧持续时间；并累计自身在纬度、经度和深度上的位移，定时将上述位移赋值到位移记录表最后一组相应项中并记录更新时间为last_up；回到空闲状态等待；
9.步骤3)当接收到任一固定节点发送的rpl数据帧并校验正确，同时满足设定的条件时，进入步骤4)；否则，返回到空闲状态继续等待；
10.步骤4)读取rpl数据帧的信源节点s、回复延时τ、纬度lat_s、经度lon_s、深度alt_s；将lat_s、lon_s和alt_s依次赋值给gps记录表的第s组；计算本移动节点与信源节点s的往返测距结果r_s：
11.r_s＝c
×
(rplt
–
reqt
–
τ
–
(reqs rpls))
ꢀꢀꢀ
(1)
12.其中，信源节点s为固定节点s，c为水中声速，rplt为由本移动节点本地时钟给出的rpl数据帧接收完毕的时刻，reqs和rpls分别为req数据帧和rpl数据帧的时长；将r_s赋值给距离记录表的第s项；读取位移记录表最后一组中相应数值nor_dis、est_dis、alt_dis，计算该固定节点s在纬度、经度和深度方向上的虚拟位移nor_vir、est_vir和alt_vir：
[0013][0014]
其中，last_up为上次更新位移记录表最后一组的时刻；将nor_vir、est_vir、alt_vir依次赋值给位移记录表中的第s组中的对应项；计算完毕后回到空闲状态继续等待；
[0015]
步骤5)如果等待rpl定时器超时，则查询距离记录表，如果其中数值大于0的项数大于2项，则进入步骤6)；否则，清空位移记录表、gps记录表和距离记录表，清零reqt和reqn，回到空闲状态继续等待；
[0016]
步骤6)计算虚拟平移参考点的坐标；
[0017]
步骤7)根据虚拟平移参考点的坐标，利用粒子滤波迭代得到迭代结果对惯导设备的位置当前值进行校正。
[0018]
作为上述方法的一种改进，每个移动节点上的惯导设备用于提供移动节点的惯导纬度lat、惯导经度lon、深度alt、北向速度nor_vel、东向速度est_vel和垂直速度alt_vel；每个移动节点入水之前获取其gps真实值，包括经度、纬度，并将gps真实值作为该节点惯导设备的经度、纬度初始值；深度初始值为0，东向速度、北向速度、垂直速度的初始值也均为0。
[0019]
作为上述方法的一种改进，所述req数据帧应至少包括以下内容：帧类型、信源节点、帧序号和校验和；其中帧类型赋值为req，信源节点为该移动节点的编号，帧序号计数器加1，帧序号赋值为帧序号计数器当前取值，帧序号计数器的初始值为0；校验和赋值为上述各项的校验结果；记录req帧序号reqn为帧序号计数器当前值。
[0020]
作为上述方法的一种改进，两个req的发射时间间隔为m
×
(t m)
×
delayslot，m为移动节点的个数。
[0021]
作为上述方法的一种改进，所述方法还包括：固定节点生成和发射rpl数据帧的步骤；具体包括：
[0022]
当接收到校验正确的req数据帧，则读取接收到的req数据帧中的信源节点s、帧序号n；建立空白rpl数据帧；空白rpl数据帧至少包含以下内容：帧类型、信源节点、目的节点、帧序号、纬度、经度、深度、回复延时和校验和；其中，帧类型赋值为rpl，信源节点赋值为t，目的节点赋值为s，帧序号赋值为n，纬度、经度、深度赋值为标定之后的本固定节点的纬度、经度、深度，回复延时赋值为(1 mod(t
–
s t m,t))
×
delayslot，其中mod(t
–
s t m,t)为t
–
s t m对t取余数运算；校验和赋值为上述各项的校验结果；等候delayslot
×
(1 mod(t
–
s t m,t))秒后发射rpl数据帧。
[0023]
作为上述方法的一种改进，所述步骤2)中设定的条件包括：接收到的rpl数据帧的帧序号n等于reqn和接收到的rpl数据帧的目的节点为本移动节点。
[0024]
作为上述方法的一种改进，所述步骤6)具体包括：
[0025]
步骤6-1)读取位移记录表最后一组中相应数值nor_dis、est_dis、alt_dis，计算本移动节点从发射req数据帧到rpl等待超时期间的纬度、经度和深度方向上三个位移：
[0026][0027]
其中，nor_all为纬度位移，est_all为经度位移，alt_all为深度位移，cur为本移动节点本地时钟的当前读数；令i初始值为0，进入步骤6-2)，开始遍历距离记录表；
[0028]
步骤6-2)若i《t，则读取距离记录表的第i项，转入步骤6-3)；否则转入步骤7)；
[0029]
步骤6-3)若距离记录表第i项的r_i大于0时，则转入步骤6-4)；否则，i加1后转入步骤6-2)；
[0030]
步骤6-4)计算第i个参考坐标a(lata_i,lona_i,alta_i)、第i个参考坐标b(latb_i,lonb_i,altb_i)以及第i个参考坐标a与第i个参考坐标b的间距rab_i：
[0031][0032]
其中，latr_i、lonr_i、altr_i为gps记录表中第i组的三项数值，lon1为纬度相差1度时对应经度上相差的距离，为110945.8米，lat1为赤道经度相差1度时对应纬度上相差的距离，为111319.5米，π为圆周率；然后，i加1后转入步骤6-2)；
[0033]
作为上述方法的一种改进，所述步骤7)具体包括：
[0034]
步骤7-1)设置粒子滤波的迭代次数k的初始值为1，划定第1次迭代的本移动节点所在位置可能达到的范围；
[0035]
步骤7-2)在第k次迭代的本移动节点当前位置可能达到的范围内随机生成l个位置粒子；对于第l个位置粒子，l∈[0,l)，其中包含纬度lat_l、经度lon_l、和深度alt_l三项信息，记该粒子权重为wei_l[k]，且wei_l[k]初始值为1；令i初始值为0，进入步骤7-3)，开始遍历距离记录表；
[0036]
步骤7-3)若i《t，则读取距离记录表的第i项，转入步骤7-4)；否则转入步骤7-6)；
[0037]
步骤7-4)若距离记录表第i项的r_i大于0时，则转入步骤7-5)；否则，i加1后转入步骤7-3)；
[0038]
步骤7-5)依次计算第l个位置粒子与第i个参考坐标a间的距离ral_i和与第i个参考坐标b间的距离rbl_i：
[0039][0040]
如果ral_i和rbl_i差的绝对值大于rab_i，则直接将wei_l[k]置为0；否则，将当前wei_l[k]与f(ral_i rbl_i,r_i)的乘积作为新的wei_l[k]；其中，f(ral_i rbl_i,r_i)表示与ral_i rbl_i、r_i有关的函数，该函数取值范围为[0,1]，计算原则为：ral_i rbl_i与r_i差的绝对值越小，其取值越接近1，反之则越接近0，计算完毕后，i加1后转入步骤7-3)；
[0041]
步骤7-6)将wei_l[k]开i次方作为新的wei_l[k]，其中i为距离记录表中数值大于0的项数；根据wei_l[k]对l个粒子进行重采样，重采样原则为：在粒子数量保持l不变的前提下，wei_l[k]越大的粒子的后代粒子越多，wei_l[k]越小的粒子的后代粒子越少甚至被抛弃；通过重采样得到后代粒子，从而缩小本移动节点所在位置的可能范围；
[0042]
步骤7-7)计算第k次迭代经过重采样得到的所有l个位置粒子(lat_l’,lon_l’,alt_l’)，l∈[0,l)的加权平均值(lat_k,lon_k,alt_k)：
[0043][0044]
令i初始值为0，所有粒子的加权平均值wei_k的初始值为1，进入步骤7-8)；
[0045]
步骤7-8)若i《t，则读取距离记录表的第i项，转入步骤7-9)；否则转入步骤7-11)；
[0046]
步骤7-9)若距离记录表第i项的r_i大于0时，则转入步骤7-10)；否则，i加1后转入步骤7-8)；
[0047]
步骤7-10)计算粒子(lat_k,lon_k,alt_k)与第i个参考坐标a间的距离ra_i和与第i个参考坐标b间的距离rb_i并更新权重wei_k：
[0048]
ra_i＝sqrt[(lata_i
–
lat_k)2×
lon1
2
(alta_i
–
alt_k)
2
[0049]
(lona_i
–
lon_k)2×
lat12×
cos((lata_i lat_k)
×
π/360.0)2]
[0050]
rb_i＝sqrt[(latb_i
–
lat_k)2×
lon1
2
(altb_i
–
alt_k)
2
[0051]
(lonb_i
–
lon_k)2×
lat12×
cos((latb_i lat_k)
×
π/360.0)2]
ꢀꢀꢀ
(7)
[0052]
将当前wei_k与f(ra_i rb_i,r_i)的乘积作为新的wei_k；i加1后转入步骤7-8)；
[0053]
步骤7-11)将wei_k开i次方作为新的wei_k，当wei_k满足阈值要求或者k大于等于迭代上限，进入步骤7-12)；否则，将步骤7-6)通过重采样得到的本移动节点所在位置的可能范围，作为第k 1次迭代的本移动节点当前位置可能达到的范围，k加1后转入步骤7-2)；
[0054]
步骤7-12)将(lat_k,lon_k,alt_k)与惯导设备的位置当前值(lat,lon,alt)进行加权平均：
[0055][0056]
其中α∈[0,1)，为对惯导设备读数的置信度；(lat1,lon1,alt1)为校正后的惯导设备的位置当前值。
[0057]
本发明的优势在于：
[0058]
本发明的方法在不需要时钟同步的前提下，利用水下移动节点与水声网络节点间的应答通信，在考虑到应答通信信号往返距离不相等的前提下，使用虚拟平移结合粒子滤波的方法，一定程度上减小了惯导误差。
附图说明：
[0059]
图1为本发明的无需时钟同步的移动节点惯导误差矫正方法的流程示意图；
[0060]
图2为opnet仿真中的节点模型；
[0061]
图3为本发明的虚拟平移方法示意图；
[0062]
图4(a)为4个固定节点的示意图；
[0063]
图4(b)为4个固定节点的仿真情况及结果示意图；
[0064]
图5(a)为5个固定节点的示意图；
[0065]
图5(b)为5个固定节点的仿真情况及结果示意图；
[0066]
图6(a)为第一种情况的6个固定节点的示意图；
[0067]
图6(b)为图6(a)的仿真情况及结果示意图；
[0068]
图7(a)为第二种情况的6个固定节点的示意图；
[0069]
图7(b)为图7(a)的仿真情况及结果示意图。
具体实施方式：
[0070]
下面结合附图和具体实施例对本网络自组织方法进行详细的说明。
[0071]
本发明的一种无需时钟同步的移动节点惯导误差矫正方法，是当移动节点依靠惯性导航(下文简称为惯导)设备在水下航行一段时间后，惯导误差有一定积累的情况下，接入到水声网络中，在不需要进行节点间时钟同步的前提下，使用虚拟平移结合粒子滤波的方法，对移动节点的惯导误差进行矫正，包括以下步骤：
[0072]
一、固定节点初始化
[0073]
1)、为固定节点设定网络包含的固定节点总数量t、移动节点总数量m及固定节点
标识；t为整数，t∈[0,t)，且应保证不同固定节点的标识t在网络中是唯一的；初始化完毕后，t、m与t均不能修改；
[0074]
2)、为每一个固定节点标定gps绝对坐标位置和深度
[0075]
如使用带有gps天线的浮标作为固定节点，应保证浮标的漂移范围尽可能小；如使用潜标作为固定节点，则需要使用其他水下定位手段对潜标位置进行尽可能精确的标定；具体标定方法不在本发明的方法讨论范围之内；
[0076]
二、固定节点空闲状态
[0077]
1)、如果接收到校验正确的req数据帧，则按照步骤2)回复rpl数据帧；否则回到空闲状态继续等待；
[0078]
2)、读取接收到的req数据帧中的信源节点s、帧序号为n；建立空白rpl数据帧；空白rpl数据帧至少包含以下内容：帧类型、信源节点、目的节点、帧序号、纬度、经度、深度、回复延时、校验和；其中，帧类型赋值为rpl，信源节点赋值为t，目的节点赋值为s，帧序号赋值为n，纬度、经度、深度赋值为标定之后的本固定节点的纬度(度)、经度(度)、深度(米)，回复延时赋值为(1 mod(t
–
s t m,t))
×
delayslot(秒)，其中mod(t
–
s t m,t)为t
–
s t m对t取余数运算，delayslot应不小于节点最大通信距离(米)/水中声速(米/秒) 最长数据帧持续时间(秒)；校验和赋值为上述各项的校验结果；等候delayslot
×
(1 mod(t
–
s t m,t))秒后发射rpl数据帧；发射完毕后，回到空闲状态继续等待；
[0079]
三、移动节点初始化
[0080]
1)、移动节点应装配惯导设备
[0081]
惯导设备应能够实时提供移动节点的惯导纬度lat(度)、惯导经度lon(度)、深度alt(米)、北向速度nor_vel(米/秒)、东向速度est_vel(米/秒)和垂直速度alt_vel(米/秒)等参数；每个节点入水之前获取其gps真实值，包括经度、纬度，并将gps真实值作为该节点惯导设备的经度、纬度初始值；深度初始值为0，东向速度、北向速度、垂直速度的初始值也均为0；
[0082]
2)、为移动节点设定网络包含的固定节点总数量t、移动节点总数量m及本移动点标识m，m为整数，m∈[t,t m)，且应保证不同移动节点的地址m在网络中是唯一的；初始化完毕后，m、m与t均不能修改；
[0083]
3)、建立空白位移记录表、空白gps记录表和空白距离记录表
[0084]
位移记录表包含t 1组，每一组包含经度位移(米)、纬度位移(米)、深度位移(米)三项内容；其中前t组用于记录固定节点在经度、纬度、深度三个维度上的虚拟位移，最后一组用于记录移动节点自身惯导设备给出的从发射req到等待rpl超时过程中的在经度、纬度、深度三个维度上的位移；
[0085]
gps记录表包含t组，每一组包含经度、纬度、深度三项内容，用于记录固定节点的经度(度)、纬度(度)、深度(米)；
[0086]
距离记录表包含t项，其中的第i,i∈[0,t)项用于记录本移动节点与固定节点i的往返测距结果(米)；
[0087]
4)、清零所有定时器和计数器
[0088]
req发射时间清零，等待rpl定时器清零，帧序号计数器清零；
[0089]
5)、设定req发射时间
[0090]
设定req发射时间间隔为m
×
(t m)
×
delayslot秒，并设定初次发射req时间在惯导误差积累到一定程度之后，例如120小时，则移动节点m初次发射req时间应设定为120
×
3600 m
×
(t m)
×
delayslot秒；
[0091]
6)、入水后开始依靠惯导设备航行，进入空闲状态等待；
[0092]
四、移动节点空闲状态
[0093]
1)、如果是发射req，则建立空白req数据帧；空白req数据帧应至少包括以下内容：帧类型、信源节点、帧序号、校验和；其中帧类型赋值为req，信源节点赋值为m，帧序号计数器加1，帧序号赋值为帧序号计数器当前取值，校验和赋值为上述各项的校验结果；记录req帧序号reqn为帧序号计数器当前值，记录由本移动节点本地时钟给出的req数据帧发射开始时刻reqt(秒)并开始发射；发射完毕后，开启等待rpl定时器，等待rpl定时器应不少于t
×
delayslot秒；开始累计自身在纬度、经度和深度上的位移，定时将上述位移赋值到位移记录表最后一组相应项中并记录更新时间为last_up；回到空闲状态等待；
[0094]
2)、如果接收到rpl数据帧校验正确，且同时满足如下两个条件：接收到的rpl数据帧的帧序号n等于reqn、接收到的rpl数据帧的目的节点的序号等于m时，则按照步骤3)处理；校验不正确或者不满足上述两个条件中的任意一个时，则回到空闲状态继续等待；
[0095]
3)、读取接收到的rpl数据帧的信源节点s、回复延时τ、纬度lat_s、经度lon_s、深度alt_s；将lat_s、lon_s和alt_s依次赋值给gps记录表的第s组；按照公式(1)计算本移动节点与信源节点s的往返测距结果r_s(米)：
[0096]
r_s＝c
×
(rplt
–
reqt
–
τ
–
(reqs rpls))
ꢀꢀꢀ
(1)
[0097]
其中，c为水中声速(米/秒)，rplt(秒)为由本移动节点本地时钟给出的rpl数据帧接收完毕的时刻，reqs和rpls分别为req数据帧和rpl数据帧的时长(秒)；将r_s赋值给距离记录表的第s项；读取位移记录表最后一组中相应数值nor_dis(米)、est_dis(米)、alt_dis(米)，按照(2)式计算固定节点t在纬度、经度和深度方向上的虚拟位移nor_vir(米)、est_vir(米)和alt_vir(米)；
[0098][0099]
其中，last_up(秒)为上次更新位移记录表最后一组的时刻；将nor_vir、est_vir、alt_vir依次赋值给位移记录表中的第s组中的对应项；计算完毕后回到空闲状态继续等待；
[0100]
4)、如果等待rpl定时器超时，则查询距离记录表，如果其中数值大于0的项数i大于2项，则按照步骤5)进行惯导误差矫正；否则，清空位移记录表、gps记录表和距离记录表，清零reqt和reqn，回到空闲状态继续等待；
[0101]
5)、计算虚拟平移参考点；
[0102]
步骤5-1)读取位移记录表最后一组中相应数值nor_dis、est_dis、alt_dis，计算本移动节点从发射req数据帧到rpl等待超时期间的纬度、经度和深度方向上三个位移：
[0103][0104]
其中，nor_all为纬度位移，est_all为经度位移，alt_all为深度位移，cur为本移动节点本地时钟的当前读数；令i初始值为0，进入步骤5-2)，开始遍历距离记录表；
[0105]
步骤5-2)若i《t，则读取距离记录表的第i项，转入步骤5-3)；否则转入步骤6)；
[0106]
步骤5-3)若距离记录表第i项的r_i大于0时，则转入步骤5-4)；否则，i加1后转入步骤5-2)；
[0107]
步骤5-4)计算第i个参考坐标a(lata_i,lona_i,alta_i)、第i个参考坐标b(latb_i,lonb_i,altb_i)以及第i个参考坐标a与第i个参考坐标b的间距rab_i：
[0108][0109]
其中，latr_i、lonr_i、altr_i为gps记录表中第i组的三项数值，lon1为纬度相差1度时对应经度上相差的距离，为110945.8米，lat1为赤道经度相差1度时对应纬度上相差的距离，为111319.5米，π为圆周率；然后，i加1后转入步骤5-2)；
[0110]
6)、粒子滤波迭代
[0111]
步骤6-1)设置粒子滤波的迭代次数k的初始值为1，划定第1次迭代的本移动节点所在位置可能达到的范围；
[0112]
步骤6-2)在第k次迭代的本移动节点当前位置可能达到的范围内随机生成l个位置粒子；对于第l个位置粒子，l∈[0,l)，其中包含纬度lat_l、经度lon_l、和深度alt_l三项信息，记该粒子权重为wei_l[k]，且wei_l[k]初始值为1；令i初始值为0，进入步骤6-3)，开始遍历距离记录表；
[0113]
步骤6-3)若i《t，则读取距离记录表的第i项，转入步骤6-4)；否则转入步骤6-6)；
[0114]
步骤6-4)若距离记录表第i项的r_i大于0时，则转入步骤6-5)；否则，i加1后转入步骤6-3)；
[0115]
步骤6-5)依次计算第l个位置粒子与第i个参考坐标a间的距离ral_i和与第i个参考坐标b间的距离rbl_i：
[0116][0117]
如果ral_i和rbl_i差的绝对值大于rab_i，则直接将wei_l[k]置为0；否则，将当前wei_l[k]与f(ral_i rbl_i,r_i)的乘积作为新的wei_l[k]；其中，f(ral_i rbl_i,r_i)表示与ral_i rbl_i、r_i有关的函数，该函数取值范围为[0,1]，计算原则为：ral_i rbl_i与r_i差的绝对值越小，其取值越接近1，反之则越接近0，计算完毕，i加1后转入步骤6-3)；
[0118]
步骤6-6)将wei_l[k]开i次方作为新的wei_l[k]，其中i为距离记录表中数值大于0的项数；根据wei_l[k]对l个粒子进行重采样，重采样原则为：在粒子数量保持l不变的前提下，wei_l[k]越大的粒子的后代粒子越多，wei_l[k]越小的粒子的后代粒子越少甚至被抛弃；通过重采样得到后代粒子，从而缩小本移动节点所在位置的可能范围；
[0119]
步骤6-7)计算第k次迭代经过重采样得到的所有l个位置粒子(lat_l’,lon_l’,alt_l’)，l∈[0,l)的加权平均值(lat_k,lon_k,alt_k)：
[0120][0121][0122]
令i初始值为0，所有粒子的加权平均值wei_k的初始值为1，进入步骤6-8)；
[0123]
步骤6-8)若i《t，则读取距离记录表的第i项，转入步骤6-9)；否则转入步骤6-11)；
[0124]
步骤6-9)若距离记录表第i项的r_i大于0时，则转入步骤6-10)；否则，i加1后转入步骤6-8)；
[0125]
步骤6-10)计算粒子(lat_k,lon_k,alt_k)与第i个参考坐标a间的距离ra_i和与第i个参考坐标b间的距离rb_i并更新权重wei_k：
[0126]
ra_i＝sqrt[(lata_i
–
lat_k)2×
lon1
2
(alta_i
–
alt_k)
2
[0127]
(lona_i
–
lon_k)2×
lat12×
cos((lata_i lat_k)
×
π/360.0)2]
[0128]
rb_i＝sqrt[(latb_i
–
lat_k)2×
lon1
2
(altb_i
–
alt_k)
2
[0129]
(lonb_i
–
lon_k)2×
lat12×
cos((latb_i lat_k)
×
π/360.0)2]
ꢀꢀꢀ
(7)
[0130]
将当前wei_k与f(ra_i rb_i,r_i)的乘积作为新的wei_k；i加1后转入步骤6-8)；
[0131]
步骤6-11)将wei_k开i次方作为新的wei_k，当wei_k满足阈值要求或者k大于等于迭代上限，进入步骤6-12)；否则，将步骤6-6)通过重采样得到的本移动节点所在位置的可能范围，作为第k 1次迭代的本移动节点当前位置可能达到的范围，k加1后转入步骤6-2)；
[0132]
步骤6-12)将(lat_k,lon_k,alt_k)与惯导设备的位置当前值(lat,lon,alt)进行加权平均：
[0133]
[0134]
其中α∈[0,1)，为对惯导设备读数的置信度；(lat1,lon1,alt1)为校正后的惯导设备的位置当前值。
[0135]
本发明的方法利用opnet软件，针对条件比较恶劣的水声网络进行仿真，并设计半双工不对称的水声通信方式。
[0136]
一、半双工水声通信设计
[0137]
opnet中，将无线链路分为14个管道模型阶段进行建模。需要在节点模型中设定发射机与接收机的中心频率、带宽、调制方式、传输速率、纠错编码。opnet中默认无线链路采用全双工方式，而目前大多数水声通信只能采用半双工方式。
[0138]
1、管道模型阶段
[0139]
仿真中，将水声通信参数设置为：bpsk调制、半双工方式、中心频率6khz、带宽4khz、通信速率1kbps、有效距离5km。具体到水声通信中，还需要注意的管道模型阶段有如下几点：
[0140]
(1)第6阶段中，水下声波传播速度为1500m/s，与默认电磁波传播速度不同，需要修改。
[0141]
(2)第8阶段需要根据水声传播衰减模型重新编写代码计算接收功率。本方法的仿真中采用了经典的marsh-schulkin模型。
[0142]
(3)实际水声通信中，一旦多个接收信号之间发生碰撞，这些接收信号都无法正确接收。因此在第9阶段的噪声串扰时，放大了信号间噪声串扰的影响。
[0143]
(4)第10阶段中应计算海洋背景噪声级，仿真中采用经典的经验公式将其分为四类进行计算。
[0144]
(5)通过调整发射功率等参数，使得第11阶段在没有碰撞时的误码率在10
－3
左右，而发生碰撞时误码率在10
－1
量级。
[0145]
2、半双工设计
[0146]
opnet中使用的无线链路均为全双工通信方式，不论同一节点的发射机与接收机是否设置为相同中心频率以及带宽。因此，需要自行设定半双工通信方式。
[0147]
opnet仿真中设计的节点模型如图2所示。图中router为惯导误差矫正方法模块，transmitter为发射换能器，receiver为接收水听器。实线表示数据帧流向。虚线为统计中断线，一根为接收信号上升沿触发中断，标志接收信号起始，另一根为接收信号下降沿触发中断，标志接收信号结束。当发射换能器处于发射状态时，忽略接收信号的上升沿及下降沿触发中断，并丢弃所有接收到的数据帧。反之，设置水听器接收状态标志，将待发射信号延迟到下降沿触发中断之后再发送。
[0148]
需要补充的是：在无误码的情况下，数据帧成功接收时的流中断(由receiver指向router的数据帧流向线产生)与接收信号下降沿中断是同时产生的，取其一即可。但在因为误码而丢弃数据帧的情况下，不会产生流中断，但还是会产生下降沿中断。因此必须使用两条统计中断线来设置和清空接收水听器的接收状态标志位。
[0149]
二、节点运动轨迹及误差设计
[0150]
对于节点匀速直线运动或者匀速折线运动的情况，可以直接使用opnet中对于移动节点轨迹定义命令，并在节点属性中添加相应的轨迹文件即可。但是对于节点曲线运动或者变速运动的情况，使用轨迹文件便不方便。此时可以使用op_ima_obj_pos_get()和
op_ima_obj_pos_set_geocentric()函数来获取和设置节点坐标。首先，设计好节点的真实运动轨迹函数，并使用op_ima_obj_pos_get()和op_ima_obj_pos_set_geocentric()函数设定节点的真实位置，而且这个真实位置在仿真过程中，节点自身是无法获悉的。考虑到匀速直线运动模型较为简单，本方法的仿真中节点在仿真区域内做碰壁反弹运动，并保持深度不变。以靠近仿真区域内的东部边界为例，此时北向速度和垂直速度不变，东向速度反向。
[0151]
然后根据惯导设备的误差模型以及深度计的误差模型给真实位置添加误差，并将其结果作为惯导设备及深度计的输出结果。根据调研，不同条件下惯导设备精度差别较大，目前国际最高水准的惯导设备精度已能达到4米/小时。
[0152]
三、仿真设置及流程
[0153]
以图4(a)所示场景为例，t等于4，m等于1，粒子数量l设置为8192，粒子滤波迭代轮数限制设置为6，粒子加权平均值的权重阈值设置为0.999。delayslot设置为4秒。以4米/小时作为惯导误差模型数值上限，随机设置三种惯导误差。假设4个固定节点中的0号和3号的gps无标定误差，1号和2号的gps标定误差在20米以内。移动节点时速5米/秒，其运动轨迹为在仿真区域内做碰壁反弹运动，仿真时长为240小时，移动节点于120小时后开始发射req。
[0154]
首先，移动节点开始发射req数据帧，固定节点2、0、3、1依次开始接收该req数据帧。固定节点2于接收处理完毕后12秒后回复rpl数据帧，固定节点0于接收处理完毕后4秒回复rpl数据帧，固定节点3于接收处理完毕后16秒回复rpl数据帧，固定节点1于接收处理完毕后8秒回复rpl数据帧。移动节点接收到各个节点回复的rpl数据帧后测得与固定节点2往返距离为2894.71米，与固定节点0往返距离为7780.27米，与固定节点1往返距离为11029.56米。固定节点3回复的rpl数据帧由于误码导致校验错误未正确解码而被丢弃。以移动节点与固定节点1为例，虚拟平移方法示意图如图3所示。假设t0时刻，移动节点开始发出req数据帧，并开始定时累计自身位移并更新位移记录表的最后一项；t1时刻，移动节点接收到了固定节点1回复的rpl帧，此时假设移动节点已经位移了s1，但是等待rpl定时器并未超时；直到t2时刻，移动节点的等待rpl定时器超时，此时假设移动节点已经位移了s2。固定节点1的虚拟平移参考点a为固定节点1从其标定位置位移s2
–
s1，固定节点1的虚拟平移参考点b为固定节点1从其标定位置位移s2。经虚拟平移后，t2时刻的移动节点与固定节点1的参考点a和参考点b的距离和，是等于固定节点1与t0时刻的移动节点和t1时刻的移动节点的距离和的。移动节点通过6轮粒子滤波迭代，将其惯导误差由(
–
0.001179,0.001216,0.000)降低到(
–
0.000986,0.000997,0.000)，括号内的数字依次为：纬度误差(度)、经度误差(度)、深度误差(米)。迭代后粒子加权平均值的权重为0.9934。此后，该移动节点每20秒发射一次req数据帧直到仿真结束。
[0155]
需要补充说明的是：上述s1、s2为包含纬度、经度、深度三个方向上的位移的矢量；如果仿真区域内包含多个移动节点可通过时分多址或其他多址方式轮流接入到水声网络中进行定位，互不干扰；本发明的方法虽然没有进行节点间的时钟同步，但仍是基于各个节点的本地时钟及惯导设备不会在较短的时间(例如：1分钟)内产生较大误差的假设。
[0156]
四、仿真结果分析
[0157]
在图4(a)、图5(a)、图6(a)和图7(a)的四种场景下进行软件仿真，由4(b)、图5(b)、图6(b)和图7(b)中结果可见：大部分情况下，本发明的方法可以将移动节点的惯导误差维
持在一个较低的水平。对比分析了移动节点在不同情况下的惯导误差矫正性能，如表1所示。其中，惯导误差为随机生成的4米/小时以下的数值；最小误差为整个仿真过程中矫正后的移动节点的惯导误差最小值；误差均值为整个仿真过程中矫正后的移动节点的惯导误差平均值；平均误差按照下式计算：
[0158]
平均误差＝σ
ii＝1
|误差i–
误差均值|/i
[0159]
根据表1可见，4固定节点情况误差均值最小，5固定节点情况误差均值次之，6固定节点情况二的误差均值又略小于6固定节点情况一。其原因在于：仿真过程中对于所有的固定节点，假设仅0号固定节点和t
–
1号固定节点无标定误差，其余节点均存在一定的标定误差。所以固定节点数量越多，存在标定误差的固定节点数量越多，对移动节点的定位越不准确。另外，对比6固定节点的两种情况，可见6固定节点情况一所采用的环形拓扑结构的精确度要低于6固定节点情况一所采用的双排拓扑结构，其原因在于环形拓扑结构在固定节点位置标定的精确度要差于双排拓扑结构，导致移动节点的定位精确度也变差。
[0160]
表1矫正性能对比
[0161][0162][0163]
最后所应说明的是，以上实施例仅用以说明本发明的技术方案而非限制。尽管参照实施例对本发明进行了详细说明，本领域的普通技术人员应当理解，对本发明的技术方案进行修改或者等同替换，都不脱离本发明技术方案的精神和范围，其均应涵盖在本发明的权利要求范围当中。