CN113075937A - 基于目标加速度估计的无人机捕获目标的控制方法 - Google Patents

Abstract

本发明公开了一种基于目标加速度估计的无人机捕获目标的控制方法，该方法中通过机载摄像机实时拍摄目标无人机的画面，从而获得带有目标无人机的连续图像信息，利用卷积神经网络，通过图像识别的方式获得每一帧图像上目标无人机的特征点位置，再根据特征点位置实时解算出无人机相对于摄像机的状态估计，在此基础上解算追踪无人机的控制指令，进而控制所述追踪无人机捕获目标。

Description

基于目标加速度估计的无人机捕获目标的控制方法

技术领域

本发明涉及旋翼无人机识别及追踪目标的控制方法，基于目标加速度估计的无人机捕获目标的控制方法。

背景技术

随着无人机应用的兴起，利用光学传感器进行无人机检测和姿态估计成为协同飞行和低空安全的重要研究课题。现有的技术仅基于目标检测方法获得目标无人机的位置，进而控制无人机追踪或捕获目标，然而现有技术中，对于目标位置、速度等信息的实时估计的准确性、即时性都有待提高，在获得目标的运动信息后，追踪无人机的制导控制方案一般为PID控制算法，但是实际结果并不够理想，其精度和即时性也难以满足所有的飞行任务要求。

为了获得更好的适应性和增强协同性能，目标无人机的姿态信息成为了解其状态和意图的关键信息，例如四旋翼的加速度，目前，大多数的目标6D位姿估计算法都依赖于精确的位姿标注或三维目标模型，这些算法耗费大量人力资源，难以应用于非合作目标。

然而，对于非合作目标，不可能提前在目标无人机上绘制特殊识别标记，所以需要基于无人机的一般特征进行识别和判断，但目前并没有相关领域的研究。

由于上述原因，本发明人对现有的四旋翼无人机状态估计做了深入研究，并研究新的追踪无人机控制方案，以期待设计出一种能够解决上述问题的基于目标加速度估计的无人机捕获目标的控制方法。

发明内容

为了克服上述问题，本发明人进行了锐意研究，设计出一种基于目标加速度估计的无人机捕获目标的控制方法，该方法中通过机载摄像机实时拍摄目标无人机的画面，从而获得带有目标无人机的连续图像信息，利用卷积神经网络，通过图像识别的方式获得每一帧图像上目标无人机的特征点位置，再根据特征点位置实时解算出无人机相对于摄像机的状态估计，在此基础上解算追踪无人机的控制指令，进而控制所述追踪无人机捕获目标，从而完成本发明。

具体来说，本发明的目的在于提供一种无人机捕获目标的控制方法，该方法中实时获得目标无人机的状态估计，并据此解算追踪无人机的控制指令，通过该控制指令控制所述追踪无人机捕获目标。

其中，所述控制指令通过下式(一)实时获得：

a_c＝K_p(P_t-P)+K_v(V_t-V)+K_α(a_t-a) (一)

其中，K_p、K_v和K_α都表示控制参数；优选地，K_p的取值为1.6，K_v的取值为3.6，K_α的取值为0.8；

P_t表示目标无人机的位置，V_t表示目标无人机的速度，α_t表示目标无人机的加速度；

P表示追踪无人机的位置，V表示追踪无人机的速度，a表示追踪无人机的加速度。

其中，所述目标无人机的状态估计通过下述步骤获得：

步骤1，通过携带有摄像机的追踪无人机追踪并拍摄目标无人机，获得连续的包含目标无人机的图像；

步骤2，通过卷积神经网络处理所述图像，并从每帧图像中都提取出目标无人机的4个关键点的像素坐标；

步骤3，根据所述目标无人机的4个关键点的像素坐标获得目标无人机的状态估计。

其中，在步骤2中，首先通过图像识别的方式框选出目标无人机的基础关键点位置，

