CN113741522A - 一种基于无人机协同对峙监视的有害物扩散源搜索方法 - Google Patents

Abstract

本发明公开了一种基于无人机协同对峙监视的有害物扩散源搜索方法，属于导航制导与控制领域；首先，从参与搜索任务的无人机中定义领导者和为跟随者，并获取搜索任务空域信息；领导者带领跟随者一同飞行，利用最小二乘法进行无人机协同梯度的估计；领导者利用梯度估计信息，执行对峙监视搜索和禁飞区规避；各跟随者根据领导者的运动信息和梯度估计信息，执行队形保持；对每个无人机的期望速度进行运动学约束，得到实际的规划速度，并计算各自下一步期望的航路点；重复进行下个时刻的飞行，直至领导者到达有害物扩散源的球形领域内，识别出有害物扩散源的位置；本发明提高了无人机在执行源搜索任务时的航路平滑性和飞行安全性。

Description

一种基于无人机协同对峙监视的有害物扩散源搜索方法

技术领域

本发明涉及一种基于无人机协同对峙监视的有害物扩散源搜索方法，属于无人机导航制导与控制技术领域。

背景技术

过去数十年间，有害扩散物泄漏的事故严重威胁着人们的生命健康，并造成了巨大的财产损失，如切尔诺贝利事故、墨西哥湾漏油事故和福岛核泄漏事故等。事故发生后，定位未知有害物扩散源对于应急响应和救援资源部署具有重要意义。由于有害物扩散源搜索通常是沉闷、肮脏且危险的，如何使用移动机器人执行任务引起了研究者广泛的兴趣。与地面机器人相比，无人机具备快速响应、高性价比和三维搜索能力等优势，因而在该领域拥有巨大的潜力和广阔的应用前景。

现有的源搜索方法可分为逆风运动法、信息增益法和梯度上升法三类。其中，逆风运动法又称“风趋向性”法，其基本思想为模拟飞蛾等昆虫寻觅食物或配偶的行为，通过逆风锯齿形运动和横越风运动来保持对气味等信息的跟踪；信息增益法又称“信息趋向性”法，该方法将源搜索问题转化为源位置信息的获取问题，进而使智能体向局部最大化期望信息增益速率的方向运动；梯度上升法又称“化学趋向性”法，该方法驱动智能体沿着其当前所在位置对应的有害物场的正梯度来趋近场源。

虽然学者们在源搜索领域已取得了丰富的成果，但现有技术仍普遍存在以下问题：

(1)许多研究仍局限于二维平面和静态的标量有害物场，而目前考虑有害物的动态时空分布的三维源搜索方法仍较少。由于实际的有害物传输过程往往发生在三维空间中，地面机器人可能由于维度限制而接触不到有害物的信息线索。此外，当有害物扩散源在空中时，地面机器人无法完成源搜索任务；

(2)许多研究假设有害物场的系数已知，如对流系数和扩散系数等，而在实际情况下该假设往往过于理想；

(3)对多约束情况缺乏较为全面的考虑，如智能体的运动学约束、禁飞区约束，特别是危险浓度有害物累积暴露量约束。尽管过长时间地暴露在危险浓度有害物中可能会导致智能体受损和任务降级，目前极少有文献讨论如何在源搜索过程中减少累积暴露量。

发明内容

本发明为了解决上述问题，面向具有复杂动态特性的三维有害物场，提出了一种基于无人机协同对峙监视的有害物扩散源搜索方法，由多无人机协同以估计有害物场梯度这一关键信息，摆脱了对有害物场先验知识的依赖，并通过对梯度估计信息的充分利用使无人机在向未知有害物扩散源不断趋近的同时满足运动学约束、禁飞区约束以及危险浓度有害物累积暴露量约束，为源搜索任务背景下无人机的航路平滑性和飞行安全性提供了坚实保障。

所述基于无人机协同对峙监视的有害物扩散源搜索方法，具体包括如下步骤：

步骤一、针对有害物扩散源，将参与搜索任务的N_u架无人机划分为1架领导者和N_u-1架跟随者，依次编号为1,2,…,N_u；

领导者无人机根据实际环境人为指定；

步骤二、在N_u架无人机执行源搜索任务的过程中，获取当前采样时刻对应的搜索任务空域的综合信息；

所述的综合信息具体包括：各架无人机的初始位置、地速向量和每架无人机测得的有害物浓度值，以及各个禁飞区的位置、形状和尺寸。

无人机的位置信息计算公式为：

