CN111194081B - 一种结合水声网络协议的节点定位方法 - Google Patents

Abstract

本发明公开了一种结合水声网络协议的节点定位方法，所述方法包括：将水声网络节点按照一定规则布放，布放时为各个节点设定一个初始GPS值，在网络自组织过程中利用不需要精确时钟同步的类TOA方法完成节点之间的测距，将测距结果对节点的初始GPS值进行矫正，实现对节点的定位。本发明的方法在节点自组织过程中完成测距，且在节点布放水下后就无需时间同步，再通过矫正节点GPS值完成节点定位，减少了节点能量消耗，达到较为精准定位的目的。

Description

一种结合水声网络协议的节点定位方法

技术领域

本发明属于水下无线传感器网络节点定位领域，特别涉及一种结合水声网络协议的节点定位方法。

背景技术

将水声网络节点随机布放在水下感兴趣的区域内并形成传感器网络，而网络节点的定位技术是水下通信网络的支撑技术之一。目前，关于水下无线传感器网络定位技术方面的研究有多种分类方法，其中依据定位是否依赖节点间的距离测量可将定位算法分为基于测距的定位和基于非测距的定位。

基于测距的定位算法一般通过接收信号强度(RSSI)、到达时间(TOA)、到达时间差(TDOA)、接收信号角度(AOA)等距离测量手段得到节点间距离，然后利用节点的位置坐标之间的数学关系建立解算方程，求解得到节点的定位坐标。目前，基于测距的水下节点定位算法的研究有很多，文献[1](高峰.水下传感器网络定位算法及路由技术研究)提出了考虑翻转歧义的UWSN自上而下的节点定位改进方案，进一步降低了网络的平均定位误差。文献[2](Huai Huang，Yahong Rosa Zheng.Node localization with AoA assistance in multi-hop underwater sensor networks)提出了一种新的稀疏水下无线传感器网络中的节点定位方法，其中只有少量锚节点的位置可用。所提出的方法借助于路由节点的局部坐标系中的到达角(AOA)测量来估计从锚节点到多跳传感器节点的距离。文献[3](吕振，谭鹏立.一种基于RSSI校正的三角形质心定位算法)利用锚节点间的节点信息与三角形质心算法结合校正了RSSI值。文献[4](周伟，来鸿飞，刘忠.水下无线传感器网络的节点精确定位)提出了一种可实现网络规模升级的分布式无锚节点网络节点自定位机制，网内节点均无绝对坐标信息，通过TOA技术获取节点间的相对距离信息，并以网络节点分簇为基础，分别在簇内计算节点局部坐标，然后实现各簇间的坐标统一，形成整体相对坐标系统。

基于非测距的定位算法利用节点间连接关系或空间位置关系计算来对节点进行定位，例如DV-Hop定位、APIT定位等，由于基于非测距的定位算法定位精度较低，故在实际要求较高定位精度的定位中，大多采用基于测距的定位算法，但大多数基于测距的定位算法要求的设备复杂，且需要时间同步，这更使得节点能量损耗加快。

发明内容

本发明的发明目的在于克服水声网络潜标节点定位方法复杂、能量消耗大、精度不高的问题，将水声网络节点按照一定规则布放，布放时为各个节点设定一个粗略GPS值，自组织过程中利用不需要精确时钟同步的类TOA方法完成测距，结合粗略GPS值与测距结果进行运算，达到利用较少的能耗实现较为精准的节点自定位的目的。

为了实现上述目的，本发明提出了一种结合水声网络协议的节点定位方法，所述方法包括：将水声网络节点按照一定规则布放，布放时为各个节点设定一个初始GPS值，在网络自组织过程中利用不需要精确时钟同步的类TOA方法完成节点之间的测距，将测距结果对节点的初始GPS值进行矫正，实现对节点的定位。

作为上述方法的一种改进，所述方法具体包括：

步骤1)将水声网络节点按照一定规则布放，布放时为各个节点设定一个初始GPS值并进行初始化；

步骤2)每个节点利用找邻居和泛洪广播及问答获取能直接通信的两两节点的距离，由此生成路由矩阵和距离矩阵；

步骤3)利用距离矩阵对节点的初始GPS值进行矫正，实现对节点的定位。

作为上述方法的一种改进，所述步骤1)具体包括：

步骤1-1)设定水声网络包含的总节点数量T及本节点地址t；

总节点数量T应不大于当前地址、下跳地址、目的地址、跳数4项所占用的bit位数总和；本节点地址t为整数，t∈[0,T)，且应保证不同节点的t在网络中是唯一的；

步骤1-2)设定找邻居次数的上限L、询问次数的上限W；

步骤1-3)设定每个节点的延迟时间u，且所有节点延迟时间u不相同；

步骤1-4)设定每个节点的GPS初始值，包括纬度、经度，在此基础上设定GPS设定值；

步骤1-5)为每一个节点建立空白路由矩阵、空白路由表、空白距离矩阵、空白距离表和空白GPS表；所述路由矩阵用于记录由本节点出发如何访问到其他节点的路径的矩阵；所述距离矩阵用于记录两个能直接通信的节点间的距离的矩阵；

路由矩阵包含的行数、列数均等于总节点数量T；路由矩阵中的所有元素赋值为0；路由矩阵行、列序号均从0开始；路由表的所包含的项数也等于总节点数量T；路由表中每一项中至少应包含：目的节点地址、下一跳节点地址、到达目的节点所需要的跳数等信息；将所有节点的地址依次填写到路由表目的节点地址栏中；下一跳节点地址赋值为空值；到达目的节点所需要的跳数赋值为无限大；距离矩阵包含的行数、列数均等于总节点数量T；距离矩阵中的所有元素赋值为0；距离矩阵行、列序号均从0开始；GPS表分为纬度、经度两部分，每部分项数均等于总节点数量T；

步骤1-6)清零所有定时器、帧序号计数器、找邻居次数和询问次数；

步骤1-7)设置每个节点的找邻居中断定时器。

作为上述方法的一种改进，所述步骤2)具体包括：

步骤2-1)节点A进入找邻居空闲状态，等待中断，如果是找邻居中断，节点A查验找邻居次数，如果找邻居次数不小于L，则将找邻居次数归零，进入步骤2-2)，如果是数据接收中断，进行数据校验；在校验不正确的情况下，丢弃该数据，在校验正确的情况下，进入步骤2-4)，当前空闲状态为找邻居阶段空闲状态；

