CN113514052A - 一种多机协同高精度有源目标定位方法及系统 - Google Patents

Abstract

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

Description

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

技术领域

本发明属于无人机技术领域，特别涉及一种多机协同高精度有源目标定位方法及系统。

背景技术

近年来随着科学技术水平的提高，无人机已被广泛应用。在军用领域，无人机常用于侦察监视、目标打击、作战效能评估等；在民用领域，无人机常用于抢险救灾、地形勘测、短距离运输等。对地面目标定位是无人机的重要功能之一，其目的为求取目标在大地坐标系下的三维坐标。

无人机的目标定位根据其工作机制不同可以分为无源定位和有源定位。无源定位是通过摄像机采集目标图像，利用图像分析算法获取目标位置，主要方法有:1)基于图像匹配模式的目标定位，这种方法利用获取的多源图像，以预先装入的目标图像为基准，将经过校正的无人机电视图像与基准图片进行匹配，从而实现目标定位，该方法有较高的定位精度，但基准图片的获取有一定的限制性，且图片匹配实时性较差，实用性不高；2) 基于成像模型的目标定位，该方法需要获取飞行器的航高，以及摄像机的内、外方位元素，根据共线条件方程计算地面目标位置。在实际使用中，该方法需要假定待测目标区域为平坦地面，目标定位精度较低。有源定位以基于姿态测量/激光测距定位模型为主，这种定位模型下，飞行器需要装备激光测距机，要求飞行器具有较大的载荷，但是该方法误差来源较多，包括：吊舱方位角误差和高低角误差、激光测距误差、无人机姿态误差以及无人机位置误差等，以上所有误差均会影响该方法定位精度。

发明内容

本发明的目的在于克服上述现有技术的缺点，提供一种多机协同高精度有源目标定位方法及系统，该方法不仅能够实时输出目标高精度位置信息还能输出目标的速度信息，解决现有技术中使用局限性较多、定位精度不高以及实时性较差的问题。

为了实现上述目的，本发明采用以下技术方案：

一种多机协同高精度有源目标定位方法，包括以下步骤：

采集合作目标点的精确经纬高，规划标定航线；

多架无人机根据航线等间隔飞行，每架无人机上的吊舱对目标进行跟踪采集数据，每架无人机根据采集到的数据对目标位置与速度进行估计；

根据每架无人机估计的目标的位置、速度估计值与每架无人机定位算法滤波器的误差协方差矩阵，计算高精度的目标位置、速度值。

进一步的，标定航线要求无人机保持匀速运动、航向和航高稳定、俯仰角和滚转角为零度；标定航线的规划使得无人机能充分利用前后左右四个方向对合作目标进行定位。

进一步的，选取目标在大地直角坐标系中的位置、速度以及吊舱方位角误差和高低角误差为状态，即：X＝[p_x p_y p_z v_x v_y vz δα δβ]^T，目标在大地直角坐标系中的位置与吊舱方位角误差和高低角误差为量测，即：Z＝[p_x p_y p_z δα δβ]^T；假设目标运动状态为匀速直线运动，状态方程为

吊舱直接输出的信息为无人机与目标的距离r以及吊舱的方位角α_p和高低角β_p，对量测进行转换。

进一步的，目标位置量测的构造：

假设目标在摄像机坐标系中的坐标值

其中r为激光测距值；设大地直角坐标系中目标坐标为

无人机在大地直角坐标系中的坐标为

则根据坐标转换关系可得：

其中：

——表示大地直角坐标系e到地理坐标系n的转换矩阵；

——表示地理坐标系n到无人机机体坐标系b的转换矩阵；

——表示无人机机体坐标系b到摄像机坐标系c的转换矩阵；

将式(2)进行变换可得：

式(4)中各个矩阵定义如下：

其中：

L——无人机纬度；λ——无人机经度；

——无人机航向角；θ——无人机俯仰角；γ——无人机滚转角；α_p——吊舱方位角(α∈[0，2π))；β_p——吊舱高低角

