[0001] 本发明涉及一种用于卫星导航定位系统的因子图协同定位方法，特别涉及一种基于因子图(Factor graph)理论的协同定位(Cooperative location)卫星导航定位系统方法。

[0002] 随着智慧城市的发展，导航定位逐渐成为人类生活中必不可少的元素。现有的卫星导航精度的每一步提升都是极为困难的。自动车辆驾驶，无人机运输等智慧城市所需的服务都需要高精度导航定位服务的支持，而现有的导航定位主要依赖于卫星导航，但是卫星导航由于卫星轨道误差，卫星到地面的通信链路较长等客观因素，在导航定位精度上已经接近极限。

[0003] 为了进一步提高卫星导航定位系统的定位精度，现有的卫星导航定位增强技术主要通过提高卫星导航信号质量，增强卫星导航信号强度，地图匹配以及地面基站辅助等技术来实现卫星导航定位精度的提升。这些技术在人烟稀少区域可以显著提高定位精度。但在城市环境中，由于多径效应，建筑物高层效应等影响，很难在实质上进一步提高病态无线环境下的定位精度。

[0004] 在城市环境中，由于定位终端用户较多，通过多用户终端之间的协同定位进一步提高定位精度在理论上是切实可行的。目前主要的基于协同定位的卫星导航方法有基于无线传感器网络的协同定位方法，半定规划协同定位方法，二阶锥优化协同定位方法和准线性优化的协同定位方法等。其中，基于无线传感器网络的协同定位方法通过参考节点在无线网络中广播自身位置，待定位的用户终端计算其接收到广播信号的参考点的质心，将此作为终端位置的估计，但参考节点的定位精度对整体的性能影响很大。半定规划协同定位方法通过半定规划求解所有协同节点的最优点，在不同拓扑的网络中都可以取得较高的鲁棒性，但未考虑传感器用户自身定位模糊度的问题。阶锥优化协同定位方法以牺牲一定的定位精度，换取更快的计算速度，但未考虑测距误差对定位结果的影响，当测距误差较大时，会造成定位精度的快速下降。准线性优化的协同定位方法同时考虑了节点之间的协同与非视距误差的影响，然而该算法由于计算复杂度较高，所以导航定位的性能提升十分有限，无法满足导航定位的需要。

[0025] 下面详细描述本发明的实施例，所述实施例是示例性的，旨在用于解释本发明，而不能理解为对本发明的限制。

[0026] 本实施例的整体思路是：利用卫星导航解算模块完成协同定位节点卫星导航信号的解算，得到卫星导航坐标。其次，利用协同节点数据交互模块完成协同定位节点和其他节点之间的距离测量和卫星导航定位坐标的交换。最后，通过总体最小二乘求解得到协同定位节点的最优值，提高卫星导航定位的精度。

[0027] 具体步骤如下：

[0028] 步骤1：各个协同定位节点对接收到的卫星信号进行解算，得到各个协同定位节点自身的位置坐标。

[0029] 导航卫星的自身坐标为Xi，Yi，Zi，其中，i表示导航卫星编号，可以构建导航卫星求解方程如下：

[0030] (Xi-x)2+(Yi-y)2+(Zi-z)2＝Ri+n  (1)

[0031] 式中，x，y，z表示协同定位节点自身定位目标，n表示导航卫星钟差，Ri表示节点到第i个卫星的测距信息。在上式中，由于协同定位节点自身定位坐标和导航卫星钟差为未知的，而导航卫星自身坐标和测距信息都是可以直接得到，所以需要4颗导航卫星构建联立方程完成协同定位节点坐标的解算。

[0032] 步骤2：协同定位节点之间进行数据交互：对于某一协同定位节点，通过数据交互得到其视距范围内其他协同定位节点的位置坐标，并解算自身与其视距范围内其他协同定位节点的测距信息。

[0033] 第k个协同定位节点和第i个协同定位节点之间的距离可以表示为：