其中，p_u＝[x_u,y_u,z_u]^T为无人机在地面系S_g中的三维位置向量，V，χ和γ分别为无人机的地速大小、航迹偏角和航迹倾角，n_x，n_y和n_z分别为无人机沿航迹系S_k三轴的过载即控制输入，g＝9.8m/s²为重力加速度。

无人机所受的运动学约束表示为：n_x∈[n_xmin,n_xmax]，n_y∈[n_ymin,n_ymax]，n_z∈[n_zmin,n_zmax]，V∈[V_min,V_max]，z∈[z_min,z_max]，γ∈[γ_min,γ_max],

以及

每架无人机都装有逐点式传感器，用以测量有害物浓度，测量值的表达式为：

f(p_u,t)＝c(p_u,t) (2)

其中，c(p_u,t)为时刻t时位置p_u＝[x_u,y_u,z_u]^T处的有害物浓度。

禁飞区信息的表达式为：

其中，p_o＝[x_o,y_o,z_o]^T为禁飞区中心在S_g中的位置向量，a，b，c为决定禁飞区大小的参数；p，q，r为决定禁飞区形状的参数。

禁飞区的典型形状包括长方体(p＞1，q＞1，r＞1)和圆柱(a＝b，p＝q＝1，r＞1)等。Γ(x,y,z)＝1为禁飞区表面方程，Γ(x,y,z)＜1与Γ(x,y,z)＞1分别表示禁飞区的内部区域和外部区域。

步骤三、当前采样时刻下领导者带领跟随者一同飞行，判断领导者是否到达半径为R_s的有害物扩散源球形领域内，如果是，此时领导者的感知能力足以识别出有害物扩散源的位置，源搜索任务完成，算法结束；否则，进入步骤四；

半径R_s取决于无人机的感知能力，视实际情况而定。即在该距离范围内，无人机能够识别出有害物扩散源的位置。

步骤四、利用最小二乘法进行N_u架无人机协同梯度的估计；

最优梯度估计为：

式中，

为时刻t对应的编队中心p_c(t)处的估计梯度，I为(d×d)维单位矩阵，0是元素都为0的d维列向量，θ(t)为代表估计参数的(d+1)维列向量。

步骤五、逐个选择各无人机i，判断i≤N_u是否成立；如果是，则执行步骤六；否则，返回步骤二，进行下一采样时刻的无人机协同源搜索；

初始值i＝1，无人机为领导者；

步骤六、判断第i架无人机是否为领导者；如果是，则执行步骤七；否则，进入步骤九；

步骤七、利用梯度估计信息，执行对峙监视搜索，控制领导者的速度，使领导者收敛至对应于危险阈值的有害物场的边界，并沿该边界向趋近于有害物扩散源的方向运动直至到达有害物扩散源的领域内。

危险阈值为不影响正常工作时无人机所能承受的最大浓度值；

领导者的速度表示为：

其中，u₁(p₁)为地面坐标系S_g下领导者的期望速度；k₁＞0为常量；e(p₁,t)为领导者的水平集跟踪误差；V₀为领导者的巡航速度；

为估计梯度

在水平面上的投影；

在水平面内且正交于

有

k₂＞0为决定下降速率的常数。

步骤八、在对峙监视搜索的基础上，领导者执行禁飞区规避，得到规避后的期望速度；然后进入步骤十；

设u(p₁)＝u₁(p₁,t)，任务区域中存在N_o个禁飞区。

使用扰动矩阵修正领导者当前期望速度，得到考虑禁飞区规避后的修正期望速度为：

扰动矩阵

的表达式为：

其中，ω_k(p₁)表示第k个禁飞区的权重系数，其表达式为：

式中，Γ_i(p₁)表示第i个禁飞区。

第k个禁飞区的扰动矩阵的定义为：

其中，I为d×d维单位矩阵，又被称为吸引矩阵。ρ_k的定义为：

式中，

为正常数，又称排斥反应系数；||p₁,O_k||为从无人机到第k个禁飞区表面的距离。

的定义为：

其中，

为正的常数，又称切向反应系数。

第k个禁飞区的径向法向量为

在垂直于n_k(p₁)的切平面S内，定义如下两个正交的切向量：

以t_k,1(p₁)，t_k,2(p₁)和n_k(p₁)分别为x′轴，y′轴和z′轴建立坐标系O′x′y′z′。在该坐标系中，切平面S上的任意单位切向量可表示为：

t′_k(p₁)＝[cosθ_k,sinθ_k,0]^T (14)

其中，θ_k∈[-π,π]为t′_k(p₁)和x′轴之间的夹角，称其为切向方向系数。

t′_k(p₁)在地面系中表示为t_k(p₁)，后者可通过如下坐标变化得到：

t_k(p₁)＝T_kt′_k(p₁) (15)

