CN115790603A - 一种用于信息拒止环境下的无人机动态目标估计方法 - Google Patents

Abstract

本发明公开了一种用于信息拒止环境下的无人机动态目标估计方法。该方法包括：获取三维角度信息，所述三维角度信息包括无人机到第一锚点的到达角、无人机到第二锚点的到达角以及无人机到目标的到达角；对于所述三维角度信息，利于拓展卡尔曼滤波器进行滤波，获得估计的无人机状态信息以及对应的状态协方差信息，所述无人机状态信息用于表征无人机的位置、速度以及目标的位置和速度；利用所述无人机状态信息和所述状态协方差信息构建损失函数，以优化无人机的飞行路径，实现对目标进行观测。本发明采用两个已知绝对位置的锚点信息即可实现无人机和目标的位置跟踪。

Description

一种用于信息拒止环境下的无人机动态目标估计方法

技术领域

本发明涉及无人机技术领域，更具体地，涉及一种用于信息拒止环境下的无人机动态目标估计方法。

背景技术

目标跟踪技术在各种应用中被广泛应用，如空中摄影、公共安全和人道主义搜索和救援。从不同传感器获得的噪声测量值中估计出目标的位置和速度是一个研究较为广泛的方向。由于无人机(UAV)出色的可操作性，在配备了到达角(AOA)传感器后，可被用于目标跟踪。通过解算三角几何问题配合滤波估计器，自身和目标的位置及速度可以从嘈杂的AOA测量数据中估计出来。此外，研究人员还提出了不同的估计算法，以便用非线性AOA测量值估计目标状态。例如，最大似然估计器(MLE)和伪线性估计器(PLE)作为经典的批处理滤波器，被用于从带有高斯噪声的AOA目标测量中估计目标位置及速度。基于卡尔曼的方法，被视为递归滤波器，则被更普遍地应用于目标跟踪领域。对于AOA目标跟踪，扩展卡尔曼滤波(EKF)、无迹卡尔曼滤波(UKF)、伪线性卡尔曼滤波(PLKF)和容积卡尔曼滤波器(CKF)均有不同程度的应用。

现有研究表明，通过优化无人机飞行路径，使从测量中收集到的信息更加有效，可以显著提高目标跟踪性能。为提高目标估计精度，应设计一个成本函数用于无人机路径优化。常见的成本函数包括估计协方差矩阵和费雪信息矩阵(FIM)等。可采用各种算法优化成本函数，如基于梯度、穷举搜索和基于学习的方法。

在现有技术中，无人机的位置被假定为使用外部信息并且可以准确获取，例如全球定位***(GPS)。然而，无人机可能在外部信号缺失的环境中工作(或称为信息拒止环境)，例如，室内空间和干扰区域。在这些区域中，无人机的外部信号缺失，无法获得其位置。目前，在未知自身位置的情况下进行目标定位引起了很多人的兴趣。例如，在同步定位和测绘(SLAM)应用中，增加周围锚点的额外信息(如附近建筑物的位置)来获得绝对目标位置。在无线通信中，通过多锚点进行目标定位(如基站定位)已经较为成熟，但由于其基本通过到达时间差(TDOA)的方式进行，所需的锚点数必须大于等于3或者引入其它先验知识，才能唯一确定目标位置。

经分析，在现有技术中，无论是单纯的基于到达时间差(TDOA)还是到达角(AOA)的目标估计方案，都只能获取到目标相对于无人机的距离及角度，不能获得其绝对位置。

发明内容

本发明的目的是克服上述现有技术的缺陷，提供一种用于信息拒止环境下的无人机动态目标估计方法，该方法包括：

获取三维角度信息，所述三维角度信息包括无人机到第一锚点的到达角、无人机到第二锚点的到达角以及无人机到目标的到达角；

对于所述三维角度信息，利于拓展卡尔曼滤波器进行滤波，获得估计的无人机状态信息以及对应的状态协方差信息，所述无人机状态信息用于表征无人机的位置、速度以及目标的位置和速度；

利用所述无人机状态信息和所述状态协方差信息构建损失函数，以优化无人机后续时刻的飞行路径，实现对目标的观测。