再根据基础关键点位置寻找并框选出相关关键点的位置，

进而获得目标无人机的4个关键点的像素坐标。

其中，所述步骤3包括如下子步骤：

子步骤1，通过目标无人机关键点的像素坐标获得旋转矩阵，

子步骤2，通过旋转矩阵获得目标无人机的姿态，

子步骤3，通过目标无人机的姿态获得目标无人机的加速度，

子步骤4，通过目标无人机的加速度获得目标无人机的实际位置和速度。

其中，在所述子步骤1中，通过下式(二)获得旋转矩阵：

其中，R表示旋转矩阵，即用于从正交坐标系到相机坐标系做转换的3×3旋转矩阵；

R′表示任意3×3旋转矩阵，其第三列[r₇ r₈ r₉]^T等于旋转轴Za，且R′满足旋转矩阵的正交约束；

旋转轴

表示点P_i0指向点P_j0的向量，||P_i0P_j0||表示点P_i0指向点P_j0的向量的模；

rot(Z，α_l)表示目标无人机绕Z轴的旋转角度为α_l；

c＝cosα_l，s＝sin α_l。

其中，在所述子步骤2中，通过下式(三)获得目标无人机的姿态：

其中，当-a sin(R₃₁)的值落在

范围内时，θ₁表示目标无人机的俯仰角，θ₂无意义；

当-a sin(R₃₁)的值未落在

范围内时，θ₂表示目标无人机的俯仰角，θ₁无意义；

ψ₁表示俯仰角为θ₁时对应求解得到的目标无人机的偏航角，

ψ₂表示俯仰角为θ₂时对应求解得到的目标无人机的偏航角，

φ₁表示俯仰角为θ₁时对应求解得到的目标无人机的滚转角，

φ₂表示俯仰角为θ₂时对应求解得到的目标无人机的滚转角，

R₃₁、R₃₂、R₃₃表示式(二)中求解得到的旋转矩阵R中第三行的三个元素，

R₂₁表示式(二)中求解得到的旋转矩阵R中第二行的第一个元素，

R₁₁表示式(二)中求解得到的旋转矩阵R中第一行的第一个元素；

a sin表示反正弦函数，a tan表示反正切函数。

其中，在所述子步骤3中，通过下式(四)获得目标无人机的加速度：

a_t＝[a_x，a_y，a_z]^T (四)

其中，a_t表示目标无人机的加速度，

a_x表示惯性坐标系中X轴方向的加速度分量，

a_y表示惯性坐标系中Y轴方向的加速度分量，

a_z表示竖直方向的加速度分量，a_z＝0

其中，g表示重力加速度；

θ表示式(三)中求解所得目标无人机的俯仰角，

表示式(三)中求解所得目标无人机的滚转角，

ψ表示式(三)中求解所得目标无人机的偏航角。

其中，在所述子步骤4中，通过下式(五)获得目标无人机的实际位置，

其中，K_k代表卡尔曼增益，γ_k表示用于模拟间歇测量的二进制随机变量，如果目标无人机在第k帧图像中被检测出，则γ_k＝1，如果目标无人机在第k帧图像中未被检测出，则γ_k＝0；

w_k表示第k帧图像对应的过程噪声，w_k-1表示第k-1帧图像对应的过程噪声；

表示基于k-1帧图像估计出的第k帧图像对应的状态量，

表示估计得到的最优的第k-1帧图像对应的状态量，

表示估计得到的最优的第k帧图像对应的状态量，

出值

知

都可以表示为X，

Z_k表示第k帧图像对应的量测量，可表示为Z；

A表示过程矩阵，H表示观测矩阵；

P_t表示目标无人机的位置，v_t表示目标无人机的速度，a_t表示目标无人机的加速度，h表示图像的采样周期，优选为25Hz，I₃表示三维单位矩阵。

本发明所具有的有益效果包括：