其中，T_k为由O′x′y′z′到地面系的变换矩阵。

步骤九、当前无人机为跟随者，根据领导者的运动信息和梯度估计信息，计算跟随者的速度，执行队形保持；

当前跟随者队形保持的速度，计算公式为：

其中，k₃＞0为常量，u_j(p_j)为第j架跟随者的速度；

为领导者实际的规划速度，d_j为编队误差向量，其定义为：

d_j(t)＝p_j(t)-p₁(t)+σ_j(t) (17)

式中，p_j(t)为第j架无人机的位置向量；σ_j(t)为当前跟随者与领导者之间期望的相对位置向量，其定义为：

l_j1为期望相对距离在水平面内梯度投影方向上的分量；l_j2为期望相对距离在水平面内正交梯度投影方向上的分量；l_j3为期望相对距离在铅垂方向上的分量；

步骤十、对所得到的当前无人机期望速度进行运动学约束，得到各无人机实际的规划速度；

假设V_t+Δt，χ_t+Δt和γ_t+Δt为t+Δt时刻规划得到的运动变量。对于领导者和跟随者，V_t+Δt分别为巡航速度V₀和||u_j(p_j)||，χ_t+Δt和γ_t+Δt可表示为：

设ΔT为采样步长，

和

可表示为：

根据无人机的最大转弯率和最大爬升角速率分别对

和

进行限制，该过程表示为：

将

和约束后

与

的代入到无人机的位置计算公式(1)中，得到需用过载n_i(i＝x,y,z)。对需用过载进行约束，有：

将约束后的需用过载作为控制输入再代入到公式(1)中，得到实际的V,χ和γ，即实际的规划速度

步骤十一、利用各无人机实际的规划速度，计算各自下一步期望的航路点；

下一步期望航路点的表达式为：

步骤十二：令i＝i+1，返回步骤五，重复进行下一架无人机自主源搜索航路规划。

本发明的优点在于：

(1)本发明一种基于无人机协同对峙监视的有害物扩散源搜索方法，系统地考虑了三维有害物场的动态特性和多约束条件，从而极大地提高了无人机在执行源搜索任务时的航路平滑性和飞行安全性；

(2)本发明一种基于无人机协同对峙监视的有害物扩散源搜索方法，不需要事先已知有害物场模型参数，更贴近于实际应用；

(3)本发明一种基于无人机协同对峙监视的有害物扩散源搜索方法，具有较低的计算成本和较好的实时性，易于工程实现。

附图说明

图1是本发明基于无人机协同对峙监视的有害物扩散源搜索方法的整体流程图；

图2是本发明基于无人机协同对峙监视的有害物扩散源搜索方法中无人机期望队形示意图；

图3是本发明实施例中采取传统梯度爬升法的无人机航路图；

图3(a)是三维航路；图3(b)是航路在水平面投影；图3(c)是航路在铅垂面投影；

图4是本发明实施例中采取传统梯度爬升法的领导者-有害物扩散源距离图；

图5是本发明实施例中采取协同对峙监视的无人机航路图；

图5(a)是三维航路；图5(b)是航路在水平面投影；图5(c)是航路在铅垂面投影；

图6是本发明实施例中采取协同对峙监视的领导者-有害物扩散源距离图；

图7是本发明实施例中采取协同对峙监视的跟随者编队误差图；

图8是本发明实施例中采取协同对峙监视的无人机沿航迹系各轴过载图；

图8(a)表示沿航迹系x轴过载；图8(b)表示沿航迹系y轴过载；图8(c)表示沿航迹系z轴过载；

图9是本发明实施例中分别采取传统方法与所提出方法时的平均危险浓度有害物累积暴露量对比图。

具体实施方式

下面将结合附图和实施例对本发明作进一步的详细说明。

本发明提出了一种基于无人机协同对峙监视的有害物扩散源搜索方法，面向具有复杂动态特性的三维有害物场，由多无人机协同以估计有害物场梯度这一关键信息，摆脱了对有害物场先验知识的依赖，并通过对梯度估计信息的充分利用使无人机在向未知有害物扩散源不断趋近的同时满足运动学约束、禁飞区约束以及危险浓度有害物累积暴露量约束，为源搜索任务背景下无人机的航路平滑性和飞行安全性提供了坚实保障。

所述基于无人机协同对峙监视的有害物扩散源搜索方法，流程如图1所示，具体包括如下步骤：

步骤一、针对有害物扩散源，将参与源搜索任务的N_u架无人机划分为1架领导者和N_u-1架跟随者，依次编号为1,2,…,N_u；