进一步的，吊舱方位角误差δα和高低角误差δβ的构造：

假设目标在摄像机坐标系中的坐标值为T_c，目标在导航坐标系中的坐标值为T_n，则两个向量存在如下关系：

假设T_c＝[1 0 0]，则T_n＝[cos(α_v)·cos(β_v) sin(α_v)·cos(β_v) sin(β_v)]，将式(5)展开可得：

由于左＝右，令左＝T_b，可得吊舱方位角真值和高低角真值的计算公式：

β_true＝a sin(T_b[3]) (24)

其中：

——无人机航向角；θ——无人机俯仰角；γ——无人机滚转角；α_true——吊舱真实方位角(α∈[0，2π))；β_true——吊舱真实高低角

α_v——视轴方位角(α∈[0，2π))；β_v——视轴高低角

视轴方位角和高低角计算方法如下：

其中：

——静止合作目标在大地直角坐标系X方向坐标值；

——静止合作目标在大地直角坐标系Y方向坐标值；

——静止合作目标在大地直角坐标系Z方向坐标值

——无人机在大地直角坐标系X方向坐标值；

——无人机在大地直角坐标系Y方向坐标值；

——无人机在大地直角坐标系Z方向坐标值；

计算出吊舱真实的方位角和高低角后与吊舱输出的方位角和高低角做差即可得到吊舱方位角误差和高低角误差，公式如下：

δα＝α_true-α_p

δβ＝β_true-β_p (26)

以上是所有量测的构造过程，下面介绍量测方程：

进一步的，吊舱对目标进行跟踪采集数据包括：吊舱方位角、高低角与激光测距值；

选取目标大地直角坐标系中的位置、速度为状态，即：X＝[p_x p_y p_z v_x v_y v_z]^T，目标在大地直角坐标系中的位置为量测，即：Z＝[p_x p_y p_z]^T；假设目标运动状态为匀速直线运动，系统状态方程为

对无人机与目标的距离r以及吊舱的方位角α_p和高低角β_p1量测转换。

进一步的，转换过程：

假设摄像机坐标系中的目标坐标值

其中r为激光测距值；设大地直角坐标系中目标坐标为

无人机在大地直角坐标系中的坐标为

则根据坐标转换关系可得：

其中：

—一表示大地直角坐标系e到地理坐标系n的转换矩阵；

——表示地理坐标系n到无人机机体坐标系b的转换矩阵；

——表示无人机机体坐标系b到摄像机坐标系c的转换矩阵；

将式(2)进行变换可得：

式(4)中各个矩阵定义如下：

其中：L——无人机纬度；λ——无人机经度；

——无人机航向角；θ——无人机俯仰角；γ——无人机滚转角；α_p——补偿后的吊舱方位角(α∈[0，2π))；β_p——补偿后的吊舱高低角

量测方程：

以上是高精度目标定位测速方法线性模型，将上述模型应用于卡尔曼滤波，对该模型的卡尔曼滤波具体实施过程如下：

状态预测

状态误差协方差矩阵预测

滤波器增益

状态更新

状态误差协方差矩阵更新

其中：

P_k-1——k-1时刻卡尔曼滤波状态误差协方差矩阵；

——k时刻卡尔曼滤波状态误差协方差矩阵预测值；Q_k-1——k-1时刻系统噪声矩阵；R_k——k时刻量测噪声矩阵；K_k——k时刻滤波器增益矩阵；P_k——k时刻卡尔曼滤波状态误差协方差矩阵。

8.根据权利要求1所述的一种多机协同高精度有源目标定位方法，其特征在于，计算高精度的目标位置：

假设第i架无人机的定位结果为Xⁱ＝[p_x p_y p_z v_x v_y v_z]^T，卡尔曼滤波的误差协方差阵主对角元素为Pⁱ＝[P_11i P_22i P_33i P_44i P_55i P_66i]^T，各个无人机定位结果的置信度计算方法如下：

