一种基于伪观测的异构飞行集群惯导误差协同修正方法技术

本发明专利技术公开了一种基于伪观测的异构飞行集群惯导误差协同修正方法，由多架无人机组成无人机集群，无人机间相互观测，包括以下步骤：S1，获取当前时刻各无人机惯导位置和惯导位置估计噪声；S2，选定基准锚点，获得集群内部无人机间相对距离观测、相对方位角观测和相对俯仰角观测；S3，根据步骤S2获得的观测数据，建立异构集群无人机观测方程；S4，根据观测方程是否充足，构建伪观测方程；S5，根据观测矩阵的秩进行分类，并求解观测方程；S6，更新各无人机的惯导位置、姿态和速度。本发明专利技术通过修正后的位置信息与惯导信息组合对导航位置姿态速度进行修正更新，可用观测量不足时取得较好的集群整体定位精度。体定位精度。体定位精度。

【技术实现步骤摘要】
一种基于伪观测的异构飞行集群惯导误差协同修正方法


[0001]本专利技术涉及惯导误差协同修正方法，尤其涉及一种基于伪观测的异构飞行集群惯导误差协同修正方法。

技术介绍

[0002]无人机集群技术是未来无人机技术发展方向之一，具有可靠性高、任务完成率高、可执行更加复杂任务等特点。无人机集群技术并不是简单的多无人机编队飞行，而是多无人机高度有序组成智能群体，通过无人机间相互协作及自主调整来共同完成某项复杂多变的任务。
[0003]集群无人机较高的任务完成率离不开高精度导航技术的支持，在现有技术条件下，高精度的惯性导航设备价格昂贵，经济上难以实现在集群无人机的每一架无人机都装备高精度惯导，而依赖GNSS(全球导航卫星系统)修正惯导误差又受到城市等环境中GNSS信号可能发生拒止的问题的影响，此时异构多无人机集群中装备低精度惯导的无人机往往会出现导航误差发散过快的问题。对于某些装备低精度惯导的无人机群，其在GNSS信号拒止环境下，很难获得良好的位置信息，其定位误差会随着时间而发散。传统协同方法主要针对误差稳定状态下的定位信息进行融合处理，拒止环境下无人集群中惯导误差始终处于快速发散状态，传统方法难对误差进行有效修正甚至加剧误差发散速度。

技术实现思路

[0004]专利技术目的：本专利技术的目的是提供一种能解决因观测量不足造成的惯导定位误差发散问题的基于伪观测的异构飞行集群惯导误差协同修正方法。
[0005]技术方案：一种基于伪观测的异构飞行集群惯导误差协同修正方法，由多架无人机组成无人机集群，无人机间相互观测，包括以下步骤：
[0006]S1，获取当前时刻各无人机惯导位置和惯导位置估计噪声；
[0007]S2，选定基准锚点，获得集群内部无人机间相对距离观测、相对方位角观测和相对俯仰角观测；
[0008]S3，根据步骤S2获得的观测数据，建立异构集群无人机观测方程；
[0009]S4，根据观测方程是否充足，构建伪观测方程；
[0010]S5，根据步骤S3观测方程中观测矩阵的秩进行分类，并求解观测方程；
[0011]S6，更新各无人机的惯导位置、姿态和速度。
[0012]进一步，步骤S1中，获取当前时刻各无人机惯导位置和惯导位置估计噪声的具体步骤如下：
[0013]S11，通过无人机的机载惯导数据，获取当前时刻各无人机的惯导位置，在地心地固坐标系中表示为其中i表示第i个无人机，i＝1,
…
,n，n为集群中无人机的数量；
[0014]S12，获取当前时刻无人机的惯导位置估计噪声，其在地心地固坐标系中表示为
[0015]进一步，步骤S2中，选定锚点的原则如下：
[0016]在高空非GNSS拒止区域，以高空自主GNSS信息的无人机作为锚点k；
[0017]在GNSS拒止区域内，无人机通过机载通信系统获取锚点k的地心地固坐标(x
k
,y
k
,z
k
)；通过机载测距传感器获取锚点k对无人机i的相对距离观测相对方位角观测和相对俯仰角观测m为基准锚点的数量；
[0018]通过各无人机装备的测距测角传感器，获得集群内部无人机i与无人机j间的相对距离观测相对方位角观测和相对俯仰角观测
[0019]进一步，步骤S3中，建立异构集群无人机观测模型表达式如下：
[0020]Y＝HX+V
[0021]其中，H为测距观测矩阵，V为测距观测误差矩阵，Y为测距观测向量；X为集群无人机惯导位置误差状态向量，表达式如下：
[0022][0023][0024]其中，(x
i
,y
i
,z
i
)表示无人机的实际位置。
[0025]进一步，步骤S4中，根据观测方程是否充足，构建伪观测方程的详细步骤如下：
[0026]S41，计算步骤S3中得到的观测矩阵的秩r，则有r＝rank[H]；
[0027]若r≥3
×
n，锚点充足，观测方程充足，求解公式如下：
[0028][0029]测量权阵P
Y
表示如下：
[0030]分别为距离测量噪声、方位角测量噪声和俯仰角测量噪声；
[0031]若r<3
×
n，观测方程不充足，需要构建伪观测方程，执行步骤S42；
[0032]S42，计算观测方程不充足数t＝3
×
n
‑
r，为伪观测方程数；
[0033]S43，求取H
T
P
Y
H的全部零特征值特征向量e1,e2,
…
,e
t
；
[0034]S44，构造伪观测方程如下：
[0035][0036]其中，H
e
为伪观测矩阵，V
e
为伪观测误差矩阵。
[0037]进一步，步骤S5中，根据观测矩阵的秩进行分类，并求解观测方程，详细实现步骤如下：
[0038]S51，根据获取的无人机的惯导位置估计噪声则惯导误差权阵变换矩阵设置为：
[0039][0040]S52，求解集群各无人机的惯导位置误差：
[0041]若观测矩阵的秩r≥3
×
n，求解公式如下：
[0042][0043]若观测矩阵的秩r<3
×
n，利用步骤S44中构造的伪观测方程解决观测不充足问题，则求解公式如下：
[0044][0045]其中，为集群无人机惯导位置误差状态向量估计值；
[0046]将在地心地固坐标系下的表示转换为经度误差、纬度误差和高度误差：表达式如下：
[0047][0048]其中，λ、L、h分别为无人机的经度、纬度和高度，R
N
、f分别为地球卯酉圈曲率半径和地球扁率；
[0049]得到修正后的无人机i惯导位置的求解公式如下：
[0050][0051]其中，为无人机i未修正的惯导位置。
[0052]进一步，步骤S6中，更新各无人机惯导位置、姿态和速度的详细步骤如下：
[0053]S61，若观测矩阵的秩r≥m，则执行步骤S62；
[0054]若观测矩阵的秩r＜m，不进行各无人机惯导位置姿态和速度的更新；
[0055]S62，基于待辅助飞行器的惯导信息，建立18维的状态方程：
[0056][0057]其中，F
I
为状态系数矩阵，G
I
为误差系数矩阵，W
I
为白噪声随机误差矢量；状态矢量X
I
：
[0058][0059]其中，分别为惯性平台误差角在地理系东、北、天方向分量；分别为速度误差角在地理系东、北、天方向分量；δL、δλ、δh分别为经度误差、纬度误差和高度误差；ε
bx
、ε
by
、ε
bz...