本发明中，N_u＝4；为协同地完成源搜索任务，多无人机根据领导-跟随方法组成编队，即将某一架无人机作为领导者，其它无人机跟随领导者运动；领导者根据实际环境人为指定。

步骤二、在多架无人机执行源搜索任务的过程中，获取当前采样时刻对应的搜索任务空域的综合信息；

所述的综合信息具体包括：各架无人机的位置、地速向量和每架无人机测得的有害物浓度测量值，以及各个禁飞区的位置、形状和尺寸。

综合信息的数学描述如下所示：

定义地面系O_gx_gy_gz_g(S_g)的原点O_g为地面上某一固定点，x_g轴指向地平面的东向，y_g轴指向地平面的北向，z_g轴按右手定则铅垂向上。

在地面系S_g中，三维空间及其边界可分别表示为

和

由单个有害物扩散源所产生的动态有害物场可由如下三维对流扩散偏微分方程描述：

其边界条件为：

初始条件为：

c(p,t₀)＝0 (3)

其中，c(p,t)为时刻t时位置p＝[x,y,z]^T处的有害物浓度；v^T(p,t)＝[v_x,v_y,v_z]^T为环境中介质的流速，v_x，v_y和v_z表示沿S_g各轴的对流系数；

为c(p,t)的梯度；

为

的散度，且D_x，D_y和D_z表示沿S_g各轴的扩散系数；源项S(p,t)可表示为：

其中，Q为有害物释放率，p_s＝[x_s,y_s,z_s]^T为有害物扩散源位置。

定义航迹系O_kx_ky_kz_k(S_k)的原点O_k为无人机质心，x_k轴指向无人机的地速方向，z_k轴在通过x_k轴的铅垂平面内垂直于x_k轴并指向上，y_k轴垂直于O_kx_kz_k平面并指向左。

假设飞行控制系统可以保证无人机自身姿态与速度的稳定性，则可以将无人机视为三自由度质点，其运动模型为：

其中，p_u＝[x_u,y_u,z_u]^T为无人机在地面系S_g中的三维位置向量，V，χ和γ分别为无人机的地速大小、航迹偏角和航迹倾角，n_x，n_y和n_z分别为无人机沿航迹系S_k三轴的过载即控制输入，g＝9.8m/s²为重力加速度。

无人机所受的运动学约束可表示为：n_x∈[n_xmin,n_xmax]，n_y∈[n_ymin,n_ymax]，n_z∈[n_zmin,n_zmax]，V∈[V_min,V_max]，z∈[z_min,z_max]，γ∈[γ_min,γ_max],

以及

每架无人机都装备有逐点式传感器，用以测量有害物浓度，测量值的表达式为：

f(p_u,t)＝∫_Ωδ(p-p_u)c(p,t)dp＝c(p_u,t) (6)

其中，δ(p-p_u)为单位脉冲函数，有：

且满足∫_Ωδ(p-p_u)＝1。

考虑到多无人机协同，假设每一对无人机之间可进行双向通信。

任务空域中的禁飞区包括：火灾导致的高温区和重要资产上方空域等，将其视为虚拟障碍物，表达式为：

其中，p_o＝[x_o,y_o,z_o]^T为禁飞区中心在S_g中的位置向量，a，b，c和p，q，r分别为决定禁飞区大小和形状的参数。禁飞区的典型形状包括长方体(p＞1，q＞1，r＞1)和圆柱(a＝b，p＝q＝1，r＞1)等。Γ(x,y,z)＝1为禁飞区表面方程，Γ(x,y,z)＜1与Γ(x,y,z)＞1分别表示禁飞区的内部区域和外部区域。

步骤三、针对当前采样时刻领导者带领跟随者一同飞行，判断源搜索任务是否完成，如果是，则算法结束；否则，进入步骤四；

当领导者到达有害物扩散源的半径为R_s的球形领域内时，源搜索过程结束。R_s取决于无人机的感知能力，如光电平台的探测距离等。

步骤四、利用最小二乘法进行N_u架无人机协同梯度的估计；

无人机可以沿其所在位置的正梯度方向运动，从而到达有害物扩散源处，因此梯度信息在源搜索中具有关键性作用。然而，由于传感器性能限制，无人机不能直接测量梯度信息。因此，本发明中基于最小二乘法使多无人机协同获取梯度信息的估计值。

无人机编队的中心为

假设f_i(t),i＝1,2,…,N_u为多架无人机的浓度测量值，则测量值向量f(t)可表示为：

f(t)＝[f₁(t),f₂(t),…,f_Nu(t)]^T (9)

测量值估计

可由以下线性化模型表示：