步骤2-2)节点A进入泛洪广播空闲状态，清零找邻居阶段定时器，设置泛洪广播中断定时器，进入泛洪广播阶段，等待中断，如果是泛洪广播中断，则生成并发送泛洪广播数据帧，进入步骤2-3)；如果是数据接收中断，进行数据校验；在校验不正确的情况下，丢弃该数据；在校验正确的情况下，进入步骤2-4)；当前空闲状态为泛洪广播阶段空闲状态；

步骤2-3)节点A进入问答阶段空闲状态，清零泛洪广播中断定时器，设置问答中断定时器，进入问答阶段，等待中断，如果是问答中断，如果路由矩阵已经完整或者询问次数不小于W，不做处理；否则，完善路由矩阵；如果是数据接收中断，进行数据校验；在校验不正确的情况下，丢弃该数据；在校验正确的情况下，进入步骤2-4)；当前空闲状态为问答阶段空闲状态；

步骤2-4)根据接收的数据类型进行数据处理，生成路由矩阵和距离矩阵。

作为上述方法的一种改进，所述步骤2-1)具体包括：

步骤2-1-1)节点A进入找邻居空闲状态，等待中断，如果是找邻居中断，节点A查验找邻居次数，如果找邻居次数不小于L，则将找邻居次数归零，进入步骤2-2)，否则，转入步骤2-1-2)；如果是数据接收中断，进行数据校验；在校验不正确的情况下，丢弃该数据，在校验正确的情况下，进入步骤2-4)，当前空闲状态为找邻居阶段空闲状态；

步骤2-1-2)根据“找邻居”的数据帧格式生成空数据帧，其中帧同步交由物理层赋值，数据类型赋值为“找邻居”，信源地址赋值为“本节点地址”，当前地址与下跳地址赋值为“无效”，目的地址赋值为“广播”，跳数赋值为1，帧序号赋值为帧序号计数器当前值，帧序号计数器加1，GPS赋值为本节点的“GPS设定值”，校验赋值为上述数据校验和；发送该数据帧后，记录下发送时间t1，找邻居次数加1，转入步骤2-1-1)。

作为上述方法的一种改进，所述步骤2-2)具体包括：

步骤2-2-1)节点A进入泛洪广播空闲状态，清零找邻居阶段定时器，设置泛洪广播中断定时器，进入泛洪广播阶段，等待中断，如果是泛洪广播中断，进入步骤2-2-2)，如果是数据接收中断，进行数据校验；在校验不正确的情况下，丢弃该数据；在校验正确的情况下，进入步骤2-4)；当前空闲状态为泛洪广播阶段空闲状态；

步骤2-2-2)根据“泛洪”的数据帧格式生成空数据帧，其中帧同步交由物理层赋值，数据类型赋值为“泛洪”，信源地址赋值为t，将路由矩阵第t列的所有元素的最低bit位按照行序号从低到高的顺序提取出来，再依次按照当前地址、下跳地址、目的地址、跳数的bit位从高到低的顺序赋值，不足的部分逐bit补0；帧序号赋值为帧序号计数器当前值，帧序号计数器加1，校验赋值为上述数据校验和，距离值赋值为距离表中第d列的所有元素；发送泛洪广播数据帧后，进入步骤2-3)。

作为上述方法的一种改进，所述步骤2-3)具体包括：

步骤2-3-1)节点A进入问答阶段空闲状态，清零泛洪广播中断定时器，设置问答中断定时器，进入问答阶段，等待中断，如果是问答中断，进入步骤2-3-2)，如果是数据接收中断，进行数据校验；在校验不正确的情况下，丢弃该数据；在校验正确的情况下，进入步骤2-4)；当前空闲状态为问答阶段空闲状态；

步骤2-3-2)查看节点的路由矩阵，若其每一列都已存在非零元素，表明路由矩阵已完整，则清零问答中断定时器；若仍有部分列的元素全为零，则查看询问次数，若询问次数不小于W，则清零问答中断定时器，若询问次数小于W，则按照步骤2-3-2)询问路由矩阵所缺少的某一列；

步骤2-3-3)记路由矩阵所缺少的列序号为x，根据“询问”的数据帧格式生成空数据帧，其中帧同步交由物理层赋值，数据类型赋值为“询问”，信源地址赋值为t，当前地址、下跳地址赋值为“无效”，目的地址赋值为x，跳数赋值为0，帧序号赋值为帧序号计数器当前值，帧序号计数器加1，校验赋值为上述数据校验和；发送该数据帧后，询问次数计数器+1，转入步骤2-3-1)。

作为上述方法的一种改进，所述步骤2-4)具体包括：

步骤2-4-1)如果数据类型是找邻居类型的数据，则进入步骤2-4-2)；如果数据类型是是回复类型的数据，则进入步骤2-4-3)；如果数据类型是泛洪类型的数据，则则进入步骤2-4-4)；如果数据类型是询问类型的数据，则进入步骤2-4-5)；如果数据类型是回答类型的数据，则进入步骤2-4-6)；

步骤2-4-2)读取信源地址的数值s，将路由矩阵中第s行第t列的元素置为1；读取GPS值，并将其记录到GPS表中对应信源地址s的位置；根据设定的自身的延迟时间，对应延迟u后发送回复包，帧格式为“回复”的数据帧格式，其中帧同步交由物理层赋值，数据类型赋值为“回复”，信源地址赋值为“本节点地址”，当前地址赋值为“本节点地址”，下跳地址赋值为s，目的地址赋值为s，跳数赋值为1，帧序号赋值为帧序号计数器当前值，帧序号计数器加1，GPS赋值为本节点的“GPS设定值”，延迟时间赋值为对应u值，校验赋值为上述数据校验和；发送该数据帧后，返回当前空闲状态；

步骤2-4-3)读取目的地址的数值d，若d与本节点地址t不同，丢弃该数据；若d与本节点地址t相同，读取信源地址s、GPS值、延迟u，将GPS值记录到GPS表中对应信源地址s的位置，记录收到“回复”包的时间t2，根据下述公式计算信源地址节点与本节点之间的距离：

其中，t1为本节点广播此次“找邻居”数据时的发送时间，c为海水中的声速；若找邻居次数小于L，将距离值对应信源地址s记录下来；若找邻居次数等于L，将记录的对应信源地址s的所有距离值取平均，将平均值记录到距离表中s行t列的位置；