【技术保护点】

【技术特征摘要】
1.一种基于伪观测的异构飞行集群惯导误差协同修正方法，其特征在于，由多架无人机组成无人机集群，无人机间相互观测，包括以下步骤：S1，获取当前时刻各无人机惯导位置和惯导位置估计噪声；S2，选定基准锚点，获得集群内部无人机间相对距离观测、相对方位角观测和相对俯仰角观测；S3，根据步骤S2获得的观测数据，建立异构集群无人机观测方程；S4，根据观测方程是否充足，构建伪观测方程；S5，根据步骤S3观测方程中观测矩阵的秩进行分类，并求解观测方程；S6，更新各无人机的惯导位置、姿态和速度。2.根据权利要求1所述基于伪观测的异构飞行集群惯导误差协同修正方法，其特征在于，步骤S1中，获取当前时刻各无人机惯导位置和惯导位置估计噪声的具体步骤如下：S11，通过无人机的机载惯导数据，获取当前时刻各无人机的惯导位置，在地心地固坐标系中表示为其中i表示第i个无人机，i＝1,
…
,n，n为集群中无人机的数量；S12，获取当前时刻无人机的惯导位置估计噪声，其在地心地固坐标系中表示为3.根据权利要求2所述基于伪观测的异构飞行集群惯导误差协同修正方法，其特征在于，步骤S2中，选定锚点的原则如下：在高空非GNSS拒止区域，以高空自主GNSS信息的无人机作为锚点k；在GNSS拒止区域内，无人机通过机载通信系统获取锚点k的地心地固坐标(x
k
,y
k
,z
k
)；通过机载测距传感器获取锚点k对无人机i的相对距离观测相对方位角观测和相对俯仰角观测m为基准锚点的数量；通过各无人机装备的测距测角传感器，获得集群内部无人机i与无人机j间的相对距离观测相对方位角观测和相对俯仰角观测4.根据权利要求2所述基于伪观测的异构飞行集群惯导误差协同修正方法，其特征在于，步骤S3中，建立异构集群无人机观测模型表达式如下：Y＝HX+V其中，H为测距观测矩阵，V为测距观测误差矩阵，Y为测距观测向量；X为集群无人机惯导位置误差状态向量，表达式如下：导位置误差状态向量，表达式如下：其中，(x
i
,y
i
,z
i
)表示无人机的实际位置。5.根据权利要求4所述基于伪观测的异构飞行集群惯导误差协同修正方法，其特征在于，步骤S4中，根据观测方程是否充足，构建伪观测方程的详细步骤如下：S41，计算步骤S3中得到的观测矩阵的秩r，则有r＝rank[H]；若r≥3
×
n，锚点充足，观测方程充足，求解公式如下：
测量权阵P
Y
表示如下：分别为距离测量噪声、方位角测量噪声和俯仰角测量噪声；若r<3
×
n，观测方程不充足，需要构建伪观测方程，执行步骤S42；S42，计算观测方程不充足数t＝3
×
n
‑
r，为伪观测方程数；S43，求取H
T
P
Y
H的全部零特征值特征向量e1,e2,
...

【专利技术属性】
技术研发人员：王融，张慧媛，顾晨，熊智，刘建业，赵惟成，何辉，刘瑶凯，王思晨，
申请(专利权)人：南京航空航天大学，
类型：发明
国别省市：