CN111238469A - 一种基于惯性/数据链的无人机编队相对导航方法 - Google Patents

一种基于惯性/数据链的无人机编队相对导航方法 Download PDF

Info

Publication number
CN111238469A
CN111238469A CN201911282280.9A CN201911282280A CN111238469A CN 111238469 A CN111238469 A CN 111238469A CN 201911282280 A CN201911282280 A CN 201911282280A CN 111238469 A CN111238469 A CN 111238469A
Authority
CN
China
Prior art keywords
aerial vehicle
unmanned aerial
relative
node
unmanned plane
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Granted
Application number
CN201911282280.9A
Other languages
English (en)
Other versions
CN111238469B (zh
Inventor
朱徐东
赖际舟
吕品
周子寒
戴宇庭
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Nanjing University of Aeronautics and Astronautics
Original Assignee
Nanjing University of Aeronautics and Astronautics
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Nanjing University of Aeronautics and Astronautics filed Critical Nanjing University of Aeronautics and Astronautics
Priority to CN201911282280.9A priority Critical patent/CN111238469B/zh
Publication of CN111238469A publication Critical patent/CN111238469A/zh
Application granted granted Critical
Publication of CN111238469B publication Critical patent/CN111238469B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • GPHYSICS
    • G01MEASURING; TESTING
    • G01CMEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • BPERFORMING OPERATIONS; TRANSPORTING
    • B64AIRCRAFT; AVIATION; COSMONAUTICS
    • B64CAEROPLANES; HELICOPTERS
    • B64C39/00Aircraft not otherwise provided for
    • B64C39/02Aircraft not otherwise provided for characterised by special use
    • BPERFORMING OPERATIONS; TRANSPORTING
    • B64AIRCRAFT; AVIATION; COSMONAUTICS
    • B64UUNMANNED AERIAL VEHICLES [UAV]; EQUIPMENT THEREFOR
    • B64U2201/00UAVs characterised by their flight controls
    • B64U2201/10UAVs characterised by their flight controls autonomous, i.e. by navigating independently from ground or air stations, e.g. by using inertial navigation systems [INS]
    • B64U2201/102UAVs characterised by their flight controls autonomous, i.e. by navigating independently from ground or air stations, e.g. by using inertial navigation systems [INS] adapted for flying in formations

Abstract

本发明公开了一种基于惯性/数据链的无人机编队相对导航方法,属于无人机编队相对导航领域。本发明首先通过无人机节点机载惯导输出构成的惯导多边形与相对测距量测多边形根据双中心转换法得到无人机编队各无人机节点初始位置坐标;然后通过建立相对导航状态方程完成对无人机节点间相对位置、相对速度、相对加速度、相对四元数的预测,最后利用无人机节点间的相对测距信息作为量测量实现无人机编队节点间相对导航。本发明不仅仅能够得到编队中无人机节点间高精度的相对导航信息,同时能够提高无人机节点机载惯导系统的位置输出精度。本发明适用于卫星拒止环境下至少7架无人机以上的无人机编队相对导航。

Description

一种基于惯性/数据链的无人机编队相对导航方法
技术领域
本发明涉及一种基于惯性/数据链的无人机编队相对导航方法,属于编队无人机相对导航技术领域。
背景技术
无人机在现代战争中主要用于军事侦察、战略打击等任务,传统的单一无人机在执行任务时受载体承重、成本等条件的限制,同时单一的无人机在执行任务时抗毁伤能力不足,已无法满足现阶段作战需求。编队无人机在此背景下应运而生,为了提高无人机编队作战能力,无人机间应具有高精度相对导航能力就显得尤为重要。同时无人机在战场环境下容易受环境、电磁干扰等影响,GPS在战场环境不可用,迫切需要研究GPS卫星拒止环境下无人机编队相对导航方法。
现阶段编队无人机相对导航一般采用GPS卫星、测角/测距传感器辅助来实现。然而在战场环境下GPS信号易受到干扰而导致不可用,编队无人机需要大量搭载测距/测角传感器增加了项目成本,同时现阶段测距/测角设备主要用视觉/雷达传感器来实现,战场环境特征复杂对现有视觉/雷达匹配算法的实时性、可靠性要求较高,在实际情况下很难得到应用。
发明内容
本发明提出了一种基于惯性/数据链的无人机编队相对导航方法,首先通过无人机节点机载惯导输出构成的惯导多边形与相对测距量测多边形根据双中心转换法得到无人机编队各无人机节点初始位置坐标;然后通过建立相对导航状态方程完成对无人机节点间相对位置、相对速度、相对加速度、相对四元数的预测,最后利用无人机节点间的相对测距信息作为量测量实现无人机编队节点间相对导航。本发明不仅仅能够得到编队中无人机节点间高精度的相对导航信息同时能够提高无人机节点机载惯导系统的位置输出精度。
本发明为解决上述技术问题采用以下技术方案:
一种基于惯性/数据链的无人机编队相对导航方法,包括如下步骤:
步骤一:无人机节点搭载惯性导航系统,数据链收发装置,周期性读取k时刻无人机节点传感器数据,包括陀螺仪信息
Figure RE-GDA0002445501180000021
加速度计信息
Figure RE-GDA0002445501180000022
其中 i=7,8…,N以及无人机节点间相对距离dut,其中u,t表示不同的无人机节点;
步骤二:定义无人机节点机体坐标系及导航坐标系并建立无人机节点u,t之间相对位置、相对速度、相对姿态四元数误差方程、陀螺噪声误差方程,加速度计噪声误差方程;
步骤三:利用无人机节点惯导输出构成的惯导多边型与机载数据链相对测距构成的量测多边形计算k时刻编队中各无人机节点初始位置坐标;
步骤四:利用卡尔曼滤波器对k时刻无人机节点间的相对位置信息进行校正。
步骤二中所述无人机节点机体坐标系及导航坐标系定义如下:机体坐标系的 X、Y、Z轴分别为无人机的右向、前向和上向,导航坐标系分别为东、北、天地理坐标系。
步骤二中采取如下方式建立无人机节点u,t之间的相对位置运动方程、相对速度误差方程、相对姿态四元数误差方程,陀螺噪声误差方程,加速度计噪声误差方程:
1)无人机节点u,t之间的相对位置运动方程
Figure RE-GDA0002445501180000031
其中
Figure RE-GDA0002445501180000032
表示无人机节点u机体坐标系相对于无人机节点t的机体坐标系的位置在无人机u坐标系下的投影,可表示为:
Figure RE-GDA0002445501180000033
其中
Figure RE-GDA0002445501180000034
为惯性坐标系到无人机节点u机体系的坐标转换矩阵;
Figure RE-GDA0002445501180000035
为无人机节点u机体坐标系相对惯性坐标系的位置在惯性坐标系下的投影;
Figure RE-GDA0002445501180000036
为无人机节点t机体坐标系相对惯性坐标系下的位置在惯性坐标系下的投影;
Figure RE-GDA0002445501180000037
为无人机节点u机体系相对于惯性系的角速度在机体系上的投影;
Figure RE-GDA0002445501180000038
为无人机节点u机体系下的比力;
Figure RE-GDA0002445501180000039
为无人机节点t机体系下的比力;
Figure RE-GDA00024455011800000310
为无人机节点t机体系到无人机节点u机体系的坐标转换矩阵;
Figure RE-GDA00024455011800000311
为无人机节点u机体系相对惯性系的角速率在机体系下的投影;
Figure RE-GDA00024455011800000312
为无人机节点u相对无人机节点t的速度在无人机节点u机体下的投影;
Figure RE-GDA00024455011800000313
为无人机节点u相对无人机节点t的加速度在无人机节点u机体坐标系下的投影;
2)无人机节点u,t之间的相对速度误差方程如下式表示:
Figure RE-GDA00024455011800000314
其中
Figure RE-GDA00024455011800000315
为无人机节点u机体系相对惯性系的角速率估计值在机体系下的投影;
Figure RE-GDA00024455011800000316
为无人机节点u机体系相对惯性系角速度估计值在机体下的投影;
Figure RE-GDA00024455011800000317
为陀螺随机常数误差,
Figure RE-GDA00024455011800000318
为陀螺一阶马尔科夫过程白噪声,
Figure RE-GDA00024455011800000319
为无人机节点u,t之间的相对位置误差在无人机节点u机体系下的投影;
Figure RE-GDA0002445501180000041
为无人机节点t到无人机节点u姿态转换矩阵的估计值;
Figure RE-GDA0002445501180000042
为无人机节点u机体坐标系相对惯性坐标系的角速度的估计值在无人机节点u机体下的投影;
Figure RE-GDA0002445501180000043
为无人机节点u,t之间的相对距离估计值在无人机节点u机体系下的投影;Ju表示无人机节点u的惯量矩阵;
Figure RE-GDA0002445501180000044
为节点u惯量矩阵的逆矩阵;
Figure RE-GDA0002445501180000045
表示为无人机节点u,t之间的相对速度误差在无人机节点u机体系下的投影;δεra表示为零均值的加速度计一阶马尔科夫过程白噪声;wa为零均值的加速度计白噪声;
3)相对姿态四元数误差方程如下式表示:
Figure RE-GDA0002445501180000046
其中
Figure RE-GDA0002445501180000047
为无人机节点u相对无人机节点t的角速度估计值在无人机节点u机体坐标系下的投影;δqrel为无人机节点间的相对四元数;δεrg为陀螺一阶马尔科夫过程白噪声;δεbg为陀螺随机常数;wg为陀螺零均值的白噪声;
4)惯导噪声误差方程;
陀螺的噪声主要考虑陀螺随机常数、陀螺一阶马尔科夫驱动白噪声、陀螺白噪声,即:
ε=εbgrg+wg
其中εbg为陀螺的随机常数,εrg为陀螺一阶马尔科夫过程随机噪声,wg为陀螺白噪声;
考虑无人机节点机载三轴陀螺误差类型相同,则陀螺随机常数误差数学表达式表示为:
Figure RE-GDA0002445501180000048
陀螺一阶马尔科夫驱动白噪声误差数学表示式表示为:
Figure RE-GDA0002445501180000049
其中εrg为陀螺一阶马尔科夫过程随机噪声,Tg为陀螺相关时间常数,wrg为陀螺一阶马尔科夫过程驱动白噪声;
加速度计噪声误差方程
加速度计的噪声误差仅考虑一阶马尔科夫过程,同样假定加速度计三轴的误差模型相同,则加速度计噪声误差方程表示为:
Figure RE-GDA0002445501180000051
其中▽ra为加速度计一阶马尔科夫过程,Ta为加速度计相关时间常数,wag为加速度计一阶马尔科夫过程驱动白噪声。
步骤三的具体过程如下:
a)解算获得惯导多边形与量测多边形双中心转换矩阵B(tk),表示为:
Figure RE-GDA0002445501180000052
上式中D(tk)为无人机节点相对测距构成的相对距离矩阵
Figure RE-GDA0002445501180000053
其中dut表示无人机节点u与无人机节点t相对距离;
Figure RE-GDA0002445501180000054
其中I为n维单位向量,K为n维向量的元素;
b)解算双中心转换矩阵的特征值和特征向量
B(tk)=V(tk)Λ(tk)V(tk)T
其中Λ(tk)=diag(λ12,...λn)为矩阵B(tk)的特征值对应的对角矩阵,λ12,...λn矩阵B(tk)的特征值,V(tk)=[ν12,...,νn]为特征值所对应的特征向量,其中λ1特征值对应的特征向量为ν1,λ2特征值对应的特征向量为ν2,λn特征值对应的特征向量为νn,且λ1≥λ2≥…≥λn≥0;
c)解算编队中各无人机节点初始位置坐标:
各无人机节点的初始坐标由下式计算得到:
Figure RE-GDA0002445501180000055
Figure RE-GDA0002445501180000061
其中n≥7
其中:
Figure RE-GDA0002445501180000062
为无人机节点1k时刻三维初始位置;
Figure RE-GDA0002445501180000063
为无人机节点2k时刻三维初始位置;
Figure RE-GDA0002445501180000064
为无人机节点n k时刻初始位置;Λ3(tk)为k时刻双中心变换矩阵B(tk)前三个最大特征值构成的对角矩阵;VP(tk)=[ν123]为k时刻双中心变换矩阵B(tk)前三个最大特征值所对应的特征向量。
步骤四的具体过程如下:
卡尔曼滤波滤波时间更新以及量测更新过程如下所示:
状态一步预测方程:
Figure RE-GDA0002445501180000065
状态估值方程:
Figure RE-GDA0002445501180000066
滤波增益方程:
Figure RE-GDA0002445501180000067
一步预测均方误差方程:
Figure RE-GDA0002445501180000068
估计均方误差方程:
Figure RE-GDA0002445501180000069
上式中Φk,k-1为系统k-1到k时刻状态转移矩阵;
Figure RE-GDA00024455011800000610
为系统k-1到k时刻状态转移矩阵的转置;
Figure RE-GDA00024455011800000611
为系统k-1时刻状态估计值;
Figure RE-GDA00024455011800000612
为k时刻状态估计值;Zk为k时刻系统量测值;Pk/k-1为k-1时刻到k时刻预测协方差;Pk-1为k-1时刻系统协方差;Pk/k为k时刻估计协方差;
Figure RE-GDA00024455011800000613
为k时刻滤波增益的转置;Kk为系统k时刻滤波增益;Hk为系统k时刻量测矩阵;Qk-1为系统k-1时刻系统噪声阵;Γk,k-1为系统k-1到k时刻系统输入矩阵; Rk为系统k时刻量测噪声矩阵;
Figure RE-GDA0002445501180000071
为k时刻量测矩阵转置;
Figure RE-GDA0002445501180000072
为系统k-1到k时刻系统输入矩阵的转置。
本发明的有益效果如下:
在本发明中,无人机编队各无人机节点搭载惯导系统及机载数据链收发装置,利用机载数据链实时得到无人机节点间的相对距离信息作为量测量,在精确相对导航建模的基础上实现无人机节点间高精度相对导航,解决GPS拒止环境下无人机编队相对导航问题。本发明所提出的方法不仅仅能够得到无人机节点高精度相对导航信息,而且还能够一定程度上抑制各无人机节点机载惯导系统发散。
附图说明
图1为基于惯导/数据链的无人机编队相对导航算法流程图。
图2为无人机节点1与无人机节点2之间的相对距离误差图。
图3为无人机节点1与无人机节点3之间的相对距离误差图。
图4为无人机节点1与无人机节点4之间的相对距离误差图。
图5为无人机节点1与无人机节点5之间的相对距离误差图。
图6为无人机节点1与无人机节点6之间的相对距离误差图。
图7为无人机节点1与无人机节点7之间的相对距离误差图。
具体实施方式
下面结合附图对本发明创造做进一步详细说明。
本发明方法的总体框架图如图1所示,其具体步骤如下:
一种基于惯性/数据链的无人机编队相对导航方法,包括如下步骤:
步骤一:无人机节点搭载惯性导航系统,数据链收发装置。周期性读取k刻无人机节点传感器数据,包括陀螺仪信息
Figure RE-GDA0002445501180000081
加速度计信息
Figure RE-GDA0002445501180000082
其中i=7,8…,N以及无人机节点间相对距离dut,其中u, t示不同的无人机节点。
步骤二:定义无人机节点机体坐标系及导航坐标系并建立无人机节点u,t间相对位置、相对速度、相对姿态四元数误差方程、陀螺噪声误差方程,加速度计噪声误差方程。
步骤三:利用无人机节点惯导输出构成的惯导多边型与机载数据链相对测距构成的量测多边形计算k时刻编队中各无人机节点初始位置坐标。
步骤四:利用卡尔曼滤波器对k时刻无人机节点间的相对位置信息进行校正。
相关坐标系定义为:机体系的X、Y、Z轴分别为无人机的右向、前向(无人机机头方向)、上向,导航坐标系分别为东、北、天地理坐标系。
步骤二中采取如下方式解算得到无人机节点u,t之间的相对位置运动方程、相对速度误差方程、相对姿态四元数误差方程,陀螺噪声误差方程,加速度计噪声误差方程。
1)无人机节点u,t之间的相对位置运动方程
Figure RE-GDA0002445501180000083
其中
Figure RE-GDA0002445501180000091
表示无人机节点u机体坐标系相对于无人机节点t的机体坐标系的位置在无人机u坐标系下的投影,可表示为:
Figure RE-GDA0002445501180000092
其中
Figure RE-GDA0002445501180000093
为惯性坐标系到无人机节点u机体系的坐标转换矩阵;
Figure RE-GDA0002445501180000094
为无人机节点u机体坐标系相对惯性坐标系的位置在惯性坐标系下的投影;
Figure RE-GDA0002445501180000095
为无人机节点t机体坐标系相对惯性坐标系下的位置在惯性坐标系下的投影;
Figure RE-GDA0002445501180000096
为无人机节点u机体系相对于惯性系的角速度在机体系上的投影;
Figure RE-GDA0002445501180000097
为无人机节点u机体系下的比力;
Figure RE-GDA0002445501180000098
为无人机节点t机体系下的比力;
Figure RE-GDA0002445501180000099
为无人机节点t机体系到无人机节点u机体系的坐标转换矩阵;
Figure RE-GDA00024455011800000910
为无人机节点u机体系相对惯性系的角速率在机体系下的投影;
Figure RE-GDA00024455011800000911
为无人机节点u相对无人机节点t的速度在无人机节点u机体下的投影;
Figure RE-GDA00024455011800000912
为无人机节点u相对无人机节点t的加速度在无人机节点u机体坐标系下的投影;
无人机节点u,t之间的相对速度误差方程
无人机节点u,t之间的相对速度误差方程可用如下式表示:
Figure RE-GDA00024455011800000913
其中
Figure RE-GDA00024455011800000914
为无人机节点u机体系相对惯性系的角速率估计值在机体系下的投影;
Figure RE-GDA00024455011800000915
为无人机节点u机体系相对惯性系角速度估计值在机体下的投影;
Figure RE-GDA0002445501180000101
为陀螺随机常数误差,
Figure RE-GDA0002445501180000102
为陀螺一阶马尔科夫过程白噪声,
Figure RE-GDA0002445501180000103
为无人机节点u,t之间的相对位置误差在无人机节点u机体系下的投影;
Figure RE-GDA0002445501180000104
为无人机节点t到无人机节点u姿态转换矩阵的估计值;
Figure RE-GDA0002445501180000105
为无人机节点u机体坐标系相对惯性坐标系的角速度的估计值在无人机节点u机体下的投影;
Figure RE-GDA0002445501180000106
为无人机节点u,t之间的相对距离估计值在无人机节点u机体系下的投影;Ju表示无人机节点u的惯量矩阵;
Figure RE-GDA0002445501180000107
为节点u惯量矩阵的逆矩阵;
Figure RE-GDA0002445501180000108
表示为无人机节点u,t之间的相对速度误差在无人机节点u机体系下的投影;δεra表示为零均值的加速度计一阶马尔科夫过程白噪声;wa为零均值的加速度计白噪声。
2)相对姿态四元数误差方程
相对姿态四元数误差方程可用如下式表示:
Figure RE-GDA0002445501180000109
其中
Figure RE-GDA00024455011800001010
为无人机节点u相对无人机节点t的角速度估计值在无人机节点u机体坐标系下的投影;δqrel为无人机节点间的相对四元数;δεrg为陀螺一阶马尔科夫过程白噪声;δεbg为陀螺随机常数;wg为陀螺零均值的白噪声。
3)惯导噪声误差方程;
陀螺噪声误差方程
陀螺的噪声这里主要考虑陀螺随机常数、陀螺一阶马尔科夫驱动白噪声、陀螺白噪声。即:
ε=εbgrg+wg
其中εbg为陀螺的随机常数,εrg为陀螺一阶马尔科夫过程随机噪声,wg为陀螺白噪声。
考虑无人机节点机载三轴陀螺误差类型相同,则陀螺随机常数误差数学表达式可表示为:
Figure RE-GDA0002445501180000111
陀螺一阶马尔科夫驱动白噪声误差数学表示式可表示为:
Figure RE-GDA0002445501180000112
其中εrg为陀螺一阶马尔科夫过程随机噪声,Tg为陀螺相关时间常数,wrg为陀螺一阶马尔科夫过程驱动白噪声。
加速度计噪声误差方程
加速度计的噪声误差这里仅考虑一阶马尔科夫过程,同样假定加速度计三轴的误差模型相同,则加速度计噪声误差方程可表示为:
Figure RE-GDA0002445501180000113
其中▽ra为加速度计一阶马尔科夫过程,Ta为加速度计相关时间常数,wag为加速度计一阶马尔科夫过程驱动白噪声。
步骤三利用无人机节点惯导输出构成的惯导多边形与机载数据链相对测距构成的量测多边形计算k时刻编队中各无人机节点的初始位置坐标。
1)根据无人机节点惯导输出构成的惯导多边形与机载数据链相对测距构成的量测多边形利用多边形重心一致性得到双中心转换矩阵B(tk),可表示为:
Figure RE-GDA0002445501180000121
上式中D(tk)为无人机节点相对测距构成的相对距离矩阵
Figure RE-GDA0002445501180000122
其dut表示无人机节点u与无人机节点t相对距离;
Figure RE-GDA0002445501180000123
其中I为n维单位向量,K为n维向量的元素。
2)解算双中心转换矩阵的特征值和特征向量
B(tk)=V(tk)Λ(tk)V(tk)T
其中Λ(tk)=diag(λ12,...λn)为矩阵B(tk)的特征值对应的对角矩阵,λ12,...λn矩阵B(tk)的特征值,V(tk)=[ν12,...,νn]为特征值所对应的特征向量,其中λ1特征值对应的特征向量为ν1,λ2特征值对应的特征向量为ν2,λn特征值对应的特征向量为νn,且λ1≥λ2≥...≥λn≥0。
3)解算编队中各无人机节点初始位置坐标:
由于无人机初始位置坐标为三维坐标,选取双中心变换矩阵B(tk) 前三个最大的特征值,则构成的对角矩阵可表示为:Λ3(tk)=diag(λ123),特征值所对应的特征向量为VP(tk)=[ν123],其中ν1为特征值λ1对应的特征向量,ν2为特征值λ2对应的特征向量,ν3为特征值λ3对应的特征向量,则各无人机节点的初始坐标可由下式计算得到:
Figure RE-GDA0002445501180000124
Figure RE-GDA0002445501180000131
其中n≥7。
其中:
Figure RE-GDA0002445501180000132
为无人机节点1k时刻三维初始位置;
Figure RE-GDA0002445501180000133
为无人机节点2k时刻三维初始位置;
Figure RE-GDA0002445501180000134
为无人机节点n k时刻初始位置;Λ3(tk)为k时刻双中心变换矩阵B(tk)前三个最大特征值构成的对角矩阵; VP(tk)=[ν123]为k时刻双中心变换矩阵B(tk)前三个最大特征值所对应的特征向量。
步骤四中利用卡尔曼滤波器对k时刻无人机节点间的相对位置信息进行校正。
卡尔曼滤波滤波时间更新以及量测更新过程如下所示:
状态一步预测方程:
Figure RE-GDA0002445501180000135
状态估值方程:
Figure RE-GDA0002445501180000136
滤波增益方程:
Figure RE-GDA0002445501180000137
一步预测均方误差方程:
Figure RE-GDA0002445501180000138
估计均方误差方程:
Figure RE-GDA0002445501180000139
上式中Φk,k-1为系统k-1到k时刻状态转移矩阵;
Figure RE-GDA00024455011800001310
为系统k-1到k时刻状态转移矩阵的转置;
Figure RE-GDA0002445501180000141
为系统k-1时刻状态估计值;
Figure RE-GDA0002445501180000142
为k时刻状态估计值;Pk/k-1为k-1时刻到k时刻预测协方差;Pk-1为k-1时刻系统协方差;Pk/k为k时刻估计协方差;
Figure RE-GDA0002445501180000143
为k时刻滤波增益的转置;Kk为系统k 时刻滤波增益;Hk为系统k时刻量测矩阵;Qk-1为系统k-1时刻系统噪声阵;Γk,k-1为系统k-1到k时刻系统输入矩阵;Rk为系统k时刻量测噪声矩阵;
Figure RE-GDA0002445501180000144
为k时刻量测矩阵转置;
Figure RE-GDA0002445501180000145
为系统k-1到k时刻系统输入矩阵的转置;
表1无人机节点机载纯惯导递推与基于构型的无人机节点位置1h发散程度对比
Figure RE-GDA0002445501180000146
Figure RE-GDA0002445501180000151
Figure RE-GDA0002445501180000161
由图2,3,4,5,6,7可知无人机编队中系统稳态时无人机节点1相对其它无人机节点相对距离误差保持在3米以内,其它无人机节点与无人机节点1类似。由表1可知通过本发明提出的算法各无人机节点机载惯导系统位置输出与机载惯导系统纯惯导递推位置输出精度提高了至少30%以上,有效验证了本发明所提出算法的有效性及可行性。

