CN103149939A - 一种基于视觉的无人机动态目标跟踪与定位方法 - Google Patents

Abstract

本发明公开了一种基于视觉的无人机动态目标跟踪与定位方法，属于无人机导航领域。所述方法包括视频处理，进行动态目标的检测和图像跟踪；云台伺服控制；建立影像中目标与现实环境中目标的对应关系，进一步测量摄像机与动态目标之间的距离，完成动态目标的精确定位；无人机飞控系统进行自主跟踪地面动态目标飞行。本发明不需要人的全程参与，可自行完成对运动目标的检测、图像跟踪，自动偏转光轴，使动态目标始终呈现在成像平面中央，在获取无人机高度信息的基础上依据建立的模型实时测量无人机与动态目标之间的距离，从而实现对动态目标的定位，以此作为反馈信号，形成闭环控制，引导无人机的跟踪飞行。

Description

一种基于视觉的无人机动态目标跟踪与定位方法

技术领域

本发明属于无人机导航领域。具体涉及一种基于视觉的无人机动态目标跟踪与定位方法。

背景技术

无人机通常通过视觉传感器感知周围环境，地面站对下传的传感器信息处理分析获取有效信息引导无人机飞行。无人机常常携带的光电设备包括CCD光电摄像机、红外热像仪、合成孔径雷达成像等。其中CCD摄像机不仅能够获得高解析度的图像情报，而且容易与红外焦平面阵列结合形成多光谱成像系统，因此成为小型无人机首选甚至是唯一的光电视觉传感器。

无人机利用机载CCD摄像机对飞行区域进行监控，获取的航空图像经过压缩后通过卫星链路下传回地面，地面站对数据进行解压缩处理，得到原始的无人机航空图像。对下传的航空视频进行预处理，图像增强、滤波和去噪，以提高图像的清晰度，经过图像分割、特征提取等步骤后标识出目标，在后续的视频帧中图像跟踪该运动目标，计算目标相应于光轴的偏差大小及方向，并将水平与俯仰方向的偏差信号上传给无人机机载光轴伺服系统，控制光轴角度，使所跟踪的目标始终保持在成像平面中央，从而可以进行长时间的锁定该目标，引导无人机跟踪飞行。

要实现在无人机飞行过程中实时监测地面，获取感兴趣目标的方位信息，首先需要从序列航空图像中检测出运动目标。无人机机载摄像机的运动使得航空视频的背景环境也是运动的，因此变化的背景对动态目标的检测将会造成极大的干扰。运动检测主要包括帧差法、背景差分法和光流法。前两种方法虽然实现简单，但是对动态场景中的运动目标之外的变化因素都非常敏感，不能满足动态背景下的动态目标的检测需求；光流法采用了运动目标随时间变化的光流特性，该方法的优点是在摄像机运动存在的前提下也能检测出独立的运动目标，但计算量很大且易受噪声、光照等干扰，因此也不适合无人机机载视频处理的需求。针对动态背景中运动目标检测，不少文献也做了相关方面的研究。基本思想是通过机载传感器或者图像处理估计出背景的运动模型，补偿背景的运动，从而将动态背景下动态目标的检测问题转化为静态背景下动态目标的检测。

检测出运动目标之后，需要在后续视频中连续跟踪该运动目标，实时获取运动目标的位置信息。运动目标跟踪算法一般分为基于区域、基于特征和基于模型的跟踪算法。在无人机航空视频的图像跟踪中，为了满足引导无人机飞行的需求，跟踪算法需要精确锁定特定目标并且算法实时性要高。MeanShift算法是一种有效且常用的跟踪算法，其本质上属于基于特征的跟踪算法。分为以下几步：首先在初始帧中建立目标特征，使用颜色直方图来描述目标区域的特征；然后在后续帧中进行目标候选区域特征提取；最后通过相似性函数判断当前帧中候选区域是否为真实目标。MeanShift算法实时性较好；采用归一化核函数直方图建模，对边缘遮挡、目标形变以及背景变化不敏感，但其仍然存在使用上的局限性，一方面，由于MeanShift采用核函数颜色直方图建模，颜色直方图是一种比较弱的对目标特征的描述，当背景和目标的颜色分布较相似的时候，算法效果欠佳，搜索目标往往收敛于背景中与目标颜色分布比较相似的物体，另一方面不能跟踪快速运动的物体。Kalman滤波是在协方差估计误差最小的意义上的最优的递归算法，即只要获知上一时刻状态的估计值以及当前状态的观测值就可以计算出当前状态的估计值，视觉的跟踪看作推理的过程。缺点在于方法的鲁棒性还有待加强，需要有较为准确的初始值，适用于规律运动的目标跟踪。

CCD摄像机中呈现的只是运动目标二维的运动信息，因此需要加入其它传感器和采用一定的算法才能实现运动目标的相对定位。通常情况下用激光测距仪测量目标相对无人机的距离得到精确的目标相对位置和相对速度。激光测距是采用脉冲激光，无法做到实时的连续测距，因此我们应当考虑实时的测距算法。

在现有的技术中，复杂背景下无人机对运动目标的跟踪与定位，通常辅助人工搜索的方法检测特定运动目标，并用跟踪算法实现运动目标在图像中的跟踪，计算脱靶量，从而控制光轴伺服系统实现光轴稳定跟踪，是一种半自主的引导方式。并且对无人机的引导，大多数情况下是角度导引，并非位置导引，因此有一定的局限性。

发明内容

本发明以无人机导引飞行为应用背景，利用无人机携带的CCD摄像机获得视觉信息，在航空视频中检测出运动目标并在图像中稳定实时跟踪，计算脱靶量，自动调整机载云台的旋转和俯仰角度使被跟踪的地面运动目标始终保持在摄像机成像平面的中心，根据单目测距模型实时测量无人机与运动目标之间的距离，确定运动目标在三维世界中的位置坐标，引导无人机跟踪飞行。本发明在基本上或完全不依赖于地面设备的辅助，依靠视觉传感器和机载惯性设备实现无人机进入目标区域后，自主检测、跟踪并为目标精确定位功能，引导无人机跟踪飞行。

