一种多机协同高精度有源目标定位方法及系统技术方案

一种多机协同高精度有源目标定位方法及系统，包括以下步骤：采集合作目标点的精确经纬高，规划标定航线；多架无人机根据航线等间隔飞行，每架无人机上的吊舱对目标进行跟踪采集数据，每架无人机根据采集到的数据对目标位置与速度进行估计；根据每架无人机估计的目标的位置、速度估计值与每架无人机定位算法滤波器的误差协方差矩阵，计算高精度的目标位置、速度值。本发明专利技术提出的多机协同高精度有源目标定位方法对地形无要求，使用没有局限性。使用没有局限性。使用没有局限性。

【技术实现步骤摘要】
一种多机协同高精度有源目标定位方法及系统


[0001]本专利技术属于无人机
，特别涉及一种多机协同高精度有源目标定位方法及系统。

技术介绍

[0002]近年来随着科学技术水平的提高，无人机已被广泛应用。在军用领域，无人机常用于侦察监视、目标打击、作战效能评估等；在民用领域，无人机常用于抢险救灾、地形勘测、短距离运输等。对地面目标定位是无人机的重要功能之一，其目的为求取目标在大地坐标系下的三维坐标。
[0003]无人机的目标定位根据其工作机制不同可以分为无源定位和有源定位。无源定位是通过摄像机采集目标图像，利用图像分析算法获取目标位置，主要方法有:1)基于图像匹配模式的目标定位，这种方法利用获取的多源图像，以预先装入的目标图像为基准，将经过校正的无人机电视图像与基准图片进行匹配，从而实现目标定位，该方法有较高的定位精度，但基准图片的获取有一定的限制性，且图片匹配实时性较差，实用性不高；2) 基于成像模型的目标定位，该方法需要获取飞行器的航高，以及摄像机的内、外方位元素，根据共线条件方程计算地面目标位置。在实际使用中，该方法需要假定待测目标区域为平坦地面，目标定位精度较低。有源定位以基于姿态测量/激光测距定位模型为主，这种定位模型下，飞行器需要装备激光测距机，要求飞行器具有较大的载荷，但是该方法误差来源较多，包括：吊舱方位角误差和高低角误差、激光测距误差、无人机姿态误差以及无人机位置误差等，以上所有误差均会影响该方法定位精度。

技术实现思路