Claims (5)

1.一种基于惯性/数据链的无人机编队相对导航方法,其特征在于,包括如下步骤:
步骤一:无人机节点搭载惯性导航系统,数据链收发装置,周期性读取k时刻无人机节点传感器数据,包括陀螺仪信息
Figure RE-FDA0002445501170000011
加速度计信息
Figure RE-FDA0002445501170000012
其中i=7,8…,N以及无人机节点间相对距离dut,其中u,t表示不同的无人机节点;
步骤二:定义无人机节点机体坐标系及导航坐标系并建立无人机节点u,t之间相对位置、相对速度、相对姿态四元数误差方程、陀螺噪声误差方程,加速度计噪声误差方程;
步骤三:利用无人机节点惯导输出构成的惯导多边型与机载数据链相对测距构成的量测多边形计算k时刻编队中各无人机节点初始位置坐标;
步骤四:利用卡尔曼滤波器对k时刻无人机节点间的相对位置信息进行校正。
2.根据权利要求1所述的一种基于惯性/数据链的无人机编队相对导航方法,其特征在于步骤二中所述无人机节点机体坐标系及导航坐标系定义如下:机体坐标系的X、Y、Z轴分别为无人机的右向、前向和上向,导航坐标系分别为东、北、天地理坐标系。
3.根据权利要求1所述的一种基于惯性/数据链的无人机编队相对导航方法,其特征在于步骤二中采取如下方式建立无人机节点u,t之间的相对位置运动方程、相对速度误差方程、相对姿态四元数误差方程,陀螺噪声误差方程,加速度计噪声误差方程:
1)无人机节点u,t之间的相对位置运动方程
Figure RE-FDA0002445501170000013
其中
Figure RE-FDA0002445501170000014
表示无人机节点u机体坐标系相对于无人机节点t的机体坐标系的位置在无人机u坐标系下的投影,表示为:
Figure RE-FDA0002445501170000015
其中
Figure RE-FDA0002445501170000016
为惯性坐标系到无人机节点u机体系的坐标转换矩阵;
Figure RE-FDA0002445501170000017
为无人机节点u机体坐标系相对惯性坐标系的位置在惯性坐标系下的投影;
Figure RE-FDA0002445501170000021
为无人机节点t机体坐标系相对惯性坐标系下的位置在惯性坐标系下的投影;
Figure RE-FDA0002445501170000022
为无人机节点u机体系相对于惯性系的角速度在机体系上的投影;
Figure RE-FDA0002445501170000023
为无人机节点u机体系下的比力;
Figure RE-FDA0002445501170000024
为无人机节点t机体系下的比力;
Figure RE-FDA0002445501170000025
为无人机节点t机体系到无人机节点u机体系的坐标转换矩阵;
Figure RE-FDA0002445501170000026
为无人机节点u机体系相对惯性系的角速率在机体系下的投影;
Figure RE-FDA0002445501170000027
为无人机节点u相对无人机节点t的速度在无人机节点u机体下的投影;
Figure RE-FDA0002445501170000028
为无人机节点u相对无人机节点t的加速度在无人机节点u机体坐标系下的投影;
2)无人机节点u,t之间的相对速度误差方程如下式表示:
Figure RE-FDA0002445501170000029
其中
Figure RE-FDA00024455011700000210
为无人机节点u机体系相对惯性系的角速率估计值在机体系下的投影;
Figure RE-FDA00024455011700000211
为无人机节点u机体系相对惯性系角速度估计值在机体下的投影;
Figure RE-FDA00024455011700000212
为陀螺随机常数误差,
Figure RE-FDA00024455011700000213
为陀螺一阶马尔科夫过程白噪声,
Figure RE-FDA00024455011700000214
为无人机节点u,t之间的相对位置误差在无人机节点u机体系下的投影;
Figure RE-FDA00024455011700000215
为无人机节点t到无人机节点u姿态转换矩阵的估计值;
Figure RE-FDA00024455011700000216
为无人机节点u机体坐标系相对惯性坐标系的角速度的估计值在无人机节点u机体下的投影;
Figure RE-FDA00024455011700000217
为无人机节点u,t之间的相对距离估计值在无人机节点u机体系下的投影;Ju表示无人机节点u的惯量矩阵;
Figure RE-FDA00024455011700000218
为节点u惯量矩阵的逆矩阵;
Figure RE-FDA00024455011700000219
表示为无人机节点u,t之间的相对速度误差在无人机节点u机体系下的投影;δεra表示为零均值的加速度计一阶马尔科夫过程白噪声;wa为零均值的加速度计白噪声;
3)相对姿态四元数误差方程如下式表示:
Figure RE-FDA0002445501170000031
其中
Figure RE-FDA0002445501170000032
为无人机节点u相对无人机节点t的角速度估计值在无人机节点u机体坐标系下的投影;δqrel为无人机节点间的相对四元数;δεrg为陀螺一阶马尔科夫过程白噪声;δεbg为陀螺随机常数;wg为陀螺零均值的白噪声;
4)惯导噪声误差方程;
陀螺的噪声主要考虑陀螺随机常数、陀螺一阶马尔科夫驱动白噪声、陀螺白噪声,即:
ε=εbgrg+wg
其中εbg为陀螺的随机常数,εrg为陀螺一阶马尔科夫过程随机噪声,wg为陀螺白噪声;
考虑无人机节点机载三轴陀螺误差类型相同,则陀螺随机常数误差数学表达式表示为:
Figure RE-FDA0002445501170000033
陀螺一阶马尔科夫驱动白噪声误差数学表示式表示为:
Figure RE-FDA0002445501170000034
其中εrg为陀螺一阶马尔科夫过程随机噪声,Tg为陀螺相关时间常数,wrg为陀螺一阶马尔科夫过程驱动白噪声;
加速度计噪声误差方程
加速度计的噪声误差仅考虑一阶马尔科夫过程,同样假定加速度计三轴的误差模型相同,则加速度计噪声误差方程表示为:
Figure RE-FDA0002445501170000035
其中▽ra为加速度计一阶马尔科夫过程,Ta为加速度计相关时间常数,wag为加速度计一阶马尔科夫过程驱动白噪声。
4.根据权利要求1所述的一种基于惯性/数据链的无人机编队相对导航方法,其特征在于,步骤三的具体过程如下:
a)解算获得惯导多边形与量测多边形双中心转换矩阵B(tk),表示为:
Figure RE-FDA0002445501170000041
上式中D(tk)为无人机节点相对测距构成的相对距离矩阵
Figure RE-FDA0002445501170000042
其中dut表示无人机节点u与无人机节点t相对距离;
Figure RE-FDA0002445501170000043
其中I为n维单位向量,K为n维向量的元素;
b)解算双中心转换矩阵的特征值和特征向量
B(tk)=V(tk)Λ(tk)V(tk)T
其中Λ(tk)=diag(λ12,...λn)为矩阵B(tk)的特征值对应的对角矩阵,λ12,...λn矩阵B(tk)的特征值,V(tk)=[ν12,...,νn]为特征值所对应的特征向量,其中λ1特征值对应的特征向量为ν1,λ2特征值对应的特征向量为ν2,λn特征值对应的特征向量为νn,且λ1≥λ2≥...≥λn≥0;
c)解算编队中各无人机节点初始位置坐标:
各无人机节点的初始坐标由下式计算得到:
Figure RE-FDA0002445501170000044
Figure RE-FDA0002445501170000045
其中n≥7
其中:
Figure RE-FDA0002445501170000046
为无人机节点1k时刻三维初始位置;
Figure RE-FDA0002445501170000047
为无人机节点2k时刻三维初始位置;
Figure RE-FDA0002445501170000048
为无人机节点n k时刻初始位置;Λ3(tk)为k时刻双中心变换矩阵B(tk)前三个最大特征值构成的对角矩阵;VP(tk)=[ν123]为k时刻双中心变换矩阵B(tk)前三个最大特征值所对应的特征向量。
5.根据权利要求1所述的一种基于惯性/数据链的无人机编队相对导航方法,其特征在于,步骤四的具体过程如下:
卡尔曼滤波滤波时间更新以及量测更新过程如下所示:
状态一步预测方程:
Figure RE-FDA0002445501170000051
状态估值方程:
Figure RE-FDA0002445501170000052
滤波增益方程:
Figure RE-FDA0002445501170000053
一步预测均方误差方程:
Figure RE-FDA0002445501170000054
估计均方误差方程:
Figure RE-FDA0002445501170000055
上式中Φk,k-1为系统k-1到k时刻状态转移矩阵;
Figure RE-FDA0002445501170000056
为系统k-1到k时刻状态转移矩阵的转置;
Figure RE-FDA0002445501170000057
为系统k-1时刻状态估计值;
Figure RE-FDA0002445501170000058
为k时刻状态估计值;Zk为k时刻系统量测值;Pk/k-1为k-1时刻到k时刻预测协方差;Pk-1为k-1时刻系统协方差;Pk/k为k时刻估计协方差;
Figure RE-FDA0002445501170000059
为k时刻滤波增益的转置;Kk为系统k时刻滤波增益;Hk为系统k时刻量测矩阵;Qk-1为系统k-1时刻系统噪声阵;Γk,k-1为系统k-1到k时刻系统输入矩阵;Rk为系统k时刻量测噪声矩阵;
Figure RE-FDA00024455011700000510
为k时刻量测矩阵转置;
Figure RE-FDA00024455011700000511
为系统k-1到k时刻系统输入矩阵的转置。
CN201911282280.9A 2019-12-13 2019-12-13 一种基于惯性/数据链的无人机编队相对导航方法 Active CN111238469B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201911282280.9A CN111238469B (zh) 2019-12-13 2019-12-13 一种基于惯性/数据链的无人机编队相对导航方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201911282280.9A CN111238469B (zh) 2019-12-13 2019-12-13 一种基于惯性/数据链的无人机编队相对导航方法