本发明提供的一种基于视觉的无人机动态目标跟踪与定位方法，包括如下步骤：

第一步，视频处理，进行动态目标的检测和图像跟踪。

（1）动态目标检测：在连续两帧图像中，第一帧图像中提取特征点集，在第二帧图像中对这些特征点进行跟踪，得到第一帧图像中特征点集的对应特征点集，进一步剔除对应特征点集中属于动态目标的特征点。用剩余的有效特征点集对中的各特征点对的运动矢量估计全局的运动，得到变换矩阵，从而补偿背景的运动，将动态背景下的动态目标检测转换为静态背景下的动态目标检测，帧差补偿背景后的两帧图像，用最小类间方差阈值法自动选取阈值产生相应的二值图像，对二值图像进行滤波处理，接着用最小外接矩形描述该动态目标，实现动态目标的检测；

（2）对检测出的动态目标在后续帧中采用融合基于偏移的均值向量跟踪和卡尔曼先验预估的算法连续跟踪，首先用卡尔曼滤波预估出动态目标在当前帧图像中的位置，然后将该位置信息传递给基于偏移的均值向量跟踪算法，该算法在该位置附近搜索目标，判定搜索结果是否有效，若是有效则进一步传递给卡尔曼滤波作为观测值进行下一步的预测。

第二步，云台伺服控制，根据动态目标在成像平面中与像平面中心的偏差，实时调整云台的俯仰偏转角度，使目标保持在图像的中央。

第三步，建立影像中目标与现实环境中目标的对应关系，进一步测量摄像机与动态目标之间的距离，完成动态目标的精确定位。

第四步，无人机飞控系统进行自主跟踪地面动态目标飞行。

本发明一种基于视觉的无人机动态目标跟踪与定位方法的优点在于：

不需要人的全程参与，可自行完成对运动目标的检测、图像跟踪，自动偏转光轴，使动态目标始终呈现在成像平面中央，在获取无人机高度信息的基础上依据建立的模型实时测量无人机与动态目标之间的距离，从而实现对动态目标的定位，以此作为反馈信号，形成闭环控制，引导无人机的跟踪飞行。

附图说明

图1是本发明提供的基于视觉的无人机目标跟踪与定位实现框图；

图2动态背景下运动目标检测流程；

图3(a)没有经过背景补偿的帧差效果图，图3(b)经过背景补偿后的帧差效果图；

图4阈值化分割后检测效果；

图5形态学滤波后结果；

图6融合Kalman与MeanShift的跟踪算法改进流程；

图7无人机地面目标跟踪系统坐标系；

图8小孔成像模型；

图9单目测距模型。

具体实施方式

下面结合附图和实施例对本发明进行详细说明。

本发明一种基于视觉的无人机动态目标跟踪与定位方法，是利用机载CCD摄像机感知无人机飞行区域的周围环境，从航空视频中利用图像处理算法自主检测出运动目标并在图像中稳定实时跟踪，计算脱靶量，自动调整机载云台的旋转和俯仰角度使被跟踪的地面目标始终保持在摄像机的光轴附近，同时根据单目测距模型，实时测量无人机与运动目标之间的距离，确定运动目标在三维世界中的位置坐标，从而引导无人机跟踪飞行。

要实现对运动目标的自主检测、跟踪与定位，主要分为以下几个部分：视频处理部分，云台伺服控制部分，目标定位部分和无人机跟踪飞行部分。结合附图1和图2，本发明提供的基于视觉的无人机动态目标跟踪与定位方法具体包括如下步骤：

步骤一:视频处理，包括在图像中的运动目标检测和运动跟踪。无人机携带机载CCD摄像机在某感兴趣的运动目标活动区域内飞行，感知周围环境，获取的环境信息以航空视频的形式经过压缩后通过链路下传给地面站，地面站对解压缩后的航空视频进行图像处理，以提取出感兴趣的运动目标并连续跟踪。视频处理使得无人机依靠机载CCD摄像机不借助外界的交互，就能够自主地检测到动态背景中的动态目标并在图像中稳定跟踪。

步骤1.1在图像中的运动目标检测：

首先提取航空视频中的连续两帧图像(I^t-1，I^t)，在第t-1时刻的图像I^t-1中运用KLT特征点检测算法提取特征点。假设I^t-1提取的特征点集为F^t-1。特征点集F^t-1中特征点对应着在水平和垂直方向都有较大梯度的点。接着在第t时刻的图像I^t中对特征点集F^t-1进行跟踪。特征点集F^t-1跟踪的过程即为求取特征点集F^t-1在图像I^t中对应的像素坐标点集F^t的过程，特征点集F^t-1的跟踪可以描述为：

F^t＝F^t-1+dⁱ

其中，dⁱ表示特征点的偏移量。因此，特征点集的跟踪可以理解为对偏移量dⁱ的求解。dⁱ的具体求解过程用金字塔光流来求解。

利用KLT特征点检测算法选取的特征点中不可避免包含属于运动目标的特征点，如果运动目标上的特征点参与了背景运动的估计，势必会影响全局运动估计的可靠性。所以有必要在全局运动估计前去除属于运动目标的特征点，有效性判定法则如式(1)所示：

