[发明专利]基于卫星导航定位系统的因子图协同定位方法

摘要

本发明设计了一种基于因子图的卫星导航协同定位方法，结合了卫星导航定位可以直接得到定位结果的优点和因子图可以快速融合数据的优点。该方法主要包括卫星导航解算，协同节点数据交互和协同定位解算三个部分。卫星导航解算模块位于协同定位节点，主要完成所有协同节点卫星导航信号的解算，得到卫星导航坐标。协同节点数据交互模块也位于协同定位节点，主要用于完成该协同节点和其他节点之间的距离测量和卫星导航定位坐标的交换。协同定位解算模块也位于协同定位节点，主要是将所接收得到其他协同定位节点的卫星导航坐标结合测距信息在协同定位节点和自身所得到的卫星导航坐标实现融合，从而减小由于卫星轨道偏差、多径传播误差等造成卫星导航定位精度的下降。

主权项

1.一种基于卫星导航定位系统的因子图协同定位方法，其特征在于：包括以下步骤：步骤1：各个协同定位节点对接收到的卫星信号进行解算，得到各个协同定位节点自身的位置坐标；步骤2：协同定位节点之间进行数据交互：对于某一协同定位节点，通过数据交互得到其视距范围内其他协同定位节点的位置坐标，并解算自身与其视距范围内其他协同定位节点的测距信息；步骤3：根据步骤2得到的测距信息，依据因子图理论建立各个协同定位节点的因子图；所述因子图包括变量节点和函数节点，因子图的每条边均连接一个变量节点和一个函数节点；采用协同定位节点Ti作为变量节点，协同定位节点与其视距范围内其他协同定位节点之间具有函数节点gj；确定总体定位最优代价函数S为每个协同定位节点代价函数的求和形式；其中Sk为第k个协同定位节点代价函数，K为协同定位节点总数，ωki为第k个协同定位节点和第i个协同定位节点之间的权重因子，lki为根据步骤2得到的第k个协同定位节点和第i个协同定位节点之间的测距信息，第k个协同定位节点以及第i个协同定位节点的坐标为pk＝[xk,yk,zk]T，pi＝[xi,yi,zi]T；ωki采用归一化后的信度信息：其中max(BI)为因子图中两个协同定位节点之间传递的信度信息的最大值；BI(Tk,Ti)为因子图中第k个协同定位节点Tk传递到第i个协同定位节点Ti的信度信息；步骤4：对步骤3建立的总体定位最优代价函数S，通过总体最小二乘方法实现局部代价函数Sk构成的K个子优化问题的求解，所求解得到最优化协同定位节点坐标即为全局条件估计得到协同定位节点的最优定位坐标。