Publications (2)

Publication Number Publication Date
CN111238469A true CN111238469A (zh) 2020-06-05
CN111238469B CN111238469B (zh) 2023-09-29

Family

ID=70875837

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201911282280.9A Active CN111238469B (zh) 2019-12-13 2019-12-13 一种基于惯性/数据链的无人机编队相对导航方法

Country Status (1)

Country Link
CN (1) CN111238469B (zh)

Cited By (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112013839A (zh) * 2020-08-18 2020-12-01 重庆交通大学 一种gps拒止环境下无人机集群实时定位方法
CN112801160A (zh) * 2021-01-22 2021-05-14 中国人民解放军国防科技大学 基于蜂窝图模型的无人机编队脆弱性分析方法及系统
CN112985391A (zh) * 2021-04-19 2021-06-18 中国人民解放军国防科技大学 一种基于惯性和双目视觉的多无人机协同导航方法和装置
CN114526735A (zh) * 2022-04-24 2022-05-24 南京航空航天大学 一种无人飞行器集群仅测距初始相对位姿确定方法

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102620736A (zh) * 2012-03-31 2012-08-01 贵州贵航无人机有限责任公司 一种无人机的导航方法
CN105021184A (zh) * 2015-07-08 2015-11-04 西安电子科技大学 一种用于移动平台下视觉着舰导航的位姿估计系统及方法
CN106595688A (zh) * 2016-12-08 2017-04-26 济南佰意兴网络科技有限公司 一种多agv导向和动态路径规划方法
CN109813311A (zh) * 2019-03-18 2019-05-28 南京航空航天大学 一种无人机编队协同导航方法
CN110207691A (zh) * 2019-05-08 2019-09-06 南京航空航天大学 一种基于数据链测距的多无人车协同导航方法
CN110398980A (zh) * 2019-06-05 2019-11-01 西安电子科技大学 一种无人机群协同探测及避障的航迹规划方法

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102620736A (zh) * 2012-03-31 2012-08-01 贵州贵航无人机有限责任公司 一种无人机的导航方法
CN105021184A (zh) * 2015-07-08 2015-11-04 西安电子科技大学 一种用于移动平台下视觉着舰导航的位姿估计系统及方法
CN106595688A (zh) * 2016-12-08 2017-04-26 济南佰意兴网络科技有限公司 一种多agv导向和动态路径规划方法
CN109813311A (zh) * 2019-03-18 2019-05-28 南京航空航天大学 一种无人机编队协同导航方法
CN110207691A (zh) * 2019-05-08 2019-09-06 南京航空航天大学 一种基于数据链测距的多无人车协同导航方法
CN110398980A (zh) * 2019-06-05 2019-11-01 西安电子科技大学 一种无人机群协同探测及避障的航迹规划方法

Cited By (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112013839A (zh) * 2020-08-18 2020-12-01 重庆交通大学 一种gps拒止环境下无人机集群实时定位方法
CN112801160A (zh) * 2021-01-22 2021-05-14 中国人民解放军国防科技大学 基于蜂窝图模型的无人机编队脆弱性分析方法及系统
CN112801160B (zh) * 2021-01-22 2022-09-06 中国人民解放军国防科技大学 基于蜂窝图模型的无人机编队脆弱性分析方法及系统
CN112985391A (zh) * 2021-04-19 2021-06-18 中国人民解放军国防科技大学 一种基于惯性和双目视觉的多无人机协同导航方法和装置
CN114526735A (zh) * 2022-04-24 2022-05-24 南京航空航天大学 一种无人飞行器集群仅测距初始相对位姿确定方法
CN114526735B (zh) * 2022-04-24 2022-08-05 南京航空航天大学 一种无人飞行器集群仅测距初始相对位姿确定方法

Also Published As

Publication number Publication date
CN111238469B (zh) 2023-09-29

Similar Documents

Publication Publication Date Title
CN111238469B (zh) 一种基于惯性/数据链的无人机编队相对导航方法
Kim et al. Autonomous airborne navigation in unknown terrain environments
Jun et al. State estimation of an autonomous helicopter using Kalman filtering
CN107014376B (zh) 一种适用于农业机械精准作业的姿态倾角估计方法
CN111380514A (zh) 机器人位姿估计方法、装置、终端及计算机存储介质
Taylor et al. Comparison of two image and inertial sensor fusion techniques for navigation in unmapped environments
CN110849360B (zh) 面向多机协同编队飞行的分布式相对导航方法
Goppert et al. Invariant Kalman filter application to optical flow based visual odometry for UAVs
CN111207745A (zh) 一种适用于大机动无人机垂直陀螺仪的惯性测量方法
CN112146655A (zh) 一种BeiDou/SINS紧组合导航系统弹性模型设计方法
CN111504323A (zh) 基于异源图像匹配与惯性导航融合的无人机自主定位方法
JPH095104A (ja) 移動物体の三次元姿勢角測定法および三次元姿勢角計測装置
Kritskiy et al. Increasing the reliability of drones due to the use of quaternions in motion
Patel et al. Multi-IMU Based Alternate Navigation Frameworks: Performance & Comparison for UAS
CN112254723B (zh) 基于自适应ekf算法的小型无人机marg航姿估计方法
Emran et al. A cascaded approach for quadrotor's attitude estimation
CN114435630B (zh) 一种利用有限次视线测量对非合作目标进行相对定轨的方法
CN115388890A (zh) 基于视觉的多无人机协同对地目标定位方法
Soloviev et al. Fusion of inertial, optical flow, and airspeed measurements for UAV navigation in GPS-denied environments
Huang et al. Integration of MEMS inertial sensor-based GNC of a UAV
CN111473786A (zh) 一种基于局部反馈的两层分布式多传感器组合导航滤波方法
Jun et al. State estimation via sensor modeling for helicopter control using an indirect kalman filter
Taylor Fusion of inertial, vision, and air pressure sensors for MAV navigation
CN116192571B (zh) 一种波束抖动效应下无人机isac信道估计方法
CN109798884B (zh) 一种多旋翼无人机磁力计动态实时校准方法

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant