【技术实现步骤摘要】
基于秩亏约束级联滤波的惯性导航协同方法
[0001]本专利技术涉及惯性导航协同
，具体地，涉及一种基于秩亏约束级联滤波的惯性导航协同方法。
技术介绍
[0002]随着通信以及测距技术的发展，考虑到集群编队具有数量多、散布式、无中心、自组织、成本低、机动灵活等特点，以无人机为代表的各类飞行器开始进入集群编队的时代。
[0003]传统的车辆、水下潜航器、UAV等运动载体在仅采用纯惯性导航时，由于纯惯性导航对惯性器件的性能和导航算法的要求较高，且导航信息通过积分产生，定位误差会随时间快速累积，为解决该问题，传统的协同导航方法提出集群编队内存在一个高精度节点，通过数据链测得的节点相对距离信息，利用高精度节点的位置信息来校正纯惯性的低精度节点，高精度节点依赖惯性/卫星组合导航实现，然而，当卫星导航受干扰无法使用时，高精度节点的精度下降，集群内其他节点的惯性导航精度也将难以保证，这种方法无法有效实施。
[0004]公开号为CN107246868B的专利文献公开了一种协同导航定位系统及导航定位方法，包括传感器数据采集模块、RGB
‑
D图像生成模块、运动学建模模块、零偏修正模块、位姿跟踪模块、全局优化模块和地图存储模块。但是该专利文献仍然存在精度不高的缺陷。
技术实现思路
[0005]针对现有技术中的缺陷，本专利技术的目的是提供一种基于秩亏约束级联滤波的惯性导航协同方法。
[0006]根据本专利技术提供的一种基于秩亏约束级联滤波的惯性导航协同方法，包括如下步骤：>[0007]步骤1：确定飞行器集群网的节点的数量，通过数据链测量各所述节点间的相对距离，得出各所述节点的惯导误差分布；
[0008]步骤2：根据所述惯导误差分布，确定各所述节点的惯导位置误差权值和秩亏约束条件，通过秩亏约束最小二乘滤波器构建级联滤波第一级滤波；
[0009]步骤3：利用所述秩亏约束最小二乘滤波器估计出各所述节点的惯导误差估计值，用所述惯导误差估计值补偿纯惯导位置的误差；
[0010]步骤4：通过一阶惯性平滑滤波器构建级联滤波第二级滤波，通过所述一阶惯性平滑滤波器对补偿后的所述纯惯导位置的结果进行平滑；
[0011]步骤5：基于平滑后的所述纯惯导位置的信息，通过组合导航滤波器构建级联滤波第三级滤波；
[0012]步骤6：利用所述组合导航滤波器估计出各所述节点的惯导位置和速度误差估计值；
[0013]步骤7：利用所述惯导位置和所述速度误差估计值进行惯导闭环校正。
[0014]优选的，所述步骤2中，所述惯导位置误差权值、所述秩亏约束条件以及所述秩亏
约束最小二乘滤波器均包括测量方程，所述节点为多个节点，多个所述节点分别为1,
…
,n；所述测量方程具体为：
[0015]L＝HX+Δ，
[0016]式中，为待估计的惯导误差向量，表示飞行器集群网中各所述节点在导航坐标系下的惯导误差；分别表示i节点的惯导解算的纬度、经度、高度；表示i节点的惯导解算纬度误差；表示i节点的惯导解算经度误差；表示i节点的惯导解算高度误差；i表示节点编号，i＝1,
…
,n；L是维观测量矩阵；H为维观测系数矩阵；Δ是维观测噪声矩阵，表示n中取2的组合数。
[0017]优选的，所述观测量矩阵L为：
[0018][0019]式中，为节点i与节点j间数据链测量距离，单位为m，i,j＝1,...,n且i≠j，为节点i惯导计算位置与节点j惯导计算位置之间的距离，为节点i的坐标，为节点j的坐标。
[0020]优选的，所述观测系数矩阵H为：
[0021][0022]上式中，坐标变换矩阵D
a
为
[0023][0024]式中，分别表示编号为i的节点惯导解算的纬度、经度、高度，R
n
表示地
球卯酉圈曲率半径，e表示地球曲率，为节点i的x轴坐标，为节点i的y轴坐标，为节点i的z轴坐标。
[0025]优选的，所述步骤2中，所述秩亏约束条件包括：
[0026]C
T
W
x
D
a
X＝0，
[0027]式中，X为待估计的惯导误差向量；D
a
为坐标变换矩阵；C是由矩阵H
T
W
x
H的零特征值对应的特征向量组成的维矩阵，C
T
表示C的转置；H
T
表示H的转置；W
x
为根据各惯导系统精度水平确定的3n
×
3n维权阵，如下式所示：
[0028]W
x
＝[Δ1ꢀ…ꢀ
Δ
n
]T
，
[0029]式中，Δ
i
为i节点的误差加权系数，i＝1，2，
…
，n。
[0030]优选的，所述步骤2中，所述级联滤波第一级滤波具体为：
[0031]X＝(H
T
RH+W
x
CC
T
W
x
)
‑1H
T
RL，
[0032]式中，R为观测噪声协方差矩阵，(H
T
RH+W
x
CC
T
W
x
)为列满秩矩阵，通过上式最小二乘法实时估计出各所述节点的待估惯导误差向量X。
[0033]优选的，所述步骤3中，补偿所述纯惯导位置的误差具体为：
[0034][0035][0036][0037]其中，表示编号为i的节点补偿后的纯惯导纬度、经度、高度。
[0038]优选的，所述步骤4中，对补偿后的所述纯惯导位置的结果进行平滑具体为：
[0039]m1＝(2*b1‑
b2*T)/(2*b1+b2*T)，
[0040]n0＝(2*a1+a2*T)/(2*b1+b2*T)，
[0041]n1＝(
‑
2*a1+a2*T)/(2*b1+b2*T)，
[0042][0043][0044][0045]式中：k＝2,3....；a1＝0，a2＝1，b1＝1，b2＝1，T＝0.1；m1、n0、n1为平滑系数；分别表示为编号为i的节点平滑后的纬度、经度、高度，
[0046]优选的，所述步骤5中，所述组合导航滤波器包括状态方程，所述状态方程具体为：
[0047][0048]式中，滤波状态向量X
s
各元素依次为北向失准角东向失准角天向失准角东向速度误差δV
E
、北向速度误差δV
N
、天向速度误差δV
U
；纬度误差δL、经度误差δλ、高度误差δh；F为状态转移矩阵，W为系统噪声；
[0049]所述组合导航滤波器还包括量测方程，所述量测方程如下：
[0050]Z＝AX
s
+V，
[本文档来自技高网...
【技术保护点】
【技术特征摘要】
1.一种基于秩亏约束级联滤波的惯性导航协同方法，其特征在于，包括如下步骤：步骤1：确定飞行器集群网的节点的数量，通过数据链测量各所述节点间的相对距离，得出各所述节点的惯导误差分布；步骤2：根据所述惯导误差分布，确定各所述节点的惯导位置误差权值和秩亏约束条件，通过秩亏约束最小二乘滤波器构建级联滤波第一级滤波；步骤3：利用所述秩亏约束最小二乘滤波器估计出各所述节点的惯导误差估计值，用所述惯导误差估计值补偿纯惯导位置的误差；步骤4：通过一阶惯性平滑滤波器构建级联滤波第二级滤波，通过所述一阶惯性平滑滤波器对补偿后的所述纯惯导位置的结果进行平滑；步骤5：基于平滑后的所述纯惯导位置的信息，通过组合导航滤波器构建级联滤波第三级滤波；步骤6：利用所述组合导航滤波器估计出各所述节点的惯导位置和速度误差估计值；步骤7：利用所述惯导位置和所述速度误差估计值进行惯导闭环校正。2.根据权利要求1所述的基于秩亏约束级联滤波的惯性导航协同方法，其特征在于，所述步骤2中，所述惯导位置误差权值、所述秩亏约束条件以及所述秩亏约束最小二乘滤波器均包括测量方程，所述节点为多个节点，多个所述节点分别为1,
…
,n；所述测量方程具体为：L＝HX+Δ，式中，为待估计的惯导误差向量，表示飞行器集群网中各所述节点在导航坐标系下的惯导误差；分别表示i节点的惯导解算的纬度、经度、高度；表示i节点的惯导解算纬度误差；表示i节点的惯导解算经度误差；表示i节点的惯导解算高度误差；i表示节点编号，i＝1,
…
,n；L是维观测量矩阵；H为维观测系数矩阵；Δ是维观测噪声矩阵，表示n中取2的组合数。3.根据权利要求2所述的基于秩亏约束级联滤波的惯性导航协同方法，其特征在于，所述观测量矩阵L为：式中，为节点i与节点j间数据链测量距离，单位为m，i,j＝1,...,n且i≠j，为节点i惯导计算位置与节点j惯导计算位置之间的距离，为节点i的坐标，为节点j的坐标。
4.根据权利要求3所述的基于秩亏约束级联滤波的惯性导航协同方法，其特征在于，所述观测系数矩阵H为：上式中，坐标变换矩阵D
a
为式中，分别表示编号为i的节点惯导解算的纬度、经度、高度，R
n
表示地球卯酉圈曲率半径，e表示地球曲率，为节点i的x轴坐标，为节点i的y轴坐标，为节点i的z轴坐标。5.根据权利要求4所述的基于秩亏约束级联滤波的惯性导航协同方法，其特征在于，所述步骤2中，所述秩亏约束条件包括：C
T
W
x
D
a
X＝0，式中，X为待估计的惯导误差向量；D
a
为坐标变换矩阵；C是由矩阵H
T
W
x
H的零特征值对应的特征向量组成的维矩阵，C
T
表示C的转置；H
T
表示H的转置；W
x
为根据各惯导系统精度水平确定的3n
×
3n维权阵，如下式所示：W
x
＝[Δ1ꢀ…ꢀ
Δ
n
]
T
，式中，Δ
i
为i节点的误差加权系数，i＝1，2，
…
，n。6.根据权利要求5所述的基于秩亏约束级联滤波的惯性导航协同方法，其特征在于，所述步骤2中，所述级联滤波第一级滤波具体为：X＝(H
T
RH+W
x
CC
T<...
【专利技术属性】
技术研发人员：王璞，秦峰，宋振华，张佳梁，刘伟鹏，张迪，李云龙，
申请(专利权)人：上海机电工程研究所，
类型：发明
国别省市：
还没有人留言评论。发表了对其他浏览者有用的留言会获得科技券。