与现有技术相比，本发明的优点在于，提供一种基于方位角(角度，可由PTZ相机获取)与锚点的无人机自主目标跟踪方法，通过引入已知绝对位置的锚点，可为无人机在外部信号(GPS、RTK等)丢失环境下提供自身及目标的绝对位置信息，并对其进行跟踪，为无人机自身与目标的定位跟踪提供了更健壮的解决方案。本发明可应用于多旋翼无人机、固定翼无人机等多种无人飞行器的定位***，既可作为自身定位的冗余手段，又可根据方位角提供目标的绝对地理坐标，在提升定位速度的同时，也提高了定位精度。

通过以下参照附图对本发明的示例性实施例的详细描述，本发明的其它特征及其优点将会变得清楚。

附图说明

被结合在说明书中并构成说明书的一部分的附图示出了本发明的实施例，并且连同其说明一起用于解释本发明的原理。

图1是根据本发明一个实施例的目标跟踪的几何示意图；

图2是根据本发明一个实施例的用于信息拒止环境下的无人机动态目标估计方法的流程图；

图3是根据本发明一个实施例的拓展卡尔曼滤波的流程图；

图4是根据本发明一个实施例的构建损失函数的示意图；

图5是根据本发明一个实施例的路径优化方法的过程示意图。

具体实施方式

现在将参照附图来详细描述本发明的各种示例性实施例。应注意到：除非另外具体说明，否则在这些实施例中阐述的部件和步骤的相对布置、数字表达式和数值不限制本发明的范围。

以下对至少一个示例性实施例的描述实际上仅仅是说明性的，决不作为对本发明及其应用或使用的任何限制。

对于相关领域普通技术人员已知的技术、方法和设备可能不作详细讨论，但在适当情况下，所述技术、方法和设备应当被视为说明书的一部分。

在这里示出和讨论的所有例子中，任何具体值应被解释为仅仅是示例性的，而不是作为限制。因此，示例性实施例的其它例子可以具有不同的值。

应注意到：相似的标号和字母在下面的附图中表示类似项，因此，一旦某一项在一个附图中被定义，则在随后的附图中不需要对其进行进一步讨论。

本发明提供一套外部信号缺失环境下的无人机自主目标定位方法，主要包括目标滤波估计、无人机路径优化两部分。该方法引入两个已知绝对位置的静止标志物，以到达角(AOA)传感器采集的三维角度信息(包括无人机与锚点1的到达角θ_ub1，无人机与锚点2的到达角θ_ub2，无人机与移动目标的到达角θ_up)为输入，在连续的观测一段时间后，可以将移动目标和无人机自身的位置与速度误差收敛到较低的范围内，最终输出自身和目标的位置及速度。由于传感器位置会影响滤波估计器对目标的观测，在滤波后引入路径优化算法改善目标定位的效率及准确度。此外，为了无人机的安全，在路径规划时为无人机在目标及地标周围设置了不同大小的禁飞区。

参见图1的目标跟踪几何示意图，示出了k时刻移动目标、无人机、锚点1(anchor1)和锚点2(anchor2)之间的位置关系，其中锚点1和锚点2是已知绝对位置的标志物。例如，锚点1的坐标为b₁＝[x_b1，y_b1]^T，锚点2的坐标为b₂＝[x_b2，y_b2]^T，T表示转置；移动目标在k时刻的坐标为p_k＝[x_pk，y_pk]^T，坐标未知，无人机在k时刻的位置标记为u_k(未知)。k是离散时间标识，设定离散的时间间隔为M。

参见图2所示，所提供的用于信息拒止环境下的无人机动态目标估计方法主要包括：步骤S110，获取AOA传感器量测；步骤S120，拓展卡尔曼滤波器；步骤S130，基于梯度下降的路径优化算法；步骤S140，判断是否在禁飞距离范围内；如判断为是，则执行路径重规划(步骤S150)；进而以固定速度向指定方向移动(步骤S160)。

在下文中，将具体介绍拓展卡尔曼滤波器及梯度下降优化算法。滤波器可采用递归式的卡尔曼滤波器，由于传感器量测与模型成非线性关系，因此最终以拓展卡尔曼滤波器为基础进行非线性滤波。

参见图3所示的拓展卡尔曼滤波的过程，主要包括以下步骤：

步骤S210，根据状态转移模型和初始状态，预测先验状态和协方差。