其中：w_i——第i个无人机定位结果置信度系数；i——第i个无人机；n——参与定位的无人机数量；Pⁱ——第i个无人机定位算法滤波器误差协方差矩阵主对角线元素；

多机协同有源目标定位结果计算方法如下：

其中：X——多机协同有源目标定位结果；w_i——第i个无人机定位结果置信度系数；Xⁱ——第i个无人机定位结果。

进一步的，一种多机协同高精度有源目标定位系统，包括：

采集模块用于采集合作目标点的精确经纬高，规划标定航线；

目标位置与速度估计模块用于多架无人机根据航线等间隔飞行，每架无人机上的吊舱对目标进行跟踪采集数据，每架无人机根据采集到的数据对目标位置与速度进行估计；

目标位置、速度值计算模块用于根据每架无人机估计的目标的位置、速度估计值与每架无人机定位算法滤波器的误差协方差矩阵，计算高精度的目标位置、速度值。

与现有技术相比，本发明有以下技术效果：

1)本发明提出的多机协同高精度有源目标定位方法对地形无要求，使用没有局限性；

2)本发明提出的多机协同高精度有源目标定位方法可实时输出目标的高精度位置与速度信息；

3)本发明提出的多机协同高精度有源目标定位方法不仅适用于静态目标，还适用于动态目标；

4)本发明提出的多机协同高精度有源目标定位方法相比较于单无人机有源目标定位精度更高。

附图说明

图1是标定航线示意图；

图2是多机协同高精度有源目标定位示意图；

具体实施方式

本发明提出的多机协同高精度目标定位方法的具体实施方式如下：

第一步：采集合作目标点的精确经纬高，规划标定航线，标定航线要求无人机保持匀速运动、航向和航高稳定、俯仰角和滚转角为零度。

标定航线的规划要考虑无人机与合作目标的相对位置，使得无人机能充分利用前后左右四个方向对合作目标进行定位，标定航线应采用如图1所示的航线或与之相似的航线。在航路1、3，目标在无人机的机头或机尾。在航路2，目标在无人机的左翼或右翼。

第二步：多架无人机根据图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)，因此量测需要进行转换，下面首先介绍位置量测的转换过程：

假设目标在摄像机坐标系中的坐标值

其中r为激光测距值。设大地直角坐标系中目标坐标为

无人机在大地直角坐标系中的坐标为

则根据坐标转换关系可得：

其中：

——表示大地直角坐标系e到地理坐标系n的转换矩阵；

——表示地理坐标系n到无人机机体坐标系b的转换矩阵；

——表示无人机机体坐标系b到摄像机坐标系c的转换矩阵。

将式(2)进行变换可得：

式(4)中各个矩阵定义如下：

其中：

L——无人机纬度

λ——无人机经度

——无人机航向角

θ——无人机俯仰角

γ——无人机滚转角

α_p——吊舱方位角(α∈[0，2π))

β_p——吊舱高低角

以上是对目标位置量测的构造，下面介绍对吊舱方位角误差(δα)和高低角误差(δβ) 的构造。

假设目标在摄像机坐标系中的坐标值为T_c，目标在导航坐标系中的坐标值为T_n，则两个向量存在如下关系：

假设T_c＝[1 0 0]，则T_n＝[cos(α_v)·cos(β_v) sin(α_v)·cos(β_v) sin(β_v)]，将式(5) 展开可得：

由于左＝右，令左＝T_b，可得吊舱方位角真值和高低角真值的计算公式：

其中：

——无人机航向角

θ——无人机俯仰角

γ——无人机滚转角

α_true——吊舱真实方位角(α∈[0，2π))

β_true——吊舱真实高低角

α_v——视轴方位角(α∈[0，2π))

β_v——视轴高低角

视轴方位角和高低角计算方法如下：

其中：

——静止合作目标在大地直角坐标系X方向坐标值

——静止合作目标在大地直角坐标系Y方向坐标值