[0034]

[0035] 接下来就可以根据协同定位节点的定位坐标，建立包含所有协同定位节点的协同网络，将与每一个协同定位节点具有可视条件的所有其他协同定位节点之间建立协同网络连接，该链接用变量函数gj表示。

[0036] 步骤3：根据步骤2得到的测距信息，依据因子图理论建立各个协同定位节点的因子图；所述因子图包括变量节点和函数节点，因子图的每条边均连接一个变量节点和一个函数节点；采用协同定位节点Ti作为变量节点，协同定位节点与其视距范围内其他协同定位节点之间具有函数节点gj；

[0037] 确定总体定位最优代价函数S为每个协同定位节点代价函数的求和形式；

[0038]

[0039] 其中Sk为第k个协同定位节点代价函数，K为协同定位节点总数，ωki为第k个协同定位节点和第i个协同定位节点之间的权重因子，lki为根据步骤2得到的第k个协同定位节点和第i个协同定位节点之间的测距信息，第k个协同定位节点以及第i个协同定位节点的坐标为pk＝[xk，yk，zk]T，pi＝[xi，yi，zi]T；

[0040] ωki采用归一化后的信度信息：

[0041]

[0042] 其中max(BI)为因子图中两个协同定位节点之间传递的信度信息的最大值；BI(Tk，Ti)为因子图中第k个协同定位节点Tk传递到第i个协同定位节点Ti的信度信息。

[0043] 由于受到协同定位节点定位信息准确度的影响，在分布式协同定位中难以得到最优解。本发明从参数估计理论中引入基于因子图元素加权的总体最小二乘的概念。

[0044] 因子图可以将一个复杂度多元的全局函数问题拆分为多个简单的本地子问题的积。因子图包括两种节点：变量节点和函数节点。每条边都连接一个变量节点和一个函数节点gj。变量节点在协同定位中表示协同定位节点Ti，函数节点代表着一个因子图所拆分的局部函数。因子图理论中，通过“信度信息”在变量节点和函数节点之间的迭代传递求解全局函数的最优值，而这一传递是通过和积算法实现的。信度信息是指描述相关随机变量的均值和标准差等的信息。在协同定位中主要通过协同定位节点之间的测量距离和其自身定位误差所决定。基于因子图的协同定位方法结构如图1所示。

[0045] 从协同定位节点传递到函数节点的信度信息是所有来自其他邻居函数节点到达该协同定位节点的信度信息的乘积。例如，由协同定位节点T3传递给函数节点g2的信度信息表示：

[0046] BI(T3，g2)＝BI(g3，T3)BI(g4，T3)  (3)

[0047] 式中，BI(g3，T3)表示函数节点g3传递给协同定位节点T3的信度信息，BI(g4，T3)表示函数节点g4传递给协同定位节点T3的信度信息。

[0048] 由函数节点传递到协同定位节点的信息是所有协同定位节点传递给该函数节点的信度信息与本地变量的函数的乘积。例如，由函数节点g2传递给协同定位节点T3的信度信息表示：

[0049] BI(g2，T3)＝BI(T1，g2)BI(T4，g2)g2(T1，T4)  (4)

[0050] 式中，BI(T1，g2)表示协同定位节点T1传递给函数节点g2的信度信息，BI(T4，g2)表示协同定位节点T4传递给函数节点g2的信度信息，g2(T1，T4)表示函数节点g2的本地变量函数，在本发明中该变量函数采用信号传播衰减模型，服从高斯分布。

[0051] 每个协同定位节点的信度信息可以表示为所有与该协同定位节点有连接的函数节点传递到该协同定位节点的信度信息乘积。例如，协同定位节点T3的信度信息表示：

[0052] BI(T3)＝BI(g2，T3)BI(g3，T3)BI(g4，T3)  (5)

