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.)
Pending
Application number
CN201911282280.9A
Other languages
English (en)
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/CN111238469A/zh
Publication of CN111238469A publication Critical patent/CN111238469A/zh
Pending legal-status Critical Current

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; 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/16—Navigation; 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/165—Navigation; 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
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B64—AIRCRAFT; AVIATION; COSMONAUTICS
    • B64C—AEROPLANES; HELICOPTERS
    • B64C39/00—Aircraft not otherwise provided for
    • B64C39/02—Aircraft not otherwise provided for characterised by special use
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B64—AIRCRAFT; AVIATION; COSMONAUTICS
    • B64U—UNMANNED AERIAL VEHICLES [UAV]; EQUIPMENT THEREFOR
    • B64U2201/00—UAVs characterised by their flight controls
    • B64U2201/10—UAVs 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/102—UAVs 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时刻无人机节点传感器数据，包括陀螺仪信息加速度计信息其中 i＝7,8…,N以及无人机节点间相对距离dut，其中u,t表示不同的无人机节点；
步骤二：定义无人机节点机体坐标系及导航坐标系并建立无人机节点u,t之间相对位置、相对速度、相对姿态四元数误差方程、陀螺噪声误差方程，加速度计噪声误差方程；
步骤三：利用无人机节点惯导输出构成的惯导多边型与机载数据链相对测距构成的量测多边形计算k时刻编队中各无人机节点初始位置坐标；
步骤四：利用卡尔曼滤波器对k时刻无人机节点间的相对位置信息进行校正。
步骤二中所述无人机节点机体坐标系及导航坐标系定义如下：机体坐标系的 X、Y、Z轴分别为无人机的右向、前向和上向，导航坐标系分别为东、北、天地理坐标系。
步骤二中采取如下方式建立无人机节点u,t之间的相对位置运动方程、相对速度误差方程、相对姿态四元数误差方程，陀螺噪声误差方程，加速度计噪声误差方程：
1)无人机节点u,t之间的相对位置运动方程
其中表示无人机节点u机体坐标系相对于无人机节点t的机体坐标系的位置在无人机u坐标系下的投影，可表示为：其中为惯性坐标系到无人机节点u机体系的坐标转换矩阵；为无人机节点u机体坐标系相对惯性坐标系的位置在惯性坐标系下的投影；为无人机节点t机体坐标系相对惯性坐标系下的位置在惯性坐标系下的投影；
Figure RE-GDA0002445501180000037
为无人机节点u机体系相对于惯性系的角速度在机体系上的投影；为无人机节点u机体系下的比力；为无人机节点t机体系下的比力；为无人机节点t机体系到无人机节点u机体系的坐标转换矩阵；
Figure RE-GDA00024455011800000311
为无人机节点u机体系相对惯性系的角速率在机体系下的投影；为无人机节点u相对无人机节点t的速度在无人机节点u机体下的投影；
Figure RE-GDA00024455011800000313
为无人机节点u相对无人机节点t的加速度在无人机节点u机体坐标系下的投影；
2)无人机节点u,t之间的相对速度误差方程如下式表示：
其中为无人机节点u机体系相对惯性系的角速率估计值在机体系下的投影；为无人机节点u机体系相对惯性系角速度估计值在机体下的投影；为陀螺随机常数误差，为陀螺一阶马尔科夫过程白噪声，为无人机节点u,t之间的相对位置误差在无人机节点u机体系下的投影；为无人机节点t到无人机节点u姿态转换矩阵的估计值；
Figure RE-GDA0002445501180000042
为无人机节点u机体坐标系相对惯性坐标系的角速度的估计值在无人机节点u机体下的投影；为无人机节点u,t之间的相对距离估计值在无人机节点u机体系下的投影；Ju表示无人机节点u的惯量矩阵；为节点u惯量矩阵的逆矩阵；表示为无人机节点u,t之间的相对速度误差在无人机节点u机体系下的投影；δεra表示为零均值的加速度计一阶马尔科夫过程白噪声；wa为零均值的加速度计白噪声；
3)相对姿态四元数误差方程如下式表示：
其中为无人机节点u相对无人机节点t的角速度估计值在无人机节点u机体坐标系下的投影；δqrel为无人机节点间的相对四元数；δεrg为陀螺一阶马尔科夫过程白噪声；δεbg为陀螺随机常数；wg为陀螺零均值的白噪声；
4)惯导噪声误差方程；
陀螺的噪声主要考虑陀螺随机常数、陀螺一阶马尔科夫驱动白噪声、陀螺白噪声，即：
ε＝εbg+εrg+wg
其中εbg为陀螺的随机常数，εrg为陀螺一阶马尔科夫过程随机噪声，wg为陀螺白噪声；
考虑无人机节点机载三轴陀螺误差类型相同，则陀螺随机常数误差数学表达式表示为：
陀螺一阶马尔科夫驱动白噪声误差数学表示式表示为：
其中εrg为陀螺一阶马尔科夫过程随机噪声，Tg为陀螺相关时间常数，wrg为陀螺一阶马尔科夫过程驱动白噪声；
加速度计噪声误差方程
加速度计的噪声误差仅考虑一阶马尔科夫过程，同样假定加速度计三轴的误差模型相同，则加速度计噪声误差方程表示为：
其中▽ra为加速度计一阶马尔科夫过程，Ta为加速度计相关时间常数，wag为加速度计一阶马尔科夫过程驱动白噪声。
步骤三的具体过程如下：
a)解算获得惯导多边形与量测多边形双中心转换矩阵B(tk)，表示为：
上式中D(tk)为无人机节点相对测距构成的相对距离矩阵其中dut表示无人机节点u与无人机节点t相对距离；其中I为n维单位向量，K为n维向量的元素；
b)解算双中心转换矩阵的特征值和特征向量
B(tk)＝V(tk)Λ(tk)V(tk)T
其中Λ(tk)＝diag(λ1,λ2,...λn)为矩阵B(tk)的特征值对应的对角矩阵，λ1,λ2,...λn矩阵B(tk)的特征值，V(tk)＝[ν1,ν2,...,νn]为特征值所对应的特征向量，其中λ1特征值对应的特征向量为ν1，λ2特征值对应的特征向量为ν2，λn特征值对应的特征向量为νn，且λ1≥λ2≥…≥λn≥0；
c)解算编队中各无人机节点初始位置坐标：
各无人机节点的初始坐标由下式计算得到：
其中n≥7
其中：为无人机节点1k时刻三维初始位置；
Figure RE-GDA0002445501180000063
为无人机节点2k时刻三维初始位置；为无人机节点n k时刻初始位置；Λ3(tk)为k时刻双中心变换矩阵B(tk)前三个最大特征值构成的对角矩阵；VP(tk)＝[ν1,ν2,ν3]为k时刻双中心变换矩阵B(tk)前三个最大特征值所对应的特征向量。
步骤四的具体过程如下：
卡尔曼滤波滤波时间更新以及量测更新过程如下所示：
状态一步预测方程：
Figure RE-GDA0002445501180000065
状态估值方程：
滤波增益方程：
一步预测均方误差方程：
估计均方误差方程：
上式中Φk,k-1为系统k-1到k时刻状态转移矩阵；为系统k-1到k时刻状态转移矩阵的转置；为系统k-1时刻状态估计值；为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时刻量测矩阵转置；为系统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刻无人机节点传感器数据，包括陀螺仪信息加速度计信息其中i＝7,8…,N以及无人机节点间相对距离dut，其中u, t示不同的无人机节点。
步骤二：定义无人机节点机体坐标系及导航坐标系并建立无人机节点u,t间相对位置、相对速度、相对姿态四元数误差方程、陀螺噪声误差方程，加速度计噪声误差方程。
步骤三：利用无人机节点惯导输出构成的惯导多边型与机载数据链相对测距构成的量测多边形计算k时刻编队中各无人机节点初始位置坐标。
步骤四：利用卡尔曼滤波器对k时刻无人机节点间的相对位置信息进行校正。
相关坐标系定义为：机体系的X、Y、Z轴分别为无人机的右向、前向(无人机机头方向)、上向，导航坐标系分别为东、北、天地理坐标系。
步骤二中采取如下方式解算得到无人机节点u,t之间的相对位置运动方程、相对速度误差方程、相对姿态四元数误差方程，陀螺噪声误差方程，加速度计噪声误差方程。
1)无人机节点u,t之间的相对位置运动方程
其中表示无人机节点u机体坐标系相对于无人机节点t的机体坐标系的位置在无人机u坐标系下的投影，可表示为：其中
Figure RE-GDA0002445501180000093
为惯性坐标系到无人机节点u机体系的坐标转换矩阵；
Figure RE-GDA0002445501180000094
为无人机节点u机体坐标系相对惯性坐标系的位置在惯性坐标系下的投影；为无人机节点t机体坐标系相对惯性坐标系下的位置在惯性坐标系下的投影；为无人机节点u机体系相对于惯性系的角速度在机体系上的投影；
Figure RE-GDA0002445501180000097
为无人机节点u机体系下的比力；
Figure RE-GDA0002445501180000098
为无人机节点t机体系下的比力；
Figure RE-GDA0002445501180000099
为无人机节点t机体系到无人机节点u机体系的坐标转换矩阵；为无人机节点u机体系相对惯性系的角速率在机体系下的投影；为无人机节点u相对无人机节点t的速度在无人机节点u机体下的投影；为无人机节点u相对无人机节点t的加速度在无人机节点u机体坐标系下的投影；
无人机节点u,t之间的相对速度误差方程
无人机节点u,t之间的相对速度误差方程可用如下式表示：
Figure RE-GDA00024455011800000913
其中为无人机节点u机体系相对惯性系的角速率估计值在机体系下的投影；
Figure RE-GDA00024455011800000915
为无人机节点u机体系相对惯性系角速度估计值在机体下的投影；
Figure RE-GDA0002445501180000101
为陀螺随机常数误差，为陀螺一阶马尔科夫过程白噪声，
Figure RE-GDA0002445501180000103
为无人机节点u,t之间的相对位置误差在无人机节点u机体系下的投影；为无人机节点t到无人机节点u姿态转换矩阵的估计值；
Figure RE-GDA0002445501180000105
为无人机节点u机体坐标系相对惯性坐标系的角速度的估计值在无人机节点u机体下的投影；为无人机节点u,t之间的相对距离估计值在无人机节点u机体系下的投影；Ju表示无人机节点u的惯量矩阵；为节点u惯量矩阵的逆矩阵；表示为无人机节点u,t之间的相对速度误差在无人机节点u机体系下的投影；δεra表示为零均值的加速度计一阶马尔科夫过程白噪声；wa为零均值的加速度计白噪声。
2)相对姿态四元数误差方程
相对姿态四元数误差方程可用如下式表示：
其中为无人机节点u相对无人机节点t的角速度估计值在无人机节点u机体坐标系下的投影；δqrel为无人机节点间的相对四元数；δεrg为陀螺一阶马尔科夫过程白噪声；δεbg为陀螺随机常数；wg为陀螺零均值的白噪声。
3)惯导噪声误差方程；
陀螺噪声误差方程
陀螺的噪声这里主要考虑陀螺随机常数、陀螺一阶马尔科夫驱动白噪声、陀螺白噪声。即：
ε＝εbg+εrg+wg
其中εbg为陀螺的随机常数，εrg为陀螺一阶马尔科夫过程随机噪声，wg为陀螺白噪声。
考虑无人机节点机载三轴陀螺误差类型相同，则陀螺随机常数误差数学表达式可表示为：
陀螺一阶马尔科夫驱动白噪声误差数学表示式可表示为：
其中εrg为陀螺一阶马尔科夫过程随机噪声，Tg为陀螺相关时间常数，wrg为陀螺一阶马尔科夫过程驱动白噪声。
加速度计噪声误差方程
加速度计的噪声误差这里仅考虑一阶马尔科夫过程，同样假定加速度计三轴的误差模型相同，则加速度计噪声误差方程可表示为：
其中▽ra为加速度计一阶马尔科夫过程，Ta为加速度计相关时间常数，wag为加速度计一阶马尔科夫过程驱动白噪声。
步骤三利用无人机节点惯导输出构成的惯导多边形与机载数据链相对测距构成的量测多边形计算k时刻编队中各无人机节点的初始位置坐标。
1)根据无人机节点惯导输出构成的惯导多边形与机载数据链相对测距构成的量测多边形利用多边形重心一致性得到双中心转换矩阵B(tk)，可表示为：
上式中D(tk)为无人机节点相对测距构成的相对距离矩阵其dut表示无人机节点u与无人机节点t相对距离；其中I为n维单位向量，K为n维向量的元素。
2)解算双中心转换矩阵的特征值和特征向量
B(tk)＝V(tk)Λ(tk)V(tk)T
其中Λ(tk)＝diag(λ1,λ2,...λn)为矩阵B(tk)的特征值对应的对角矩阵，λ1,λ2,...λn矩阵B(tk)的特征值，V(tk)＝[ν1,ν2,...,νn]为特征值所对应的特征向量，其中λ1特征值对应的特征向量为ν1，λ2特征值对应的特征向量为ν2，λn特征值对应的特征向量为νn，且λ1≥λ2≥...≥λn≥0。
3)解算编队中各无人机节点初始位置坐标：
由于无人机初始位置坐标为三维坐标，选取双中心变换矩阵B(tk) 前三个最大的特征值，则构成的对角矩阵可表示为：Λ3(tk)＝diag(λ1,λ2,λ3)，特征值所对应的特征向量为VP(tk)＝[ν1,ν2,ν3]，其中ν1为特征值λ1对应的特征向量，ν2为特征值λ2对应的特征向量，ν3为特征值λ3对应的特征向量，则各无人机节点的初始坐标可由下式计算得到：
其中n≥7。
其中：为无人机节点1k时刻三维初始位置；为无人机节点2k时刻三维初始位置；为无人机节点n k时刻初始位置；Λ3(tk)为k时刻双中心变换矩阵B(tk)前三个最大特征值构成的对角矩阵； VP(tk)＝[ν1,ν2,ν3]为k时刻双中心变换矩阵B(tk)前三个最大特征值所对应的特征向量。
步骤四中利用卡尔曼滤波器对k时刻无人机节点间的相对位置信息进行校正。
卡尔曼滤波滤波时间更新以及量测更新过程如下所示：
状态一步预测方程：
状态估值方程：
滤波增益方程：
Figure RE-GDA0002445501180000137
一步预测均方误差方程：
估计均方误差方程：
Figure RE-GDA0002445501180000139
上式中Φk,k-1为系统k-1到k时刻状态转移矩阵；为系统k-1到k时刻状态转移矩阵的转置；
Figure RE-GDA0002445501180000141
为系统k-1时刻状态估计值；为k时刻状态估计值；Pk/k-1为k-1时刻到k时刻预测协方差；Pk-1为k-1时刻系统协方差；Pk/k为k时刻估计协方差；为k时刻滤波增益的转置；Kk为系统k 时刻滤波增益；Hk为系统k时刻量测矩阵；Qk-1为系统k-1时刻系统噪声阵；Γk,k-1为系统k-1到k时刻系统输入矩阵；Rk为系统k时刻量测噪声矩阵；为k时刻量测矩阵转置；为系统k-1到k时刻系统输入矩阵的转置；
表1无人机节点机载纯惯导递推与基于构型的无人机节点位置1h发散程度对比
Figure RE-GDA0002445501180000146
由图2,3,4,5,6,7可知无人机编队中系统稳态时无人机节点1相对其它无人机节点相对距离误差保持在3米以内，其它无人机节点与无人机节点1类似。由表1可知通过本发明提出的算法各无人机节点机载惯导系统位置输出与机载惯导系统纯惯导递推位置输出精度提高了至少30％以上，有效验证了本发明所提出算法的有效性及可行性。

