CN113759982B - 一种仅基于视线测量信息的无人机编队相对状态估计方法 - Google Patents
一种仅基于视线测量信息的无人机编队相对状态估计方法 Download PDFInfo
- Publication number
- CN113759982B CN113759982B CN202111217659.9A CN202111217659A CN113759982B CN 113759982 B CN113759982 B CN 113759982B CN 202111217659 A CN202111217659 A CN 202111217659A CN 113759982 B CN113759982 B CN 113759982B
- Authority
- CN
- China
- Prior art keywords
- unmanned aerial
- aerial vehicle
- wing unmanned
- sight
- fixed wing
- 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.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 38
- 238000005259 measurement Methods 0.000 title claims abstract description 37
- 230000015572 biosynthetic process Effects 0.000 title claims abstract description 31
- 230000000007 visual effect Effects 0.000 claims abstract description 16
- 238000004891 communication Methods 0.000 claims abstract description 14
- 230000008859 change Effects 0.000 claims description 21
- 239000013598 vector Substances 0.000 claims description 12
- 238000004458 analytical method Methods 0.000 claims description 9
- 238000012937 correction Methods 0.000 claims description 5
- 239000011159 matrix material Substances 0.000 claims description 4
- 230000009466 transformation Effects 0.000 claims description 4
- 230000001133 acceleration Effects 0.000 claims description 3
- 238000004364 calculation method Methods 0.000 claims description 3
- 230000009194 climbing Effects 0.000 claims description 3
- 238000001914 filtration Methods 0.000 claims description 3
- 230000008569 process Effects 0.000 claims description 3
- 238000001514 detection method Methods 0.000 claims description 2
- 230000004438 eyesight Effects 0.000 abstract description 5
- 230000003993 interaction Effects 0.000 description 5
- 230000006870 function Effects 0.000 description 4
- 230000003287 optical effect Effects 0.000 description 4
- 230000016776 visual perception Effects 0.000 description 3
- 230000008901 benefit Effects 0.000 description 2
- 238000003384 imaging method Methods 0.000 description 2
- 230000009471 action Effects 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000006243 chemical reaction Methods 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 238000010586 diagram Methods 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 238000002474 experimental method Methods 0.000 description 1
- 230000008447 perception Effects 0.000 description 1
- 238000012545 processing Methods 0.000 description 1
- 238000011160 research Methods 0.000 description 1
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/10—Simultaneous control of position or course in three dimensions
- G05D1/101—Simultaneous control of position or course in three dimensions specially adapted for aircraft
- G05D1/104—Simultaneous control of position or course in three dimensions specially adapted for aircraft involving a plurality of aircrafts, e.g. formation flying
Landscapes
- Engineering & Computer Science (AREA)
- Aviation & Aerospace Engineering (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
本发明属于无人机集群编队技术领域,提供一种仅基于视觉测量新息的固定翼无人机编队的相对状态估计方法。首先测量相邻无人机的视线高低角qε和方位角qβ;搜索与锁定目标固定翼无人机,测量观测固定翼无人机飞行状态和吊舱框架角并依据所得数据计算视线角;观测固定翼无人机主动机动,估计目标固定翼无人机初始状态;利用观测固定翼无人机视线测量信息,实时刷新无人机相对状态;观测固定翼无人机主动机动,矫正目标固定翼无人机高度估计值;若无人集群编队完成预定任务或者恢复正常通信后,则算法终止。本发明极大的降低了相对状态估计对视觉信息测量的要求,提高了视觉作为集群获取协同信息重要补充手段的实用性。
Description
技术领域
本发明属于无人机集群编队技术领域,特别是涉及一种仅基于视觉测量新息的固定翼无人机编队的相对状态估计方法。
背景技术
信息的感知与交互是无人集群编队协同的基础。当前集群编队协同的研究主要集中于实现各无人机飞行时间、空间位置和任务功能的协同,其中时间协同侧重于各飞行器飞行时间的交互,空间协同侧重于各飞行器位置、速度等状态的交互,而功能协同则侧重于各无人平台载荷特性互补与增强的协同。通信是集群编队协同所需信息的主要手段,目前编队稳定条件下的通信拓扑约束和有限通信带宽条件下的编队协同已得到深入研究,基于通信的集群编队协同技术有效性也在试验和应用中得到广泛验证。但在实际应用中无人机集群商演坠机等事件也暴漏了通信在电磁干扰环境下易受干扰,信息交互可靠性降低的缺点,严重影响了集群协同的效果。
视觉感知是集群编队获取空间协同需要信息的重要补充手段。相比于通信,视觉感知信息不受电磁干扰,可显著提高集群编队对复杂环境的适应能力。此外,通信作为一种典型的信息有源感知和交互手段,在敏感争议地区执行任务的通信集群容易暴漏,视觉感知具有较强的隐蔽性,且可同时完成对合作目标与非合作目标的信息感知,实现编队协同和目标搜索侦察等多项任务。目前视觉已广泛应用于无人***的导航,但在编队协同领域的应用还较少,这主要受限于目前的视觉图像处理算法主要用于目标辨识,而编队空间协同依赖于相对状态的估计,需要利用图像完成相邻无人机状态的估计,这不仅仅要完成目标的辨识,还要完成目标外形特征部位的辨识后,并结合目标外形先验信息、视觉传感器等先验信息才能准确结算相对状态。为保证相对状态的估计精度,目标视觉成像质量和目标先验信息可信度都要满足较高的要求,同时无人机运动时可能会引起机身特征部分遮挡,降低了该方法的可用性。
发明内容
本发明结合固定翼无人机的运动特性,提出了基于仅基于视线的目标相对状态信息估计方法。与现有基于视觉图像辨识特征部分结算相对运动状态的思路不同,本发明只需要测量相邻无人机的视线高低角和方位角信息,极大的降低了相对状态估计对视觉信息测量的要求,提高了视觉作为集群获取协同信息重要补充手段的实用性。
为达到上述目的,本发明的技术方案如下:
一种基于视线测量信息的无人机编队相对状态估计方法,包括如下步骤:
第一步:配置视觉传感器,准确测量相邻无人机的视线高低角qε和方位角qβ;
第二步:搜索与锁定目标固定翼无人机,确保目标固定翼无人机在观测固定翼无人机见光吊舱图像下;
第三步:测量观测固定翼无人机飞行状态和吊舱框架角;
第四步:根据步骤三所得数据计算视线角;
第五步:观测固定翼无人机主动机动,估计目标固定翼无人机初始状态;
固定翼无人机执行任务时,飞行速度大小、航向角变化率和高度在短时间内视为常值,基于此,令观测固定翼无人机保持其当前巡航水平飞行速度的情况下,以常爬升率机动;在机动期间不断解算其与目标固定翼无人机的视线,依次记为
基于目标固定翼无人机飞行速度大小、航向角变化率和高度短时间内视为常值的假设,其运动特性由下式表示:
其中xg,l、yg,l和zg,l分别表示目标固定翼无人机在东北天惯性坐标系下的位置,vl和ψv,l分别表示无人机的速度大小和航向角,ωl为无人机的航向角变化率;为避免目标固定翼无人机位置求解时因航向角速度为零出现歧义,目标固定翼无人机各时刻的位置解析表达式需分情况讨论;
(1)工况1:目标固定翼无人机直线运动
若观测固定翼无人机各时刻视线与水平面的交点共线,则目标固定翼无人机做直线运动,记观测初始时刻为0,则目标固定翼无人机各时刻的位置解析表达式为:
结合观测固定翼无人机各时刻的位置解算目标固定翼无人机的视线角,即有:
其中:
式中xg,f、yl,g和zg,f为僚机状态;记各时刻观测固定翼无人机利用上式估算得到的视线信息为则目标固定翼无人机初始时刻的xg,l,0、yg,l,0和zg,l,0确定,/>确定,因此各时刻视线信息视为以X0为变量的函数:
X0=[xg,l,0,yg,l,0,zg,l,0,vl,0,ψv,l,0]T
考虑各时刻视线单位方向矢量的测量偏差,以观测固定翼无人机各时刻的视线单位方向矢量估计值与测量值的平方差和J最小为目标,确定X0;
即:
其中
W=diag(W1,W2,...,Wn)
式中Re为视线单位方向矢量的测量协方差,即视线高低角和方位角的测量偏差分别为σqε和σqβ,则
其中:
实际计算时,各时刻的权重W1、W2、…、Wn由当前时刻的视线角利用上式求得;
记为X0的最优估计值,则各时刻的视线单位矢量测量值用下式近似:
则J中表示为:
相应有
J=(ΔeLOS-HΔX)TW(ΔeLOS-HΔX)
其中:
H=diag(h1,h2,...,hn)
对于一X0,若J取值最小时,应有:
即有:
ΔX=(HTWH)-1HTWΔeLOS
其中:
与之对应:
其中:
利用上式确定ΔX后,首先按下式更新X0:
X0=X0+ΔX
然后以更新后的X0计算ΔX,并按上式更新得到新的X0,不断迭代,直至|ΔX|<ζ,其中ζ为一近似于0的小量;
(2)工况2:目标固定翼无人机曲线运动
若观测固定翼无人机各时刻视线与水平面的交点共线,则目标固定翼无人机做直线运动,记观测初始时刻为0,则目标固定翼无人机各时刻的位置解析表达式为:
其中xg,l,0、yg,l,0和zg,l,0分别表示初始时刻目标固定翼无人机在东北天惯性坐标系下的位置,vl,0和ψv,l,0分别表示初始时的速度大小和航向角,ωl,0为初始时刻的航向角变化率;
采用与工况1相同的方法确定J,并得:
ΔX=(HTWH)-1HTWΔeLOS
其中:
H=diag(h1,h2,...,hn)
X0=[xg,l,0 yg,l,0 zg,l,0 vl,0 ψl,0 ωl,0]T
利用上式确定ΔX后,首先按下式更新X0:
X0=X0+ΔX
然后以更新后的X0计算ΔX,并按上式更新得到新的X0;反复迭代,直至|ΔX|<ζ,其中ζ为一近似于0的小量;
第六步:利用观测固定翼无人机视线测量信息,实时刷新无人机相对状态;
利用第4步估计的目标固定翼无人机高度信息,结合其实时测得的视线角信息,估计长僚机相对状态;此时不需要观测固定翼无人机进行机动;
利用观测固定翼无人机的测量信息和目标固定翼无人机高度估计值,得两无人机的相对位置测量值:
结合观测固定翼无人机的位置测量值和/>得:
记k时刻的目标无人滤波状态量为Xk=[xg,l,k,yg,l,k,vg,l,k,ψg,l,k,ωg,l,k]T,若|ωg,l,k|<ξ,ξ为近似于0的小量,则按下式计算k+1时刻的状态均值递推值和状态协方差递推值Pk+1|k:
其中σa和σω对应为以白噪声近似速度大小变化率和航向角加速度特性的标准方差;
若|ωg,l,k|>ξ,则按下式计算k+1时刻的状态均值递推值和状态协方差递推值Pk+1|k结合观测固定翼无人机各时刻的位置解算目标固定翼无人机的视线角,即有:
JP(:,1)=[1,0,0,0,0,0]T
JP(:,2)=[0,1,0,0,0,0]T
JP(3,5)=0,JP(4,5)=T,JP(5,5)=1
利用相对位置在x和y方向的侧量值,对状态均值和协方差进行更新:
Kk=Pk+1|kHT(HPk+1|kHT+Rr_xy)
Pk=(I5-KkH)Pk+1|k
其中
则k+1时刻的相对位置和速度估计值为:
第七步,观测固定翼无人机主动机动,矫正目标固定翼无人机高度估计值;
为避免第五步执行过程中,目标固定翼无人机高度发生变化,引起相对状态估计偏差,应在一定矫正时间间隔后进行主动机动,重新矫正高度估计值,然后继续执行第六步。待固定翼无人机速度大小估计值再次发生明显变化后,再执行第五步,保持循环。矫正时间间隔定义为上一次主动机动结束至检测到目标固定翼无人机速度大小估计值发生显著变化的时间区间,若固定翼无人机速度大小估计值始终稳定在某个值附近,则无需主动机动执行高度矫正。
第八步,任务结束
根据任务设置,若无人集群编队完成预定任务或者恢复正常通信后,则算法终止。
本发明的有效收益:
本发明针对固定翼无人机集群编队提出了一种仅基于视线角测量信息的相对状态估计方法,有助于降低集群内固定翼无人机相对状态协调对通信的依赖,提高固定翼无人机集群对复杂电磁环境的适应性。同时,视觉感知相对于通信具有隐蔽性强、装置功耗低、平台适应性强等多种优势,所提方法操作简单,在未来军民领域具有显著的应用潜力。
附图说明
图1为东北天坐标系下的视线角示意图;
图2为机体坐标系到吊舱坐标系的几何变换关系示意图;
图3为本发明无人机编队相对状态估计方法实现流程图。
具体实施方式
下面结合附图对本发明的实现过程进行详细的解释和说明。
第一步:配置测量装置
根据集群编队规模的大小,确定状态估计时无人机的相对间距。以此为视觉传感器的作用距离确定视觉传感器型号,确保集群正常编队规模情况下,各无人机可采用该视觉传感器准确测量相邻无人机的视线高低角qε和方位角qβ。定义东北天坐标系OgXgYgZg为参考惯性坐标系,即Xg轴指向正东,Yg轴指向正北,Zg轴垂直当地水平面指向正上,原点Og取为无人机起飞点。视线高低角和方位角在东北天惯性坐标系下如图1所示。
第二步:搜索与锁定目标无人机,确保目标无人机在观测无人机可见光吊舱图像下。
确保目标无人机在观测无人机可见光吊舱图像下是实现两无人机相对状态估计的基本前提。可见光吊舱是目前固定翼无人机挂载的常规视觉传感器,当目标无人机在其作用距离范围内时,它可在飞手操作下或者自主完成目标无人机的搜索和锁定。稳定锁定情况下,吊舱将会自动调整其框架角,使得目标位于其视场中心,即目标在无人吊舱成像的中心。
第三步:测量观测无人机飞行状态和吊舱框架角
无人机飞行状态通常可采用无人机的惯性导航***或者GPS测得,具体测量包括观测无人机姿态俯仰角偏航角ψb和滚转角γb,飞行速度在惯性参考坐标系三轴的分量vx,g,f、vy,g,f和vz,g,f,飞行位置在惯性参考坐标系三轴的分量xg,f、yg,f和zg,f。无人机姿态角主要用于描述无人机体系obxbybzb与惯性参考坐标系间的转换惯性。无人机体系ob原点定义为无人机质心,xb由质心指向机头,yb位于机身纵平面内垂直xb轴指向上,zb轴由xb和yb指向按右手定则确定。
无人机吊舱框架角主要用于描述吊舱光轴的指向与无人机体系间的关系,具体包含方位框架角βc和高低框架角εc,可由吊舱自带伺服***的传感器测得。吊舱安装时,通过会通过校准使得其光轴指向与机体xg轴指向平行,吊舱基准平面与无人机体系obxbyb平面平行。当吊舱通过调整框架角使得目标位于视场中心时,目标无人机相对于观测无人机的视线角与吊舱光轴指向平行。
第四步:计算视线角
吊舱稳定锁定目标无人机时,目标位于其视场中心,因此观测无人机与目标无人机的视线与吊舱光轴指向平行。据此,可结合无人机姿态角和吊舱框架角解算无人机长僚机视线方向,具体如下:
其中eLOS,c=[1,0,0]T,Mg2b为东北天惯性坐标系到无人机体系的余弦转换矩阵,可由俯仰角ψb和γb确定,/>为无人机机体坐标系到吊舱坐标系的余弦转换矩阵,可由高低框架角εc和方位框架角βc确定。/>
同样eLOS,g可由视线高低角qε和方位角qβ表示为:
eLOS,g=[cos qε cos qβ cos qε sin qβ sin qε]T
相应可由eLOS,g解得:
为避免视线角解算时出现歧义性,可直接利用eLOS,g等价描述视线信息。
第五步:观测无人机主动机动,估计目标无人机初始状态
固定翼无人机执行任务时,飞行速度大小、航向角变化率和高度在短时间内可视为常值,基于此,可令观测固定翼无人机保持其当前巡航水平飞行速度的情况下,以常爬升率机动。在机动期间不断解算其与目标无人机的视线,依次记为
基于目标固定翼无人机飞行速度大小、航向角变化率和高度短时间内可视为常值的假设,其运动特性可由下式表示:
其中xg,l、yg,l和zg,l分别表示目标无人机在东北天惯性坐标系下的位置,vl和ψv,l分别表示无人机的速度大小和航向角,ωl为无人机的航向角变化率。为避免目标固定翼无人机位置求解时因航向角速度为零出现歧义,目标无人机各时刻的位置解析表达式需分情况讨论。
(1)工况1:目标无人机直线运动
若观测无人机各时刻视线与水平面的交点共线,则目标无人机做直线运动。记观测初始时刻为0,则目标无人机各时刻的位置解析表达式为:
结合观测无人机各时刻的位置可解算目标无人机的视线角,即有:
其中:
式中xg,f、yl,g和zg,f为僚机状态。记各时刻观测无人机利用上式估算得到的视线信息为可以看出若目标无人机初始时刻的xg,l,0、yg,l,0和zg,l,0确定,则/>确定,因此各时刻视线信息可视为以X0为变量的函数。
X0=[xg,l,0,yg,l,0,zg,l,0,vl,0,ψv,l,0]T
考虑各时刻视线单位方向矢量的测量偏差,以观测无人机各时刻的视线单位方向矢量估计值与测量值的平方差和J最小为目标,确定X0。
即:
其中
W=diag(W1,W2,...,Wn)
式中Re为视线单位方向矢量的测量协方差。即视线高低角和方位角的测量偏差分别为σqε和σqβ,则
其中:
实际计算时,各时刻的权重W1、W2、…、Wn可由当前时刻的视线角利用上式求得。
记为X0的最优估计值,则各时刻的视线单位矢量测量值可用下式近似:
则J中可表示为:
相应有
J=(ΔeLOS-HΔX)TW(ΔeLOS-HΔX)
其中:
H=diag(h1,h2,...,hn)
对于一X0,若J取值最小时,应有:
/>
即有:
ΔX=(HTWH)-1HTWΔeLOS
其中:
与之对应:
其中:
利用上式确定ΔX后,首先按下式更新X0:
X0=X0+ΔX
然后以更新后的X0计算ΔX,并按上式更新得到新的X0。不断迭代,直至|ΔX|<ζ,其中ζ为一近似于0的小量。
(2)工况2:目标无人机曲线运动
若观测无人机各时刻视线与水平面的交点共线,则目标无人机做直线运动。记观测初始时刻为0,则目标无人机各时刻的位置解析表达式为:
/>
其中xg,l,0、yg,l,0和zg,l,0分别表示初始时刻目标无人机在东北天惯性坐标系下的位置,vl,0和ψv,l,0分别表示初始时的速度大小和航向角,ωl,0为初始时刻的航向角变化率。
采用与工况1相同的方法确定J,并可得:
ΔX=(HTWH)-1HTWΔeLOS
其中:
H=diag(h1,h2,...,hn)
X0=[xg,l,0 yg,l,0 zg,l,0 vl,0 ψl,0 ωl,0]T
利用上式确定ΔX后,首先按下式更新X0:
X0=X0+ΔX
然后以更新后的X0计算ΔX,并按上式更新得到新的X0。不断迭代,直至|ΔX|<ζ,其中ζ为一近似于0的小量。
第六步:利用观测无人机视线测量信息,实时刷新无人机相对状态
考虑到目标无人机正常执行任务时,高度变化很小,因此可利用第四步估计的目标无人机高度信息,结合其实时测得的视线角信息,估计长僚机相对状态。此时不需要观测无人机进行机动。
利用观测无人机的测量信息和目标无人机高度估计值,可得两无人机的相对位置测量值:
结合观测无人机的位置测量值和/>可得:
记k时刻的目标无人滤波状态量为Xk=[xg,l,k,yg,l,k,vg,l,k,ψg,l,k,ωg,l,k]T,若|ωg,l,k|<ξ,ξ为近似于0的小量,则按下式计算k+1时刻的状态均值递推值和状态协方差递推值Pk+1|k:
/>
其中σa和σω对应为以白噪声近似速度大小变化率和航向角加速度特性的标准方差。
若|ωg,l,k|>ξ,则按下式计算k+1时刻的状态均值递推值和状态协方差递推值Pk+1|k结合观测无人机各时刻的位置可解算目标无人机的视线角,即有:
JP(:,1)=[1,0,0,0,0,0]T
JP(:,2)=[0,1,0,0,0,0]T
JP(3,5)=0,JP(4,5)=T,JP(5,5)=1
利用相对位置在x和y方向的侧量值,对状态均值和协方差进行更新:
Kk=Pk+1|kHT(HPk+1|kHT+Rr_xy)
Pk=(I5-KkH)Pk+1|k
其中
则k+1时刻的相对位置和速度估计值为:
第七步,观测无人机主动机动,矫正目标无人机高度估计值
为避免第五步执行过程中,目标无人机高度发生变化,引起相对状态估计偏差,应在一定间隔时间后返回第五步,重新矫正高度估计值,然后继续执行第六步。一定时间后,再执行第五步,保持循环。
第八步,任务结束
根据任务设置,若无人集群编队完成预定任务或者恢复正常通信后,则算法终止。
Claims (4)
1.一种仅基于视线测量信息的无人机编队相对状态估计方法,其特征在于,包括如下步骤:
第一步:根据视觉集群编队规模,确定视觉传感器型号;
第二步:搜索并锁定目标固定翼无人机,确保目标固定翼无人机在观测固定翼无人机见光吊舱图像下;
第三步:测量观测固定翼无人机飞行状态、视线角、吊舱框架角;
第四步:根据步骤三计算观测固定翼无人机与目标固定翼无人机视线角;
第五步:观测固定翼无人机主动机动,估计目标固定翼无人机初始状态;
固定翼无人机执行任务时,飞行速度大小、航向角变化率和高度在短时间内视为常值,基于此,令观测固定翼无人机保持其当前巡航水平飞行速度的情况下,以常爬升率机动;在机动期间不断解算其与目标固定翼无人机的视线,依次记为,/>
基于目标固定翼无人机飞行速度大小、航向角变化率和高度短时间内视为常值的假设,其运动特性由下式表示:
其中xg,l、yg,l和zg,l分别表示目标固定翼无人机在东北天惯性坐标系下的位置,vl和ψv,l分别表示无人机的速度大小和航向角,ωl为无人机的航向角变化率;为避免目标固定翼无人机位置求解时因航向角速度为零出现歧义,目标固定翼无人机各时刻的位置解析表达式需分情况讨论;
(1)工况1:目标固定翼无人机直线运动
若观测固定翼无人机各时刻视线与水平面的交点共线,则目标固定翼无人机做直线运动,记观测初始时刻为0,则目标固定翼无人机各时刻的位置解析表达式为:
结合观测固定翼无人机各时刻的位置解算目标固定翼无人机的视线角,即有:
其中:
式中xg,f、yl,g和zg,f为僚机状态;记各时刻观测固定翼无人机利用上式估算得到的视线信息为则目标固定翼无人机初始时刻的xg,l,0、yg,l,0和zg,l,0确定,/>确定,因此各时刻视线信息视为以X0为变量的函数:
X0=[xg,l,0,yg,l,0,zg,l,0,vl,0,ψv,l,0]T
考虑各时刻视线单位方向矢量的测量偏差,以观测固定翼无人机各时刻的视线单位方向矢量估计值与测量值的平方差和J最小为目标,确定X0;
即:
其中
W=diag(W1,W2,...,Wn)
式中Re为视线单位方向矢量的测量协方差,即视线高低角和方位角的测量偏差分别为σqε和σqβ,则
q=[qε,qβ]T
其中:
实际计算时,各时刻的权重W1、W2、…、Wn由当前时刻的视线角利用上式求得;
记为X0的最优估计值,则各时刻的视线单位矢量测量值用下式近似:
则J中表示为:
相应有
J=(ΔeLOS-HΔX)TW(ΔeLOS-HΔX)
其中:
H=diag(h1,h2,...,hn)
对于一X0,若J取值最小时,应有:
即有:
ΔX=(HTWH)-1HTWΔeLOS
其中:
与之对应:
其中:
利用上式确定ΔX后,首先按下式更新X0:
X0=X0+ΔX
然后以更新后的X0计算ΔX,并按上式更新得到新的X0,不断迭代,直至|ΔX|<ζ,其中ζ为一近似于0的小量;
(2)工况2:目标固定翼无人机曲线运动
若观测固定翼无人机各时刻视线与水平面的交点共线,则目标固定翼无人机做直线运动,记观测初始时刻为0,则目标固定翼无人机各时刻的位置解析表达式为:
其中xg,l,0、yg,l,0和zg,l,0分别表示初始时刻目标固定翼无人机在东北天惯性坐标系下的位置,vl,0和ψv,l,0分别表示初始时的速度大小和航向角,ωl,0为初始时刻的航向角变化率;
采用与工况1相同的方法确定J,并得:
ΔX=(HTWH)-1HTWΔeLOS
其中:
H=diag(h1,h2,...,hn)
X0=[xg,l,0 yg,l,0 zg,l,0 vl,0 ψl,0 ωl,0]T
利用上式确定ΔX后,首先按下式更新X0:
X0=X0+ΔX
然后以更新后的X0计算ΔX,并按上式更新得到新的X0;反复迭代,直至|ΔX|<ζ,其中ζ为一近似于0的小量;
第六步:利用观测固定翼无人机视线测量信息,滤波估计其与目标无人机相对状态;
利用第四步估计的目标固定翼无人机高度信息,结合其实时测得的视线角信息,估计长僚机相对状态;此时不需要观测固定翼无人机进行机动;
利用观测固定翼无人机的测量信息和目标固定翼无人机高度估计值,得两无人机的相对位置测量值:
结合观测固定翼无人机的位置测量值和/>得:
记k时刻的目标无人滤波状态量为Xk=[xg,l,k,yg,l,k,vg,l,k,ψg,l,k,ωg,l,k]T,若|ωg,l,k|<ξ,ξ为近似于0的小量,则按下式计算k+1时刻的状态均值递推值和状态协方差递推值Pk+1|k:
其中σa和σω对应为以白噪声近似速度大小变化率和航向角加速度特性的标准方差;
若|ωg,l,k|>ξ,则按下式计算k+1时刻的状态均值递推值和状态协方差递推值Pk+1|k结合观测固定翼无人机各时刻的位置解算目标固定翼无人机的视线角,即有:
JP(:,1)=[1,0,0,0,0,0]T
JP(:,2)=[0,1,0,0,0,0]T
JP(3,5)=0,JP(4,5)=T,JP(5,5)=1
利用相对位置在x和y方向的侧量值,对状态均值和协方差进行更新:
Kk=Pk+1|kHT(HPk+1|kHT+Rr_xy)
Pk=(I5-KkH)Pk+1|k
其中
则k+1时刻的相对位置和速度估计值为:
第七步,观测固定翼无人机主动机动,矫正目标固定翼无人机高度估计值
为避免第五步执行过程中,目标固定翼无人机高度发生变化,引起相对状态估计偏差,应在一定间隔时间后返回第五步,重新矫正高度估计值,然后继续执行第六步,待固定翼无人机速度大小估计值再次发生明显变化后,再执行第五步,保持循环;矫正时间间隔定义为上一次主动机动结束至检测到目标固定翼无人机速度大小估计值发生显著变化的时间区间,若固定翼无人机速度大小估计值始终稳定在某个值附近,则无需主动机动执行高度矫正;
第八步,任务结束
根据任务设置,若无人集群编队完成预定任务或者恢复正常通信后,则算法终止。
2.根据权利要求1所述的一种仅基于视线测量信息的无人机编队相对状态估计方法,其特征在于,所述步骤3固定翼无人机飞行状态采用固定翼无人机的惯性导航***或者GPS测得,固定翼无人机吊舱框架角由吊舱自带伺服***的传感器测得。
3.根据权利要求1所述的一种仅基于视线测量信息的无人机编队相对状态估计方法,其特征在于,所述步骤4视线角计算如下:
结合无人机姿态角和吊舱框架角解算固定翼无人机长僚机视线方向,具体如下:
其中eLOS,c=[1,0,0]T,Mg2b为东北天惯性坐标系到无人机体系的余弦转换矩阵,由俯仰角ψb和γb确定,/>为无人机机体坐标系到吊舱坐标系的余弦转换矩阵,由高低框架角εc和方位框架角βc确定;
同样eLOS,g由视线高低角qε和方位角qβ表示为:
eLOS,g=[cosqεcosqβ cosqεsinqβ sinqε]T
相应由eLOS,g解得:
。
4.根据权利要求3所述的一种仅基于视线测量信息的无人机编队相对状态估计方法,其特征在于,所述步骤4为避免视线角解算时出现歧义性,直接利用eLOS,g等价描述视线信息。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202111217659.9A CN113759982B (zh) | 2021-10-19 | 2021-10-19 | 一种仅基于视线测量信息的无人机编队相对状态估计方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202111217659.9A CN113759982B (zh) | 2021-10-19 | 2021-10-19 | 一种仅基于视线测量信息的无人机编队相对状态估计方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN113759982A CN113759982A (zh) | 2021-12-07 |
CN113759982B true CN113759982B (zh) | 2024-05-28 |
Family
ID=78784147
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202111217659.9A Active CN113759982B (zh) | 2021-10-19 | 2021-10-19 | 一种仅基于视线测量信息的无人机编队相对状态估计方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN113759982B (zh) |
Citations (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN110162094A (zh) * | 2019-06-13 | 2019-08-23 | 中国人民解放军军事科学院国防科技创新研究院 | 一种基于视觉测量信息的密集编队控制方法 |
CN110703798A (zh) * | 2019-10-23 | 2020-01-17 | 中国人民解放军军事科学院国防科技创新研究院 | 基于视觉的无人机编队飞行控制方法 |
CN110737283A (zh) * | 2019-11-04 | 2020-01-31 | 中国人民解放军军事科学院国防科技创新研究院 | 一种面向视觉集群的编队解耦控制方法 |
WO2020087846A1 (zh) * | 2018-10-31 | 2020-05-07 | 东南大学 | 基于迭代扩展卡尔曼滤波融合惯性与单目视觉的导航方法 |
CN111650963A (zh) * | 2020-06-03 | 2020-09-11 | 中国人民解放军军事科学院国防科技创新研究院 | 一种面向垂直起降固定翼无人机的视觉集群编队控制方法 |
CN112363528A (zh) * | 2020-10-15 | 2021-02-12 | 北京理工大学 | 基于机载视觉的无人机抗干扰集群编队控制方法 |
-
2021
- 2021-10-19 CN CN202111217659.9A patent/CN113759982B/zh active Active
Patent Citations (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
WO2020087846A1 (zh) * | 2018-10-31 | 2020-05-07 | 东南大学 | 基于迭代扩展卡尔曼滤波融合惯性与单目视觉的导航方法 |
CN110162094A (zh) * | 2019-06-13 | 2019-08-23 | 中国人民解放军军事科学院国防科技创新研究院 | 一种基于视觉测量信息的密集编队控制方法 |
CN110703798A (zh) * | 2019-10-23 | 2020-01-17 | 中国人民解放军军事科学院国防科技创新研究院 | 基于视觉的无人机编队飞行控制方法 |
CN110737283A (zh) * | 2019-11-04 | 2020-01-31 | 中国人民解放军军事科学院国防科技创新研究院 | 一种面向视觉集群的编队解耦控制方法 |
CN111650963A (zh) * | 2020-06-03 | 2020-09-11 | 中国人民解放军军事科学院国防科技创新研究院 | 一种面向垂直起降固定翼无人机的视觉集群编队控制方法 |
CN112363528A (zh) * | 2020-10-15 | 2021-02-12 | 北京理工大学 | 基于机载视觉的无人机抗干扰集群编队控制方法 |
Non-Patent Citations (2)
Title |
---|
现实真比例导引拦截任意机动目标捕获区域;白志会;黎克波;苏文山;陈磊;航空学报;第41卷(第008期);332-342 * |
考虑几何约束的无人机双机编队相对姿态确定方法;张旭;崔乃刚;王小刚;崔祜涛;秦武韬;战术导弹技术(第001期);17-21, 39 * |
Also Published As
Publication number | Publication date |
---|---|
CN113759982A (zh) | 2021-12-07 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
US10657832B2 (en) | Method and apparatus for target relative guidance | |
Redding et al. | Vision-based target localization from a fixed-wing miniature air vehicle | |
CN109911188B (zh) | 非卫星导航定位环境的桥梁检测无人机*** | |
CN101270993B (zh) | 一种远程高精度自主组合导航定位方法 | |
CN109709537B (zh) | 一种基于卫星编队的非合作目标位置速度跟踪方法 | |
CN111426320B (zh) | 一种基于图像匹配/惯导/里程计的车辆自主导航方法 | |
CN105929836B (zh) | 用于四旋翼飞行器的控制方法 | |
CN111238469B (zh) | 一种基于惯性/数据链的无人机编队相对导航方法 | |
CN112229405A (zh) | 一种基于图像跟踪与激光测距的无人机目标运动估计方法 | |
CN110793515A (zh) | 一种基于单天线gps和imu的大机动条件下无人机姿态估计方法 | |
CN109143303B (zh) | 飞行定位方法、装置及固定翼无人机 | |
CN111024091A (zh) | 视觉辅助微型无人机室内飞行三维姿态算法 | |
CN111504323A (zh) | 基于异源图像匹配与惯性导航融合的无人机自主定位方法 | |
Wang et al. | Monocular vision and IMU based navigation for a small unmanned helicopter | |
CN109186614B (zh) | 一种航天器间近距离自主相对导航方法 | |
CN108981691A (zh) | 一种天空偏振光组合导航在线滤波与平滑方法 | |
Miller et al. | Optical Flow as a navigation means for UAV | |
CN113670301A (zh) | 一种基于惯导***参数的机载sar运动补偿方法 | |
CN113759982B (zh) | 一种仅基于视线测量信息的无人机编队相对状态估计方法 | |
CN113129377A (zh) | 一种三维激光雷达快速鲁棒slam方法和装置 | |
Gonçalves et al. | Vision-based automatic approach and landing of fixed-wing aircraft using a dense visual tracking | |
Emran et al. | A cascaded approach for quadrotor's attitude estimation | |
KR101862065B1 (ko) | 비행체를 이용한 영상 기반 바람 추정 장치 및 방법 | |
Li et al. | Small UAV autonomous localization based on multiple sensors fusion | |
Liu et al. | Motion estimation using optical flow sensors and rate gyros |
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 |