$\left\{\begin{array}{cc}{f}_i\&Element;F_{\mathrm{in}}& \mathrm{if}\left|\right|\left|V_i\right||-{\mathrm{\&mu;}}_{\left|\right|\&CenterDot;\left|\right|}|<3\&CenterDot;{\mathrm{\&sigma;}}_{\left|\right|\&CenterDot;\left|\right|}\mathrm{and}|\mathrm{Ang}\left(V_i\right)-{\mathrm{\&mu;}}_{\mathrm{Ang}}|<3\&CenterDot;{\mathrm{\&sigma;}}_{\mathrm{Ang}}\\ f_i\&Element;F_{\mathrm{out}}& \mathrm{otherwise}\end{array}\right.---\left(1\right)$

其中，f_i代表特征点集F^t中第i个特征点，V_i代表第i个特征点在连续帧图像中的运动光流向量，||V_i||·Ang(V_i)分别代表该运动光流向量的模和方向，(μ_||·||，σ_||·_||)代表KLT特征点检测算法选定特征点在连续两帧图像中所有光流向量的模的期望和方差，(μ_Ang，σ_Ang)代表KLT特征点检测算法选定特征点在连续两帧图像中所有光流向量的方向的期望、方差，F_in代表有效特征点，F_out代表无效特征点。

在得到相邻两幅图像(I^t-1，I^t)中对应的有效特征点集F＝＜F^t-1，F^t＞后，利用这些特征点的像素坐标估计出两幅图像的运动变换。采用六参数仿射模型来表示I^t-1到I^t的图像变换矩阵

：全局运动模型如公式(2)所示：

$\left[\begin{array}{c}{F}_x^t\\ F_y^t\\ 1\end{array}\right]=\left[\begin{array}{c}a{F}_x^{t-1}+b{F}_y^{t-1}+t_1\\ c{F}_x^{t-1}+d{F}_y^{t-1}+t_2\\ 1\end{array}\right]=\left[\begin{array}{ccc}a& b& t_1\\ c& d& t_2\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}{F}_x^{t-1}\\ F_y^{t-1}\\ 1\end{array}\right]=T_{t-1}^t\left[\begin{array}{c}{F}_x^{t-1}\\ F_y^{t-1}\\ 1\end{array}\right]---\left(2\right)$

其中

分别代表特征点集坐标的x和y值。

用随机采样一致性(RANSAC，RANdom Sample Consensus)法来拟合公式(2)中参数a，b，c，d，t₁，t₂，进而拟合出背景的运动模型，补偿背景，将动态背景下的运动目标检测问题转换为静态背景下的运动目标检测，运用帧差法进行运动目标检测。区别于以往的数据拟合方法，RANSAC并不是使用所有的数据集对模型进行拟合，因为线性拟合方法极易受到整体数据集中离群点的影响，从而使所拟合的模型与真实模型之间存在较大偏差。

利用得到的图像变换矩阵

对第t-1时刻的图像I^t-1进行运动补偿，以减少运动的背景对前景运动目标检测的影响。补偿后的图像表示为：

$I_{\mathrm{comp}}^{t-1}=T_{t-1}^t{I}^{t-1}---\left(3\right)$

补偿后的图像，由于在摄像机运动过程中，会有一部分图像信息在旋转和平移补偿后丢失，稳定后的视频上容易形成无定义区域(undefined area)，为了保证图像大小的一致性，必须先对无定义区域补充定义，因为无定义区域一定出现在图像边缘区域，由于运动目标在这附近的几率很小，所以定义无定义区域的差分过后值为0。用下面的式子来定义差分后的图像I_diff(x,y)：

经过背景补偿后的动态背景下的动态目标检测的效果与未经背景补偿的动态目标检测效果分别如图3(a)和图3(b)所示。经过背景补偿后，背景的运动对运动目标的检测的影响已经大大减少。

步骤1.2检测出运动目标之后，必须选取合适的阈值实现运动目标分割。阈值分割的目的是检测图像中与运动目标相对应的区域，用作后续的跟踪和行为分析。它利用了图像中要提取的运动目标与其背景在灰度特性上的差异，把图像视为具有不同灰度级的两类区域即目标和背景的组合，选取一个合适的阈值，以确定图像中的每一个像素点应该属于目标还是背景区域，从而产生相应的二值图像。确定合适的阈值是图像二值化的关键。本发明中采用Ostu提出的一种不依赖于物体和背景像素的概率密度分布模型的方法，最大类间方差阈值法自动选取阈值，它是在最小二乘法基础上推导得出的。算法比较简单，通过利用直方图的零阶、一阶累积矩来最大化判别函数，自动选择最佳阈值。所述的最大类间方差阈值法步骤如下：

假设一幅图像的灰度级为L，灰度值为i像素个数为n_i，总的像素数为N，每一个灰度值出现的概率为P_i，则有：

$N=\underset{i=1}{\overset{L}{\mathrm{\&Sigma;}}}{n}_i$ $P_i=\frac{n_i}{N}$ $\underset{i=0}{\overset{L-1}{\mathrm{\&Sigma;}}}{p}_i=1---\left(5\right)$

选择阈值T，将图像划分为两类：C₀：(0，1，2，...，T)；C₁：(T+1，T+2，...，L-1)。C₀和C₁类出现的概率分别为：

$p_0\left(T\right)=\underset{i=0}{\overset{T}{\mathrm{\&Sigma;}}}{p}_{i,}$ $p_1\left(T\right)=\underset{i=T+1}{\overset{L-1}{\mathrm{\&Sigma;}}}{p}_i---\left(6\right)$

这两类像素的平均灰度值分别为：

${\mathrm{\&mu;}}_0\left(T\right)=\frac{{\mathrm{\&Sigma;}}_{i=0}^T{\mathrm{ip}}_i}{p_0\left(T\right)},$ ${\mathrm{\&mu;}}_1\left(T\right)=\frac{{\mathrm{\&Sigma;}}_{i=T+1}^{L-1}{\mathrm{ip}}_i}{p_1\left(T\right)}---\left(7\right)$

整幅图像的平均灰度值为：

${\mathrm{\&mu;}}_T=\frac{{\mathrm{\&Sigma;}}_{i=0}^{L-1}{\mathrm{ip}}_i}{{\mathrm{\&Sigma;}}_{i=0}^{L-1}{p}_i}---\left(8\right)$

C₀和C₁类的方差分别为：

${\mathrm{\&sigma;}}_0^2\left(T\right)=\underset{i=0}{\overset{T}{\mathrm{\&Sigma;}}}\frac{{(i-{\mathrm{\&mu;}}_0)}^2{p}_i}{p_0}---\left(9\right)$

${\mathrm{\&sigma;}}_1^2\left(T\right)=\underset{i=T+1}{\overset{L-1}{\mathrm{\&Sigma;}}}\frac{{(i-{\mathrm{\&mu;}}_1)}^2{p}_i}{p_1}---\left(10\right)$

总方差为：