其中，P(t)＝[p₁(t),p₂(t),…,p_n(t)]^T，1是元素都为1的N_u维列向量，θ(t)为表示估计参数的(d+1)维列向量。估计误差的定义为

与f(t)之差，其模值在以下等式成立时达到最小：

其中，[P(t),1]⁺为[P(t),1]的伪逆。因此，最小二乘意义下的最优梯度估计为：

式中，

为在时刻t时编队中心p_c(t)处的估计梯度，I为(d×d)维单位矩阵，0是元素都为0的d维列向量。

步骤五、逐个选择各无人机i，判断i≤N_u是否成立；如果是，则执行步骤六；否则，返回步骤二，进行下一采样时刻的无人机协同源搜索；

初始值i＝1，无人机为领导者；

步骤六、判断第i架无人机是否为领导者；如果是，则执行步骤七；否则，进入步骤九；

步骤七、当前无人机为领导者，利用梯度估计信息，执行对峙监视搜索，控制领导者的速度，使领导者收敛至对应于危险阈值的有害物场的边界，并沿该边界向趋近于有害物扩散源的方向运动直至到达有害物扩散源的领域内。

在获取梯度估计信息后，理论上而言，无人机可沿正梯度方向运动从而不断向源趋近。然而，在实际应用特别是三维空间问题中，实施该方法可能会存在一系列困难。首先，由于无人机的运动学约束和有害物场的随机分布特性，梯度方向可能难以跟踪；其次，无人机在沿正梯度方向运动时会进入有害物浓度更高的区域，而当有害物浓度超过危险阈值时无人机的飞行安全会受到严重威胁。可见，仅依靠梯度信息不足以保证航路平滑性和飞行安全性。受到水平集跟踪方法的启发，本发明引入正交梯度信息作为源搜索的引导信息之一。

有害物场对应于某一浓度水平时的轮廓或边界可用水平集表示。通常情况下，水平集是有界且闭合的。称不影响正常工作时无人机所能承受的最大浓度值C_L为危险阈值，则对应于危险阈值的水平集可定义为：

L_s(c,c_L)＝{p_L∈R^d|c(p_L,t)＝c_L} (13)

称该水平集为危险浓度区。在有害物浓度意义上，无人机相对于危险浓度区的跟踪误差为：

e(p_i,t)＝f(p_i,t)-c_L (14)

尽管有害物扩散源的位置未知，但由于无人机飞行高度通常较高，可假设初始时刻领导者位于有害物扩散源的上方。在对峙监视中，领导者收敛至危险浓度区的边界，并沿该边界向下运动直至到达有害物扩散源的领域内。该行为可表示为：

其中，u₁(p₁)为S_g下领导者的期望速度；k₁＞0为常量；e(p₁,t)为领导者的水平集跟踪误差；V₀为领导者的巡航速度；

为

在水平面上的投影；

在水平面内且正交于

有

||·||表示向量的模；k₂＞0为决定下降速率的常数。

的表达式为：

本发明中，取

当编队较为紧密时，编队中心接近于领导者，有

和

分别近似于

和

需要说明的是，在式(15)中等式右端共包含三项：梯度项、正交梯度项和源趋近项。

其中，梯度项驱动无人机沿梯度的水平投影向期望的水平集运动，其幅值与e(p₁,t)成比例。当e(p₁,t)＞0时，领导者位于期望的有害物场边界内，因此应该沿负梯度方向运动以到达边界上，反之同理；正交梯度项驱动领导者沿有害物场边界运动，当e(p₁,t)足够小时，领导者沿边界的运动速度趋于巡航速度V₀。此外，该项的引入避免了领导者直接跟随梯度方向运动，从而降低了航路跟踪的难度；源趋近项使领导者向有害物扩散源趋近。

定义危险浓度有害物累积暴露量为正水平集跟踪误差的积分值。对峙监视的优势在于，避免了领导者在源搜索过程中直接进入危险浓度区，从而尽可能地减少危险浓度有害物累积暴露量。

步骤八、在对峙监视搜索的基础上，领导者执行禁飞区规避，得到规避后的期望速度；然后进入步骤十；

本发明中，将任务区域中的禁飞区视为虚拟障碍物，并结合扰动流体动态系统(interfered fluid dynamical system，IFDS)算法使无人机规避禁飞区。IFDS算法基于自然界中流水避石现象的启发，共包括汇流和扰流两部分。

传统的IFDS算法中，汇流方向u(p₁)指向目标点。本发明中，取u(p₁)＝u₁(p₁,t)。

设任务区域中存在N_o个禁飞区。禁飞区对当前期望速度的影响可量化表示为扰动矩阵

有：