[0004]本专利技术的目的在于克服上述现有技术的缺点，提供一种多机协同高精度有源目标定位方法及系统，该方法不仅能够实时输出目标高精度位置信息还能输出目标的速度信息，解决现有技术中使用局限性较多、定位精度不高以及实时性较差的问题。
[0005]为了实现上述目的，本专利技术采用以下技术方案：
[0006]一种多机协同高精度有源目标定位方法，包括以下步骤：
[0007]采集合作目标点的精确经纬高，规划标定航线；
[0008]多架无人机根据航线等间隔飞行，每架无人机上的吊舱对目标进行跟踪采集数据，每架无人机根据采集到的数据对目标位置与速度进行估计；
[0009]根据每架无人机估计的目标的位置、速度估计值与每架无人机定位算法滤波器的误差协方差矩阵，计算高精度的目标位置、速度值。
[0010]进一步的，标定航线要求无人机保持匀速运动、航向和航高稳定、俯仰角和滚转角为零度；标定航线的规划使得无人机能充分利用前后左右四个方向对合作目标进行定位。
[0011]进一步的，选取目标在大地直角坐标系中的位置、速度以及吊舱方位角误差和高低角误差为状态，即：X＝[p
x p
y p
z v
x v
y vz δα δβ]T
，目标在大地直角坐标系中的位置与
吊舱方位角误差和高低角误差为量测，即：Z＝[p
x p
y p
z δα δβ]T
；假设目标运动状态为匀速直线运动，状态方程为
[0012][0013]吊舱直接输出的信息为无人机与目标的距离r以及吊舱的方位角α
p
和高低角β
p
，对量测进行转换。
[0014]进一步的，目标位置量测的构造：
[0015]假设目标在摄像机坐标系中的坐标值其中r为激光测距值；设大地直角坐标系中目标坐标为无人机在大地直角坐标系中的坐标为则根据坐标转换关系可得：
[0016][0017]其中：
[0018]——表示大地直角坐标系e到地理坐标系n的转换矩阵；——表示地理坐标系n到无人机机体坐标系b的转换矩阵；——表示无人机机体坐标系b到摄像机坐标系c的转换矩阵；
[0019]将式(2)进行变换可得：
[0020][0021][0022][0023][0024]式(4)中各个矩阵定义如下：
[0025][0026][0027][0028][0029][0030][0031]其中：
[0032]L——无人机纬度；λ——无人机经度；——无人机航向角；θ——无人机俯仰角；γ——无人机滚转角；α
p
——吊舱方位角(α∈[0，2π))；β
p
——吊舱高低角
[0033]进一步的，吊舱方位角误差δα和高低角误差δβ的构造：
[0034]假设目标在摄像机坐标系中的坐标值为T
c
，目标在导航坐标系中的坐标值为T
n
，则两个向量存在如下关系：
[0035][0036]假设T
c
＝[1 0 0]，则T
n
＝[cos(α
v
)
·
cos(β
v
) sin(α
v
)
·
cos(β
v
) sin(β
v
)]，将式(5)展开可得：
[0037][0038]由于左＝右，令左＝T
b
，可得吊舱方位角真值和高低角真值的计算公式：
[0039][0040]β
true
＝a sin(T
b
[3])
ꢀꢀ
(24)
[0041]其中：——无人机航向角；θ——无人机俯仰角；γ——无人机滚转角；α
true
——吊舱真实方位角(α∈[0，2π))；β
true
——吊舱真实高低角α
v
——视轴方位角(α∈[0，2π))；β
v
——视轴高低角
[0042]视轴方位角和高低角计算方法如下：
[0043][0044]其中：
[0045]——静止合作目标在大地直角坐标系X方向坐标值；——静止合作目标在大地直角坐标系Y方向坐标值；——静止合作目标在大地直角坐标系Z方向坐标值
[0046]——无人机在大地直角坐标系X方向坐标值；——无人机在大地直角坐标系Y方向坐标值；——无人机在大地直角坐标系Z方向坐标值；
[0047]计算出吊舱真实的方位角和高低角后与吊舱输出的方位角和高低角做差即可得到吊舱方位角误差和高低角误差，公式如下：
[0048]δα＝α
true
‑
α
p
[0049]δβ＝β
true
‑
β
p
ꢀꢀ
(26)
[0050]以上是所有量测的构造过程，下面介绍量测方程：
[0051][0052]进一步的，吊舱对目标进行跟踪采集数据包括：吊舱方位角、高低角与激光测距值；
[0053]选取目标大地直角坐标系中的位置、速度为状态，即：X＝[p
x p
y p
z v
x v
y v
z
]T
，目标在大地直角坐标系中的位置为量测，即：Z＝[p
x p...

【技术保护点】

【技术特征摘要】
1.一种多机协同高精度有源目标定位方法，其特征在于，包括以下步骤：采集合作目标点的精确经纬高，规划标定航线；多架无人机根据航线等间隔飞行，每架无人机上的吊舱对目标进行跟踪采集数据，每架无人机根据采集到的数据对目标位置与速度进行估计；根据每架无人机估计的目标的位置、速度估计值与每架无人机定位算法滤波器的误差协方差矩阵，计算高精度的目标位置、速度值。2.根据权利要求1所述的一种多机协同高精度有源目标定位方法，其特征在于，标定航线要求无人机保持匀速运动、航向和航高稳定、俯仰角和滚转角为零度；标定航线的规划使得无人机能充分利用前后左右四个方向对合作目标进行定位。3.根据权利要求2所述的一种多机协同高精度有源目标定位方法，其特征在于，选取目标在大地直角坐标系中的位置、速度以及吊舱方位角误差和高低角误差为状态，即：X＝[p
x p
y p
z v
x v
y v
z δα δβ]
T
，目标在大地直角坐标系中的位置与吊舱方位角误差和高低角误差为量测，即：Z＝[p
x p
y p
z δα δβ]
T
；假设目标运动状态为匀速直线运动，状态方程为；假设目标运动状态为匀速直线运动，状态方程为吊舱直接输出的信息为无人机与目标的距离r以及吊舱的方位角α
p
和高低角β
p
，对量测进行转换。4.根据权利要求3所述的一种多机协同高精度有源目标定位方法，其特征在于，目标位置量测的构造：假设目标在摄像机坐标系中的坐标值其中r为激光测距值；设大地直角坐标系中目标坐标为无人机在大地直角坐标系中的坐标为则根据坐标转换关系可得：其中：——表示大地直角坐标系e到地理坐标系n的转换矩阵；——表示地理坐标系n到无人机机体坐标系b的转换矩阵；——表示无人机机体坐标系b到摄像机坐标系c的转换
矩阵；将式(2)进行变换可得：将式(2)进行变换可得：将式(2)进行变换可得：将式(2)进行变换可得：式(4)中各个矩阵定义如下：式(4)中各个矩阵定义如下：式(4)中各个矩阵定义如下：式(4)中各个矩阵定义如下：式(4)中各个矩阵定义如下：式(4)中各个矩阵定义如下：其中：L——无人机纬度；λ——无人机经度；——无人机航向角；θ——无人机俯仰角；γ——无人机滚转角；α
p
——吊舱方位角(α∈[0,2π))；β
p
——吊舱高低角5.根据权利要求3所述的一种多机协同高精度有源目标定位方法，其特征在于，吊舱方位角误差δα和高低角误差δβ的构造：假设目标在摄像机坐标系中的坐标值为T
c
，目标在导航坐标系中的坐标值为T
n
，则两个向量存在如下关系：
假设T
c
＝[1 0 0]，则T
n
＝[cos(α
v
)
·
cos(β
v
) sin(α
v
)
·
cos(β
v
) sin(β
v
)]，将式(5)展开可得：右＝[cos(α
true
)
·
cos(β
true
) sin(α
true
)
·
cos(β
true
) sin(β
true
)](6)由于左＝右，令左＝T
b
，可得吊舱方位角真值和高低角真值的计算公式：β
true
＝asin(T
b
[3]) (7)其中：——无人机航向角；θ——无人机俯仰角；γ——无人机滚转角；α
true
——吊舱真实方位角(α∈[0,2π))；β
true
——吊舱真实高低角α
v
——视轴方位角(α∈[0,2π))；β
v
——视轴高低角视轴方位角和高低角计算方法如下：其中：——静止合作目标在大地直角坐标系X方向坐标值；——静止合作目标在大地直角坐标系Y方向坐标值；——静止合作目标在大地直角坐标系Z方向坐标值——无人机在大地直角坐标系X方向坐标值；——无人机在大地直角坐标系Y方向坐标值；——无人机在大地直角坐标系Z方向坐标值；计算出吊舱真实的方位角和高低角后与吊舱输出的方位角和高低角...

【专利技术属性】
技术研发人员：陈朋印，李定涌，闫永驰，陈柯柯，马一鸣，曾幼涵，王圣淙，
申请(专利权)人：西安因诺航空科技有限公司，
类型：发明
国别省市：