CN110243377A - 一种基于分层式结构的集群飞行器协同导航方法 - Google Patents
一种基于分层式结构的集群飞行器协同导航方法 Download PDFInfo
- Publication number
- CN110243377A CN110243377A CN201910653600.0A CN201910653600A CN110243377A CN 110243377 A CN110243377 A CN 110243377A CN 201910653600 A CN201910653600 A CN 201910653600A CN 110243377 A CN110243377 A CN 110243377A
- Authority
- CN
- China
- Prior art keywords
- aircraft
- precision layer
- low precision
- equation
- layer aircraft
- 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
Links
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/20—Instruments for performing navigational calculations
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Automation & Control Theory (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Navigation (AREA)
Abstract
本发明公开了一种基于分层式结构的集群飞行器协同导航方法,在不同机载定位精度的飞行器集群飞行时,根据定位精度的差异,将飞行器分为高精度层和低精度层两种类型,通过机载传感器获取高精度层飞行器和低精度层的相互距离、相对速度、相对位置以及相对的方位角信息,建立低精度层飞行器的状态方程和所有高精度层飞行器的相对速度量测方程和相对距离量测方程,在此基础上,根据状态方程和量测方程通过卡尔曼滤波程序依次对所有低精度层飞行器的机载定位信息进行修正。与未采用该飞行器协同导航方法的低精度层飞行器相比,本发明在高精度层飞行器数量越多的情况下,对低精度层飞行器的机载定位信息修正作用越好,适合实际使用。
Description
技术领域
本发明涉及一种基于分层式结构的集群飞行器协同导航方法,属于定位与导航技术领域。
背景技术
近年来,无人机集群协同技术日益受到国内外的关注。无人机集群具有工作范围大、可靠性高、可同时执行多重任务、整体效率高等优点,可用于灾害勘测和救援等领域。无人机集群技术,即多架无人机为适应任务要求而进行的某种队形排列和任务分配的组织模式,它既包括集群飞行的队形产生、保持和变化,也包括集群飞行任务的规划和组织,是未来无人机飞行技术发展的一个重要趋势。
飞行器集群飞行中存在着不同用途、不同类型的飞行器协同飞行情况,但是对于不同类型和性能的飞行器组成编队飞行阵列时,由于不同飞行器的导航性能差距较大,使得各飞行器无法同时准确到达既定阵列位置,因而会影响飞行器的整体编队效果。
发明内容
本发明所要解决的技术问题是:提供一种基于分层式结构的集群飞行器协同导航方法,根据定位精度将飞行器分成高精度层和低精度层,利用高精度层飞行器对低精度层飞行器进行机载定位信息修正,使参与集群飞行的飞行器定位精度得到提高。
本发明为解决上述技术问题采用以下技术方案:
一种基于分层式结构的集群飞行器协同导航方法,包括如下步骤:
步骤1,确定所有参与集群飞行的飞行器数量N,N≥3,获取所有飞行器的经度、纬度和高度信息,以及所有飞行器自身机载导航设备定位精度,设定定位精度区分界限,将参与集群飞行的飞行器按照其定位精度分为高精度层飞行器和低精度层飞行器;
步骤2,根据步骤1的分类,确定低精度层飞行器和高精度层飞行器各自的数量N1和N2,获取低精度层飞行器i和每一个高精度层飞行器的位置信息以及相互距离,获取低精度层飞行器i相对于每一个高精度层飞行器的相对速度和相对角度,i=1,2,3,…,N1;
步骤3,设定低精度层飞行器i的状态量,根据状态量建立低精度层飞行器i的状态方程,并进行离散化;
步骤4,根据步骤2获取的相对速度和相对角度,建立低精度层飞行器i和所有高精度层飞行器的相对速度量测方程;
步骤5,根据步骤2获取的位置信息和相互距离,建立低精度层飞行器i和所有高精度层飞行器的相对距离量测方程;
步骤6,重复步骤4和步骤5,依次建立每个低精度层飞行器和所有高精度层飞行器的相对速度量测方程、相对距离量测方程,将状态方程和量测方程经过卡尔曼滤波后,输出低精度层飞行器i机载导航设备经度、纬度和高度信息修正值进行修正;
步骤7,判断所有低精度层飞行器机载导航设备位置信息是否得到修正,是则导航结束,否则重复步骤3-步骤6。
作为本发明的一种优选方案,所述步骤3的具体过程为:
步骤31,设定低精度层飞行器i的状态量,状态量表达式为:
其中,分别为低精度层飞行器i的机载惯性导航***的误差状态量中的东向平台误差角、北向平台误差角、天向平台误差角;分别为低精度层飞行器i的机载惯性导航***的误差状态量中的东向速度误差量、北向速度误差量、天向速度误差量;δL、δλ、δh分别为低精度层飞行器i的机载惯性导航***的误差状态量中的纬度误差量、经度误差量、高度误差量;εbx、εby、εbz分别为低精度层飞行器i的机载惯性导航***的误差状态量中的X轴、Y轴、Z轴方向上的陀螺仪常值漂移误差;εrx、εry、εrz分别为低精度层飞行器i的机载惯性导航***的误差状态量中的X轴、Y轴、Z轴方向上的陀螺一阶马尔可夫漂移误差状态量;Δx、Δy、Δz分别为低精度层飞行器i的机载惯性导航***的误差状态量中X轴、Y轴、Z轴方向上的加速度计零偏,上标T表示矩阵转置;
步骤32,根据步骤31设定的状态量,建立低精度层飞行器i的状态方程为:
其中,为状态量X的导数,FN为***阵, 为姿态角转移矩阵,Tgx、Tgy、Tgz分别为低精度层飞行器i机载陀螺仪X、Y、Z轴方向相关时间,Tax、Tay、Taz分别为低精度层飞行器i机载加速度计X、Y、Z轴方向相关时间,G为误差系数矩阵,I为单位矩阵,W为白噪声随机误差矢量;
步骤33,将步骤32的状态方程进行离散化,离散化后的方程为:
Xk=Φk/k-1Xk-1+Γk-1Wk-1
其中,Xk、Xk-1分别为***在tk、tk-1时刻的状态量,Φk/k-1为tk-1到tk时刻的***状态转移矩阵,Γk-1为tk-1到tk时刻的***噪声误差驱动矩阵,Wk-1为tk-1时刻的***噪声矢量。
作为本发明的一种优选方案,所述步骤4的具体过程为:
步骤41,根据低精度层飞行器i相对于每一个高精度层飞行器的相对速度vij,建立低精度层飞行器i和所有高精度层飞行器的相对速度方程,为:
其中,j为高精度层飞行器编号,为低精度层飞行器i和高精度层飞行器j相对速度标准值,δvij为相对速度的误差值;
步骤42,根据低精度层飞行器i相对于每一个高精度层飞行器的相对角度,建立低精度层飞行器i和所有高精度层飞行器的相对角度方程,为:
其中,aij、bij分别为低精度层飞行器i和高精度层飞行器j相对俯仰角测量值、相对偏航角测量值,分别为低精度层飞行器i和高精度层飞行器j相对俯仰角标准值、相对偏航角标准值,分别为相对俯仰角、相对偏航角的测量误差值;
步骤43,将相对速度vij沿机体坐标系进行分解,得到:
其中,vijx、vijy、vijz为低精度层飞行器i和高精度层飞行器j之间相对速度在机体坐标系X轴、Y轴、Z轴方向上的分解;
步骤44,将步骤41和步骤42建立的相对速度方程和相对角度方程代入步骤43中,并化简得到方程:
其中,为相对速度标准值在机体坐标系上的投影,且
步骤45,将步骤44得到的方程转换为地理坐标下的方程,从而得到依据传感器测量数据建立的相对速度方程,为:
其中, 为姿态角转移矩阵,ψi、θi、γi为低精度层飞行器i的航向角、俯仰角、横滚角;
步骤46,根据低精度层飞行器i和高精度层飞行器j自身机载导航设备,获取低精度层飞行器i和高精度层飞行器j在地理坐标系下东向速度北向速度天向速度
步骤47,计算低精度层飞行器i和高精度层飞行器j东向、北向、天向相对速度计算表达式如下:
步骤48,根据步骤47计算的低精度层飞行器i和高精度层飞行器j的相对速度,建立依据飞行器自身机载导航设备数据的相对速度方程,为:
其中,分别为低精度层飞行器i和高精度层飞行器j相对速度标准值,为低精度层飞行器i在地理坐标系东向、北向、天向上的位置误差;
步骤49,根据步骤45依据传感器测量数据建立的相对速度方程以及步骤48依据飞行器自身机载导航设备数据建立的相对速度方程,建立低精度层飞行器i的相对速度量测方程,为:
其中,HijV(t)=[03×3 diag[-1 -1 -1]03×12],X(t)为状态量,上标T表示矩阵转置。
作为本发明的一种优选方案,所述步骤5的具体过程为:
步骤51,根据低精度层飞行器i和高精度层飞行器j之间的相互距离dij,建立相对距离方程:
其中,为低精度层飞行器i和高精度层飞行器j相对距离标准值,δdij为距离误差值;
步骤52,将相互距离dij沿机体坐标系进行分解,得到:
其中,dijx、dijy、dijz分别为相互距离dij在机体坐标系X轴、Y轴、Z轴方向上的相对距离;
步骤53,将低精度层飞行器i和所有高精度层飞行器的相对角度方程代入步骤52得到的方程中,并进行化简,得到:
其中,为相对距离标准值在机体坐标系上的投影,且 分别为低精度层飞行器i和高精度层飞行器j相对俯仰角标准值、相对偏航角标准值, 分别为相对俯仰角、相对偏航角的测量误差值;
步骤54,将步骤53得到的方程转换为地理坐标下的方程,从而得到依据传感器测量数据建立的相对距离方程,为:
其中, 为姿态角转移矩阵;
步骤55,根据高精度层飞行器的经度λj、纬度Lj和高度hj位置信息和低精度层飞行器的经度λi、纬度Li和高度hi位置信息,建立依据飞行器自身机载导航设备数据的相对距离方程,为:
其中,R为地球半径,δλi、δLi、δhi分别为低精度层飞行器i的机载惯性导航经度、纬度、高度误差,分别为相对距离标准值在东、北、天方向上的分量,分别为依据机载导航设备信息计算的相对距离在东、北、天方向上的分量;
步骤56,根据步骤54依据传感器测量数据建立的相对距离方程以及步骤55依据飞行器自身机载导航设备数据建立的相对距离方程,建立低精度层飞行器i的相对距离量测方程,为:
其中,X(t)为状态量。
本发明采用以上技术方案与现有技术相比,具有以下技术效果:
1、本发明考虑到参与集群飞行的飞行器定位精度之间存在较大差异,根据定位精度将飞行器分成高精度层和低精度层,利用高精度层飞行器对低精度层飞行器进行机载定位信息修正,使参与集群飞行的飞行器定位精度得到提高。
2、本发明在高精度层飞行器数量越多的情况下,对低精度层飞行器的机载定位信息修正作用越好,适合实际使用。
附图说明
图1是本发明方法的原理流程示意图。
图2是仿真设计一架低精度层飞行器、两架高精度层集群飞行器的飞行航迹。
图3是纯惯导的低精度层飞行器定位误差变化曲线。
图4是纯惯导的低精度层飞行器速度误差变化曲线。
图5是采用本发明协同导航方法后的定位误差变化曲线。
图6是采用本发明协同导航方法后的速度误差变化曲线。
具体实施方式
下面详细描述本发明的实施方式,所述实施方式的示例在附图中示出。下面通过参考附图描述的实施方式是示例性的,仅用于解释本发明,而不能解释为对本发明的限制。
本发明是一种基于分层式结构的集群飞行器协同导航方法,应用于不同定位精度的飞行器集群飞行器的情况,根据飞行器的定位精度,将其划分成了高精度层和低精度层,并测量了低精度层飞行器和高精度层飞行器间的相对距离,相对速度,以及高精度层飞行器和低精度层飞行器间的方位角信息,建立了低精度层飞行器的状态方程和量测方程,最后根据状态方程和量测方程通过卡尔曼滤波程序依次修正了所有低精度层飞行器的机载导航设备的定位精度。本发明能够利用飞行器集群飞行中其他高精度层飞行器的信息来修正低精度层飞行器的机载导航设备定位精度,适合实际使用。
如图1所示,为一种基于分层式结构的集群飞行器协同导航方法的原理流程示意图,包括以下步骤:
步骤(1),获取所有参与集群飞行器的飞行器数量,获取所有飞行器的经度、纬度和高度位置信息,根据飞行器所携带的机载导航信息和定位精度,将参与集群的飞行器分为两层,高精度层飞行器和低精度层飞行器;
步骤(2),根据步骤(1)的分类结果,获取高精度层和低精度层飞行器的数量,获取低精度层飞行器和每一个高精度层飞行器的相对距离信息和相对速度信息以及相对俯仰角和相对偏航角信息;
步骤(3),选取低精度层飞行器的导航状态量,建立低精度层飞行器的导航状态方程;
步骤(4),根据步骤(2)获取的高精度层飞行器相对速度信息和角度信息,建立低精度层飞行器的相对速度量测方程;
步骤(5),根据步骤(2)获取的高精度层飞行器相对距离和角度信息,建立低精度层飞行器相对距离量测方程;
步骤(6),重复上述步骤(4)、步骤(5),依次建立低精度飞行器和所有高精度层飞行器的相对速度量测方程和相对距离量测方程并利用卡尔曼滤波方程修正低精度层飞行器;
步骤(7),判断所有低精度层飞行器机载惯性导航位置信息是否得到修正,是则导航结束,否则重复上述步骤(3)至步骤(6)。
步骤(1)包括如下具体步骤:
步骤(1-1),确定所有参与集群飞行器的飞行器数量N,若N<3,则算法无法使用,应继续等待,直至N≥3;
步骤(1-2),获取所有参与集群飞行的飞行器的经度λ、纬度L和高度h位置信息和所有集群飞行器机载设备定位精度E;
步骤(1-3),设定定位精度区分界限τ,根据步骤(1-2)获取的所有飞行器的定位精度E,将参与集群飞行的飞行器划分为高精度层飞行器(E≥τ)和低精度层飞行器(E<τ)。
步骤(2)包括如下具体步骤:
步骤(2-1),根据步骤(1)对集群飞行器进行分类的结果,确定低精度层飞行器数量N1,高精度层飞行器数量N2;
步骤(2-2),对所有高精度层飞行器和低精度层飞行器分别进行编号,i(i=1,2,3,…,N1)代表低精度层飞行器编号,j(j=1,2,3,…,N2)代表高精度层飞行器的编号;
步骤(2-3),利用机载传感器获取低精度层飞行器i和高精度层飞行器j之间的相互距离信息dij、相对速度信息vij;
步骤(2-4),利用机载传感器获取低精度层飞行器i和高精度层飞行器j之间的相对俯仰角和相对偏航角,用aij、bij代表俯仰角和偏航角,其中i,j分别代表低精度层飞行器和高精度层飞行器的编号。
步骤(3)包括如下具体步骤:
步骤(3-1),首先选取低精度层飞行器编号i=1的飞行器进行状态方程建立;
步骤(3-2),获取低精度层飞行器i的航向角ψi,俯仰角θi和横滚角γi;
步骤(3-3),对编号为i的低精度层飞行器建立状态矢量X,其中状态矢量X表达式如下:
其中,分别为编号为i的低精度层飞行器机载惯性导航***中误差状态量中的东向平台误差角、北向平台误差角、天向平台误差角; 分别为低精度层飞行器i的机载惯性导航***中的误差状态量中的东向速度误差量、北向速度误差量、天向速度误差量;δL、δλ、δh分别表示低精度层飞行器i的机载惯性导航***中误差状态量中的纬度误差量、经度误差量和高度误差量;εbx、εby、εbz分别为低精度层飞行器i的机载惯性导航***中的误差状态量中的X轴、Y轴、Z轴方向上的陀螺仪常值漂移误;εrx、εry、εrz分别为低精度层飞行器i的机载惯性导航***中的误差状态量中的X轴、Y轴、Z轴方向上的陀螺一阶马尔可夫漂移误差状态量;Δx、Δy、Δz分别为低精度层飞行器i的机载惯性导航***中的误差状态量中X轴、Y轴、Z轴方向上的加速度计零偏,T为矩阵转置;
步骤(3-4),根据步骤(3-3)设定的状态量,建立低精度层飞行器i的状态方程,方程如下所示:
其中,为状态矢量X的导数,FN为由地球自转角速度,低精度层飞行器i经度、纬度和高度,地球子午圈曲率半径,地球卯酉圈曲率半径以及低精度层飞行器i地理坐标系在东向、北向、天向速度,9个基本导航参数组成的***阵, 其中,Tgx、Tgy、Tgz为低精度层飞行器i机载陀螺仪X轴、Y轴、Z轴方向相关时间,Tax、Tay、Taz为低精度层飞行器i机载加速度计X轴、Y轴、Z轴方向相关时间,W为白噪声随机误差矢量,误差系数矩阵
步骤(3-5),将上述步骤(3-4)的低精度层飞行器i的状态方程进行离散化,离散化的方程如下所示:
Xk=Φk/k-1Xk-1+Γk-1Wk-1
其中,Φk/k-1为tk-1时刻到tk时刻的***状态转移矩阵,Γk-1为tk-1时刻到tk时刻的***噪声误差驱动矩阵,上述Φk/k-1,Γk-1计算公式中,n为离散点数,T为迭代周期,Wk-1为tk-1时刻的***噪声矢量,Xk-1、Xk为***在tk-1时刻和tk时刻的***状态矢量。
步骤(4)包括如下具体步骤:
步骤(4-1),取高精度层飞行器编号j=1的飞行器;
步骤(4-2),根据步骤(2-3)获取的低精度层飞行器i和所有高精度层飞行器j相对速度vij,建立低精度层飞行器i和所有高精度层飞行器的相对速度方程,方程如下所示:
其中,j为高精度层飞行器编号,为低精度层飞行器i和高精度层飞行器j相对速度标准值,δvij为相对速度的误差值;
步骤(4-3),根据步骤(2-4)获取的低精度层飞行器和高精度层飞行器间的相对俯仰角aij和相对偏航角bij,建立俯仰角和偏航角测量值和标准值间的方程,如下所示:
其中,为低精度层飞行器i和高精度层飞行器j相对俯仰角真值和相对偏航角真值,分别为相对俯仰角和相对偏航角的测量误差值;
步骤(4-4),根据步骤(4-2)传感器获取的相对速度vij以及步骤(4-3)获取的相对俯仰角aij和偏航角bij,将相对速度vij沿着机体坐标系进行分解,得到方程如下所示:
其中,vijx、vijy、vijz为低精度层飞行器i和高精度层飞行器j之间相对速度在机体坐标系X轴、Y轴、Z轴方向上的分解;
步骤(4-5),根据步骤(4-2)和步骤(4-3)建立的相对速度方程和相对角度方程,将其代入到步骤(4-4)的方程中,得到如下方程:
因为都是角度误差,且数值较小,所以 且设定小量乘积为0,相对速度误差和角度误差乘积为0。上述公式化简为:
继续化简得到:
其中,为相对速度真值在机体坐标系上的投影,
步骤(4-6),根据步骤(4-5)得到的化简后的相对速度方程,因该方程是建立在机体坐标系下,所以需要将该坐标系方程转换为地理坐标系下,步骤(4-5)方程变换如下所示:
其中,各参数计算公式如下所示:
其中,为姿态角转移矩阵,根据步骤(3-2),ψi、θi、γi为低精度层飞行器i的航向角、俯仰角和横滚角;
步骤(4-7),上述步骤(4-6)建立的是传感器测量的相对速度方程,接下来需要建立低精度层飞行器i和高精度层飞行器j自身机载导航设备信息相对速度方程,首先获取低精度层飞行器i和高精度层飞行器j在地理坐标系下东向速度北向速度天向速度
步骤(4-8),计算低精度层飞行器i和高精度层飞行器j相对速度 计算表达式如下:
步骤(4-9),根据步骤(4-8)计算的低精度层飞行器i和高精度层飞行器j的相对速度,建立低精度层飞行器i和高精度层飞行器j自身机载导航设备信息相对速度方程如下所示:
其中,为低精度层飞行器i和高精度层飞行器j相对速度真值,为低精度层飞行器i在地理坐标系东向、北向和天向上的位置误差;
步骤(4-10),根据步骤(4-6)依据传感器测量数据建立的相对速度方程以及步骤(4-9)飞行器自身机载导航数据建立的相对速度方程建立低精度层飞行器i的量测方程,方程如下所示:
其中,HijV(t)=[03×3 diag[-1 -1 -1]03×12], T为矩阵转置。
步骤(5)包括如下具体步骤:
步骤(5-1),根据步骤(2-3)获取低精度层飞行器i和高精度层飞行器j之间的相互距离信息dij,建立相对距离方程:
其中,为低精度层飞行器i和高精度层飞行器j相对距离真值,δdij为距离误差值;
步骤(5-2),根据步骤(2-4)获取的低精度层飞行器和高精度层飞行器间的相对俯仰角aij和相对偏航角bij将相对距离沿机体坐标系进行分解,得到相对距离在机体坐标系X轴、Y轴、Z轴方向上的相对距离dijx、dijy、dijz,方程如下所示:
步骤(5-3),根据步骤(4-3)建立的俯仰角和偏航角测量值和标准值间的方程,以及步骤(5-1)的相对距离方程,对步骤(5-2)方程进行变量替换:
对该方程进行化简,因为都是角度误差,且数值较小,所以且设定小量乘积为0,相对速度误差和角度误差乘积为0。上述公式化简为:
继续化简得到:
其中,为相对速度真值在机体坐标系上的投影,
步骤(5-4),根据步骤(4-6)将机体系转化为地理系,对步骤(5-3)的方程化简为地理系,如下所示:
其中的计算方式和步骤(4-6)相同;
步骤(5-5),根据步骤(1-2)获取的高精度层飞行器的经度λj、纬度Lj和高度hj位置信息和低精度层飞行器的经度λi、纬度Li和高度hi位置信息,计算低精度层飞行器i和高精度层飞行器j相对距离,计算表达式如下:
其中,R为地球半径,δλi、δLi、δhi分别为低精度层飞行器i的机载惯性导航经度,纬度和高度误差,为相对距离真值在东北天方向上的分量,分别为依据机载导航信息计算的相对距离在东北天方向上的分量;
步骤(5-6),根据步骤(5-4)依据传感器测量数据建立的相对距离方程以及步骤(5-5)飞行器自身机载导航数据建立的相对距离方程建立的低精度层飞行器i的量测方程,方程如下所示:
其中,
步骤(6)包括如下具体步骤:
步骤(6-1),上述步骤已经建立了低精度层飞行器i和高精度层飞行器j之间的相对速度量测方程和相对距离量测方程;
步骤(6-2),判断j和N2的大小,如果j<N2,则j=j+1,则跳转执行步骤(4-2),如果j≥N2,则执行步骤(6-3);
步骤(6-3),根据步骤(6-2),已经通过循环建立了低精度层飞行器i和所有高精度层飞行器(j=1,2,3,…,N2)之间的相对速度量测方程和相对位置量测方程如下所示:
其中,HijV(t)=[03×3 diag[-1 -1 -1]03×12], T为矩阵转置,
步骤(6-4),联立步骤(6-3)的所有相对速度方程和相对位置方程,方程如下所示:
步骤(6-5),根据步骤(3-5)和步骤(6-4)建立的状态方程和量测方程,经过卡尔曼滤波程序后,输出低精度层飞行器i机载惯性导航***经度、纬度和高度位置信息修正值(λ′,L′,h′),λ′为经度修正值,L′为纬度修正值,h′为高度修正值。
步骤(7)包括如下具体步骤:
判断i和N1的大小,如果i<N1,则i=i+1,且跳转执行步骤(3-2),如果i≥N1,则所有低精度层飞行器机载定位信息得到修正,则导航结束。
下面以一实施例来对本发明进行说明。仿真设计1架低精度层飞行器,两架高精度层飞行器集群飞行,低精度层飞行器仅装有惯导设备,高精度层飞行器装有GNSS导航设备,通过对比低精度层飞行器在使用本发明的协同导航算法后位置和速度误差,如图2所示,为仿真设计一架低精度层飞行器,两架高精度层飞行器集群飞行航迹。图3、图4为在没有使用协同导航算法前的纯惯导的低精度层飞行器位置、速度误差变化曲线,该曲线误差较大,处于发散状态。图5、图6为采用本发明协同导航方法后的定位、速度误差变化曲线。通过对比位置误差和速度误差曲线,可以看出本发明设计的协同导航算法能有效提高低精度层飞行器的定位精度和速度精度。
以上实施例仅为说明本发明的技术思想,不能以此限定本发明的保护范围,凡是按照本发明提出的技术思想,在技术方案基础上所做的任何改动,均落入本发明保护范围之内。
Claims (4)
1.一种基于分层式结构的集群飞行器协同导航方法,其特征在于,包括如下步骤:
步骤1,确定所有参与集群飞行的飞行器数量N,N≥3,获取所有飞行器的经度、纬度和高度信息,以及所有飞行器自身机载导航设备定位精度,设定定位精度区分界限,将参与集群飞行的飞行器按照其定位精度分为高精度层飞行器和低精度层飞行器;
步骤2,根据步骤1的分类,确定低精度层飞行器和高精度层飞行器各自的数量N1和N2,获取低精度层飞行器i和每一个高精度层飞行器的位置信息以及相互距离,获取低精度层飞行器i相对于每一个高精度层飞行器的相对速度和相对角度,i=1,2,3,…,N1;
步骤3,设定低精度层飞行器i的状态量,根据状态量建立低精度层飞行器i的状态方程,并进行离散化;
步骤4,根据步骤2获取的相对速度和相对角度,建立低精度层飞行器i和所有高精度层飞行器的相对速度量测方程;
步骤5,根据步骤2获取的位置信息和相互距离,建立低精度层飞行器i和所有高精度层飞行器的相对距离量测方程;
步骤6,重复步骤4和步骤5,依次建立每个低精度层飞行器和所有高精度层飞行器的相对速度量测方程、相对距离量测方程,将状态方程和量测方程经过卡尔曼滤波后,输出低精度层飞行器i机载导航设备经度、纬度和高度信息修正值进行修正;
步骤7,判断所有低精度层飞行器机载导航设备位置信息是否得到修正,是则导航结束,否则重复步骤3-步骤6。
2.根据权利要求1所述基于分层式结构的集群飞行器协同导航方法,其特征在于,所述步骤3的具体过程为:
步骤31,设定低精度层飞行器i的状态量,状态量表达式为:
其中,分别为低精度层飞行器i的机载惯性导航***的误差状态量中的东向平台误差角、北向平台误差角、天向平台误差角;分别为低精度层飞行器i的机载惯性导航***的误差状态量中的东向速度误差量、北向速度误差量、天向速度误差量;δL、δλ、δh分别为低精度层飞行器i的机载惯性导航***的误差状态量中的纬度误差量、经度误差量、高度误差量;εbx、εby、εbz分别为低精度层飞行器i的机载惯性导航***的误差状态量中的X轴、Y轴、Z轴方向上的陀螺仪常值漂移误差;εrx、εry、εrz分别为低精度层飞行器i的机载惯性导航***的误差状态量中的X轴、Y轴、Z轴方向上的陀螺一阶马尔可夫漂移误差状态量;Δx、Δy、Δz分别为低精度层飞行器i的机载惯性导航***的误差状态量中X轴、Y轴、Z轴方向上的加速度计零偏,上标T表示矩阵转置;
步骤32,根据步骤31设定的状态量,建立低精度层飞行器i的状态方程为:
其中,为状态量X的导数,FN为***阵, 为姿态角转移矩阵,Tgx、Tgy、Tgz分别为低精度层飞行器i机载陀螺仪X、Y、Z轴方向相关时间,Tax、Tay、Taz分别为低精度层飞行器i机载加速度计X、Y、Z轴方向相关时间,G为误差系数矩阵,I为单位矩阵,W为白噪声随机误差矢量;
步骤33,将步骤32的状态方程进行离散化,离散化后的方程为:
Xk=Φk/k-1Xk-1+Γk-1Wk-1
其中,Xk、Xk-1分别为***在tk、tk-1时刻的状态量,Φk/k-1为tk-1到tk时刻的***状态转移矩阵,Γk-1为tk-1到tk时刻的***噪声误差驱动矩阵,Wk-1为tk-1时刻的***噪声矢量。
3.根据权利要求1所述基于分层式结构的集群飞行器协同导航方法,其特征在于,所述步骤4的具体过程为:
步骤41,根据低精度层飞行器i相对于每一个高精度层飞行器的相对速度vij,建立低精度层飞行器i和所有高精度层飞行器的相对速度方程,为:
其中,j为高精度层飞行器编号,为低精度层飞行器i和高精度层飞行器j相对速度标准值,δvij为相对速度的误差值;
步骤42,根据低精度层飞行器i相对于每一个高精度层飞行器的相对角度,建立低精度层飞行器i和所有高精度层飞行器的相对角度方程,为:
其中,aij、bij分别为低精度层飞行器i和高精度层飞行器j相对俯仰角测量值、相对偏航角测量值,分别为低精度层飞行器i和高精度层飞行器j相对俯仰角标准值、相对偏航角标准值,分别为相对俯仰角、相对偏航角的测量误差值;
步骤43,将相对速度vij沿机体坐标系进行分解,得到:
其中,vijx、vijy、vijz为低精度层飞行器i和高精度层飞行器j之间相对速度在机体坐标系X轴、Y轴、Z轴方向上的分解;
步骤44,将步骤41和步骤42建立的相对速度方程和相对角度方程代入步骤43中,并化简得到方程:
其中,为相对速度标准值在机体坐标系上的投影,且
步骤45,将步骤44得到的方程转换为地理坐标下的方程,从而得到依据传感器测量数据建立的相对速度方程,为:
其中, 为姿态角转移矩阵,ψi、θi、γi为低精度层飞行器i的航向角、俯仰角、横滚角;
步骤46,根据低精度层飞行器i和高精度层飞行器j自身机载导航设备,获取低精度层飞行器i和高精度层飞行器j在地理坐标系下东向速度北向速度天向速度
步骤47,计算低精度层飞行器i和高精度层飞行器j东向、北向、天向相对速度计算表达式如下:
步骤48,根据步骤47计算的低精度层飞行器i和高精度层飞行器j的相对速度,建立依据飞行器自身机载导航设备数据的相对速度方程,为:
其中,分别为低精度层飞行器i和高精度层飞行器j相对速度标准值,为低精度层飞行器i在地理坐标系东向、北向、天向上的位置误差;
步骤49,根据步骤45依据传感器测量数据建立的相对速度方程以及步骤48依据飞行器自身机载导航设备数据建立的相对速度方程,建立低精度层飞行器i的相对速度量测方程,为:
其中,HijV(t)=[03×3 diag[-1 -1 -1] 03×12],X(t)为状态量,上标T表示矩阵转置。
4.根据权利要求1所述基于分层式结构的集群飞行器协同导航方法,其特征在于,所述步骤5的具体过程为:
步骤51,根据低精度层飞行器i和高精度层飞行器j之间的相互距离dij,建立相对距离方程:
其中,为低精度层飞行器i和高精度层飞行器j相对距离标准值,δdij为距离误差值;
步骤52,将相互距离dij沿机体坐标系进行分解,得到:
其中,dijx、dijy、dijz分别为相互距离dij在机体坐标系X轴、Y轴、Z轴方向上的相对距离;
步骤53,将低精度层飞行器i和所有高精度层飞行器的相对角度方程代入步骤52得到的方程中,并进行化简,得到:
其中,为相对距离标准值在机体坐标系上的投影,且 分别为低精度层飞行器i和高精度层飞行器j相对俯仰角标准值、相对偏航角标准值, 分别为相对俯仰角、相对偏航角的测量误差值;
步骤54,将步骤53得到的方程转换为地理坐标下的方程,从而得到依据传感器测量数据建立的相对距离方程,为:
其中, 为姿态角转移矩阵;
步骤55,根据高精度层飞行器的经度λj、纬度Lj和高度hj位置信息和低精度层飞行器的经度λi、纬度Li和高度hi位置信息,建立依据飞行器自身机载导航设备数据的相对距离方程,为:
其中,R为地球半径,δλi、δLi、δhi分别为低精度层飞行器i的机载惯性导航经度、纬度、高度误差,分别为相对距离标准值在东、北、天方向上的分量,分别为依据机载导航设备信息计算的相对距离在东、北、天方向上的分量;
步骤56,根据步骤54依据传感器测量数据建立的相对距离方程以及步骤55依据飞行器自身机载导航设备数据建立的相对距离方程,建立低精度层飞行器i的相对距离量测方程,为:
其中,X(t)为状态量。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201910653600.0A CN110243377B (zh) | 2019-07-19 | 2019-07-19 | 一种基于分层式结构的集群飞行器协同导航方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201910653600.0A CN110243377B (zh) | 2019-07-19 | 2019-07-19 | 一种基于分层式结构的集群飞行器协同导航方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN110243377A true CN110243377A (zh) | 2019-09-17 |
CN110243377B CN110243377B (zh) | 2022-09-30 |
Family
ID=67892895
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201910653600.0A Active CN110243377B (zh) | 2019-07-19 | 2019-07-19 | 一种基于分层式结构的集群飞行器协同导航方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN110243377B (zh) |
Cited By (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN111947654A (zh) * | 2020-08-13 | 2020-11-17 | 杭州北斗东芯科技有限公司 | 一种导航与控制一体化芯片及其控制方法 |
CN112698664A (zh) * | 2020-12-11 | 2021-04-23 | 南京航空航天大学 | 一种用于无人集群协同导航优化的视线扇区动态估计方法 |
CN113038376A (zh) * | 2021-04-07 | 2021-06-25 | 南京大翼航空科技有限公司 | 基于一个航空器辅助的建筑物室内目标定位方法 |
CN113048983A (zh) * | 2021-03-29 | 2021-06-29 | 河海大学 | 一种异时序贯量测的改进分层式auv协同导航定位方法 |
CN113341385A (zh) * | 2021-03-30 | 2021-09-03 | 西南电子技术研究所(中国电子科技集团公司第十研究所) | 机载平台协同综合传感器***马尔科夫链误差传递模型 |
CN113608555A (zh) * | 2021-10-08 | 2021-11-05 | 广东博创佳禾科技有限公司 | 多无人机协同控制方法和装置 |
Citations (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN102809377A (zh) * | 2012-08-15 | 2012-12-05 | 南京航空航天大学 | 飞行器惯性/气动模型组合导航方法 |
CN106873628A (zh) * | 2017-04-12 | 2017-06-20 | 北京理工大学 | 一种多无人机跟踪多机动目标的协同路径规划方法 |
CN108151737A (zh) * | 2017-12-19 | 2018-06-12 | 南京航空航天大学 | 一种动态互观测关系条件下的无人机蜂群协同导航方法 |
CN108268054A (zh) * | 2018-02-06 | 2018-07-10 | 哈尔滨工业大学 | 亚轨道蜂群飞行器分层式协同控制方法 |
CN109708629A (zh) * | 2018-11-15 | 2019-05-03 | 南京航空航天大学 | 一种用于差异定位性能条件的飞行器集群协同导航方法 |
-
2019
- 2019-07-19 CN CN201910653600.0A patent/CN110243377B/zh active Active
Patent Citations (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN102809377A (zh) * | 2012-08-15 | 2012-12-05 | 南京航空航天大学 | 飞行器惯性/气动模型组合导航方法 |
CN106873628A (zh) * | 2017-04-12 | 2017-06-20 | 北京理工大学 | 一种多无人机跟踪多机动目标的协同路径规划方法 |
CN108151737A (zh) * | 2017-12-19 | 2018-06-12 | 南京航空航天大学 | 一种动态互观测关系条件下的无人机蜂群协同导航方法 |
CN108268054A (zh) * | 2018-02-06 | 2018-07-10 | 哈尔滨工业大学 | 亚轨道蜂群飞行器分层式协同控制方法 |
CN109708629A (zh) * | 2018-11-15 | 2019-05-03 | 南京航空航天大学 | 一种用于差异定位性能条件的飞行器集群协同导航方法 |
Non-Patent Citations (6)
Title |
---|
S. ÇAŞKA等: "A SURVEY OF UAV/UGV COLLABORATIVE SYSTEMS", 《IMS》 * |
刘晓洋等: "基于测距/测速信息的无人机协同导航算法研究", 《战术导弹技术》 * |
徐博等: "多AUV协同导航问题的研究现状与进展", 《自动化学报》 * |
杜君南等: "基于相对距离差模型的集群飞行器协同导航方法研究", 《第十界中国卫星导航年会论文集-S02导航与位置服务》 * |
潘瑞鸿等: "基于几何特性的多无人机协同导航算法", 《兵器装备工程学报》 * |
许晓伟等: "多无人机协同导航技术研究现状及进展", 《导航定位与授时》 * |
Cited By (9)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN111947654A (zh) * | 2020-08-13 | 2020-11-17 | 杭州北斗东芯科技有限公司 | 一种导航与控制一体化芯片及其控制方法 |
CN112698664A (zh) * | 2020-12-11 | 2021-04-23 | 南京航空航天大学 | 一种用于无人集群协同导航优化的视线扇区动态估计方法 |
CN112698664B (zh) * | 2020-12-11 | 2022-03-25 | 南京航空航天大学 | 一种用于无人机集群协同导航优化的视线扇区动态估计方法 |
CN113048983A (zh) * | 2021-03-29 | 2021-06-29 | 河海大学 | 一种异时序贯量测的改进分层式auv协同导航定位方法 |
CN113048983B (zh) * | 2021-03-29 | 2023-12-26 | 河海大学 | 一种异时序贯量测的改进分层式auv协同导航定位方法 |
CN113341385A (zh) * | 2021-03-30 | 2021-09-03 | 西南电子技术研究所(中国电子科技集团公司第十研究所) | 机载平台协同综合传感器***马尔科夫链误差传递模型 |
CN113341385B (zh) * | 2021-03-30 | 2023-09-05 | 西南电子技术研究所(中国电子科技集团公司第十研究所) | 机载平台协同综合传感器***马尔科夫链误差传递模型 |
CN113038376A (zh) * | 2021-04-07 | 2021-06-25 | 南京大翼航空科技有限公司 | 基于一个航空器辅助的建筑物室内目标定位方法 |
CN113608555A (zh) * | 2021-10-08 | 2021-11-05 | 广东博创佳禾科技有限公司 | 多无人机协同控制方法和装置 |
Also Published As
Publication number | Publication date |
---|---|
CN110243377B (zh) | 2022-09-30 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN110243377A (zh) | 一种基于分层式结构的集群飞行器协同导航方法 | |
CN104236546B (zh) | 一种卫星星光折射导航误差确定与补偿方法 | |
CN108413887B (zh) | 光纤光栅辅助分布式pos的机翼形变测量方法、装置和平台 | |
CN104655152B (zh) | 一种基于联邦滤波的机载分布式pos实时传递对准方法 | |
CN104406605B (zh) | 机载多导航源综合导航仿真*** | |
CN100585602C (zh) | 惯性测量***误差模型验证试验方法 | |
CN110487301A (zh) | 一种雷达辅助机载捷联惯性导航***初始对准方法 | |
Gebre-Egziabher | Design and performance analysis of a low-cost aided dead reckoning navigator | |
CN105806365B (zh) | 一种基于自抗扰控制的车载惯导行进间快速初始对准方法 | |
CN105371844B (zh) | 一种基于惯性/天文互助的惯性导航***初始化方法 | |
CN104061932B (zh) | 一种利用引力矢量和梯度张量进行导航定位的方法 | |
CN106767900A (zh) | 一种基于组合导航技术的船用光纤捷联惯导***的在线标定方法 | |
CN106643737A (zh) | 风力干扰环境下四旋翼飞行器姿态解算方法 | |
CN104344836B (zh) | 一种基于姿态观测的冗余惯导***光纤陀螺***级标定方法 | |
CN101246012B (zh) | 一种基于鲁棒耗散滤波的组合导航方法 | |
CN103837151B (zh) | 一种四旋翼飞行器的气动模型辅助导航方法 | |
CN109708629B (zh) | 一种用于差异定位性能条件的飞行器集群协同导航方法 | |
CN110395390A (zh) | 一种多旋翼无人机免像控点三维建模与测图的装置及方法 | |
CN106871927A (zh) | 一种无人机光电吊舱安装误差标校方法 | |
CN106871928A (zh) | 基于李群滤波的捷联惯性导航初始对准方法 | |
CN103913181A (zh) | 一种基于参数辨识的机载分布式pos传递对准方法 | |
CN108387227A (zh) | 机载分布式pos的多节点信息融合方法及*** | |
CN104655135B (zh) | 一种基于地标识别的飞行器视觉导航方法 | |
CN110849360B (zh) | 面向多机协同编队飞行的分布式相对导航方法 | |
CN105737858A (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 |