CN102081404A - 一种通信约束下双无人机协同目标跟踪方法 - Google Patents

Abstract

本发明公开了一种通信约束下双无人机协同目标跟踪方法，控制两架无人机协同地跟踪一个地面目标；跟踪过程中，长机判断自身进入强制接近状态后，迅速重新满足探测距离约束的要求，若未进入强制接近状态，以精度优先，提高目标观察数据的精度；僚机以强制接近状态和以及提高精度的原理与长机相同，僚机的活动范围被限制在以两机连线为中心的一个小范围内，以足够的速度分量去追踪长机，从而确保长机和僚机之间的距离小于给定值。本发明能够确保通信距离约束得到满足，为协同提供通信保障；能够确保长机的探测距离约束得到满足，为目标跟踪提供保证；能够在飞行中实时分析跟踪精度并优化轨迹，为跟踪质量提供保证。

Description

一种通信约束下双无人机协同目标跟踪方法

技术领域

本发明属于双无人机自治跟踪一个地面活动目标时的飞行控制方法，涉及目标估计和飞行控制领域。

背景技术

自治跟踪运动目标是无人机的一种新颖用途，具有广阔的应用前景，如：动物迁徙研究，体育赛事实况转播，紧急救援，物资空投指引等。Steven A.P.Quintero，Francesco Papi，Daniel J.Klein，Luigi Chisci和Joao P.Hespanha提出的“使用动态规划的最优无人机目标跟踪协同算法”(Optimal UAV Coordination for Target Tracking using Dynamic Programming)运用动态规划原理，为无人机跟踪地面目标提供了一种跟踪精度全局最优的方法。该算法首先给出了无人机跟踪目标时对目标进行定位的精度分析方法；在此基础上，将无人机在每一个决策周期中的航向角改变量离散化，以精度最高为规划目标，使用动态规划原理逆向求解得到精度全局最优的跟踪轨迹。

由于该算法是全局最优方法，需要在跟踪之前先验地知道无人机在整个运动过程中每时每刻的位置，这一点不具有可行性；并且这种算法没有提出对通信距离约束(通信距离约束指无人机之间能够进行通信的最大距离限制)的保证措施，而保证通信是协同的基本要素；虽然文中采用的精度优化能够间接调整与目标之间的距离，但是仅仅通过调控精度对无人机与目标之间的距离间接施加影响并没有足够可靠的约束力，而保证探测距离约束(探测距离约束指无人机能够有效探测目标参数的最大距离限制)是目标跟踪的首要前提。

发明内容

为了克服现有技术不能满足通信距离约束和探测距离约束的不足，本发明提供一种通信约束下双无人机协同目标跟踪方法，能够控制两架无人机(一架长机和一架僚机)协同地跟踪一个地面目标；跟踪过程中，能够确保通信距离约束得到满足，为协同提供通信保障；能够确保长机的探测距离约束得到满足，为目标跟踪提供保证；能够在飞行中实时分析跟踪精度并优化轨迹，为跟踪质量提供保证。

本发明解决其技术问题所采用的技术方案是：假设算法的执行周期是Δt，目标、长机和僚机在惯性系中的三维坐标分别是x_t、x_m和x_s，长机和僚机航向角分别为ψ_m和ψ_s，速率均恒定为v，传感器最大探测距离为r_max，长机、僚机到目标的距离分别是r₁、r₂，v^⊥ _t与v^- _t、v^⊥ ₁与v^- ₁、v^⊥ ₂与v^- ₂分别是目标、长机和僚机速度在无人机-目标连线上及其垂直方向上的投影，并引入布尔型标志变量flag_m＝0和flag_s＝0。若没有接收到退出命令或任务没有完成，长机和僚机分别各自重复执行以下步骤：

1)长机执行以下步骤：

步骤1：计算目标冲出长机探测范围所需时间t_r＝(r_max-r₁)×(v^- _t-v^- ₁)^-1，

估算的无人机调整姿态至对准目标时无人机-目标连线转过的角度Δ＝(0.5v^⊥ _t-v^⊥ ₁)t_r/r₁，

中间变量ξ＝±acos(v_tcos(η_t+Δ)/v₁)+2kπ-η₁-Δ，k＝0，±1，

估算的无人机机动到能够接近目标时需要转过的航向角ξ₀＝sign(ξ)min(|ξ|)，

