CN113359837A - 一种使用单架无人机追踪移动源的方法 - Google Patents

Abstract

本发明公开了一种使用单架无人机追踪移动源的方法，具体为：步骤1：安装在单架无人机上的UWB传感器实时采集单架无人机与移动源之间的距离；步骤2：当移动源按照某个速度方向匀速直线运动时，根据UWB传感器实时采集到的距离以及相对速度变换的方法计算移动源相对于单架无人机的相对速度，从而估计移动源速度方向，根据移动源速度方向计算移动源的速度v_s；步骤3：根据移动源的速度，计算得到单架无人机相对于移动源的相对速度，并基于多点数据融合策略，估计移动源的位置。本发明算法简单有效，逻辑清晰，针对移动源追踪问题给出了一种精确的速度估计算法和快速的源位置定位方法。

Description

一种使用单架无人机追踪移动源的方法

技术领域

本发明属于源追踪研究技术领域。

背景技术

无人机往往被定义为不需要人类操作人员控制，使用空气动力学提供升力，可以自主飞行或被远程操控，并可携带特定传感器的动力飞行器.与传统的载人飞机相比，无人机更适合复杂危险环境下的任务执行。如今许多科研工作者对许多民用和军事任务的无人机发展具有极大的兴趣，无人机的应用场景也很多，例如水面侦察，辅助执法，灾难援助，电信中继，边界监视，农业测量，电力系统监测，考古现场控制等。

源追踪问题中的源是一个很抽象的概念，具体到现实世界中，源可以看作是对外释放气体浓度信号的化学源，如1984年印度发生的Bhopal气体泄漏是工业化学界最严中的一次事故，超过五十万人被暴露在有毒气体中.又比如加州Aliso Canyon气体泄漏致使超过100000吨的甲烷气体释放到空气中，对附近居民的安全造成了极大的威胁。因此可以说，如果这样的化学源能被快速的定位追踪到，那么这种事故的负面影响就能被很大程度的降至最低。同时，源也可以看作是一种对外发出无线电信号的无线源，例如车载移动伪基站就是一种移动的无线源，而追踪捕获移动伪基站也是一件非常有实际意义的事。一些生物学家通过将RF(Radio Frequency)源安装在野生动物身上，以此来研究野生动物种群的习性，这里的RF源也可看作是一种无线源。通过上面提及的几个例子可以总结，源追踪问题通常是指通过感知源的信息来定位源的位置，或者跟踪、监视、捕获源。

无人机用于源追踪问题场景具有很多优点：1)无人机不会受到地形的约束，且能在危险的环境中执行寻源任务。2)无人机本身又具有高度的灵活性，能提升寻源效率。3)无人机硬件设计往往采用模块化的设计思想，因而也更易维护，且能在不同寻源场景下按需配置相应的传感器。虽然使用无人机进行源追踪已经被广泛的研究了，但是以往的研究往往假设源是固定不动的，而在现实场景下源很有可能是移动的，比如带有RF的野生动物和移动伪基站等，可能使得估计结果不够精确。

发明内容

发明目的：为了解决上述现有技术存在的问题，本发明提供了一种使用单架无人机追踪移动源的方法。

技术方案：本发明公开了一种使用单架无人机追踪移动源的方法，具体包括如下步骤：

步骤1：安装在单架无人机上的UWB传感器实时采集单架无人机与移动源之间的距离；

步骤2：当移动源按照某个速度方向匀速直线运动时，根据UWB传感器实时采集到的距离以及相对速度变换的方法计算移动源相对于单架无人机的相对速度，从而估计移动源的速度矢量；

步骤3：根据移动源的速度矢量，计算得到单架无人机相对于移动源的相对速度，并基于多点数据融合策略，估计移动源的位置。

进一步的，所述步骤1中UWB传感器采集到的单架无人机与移动源之间的距离为：

y_t＝f(s_t，x_t)+ε

f(s_t，x_t)表示第t次采样时无人机与移动源之间的真实距离，ε为UWB传感器固有的测量误差，ε服从N(0，δ²)，N(.)为高斯分布，δ为标准差。

进一步的，所述步骤3具体为：

