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

(19)国家知识产权局(12)发明专利申请(10)申请公布号CN115773753A(43)申请公布日2023.03.10(21)申请号202211430246.3H04W4/44(2010.01)(22)申请日2022.11.15G01C21/16(2018.01)G01C21/18(2018.01)(71)申请人江苏方天电力技术有限公司G01C5/06(2006.01)地址211102江苏省南京市江宁区科学园G01S19/49(2006.01)天元中路19号G05D1/10(2006.01)申请人南京航空航天大学(72)发明人黄郑王红星孟嘉朱徐东吴召龙吕品杜彪朱洁顾徐霍丹江陈露(74)专利代理机构南京钟山专利代理有限公司32252专利代理师张明浩(51)Int.Cl.G01C21/20(2006.01)H04W4/024(2006.01)权利要求书6页说明书15页附图3页(54)发明名称基于5G信号异步时钟误差补偿的多无人机协同定位方法(57)摘要本发明提供了基于5G信号异步时钟误差补偿的多无人机协同定位方法，属于无人机导航领域，主要包括以下过程：通过机载设备，从机节点周期性读取t时刻机载陀螺仪和加速度计信息，并通过5G收发装置实时获得主机节点位置信息以及与主机节点的测距、测角信息；从机节点预测t时刻的姿态四元数、速度和位置信息；基于5G信号异步时钟误差建立相对测距模型；利用主机的位置信息、主从机节点之间的测距与测角信息、气压高度计信息，构建协同定位状态方程和量测方程；利用卡尔曼滤波器对从机节点导航状态与主机和从机节点之间的异步时钟误差进行实时最优估计与在线补偿；本发明能减小异步时钟误差对无人机协同定位精度的影响，提高从机的绝对定位精度。CN115773753ACN115773753A权利要求书1/6页1.基于5G信号异步时钟误差补偿的多无人机协同定位方法，其特征在于，包括以下步骤：S1、主机节点搭载惯性导航系统、卫星接收机以及5G数据通讯装置，从机节点搭载惯性导航系统和5G数据通讯装置；S2、从机节点周期性读取t时刻机载陀螺仪和加速度计信息，并通过5G收发装置实时获得主机节点位置信息以及与主机节点的测距、测角信息；S3、从机节点预测t时刻的姿态四元数、速度和位置信息；S4、基于5G信号异步时钟误差建立相对测距模型；S5、利用主机的位置信息、主从机节点之间的测距与测角信息、气压高度计信息，构建协同定位状态方程和量测方程；S6、利用卡尔曼滤波器对从机节点导航状态与主机和从机节点之间的异步时钟误差进行实时最优估计与在线补偿，提高从机节点的定位精度。2.根据权利要求1所述的基于5G信号异步时钟误差补偿的多无人机协同定位方法，其特征在于：在步骤S3中，采用下式预测从机节点在t时刻的姿态四元数：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‑1时刻的姿态四元数；ΔT为离散采样周期；通过下式计算：其中，为t时刻陀螺仪读取的从机节点机体坐标系相对于导航坐标系的角速度在机体坐标系X、Y、Z轴上的分量。3.根据权利要求2所述的基于5G信号异步时钟误差补偿的多无人机协同定位方法，其特征在于：在步骤S3中，采用下式预测从机节点在t时刻的速度信息：其中，为t时刻加速度计读取的从机节点机体系相对于导航系的加速度在机体坐标系X、Y、Z轴上的分量；g＝[00g]T，g为当地重力加速度值；为t时刻机体坐标系相对于导航坐标系的线速度在导航坐标系X、Y、Z轴上的分量；为t‑1时刻机体坐标系相对于导航坐标系的线速度在导航坐标系X、Y、Z轴上的分量；为机体坐标系到导航坐标系的姿态矩阵，通过下式计算：2CN115773753A权利要求书2/6页。4.根据权利要求3所述的基于5G信号异步时钟误差补偿的多无人机协同定位方法，其特征在于：在步骤S3中，采用下式预测从机节点在t时刻的位置信息：上式中，分别为t时刻从机节点在导航坐标系X、Y、Z轴上的位置坐标；分别为t‑1时刻从机节点在导航坐标系X、Y、Z轴上的位置坐标。5.根据权利要求4所述的基于5G信号异步时钟误差补偿的多无人机协同定位方法，其特征在于：步骤S4具体过程如下：由于主机与从机节点的时钟不同步，会导致获取的测距信息存在较大的误差；因此，在相对测距信息的建模过程中需要考虑异步时钟误差的影响，可以表示为：上式中，为t时刻从机节点5G通讯系统获得的主机与从机节点之间的三维相对距离测t量值，r为机间5G通讯系统相对距离真值，Δρ为异步时钟误差造成的相对测距误差，wr为5G通讯系统的相对测距噪声。6.根据权利要求5所述的基于5G信号异步时钟误差补偿的多无人机协同定位方法，其特征在