其中，ω_k(p₁)表示第k个禁飞区的权重系数，其大小主要取决于无人机到该禁飞区表面的距离。一般而言，距离越大，权重系数越小，即该禁飞区对流场的影响越小。ω_k(p₁)的表达式为：

式中，Γ_i(p₁)表示第i个禁飞区。

第k个禁飞区的径向法向量为

在垂直于n_k(p₁)的切平面S内，定义如下两个正交的切向量：

以t_k,1(p₁)，t_k,2(p₁)和n_k(p₁)分别为x′轴，y′轴和z′轴建立坐标系O′x′y′z′。在该坐标系中，切平面S上的任意单位切向量可表示为：

t′_k(p₁)＝[cosθ_k,sinθ_k,0]^T (21)

其中，θ_k∈[-π,π]为t′_k(p₁)和x′轴之间的夹角，称其为切向方向系数。

t′_k(p₁)在地面系中表示为t_k(p₁)，后者可通过如下坐标变化得到：

t_k(p₁)＝T_kt′_k(p₁) (22)

其中，T_k为由O′x′y′z′到地面系的变换矩阵。

综上，第k个禁飞区的扰动矩阵的定义为：

其中，I为(d×d)维单位矩阵，又称吸引矩阵。ρ_k的定义为：

式中，

为正的常数，又称排斥反应系数；||p₁,O_k||为从无人机到第k个禁飞区表面的距离。

σ_k的定义为：

其中，

为正的常数，又称切向反应系数。

使用扰动矩阵修正领导者当前期望速度，得到考虑禁飞区规避后的修正期望速度为：

步骤九、当前无人机为跟随者，根据领导者的运动信息和梯度估计信息，计算跟随者的速度，执行队形保持；

为协同估计三维有害物场的梯度，无人机编队的队形设计为正四面体，如图2所示。其中，作为领导者，无人机1沿危险浓度区边界飞行，而跟随者均在该边界外以保证飞行安全。

队形保持算法为：

其中，

为经过运动学约束后最终的领导者期望速度，k₃＞0为常量，d_j为编队误差向量，其定义为：

d_j(t)＝p_j(t)-p₁(t)+σ_j(t) (28)

式中，σ_j(t)为当前跟随者与领导者之间期望的相对位置向量，其定义为：

r＞0为领导者和跟随者之间的期望相对距离。本发明中，

l₁₃＝0；

l₂₃＝0；

需要说明的是，由于无人机在进行协同梯度估计时需保持密集编队，r远小于禁飞区的尺寸。因此，当源搜索算法的参数

取值合理时，跟随者可通过队形保持来规避禁飞区。以上参数可通过滚动时域控制等方法进行优化。

步骤十一、对所得到的当前无人机期望速度进行运动学约束，得到各无人机实际的规划速度；

考虑到航路可飞性的要求，应引入无人机的运动学约束，并将约束后的过载作为控制输入。假设V_t+Δt，χ_t+Δt和γ_t+Δt为t+Δt时刻规划得到的运动变量。对于领导者和跟随者，V_t+Δt分别为巡航速度V₀和||u_j(p_j)||，χ_t+Δt和γ_t+Δt可表示为：

设ΔT为采样步长，

和

可表示为：

根据无人机的最大转弯率和最大爬升角速率分别对

和

进行限制，该过程可表示为：

将

和约束后

与

的代入到无人机的位置计算公式(5)中，可计算得到需用过载n_i(i＝x,y,z)。对需用过载进行约束，有：

将约束后的需用过载作为控制输入再代入到无人机的位置计算公式(5)中，可计算得到最终的V,χ和γ，即最终的规划速度

步骤十一、利用各无人机实际的规划速度，计算得到各无人机下一步期望航路点；

下一步期望航路点的表达式为：

步骤十二：令i＝i+1，返回步骤五，重复进行下一架无人机自主源搜索航路规划。

实施例

采用结合了IFDS与运动学约束后的梯度爬升法的无人机源搜索效果，无人机航路如图3所示，三维航路如图3(a)所示，航路在水平面投影如图3(b)所示，航路在铅垂面投影如图3(c)所示。图中阴影部分为动态变化的危险浓度区。考虑到无人机安全区域半径和禁飞区大小，已对禁飞区进行膨胀处理，即只要无人机质心在禁飞区外，则认为无人机安全。

领导者-有害物扩散源距离如图4所示。可见，无人机可在规避禁飞区的同时完成源搜索任务，但由于无人机的运动学约束和有害物场的随机时空分布特性，梯度方向难以跟踪，航路十分曲折复杂；此外，由于无人机直接向有害物浓度更高的区域运动，无人机长时间位于危险浓度区内，危险浓度有害物累积暴露量不受控制地快速增长，对飞行安全构成严重威胁。