[0053] 由此可以得到协同定位节点Ti传递到协同定位节点Tj的信度信息为所经过的所有函数节点和协同定位节点的信度信息乘积。例如，协同定位节点T2传递到协同定位节点T3的信度信息表示(从协同定位节点T2到协同定位节点T3之间各个函数节点与协同定位节点之间信度信息的乘积以及与协同定位节点T2本身的信度信息的乘积)

[0054] BI(T2，T3)＝BI(T2)BI(T2，g1)BI(g1，T1)BI(T1，g2)BI(g2，T3)  (6)[0055] 由于因子图的信度信息最大值为1，但当协同定位节点数量较多时，会导致每个协同定位节点的信度信息实际值较小，从而引入较大的计算误差。所以权重因子ωki采用归一化后的信度信息

[0056]

[0057] 确定总体定位最优代价函数S为每个协同定位节点代价函数的求和形式；

[0058]

[0059] 步骤4：对步骤3建立的总体定位最优代价函数S，通过总体最小二乘方法实现局部代价函数Sk构成的K个子优化问题的求解，所求解得到最优化协同定位节点坐标即为全局条件估计得到协同定位节点的最优定位坐标。

[0060] 性能分析

[0061] 1、距离测量误差性能分析

[0062] 在协同定位中，定位精度主要受到协同定位误差和测距误差的影响。而距离测量误差会显著影响协同定位的结果。将本发明所提出的方法和基于测量距离辅助的协同定位方法，半定规划协同定位方法，二阶锥优化协同定位方法和准线性规划的协同定位方法进行仿真比较，仿真中距离测量相互独立，网络拓扑如图1所示，网络拓扑半径为5km。理想条件下，协同定位节点定位误差的标准差为0m，距离测量的标准差(Standard Deviation，标准差)从0到70m。仿真结果经过10次迭代稳定后的1000次独立仿真其结果如图2所示。

[0063] 从图2中可以看出，随着距离测量的标准差的增大，所有的协同定位方法的最小均方误差均会变大，但是，基于测量距离辅助的协同定位方法和半定规划协同定位方法的方法具有明显的增加，当测距的标准差达到40米时，协同定位最小均方误差的误差分别达到90m和80m。而二阶锥优化协同定位方法，准线性规划的协同定位方法和本发明方法，均能有效控制最小均方误差的增加，当测距的标准差达到40米时，协同定位最小均方误差的误差分别为45m、50m和40m。说明本发明所提出的协同定位方法可以有效减小测距误差对定位结果的影响。

[0064] 2、协同定位误差性能分析

[0065] 在协同定位中，定位精度除了收到测距误差的影响，协同定位误差也会对影响协同定位的精度产生明显影响。在测距误差为0m的理想条件下，定位误差的标准差从0到50m。将将本发明所提出的方法和基于测量距离辅助的协同定位方法，半定规划协同定位方法，二阶锥优化协同定位方法和准线性规划的协同定位方法进行仿真比较，仿真结果经过10次迭代稳定后的1000次独立仿真其结果如图3所示。

[0066] 从图3中可以看出，随着定位误差的标准差的增大，所有的协同定位方法的最小均方误差均会变大，但是，基于测量距离辅助的协同定位方法和二阶锥优化协同定位方法的方法具有明显的增加，当定位误差的标准差达到30米时，协同定位最小均方误差的误差分别达到75m和45m。而半定规划协同定位方法，准线性规划的协同定位方法和本发明方法，均能有效控制最小均方误差的增加，当定位误差的标准差达到30米时，协同定位最小均方误差的误差分别为22m、22m和18m。说明本发明所提出的协同定位方法可以有效减小定位误差对协同定位结果的影响。

[0067] 尽管上面已经示出和描述了本发明的实施例，可以理解的是，上述实施例是示例性的，不能理解为对本发明的限制，本领域的普通技术人员在不脱离本发明的原理和宗旨的情况下在本发明的范围内可以对上述实施例进行变化、修改、替换和变型。