步骤2-4-4)如果本节点已对该数据进行过中继广播，则返回当前空闲状态；如果本节点尚未对该数据进行过中继广播，则根据步骤2-4-5)更新路由矩阵对应列与距离矩阵对应列，并中继该广播数据，返回当前空闲状态；

步骤2-4-5)读取目的地址的数值d，查看路由矩阵第d列，若第d列的元素全为0，则不回答该询问，返回当前空闲状态；若第d列的元素不全为0，则根据“询问”的数据帧格式生成空数据帧，其中帧同步交由物理层赋值，数据类型赋值为“回答”，信源地址赋值为d，将路由矩阵第d列的所有元素的最低bit位按照行序号从低到高的顺序提取出来，再依次按照当前地址、下跳地址、目的地址、跳数的bit位从高到低的顺序赋值，不足的部分逐bit补0，帧序号赋值为帧序号计数器当前值，帧序号计数器加1，校验赋值为上述数据校验和，距离值赋值为距离表中第d列的所有元素；发送该数据帧后，回到当前空闲状态；

步骤2-4-6)读取信源地址s、当前地址的数值c、下跳地址的数值n、目的地址的数值d、跳数的数值h，距离值，并将c、n、d、h依次按照bit位从高到低的顺序排列后，提取前0到T-1个bit，依次赋值给路由矩阵第s列的从0到T-1元素的最低bit位，路由矩阵第s列从0到T-1元素的其余bit位全部为0，读取距离值，并依次赋值到距离表中的第d列。

作为上述方法的一种改进，所述步骤3)具体包括：

步骤3-1)判定组成三角形；

根据T行T列的距离矩阵，判定在此拓扑结构中，哪些节点可以组成三条边边长均已知的三角形；依次遍历距离矩阵的每一列，在遍历每一列时，按以下步骤进行：

a)记该列为j，j∈[0,T)，按从第0行到第T-1行的顺序找到第一个元素不为0的行数m，再按从第m+1行到第T-1行的顺序获取每个元素的值，若第n行元素不为0(n∈[m+1,T))，即节点j到节点m、节点j到节点n的距离均已知，则判断距离矩阵中第m行n列或第n行m列的元素是否为0，若为0则无需记录，若不为0则节点m到节点n的距离也已知，将j、m、n的数值按从小到大的顺序排列并记录下来；

b)继续按从第m+1行到第T-1行的顺序找到第二个元素不为0的行数p，再按从p+1行到第T行的顺序获取每个元素的值，若第q行元素不为0(q∈[p+1,T))，即节点j到节点p、节点j到节点q的距离均已知，则判断距离矩阵中第p行q列或第q行p列的元素是否为0，若为0则无需记录，若不为0则节点p到节点q的距离也已知，将j、p、q的数值按从小到大的顺序排列并记录下来；

c)依次按上述顺序查找并判断，完成此列的遍历；

d)在进行其他列的遍历时，将得到的3个节点的值与已记录下来的所有可组成三角形的节点值进行比较，若有重复的三角形，便舍弃此数据；

e)完成遍历，得到所有可以组成三条边边长均已知的三角形的节点值；

步骤3-2)一次矫正；

每个节点目前已知的有以下三项：可以组成三条边边长均已知的三角形的节点值、自身GPS设定值、收到的一跳内可通信节点的GPS设定值，在此基础上矫正每个三角形中三个节点的GPS，以下以由节点a b c组成的三角形为例，其中a、b、c∈[0,T)且a、b、c互不相等；

a)在三角形abc中，令顶点a为坐标原点，取节点b的GPS设定值与节点a的GPS设定值的差值作为b的相对坐标，为(x_b,y_b)，同理可得c的相对坐标为(x_c,y_c)；

b)按以下公式在边ab上根据ab将距离及ab的斜率矫正b：

其中，(x_b1,y_b1)为b的第一对矫正值，d_ab为ab间距离；

c)按以下公式根据ac间距离，bc间距离，通过两圆相交求交点矫正c：

其中，(x_c1,y_c1)为c的第一对矫正值，d_ac为ac间距离，d_bc为bc间距离；

d)按以下公式在边ac上根据ac间距离及ac的斜率矫正c：

其中，(x_c2,y_c2)为c的第二对矫正值；

e)按以下公式根据ab间距离，cb间距离，通过两圆相交求交点矫正b：

其中，(x_b2,y_b2)为b的第二对矫正值；

f)再分别以b、c为坐标原点，重复步骤a)～e)分别矫正a、c与a、b，矫正完成后，在一个三角形中，三个顶点各有三对矫正值；

g)遍历所有三角形，将每个节点的所有矫正值都对应记录下来，最后将节点所有矫正值与其设定值的平均值作为一次矫正的结果；

步骤3-3)二次矫正；

设定三角形abc的质心为O；经过一次矫正后的三角形顶点为a’、b’、c’，其质心为O’；经过二次矫正后的三角形顶点为a”、b”、c”，其质心为O”；

a)利用设定三角形顶点的设定GPS值，构成设定三角形abc，求出其质心的位置O：

b)利用第一次矫正得到的三角形顶点的一次矫正GPS值，构成一次矫正三角形a’b’c’，求出其质心的位置O’：

c)计算一次矫正三角形质心O’对于设定三角形质心O的相对位置坐标，将此结果作为二次矫正三角形顶点对于一次矫正三角形顶点的相对位置坐标，以此求得二次矫正的结果：

O_相对gps＝O′_gps-O_gps

a″_gps＝a′_gps-O_相对gps

b″_gps＝b′_gps-O_相对gps

c″_gps＝c′_gps-O_相对gps

d)将每个三角形均按步骤a)～c)进行矫正，进而完成所有节点的二次矫正；完成网络节点自定位。

本发明的优势在于：

本发明的方法在节点自组织过程中完成测距，且在节点布放水下后就无需时间同步，再通过矫正节点GPS值完成节点定位，减少了节点能量消耗，达到较为精准定位的目的。

附图说明

图1为本发明的结合水声网络协议的节点定位方法的流程图；

图2为本发明的数据帧格式图；

图3为本发明的数据处理流程图；

图4为本发明的路由矩阵列元素提取方法示意图；

图5为本发明的路由矩阵列元素更新方法示意图；

图6为OPNET仿真中的节点模型示意图；

图7为软件仿真中采用的5节点星型拓扑结构图；