步骤3.1：将单架无人机与移动源所在的空间视为二维平面，对该二维平面进行栅格化，并为二维平面中每个栅格点赋予初始的权重值；将二维平面内所有的栅格点的集合作为后候选栅格点集合Φ；

步骤3.2：令迭代计算次数t’＝1；

步骤3.3：当t’＝1时，单架无人机以预设的速度按照预设的方向飞行，当t’＞1时，单架无人机以预设的速度朝着第t’-1次估计的移动源的位置飞行；

第t’迭代计算时，在单架无人机飞行过程中UWB传感器进行2k’+1次采样，将2k’+1次采样得到的数据组成第t’段数据段，k’为预设的正整数，

其中ΔL为单架无人机在UWB传感器的采样时间间隔Δt内以相对速度移动的距离；

步骤3.4：计算第t’次迭代计算过程中UWB传感器第k’次采样时无人机与移动源之间的真实距离f(s_k’，x_k’)的估计值：

其中，y_k’+j’表示第t’次迭代计算过程中UWB传感器第k’+j’次采集得到的数据；

步骤3.5：更新后候选栅格点集合Φ：若Φ中第q个栅格点Φ_q满足条件：

则保留该栅格点，否则在Φ中删除该栅格点，q＝1，2，…，W，W为更新前Φ中栅格点的总个数，q_x，y为栅格点Φ_q在二维平面中的坐标，x_t’k为第t’次迭代计算中UWB传感器进行第k’次采样时，单架无人机在二维平面中的坐标；其中

的表达式为：

其中σ＝3δ；

步骤3.6：为计算更新后的Φ中每个栅格点重新赋予权重：

其中，Φ_q’为更新后的Φ中第q’个栅格点，w_t’(Φ_q’)为第t’次迭代计算时栅格点Φ_q，的权重；y为随机变量，exp(.)为指数函数；q’＝1，2…，Q；Q为更新后的Φ中栅格点的总个数，q’_x，y为栅格点Φ_q’在二维平面上的坐标；

步骤3.7：按照如下公式估计移动源的位置

无人机朝着估计的位置

飞行一段时间t_f后，若UWB传感器采集到的距离值在预设的距离范围之内，则将

作为移动源的位置；否则迭代计算次数1，并转步骤3.3；

当t’＝1时，按照如下公式估计移动源的位置：

v_s为移动源的速度矢量；

当t’＞1时，按照如下公式估计移动源的位置：

其中ΔT₁为第t’次迭代计算过程中UWB传感器第k’次采样与第t’-1次迭代计算过程中UWB传感器第k’次采样的时间差。

进一步的，所述步骤2具体为：

步骤2.1：令迭代次数e＝1，建立直角坐标系；

步骤2.2：第e次迭代计算时，单架无人机以第e次预设的速度v_e按照第e次预设的方向飞行，在飞行过程中UWB传感器进行2k+1次采样；将该2k+1次采样得到的数据组合成第e段数据；k为预设的整数系数；

步骤2.3：根据第e段数据计算移动源的相对速度：

其中，i＝k，y_i为在第e次迭代计算中UWB传感器第i次采集得到的数据，y_i+j表示在第e次迭代计算中UWB传感器第i+j次采集得到的数据，Δt为UWB传感器的采样时间间隔；

步骤2.4：将v_e分解成水平方向上的速度矢量v_e，x和垂直方向上的速度矢量v_e，y，根据v_e，x和v_e，y得到v_e在直角坐标系上的坐标，以v_e在直角坐标系上的坐标为圆点，相对速度

的矢量长度为半径，得到第e次迭代计算时的圆形区域；

步骤2.5：若e＝1，则令迭代次数加1，重新预设k的值，并返回步骤1.2；否则转步骤2.6；

步骤2.6：若e＝2且两个圆形区域之间存在切点，根据该切点在直角坐标系上的位置得到移动源的速度矢量，并停止计算；若e＞2则转步骤2.7；若e＝2且两个圆形区域之间不存在切点，则令迭代次数加1，重新预设k的值，并返回步骤2.2；

