CN109283562B - 一种车联网中车辆三维定位方法及装置 - Google Patents

一种车联网中车辆三维定位方法及装置 Download PDF

Info

Publication number
CN109283562B
CN109283562B CN201811132281.0A CN201811132281A CN109283562B CN 109283562 B CN109283562 B CN 109283562B CN 201811132281 A CN201811132281 A CN 201811132281A CN 109283562 B CN109283562 B CN 109283562B
Authority
CN
China
Prior art keywords
vehicle
dimensional
dimensional estimated
ranging
coordinates
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
CN201811132281.0A
Other languages
English (en)
Other versions
CN109283562A (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 CN201811132281.0A priority Critical patent/CN109283562B/zh
Publication of CN109283562A publication Critical patent/CN109283562A/zh
Application granted granted Critical
Publication of CN109283562B publication Critical patent/CN109283562B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • 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
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/42—Determining position
    • G01S19/45—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement

Landscapes

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

Abstract

本发明实施例提供了一种车联网中车辆三维定位方法及装置，为了解决现有技术中测量的相对距离存在较大偏差，导致最终确定的定位结果不准确的技术问题，所述方法通过采用待定位车辆的三维预估坐标进行定位，不仅可以掌握待定位车辆的二维预估坐标，而且可以掌握每个待定位车辆的高度，使用预设测距方法进行测距时，考虑每个待定位车辆的高度，使得得到的测距距离更接近于现实情况；并且，鉴别测距距离中的非视距NLOS，从测距距离中选择视距LOS对应的测距距离，这样可以减少NLOS对测距距离的影响，进而基于视距LOS对应的测距距离和待定位车辆的预估坐标，重新确定待定位车辆的三维预估坐标相较于现有技术更加准确。

Description

一种车联网中车辆三维定位方法及装置
技术领域
本发明涉及定位技术领域，特别是涉及一种车联网中车辆三维定位方法及装置。
背景技术
VANETs(VehicularAdHocNetworks，车联网)中位置感知能力已成为一个重要的研究方向。目前VANETs中，基于卫星的定位***主要有GNSS(the Global NavigationSatellite System，全球卫星导航***)，包括美国的GPS(Global Positioning System，全球定位***)，中国的北斗***等。这些卫星的定位***能够提供全天候、实时、连续且大范围内的定位服务，但是它们的使用受环境限制较大。例如，在城市环境中，GNSS发出的信号受到高楼的阻挡，接收机接收到的卫星信号不准确，这样很容易造成定位效果差。
目前可以采用协作定位来解决上述问题。协作定位不仅可以使用待定位车辆与已知坐标的锚点之间的绝对测距进行定位，而且可以利用待定位的车辆之间的相对测距来进行定位，其中待定位车辆之间的相对测距就是待定位车辆之间位置的非线性的约束关系，通过设计贝叶斯算法，利用与已知坐标的锚点之间的绝对测距以及待定位车辆之间的相对测距来计算得到待定位车辆的坐标，其中已知坐标的锚点为RSU。比如，作为待定位车辆A，作为待定位车辆B，车辆A通过测距方法，测量与车辆B之间的相对距离，然后，基于车辆B的相对位置和由GNSS得到的车辆A的初始位置，确定出车辆A的估计位置，从而通过已知位置较好地车辆，获得更好的定位结果，完成车车协作定位。
但是，在城市环境中，在使用测距方法，比如TOA(time of arrival，到达时间)时，车辆间LOS(line of sight，视距)很容易被建筑物遮挡，车辆的发射信号，通过反射、绕射以及散射等NLOS(non line of sight，非视距)将发射信号传递给接收信号的车辆时，TOA相对于TOA的直线距离存在较大的延迟，并且换算得到的距离，与车辆间的真实距离之间存在很大差异，这样会影响测量的相对距离存在较大偏差，导致最终确定的定位结果不准确。
发明内容
本发明实施例的目的在于提供一种车联网中车辆三维定位方法及装置，用以解决现有技术中测量的相对距离存在较大偏差，导致最终确定的定位结果不准确的技术问题。具体技术方案如下：
第一方面，本发明实施提供了车联网中车辆三维定位方法，应用于***，所述方法包括：
步骤A，获取待定位车辆的车辆高度以及由全球卫星导航***得到所述待定位车辆的二维预估坐标，形成的三维预估坐标；
步骤B，获取每个待定位车辆使用预设测距方法中的任一或者多种测距方法，进行与其他节点之间的测距，得到所述待定位车辆与所述其他节点之间的距离，作为测距距离，其中，所述其他节点包括：路边单元RSU以及除所述待定位车辆以外的其他车辆，所述其他车辆包括：待定位车辆；
步骤C，利用车辆的二维轮廓构建的第一R树以及建筑物的二维轮廓构建的第二R树；利用所述第二R树的搜索功能，鉴别所述测距距离中的建筑物引起的非视距NLOSb；以及利用所述车辆的高度以及所述第一R树的搜索功能，鉴别所述测距距离中的其他车辆引起的非视距NLOSv，从所述测距距离中选择除所述NLOSb和所述NLOSv以外的视距LOS对应的测距距离，其中所述第一R树包含单个车辆构成的最小矩形，所述第二R树包含单个建筑物构成的最小矩形；
步骤D，基于LOS对应的测距距离和所述待定位车辆的三维预估坐标，重新确定所述待定位车辆的三维预估坐标；确定所述三维预估坐标的置信度是否高于预设置信度阈值；如果不高于所述预设置信度阈值，继续执行步骤C和步骤D，直至所述置信度高于所述预设置信度阈值，将所述三维预估坐标，作为待定位车辆的定位坐标。
进一步的，所述利用车辆的二维轮廓构建的第一R树以及建筑物的二维轮廓构建的第二R树；利用所述第二R树的搜索功能，鉴别所述测距距离中的建筑物引起的非视距NLOSb；以及利用所述车辆的高度以及所述第一R树的搜索功能，鉴别所述测距距离中的其他车辆引起的非视距NLOSv，从所述测距距离中选择除所述NLOSb和所述NLOSv以外的视距LOS对应的测距距离，包括：
利用真实的每个车辆的二维轮廓及每个车辆的二维预估坐标，得到所有车辆的二维轮廓的二维预估坐标；
将所有车辆的二维轮廓的二维预估坐标，构建得到第一R树；
从开源可编辑的地图服务中获取所有建筑物的二维轮廓的二维预估坐标；
将所有建筑物的二维轮廓的二维预估坐标，构建得到第二R树；
利用第二R树的搜索功能，若判定所述测距距离中存在NLOSb；以及
利用所有车辆的高度以及第一R树的搜索功能，若判定所述测距距离中存在NLOSv，从所述测距距离中选择除所述NLOSb和所述NLOSv以外的LOS对应的测距距离。
进一步的，所述基于所述视距LOS对应的测距距离和所述待定位车辆的预估坐标，重新确定所述待定位车辆的三维预估坐标；确定所述三维预估坐标的置信度是否高于预设置信度阈值；如果不高于所述预设置信度阈值，继续执行步骤C和步骤D，直至所述置信度高于所述预设置信度阈值，将所述三维预估坐标，作为待定位车辆的定位坐标，包括：
利用所述LOS对应的测距距离和所述待定位车辆的三维预估坐标，采用广义近似消息传递GAMP算法，第t次重新确定所述待定位车辆的三维预估坐标，并判断所有待定位车辆重新确定的三维预估坐标是否达到收敛条件；如果未达到，则继续执行步骤C和步骤D，直至达到所述收敛条件，将最后一次重新确定的所述待定位车辆的三维预估坐标，作为所述待定位车辆的定位坐标，其中，t大于0，小于或等于最大迭代次数。
进一步的，所述利用所述LOS对应的测距距离和所述待定位车辆的三维预估坐标，采用广义近似消息传递GAMP算法，第t次重新确定所述待定位车辆的三维预估坐标，并判断所有待定位车辆重新确定的三维预估坐标是否达到收敛条件；如果未达到，则继续执行步骤C和步骤D，直至达到所述收敛条件，将最后一次重新确定的所述待定位车辆的三维预估坐标，作为所述待定位车辆的定位坐标，包括：
利用所述LOS对应的测距距离和所述待定位车辆的三维预估坐标，采用广义近似消息传递GAMP算法，本次重新确定所述待定位车辆的三维预估坐标；
判断本次重新确定的所述待定位车辆的三维预估坐标与上次重新确定的所述待定位车辆的三维预估坐标之间的差值是否小于阈值；
若本次重新确定的所述待定位车辆的三维预估坐标与上次重新确定的所述待定位车辆的三维预估坐标之间的差值小于阈值，则将所述待定位车辆，作为已定位车辆，并将本次重新确定的所述待定位车辆的三维预估坐标，作为已定位车辆的三维预估坐标；
删除所述待定位车辆的三维预估坐标中对应的所述已定位车辆的三维预估坐标，将已定位车辆加入到RSU集合中，以使由车辆和RSU形成的网络结构发生变化；
利用除所述已定位车辆的三维预估坐标以外的其他待定位车辆的三维预估坐标，返回继续执行步骤C，直至达到所述收敛条件，将最后一次重新确定的所述待定位车辆的三维预估坐标，作为所述待定位车辆的定位坐标。
进一步的，所述从所述测距距离中选择除所述NLOSb和所述NLOSv以外的视距LOS对应的测距距离之后，所述方法还包括：
判定上一次重新确定的重要性采样的后验概率的均值与上一次之前的一次重新确定的重要性采样的后验概率的均值之间的误差大于或等于阀值，采用重要性采样方法，确定GAMP算法中的后验概率的方差和均值；
基于所述后验概率的方差和均值，利用GAMP算法，重新确定所述待定位车辆的三维预估坐标；以及
若判定上一次重新确定的重要性采样的后验概率的均值与上一次之前的一次重新确定的重要性采样方法的后验概率的均值之间的误差小于阀值，则删减此次重要性采样方法中所用的粒子的数量，继续执行采用重要性采样方法，确定GAMP算法中的后验概率的方差和均值的步骤；
基于所述本次的后验概率的方差和均值，利用GAMP算法，重新确定所述待定位车辆的三维预估坐标以外的其他待定位车辆的三维预估坐标，返回继续执行步骤C，直至达到所述收敛条件，将最后一次重新确定的所述待定位车辆的三维预估坐标，作为所述待定位车辆的定位坐标。
第二方面，本发明实施提供了车联网中车辆三维定位装置，应用于***，所述装置包括：
第一获取模块，用于获取待定位车辆的车辆高度以及由全球卫星导航***得到所述待定位车辆的二维预估坐标，形成的三维预估坐标；
第二获取模块，用于获取每个待定位车辆使用预设测距方法中的任一或者多种测距方法，进行与其他节点之间的测距，得到所述待定位车辆与所述其他节点之间的距离，作为测距距离，其中，所述其他节点包括：路边单元RSU以及除所述待定位车辆以外的其他车辆，所述其他车辆包括：待定位车辆；
鉴别选择模块，用于利用车辆的二维轮廓构建的第一R树以及建筑物的二维轮廓构建的第二R树；利用所述第二R树的搜索功能，鉴别所述测距距离中的建筑物引起的非视距NLOSb；以及利用所述车辆的高度以及所述第一R树的搜索功能，鉴别所述测距距离中的其他车辆引起的非视距NLOSv，从所述测距距离中选择除所述NLOSb和所述NLOSv以外的视距LOS对应的测距距离，其中所述第一R树包含单个车辆构成的最小矩形，所述第二R树包含单个建筑物构成的最小矩形；
确定模块，用于基于LOS对应的测距距离和所述待定位车辆的三维预估坐标，重新确定所述待定位车辆的三维预估坐标；确定所述三维预估坐标的置信度是否高于预设置信度阈值；如果不高于所述预设置信度阈值，继续执行所述鉴别选择模块和所述确定模块中的步骤，直至所述置信度高于所述预设置信度阈值，将所述三维预估坐标，作为待定位车辆的定位坐标。
进一步的，所述确定模块，具体用于：
利用所述LOS对应的测距距离和所述待定位车辆的三维预估坐标，采用广义近似消息传递GAMP算法，第t次重新确定所述待定位车辆的三维预估坐标，并判断所有待定位车辆重新确定的三维预估坐标是否达到收敛条件；如果未达到，则继续执行所述鉴别选择模块和所述确定模块中的步骤，直至达到所述收敛条件，将最后一次重新确定的所述待定位车辆的三维预估坐标，作为所述待定位车辆的定位坐标，其中，t大于0，小于或等于最大迭代次数。
进一步的，所述确定模块，具体用于：
利用所述LOS对应的测距距离和所述待定位车辆的三维预估坐标，采用广义近似消息传递GAMP算法，本次重新确定所述待定位车辆的三维预估坐标；
判断本次重新确定的所述待定位车辆的三维预估坐标与上次重新确定的所述待定位车辆的三维预估坐标之间的差值是否小于阈值；
若本次重新确定的所述待定位车辆的三维预估坐标与上次重新确定的所述待定位车辆的三维预估坐标之间的差值小于阈值，则将所述待定位车辆，作为已定位车辆，并将本次重新确定的所述待定位车辆的三维预估坐标，作为已定位车辆的三维预估坐标；
删除所述待定位车辆的三维预估坐标中对应的所述已定位车辆的三维预估坐标，将已定位车辆加入到RSU集合中，以使由车辆和RSU形成的网络结构发生变化；
利用除所述已定位车辆的三维预估坐标以外的其他待定位车辆的三维预估坐标，返回继续执行所述鉴别选择模块和所述确定模块中的步骤，直至达到所述收敛条件，将最后一次重新确定的所述待定位车辆的三维预估坐标，作为所述待定位车辆的定位坐标；
所述确定模块，具体还用于：
判定上一次重新确定的重要性采样的后验概率的均值与上一次之前的一次重新确定的重要性采样的后验概率的均值之间的误差大于或等于阀值，采用重要性采样方法，确定GAMP算法中的后验概率的方差和均值；
基于所述后验概率的方差和均值，利用GAMP算法，重新确定所述待定位车辆的三维预估坐标；以及
若判定上一次重新确定的重要性采样的后验概率的均值与上一次之前的一次重新确定的重要性采样方法的后验概率的均值之间的误差小于阀值，则删减此次重要性采样方法中所用的的粒子的数量，继续执行采用重要性采样方法，确定GAMP算法中的后验概率的方差和均值的步骤；
基于所述本次的后验概率的方差和均值，利用GAMP算法，重新确定所述待定位车辆的三维预估坐标以外的其他待定位车辆的三维预估坐标，返回继续执行所述鉴别选择模块和所述确定模块的步骤，直至达到所述收敛条件，将最后一次重新确定的所述待定位车辆的三维预估坐标，作为所述待定位车辆的定位坐标。
第三方面，本发明实施提供了一种电子设备，包括处理器、通信接口、存储器和通信总线，其中，处理器，通信接口，存储器通过通信总线完成相互间的通信；
存储器，用于存放计算机程序；
处理器，用于执行存储器上所存放的程序时，实现第一方面所述的方法步骤。
第四方面，本发明实施提供了一种计算机可读存储介质，所述计算机可读存储介质中存储有指令，当其在计算机上运行时，使得计算机执行上述第一方面任一的方法。
第五方面，本发明实施还提供了一种包含指令的计算机程序产品，当其在计算机上运行时，使得计算机执行上述第一方面任一的方法。
本发明实施例提供的一种车联网中车辆三维定位方法及装置，步骤A，获取待定位车辆的车辆高度以及由全球卫星导航***得到待定位车辆的二维预估坐标，形成的三维预估坐标；步骤B，获取每个待定位车辆使用预设测距方法中的任一测距方法，进行与其他节点之间的测距，得到待定位车辆与其他节点之间的距离，作为测距距离，其中，其他节点包括：路边单元RSU以及除待定位车辆以外的其他车辆；步骤C，利用车辆的二维维预估坐标以及建筑物的二维轮廓构建R树，其中建筑物和车辆表示为能包含车辆或者建筑物的最小的矩形，利用R树的搜索功能，鉴别距离中的建筑物引起的非视距NLOSb，利用车辆的高度以及R树来判断距离中由于其他车辆引起的非视距NLOSv，从测距距离中选择视距LOS对应的测距距离；步骤D，基于LOS对应的测距距离和待定位车辆的三维预估坐标，重新确定待定位车辆的三维预估坐标；确定三维预估坐标的置信度是否高于预设置信度阈值；如果不高于预设置信度阈值，继续执行步骤C和步骤D，直至置信度高于预设置信度阈值，将三维预估坐标，作为待定位车辆的定位坐标。
由此可见，相较于现有技术，采用待定位车辆的三维预估坐标进行定位，不仅可以掌握待定位车辆的二维预估坐标，而且可以掌握每个待定位车辆的高度，使用预设测距方法进行测距时，考虑每个待定位车辆的高度，使得得到的测距距离更接近于现实情况；并且，鉴别测距距离中的非视距NLOS，从测距距离中选择视距LOS对应的测距距离，这样可以减少NLOS对测距距离的影响，进而基于视距LOS对应的测距距离和待定位车辆的预估坐标，重新确定待定位车辆的三维预估坐标相较于现有技术更加准确。
当然，实施本发明的任一产品或方法并不一定需要同时达到以上所述的所有优点。
附图说明
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
图1为本发明实施例提供的车联网中车辆三维定位方法的第一流程示意图；
图2为本发明实施例提供实际应用中的NLOSv与NLOSb的实际情况示意图；
图3为本发明实施例提供的车联网中车辆三维定位方法的第二流程示意图；
图4为本发明实施例的车联网中车辆三维定位装置的结构示意图；
图5为本发明实施例提供的电子设备的结构示意图。
具体实施方式
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
针对现有技术中测量的相对距离存在较大偏差，导致最终确定的定位结果不准确的技术问题，本发明实施例提供一种车联网中车辆三维定位方法及装置，可以采用待定位车辆的三维预估坐标进行定位，不仅可以掌握待定位车辆的二维预估坐标，而且可以掌握每个待定位车辆的高度，使用预设测距方法进行测距时，考虑每个待定位车辆的高度，使得得到的测距距离更接近于现实情况；并且，鉴别测距距离中的非视距NLOS，从测距距离中选择视距LOS对应的测距距离，这样可以减少NLOS对测距距离的影响，进而基于视距LOS对应的测距距离和待定位车辆的预估坐标，重新确定所述待定位车辆的三维预估坐标相较于现有技术更加准确。
下面首先对本发明实施例提供的车联网中车辆三维定位方法进行介绍。
本发明实施例所提供的一种车联网中车辆三维定位方法，应用于VANETs中，具体可以应用于车载终端，VANETs中包括：待定位车辆和已知坐标的RSU(Roadside Unit，路边单元)。
如图1所示，本发明实施例所提供的车联网中车辆三维定位方法，应用于***，该方法可以包括如下步骤：
步骤A，获取待定位车辆的车辆高度以及由全球卫星导航***得到所述待定位车辆的二维预估坐标，形成的三维预估坐标。
待定位车辆需要通过全球卫星导航***来获取位置信息的车辆，这个待定位车辆的数目可以是一个，也可以是多个。
这里的二维预估坐标是指由GNSS直接定位出来的坐标，这个坐标一般属于二维坐标。GNSS(the Global Navigation Satellite System，全球卫星导航***)包括美国的GPS(Global Positioning System，全球定位***)、中国的北斗***、俄罗斯的GLONASS(GLOBAL NAVIGATION SATELLITE SYSTEM，格洛纳斯)以及欧盟的伽利略***等。
这些GNSS的使用受环境限制较大，因此为了体现更加精细的环境条件，考虑将车辆高度作为一个维度，那么上述二维坐标再加上一个车辆高度的维度，形成的三维坐标，称为三维预估坐标。
上述三维预估坐标可以通过向量
Figure BDA0001813868320000091
进行存储表示，这样方便使用。其中，本文中的T为转置，
Figure BDA0001813868320000092
中的p为待定位车辆三维预估坐标组成的向量，
Figure BDA0001813868320000093
为第N辆待定位车辆的三维预估坐标，
Figure BDA0001813868320000094
中的下角标N-1为表示向量元素p的序号。待定位车辆与路边单元RSU的坐标存储为
Figure BDA0001813868320000095
其中，n大于等于0，且小于等于N+A-1，
Figure BDA0001813868320000096
中的pn为第n辆待定位车辆的三维预估坐标组成的向量或者RSU的三维预估坐标组成的向量，xn为第n辆待定位车辆或者RSU的x轴坐标，yn为第n辆待定位车辆或者RSU的y轴坐标，zn为第n辆待定位车辆或者RSU的车辆高度，A为RSU的数量。
步骤B，获取每个待定位车辆使用预设测距方法中的任一或者多种测距方法，进行与其他节点之间的测距，得到待定位车辆与其他节点之间的距离，作为测距距离，其中，其他节点包括：路边单元RSU以及除待定位车辆以外的其他车辆，其他车辆包括：待定位车辆和已定位车辆。
本发明实施例中的任一种或者多种测距方法为预设测距方法中的任一种或者多种方法，一个待定位车辆也可以使用相同于其他车辆以及RSU的测距方法，一个待定位车辆可以使用不同于其他车辆以及RSU的测距方法。这里的预设测距方法包括：预设到达时间测距方法、预设接收信号强度测距方法、预设到达角度测距方法、以及预设多普勒频移测距方法中的一种或多种，其中，所述预设到达测距方法为预设到达测距公式1：
Figure BDA0001813868320000097
其中，mk表示第mk个节点，nk表示第nk个节点，k表示第mk个节点获得的来自第nk个节点的第k个测距观测的序号，
Figure BDA0001813868320000098
表示使用预设的到达时间测距TOA，得到的第mk个节点和第nk个节点之间的测距，
Figure BDA0001813868320000099
中的下标k表示测距的序号，c表示光速，x表示所述二维预估坐标中的横坐标，y表示所述二维预估坐标中的纵坐标，z表示所述待节点的高度；
Figure BDA00018138683200000910
表示第n个节点的速度向量，
Figure BDA00018138683200000911
表示第n个节点的速度向量中的x方向上的速度；
Figure BDA00018138683200000912
表示第n个节点的速度向量中的y方向上的速度；
Figure BDA00018138683200000913
表示第n个节点的速度向量中的z方向上的速度；
Figure BDA0001813868320000101
表示第mk个节点的三维预估坐标，
Figure BDA0001813868320000102
表示第nk个节点的三维预估坐标，
Figure BDA0001813868320000103
Figure BDA0001813868320000104
分别表示第mk个节点与第nk个节点之间的相减混合向量；
所述预设接收信号强度测距方法为预设接收信号强度测距公式2：
Figure BDA0001813868320000105
其中，
Figure BDA0001813868320000106
表示使用预设的接收信号强度RSS，得到的第mk个节点和第nk个节点之间的测距，P0是一个整体，P0表示自由空间发射功率为1w时，在1m位置接收信号功率的标准值，np表示路径损耗指数，np是一个整体，np表示路径损耗指数；
所述预设到达角度测距方法为预设到达角度测距公式3：
Figure BDA0001813868320000107
其中，
Figure BDA0001813868320000108
表示使用预设的到达角度测距AOA，得到的第mk个节点和第nk个节点之间的测距；
Figure BDA0001813868320000109
Figure BDA00018138683200001010
其中，
Figure BDA00018138683200001011
为第mk个节点与第nk个节点的连线与z轴的夹角的互余，
Figure BDA00018138683200001012
大于或等于
Figure BDA00018138683200001013
且小于或等于
Figure BDA00018138683200001014
Figure BDA00018138683200001015
为第mk个节点与第nk个节点的二维平面内坐标的连线与x轴正向的逆时针的夹角，
Figure BDA00018138683200001016
大于或等于0，且小于或等于2π；
所述预设多普勒频移测距方法为预设多普勒频移测距公式4：
Figure BDA00018138683200001017
其中，
Figure BDA00018138683200001018
表示使用预设的多普勒频移DOP，得到的第mk个节点和第nk个节点之间的测距，fc表示载波频率，fc是一个整体，fc表示载波频率。
这样，本发明实施例采用TOA、RSS、AOA或DOP中的一种测距方法，或者这些测距方法中任意多种的组合。
步骤C，利用车辆的二维轮廓构建的第一R树以及建筑物的二维轮廓构建的第二R树；利用第二R树的搜索功能，鉴别测距距离中的NLOSb(Non line of sight caused byBuilding，建筑物引起的非视距)；以及利用车辆的高度以及第一R树的搜索功能，鉴别测距距离中的NLOSv(Non line of sight caused by vehicle，其他车辆引起的非视距)，从测距距离中选择除NLOSb和NLOSv以外的视距LOS对应的测距距离，其中第一R树包含单个车辆构成的最小矩形，第二R树包含单个建筑物构成的最小矩形。
这里鉴别测距距离中的NLOS可以直接车辆的二维轮廓构建的第一R树以及建筑物的二维轮廓构建的第二R树进行NLOS鉴别。NLOS包括：NLOSv以及NLOSb。
上述步骤C可以采用如下的实现方式，利用车辆的二维轮廓构建的第一R树以及建筑物的二维轮廓构建的第二R树；利用第二R树的搜索功能，鉴别测距距离中的建筑物引起的非视距NLOSb；以及利用车辆的高度以及第一R树的搜索功能，鉴别测距距离中的其他车辆引起的非视距NLOSv，从测距距离中选择除NLOSb和NLOSv以外的视距LOS对应的测距距离：
第1步骤，利用真实的每个车辆的二维轮廓及每个车辆的二维预估坐标，得到所有车辆的二维轮廓的二维预估坐标；将所有车辆的二维轮廓的二维预估坐标，构建得到第一R树，其中，真实的车辆的二维轮廓是第一预设矩形，第一预设矩形的长宽，与车辆的二维轮廓的长宽对应。第一预设矩形可以根据用户需求进行设定。
第2步骤，从开源可编辑的地图服务中获取所有建筑物的二维轮廓的二维预估坐标，其中，建筑物的二维轮廓是第二预设矩形，第二预设矩形的长，与建筑物的二维轮廓的最长的长对应，并且，第二预设矩形的宽，与建筑物的二维轮廓的最宽的宽对应；第二预设矩形可以根据用户需求进行设定。
第3步骤，将所有建筑物的二维轮廓的二维预估坐标，构建得到第二R树；
第4步骤利用第二R树的搜索功能，判断测距距离中是否存在NLOSb；如是，则执行第5步骤；
其中，第4步骤的第二R树的搜索功能，可以通过如下步骤实现：第一步将相互通信的两辆车辆表示成一个矩形，即以车辆的二维预估坐标确定这个矩形；该矩形的坐标最大值和最小值分别可以由相互通信的两车辆对应车辆的二维坐标确定；第二步，判断该矩形与R树根节点表示的矩形是否相交，若相交则判断与根节点表示的矩形中包含的矩形是否相交，这个矩形中包含的矩形，称为次大矩形；若相交并且这些次大矩形对应的节点为叶子结点，则确定出为NLOSb，第三步如果这些次大矩形对应的节点不为叶子结点，则继续对次大矩形中包含的矩形进行搜索，直至搜索到当前单个矩形对应的节点为叶子结点。这里的大矩形中包含的矩形，也可以称为小矩形，然后小矩形中包含的矩形，可以称为小小矩形。其中，当前单个矩阵可以根据实际情况，包括但不限于：大矩阵，次矩阵小矩阵，小小矩阵等。具体的，如果搜索到单个矩形对应的节点为叶子结点，确定通信的两辆车之间存在NLOSb，如果未搜索出来，则为LOS。
对于上述矩形的坐标最大值和最小值分别可以由相互通信的两辆车的坐标确定。示例性的，相互通信的两车辆中一车辆的二维预估坐标为(X1 0,Y1 0)，相互通信的两车辆中另一车辆的二维预估坐标为(X2 3，Y2 3)，其中，X1 0<X2 3，Y1 0<Y2 3，上述将相互通信的两辆车辆表示成的一个矩形的坐标最大值分别为(X2 3，Y2 3)，该一个矩形的坐标最小值分别为(X1 0,Y1 0)，然后按照，这个坐标最大值和坐标最小值，形成一个矩形，矩形的四个顶点为(X1 0,Y1 0)，(X1 0,Y2 3)，(X2 3，,Y1 0)，(X2 3，Y2 3)。
第5步骤，从测距距离中去除NLOSb；
第6步骤，利用所有车辆的高度以及第一R树的搜索功能，判断测距距离中是否存在NLOSv，则执行第7步骤；
其中，第6步骤的第一R树的搜索功能可以通过如下步骤实现：第一步将相互通信的两辆车辆表示成一个矩形；该矩形的坐标最大值和最小值分别可以由相互通信的两辆车对应的车辆的二维预估坐标确定，第二步，判断该矩形与R树根节点表示的矩形是否相交，若相交则判断与根节点表示的矩形下面的次大矩形是否相交，若相交并且这些次大矩形对应的节点为叶子结点，且该节点对应的车辆的高度高于相互通信的两辆车，则确定出为NLOSv，第三步如果这些次大矩形对应的节点不为叶子结点，则继续对次大矩形中的矩形(即，小矩形)进行搜索，直至搜索到当前单个矩形对应的节点为叶子结点，如果搜索到单个矩形对应的节点为叶子结点，且该节点对应的车辆的高度高于相互通信的两辆车，确定通信的两辆车之间存在NLOSv，如果未搜索出来或者该节点对应的车辆的高度低于相互通信的两辆车，则为LOS。
第7步骤，从测距距离中去除NLOSv；
第8步骤，从测距距离中选择除NLOSb和NLOSv以外的LOS对应的测距距离。
这里的车辆包括：待定位车辆。一般来说，待定位车辆的发射信号，容易被高于该发射信号的障碍物遮挡，这些障碍物包括：建筑物和车辆。参见图2所示NLOSv与NLOSb的情况。车辆e、车辆f、车辆c、车辆b、车辆a、车辆d可以统称为车辆，这些车辆可以是待定位车辆，也可以是其他车辆。车辆c与车辆d之间的链路是LOS；车辆a与车辆b之间的链路是NLOSv；车辆a与车辆f之间的链路是NLOSb。
从图2中可以看出，由于车辆高度的影响，如果凭借车辆的二维轮廓构建的第一R树进行NLOSv判断是不准确的，需要利用车辆高度，即利用车辆的二维轮廓构建第一R树判断车辆间的路径有车辆进行遮挡；再进一步，根据车辆高度进行判断，如图2中车辆d，车辆e与车辆c，虽然在二维平面上，车辆e阻挡了车辆c与车辆d之间的路径，但是由于车辆e的车辆高度低于车辆c与车辆d，车辆e实际上并未阻挡车辆c与车辆d之间的测距。而由于车辆c的车辆高度分别高于车辆b与车辆a，故车辆c阻挡了车辆b与车辆a之间的测距。其中，利用车辆的二维轮廓构建的第一R树以及建筑物的二维轮廓构建的第二R树，“第一R树”中的“第一”和“第二R树”中的“第二”仅仅是为了区别不同车辆的二维轮廓与建筑物的二维轮廓分别构建的R树，没有顺序的限制。第一R树和第二R树可以统称为R树，R树是一种树状存储结构，利用R树的搜索功能，可以判断车辆是否被建筑物以及车辆遮挡。
上述R树的搜索功能返回的结果是所有符合查找的二维轮廓。输入不仅仅是一个范围了，这个范围可以看成是一个空间中的矩形。也就是说，输入的是一个搜索矩形。
假设一棵R树包括根结点和叶子结点，查找搜索矩形S覆盖的记录条目，该搜索矩形S称为较大搜索矩形S，单个车辆构成的最小矩形或者单个建筑物构成的最小矩形，以及由多个最小矩形构成的次大矩形。这种利用R树的搜索功能的方式，使不必遍历所有数据即可获得答案，效率显著提高。
第一步，较大搜索矩形S包含多个次大矩形，如果这些次大矩形对应的节点为叶子结点，则确定出这些为叶子结点的次大矩形所对应的多个车辆的二维轮廓或者多个建筑物的二维轮廓；
第二步，较大搜索矩形S包含多个次大矩形，如果这些次大矩形对应的节点不为叶子结点，则继续对次大矩形中的矩形(即，小矩形)进行搜索，直至搜索到当前单个矩形对应的节点为叶子结点，然后确定出为叶子结点的单个矩形所对应的车辆的二维轮廓或者建筑物的二维轮廓。
步骤D，基于LOS对应的测距距离和待定位车辆的三维预估坐标，重新确定待定位车辆的三维预估坐标；
步骤E，确定三维预估坐标的置信度是否高于预设置信度阈值；如果否，则继续执行步骤C、步骤D和步骤E；如果是，则执行步骤F；步骤F，将三维预估坐标，作为待定位车辆的定位坐标。
预设置信度阈值可以根据用户需要进行设置。这里的置信度用于表明经过重新确定的待定位车辆的三维预估坐标是否合适，这样选择一个更加准确的三维预估坐标。
上述步骤D可以采用广义近似消息传递GAMP算法，优选地，采用广义近似消息传递GAMP算法。
步骤D采用广义近似消息传递GAMP算法，确定待定位车辆的定位坐标，具体实现如下：
利用LOS对应的测距距离和待定位车辆的三维预估坐标，采用广义近似消息传递GAMP算法，第t次重新确定待定位车辆的三维预估坐标，并判断所有待定位车辆重新确定的三维预估坐标是否达到收敛条件；如果未达到，则继续执行步骤C和步骤D，直至达到收敛条件，将最后一次重新确定的待定位车辆的三维预估坐标，作为待定位车辆的定位坐标，其中，t大于0，小于或等于最大迭代次数。
上述收敛条件包括：在未达到最大迭代次数时，待定位车辆的三维预估坐标趋于稳定，或者达到最大迭代次数。可以获取到最大迭代次数以后，每次只需要判断迭代时，判断是否达到最大迭代次数，在没有达到最大迭代次数，再判断待定位车辆的三维预估坐标是否趋于稳定，这样任一收敛条件达到时，均可以确定待定位车辆的定位坐标。
本发明实施例，以VANETS中第nk节点和第mk节点为例，介绍如何步骤D具体可以采用如下广义近似消息传递GAMP算法，确定待定位车辆的定位坐标：
使用上述预设测距方法中预设到达测距公式1、预设接收信号强度测距公式2、预设到达角度测距公式3及预设多普勒频移测距公式4中任一公式，都可以使用
Figure BDA0001813868320000141
进行替代，也就是，
Figure BDA0001813868320000142
表示上述任一公式。
首先，先将三维定位问题转化为线性混合问题。
VANETS中采用如下公式(5)对第nk节点和第mk节点之间进行测距：
Figure BDA0001813868320000143
其中，
Figure BDA0001813868320000144
为第nk个待定位节点与第mk个待定位节点之间的通过公式计算的测距，*∈{TOA,RSS,DOP,AOA}，
Figure BDA0001813868320000151
其中，
Figure BDA0001813868320000152
为第mk待定位车辆或者RSU的三维预估坐标，
Figure BDA0001813868320000153
为第nk待定位车辆或者RSU的三维预估坐标，
Figure BDA0001813868320000154
为VANETS中第nk待定位车辆或者RSU和第mk待定位车辆或者RSU的三维预估坐标相减获得的向量，且这两个节点不能同时为RSU，
Figure BDA0001813868320000155
s3k,s3k+1,s3k+2表示在x,y,z轴相减的结果。
由于测距受到噪声等因素的影响，
Figure BDA0001813868320000156
为：
Figure BDA0001813868320000157
其中，
Figure BDA0001813868320000158
为对上述三维预估坐标施加的干扰，
Figure BDA0001813868320000159
中的·为对参数的替代，
Figure BDA00018138683200001510
在本发明实施例中使用加性高斯白噪声，所以
Figure BDA00018138683200001511
其中
Figure BDA00018138683200001512
服从均值为0，方差为
Figure BDA00018138683200001513
的高斯分布。其中
Figure BDA00018138683200001514
为VANETS中第nk节点和第mk节点的之间的测距受到环境的干扰，本发明实施例测距的噪声为高斯白噪声，
Figure BDA00018138683200001515
为测距的噪声方差。将
Figure BDA00018138683200001516
存储为3K维的向量
Figure BDA00018138683200001517
其中，s＝Ap+μ。向量s为
Figure BDA00018138683200001518
组成的向量，
Figure BDA00018138683200001519
是一个3K×3N维度稀疏矩阵，A为存储1和-1的稀疏矩阵，也就是，每一行只有一个或者两个非零元素1或者-1，N为车辆的个数，K为测距的中的数量。当
Figure BDA00018138683200001520
是一个3K维度的向量，μ是常向量，μ的值由VANETS中的节点类型有关，当第nk节点和第mk节点分别为待定位的车辆时，μ相应的RSU中对应的值为0，当第nk节点或者第mk节点分别为RSU时，μ为存储相应的RSU坐标的正值或者负值。
公式(6)转化为广义线性混合方程：
Figure BDA00018138683200001521
其中，如果是不同类型的测距则
Figure BDA00018138683200001522
为不同的类型，如果是多种类型的测距，则无非是A和μ行数增加，p保持不变，最终存储为向量o。向量o可以通过公式(7)计算。之所以转换成广义线性方程是为了利用GAMP算法。
当mk节点和第nk节点是车辆时，
Figure BDA0001813868320000161
其中i＝0,1,2，i为一个为矩阵中某个元素位置的数值，
Figure BDA0001813868320000162
为稀疏矩阵A中3k+i行3mk+i列的元素，
Figure BDA0001813868320000163
为稀疏矩阵A中3k+i行3nk+i列的元素；
当mk节点是车辆，第nk节点是RSU时，
Figure BDA0001813868320000164
Figure BDA0001813868320000165
为向量μ中的第3nk+i个元素，
Figure BDA0001813868320000166
为i＝0,为VANETS第nk个节点的x轴坐标，i＝1,
Figure BDA0001813868320000167
为VANETS第nk个节点的y轴坐标，i＝2,为VANETS第nk个节点的z轴坐标。
当mk节点是RSU，第nk节点是车辆时，
Figure BDA0001813868320000168
Figure BDA0001813868320000169
为向量μ中的第3mk+i个元素，i＝0,
Figure BDA00018138683200001610
为VANETS第mk个节点的x轴坐标，i＝1,
Figure BDA00018138683200001611
为VANETS第mk个节点的y轴坐标，i＝2,
Figure BDA00018138683200001612
为VANETS第mk个节点的z轴坐标，其他未提及的A和μ中的值为0，一般待定位车辆类型的不同，会影响A和μ中的值。
其次，采用了GAMP算法解决上述线性混合问题，方法如下：
第一步，初始化参数，参数包括：迭代次数t；迭代次数t＝0，k∈{0,1,…,K-1}，
Figure BDA00018138683200001613
n∈{0,1,…,3N-1}，
Figure BDA00018138683200001614
是根据先验概率分布{pP(pn)}n中的{pn}n取值。其中N为待定位的车辆的数目，K为测距的总的数目，k为测距的序号，
Figure BDA00018138683200001615
为序号，n为序号，
Figure BDA00018138683200001616
为GAMP公式(10)中的中间变量
Figure BDA00018138683200001617
向量中第
Figure BDA00018138683200001618
个元素，向量中存储的是零或者RSU的坐标的正值或者负值。{pP(pn)}n为{pn}n的先验分布，
Figure BDA00018138683200001619
分别为迭代次数t＝1时的pn的均值和方差。{pP(pn)}n为{pn}n的先验分布。其中，pP(pn)中的pn为一个标量，与第n辆车的三维坐标组成的向量pn是不相同，两者的下标的n的范围不同，pn中的n的范围是大于或等于0，且小于N，pn中的n的范围是大于或等于0，且小于3N，pn表示一个向量，其中包括第n辆车的xyz坐标，
Figure BDA0001813868320000171
为迭代次数t时的pn的均值，pP(pn)中的下角标P为数学符号，无物理意义。
第二步，每次迭代过程中，根据如下公式计算：
Figure BDA0001813868320000172
公式(8)中，
Figure BDA0001813868320000173
为矩阵A中第
Figure BDA0001813868320000174
行第n列的元素，
Figure BDA0001813868320000175
中
Figure BDA0001813868320000176
为矩阵A的行序号)，
Figure BDA0001813868320000177
为迭代次数t时的pn的方差，
Figure BDA0001813868320000178
中的上标p为数学符号，用于与其他τ进行区分，
Figure BDA0001813868320000179
为第t次迭代p向量中第n个元素的方差，
Figure BDA00018138683200001710
为t次迭代s向量中第
Figure BDA00018138683200001711
个元素的方差此项为中间变量；
Figure BDA00018138683200001712
上述公式(9)中，
Figure BDA00018138683200001713
为中间变量，
Figure BDA00018138683200001734
为第t时刻pn的均值，
Figure BDA00018138683200001733
表示第t次迭代
Figure BDA00018138683200001714
的估计值，
Figure BDA00018138683200001715
为向量μ中第
Figure BDA00018138683200001716
个元素，
Figure BDA00018138683200001717
为向量s中的第
Figure BDA00018138683200001718
个元素，公式(9)是个中间量，用于计算公式(10)
Figure BDA00018138683200001732
上述公式(10)中，
Figure BDA00018138683200001719
通过后验概率
Figure BDA00018138683200001720
估计的期望，其中Cs是概率归一化常数，
Figure BDA00018138683200001721
是s＝Ap+μ中的第
Figure BDA00018138683200001735
个元素，
Figure BDA00018138683200001722
为以
Figure BDA00018138683200001723
为变量的均值为
Figure BDA00018138683200001724
方差为
Figure BDA00018138683200001725
的高斯分布。
Figure BDA00018138683200001726
表示的是一个元素，sk表示的是一个向量，sk表示的是第mk与nk个节点之间的坐标相减，sk表示的是向量s中的第3k，3k+1，3k+2三个元素，
Figure BDA00018138683200001727
是向量s中的第
Figure BDA00018138683200001728
个元素，
Figure BDA00018138683200001729
为
Figure BDA00018138683200001730
已知情况下ok的概率密度函数。
Figure BDA00018138683200001731
公式(11)中，
Figure BDA0001813868320000181
通过后验概率
Figure BDA0001813868320000182
估计的方差，
Figure BDA0001813868320000183
公式(12)中，
Figure BDA0001813868320000184
为中间变量用于公式(14)和公式(15)的计算；
Figure BDA0001813868320000185
公式(13)中，
Figure BDA0001813868320000186
为中间变量用于公式(14)和公式(15)的计算；
Figure BDA0001813868320000187
公式(14)中，
Figure BDA0001813868320000188
为t+1个时刻的pn的均值，
Figure BDA0001813868320000189
为在
Figure BDA00018138683200001810
以及
Figure BDA00018138683200001811
已知时pn的期望，是针对
Figure BDA00018138683200001812
求得的期望，其中Cp是概率归一化常数，pP(pn)为pn的先验分布，
Figure BDA00018138683200001813
为以pn为变量的均值为
Figure BDA00018138683200001814
方差为
Figure BDA00018138683200001815
的高斯分布。
Figure BDA00018138683200001816
公式(15)中，
Figure BDA00018138683200001817
为t+1时刻的pn的方差，
Figure BDA00018138683200001818
为在
Figure BDA00018138683200001819
以及
Figure BDA00018138683200001820
已知时pn的方差，是针对后验概率密度
Figure BDA00018138683200001821
求得的方差，其中，Cp是概率归一化常数，pP(pn)为pn的先验分布，
Figure BDA00018138683200001822
为以pn为变量的均值为
Figure BDA00018138683200001823
方差为
Figure BDA00018138683200001824
的高斯分布。
其中，利用重要性采样方法来改进GAMP算法进行数值分析。上述后验概率分布函数在引入J采样，GAMP算法中公式(10)的期望和公式(11)的方差可以近似计算为：
Figure BDA00018138683200001825
Figure BDA00018138683200001826
其中，
Figure BDA0001813868320000191
为ok已知的情况下，
Figure BDA0001813868320000192
的均值，
Figure BDA0001813868320000193
为ok已知的情况下，
Figure BDA0001813868320000194
的方差，小写
Figure BDA0001813868320000195
是大写S|O的特定实现,，k大于或等于0，小于K，
Figure BDA0001813868320000196
为序号，范围为
Figure BDA0001813868320000197
wj为J个权重值中的第j个权重值，第j大于0，小于或等于J，J为重要性采样方法的粒子数量。
Figure BDA0001813868320000198
为进行重要性采样方法获得的J个采样值，j为重要性采样方法中的采样值的序号，j的取值范围为大于0小于等于J，数据采样
Figure BDA0001813868320000199
是基于权重函数
Figure BDA00018138683200001910
随机生成，其中λ是一个正数，si，
Figure BDA00018138683200001911
的下角标i为相同，i为向量中第i个元素，i∈Sk＝{3k,3k+1,3k+2}，λ为一个正数，根据经验选择λ在30-40之间，
Figure BDA00018138683200001912
为高斯分布，si为向量s中第i个元素，
Figure BDA00018138683200001913
为t时刻si的均值，
Figure BDA00018138683200001914
为t时刻si的方差，
Figure BDA00018138683200001915
的上角标s为数学符号，用于与其他的τ相区别。
权重值计算表达式为：
Figure BDA00018138683200001916
其中，Sk＝{3k,3k+1,3k+2}，Cw是概率归一化因子，保证
Figure BDA00018138683200001917
Cw中的下角标w为用于与其他概率归一化因子相区分，
Figure BDA00018138683200001918
为s3k的第j个采样值，
Figure BDA00018138683200001919
为s3k+1的第j个采样值，
Figure BDA00018138683200001920
为s3k+2的第j个采样值，
Figure BDA00018138683200001921
为si的第j个采样值，
Figure BDA00018138683200001922
为t时刻，si的均值，
Figure BDA00018138683200001923
为t时刻，si的方差，
Figure BDA00018138683200001924
中的下角标i为表示向量中某个元素的标号，i∈Sk＝{3k,3k+1,3k+2}，
Figure BDA00018138683200001925
中的上角标j为J个重要性采样值中的标号，j大于0小于等于J，
Figure BDA00018138683200001926
中的上角标s用于与其他的τ区分，无物理含义。
第三步，在每次迭代过程完成后，判断所有待定位车辆重新确定的三维预估坐标是否达到收敛条件；若达到，在终止迭代，输出当前计算得到的{pn}n的均值作为车辆的坐标，也就是最后一次迭代计算的待定位车辆的三维预估坐标；若未达到，返回继续执行第二步。
本发明实施例中采用待定位车辆的三维预估坐标进行定位，不仅可以掌握待定位车辆的二维预估坐标，而且可以掌握每个待定位车辆的高度，使用预设测距方法进行测距时，考虑每个待定位车辆的高度，使得得到的测距距离更接近于现实情况；并且，鉴别测距距离中的非视距NLOS，从测距距离中选择视距LOS对应的测距距离，这样可以减少NLOS对测距距离的影响，进而基于视距LOS对应的测距距离和待定位车辆的预估坐标，重新确定所述待定位车辆的三维预估坐标相较于现有技术更加准确。
为了计算
Figure BDA0001813868320000201
的中的
Figure BDA0001813868320000202
和
Figure BDA0001813868320000203
本发明实施例采用了重要性采样方法。随着GAMP算法中的迭代的进行，本发明实施例以
Figure BDA0001813868320000204
也可为
Figure BDA0001813868320000205
在迭代中的误差作为判断门限来删减粒子数量。在结合上述图1的基础上，为了减少了进行重要性采样方法的测距的数量K和需要更新坐标的车辆的数量，加速计算速度，参见图3所示，在本发明一种可能的实施例中，具体步骤包括：
步骤A，获取待定位车辆的车辆高度以及由全球卫星导航***得到待定位车辆的二维预估坐标，形成的三维预估坐标。
步骤B，获取每个待定位车辆使用预设测距方法中的任一或者多种测距方法，进行与其他节点之间的测距，得到待定位车辆与其他节点之间的距离，作为测距距离，其中，其他节点包括：路边单元RSU以及除待定位车辆以外的其他车辆，其他车辆包括：待定位车辆。
步骤C，利用车辆的二维轮廓构建的第一R树以及建筑物的二维轮廓构建的第二R树；利用第二R树的搜索功能，鉴别测距距离中的建筑物引起的非视距NLOSb；以及利用车辆的高度以及第一R树的搜索功能，鉴别测距距离中的其他车辆引起的非视距NLOSv，从测距距离中选择除NLOSb和NLOSv以外的视距LOS对应的测距距离，其中第一R树不限于仅包含单个车辆构成的最小矩形，第二R树包含单个建筑物构成的最小矩形。
步骤D1，利用LOS对应的测距距离和待定位车辆的三维预估坐标，采用广义近似消息传递GAMP算法，判断上一次重新确定的重要性采样的后验概率的均值与上一次之前的一次重新确定的重要性采样的后验概率的均值之间的误差是否小于阀值；如果是，则执行步骤D2，如果否，则执行步骤D3。
其中，可以通过采用广义近似消息传递GAMP算法，计算得到第一次重新确定的重要性采样的后验概率的均值，以及计算出第二次重新确定的重要性采样的后验概率的均值；
然后，再将第二次重置确定的重要性采样的后验概率的均值，作为上一次确定的重要性采样的后验概率的均值，第一次重新确定的重要性采样的后验概率的均值作为上一次之前的一次重新确定的重要性采样的后验概率的均值，再然后执行本步骤D1。
步骤D2，删减此次重要性采样方法中所用的的粒子的数量，继续执行步骤D3。
步骤D3，采用重要性采样方法，确定GAMP算法中的后验概率的方差和均值；
步骤D4，基于本次的后验概率的方差和均值，利用GAMP算法，重新确定待定位车辆的三维预估坐标以外的其他待定位车辆的三维预估坐标。
示例性的，如果
Figure BDA0001813868320000211
在t次迭代中的值与t+1次的值误差的绝对值小于一个门限，根据经验可以门限选为3-10，在t+2次迭代中可以删减用于计算
Figure BDA0001813868320000212
的粒子数目J。
步骤D5，判断本次重新确定的待定位车辆的三维预估坐标与上次重新确定的待定位车辆的三维预估坐标之间的差值是否小于阈值，如是，则执行步骤D6；如否，则执行步骤D8。
其中，本次重新确定的待定位车辆的三维预估坐标是第一次确定的待定位车辆的三维预估坐标时，那么，上次重新确定的待定位车辆的三维预估坐标是步骤A的待定位车辆的三维预估坐标。
步骤D6，将待定位车辆，作为已定位车辆，并将本次重新确定的待定位车辆的三维预估坐标，作为已定位车辆的三维预估坐标。
步骤D7，删除待定位车辆的三维预估坐标中对应的已定位车辆的三维预估坐标。这样将待定位车辆，作为已定位车辆，相当于车辆锚点升级，将在迭代过程中坐标值趋于稳定的车辆升级为伪RSU，判断的标准为两次迭代过程中车辆的坐标的变化范围，当车辆升级为伪RSU，此时，这些节点接收到的来自于RSU的测距信息可以删除，伪RSU之间的测距信息也可以删除，这样就减少了进行重要性采样算法的测距的数量K加速GAMP算法中的重要性采样和需要更新坐标的车辆的数量，加速计算速度。
步骤D8，判断所有待定位车辆重新确定的三维预估坐标是否达到收敛条件；如果否，返回执行步骤C，直至达到收敛条件；如果是，执行步骤D9。
步骤D9，将三维预估坐标，作为待定位车辆的定位坐标。也就是，将最后一次重新确定的待定位车辆的三维预估坐标，作为待定位车辆的定位坐标。
本发明实施例，与现有技术相比，定位效果更好。在VANETs中，利用了三维预估坐标，解决了3D车辆网中NLOS路径对于定位效果的影响，将NLOS鉴别与提出的定位算法结合，利用迭代的方式使得NLOS/LOS估计与协作定位相互促进，最终提升了定位精度。
下面继续对本发明实施例提供的车联网中车辆三维定位装置进行介绍。
如图4所示，本发明实施例还提供一种车联网中车辆三维定位装置，应用于***，所述装置包括：
第一获取模块21，用于用于获取待定位车辆的车辆高度以及由全球卫星导航***得到所述待定位车辆的二维预估坐标，形成的三维预估坐标；
第二获取模块22，用于获取每个待定位车辆使用预设测距方法中的任一或者多种测距方法，进行与其他节点之间的测距，得到所述待定位车辆与其他节点之间的距离，作为测距距离，其中，所述其他节点包括：路边单元RSU以及除所述待定位车辆以外的其他车辆，所述其他车辆包括：待定位车辆；
鉴别选择模块23，用于利用车辆的二维轮廓构建的第一R树以及建筑物的二维轮廓构建的第二R树；利用所述第二R树的搜索功能，鉴别所述测距距离中的建筑物引起的非视距NLOSb；以及利用所述车辆的高度以及所述第一R树的搜索功能，鉴别所述测距距离中的其他车辆引起的非视距NLOSv，从所述测距距离中选择除所述NLOSb和所述NLOSv以外的视距LOS对应的测距距离，其中所述第一R树包含单个车辆构成的最小矩形，所述第二R树包含单个建筑物构成的最小矩形；
确定模块24，用于基于LOS对应的测距距离和所述待定位车辆的三维预估坐标，重新确定所述待定位车辆的三维预估坐标；确定所述三维预估坐标的置信度是否高于预设置信度阈值；如果不高于所述预设置信度阈值，继续执行确定模块中的步骤，直至所述置信度高于所述预设置信度阈值，将所述三维预估坐标，作为待定位车辆的定位坐标。
在一种可能的实现方式中，所述鉴别选择模块，具体用于：
利用真实的每个车辆的二维轮廓及每个车辆的二维预估坐标，得到所有车辆的二维轮廓的二维预估坐标；
将所有车辆的二维轮廓的二维预估坐标，构建得到第一R树；
从开源可编辑的地图服务中获取所有建筑物的二维轮廓的二维预估坐标；
将所有建筑物的二维轮廓的二维预估坐标，构建得到第二R树；
利用第二R树的搜索功能，若判定所述测距距离中存在NLOSb；以及
利用所有车辆的高度以及第一R树的搜索功能，若判定所述测距距离中存在NLOSv，从所述测距距离中选择除所述NLOSb和所述NLOSv以外的LOS对应的测距距离。
在一种可能的实现方式中，所述确定模块，具体用于：
利用所述LOS对应的测距距离和所述待定位车辆的三维预估坐标，采用广义近似消息传递GAMP算法，第t次重新确定所述待定位车辆的三维预估坐标，并判断所有待定位车辆重新确定的三维预估坐标是否达到收敛条件；如果未达到，则继续执行所述鉴别选择模块和所述确定模块中的步骤，直至达到所述收敛条件，将最后一次重新确定的所述待定位车辆的三维预估坐标，作为所述待定位车辆的定位坐标，其中，t大于0，小于或等于最大迭代次数。
在一种可能的实现方式中，所述确定模块，具体用于：
利用所述LOS对应的测距距离和所述待定位车辆的三维预估坐标，采用广义近似消息传递GAMP算法，本次重新确定所述待定位车辆的三维预估坐标；
判断本次重新确定的所述待定位车辆的三维预估坐标与上次重新确定的所述待定位车辆的三维预估坐标之间的差值是否小于阈值；
若本次重新确定的所述待定位车辆的三维预估坐标与上次重新确定的所述待定位车辆的三维预估坐标之间的差值小于阈值，则将所述待定位车辆，作为已定位车辆，并将本次重新确定的所述待定位车辆的三维预估坐标，作为已定位车辆的三维预估坐标；
删除所述待定位车辆的三维预估坐标中对应的所述已定位车辆的三维预估坐标，将已定位车辆加入到RSU集合中，以使由车辆和RSU形成的网络结构发生变化；
利用除所述已定位车辆的三维预估坐标以外的其他待定位车辆的三维预估坐标，返回继续执行所述鉴别选择模块和所述确定模块中的步骤，直至达到所述收敛条件，将最后一次重新确定的所述待定位车辆的三维预估坐标，作为所述待定位车辆的定位坐标。
在一种可能的实现方式中，所述确定模块，具体还用于：判定上一次重新确定的重要性采样的后验概率的均值与上一次之前的一次重新确定的重要性采样的后验概率的均值之间的误差大于或等于阀值，采用重要性采样方法，确定GAMP算法中的后验概率的方差和均值；
基于所述后验概率的方差和均值，利用GAMP算法，重新确定所述待定位车辆的三维预估坐标；以及
若判定上一次重新确定的重要性采样的后验概率的均值与上一次之前的一次重新确定的重要性采样方法的后验概率的均值之间的误差小于阀值，则删减此次重要性采样方法中所用的的粒子的数量，继续执行采用重要性采样方法，确定GAMP算法中的后验概率的方差和均值的步骤；
基于所述本次的后验概率的方差和均值，利用GAMP算法，重新确定所述待定位车辆的三维预估坐标以外的其他待定位车辆的三维预估坐标，返回继续执行所述鉴别选择模块和所述确定模块的步骤，直至达到所述收敛条件，将最后一次重新确定的所述待定位车辆的三维预估坐标，作为所述待定位车辆的定位坐标。
在一种可能的实现方式中，所述预设测距方法包括：预设到达时间测距方法、预设接收信号强度测距方法、预设到达角度测距方法、以及预设多普勒频移测距方法中的一种或多种，其中，所述预设到达测距方法为预设到达测距公式1：
Figure BDA0001813868320000241
其中，mk表示第mk个节点，nk表示第nk个节点，k表示第mk个节点获得的来自第nk个节点的第k个测距观测的序号，
Figure BDA0001813868320000242
表示使用预设的到达时间测距TOA，得到的第mk个节点和第nk个节点之间的测距，
Figure BDA0001813868320000243
中的下标k表示测距的序号，c表示光速，x表示所述二维预估坐标中的横坐标，y表示所述二维预估坐标中的纵坐标，z表示所述节点的高度；
Figure BDA0001813868320000244
表示第n个节点的速度向量，
Figure BDA0001813868320000245
表示第n个节点的速度向量中的x方向上的速度；
Figure BDA0001813868320000246
表示第n个节点的速度向量中的y方向上的速度；
Figure BDA0001813868320000247
表示第n个节点的速度向量中的z方向上的速度；
Figure BDA0001813868320000248
表示第mk个节点的三维预估坐标，
Figure BDA0001813868320000249
表示第nk个节点的三维预估坐标，
Figure BDA00018138683200002410
分别表示第mk个节点与第nk个节点之间的相减混合向量；
所述预设接收信号强度测距方法为预设接收信号强度测距公式2：
Figure BDA0001813868320000251
其中，
Figure BDA0001813868320000252
表示使用预设的接收信号强度RSS，得到的第mk个节点和第nk个节点之间的测距，P0表示自由空间发射功率为1w时，在1m位置接收信号功率的标准值，np表示路径损耗指数；
所述预设到达角度测距方法为预设到达角度测距公式3：
Figure BDA0001813868320000253
其中，
Figure BDA0001813868320000254
表示使用预设的到达角度测距AOA，得到的第mk个节点和第nk个节点之间的测距；
Figure BDA0001813868320000255
Figure BDA0001813868320000256
其中，
Figure BDA0001813868320000257
为第mk个节点与第nk个节点的连线与z轴的夹角的互余，
Figure BDA0001813868320000258
大于或等于
Figure BDA0001813868320000259
且小于或等于
Figure BDA00018138683200002510
Figure BDA00018138683200002511
为第mk个节点与第nk个节点的二维平面内坐标的连线与x轴正向的逆时针夹角，
Figure BDA00018138683200002512
大于或等于0，且小于或等于2π；
所述预设多普勒频移测距方法为预设多普勒频移测距公式4：
Figure BDA00018138683200002513
其中，
Figure BDA00018138683200002514
表示使用预设的多普勒频移DOP，得到的第mk个节点和第nk个节点之间的测距，fc表示载波频率。
下面继续对本发明实施例提供的电子设备进行介绍。
本发明实施例还提供了一种电子设备，如图5所示，包括处理器31、通信接口32、存储器33和通信总线34，其中，处理器31，通信接口32，存储器33通过通信总线34完成相互间的通信，
存储器33，用于存放计算机程序；
处理器31，用于执行存储器33上所存放的程序时，实现如下步骤：
步骤A，获取待定位车辆的车辆高度以及由全球卫星导航***得到所述待定位车辆的二维预估坐标，形成的三维预估坐标；
步骤B，获取每个待定位车辆使用预设测距方法中的任一或者多种测距方法，进行与其他节点之间的测距，得到所述待定位车辆与所述其他节点之间的距离，作为测距距离，其中，所述其他节点包括：路边单元RSU以及除所述待定位车辆以外的其他车辆，所述其他车辆包括：待定位车辆；
步骤C，利用车辆的二维轮廓构建的第一R树以及建筑物的二维轮廓构建的第二R树；利用所述第二R树的搜索功能，鉴别所述测距距离中的建筑物引起的非视距NLOSb；以及利用所述车辆的高度以及所述第一R树的搜索功能，鉴别所述测距距离中的其他车辆引起的非视距NLOSv，从所述测距距离中选择除所述NLOSb和所述NLOSv以外的视距LOS对应的测距距离，其中所述第一R树包含单个车辆构成的最小矩形，所述第二R树包含单个建筑物构成的最小矩形；
步骤D，基于LOS对应的测距距离和所述待定位车辆的三维预估坐标，重新确定所述待定位车辆的三维预估坐标；确定所述三维预估坐标的置信度是否高于预设置信度阈值；如果不高于所述预设置信度阈值，继续执行步骤C和步骤D，直至所述置信度高于所述预设置信度阈值，将所述三维预估坐标，作为待定位车辆的定位坐标。
上述电子设备提到的通信总线可以是外设部件互连标准(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.一种车联网中车辆三维定位方法，其特征在于，应用于***，所述方法包括：
步骤A，获取待定位车辆的车辆高度以及由全球卫星导航***得到所述待定位车辆的二维预估坐标，形成的三维预估坐标；
步骤B，获取每个待定位车辆使用预设测距方法中的任一或者多种测距方法，进行与其他节点之间的测距，得到所述待定位车辆与所述其他节点之间的距离，作为测距距离，其中，所述其他节点包括：路边单元RSU以及除所述待定位车辆以外的其他车辆，所述其他车辆包括：待定位车辆；
步骤C，利用车辆的二维轮廓构建的第一R树以及建筑物的二维轮廓构建的第二R树；利用所述第二R树的搜索功能，鉴别所述测距距离中的建筑物引起的非视距NLOSb；以及利用所述车辆的高度以及所述第一R树的搜索功能，鉴别所述测距距离中的其他车辆引起的非视距NLOSv，从所述测距距离中选择除所述NLOSb和所述NLOSv以外的视距LOS对应的测距距离，其中所述第一R树包含单个车辆构成的最小矩形，所述第二R树包含单个建筑物构成的最小矩形；
步骤D，基于LOS对应的测距距离和所述待定位车辆的三维预估坐标，重新确定所述待定位车辆的三维预估坐标；确定所述三维预估坐标的置信度是否高于预设置信度阈值；如果不高于所述预设置信度阈值，继续执行步骤C和步骤D，直至所述置信度高于所述预设置信度阈值，将所述三维预估坐标，作为待定位车辆的定位坐标。
2.如权利要求1所述的方法，其特征在于，所述利用车辆的二维轮廓构建的第一R树以及建筑物的二维轮廓构建的第二R树；利用所述第二R树的搜索功能，鉴别所述测距距离中的建筑物引起的非视距NLOSb；以及利用所述车辆的高度以及所述第一R树的搜索功能，鉴别所述测距距离中的其他车辆引起的非视距NLOSv，从所述测距距离中选择除所述NLOSb和所述NLOSv以外的视距LOS对应的测距距离，包括：
利用真实的每个车辆的二维轮廓及每个车辆的二维预估坐标，得到所有车辆的二维轮廓的二维预估坐标；
将所有车辆的二维轮廓的二维预估坐标，构建得到第一R树；
从开源可编辑的地图服务中获取所有建筑物的二维轮廓的二维预估坐标；
将所有建筑物的二维轮廓的二维预估坐标，构建得到第二R树；
利用第二R树的搜索功能，若判定所述测距距离中存在NLOSb；以及
利用所有车辆的高度以及第一R树的搜索功能，若判定所述测距距离中存在NLOSv，从所述测距距离中选择除所述NLOSb和所述NLOSv以外的LOS对应的测距距离。
3.如权利要求1或2所述的方法，其特征在于，所述基于所述视距LOS对应的测距距离和所述待定位车辆的预估坐标，重新确定所述待定位车辆的三维预估坐标；确定所述三维预估坐标的置信度是否高于预设置信度阈值；如果不高于所述预设置信度阈值，继续执行步骤C和步骤D，直至所述置信度高于所述预设置信度阈值，将所述三维预估坐标，作为待定位车辆的定位坐标，包括：
利用所述LOS对应的测距距离和所述待定位车辆的三维预估坐标，采用广义近似消息传递GAMP算法，第t次重新确定所述待定位车辆的三维预估坐标，并判断所有待定位车辆重新确定的三维预估坐标是否达到收敛条件；如果未达到，则继续执行步骤C和步骤D，直至达到所述收敛条件，将最后一次重新确定的所述待定位车辆的三维预估坐标，作为所述待定位车辆的定位坐标，其中，t大于0，小于或等于最大迭代次数。
4.如权利要求3所述的方法，其特征在于，利用所述LOS对应的测距距离和所述待定位车辆的三维预估坐标，采用广义近似消息传递GAMP算法，第t次重新确定所述待定位车辆的三维预估坐标，并判断所有待定位车辆重新确定的三维预估坐标是否达到收敛条件；如果未达到，则继续执行步骤C和步骤D，直至达到所述收敛条件，将最后一次重新确定的所述待定位车辆的三维预估坐标，作为所述待定位车辆的定位坐标，包括：
利用所述LOS对应的测距距离和所述待定位车辆的三维预估坐标，采用广义近似消息传递GAMP算法，本次重新确定所述待定位车辆的三维预估坐标；
判断本次重新确定的所述待定位车辆的三维预估坐标与上次重新确定的所述待定位车辆的三维预估坐标之间的差值是否小于阈值；
若本次重新确定的所述待定位车辆的三维预估坐标与上次重新确定的所述待定位车辆的三维预估坐标之间的差值小于阈值，则将所述待定位车辆，作为已定位车辆，并将本次重新确定的所述待定位车辆的三维预估坐标，作为已定位车辆的三维预估坐标；
删除所述待定位车辆的三维预估坐标中对应的所述已定位车辆的三维预估坐标，将已定位车辆加入到RSU集合中，以使由车辆和RSU形成的网络结构发生变化；
利用除所述已定位车辆的三维预估坐标以外的其他待定位车辆的三维预估坐标，返回继续执行步骤C，直至达到所述收敛条件，将最后一次重新确定的所述待定位车辆的三维预估坐标，作为所述待定位车辆的定位坐标。
5.如权利要求4所述的方法，其特征在于，所述从所述测距距离中选择除所述NLOSb和所述NLOSv以外的视距LOS对应的测距距离之后，所述方法还包括：
判定上一次重新确定的重要性采样的后验概率的均值与上一次之前的一次重新确定的重要性采样的后验概率的均值之间的误差大于或等于阀值，采用重要性采样方法，确定GAMP算法中的后验概率的方差和均值；
基于所述后验概率的方差和均值，利用GAMP算法，重新确定所述待定位车辆的三维预估坐标；以及
若判定上一次重新确定的重要性采样的后验概率的均值与上一次之前的一次重新确定的重要性采样方法的后验概率的均值之间的误差小于阀值，则删减此次重要性采样方法中所用的的粒子的数量，继续执行采用重要性采样方法，确定GAMP算法中的后验概率的方差和均值的步骤；
基于所述本次的后验概率的方差和均值，利用GAMP算法，重新确定所述待定位车辆的三维预估坐标以外的其他待定位车辆的三维预估坐标，返回继续执行步骤C，直至达到所述收敛条件，将最后一次重新确定的所述待定位车辆的三维预估坐标，作为所述待定位车辆的定位坐标。
6.如权利要求1或2所述的方法，其特征在于，所述预设测距方法包括：预设到达时间测距方法、预设接收信号强度测距方法、预设到达角度测距方法、以及预设多普勒频移测距方法中的一种或多种，其中，所述预设到达测距方法为预设到达测距公式1：
Figure FDA0001813868310000041
其中，mk表示第mk个节点，nk表示第nk个节点，k表示第mk个节点获得的来自第nk个节点的第k个测距观测的序号，
Figure FDA0001813868310000042
表示使用预设的到达时间测距TOA，得到的第mk个节点和第nk个节点之间的测距，
Figure FDA0001813868310000043
中的下标k表示测距的序号，c表示光速，x表示所述二维预估坐标中的横坐标，y表示所述二维预估坐标中的纵坐标，z表示节点的高度；
Figure FDA0001813868310000044
表示第n个节点的速度向量，
Figure FDA0001813868310000045
表示第n个节点的速度向量中的x方向上的速度；
Figure FDA0001813868310000046
表示第n个节点的速度向量中的y方向上的速度；
Figure FDA0001813868310000047
表示第n个节点的速度向量中的z方向上的速度；
Figure FDA0001813868310000048
表示第mk个节点的三维预估坐标，
Figure FDA0001813868310000049
表示第nk个节点的三维预估坐标，
Figure FDA00018138683100000410
Figure FDA00018138683100000411
分别表示第mk个节点与第nk个节点之间相减的混合向量；
所述预设接收信号强度测距方法为预设接收信号强度测距公式2：
Figure FDA00018138683100000412
其中，
Figure FDA00018138683100000413
表示使用预设的接收信号强度RSS，得到的第mk个节点和第nk个节点之间的测距，P0表示自由空间发射功率为1w时，在1m位置接收信号功率的标准值，np表示路径损耗指数；
所述预设到达角度测距方法为预设到达角度测距公式3：
Figure FDA00018138683100000414
其中，
Figure FDA00018138683100000415
表示使用预设的到达角度测距AOA，得到的第mk个节点和第nk个节点之间的测距；
Figure FDA0001813868310000051
Figure FDA0001813868310000052
其中，
Figure FDA0001813868310000053
为第mk个节点与第nk个节点的连线与z轴的夹角的互余，
Figure FDA0001813868310000054
大于或等于
Figure FDA0001813868310000055
且小于或等于
Figure FDA0001813868310000056
Figure FDA0001813868310000057
为第mk个节点与第nk个节点的二维平面内坐标的连线与x轴正向逆时针的夹角，
Figure FDA0001813868310000058
大于或等于0，且小于或等于2π；
所述预设多普勒频移测距方法为预设多普勒频移测距公式4：
Figure FDA0001813868310000059
其中，
Figure FDA00018138683100000510
表示使用预设的多普勒频移DOP，得到的第mk个节点和第nk个节点之间的测距，fc表示载波频率。
7.一种车联网中车辆三维定位装置，其特征在于，应用于***，所述装置包括：
第一获取模块，用于获取待定位车辆的车辆高度以及由全球卫星导航***得到所述待定位车辆的二维预估坐标，形成的三维预估坐标；
第二获取模块，用于获取每个待定位车辆使用预设测距方法中的任一或者多种测距方法，进行与其他节点之间的测距，得到所述待定位车辆与所述其他节点之间的距离，作为测距距离，其中，所述其他节点包括：路边单元RSU以及除所述待定位车辆以外的其他车辆，所述其他车辆包括：待定位车辆；
鉴别选择模块，用于利用车辆的二维轮廓构建的第一R树以及建筑物的二维轮廓构建的第二R树；利用所述第二R树的搜索功能，鉴别所述测距距离中的建筑物引起的非视距NLOSb；以及利用所述车辆的高度以及所述第一R树的搜索功能，鉴别所述测距距离中的其他车辆引起的非视距NLOSv，从所述测距距离中选择除所述NLOSb和所述NLOSv以外的视距LOS对应的测距距离，其中所述第一R树包含单个车辆构成的最小矩形，所述第二R树包含单个建筑物构成的最小矩形；
确定模块，用于基于LOS对应的测距距离和所述待定位车辆的三维预估坐标，重新确定所述待定位车辆的三维预估坐标；确定所述三维预估坐标的置信度是否高于预设置信度阈值；如果不高于所述预设置信度阈值，继续执行所述鉴别选择模块和所述确定模块中的步骤，直至所述置信度高于所述预设置信度阈值，将所述三维预估坐标，作为待定位车辆的定位坐标。
8.如权利要求7所述的装置，其特征在于，所述鉴别选择模块，具体用于：
利用真实的每个车辆的二维轮廓及每个车辆的二维预估坐标，得到所有车辆的二维轮廓的二维预估坐标；
将所有车辆的二维轮廓的二维预估坐标，构建得到第一R树；
从开源可编辑的地图服务中获取所有建筑物的二维轮廓的二维预估坐标；
将所有建筑物的二维轮廓的二维预估坐标，构建得到第二R树；
利用第二R树的搜索功能，若判定所述测距距离中存在NLOSb；以及
利用所有车辆的高度以及第一R树的搜索功能，若判定所述测距距离中存在NLOSv，从所述测距距离中选择除所述NLOSb和所述NLOSv以外的LOS对应的测距距离。
9.如权利要求7或8所述的装置，其特征在于，所述确定模块，具体用于：
利用所述LOS对应的测距距离和所述待定位车辆的三维预估坐标，采用广义近似消息传递GAMP算法，第t次重新确定所述待定位车辆的三维预估坐标，并判断所有待定位车辆重新确定的三维预估坐标是否达到收敛条件；如果未达到，则继续执行所述鉴别选择模块和所述确定模块中的步骤，直至达到所述收敛条件，将最后一次重新确定的所述待定位车辆的三维预估坐标，作为所述待定位车辆的定位坐标，其中，t大于0，小于或等于最大迭代次数。
10.如权利要求9所述的装置，其特征在于，所述确定模块，具体用于：
利用所述LOS对应的测距距离和所述待定位车辆的三维预估坐标，采用广义近似消息传递GAMP算法，本次重新确定所述待定位车辆的三维预估坐标；
判断本次重新确定的所述待定位车辆的三维预估坐标与上次重新确定的所述待定位车辆的三维预估坐标之间的差值是否小于阈值；
若本次重新确定的所述待定位车辆的三维预估坐标与上次重新确定的所述待定位车辆的三维预估坐标之间的差值小于阈值，则将所述待定位车辆，作为已定位车辆，并将本次重新确定的所述待定位车辆的三维预估坐标，作为已定位车辆的三维预估坐标；
删除所述待定位车辆的三维预估坐标中对应的所述已定位车辆的三维预估坐标，将已定位车辆加入到RSU集合中，以使由车辆和RSU形成的网络结构发生变化；
利用除所述已定位车辆的三维预估坐标以外的其他待定位车辆的三维预估坐标，返回继续执行所述鉴别选择模块和所述确定模块中的步骤，直至达到所述收敛条件，将最后一次重新确定的所述待定位车辆的三维预估坐标，作为所述待定位车辆的定位坐标；
所述确定模块，具体还用于：
判定上一次重新确定的重要性采样的后验概率的均值与上一次之前的一次重新确定的重要性采样的后验概率的均值之间的误差大于或等于阀值，采用重要性采样方法，确定GAMP算法中的后验概率的方差和均值；
基于所述后验概率的方差和均值，利用GAMP算法，重新确定所述待定位车辆的三维预估坐标；以及
若判定上一次重新确定的重要性采样的后验概率的均值与上一次之前的一次重新确定的重要性采样方法的后验概率的均值之间的误差小于阀值，则删减此次重要性采样方法中所用的的粒子的数量，继续执行采用重要性采样方法，确定GAMP算法中的后验概率的方差和均值的步骤；
基于所述本次的后验概率的方差和均值，利用GAMP算法，重新确定所述待定位车辆的三维预估坐标以外的其他待定位车辆的三维预估坐标，返回继续执行所述鉴别选择模块和所述确定模块的步骤，直至达到所述收敛条件，将最后一次重新确定的所述待定位车辆的三维预估坐标，作为所述待定位车辆的定位坐标。
CN201811132281.0A 2018-09-27 2018-09-27 一种车联网中车辆三维定位方法及装置 Active CN109283562B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201811132281.0A CN109283562B (zh) 2018-09-27 2018-09-27 一种车联网中车辆三维定位方法及装置

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201811132281.0A CN109283562B (zh) 2018-09-27 2018-09-27 一种车联网中车辆三维定位方法及装置

Publications (2)

Publication Number Publication Date
CN109283562A CN109283562A (zh) 2019-01-29
CN109283562B true CN109283562B (zh) 2020-08-14

Family

ID=65181572

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201811132281.0A Active CN109283562B (zh) 2018-09-27 2018-09-27 一种车联网中车辆三维定位方法及装置

Country Status (1)

Country Link
CN (1) CN109283562B (zh)

Families Citing this family (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110018678B (zh) * 2019-02-14 2021-05-14 电子科技大学 一种网联汽车控制***的故障诊断方法
CN109756284B (zh) * 2019-02-18 2021-05-25 南京航空航天大学 面向动态拓扑车联网的车载节点通信模型快速构建方法
CN109884583B (zh) * 2019-03-26 2023-03-14 电子科技大学 利用一维测向确定目标三维坐标的凸优化方法
CN110095753B (zh) * 2019-05-14 2020-11-24 北京邮电大学 一种基于到达角度aoa测距的定位方法及装置
CN110286353B (zh) * 2019-05-23 2021-01-26 宁波大学 基于非视距环境下RSS-ToA的无线传感器网络目标定位方法
CN110398729B (zh) * 2019-07-16 2022-03-15 启迪云控(北京)科技有限公司 一种基于车联网的车辆定位方法及***
CN110850457A (zh) * 2019-10-22 2020-02-28 同济大学 一种用于室内煤场的无人机定位导航方法
CN113115205B (zh) * 2021-03-31 2022-05-17 北京理工大学 一种基于角度测量的分布式协作定位方法
CN116068593B (zh) * 2023-01-28 2023-06-16 中国铁建电气化局集团有限公司 基于贝叶斯的卫星定位权重计算方法、装置、设备及介质
CN117290537B (zh) * 2023-09-28 2024-06-07 腾讯科技（深圳）有限公司 图像搜索方法、装置、设备及存储介质

Family Cites Families (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2003194909A (ja) * 2001-12-26 2003-07-09 Hitachi Zosen Corp 三次元測位装置および三次元測位システム
WO2010020260A1 (en) * 2008-08-18 2010-02-25 Nec Europe Ltd. Method for enabling communication between communication devices in mobile networks forming mobile ad hoc networks and mobile ad hoc network
CN102984200B (zh) * 2012-09-13 2015-07-15 大连理工大学 一种适用于稀疏和稠密多种VANETs场景的方法
CN104680820B (zh) * 2015-02-12 2017-02-01 浙江大学 一种基于梯度场的交通流车联网***及交通流控制方法
CN107238814A (zh) * 2016-03-29 2017-10-10 茹景阳 一种车辆定位的装置与方法
CN106303968B (zh) * 2016-09-27 2019-06-11 北京航空航天大学 一种基于专用短程通信网络的路网黑客车辆定位方法
CN107508855B (zh) * 2017-07-12 2020-03-27 北京邮电大学 车联网中的节点通信方法和车联网节点
CN107484139B (zh) * 2017-08-14 2019-10-18 北京邮电大学 一种基于地理位置信息的车联网协作定位方法和装置

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
Geographical Information Enhanced Cooperative Localization in Vehicular Ad-Hoc Networks;Feng Luo, Shengchu Wang;《IEEE SIGNAL PROCESSING LETTERS》;20180430;第25卷(第4期);第556-560页 *

Also Published As

Publication number Publication date
CN109283562A (zh) 2019-01-29

Similar Documents

Publication Publication Date Title
CN109283562B (zh) 一种车联网中车辆三维定位方法及装置
CN107659893B (zh) 一种误差补偿方法、装置、电子设备及可读存储介质
CN106912105B (zh) 基于pso_bp神经网络的三维定位方法
CN108732603B (zh) 用于定位车辆的方法和装置
EP3513218A1 (en) Localization and tracking using location, signal strength, and pseudorange data
CN108253976B (zh) 一种充分借助车辆航向的三阶段在线地图匹配算法
KR20180091372A (ko) 레이더의 표적 위치 추적 방법
Teoman et al. Trilateration in indoor positioning with an uncertain reference point
JP2020063958A (ja) 位置推定装置及び方法
CN108871365B (zh) 一种航向约束下的状态估计方法及***
CN115204212A (zh) 一种基于stm-pmbm滤波算法的多目标跟踪方法
CN108279007B (zh) 一种基于随机信号的定位方法及装置
Gentner et al. Log‐PF: Particle Filtering in Logarithm Domain
CN111678513A (zh) 一种超宽带/惯导紧耦合的室内定位装置与***
CN104635206B (zh) 一种无线定位的方法及装置
CN113758492A (zh) 地图检测方法和装置
CN108834053B (zh) 一种定位方法、装置及设备
CN109031188B (zh) 一种基于蒙特卡罗的窄带辐射源频差估计方法及装置
US20100289699A1 (en) Method and Apparatus for Determining a Position of a Mobile Station in a Wireless Communication System
Havyarimana et al. A Hybrid Approach‐Based Sparse Gaussian Kernel Model for Vehicle State Determination during Outage‐Free and Complete‐Outage GPS Periods
KR102026114B1 (ko) 무인 항공기의 도심 항법 위치 추정장치 및 방법
CN110207699B (zh) 一种定位方法和装置
CN112566242A (zh) 一种基于贝叶斯估计的定位方法、装置及电子设备
Ersan et al. Map matching with kalman filter and location estimation
CN113701768A (zh) 一种路径确定方法、装置及电子设备

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