(1)根据本发明提供的基于目标加速度估计的无人机捕获目标的控制方法能够针对非合作的目标无人机进行追踪，进行能够执行打击、捕获等操作；

(2)根据本发明提供的基于目标加速度估计的无人机捕获目标的控制方法能够实时根据获得的目标无人机的状态估计调整追踪无人机的飞行状态，使得追踪无人机与目标无人机之间的距离快速缩小，实现对目标的稳定快速追踪；

(3)根据本发明提供的基于目标加速度估计的无人机捕获目标的控制方法中算法相对简单，运算量小，能够满足实时计算输出的控制需要。

附图说明

图1示出实施例中无人机捕捉跟踪目标轨迹图；

图2示出实施例中目标位置变化曲线；

图3示出实施例中目标速度变化曲线；

图4示出实施例中目标加速度变化曲线；

图5示出实施例及对比例中无人机跟踪目标过程相对位置变化曲线。

具体实施方式

下面通过附图和实施例对本发明进一步详细说明。通过这些说明，本发明的特点和优点将变得更为清楚明确。

在这里专用的词“示例性”意为“用作例子、实施例或说明性”。这里作为“示例性”所说明的任何实施例不必解释为优于或好于其它实施例。尽管在附图中示出了实施例的各种方面，但是除非特别指出，不必按比例绘制附图。

根据本发明提供的基于目标加速度估计的无人机捕获目标的控制方法，该方法中实时获得目标无人机的状态估计，并据此解算追踪无人机的控制指，通过该控制指令控制所述追踪无人机捕获目标。

所述控制指令通过下式(一)实时获得：

a_c＝K_p(P_t-P)+K_v(V_t-V)+K_α(a_t-a) (一)

其中，K_p、K_v和K_α都表示控制参数；优选地，K_p的取值为1.6，K_v的取值为3.6，K_α的取值为0.8；

所P_t表示目标无人机的位置，V_t表示目标无人机的速度，α_t表示目标无人机的加速度；所述目标无人机的状态估计包括目标无人机的位置、速度和加速度；

P表示追踪无人机的位置，V表示追踪无人机的速度，a表示追踪无人机的加速度。所述追踪无人机的位置和速度都通过追踪无人机上的卫星信号接收机实时获得，所述追踪无人机的加速度通过追踪无人机上的加速度计实时获得。

在一个优选的实施方式中，所述目标无人机的状态估计通过下述步骤获得：

步骤1，通过携带有摄像机的追踪无人机追踪并拍摄目标无人机，获得连续的包含目标无人机的图像。所述目标无人机为四旋翼无人机，所述追踪无人机可以是任意型号的无人机，其能够携带摄像机即可，优选地，所述摄像机为单目摄像机等设备，能够连续拍摄包含目标无人机的图像即可，每秒拍摄获得20～50帧图像，优选为30帧图像，所述图像的尺寸大小及像素大小都一致，以使得通过相邻帧图像可以比较清晰地追踪目标无人机的运动轨迹。

步骤2，通过卷积神经网络处理所述图像，并从每帧图像中都提取出目标无人机的4个关键点的像素坐标。

在步骤2中，首先通过图像识别的方式框选出目标无人机的基础关键点位置，

再根据基础关键点位置寻找并框选出相关关键点的位置，

进而获得目标无人机的4个关键点的像素坐标，即4个相关关键点的像素坐标。

所述基础关键点是指目标无人机上具有明显的外观特征的位置区域，包括机头、机尾、机顶和机腹。所述4个相关关键点是指四旋翼无人机上具有相似外观特征的四个电机所在位置。其中，第一个电机即第一个相关关键点位于机头的左上角，第二的电机即第二个相关关键点位于机头的右上角，第三个电机即第三个相关关键点位于机尾的左下角，第四个电机即第四个相关关键点位于机尾的左下角。本申请中，在确定关键点所在区域后，选取该区域中心点的像素坐标作为该关键点的像素坐标。