X_k|k-1＝FX_k-1|k-1+m_k (1)

P_k|k-1＝FX_k-1|k-1F^T+Q_k (2)

其中，X_k|k-1为基于k-1时刻状态X_k-1|k-1的先验估计；F为状态转移矩阵；P_k|k-1为状态X_k|k-1的协方差矩阵，用于表征状态X_k|k-1的不确定程度；m_k为过程噪声，用于量化未能充分考虑的***误差，例如，m_k是独立的零均值加性高斯白噪声，即m_k～N(0，Q_k)，Q_k为***误差m_k的协方差矩阵；

状态矩阵X由无人机自身的位置、速度以及目标的位置、速度构成，例如，

其中，

表示x_uk对时间的导数(速度)，具体第，x_uk是无人机在x轴的位置，

是无人机在x轴的速度分量，y_uk是无人机在y轴的位置，

是无人机在y轴的速度分量，x_pk是目标在x轴的位置，

是目标在x轴的速度分量，y_pk是目标在y轴的位置，

是目标在y轴的速度分量。

K-1时刻的状态X_k-1|k-1通过状态转移矩阵F转移到先验状态X_k|k-1，其中状态转移假设为速度恒定模型，因此，状态转移矩阵为

其中Fi为

即P_k+1＝P_k+V_kM。

步骤S220，根据当前状态和量测计算雅各比矩阵。

H_k＝Jacobian(X_k|k-1，Z_k) (2)

其中，Z_k为k时刻通过AOA传感器观测到的三维角度信息，即Z_k＝[θ_b1k，θ_b2k，θ_pk]^T，H_k为k时刻的量测矩阵，该矩阵为3行8列的雅各比矩阵，表示为：

其中，d_ub1＝||u_k-b₁||₂，d_ub2＝||u_k-b₂||₂，d_up＝||u_k-p_k||₂。

步骤S230，根据量测模型计算估计状态残差。

其中，

为残差，表示量测与估计之间的误差；函数h(·)为非线性的量测函数，将数据从状态空间转换到量测空间，具体展开为：

其中，n_k表示传感器量测噪声。例如，是独立的零均值加性高斯白噪声，即n_k～N(0，R_k)。

步骤S240，计算卡尔曼增益。

其中，R_k为测量噪声，用于表征传感器量测数据的噪声情况，例如，可以从传感器的规格书中获取；S_k为简化书写的中间变量；K_k为k时刻的卡尔曼增益。

步骤S250，更新后验估计和协方差。

P_k|k＝(I-K_kH_k)P_k|k-1 (9)

其中，X_k|k为后验估计状态；P_k|k为后验协方差矩阵；I为8*8的单位矩阵。

对于路径优化算法，其以当前状态作为输入，通过梯度下降的方式预测能有效降低损失函数(或称代价函数)的无人机的方向，从而使无人机在沿该方向移动时能获得最佳的观测路径。

图4是构建损失函数的示意，其以当前状态X_k|k和小位移向量δ为输入，将其传入原滤波器的拷贝，完成一步预测，从而得到以小位移δ移动得到的状态协方差矩阵，最终以协方差矩阵的迹为输出。

结合图4所示，构建损失函数具体包括：

X_k|k-1＝FX_k-1|k-1+m_k(10)

P_k|k-1＝FX_k-1|k-1F^T+Q_k (11)

X_k+1|k，δ＝X_k+1|k+δ (12)

其中，δ为小位移向量；X_k+1|k，δ为状态X_k+1|k经过小位移之后的状态。

然后，根据X_k+1lk，δ计算H_k+1。

计算：

计算：

计算：P_k+1|k+1＝(I-K_k+1H_k+1)P_k+1|k (15)