步骤2.7：判断e个圆形区域之间是否有共同的交点，若存在一个共同的交点，则根据该交点在直角坐标系上的位置得到移动源的速度矢量；若存在多个共同的交点，则剔除离散的交点，在剩余的的交点中随机选择一个交点，根据选择的交点在直角坐标系上的位置得到移动源的速度矢量；若不存在交点，则令迭代次数加1，重新预设k的值，并返回步骤2.2。

有益效果：本发明算法简单有效，逻辑清晰，针对移动源追踪问题给出了一种精确的速度估计算法和快速的源位置定位方法，对无人机相关领域其他应用也具有积极作用；现有的很多的源追踪问题场景中都假设源是静止不动的，而本发明正对针对动态源提出的速度估计器算法和数据融合策略都有误差分析的探讨，对解决动态源的追踪问题具有更加深刻的现实意义。

附图说明

图1为本发明的方法的流程图。

图2为计算相对速度估计的示意图。

图3为本发明计算移动源速度方向时建立第一个圆形区域的示意图。

图4为本发明计算移动源速度方向时建立第二个圆形区域的示意图。

具体实施方式

构成本发明的一部分的附图用来提供对本发明的进一步理解，本发明的示意性实施例及其说明用于解释本发明，并不构成对本发明的不当限定。

如图1所示本发明包括如下步骤：

步骤1：根据系统内的各个模型的特点进行建模，包括单架无人机和移动源的动力学模型以及单架无人机携带的UWB传感器模型，并初始化模型相关参数。

步骤2：基于步骤1建立的传感器模型和无人机动力学模型的特点，使用相对速度变换的观点并通过几何关系来推导出移动源的相对速度大小并设计策略去估计源的速度方向。

步骤3：基于步骤2获取的移动源的速度信息(方向和大小)，可以将移动源看作是相对静止的，从而将原问题转化为更易处理的静态源追踪问题，并结合多点数据融合策略和历史采集数据来快速推断源点的可能位置。

所述步骤1中，系统各组成部分的建模，具体包括以下步骤：

步骤1.1：针对单架无人机的飞行特点，可以将单架无人机的状态转移建模成由输入速度控制的动力学模型；

在任意采样时刻t,单架无人机的状态可以定义为x_t＝[x_t1,x_t2]^T,其中x_t1和x_t2分别代表追踪无人机在t时刻时在二维平面上的横纵坐标值。因此在UWB采样间隔Δt后，速度输入控制u_t会造成无人机的状态转变为：

x_t+Δt＝x_t+u_tΔt

步骤1.2：针对移动源的移动特点，可以将移动源的状态转移建模成起始位置未知和速度大小方向都未知的匀速直线运动方式的动力学模型：

在任意采样时刻t，移动源的状态可以定义为s_t＝[s_t1，s_t2]^T，其中s_tl和s_t2分别代表移动源在t时刻时在二维平面上的坐标值。假设移动源是以一个固定的速度运行的，所以移动源的状态转变为：

其中

为s_t的一阶导数。

针对移动源信号强度和距离的关系，可以将追踪无人机上的传感器模型建模成信号强度值大小与其之间距离成正比关系的传感器模型：

y_t＝f(s_t，x_t)+ε

f(s_t，x_t)表示第t次采样时无人机与移动源之间的真实距离，ε为UWB传感器本身固有的测量误差，ε服从N(0，δ²)，N(.)为高斯分布，δ为标准差。

所述步骤2中估计移动源的速度信息的方法，具体包括以下步骤：

步骤2.1：针对步骤1所提模型的特点，利用几何关系推导出源的相对速度大小；

因为单架无人机和移动源在一段时间上都是匀速直线运动的，单架无人机的运动速度v_e以及运动方向均为预设，所以可以将追踪无人机看作是静止不动，而移动源以相对速度进行移动，在该段时间上单架无人机进行第2k+1次采样，并将采样得到的数据组合成第e段数据，通过图2可以看出，相对速度大小值可以通过几何关系推导出，具体如下：

其中，i＝k，y_i为在第e段数据中UWB传感器第i次采集得到的数据，y_i+j表示在第e段数据中UWB传感器第i+j次采集得到的数据；

