CN116794699A - 一种集群系统高精度相对导航定位信息确定方法 - Google Patents

Abstract

一种集群系统高精度相对导航定位信息确定方法，包括(1)使用集群系统挂载的设备获取测距、惯导、卫导传感器数据；(2)补偿相对速度带来的测距误差；(3)惯导/卫导定位解算，通过持续的航姿解算获得集群内初始的定位信息；(4)测距定位解算。根据步骤3得到的初步定位结果，结合测距信息，进行集群内相对测距定位解算修正；(5)对集群内所有个体按照步骤4进行测距定位解算，并求解测距定位的协方差矩阵作为置信度评估；(6)将基于测距信息的相对定位数据与惯导等定位数据进行融合，得到最终的集群系统高精度相对定位信息。本发明解决了现有GPS定位易受干扰，惯导定位误差漂移大的问题，提升了集群系统的相对定位精度。

Description

一种集群系统高精度相对导航定位信息确定方法

技术领域

本发明涉及一种集群系统高精度相对导航定位信息确定方法，尤其适用于无人机、无人车、无人船等集群系统在复杂电磁干扰，GPS失效情况下实现高精度集群间相对导航信息确定。

背景技术

集群系统采用集群的方式可以拓展单体系统的能力，高效合作协调完成任务，各单体之间互相功能备份，可以避免单体失效造成任务失败。集群系统一般都需要通过保持一定的编队队形来完成任务，且编队队形的精度可能会直接影响任务的完成与否。目前集群系统的导航信息可分为绝对导航与相对导航两种。绝对导航主要是对集群任务目标信息的表示，或者用于单机的导航控制，通常是基于地理坐标系，采用GPS或者GPS/惯导组合的方式得到的绝对导航信息。

而相对导航关注的则是集群之间的相对位置信息，在此基础上完成集群间协同任务，如协同目标定位、高精度编队队形保持、集群间避障等均需要集群间高精度相对定位信息。相对导航信息可以通过绝对导航信息差分来获取，但是由于传统的GPS、惯导绝对定位存在抗干扰性能不佳，随机漂移，定位精度不高的问题，因此根据集群特点在内部建立测量网络获得更高精度和稳定性的相对导航数据成为了研究热点。

数据链测距可以在小范围内建立稳定的测距网络，UWB超宽带测距可以获得厘米级的测量精度，因此可以获得相对GPS更高精度的相对导航定位数据。导航定位流程是首先在集群内部建立导航基准坐标系，以该基准为基础，集群之间相互完成测距，然后根据定位模型采用数值优化方法可以获得高精度的相对位置信息。

综述，目前飞行器多采用GPS/惯导组合导航系统，但受限于绝对定位精度，其不能满足高精度相对定位的要求且容易受干扰，在无GPS环境下不能实现相对定位，且集群间相对位置信息一般需要通过数据链传送同步，增加延迟，在占据数据链带宽同时还因为数据链的引入降低了定位信息获取的可靠性。因此，集群间高精度相对测距是实现高精度稳定相对定位的重要手段，研究集群系统高精度相对导航定位技术也显得尤为重要。

发明内容

本发明要解决的技术问题是：克服现有技术的不足，解决了无人机、无人车、无人船等集群系统编队队形精度不高的问题。

本发明目的通过以下技术方案予以实现：

一种集群系统高精度相对导航定位信息确定方法，包括：

S1、使用集群系统挂载的设备获取测距、惯导、卫导传感器数据；

S2、对测距过程的相对运动导致的误差进行建模补偿，并节点测距误差补偿到测距结果中；

S3、惯导/卫导定位解算；

S4、根据S3得到的初步定位结果，结合测距信息，进行集群内相对测距定位解算修正；

S5、对集群内所有个体按照S4步骤进行测距定位解算，并求解测距定位的协方差矩阵ζ₃作为置信度评估；

S6、将数据链测距相对定位获得的导航信息与惯导系统、卫导系统中的导航信息进行数据融合获得最终的导航信息。