${\mathrm{\&sigma;}}_T^2=\underset{i=0}{\overset{L-1}{\mathrm{\&Sigma;}}}{(i-{\mathrm{\&mu;}}_T)}^2{p}_i={\mathrm{\&sigma;}}_W^2+{\mathrm{\&sigma;}}_B^2---\left(11\right)$

其中类内方差

和类间方差

分别为：

${\mathrm{\&sigma;}}_W^2\left(T\right)=p_0\left(T\right){\mathrm{\&sigma;}}_0^2\left(T\right)+p_1\left(T\right){\mathrm{\&sigma;}}_1^2\left(T\right)---\left(12\right)$

${\mathrm{\&sigma;}}_B^2\left(T\right)=p_0\left(T\right){({\mathrm{\&mu;}}_0\left(T\right)-{\mathrm{\&mu;}}_T)}^2+p_1\left(T\right){({\mathrm{\&mu;}}_1\left(T\right)-{\mathrm{\&mu;}}_T)}^2---\left(13\right)$

以类间方差作为衡量不同阈值导出的类别分离性能的测量准则，从直方图的左端开始，计算每个灰度值导出的类别的类间方差

自动确定使得类间方差

最大的阈值，当类间方差最大的时候，可以认为此时前景和背景差异最大，因此，最佳阈值为：

$t^{*}=\arg \underset{0\&le;T\&le;L-1}{\max }{\mathrm{\&sigma;}}_B^2\left(T\right)---\left(14\right)$

经过最大类间方差阈值分割的效果图如图4所示。此时二值图像中由于相邻两帧间的光照变化、背景的微变等，都会引发噪声点的出现，因此本发明中继续采用形态学滤波方法进一步处理。设A为待处理图像，B为滤波矩阵。A被B膨胀(Dilation)定义为：

用B来膨胀A得到的集合是与A至少有一个非零元素相交时的结构元素的集合。膨胀的用途之一是将二值图像中的裂缝桥接起来。使用B对A进行腐蚀(Erosion)，定义为：

用B来腐蚀A得到的集合是B完全包括在A中时的参考点的集合。腐蚀的用途之一是从二值图像中消除不相关的细节，只保留连续的较大的区域。因此对帧差后的二值图像先腐蚀后膨胀凸显运动目标。接着用最小外接矩形描述该运动目标，为后续的运动目标跟踪做准备。

经过开运算处理后的二值图像如图5所示，消除了小目标，并且桥接了一些裂缝，使运动更突显出来，在这一系列阈值分割、滤波的基础上用最小外接矩形描述该运动目标。

步骤1.3从无人机航空视频中检测出运动目标之后，对该用矩形框包围描述的目标在后续帧中连续跟踪。

本发明中采用融合基于特征跟踪的偏移的均值向量(MeanShift)算法和基于先验预估的卡尔曼(Kalman)滤波方法实现对运动目标实时精确地跟踪。本发明融合了Kalman与MeanShift进行图像跟踪，用Kalman预测出目标在下一时刻最有可能出现的位置，MeanShift在以该预估位置为中心的窗口范围内搜索，以增强跟踪的快速性能，同时将搜索结果作为观测值再次传递给Kalman，以预测下一时刻的位置。鉴于MeanShift的跟踪特征颜色直方图相对较弱，遭遇相似目标或者大面积遮挡的时候，会造成误跟踪，因此引入一个判定原则，当判定MeanShift搜索值有效时再传递给Kalman作为其观测值，否则放弃使用，以增强跟踪的精准度。实验结果表明改进后的融合算法大大增加了跟踪的快速性和准确性。

在卡尔曼滤波跟踪算法中，首先建立目标跟踪的状态空间模型，以目标的位置和速度作为状态变量，

其中x_k，y_k表示第k时刻目标的位置，表示第k时刻目标的速度。因此卡尔曼滤波器的状态方程可以定义为：

X_k＝FX_k-1+w_k    (17)

相应的观测值为目标的位置{Z_k}_k＝1，...＝{x_k，y_k}，观测方程为：

Z_k＝HX_k+v_k    (18)

其中F和H分别为系统矩阵和观测矩阵，w_k和v_k均为零均值的正态白噪声序列。在x轴上，按照牛顿运动定理，有运动方程：

$x\left(k\right)=x(k-1)+v_{x_{k-1}}t+\frac{1}{2}{w}_k{t}^2{v_{x_k}=v}_{x_{k-1}}+w_{k}t---\left(19\right)$

式中，t为采样时间，可以认为是帧数，通常为1。同理在y轴上有相似的方程，则具体化的状态方程和观测方程为：

$\left(\begin{array}{c}{x}_k\\ y_k\\ u_{x_k}\\ u_{y_k}\end{array}\right)=\left(\begin{array}{cccc}1& 0& 1& 0\\ 0& 1& 0& 1\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right)\left(\begin{array}{c}{x}_k-1\\ y_k-1\\ v_{x_k-1}\\ v_{y_k-1}\end{array}\right)+\left(\begin{array}{c}\frac{1}{2}\\ \frac{1}{2}\\ 1\\ 1\end{array}\right)w_k---\left(20\right)$

$\left(\begin{array}{c}{x}_k\\ y_k\end{array}\right)=\left(\begin{array}{cccc}1& 0& 0& 0\\ 0& 1& 0& 0\end{array}\right)\left(\begin{array}{c}{x}_{k-1}\\ y_{k-1}\\ v_{x_{k-1}}\\ v_{y_{k-1}}\end{array}\right)+\left(\begin{array}{c}1\\ 1\end{array}\right)v_k---\left(21\right)$

利用Kalman滤波预测下一时刻运动目标的运动状态，具体实现步骤参见参考文献[1]：秦永元，张洪钺.卡尔曼滤波与组合导航原理.1998，西北工业大学出版社。

基于偏移的均值向量MeanShift跟踪是以颜色直方图核函数为特征的目标跟踪算法。首先在初始帧中建立目标特征，使用颜色直方图来描述目标区域的特征；然后在后续帧中进行目标候选区域特征提取；最后通过相似性函数判断当前帧中候选区域是否为真实目标。

