CN109085533B - 局部无线波通讯定位方法、系统、设备及存储介质 - Google Patents

局部无线波通讯定位方法、系统、设备及存储介质 Download PDF

Info

Publication number
CN109085533B
CN109085533B CN201810716624.1A CN201810716624A CN109085533B CN 109085533 B CN109085533 B CN 109085533B CN 201810716624 A CN201810716624 A CN 201810716624A CN 109085533 B CN109085533 B CN 109085533B
Authority
CN
China
Prior art keywords
positioning
coordinate
axis
particle
vehicle
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
CN201810716624.1A
Other languages
English (en)
Other versions
CN109085533A (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.)
Shanghai Xijing Technology Co ltd
Original Assignee
Shanghai Westwell Information Technology Co Ltd
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 Shanghai Westwell Information Technology Co Ltd filed Critical Shanghai Westwell Information Technology Co Ltd
Priority to CN201810716624.1A priority Critical patent/CN109085533B/zh
Publication of CN109085533A publication Critical patent/CN109085533A/zh
Application granted granted Critical
Publication of CN109085533B publication Critical patent/CN109085533B/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
    • 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

Landscapes

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

Abstract

本发明提供了局部无线波通讯定位方法、系统、设备及存储介质，该方法包括：获得两个定位通讯单元与定位通讯传感器三者的距离和定位通讯传感器的离地高度，以第一定位通讯单元为坐标系原点，建立空间坐标系和建立平面坐标系，根据观测方程，获得在平面坐标内车辆的中心的坐标；建立平面坐标系中表示车辆的中心在预测方程；在当前时刻根据观测方程得到的车辆的中心的坐标根据正态分布公式随机分布n个粒子；获得t时刻下n个粒子各自的预测坐标和观测坐标；根据权重更新公式更新n个粒子的权重；将权重的值最大的粒子的坐标作为t时刻下车辆的中心的预测坐标。本发明能够在遮挡比较严重，GPS基本失效的地方能够给自动驾驶的汽车坐标位置方向。

Description