图8为软件仿真中采用的5节点蜂窝式拓扑结构图；

图9为软件仿真中采用的8节蜂窝式拓扑结构图；

图10为软件仿真中5节点星型拓扑结构设定值偏差1图；

图11为软件仿真中5节点星型拓扑结构设定值偏差2图。

具体实施方式

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

如图1所示，本发明的结合水声网络协议的节点定位方法，是在网路自组织过程中完成测距，进而在此基础上完成定位，所述方法包括以下步骤：

一、初始化

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

将水声网络节点按照一定规则布放，布放时为各个节点设定一个粗略的GPS值，自组织过程中利用不需要精确时钟同步的类TOA方法完成测距，结合粗略GPS值与测距结果进行运算，达到利用较少的能耗实现较为精准的节点自定位的目的。

总节点数量T应不大于图2中当前地址、下跳地址、目的地址、跳数4项所占用的bit位数总和；

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

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

2)设定找邻居次数的上限L、询问次数的上限W

初始化完毕后，L与W均不能修改；

3)设定每个节点的延迟时间u，且所有节点延迟时间u各不相同

初始化完毕后，所有u均不能修改；

4)设定每个节点的GPS真实值，包括纬度、经度，在此基础上设定GPS设定值

初始化完毕后，所有GPS均不能修改；

5)每一个节点建立空白路由矩阵、空白路由表、空白距离矩阵、空白GPS表

路由矩阵包含的行数、列数均等于总节点数量T；路由矩阵中的所有元素赋值为0；路由矩阵行、列序号均从0开始；路由表的所包含的项数也等于总节点数量T；路由表中每一项中至少应包含：目的节点地址、下一跳节点地址、到达目的节点所需要的跳数等信息；将所有节点的地址依次填写到路由表目的节点地址栏中；下一跳节点地址赋值为空值；到达目的节点所需要的跳数赋值为无限大；距离矩阵包含的行数、列数均等于总节点数量T；距离矩阵中的所有元素赋值为0；距离矩阵行、列序号均从0开始；GPS表分为纬度、经度两部分，每部分项数均等于总节点数量T；

6)清零所有定时器、帧序号计数器、找邻居次数、询问次数；

7)设置找邻居中断定时器

根据本节点地址进行设置，不同节点的中断时间不同；设置完毕后进入找邻居阶段空闲状态；

二、找邻居阶段空闲状态

1)等待中断、判断中断类型并响应中断

a)如果是找邻居中断，则查验找邻居次数，如果找邻居次数不小于L，则将找邻居次数归零，进入泛洪广播阶段空闲状态，清零找邻居阶段定时器，设置泛洪广播中断定时器；如果找邻居次数小于L，则参考图2所示中“找邻居”的数据帧格式生成空数据帧，其中帧同步交由物理层赋值，不在本方法规定范围之内，数据类型赋值为“找邻居”，信源地址赋值为“本节点地址”，当前地址与下跳地址赋值为“无效”，目的地址赋值为“广播”，跳数赋值为1，帧序号赋值为帧序号计数器当前值，帧序号计数器加1，GPS赋值为本节点的“GPS设定值”，校验赋值为上述数据校验和；发送该数据帧后，记录下发送时间t1，找邻居次数加1，回到找邻居节点空闲状态；

b)如果是数据接收中断，进行数据校验；在校验不正确的情况下，丢弃该数据，不做任何处理；在校验正确的情况下，进入步骤三所描述的数据处理流程，如图3所示；当前空闲状态为找邻居阶段空闲状态；

三、数据处理流程

1)判断数据类型，根据数据类型处理

如果是找邻居类型的数据，则根据步骤a)进行数据处理；如果是回复类型的数据，则根据步骤b)进行数据处理；如果是泛洪类型的数据，则根据步骤c)进行数据处理；如果是询问类型的数据，则根据步骤d)进行数据处理；如果是回答类型的数据，则根据步骤e)进行数据处理；

a)读取信源地址的数值s，将路由矩阵中第s行第t列的元素置为1；读取GPS值，并将其记录到GPS表中对应信源地址s的位置；根据设定的自身的延迟时间，对应延迟u后发送回复包，帧格式参考图2所示中“回复”的数据帧格式，其中帧同步交由物理层赋值，不在本方法规定范围之内，数据类型赋值为“回复”，信源地址赋值为“本节点地址”，当前地址赋值为“本节点地址”，下跳地址赋值为s，目的地址赋值为s，跳数赋值为1，帧序号赋值为帧序号计数器当前值，帧序号计数器加1，GPS赋值为本节点的“GPS设定值”，延迟时间赋值为对应u值，校验赋值为上述数据校验和；发送该数据帧后，返回当前空闲状态；

b)读取目的地址的数值d，若d与本节点地址t不同，丢弃该数据，不做任何处理；若d与本节点地址t相同，读取信源地址s、GPS值、延迟u，将GPS值记录到GPS表中对应信源地址s的位置，记录收到“回复”包的时间t2，根据下述公式计算信源地址节点与本节点之间的距离：

其中，t1为本节点广播此次“找邻居”数据时的发送时间，c为海水中的声速。

若找邻居次数小于L，将距离值对应信源地址s记录下来；若找邻居次数等于L，将记录的对应信源地址s的所有距离值取平均，将平均值记录到距离表中s行t列的位置。

c)如果本节点已对该数据进行过中继广播，则返回当前空闲状态；如果本节点尚未对该数据进行过中继广播，则根据步骤d)更新路由矩阵对应列与距离矩阵对应列，并中继该广播数据，返回当前空闲状态；

d)读取目的地址的数值d，查看路由矩阵第d列，若第d列的元素全为0，则不回答该询问，返回当前空闲状态；若第d列的元素不全为0，则参考附图2中“询问”的所示数据帧格式生成空数据帧，其中帧同步交由物理层赋值，不在本方法规定范围之内，数据类型赋值为“回答”，信源地址赋值为d，将路由矩阵第d列的所有元素的最低bit位按照行序号从低到高的顺序提取出来，再依次按照当前地址、下跳地址、目的地址、跳数的bit位从高到低的顺序赋值，不足的部分逐bit补0，具体方法参考图4，帧序号赋值为帧序号计数器当前值，帧序号计数器加1，校验赋值为上述数据校验和，距离值赋值为距离表中第d列的所有元素；发送该数据帧后，回到当前空闲状态；