计算：J(X`_k+1|k+1)＝tr(P_k+1|k+1) (16)

其中，tr(·)为取矩阵迹；J(·)为经过小位移之后的代价值。

在一个实施例中，假设无人机在2D平面进行移动，无人机的移动具有两个自由度，因此可分解为沿x轴与y轴共两个分量，又可分正负两个方向，组合后得到4个可移动的方向[(d，0)，(-d，0)，(0，d)，(0，-d)]。以上述四个方向进行移动，得到各个方向移动的代价值，从而得到各个方向移动进行观测好坏程度的度量。

图5是路径优化过程的示意图，其中一个分支用于计算无人机沿x轴飞行的代价值，另一个分支用于计算无人机沿y轴飞行的代价值，具体包括以下步骤：

以δ＝[d，0]为参数计算沿x轴正方向移动的代价值J_xp，其中，d为较小的标量，表示步长；

以δ＝[-d，0]为参数计算沿x轴负方向移动的代价值J_xn；

计算无人机沿x轴方向飞行的总代价

以δ＝[0，d]为参数计算沿y轴正方向移动的代价值J_yp；

以δ＝[0，-d]为参数计算沿y轴负方向移动的代价值J_yn；

计算无人机沿y轴方向飞行的总代价

计算总代价值：J＝[J_x，J_y]；

计算k时刻经过优化后的方向，表示为：

其中，||·||₂为L2正则，v表示无人机恒定的飞行速度。

在完成路径规划后，由于越靠近标志物或目标，获得的信息会更加准确，这个现象会导致无人机会向标志物或目标不断靠近，直至重合发生撞击。因此，在路径规划完成后，需要根据无人机与标志物或目标的位置设置禁飞区，以避免无人机飞行过近，发生危险。

综上，本发明引入标志物(一个或多个)配合AOA传感器完成无人机自身坐标获取，并通过引入标志物(一个或多个)配合AOA传感器完成目标坐标获取。此外，通过随机扰动的方式完成代价函数梯度下降进而优化无人机飞行路径，获得针对移动目标的最佳观测。本发明仅需要例如PTZ相机量测无人机与锚点、目标之间的方位角，无需其它任何信息即可获得无人机与目标的绝对位置信息，至少采用两个已知绝对位置的锚点信息即可完成无人机和目标的位置跟踪。经验证，本发明提升了基于AOA实现目标跟踪的效率及精度。

本发明可以是***、方法和/或计算机程序产品。计算机程序产品可以包括计算机可读存储介质，其上载有用于使处理器实现本发明的各个方面的计算机可读程序指令。

计算机可读存储介质可以是可以保持和存储由指令执行设备使用的指令的有形设备。计算机可读存储介质例如可以是但不限于电存储设备、磁存储设备、光存储设备、电磁存储设备、半导体存储设备或者上述的任意合适的组合。计算机可读存储介质的更具体的例子(非穷举的列表)包括：便携式计算机盘、硬盘、随机存取存储器(RAM)、只读存储器(ROM)、可擦式可编程只读存储器(EPROM或闪存)、静态随机存取存储器(SRAM)、便携式压缩盘只读存储器(CD-ROM)、数字多功能盘(DVD)、记忆棒、软盘、机械编码设备、例如其上存储有指令的打孔卡或凹槽内凸起结构、以及上述的任意合适的组合。这里所使用的计算机可读存储介质不被解释为瞬时信号本身，诸如无线电波或者其他自由传播的电磁波、通过波导或其他传输媒介传播的电磁波(例如，通过光纤电缆的光脉冲)、或者通过电线传输的电信号。

这里所描述的计算机可读程序指令可以从计算机可读存储介质下载到各个计算/处理设备，或者通过网络、例如因特网、局域网、广域网和/或无线网下载到外部计算机或外部存储设备。网络可以包括铜传输电缆、光纤传输、无线传输、路由器、防火墙、交换机、网关计算机和/或边缘服务器。每个计算/处理设备中的网络适配卡或者网络接口从网络接收计算机可读程序指令，并转发该计算机可读程序指令，以供存储在各个计算/处理设备中的计算机可读存储介质中。

用于执行本发明操作的计算机程序指令可以是汇编指令、指令集架构(ISA)指令、机器指令、机器相关指令、微代码、固件指令、状态设置数据、或者以一种或多种编程语言的任意组合编写的源代码或目标代码，所述编程语言包括面向对象的编程语言—诸如Smalltalk、C++、Python等，以及常规的过程式编程语言—诸如“C”语言或类似的编程语言。计算机可读程序指令可以完全地在用户计算机上执行、部分地在用户计算机上执行、作为一个独立的软件包执行、部分在用户计算机上部分在远程计算机上执行、或者完全在远程计算机或服务器上执行。在涉及远程计算机的情形中，远程计算机可以通过任意种类的网络—包括局域网(LAN)或广域网(WAN)—连接到用户计算机，或者，可以连接到外部计算机(例如利用因特网服务提供商来通过因特网连接)。在一些实施例中，通过利用计算机可读程序指令的状态信息来个性化定制电子电路，例如可编程逻辑电路、现场可编程门阵列(FPGA)或可编程逻辑阵列(PLA)，该电子电路可以执行计算机可读程序指令，从而实现本发明的各个方面。

这里参照根据本发明实施例的方法、装置(***)和计算机程序产品的流程图和/或框图描述了本发明的各个方面。应当理解，流程图和/或框图的每个方框以及流程图和/或框图中各方框的组合，都可以由计算机可读程序指令实现。

这些计算机可读程序指令可以提供给通用计算机、专用计算机或其它可编程数据处理装置的处理器，从而生产出一种机器，使得这些指令在通过计算机或其它可编程数据处理装置的处理器执行时，产生了实现流程图和/或框图中的一个或多个方框中规定的功能/动作的装置。也可以把这些计算机可读程序指令存储在计算机可读存储介质中，这些指令使得计算机、可编程数据处理装置和/或其他设备以特定方式工作，从而，存储有指令的计算机可读介质则包括一个制造品，其包括实现流程图和/或框图中的一个或多个方框中规定的功能/动作的各个方面的指令。

也可以把计算机可读程序指令加载到计算机、其它可编程数据处理装置、或其它设备上，使得在计算机、其它可编程数据处理装置或其它设备上执行一系列操作步骤，以产生计算机实现的过程，从而使得在计算机、其它可编程数据处理装置、或其它设备上执行的指令实现流程图和/或框图中的一个或多个方框中规定的功能/动作。

附图中的流程图和框图显示了根据本发明的多个实施例的***、方法和计算机程序产品的可能实现的体系架构、功能和操作。在这点上，流程图或框图中的每个方框可以代表一个模块、程序段或指令的一部分，所述模块、程序段或指令的一部分包含一个或多个用于实现规定的逻辑功能的可执行指令。在有些作为替换的实现中，方框中所标注的功能也可以以不同于附图中所标注的顺序发生。例如，两个连续的方框实际上可以基本并行地执行，它们有时也可以按相反的顺序执行，这依所涉及的功能而定。也要注意的是，框图和/或流程图中的每个方框、以及框图和/或流程图中的方框的组合，可以用执行规定的功能或动作的专用的基于硬件的***来实现，或者可以用专用硬件与计算机指令的组合来实现。对于本领域技术人员来说公知的是，通过硬件方式实现、通过软件方式实现以及通过软件和硬件结合的方式实现都是等价的。

以上已经描述了本发明的各实施例，上述说明是示例性的，并非穷尽性的，并且也不限于所披露的各实施例。在不偏离所说明的各实施例的范围和精神的情况下，对于本技术领域的普通技术人员来说许多修改和变更都是显而易见的。本文中所用术语的选择，旨在最好地解释各实施例的原理、实际应用或对市场中的技术改进，或者使本技术领域的其它普通技术人员能理解本文披露的各实施例。本发明的范围由所附权利要求来限定。

Claims (10)

1.一种用于信息拒止环境下的无人机动态目标估计方法，包括以下步骤：

获取三维角度信息，所述三维角度信息包括无人机到第一锚点的到达角、无人机到第二锚点的到达角以及无人机到目标的到达角；

对于所述三维角度信息，利于拓展卡尔曼滤波器进行滤波，获得估计的无人机状态信息以及对应的状态协方差信息，所述无人机状态信息用于表征无人机的位置、速度以及目标的位置和速度；

利用所述无人机状态信息和所述状态协方差信息构建损失函数，以优化无人机的飞行路径，实现对目标的观测。

2.根据权利要求1所述的方法，其特征在于，对于所述三维角度信息，利于拓展卡尔曼滤波器进行滤波，获得估计的无人机状态信息以及对应的状态协方差信息包括：

预测无人机的先验状态和协方差，表示为：

X_k|k-1＝FX_k-1|k-1+m_k

P_k|k-1＝FX_k-1|k-1F^T+Q_k

其中，X_k|k-1为基于k-1时刻状态X_k-1|k-1的先验估计，P_k|k-1为状态X_k|k-1的协方差矩阵，m_k为过程噪声，Q_k是m_k的协方差矩阵，

x_uk是无人机在x轴的位置，

是无人机在x轴的速度分量，y_uk是无人机在y轴的位置，

是无人机在y轴的速度分量，x_pk是目标在x轴的位置，

是目标在x轴的速度分量，y_pk是目标在y轴的位置，

是目标在y轴的速度分量，F是状态转移矩阵；

计算雅各比矩阵，表示为：

H_k＝Jacobian(X_k|k-1,Z_k)

其中，Z_k为k时刻观测到的三维角度信息，H_k是3行8列的雅各比矩阵

计算估计状态残差，表示为：

其中，

为残差，h(X_k|k-1)表示为：

计算卡尔曼增益，表示为：

其中，R_k为测量噪声，S_k是中间变量，K_k为k时刻的卡尔曼增益；

更新后验估计和协方差，作为估计的无人机状态信息以及对应的状态协方差信息，表示为：

P_k|k＝(I-K_kH_k)P_k|k-1

其中，X_k|k为后验估计状态，P_k|k为后验协方差矩阵，I为8*8的单位矩阵，x_b1是第一锚点的x轴坐标，y_b1是第一锚点的y轴坐标，x_b2是第二锚点的x轴坐标，y_b2是第二锚点的y轴坐标，x_pk是目标在k时刻的x轴坐标，y_pk是目标在k时刻的y轴坐标，d_ub1＝||u_k-b₁||₂，d_ub2＝||u_k-b₂||₂，d_up＝||u_k-p_k||₂，b₁＝[x_b1，y_b1]^T，b₂＝[x_b2，y_b2]^T，p_k＝[x_pk，y_pk]^T，u_k＝[x_uk，y_uk]^T，n_k是传感器的量测噪声。

3.根据权利要求2所述的方法，其特征在于，所述损失函数根据以下步骤构建：

计算状态X_k+1|k经过小位移δ之后的状态，表示为：

X_k+1|k，δ＝X_k+1|k+δ；

根据X_k+1|k，δ计算H_k+1；

计算

计算

计算P_k+1|k+1＝(I-K_k+1H_k+1)P_k+1|k；

计算J(X`_k+1|k+1)＝tr(P_k+1|k+1)；