局部无线波通讯定位方法、系统、设备及存储介质
技术领域
本发明涉及车辆定位领域，具体地说，涉及局部无线波通讯定位方法、系统、设备及存储介质。
背景技术
随着科技的不断进步，自动驾驶技术也变得越来越平民化，可以实现。车辆的定位在自动驾驶里尤为重要，然而GPS这种全局定位的精准度是有限制，而在码头岸桥还有轮胎吊下抓放箱，对于定位的要求都是厘米级别的，GPS在岸桥或者龙门吊下由于严重的遮挡，很可能是没有卫星数据的。在这种情况下，精准的局部定位就显得很重要。
发明内容
针对现有技术中的问题，本发明的目的在于提供局部无线波通讯定位方法、系统、设备及存储介质，能够在遮挡比较严重，GPS基本失效的地方能够给自动驾驶的汽车坐标位置方向。
本发明的实施例提供一种局部无线波通讯定位方法，包括以下步骤：
S101、一车辆设有一定位通讯传感器，通过第一定位通讯单元和第二定位通讯单元分别实时与所述定位通讯传感器进行通讯，获得所述定位通讯传感器到第一定位通讯单元的第一距离L2和第二定位通讯单元的第二距离L3，测得所述第一定位通讯单元和第二定位通讯单元的间距为L1、所述定位通讯传感器与所述车辆的中心的距离为L4、所述定位通讯传感器的离地高度为h；
S102、以第一定位通讯单元为坐标系原点，建立空间坐标系，第二定位通讯单元位于Y轴，获得所述定位通讯传感器的坐标为(xm，ym，h)，其中，ym＝(L12+L32-L22)/(2*L1)；
Figure GDA0002681856390000011
S103、以第一定位通讯单元为坐标系原点，建立平面坐标系，第二定位通讯单元位于Y轴，设所述定位通讯传感器与车辆的中心的连线与Y轴方向的夹角为θ1，所述车辆的行驶方向与Y轴方向的夹角为θ2；
根据观测方程：
Figure GDA0002681856390000021
获得在平面坐标内所述车辆的中心的坐标(xc，yc)；
S104、建立所述平面坐标系中表示车辆的中心在预测方程的预测坐标(xt，yt)：
Figure GDA0002681856390000022
其中；xt为t时刻的X轴坐标，xt-1为t-1时刻的X轴坐标，yt为t时刻的Y轴坐标，yt-1为t-1时刻的Y轴坐标；
S105、在当前时刻根据所述观测方程得到的所述车辆的中心的坐标(xc，yc)根据正态分布公式随机分布n个粒子，所述粒子的坐标为(x，y)，每一个粒子的权重为
Figure GDA0002681856390000023
所述正态分布公式为
Figure GDA0002681856390000024
其中，f(x)代表是粒子基于X轴坐标的概率，其中的μx代表的是当前粒子在X轴上随机分布之前的坐标值，σx代表的是该粒子在X轴上随机分布的标准差stdx；
f(y)代表是粒子基于Y轴坐标的概率，其中的μy代表的是当前粒子在Y轴上随机分布之前的坐标值，σy代表的是该粒子在Y轴上随机分布的标准差stdy；
S106、根据预测方程
Figure GDA0002681856390000025
获得t时刻下n个粒子各自的预测坐标(xt，yt)；
S107、根据观测方程
Figure GDA0002681856390000026
获得t时刻下n个粒子各自的观测坐标(xc，yc)；
S108、根据权重更新公式：
Figure GDA0002681856390000027
更新n个粒子的权重；
S109、将权重的值最大的粒子的坐标(x，y)作为t时刻下车辆的中心的预测坐标；
S110、将n个粒子根据权重进行重采样；
S111、将n个粒子的权重全部恢复为
Figure GDA0002681856390000031
S112、返回步骤S105，进入t+1时刻。
优选地，所述步骤S110中，包括：
步骤S1101、将n个粒子的权重生成n个依次连续的区间，每个区间的大小与每个粒子的权重值呈正比；
步骤S1102、在所有区间的最大值和最小值之间生成n个随机数；
步骤S1103、保留包含n个所述随机数的区间所对应的所有粒子的坐标(x，y)。
优选地，所述步骤S105中n等于100，或者等于1000，或者等于10000。
优选地，所述步骤S101中的间距L1、距离为L4以及离地高度h都是预先测得的，
第一距离L2是根据所述定位通讯传感器与第一定位通讯单元的信号强弱实时测得的，
第二距离L3是根据所述定位通讯传感器与第二定位通讯单元的信号强弱实时测得的。
优选地，所述步骤S103中夹角θ1是通过车辆上的转弯角度传感器实时获得的，夹角θ2是预设的。
优选地，所述步骤S105中所述粒子的X轴坐标越接近当前粒子在X轴上随机分布之前的坐标值μx，则所述粒子基于X轴坐标的概率f(x)越大；
所述粒子的Y轴坐标越接近当前粒子在Y轴上随机分布之前的坐标值μy，则所述粒子基于Y轴坐标的概率f(y)越大。
优选地，所述步骤S105中粒子的X轴坐标的概率f(x)从负无穷到正无穷的积分为1，粒子的Y轴坐标的概率f(y)从负无穷到正无穷的积分为1。
本发明的实施例还提供一种局部无线波通讯定位系统，用于实现上述的局部无线波通讯定位方法，局部无线波通讯定位系统包括：
测量模块，所述车辆设有一定位通讯传感器，通过第一定位通讯单元和第二定位通讯单元分别实时与所述定位通讯传感器进行通讯，获得所述定位通讯传感器到第一定位通讯单元的第一距离L2和第二定位通讯单元的第二距离L3，测得所述第一定位通讯单元和第二定位通讯单元的间距为L1、所述定位通讯传感器与所述车辆的中心的距离为L4、所述定位通讯传感器的离地高度为h；
立体坐标系模块，以第一定位通讯单元为坐标系原点，建立空间坐标系，第二定位通讯单元位于Y轴，获得所述定位通讯传感器的坐标为(xm，ym，h)，其中，ym＝(L12+L32-L22)/(2*L1)；
Figure GDA0002681856390000041
观测方程生成模块，以第一定位通讯单元为坐标系原点，建立平面坐标系，第二定位通讯单元位于Y轴，设所述定位通讯传感器与车辆的中心的连线与Y轴方向的夹角为θ1，所述车辆的行驶方向与Y轴方向的夹角为θ2；
根据观测方程：
Figure GDA0002681856390000042
获得在平面坐标内所述车辆的中心的坐标(xc，yc)；
预测方程生成模块，建立所述平面坐标系中表示车辆的中心在预测方程的预测坐标(xt，yt)：
Figure GDA0002681856390000043
其中；xt为t时刻的X轴坐标，xt-1为t-1时刻的X轴坐标，yt为t时刻的Y轴坐标，yt-1为t-1时刻的Y轴坐标；
粒子分布模块，在当前时刻根据所述观测方程得到的所述车辆的中心的坐标(xc，yc)根据正态分布公式随机分布n个粒子，所述粒子的坐标为(x，y)，每一个粒子的权重为
Figure GDA0002681856390000044
所述正态分布公式为
Figure GDA0002681856390000045
其中，f(x)代表是粒子基于X轴坐标的概率，其中的μx代表的是当前粒子在X轴上随机分布之前的坐标值，σx代表的是该粒子在X轴上随机分布的标准差stdx；
f(y)代表是粒子基于Y轴坐标的概率，其中的μy代表的是当前粒子在Y轴上随机分布之前的坐标值，σy代表的是该粒子在Y轴上随机分布的标准差stdy；
预测方程计算模块，根据预测方程
Figure GDA0002681856390000051
获得t时刻下n个粒子各自的预测坐标(xt，yt)；
观测方程生成模块，根据观测方程
Figure GDA0002681856390000052
获得t时刻下n个粒子各自的观测坐标(xc，yc)；
权重更新模块，根据权重更新公式：
Figure GDA0002681856390000053
更新n个粒子的权重；
预测坐标输出模块，将权重的值最大的粒子的坐标(x，y)作为t时刻下车辆的中心的预测坐标；
粒子重采样模块，将n个粒子根据权重进行重采样；
粒子权重恢复模块，将n个粒子的权重全部恢复为
Figure GDA0002681856390000054
返回粒子分布模块，进入t+1时刻。
本发明的实施例还提供一种局部无线波通讯定位设备，包括：
处理器；
存储器，其中存储有处理器的可执行指令；
其中，处理器配置为经由执行可执行指令来执行上述局部无线波通讯定位方法的步骤。
本发明的实施例还提供一种计算机可读存储介质，用于存储程序，程序被执行时实现上述局部无线波通讯定位方法的步骤。
本发明的局部无线波通讯定位方法、系统、设备及存储介质，能够在遮挡比较严重，GPS基本失效的地方能够给自动驾驶的汽车坐标位置方向，该技术未来应用前景广泛，能够在除了码头之外其他领域如隧道，高架底等地方应用。相对于其他方式如用视觉定位，该方式受到环境因素的影响较小。能够全天候7×24小时工作。
附图说明
通过阅读参照以下附图对非限制性实施例所作的详细描述，本发明的其它特征、目的和优点将会变得更明显。
图1是本发明的局部无线波通讯定位方法的流程图；
图2至4是本发明的局部无线波通讯定位方法的实施状态示意图；
图5是车辆在停止状态时，未使用和使用本发明的局部无线波通讯定位方法的定位效果对比图；
图6是车辆在行驶状态时，未使用和使用本发明的局部无线波通讯定位方法的定位效果对比图；
图7是本发明的局部无线波通讯定位系统的结构示意图；
图8是本发明的局部无线波通讯定位设备的结构示意图；以及
图9是本发明一实施例的计算机可读存储介质的结构示意图。
具体实施方式
现在将参考附图更全面地描述示例实施方式。然而，示例实施方式能够以多种形式实施，且不应被理解为限于在此阐述的实施方式。相反，提供这些实施方式使得本发明将全面和完整，并将示例实施方式的构思全面地传达给本领域的技术人员。在图中相同的附图标记表示相同或类似的结构，因而将省略对它们的重复描述。
图1是本发明的局部无线波通讯定位方法的流程图。如图1至4所示，本发明的实施例提供一种局部无线波通讯定位方法，包括以下步骤：
S101、一车辆30设有一定位通讯传感器31，通过第一定位通讯单元10和第二定位通讯单元20分别实时与所述定位通讯传感器31进行通讯，获得所述定位通讯传感器31到第一定位通讯单元10的第一距离L2和第二定位通讯单元20的第二距离L3，测得所述第一定位通讯单元10和第二定位通讯单元20的间距为L1、所述定位通讯传感器31与所述车辆30的中心的距离为L4、所述定位通讯传感器31的离地高度为h；
S102、以第一定位通讯单元10为坐标系原点O，建立空间坐标系，第二定位通讯单元20位于Y轴，获得所述定位通讯传感器31的坐标为(xm，ym，h)，其中，ym＝(L12+L32-L22)/(2*L1)；
Figure GDA0002681856390000071
S103、以第一定位通讯单元10为坐标系原点，建立平面坐标系，第二定位通讯单元20位于Y轴，设所述定位通讯传感器31与车辆30的中心的连线与Y轴方向的夹角为θ1，所述车辆30的行驶方向与Y轴方向的夹角为θ2；
根据观测方程：
Figure GDA0002681856390000072
获得在平面坐标内所述车辆30的中心的坐标(xc，yc)；
S104、建立所述平面坐标系中表示车辆30的中心在预测方程的预测坐标(xt，yt)：
Figure GDA0002681856390000073
其中；xt为t时刻的X轴坐标，xt-1为t-1时刻的X轴坐标，yt为t时刻的Y轴坐标，yt-1为t-1时刻的Y轴坐标；
S105、在当前时刻根据所述观测方程得到的所述车辆30的中心的坐标(xc，yc)根据正态分布公式随机分布n个粒子，所述粒子的坐标为(x，y)，每一个粒子的权重为
Figure GDA0002681856390000074
所述正态分布公式为
Figure GDA0002681856390000075
其中，f(x)代表是粒子基于X轴坐标的概率，其中的μx代表的是当前粒子在X轴上随机分布之前的坐标值，σx代表的是该粒子在X轴上随机分布的标准差stdx；
f(y)代表是粒子基于Y轴坐标的概率，其中的μy代表的是当前粒子在Y轴上随机分布之前的坐标值，σy代表的是该粒子在Y轴上随机分布的标准差stdy；
S106、根据预测方程
Figure GDA0002681856390000076
获得t时刻下n个粒子各自的预测坐标(xt，yt)；
S107、根据观测方程
Figure GDA0002681856390000077
获得t时刻下n个粒子各自的观测坐标(xc，yc)；
S108、根据权重更新公式：
Figure GDA0002681856390000081
更新n个粒子的权重；
S109、将权重的值最大的粒子的坐标(x，y)作为t时刻下车辆30的中心的预测坐标；
S110、将n个粒子根据权重进行重采样；以及
S111、将n个粒子的权重全部恢复为
Figure GDA0002681856390000082
返回步骤S105，进入t+1时刻。
在一个优选实施例中，所述步骤S110中，包括：
步骤S1101、将n个粒子的权重生成n个依次连续的区间，每个区间的大小与每个粒子的权重值呈正比；
步骤S1102、在所有区间的最大值和最小值之间生成n个随机数；
步骤S1103、保留包含n个所述随机数的区间所对应的所有粒子的坐标(x，y)。
在一个优选实施例中，所述步骤S105中n等于100，或者等于1000，或者等于10000。
在一个优选实施例中，所述步骤S101中的间距L1、距离为L4以及离地高度h都是预先测得的，
第一距离L2是根据所述定位通讯传感器31与第一定位通讯单元10的信号强弱实时测得的，
第二距离L3是根据所述定位通讯传感器31与第二定位通讯单元20的信号强弱实时测得的。
在一个优选实施例中，所述步骤S103中夹角θ1是通过车辆上的转弯角度传感器实时获得的，夹角θ2是预设的。
在一个优选实施例中，所述步骤S105中所述粒子的X轴坐标越接近当前粒子在X轴上随机分布之前的坐标值μx，则所述粒子基于X轴坐标的概率f(x)越大；
所述粒子的Y轴坐标越接近当前粒子在Y轴上随机分布之前的坐标值μy，则所述粒子基于Y轴坐标的概率f(y)越大。
在一个优选实施例中，所述步骤S105中粒子的X轴坐标的概率f(x)从负无穷到正无穷的积分为1，粒子的Y轴坐标的概率f(y)从负无穷到正无穷的积分为1。
参考图2，在岸桥下，由于是全部金属的遮挡，GPS的信号基本已经丢失。而此时跨运车或者货车的抓箱与取箱又需要厘米级别的定位，这时候局部无线定位设备的必要性就体现出来了。两个固定无线定位设备会被放置在岸桥下，一个移动节点被固定在车上，随着车子的移动而移动。在运行我们的驱动后，岸桥上两个固定的节点会不断地向车上的移动节点发送两者的距离信息。建立了一个以图2下侧的无线定位设备为原点，两个设备连线为y轴正方向，垂直于地面的方向为z轴的坐标系。让无人车不断的和无线定位设备交互，得到相互之间的位置信息。设备的驱动负责配置相关信息，发送和读取位置信息。
得到无线定位设备的距离信息后，我们要根据其来获得坐标位置信息，该位置信息被称为观测方程。如图2，图3所示，假设初始阶段车子前进方向沿着无线电定位设备坐标y轴方向，移动节点距离车中心长度为L4,与y轴角度为θ1,随着车子移动，假设车子和y轴形成了θ2的角度。我们可以首先计算出移动无线电通信设备在此坐标系下位置，由于车和移动无线电通信设备是固连的，由此也可以推算出车中心的位置坐标。假设两个固定节点的距离L1已经测量得到，移动节点和固定节点的距离L2，L3从驱动中得到，移动节点和固定节点的高度差h测量得到。则移动节点在无线通信设备坐标系下的位置可以求得。所述定位通讯传感器31的坐标为(xm，ym，h)，其中，ym＝(L12+L32-L22)/(2*L1)；
Figure GDA0002681856390000091
已知移动节点实时在无线电定位设备坐标下的位置，根据观测方程：
Figure GDA0002681856390000092
获得在平面坐标内所述车辆30的中心的坐标(xc，yc)；
建立所述平面坐标系中表示车辆30的中心在预测方程的预测坐标(xt，yt)：
Figure GDA0002681856390000101
其中；xt为t时刻的X轴坐标，xt-1为t-1时刻的X轴坐标，yt为t时刻的Y轴坐标，yt-1为t-1时刻的Y轴坐标；
在当前时刻根据所述观测方程得到的所述车辆30的中心的坐标(xc，yc)根据正态分布公式随机分布1000个粒子，所述粒子的坐标为(x，y)，每一个粒子的权重为
Figure GDA0002681856390000102
所述正态分布公式为
Figure GDA0002681856390000103
其中，f(x)代表是粒子基于X轴坐标的概率，其中的μx代表的是1000个粒子X轴坐标的平均值，σ代表的是1000个粒子X轴坐标的标准差stdx；
f(y)代表是粒子基于Y轴坐标的概率，其中的μy代表的是1000个粒子Y轴坐标的平均值，σ代表的是1000个粒子Y轴坐标的标准差stdy；
根据预测方程
Figure GDA0002681856390000104
获得t时刻下1000个粒子各自的预测坐标(xt，yt)；
根据观测方程
Figure GDA0002681856390000105
获得t时刻下1000个粒子各自的观测坐标(xc，yc)；
根据权重更新公式：
Figure GDA0002681856390000106
更新1000个粒子的权重，通过此时比较观测方程也预测方程的值对于预测方程中的粒子重新赋权重。
将权重的值最大的粒子的坐标(x，y)作为t时刻下车辆30的中心的预测坐标；预测坐标就是本发明的局部无线波通讯定位方法的输出结果，以此来定位车辆。
将1000个粒子根据权重进行重采样；将1000个粒子的权重生成1000个依次连续的区间，每个区间的跨度大小与每个粒子的权重值呈正比；获得所有区间的最大值和最小值，在最大值和最小值之间生成1000个随机数；保留包含1000个所述随机数的区间所对应的所有粒子的坐标(x，y)。
最后，将1000个粒子的权重全部恢复为
Figure GDA0002681856390000111
进入下一时刻。
图5是车辆在停止状态时，未使用和使用本发明的局部无线波通讯定位方法的定位效果对比图。从图中对比可知，使用了本发明的局部无线波通讯定位方法后的曲线(实线)的定位波动明显小于没有使用本发明的现有技术(虚线)的定位波动，经过本发明的滤波过后的位置信息明显比滤波前稳定，抖动小。
图6是车辆在行驶状态时，未使用和使用本发明的局部无线波通讯定位方法的定位效果对比图。从图中对比可知，没有使用本发明的现有技术(虚线)的定位很容易受到环境因素的影响，例如e和f部分收到遮挡等情况下，定位波动非常明显，容易出现瞬时大幅波动的情况，严重干扰定位的精准度。使用本发明后的曲线(实线)的定位则明显不易受到环境因素的影响，基本没有出现大幅波动的情况。
本发明的局部无线波通讯定位方法能够在遮挡比较严重，GPS基本失效的地方能够给自动驾驶的汽车坐标位置方向，该技术未来应用前景广泛，能够在除了码头之外其他领域如隧道，高架底等地方应用。相对于其他方式如用视觉定位，该方式受到环境因素的影响较小。能够全天候7×24小时工作。
图7是本发明的局部无线波通讯定位系统的模块示意图。如图7所示，本发明的实施例还提供一种局部无线波通讯定位系统，用于实现上述的局部无线波通讯定位方法，局部无线波通讯定位系统500包括：
测量模块501，所述车辆设有一定位通讯传感器，通过第一定位通讯单元和第二定位通讯单元分别实时与所述定位通讯传感器进行通讯，获得所述定位通讯传感器到第一定位通讯单元的第一距离L2和第二定位通讯单元的第二距离L3，测得所述第一定位通讯单元和第二定位通讯单元的间距为L1、所述定位通讯传感器与所述车辆的中心的距离为L4、所述定位通讯传感器的离地高度为h；
立体坐标系模块502，以第一定位通讯单元为坐标系原点，建立空间坐标系，第二定位通讯单元位于Y轴，获得所述定位通讯传感器的坐标为(xm，ym，h)，其中，ym＝(L12+L32-L22)/(2*L1)；
Figure GDA0002681856390000121
观测方程生成模块503，以第一定位通讯单元为坐标系原点，建立平面坐标系，第二定位通讯单元位于Y轴，设所述定位通讯传感器与车辆的中心的连线与Y轴方向的夹角为θ1，所述车辆的行驶方向与Y轴方向的夹角为θ2；
根据观测方程：
Figure GDA0002681856390000122
获得在平面坐标内所述车辆的中心的坐标(xc，yc)；
预测方程生成模块504，建立所述平面坐标系中表示车辆的中心在预测方程的预测坐标(xt，yt)：
Figure GDA0002681856390000123
其中；xt为t时刻的X轴坐标，xt-1为t-1时刻的X轴坐标，yt为t时刻的Y轴坐标，yt-1为t-1时刻的Y轴坐标；
粒子分布模块505，在当前时刻根据所述观测方程得到的所述车辆的中心的坐标(xc，yc)根据正态分布公式随机分布n个粒子，所述粒子的坐标为(x，y)，每一个粒子的权重为
Figure GDA0002681856390000124
所述正态分布公式为
Figure GDA0002681856390000125
其中，f(x)代表是粒子基于X轴坐标的概率，其中的μx代表的是当前粒子在X轴上随机分布之前的坐标值，σx代表的是该粒子在X轴上随机分布的标准差stdx；
f(y)代表是粒子基于Y轴坐标的概率，其中的μy代表的是当前粒子在Y轴上随机分布之前的坐标值，σy代表的是该粒子在Y轴上随机分布的标准差stdy；
预测方程计算模块506，根据预测方程
Figure GDA0002681856390000126
获得t时刻下n个粒子各自的预测坐标(xt，yt)；
观测方程生成模块507，根据观测方程
Figure GDA0002681856390000127
获得t时刻下n个粒子各自的观测坐标(xc，yc)；
权重更新模块508，根据权重更新公式：
Figure GDA0002681856390000131
更新n个粒子的权重；
预测坐标输出模块509，将权重的值最大的粒子的坐标(x，y)作为t时刻下车辆的中心的预测坐标；
粒子重采样模块510，将n个粒子根据权重进行重采样；
粒子权重恢复模块511，将n个粒子的权重全部恢复为
Figure GDA0002681856390000132
返回粒子分布模块，进入t+1时刻。
本发明的局部无线波通讯定位系统能够在遮挡比较严重，GPS基本失效的地方能够给自动驾驶的汽车坐标位置方向，该技术未来应用前景广泛，能够在除了码头之外其他领域如隧道，高架底等地方应用。相对于其他方式如用视觉定位，该方式受到环境因素的影响较小。能够全天候7×24小时工作。
本发明实施例还提供一种局部无线波通讯定位设备，包括处理器。存储器，其中存储有处理器的可执行指令。其中，处理器配置为经由执行可执行指令来执行的局部无线波通讯定位方法的步骤。
如上，本发明的局部无线波通讯定位设备能够在遮挡比较严重，GPS基本失效的地方能够给自动驾驶的汽车坐标位置方向，该技术未来应用前景广泛，能够在除了码头之外其他领域如隧道，高架底等地方应用。相对于其他方式如用视觉定位，该方式受到环境因素的影响较小。能够全天候7×24小时工作。
所属技术领域的技术人员能够理解，本发明的各个方面可以实现为系统、方法或程序产品。因此，本发明的各个方面可以具体实现为以下形式，即：完全的硬件实施方式、完全的软件实施方式(包括固件、微代码等)，或硬件和软件方面结合的实施方式，这里可以统称为“电路”、“模块”或“平台”。
图8是本发明的局部无线波通讯定位设备的结构示意图。下面参照图8来描述根据本发明的这种实施方式的电子设备600。图8显示的电子设备600仅仅是一个示例，不应对本发明实施例的功能和使用范围带来任何限制。
如图8所示，电子设备600以通用计算设备的形式表现。电子设备600的组件可以包括但不限于：至少一个处理单元610、至少一个存储单元620、连接不同平台组件(包括存储单元620和处理单元610)的总线630、显示单元640等。
其中，存储单元存储有程序代码，程序代码可以被处理单元610执行，使得处理单元610执行本说明书上述电子处方流转处理方法部分中描述的根据本发明各种示例性实施方式的步骤。例如，处理单元610可以执行如图1中所示的步骤。
存储单元620可以包括易失性存储单元形式的可读介质，例如随机存取存储单元(RAM)6201和/或高速缓存存储单元6202，还可以进一步包括只读存储单元(ROM)6203。
存储单元620还可以包括具有一组(至少一个)程序模块6205的程序/实用工具6204，这样的程序模块6205包括但不限于：操作系统、一个或者多个应用程序、其它程序模块以及程序数据，这些示例中的每一个或某种组合中可能包括网络环境的实现。
总线630可以为表示几类总线结构中的一种或多种，包括存储单元总线或者存储单元控制器、外围总线、图形加速端口、处理单元或者使用多种总线结构中的任意总线结构的局域总线。
电子设备600也可以与一个或多个外部设备700(例如键盘、指向设备、蓝牙设备等)通信，还可与一个或者多个使得用户能与该电子设备600交互的设备通信，和/或与使得该电子设备600能与一个或多个其它计算设备进行通信的任何设备(例如路由器、调制解调器等等)通信。这种通信可以通过输入/输出(I/O)接口650进行。并且，电子设备600还可以通过网络适配器660与一个或者多个网络(例如局域网(LAN)，广域网(WAN)和/或公共网络，例如因特网)通信。网络适配器660可以通过总线630与电子设备600的其它模块通信。应当明白，尽管图中未示出，可以结合电子设备600使用其它硬件和/或软件模块，包括但不限于：微代码、设备驱动器、冗余处理单元、外部磁盘驱动阵列、RAID系统、磁带驱动器以及数据备份存储平台等。
本发明实施例还提供一种计算机可读存储介质，用于存储程序，程序被执行时实现的局部无线波通讯定位方法的步骤。在一些可能的实施方式中，本发明的各个方面还可以实现为一种程序产品的形式，其包括程序代码，当程序产品在终端设备上运行时，程序代码用于使终端设备执行本说明书上述电子处方流转处理方法部分中描述的根据本发明各种示例性实施方式的步骤。
如上所示，该实施例的计算机可读存储介质的程序在执行时，能够在遮挡比较严重，GPS基本失效的地方能够给自动驾驶的汽车坐标位置方向，该技术未来应用前景广泛，能够在除了码头之外其他领域如隧道，高架底等地方应用。相对于其他方式如用视觉定位，该方式受到环境因素的影响较小。能够全天候7×24小时工作。
图9是本发明的计算机可读存储介质的结构示意图。参考图9所示，描述了根据本发明的实施方式的用于实现上述方法的程序产品800，其可以采用便携式紧凑盘只读存储器(CD-ROM)并包括程序代码，并可以在终端设备，例如个人电脑上运行。然而，本发明的程序产品不限于此，在本文件中，可读存储介质可以是任何包含或存储程序的有形介质，该程序可以被指令执行系统、装置或者器件使用或者与其结合使用。
程序产品可以采用一个或多个可读介质的任意组合。可读介质可以是可读信号介质或者可读存储介质。可读存储介质例如可以为但不限于电、磁、光、电磁、红外线、或半导体的系统、装置或器件，或者任意以上的组合。可读存储介质的更具体的例子(非穷举的列表)包括：具有一个或多个导线的电连接、便携式盘、硬盘、随机存取存储器(RAM)、只读存储器(ROM)、可擦式可编程只读存储器(EPROM或闪存)、光纤、便携式紧凑盘只读存储器(CD-ROM)、光存储器件、磁存储器件、或者上述的任意合适的组合。
计算机可读存储介质可以包括在基带中或者作为载波一部分传播的数据信号，其中承载了可读程序代码。这种传播的数据信号可以采用多种形式，包括但不限于电磁信号、光信号或上述的任意合适的组合。可读存储介质还可以是可读存储介质以外的任何可读介质，该可读介质可以发送、传播或者传输用于由指令执行系统、装置或者器件使用或者与其结合使用的程序。可读存储介质上包含的程序代码可以用任何适当的介质传输，包括但不限于无线、有线、光缆、RF等等，或者上述的任意合适的组合。
可以以一种或多种程序设计语言的任意组合来编写用于执行本发明操作的程序代码，程序设计语言包括面向对象的程序设计语言—诸如Java、C++等，还包括常规的过程式程序设计语言—诸如“C”语言或类似的程序设计语言。程序代码可以完全地在用户计算设备上执行、部分地在用户设备上执行、作为一个独立的软件包执行、部分在用户计算设备上部分在远程计算设备上执行、或者完全在远程计算设备或服务器上执行。在涉及远程计算设备的情形中，远程计算设备可以通过任意种类的网络，包括局域网(LAN)或广域网(WAN)，连接到用户计算设备，或者，可以连接到外部计算设备(例如利用因特网服务提供商来通过因特网连接)。
综上，本发明的局部无线波通讯定位方法、系统、设备及存储介质，能够在遮挡比较严重，GPS基本失效的地方能够给自动驾驶的汽车坐标位置方向，该技术未来应用前景广泛，能够在除了码头之外其他领域如隧道，高架底等地方应用。相对于其他方式如用视觉定位，该方式受到环境因素的影响较小。能够全天候7×24小时工作。
以上内容是结合具体的优选实施方式对本发明所作的进一步详细说明，不能认定本发明的具体实施只局限于这些说明。对于本发明所属技术领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干简单推演或替换，都应当视为属于本发明的保护范围。