所述卷积神经网络是一种基于生物启发的分层特征计算结构，是一类包含卷积计算且具有深度结构的前馈神经网络。卷积神经网络具有表征学习(representationlearning)能力，能够按其阶层结构对输入信息进行平移不变分类(shift-invariantclassification)。

步骤3，根据所述目标无人机的4个关键点的像素坐标及摄像机参数，获得目标无人机的状态估计。所述目标无人机的状态估计包括目标无人机的位置、目标无人机的姿态和目标无人机的速度，优选地，所述目标无人机的状态估计是在惯性坐标系中解算的，所述惯性坐标系是指以目标起飞点为原点的坐标系，该坐标系的X轴方向指向地理北向，Y轴指向地理东向，Z轴与X轴和Y轴正交构成右手法则，指向下为正。

优选地，所述步骤3包括如下子步骤：

子步骤1，通过目标无人机关键点的像素坐标获得旋转矩阵，

子步骤2，通过旋转矩阵获得目标无人机的姿态，

子步骤3，通过目标无人机的姿态获得目标无人机的加速度，

子步骤4，通过目标无人机的加速度获得目标无人机的实际位置和速度。

在一个优选的实施方式中，在所述子步骤1中，通过下式(二)获得旋转矩阵：

其中，R表示旋转矩阵，即用于从正交坐标系到相机坐标系做转换的3×3旋转矩阵；

R′表示任意3×3旋转矩阵，其第三列[r₇ r₈ r₉]^T等于旋转轴Za，且R′满足旋转矩阵的正交约束；

旋转轴

表示点P_i0指向点P_j0的向量，||P_i0P_j0||表示点P_i0指向点P_j0的向量的模；步骤2中获得4个关键点的像素坐标后可以求解出点P_i0和点P_j0的深度，从而确定式(二)中的旋转轴Za，即[r₇ r₈ r₉]^T。

rot(Z，α_l)表示目标无人机绕Z轴的旋转角度为α_l；

c和s都表示中间变量，c＝cos α_l，s＝sin α_l。

r₁到r₉分别表示任意3×3旋转矩阵R′中的每个元素，第三列[r₇ r₈ r₉]^T等于旋转轴Za。

优选地，在所述子步骤2中，通过下式(三)获得目标无人机的姿态：

其中，

当-a sin(R₃₁)的值落在

范围内时，θ₁表示目标无人机的俯仰角，θ₂无意义；

当-a sin(R₃₁)的值未落在

范围内时，θ₂表示目标无人机的俯仰角，θ₁无意义；

ψ₁表示俯仰角为θ₁时对应求解得到的目标无人机的偏航角，

ψ₂表示俯仰角为θ₂时对应求解得到的目标无人机的偏航角，

φ₁表示俯仰角为θ₁时对应求解得到的目标无人机的滚转角，

φ₂表示俯仰角为θ₂时对应求解得到的目标无人机的滚转角，

R₃₁、R₃₂、R₃₃表示式(二)中求解得到的旋转矩阵R中第三行的三个元素，

R₂₁表示式(二)中求解得到的旋转矩阵R中第二行的第一个元素，

R₁₁表示式(二)中求解得到的旋转矩阵R中第一行的第一个元素；

a sin表示反正弦函数，a tan表示反正切函数。

目标无人机姿态包括无人机机体坐标系与惯性坐标的三个夹角，即滚转角、俯仰角和偏航角，通过上述式(三)可以获得。

优选地，在所述子步骤3中，通过下式(四)获得目标无人机的加速度：

a_t＝[a_x，a_y，a_z]^T (四)

其中，a_t表示目标无人机的加速度，

a_x表示惯性坐标系中X轴方向的加速度分量，

a_y表示惯性坐标系中Y轴方向的加速度分量，

a_z表示竖直方向的加速度分量，a_z＝0

其中，g表示重力加速度；

