[发明专利]局部无线波通讯定位方法、系统、设备及存储介质

摘要

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

主权项

1.一种局部无线波通讯定位方法，其特征在于，包括以下步骤：S101、一车辆设有一定位通讯传感器，通过第一定位通讯单元和第二定位通讯单元分别实时与所述定位通讯传感器进行通讯，获得所述定位通讯传感器到第一定位通讯单元的第一距离L2和第二定位通讯单元的第二距离L3，测得所述第一定位通讯单元和第二定位通讯单元的间距为L1、所述定位通讯传感器与所述车辆的中心的距离为L4、所述定位通讯传感器的离地高度为h；S102、以第一定位通讯单元为坐标系原点，建立空间坐标系，第二定位通讯单元位于Y轴，获得所述定位通讯传感器的坐标为(x_m，y_m，h)，其中，y_m＝(L1²+L3²‑L2²)/(2*L1)；S103、以第一定位通讯单元为坐标系原点，建立平面坐标系，第二定位通讯单元位于Y轴，设所述定位通讯传感器与车辆的中心的连线与Y轴方向的夹角为θ1，所述车辆的行驶方向与Y轴方向的夹角为θ2；根据观测方程：获得在平面坐标内所述车辆的中心的坐标(x_c，y_c)；S104、建立所述平面坐标系中表示车辆的中心在预测方程：其中；x_t为t时刻的X轴坐标，x_t‑1为t‑1时刻的X轴坐标，y_t为t时刻的Y轴坐标，y_t‑1为(t‑1)时刻的Y轴坐标；S105、在当前时刻根据所述观测方程得到的所述车辆的中心的坐标(x_c，y_c)根据正态分布公式随机分布n个粒子，所述粒子的坐标为(x，y)，每一个粒子的权重为所述正态分布公式为其中，f(x)代表是粒子基于X轴坐标的概率，其中的μx代表的是当前粒子在X轴上随机分布之前的坐标值，σx代表的是该粒子在X轴上随机分布的标准差stdx；f(y)代表是粒子基于Y轴坐标的概率，其中的μy代表的是当前粒子在Y轴上随机分布之前的坐标值，σy代表的是该粒子在Y轴上随机分布的标准差stdy；S106、根据预测方程获得t时刻下n个粒子各自的预测坐标(x_t，y_t)；S107、根据观测方程获得t时刻下n个粒子各自的观测坐标(x_c，y_c)；S108、根据权重更新公式：更新n个粒子的权重；S109、将权重的值最大的粒子的坐标(x，y)作为t时刻下车辆的中心的预测坐标；S110、将n个粒子根据权重进行重采样；以及S111、将n个粒子的权重全部恢复为返回步骤S105，进入(t+1)时刻。