Claims (10)

1.一种局部无线波通讯定位方法，其特征在于，包括以下步骤：
S101、一车辆设有一定位通讯传感器，通过第一定位通讯单元和第二定位通讯单元分别实时与所述定位通讯传感器进行通讯，获得所述定位通讯传感器到第一定位通讯单元的第一距离L2和第二定位通讯单元的第二距离L3，测得所述第一定位通讯单元和第二定位通讯单元的间距为L1、所述定位通讯传感器与所述车辆的中心的距离为L4、所述定位通讯传感器的离地高度为h；
S102、以第一定位通讯单元为坐标系原点，建立空间坐标系，第二定位通讯单元位于Y轴，获得所述定位通讯传感器的坐标为(xm，ym，h)，其中，ym＝(L12+L32-L22)/(2*L1)；
Figure FDA0002681856380000011
S103、以第一定位通讯单元为坐标系原点，建立平面坐标系，第二定位通讯单元位于Y轴，设所述定位通讯传感器与车辆的中心的连线与Y轴方向的夹角为θ1，所述车辆的行驶方向与Y轴方向的夹角为θ2；
根据观测方程：
Figure FDA0002681856380000012
获得在平面坐标内所述车辆的中心的坐标(xc，yc)；
S104、建立所述平面坐标系中表示车辆的中心在预测方程的预测坐标(xt，yt)：
Figure FDA0002681856380000013
其中；xt为t时刻的X轴坐标，xt-1为t-1时刻的X轴坐标，yt为t时刻的Y轴坐标，yt-1为t-1时刻的Y轴坐标；
S105、在当前时刻根据所述观测方程得到的所述车辆的中心的坐标(xc，yc)根据正态分布公式随机分布n个粒子，所述粒子的坐标为(x，y)，每一个粒子的权重为
Figure FDA0002681856380000014
所述正态分布公式为
Figure FDA0002681856380000015
其中，f(x)代表是粒子基于X轴坐标的概率，其中的μx代表的是当前粒子在X轴上随机分布之前的坐标值，σx代表的是该粒子在X轴上随机分布的标准差stdx；
f(y)代表是粒子基于Y轴坐标的概率，其中的μy代表的是当前粒子在Y轴上随机分布之前的坐标值，σy代表的是该粒子在Y轴上随机分布的标准差stdy；
S106、根据预测方程
Figure FDA0002681856380000021
获得t时刻下n个粒子各自的预测坐标(xt，yt)；
S107、根据观测方程
Figure FDA0002681856380000022
获得t时刻下n个粒子各自的观测坐标(xc，yc)；
S108、根据权重更新公式：
Wt＝e(-0.5*((xc-xt)2/stdx2+(yc-yt)2/stdy2))/(2*π*stdx*stdy)*Wt-1更新n个粒子的权重；
S109、将权重的值最大的粒子的坐标(x，y)作为t时刻下车辆的中心的预测坐标；
S110、将n个粒子根据权重进行重采样；以及
S111、将n个粒子的权重全部恢复为
Figure FDA0002681856380000023
返回步骤S105，进入t+1时刻。
2.根据权利要求1所述的局部无线波通讯定位方法，其特征在于：所述步骤S110中，包括：
步骤S1101、将n个粒子的权重生成n个依次连续的区间，每个区间的大小与每个粒子的权重值呈正比；
步骤S1102、在所有区间的最大值和最小值之间生成n个随机数；
步骤S1103、保留包含n个所述随机数的区间所对应的所有粒子的坐标(x，y)。
3.根据权利要求1所述的局部无线波通讯定位方法，其特征在于：所述步骤S105中n等于100，或者等于1000，或者等于10000。
4.根据权利要求1所述的局部无线波通讯定位方法，其特征在于：所述步骤S101中的间距L1、距离为L4以及离地高度h都是预先测得的，
第一距离L2是根据所述定位通讯传感器与第一定位通讯单元的信号强弱实时测得的，
第二距离L3是根据所述定位通讯传感器与第二定位通讯单元的信号强弱实时测得的。
5.根据权利要求1所述的局部无线波通讯定位方法，其特征在于：所述步骤S103中夹角θ1是通过车辆上的转弯角度传感器实时获得的，夹角θ2是预设的。
6.根据权利要求1所述的局部无线波通讯定位方法，其特征在于：所述步骤S105中所述粒子的X轴坐标越接近当前粒子在X轴上随机分布之前的坐标值μx，则所述粒子基于X轴坐标的概率f(x)越大；
所述粒子的Y轴坐标越接近当前粒子在Y轴上随机分布之前的坐标值μy，则所述粒子基于Y轴坐标的概率f(y)越大。
7.根据权利要求1所述的局部无线波通讯定位方法，其特征在于：所述步骤S105中粒子的X轴坐标的概率f(x)从负无穷到正无穷的积分为1，粒子的Y轴坐标的概率f(y)从负无穷到正无穷的积分为1。
8.一种局部无线波通讯定位系统，用于实现权利要求1至7中任一项所述的局部无线波通讯定位方法，其特征在于，包括：
测量模块，所述车辆设有一定位通讯传感器，通过第一定位通讯单元和第二定位通讯单元分别实时与所述定位通讯传感器进行通讯，获得所述定位通讯传感器到第一定位通讯单元的第一距离L2和第二定位通讯单元的第二距离L3，测得所述第一定位通讯单元和第二定位通讯单元的间距为L1、所述定位通讯传感器与所述车辆的中心的距离为L4、所述定位通讯传感器的离地高度为h；
立体坐标系模块，以第一定位通讯单元为坐标系原点，建立空间坐标系，第二定位通讯单元位于Y轴，获得所述定位通讯传感器的坐标为(xm，ym，h)，其中，ym＝(L12+L32-L22)/(2*L1)；
Figure FDA0002681856380000031
观测方程生成模块，以第一定位通讯单元为坐标系原点，建立平面坐标系，第二定位通讯单元位于Y轴，设所述定位通讯传感器与车辆的中心的连线与Y轴方向的夹角为θ1，所述车辆的行驶方向与Y轴方向的夹角为θ2；
根据观测方程：
Figure FDA0002681856380000041
获得在平面坐标内所述车辆的中心的坐标(xc，yc)；
预测方程生成模块，建立所述平面坐标系中表示车辆的中心在预测方程的预测坐标(xt，yt)：
Figure FDA0002681856380000042
其中；xt为t时刻的X轴坐标，xt-1为t-1时刻的X轴坐标，yt为t时刻的Y轴坐标，yt-1为t-1时刻的Y轴坐标；
粒子分布模块，在当前时刻根据所述观测方程得到的所述车辆的中心的坐标(xc，yc)根据正态分布公式随机分布n个粒子，所述粒子的坐标为(x，y)，每一个粒子的权重为
Figure FDA0002681856380000043
所述正态分布公式为
Figure FDA0002681856380000044
其中，f(x)代表是粒子基于X轴坐标的概率，其中的μx代表的是当前粒子在X轴上随机分布之前的坐标值，σx代表的是该粒子在X轴上随机分布的标准差stdx；
f(y)代表是粒子基于Y轴坐标的概率，其中的μy代表的是当前粒子在Y轴上随机分布之前的坐标值，σy代表的是该粒子在Y轴上随机分布的标准差stdy；
预测方程计算模块，根据预测方程
Figure FDA0002681856380000045
获得t时刻下n个粒子各自的预测坐标(xt，yt)；
观测方程生成模块，根据观测方程
Figure FDA0002681856380000046
获得t时刻下n个粒子各自的观测坐标(xc，yc)；
权重更新模块，根据权重更新公式：
Wt＝e(-0.5*((xc-xt)2/stdx2+(yc-yt)2/stdy2))/(2*π*stdx*stdy)*Wt-1更新n个粒子的权重；
预测坐标输出模块，将权重的值最大的粒子的坐标(x，y)作为t时刻下车辆的中心的预测坐标；
粒子重采样模块，将n个粒子根据权重进行重采样；
粒子权重恢复模块，将n个粒子的权重全部恢复为
Figure FDA0002681856380000051
返回粒子分布模块，进入t+1时刻。
9.一种局部无线波通讯定位设备，其特征在于，包括：
处理器；
存储器，其中存储有处理器的可执行指令；
其中，处理器配置为经由执行可执行指令来执行权利要求1至7中任意一项局部无线波通讯定位方法的步骤。
10.一种计算机可读存储介质，用于存储程序，其特征在于，程序被执行时实现权利要求1至7中任意一项局部无线波通讯定位方法的步骤。
CN201810716624.1A 2018-07-03 2018-07-03 局部无线波通讯定位方法、系统、设备及存储介质 Active CN109085533B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201810716624.1A CN109085533B (zh) 2018-07-03 2018-07-03 局部无线波通讯定位方法、系统、设备及存储介质

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201810716624.1A CN109085533B (zh) 2018-07-03 2018-07-03 局部无线波通讯定位方法、系统、设备及存储介质