计算目标转向减小与目标距离方向所需时间t_n＝|ξ₀|/ω_max；

步骤2：若|Δ|＜π并且r₁＞D₀并且t_n＞t_r，长机进入强制接近状态，令flag_m＝1；否则直接转到步骤3。

步骤3：计算此时的航向增量λ_urgent：

${\mathrm{\&lambda;}}_{\mathrm{urgent}}=\left\{\begin{array}{c}-{\mathrm{\&omega;}}_{\max },{\mathrm{\&xi;}}_0\&le;-{\mathrm{\&omega;}}_{\max }\\ {\mathrm{\&xi;}}_0,-{\mathrm{\&omega;}}_{\max }<{\mathrm{\&xi;}}_0\&le;{\mathrm{\&omega;}}_{\max }\\ {\mathrm{\&omega;}}_{\max },{\mathrm{\&xi;}}_0>{\mathrm{\&omega;}}_{\max }\end{array}\right.,$

若flag_m＝1且r₁＞D₀，长机下一时刻的航向角增加λ_urgent，即：

ψ_m：＝ψ_m+λ_urgent，x_m：＝x_m+vΔt[cosψ_m，sinψ_m，0]，

然后返回步骤1；若flag_m＝1且r₁≤D₀，则令flag_m＝0，并转到步骤4；若flag_m＝0，转到步骤4。参数D₀(D₀＜r_max)用来调节步骤2的灵敏度，D₀越大则步骤2执行越频繁；建议D₀在r_max的0.2至0.8倍之间取值。

步骤4：当临时变量i，j分别取{-1，0，1}时，计算不同i，j组合下的tr值：

ψⁱ _m＝ψ_m+i×ω_max；ψ^j _s＝ψ_s+j×ω_max；

${T^i}_{0k}=\left[\begin{array}{ccc}\cos {{\mathrm{\&psi;}}^i}_k& \sin {{\mathrm{\&psi;}}^i}_k& 0\\ \cos {{\mathrm{\&gamma;}}^i}_k\sin {{\mathrm{\&psi;}}^i}_k& -\cos {{\mathrm{\&gamma;}}^i}_k{\cos {\mathrm{\&psi;}}^i}_k& -\sin {{\mathrm{\&gamma;}}^i}_k\\ -\sin {{\mathrm{\&gamma;}}^i}_k\sin {{\mathrm{\&psi;}}^i}_k& \sin {{\mathrm{\&gamma;}}^i}_k\cos {{\mathrm{\&psi;}}^i}_k& -\cos {{\mathrm{\&gamma;}}^i}_k\end{array}\right];k=m,s;$

Q_k ⁱ＝‖r_{t_k}‖²Tⁱ _0k ^TM_k ^TP_kM_kTⁱ _0k；k＝m，s；

$\mathrm{tr}{|}_{i,j}=\mathrm{trace}\left[{\stackrel{\&OverBar;}{Q}}_m^i{({\stackrel{\&OverBar;}{Q}}_m^i+{{\stackrel{\&OverBar;}{Q}}_s}^j)}^{-1}{{\stackrel{\&OverBar;}{Q}}_s}^j\right];$

其中，θ_m和θ_s分别为长机和僚机传感器的俯仰角，

和

分别为长机和僚机传感器的方位角，r_{t_m}和r_{t_s}分别为长机和僚机指向目标的向量，这些参数由机载传感器提供；Q_m ⁱ和Q_s ⁱ分别为长机和僚机各自对目标定位的协方差阵，Tⁱ _0m和M_m是计算Q_m ⁱ时的系数矩阵；Tⁱ _0s和M_s是计算Q_s ⁱ时的系数矩阵；

和

分别表示矩阵Qⁱ _m和Q^j _s左上角的2×2子块；γⁱ _k＝-sign(i)γ₀，γ₀为最大转弯滚转角。

在所有的tr中搜索得到tr的最优值tr_opt，以及对应的i_opt，j_opt。如下式所示：

${\mathrm{tr}}_{\mathrm{opt}}=\underset{i,j=\{-\mathrm{1,0,1}\}}{\min }\left(\mathrm{tr}{|}_{i,j}\right)=\mathrm{tr}{|}_{i_{\mathrm{opt}},j_{\mathrm{opt}}}$

长机下一时刻的航向角增加i_opt×ω_max，即：

ψ_m：＝ψ_m+i_opt×ω_max

x_m：＝x_m+vΔt[cosψ_m，sinψ_m，0]

如果达到人为设定的跟踪终止条件(如跟踪时间、位置等)，跟踪过程结束；否则返回步骤1。

2)僚机执行以下步骤：

