CN111093265B - 一种基于到达角度测距的协作定位方法及装置 - Google Patents

一种基于到达角度测距的协作定位方法及装置 Download PDF

Info

Publication number
CN111093265B
CN111093265B CN201911227938.6A CN201911227938A CN111093265B CN 111093265 B CN111093265 B CN 111093265B CN 201911227938 A CN201911227938 A CN 201911227938A CN 111093265 B CN111093265 B CN 111093265B
Authority
CN
China
Prior art keywords
node
currently
mth
angle
arrival
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN201911227938.6A
Other languages
English (en)
Other versions
CN111093265A (zh
Inventor
王生楚
姜贤波
聂珂
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Beijing University of Posts and Telecommunications
Original Assignee
Beijing University of Posts and Telecommunications
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Beijing University of Posts and Telecommunications filed Critical Beijing University of Posts and Telecommunications
Priority to CN201911227938.6A priority Critical patent/CN111093265B/zh
Publication of CN111093265A publication Critical patent/CN111093265A/zh
Application granted granted Critical
Publication of CN111093265B publication Critical patent/CN111093265B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • H—ELECTRICITY
    • H04—ELECTRIC COMMUNICATION TECHNIQUE
    • H04W—WIRELESS COMMUNICATION NETWORKS
    • H04W64/00—Locating users or terminals or network equipment for network management purposes, e.g. mobility management
    • H04W64/006—Locating users or terminals or network equipment for network management purposes, e.g. mobility management with additional information processing, e.g. for direction or speed determination
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S5/00—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations
    • G01S5/02—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations using radio waves
    • G01S5/0257—Hybrid positioning
    • H—ELECTRICITY
    • H04—ELECTRIC COMMUNICATION TECHNIQUE
    • H04W—WIRELESS COMMUNICATION NETWORKS
    • H04W84/00—Network topologies
    • H04W84/18—Self-organising networks, e.g. ad-hoc networks or sensor networks

Landscapes

  • Engineering & Computer Science (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Signal Processing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Position Fixing By Use Of Radio Waves (AREA)

Abstract

本发明实施例提供了一种基于到达角度测距的协作定位方法及装置，其中方法包括：获取当前待定位节点的初始位置坐标；基于当前待定位节点与当前待定位节点的各邻居节点间的信号交互，获取当前待定位节点接收邻居节点信号时的到达角度；获取当前待定位节点的方向角；利用所获取的到达角度，初始位置坐标，以及方向角，使用梯度下降法更新当前待定位节点的初始位置坐标，得到当前待定位节点的当前位置坐标；当当前待定位节点的当前位置坐标达到第一预设收敛条件时，将当前位置坐标确定为当前待定位节点的目标定位坐标。本发明实施例，能够提高节点定位的准确度。

Description

一种基于到达角度测距的协作定位方法及装置
技术领域
本发明涉及定位技术领域，特别是涉及一种基于到达角度测距的协作定位方法及装置。
背景技术
基于信号到达角度的定位算法是一种常见的无线传感器网络节点定位算法。现有基于到达角度测距的定位方法为：首先，获取各待定位节点(位置未知的节点)接收参考节点(位置已知的节点)信号时的到达角度，得到各待定位节点接收参考节点信号时的到达角度测距。然后将所获取的到达角度测距与没有噪声的理论测距相减，该理论测距可以使用坐标进行表示，再最小化到达角度测距与理论测距相减结果的平方，将待定位节点的定位问题，确定为最小化到达角度测距与理论测距相减结果的平方的问题。最后，利用凸优化中的半正定规划，将待定位节点的定位问题转化为凸问题进行求解，得到待定位节点的坐标。
实际应用中，在3维空间内，还需获取各待定位节点与参考节点之间的接收信号强度或接收信号到达时间。上述将所获取的到达角度测距与没有噪声的理论测距相减的步骤相应为：分别将所获取的到达角度测距与没有噪声的理论测距，所获取的接收信号强度与理论接收信号强度、所获取的接收信号到达时间与理论接收信号到达时间相减。上述最小化到达角度测距与理论测距相减结果的平方相应为，最小化各个相减结果的平方和。
上述实现过程中，由于各待定位节点的定位只是参考了位置已知的节点，即参考的是位置固定的节点进行定位的，实际应用中，位置已知的节点数量可能较少，那么待定位节点能够获取的接收位置已知的节点信号时的到达角度相应较少，进而导致节点的定位准确度相对较低。
发明内容
本发明实施例的目的在于提供一种基于到达角度测距的协作定位方法及装置，以提高在对节点定位时的定位准确度。具体技术方案如下：
第一方面，本发明实施例提供了一种基于到达角度测距的协作定位方法，所述方法包括：
获取当前待定位节点的初始位置坐标，所述初始位置坐标为卫星定位坐标；基于所述当前待定位节点与当前待定位节点的各邻居节点间的信号交互，获取所述当前待定位节点接收所述邻居节点信号时的到达角度；所述邻居节点包括：与所述当前待定位节点的通信半径小于预设阈值的，位置已知的节点和位置未知的节点；获取所述当前待定位节点的方向角；利用所获取的所述到达角度，所述初始位置坐标，以及所述方向角，使用梯度下降法更新所述当前待定位节点的初始位置坐标，得到所述当前待定位节点的当前位置坐标；当所述当前待定位节点的所述当前位置坐标达到第一预设收敛条件时，将所述当前位置坐标确定为所述当前待定位节点的目标定位坐标。
可选地，所述利用所获取的所述到达角度，所述初始位置坐标，以及所述方向角，使用梯度下降法更新所述当前待定位节点的初始位置坐标，得到所述当前待定位节点的当前位置坐标之后，所述方法还包括：判断所述当前待定位节点的所述当前位置坐标是否达到第一预设收敛条件；如果所述当前待定位节点的所述当前位置坐标没有达到第一预设收敛条件，则返回执行所述利用所获取的所述到达角度，所述初始位置坐标，以及所述方向角，使用梯度下降法更新所述当前待定位节点的初始位置坐标，得到所述当前待定位节点的当前位置坐标的步骤。
可选地，所述获取所述当前待定位节点的方向角的步骤，包括：利用所述当前待定位节点的初始位置坐标，及预设的最大似然估计算法，得到所述当前待定位节点的方向角；所述当所述当前待定位节点的所述当前位置坐标达到第一预设收敛条件时，将所述当前位置坐标确定为所述当前待定位节点的目标定位坐标之后，所述方法还包括：判断所述当前待定位节点的目标定位坐标是否达到第二预设收敛条件；如果所述当前待定位节点的目标定位坐标达到第二预设收敛条件，则将所述当前待定位节点的目标定位坐标，确定为所述当前待定位节点的最终目标定位坐标；如果所述当前待定位节点的目标定位坐标没有达到第二预设收敛条件，则将所述当前待定位节点的初始位置坐标更新为所述目标定位坐标，并执行利用所述当前待定位节点的初始位置坐标，及预设的最大似然估计算法，得到所述当前待定位节点的方向角的步骤。
可选地，在所述当前待定位节点处于三维空间的情况下，所述利用所获取的所述到达角度，所述初始位置坐标，以及所述方向角，使用梯度下降法更新所述当前待定位节点的初始位置坐标的步骤，包括：在所述到达角度在2π周期内的情况下，利用所述初始位置坐标，以及所述方向角，构建所述当前待定位节点接收所述邻居节点信号时的到达角度，所构建的到达角度表示为：
Figure BDA0002302757440000021
和
Figure BDA0002302757440000022
其中，
Figure BDA0002302757440000023
表示所构建的第m个当前待定位节点接收到来自第n个节点的垂直方向到达角度，
Figure BDA0002302757440000024
表示所构建的第m个当前待定位节点接收到来自第n个节点的水平方向到达角度，
Figure BDA0002302757440000025
分别表示
Figure BDA0002302757440000026
向量在第m个当前待定位节点的局部坐标系中x′轴、y′轴、z′轴上的投影，
Figure BDA0002302757440000031
向量表示第m个当前待定位节点和第n个节点的坐标相减得到的向量，Vm表示
Figure BDA0002302757440000032
的值是在以第m个当前待定位节点为原点建立的局部坐标系中，mn表示电磁波从第n个节点发出到第m个当前待定位节点接收的链路，dmn表示
Figure BDA0002302757440000033
向量的长度；
将链路损耗的最小化目标函数表示为：
Figure BDA0002302757440000034
其中，θmn表示实际所获取的第m个当前待定位节点接收到来自第n个节点的水平方向到达角度，
Figure BDA0002302757440000035
表示实际所获取的第m个当前待定位节点接收到来自第n个节点的垂直方向到达角度，lossA(θmn)表示实际获取水平方向到达角度的链路损耗，
Figure BDA0002302757440000036
表示实际获取垂直方向到达角度的链路损耗，[N]表示当前待定位节点的集合，Rm表示第m个当前待定位节点的邻居节点的集合；
使用如下表达式更新所述当前待定位节点的初始位置坐标：
Figure BDA0002302757440000037
Figure BDA0002302757440000038
Figure BDA0002302757440000039
其中，λt表示第t次迭代的步长，
Figure BDA00023027574400000310
分别表示第t次迭代中第m个当前待定位节点在世界坐标系x轴、y轴和z轴的坐标估计值，
Figure BDA00023027574400000311
和
Figure BDA00023027574400000312
分别表示xm、ym、zm的梯度方向；
Figure BDA00023027574400000313
和
Figure BDA00023027574400000314
的值，通过分别对所述最小化目标函数求xm、ym、zm的一阶导数得到。
可选地，在所述当前待定位节点处于三维空间的情况下，所述利用所获取的所述到达角度，所述初始位置坐标，以及所述方向角，使用梯度下降法更新所述当前待定位节点的初始位置坐标的步骤，包括：在所述到达角度在π周期内的情况下，利用所述初始位置坐标，以及所述方向角，构建所述当前待定位节点接收所述邻居节点信号时的到达角度，所构建的到达角度表示为：
Figure BDA0002302757440000041
和
Figure BDA0002302757440000042
其中，
Figure BDA0002302757440000043
表示所构建的第m个当前待定位节点接收到来自第n个节点的垂直方向到达角度，
Figure BDA0002302757440000044
表示所构建的第m个当前待定位节点接收到来自第n个节点的水平方向到达角度，
Figure BDA0002302757440000045
分别表示
Figure BDA0002302757440000046
向量在第m个当前待定位节点的局部坐标系中x′轴、y′轴、z′轴上的投影，
Figure BDA0002302757440000047
向量表示第m个当前待定位节点和第n个节点的坐标相减得到的向量，Vm表示
Figure BDA0002302757440000048
的值是在以第m个当前待定位节点为原点建立的局部坐标系中，mn表示电磁波从第n个节点发出到第m个当前待定位节点接收的链路，dmn表示
Figure BDA0002302757440000049
向量的长度；
将链路损耗的最小化目标函数表示为：
Figure BDA00023027574400000410
其中，约束条件为gmn∈[0,1]，θmn表示实际所获取的第m个当前待定位节点接收到来自第n个节点的水平方向到达角度，
Figure BDA00023027574400000411
表示实际所获取的第m个当前待定位节点接收到来自第n个节点的垂直方向到达角度，lossA(θmn)，lossA(-θmn)表示实际获取水平方向到达角度的链路损耗，
Figure BDA00023027574400000412
表示实际获取垂直方向到达角度的链路损耗，[N]表示当前待定位节点的集合，Rm表示第m个当前待定位节点的邻居节点的集合，gmn表示π周期内整数变量；
使用如下表达式更新所述当前待定位节点的初始位置坐标：
Figure BDA00023027574400000413
Figure BDA00023027574400000414
Figure BDA00023027574400000415
Figure BDA00023027574400000416
其中，λt表示第t次迭代的步长，
Figure BDA00023027574400000417
分别表示第t次迭代中第m个当前待定位节点在世界坐标系x轴、y轴和z轴的坐标估计值，
Figure BDA00023027574400000418
表示第t次迭代中第m个当前待定位节点整数变量gmn的估计值，
Figure BDA0002302757440000051
和
Figure BDA0002302757440000052
分别表示xm、ym、zm的梯度方向；
Figure BDA0002302757440000053
和
Figure BDA0002302757440000054
的值，通过分别对最小化目标函数求xm、ym、zm的一阶导数得到，Proj(x)＝min(max(x,0),1)。
可选地，在所述当前待定位节点处于二维空间的情况下，所述利用所获取的所述到达角度，所述初始位置坐标，以及所述方向角，使用梯度下降法更新所述当前待定位节点的初始位置坐标的步骤，包括：在所述到达角度在2π周期内的情况下，利用所述初始位置坐标，以及所述方向角，构建所述当前待定位节点接收所述邻居节点信号时的到达角度，所构建的到达角度表示为：
Figure BDA0002302757440000055
其中，
Figure BDA0002302757440000056
表示所构建的第m个当前待定位节点接收到来自第n个节点的水平方向到达角度，
Figure BDA0002302757440000057
分别表示
Figure BDA0002302757440000058
向量在第m个当前待定位节点的局部坐标系中x′轴、y′轴上的投影，
Figure BDA0002302757440000059
向量表示第m个当前待定位节点和第n个节点的坐标相减得到的向量，Vm表示
Figure BDA00023027574400000510
的值是在以第m个当前待定位节点为原点建立的局部坐标系中，mn表示电磁波从第n个节点发出到第m个当前待定位节点接收的链路；
将链路损耗的最小化目标函数表示为：
Figure BDA00023027574400000511
其中，θmn表示实际所获取的第m个当前待定位节点接收到来自第n个节点的水平方向到达角度，lossA(θmn)表示实际获取水平方向到达角度的链路损耗，[N]表示当前待定位节点的集合，Rm表示第m个当前待定位节点的邻居节点的集合；
使用如下表达式更新所述当前待定位节点的初始位置坐标：
Figure BDA00023027574400000512
Figure BDA00023027574400000513
其中，λt表示第t次迭代的步长，
Figure BDA00023027574400000514
分别表示第t次迭代中第m个当前待定位节点在世界坐标系x轴和y轴的坐标估计值，
Figure BDA00023027574400000515
和
Figure BDA00023027574400000516
分别表示xm、ym的梯度方向；
Figure BDA0002302757440000061
和
Figure BDA0002302757440000062
的值，通过分别对所述最小化目标函数求xm、ym的一阶导数得到。
可选地，在所述当前待定位节点处于二维空间的情况下，所述利用所获取的所述到达角度，所述初始位置坐标，以及所述方向角，使用梯度下降法更新所述当前待定位节点的初始位置坐标的步骤，包括：在所述到达角度在π周期内的情况下，利用所述初始位置坐标，以及所述方向角，构建所述当前待定位节点接收所述邻居节点信号时的到达角度，所构建的到达角度表示为：
Figure BDA0002302757440000063
其中，
Figure BDA0002302757440000064
表示所构建的第m个当前待定位节点接收到来自第n个节点的水平方向到达角度，
Figure BDA0002302757440000065
分别表示
Figure BDA0002302757440000066
向量在第m个当前待定位节点的局部坐标系中x′轴、y′轴上的投影，
Figure BDA0002302757440000067
向量表示第m个当前待定位节点和第n个节点的坐标相减得到的向量，Vm表示
Figure BDA0002302757440000068
的值是在以第m个当前待定位节点为原点建立的局部坐标系中，mn表示电磁波从第n个节点发出到第m个当前待定位节点接收的链路；
将链路损耗的最小化目标函数表示为：
Figure BDA0002302757440000069
其中，约束条件为gmn∈[0,1]，θmn表示实际所获取的第m个当前待定位节点接收到来自第n个节点的水平方向到达角度，lossA(θmn)，lossA(-θmn)表示实际获取水平方向到达角度的链路损耗，[N]表示当前待定位节点的集合，Rm表示第m个当前待定位节点的邻居节点的集合，gmn表示π周期内整数变量；
使用如下表达式更新所述当前待定位节点的初始位置坐标：
Figure BDA00023027574400000610
Figure BDA00023027574400000611
Figure BDA00023027574400000612
其中，λt表示第t次迭代的步长，
Figure BDA00023027574400000613
分别表示第t次迭代中第m个当前待定位节点在世界坐标系x轴和y轴的坐标估计值，
Figure BDA00023027574400000614
表示第t次迭代中第m个当前待定位节点整数变量gmn的估计值，
Figure BDA0002302757440000071
和
Figure BDA0002302757440000072
分别表示xm、ym的梯度方向；
Figure BDA0002302757440000073
和
Figure BDA0002302757440000074
的值，通过分别对最小化目标函数求xm、ym的一阶导数得到，Proj(x)＝min(max(x,0),1)。
第二方面，本发明实施例提供了一种基于到达角度测距的协作定位装置，所述装置包括：第一获取模块，用于获取当前待定位节点的初始位置坐标，所述初始位置坐标为卫星定位坐标；第二获取模块，用于基于所述当前待定位节点与当前待定位节点的各邻居节点间的信号交互，获取所述当前待定位节点接收所述邻居节点信号时的到达角度；所述邻居节点包括：与所述当前待定位节点的通信半径小于预设阈值的，位置已知的节点和位置未知的节点；第三获取模块，用于获取所述当前待定位节点的方向角；位置更新模块，用于利用所获取的所述到达角度，所述初始位置坐标，以及所述方向角，使用梯度下降法更新所述当前待定位节点的初始位置坐标，得到所述当前待定位节点的当前位置坐标；节点定位模块，用于当所述当前待定位节点的所述当前位置坐标达到第一预设收敛条件时，将所述当前位置坐标确定为所述当前待定位节点的目标定位坐标。
可选地，所述装置还包括：第一判断模块，用于判断所述当前待定位节点的所述当前位置坐标是否达到第一预设收敛条件；第一触发模块，用于在所述第一判断模块判断出所述当前待定位节点的所述当前位置坐标没有达到第一预设收敛条件时，触发所述位置更新模块，利用所获取的所述到达角度，所述初始位置坐标，以及所述方向角，使用梯度下降法更新所述当前待定位节点的初始位置坐标，得到所述当前待定位节点的当前位置坐标。
可选地，所述第三获取模块，具体用于利用所述当前待定位节点的初始位置坐标，及预设的最大似然估计算法，得到所述当前待定位节点的方向角；所述装置还包括：第二判断模块，用于判断所述当前待定位节点的目标定位坐标是否达到第二预设收敛条件；定位确定模块，用于在所述第二判断模块判断出所述当前待定位节点的目标定位坐标达到第二预设收敛条件时，将所述当前待定位节点的目标定位坐标，确定为所述当前待定位节点的最终目标定位坐标；第二触发模块，用于在所述第二判断模块判断出所述当前待定位节点的目标定位坐标没有达到第二预设收敛条件时，将所述当前待定位节点的初始位置坐标更新为所述目标定位坐标，并触发所述第三获取模块，利用所述当前待定位节点的初始位置坐标，及预设的最大似然估计算法，得到所述当前待定位节点的方向角。
第三方面，本发明实施例还提供了一种电子设备，包括处理器、通信接口、存储器和通信总线，其中，处理器，通信接口，存储器通过通信总线完成相互间的通信；存储器，用于存放计算机程序；处理器，用于执行存储器上所存放的程序时，实现上述第一方面所述的一种基于到达角度测距的协作定位方法。
第四方面，本发明实施例还提供了一种计算机可读存储介质，所述计算机可读存储介质中存储有指令，当其在计算机上运行时，使得计算机执行上述第一方面所述的一种基于到达角度测距的协作定位方法的步骤。
本发明实施例提供的一种基于到达角度测距的协作定位方法及装置，由于使用梯度下降法更新当前待定位节点的初始位置坐标，相较于使用半正定规划将当前待定位节点的定位问题转化为凸问题进行求解的过程，降低了计算的复杂度；且本发明实施例中，基于所获取的当前待定位节点接收通信半径内邻居节点信号时的到达角度更新当前待定位节点的初始位置坐标，即在定位过程中不仅考虑了当前待定位节点接收位置已知的节点信号时的到达角度，还考虑了当前待定位节点接收位置未知的节点信号时的到达角度，以及当前待定位节点与邻居节点之间的通信半径，可以使用通信半径内多个待定位节点相互协作实现定位，提高节点定位的准确度。当然，实施本发明的任一产品或方法并不一定需要同时达到以上所述的所有优点。
附图说明
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
图1为本发明实施例提供的一种基于到达角度测距的协作定位方法的流程示意图；
图2为本发明实施例提供的另一种基于到达角度测距的协作定位方法的流程示意图；
图3为本发明实施例提供的再一种基于到达角度测距的协作定位方法的流程示意图；
图4为本发明实施例提供的一种节点坐标系的示意图；
图5为本发明实施例提供的一种节点接收信号时到达角度的示意图；
图6为本发明实施例提供的另一种节点接收信号时到达角度的示意图；
图7为本发明实施例提供的一种基于到达角度测距的协作定位装置的结构示意图；
图8为本发明实施例提供的一种电子设备的结构示意图。
具体实施方式
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
本发明提供了一种基于到达角度测距的协作定位方法，如图1所示，包括：
S101，获取当前待定位节点的初始位置坐标。
本发明实施例可以应用于车联网中用于定位的管理模块，也可以用于待定位节点。待定位节点可以是一个，也可以是多个。在对节点定位时，可以获取当前待定位节点的初始位置坐标，该初始位置坐标为卫星定位坐标，可以通过全球卫星导航系统(GlobalNavigation Satellite System，GNSS)或全球卫星定位系统(Global PositioningSystem，GPS)获取。
示例性的，本发明实施例中的待定位节点可以是车联网中的车辆，在一个车联网网络中，有N辆未知坐标的车辆，A个已知坐标的路边单元(Roadside Unit,RSU)，节点的坐标定义在世界坐标系o-xyz中，其中o是原点(0,0,0)，x和y轴分别指向正东方向和正北方向，z轴垂直于xoy平面，并且指向上方。第n个节点的坐标表示为
Figure BDA0002302757440000091
xn,yn,zn分别表示第n个节点在世界坐标系o-xyz中x轴、y轴、z轴上的坐标值，前N个节点是位置未知的车辆，后A个是位置已知的路边单元。通过全球卫星导航系统或全球卫星定位系统，获取的第m个当前待定位节点的初始位置坐标可以表示为pm＝[xm,ym,zm]T。本发明实施例中，位置未知的任一节点都可以是当前待定位节点。
S102，基于当前待定位节点与当前待定位节点的各邻居节点间的信号交互，获取当前待定位节点接收邻居节点信号时的到达角度。
待定位节点与待定位节点之间、待定位节点与路边单元之间都可以进行信号交互。示例性的，为每一车辆可以设置一个矩形天线阵列，阵列元素以行和列的形式分布在天线阵列上，每一阵列元素表示一个天线单元。对于车辆，天线阵列可以放置于车辆的顶部，朝向车头的方向，对于路边单元，天线阵列可以垂直于地面朝向北方。
位于车联网中包括待定位节点与路边单元的每一节点上都有相应的电子单元，该电子单元可以感应邻居节点发出的电磁波信号，也可以向邻居节点发送电磁波信号。因路边单元的位置是已知的，路边单元之间接收信号时的到达角度不包含有用的信息，故而不考虑路边单元之间接收信号时的到达角度，只需获取待定位节点接收邻居节点信号时的到达角度，所获取的待定位节点接收邻居节点信号时的到达角度为实际获取的到达角度，获取的方式可以与现有技术相同，本发明实施例在此不做赘述。
具体的，邻居节点可以包括：与当前待定位节点的通信半径小于预设阈值的，位置已知的节点和位置未知的节点。该通信半径，可以指能够获取接收节点信号时的到达角度的最远距离。本领域技术人员可以根据实际需求对预设阈值进行设置。获取当前待定位节点接收邻居节点信号时的到达角度，进一步可以参考包括位置已知和位置未知的邻居节点，对该当前待定位节点进行定位。
S103，获取当前待定位节点的方向角。
本发明实施例中，当待定位节点与路边单元上带有罗盘的情况下，可以通过罗盘直接获取各节点的方向角。当没有带罗盘的情况下，可以对各节点的方向角进行估计，得到各节点的方向角，具体的对各节点的方向角进行估计的实施方式在下文进行详细介绍。
S104，利用所获取的到达角度，初始位置坐标，以及方向角，使用梯度下降法更新当前待定位节点的初始位置坐标，得到当前待定位节点的当前位置坐标。
在获取到达角度，初始位置坐标，以及方向角后，可以使用梯度下降法对当前待定位节点的初始位置坐标进行更新，得到当前待定位节点的当前位置坐标，其具体的实施方式在下文进行详细介绍。
S105，当当前待定位节点的当前位置坐标达到第一预设收敛条件时，将当前位置坐标确定为当前待定位节点的目标定位坐标。
得到当前待定位节点的当前位置坐标后，可以确定当前待定位节点的当前位置坐标是否达到第一预设收敛条件，当达到第一预设收敛条件时，表明当前待定位节点的定位坐标已收敛，此时将当前位置坐标确定为当前待定位节点的目标定位坐标。当没有达到第一预设收敛条件时，表明当前待定位节点的定位坐标没有收敛，此时继续对当前待定位节点的当前位置坐标进行更新。示例性的，可以当所有待定位节点的当前位置坐标均达到第一预设收敛条件时，将当前位置坐标对应确定为各待定位节点的目标定位坐标。
本发明实施例中，第一预设收敛条件可以是预设迭代次数，例如：对当前待定位节点的初始位置坐标更新的次数达到预设的迭代次数，该迭代次数本领域技术人员可以根据实际需求进行设置，示例性的，迭代次数可以设置为100、200等。第一预设收敛条件还可以是，本次更新后的当前位置坐标与更新前的位置坐标之间的欧氏距离小于一预设阈值。
本发明实施例使用梯度下降法更新当前待定位节点的初始位置坐标，降低了计算复杂度；且本发明实施例中在定位过程中不仅考虑了当前待定位节点接收位置已知的节点信号时的到达角度，还考虑了当前待定位节点接收位置未知的节点信号时的到达角度，可以使用通信半径内多个待定位节点相互协作实现定位，提高节点定位的准确度。
在图1所示实施例的基础上，如图2所示，本发明所提供的一种基于到达角度测距的协作定位方法，还可以包括：
S106，判断当前待定位节点的当前位置坐标是否达到第一预设收敛条件。
本发明实施例可以判断当前待定位节点的当前位置坐标是否达到第一预设收敛条件，如果当前待定位节点的当前位置坐标没有达到第一预设收敛条件，表明当前待定位节点节点的定位坐标没有收敛，此时返回执行S104的步骤。
本发明实施例中，对当前待定位节点定位过程中，利用该当前待定位节点的初始位置坐标、接收各邻居节点信号时的到达角度、以及自身的方向角，对当前待定位节点的初始位置坐标进行迭代更新，使用位置已知的节点和位置未知的节点，对位置未知的待定位节点实现协作定位。
在图1所示实施例的基础上，如图3所示，图3为本发明实施例提供的再一种基于到达角度测距的协作定位方法的流程示意图，包括：
S201，获取当前待定位节点的初始位置坐标。
S202，基于当前待定位节点与当前待定位节点的各邻居节点间的信号交互，获取当前待定位节点接收邻居节点信号时的到达角度。
其中，步骤S201-S202的实现方式，分别与S101-S102相同，本发明实施例在此不做赘述。
S203，利用当前待定位节点的初始位置坐标，及预设的最大似然估计算法，得到当前待定位节点的方向角。
本发明实施例中，当待定位节点与路边单元上没有带罗盘的情况下，可以对各节点的方向角进行估计，具体的，可以利用当前待定位节点的初始位置坐标，及预设的最大似然估计算法，得到当前待定位节点的方向角，详细的实现过程在下文进行介绍。
S204，利用所获取的到达角度，初始位置坐标，以及方向角，使用梯度下降法更新当前待定位节点的初始位置坐标，得到当前待定位节点的当前位置坐标。
S205，当当前待定位节点的当前位置坐标达到第一预设收敛条件时，将当前位置坐标确定为当前待定位节点的目标定位坐标。
其中，步骤S204-S205的实现方式，分别与S104-S105相同，本发明实施例在此不做赘述。
S206，判断当前待定位节点的目标定位坐标是否达到第二预设收敛条件。
本发明实施例可以判断当前待定位节点的目标定位坐标是否达到第二预设收敛条件。该第二预设收敛条件可以与上述第一预设收敛条件相同，本发明实施例在此不作限定。
S207，如果当前待定位节点的目标定位坐标达到第二预设收敛条件，则将当前待定位节点的目标定位坐标，确定为当前待定位节点的最终目标定位坐标。
当当前待定位节点的目标定位坐标达到第二预设收敛条件，表明此时估计得到的待定位节点的方向角相对准确，且定位得到的目标定位坐标也已达到收敛条件，定位较为准确，将当前待定位节点的目标定位坐标，确定为当前待定位节点的最终目标定位坐标。
当当前待定位节点的目标定位坐标没有达到第二预设收敛条件，表明此时估计得到的待定位节点的方向角还不够准确，则将当前待定位节点的初始位置坐标更新为目标定位坐标，并执行S203的步骤。
本发明实施例提供的一种基于到达角度测距的协作定位方法，使用梯度下降法更新当前待定位节点的初始位置坐标，降低了计算的复杂度；且，本发明实施例还可以在方向角未知的情况下，对方向角进行估计，进而对节点进行定位。另外，本发明实施例中基于所获取的当前待定位节点接收通信半径内邻居节点信号时的到达角度更新当前待定位节点的初始位置坐标，即在定位过程中不仅考虑了当前待定位节点接收位置已知的节点信号时的到达角度，还考虑了当前待定位节点接收位置未知的节点信号时的到达角度，可以使用通信半径内多个待定位节点相互协作实现定位，提高节点定位的准确度。
本发明一种实施方式中，在当前待定位节点处于三维空间的情况下，上述步骤S104的实现方式，可以包括：
在到达角度在2π周期内的情况下，可以利用得到的当前待定位节点的初始位置坐标，以及方向角，构建当前待定位节点接收邻居节点信号时的到达角度，具体的构建过程可以为：如图4所示，图4定义了节点的世界坐标系和局部坐标系，每一个节点都设置有自己的局部坐标系，表示为o′-x′y′z′，局部坐标系中的原点o′选为天线阵列的中心，在世界坐标系中的坐标表示为该节点的三维坐标。示例性的，以第m个节点为例，局部坐标系中的原点o′表示为世界坐标系中的pm＝[xm,ym,zm]T，pm表示第m个节点在世界坐标系中的位置，xm,ym,zm分别表示第m个节点在x、y、z轴上的坐标。当节点是车辆时，x′轴与天线阵列的每行平行并指向车辆的右侧，y′轴与天线阵列平面的法向量平行并指向车头。当节点是路边单元时，x′轴和y′轴与世界坐标系的相应坐标轴平行，z′轴指向上方并垂直于x′o′y′平面，y′轴指向的半个空间定义为天线阵列的正面，另外半个空间定义为天线阵列的背面。
以第m个节点为例进行说明，当第m个节点为待定位节点时，在三维空间情况下，由于地面不平和车辆行驶方向的改变，局部坐标系与世界坐标系相比存在三种类型方向角的旋转，分别是对世界坐标系中的z轴，y轴以及x轴逆时针旋转的角度ψm∈[-π,π)，
Figure BDA0002302757440000121
以及
Figure BDA0002302757440000122
其中，ψm(偏航角)表示z轴逆时针旋转的角度，αm(翻滚角)表示y轴逆时针旋转的角度，φm(俯仰角)表示x轴逆时针旋转的角度。图4中，第m个节点的局部坐标系为通过将世界坐标系的原点移到pm，并逆时针旋转z轴、y轴以及x轴以角度ψm，αm，φm得到的。
在第m个节点的局部坐标中，第m个节点和第n个节点的坐标相减的结果可以表示为：
Figure BDA0002302757440000123
其中，[Δxmn,Δymn,Δzmn]T＝(pm-pn)＝[xm-xn,ym-yn,zm-zn]T，pm、pn分别表示第m个节点和第n个节点在世界坐标系中的位置。令
Figure BDA0002302757440000131
Δxmn,Δymn,Δzmn分别表示
Figure BDA0002302757440000132
向量在世界坐标系中x轴、y轴、z轴上的投影。
Figure BDA0002302757440000133
表示
Figure BDA0002302757440000134
向量在第m个节点的局部坐标系中x′轴、y′轴、z′轴上的投影，mn表示电磁波信号从第n个节点发出到第m个节点接收的链路，Vm表示在以第m个节点建立的局部坐标系中。旋转矩阵F(φm,αm,ψm)定义为：
Figure BDA0002302757440000135
Figure BDA0002302757440000136
Fv1,I(ψ)、Fv2,v1(α)、FV,v2(φ)分别表示只旋转z轴、y轴、x轴时的旋转矩阵。
当第m个节点为路边单元时，局部坐标系的x′y′z′与世界坐标系的坐标轴方向相同。因此，俯仰角，翻滚角以及偏航角为0，三个旋转矩阵变为单位矩阵。
由于矩形天线阵列具有水平方向和垂直方向的传感器，因此可以分辨出入射电磁波信号的水平方向和垂直方向的到达角度。因阵列的行与列分别与x′轴和z′轴平行，将直角坐标系转换为极坐标系后，两个方向的到达角度分别可以根据水平方向的夹角和垂直方向的夹角确定。所构建的到达角度可以表示为：
Figure BDA0002302757440000137
和
Figure BDA0002302757440000138
其中，
Figure BDA0002302757440000139
表示所构建的第m个当前待定位节点接收到来自第n个节点的垂直方向到达角度，
Figure BDA00023027574400001310
表示所构建的第m个当前待定位节点接收到来自第n个节点的水平方向到达角度，
Figure BDA00023027574400001311
分别表示
Figure BDA00023027574400001312
向量在第m个当前待定位节点的局部坐标系中x′轴、y′轴、z′轴上的投影，
Figure BDA00023027574400001313
向量表示第m个当前待定位节点和第n个节点的坐标相减得到的向量，Vm表示
Figure BDA00023027574400001314
的值是在以第m个当前待定位节点为原点建立的局部坐标系中，mn表示电磁波从第n个节点发出到第m个当前待定位节点接收的链路，dmn表示
Figure BDA0002302757440000141
向量的长度。
如图5所示，图5为本发明实施例提供的一种节点接收信号时到达角度的示意图，在2π周期内，第m个当前待定位节点的天线阵列可以区分接收第n个节点信号时的到达角度是来自天线阵列的正面还是背面，水平方向的到达角度与x′轴的夹角大小相等，故而，
Figure BDA0002302757440000142
将所构建的到达角度表示为：
Figure BDA0002302757440000143
Figure BDA0002302757440000144
表示所构建的垂直方向到达角度与水平方向到达角度的组合矩阵。
因噪声的影响，实际所获取的到达角度存在链路损耗，将噪声表示为高斯白噪声，所构建的实际到达角度可以表示为：
Figure BDA0002302757440000145
其中，
Figure BDA0002302757440000146
表示所构建的实际到达角度，
Figure BDA0002302757440000147
θmn分别表示实际所构建的(加噪声后)垂直方向到达角度、水平方向到达角度，wmn～N(02,Λmn)表示均值为0的高斯白噪声，方差表示为：
Figure BDA0002302757440000148
其中，Λmn表示噪声协方差矩阵，因为水平方向到达角度θmn与垂直方向到达角度
Figure BDA00023027574400001415
相互独立，所以Λmn(1,2)和Λmn(1,2)均为0，
Figure BDA0002302757440000149
和
Figure BDA00023027574400001410
分别表示垂直方向和水平方向的到达角度的噪声方差。
基于所构建的到达角度，可以得到：
Figure BDA00023027574400001411
然而，由于实际存在噪声的影响，上述等式不再成立，故而，将链路损耗表示为：
Figure BDA00023027574400001412
进一步的，将链路损耗的最小化目标函数表示为：
Figure BDA00023027574400001413
其中，θmn表示实际所获取的第m个当前待定位节点接收到来自第n个节点的水平方向到达角度，
Figure BDA00023027574400001414
表示实际所获取的第m个当前待定位节点接收到来自第n个节点的垂直方向到达角度，lossA(θmn)表示实际获取水平方向到达角度的链路损耗，
Figure BDA0002302757440000151
表示实际获取垂直方向到达角度的链路损耗，[N]表示当前待定位节点的集合，Rm表示第m个当前待定位节点的邻居节点的集合。
将上述最小化问题通过梯度下降法进行求解，使用如下表达式更新当前待定位节点的初始位置坐标：
Figure BDA0002302757440000152
Figure BDA0002302757440000153
Figure BDA0002302757440000154
其中，λt表示第t次迭代的步长，
Figure BDA0002302757440000155
分别表示第t次迭代中第m个当前待定位节点在世界坐标系x轴、y轴和z轴的坐标估计值，
Figure BDA0002302757440000156
和
Figure BDA0002302757440000157
分别表示xm、ym、zm的梯度方向；
Figure BDA0002302757440000158
和
Figure BDA0002302757440000159
的值，通过分别对最小化目标函数求xm、ym、zm的一阶导数得到。
Figure BDA00023027574400001510
和
Figure BDA00023027574400001511
可以表示为：
Figure BDA00023027574400001512
Figure BDA00023027574400001513
Figure BDA00023027574400001514
其中，
Figure BDA00023027574400001515
Figure BDA00023027574400001516
Figure BDA0002302757440000161
Figure BDA0002302757440000162
Figure BDA0002302757440000163
Figure BDA0002302757440000164
本发明一种实施方式中，在当前待定位节点处于三维空间的情况下，上述步骤S104的实现方式，可以包括：
在到达角度在π周期内的情况下，利用初始位置坐标，以及方向角，构建当前待定位节点接收邻居节点信号时的到达角度，具体的构建过程可参见上文描述，不同之处在于，所构建的水平方向到达角度不同。所构建的到达角度表示为：
Figure BDA0002302757440000165
和
Figure BDA0002302757440000166
式中各字符的含义可参见上文描述，此处不做赘述。
如图6所示，图6为本发明实施例提供的另一种节点接收信号时到达角度的示意图，在π周期内，第m个当前待定位节点的天线阵列不能够区分接收第n个节点信号时的到达角度是来自天线阵列的正面还是背面，相应的水平方向到达角度在2π周期内应该为-θm或者θm，但是水平方向的到达角度与x′轴的夹角的绝对值相等，故而，
Figure BDA0002302757440000167
为了解决π周期内到达角度的模糊性，引入整数变量gmn，如果信号的入射方向是从天线阵列正面入射，则gmn的值设置为0，否则为1。将水平方向到达角度的链路损耗表示为：
(1-gmn)lossA(θmn)+gmnlossA(-θmn)
进一步，将链路损耗的最小化目标函数表示为：
Figure BDA0002302757440000168
其中，约束条件为gmn∈[0,1]，θmn表示实际所获取的第m个当前待定位节点接收到来自第n个节点的水平方向到达角度，
Figure BDA0002302757440000171
表示实际所获取的第m个当前待定位节点接收到来自第n个节点的垂直方向到达角度，lossA(θmn)，lossA(-θmn)表示实际获取水平方向到达角度的链路损耗，
Figure BDA0002302757440000172
表示实际获取垂直方向到达角度的链路损耗，[N]表示当前待定位节点的集合，Rm表示第m个当前待定位节点的邻居节点的集合，gmn表示π周期内整数变量。本发明实施例中gmn的初始值可以设置为0.5。
将上述最小化问题通过梯度下降法进行求解，使用如下表达式更新当前待定位节点的初始位置坐标：
Figure BDA0002302757440000173
Figure BDA0002302757440000174
Figure BDA0002302757440000175
Figure BDA0002302757440000176
其中，λt表示第t次迭代的步长，
Figure BDA0002302757440000177
分别表示第t次迭代中第m个当前待定位节点在世界坐标系x轴、y轴和z轴的坐标估计值，
Figure BDA0002302757440000178
表示第t次迭代中第m个当前待定位节点整数变量gmn的估计值，
Figure BDA0002302757440000179
和
Figure BDA00023027574400001710
分别表示xm、ym、zm的梯度方向；
Figure BDA00023027574400001711
和
Figure BDA00023027574400001712
的值，通过分别对最小化目标函数求xm、ym、zm的一阶导数得到，Proj(x)＝min(max(x,0),1)。
Figure BDA00023027574400001713
和
Figure BDA00023027574400001714
可以表示为：
Figure BDA00023027574400001715
Figure BDA00023027574400001716
Figure BDA0002302757440000181
Figure BDA0002302757440000182
本发明一种实施方式中，在当前待定位节点处于二维空间的情况下，上述步骤S104的实现方式，可以包括：
在到达角度在2π周期内的情况下，利用初始位置坐标，以及方向角，构建当前待定位节点接收邻居节点信号时的到达角度，具体的构建过程可以为：
参见上述三维空间的构建过程，与上述三维空间不同的是，在二维空间不需要考虑世界坐标系与局部坐标系中的z轴。示例性的，以第m个节点为例，局部坐标系中的原点o′表示为世界坐标系中的pm＝[xm,ym]T，将世界坐标系中的原点移动到第m个节点的局部坐标系中，等效为将三维空间世界坐标系中的z轴旋转角ψm得到。在第m个节点的局部坐标中，第m个节点和第n个节点的坐标相减的结果可以表示为：
Figure BDA0002302757440000183
旋转矩阵F(ψm)定义为：
Figure BDA0002302757440000184
ψm为二维空间情况下第m个节点的方向角，在二维空间情况下，仅能获取水平方向的到达角度，所构建的到达角度表示为：
Figure BDA0002302757440000185
式中各字符的含义可参见上文描述，此处不做赘述。
由于实际存在噪声的影响，将链路损耗表示为：
Figure BDA0002302757440000186
进一步，将链路损耗的最小化目标函数表示为：
Figure BDA0002302757440000187
式中各字符的含义可参见上文描述，此处不做赘述。
将上述最小化问题通过梯度下降法进行求解，使用如下表达式更新当前待定位节点的初始位置坐标：
Figure BDA0002302757440000191
Figure BDA0002302757440000192
式中各字符的含义可参见上文描述，此处不做赘述。
本发明一种实施方式中，在当前待定位节点处于二维空间的情况下，上述步骤S104的实现方式，可以包括：
在到达角度在π周期内的情况下，利用初始位置坐标，以及方向角，构建当前待定位节点接收邻居节点信号时的到达角度，具体的构建过程可参见上文描述，不同之处在于，所构建的水平方向到达角度不同。所构建的到达角度表示为：
Figure BDA0002302757440000193
式中各字符的含义可参见上文描述，此处不做赘述。
为了解决π周期内到达角度的模糊性，引入整数变量gmn，如果信号的入射方向是从天线阵列正面入射，则gmn的值设置为0，否则为1。将水平方向到达角度的链路损耗表示为：
(1-gmn)lossA(θmn)+gmnlossA(-θmn)
进一步，将链路损耗的最小化目标函数表示为：
Figure BDA0002302757440000194
其中，约束条件为gmn∈[0,1]，θmn表示实际所获取的第m个当前待定位节点接收到来自第n个节点的水平方向到达角度，lossA(θmn)，lossA(-θmn)表示实际获取水平方向到达角度的链路损耗，[N]表示当前待定位节点的集合，Rm表示第m个当前待定位节点的邻居节点的集合，gmn表示π周期内整数变量。本发明实施例中gmn的初始值可以设置为0.5。
将上述最小化问题通过梯度下降法进行求解，使用如下表达式更新当前待定位节点的初始位置坐标：
Figure BDA0002302757440000195
Figure BDA0002302757440000196
Figure BDA0002302757440000197
式中各字符的含义可参见上文描述，此处不做赘述。
本发明一种实施方式中，在当前待定位节点处于三维空间的情况下，上述步骤S203，利用当前待定位节点的初始位置坐标，及预设的最大似然估计算法，得到当前待定位节点的方向角的实现方式，可以包括：
利用当前待定位节点的初始位置坐标，及最大似然估计算法准则，将当前待定位节点的方向角{qm}m∈[N]＝{φm,αm,ψm}m∈[N]([N]＝0,1,…,N-1)估计表示为：
Figure BDA0002302757440000201
Figure BDA0002302757440000202
Figure BDA0002302757440000203
其中，omn表示第m个当前待定位节点接收到来自第n个节点的水平方向到达角度与垂直方向到达角度，Δxmn、Δymn、Δzmn表示世界坐标系中第m个当前待定位节点与第n个节点坐标的差值，Rm表示第m个当前待定位节点的邻居节点的集合，qm表示第m个当前待定位节点的方向角，
Figure BDA0002302757440000204
表示qm的估计值，p(omn|Δxmn,Δymn,Δzmn；qm)表示qm作为估计参数，在Δxmn,Δymn,Δzmn条件下omn出现的概率；
Figure BDA0002302757440000205
θmn分别表示实际获取第m个当前待定位节点接收到来自第n个节点的垂直方向到达角度和水平方向到达角度，
Figure BDA0002302757440000206
分别表示所构建的第m个当前待定位节点接收到来自第n个节点的垂直方向到达角度和水平方向到达角度，
Figure BDA0002302757440000207
表示垂直方向到达角度
Figure BDA0002302757440000208
是以均值为
Figure BDA0002302757440000209
方差为
Figure BDA00023027574400002010
的高斯分布变量，
Figure BDA00023027574400002011
表示上水平方向的到达角度θmn是以均值为
Figure BDA00023027574400002012
方差为
Figure BDA00023027574400002013
的高斯分布变量，[N]表示当前待定位节点的集合。
上述
Figure BDA00023027574400002014
可以通过暴力搜索进行求解，例如，使用网格算法和穷举法，示例性的，将φm，αm和ψm的范围值均匀分成O份，把O3个可能的取值带入该表达式中，该表达式等号右边最大化的输出即为最终方向角的估计。
本发明一种实施方式中，在当前待定位节点处于二维空间的情况下，上述步骤S203，利用当前待定位节点的初始位置坐标，及预设的最大似然估计算法，得到当前待定位节点的方向角的实现方式，可以包括：
利用当前待定位节点的初始位置坐标，及最大似然估计算法准则，将当前待定位节点的方向角ψm估计表示为：
Figure BDA0002302757440000211
Figure BDA0002302757440000212
其中，Δxmn、Δymn表示世界坐标系中第m个当前待定位节点与第n个节点坐标的差值，Rm表示第m个当前待定位节点的邻居节点的集合，ψm表示第m个当前待定位节点的方向角，
Figure BDA0002302757440000213
表示ψm的估计值，p(θmn|Δxmn,Δymn；ψm)表示ψm作为估计参数，在Δxmn,Δymn条件下θmn出现的概率；θmn表示实际获取第m个当前待定位节点接收到来自第n个节点的水平方向到达角度，
Figure BDA0002302757440000214
表示所构建的第m个当前待定位节点接收到来自第n个节点的水平方向到达角度，
Figure BDA0002302757440000215
表示水平方向到达角度θmn是以均值为
Figure BDA0002302757440000216
方差为
Figure BDA0002302757440000217
的高斯分布变量。
本发明实施例提供的一种基于到达角度测距的协作定位方法，使用梯度下降法更新当前待定位节点的初始位置坐标，降低了计算的复杂度；且，本发明实施例中基于所获取的当前待定位节点接收通信半径内邻居节点信号时的到达角度更新当前待定位节点的初始位置坐标，即在定位过程中不仅考虑了当前待定位节点接收位置已知的节点信号时的到达角度，还考虑了当前待定位节点接收位置未知的节点信号时的到达角度，可以使用通信半径内多个待定位节点相互协作实现定位，提高节点定位的准确度。
相应于上述方法实施例，本发明实施例提供了一种基于到达角度测距的协作定位装置，如图7所示，所述装置可以包括：
第一获取模块301，用于获取当前待定位节点的初始位置坐标，初始位置坐标为卫星定位坐标。第二获取模块302，用于基于当前待定位节点与当前待定位节点的各邻居节点间的信号交互，获取当前待定位节点接收邻居节点信号时的到达角度；邻居节点包括：与当前待定位节点的通信半径小于预设阈值的，位置已知的节点和位置未知的节点。第三获取模块303，用于获取当前待定位节点的方向角。位置更新模块304，用于利用所获取的到达角度，初始位置坐标，以及方向角，使用梯度下降法更新当前待定位节点的初始位置坐标，得到当前待定位节点的当前位置坐标。节点定位模块305，用于当当前待定位节点的当前位置坐标达到第一预设收敛条件时，将当前位置坐标确定为当前待定位节点的目标定位坐标。
本发明实施例提供的一种基于到达角度测距的协作定位装置，使用梯度下降法更新当前待定位节点的初始位置坐标，降低了计算的复杂度；且，本发明实施例中基于所获取的当前待定位节点接收通信半径内邻居节点信号时的到达角度更新当前待定位节点的初始位置坐标，即在定位过程中不仅考虑了当前待定位节点接收位置已知的节点信号时的到达角度，还考虑了当前待定位节点接收位置未知的节点信号时的到达角度，可以使用通信半径内多个待定位节点相互协作实现定位，提高节点定位的准确度。
可选地，上述装置还可以包括：
第一判断模块，用于判断当前待定位节点的当前位置坐标是否达到第一预设收敛条件。第一触发模块，用于在第一判断模块判断出当前待定位节点的当前位置坐标没有达到第一预设收敛条件时，触发位置更新模块304，利用所获取的到达角度，初始位置坐标，以及方向角，使用梯度下降法更新当前待定位节点的初始位置坐标，得到当前待定位节点的当前位置坐标。
可选的，该装置可以包括：第一获取模块，用于获取当前待定位节点的初始位置坐标，初始位置坐标为卫星定位坐标。第二获取模块，用于基于当前待定位节点与当前待定位节点的各邻居节点间的信号交互，获取当前待定位节点接收邻居节点信号时的到达角度；邻居节点包括：与当前待定位节点的通信半径小于预设阈值的，位置已知的节点和位置未知的节点。第三获取模块具体用于，利用当前待定位节点的初始位置坐标，及预设的最大似然估计算法，得到当前待定位节点的方向角。位置更新模块，用于利用所获取的到达角度，初始位置坐标，以及方向角，使用梯度下降法更新当前待定位节点的初始位置坐标，得到当前待定位节点的当前位置坐标。节点定位模块，用于当当前待定位节点的当前位置坐标达到第一预设收敛条件时，将当前位置坐标确定为当前待定位节点的目标定位坐标。第二判断模块，用于判断当前待定位节点的目标定位坐标是否达到第二预设收敛条件。定位确定模块，用于在第二判断模块判断出当前待定位节点的目标定位坐标达到第二预设收敛条件时，将当前待定位节点的目标定位坐标，确定为当前待定位节点的最终目标定位坐标。第二触发模块，用于在第二判断模块判断出当前待定位节点的目标定位坐标没有达到第二预设收敛条件时，将当前待定位节点的初始位置坐标更新为目标定位坐标，并触发第三获取模块，利用当前待定位节点的初始位置坐标，及预设的最大似然估计算法，得到当前待定位节点的方向角。
可选地，在当前待定位节点处于三维空间的情况下，位置更新模块304，具体用于：
在到达角度在2π周期内的情况下，利用初始位置坐标，以及方向角，构建当前待定位节点接收邻居节点信号时的到达角度，所构建的到达角度表示为：
Figure BDA0002302757440000221
和
Figure BDA0002302757440000222
其中，
Figure BDA0002302757440000223
表示所构建的第m个当前待定位节点接收到来自第n个节点的垂直方向到达角度，
Figure BDA0002302757440000231
表示所构建的第m个当前待定位节点接收到来自第n个节点的水平方向到达角度，
Figure BDA0002302757440000232
分别表示
Figure BDA0002302757440000233
向量在第m个当前待定位节点的局部坐标系中x′轴、y′轴、z′轴上的投影，
Figure BDA0002302757440000234
向量表示第m个当前待定位节点和第n个节点的坐标相减得到的向量，Vm表示
Figure BDA0002302757440000235
的值是在以第m个当前待定位节点为原点建立的局部坐标系中，mn表示电磁波从第n个节点发出到第m个当前待定位节点接收的链路，dmn表示
Figure BDA0002302757440000236
向量的长度。
将链路损耗的最小化目标函数表示为：
Figure BDA0002302757440000237
其中，θmn表示实际所获取的第m个当前待定位节点接收到来自第n个节点的水平方向到达角度，
Figure BDA0002302757440000238
表示实际所获取的第m个当前待定位节点接收到来自第n个节点的垂直方向到达角度，lossA(θmn)表示实际获取水平方向到达角度的链路损耗，
Figure BDA0002302757440000239
表示实际获取垂直方向到达角度的链路损耗，[N]表示当前待定位节点的集合，Rm表示第m个当前待定位节点的邻居节点的集合。
使用如下表达式更新当前待定位节点的初始位置坐标。
Figure BDA00023027574400002310
Figure BDA00023027574400002311
Figure BDA00023027574400002312
其中，λt表示第t次迭代的步长，
Figure BDA00023027574400002313
分别表示第t次迭代中第m个当前待定位节点在世界坐标系x轴、y轴和z轴的坐标估计值，
Figure BDA00023027574400002314
和
Figure BDA00023027574400002315
分别表示xm、ym、zm的梯度方向；
Figure BDA00023027574400002316
和
Figure BDA00023027574400002317
的值，通过分别对最小化目标函数求xm、ym、zm的一阶导数得到。
可选地，在当前待定位节点处于三维空间的情况下，位置更新模块304，具体用于：
在到达角度在π周期内的情况下，利用初始位置坐标，以及方向角，构建当前待定位节点接收邻居节点信号时的到达角度，所构建的到达角度表示为：
Figure BDA0002302757440000241
和
Figure BDA0002302757440000242
其中，
Figure BDA0002302757440000243
表示所构建的第m个当前待定位节点接收到来自第n个节点的垂直方向到达角度，
Figure BDA0002302757440000244
表示所构建的第m个当前待定位节点接收到来自第n个节点的水平方向到达角度，
Figure BDA0002302757440000245
分别表示
Figure BDA0002302757440000246
向量在第m个当前待定位节点的局部坐标系中x′轴、y′轴、z′轴上的投影，
Figure BDA0002302757440000247
向量表示第m个当前待定位节点和第n个节点的坐标相减得到的向量，Vm表示
Figure BDA0002302757440000248
的值是在以第m个当前待定位节点为原点建立的局部坐标系中，mn表示电磁波从第n个节点发出到第m个当前待定位节点接收的链路，dmn表示
Figure BDA0002302757440000249
向量的长度。
将链路损耗的最小化目标函数表示为：
Figure BDA00023027574400002410
其中，约束条件为gmn∈[0,1]，θmn表示实际所获取的第m个当前待定位节点接收到来自第n个节点的水平方向到达角度，
Figure BDA00023027574400002411
表示实际所获取的第m个当前待定位节点接收到来自第n个节点的垂直方向到达角度，lossA(θmn)，lossA(-θmn)表示实际获取水平方向到达角度的链路损耗，
Figure BDA00023027574400002412
表示实际获取垂直方向到达角度的链路损耗，[N]表示当前待定位节点的集合，Rm表示第m个当前待定位节点的邻居节点的集合，gmn表示π周期内整数变量。
使用如下表达式更新当前待定位节点的初始位置坐标：
Figure BDA00023027574400002413
Figure BDA00023027574400002414
Figure BDA00023027574400002415
Figure BDA00023027574400002416
其中，λt表示第t次迭代的步长，
Figure BDA00023027574400002417
分别表示第t次迭代中第m个当前待定位节点在世界坐标系x轴、y轴和z轴的坐标估计值，
Figure BDA00023027574400002418
表示第t次迭代中第m个当前待定位节点整数变量gmn的估计值，
Figure BDA00023027574400002419
和
Figure BDA00023027574400002420
分别表示xm、ym、zm的梯度方向；
Figure BDA0002302757440000251
和
Figure BDA0002302757440000252
的值，通过分别对最小化目标函数求xm、ym、zm的一阶导数得到，Proj(x)＝min(max(x,0),1)。
可选地，在当前待定位节点处于二维空间的情况下，位置更新模块304，具体用于：
在到达角度在2π周期内的情况下，利用初始位置坐标，以及方向角，构建当前待定位节点接收邻居节点信号时的到达角度，所构建的到达角度表示为：
Figure BDA0002302757440000253
其中，
Figure BDA0002302757440000254
表示所构建的第m个当前待定位节点接收到来自第n个节点的水平方向到达角度，
Figure BDA0002302757440000255
分别表示
Figure BDA0002302757440000256
向量在第m个当前待定位节点的局部坐标系中x′轴、y′轴上的投影，
Figure BDA0002302757440000257
向量表示第m个当前待定位节点和第n个节点的坐标相减得到的向量，Vm表示
Figure BDA0002302757440000258
的值是在以第m个当前待定位节点为原点建立的局部坐标系中，mn表示电磁波从第n个节点发出到第m个当前待定位节点接收的链路。
将链路损耗的最小化目标函数表示为：
Figure BDA0002302757440000259
其中，θmn表示实际所获取的第m个当前待定位节点接收到来自第n个节点的水平方向到达角度，lossA(θmn)表示实际获取水平方向到达角度的链路损耗，[N]表示当前待定位节点的集合，Rm表示第m个当前待定位节点的邻居节点的集合。
使用如下表达式更新当前待定位节点的初始位置坐标：
Figure BDA00023027574400002510
Figure BDA00023027574400002511
其中，λt表示第t次迭代的步长，
Figure BDA00023027574400002512
分别表示第t次迭代中第m个当前待定位节点在世界坐标系x轴和y轴的坐标估计值，
Figure BDA00023027574400002513
和
Figure BDA00023027574400002514
分别表示xm、ym的梯度方向；
Figure BDA00023027574400002515
和
Figure BDA00023027574400002516
的值，通过分别对最小化目标函数求xm、ym的一阶导数得到。
可选地，在当前待定位节点处于二维空间的情况下，位置更新模块304，具体用于：
在到达角度在π周期内的情况下，利用初始位置坐标，以及方向角，构建当前待定位节点接收邻居节点信号时的到达角度，所构建的到达角度表示为：
Figure BDA0002302757440000261
其中，
Figure BDA0002302757440000262
表示所构建的第m个当前待定位节点接收到来自第n个节点的水平方向到达角度，
Figure BDA0002302757440000263
分别表示
Figure BDA0002302757440000264
向量在第m个当前待定位节点的局部坐标系中x′轴、y′轴上的投影，
Figure BDA0002302757440000265
向量表示第m个当前待定位节点和第n个节点的坐标相减得到的向量，Vm表示
Figure BDA0002302757440000266
的值是在以第m个当前待定位节点为原点建立的局部坐标系中，mn表示电磁波从第n个节点发出到第m个当前待定位节点接收的链路。
将链路损耗的最小化目标函数表示为：
Figure BDA0002302757440000267
其中，约束条件为gmn∈[0,1]，θmn表示实际所获取的第m个当前待定位节点接收到来自第n个节点的水平方向到达角度，lossA(θmn)，lossA(-θmn)表示实际获取水平方向到达角度的链路损耗，[N]表示当前待定位节点的集合，Rm表示第m个当前待定位节点的邻居节点的集合，gmn表示π周期内整数变量。
使用如下表达式更新当前待定位节点的初始位置坐标：
Figure BDA0002302757440000268
Figure BDA0002302757440000269
Figure BDA00023027574400002610
其中，λt表示第t次迭代的步长，
Figure BDA00023027574400002611
分别表示第t次迭代中第m个当前待定位节点在世界坐标系x轴和y轴的坐标估计值，
Figure BDA00023027574400002612
表示第t次迭代中第m个当前待定位节点整数变量gmn的估计值，
Figure BDA00023027574400002613
和
Figure BDA00023027574400002614
分别表示xm、ym的梯度方向；
Figure BDA00023027574400002615
和
Figure BDA00023027574400002616
的值，通过分别对最小化目标函数求xm、ym的一阶导数得到，Proj(x)＝min(max(x,0),1)。
本发明实施例还提供了一种电子设备，如图8所示，包括处理器401、通信接口402、存储器403和通信总线404，其中，处理器401，通信接口402，存储器403通过通信总线404完成相互间的通信，存储器403，用于存放计算机程序；处理器401，用于执行存储器403上所存放的程序时，实现本发明实施例所提供的一种基于到达角度测距的协作定位方法的步骤。
本发明实施例提供的一种电子设备，基于所获取的当前待定位节点接收通信半径内邻居节点信号时的到达角度更新当前待定位节点的初始位置坐标，即在定位过程中不仅考虑了当前待定位节点接收位置已知的节点信号时的到达角度，还考虑了当前待定位节点接收位置未知的节点信号时的到达角度，可以使用通信半径内多个待定位节点相互协作实现定位，提高节点定位的准确度。
上述电子设备提到的通信总线可以是外设部件互连标准(Peripheral ComponentInterconnect，PCI)总线或扩展工业标准结构(Extended Industry StandardArchitecture，EISA)总线等。该通信总线可以分为地址总线、数据总线、控制总线等。为便于表示，图中仅用一条粗线表示，但并不表示仅有一根总线或一种类型的总线。
通信接口用于上述电子设备与其他设备之间的通信。
存储器可以包括随机存取存储器(Random Access Memory，RAM)，也可以包括非易失性存储器(Non-Volatile Memory，NVM)，例如至少一个磁盘存储器。可选的，存储器还可以是至少一个位于远离前述处理器的存储装置。
上述的处理器可以是通用处理器，包括中央处理器(Central Processing Unit，CPU)、网络处理器(Network Processor，NP)等；还可以是数字信号处理器(Digital SignalProcessing，DSP)、专用集成电路(Application Specific Integrated Circuit，ASIC)、现场可编程门阵列(Field-Programmable Gate Array，FPGA)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件。
在本发明提供的又一实施例中，还提供了一种计算机可读存储介质，该计算机可读存储介质内存储有计算机程序，所述计算机程序被处理器执行时实现上述任一一种基于到达角度测距的协作定位方法的步骤。在本发明提供的又一实施例中，还提供了一种包含指令的计算机程序产品，当其在计算机上运行时，使得计算机执行上述实施例中任一一种基于到达角度测距的协作定位方法方法。
需要说明的是，在本文中，诸如第一和第二等之类的关系术语仅仅用来将一个实体或者操作与另一个实体或操作区分开来，而不一定要求或者暗示这些实体或操作之间存在任何这种实际的关系或者顺序。而且，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者设备不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、物品或者设备所固有的要素。在没有更多限制的情况下，由语句“包括一个……”限定的要素，并不排除在包括所述要素的过程、方法、物品或者设备中还存在另外的相同要素。本说明书中的各个实施例均采用相关的方式描述，各个实施例之间相同相似的部分互相参见即可，每个实施例重点说明的都是与其他实施例的不同之处。尤其，对于装置/电子设备实施例而言，由于其基本相似于方法实施例，所以描述的比较简单，相关之处参见方法实施例的部分说明即可。以上所述仅为本发明的较佳实施例而已，并非用于限定本发明的保护范围。凡在本发明的精神和原则之内所作的任何修改、等同替换、改进等，均包含在本发明的保护范围内。

Claims (10)

1.一种基于到达角度测距的协作定位方法，其特征在于，所述方法包括：
获取当前待定位节点的初始位置坐标，所述初始位置坐标为卫星定位坐标；
基于所述当前待定位节点与当前待定位节点的各邻居节点间的信号交互，获取所述当前待定位节点接收所述邻居节点信号时的到达角度；所述邻居节点包括：与所述当前待定位节点的通信半径小于预设阈值的，位置已知的节点和位置未知的节点；
获取所述当前待定位节点的方向角；
利用所获取的所述到达角度，所述初始位置坐标，以及所述方向角，使用梯度下降法更新所述当前待定位节点的初始位置坐标，得到所述当前待定位节点的当前位置坐标；
当所述当前待定位节点的所述当前位置坐标达到第一预设收敛条件时，将所述当前位置坐标确定为所述当前待定位节点的目标定位坐标。
2.根据权利要求1所述的方法，其特征在于，所述利用所获取的所述到达角度，所述初始位置坐标，以及所述方向角，使用梯度下降法更新所述当前待定位节点的初始位置坐标，得到所述当前待定位节点的当前位置坐标之后，所述方法还包括：
判断所述当前待定位节点的所述当前位置坐标是否达到第一预设收敛条件；
如果所述当前待定位节点的所述当前位置坐标没有达到第一预设收敛条件，则返回执行所述利用所获取的所述到达角度，所述初始位置坐标，以及所述方向角，使用梯度下降法更新所述当前待定位节点的初始位置坐标，得到所述当前待定位节点的当前位置坐标的步骤。
3.根据权利要求1所述的方法，其特征在于，所述获取所述当前待定位节点的方向角的步骤，包括：
利用所述当前待定位节点的初始位置坐标，及预设的最大似然估计算法，得到所述当前待定位节点的方向角；
所述当所述当前待定位节点的所述当前位置坐标达到第一预设收敛条件时，将所述当前位置坐标确定为所述当前待定位节点的目标定位坐标之后，所述方法还包括：
判断所述当前待定位节点的目标定位坐标是否达到第二预设收敛条件；
如果所述当前待定位节点的目标定位坐标达到第二预设收敛条件，则将所述当前待定位节点的目标定位坐标，确定为所述当前待定位节点的最终目标定位坐标；
如果所述当前待定位节点的目标定位坐标没有达到第二预设收敛条件，则将所述当前待定位节点的初始位置坐标更新为所述目标定位坐标，并执行利用所述当前待定位节点的初始位置坐标，及预设的最大似然估计算法，得到所述当前待定位节点的方向角的步骤。
4.根据权利要求1-3任一所述的方法，其特征在于，在所述当前待定位节点处于三维空间的情况下，所述利用所获取的所述到达角度，所述初始位置坐标，以及所述方向角，使用梯度下降法更新所述当前待定位节点的初始位置坐标的步骤，包括：
在所述到达角度在2π周期内的情况下，利用所述初始位置坐标，以及所述方向角，构建所述当前待定位节点接收所述邻居节点信号时的到达角度，所构建的到达角度表示为：
Figure FDA00027009247000000211
和
Figure FDA00027009247000000212
其中，
Figure FDA0002700924700000021
表示所构建的第m个当前待定位节点接收到来自第n个节点的垂直方向到达角度，
Figure FDA0002700924700000022
表示所构建的第m个当前待定位节点接收到来自第n个节点的水平方向到达角度，
Figure FDA0002700924700000023
分别表示
Figure FDA0002700924700000024
向量在第m个当前待定位节点的局部坐标系中x′轴、y′轴、z′轴上的投影，
Figure FDA0002700924700000025
向量表示第m个当前待定位节点和第n个节点的坐标相减得到的向量，Vm表示
Figure FDA0002700924700000026
的值是在以第m个当前待定位节点为原点建立的局部坐标系中，mn表示电磁波从第n个节点发出到第m个当前待定位节点接收的链路，dmn表示
Figure FDA0002700924700000027
向量的长度；
将链路损耗的最小化目标函数表示为：
Figure FDA0002700924700000028
其中，θmn表示实际所获取的第m个当前待定位节点接收到来自第n个节点的水平方向到达角度，
Figure FDA0002700924700000029
表示实际所获取的第m个当前待定位节点接收到来自第n个节点的垂直方向到达角度，lossA(θmn)表示实际获取水平方向到达角度的链路损耗，
Figure FDA00027009247000000210
表示实际获取垂直方向到达角度的链路损耗，[N]表示当前待定位节点的集合，Rm表示第m个当前待定位节点的邻居节点的集合；
使用如下表达式更新所述当前待定位节点的初始位置坐标：
Figure FDA0002700924700000031
Figure FDA0002700924700000032
Figure FDA0002700924700000033
其中，λt表示第t次迭代的步长，xm、ym、zm分别表示第m个当前待定位节点在世界坐标系x轴、y轴和z轴的坐标值，
Figure FDA0002700924700000034
分别表示第t次迭代中第m个当前待定位节点在世界坐标系x轴、y轴和z轴的坐标估计值，
Figure FDA0002700924700000035
分别表示第t+1次迭代中第m个当前待定位节点在世界坐标系x轴、y轴和z轴的坐标估计值，
Figure FDA0002700924700000036
和
Figure FDA0002700924700000037
分别表示xm、ym、zm的梯度方向；
Figure FDA0002700924700000038
和
Figure FDA0002700924700000039
的值，通过分别对所述最小化目标函数求xm、ym、zm的一阶导数得到。
5.根据权利要求1-3任一所述的方法，其特征在于，在所述当前待定位节点处于三维空间的情况下，所述利用所获取的所述到达角度，所述初始位置坐标，以及所述方向角，使用梯度下降法更新所述当前待定位节点的初始位置坐标的步骤，包括：
在所述到达角度在π周期内的情况下，利用所述初始位置坐标，以及所述方向角，构建所述当前待定位节点接收所述邻居节点信号时的到达角度，所构建的到达角度表示为：
Figure FDA00027009247000000317
和
Figure FDA00027009247000000318
其中，
Figure FDA00027009247000000310
表示所构建的第m个当前待定位节点接收到来自第n个节点的垂直方向到达角度，
Figure FDA00027009247000000311
表示所构建的第m个当前待定位节点接收到来自第n个节点的水平方向到达角度，
Figure FDA00027009247000000312
分别表示
Figure FDA00027009247000000313
向量在第m个当前待定位节点的局部坐标系中x′轴、y′轴、z′轴上的投影，
Figure FDA00027009247000000314
向量表示第m个当前待定位节点和第n个节点的坐标相减得到的向量，Vm表示
Figure FDA00027009247000000315
的值是在以第m个当前待定位节点为原点建立的局部坐标系中，mn表示电磁波从第n个节点发出到第m个当前待定位节点接收的链路，dmn表示
Figure FDA00027009247000000316
向量的长度；
将链路损耗的最小化目标函数表示为：
Figure FDA0002700924700000041
其中，约束条件为gmn∈[0,1]，θmn表示实际所获取的第m个当前待定位节点接收到来自第n个节点的水平方向到达角度，
Figure FDA0002700924700000042
表示实际所获取的第m个当前待定位节点接收到来自第n个节点的垂直方向到达角度，lossA(θmn)，lossA(-θmn)表示实际获取水平方向到达角度的链路损耗，
Figure FDA0002700924700000043
表示实际获取垂直方向到达角度的链路损耗，[N]表示当前待定位节点的集合，Rm表示第m个当前待定位节点的邻居节点的集合，gmn表示π周期内整数变量；
使用如下表达式更新所述当前待定位节点的初始位置坐标：
Figure FDA0002700924700000044
Figure FDA0002700924700000045
Figure FDA0002700924700000046
Figure FDA0002700924700000047
其中，λt表示第t次迭代的步长，xm、ym、zm分别表示第m个当前待定位节点在世界坐标系x轴、y轴和z轴的坐标值，
Figure FDA0002700924700000048
分别表示第t次迭代中第m个当前待定位节点在世界坐标系x轴、y轴和z轴的坐标估计值，
Figure FDA0002700924700000049
分别表示第t+1次迭代中第m个当前待定位节点在世界坐标系x轴、y轴和z轴的坐标估计值，
Figure FDA00027009247000000410
表示第t次迭代中第m个当前待定位节点整数变量gmn的估计值，
Figure FDA00027009247000000411
表示第t+1次迭代中第m个当前待定位节点整数变量gmn的估计值，
Figure FDA00027009247000000412
表示整数变量gmn的梯度方向，
Figure FDA00027009247000000413
和
Figure FDA00027009247000000414
分别表示xm、ym、zm的梯度方向；
Figure FDA00027009247000000415
和
Figure FDA00027009247000000416
的值，通过分别对最小化目标函数求xm、ym、zm的一阶导数得到，Proj(x)＝min(max(x,0),1)。
6.根据权利要求1-3任一所述的方法，其特征在于，在所述当前待定位节点处于二维空间的情况下，所述利用所获取的所述到达角度，所述初始位置坐标，以及所述方向角，使用梯度下降法更新所述当前待定位节点的初始位置坐标的步骤，包括：
在所述到达角度在2π周期内的情况下，利用所述初始位置坐标，以及所述方向角，构建所述当前待定位节点接收所述邻居节点信号时的到达角度，所构建的到达角度表示为：
Figure FDA0002700924700000051
其中，
Figure FDA0002700924700000052
表示所构建的第m个当前待定位节点接收到来自第n个节点的水平方向到达角度，
Figure FDA0002700924700000053
分别表示
Figure FDA0002700924700000054
向量在第m个当前待定位节点的局部坐标系中x′轴、y′轴上的投影，
Figure FDA0002700924700000055
向量表示第m个当前待定位节点和第n个节点的坐标相减得到的向量，Vm表示
Figure FDA0002700924700000056
的值是在以第m个当前待定位节点为原点建立的局部坐标系中，mn表示电磁波从第n个节点发出到第m个当前待定位节点接收的链路；
将链路损耗的最小化目标函数表示为：
Figure FDA0002700924700000057
其中，θmn表示实际所获取的第m个当前待定位节点接收到来自第n个节点的水平方向到达角度，lossA(θmn)表示实际获取水平方向到达角度的链路损耗，[N]表示当前待定位节点的集合，Rm表示第m个当前待定位节点的邻居节点的集合；
使用如下表达式更新所述当前待定位节点的初始位置坐标：
Figure FDA0002700924700000058
Figure FDA0002700924700000059
其中，λt表示第t次迭代的步长，xm、ym分别表示第m个当前待定位节点在世界坐标系x轴和y轴的坐标值，
Figure FDA00027009247000000510
分别表示第t次迭代中第m个当前待定位节点在世界坐标系x轴和y轴的坐标估计值，
Figure FDA00027009247000000511
分别表示第t+1次迭代中第m个当前待定位节点在世界坐标系x轴和y轴的坐标估计值，
Figure FDA00027009247000000512
和
Figure FDA00027009247000000513
分别表示xm、ym的梯度方向；
Figure FDA00027009247000000514
和
Figure FDA00027009247000000515
的值，通过分别对所述最小化目标函数求xm、ym的一阶导数得到。
7.根据权利要求1-3任一所述的方法，其特征在于，在所述当前待定位节点处于二维空间的情况下，所述利用所获取的所述到达角度，所述初始位置坐标，以及所述方向角，使用梯度下降法更新所述当前待定位节点的初始位置坐标的步骤，包括：
在所述到达角度在π周期内的情况下，利用所述初始位置坐标，以及所述方向角，构建所述当前待定位节点接收所述邻居节点信号时的到达角度，所构建的到达角度表示为：
Figure FDA0002700924700000061
其中，
Figure FDA0002700924700000062
表示所构建的第m个当前待定位节点接收到来自第n个节点的水平方向到达角度，
Figure FDA0002700924700000063
分别表示
Figure FDA0002700924700000064
向量在第m个当前待定位节点的局部坐标系中x′轴、y′轴上的投影，
Figure FDA0002700924700000065
向量表示第m个当前待定位节点和第n个节点的坐标相减得到的向量，Vm表示
Figure FDA0002700924700000066
的值是在以第m个当前待定位节点为原点建立的局部坐标系中，mn表示电磁波从第n个节点发出到第m个当前待定位节点接收的链路；
将链路损耗的最小化目标函数表示为：
Figure FDA0002700924700000067
其中，约束条件为gmn∈[0,1]，θmn表示实际所获取的第m个当前待定位节点接收到来自第n个节点的水平方向到达角度，lossA(θmn)，lossA(-θmn)表示实际获取水平方向到达角度的链路损耗，[N]表示当前待定位节点的集合，Rm表示第m个当前待定位节点的邻居节点的集合，gmn表示π周期内整数变量；
使用如下表达式更新所述当前待定位节点的初始位置坐标：
Figure FDA0002700924700000068
Figure FDA0002700924700000069
Figure FDA00027009247000000610
其中，λt表示第t次迭代的步长，xm、ym分别表示第m个当前待定位节点在世界坐标系x轴和y轴的坐标值，
Figure FDA00027009247000000611
分别表示第t次迭代中第m个当前待定位节点在世界坐标系x轴和y轴的坐标估计值，
Figure FDA00027009247000000612
分别表示第t+1次迭代中第m个当前待定位节点在世界坐标系x轴和y轴的坐标估计值，
Figure FDA0002700924700000071
表示第t次迭代中第m个当前待定位节点整数变量gmn的估计值，
Figure FDA0002700924700000072
表示第t+1次迭代中第m个当前待定位节点整数变量gmn的估计值，
Figure FDA0002700924700000073
表示整数变量gmn的梯度方向，
Figure FDA0002700924700000074
和
Figure FDA0002700924700000075
分别表示xm、ym的梯度方向；
Figure FDA0002700924700000076
和
Figure FDA0002700924700000077
的值，通过分别对最小化目标函数求xm、ym的一阶导数得到，Proj(x)＝min(max(x,0),1)。
8.一种基于到达角度测距的协作定位装置，其特征在于，所述装置包括：
第一获取模块，用于获取当前待定位节点的初始位置坐标，所述初始位置坐标为卫星定位坐标；
第二获取模块，用于基于所述当前待定位节点与当前待定位节点的各邻居节点间的信号交互，获取所述当前待定位节点接收所述邻居节点信号时的到达角度；所述邻居节点包括：与所述当前待定位节点的通信半径小于预设阈值的，位置已知的节点和位置未知的节点；
第三获取模块，用于获取所述当前待定位节点的方向角；
位置更新模块，用于利用所获取的所述到达角度，所述初始位置坐标，以及所述方向角，使用梯度下降法更新所述当前待定位节点的初始位置坐标，得到所述当前待定位节点的当前位置坐标；
节点定位模块，用于当所述当前待定位节点的所述当前位置坐标达到第一预设收敛条件时，将所述当前位置坐标确定为所述当前待定位节点的目标定位坐标。
9.一种电子设备，其特征在于，包括处理器、通信接口、存储器和通信总线，其中，处理器，通信接口，存储器通过通信总线完成相互间的通信；
存储器，用于存放计算机程序；
处理器，用于执行存储器上所存放的程序时，实现权利要求1-7任一所述的方法步骤。
10.一种计算机可读存储介质，其特征在于，所述计算机可读存储介质内存储有计算机程序，所述计算机程序被处理器执行时实现权利要求1-7任一所述的方法步骤。
CN201911227938.6A 2019-12-04 2019-12-04 一种基于到达角度测距的协作定位方法及装置 Active CN111093265B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201911227938.6A CN111093265B (zh) 2019-12-04 2019-12-04 一种基于到达角度测距的协作定位方法及装置

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201911227938.6A CN111093265B (zh) 2019-12-04 2019-12-04 一种基于到达角度测距的协作定位方法及装置

Publications (2)

Publication Number Publication Date
CN111093265A CN111093265A (zh) 2020-05-01
CN111093265B true CN111093265B (zh) 2020-12-11

Family

ID=70394687

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201911227938.6A Active CN111093265B (zh) 2019-12-04 2019-12-04 一种基于到达角度测距的协作定位方法及装置

Country Status (1)

Country Link
CN (1) CN111093265B (zh)

Citations (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN108957387A (zh) * 2018-05-21 2018-12-07 西安电子科技大学 一种卫星信号二维到达角估计方法及系统

Family Cites Families (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101800581A (zh) * 2009-02-09 2010-08-11 中兴通讯股份有限公司 基于频分双工系统的多用户波束赋形方法与装置
CN106526559B (zh) * 2016-09-30 2019-04-09 杭州电子科技大学 一种基于bcd-vsmm机动目标无源协同定位方法

Patent Citations (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN108957387A (zh) * 2018-05-21 2018-12-07 西安电子科技大学 一种卫星信号二维到达角估计方法及系统

Also Published As

Publication number Publication date
CN111093265A (zh) 2020-05-01

Similar Documents

Publication Publication Date Title
CN111208492B (zh) 车载激光雷达外参标定方法及装置、计算机设备及存储介质
CN110264502B (zh) 点云配准方法和装置
EP3995988A1 (en) Method and apparatus for establishing beacon map on basis of visual beacons
Zhao et al. Learning-based bias correction for time difference of arrival ultra-wideband localization of resource-constrained mobile robots
CN110930495A (zh) 基于多无人机协作的icp点云地图融合方法、系统、装置及存储介质
CN109769206B (zh) 一种室内定位融合方法、装置、存储介质及终端设备
CN104615880B (zh) 一种三维激光雷达点云匹配的快速icp方法
CN109959898B (zh) 一种座底式水声被动定位基阵自校准方法
Masmitja et al. Range-only single-beacon tracking of underwater targets from an autonomous vehicle: From theory to practice
CN115164900A (zh) 一种基于全向相机的城市环境下视觉辅助导航方法及系统
CN114739415A (zh) 基于多传感器融合的多车定位方法、装置及计算机设备
CN112822633B (zh) 一种基于误差补偿的定位方法，系统，终端设备及可读存储介质
CN112710982B (zh) 一种天线阵列波达角估计方法、系统、介质、设备及应用
Alkhatib et al. Further results on a robust multivariate time series analysis in nonlinear models with autoregressive and t-distributed errors
CN111735443B (zh) 一种基于赋值矩阵的密集目标航迹关联方法
CN111093265B (zh) 一种基于到达角度测距的协作定位方法及装置
WO2010066306A1 (en) Apparatus and method for constructing a sensor array used for direction of arrival (doa) estimation
CN109489658B (zh) 一种运动目标定位方法、装置及终端设备
CN111915570A (zh) 基于反向传播神经网络的大气延迟估计方法
CN111609849A (zh) 一种多站交会定位方法和系统
CN113203424B (zh) 多传感器的数据融合方法、装置及相关设备
CN115775242A (zh) 一种基于匹配的点云地图质量评估方法
CN112230194B (zh) 一种基于平移阵列的解模糊方法、设备及存储介质
Zhang et al. Vision-based localization in multi-agent networks with communication constraints
Fortunati et al. An EM-based approach to the relative sensor registration in multi-target scenarios

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant