技术领域
[0001] 本发明属于协同导航技术领域,具体涉及一种考虑了不完全量测的协同导航定位方法。
相关背景技术
[0002] 协同导航是协同组网编队中平台间通过导航信息交互利用，实时解算并修正自身位置、速度、姿态等导航信息，保障协同编队保持、队形重构及后续协同任务顺利完成的一种技术。如果无法得到协同平台的精确导航信息，或者获取的相对导航精度下降，则会导致编队的控制精度变差，任务执行效果也会下降甚至出现错误。
[0003] 贝叶斯框架下协同导航定位主要由绝对导航平台和相对导航平台构成。绝对导航GPS更新率较低，相对导航引入UWB，具有高频率高精度特点，可以达到GPS十倍的频率，同时UWB精度可到达厘米级，UWB的引入使得协同导航定位精度大大提升。但是UWB通过无线通信定位，当定位区域内的障碍物比较多，需要考虑多径效应和阴影效应的影响发生UWB量测数据随机性丢包问题。因此，研究不完全量测的协同导航定位技术，设计不完全量测的协同滤波器提高系统导航定位精度问题至关重要。
具体实施方式
[0016] 下面结合附图对本发明做更进一步的解释。
[0017] 根据下述实施例，可以更好的理解本发明。如附图1所示，本发明是一种不完全测量的协同导航定位方法，具体步骤如下：
[0018] 1、一种考虑了不完全量测的协同导航定位方法，其特征在于，该协同导航方法具体步骤如下：
[0019] 步骤一，将协同导航定位系统建模为多个移动节点和多个锚点，每个移动节点利用自身的惯性传感器进行捷联解算，用GPS信号修正INS解算的导航误差，并进行反馈修正；
[0020] 步骤二，协同导航定位组网通讯，如附图2所示，每个移动节点利用可观测的锚点以及其他移动节点的UWB相对测距进行协同导航定位；当组网通讯链路发生协同数据不完全测量时，根据系统协同模型建立不完全测量估计滤波器，进行传感器误差估计和速度位置补偿绝对导航滤波器，输出导航参数。
[0021] 2、步骤一所述利用GPS进行修正INS误差，具体模型如下：
[0022]
[0023] 式中：
[0024] 是状态向量, 是观测向量，f(·)和h(·)分别对应系统状态方程和观测方程；w(t-1)是系统过程噪声序列, 是系统观测噪声序列；下标k表示平台编号；上标t表示当前时刻。
[0025] 3、步骤二所述UWB相对测距进行协同导航定位，具体为：
[0026]
[0027] 协同导航系统利用导航平台的通讯链路进行组网并相对测距，交换平台导航信息，导航平台xk和通讯链路上邻居的协同平台的状态矢量为 组网邻居集Nk＝{l1，l2，…，l|Nk|，}，k∈{1,2,L,K},l∈{1,2,L,K}\{k},{k,l}∈ε，K表示协同导航平台总个数，协同导航滤波器建立贝叶斯估计的后验概率；
[0028]
[0029] 利用BP消息传递的因子图建立置信度近似逼近法，p表示消息交互迭代次数；
[0030]
[0031] 协同相对导航滤波器的测量值 简写为： 表示与导航平台k进行通讯的观测量；测量值由与导航平台k相关联对应平台l的测量数据构成：
[0032]
[0033] 简写为 满足g(xk,xl)＝g(xl,xk)。
[0034] 4、不完全量测的协同导航用测量值主要包含以下两种情形：
[0035] 情形1、导航平台k与锚点li(anchor)的UWB通讯测距测量值
[0036]
[0037] 其中 表示agent导航平台k经由INS解算或GPS/INS组合导航解算的位置与锚点(li)的相对测距，具体为：
[0038]
[0039] 其中 表示agent导航平台k经由UWB与锚点li的UWB测距，其中导航平台k经由UWB提供高精度的定位，可通过INS解算或GPS/INS组合导航解算的位置进行补偿后得到，具体为：
[0040]
[0041] 情形2、agent导航平台k的UWB与协同导航平台l的UWB通讯测距测量值；(k,l)∈ε,(l,k)∈ε
[0042]
[0043] 其中 表示agent导航平台k和l经由INS解算或GPS/INS组合导航解算的位置的相对测距，具体为：
[0044]
[0045] 其中 表示agent导航平台k与平台l经由UWB测距，导航平台经由UWB提供高精度的定位，可通过INS解算或GPS/INS组合导航解算的位置进行位置补偿后得到，具体为：
[0046]
[0047] 5、导航平台状态变量随机游走并且各节点之间独立同分布；
[0048]
[0049] 绝对导航与相对导航的测量值之间相互独立，并满足如下关系：
[0050]
[0051] 导航数据zk传输的过程中，尝试传输时有优先级高的节点在传输数据，传感器放弃本次发送；或zk已经发送，但在传输过程中丢失；这两种情况均表示数据包随机丢失，该不完全测量系统滤波器接收到的测量值 下标rel可省略，通过如下模型来描述：
[0052] y(n)＝(1-r(n))z(n)+r(n)z(n-1) n＝0:T,y(1)＝z(1) (8)
[0053] 其中z(n)∈Rm为系统理想测量输出；y(n)∈Rm为系统实际测量值；r(n)为满足Bernouli分布的序列，其取值为0和1，其不完全测量概率为：p(n)为
[0054]
[0055] 当r(n)＝0时，表示系统获得真实观测量，观测噪声为Rv；当rk＝1时，表示系统未获得真实观测量，系统用上一时刻的测量值来代替，因此协同导航系统不仅需要估计n时刻的x(n)还需要估计观测噪声v(n)。
[0056]
[0057]
[0058] 6、相对导航的测量独立同分布，与发生测量的两两节点相邻时刻的状态和噪声有关，其中下标l/k表示相邻导航平台编号，(l,k)∈ε，
[0059]
[0060] 表示平台k收到邻居平台j信息， 表示平台k收到平台j的相对测距观测量；
[0061] 定义相对导航的状态扩维： (11)简化为
[0062]
[0063] 7、协同导航定位滤波器用相对导航的置信度 估计逼近状态变量的pdf：
[0064]
[0065] 其中 表示第k个导航平台进行交互的所有协同状态变量，具体为表示与第k个平台相邻的第一个协同平台的状态变
量， 表示 除去 后的矢量；
[0066]
[0067] 其中
[0068]
[0069] 相对导航进行信息交互发生不确定随机丢包时，采用附图3所示如下方式完成相对定位
[0070]
[0071] 其中
[0072]
[0073] 其初值
[0074]
[0075] 若 则有 则(16)
[0076]
[0077] 将式(16)带入(17)
[0078]
[0079] 其中
[0080] 将式(16)带入(18)
[0081]
[0082] 其中(22)中 根据(15)确定，式(20)中 根据(21)确定， 中用 对应 更新导航平台信息
[0083] 8、不完全量测的协同导航方法，其特征在于：
[0084]
[0085] 9、(14)导航平台信息交互可以用均值和方差表示
[0086]
[0087]
[0088] 更新导航平台信息 可以用均值和方差表示。
[0089] 10、导航平台信息交互 估计，如附图4所示，将观测噪声序列作为状态变量扩维 并将连续系统离散化为 其
满足
[0090]
[0091] 其中
[0092]
[0093] 协同平台初值
[0094]
[0095]
[0096] 用(27)更新导航平台信息
[0097]
[0098] 以上实施例仅为说明本发明的技术思想，不能以此限定本发明的保护范围，凡是按照本发明提出的技术思想，在技术方案基础上所做的任何改动，均落入本发明保护范围之内。