——静止合作目标在大地直角坐标系Z方向坐标值

——无人机在大地直角坐标系X方向坐标值

——无人机在大地直角坐标系Y方向坐标值

——无人机在大地直角坐标系Z方向坐标值

计算出吊舱真实的方位角和高低角后与吊舱输出的方位角和高低角做差即可得到吊舱方位角误差和高低角误差，公式如下：

以上是所有量测的构造过程，下面介绍量测方程：

以上是标校方法的线性模型，将上述模型应用于卡尔曼滤波器或者UKF滤波器均可。

第二步：

选取目标大地直角坐标系中的位置、速度为状态，即： 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)，因此量测需要进行转换，下面介绍量测转换过程：

假设摄像机坐标系中的目标坐标值

其中r为激光测距值。设大地直角坐标系中目标坐标为

无人机在大地直角坐标系中的坐标为

则根据坐标转换关系可得：

其中：

——表示大地直角坐标系e到地理坐标系n的转换矩阵；

——表示地理坐标系n到无人机机体坐标系b的转换矩阵；

——表示无人机机体坐标系b到摄像机坐标系c的转换矩阵。

将式(2)进行变换可得：

式(4)中各个矩阵定义如下：

其中：

L——无人机纬度

λ——无人机经度

——无人机航向角

θ——无人机俯仰角

γ——无人机滚转角

α_p——补偿后的吊舱方位角(α∈[0，2π))

β_p——补偿后的吊舱高低角

以上是量测构造过程，下面介绍量测方程：

以上是高精度目标定位测速方法线性模型，将上述模型应用于卡尔曼滤波或者UKF 滤波均可。

下面以卡尔曼滤波为例，对该模型的卡尔曼滤波具体实施过程进行介绍，卡尔曼滤波实施过程如下：

1)状态预测

2)状态误差协方差矩阵预测

3)滤波器增益

4)状态更新

5)状态误差协方差矩阵更新

其中：

P_k-1——k-1时刻卡尔曼滤波状态误差协方差矩阵；

——k时刻卡尔曼滤波状态误差协方差矩阵预测值；

Q_k-1——k-1时刻系统噪声矩阵；

R_k——k时刻量测噪声矩阵；

K_k——k时刻滤波器增益矩阵；

P_k——k时刻卡尔曼滤波状态误差协方差矩阵；

第三步，假设第i架无人机的定位结果为Xⁱ＝[p_xp_y p_z v_x v_y v_z]^T，卡尔曼滤波的误差协方差阵主对角元素为Pⁱ＝[P_11i P_22i P_33i P_44i P_55i P_66i]^T，各个无人机定位结果的置信度计算方法如下：

其中：

w_i——第i个无人机定位结果置信度系数；

i——第i个无人机；

n——参与定位的无人机数量；

Pⁱ——第i个无人机定位算法滤波器误差协方差矩阵主对角线元素；

多机协同有源目标定位结果计算方法如下：

其中：

X——多机协同有源目标定位结果；

w_i——第i个无人机定位结果置信度系数；

Xⁱ——第i个无人机定位结果。

Claims (9)

1.一种多机协同高精度有源目标定位方法，其特征在于，包括以下步骤：

采集合作目标点的精确经纬高，规划标定航线；

多架无人机根据航线等间隔飞行，每架无人机上的吊舱对目标进行跟踪采集数据，每架无人机根据采集到的数据对目标位置与速度进行估计；

根据每架无人机估计的目标的位置、速度估计值与每架无人机定位算法滤波器的误差协方差矩阵，计算高精度的目标位置、速度值。

2.根据权利要求1所述的一种多机协同高精度有源目标定位方法，其特征在于，标定航线要求无人机保持匀速运动、航向和航高稳定、俯仰角和滚转角为零度；标定航线的规划使得无人机能充分利用前后左右四个方向对合作目标进行定位。