优选的，卫导与惯导数据通过安装卫导和惯导设备直接获取，测距信息获取采用单程双向测距方法，以减少时钟和系统带来的误差。

优选的，所述的单程双向测距方法，采用3条消息进行测距消息的传播，降低测距的时钟不同步和系统误差。

优选的，惯导定位方式为：每个集群节点以其自带的惯性导航定位系统对当前t时刻位置进行定位并估算t+1时刻的位置信息nav₁；每个节点对自身t+1时刻的估算位置做衡量，并将协方差估计ζ₁保存下来作为置信度评估。

优选的，卫导定位方法：若卫导信号存在，则集群所有节点进行卫星辅助定位得到定位信息nav₂，并计算定位结果误差来评估卫星定位性能，将协方差ζ₂估计保存下来作为置信度评估。

优选的，S6步骤中，根据各导航系统误差协方差大小确定融合系数，将多套系统的数据进行融合。

本发明相比于现有技术具有如下有益效果：

相比于传统的惯导/卫导定位方法，本发明通过基于数据链测距和惯导/卫导组合的相对测量方法，结合集群间主动与被动信号获得相对定位信息经过组合滤波后实现集群间高精度相对导航，降低对于GPS等全局定位系统的依赖，提升了集群间相对导航精度。

附图说明

图1是本发明的相对导航定位信息确定方法流程图；

图2是本发明的测距信息传输流程图。

具体实施方式

为使本发明的目的、技术方案和优点更加清楚，下面将结合附图对本发明的实施方式作进一步详细描述。

图1是一种集群系统高精度相对导航定位确定方法流程图。该方法包括以下步骤：

S1、数据采集。使用集群系统挂载的设备获取测距、惯导、卫导传感器数据。其中卫导与惯导数据通过安装卫导和惯导设备直接获取，测距信息获取采用单程双向测距方法，以减少时钟和系统带来的误差。采用3条消息进行测距消息的传播，降低测距的时钟不同步和系统误差。如图2所示，测距消息的传播时间为T_fly，消息反馈间隔为T_reply。

S2、相对速度带来的测距误差补偿。由于测距节点存在相对运动速度，导致在测距间隔内引入了测距误差，对测距过程的相对运动导致的误差进行建模补偿，提升测距精度，节点测距误差补偿到测距结果中。测距误差e计算方法如下：

其中r₁₂是通过2次测距算出来的节点A、B之间距离，d₁和d₂分别是测距节点A、B初始时刻距离和经过t时间后相距距离，x₁是测距节点A收到第一次回复数据包时向节点B移动的距离，x₂是测距节点B收到第2次测距消息时节点A向节点B移动的距离。v是节点A、B的相对速度，T_reply是节点B回复测距数据的时间间隔。

S3、惯导/卫导定位解算。惯导定位：每个集群节点以其自带的惯性导航定位系统对当前t时刻位置进行定位并估算t+1时刻的位置信息nav₁。每个节点对自身t+1时刻的估算位置做衡量，并将协方差估计ζ₁保存下来作为置信度评估。

卫导定位：若卫导信号存在，则集群所有节点进行卫星辅助定位得到定位信息nav₂，并计算定位结果误差来评估卫星定位性能，同样将协方差ζ₂估计保存下来作为置信度评估。

S4、测距定位解算。根据S3得到的初步定位结果，结合测距信息，进行集群内相对测距定位解算修正。测距定位解算步骤如下：

对未知坐标为(x,y,z)的目标进行测距定位解算时，假设M+1个集群由惯导/卫导定位解算得到的坐标为(x_i,y_i,z_i)，i＝0,1,...,M。目标与第1个和第i个集群个体的距离差Δr_i为：

其中r_o和r_i分别是目标与第1个和第i个集群个体的测距距离。

带入中间变量距离d_i，令整理可得：

将上式改写为矩阵形式，如下：