θ表示式(三)中求解所得目标无人机的俯仰角，

表示式(三)中求解所得目标无人机的滚转角，

ψ表示式(三)中求解所得目标无人机的偏航角。

本申请中仅考虑目标无人机平稳稳定飞行时的状态，即假设目标四旋翼在水平平面内保持稳定飞行，所以a_z＝0，不考虑a_z不为零时的情况。俯仰角定义取值范围为

当θ₁小于90°时θ＝θ₁；当θ₁大于90°时，θ＝θ₂。偏航角ψ与ψ₁、ψ₂及滚转角

取值和φ₁、φ₂的关系与θ一致。

优选地，在所述子步骤4中，通过下式(五)获得目标无人机的实际位置，

其中，K_k代表卡尔曼增益，γ_k表示用于模拟间歇测量的二进制随机变量，如果目标无人机在第k帧图像中被检测出，则γ_k＝1，如果目标无人机在第k帧图像中未被检测出，则γ_k＝0；

w_k表示第k帧图像对应的过程噪声，w_k-1表示第k-1帧图像对应的过程噪声；

表示基于k-1帧图像估计出的第k帧图像对应的状态量，

表示估计得到的最优的第k-1帧图像对应的状态量，

表示估计得到的最优的第k帧图像对应的状态量，

输出值

知

都可以表示为X，

Z_k表示第k帧图像对应的量测量，可表示为Z；

A表示过程矩阵，H表示观测矩阵；

P_t表示目标无人机的位置，v_t表示目标无人机的速度，a_t表示目标无人机的加速度，h表示图像的采样周期，优选为25Hz，I₃表示三维单位矩阵。

实施例1：

目标无人机初始位置是[0，0，40]，其在固定高度的水平面内绕8字型轨迹飞行，其真实的运动信息如下：

目标运动轨迹为：

目标运动速度为：

目标加速度为：

目标的实际飞行状态如图1、图2、图3和图4中的虚线所示；

追踪无人机的初始位置为[0，-5，20]，其上携带有摄像机；所述追踪无人机通过下述方法实时获得目标无人机的状态估计：

步骤1，通过摄像机追踪并拍摄目标无人机，获得连续的包含目标无人机的图像；

步骤2，通过卷积神经网络处理所述图像，并从每帧图像中都提取出目标无人机的4个关键点的像素坐标，其中每个关键点都对应一个目标无人机电机所在区域中心点的像素坐标；

其中，首先通过图像识别的方式框选出目标无人机的基础关键点位置，

再根据基础关键点位置寻找并框选出相关关键点的位置，

进而获得目标无人机的4个关键点的像素坐标；

步骤3，根据所述目标无人机的4个关键点的像素坐标获得目标无人机的状态估计，

具体包括如下子步骤：

子步骤1，通过目标无人机关键点的像素坐标获得旋转矩阵，

子步骤2，通过旋转矩阵获得目标无人机的姿态，

子步骤3，通过目标无人机的姿态获得目标无人机的加速度，

子步骤4，通过目标无人机的加速度获得目标无人机的实际位置和速度。

在所述子步骤1中，通过下式(二)获得旋转矩阵：

R表示旋转矩阵，即用于从正交坐标系到相机坐标系做转换的3×3旋转矩阵；

R′表示任意3×3旋转矩阵，其第三列[r₇ r₈ r₉]^T等于旋转轴Za，且R′满足旋转矩阵的正交约束；

rot(Z，α_l)表示目标无人机绕Z轴的旋转角度为α_l；

c＝cos α_l，s＝sin α_l

在所述子步骤2中，通过下式(三)获得目标无人机的姿态：

其中，当-a sin(R₃₁)的值落在

范围内时，θ₁表示目标无人机的俯仰角，θ₂无意义；

当-a sin(R₃₁)的值未落在

范围内时，θ₂表示目标无人机的俯仰角，θ₁无意义；