步骤2.2：根据误差传播理论对相对速度值的准确性进行分析，从而提升估计准确度，因为没有办法获取第i次采样时真实的无人机与移动源之间的距离d_i(也既f(s_i，x_i))，因此只能用什么的观测值y_i代替，但是y_i本身是具有噪声影响的，认为这个噪声造成的误差分布在(-σ，σ)内，σ＝3δ，根据误差传播理论，可以得出

值的误差为：

其中Δl₁表示移动源以相对速度在时间范围Δt内移动的距离；由上述公式可知，为了使得估计的

更加准确，可以选取更大k值，即使用更多的采集数据，或者增大采样间隔Δt和相对速度大小，其中相对速度大小的改变可以通过改变追踪无人机的速度大小而实现。

步骤2.3：根据步骤2.1计算得到的移动源的相对速度的大小，通过相对速度矢量图交点区域来求解源的速度方向信息，如果交点区域未收敛则令迭代次数加1，重新预设k的值，重新给无人机一个速度以及速度方向并返回步骤2.1；否则得到移动源的速度方向信息，并根据速度方向信息计算移动源的速度。

所述通过相对速度矢量图交点区域来求解源的速度方向信息，如图3，4所示具体计算方式为：建立直角坐标系，将v_e分解成水平方向上的速度矢量v_e，x和垂直方向上的速度矢量v_e，y，根据v_e，x和v_e，y得到v_e在直角坐标系上的坐标，以v_e在直角坐标系上的坐标为圆点，相对速度

的矢量长度为半径，得到第e次迭代计算时的圆形区域；当e＝1时，只有一个圆形区域，不存在交点，则肯定没有收敛；当e＝2，则有两个圆形区域若两个圆形区域之间存在切点，则认为交点区域收敛，根据切点的坐标得到移动源的速度矢量，如果两个圆形相交则不认为交点区域收敛；当e＞2时，如果e个圆形区域之间存在一个或多个共同的交点，则认为交点区域收敛，若e个圆形区域之间存在一个共同的交点，则根据该交点的坐标得到移动源的速度矢量；若存在多个共同的交点，则剔除离散的交点，在剩余的的交点中随机选择一个交点，根据选择的交点坐标得到移动源的速度矢量；本实施例中e＝3时，3个圆形区域之间存在一个共同的交点；

所述步骤3中利用步骤2获取到的移动源的运动信息，将原始的难处理的移动源追踪问题转化成了更易处理的静态源追踪问题，具体为：

步骤3.1结合步骤2.3获取的移动源的运动信息，可以将移动源看作是相对静止不动的，追踪无人机看做以相对速度飞行。将单架无人机与移动源所在的空间视为二维平面，对该二维平面进行栅格化，并为二维平面中每个栅格点赋予初始的权重值，该点可能为源点位置的可能性大；将二维平面内所有的栅格点的集合作为后候选栅格点集合Φ；

步骤3.2：根据步骤3.1的问题转化，使单架无人机采用预设的速度向可能源点位置处飞行一段距离(第一次迭代计算时单架无人机朝着预设的方向直线飞行，无人机以预设的速度朝着第t’-1次估计的移动源的位置(可能源点位)直线飞行)；

第t’迭代计算时，单架无人机在飞行过程中UWB传感器进行2k’+1次采样，将2k’+1次采样得到的数据组成第t’段数据段，k’为预设的正整数，用数据融合策略来减小传感器噪声的影响从而对距离的真实值进行估更加准确的估计，具体融合策略为，将移动源直线运动上的2k’+1个数据融合到该直线中点位置上去，从而使得距离测量值与真实值f(s_k’，x_k，)更为接近，具体为：

为第t’次迭代计算过程中UWB传感器第k’次采样时无人机与移动源之间的真实距离f(s_k’，x_k’)的估计值；ΔL为单架无人机在UWB传感器的采样间隔时间Δt内以相对速度移动的距离；所述单架无人机的为相对速度为无人机的速度减去v_s。

通过误差传播理论分析可以得出，

的误差范围如下：

可以看出这个误差范围是波动的，并且由函数

来决定，最小化误差范围可以得到：

