基于5G信号异步时钟误差补偿的多无人机协同定位方法技术

本发明专利技术提供了基于5G信号异步时钟误差补偿的多无人机协同定位方法，属于无人机导航领域，主要包括以下过程：通过机载设备，从机节点周期性读取t时刻机载陀螺仪和加速度计信息，并通过5G收发装置实时获得主机节点位置信息以及与主机节点的测距、测角信息；从机节点预测t时刻的姿态四元数、速度和位置信息；基于5G信号异步时钟误差建立相对测距模型；利用主机的位置信息、主从机节点之间的测距与测角信息、气压高度计信息，构建协同定位状态方程和量测方程；利用卡尔曼滤波器对从机节点导航状态与主机和从机节点之间的异步时钟误差进行实时最优估计与在线补偿；本发明专利技术能减小异步时钟误差对无人机协同定位精度的影响，提高从机的绝对定位精度。的绝对定位精度。的绝对定位精度。

【技术实现步骤摘要】
基于5G信号异步时钟误差补偿的多无人机协同定位方法


[0001]本专利技术专利属于无人机导航
，具体涉及基于5G信号异步时钟误差补偿的多无人机协同定位方法。

技术介绍

[0002]近年来，随着无人机技术的快速发展，其在军民用领域都得到了广泛应用，如远程侦察、城市测绘、电力巡检等。考虑到单架无人机载荷小、抗毁伤能力差、执行复杂任务成功率低的缺点，多无人机协作分工可显著提高复杂环境下任务执行的效率和成功率。导航信息作为无人机飞行控制系统的输入，是多无人机协同完成任务的前提。在卫星信号可用的条件下，惯性导航系统(Inertial Navigation System,INS)与全球卫星导航系统(Global Navigation Satellite System,GNSS)的组合广泛应用于无人机编队，可以为多无人机提供高精度的定位服务。
[0003]然而，卫星信号易受外界干扰。当卫星信号不可用时，由于惯导系统的误差会随时间累积，因此无法满足长时间的精确导航需求。相对测距/测角信息作为一种可靠的外源信息，能够在卫星受扰或不可用时对多无人机的本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.基于5G信号异步时钟误差补偿的多无人机协同定位方法，其特征在于，包括以下步骤：S1、主机节点搭载惯性导航系统、卫星接收机以及5G数据通讯装置，从机节点搭载惯性导航系统和5G数据通讯装置；S2、从机节点周期性读取t时刻机载陀螺仪和加速度计信息，并通过5G收发装置实时获得主机节点位置信息以及与主机节点的测距、测角信息；S3、从机节点预测t时刻的姿态四元数、速度和位置信息；S4、基于5G信号异步时钟误差建立相对测距模型；S5、利用主机的位置信息、主从机节点之间的测距与测角信息、气压高度计信息，构建协同定位状态方程和量测方程；S6、利用卡尔曼滤波器对从机节点导航状态与主机和从机节点之间的异步时钟误差进行实时最优估计与在线补偿，提高从机节点的定位精度。2.根据权利要求1所述的基于5G信号异步时钟误差补偿的多无人机协同定位方法，其特征在于：在步骤S3中，采用下式预测从机节点在t时刻的姿态四元数：上式中，Q(t)＝[q0(t) q1(t) q2(t) q3(t)]
T
为t时刻的姿态四元数，上标T表示矩阵的转置；Q(t
‑
1)＝[q0(t
‑
1) q1(t
‑
1) q2(t
‑
1) q3(t
‑
1)]
T
为t
‑
1时刻的姿态四元数；ΔT为离散采样周期；通过下式计算：其中，为t时刻陀螺仪读取的从机节点机体坐标系相对于导航坐标系的角速度在机体坐标系X、Y、Z轴上的分量。3.根据权利要求2所述的基于5G信号异步时钟误差补偿的多无人机协同定位方法，其特征在于：在步骤S3中，采用下式预测从机节点在t时刻的速度信息：其中，为t时刻加速度计读取的从机节点机体系相对于导航系的加速度在机体坐标系X、Y、Z轴上的分量；g＝[0 0 g]
T
，g为当地重力加速度值；为t时刻机体坐标系相对于导航坐标系的线速度在导航坐标系X、Y、Z轴上的分量；为t
‑
1时刻机体坐标系相对于导航坐标系的线速度在导航坐标系X、Y、Z轴上的分量；为机体坐标系到导航坐标系的姿态矩阵，通过下式计算：
。4.根据权利要求3所述的基于5G信号异步时钟误差补偿的多无人机协同定位方法，其特征在于：在步骤S3中，采用下式预测从机节点在t时刻的位置信息：上式中，分别为t时刻从机节点在导航坐标系X、Y、Z轴上的位置坐标；导航坐标系X、Y、Z轴上的位置坐标；分别为t
‑
1时刻从机节点在导航坐标系X、Y、Z轴上的位置坐标。5.根据权利要求4所述的基于5G信号异步时钟误差补偿的多无人机协同定位方法，其特征在于：步骤S4具体过程如下：由于主机与从机节点的时钟不同步，会导致获取的测距信息存在较大的误差；因此，在相对测距信息的建模过程中需要考虑异步时钟误差的影响，可以表示为：上式中，为t时刻从机节点5G通讯系统获得的主机与从机节点之间的三维相对距离测量值，r
t
为机间5G通讯系统相对距离真值，Δρ为异步时钟误差造成的相对测距误差，w
r
为5G通讯系统的相对测距噪声。6.根据权利要求5所述的基于5G信号异步时钟误差补偿的多无人机协同定位方法，其特征在于：步骤S5包括如下过程：S51、需要在状态量中包含异步时钟误差造成的相对测距误差，因此从机节点的卡尔曼滤波状态量为：其中，为惯导系统三轴平台误差角，ΔV
n
＝[Δv
E Δv
N Δv
U
]为惯导系统东向、北向、天向三轴速度误差，ΔP
n
＝[ΔL Δλ Δh]为纬度、经度、高度三轴位置误差，分别为陀螺仪随机常数和一阶马尔可夫随机噪声，为加速度计一阶马尔可夫随机漂移，Δρ为主从无人机节点间的异步时钟误差造成的相对测距误差；S52、建立惯性导航系统的误差方程，包括平台误差角方程、速度误差方程和位置误差方程：平台误差角方程：
其中，w
e
为地球自转角速度，v
E
、v
N
为从机节点惯性导航系统东向、北向速度，δv
E
、δv
N
为对应的速度误差，ε
E
、ε
N
、ε
U
为从机节点东、北、天向陀螺噪声角，L,λ,h为从机节点i的陀螺仪纬度、经度、高度值，R
M
＝R
e
(1
‑
2f+3fsin2L)，R
N
＝R
e
(1+fsin2L)，R
e
＝6378137米，f＝1/298.257，h为机体与地面的距离；速度误差方程：其中，v
U
为从机节点惯性导航系统天向速度，δv
U
为对应的速度误差，f
E

【专利技术属性】
技术研发人员：黄郑，王红星，孟嘉，朱徐东，吴召龙，吕品，杜彪，朱洁，顾徐，霍丹江，陈露，
申请(专利权)人：南京航空航天大学，
类型：发明
国别省市：