步骤1：计算目标冲出僚机探测范围所需时间t_r：

t_r＝(r_max-r₂)×(v^- _t-v^- ₂)^-1

依次计算：

Δ＝(0.5v^⊥ _t-v^⊥ ₂)t_r/r₂；

ξ＝±acos(v_tcos(η_t+Δ)/v₂)+2kπ-η₂-Δ，k＝0，±1

ξ₀＝sign(ξ)min(|ξ|)

t_n＝|ξ₀|/ω_max

计算漏斗角范围[δ_min，δ_max]＝[-f(r_ms)，f(r_ms)]+[α，α]，

其中，r_ms表示长机和僚机之间的距离，α表示僚机指向长机向量的方位角。函数f满足

f(x)＝π；x＝0

f(x)＝ε；x≥R_comm，0＜ε□0.1

f′(x)＜0，f″(x)＞0；0≤x＜R_comm；

步骤2：若|Δ|＜π并且r₂＞D₀并且t_n＞t_r，僚机进入强制接近状态，令flag_s＝1；否则直接转到步骤3。

步骤3：若flag_s＝1且r₂＞D₀，计算此时的航向增量：

${\mathrm{\&lambda;}}_{\mathrm{urgent}}=\left\{\begin{array}{c}-{\mathrm{\&omega;}}_{\max },{\mathrm{\&xi;}}_0\&le;-{\mathrm{\&omega;}}_{\max }\\ {\mathrm{\&xi;}}_0,-{\mathrm{\&omega;}}_{\max }<{\mathrm{\&xi;}}_0\&le;{\mathrm{\&omega;}}_{\max }\\ {\mathrm{\&omega;}}_{\max },{\mathrm{\&xi;}}_0>{\mathrm{\&omega;}}_{\max }\end{array}\right.,$

若ψ_s+λ_urgent不在漏斗区间[δ_min，δ_max]内，转步骤4；否则，僚机下一时刻的航向角增加λ_urgent，即：

ψ_s：＝ψ_s+λ_urgent

x_s：＝x_s+vΔt[cosψ_s，sinψ_s，0]

然后返回步骤1。若flag_s＝1且r₂＜D₀，则令flag_s＝0，并转到步骤4；若flag_s＝0，转到步骤4。

步骤4：根据长机在当前时刻计算得到的j_opt，取λ＝ψ_s+j_opt×ω_max，若λ在漏斗区间[δ_min，δ_max]内，则僚机下一时刻的航向角增加j_opt×ω_max，即：

ψ_s：＝ψ_s+j_opt×ω_max

x_s：＝x_s+vΔt[cosψ_s，sinψ_s，0]

然后返回步骤1。否则，转步骤5。

步骤5：在临时变量j＝{-1，0，1}时，取λ＝ψ_s+j×ω_max，按下式求取临时变量k_max

$k_{\max }=\underset{j=\{-\mathrm{1,0,1}\}}{\max }\{\cos (\mathrm{\&lambda;}-{\mathrm{\&delta;}}_{\min }),\cos (\mathrm{\&lambda;}-{\mathrm{\&delta;}}_{\max })\}$

对应于k_max的j记为j_min，则僚机下一时刻的航向角增加j_min×ω_max，即：

ψ_s：＝ψ_s+j_min×ω_max

x_s：＝x_s+vΔt[cosψ_s，sinψ_s，0]

如果达到人为设定的跟踪终止条件(如跟踪时间、位置等)，跟踪过程结束；否则返回步骤1。

本发明的有益效果是：本发明使得长机判断自身进入强制接近状态后，采用步骤2可以迅速重新满足探测距离约束的要求，这样优先保证了探测距离约束。若未进入强制接近状态，无人机的活动范围相对自由，以精度优先的步骤3可以提高目标观察数据的精度。

僚机以强制接近状态和以及提高精度的原理与长机相同。在僚机的步骤4中漏斗函数作为最终的决定因素，保证了僚机在跟踪目标的同时，必须优先满足通信约束的要求。漏斗函数发挥作用的原理是随着僚机与长机的距离增加，漏斗角范围将缩小，僚机的活动范围被漏斗角约束限制在以两机连线为中心的一个小范围内，促使僚机以足够的速度分量去追踪长机，从而确保长机和僚机之间的距离小于给定值。

经验证，当目标做匀速直线运动时，在合适的约束参数下，算法整体上能自主维持通讯约束(通信完好率在95％以上)以及探测距离约束(探测约束完好率在90％以上)，并以较低的误差(协同时X、Y、Z轴的修正标准差分别是单机的60.99％，57.62％和58.53％)跟踪目标。

下面结合附图和实施例对本发明进一步说明。

附图说明

图1是长机控制流程图；

图2是僚机控制流程图；

图3是探测约束示意图；

图4是f(x)示意图；

图5是漏斗角示意图；

图6是实施例的仿真轨迹图；

图7是实施例的估计协方差阵的迹/无人机-目标距离曲线；

图8是实施例的无人机间距图。

具体实施方式

通过计算机仿真对本算法进行了虚拟实施；在仿真的每个周期，设定长机先飞，僚机后飞。实施步骤如下：

仿真环境设置为：目标高度为0，以18m/s的速率沿直线向东运动；长机飞行高度400m，僚机飞行高度350m；角度误差协方差矩阵均为P＝diag(0.0175²，0.0175²)rad²；停止条件为仿真时间大于600s；D₀＝900m；Δt＝1s。其它设置参见下表。

仿真设置

长机：

M1.长机计算时间t_r：

t_r＝(1800-r₁)×(v^- _t-v^- ₂)^-1

长机计算t_n：

Δ＝(0.5v^⊥ _t-v^⊥ ₁)t_r/r₁

ξ＝±acos(cos(η_t+Δ)/5)+2kπ-η₁-Δ，k＝0，±1

ξ₀＝sign(ξ)min(|ξ|)

t_n＝8×|ξ₀|

M2.若|Δ|＜π并且r₁＞900并且t_n＞t_r，则令flag_m＝1；否则直接转到步骤3。

M3.长机计算λ_urgent：

${\mathrm{\&lambda;}}_{\mathrm{urgent}}=\left\{\begin{array}{c}-1/8,{\mathrm{\&xi;}}_0\&le;-1/8\\ {\mathrm{\&xi;}}_0,-1/8<{\mathrm{\&xi;}}_0\&le;1/8\\ 1/8,{\mathrm{\&xi;}}_0>1/8\end{array}\right.$

若flag_m＝1并且r₁＞900，长机下一时刻的航向角增加λ_urgent，即：

ψ_m：＝ψ_m+λ_urgent

x_m：＝x_m+90×[cosψ_m，sinψ_m，0]

跳转至M5；若flag_m＝1且r₁＜900，则令flag_m＝0，并转到M4；若flag_m＝0，转至M4。

M4.对于i，j分别取{-1，0，1}时，长机计算不同i，j组合下的tr值：

ψⁱ _m＝ψ_m+i×1/8；ψ^j _s＝ψ_s+j×1/8；

${T^i}_{0k}=\left[\begin{array}{ccc}\cos {{\mathrm{\&psi;}}^i}_k& \sin {{\mathrm{\&psi;}}^i}_k& 0\\ \cos {{\mathrm{\&gamma;}}^i}_k\sin {{\mathrm{\&psi;}}^i}_k& -\cos {{\mathrm{\&gamma;}}^i}_k{\cos {\mathrm{\&psi;}}^i}_k& -\sin {{\mathrm{\&gamma;}}^i}_k\\ -\sin {{\mathrm{\&gamma;}}^i}_k\sin {{\mathrm{\&psi;}}^i}_k& \sin {{\mathrm{\&gamma;}}^i}_k\cos {{\mathrm{\&psi;}}^i}_k& -\cos {{\mathrm{\&gamma;}}^i}_k\end{array}\right];k=m,s$

k＝m，s

Q_k ⁱ＝‖r_{t_k}‖²T_i0k ^TM_k ^TP_kM_kTⁱ _0k；k＝m，s；

$\mathrm{tr}{|}_{i,j}=\mathrm{trace}\left[{\stackrel{\&OverBar;}{Q}}_m^i{({\stackrel{\&OverBar;}{Q}}_m^i+{{\stackrel{\&OverBar;}{Q}}_s}^j)}^{-1}{{\stackrel{\&OverBar;}{Q}}_s}^j\right]$

长机下一时刻的航向角增加i_opt×1/8，即：

ψ_m：＝ψ_m+i_opt×1/8

x_m：＝x_m+90×[cosψ_m，sinψ_m，0]

转至M5。

M5.仿真时间增加1秒。如果仿真总时间大于600秒，退出；否则转至M1。

僚机：

S1.僚机计算t_r时间：

t_r＝(1800-r₂)×(v^- _t-v^- ₂)^-1

僚机计算t_n：

Δ＝(0.5v^⊥ _t-v^⊥ ₂)t_r/r₂

ξ＝±acos(cos(η_t+Δ)/5)+2kπ-η₂-Δ，k＝0，±1

ξ₀＝sign(ξ)min(|ξ|)

t_n＝8×|ξ₀|

僚机计算漏斗角[δ_min，δ_max]：

[δ_min，δ_max]＝[-f(r_ms)，f(r_ms)]+[α，α]

其中， $f\left(x\right)=\left\{\begin{array}{c}{(x/3000-1)}^2\mathrm{\&pi;};0\&le;x<3000\\ 0.01;x\&GreaterEqual;3000\end{array}\right..$

S2.若|Δ|＜π并且r₂＞900并且t_n＞t_r，则令flag_s＝1；否则直接转到步骤3。

S3.若flag_s＝1且r₂＞900，僚机计算λ_urgent：

${\mathrm{\&lambda;}}_{\mathrm{urgent}}=\left\{\begin{array}{c}-1/8,{\mathrm{\&xi;}}_0\&le;-1/8\\ {\mathrm{\&xi;}}_0,-1/8<{\mathrm{\&xi;}}_0\&le;1/8\\ 1/8,{\mathrm{\&xi;}}_0>1/8\end{array}\right.,$

若ψ_s+λ_urgent不在漏斗区间[δ_min，δ_max]内，转至S5；否则，僚机下一时刻的航向角增加λ_urgent，即：

ψ_s：＝ψ_s+λ_urgent

x_s：＝x_s+vΔt[cosψ_s，sinψ_s，0]

然后转至S6。若flag_s＝1且r₂＜900，则令flag_s＝0，并转到S4；若flag_s＝0，转至S4。

S4.根据长机在步骤M3同步计算得到的j_opt，取λ＝ψ_s+j_opt×1/8，若λ在漏斗区间[δ_min，δ_max]内，则僚机下一时刻的航向角增加j_opt×1/8，即：

ψ_s：＝ψ_s+j_opt×1/8

x_s：＝x_s+90×[cosψ_s，sinψ_s，0]

然后转至S6；否则转至S5。

S5.在j＝{-1，0，1}时，取λ＝ψ_s+j×1/8，按下式求取k_max

$k_{\max }=\underset{j=\{-\mathrm{1,0,1}\}}{\max }\{\cos (\mathrm{\&lambda;}-{\mathrm{\&delta;}}_{\min }),\cos (\mathrm{\&lambda;}-{\mathrm{\&delta;}}_{\max })\}$

对应于k_max的j记为j_min，则僚机下一时刻的航向角增加j_min×1/8，即：

ψ_s：＝ψ_s+j_min×1/8

x_s：＝x_s+90×[cosψ_s，sinψ_s，0]

转至S6。

S6.仿真时间增加1秒。若仿真时间小于600个周期，返回至S1；否则结束。

在Windows XP环境下，使用Matlab 7.0软件仿真的结果如图6～8所示，图6是轨迹图，图7是估计协方差阵的迹/无人机-目标距离曲线(浅色虚线的高出部分表示出现强制接近状态，其高度表示最大探测距离)，图8是无人机间距图(浅色虚线的高出部分表示违反了漏斗角约束，其高度不表示通讯约束距离)。

Claims (2)

1.一种通信约束下双无人机协同目标跟踪方法，其特征在于包括下述步骤：

假设算法的执行周期是Δt，目标、长机和僚机在惯性系中的三维坐标分别是x_t、x_m和x_s，长机和僚机航向角分别为ψ_m和ψ_s，速率均恒定为v，传感器最大探测距离为r_max，长机、僚机到目标的距离分别是r₁、r₂，v^⊥ _t与v^- _t、v^⊥ ₁与v^- ₁、v^⊥ ₂与v^- ₂分别是目标、长机和僚机速度在无人机-目标连线上及其垂直方向上的投影，并引入布尔型标志变量flag_m＝0和flag_s＝0；若没有接收到退出命令或任务没有完成，长机和僚机分别各自重复执行以下步骤：

1)长机执行以下步骤：

步骤1：计算目标冲出长机探测范围所需时间t_r＝(r_max-r₁)×(v^- _t-v^- ₁)^-1，

估算的无人机调整姿态至对准目标时无人机-目标连线转过的角度Δ＝(0.5v^⊥ _t-v^⊥ ₁)t_r/r₁，

中间变量ξ＝±acos(v_t cos(η_t+Δ)/v₁)+2kπ-η₁-Δ，k＝0，±1，

估算的无人机机动到能够接近目标时需要转过的航向角ξ₀＝sign(ξ)min(|ξ|)，

计算目标转向减小与目标距离方向所需时间t_n＝|ξ₀|/ω_max；

步骤2：若|Δ|＜π并且r₁＞D₀并且t_n＞t_r，长机进入强制接近状态，令flag_m＝1；否则直接转到步骤3；

步骤3：计算此时的航向增量

若flag_m＝1且r₁＞D₀，D₀＜r_max，长机下一时刻的航向角增加λ_urgent，即：

ψ_m：＝ψ_m+λ_urgent，x_m：＝x_m+vΔt[cosψ_m，sinψ_m，0]，

然后返回步骤1；若flag_m＝1且r₁≤D₀，则令flag_m＝0，并转到步骤4；若flag_m＝0，直接转到步骤4；

步骤4：当临时变量i，j分别取{-1，0，1}时，计算不同i，j组合下的tr值：

ψⁱ _m＝ψ_m+i×ω_max；ψ^j _s＝ψ_s+j×ω_max；

${T^i}_{0k}=\left[\begin{array}{ccc}\cos {{\mathrm{\&psi;}}^i}_k& \sin {{\mathrm{\&psi;}}^i}_k& 0\\ \cos {{\mathrm{\&gamma;}}^i}_k\sin {{\mathrm{\&psi;}}^i}_k& -\cos {{\mathrm{\&gamma;}}^i}_k{\cos {\mathrm{\&psi;}}^i}_k& -\sin {{\mathrm{\&gamma;}}^i}_k\\ -\sin {{\mathrm{\&gamma;}}^i}_k\sin {{\mathrm{\&psi;}}^i}_k& \sin {{\mathrm{\&gamma;}}^i}_k\cos {{\mathrm{\&psi;}}^i}_k& -\cos {{\mathrm{\&gamma;}}^i}_k\end{array}\right];k=m,s;$

k＝m，s；

Q_k ⁱ＝‖r_{t_k}‖²Tⁱ _0k ^TM_k ^TP_kM_kTⁱ _0k；k＝m，s；

$\mathrm{tr}{|}_{i,j}=\mathrm{trace}\left[{\stackrel{\&OverBar;}{Q}}_m^i{({\stackrel{\&OverBar;}{Q}}_m^i+{{\stackrel{\&OverBar;}{Q}}_s}^j)}^{-1}{{\stackrel{\&OverBar;}{Q}}_s}^j\right];$

其中，θ_m和θ_s分别为长机和僚机传感器的俯仰角，

和

分别为长机和僚机传感器的方位角，r_{t_m}和r_{t_s}分别为长机和僚机指向目标的向量，这些参数由机载传感器提供；Q_m ⁱ和Q_s ⁱ分别为长机和僚机各自对目标定位的协方差阵，Tⁱ _0m和M_m是计算Q_m ⁱ时的系数矩阵；Tⁱ _0s和M_s是计算Q_s ⁱ时的系数矩阵；

和

分别表示矩阵Qⁱ _m和Q^j _s左上角的2×2子块；γⁱ _k＝-sign(i)γ₀，γ₀为最大转弯滚转角；

在所有的tr中搜索得到tr的最优值tr_opt，以及对应的i_opt，j_opt，如下式所示：

${\mathrm{tr}}_{\mathrm{opt}}=\underset{i,j=\{-\mathrm{1,0,1}\}}{\min }\left(\mathrm{tr}{|}_{i,j}\right)=\mathrm{tr}{|}_{i_{\mathrm{opt}},j_{\mathrm{opt}}}$

长机下一时刻的航向角增加i_opt×ω_max，即：

ψ_m：＝ψ_m+i_opt×ω_max

x_m：＝x_m+vΔt[cosψ_m，sinψ_m，0]

如果达到设定的跟踪终止条件，跟踪过程结束；否则返回步骤1；

2)僚机执行以下步骤：

步骤1：计算目标冲出僚机探测范围所需时间t_r：

t_r＝(r_max-r₂)×(v^- _t-v^- ₂)^-1

依次计算：

Δ＝(0.5v^⊥ _t-v^⊥ ₂)t_r/r₂；

ξ＝±acos(v_tcos(η_t+Δ)/v₂)+2kπ-η₂-Δ，k＝0，±1

ξ₀＝sign(ξ)min(|ξ|)

t_n＝|ξ₀|/ω_max

计算漏斗角范围[ξ_min，ξ_max]＝[-f(r_ms)，f(r_ms)]+[α，α]，其中，r_ms表示长机和僚机之间的距离，α表示僚机指向长机向量的方位角，函数f满足

f(x)＝π；x＝0

f(x)＝ε；x≥R_comm，0＜ε□0.1

f′(x)＜0，f″(x)＞0；0≤x＜R_comm；

步骤2：若|Δ|＜π并且r₂＞D₀并且t_n＞t_r，僚机进入强制接近状态，令flag_s＝1；否则直接转到步骤3；

步骤3：若flag_s＝1且r₂＞D₀，计算此时的航向增量：

${\mathrm{\&lambda;}}_{\mathrm{urgent}}\left\{\begin{array}{c}{-\mathrm{\&omega;}}_{\max },{\mathrm{\&xi;}}_0\&le;{-\mathrm{\&omega;}}_{\max }\\ {\mathrm{\&xi;}}_0,{-\mathrm{\&omega;}}_{\max }<{\mathrm{\&xi;}}_0\&le;{\mathrm{\&omega;}}_{\max }\\ {\mathrm{\&omega;}}_{\max },{\mathrm{\&xi;}}_0>{\mathrm{\&omega;}}_{\max }\end{array}\right.,$

若ψ_s+λ_urgent不在漏斗区间[δ_min，δ_max]内，转步骤4；否则，僚机下一时刻的航向角增加λ_urgent，即：

ψ_s：＝ψ_s+λ_urgent

x_s：＝x_s+vΔt[cosψ_s，sinψ_s，0]

然后返回步骤1；若flag_s＝1且r₂＜D₀，则令flag_s＝0，并转到步骤4；若flag_s＝0，转到步骤4；

步骤4：根据长机在当前时刻计算得到的j_opt，取λ＝ψ_s+j_opt×ω_max，若λ在漏斗区间[δ_min，δ_max]内，则僚机下一时刻的航向角增加j_opt×ω_max，即：

ψ_s：＝ψ_s+j_opt×ω_max

x_s：＝x_s+vΔt[cosψ_s，sinψ_s，0]

然后返回步骤1；否则，转步骤5；

步骤5：在临时变量j＝{-1，0，1}时，取λ＝ψ_s+j×ω_max，按下式求取临时变量k_max

$k_{\max }=\underset{j=\{-\mathrm{1,0,1}\}}{\max }\{\cos (\mathrm{\&lambda;}-{\mathrm{\&delta;}}_{\min }),\cos (\mathrm{\&lambda;}-{\mathrm{\&delta;}}_{\max })\}$

对应于k_max的j记为j_min，则僚机下一时刻的航向角增加j_min×ω_max，即：

ψ_s：＝ψ_s+j_min×ω_max

x_s：＝x_s+vΔt[cosψ_s，sinψ_s，0]

如果达到设定的跟踪终止条件，跟踪过程结束；否则返回步骤1。

2.根据权利要求1的一种通信约束下双无人机协同目标跟踪方法，其特征在于：所述的参数D₀在r_max的0.2至0.8倍之间取值。

Images