采用本发明方法的无人机源搜索效果中无人机航路如图5所示，三维航路如图5(a)所示，航路在水平面投影如图5(b)所示，航路在铅垂面投影如图5(c)所示。领导者-有害物扩散源距离如图6所示。可见，无人机能在规避禁飞区的同时完成源搜索任务。此外，无人机在危险浓度区的边界上或边界外飞行而非直接进入危险浓度区，所规划航路相较于传统方法十分规则，且危险浓度有害物累积暴露量的增长受到控制。

跟随者编队误差如图7所示。编队误差在初始时刻较大，然后快速收敛到零附近。当危险浓度区的边界变化距离时编队误差出现局部极大值。无人机沿航迹系各轴过载如图8所示。各架无人机的各轴过载均处于约束的范围内。

分别采取传统方法与本发明方法时的平均危险浓度有害物累积暴露量对比如图9所示。由于源搜索所用时间不同，为进行对比，假设源搜索过程结束后平均累积暴露量保持不变。可见，采用本发明方法可极大地减少平均累积暴露量。

综合以上仿真结果可见，本发明能在满足运动学约束、禁飞区约束和危险浓度有害物累积暴露量约束的条件下实现无人机源搜索。从航路平滑性与飞行安全性两个方面而言，所提出方法均远优于传统的梯度爬升法。

Claims (6)

1.一种基于无人机协同对峙监视的有害物扩散源搜索方法，其特征在于，具体包括如下步骤：

步骤一、针对有害物扩散源，将参与搜索任务的N_u架无人机划分为1架领导者和N_u-1架跟随者，并顺序编号；

步骤二、在N_u架无人机执行源搜索任务的过程中，获取当前采样时刻对应的搜索任务空域的综合信息；

所述的综合信息具体包括：各架无人机的初始位置、地速向量和每架无人机测得的有害物浓度值，以及各个禁飞区的位置、形状和尺寸；

步骤三、当前采样时刻下领导者带领跟随者一同飞行，判断领导者是否到达半径为R_s的有害物扩散源球形领域内，如果是，此时领导者的感知能力足以识别出有害物扩散源的位置，源搜索任务完成，算法结束；否则，进入步骤四；

步骤四、利用最小二乘法进行N_u架无人机协同梯度的估计；

最优梯度估计为：

式中，

为时刻t对应的编队中心p_c(t)处的估计梯度，I为(d×d)维单位矩阵，0是元素都为0的d维列向量，θ(t)为代表估计参数的(d+1)维列向量；

步骤五、逐个选择各无人机i，判断i≤N_u是否成立；如果是，则执行步骤六；否则，返回步骤二，进行下一采样时刻的无人机协同源搜索；

初始值i＝1，无人机为领导者；

步骤六、判断第i架无人机是否为领导者；如果是，则执行步骤七；否则，进入步骤九；

步骤七、利用梯度估计信息，执行对峙监视搜索，控制领导者的速度，使领导者收敛至对应于危险阈值的有害物场的边界，并沿该边界向趋近于有害物扩散源的方向运动直至到达有害物扩散源的领域内；

步骤八、在对峙监视搜索的基础上，领导者执行禁飞区规避，得到规避后的期望速度；然后进入步骤十；

设u(p₁)＝u₁(p₁,t)，任务区域中存在N_o个禁飞区；

使用扰动矩阵修正领导者当前期望速度，得到考虑禁飞区规避后的修正期望速度为：

扰动矩阵

的表达式为：

其中，ω_k(p₁)表示第k个禁飞区的权重系数，其表达式为：

式中，Γ_i(p₁)表示第i个禁飞区；

第k个禁飞区的扰动矩阵的定义为：

其中，I为d×d维单位矩阵，又被称为吸引矩阵；ρ_k的定义为：

式中，

为正常数，又称排斥反应系数；||p₁,O_k||为从无人机到第k个禁飞区表面的距离；

的定义为：

其中，

为正的常数，又称切向反应系数；

第k个禁飞区的径向法向量为

在垂直于n_k(p₁)的切平面S内，定义如下两个正交的切向量：

以t_k,1(p₁)，t_k,2(p₁)和n_k(p₁)分别为x′轴，y′轴和z′轴建立坐标系O′x′y′z′；在该坐标系中，切平面S上的任意单位切向量可表示为：

t′_k(p₁)＝[cosθ_k,sinθ_k,0]^T

其中，θ_k∈[-π,π]为t′_k(p₁)和x′轴之间的夹角，称其为切向方向系数；