e)读取信源地址s、当前地址的数值c、下跳地址的数值n、目的地址的数值d、跳数的数值h，距离值，并将c、n、d、h依次按照bit位从高到低的顺序排列后，提取前0到T-1个bit，依次赋值给路由矩阵第s列的从0到T-1元素的最低bit位，路由矩阵第s列从0到T-1元素的其余bit位全部为0，具体方法参考图5，读取距离值，并依次赋值到距离表中的第d列；

四、泛洪广播阶段空闲状态

1)等待中断、判断中断类型并响应中断

a)如果是泛洪广播中断，则参考图2中“泛洪”的所示数据帧格式生成空数据帧，其中帧同步交由物理层赋值，不在本方法规定范围之内，数据类型赋值为“泛洪”，信源地址赋值为t，将路由矩阵第t列的所有元素的最低bit位按照行序号从低到高的顺序提取出来，再依次按照当前地址、下跳地址、目的地址、跳数的bit位从高到低的顺序赋值，不足的部分逐bit补0，具体方法参考图4，帧序号赋值为帧序号计数器当前值，帧序号计数器加1，校验赋值为上述数据校验和，距离值赋值为距离表中第d列的所有元素；发送泛洪广播数据帧后，清零泛洪广播中断定时器，设置问答中断定时器，进入问答阶段空闲状态；

b)如果是数据接收中断，进行数据校验；在校验不正确的情况下，丢弃该数据，不做任何处理；在校验正确的情况下，进入步骤三所描述的数据处理流程，如图3所示；当前空闲状态为泛洪广播阶段空闲状态；

五、问答阶段空闲状态

1)等待中断、判断中断类型并响应中断

a)如果是问答中断，则查看路由矩阵，若其每一列都已存在非零元素，表明路由矩阵已完整，则清零问答中断定时器，进入步骤六，根据路由矩阵生成路由表；若仍有部分列的元素全为零，则查看询问次数，若询问次数不小于W，则清零问答中断定时器，进入步骤六根据路由矩阵生成路由表，若询问次数小于W，则按照步骤b)询问路由矩阵所缺少的某一列；

b)记路由矩阵所缺少的列序号为x，参考图2中“询问”的所示数据帧格式生成空数据帧，其中帧同步交由物理层赋值，不在本发明规定范围之内，数据类型赋值为“询问”，信源地址赋值为t，当前地址、下跳地址赋值为“无效”，目的地址赋值为x，跳数赋值为0，帧序号赋值为帧序号计数器当前值，帧序号计数器加1，校验赋值为上述数据校验和；发送该数据帧后，询问次数计数器+1，回到问答阶段空闲状态；

c)如果是数据接收中断，进行数据校验；在校验不正确的情况下，丢弃该数据，不做任何处理；在校验正确的情况下，进入步骤三所描述的数据处理流程，如附图3所示；当前空闲状态为问答阶段空闲状态；

d)当每个节点的路由矩阵都是完整的时，距离矩阵也都是完整的，即完成了测距；

六、根据路由矩阵生成路由表

1)将路由矩阵中所有0元素替换为无限大，利用经典最短路径算法，例如Dijkstra算法，依次计算以本节点为信源地址、其他节点为目的地址的最短路由路径，计算结果为每一条路由路径的下一跳交付节点地址和到达目的地址所需要的跳数，并按照步骤2)将计算结果赋值到路由表中；

2)以目的节点y为例，路由表的赋值方法

a)如果y等于t，将路由表中第y项的下一跳节点地址赋值为t，到达目的节点所需要的跳数赋值为0；

b)如果y不等于t、而且计算结果中对应项的下一跳交付节点地址以及到达目的节点所需要的跳数均不为无限大，那么将路由表中第y项的下一跳节点地址和到达目的节点所需要的跳数分别赋值为计算结果中对应项的下一跳交付节点地址以及到达目的节点所需要的跳数；

c)如果y不等于t、而且计算结果中对应项的下一跳交付节点地址或者到达目的节点所需要的跳数为无限大，那么将路由表中第y项的下一跳节点地址赋值为“广播”，到达目的节点所需要的跳数赋值为T；

3)将路由矩阵中所有无限大元素替换回0；

七、定位：

1)判定组成三角形；

根据T行T列的距离矩阵，判定在此拓扑结构中，哪些节点可以组成三条边边长均已知的三角形；依次遍历距离矩阵的每一列，在遍历每一列时，按以下步骤进行：

a)记该列为j，j∈[0,T)，按从第0行到第T-1行的顺序找到第一个元素不为0的行数m，再按从第m+1行到第T-1行的顺序获取每个元素的值，若第n行元素不为0(n∈[m+1,T))，即节点j到节点m、节点j到节点n的距离均已知，则判断距离矩阵中第m行n列或第n行m列的元素是否为0，若为0则无需记录，若不为0则节点m到节点n的距离也已知，将j、m、n的数值按从小到大的顺序排列并记录下来；

b)继续按从第m+1行到第T-1行的顺序找到第二个元素不为0的行数p，再按从p+1行到第T行的顺序获取每个元素的值，若第q行元素不为0(q∈[p+1,T))，即节点j到节点p、节点j到节点q的距离均已知，则判断距离矩阵中第p行q列或第q行p列的元素是否为0，若为0则无需记录，若不为0则节点p到节点q的距离也已知，将j、p、q的数值按从小到大的顺序排列并记录下来；

c)依次按上述顺序查找并判断，完成此列的遍历；

d)在进行其他列的遍历时，将得到的3个节点的值与已记录下来的所有可组成三角形的节点值进行比较，若有重复的三角形，便舍弃此数据；

e)完成遍历，得到所有可以组成三条边边长均已知的三角形的节点值；

2)一次矫正；

每个节点目前已知的有以下三项：可以组成三条边边长均已知的三角形的节点值、自身GPS设定值、收到的一跳内可通信节点的GPS设定值，在此基础上矫正每个三角形中三个节点的GPS，以下以由节点a b c组成的三角形为例，其中a、b、c∈[0,T)且a、b、c互不相等；

a)在三角形abc中，令顶点a为坐标原点，取节点b的GPS设定值与节点a的GPS设定值的差值作为b的相对坐标，为(x_b,y_b)，同理可得c的相对坐标为(x_c,y_c)；

b)按以下公式在边ab上根据ab将距离及ab的斜率矫正b：

其中，(x_b1,y_b1)为b的第一对矫正值，d_ab为ab间距离；