真正实现时，因为

会是较大的取值，所以往往不直接选用k^*，而函数

在区间

为减函数，所以k’可以选择该区间中一个较为合适的值，也就是说

从而使得融合后的测量误差也会比较小。

步骤3.3：更新后候选栅格点集合Φ：若Φ中第q个栅格点Φ_q满足条件：

则保留该栅格点，否则在Φ中删除该栅格点，q＝1，2，…，W，W为更新前Φ中栅格点的总个数，q_x，y为栅格点Φ_q在二维平面中的坐标，x_t’k’为第t’次迭代计算中UWB传感器进行第k’次采样时，单架无人机在二维平面中的坐标；

t’次迭代计算后(也既无人机飞行t’段后)，筛选的依据如下：

其中II函数代表指示函数，当里面的条件为真时取1，否则取0。Φ^t’是指飞行完t’段直线后，可能为源点位置的网格点集合，也既第t’次迭代计算时更新后的集合Φ。

筛选完网格点后，为更新后的集合Φ中每个网格点重新赋予一个概率权重值，具体如下：

其中，Φ_q’为更新后的Φ中第q’个栅格点，w_t’(Φ_q’)为第t’次迭代计算时更新后的Φ中第q’个栅格点的权重；y为随机变量，exp(.)为指数函数；q’＝1，2…，Q；Q为更新后的Φ中栅格点的总个数，q’_x，y为栅格点Φ_q’在二维平面上的坐标；。

为了使得概率权重和为1，需要进行归一化操作，具体为：

归一化操作后，便可以估计源点的位置为：

当t’＝1时，估计移动源的位置为：

当t’＞1时，估计移动源的位置为：

其中ΔT₁为第t’次迭代计算过程中UWB传感器第k’次采样与第t’-1次迭代计算过程中UWB传感器第k’次采样的时间差，vs为移动源的速度矢量，

其中X₁和Y₁分别表示步骤2种交点或切点的坐标值。|.|表示求模。

得到移动源的位置后，单架无人机朝着该移动源的位置的飞行一段时间t_f后，则此时移动源的估计位置为：

当t’＝1，

当t’＞1时：

若此时UWB传感器采集到的距离值在预设的距离范围之内，则将

作为移动源的位置，寻源成功，算法终止。否则否则迭代次数加1，并返回步骤3.2。

上面结合附图对本发明的实施方式作了详细说明，但是本发明并不限于上述实施方式，在本领域普通技术人员所具备的知识范围内，还可以在不脱离本发明宗旨的前提下做出各种变化。

Claims (4)

1.一种使用单架无人机追踪移动源的方法，其特征在于，具体包括如下步骤：

步骤1：安装在单架无人机上的UWB传感器实时采集单架无人机与移动源之间的距离；

步骤2：当移动源按照某个速度方向匀速直线运动时，根据UWB传感器实时采集到的距离以及相对速度变换的方法计算移动源相对于单架无人机的相对速度，从而估计移动源的速度矢量；

步骤3：根据移动源的速度矢量，计算得到单架无人机相对于移动源的相对速度，并基于多点数据融合策略，估计移动源的位置。

2.根据权利要求1所述的一种使用单架无人机追踪移动源的方法，其特征在于，所述步骤1中UWB传感器采集到的单架无人机与移动源之间的距离为：

y_t＝f(s_t，x_t)+ε

f(s_t，x_t)表示第t次采样时无人机与移动源之间的真实距离，ε为UWB传感器固有的测量误差，ε服从N(0，δ²)，N(.)为高斯分布，δ为标准差。

3.根据权利要求2所述的一种使用单架无人机追踪移动源的方法，其特征在于，所述步骤3具体为：

步骤3.1：将单架无人机与移动源所在的空间视为二维平面，对该二维平面进行栅格化，并为二维平面中每个栅格点赋予初始的权重值；将二维平面内所有的栅格点的集合作为后候选栅格点集合Φ；

步骤3.2：令迭代计算次数t’＝1；

步骤3.3：当t’＝1时，单架无人机以预设的速度按照预设的方向飞行，当t’＞1时，单架无人机以预设的速度朝着第t’-1次估计的移动源的位置飞行；