Claims (5)

1.一种基于惯性/数据链的无人机编队相对导航方法，其特征在于，包括如下步骤：
步骤一：无人机节点搭载惯性导航系统，数据链收发装置，周期性读取k时刻无人机节点传感器数据，包括陀螺仪信息加速度计信息其中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
其中表示无人机节点u机体坐标系相对于无人机节点t的机体坐标系的位置在无人机u坐标系下的投影，表示为：其中为惯性坐标系到无人机节点u机体系的坐标转换矩阵；
Figure RE-FDA0002445501170000017
为无人机节点u机体坐标系相对惯性坐标系的位置在惯性坐标系下的投影；为无人机节点t机体坐标系相对惯性坐标系下的位置在惯性坐标系下的投影；
Figure RE-FDA0002445501170000022
为无人机节点u机体系相对于惯性系的角速度在机体系上的投影；为无人机节点u机体系下的比力；为无人机节点t机体系下的比力；为无人机节点t机体系到无人机节点u机体系的坐标转换矩阵；为无人机节点u机体系相对惯性系的角速率在机体系下的投影；为无人机节点u相对无人机节点t的速度在无人机节点u机体下的投影；为无人机节点u相对无人机节点t的加速度在无人机节点u机体坐标系下的投影；
2)无人机节点u,t之间的相对速度误差方程如下式表示：
其中为无人机节点u机体系相对惯性系的角速率估计值在机体系下的投影；为无人机节点u机体系相对惯性系角速度估计值在机体下的投影；为陀螺随机常数误差，
Figure RE-FDA00024455011700000213
为陀螺一阶马尔科夫过程白噪声，为无人机节点u,t之间的相对位置误差在无人机节点u机体系下的投影；为无人机节点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)相对姿态四元数误差方程如下式表示：
其中为无人机节点u相对无人机节点t的角速度估计值在无人机节点u机体坐标系下的投影；δqrel为无人机节点间的相对四元数；δεrg为陀螺一阶马尔科夫过程白噪声；δεbg为陀螺随机常数；wg为陀螺零均值的白噪声；
4)惯导噪声误差方程；
陀螺的噪声主要考虑陀螺随机常数、陀螺一阶马尔科夫驱动白噪声、陀螺白噪声，即：
ε＝εbg+εrg+wg
其中εbg为陀螺的随机常数，εrg为陀螺一阶马尔科夫过程随机噪声，wg为陀螺白噪声；
考虑无人机节点机载三轴陀螺误差类型相同，则陀螺随机常数误差数学表达式表示为：
陀螺一阶马尔科夫驱动白噪声误差数学表示式表示为：
其中εrg为陀螺一阶马尔科夫过程随机噪声，Tg为陀螺相关时间常数，wrg为陀螺一阶马尔科夫过程驱动白噪声；
加速度计噪声误差方程
加速度计的噪声误差仅考虑一阶马尔科夫过程，同样假定加速度计三轴的误差模型相同，则加速度计噪声误差方程表示为：
Figure RE-FDA0002445501170000035
其中▽ra为加速度计一阶马尔科夫过程，Ta为加速度计相关时间常数，wag为加速度计一阶马尔科夫过程驱动白噪声。
4.根据权利要求1所述的一种基于惯性/数据链的无人机编队相对导航方法，其特征在于，步骤三的具体过程如下：
a)解算获得惯导多边形与量测多边形双中心转换矩阵B(tk)，表示为：
上式中D(tk)为无人机节点相对测距构成的相对距离矩阵其中dut表示无人机节点u与无人机节点t相对距离；其中I为n维单位向量，K为n维向量的元素；
b)解算双中心转换矩阵的特征值和特征向量
B(tk)＝V(tk)Λ(tk)V(tk)T
其中Λ(tk)＝diag(λ1,λ2,...λn)为矩阵B(tk)的特征值对应的对角矩阵，λ1,λ2,...λn矩阵B(tk)的特征值，V(tk)＝[ν1,ν2,...,νn]为特征值所对应的特征向量，其中λ1特征值对应的特征向量为ν1，λ2特征值对应的特征向量为ν2，λn特征值对应的特征向量为νn，且λ1≥λ2≥...≥λn≥0；
c)解算编队中各无人机节点初始位置坐标：
各无人机节点的初始坐标由下式计算得到：
其中n≥7
其中：为无人机节点1k时刻三维初始位置；为无人机节点2k时刻三维初始位置；为无人机节点n k时刻初始位置；Λ3(tk)为k时刻双中心变换矩阵B(tk)前三个最大特征值构成的对角矩阵；VP(tk)＝[ν1,ν2,ν3]为k时刻双中心变换矩阵B(tk)前三个最大特征值所对应的特征向量。
5.根据权利要求1所述的一种基于惯性/数据链的无人机编队相对导航方法，其特征在于，步骤四的具体过程如下：
卡尔曼滤波滤波时间更新以及量测更新过程如下所示：
状态一步预测方程：
状态估值方程：
滤波增益方程：
一步预测均方误差方程：
估计均方误差方程：
Figure RE-FDA0002445501170000055
上式中Φk,k-1为系统k-1到k时刻状态转移矩阵；为系统k-1到k时刻状态转移矩阵的转置；为系统k-1时刻状态估计值；为k时刻状态估计值；Zk为k时刻系统量测值；Pk/k-1为k-1时刻到k时刻预测协方差；Pk-1为k-1时刻系统协方差；Pk/k为k时刻估计协方差；为k时刻滤波增益的转置；Kk为系统k时刻滤波增益；Hk为系统k时刻量测矩阵；Qk-1为系统k-1时刻系统噪声阵；Γk,k-1为系统k-1到k时刻系统输入矩阵；Rk为系统k时刻量测噪声矩阵；为k时刻量测矩阵转置；为系统k-1到k时刻系统输入矩阵的转置。
CN201911282280.9A 2019-12-13 2019-12-13 一种基于惯性/数据链的无人机编队相对导航方法 Pending CN111238469A (zh)