在所述子步骤3中，通过下式(四)获得目标无人机的加速度：

a_t＝[a_x，a_y，a_z]^T (四)

在所述子步骤4中，通过下式(五)获得目标无人机的实际位置，

A表示过程矩阵，H表示观测矩阵；

P_t表示目标无人机的位置，v_t表示目标无人机的速度，a_t表示目标无人机的加速度，h表示图像25Hz的采样周期，I₃表示三维单位矩阵。

通过上述方法实时获得的目标无人机的状态估计如图2、图3和图4中的实线所示；相应地，目标无人机的真实轨迹如图2、图3和图4中的虚线所示，可知本方法能够及时准确地获知目标的状态估计。

基于上述实时获得的目标的状态估计，利用式(一)实时解算追踪无人机的控制指令，并通过该控制指令控制追踪无人机捕获目标。

a_c＝K_p(P_t-P)+K_v(V_t-V)+K_α(a_t-a) (一)

K_p的取值为1.6，K_v的取值为3.6，K_α的取值为0.8。

得到的无人机捕捉跟踪目标轨迹图如图1中所示，无人机跟踪目标过程相对位置变化曲线如5中所示。

对比例：

基于上述实施例1相同的目标的状态估计，选用传统的PID控制算法对追踪无人机进行制导控制，其制导指令为：

ΔP＝P_t-P，K_p＝1.6，K_d＝0.3，K_i＝0.3

得到的无人机捕捉跟踪目标轨迹图如图1中所示，无人机跟踪目标过程相对位置变化曲线如5中所示。

通过上述实验例和对比例的比较可知，实施例1的基于目标加速度估计的无人机捕获目标的控制方法能够更快更好地完成对目标的捕捉工作，即能够更快地相遇/拦截目标。

以上结合了优选的实施方式对本发明进行了说明，不过这些实施方式仅是范例性的，仅起到说明性的作用。在此基础上，可以对本发明进行多种替换和改进，这些均落入本发明的保护范围内。

Claims (9)

1.一种无人机捕获目标的控制方法，其特征在于，该方法中实时获得目标无人机的状态估计，并据此解算追踪无人机的控制指令，通过该控制指令控制所述追踪无人机捕获目标。

2.根据权利要求1所述的无人机捕获目标的控制方法，其特征在于，

所述控制指令通过下式(一)实时获得：

a_c＝K_p(P_t-P)+K_v(V_t-V)+K_a(a_t-a) (一)

其中，K_p、K_v和K_α都表示控制参数；优选地，K_p的取值为1.6，K_v的取值为3.6，K_α的取值为0.8；

P_t表示目标无人机的位置，V_t表示目标无人机的速度，α_t表示目标无人机的加速度；所述目标无人机的状态估计包括目标无人机的位置、速度和加速度；

P表示追踪无人机的位置，V表示追踪无人机的速度，a表示追踪无人机的加速度。

3.根据权利要求1所述的无人机捕获目标的控制方法，其特征在于，

所述目标无人机的状态估计通过下述步骤获得：

步骤1，通过携带有摄像机的追踪无人机追踪并拍摄目标无人机，获得连续的包含目标无人机的图像；

步骤2，通过卷积神经网络处理所述图像，并从每帧图像中都提取出目标无人机的4个关键点的像素坐标；

步骤3，根据所述目标无人机的4个关键点的像素坐标获得目标无人机的状态估计。

4.根据权利要求3所述的无人机捕获目标的控制方法，其特征在于，

在步骤2中，首先通过图像识别的方式框选出目标无人机的基础关键点位置，

再根据基础关键点位置寻找并框选出相关关键点的位置，进而获得目标无人机的4个关键点的像素坐标。

5.根据权利要求3所述的无人机捕获目标的控制方法，其特征在于，

所述步骤3包括如下子步骤：

子步骤1，通过目标无人机关键点的像素坐标获得旋转矩阵，

