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

摘要

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

主权项

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 {{\psi }^i}_k& \sin {{\psi }^i}_k& 0\\ \cos {{\gamma }^i}_k\sin {{\psi }^i}_k& -\cos {{\gamma }^i}_k{\cos {\psi }^i}_k& -\sin {{\gamma }^i}_k\\ -\sin {{\gamma }^i}_k\sin {{\psi }^i}_k& \sin {{\gamma }^i}_k\cos {{\psi }^i}_k& -\cos {{\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[{\bar{Q}}_m^i{({\bar{Q}}_m^i+{{\bar{Q}}_s}^j)}^{-1}{{\bar{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×ω_maxx_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＝0f(x)＝ε；x≥R_comm，0＜ε□0.1f′(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₀，计算此时的航向增量：${\lambda }_{\mathrm{urgent}}\left\{\begin{array}{c}{-\omega }_{\max },{\xi }_0\le {-\omega }_{\max }\\ {\xi }_0,{-\omega }_{\max }<{\xi }_0\le {\omega }_{\max }\\ {\omega }_{\max },{\xi }_0>{\omega }_{\max }\end{array}\right.,$若ψ_s+λ_urgent不在漏斗区间[δ_min，δ_max]内，转步骤4；否则，僚机下一时刻的航向角增加λ_urgent，即：ψ_s：＝ψ_s+λ_urgentx_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×ω_maxx_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 (\lambda -{\delta }_{\min }),\cos (\lambda -{\delta }_{\max })\}$对应于k_max的j记为j_min，则僚机下一时刻的航向角增加j_min×ω_max，即：ψ_s：＝ψ_s+j_min×ω_maxx_s：＝x_s+vΔt[cosψ_s，sinψ_s，0]如果达到设定的跟踪终止条件，跟踪过程结束；否则返回步骤1。