c)按以下公式根据ac间距离，bc间距离，通过两圆相交求交点矫正c：

其中，(x_c1,y_c1)为c的第一对矫正值，d_ac为ac间距离，d_bc为bc间距离；

d)按以下公式在边ac上根据ac间距离及ac的斜率矫正c：

其中，(x_c2,y_c2)为c的第二对矫正值；

e)按以下公式根据ab间距离，cb间距离，通过两圆相交求交点矫正b：

其中，(x_b2,y_b2)为b的第二对矫正值；

f)再分别以b、c为坐标原点，重复步骤a)～e)分别矫正a、c与a、b，矫正完成后，在一个三角形中，三个顶点各有三对矫正值；

g)遍历所有三角形，将每个节点的所有矫正值都对应记录下来，最后将节点所有矫正值与其设定值的平均值作为一次矫正的结果；

3)二次矫正；

设定三角形abc的质心为O；经过一次矫正后的三角形顶点为a’、b’、c’，其质心为O’；经过二次矫正后的三角形顶点为a”、b”、c”，其质心为O”；

a)利用设定三角形顶点的设定GPS值，构成设定三角形abc，求出其质心的位置O：

b)利用第一次矫正得到的三角形顶点的一次矫正GPS值，构成一次矫正三角形a’b’c’，求出其质心的位置O’：

c)计算一次矫正三角形质心O’对于设定三角形质心O的相对位置坐标，将此结果作为二次矫正三角形顶点对于一次矫正三角形顶点的相对位置坐标，以此求得二次矫正的结果：

O_相对gps＝O′_gps-O_gps

a″_gps＝a′_gps-O_相对gps

b″_gps＝b′_gps-O_相对gps

c″_gps＝c′_gps-O_相对gps

d)将每个三角形均按步骤a)～c)进行矫正，进而完成所有节点的二次矫正；完成网络节点自定位。

八、完成网络节点自定位。

本网络节点定位方法利用OPNET软件，针对条件比较恶劣的水声传感器网络进行仿真，并设计半双工的水声通信方式。在图7-图9所示几种典型的拓扑结构下进行软件仿真，并对仿真结果进行分析。

一、半双工水声通信设计

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

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

二、仿真结果分析

在图7-图9所示几种典型的拓扑结构下进行软件仿真，并分析了节点在不同情况下的定位误差。

在自组织完成后，所有节点的距离矩阵均已完整，表1列出了本发明的方法中5节点星型拓扑情况下的距离矩阵。

表1本发明方法中5节点星型拓扑情况下的距离矩阵(单位：km)

测距完成后，进行GPS矫正，并设定3种GPS设定值与真实值的偏差情况，其中第1种如图10所示，第2种如图11所示，即所有节点向同一个方向偏，第3种为随机设定。将5节点蜂窝式拓扑结构同样设定此3种情况，8节点只设定一种情况，即第3中，其拓扑结构随机设定。

表2列出了这几种拓扑结构在不同设定值偏向的定位误差与设定值误差之间的比较，其中平均相对误差可用下式得出：

其中，矫正误差与设定误差有下式得出：

矫正误差＝|矫正GPS值-真实GPS值|

设定误差＝|设定GPS值-真实GPS值|

因此，若平均相对误差为正值，即经过矫正后矫正值与真实值间的平均差距比设定值与真实值间的平均差距大，说明矫正效果并不好；若平均相对误差为负值，即经过矫正后矫正值与真实值间的平均差距比设定值与真实值间的平均差距小，说明矫正效果好，而且平均相对误差越小，说明矫正值越接近真实值，矫正效果越好。

表2不同拓扑结构不同设定值偏向下各结果相对误差

从表2可以看出，除去设定值均向一个方向偏的第2种情况，其他的平均相对误差均为负值，即矫正值更接近真实值，且误差均减小了大约一半；并且随着网络中节点个数的增加，所能组成的三角形个数在增加，节点可以进行GPS矫正的次数也在增加，可以看到平均相对误差也在逐渐减小，说明矫正值越来越接近真实值。

综上所述，本方法中通过结合网络协议，在自组织过程中完成节点间测距，进而相对于节点布放时记录的一个模糊GPS，即设定值，在用本方法进行矫正后，其GPS更加精确，误差减小了一半，有效提高了定位精度

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

Claims (1)

1.一种结合水声网络协议的节点定位方法，所述方法包括：将水声网络节点按照一定规则布放，布放时为各个节点设定一个初始GPS值，在网络自组织过程中利用不需要精确时钟同步的类TOA方法完成节点之间的测距，将测距结果对节点的初始GPS值进行矫正，实现对节点的定位；

所述方法具体包括：

步骤1)将水声网络节点按照一定规则布放，布放时为各个节点设定一个初始GPS值并进行初始化；

步骤2)每个节点利用找邻居和泛洪广播及问答获取能直接通信的两两节点的距离，由此生成路由矩阵和距离矩阵；

步骤3)利用距离矩阵对节点的初始GPS值进行矫正，实现对节点的定位；

所述步骤1)具体包括：

步骤1-1)设定水声网络包含的总节点数量T及本节点地址t；

总节点数量T应不大于当前地址、下跳地址、目的地址、跳数4项所占用的bit位数总和；本节点地址t为整数，t∈[0，T)，且应保证不同节点的t在网络中是唯一的；

步骤1-2)设定找邻居次数的上限L、询问次数的上限W；

步骤1-3)设定每个节点的延迟时间u，且所有节点延迟时间u不相同；

步骤1-4)设定每个节点的GPS初始值，包括纬度、经度，在此基础上设定GPS设定值；

步骤1-5)为每一个节点建立空白路由矩阵、空白路由表、空白距离矩阵、空白距离表和空白GPS表；所述路由矩阵用于记录由本节点出发如何访问到其他节点的路径的矩阵；所述距离矩阵用于记录两个能直接通信的节点间的距离的矩阵；

路由矩阵包含的行数、列数均等于总节点数量T；路由矩阵中的所有元素赋值为0；路由矩阵行、列序号均从0开始；路由表的所包含的项数也等于总节点数量T；路由表中每一项中至少应包含：目的节点地址、下一跳节点地址、到达目的节点所需要的跳数信息；将所有节点的地址依次填写到路由表目的节点地址栏中；下一跳节点地址赋值为空值；到达目的节点所需要的跳数赋值为无限大；距离矩阵包含的行数、列数均等于总节点数量T；距离矩阵中的所有元素赋值为0；距离矩阵行、列序号均从0开始；GPS表分为纬度、经度两部分，每部分项数均等于总节点数量T；