子步骤2，通过旋转矩阵获得目标无人机的姿态，

子步骤3，通过目标无人机的姿态获得目标无人机的加速度，

子步骤4，通过目标无人机的加速度获得目标无人机的实际位置和速度。

6.根据权利要求5所述的无人机捕获目标的控制方法，其特征在于，

在所述子步骤1中，通过下式(二)获得旋转矩阵：

其中，R表示旋转矩阵，即用于从正交坐标系到相机坐标系做转换的3×3旋转矩阵；

R′表示任意3×3旋转矩阵，其第三列[r₇ r₈ r₉]^T等于旋转轴Za，且R′满足旋转矩阵的正交约束；

旋转轴

表示点P_i0指向点P_j0的向量，||P_i0 P_j0||表示点P_i0指向点P_j0的向量的模；

rot(Z，α_l)表示目标无人机绕Z轴的旋转角度为α_l；

c＝cosα_l，s＝sinα_l。

7.根据权利要求5所述的无人机捕获目标的控制方法，其特征在于，

在所述子步骤2中，通过下式(三)获得目标无人机的姿态：

其中，当-asin(R₃₁)的值落在

范围内时，θ₁表示目标无人机的俯仰角，θ₂无意义；

当-asin(R₃₁)的值未落在

范围内时，θ₂表示目标无人机的俯仰角，θ₁无意义；

ψ₁表示俯仰角为θ₁时对应求解得到的目标无人机的偏航角，

ψ₂表示俯仰角为θ₂时对应求解得到的目标无人机的偏航角，

φ₁表示俯仰角为θ₁时对应求解得到的目标无人机的滚转角，

φ₂表示俯仰角为θ₂时对应求解得到的目标无人机的滚转角，

R₃₁、R₃₂、R₃₃表示式(二)中求解得到的旋转矩阵R中第三行的三个元素，

R₂₁表示式(二)中求解得到的旋转矩阵R中第二行的第一个元素，

R₁₁表示式(二)中求解得到的旋转矩阵R中第一行的第一个元素；

a sin表示反正弦函数，a tan表示反正切函数。

8.根据权利要求5所述的无人机捕获目标的控制方法，其特征在于，

在所述子步骤3中，通过下式(四)获得目标无人机的加速度：

a_t＝[a_x，a_y，a_z]^T (四)

其中，a_t表示目标无人机的加速度，

a_x表示惯性坐标系中X轴方向的加速度分量，

a_y表示惯性坐标系中Y轴方向的加速度分量，

a_z表示竖直方向的加速度分量，a_z＝0

其中，g表示重力加速度；

θ表示式(三)中求解所得目标无人机的俯仰角，

表示式(三)中求解所得目标无人机的滚转角，

ψ表示式(三)中求解所得目标无人机的偏航角。

9.根据权利要求5所述的无人机捕获目标的控制方法，其特征在于，

在所述子步骤4中，通过下式(五)获得目标无人机的实际位置，

其中，K_k代表卡尔曼增益，γ_k表示用于模拟间歇测量的二进制随机变量，如果目标无人机在第k帧图像中被检测出，则γ_k＝1，如果目标无人机在第k帧图像中未被检测出，则γ_k＝0；

w_k表示第k帧图像对应的过程噪声，w_k-1表示第k-1帧图像对应的过程噪声；

表示基于k-1帧图像估计出的第k帧图像对应的状态量，

表示估计得到的最优的第k-1帧图像对应的状态量，

表示估计得到的最优的第k帧图像对应的状态量，

输出值

知

都可以表示为X，

Z_k表示第k帧图像对应的量测量，可表示为Z；

A表示过程矩阵，H表示观测矩阵；

P_t表示目标无人机的位置，v_t表示目标无人机的速度，a_t表示目标无人机的加速度，h表示图像的采样周期，优选为25Hz，I₃表示三维单位矩阵。

Images