MeanShift的实现步骤参见参考文献[2]:COMANICIU D,MEER P.Mean shift:Arobustapproach toward feature space analysis[J].Pattern Analysis and Machine Intelligence,IEEETransactions on,2002,24(5):603–619。

如图6所示，融合的思路为，先由Kalman滤波器预测出目标在下一时刻最有可能出现的位置，用MeanShift在以该预估位置为中心的窗口范围内搜索，这样必然会增强跟踪的快速性能，同时将搜索结果作为观测值再次传递给Kalman滤波器，以预测下一时刻的位置。考虑到MeanShift搜索算法虽然对目标的旋转和变形都有很好地处理，但是由于其颜色直方图这种比较弱的特征描述会引起误跟踪，或者是当运动目标遭遇大比例的遮挡时，MeanShift搜索算法也会失效，此时若将不精确的观测值传递给Kalman，得到的预估值必然也不准确。因此先做判定。令Kalman预测值为

MeanShift搜索值为(x_k，y_k)，判定MeanShift的搜索值是否有效，即判定

值的大小。一般预测值和观测值接近，e_k较小，如果e_k，较大，则认为MeanShift搜索失效，继续利用以前的预测值作为观测值估算下一时刻的起始点

如果判定有效，则继续使用MeanShift的结果作为观测值。e_k的参考值的确定按照如下的步骤，计算初始阶段Kalman预测值与MeanShift搜索值的差值的方差(一般认为在跟踪初始阶段MeanShift能够较好地跟踪)，以初始阶段的方差为标准，在后续帧的计算过程中，当该帧图像中MeanShift搜索值与Kalman预测值的误差与初始阶段误差平均数的差平方落在2倍标准方差之外时，判定MeanShift搜索无效。本发明中跟踪算法的优点在于跟踪实时并且较为精确，基本能够满足无人机导引的需求。

步骤二：云台伺服控制部分。除了在图像中跟踪目标，仍然需要实时稳定的调节云台的偏转角度，使得运动目标始终保持在摄像机光轴位置或者离视野中心较近的区域。如果没有能够及时调整，运动目标将会跑出摄像机的视野范围，导致跟踪失败。具体实施过程为计算出脱靶量，反馈给伺服控制系统，驱动平台框架旋转，跟踪目标。脱靶量是指运动目标在成像平面中的位置坐标与成像平面中心在水平方向和垂直方向上的偏差。如果云台转动的幅度过大，可能会漏掉正在跟踪的目标；如果云台转动幅度过小，会由于赶不上被跟踪目标的运动速度而导致目标的丢失。因此必须设定合理的云台转动策略，才能提高跟踪的成功率，实现对目标的平滑跟踪。云台的伺服控制由位置环和速度环两个回路组成，为了满足光轴的稳定实时跟踪特性，根据速度回路和位置回路的开环剪切频率和相位裕度分别设计水平方向、俯仰方向的速度和位置超前、滞后控制器，实时消除目标与光轴的水平和俯仰方向上的偏差。

步骤三：目标定位。在视频处理部分我们得到了运动目标在摄像机成像平面中的位置信息，为了进一步解算出运动目标在世界坐标系中的位置，有必要建立影像中目标与现实环境中目标的对应关系。

在基于视觉的无人机动态目标跟踪与定位方法中，涉及的坐标系包括：惯性坐标系O_sX_sY_sZ_s（记为∑_s），机体坐标系O_bX_bY_bZ_b（记为∑_b），摄像机坐标系O_cX_cY_cZ_c（记为∑_c），参见图7。根据运动目标、摄像机和无人机的相对位姿关系，运动目标的位置p_t＝[x_t，y_t，z_t]^T可表示为：

p_t＝p₁+p₂+p₃    (22)

其中，向量p₁＝[x₁，y₁，z₁]^T表示无人机在惯性坐标系∑_s下的位置，它和无人机的姿态角都可以由传感器(如加速度计、陀螺仪等)直接测量得到；向量p₂＝[x₂，y₂，z₂]^T表示无人机质心到摄像机光心的距离，它可以由下式计算得到：

$p_2=R\&CenterDot;_b^b{}_s{p}_2---\left(23\right)$

其中^bp₂＝[^bx₂，^by₂，^bz₂]表示摄像机在机体坐标系∑_b下的位置，它可以通过标定预先得到。矩阵表示机体坐标系∑_b到惯性坐标系∑_s的旋转矩阵，它由无人机的姿态角（包括滚转角、俯仰角和偏航角）[φθψ]^T确定，具体表达见下式：

$R_b^s=\left[\begin{array}{ccc}\mathrm{c\&theta;c\&psi;}& \mathrm{s\&theta;s\&phi;c\&psi;}-\mathrm{c\&phi;s\&psi;}& \mathrm{s\&theta;c\&phi;c\&psi;}+\mathrm{s\&phi;s\&psi;}\\ \mathrm{c\&theta;s\&phi;}& \mathrm{s\&theta;s\&phi;s\&psi;}+\mathrm{c\&phi;c\&psi;}& \mathrm{s\&theta;c\&phi;s\&psi;}-\mathrm{s\&phi;c\&psi;}\\ -\mathrm{s\&theta;}& \mathrm{c\&theta;s\&phi;}& \mathrm{c\&theta;c\&phi;}\end{array}\right]---\left(24\right)$

其中c代表cos(·)，s代表sin(·)。式中，向量p₃＝[x₃，y₃，z₃]^T表示摄像机到运动目标的距离，它满足如下关系：

$p_3=R\&CenterDot;_c^b{}_b{}^s{R\&CenterDot;}^c{p}_3---\left(25\right)$

其中，^cp₃＝[^cx₃，^cy₃，^cz₃]^T为目标在摄像机坐标系∑_c下的位置，

表示摄像机坐标系∑_c到机体坐标系∑_b的旋转矩阵，它由云台摄像机的水平旋转角α和俯仰角β确定，它们可以利用云台上的码盘信息获得。具体表达见下式：

$R=_c^b\left[\begin{array}{ccc}-\mathrm{s\&alpha;}& \mathrm{s\&beta;c\&alpha;}& \mathrm{c\&beta;c\&alpha;}\\ \mathrm{c\&alpha;}& \mathrm{s\&beta;s\&alpha;}& \mathrm{c\&beta;s\&alpha;}\\ 0& \mathrm{c\&beta;}& -\mathrm{s\&beta;}\end{array}\right]---\left(26\right)$