步骤1-6)清零所有定时器、帧序号计数器、找邻居次数和询问次数；

步骤1-7)设置每个节点的找邻居中断定时器；

所述步骤2)具体包括：

步骤2-1)节点A进入找邻居空闲状态，等待中断，如果是找邻居中断，节点A查验找邻居次数，如果找邻居次数不小于L，则将找邻居次数归零，进入步骤2-2)，如果是数据接收中断，进行数据校验；在校验不正确的情况下，丢弃该数据，在校验正确的情况下，进入步骤2-4)，当前空闲状态为找邻居阶段空闲状态；

步骤2-2)节点A进入泛洪广播空闲状态，清零找邻居阶段定时器，设置泛洪广播中断定时器，进入泛洪广播阶段，等待中断，如果是泛洪广播中断，则生成并发送泛洪广播数据帧，进入步骤2-3)；如果是数据接收中断，进行数据校验；在校验不正确的情况下，丢弃该数据；在校验正确的情况下，进入步骤2-4)；当前空闲状态为泛洪广播阶段空闲状态；

步骤2-3)节点A进入问答阶段空闲状态，清零泛洪广播中断定时器，设置问答中断定时器，进入问答阶段，等待中断，如果是问答中断，如果路由矩阵已经完整或者询问次数不小于W，不做处理；否则，完善路由矩阵；如果是数据接收中断，进行数据校验；在校验不正确的情况下，丢弃该数据；在校验正确的情况下，进入步骤2-4)；当前空闲状态为问答阶段空闲状态；

步骤2-4)根据接收的数据类型进行数据处理，生成路由矩阵和距离矩阵；

所述步骤2-1)具体包括：

步骤2-1-1)节点A进入找邻居空闲状态，等待中断，如果是找邻居中断，节点A查验找邻居次数，如果找邻居次数不小于L，则将找邻居次数归零，进入步骤2-2)，否则，转入步骤2-1-2)；如果是数据接收中断，进行数据校验；在校验不正确的情况下，丢弃该数据，在校验正确的情况下，进入步骤2-4)，当前空闲状态为找邻居阶段空闲状态；

步骤2-1-2)根据“找邻居”的数据帧格式生成空数据帧，其中帧同步交由物理层赋值，数据类型赋值为“找邻居”，信源地址赋值为“本节点地址”，当前地址与下跳地址赋值为“无效”，目的地址赋值为“广播”，跳数赋值为1，帧序号赋值为帧序号计数器当前值，帧序号计数器加1，GPS赋值为本节点的“GPS设定值”，校验赋值为上述数据校验和；发送该数据帧后，记录下发送时间t1，找邻居次数加1，转入步骤2-1-1)；

所述步骤2-2)具体包括：

步骤2-2-1)节点A进入泛洪广播空闲状态，清零找邻居阶段定时器，设置泛洪广播中断定时器，进入泛洪广播阶段，等待中断，如果是泛洪广播中断，进入步骤2-2-2)，如果是数据接收中断，进行数据校验；在校验不正确的情况下，丢弃该数据；在校验正确的情况下，进入步骤2-4)；当前空闲状态为泛洪广播阶段空闲状态；

步骤2-2-2)根据“泛洪”的数据帧格式生成空数据帧，其中帧同步交由物理层赋值，数据类型赋值为“泛洪”，信源地址赋值为t，将路由矩阵第t列的所有元素的最低bit位按照行序号从低到高的顺序提取出来，再依次按照当前地址、下跳地址、目的地址、跳数的bit位从高到低的顺序赋值，不足的部分逐bit补0；帧序号赋值为帧序号计数器当前值，帧序号计数器加1，校验赋值为上述数据校验和，距离值赋值为距离表中第d列的所有元素；发送泛洪广播数据帧后，进入步骤2-3)；

所述步骤2-3)具体包括：

步骤2-3-1)节点A进入问答阶段空闲状态，清零泛洪广播中断定时器，设置问答中断定时器，进入问答阶段，等待中断，如果是问答中断，进入步骤2-3-2)，如果是数据接收中断，进行数据校验；在校验不正确的情况下，丢弃该数据；在校验正确的情况下，进入步骤2-4)；当前空闲状态为问答阶段空闲状态；

步骤2-3-2)查看节点的路由矩阵，若其每一列都已存在非零元素，表明路由矩阵已完整，则清零问答中断定时器；若仍有部分列的元素全为零，则查看询问次数，若询问次数不小于W，则清零问答中断定时器，若询问次数小于W，则按照步骤2-3-3)询问路由矩阵所缺少的某一列；

步骤2-3-3)记路由矩阵所缺少的列序号为x，根据“询问”的数据帧格式生成空数据帧，其中帧同步交由物理层赋值，数据类型赋值为“询问”，信源地址赋值为t，当前地址、下跳地址赋值为“无效”，目的地址赋值为x，跳数赋值为0，帧序号赋值为帧序号计数器当前值，帧序号计数器加1，校验赋值为上述数据校验和；发送该数据帧后，询问次数计数器+1，转入步骤2-3-1)；

所述步骤2-4)具体包括：

步骤2-4-1)如果数据类型是找邻居类型的数据，则进入步骤2-4-2)；如果数据类型是是回复类型的数据，则进入步骤2-4-3)；如果数据类型是泛洪类型的数据，则则进入步骤2-4-4)；如果数据类型是询问类型的数据，则进入步骤2-4-5)；如果数据类型是回答类型的数据，则进入步骤2-4-6)；

步骤2-4-2)读取信源地址的数值s，将路由矩阵中第s行第t列的元素置为1；读取GPS值，并将其记录到GPS表中对应信源地址s的位置；根据设定的自身的延迟时间，对应延迟u后发送回复包，帧格式为“回复”的数据帧格式，其中帧同步交由物理层赋值，数据类型赋值为“回复”，信源地址赋值为“本节点地址”，当前地址赋值为“本节点地址”，下跳地址赋值为s，目的地址赋值为s，跳数赋值为1，帧序号赋值为帧序号计数器当前值，帧序号计数器加1，GPS赋值为本节点的“GPS设定值”，延迟时间赋值为对应u值，校验赋值为上述数据校验和；发送该数据帧后，返回当前空闲状态；