3.根据权利要求2所述的一种多机协同高精度有源目标定位方法，其特征在于，选取目标在大地直角坐标系中的位置、速度以及吊舱方位角误差和高低角误差为状态，即：X＝[p_xp_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)进行变换可得：

式(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方向坐标值；

计算出吊舱真实的方位角和高低角后与吊舱输出的方位角和高低角做差即可得到吊舱方位角误差和高低角误差，公式如下：

δα＝α_true-α_p

δβ＝β_true-β_p (9)

以上是所有量测的构造过程，下面介绍量测方程：

6.根据权利要求1所述的一种多机协同高精度有源目标定位方法，其特征在于，吊舱对目标进行跟踪采集数据包括：吊舱方位角、高低角与激光测距值；

选取目标大地直角坐标系中的位置、速度为状态，即：X＝[p_x p_y p_z v_x v_y v_z]^T，目标在大地直角坐标系中的位置为量测，即：Z＝[p_x p_y p_z]^T；假设目标运动状态为匀速直线运动，系统状态方程为

对无人机与目标的距离r以及吊舱的方位角α_p和高低角β_p1量测转换。

7.根据权利要求6所述的一种多机协同高精度有源目标定位方法，其特征在于，转换过程：

假设摄像机坐标系中的目标坐标值

其中r为激光测距值；设大地直角坐标系中目标坐标为

无人机在大地直角坐标系中的坐标为

则根据坐标转换关系可得：

其中：

——表示大地直角坐标系e到地理坐标系n的转换矩阵；

——表示地理坐标系n到无人机机体坐标系b的转换矩阵；

——表示无人机机体坐标系b到摄像机坐标系c的转换矩阵；

将式(2)进行变换可得：

式(4)中各个矩阵定义如下：

其中：L——无人机纬度；λ——无人机经度；

——无人机航向角；θ——无人机俯仰角；γ——无人机滚转角；α_p——补偿后的吊舱方位角(α∈[0,2π))；β_p——补偿后的吊舱高低角

量测方程：

以上是高精度目标定位测速方法线性模型，将上述模型应用于卡尔曼滤波，对该模型的卡尔曼滤波具体实施过程如下：

状态预测

状态误差协方差矩阵预测

滤波器增益

状态更新

状态误差协方差矩阵更新

其中：

P_k-1——k-1时刻卡尔曼滤波状态误差协方差矩阵；

——k时刻卡尔曼滤波状态误差协方差矩阵预测值；Q_k-1——k-1时刻系统噪声矩阵；R_k——k时刻量测噪声矩阵；K_k——k时刻滤波器增益矩阵；P_k——k时刻卡尔曼滤波状态误差协方差矩阵。

8.根据权利要求1所述的一种多机协同高精度有源目标定位方法，其特征在于，计算高精度的目标位置：

假设第i架无人机的定位结果为Xⁱ＝[p_x p_y p_z v_x v_y v_z]^T，卡尔曼滤波的误差协方差阵主对角元素为Pⁱ＝[P_11i P_22i P_33i P_44i P_55i P_66i]^T，各个无人机定位结果的置信度计算方法如下：

其中：w_i——第i个无人机定位结果置信度系数；i——第i个无人机；n——参与定位的无人机数量；Pⁱ——第i个无人机定位算法滤波器误差协方差矩阵主对角线元素；

多机协同有源目标定位结果计算方法如下：

其中：X——多机协同有源目标定位结果；w_i——第i个无人机定位结果置信度系数；Xⁱ——第i个无人机定位结果。

9.一种多机协同高精度有源目标定位系统，其特征在于，包括：

采集模块用于采集合作目标点的精确经纬高，规划标定航线；

目标位置与速度估计模块用于多架无人机根据航线等间隔飞行，每架无人机上的吊舱对目标进行跟踪采集数据，每架无人机根据采集到的数据对目标位置与速度进行估计；

目标位置、速度值计算模块用于根据每架无人机估计的目标的位置、速度估计值与每架无人机定位算法滤波器的误差协方差矩阵，计算高精度的目标位置、速度值。

Images