其中，将上式写成数学表达式即AX＝r₀C+D，其中：

上述矩阵的解为AX＝C解的r₀倍和AX＝D解之和。当M＝3时满足：

其中，a_i为AX＝C的各个解，b_i为AX＝D的各个解。

在上述解中，r₀为未知参数，由于将解带入，有：

求解上述一元二次方程，根据求得r₀解的情况，可得到集群内测距导航坐标nav₃。

S5、对集群内所有个体按照S4步骤进行测距定位解算，并求解测距定位的协方差矩阵ζ₃作为置信度评估。ζ₃的求解方法如下:

dX＝[dx dy dz]^T

＝H^-1(dR-dS)＝(H^TH)^-1H^T(dR-dS)

ζ₃＝cov(dX)＝E[dXdX^T]

＝E[[(H^TH)^-1H^T(dR-dS)][(H^TH)^-1H^T(dR-dS)]^T]

＝E[[(H^TH)^-1H^T(dR-dS)][(dR^T-dS^T)(H(H^TH)^-1)]]

＝(H^TH)^-1H^TE[(dRdR^T)+(dRdS^T)]H(H^TH)^-1

＝(H^TH)^-1H^T[cov(dR)+cov(dS)H(H^TH)^-1]

其中cov表示求协方差，E表示求均值，dX是定位误差，dR是距离误差，dS是位置误差。

dR＝[dΔr₁ dΔr₁…dΔr_M]^T

dS＝[k₁-k₀ k₂-k₀...k_M-k₀]^T

以下是中间变量：

S6、多定位系统数据融合。将数据链测距相对定位获得的导航信息与惯导系统、卫导系统中的导航信息进行数据融合获得最终的导航信息。根据各导航系统误差协方差大小确定融合系数，将多套系统的数据进行融合。融合方法如下:

采用协方差加权得到最终的导航信息。协方差加权的方法如下：

式中，i＝1,2,3分别代表惯导系统、卫导系统、数据链测距导航系统；ζ_i表示系统i的导航数据协方差；c_i为导航数据融合的权重系数；nav_i，i＝1,2,3分别表示为惯导系统、卫导系统的定位数据、数据链测距导航系统；nav_out为协方差加权融合后导航数据。

本发明说明书中未作详细描述的内容属本领域技术人员的公知技术。

本发明虽然已以较佳实施例公开如上，但其并不是用来限定本发明，任何本领域技术人员在不脱离本发明的精神和范围内，都可以利用上述揭示的方法和技术内容对本发明技术方案做出可能的变动和修改，因此，凡是未脱离本发明技术方案的内容，依据本发明的技术实质对以上实施例所作的任何简单修改、等同变化及修饰，均属于本发明技术方案的保护范围。

Claims (10)

1.一种集群系统高精度相对导航定位信息确定方法，其特征在于，包括：

S1、使用集群系统挂载的设备获取测距、惯导、卫导传感器数据；

S2、对测距过程的相对运动导致的误差进行建模补偿，并节点测距误差补偿到测距结果中；

S3、惯导/卫导定位解算；

S4、根据S3得到的初步定位结果，结合测距信息，进行集群内相对测距定位解算修正；

S5、对集群内所有个体按照S4步骤进行测距定位解算，并求解测距定位的协方差矩阵ζ₃作为置信度评估；

S6、将数据链测距相对定位获得的导航信息与惯导系统、卫导系统中的导航信息进行数据融合获得最终的导航信息。

2.根据权利要求1所述的相对导航定位信息确定方法，其特征在于，卫导与惯导数据通过安装卫导和惯导设备直接获取，测距信息获取采用单程双向测距方法，以减少时钟和系统带来的误差。

3.根据权利要求2所述的相对导航定位信息确定方法，其特征在于，所述的单程双向测距方法，采用3条消息进行测距消息的传播，降低测距的时钟不同步和系统误差。