t′_k(p₁)在地面系中表示为t_k(p₁)，后者可通过如下坐标变化得到：

t_k(p₁)＝T_kt′_k(p₁)

其中，T_k为由O′x′y′z′到地面系的变换矩阵；

步骤九、当前无人机为跟随者，根据领导者的运动信息和梯度估计信息，计算跟随者的速度，执行队形保持；

步骤十、对所得到的当前无人机期望速度进行运动学约束，得到各无人机实际的规划速度；

步骤十一、利用各无人机实际的规划速度，计算各自下一步期望的航路点；

下一步期望航路点的表达式为：

步骤十二：令i＝i+1，返回步骤五，重复进行下一架无人机自主源搜索航路规划。

2.如权利要求1所述的一种基于无人机协同对峙监视的有害物扩散源搜索方法，其特征在于，所述步骤一中领导者无人机根据实际环境人为指定。

3.如权利要求1所述的一种基于无人机协同对峙监视的有害物扩散源搜索方法，其特征在于，所述步骤二中无人机的位置信息计算公式为：

其中，p_u＝[x_u,y_u,z_u]^T为无人机在地面系S_g中的三维位置向量，V，χ和γ分别为无人机的地速大小、航迹偏角和航迹倾角，n_x，n_y和n_z分别为无人机沿航迹系S_k三轴的过载即控制输入，g＝9.8m/s²为重力加速度；

无人机所受的运动学约束表示为：

n_x∈[n_xmin,n_xmax]，n_y∈[n_ymin,n_ymax]，n_z∈[n_zmin,n_zmax]，V∈[V_min,V_max]，z∈[z_min,z_max]，γ∈[γ_min,γ_max],

以及

每架无人机都装有逐点式传感器，用以测量有害物浓度，测量值的表达式为：

f(p_u,t)＝c(p_u,t)

其中，c(p_u,t)为时刻t时位置p_u＝[x_u,y_u,z_u]^T处的有害物浓度；

禁飞区信息的表达式为：

其中，p_o＝[x_o,y_o,z_o]^T为禁飞区中心在S_g中的位置向量，a，b，c为决定禁飞区大小的参数；p，q，r为决定禁飞区形状的参数；

禁飞区的典型形状包括长方体(p＞1，q＞1，r＞1)和圆柱(a＝b，p＝q＝1，r＞1)；Γ(x,y,z)＝1为禁飞区表面方程，Γ(x,y,z)＜1与Γ(x,y,z)＞1分别表示禁飞区的内部区域和外部区域。

4.如权利要求1所述的一种基于无人机协同对峙监视的有害物扩散源搜索方法，其特征在于，所述步骤七中危险阈值为不影响正常工作时无人机所能承受的最大浓度值；

领导者的速度表示为：

其中，u₁(p₁)为地面坐标系S_g下领导者的期望速度；k₁＞0为常量；e(p₁,t)为领导者的水平集跟踪误差；V₀为领导者的巡航速度；

为估计梯度

在水平面上的投影；

在水平面内且正交于

有

k₂＞0为决定下降速率的常数。

5.如权利要求1所述的一种基于无人机协同对峙监视的有害物扩散源搜索方法，其特征在于，所述步骤九当前跟随者队形保持的速度，计算公式为：

其中，k₃＞0为常量，u_j(p_j)为第j架跟随者的速度；

为领导者实际的规划速度，d_j为编队误差向量，其定义为：

d_j(t)＝p_j(t)-p₁(t)+σ_j(t)

式中，p_j(t)为第j架无人机的位置向量；σ_j(t)为当前跟随者与领导者之间期望的相对位置向量，其定义为：

l_j1为期望相对距离在水平面内梯度投影方向上的分量；l_j2为期望相对距离在水平面内正交梯度投影方向上的分量；l_j3为期望相对距离在铅垂方向上的分量。

6.如权利要求1所述的一种基于无人机协同对峙监视的有害物扩散源搜索方法，其特征在于，所述步骤十具体为：

假设V_t+Δt，χ_t+Δt和γ_t+Δt为t+Δt时刻规划得到的运动变量；对于领导者和跟随者，V_t+Δt分别为巡航速度V₀和||u_j(p_j)||，χ_t+Δt和γ_t+Δt可表示为：

设ΔT为采样步长，

和

可表示为：

根据无人机的最大转弯率和最大爬升角速率分别对

和

进行限制，该过程表示为：

将

和约束后

与

的代入到无人机的位置计算公式中，得到需用过载n_i(i＝x，y，z)；对需用过载进行约束，有：

将约束后的需用过载作为控制输入再代入到无人机的位置计算公式中，得到实际的V,χ和γ，即实际的规划速度

Images