Priority Applications (1)

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

Applications Claiming Priority (1)

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

Publications (1)

Publication Number Publication Date
CN111238469A true CN111238469A (zh) 2020-06-05

Family

ID=70875837

Family Applications (1)

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

Country Status (1)

Country Link
CN (1) CN111238469A (zh)

Cited By (3)

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

Cited By (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
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 南京航空航天大学 一种无人飞行器集群仅测距初始相对位姿确定方法

Similar Documents

Publication Publication Date Title
CN109029433B (zh) 一种移动平台上基于视觉和惯导融合slam的标定外参和时序的方法
CN112347840B (zh) 视觉传感器激光雷达融合无人机定位与建图装置和方法
Kim et al. Autonomous airborne navigation in unknown terrain environments
Kingston et al. Real-time attitude and position estimation for small UAVs using low-cost sensors
Jun et al. State estimation of an autonomous helicopter using Kalman filtering
CN111238469A (zh) 一种基于惯性/数据链的无人机编队相对导航方法
CN107014376B (zh) 一种适用于农业机械精准作业的姿态倾角估计方法
Redding et al. Vision-based target localization from a fixed-wing miniature air vehicle
CN106871928B (zh) 基于李群滤波的捷联惯性导航初始对准方法
CN107314718A (zh) 基于磁测滚转角速率信息的高速旋转弹姿态估计方法
CN111426318A (zh) 基于四元数-扩展卡尔曼滤波的低成本ahrs航向角补偿方法
Taylor et al. Comparison of two image and inertial sensor fusion techniques for navigation in unmapped environments
Wang et al. Vision-aided inertial navigation for small unmanned aerial vehicles in GPS-denied environments
Goppert et al. Invariant Kalman filter application to optical flow based visual odometry for UAVs
CN111207745A (zh) 一种适用于大机动无人机垂直陀螺仪的惯性测量方法
Rhudy et al. Unmanned aerial vehicle navigation using wide-field optical flow and inertial sensors
CN111189442A (zh) 基于cepf的无人机多源导航信息状态预测方法
CN112146655A (zh) 一种BeiDou/SINS紧组合导航系统弹性模型设计方法
Delaune et al. Extended navigation capabilities for a future mars science helicopter concept
Kritskiy et al. Increasing the reliability of drones due to the use of quaternions in motion
JPH095104A (ja) 移動物体の三次元姿勢角測定法および三次元姿勢角計測装置
CN111504323A (zh) 基于异源图像匹配与惯性导航融合的无人机自主定位方法
Soloviev et al. Fusion of inertial, optical flow, and airspeed measurements for UAV navigation in GPS-denied environments
CN111380514A (zh) 机器人位姿估计方法、装置、终端及计算机存储介质
Emran et al. A Cascaded Approach for Quadrotor's Attitude Estimation

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