4.根据权利要求1所述的相对导航定位信息确定方法，其特征在于，惯导定位方式为：每个集群节点以其自带的惯性导航定位系统对当前t时刻位置进行定位并估算t+1时刻的位置信息nav₁；每个节点对自身t+1时刻的估算位置做衡量，并将协方差估计ζ₁保存下来作为置信度评估。

5.根据权利要求1所述的相对导航定位信息确定方法，其特征在于，卫导定位方法：若卫导信号存在，则集群所有节点进行卫星辅助定位得到定位信息nav₂，并计算定位结果误差来评估卫星定位性能，将协方差ζ₂估计保存下来作为置信度评估。

6.根据权利要求1所述的相对导航定位信息确定方法，其特征在于，S6步骤中，根据各导航系统误差协方差大小确定融合系数，将多套系统的数据进行融合。

7.根据权利要求1至6中任一项所述的相对导航定位信息确定方法，其特征在于，测距误差e计算方法如下：

其中r₁₂是通过2次测距算出来的节点A、B之间距离，d₁和d₂分别是测距节点A、B初始时刻距离和经过t时间后相距距离，x₁是测距节点A收到第一次回复数据包时向节点B移动的距离，x₂是测距节点B收到第2次测距消息时节点A向节点B移动的距离；v是节点A、B的相对速度，T_reply是节点B回复测距数据的时间间隔。

8.根据权利要求1至6中任一项所述的相对导航定位信息确定方法，其特征在于，测距定位解算步骤如下：

对未知坐标为(x,y,z)的目标进行测距定位解算时，假设M+1个集群由惯导/卫导定位解算得到的坐标为(x_i,y_i,z_i)，i＝0,1,...,M；目标与第1个和第i个集群个体的距离差Δr_i为：

其中r_o和r_i分别是目标与第1个和第i个集群个体的测距距离；

带入中间变量距离d_i，令整理可得：

将上式改写为矩阵形式，如下：

其中，将上式写成数学表达式即AX＝r₀C+D，其中：

上述矩阵的解为AX＝C解的r₀倍和AX＝D解之和；当M＝3时满足：

其中，a_i为AX＝C的各个解，b_i为AX＝D的各个解；

在上述解中，r₀为未知参数，由于将解带入，有：

求解上述一元二次方程，根据求得r₀解的情况，可得到集群内测距导航坐标nav₃。

9.根据权利要求1至6中任一项所述的相对导航定位信息确定方法，其特征在于，求解测距定位的协方差矩阵ζ₃作为置信度评估，ζ₃的求解方法如下：

dX＝[dx dy dz]^T

＝H^-1(dR-dS)＝(H^TH)^-1H^T(dR-dS)

ζ₃＝cov(dX)＝E[dXdX^T]

＝E[[(H^TH)^-1H^T(dR-dS)][(H^TH)^-1H^T(dR-dS)]^T]

＝E[[(H^TH)^-1H^T(dR-dS)][(dR^T-dS^T)(H(H^TH)^-1)]]

＝(H^TH)^-1H^TE[(dRdR^T)+(dRdS^T)]H(H^TH)^-1

＝(H^TH)^-1H^T[cov(dR)+cov(dS)H(H^TH)^-1]

其中cov表示求协方差，E表示求均值，dX是定位误差，dR是距离误差，dS是位置误差。

dR＝[dΔr₁ dΔr₁…dΔr_M]^T

dS＝[k₁-k₀ k₂-k₀...k_M-k₀]^T

以下是中间变量：

10.根据权利要求1至6中任一项所述的相对导航定位信息确定方法，其特征在于，采用协方差加权得到最终的导航信息，协方差加权的方法如下：

式中，i＝1,2,3分别代表惯导系统、卫导系统、数据链测距导航系统；ζ_i表示系统i的导航数据协方差；c_i为导航数据融合的权重系数；nav_i，i＝1,2,3分别表示为惯导系统、卫导系统的定位数据、数据链测距导航系统；nav_out为协方差加权融合后导航数据。