将式(23)和式(25)代入式(22)，可以得到

$p_t=p_1+R\&CenterDot;_b^b{}_s{p}_2+R\&CenterDot;_c^b{}_b{}^s{R\&CenterDot;}^c{p}_3---\left(27\right)$

因此若要求取p_t，则要重点求解^cp₃(^cx₃，^cy₃，^cz₃)。如图8，O(u₀，v₀)为像平面的中心，p(u，v)为运动目标在成像平面中的像素坐标，目标点在图像平面的成像位置

可以利用针孔模型近似表示为：

$\left[\begin{array}{c}{u}_t^i\\ v_t^i\\ 1\end{array}\right]=\frac{1}{c_{z_3}}\&CenterDot;M\&CenterDot;\left[\begin{array}{c}{c}_{x_3}\\ c_{y_3}\\ c_{z_3}\end{array}\right]---\left(28\right)$

式中

表示摄像机的内参数矩阵：

$M=\left[\begin{array}{ccc}{a}_x& 0& u_0\\ 0& a_y& v_0\\ 0& 0& 1\end{array}\right]---\left(29\right)$

式中

f表示摄像机的焦距，dxdy表示每一个像素在x轴与y轴方向上的物理尺寸，[u₀，v₀]^T表示图像中心的像素值。这些值可以通过摄像机标定求得。进一步变换公式如下：

$\left[\begin{array}{c}{u}_t^i\\ v_t^i\end{array}\right]=\frac{1}{c_{z_3}}C\&CenterDot;M{\&CenterDot;}^c{p}_3---\left(30\right)$

式中

表示如下常数矩阵：

$C=\left[\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\end{array}\right]---\left(31\right)$

至此，若已知成像位置

和目标在摄像机坐标系中的深度信息^cz₃就可以求得运动目标在摄像机坐标下的位置^cp₃，根据公式(8)即可解算出惯性坐标系中的位置坐标[x_ty_tz_t]^T。运动目标在图像中的成像位置在视频处理部分已经得到，接下来重点求解深度信息。引入单目测距模型，如图9所示。

已知被跟踪运动目标的高度，由无线高度表获取无人机的飞行高度，由搭载摄像机的云台码盘信息得到光轴的俯仰角度，根据图9实时单目测距模型的几何关系，可以实时求解无人机到运动目标的水平距离，进而求解无人机到运动目标的直线距离。M表示被跟随运动目标，有效焦距为f的CCD摄像机安装在无人机的前下方，俯仰角度为β，距离地面的高度为h；被跟随运动目标上的被测点为P，P距地面高度为H，P与镜头中心的水平距离为d。

其中，O_o是镜头中心，O(x₀，y₀)是光轴与像平面的交点，作为像平面坐标系的原点；P′(x，y)是被测点P在像平面的投影。为了方便计算，通常先将运动目标与无人机的云台摄像机放置成直线，即P′点在

轴上，那么有几何关系：

j＝β+γ    (32)

tanj＝(h-H)/d    (33)

tanγ＝OP′/f    (34)

联立上面三式，根据几何关系有：

$d=\frac{h-H}{\tan (\mathrm{\&beta;}+\mathrm{\&gamma;})}=\frac{h-H}{\tan (\mathrm{\&beta;}+\arctan \frac{\mathrm{OP}\&prime;}{f})}---\left(35\right)$

其中，j为像点与物点的连线与水平方向的夹角，γ表示被测点P、镜头中心O_o连线与光轴间的夹角，代表运动目标在成像平面中与成像平面中心的角度偏差；无人机高度h可由无线高度表获得，H为被跟踪目标质心到地面高度大致可以估算出来，β为云台俯仰角度可以通过云台摄像机得到，OP′＝ΔY，目标中心与图像中心的偏差值可在检测跟踪模块中获得。至此，运动目标与摄像机的水平距离就可以计算得到。获知水平距离d，进一步计算该运动目标在摄像机坐标系中的景深^cz₃：

O_oP²＝(h-H)²+d²    (36)

$\mathrm{\&gamma;}=\arctan \frac{\mathrm{OP}\&prime;}{f}---\left(37\right)$

^cz₃＝cosγO_oP    (38)

即可求得目标在摄像机坐标系中的坐标。回到无人机目标跟踪与定位系统中，已知了无人机在惯性坐标系的位置[x₁，y₁，z₁]^T和摄像机在机体坐标系中位置[^bx₁，^bx₂ ^bx₃]^T，就可以解算出运动目标在三维世界中的位置坐标。

步骤四：无人机飞行控制部分。在完成运动目标的精确定位后，将目标的运动状态反馈给无人机，使得无人机可以根据具有双闭环结构的无人机地面目标跟踪飞行控制系统自主地跟踪地面动态目标飞行。其中，外环为导引控制器，其任务是根据无人机和动态目标的运动状态计算出期望位姿和速度，当无人机按照该位姿和速度飞行时，不仅可以与动态目标保持预期的相对位姿，同时还可以确保无人机在相对安全的模式下跟踪目标飞行。内环为无人机飞行控制器，其任务是使无人机能够按照期望的位姿和速度稳定飞行。

Claims (7)

1.一种基于视觉的无人机动态目标跟踪与定位方法，其特征在于，包括如下步骤：

第一步，视频处理，进行动态目标的检测和图像跟踪；

（1）动态目标检测：在连续两帧图像中，第一帧图像中提取特征点集，在第二帧图像中对这些特征点进行跟踪，得到第一帧图像中特征点集的对应特征点集，进一步去除对应特征点集中属于动态目标的特征点；用剩余的有效特征点集对中的各特征点对的运动矢量估计全局的运动，得到变换矩阵，从而补偿背景的运动，将动态背景下的动态目标检测转换为静态背景下的动态目标检测，帧差补偿背景后的两帧图像，用最小类间方差阈值法自动选取阈值产生相应的二值图像，对二值图像进行滤波处理，接着用最小外接矩形描述该动态目标，实现动态目标的检测；