步骤2-4-3)读取目的地址的数值d，若d与本节点地址t不同，丢弃该数据；若d与本节点地址t相同，读取信源地址s、GPS值、延迟u，将GPS值记录到GPS表中对应信源地址s的位置，记录收到“回复”包的时间t2，根据下述公式计算信源地址节点与本节点之间的距离：

其中，t1为本节点广播此次“找邻居”数据时的发送时间，c为海水中的声速；若找邻居次数小于L，将距离值对应信源地址s记录下来；若找邻居次数等于L，将记录的对应信源地址s的所有距离值取平均，将平均值记录到距离表中s行t列的位置；

步骤2-4-4)如果本节点已对该数据进行过中继广播，则返回当前空闲状态；如果本节点尚未对该数据进行过中继广播，则根据步骤2-4-5)更新路由矩阵对应列与距离矩阵对应列，并中继该广播数据，返回当前空闲状态；

步骤2-4-5)读取目的地址的数值d，查看路由矩阵第d列，若第d列的元素全为0，则不回答该询问，返回当前空闲状态；若第d列的元素不全为0，则根据“询问”的数据帧格式生成空数据帧，其中帧同步交由物理层赋值，数据类型赋值为“回答”，信源地址赋值为d，将路由矩阵第d列的所有元素的最低bit位按照行序号从低到高的顺序提取出来，再依次按照当前地址、下跳地址、目的地址、跳数的bit位从高到低的顺序赋值，不足的部分逐bit补0，帧序号赋值为帧序号计数器当前值，帧序号计数器加1，校验赋值为上述数据校验和，距离值赋值为距离表中第d列的所有元素；发送该数据帧后，回到当前空闲状态；

步骤2-4-6)读取信源地址s、当前地址的数值c、下跳地址的数值n、目的地址的数值d、跳数的数值h，距离值，并将c、n、d、h依次按照bit位从高到低的顺序排列后，提取前0到T-1个bit，依次赋值给路由矩阵第s列的从0到T-1元素的最低bit位，路由矩阵第s列从0到T-1元素的其余bit位全部为0，读取距离值，并依次赋值到距离表中的第d列；

所述步骤3)具体包括：

步骤3-1)判定组成三角形；

根据T行T列的距离矩阵，判定在此拓扑结构中，哪些节点可以组成三条边边长均已知的三角形；依次遍历距离矩阵的每一列，在遍历每一列时，按以下步骤进行：

a)记该列为j，j∈[0，T)，按从第0行到第T-1行的顺序找到第一个元素不为0的行数m，再按从第m+1行到第T-1行的顺序获取每个元素的值，若第n行元素不为0(n∈[m+1，T))，即节点j到节点m、节点j到节点n的距离均已知，则判断距离矩阵中第m行n列或第n行m列的元素是否为0，若为0则无需记录，若不为0则节点m到节点n的距离也已知，将j、m、n的数值按从小到大的顺序排列并记录下来；

b)继续按从第m+1行到第T-1行的顺序找到第二个元素不为0的行数p，再按从p+1行到第T行的顺序获取每个元素的值，若第q行元素不为0(q∈[p+1，T))，即节点j到节点p、节点j到节点q的距离均已知，则判断距离矩阵中第p行q列或第q行p列的元素是否为0，若为0则无需记录，若不为0则节点p到节点q的距离也已知，将j、p、q的数值按从小到大的顺序排列并记录下来；

c)依次按上述顺序查找并判断，完成此列的遍历；

d)在进行其他列的遍历时，将得到的3个节点的值与已记录下来的所有可组成三角形的节点值进行比较，若有重复的三角形，便舍弃此数据；

e)完成遍历，得到所有可以组成三条边边长均已知的三角形的节点值；

步骤3-2)一次矫正；

每个节点目前已知的有以下三项：可以组成三条边边长均已知的三角形的节点值、自身GPS设定值、收到的一跳内可通信节点的GPS设定值，在此基础上矫正每个三角形中三个节点的GPS，以下以由节点a b c组成的三角形为例，其中a、b、c∈[0，T)且a、b、c互不相等；

a)在三角形abc中，令顶点a为坐标原点，取节点b的GPS设定值与节点a的GPS设定值的差值作为b的相对坐标，为(x_b，y_b)，同理可得c的相对坐标为(x_c，y_c)；

b)按以下公式在边ab上根据ab将距离及ab的斜率矫正b：

其中，(x_b1，y_b1)为b的第一对矫正值，d_ab为ab间距离；

c)按以下公式根据ac间距离，bc间距离，通过两圆相交求交点矫正c：

其中，(x_c1，y_c1)为c的第一对矫正值，d_ac为ac间距离，d_bc为bc间距离；

d)按以下公式在边ac上根据ac间距离及ac的斜率矫正c：

其中，(x_c2，y_c2)为c的第二对矫正值；

e)按以下公式根据ab间距离，cb间距离，通过两圆相交求交点矫正b：

其中，(x_b2，y_b2)为b的第二对矫正值；

f)再分别以b、c为坐标原点，重复步骤a)～e)分别矫正a、c与a、b，矫正完成后，在一个三角形中，三个顶点各有三对矫正值；

g)遍历所有三角形，将每个节点的所有矫正值都对应记录下来，最后将节点所有矫正值与其设定值的平均值作为一次矫正的结果；

步骤3-3)二次矫正；

设定三角形abc的质心为O；经过一次矫正后的三角形顶点为a’、b’、c’，其质心为O’；经过二次矫正后的三角形顶点为a”、b”、c”，其质心为O”；

a)利用设定三角形顶点的设定GPS值，构成设定三角形abc，求出其质心的位置O：

b)利用第一次矫正得到的三角形顶点的一次矫正GPS值，构成一次矫正三角形a’b’c’，求出其质心的位置O’：

c)计算一次矫正三角形质心O’对于设定三角形质心O的相对位置坐标，将此结果作为二次矫正三角形顶点对于一次矫正三角形顶点的相对位置坐标，以此求得二次矫正的结果：

O_相对gps＝O′_gps-O_gps

a″_gps＝a′_gps-O_相对gps

b″_gps＝b′_gps-O_相对gps

c″_gps＝c′_gps-O_相对gps

d)将每个三角形均按步骤a)～c)进行矫正，进而完成所有节点的二次矫正；完成网络节点自定位。

Images