[发明专利]一种协同定位方法及系统、定位设备、存储介质

权利要求书

1.一种基于三维地图辅助GNSS的协同定位方法，其特征在于，包括：

根据道路使用者的原始GNSS数据确定所述道路使用者的绝对位置；

基于数字地形模型与实际场景的映射关系，在所述绝对位置附近为所述道路使用者选取一网格组，所述网格组包含m个网格，m为大于1的自然数；

校正所述网格的伪距的非视距NLOS误差得到所述网格的伪距修正值；

对于任意两个道路使用者，将一个道路使用者的网格组内每个网格分别与另一个道路使用者的网格组内各个网格分别配对，得到m×m个网格对；

根据所述伪距修正值计算所述网格对中网格之间的相对距离包括对所述网格对中的两个网格的伪距修正值使用双差分法得到双差分值，对所述双差分值线性化，得到视距LOS矩阵A，通过对矩阵A的伪逆运算得到所述网格对的相对距离

计算网格对的后验概率Λ(n_a,n_b)，包括计算所有网格对的相对距离的测量误差，确定所述测量误差的最大值及最小值，利用所述最大值及最小值计算网格对的后验概率Λ(n_a,n_b)；

利用所述m×m个网格对各自的后验概率Λ(n_a,n_b)，计算所述m×m个网格对的相对距离的加权平均值，得到所述两个道路使用者的相对距离，其中下标na和n_b表示属于不同网格组的两个网格；

针对所述两个道路使用者中的任意一个道路使用者，构建因子图，利用所述道路使用者的绝对位置和所述两个道路使用者的相对距离表示所述因子图的约束，利用GNSS定位误差预测地图评估每个约束的不确定度，优化所述因子图并输出所述道路使用者的定位结果；

优化因子图并输出所述道路使用者的定位结果包括根据代价函数计算各约束的函数值，所述函数值构成所述因子图的优化目标函数，利用Levenberg-Marquardt优化法得到所述定位结果。

2.根据权利要求1所述的基于三维地图辅助GNSS的协同定位方法，其特征在于，构建所述GNSS定位误差预测地图的步骤包括：

基于所述数字地形模型在地平面选取一网格组，所述网格组包括多个网格，利用光线追踪算法模拟卫星到所述网格的视距LOS距离及非视距NLOS距离，利用模拟的所述LOS距离及所述NLOS距离预测在给定时间内每个网格的GNSS定位误差，并对所述网格进行单点定位，得到所述GNSS定位误差预测地图。

3.根据权利要求1所述的基于三维地图辅助GNSS的协同定位方法，其特征在于，

所述道路使用者的绝对位置利用光线追踪算法获得。

4.根据权利要求1所述的基于三维地图辅助GNSS的协同定位方法，其特征在于，所述校正所述网格的伪距的非视距NLOS误差得到所述网格的伪距修正值包括：

计算卫星到网格的伪距NLOS校正因子根据所述校正因子计算所述网格的伪距修正值其中上标i表示第i个卫星，下标n表示第n个网格，n＝1,2,3...,m，m为网格组内网格的个数。

5.根据权利要求1所述的基于三维地图辅助GNSS的协同定位方法，其特征在于，所述伪距修正值包括主、从卫星到网格的伪距修正值。

6.根据权利要求5所述的基于三维地图辅助GNSS的协同定位方法，其特征在于，

所述主卫星包括最大仰角的卫星。

7.根据权利要求1所述的基于三维地图辅助GNSS的协同定位方法，其特征在于，所述利用所述m×m个网格对各自的后验概率Λ(n_a,n_b)，计算所述m×m个网格对的相对距离的加权平均值，得到所述两个道路使用者的相对距离之前还包括：

把m×m个网格对按照后验概率从大至小排序，根据设定比例筛选出部分网格对。