（2）对检测出的动态目标在后续帧中采用融合基于偏移的均值向量跟踪和卡尔曼先验预估的算法连续跟踪，首先用卡尔曼滤波预估出动态目标在当前帧图像中的位置，然后将该位置信息传递给基于偏移的均值向量跟踪算法，该算法在该位置附近搜索目标，判定搜索结果是否有效，若是有效则进一步传递给卡尔曼滤波作为观测值进行下一步的预测；

第二步，云台伺服控制，实时调整云台的俯仰偏转角度，使目标保持在图像的中央；

第三步，建立影像中目标与现实环境中目标的对应关系，进一步测量摄像机与动态目标之间的距离，完成动态目标的精确定位；

第四步，无人机飞控系统进行自主跟踪地面动态目标飞行。

2.根据权利要求1所述的一种基于视觉的无人机动态目标跟踪与定位方法，其特征在于，去除对应特征点集中属于动态目标的特征点，得到有效特征点集，有效性判定法则如式(1)所示：

$\left\{\begin{array}{cc}{f}_i\&Element;F_{\mathrm{in}}& \mathrm{if}\left|\right|\left|V_i\right||-{\mathrm{\&mu;}}_{\left|\right|\&CenterDot;\left|\right|}|<3\&CenterDot;{\mathrm{\&sigma;}}_{\left|\right|\&CenterDot;\left|\right|}\mathrm{and}|\mathrm{Ang}\left(V_i\right)-{\mathrm{\&mu;}}_{\mathrm{Ang}}|<3\&CenterDot;{\mathrm{\&sigma;}}_{\mathrm{Ang}}\\ f_i\&Element;F_{\mathrm{out}}& \mathrm{otherwise}\end{array}\right.---\left(1\right)$

其中，f_i代表特征点集F^t中第i个特征点，V_i代表第i个特征点在连续帧图像中的运动光流向量，||V_i||，Ang(V_i)分别代表该运动光流向量的模和方向，(μ_||·||，σ_||·||)代表KLT特征点检测算法选定特征点在连续两帧图像中所有光流向量的模的期望和方差，(μ_Ang，σ_Ang)代表KLT特征点检测算法选定特征点在连续两帧图像中所有光流向量的方向的期望、方差，F_in代表有效特征点，F_out代表无效特征点。

3.根据权利要求1所述的一种基于视觉的无人机动态目标跟踪与定位方法，其特征在于，所述的变换矩阵采用六参数仿射模型来表示：

$\left[\begin{array}{c}{F}_x^t\\ F_y^t\\ 1\end{array}\right]=\left[\begin{array}{c}a{F}_x^{t-1}+b{F}_y^{t-1}+t_1\\ c{F}_x^{t-1}+d{F}_y^{t-1}+t_2\\ 1\end{array}\right]=\left[\begin{array}{ccc}a& b& t_1\\ c& d& t_2\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}{F}_x^{t-1}\\ F_y^{t-1}\\ 1\end{array}\right]=T_{t-1}^t\left[\begin{array}{c}{F}_x^{t-1}\\ F_y^{t-1}\\ 1\end{array}\right]---\left(2\right)$

其中

分别代表图像I^t特征点集的x和y值坐标，参数a，b，c，d，t₁，t₂用随机采样一致性法来拟合得到。

4.根据权利要求1所述的一种基于视觉的无人机动态目标跟踪与定位方法，其特征在于，所述的补偿背景的运动后的图像表示为：

$I_{\mathrm{comp}}^{t-1}=T_{t-1}^t{I}^{t-1}---\left(3\right)$

其中为

变换矩阵，I^t-1为补偿背景前的图像。

5.根据权利要求1所述的一种基于视觉的无人机动态目标跟踪与定位方法，其特征在于，所述的帧差补偿背景后的两帧图像，用下面的式子来定义差分后的图像I_diff(x,y)：

6.根据权利要求1所述的一种基于视觉的无人机动态目标跟踪与定位方法，其特征在于，所述的最大类间方差阈值法步骤如下：

假设一幅图像的灰度级为L，灰度值为i像素个数为n_i，总的像素数为N，每一个灰度值出现的概率为P_i，则有：

$N=\underset{i=1}{\overset{L}{\mathrm{\&Sigma;}}}{n}_i$ $P_i=\frac{n_i}{N}$ $\underset{i=0}{\overset{L-1}{\mathrm{\&Sigma;}}}{p}_i=1---\left(5\right)$

选择阈值T，将图像划分为两类：C₀：(0，1，2，...，T)；C₁：(T+1，T+2，...，L-1)；C₀和C₁类出现的概率分别为：

$p_0\left(T\right)=\underset{i=0}{\overset{T}{\mathrm{\&Sigma;}}}{p}_{i,}$ $p_1\left(T\right)=\underset{i=T+1}{\overset{L-1}{\mathrm{\&Sigma;}}}{p}_i---\left(6\right)$

这两类像素的平均灰度值分别为：

${\mathrm{\&mu;}}_0\left(T\right)=\frac{{\mathrm{\&Sigma;}}_{i=0}^T{\mathrm{ip}}_i}{p_0\left(T\right)},$ ${\mathrm{\&mu;}}_1\left(T\right)=\frac{{\mathrm{\&Sigma;}}_{i=T+1}^{L-1}{\mathrm{ip}}_i}{p_1\left(T\right)}---\left(7\right)$

整幅图像的平均灰度值为：

${\mathrm{\&mu;}}_T=\frac{{\mathrm{\&Sigma;}}_{i=0}^{L-1}{\mathrm{ip}}_i}{{\mathrm{\&Sigma;}}_{i=0}^{L-1}{p}_i}---\left(8\right)$

C₀和C₁类的方差分别为：

${\mathrm{\&sigma;}}_0^2\left(T\right)=\underset{i=0}{\overset{T}{\mathrm{\&Sigma;}}}\frac{{(i-{\mathrm{\&mu;}}_0)}^2{p}_i}{p_0}---\left(9\right)$

