CN109813311A - 一种无人机编队协同导航方法 - Google Patents
一种无人机编队协同导航方法 Download PDFInfo
- Publication number
- CN109813311A CN109813311A CN201910201866.1A CN201910201866A CN109813311A CN 109813311 A CN109813311 A CN 109813311A CN 201910201866 A CN201910201866 A CN 201910201866A CN 109813311 A CN109813311 A CN 109813311A
- Authority
- CN
- China
- Prior art keywords
- unmanned plane
- plane node
- navigation
- information
- node
- 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
Abstract
本发明公开了一种无人机编队协同导航方法。在无人机编队中,长机无人机节点搭载惯性导航系统、GPS接收机,通过机载惯导/GPS融合滤波对姿态、速度、位置进行估计,得到长机无人机节点高精度的姿态、速度和位置导航信息,其余无人机节点搭载惯性导航系统、视觉导航设备、光流传感器,通过视觉导航设备获得与长机的相对距离、俯仰角、方位角数学解算出相邻与长机无人机节点的绝对位置信息,光流传感器解算得到无人机的速度信息,通过卡尔曼滤波对机载的惯导误差进行修正。此外,编队中距离长机较远的无人机节点通过视觉导航设备解算得到与其它无人机节点的相对距离,并据此解算出绝对位置信息。
Description
技术领域
本发明属于导航技术领域,特别涉及了一种无人机导航方法。
背景技术
无人机系统是一种无人驾驶,是能够自主控制或者遥控的空中飞行机器人系统,可执行对地、对海、对空的作战任务,具有可回收,可重复使用,高效等特点。随着任务需求的多样性和复杂性,单架无人机执行任务的能力有限,生存能力受到极大的挑战。而多架无人机之间的协同可以实现信息的互享和行动协调,实现单架无人机的任务扩展,提高任务执行的成功率。导航系统为无人机提供其飞行控制系统所必须的导航信息,是其完成各种复杂飞行任务的必要保障。
现阶段惯导/GPS组合导航系统广泛应用为无人机提供导航服务。无人机编队中受限于成本、载荷等因素,不可能让编队中所有的无人机都搭载高精度的导航传感器,因此主从式的导航方式成为现阶段研究无人机编队协同导航的主流,长机搭载高精度的导航传感器通过滤波融合得到长机高精度的导航信息,在编队导航数据链及其他测距传感器的辅助下,通过编队协同方式完成对编队中其他无人机节点的高精度定位。然而现有的多无人机协同导航方法的精度并不理想。
发明内容
为了解决上述背景技术提出的技术问题,本发明提出了一种无人机编队协同导航方法。
为了实现上述技术目的,本发明的技术方案为:
一种无人机编队协同导航方法,无人机编队包含4架无人机节点:无人机节点1~4,包括以下步骤:
(1)建立机体坐标系和导航坐标系;
(2)在无人机节点1上搭载惯性导航系统和GPS接收机,周期读取k时刻无人机节点1的机载传感器信息,包括速度信息、位置信息,陀螺仪信息和加速度计信息;
(3)预测k时刻无人机节点i的姿态四元数、速度和位置信息,其中i=1,2,3,4;
(4)利用卡尔曼滤波器,对k时刻的无人机节点1的姿态四元数、速度、位置信息进行校正,得到无人机节点1的高精度导航信息;
(5)在无人机节点i上搭载惯性导航系统、视觉导航设备和光流传感器,周期性读取k时刻无人机节点i的机载传感器信息,包括相对于无人机节点1的距离信息、俯仰角和方位角,以及通过光流解算出的速度信息、陀螺仪信息和加速度计信息,其中i=2,3,4;
(6)根据相对于无人机节点1的距离信息、俯仰角和方位角解算出无人机节点2和无人机节点3的绝对位置信息;
(7)利用卡尔曼滤波器,根据无人机节点2和无人机节点3的位置信息和速度信息对k时刻的姿态四元数、速度、位置信息进行校正,得到无人机节点2和无人机节点3的高精度导航信息;
(8)根据无人机节点1~3的位置信息推导出无人机节点4的绝对位置信息;
(9)利用卡尔曼滤波器,将无人机节点4的位置信息和速度信息作为量测信息,对k时刻的姿态四元数、速度、位置信息进行校正,得到无人机节点4的高精度导航信息。
进一步地,在步骤(1)中,机体坐标系的X、Y、Z轴分别为无人机的右向、机头方向、天向,导航坐标系的X、Y、Z轴分别为东向、北向、天向。
进一步地,在步骤(3)中,采用下式预测姿态四元数:
其中,Qi(k)=[qi0(k) qi1(k) qi2(k) qi3(k)]T为k时刻的姿态四元数,上标T表示矩阵的转置;Qi(k-1)=[qi0(k-1) qi1(k-1) qi2(k-1) qi3(k-1)]T为k-1时刻的姿态四元数;ΔT为离散采样周期;通过下式计算:
其中通过下式计算:
其中,为k时刻陀螺仪读取的无人机节点i机体系相对于导航系的角速度在机体系X、Y、Z轴上的分量,为k时刻陀螺仪的零偏在机体系X、Y、Z轴上的分量;i=1,2,3,4;
采用下式预测速度信息:
其中,为k时刻加速度计读取的无人机节点i机体系相对于导航系的加速度在机体系X、Y、Z轴上的分量;为k时刻加速度计零偏在机体系X、Y、Z轴上的分量;g=[0 0 g]T,g为当地重力加速度值;为k时刻机体系相对于导航系的线速度在导航系X、Y、Z轴上的分量;
为k-1时刻机体系相对于导航系的线速度在导航系X、Y、Z轴上的分量;i=1,2,3,4;为机体系到导航系之间的姿态矩阵,通过下式计算:
采用下式预测位置信息:
其中,分别为k时刻无人机节点i在导航系X、Y、Z轴上的位置坐标;k-1时刻的位置分别为k-1时刻无人机节点i在导航系X、Y、Z轴上的位置坐标。
进一步地,步骤(4)的具体过程如下:
(401)确定无人机节点1导航系统的状态向量,无人机节点1~4的状态向量相同,如下:
其中,为无人机节点i的平台误差角,δVEi,δVNi,δVUi为无人机节点i的陀螺东向、北向、天向速度误差,δLi,δλi,δhi为无人机节点i陀螺纬度、经度、高度误差,Li,λi,hi无人机节点i陀螺纬度、经度、高度值,εbxi,εbyi,εbzi为无人机节点i陀螺随机常数误差,εrxi,εryi,εrzi为无人机节点i陀螺一阶马尔科夫随机噪声,为无人机节点i加速度计一阶马尔科夫过程;
当i取1时即为无人机节点1导航系统的状态向量;
(402)建立惯性导航系统的误差方程,包括平台误差角方程、速度误差方程和位置误差方程;
平台误差角方程:
其中,wie为地球自转角速度,vEi、vNi为无人机节点i惯性导航系统东向、北向速度,δvEi、δvNi为对应的速度误差,εEi、εNi、εUi为无人机节点i东、北、天向陀螺噪声角,RM=Re(1-2f+3fsin2Li),RN=Re(1+fsin2Li),Re=6378137米,f=1/298.257;
速度误差方程:
其中,vUi为无人机节点i惯性导航系统天向速度,δvUi为对应的速度误差,fEi、fNi、fUi为无人机节点i加速度计测得的加速度值,为无人机节点i东向、北向、天向加速度计一阶马尔可夫过程白噪声,R为地球半径,g为当地重力加速度值;
位置误差方程:
当i取1时即为无人机节点1的惯性导航系统的误差方程;
(403)建立无人机节点1导航系统的状态方程,无人机节点1~4的状态方程相同,如下:
其中,Gi为系统噪声系数矩阵,Wi为噪声控制向量,FNi为平台误差角、速度误差、位置误差构成的系统矩阵, Tgxi、Tgyi、Tgzi为三轴陀螺相关时间常数,Taxi、Tayi、Tazi为三轴加速度计相关时间常数;
当i取1时即为无人机节点1导航系统的状态方程;
(404)建立无人机节点1导航系统的量测方程:
无人机节点1导航系统的速度量测矩阵Hv1和量测噪声Vv1:
Hv1=[03×3 diag[1 1 1]03×12]
Vv1=[ME1 MN1 MU1]
上式中,ME1,MN1,MU1分别为无人机节点1的GPS接收机沿东、北、天向的测速误差;
无人机节点1导航系统的位置量测矩阵Hp1和量测噪声Vp1:
Hp1=[03×6 diag[RM RNcos L1 1]03×9]
Vp1=[NE1 NN1 NU1]
上式中,NE1,NN1,NU1分别为无人机节点1上的GPS接收机在东、北、天向的位置误差;
(405)采用卡尔曼滤波器对无人机节点1的导航系统进行更新。
进一步地,采用下式解算出无人机节点2和无人机节点3的绝对位置信息(xi,yi,zi):
其中,x、y、z为无人机节点1组合导航系统得到的高精度经度、纬度、高度信息,di为无人机节点i相对于无人机节点1的距离,αi、βi为无人机节点i相对于无人机节点1的俯仰角、方位角,νiE,νiN,νiU为无人机节点i的视觉导航设备解算的东向位置误差、北向位置误差、天向位置误差;i=2,3。
进一步地,在步骤(7)中,首先建立无人机节点2和无人机节点3的导航系统量测方程:
无人机节点2和无人机节点3的导航系统速度量测矩阵Hvi和量测噪声Vvi:
Hvi=[03×3 diag[1 1 0]03×12]
Vvi=[MDEi MDNi 0]
上式中,MDEi,MDNi为无人机节点i的光流传感器在东、北向上的均值为零且方差为定值的量测白噪声;
无人机节点2和无人机节点3的导航系统位置量测矩阵Hpi和量测噪声Vpi:
Vpi=[νiE νiN νiU]
上式中,i=2,3;
然后采用卡尔曼滤波器对无人机节点2和无人机节点3的导航系统进行更新。
进一步地,在步骤(9)中,首先建立无人机节点4的导航系统量测方程:
无人机节点4的导航系统速度量测矩阵Hv4和量测噪声Vv4:
Hv4=[03×3 diag[1 1 0]03×12]
Vv4=[ME4 MN4 0]
上式中,ME4,MN4为无人机节点4的光流传感器在东向、北向满足均值为零且方差为定值的速度量测白噪声;
无人机节点4的导航系统位置量测矩阵Hp4和量测噪声Vp4:
Vp4=[ν4E ν4N ν4U]
上式中,ν4E,ν4N,ν4U为无人机节点4的视觉导航设备解算的东向位置误差、北向位置误差、天向位置误差;
然后采用卡尔曼滤波器无人机节点4的导航系统进行更新。
进一步地,在步骤(8)中,设无人机节点1的位置为(x1,y1,z1),无人机节点2的位置为(x2,y2,z2),无人机节点3的位置为(x3,y3,z3),通过无人机节点4上的视觉导航设备获得与无人机节点1的距离d4、与无人机节点2的距离d2、与无人机节点3的距离d3,则:
通过最小二乘法解算上式求出无人机节点4的位置(x4,y4,z4)。
采用上述技术方案带来的有益效果:
无人机编队协同飞行时,受限于成本、体积、重量等方面的限制,每个无人机节点不可能都搭载高精度的导航传感器以实现无人机编队高精度的导航定位,本发明采用主从式的无人机编队协同导航方式,通过编队中长机搭载高精度的导航传感器实现长机的高精度的导航定位,各从机节点通过视觉导航设备获得从机无人机节点相对长机的距离,方位信息数学解算出各从无人机节点的绝对位置信息,同时各从无人机节点通过视觉导航设备实时获得无人机节点相对地面的速度,从而实现各从无人机节点高精度导航定位,提高了导航系统的稳定性、鲁棒性以及抗干扰性能。
附图说明
图1是本发明的总体框架图;
图2是本发明中无人机节点1的导航解算流程图;
图3是本发明中无人机节点2和3的导航解算流程图;
图4是本发明中无人机节点4的导航解算流程图。
具体实施方式
以下将结合附图,对本发明的技术方案进行详细说明。
如图1所示,本发明设计的一种无人机编队协同导航方法,无人机编队包含4架无人机节点:无人机节点1~4,包括以下步骤:
步骤1:建立机体坐标系和导航坐标系;
步骤2:在无人机节点1上搭载惯性导航系统和GPS接收机,周期读取k时刻无人机节点1的机载传感器信息,包括速度信息、位置信息,陀螺仪信息和加速度计信息;
步骤3:预测k时刻无人机节点i的姿态四元数、速度和位置信息,其中i=1,2,3,4;
步骤4:利用卡尔曼滤波器,对k时刻的无人机节点1的姿态四元数、速度、位置信息进行校正,得到无人机节点1的高精度导航信息,如图2所示;
步骤5:在无人机节点i上搭载惯性导航系统、视觉导航设备和光流传感器,周期性读取k时刻无人机节点i的机载传感器信息,包括相对于无人机节点1的距离信息、俯仰角和方位角,以及通过光流解算出的速度信息、陀螺仪信息和加速度计信息,其中i=2,3,4;
步骤6:根据相对于无人机节点1的距离信息、俯仰角和方位角解算出无人机节点2和无人机节点3的绝对位置信息;
步骤7:利用卡尔曼滤波器,根据无人机节点2和无人机节点3的位置信息和速度信息对k时刻的姿态四元数、速度、位置信息进行校正,得到无人机节点2和无人机节点3的高精度导航信息,如图3所示;
步骤8:根据无人机节点1~3的位置信息推导出无人机节点4的绝对位置信息;
步骤9:利用卡尔曼滤波器,将无人机节点4的位置信息和速度信息作为量测信息,对k时刻的姿态四元数、速度、位置信息进行校正,得到无人机节点4的高精度导航信息,如图4所示。
在本实施例中,步骤1采用如下优选技术方案:
机体坐标系的X、Y、Z轴分别为无人机的右向、机头方向、天向,导航坐标系的X、Y、Z轴分别为东向、北向、天向。
在本实施例中,步骤3采用如下优选技术方案:
采用下式预测姿态四元数:
其中,Qi(k)=[qi0(k) qi1(k) qi2(k) qi3(k)]T为k时刻的姿态四元数,上标T表示矩阵的转置;Qi(k-1)=[qi0(k-1) qi1(k-1) qi2(k-1) qi3(k-1)]T为k-1时刻的姿态四元数;ΔT为离散采样周期;通过下式计算:
其中通过下式计算:
其中,为k时刻陀螺仪读取的无人机节点i机体系相对于导航系的角速度在机体系X、Y、Z轴上的分量,为k时刻陀螺仪的零偏在机体系X、Y、Z轴上的分量;i=1,2,3,4;
采用下式预测速度信息:
其中,为k时刻加速度计读取的无人机节点i机体系相对于导航系的加速度在机体系X、Y、Z轴上的分量;为k时刻加速度计零偏在机体系X、Y、Z轴上的分量;g=[0 0 g]T,g为当地重力加速度值;为k时刻机体系相对于导航系的线速度在导航系X、Y、Z轴上的分量;为k-1时刻机体系相对于导航系的线速度在导航系X、Y、Z轴上的分量;i=1,2,3,4;为机体系到导航系之间的姿态矩阵,通过下式计算:
采用下式预测位置信息:
其中,分别为k时刻无人机节点i在导航系X、Y、Z轴上的位置坐标;k-1时刻的位置分别为k-1时刻无人机节点i在导航系X、Y、Z轴上的位置坐标。
在本实施例中,步骤4采用如下技术方案:
由于4个无人机节点的状态量和状态方程都相同,因此用i=1,2,3,4统一表示。
1)无人机节点一组合导航系统的状态量:
其中,为无人机节点i的平台误差角,δVEi,δVNi,δVUi为无人机节点i的陀螺东向、北向、天向速度误差,δLi,δλi,δhi为无人机节点i陀螺纬度、经度、高度误差,Li,λi,hi无人机节点i陀螺纬度、经度、高度值,εbxi,εbyi,εbzi为无人机节点i陀螺随机常数误差,εrxi,εryi,εrzi为无人机节点i陀螺一阶马尔科夫随机噪声,为无人机节点i加速度计一阶马尔科夫过程。
2)建立惯性导航系统的误差方程,包括平台误差角方程、速度误差方程和位置误差方程;
平台误差角方程:
其中,wie为地球自转角速度,vEi、vNi为无人机节点i惯性导航系统东向、北向速度,δvEi、δvNi为对应的速度误差,εEi、εNi、εUi为无人机节点i东、北、天向陀螺噪声角,RM=Re(1-2f+3f sin2Li),RN=Re(1+f sin2Li),Re=6378137米,f=1/298.257;
速度误差方程:
其中,vUi为无人机节点i惯性导航系统天向速度,δvUi为对应的速度误差,fEi、fNi、fUi为无人机节点i加速度计测得的加速度值,为无人机节点i东向、北向、天向加速度计一阶马尔可夫过程白噪声,R为地球半径,g为当地重力加速度值;
位置误差方程:
3)建立无人机节点1导航系统的状态方程,无人机节点1~4的状态方程相同,如下:
其中,Gi为系统噪声系数矩阵,Wi为噪声控制向量,FNi为平台误差角、速度误差、位置误差构成的系统矩阵, Tgxi、Tgyi、Tgzi为三轴陀螺相关时间常数,Taxi、Tayi、Tazi为三轴加速度计相关时间常数。
3)无人机节点1的系统量测方程:
k时刻GPS接收机获得无人机载体的速度和位置信息可表示为:
上式中,λt1,Lt1,ht1,NE1,NN1,NU1,vE1,vN1,vU1,ME1,MN1,MU1分别为GPS测得载体沿地理坐标系下各轴下的位置真值、GPS接收机在东、北、天向的位置误差、GPS测得载体在地理坐标系下沿各轴的速度真值以及GPS接收机沿东、北、天向的测速误差。将GPS接收机获得载体的速度信息、位置信息与惯导解算出载体的速度、位置信息作差分并写成系统量测方程形式Z=HX+V,可得:
无人机节点1导航系统的速度量测矩阵Hv1和量测噪声Vv1:
Hv1=[03×3 diag[1 1 1]03×12]
Vv1=[ME1 MN1 MU1]
上式中,ME1,MN1,MU1分别为无人机节点1的GPS接收机沿东、北、天向的测速误差;
无人机节点1导航系统的位置量测矩阵Hp1和量测噪声Vp1:
Hp1=[03×6 diag[RM RNcos L1 1]03×9]
Vp1=[NE1 NN1 NU1]
上式中,NE1,NN1,NU1分别为无人机节点1上的GPS接收机在东、北、天向的位置误差。
4)采用卡尔曼滤波器对无人机节点一导航系统进行更新:
Xk+1=Fk+1,kXk+GkWk
Zk+1=Hk+1Xk+1+Vk+1
上式中,Xk+1为系统k+1时刻的状态向量,Wk为k时刻的噪声控制向量,Fk+1,k为k到k+1时刻的系统状态转移矩阵,Gk为系统噪声系数矩阵;Zk+1为k+1时刻的量测向量,Hk+1为k+1时刻系统的量测矩阵,Vk+1为系统k+1时刻系统的量测噪声。
首先进行系统一步状态预测:
其次求解一步预测的均方差:
上式中,为k时刻状态变量Xk的估计值,为利用计算得到的预测k+1时刻状态变量Xk+1的值,Pk为k时刻估计均方差误差,Qk为系统的噪声方差矩阵,Pk+1/k为状态向量的均方差。
然后求解滤波增益:
上式中,Rk+1为噪声的方差矩阵,通过以上各式完成对无人机节点1的状态估值,从而得到滤波结果:
同时更新估计系统的均方差误差:
Pk+1=[1-Kk+1Hk+1]Pk+1/k
在本实施例中,步骤6采用如下优选技术方案:
采用下式解算出无人机节点2和无人机节点3的绝对位置信息(xi,yi,zi):
其中,x、y、z为无人机节点1组合导航系统得到的高精度经度、纬度、高度信息,di为无人机节点i相对于无人机节点1的距离,αi、βi为无人机节点i相对于无人机节点1的俯仰角、方位角,νiE,νiN,νiU为无人机节点i的视觉导航设备解算的东向位置误差、北向位置误差、天向位置误差;i=2,3。
在本实施例中,步骤7采用如下优选技术方案:
首先建立无人机节点2和无人机节点3的导航系统量测方程:
无人机节点2和无人机节点3的导航系统速度量测矩阵Hvi和量测噪声Vvi:
Hvi=[03×3 diag[1 1 0]03×12]
Vvi=[MDEi,MDNi,0]
上式中,MDEi,MDNi为无人机节点i的光流传感器在东、北向上的均值为零且方差为定值的量测白噪声;
无人机节点2和无人机节点3的导航系统位置量测矩阵Hpi和量测噪声Vpi:
Vpi=[νiE νiN νiU]
上式中,i=2,3;
然后采用卡尔曼滤波器对无人机节点2和无人机节点3的导航系统进行更新。
在本实施例中,步骤8采用如下优选技术方案:
设无人机节点1的位置为(x1,y1,z1),无人机节点2的位置为(x2,y2,z2),无人机节点3的位置为(x3,y3,z3),通过无人机节点4上的视觉导航设备获得与无人机节点1的距离d4、与无人机节点2的距离d2、与无人机节点3的距离d3,则:
通过最小二乘法解算上式求出无人机节点4的位置(x4,y4,z4)。
在本实施例中,步骤9采用如下优选技术方案:
k时刻无人机节点4解算得到的位置信息与光流解算得到的速度信息可用如下公式表示:
无人机节点4惯导解算出的位置在地球地固坐标系(ECEF)可表示为:
上式中xI,yI,zI为无人机节点4机载惯导系统解算出的载体位置在地球地固坐标系下的位置,xt,yt,zt为惯导解算出的位置在地球地固坐标下的位置真值,δx,δy,δz为地球地固坐标系下的三个方向的位置误差。
无人机节点4在地球地固(ECEF)坐标系解算出的位置可表示为:
上式中,x、y、z为无人机节点4在地球地固坐标系下三个方向上的位置,xt,yt,zt为三个方向上的位置真值,Nx,Ny,Nz为三个方向上的白噪声。
可得:
无人机节点4机载光流传感器解算得到的无人机水平方向的速度可表示为:
上式中,vDE4,vDN4为无人机节点4搭载的光流传感器获得无人机水平方向上对地速度,vDET4,vDNT4为水平方向对地速度真值。
将无人机节点4位置、速度信息与惯导解算出来的位置、速度信息作差分得到无人机节点四的量测量并改写成量测方程形式:
无人机节点4的导航系统速度量测矩阵Hv4和量测噪声Vv4:
Hv4=[03×3,diag[1,1,1],03×3]
Vv4=[ME4 MN4 0]
上式中,ME4,MN4为无人机节点4的光流传感器在东向、北向满足均值为零且方差为定值的速度量测白噪声;
无人机节点4的导航系统位置量测矩阵Hp4和量测噪声Vp4:
Vp4=[ν4E ν4N ν4U]
上式中,ν4E,ν4N,ν4U为无人机节点4的视觉导航设备解算的东向位置误差、北向位置误差、天向位置误差。
然后采用卡尔曼滤波器无人机节点4的导航系统进行更新。
实施例仅为说明本发明的技术思想,不能以此限定本发明的保护范围,凡是按照本发明提出的技术思想,在技术方案基础上所做的任何改动,均落入本发明保护范围之内。
Claims (8)
1.一种无人机编队协同导航方法,其特征在于,无人机编队包含4架无人机节点:无人机节点1~4,包括以下步骤:
(1)建立机体坐标系和导航坐标系;
(2)在无人机节点1上搭载惯性导航系统和GPS接收机,周期读取k时刻无人机节点1的机载传感器信息,包括速度信息、位置信息,陀螺仪信息和加速度计信息;
(3)预测k时刻无人机节点i的姿态四元数、速度和位置信息,其中i=1,2,3,4;
(4)利用卡尔曼滤波器,对k时刻的无人机节点1的姿态四元数、速度、位置信息进行校正,得到无人机节点1的高精度导航信息;
(5)在无人机节点i上搭载惯性导航系统、视觉导航设备和光流传感器,周期性读取k时刻无人机节点i的机载传感器信息,包括相对于无人机节点1的距离信息、俯仰角和方位角,以及通过光流解算出的速度信息、陀螺仪信息和加速度计信息,其中i=2,3,4;
(6)根据相对于无人机节点1的距离信息、俯仰角和方位角解算出无人机节点2和无人机节点3的绝对位置信息;
(7)利用卡尔曼滤波器,根据无人机节点2和无人机节点3的位置信息和速度信息对k时刻的姿态四元数、速度、位置信息进行校正,得到无人机节点2和无人机节点3的高精度导航信息;
(8)根据无人机节点1~3的位置信息推导出无人机节点4的绝对位置信息;
(9)利用卡尔曼滤波器,将无人机节点4的位置信息和速度信息作为量测信息,对k时刻的姿态四元数、速度、位置信息进行校正,得到无人机节点4的高精度导航信息。
2.根据权利要求1所述无人机编队协同导航方法,其特征在于,在步骤(1)中,机体坐标系的X、Y、Z轴分别为无人机的右向、机头方向、天向,导航坐标系的X、Y、Z轴分别为东向、北向、天向。
3.根据权利要求2所述无人机编队协同导航方法,其特征在于,在步骤(3)中,采用下式预测姿态四元数:
其中,Qi(k)=[qi0(k) qi1(k) qi2(k) qi3(k)]T为k时刻的姿态四元数,上标T表示矩阵的转置;Qi(k-1)=[qi0(k-1) qi1(k-1) qi2(k-1) qi3(k-1)]T为k-1时刻的姿态四元数;ΔT为离散采样周期;通过下式计算:
其中通过下式计算:
其中,为k时刻陀螺仪读取的无人机节点i机体系相对于导航系的角速度在机体系X、Y、Z轴上的分量,为k时刻陀螺仪的零偏在机体系X、Y、Z轴上的分量;i=1,2,3,4;
采用下式预测速度信息:
其中, 为k时刻加速度计读取的无人机节点i机体系相对于导航系的加速度在机体系X、Y、Z轴上的分量; 为k时刻加速度计零偏在机体系X、Y、Z轴上的分量;g=[0 0 g]T,g为当地重力加速度值; 为k时刻机体系相对于导航系的线速度在导航系X、Y、Z轴上的分量;为k-1时刻机体系相对于导航系的线速度在导航系X、Y、Z轴上的分量;i=1,2,3,4;为机体系到导航系之间的姿态矩阵,通过下式计算:
采用下式预测位置信息:
其中, 分别为k时刻无人机节点i在导航系X、Y、Z轴上的位置坐标;k-1时刻的位置 分别为k-1时刻无人机节点i在导航系X、Y、Z轴上的位置坐标。
4.根据权利要求2所述无人机编队协同导航方法,其特征在于,步骤(4)的具体过程如下:
(401)确定无人机节点1导航系统的状态向量,无人机节点1~4的状态向量相同,如下:
其中,为无人机节点i的平台误差角,δVEi,δVNi,δVUi为无人机节点i的陀螺东向、北向、天向速度误差,δLi,δλi,δhi为无人机节点i陀螺纬度、经度、高度误差,Li,λi,hi无人机节点i陀螺纬度、经度、高度值,εbxi,εbyi,εbzi为无人机节点i陀螺随机常数误差,εrxi,εryi,εrzi为无人机节点i陀螺一阶马尔科夫随机噪声,为无人机节点i加速度计一阶马尔科夫过程;
当i取1时即为无人机节点1导航系统的状态向量;
(402)建立惯性导航系统的误差方程,包括平台误差角方程、速度误差方程和位置误差方程;
平台误差角方程:
其中,wie为地球自转角速度,vEi、vNi为无人机节点i惯性导航系统东向、北向速度,δvEi、δvNi为对应的速度误差,εEi、εNi、εUi为无人机节点i东、北、天向陀螺噪声角,RM=Re(1-2f+3fsin2Li),RN=Re(1+f sin2Li),Re=6378137米,f=1/298.257;
速度误差方程:
其中,vUi为无人机节点i惯性导航系统天向速度,δvUi为对应的速度误差,fEi、fNi、fUi为无人机节点i加速度计测得的加速度值,为无人机节点i东向、北向、天向加速度计一阶马尔可夫过程白噪声,R为地球半径,g为当地重力加速度值;
位置误差方程:
当i取1时即为无人机节点1的惯性导航系统的误差方程;
(403)建立无人机节点1导航系统的状态方程,无人机节点1~4的状态方程相同,如下:
其中,Gi为系统噪声系数矩阵,Wi为噪声控制向量,FNi为平台误差角、速度误差、位置误差构成的系统矩阵, Tgxi、Tgyi、Tgzi为三轴陀螺相关时间常数,Taxi、Tayi、Tazi为三轴加速度计相关时间常数;
当i取1时即为无人机节点1导航系统的状态方程;
(404)建立无人机节点1导航系统的量测方程:
无人机节点1导航系统的速度量测矩阵Hv1和量测噪声Vv1:
Hv1=[03×3 diag[1 1 1] 03×12]
Vv1=[ME1 MN1 MU1]
上式中,ME1,MN1,MU1分别为无人机节点1的GPS接收机沿东、北、天向的测速误差;
无人机节点1导航系统的位置量测矩阵Hp1和量测噪声Vp1:
Hp1=[03×6 diag[RM RNcosL1 1] 03×9]
Vp1=[NE1 NN1 NU1]
上式中,NE1,NN1,NU1分别为无人机节点1上的GPS接收机在东、北、天向的位置误差;
(405)采用卡尔曼滤波器对无人机节点1的导航系统进行更新。
5.根据权利要求4所述无人机编队协同导航方法,其特征在于,在步骤(6)中,采用下式解算出无人机节点2和无人机节点3的绝对位置信息(xi,yi,zi):
其中,x、y、z为无人机节点1组合导航系统得到的高精度经度、纬度、高度信息,di为无人机节点i相对于无人机节点1的距离,αi、βi为无人机节点i相对于无人机节点1的俯仰角、方位角,νiE,νiN,νiU为无人机节点i的视觉导航设备解算的东向位置误差、北向位置误差、天向位置误差;i=2,3。
6.根据权利要求5所述无人机编队协同导航方法,其特征在于,在步骤(7)中,首先建立无人机节点2和无人机节点3的导航系统量测方程:
无人机节点2和无人机节点3的导航系统速度量测矩阵Hvi和量测噪声Vvi:
Hvi=[03×3 diag[1 1 0] 03×12]
Vvi=[MDEi MDNi 0]
上式中,MDEi,MDNi为无人机节点i的光流传感器在东、北向上的均值为零且方差为定值的量测白噪声;
无人机节点2和无人机节点3的导航系统位置量测矩阵Hpi和量测噪声Vpi:
Vpi=[νiE νiN νiU]
上式中,i=2,3;
然后采用卡尔曼滤波器对无人机节点2和无人机节点3的导航系统进行更新。
7.根据权利要求4所述无人机编队协同导航方法,其特征在于,在步骤(9)中,首先建立无人机节点4的导航系统量测方程:
无人机节点4的导航系统速度量测矩阵Hv4和量测噪声Vv4:
Hv4=[03×3 diag[1 1 0] 03×12]
Vv4=[ME4 MN4 0]
上式中,ME4,MN4为无人机节点4的光流传感器在东向、北向满足均值为零且方差为定值的速度量测白噪声;
无人机节点4的导航系统位置量测矩阵Hp4和量测噪声Vp4:
Vp4=[ν4E ν4N ν4U]
上式中,ν4E,ν4N,ν4U为无人机节点4的视觉导航设备解算的东向位置误差、北向位置误差、天向位置误差;
然后采用卡尔曼滤波器无人机节点4的导航系统进行更新。
8.根据权利要求1-7中任意一项所述无人机编队协同导航方法,其特征在于,在步骤(8)中,设无人机节点1的位置为(x1,y1,z1),无人机节点2的位置为(x2,y2,z2),无人机节点3的位置为(x3,y3,z3),通过无人机节点4上的视觉导航设备获得与无人机节点1的距离d4、与无人机节点2的距离d2、与无人机节点3的距离d3,则:
通过最小二乘法解算上式求出无人机节点4的位置(x4,y4,z4)。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201910201866.1A CN109813311B (zh) | 2019-03-18 | 2019-03-18 | 一种无人机编队协同导航方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201910201866.1A CN109813311B (zh) | 2019-03-18 | 2019-03-18 | 一种无人机编队协同导航方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN109813311A true CN109813311A (zh) | 2019-05-28 |
CN109813311B CN109813311B (zh) | 2020-09-15 |
Family
ID=66609272
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201910201866.1A Active CN109813311B (zh) | 2019-03-18 | 2019-03-18 | 一种无人机编队协同导航方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN109813311B (zh) |
Cited By (17)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN110274588A (zh) * | 2019-06-19 | 2019-09-24 | 南京航空航天大学 | 基于无人机集群信息的双层嵌套因子图多源融合导航方法 |
CN110487267A (zh) * | 2019-07-10 | 2019-11-22 | 湖南交工智能技术有限公司 | 一种基于vio&uwb松组合的无人机导航系统及方法 |
CN110598184A (zh) * | 2019-09-18 | 2019-12-20 | 南京山鹞航空科技有限公司 | 一种编队复合跟踪系统数据注册误差校准方法 |
CN110763238A (zh) * | 2019-11-11 | 2020-02-07 | 中电科技集团重庆声光电有限公司 | 基于uwb、光流和惯性导航的高精度室内三维定位方法 |
CN110849360A (zh) * | 2019-11-25 | 2020-02-28 | 中国航空无线电电子研究所 | 面向多机协同编队飞行的分布式相对导航方法 |
CN111238469A (zh) * | 2019-12-13 | 2020-06-05 | 南京航空航天大学 | 一种基于惯性/数据链的无人机编队相对导航方法 |
CN111290395A (zh) * | 2020-03-10 | 2020-06-16 | 上海航天控制技术研究所 | 一种基于子母艇的无人艇自主回收方法 |
CN112013839A (zh) * | 2020-08-18 | 2020-12-01 | 重庆交通大学 | 一种gps拒止环境下无人机集群实时定位方法 |
CN112304304A (zh) * | 2020-10-23 | 2021-02-02 | 国网智能科技股份有限公司 | 一种适用于变电站的巡检无人机、系统及方法 |
CN112630769A (zh) * | 2020-11-30 | 2021-04-09 | 东南大学 | 一种以无人机为载体的雷达流速测量系统及方法 |
CN112762936A (zh) * | 2020-12-24 | 2021-05-07 | 中国人民解放军军事科学院国防科技创新研究院 | 一种运用于长航时无人机载荷的多源定位信息融合方法 |
CN112859923A (zh) * | 2021-01-25 | 2021-05-28 | 西北工业大学 | 一种无人机视觉编队飞行控制系统 |
CN112947503A (zh) * | 2021-03-10 | 2021-06-11 | 中国人民解放军海军潜艇学院 | 一种水下滑翔机编队的协同控制方法 |
CN113238583A (zh) * | 2021-07-14 | 2021-08-10 | 四川腾盾科技有限公司 | 一种固定翼无人机密集编队飞行与防撞控制方法 |
CN113406975A (zh) * | 2021-08-19 | 2021-09-17 | 北京科技大学 | 一种仿生智能多无人机集群自主编队导航控制方法及装置 |
CN113551671A (zh) * | 2021-06-10 | 2021-10-26 | 中国科学院西安光学精密机械研究所 | 一种无人机姿态与位置的实时高精度测量系统及方法 |
CN114608578A (zh) * | 2022-03-07 | 2022-06-10 | 北京航空航天大学 | 一种加权不确定性无人机集群协同导航方法 |
Citations (8)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US20160209849A1 (en) * | 2015-01-15 | 2016-07-21 | William Dale Arbogast | System and method for decentralized, multi-agent unmanned vehicle navigation and formation control |
CN106249759A (zh) * | 2016-10-10 | 2016-12-21 | 北京航空航天大学 | 一种导弹自主编队在队形控制过程中的冲突协调方法 |
CN107121666A (zh) * | 2017-04-17 | 2017-09-01 | 南京航空航天大学 | 一种基于无人飞行器的临近空间运动目标定位方法 |
CN107289942A (zh) * | 2017-06-20 | 2017-10-24 | 南京航空航天大学 | 一种用于编队飞行的相对导航系统及方法 |
CN107765703A (zh) * | 2016-08-17 | 2018-03-06 | 中国航空工业集团公司西安飞行自动控制研究所 | 一种基于惯性/卫星相对导航的机载编队飞行指示系统 |
CN108151737A (zh) * | 2017-12-19 | 2018-06-12 | 南京航空航天大学 | 一种动态互观测关系条件下的无人机蜂群协同导航方法 |
CN108196290A (zh) * | 2017-11-20 | 2018-06-22 | 南京航空航天大学 | 一种无人机群载极化阵列姿态测量和目标探测方法 |
CN108731670A (zh) * | 2018-05-18 | 2018-11-02 | 南京航空航天大学 | 基于量测模型优化的惯性/视觉里程计组合导航定位方法 |
-
2019
- 2019-03-18 CN CN201910201866.1A patent/CN109813311B/zh active Active
Patent Citations (8)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US20160209849A1 (en) * | 2015-01-15 | 2016-07-21 | William Dale Arbogast | System and method for decentralized, multi-agent unmanned vehicle navigation and formation control |
CN107765703A (zh) * | 2016-08-17 | 2018-03-06 | 中国航空工业集团公司西安飞行自动控制研究所 | 一种基于惯性/卫星相对导航的机载编队飞行指示系统 |
CN106249759A (zh) * | 2016-10-10 | 2016-12-21 | 北京航空航天大学 | 一种导弹自主编队在队形控制过程中的冲突协调方法 |
CN107121666A (zh) * | 2017-04-17 | 2017-09-01 | 南京航空航天大学 | 一种基于无人飞行器的临近空间运动目标定位方法 |
CN107289942A (zh) * | 2017-06-20 | 2017-10-24 | 南京航空航天大学 | 一种用于编队飞行的相对导航系统及方法 |
CN108196290A (zh) * | 2017-11-20 | 2018-06-22 | 南京航空航天大学 | 一种无人机群载极化阵列姿态测量和目标探测方法 |
CN108151737A (zh) * | 2017-12-19 | 2018-06-12 | 南京航空航天大学 | 一种动态互观测关系条件下的无人机蜂群协同导航方法 |
CN108731670A (zh) * | 2018-05-18 | 2018-11-02 | 南京航空航天大学 | 基于量测模型优化的惯性/视觉里程计组合导航定位方法 |
Non-Patent Citations (3)
Title |
---|
J.A. GUERRERO 等,: ""Flight formation of multiple mini rotorcraft via coordination control"", 《2010 IEEE INTERNATIONAL CONFERENCE ON ROBOTICS AND AUTOMATION》 * |
熊骏 等,: ""一种基于置信传播和协同信息筛选的无人机编队协同导航方法"", 《2018年无人载体与控制技术发展及应用学术研讨峰会文集》 * |
许晓伟 等,: ""多无人机协同导航技术研究现状及进展"", 《导航定位与授时》 * |
Cited By (29)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN110274588A (zh) * | 2019-06-19 | 2019-09-24 | 南京航空航天大学 | 基于无人机集群信息的双层嵌套因子图多源融合导航方法 |
CN110274588B (zh) * | 2019-06-19 | 2020-12-08 | 南京航空航天大学 | 基于无人机集群信息的双层嵌套因子图多源融合导航方法 |
CN110487267B (zh) * | 2019-07-10 | 2021-06-04 | 湖南交工智能技术有限公司 | 一种基于vio&uwb松组合的无人机导航系统及方法 |
CN110487267A (zh) * | 2019-07-10 | 2019-11-22 | 湖南交工智能技术有限公司 | 一种基于vio&uwb松组合的无人机导航系统及方法 |
CN110598184A (zh) * | 2019-09-18 | 2019-12-20 | 南京山鹞航空科技有限公司 | 一种编队复合跟踪系统数据注册误差校准方法 |
CN110598184B (zh) * | 2019-09-18 | 2020-04-28 | 南京山鹞航空科技有限公司 | 一种编队复合跟踪系统数据注册误差校准方法 |
CN110763238A (zh) * | 2019-11-11 | 2020-02-07 | 中电科技集团重庆声光电有限公司 | 基于uwb、光流和惯性导航的高精度室内三维定位方法 |
CN110849360A (zh) * | 2019-11-25 | 2020-02-28 | 中国航空无线电电子研究所 | 面向多机协同编队飞行的分布式相对导航方法 |
CN110849360B (zh) * | 2019-11-25 | 2023-08-01 | 中国航空无线电电子研究所 | 面向多机协同编队飞行的分布式相对导航方法 |
CN111238469A (zh) * | 2019-12-13 | 2020-06-05 | 南京航空航天大学 | 一种基于惯性/数据链的无人机编队相对导航方法 |
CN111238469B (zh) * | 2019-12-13 | 2023-09-29 | 南京航空航天大学 | 一种基于惯性/数据链的无人机编队相对导航方法 |
CN111290395A (zh) * | 2020-03-10 | 2020-06-16 | 上海航天控制技术研究所 | 一种基于子母艇的无人艇自主回收方法 |
CN111290395B (zh) * | 2020-03-10 | 2023-07-14 | 上海航天控制技术研究所 | 一种基于子母艇的无人艇自主回收方法 |
CN112013839A (zh) * | 2020-08-18 | 2020-12-01 | 重庆交通大学 | 一种gps拒止环境下无人机集群实时定位方法 |
CN112304304A (zh) * | 2020-10-23 | 2021-02-02 | 国网智能科技股份有限公司 | 一种适用于变电站的巡检无人机、系统及方法 |
CN112304304B (zh) * | 2020-10-23 | 2023-04-18 | 国网智能科技股份有限公司 | 一种适用于变电站的巡检无人机、系统及方法 |
CN112630769A (zh) * | 2020-11-30 | 2021-04-09 | 东南大学 | 一种以无人机为载体的雷达流速测量系统及方法 |
CN112630769B (zh) * | 2020-11-30 | 2023-12-12 | 东南大学 | 一种以无人机为载体的雷达流速测量系统及方法 |
CN112762936A (zh) * | 2020-12-24 | 2021-05-07 | 中国人民解放军军事科学院国防科技创新研究院 | 一种运用于长航时无人机载荷的多源定位信息融合方法 |
CN112859923A (zh) * | 2021-01-25 | 2021-05-28 | 西北工业大学 | 一种无人机视觉编队飞行控制系统 |
CN112947503A (zh) * | 2021-03-10 | 2021-06-11 | 中国人民解放军海军潜艇学院 | 一种水下滑翔机编队的协同控制方法 |
CN112947503B (zh) * | 2021-03-10 | 2023-09-05 | 中国人民解放军海军潜艇学院 | 一种水下滑翔机编队的协同控制方法 |
CN113551671A (zh) * | 2021-06-10 | 2021-10-26 | 中国科学院西安光学精密机械研究所 | 一种无人机姿态与位置的实时高精度测量系统及方法 |
CN113238583A (zh) * | 2021-07-14 | 2021-08-10 | 四川腾盾科技有限公司 | 一种固定翼无人机密集编队飞行与防撞控制方法 |
CN113238583B (zh) * | 2021-07-14 | 2021-09-24 | 四川腾盾科技有限公司 | 一种固定翼无人机密集编队飞行与防撞控制方法 |
CN113406975B (zh) * | 2021-08-19 | 2021-11-30 | 北京科技大学 | 一种仿生智能多无人机集群自主编队导航控制方法及装置 |
CN113406975A (zh) * | 2021-08-19 | 2021-09-17 | 北京科技大学 | 一种仿生智能多无人机集群自主编队导航控制方法及装置 |
CN114608578A (zh) * | 2022-03-07 | 2022-06-10 | 北京航空航天大学 | 一种加权不确定性无人机集群协同导航方法 |
CN114608578B (zh) * | 2022-03-07 | 2024-04-09 | 北京航空航天大学 | 一种加权不确定性无人机集群协同导航方法 |
Also Published As
Publication number | Publication date |
---|---|
CN109813311B (zh) | 2020-09-15 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN109813311A (zh) | 一种无人机编队协同导航方法 | |
CN112629538B (zh) | 基于融合互补滤波和卡尔曼滤波的舰船水平姿态测量方法 | |
CN109556632B (zh) | 一种基于卡尔曼滤波的ins/gnss/偏振/地磁组合导航对准方法 | |
CN106767752B (zh) | 一种基于偏振信息的组合导航方法 | |
KR100761011B1 (ko) | 카메라형 태양센서를 이용한 관성항법시스템의자세보정장치 및 방법 | |
JP4782111B2 (ja) | 輸送手段の位置、姿勢、および/または飛行方向を推定するシステムおよび方法 | |
CN104635251B (zh) | 一种ins/gps组合定位定姿新方法 | |
CN110274588A (zh) | 基于无人机集群信息的双层嵌套因子图多源融合导航方法 | |
CN110207697A (zh) | 基于角加速度计/陀螺/加速度计的惯性导航解算方法 | |
CN103196445B (zh) | 基于匹配技术的地磁辅助惯性的载体姿态测量方法 | |
CN105094138A (zh) | 一种用于旋翼无人机的低空自主导航系统 | |
CN109186597B (zh) | 一种基于双mems-imu的室内轮式机器人的定位方法 | |
CN110207691B (zh) | 一种基于数据链测距的多无人车协同导航方法 | |
CN105021183A (zh) | 多旋翼飞行器gps和ins低成本组合导航系统 | |
CN110849360B (zh) | 面向多机协同编队飞行的分布式相对导航方法 | |
CN105318876A (zh) | 一种惯性里程计组合高精度姿态测量方法 | |
Leutenegger et al. | A low-cost and fail-safe inertial navigation system for airplanes | |
CN108151737A (zh) | 一种动态互观测关系条件下的无人机蜂群协同导航方法 | |
CN108344413B (zh) | 一种水下滑翔器导航系统及其低精度与高精度转换方法 | |
CN110017837A (zh) | 一种姿态抗磁干扰的组合导航方法 | |
CN106352897B (zh) | 一种基于单目视觉传感器的硅mems陀螺误差估计与校正方法 | |
CN105841698A (zh) | 一种无需调零的auv舵角精确实时测量系统 | |
CN103335654A (zh) | 一种行星动力下降段的自主导航方法 | |
CN108592911A (zh) | 一种四旋翼飞行器动力学模型/机载传感器组合导航方法 | |
CN108536163A (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 |