其中，tr(·)为取矩阵迹，J(·)为经过小位移之后的代价值。

4.根据权利要求3所述的方法，其特征在于，利于所述无人机状态信息和所述状态协方差信息构建损失函数，以优化无人机的飞行路径包括：

以δ＝[d,0]为参数计算无人机沿x轴正方向移动的代价值J_xp；

以δ＝[-d,0]为参数计算无人机沿x轴负方向移动的代价值J_xn；

计算无人机沿x轴方向的总代价

以δ＝[0,d]为参数计算无人机沿y轴正方向移动的代价值J_yp；

以δ＝[0,-d]为参数计算无人机沿y轴负方向移动的代价值J_yn；

计算无人机沿y轴方向的总代价

计算总代价值J＝[J_x,J_y]；

利用计算的总代价值获得优化的k时刻的方向，表示为：

其中，||·||₂为L2正则，d表示步长。

5.根据权利要求1所述的方法，其特征在于，所述三维角度信息利用到达角传感器测量获得。

6.根据权利要求1所述的方法，其特征在于，还包括：在优化无人机的飞行路径过程中，在目标及地标周围设置禁飞区。

7.根据权利要求1所述的方法，其特征在于，所述无人机是无人飞行器，包括多旋翼无人机或固定翼无人机。

8.根据权利要求1所述的方法，其特征在于，还包括：根据所述无人机到目标的到达角来确定目标的绝对地理坐标。

9.一种计算机可读存储介质，其上存储有计算机程序，其中，该计算机程序被处理器执行时实现根据权利要求1至8中任一项所述方法的步骤。

10.一种计算机设备，包括存储器和处理器，在所述存储器上存储有能够在处理器上运行的计算机程序，其特征在于，所述处理器执行所述计算机程序时实现权利要求1至8中任一项所述的方法的步骤。

Images