Publications (2)

Publication Number Publication Date
CN109085533A CN109085533A (zh) 2018-12-25
CN109085533B true CN109085533B (zh) 2020-11-06

Family

ID=64837153

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201810716624.1A Active CN109085533B (zh) 2018-07-03 2018-07-03 局部无线波通讯定位方法、系统、设备及存储介质

Country Status (1)

Country Link
CN (1) CN109085533B (zh)

Family Cites Families (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101509969A (zh) * 2009-03-31 2009-08-19 江苏大学 联合非视距误差消除和运动状态估计的无线定位方法
CN103176164B (zh) * 2013-04-11 2016-01-13 北京空间飞行器总体设计部 基于无线传感器网络的多目标无源跟踪方法
JP6291873B2 (ja) * 2014-01-31 2018-03-14 株式会社デンソー 無線測位装置
US9247381B2 (en) * 2014-03-24 2016-01-26 Qualcomm Incorporated System, method and devices for delivering positioning assistance data
JP6330471B2 (ja) * 2014-05-16 2018-05-30 株式会社デンソー 無線測位装置
CN106093857B (zh) * 2016-06-15 2018-10-12 吉林大学 一种基于螺旋线的移动锚节点wsn辅助定位方法

Also Published As

Publication number Publication date
CN109085533A (zh) 2018-12-25

Similar Documents

Publication Publication Date Title
CN109031269B (zh) 基于毫米波雷达的定位方法、系统、设备及存储介质
CN112560680A (zh) 车道线处理方法、装置、电子设备及存储介质
CN103913162A (zh) 增强的移动平台定位
IL255050A (en) Autonomous vehicle control
CN111401779B (zh) 机器人的定位部署方法、装置、设备及存储介质
CN108876857B (zh) 无人驾驶车辆的定位方法、系统、设备及存储介质
CN110782092A (zh) 无人配送车在非结构化场景中的轨迹规划方法及装置
CN109737971B (zh) 车载辅助导航定位系统、方法、设备及存储介质
CN112466111B (zh) 一种车辆行驶控制方法、装置、存储介质及电子设备
CN110988949A (zh) 定位方法、定位装置、计算机可读存储介质与可移动设备
CN112849125A (zh) 打滑检测控制方法、装置、移动机器人及存储介质
CN116224396A (zh) 基于点云的岸桥区域导航方法、系统、设备及存储介质
CN112847434B (zh) 机器人底盘的控制方法、装置、机器人底盘及存储介质
CN114911221B (zh) 机器人的控制方法、装置及机器人
CN109085533B (zh) 局部无线波通讯定位方法、系统、设备及存储介质
CN116661450A (zh) 一种移动机器人的避障方法、装置、电子设备及存储介质
CN114234982B (zh) 基于方位角定位的三维轨迹规划方法、系统、设备及介质
CN109426275B (zh) 虚拟禁飞区设置及基于虚拟禁飞区的飞行控制方法、装置
CN115565374A (zh) 物流车驾驶优化方法、装置、电子设备及可读存储介质
CN112729349B (zh) 里程计在线标定的方法、装置、电子设备及存储介质
CN115032672A (zh) 一种基于定位子系统的融合定位方法及系统
CN113483770A (zh) 封闭场景下的路径规划方法、装置、电子设备、存储介质
CN115704688A (zh) 高精地图数据相对位置精度评估方法、系统、介质及终端
Bolognini et al. A fault-tolerant automatic mission planner for a fleet of aerial vehicles
CN114485708B (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
CP01 Change in the name or title of a patent holder
CP01 Change in the name or title of a patent holder

Address after: Room 503-3, 398 Jiangsu Road, Changning District, Shanghai 200050

Patentee after: Shanghai Xijing Technology Co.,Ltd.

Address before: Room 503-3, 398 Jiangsu Road, Changning District, Shanghai 200050

Patentee before: SHANGHAI WESTWELL INFORMATION AND TECHNOLOGY Co.,Ltd.