${\mathrm{\&sigma;}}_1^2\left(T\right)=\underset{i=T+1}{\overset{L-1}{\mathrm{\&Sigma;}}}\frac{{(i-{\mathrm{\&mu;}}_1)}^2{p}_i}{p_1}---\left(10\right)$

总方差为：

${\mathrm{\&sigma;}}_T^2=\underset{i=0}{\overset{L-1}{\mathrm{\&Sigma;}}}{(i-{\mathrm{\&mu;}}_T)}^2{p}_i={\mathrm{\&sigma;}}_W^2+{\mathrm{\&sigma;}}_B^2---\left(11\right)$

其中类内方差

和类间方差

分别为：

${\mathrm{\&sigma;}}_W^2\left(T\right)=p_0\left(T\right){\mathrm{\&sigma;}}_0^2\left(T\right)+p_1\left(T\right){\mathrm{\&sigma;}}_1^2\left(T\right)---\left(12\right)$

${\mathrm{\&sigma;}}_B^2\left(T\right)=p_0\left(T\right){({\mathrm{\&mu;}}_0\left(T\right)-{\mathrm{\&mu;}}_T)}^2+p_1\left(T\right){({\mathrm{\&mu;}}_1\left(T\right)-{\mathrm{\&mu;}}_T)}^2---\left(13\right)$

以类间方差作为衡量不同阈值导出的类别分离性能的测量准则，从直方图的左端开始，计算每个灰度值导出的类别的类间方差

自动确定使得类间方差

最大的阈值，当类间方差最大的时候，认为此时前景和背景差异最大，因此，最佳阈值为：

$t^{*}=\arg \underset{0\&le;T\&le;L-1}{\max }{\mathrm{\&sigma;}}_B^2\left(T\right)---\left(14\right).$

7.根据权利要求1所述的一种基于视觉的无人机动态目标跟踪与定位方法，其特征在于，所述的第三步具体为：

根据运动目标、摄像机和无人机的相对位姿关系，运动目标的位置p_t＝[x_t，y_t，z_t]^T表示为：

p_t＝p₁+p₂+p₃    (22)

其中，向量p₁＝[x₁，y₁，z₁]^T表示无人机在惯性坐标系∑_s下的位置；向量p₂＝[x₂，y₂，z₂]^T表示无人机质心到摄像机光心的距离，由下式计算得到：

$p_2=R\&CenterDot;_b^b{}_s{p}_2---\left(23\right)$

其中^bp₂＝[^bx₂，^by₂，^bz₂]表示摄像机在机体坐标系∑_b下的位置，矩阵

表示机体坐标系∑_b到惯性坐标系∑_s的旋转矩阵，具体表达见下式：

$R_b^s=\left[\begin{array}{ccc}\mathrm{c\&theta;c\&psi;}& \mathrm{s\&theta;s\&phi;c\&psi;}-\mathrm{c\&phi;s\&psi;}& \mathrm{s\&theta;c\&phi;c\&psi;}+\mathrm{s\&phi;s\&psi;}\\ \mathrm{c\&theta;s\&phi;}& \mathrm{s\&theta;s\&phi;s\&psi;}+\mathrm{c\&phi;c\&psi;}& \mathrm{s\&theta;c\&phi;s\&psi;}-\mathrm{s\&phi;c\&psi;}\\ -\mathrm{s\&theta;}& \mathrm{c\&theta;s\&phi;}& \mathrm{c\&theta;c\&phi;}\end{array}\right]---\left(24\right)$

其中c代表cos(·)，s代表sin(·)，其中θ为无人机的俯仰角，φ为无人机的横滚角，ψ为无人机的航向角；

向量p₃＝[x₃，y₃，z₃]^T表示摄像机到运动目标的距离，满足如下关系：

$p_3=R\&CenterDot;_c^b{}_b{}^s{R\&CenterDot;}^c{p}_3---\left(25\right)$

其中，^cp₃＝[^cx₃，^cy₃，^cz₃]^T为目标在摄像机坐标系∑_c下的位置，

表示摄像机坐标系∑_c到机体坐标系∑_b的旋转矩阵，具体表达见下式：

$R_c^b=\left[\begin{array}{ccc}-\mathrm{s\&alpha;}& \mathrm{s\&beta;c\&alpha;}& \mathrm{c\&beta;c\&alpha;}\\ \mathrm{c\&alpha;}& \mathrm{s\&beta;s\&alpha;}& \mathrm{c\&beta;s\&alpha;}\\ 0& \mathrm{c\&beta;}& -\mathrm{s\&beta;}\end{array}\right]---\left(26\right)$

将式(23)和式(25)代入式(22)，得到

$p_t=p_1+R\&CenterDot;_b^b{}_s{p}_2+R\&CenterDot;_c^b{}_b{}^s{R\&CenterDot;}^c{p}_3---\left(27\right)$

目标点在图像平面的成像位置

利用针孔模型近似表示为：

$\left[\begin{array}{c}{u}_t^i\\ v_t^i\\ 1\end{array}\right]=\frac{1}{c_{z_3}}\&CenterDot;M\&CenterDot;\left[\begin{array}{c}{c}_{x_3}\\ c_{y_3}\\ c_{z_3}\end{array}\right]---\left(28\right)$

式中

表示摄像机的内参数矩阵：

$M=\left[\begin{array}{ccc}{a}_x& 0& u_0\\ 0& a_y& v_0\\ 0& 0& 1\end{array}\right]---\left(29\right)$

式中

f表示摄像机的焦距，dx dy表示每一个像素在x轴与y轴方向上的物理尺寸，[u₀，v₀]^T表示图像中心的像素值，进一步变换公式如下：

$\left[\begin{array}{c}{u}_t^i\\ v_t^i\end{array}\right]=\frac{1}{c_{z_3}}C\&CenterDot;{M\&CenterDot;}^c{p}_3---\left(30\right)$

式中

表示如下常数矩阵：

$C=\left[\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\end{array}\right]---\left(31\right)$

^cz₃＝cosγO_oP    (38)

γ表示被测点P、镜头中心O_o连线与光轴间的夹角，O_oP表示摄像机镜头中心与运动目标上的被测点P之间的距离。

Images