第t，迭代计算时，在单架无人机飞行过程中UWB传感器进行2k’+1次采样，将2k’+1次采样得到的数据组成第t’段数据段，k’为预设的正整数，

其中ΔL为单架无人机在UWB传感器的采样时间间隔Δt内以相对速度移动的距离；

步骤3.4：计算第t’次迭代计算过程中UWB传感器第k’次采样时无人机与移动源之间的真实距离f(s_k’，x_k’)的估计值：

其中，y_k’+j’表示第t’次迭代计算过程中UWB传感器第k’+j’次采集得到的数据；

步骤3.5：更新后候选栅格点集合Φ：若Φ中第q个栅格点Φ_q满足条件：

则保留该栅格点，否则在Φ中删除该栅格点，q＝1，2，…，W，W为更新前Φ中栅格点的总个数，q_x，y为栅格点Φ_q在二维平面中的坐标，x_t’k为第t’次迭代计算中UWB传感器进行第k’次采样时，单架无人机在二维平面中的坐标；其中

的表达式为：

其中σ＝3δ；

步骤3.6：为计算更新后的Φ中每个栅格点重新赋予权重：

其中，Φ_q’为更新后的Φ中第q’个栅格点，w_t’(Φ_q’)为第t’次迭代计算时栅格点Φ_q’的权重；y为随机变量，exp(.)为指数函数；q’＝1，2…，Q；Q为更新后的Φ中栅格点的总个数，q’_x，y为栅格点Φ_q’在二维平面上的坐标；

步骤3.7：按照如下公式估计移动源的位置

无人机朝着估计的位置

飞行一段时间t_f后，若UWB传感器采集到的距离值在预设的距离范围之内，则将

作为移动源的位置；否则迭代计算次数加1，并转步骤3.3；

当t’＝1时，按照如下公式估计移动源的位置：

v_s为移动源的速度矢量；

当t’＞1时，按照如下公式估计移动源的位置：

其中ΔT₁为第t’次迭代计算过程中UWB传感器第k’次采样与第t’-1次迭代计算过程中UWB传感器第k’次采样的时间差。

4.根据权利要求1所述的一种使用单架无人机追踪移动源的方法，其特征在于，所述步骤2具体为：

步骤2.1：令迭代次数e＝1，建立直角坐标系；

步骤2.2：第e次迭代计算时，单架无人机以第e次预设的速度v_e按照第e次预设的方向飞行，在飞行过程中UWB传感器进行2k+1次采样；将该2k+1次采样得到的数据组合成第e段数据；k为预设的整数系数；

步骤2.3：根据第e段数据计算移动源的相对速度：

其中，i＝k，y_i为在第e次迭代计算中UWB传感器第i次采集得到的数据，y_i+j表示在第e次迭代计算中UWB传感器第i+j次采集得到的数据，Δt为UWB传感器的采样时间间隔；

步骤2.4：将v_e分解成水平方向上的速度矢量v_e，x和垂直方向上的速度矢量v_e，y，根据v_e，x和v_e，y得到v_e在直角坐标系上的坐标，以v_e在直角坐标系上的坐标为圆点，相对速度

的矢量长度为半径，得到第e次迭代计算时的圆形区域；

步骤2.5：若e＝1，则令迭代次数加1，重新预设k的值，并返回步骤2.2；否则转步骤2.6；

步骤2.6：若e＝2且两个圆形区域之间存在切点，根据该切点在直角坐标系上的位置得到移动源的速度矢量，并停止计算；若e＞2则转步骤2.7；若e＝2且两个圆形区域之间不存在切点，则令迭代次数加1，重新预设k的值，并返回步骤2.2；

步骤2.7：判断e个圆形区域之间是否有共同的交点，若存在一个共同的交点，则根据该交点在直角坐标系上的位置得到移动源的速度矢量；若存在多个共同的交点，则剔除离散的交点，在剩余的的交点中随机选择一个交点，根据选择的交点在直角坐标系上的位置得到移动源的速度矢量；若不存在交点，则令迭代次数加1，重新预设k的值，并返回步骤2.2。

Images