CN105652891A - 一种旋翼无人机移动目标自主跟踪装置及其控制方法 - Google Patents

Abstract

本发明涉及旋翼无人机自主跟踪的技术领域，更具体地，涉及一种基于云台摄像头和机载传感器的旋翼无人机移动目标自主跟踪装置。一种旋翼无人机移动目标自主跟踪装置，包括飞行器和地面站，飞行器包括飞行器控制器，与飞行器控制器连接的飞行器螺旋桨电机、云台电机，所述的云台电机连接云台摄像头；飞行器和地面站之间设有无线图像传输模块和无线数据传输模块，飞行器与地面站通过无线图像传输模块和无线数据传输模块连接；以机载云台摄像头获得的图像作为系统的输入，在地面站通过Struck算法获得目标在图像中的坐标，采用匀加速运动（CA）模型对目标坐标进行建模，根据建模估计的目标坐标设计云台的控制量，以保证云台摄像头跟踪的实时性和平滑性。

Description

一种旋翼无人机移动目标自主跟踪装置及其控制方法

技术领域

本发明涉及旋翼无人机自主跟踪的技术领域，更具体地，涉及一种基于云台摄像头和机载传感器的旋翼无人机移动目标自主跟踪装置。

背景技术

四旋翼飞行器由于其相对固定翼飞行器具有可垂直起落、低空悬停、机动转向快速的优点，被广泛应用在军事、农业、航空拍摄等领域。四旋翼飞行器如何对摄像头采集的目标进行有效的识别跟踪并实现跟随是飞行器研究领域中一个热点问题。对于机动目标的跟踪不同于对静止目标的跟踪，目标与飞行器的相对位置不仅会因飞行器的运动变化，目标自身的运动也会产生较大的相对距离变化。这不仅给图像目标的检测跟踪带来了很大的难度，对于具有较大惯性，无法快速变换方向的飞行器来说也是一个难题。

飞行器实现自主跟踪主要是通过设计目标检测算法、目标状态估计算法和飞行器控制算法，根据图像信息确定目标位置，从而调整云台角度和飞行器飞行方向及速度，从而使飞行器能够保持对移动目标的有效跟踪。

在目标检测算法中，基本都是只考虑摄像头固定的情况。普通跟踪常用的基于颜色直方图进行区域块匹配的方法和基于光流场对像素点运动进行检测的方法，它们对目标的跟踪都是基于环境变化比较微弱的假设下进行，实际跟踪时要求有比较固定的外部环境。但是，在飞行器跟踪问题中，外部环境时常会因为目标或者飞行器的运动而产生变化。基于机器学习的Tracking,learninganddetection(TLD)算法使用过去的目标样本对当前图像进行区域匹配，寻找最相似的区域作为跟踪的目标。由于TLD算法是对过去的目标图片使用模板匹配算法进行全图搜索比对，计算量会随着运行时间而增长，不适合于飞行器自主跟踪这种对实时性要求很高的情况。

现有的目标状态估计算法以滤波技术为主，包括卡尔曼滤波器，扩展卡尔曼滤波器、无迹卡尔曼滤波器、粒子滤波器等。此外，有利用图像检测特征点，通过特征点之间的位置约束确定它们的运动速度从而估计摄像头和目标运动状态的方法；同时还有提出利用图像背景运动速度获得目标速度和飞行器速度的方法。

对于飞行器的控制，一般分为基于当前时刻目标位置的地面目标跟踪和基于有限步长预测状态的地面目标跟踪方法。基于当前时刻目标位置的地面目标跟踪方法是每次根据测量的当前目标位置给出飞行器的期望速度或者期望位置，而基于有限步长预测状态的地面目标跟踪方法则在估计位置的基础上利用观测信息和目标运动模型的模型预测得到预测值，以此减少跟踪滞后，提高飞行器自主跟踪的实时性。

在目前对飞行器跟踪问题的研究中，大部分都是针对单个子问题进行优化，而较少工作对整个跟踪系统进行考虑。飞行器跟随目标的准确性和快速性与图像上跟踪目标的稳定性是互相影响的，不仅要考虑飞行器运动给云台摄像头跟踪带来的影响，还要考虑图像跟踪算法的实时性和鲁棒性。另外，对飞行器的跟踪引导较少考虑飞行器自身的惯性约束，且计算量较大的航迹规划因为实时性要求难以直接运用，因此通常直接以目标位置的当前运动轨迹作为飞行器的参考轨迹，而没有根据目标的不同运动状况进行分类讨论，这会导致飞行器频繁跟随移动目标进行变换方向，从而使得飞行过程不连续，降低了飞行器自主跟随运动的机动性。

发明内容

本发明为克服上述现有技术所述的至少一种缺陷，提供一种旋翼无人机移动目标自主跟踪装置及其控制方法，以机载云台摄像头获得的图像作为系统的输入，在地面站通过Struck算法获得目标在图像中的坐标，采用匀加速运动(CA)模型对目标坐标进行建模，根据建模估计的目标坐标设计云台的控制量，以保证云台摄像头跟踪的实时性和平滑性。

为解决上述技术问题，本发明采用的技术方案是：一种旋翼无人机移动目标自主跟踪装置，其中，包括飞行器和地面站，所述的飞行器包括飞行器控制器，与飞行器控制器连接的飞行器螺旋桨电机、云台电机，所述的云台电机连接云台摄像头；

所述的飞行器和地面站之间设有无线图像传输模块和无线数据传输模块，飞行器与地面站通过无线图像传输模块和无线数据传输模块连接；

所述的地面站包括图像跟踪模块、云台控制模块、目标位置估算模块、飞行器控制模块。

本发明中，使用struck算法作为移动目标检测算法，用以处理机载云台摄像头通过无线图像传输模块发回的图像信息，估计得到目标在图像上的坐标，并以此作为整个无人机跟踪系统的输入。针对云台控制在平台上的局限，采用对目标图像坐标进行运动建模的方法，将目标坐标变化假想为CA模型，预测目标坐标下一帧的可能位置，从而使云台控制更加平滑。对于弱机动目标跟踪，除了利用机载传感器获取飞行器与云台的姿态信息进行坐标系换算，还设计一个含有CV运动模型和当前统计模型的模型集，将目标的运动模式映射为该模型集，基于这两个模型的滤波器同时工作，将每个滤波器产生的估计状态进行联合加权，从而得到目标状态的最终估计。针对强机动目标跟踪，设计不同情况下的跟踪策略，尽量减少飞行器的变向次数，使飞行器飞行过程中姿态更加平稳，轨迹更加稳定。

本发明通过安装在飞行器机体下方的云台摄像头获取图像，图像经过无线图像传输模块发回至地面站，压缩成320×240的大小后作为跟踪系统的输入。跟踪系统包括图像跟踪模块、云台控制模块、目标位置估算模块、飞行器控制模块。

进一步的，所述的飞行器上设有气压计。所述的飞行器上设有磁力计。所述的飞行器上设有惯性测量单元。所述的云台摄像头设于飞行器机体下方；所述的气压计、磁力计、惯性测量单元均与无线数据传输模块连接。

气压计可测量高度，磁力计可测量朝向，惯性测量单元可测量飞行器的姿态角，气压计、磁力计、惯性测量单元均与无线数据传输模块连接，把上述高度、朝向、飞行器的姿态角发送到无线数据传输模块。

进一步的，利用所述的旋翼无人机移动目标自主跟踪装置的控制方法，其中，包括以下步骤：

S1.云台摄像头将图像信号输送到无线图像传输模块，无线图像传输模块再输送到图像跟踪模块，图像跟踪模块采用struck算法，得到的目标图像坐标发送给云台控制模块和目标位置估算模块；

S2.云台控制模块在获得图像跟踪模块发送的图像坐标后，为目标建立运动模型，得到下一时刻的估计坐标，提供给指令发送模块控制云台的转动；

S3.目标位置估算模块接收到目标图像坐标后结合机载传感器的数据计算目标与飞行器的相对位置，将经过IMM算法得到估计的目标状态，发送给飞行器控制模块；

S4.飞行器控制模块根据目标状态决定控制策略从而改变自己的姿态角进行移动。

与现有技术相比，有益效果是：本发明针对四旋翼无人机平台对移动目标的自主跟踪问题，设计了一种基于云台摄像头和机载传感器的旋翼无人机移动目标自主跟踪方法。通过云台摄像头采集图像，使用struck算法从图像中检测识别出目标。对目标的图像坐标进行建模以预测目标的坐标位置，通过预测的坐标计算云台需要偏转的偏航角和俯仰角，提高了云台摄像头跟踪的实时性和平滑性。另外，结合机载传感器和图像坐标计算得到目标的位置，使用交互多模型的算法对目标进行滤波预测，从而根据不同的距离和目标运动状态设计相应的飞行器控制方案。

(一)、使用匀加速运动(CA)模型对移动目标在图像中的坐标进行建模，结合图像的过去信息计算目标的速度和加速度，预测目标在下一时刻的图像坐标；

(二)、对估计出的目标的运动状态进行分类讨论，根据目标运动加速度和方向的不同将飞行器切换至悬停模或跟踪模式，在跟踪模式下飞行器的位置跟随目标运动，在悬停模式下只改变偏航角，只通过云台来进跟踪，减少飞行器的变向次数，使飞行器的飞行姿态更加平稳，轨迹更加稳定。

附图说明

图1是系统模块结构示意图。

图2是飞行器与目标位置关系示意图。

图3是摄像头与目标位置关系示意图。

具体实施方式

附图仅用于示例性说明，不能理解为对本专利的限制；为了更好说明本实施例，附图某些部件会有省略、放大或缩小，并不代表实际产品的尺寸；对于本领域技术人员来说，附图中某些公知结构及其说明可能省略是可以理解的。附图中描述位置关系仅用于示例性说明，不能理解为对本专利的限制。

如图1所示，一种旋翼无人机移动目标自主跟踪装置，其中，包括飞行器和地面站，所述的飞行器包括飞行器控制器，与飞行器控制器连接的飞行器螺旋桨电机、云台电机，所述的云台电机连接云台摄像头；

所述的飞行器和地面站之间设有无线图像传输模块和无线数据传输模块，飞行器与地面站通过无线图像传输模块和无线数据传输模块连接；

所述的地面站包括图像跟踪模块、云台控制模块、目标位置估算模块、飞行器控制模块。

本发明中，使用struck算法作为移动目标检测算法，用以处理机载云台摄像头通过无线图像传输模块发回的图像信息，估计得到目标在图像上的坐标，并以此作为整个无人机跟踪系统的输入。针对云台控制在平台上的局限，采用对目标图像坐标进行运动建模的方法，将目标坐标变化假想为CA模型，预测目标坐标下一帧的可能位置，从而使云台控制更加平滑。对于弱机动目标跟踪，除了利用机载传感器获取飞行器与云台的姿态信息进行坐标系换算，还设计一个含有CV运动模型和当前统计模型的模型集，将目标的运动模式映射为该模型集，基于这两个模型的滤波器同时工作，将每个滤波器产生的估计状态进行联合加权，从而得到目标状态的最终估计。针对强机动目标跟踪，设计不同情况下的跟踪策略，尽量减少飞行器的变向次数，使飞行器飞行过程中姿态更加平稳，轨迹更加稳定。

本发明通过安装在飞行器机体下方的云台摄像头获取图像，图像经过无线图像传输模块发回至地面站，压缩成320×240的大小后作为跟踪系统的输入。跟踪系统包括图像跟踪模块、云台控制模块、目标位置估算模块、飞行器控制模块。

进一步的，所述的飞行器上设有气压计。所述的飞行器上设有磁力计。所述的飞行器上设有惯性测量单元。所述的云台摄像头设于飞行器机体下方；所述的气压计、磁力计、惯性测量单元均与无线数据传输模块连接。

气压计可测量高度，磁力计可测量朝向，惯性测量单元可测量飞行器的姿态角，气压计、磁力计、惯性测量单元均与无线数据传输模块连接，把上述高度、朝向、飞行器的姿态角发送到无线数据传输模块。

本实施例中，基于机载传感器能估计得到飞行器的飞行高度以及云台和飞行器各自的姿态信息。同时，根据估计的目标坐标，并且结合机载传感器获得的飞行器与云台的姿态，可以通过坐标变换计算得到移动目标相对飞行器的位置。此外，我们设计了一套基于当前统计模型运动模型和匀速直线运动(CV)模型的交互多模型状态估计算法对移动目标的运动进行滤波预测，从而根据不同的距离和目标运动状态设计相应的飞行器控制方案，为飞行器对强机动目标的自主跟踪提供一种具有较好鲁棒性的跟踪方法。

如图2中，飞行器与目标位置关系示意图，点P是飞行器，OXY是世界坐标系下的地面，PN是目标T在摄像头坐标系下的成像深度，M是光轴与OXY平面的交点。OM与y轴夹角∠MOy可由云台偏航角与飞行器偏航角相减得到。

如图3中，摄像头与目标关系示意图，点C是摄像头，T′是T在POM面上的投影，TT’⊥PNT′。

利用所述的旋翼无人机移动目标自主跟踪装置的控制方法，其中，包括以下步骤：

S1.云台摄像头将图像信号输送到无线图像传输模块，无线图像传输模块再输送到图像跟踪模块，图像跟踪模块采用struck算法，得到的目标图像坐标发送给云台控制模块和目标位置估算模块；

S2.云台控制模块在获得图像跟踪模块发送的图像坐标后，为目标建立运动模型，得到下一时刻的估计坐标，提供给指令发送模块控制云台的转动；

S3.目标位置估算模块接收到目标图像坐标后结合机载传感器的数据计算目标与飞行器的相对位置，将经过IMM算法得到估计的目标状态，发送给飞行器控制模块；

S4.飞行器控制模块根据目标状态决定控制策略从而改变自己的姿态角进行移动。

具体的，所述的步骤S1中，

Struck算法使用结构输出的支持向量机算法，将样本描述为区域和变换的组合也即(x，y)，其中y是待求的目标区域变化函数，将目标位置变化函数与分类结合在一起可以得到一个区分函数：

这个区分函数的作用是给定坐标区域x和平面变换y，可以分别计算原坐标区域和变换后的坐标区域的haar特征值并比较两个区域之间的相似度并评分；好处是将这个区分函数用于预测目标可以同时把分类问题和判定相似度的问题合并成一个参数最优化的问题：

$y_t=f\left(x_t^{P_{t-1}}\right)=\arg \underset{y\∈\mathrm{\γ}}{max}F(x_t^{P_{t-1}},y)---\left(2\right)$

其中是目标上一帧的位置，Υ是所有可能的变化组成的空间；

通过把区分函数约束成带有核函数的形式：

F(x，y)＝〈w，Φ(x，y)〉(3)

可以将具有一系列样本{(x₁，y₁)…(x_n，y_n)}的跟踪问题简化成可以使用支持向量机模型求解的形式：

$\begin{array}{c}\underset{w}{\min }\frac{1}{2}\left|\right|w|{|}^2+C\underset{i=1}{\overset{n}{\mathrm{\&Sigma;}}}{\mathrm{\&xi;}}_i\\ \begin{array}{ccc}s.t.& \&ForAll;i:{\mathrm{\&xi;}}_i\&GreaterEqual;0& \&ForAll;i,\&ForAll;y\&NotEqual;y_i:<w,{\mathrm{\&delta;\&Phi;}}_i\left(y\right)>\&GreaterEqual;\mathrm{\&Delta;}(y_i,y)-{\mathrm{\&xi;}}_i\end{array}\\ {\mathrm{where\&delta;\&Phi;}}_i\left(y\right)=\mathrm{\&Phi;}(x_i,y_i)-\mathrm{\&Phi;}(x_i,y)\end{array}---\left(4\right)$

损失函数可以定义为区域变换后的重叠度：

$\mathrm{\&Delta;}(y,\stackrel{\&OverBar;}{y})=1-S_{P_t}^O(y,\stackrel{\&OverBar;}{y})---\left(5\right)$

使用拉格朗日算子将约束和最优化问题结合起来，可以得到式子：

$\begin{array}{c}\underset{\mathrm{\&alpha;}}{\max }\underset{i,\stackrel{\&OverBar;}{y}\&NotEqual;y_i}{\mathrm{\&Sigma;}}\mathrm{\&Delta;}(y,y_i){\mathrm{\&alpha;}}_i^y-\frac{1}{2}\underset{j,\stackrel{\&OverBar;}{y}\&NotEqual;y_j}{\underset{i,y\&NotEqual;y_i}{\mathrm{\&Sigma;}}}{\mathrm{\&alpha;}}_i^y{\mathrm{\&alpha;}}_j^{\stackrel{\&OverBar;}{y}}<{\mathrm{\&delta;\&Phi;}}_i\left(y\right),{\mathrm{\&delta;\&Phi;}}_j\left(\stackrel{\&OverBar;}{y}\right)>\\ \begin{array}{ccc}s.t.& \&ForAll;i,\&ForAll;y\&NotEqual;y_i:{\mathrm{\&alpha;}}_i^y\&GreaterEqual;0& \&ForAll;i:\underset{y\&NotEqual;y_i}{\mathrm{\&Sigma;}}{\mathrm{\&alpha;}}_i^y\&le;C\end{array}\end{array}---\left(7\right)$

定义变量

${\mathrm{\&beta;}}_i^y=\left\{\begin{array}{cc}-{\mathrm{\&alpha;}}_i^y& \begin{array}{cc}if& y\&NotEqual;y_i\end{array}\\ \underset{\stackrel{\&OverBar;}{y}\&NotEqual;y_j}{\mathrm{\&Sigma;}}{\mathrm{\&alpha;}}_i^{\stackrel{\&OverBar;}{y}}& otherwise\end{array}\right.---\left(8\right)$

可以将问题简化成：

$\begin{array}{c}\underset{\mathrm{\&alpha;}}{\max }\underset{i,y}{\mathrm{\&Sigma;}}\mathrm{\&Delta;}(y,y_i){\mathrm{\&beta;}}_i^y-\frac{1}{2}\underset{i,y,j,\stackrel{\&OverBar;}{y}}{\mathrm{\&Sigma;}}{\mathrm{\&beta;}}_i^y{\mathrm{\&beta;}}_j^{\stackrel{\&OverBar;}{y}}<\mathrm{\&Phi;}(x_i,y),\mathrm{\&Phi;}(x_j,\stackrel{\&OverBar;}{y})>\\ \begin{array}{ccc}s.t.& \&ForAll;i,\&ForAll;y:{\mathrm{\&beta;}}_i^y\&GreaterEqual;\mathrm{\&delta;}(y,y_i)C& \&ForAll;i:\underset{y}{\mathrm{\&Sigma;}}{\mathrm{\&beta;}}_i^y=0\end{array}\end{array}---\left(9\right)$

在算法过程中，使用SMO算法求解这个最优化问题，每次为当前样本选择一个最好的正样本和负样本，然后将有效的样本加入支持向量，同时调整已有支持向量的系数，舍弃调整后不符合约束的支持向量，过程中一直保持有效支持向量的数目不超过100，超过的话根据公式：

$\begin{array}{c}\left|\right|\mathrm{\&Delta;}w|{|}^2={\mathrm{\&beta;}}_r^{y^2}\{<\mathrm{\&Phi;}(x_r,y),\mathrm{\&Phi;}(x_i,y)>+<\mathrm{\&Phi;}(x_r,y_r),\mathrm{\&Phi;}(x_r,y_r)>\\ -2<\mathrm{\&Phi;}(x_r,y_r),\mathrm{\&Phi;}(x_r,y_r)>\}\end{array}---\left(10\right)$

去掉负支持向量(x_r，y)；

根据正支持向量得到的样本重新在它们周围进行采样，从众多样本中选择使区分函数的值最大的一个区域作为跟踪结果。

所述的步骤S2中，

在得到目标图像坐标后，基于帧与帧的间隔时间较短，可以认为目标的坐标在图像上的运动是在同一直线上的变加速运动；利用这样的假设，可以为目标坐标建立一个运动模型；记目标在t时刻的坐标为(u(t)，v(t))，可以分别对横坐标u(t)和纵坐标v(t)建立一个CA模型，也即：

$\left[\begin{array}{c}u\left(t\right)\\ \stackrel{\&CenterDot;}{u}\left(t\right)\\ \stackrel{\&CenterDot;\&CenterDot;}{u}\left(t\right)\end{array}\right]=\left[\begin{array}{ccc}1& \mathrm{\&Delta;}t& \frac{1}{2}{\mathrm{\&Delta;t}}^2\\ 0& 1& \mathrm{\&Delta;}t\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}u\left(t\right)\\ \stackrel{\&CenterDot;}{u}\left(t\right)\\ \stackrel{\&CenterDot;\&CenterDot;}{u}\left(t\right)\end{array}\right]---\left(11\right)$

$\left[\begin{array}{c}v\left(t\right)\\ \stackrel{\&CenterDot;}{v}\left(t\right)\\ \stackrel{\&CenterDot;\&CenterDot;}{v}\left(t\right)\end{array}\right]=\left[\begin{array}{ccc}1& \mathrm{\&Delta;}t& \frac{1}{2}{\mathrm{\&Delta;t}}^2\\ 0& 1& \mathrm{\&Delta;}t\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}v\left(t\right)\\ \stackrel{\&CenterDot;}{v}\left(t\right)\\ \stackrel{\&CenterDot;\&CenterDot;}{v}\left(t\right)\end{array}\right]---\left(12\right)$

同时基于前四帧的图像坐标可以差分得到目标坐标的前一时刻的速度和加速度：

$\stackrel{\&CenterDot;}{u}\left(t\right)=\frac{1}{4}\left(u\right(t-1)-u(t-3)+u(t-2)-u(t-4\left)\right)---\left(13\right)$

$\stackrel{\&CenterDot;\&CenterDot;}{u}\left(t\right)=\frac{1}{4}\left(u\right(t-1)-u(t-3)-u(t-2)+u(t-4\left)\right)---\left(14\right)$

$\stackrel{\&CenterDot;}{v}\left(t\right)=\frac{1}{4}\left(v\right(t-1)-v(t-3)+v(t-2)-v(t-4\left)\right)---\left(15\right)$

$\stackrel{\&CenterDot;\&CenterDot;}{v}\left(t\right)=\frac{1}{4}\left(v\right(t-1)-v(t-3)-v(t-2)+v(t-4\left)\right)---\left(16\right)$

将式(14-16)代入CA模型用以预测移动目标下一时刻的坐标，这个预测的坐标能够结合目标在图像的运动趋势和云台的控制性能，可使云台在跟踪过程中的运动更为平滑，从而避免由于开环控制所带来的云台控制超调振荡的情况；通过预测的坐标计算云台需要偏转的偏航角和俯仰角：

$E\left(\mathrm{\&alpha;}\right)=\frac{\hat{v}}{f_W}{\mathrm{\&theta;}}_W---\left(17\right)$

$E\left(\mathrm{\&beta;}\right)=\frac{\hat{u}}{f_H}{\mathrm{\&theta;}}_H---\left(18\right)$

其中f_H、f_W是图像高度和宽度，θ_H和θ_W是摄像头纵向和横向成像视角。

所述的步骤S3中，基于目标在世界坐标系下的运动高度不变的假设，可以将目标运动模型简化到二维平面，目标与飞行器的位置关系如图2所示。

摄像头与目标位置的关系如图3所示。

图3中，∠TPT′和∠NPT′可通过摄像头成像原理计算得到：

$\&angle;{\mathrm{TPT}}^{\&prime;}=\frac{u}{f_H}{\mathrm{\&theta;}}_H---\left(19\right)$

$\&angle;{\mathrm{NPT}}^{\&prime;}=\frac{v}{f_W}{\mathrm{\&theta;}}_W---\left(20\right)$

其中，f_H、f_W分别为图像高度和宽度，θ_H和θ_H分别为摄像头纵向和横向成像视角；OP是通过机载气压计或超声波传感器估计的飞行器的高度，∠OPM可以通过云台俯仰角获得；由三角关系可得

${\mathrm{PT}}^{,}=\frac{OP}{\cos {\mathrm{OPT}}^{,}}---\left(21\right)$

OT’＝OPtanOPT’(22)

TM＝OP(tanOPT’-tanOPM)(23)

T’N＝T’McosOPM(24)

$T^{,}T=T^{,}N\frac{u}{v}---\left(25\right)$

$\&angle;{\mathrm{TOT}}^{\&prime;}={\tan }^{-1}\frac{T^{,}T}{{\mathrm{OT}}^{,}}---\left(26\right)$

$OT=\sqrt{{\mathrm{OT}}^{,2}+T^{,}{T}^2}---\left(27\right)$

由此，可以得到目标相对于飞行器的坐标为：

d_x＝OTsin(∠MOy+∠TOM)(28)

d_y＝OTcos(∠MOy+∠TOM)(29)

因为得到的目标位置是通过几何换算得到的，传感器精度带来的观测误差会影响飞行器的跟踪，同时基于目标当前状态的跟踪会带来较大的超调，所以需要对目标状态进行滤波和预测；跟踪的目标状态可能是静止、弱机动、强机动，无法用一个确定的模型来描述，因此在对目标状态进行估计的时候需要考虑模型的改变；使用匀速直线运动CV模型和当前统计模型作为系统模型集使用交互式多模型算法IMM算法去预测目标状态；记采样间隔为T，由于x方向和y方向的预测是等价的，以x方向为例说明目标状态的预测；

CV模型也即匀速直线运动模型，设目标离散运动模型和观测模型为：

X(k+1)＝ΦX(k)+Γ(k)W(k)(30)

Z(k)＝C(k)X(k)+V(k)(31)

其中状态量 $X\left(k\right)=\&lsqb;\begin{array}{cccccc}x& y& \stackrel{\&CenterDot;}{x}& \stackrel{\&CenterDot;}{y}& \stackrel{\&CenterDot;\&CenterDot;}{x}& \stackrel{\&CenterDot;\&CenterDot;}{y}\end{array}\&rsqb;,$ Φ是状态转移矩阵，Γ是过程噪声分布矩阵，W(k)和V(k)是均值为零相互独立且不相关的高斯白噪声，Z(k)是观测向量，C(k)是测量函数；

其状态转移矩阵为：

$\mathrm{\&Phi;}=\left[\begin{array}{ccc}I& TI& 0\\ 0& 1& 0\\ 0& 0& I\end{array}\right]---\left(32\right)$

过程噪声分布矩阵为：

$\mathrm{\&Gamma;}\left(k\right)=\left[\begin{array}{c}TI\\ I\\ 0\end{array}\right]---\left(33\right)$

当前统计模型是一种非零均值时间相关模型，它假设当前时刻加速度均值为前一时刻加速度的估计值，可以用来对机动目标进行建模；当前统计模型认为目标当前下一时刻的加速度只能在当前加速度的领域内，即假定加速度a(k)满足：

$a\left(k\right)=\tilde{a}\left(k\right)+\stackrel{\&OverBar;}{a}\left(k\right)---\left(34\right)$

其中，是零均值的一阶马尔可夫过程，是加速度的均值；

当前统计模型的离散状态方程为：

$X(k+1)=\mathrm{\&Phi;}X\left(k\right)+U\left(k\right)\stackrel{\&OverBar;}{a}\left(k\right)+W\left(k\right)---\left(35\right)$

其中，

$\mathrm{\&Phi;}=\left[\begin{array}{ccc}I& TI& \frac{e^{-\mathrm{\&alpha;}T}+\mathrm{\&alpha;}T-1}{{\mathrm{\&alpha;}}^2}I\\ 0& I& \frac{1-e^{-\mathrm{\&alpha;}T}}{\mathrm{\&alpha;}}1\\ 0& 0& e^{-\mathrm{\&alpha;}T}I\end{array}\right]---\left(36\right)$

$U\left(k\right)=\left[\begin{array}{c}\frac{-\mathrm{\&alpha;}T+{\mathrm{\&alpha;}}^2\frac{T}{2}+(1-e^{-\mathrm{\&alpha;}T})}{{\mathrm{\&alpha;}}^2}\\ \frac{\mathrm{\&alpha;}T-(1-e^{-\mathrm{\&alpha;}T})}{\mathrm{\&alpha;}}\\ 1-e^{-\mathrm{\&alpha;}T}\end{array}\right]---\left(37\right)$

IMM算法通过马尔可夫链描述模型集中的M个模型的切换，它的步骤归纳如下：

交互

计算模型i和模型j的混合概率μ^i|j：

${\stackrel{\&OverBar;}{c}}_j=\underset{i=1}{\overset{M}{\mathrm{\&Sigma;}}}{p}_{ij}{\mathrm{\&mu;}}^i\&lsqb;k-1\&rsqb;,{\mathrm{\&mu;}}^{i|j}=\frac{1}{{\stackrel{\&OverBar;}{c}}_j}\underset{i=1}{\overset{M}{\mathrm{\&Sigma;}}}{p}_{ij}{\mathrm{\&mu;}}^i\&lsqb;k-1\&rsqb;---\left(38\right)$

p_ij为下一时刻从模型i切换到模型j的概率，μⁱ[k-1]为模型i在k-1时刻的概率，表示归一化系数；

每个滤波器的混合输入计算为：

${\hat{X}}_{0j}(k-1)=\underset{i=1}{\overset{M}{\mathrm{\&Sigma;}}}{\mathrm{\&mu;}}^{i|j}\left(k\right){\hat{X}}_i(k-1)---\left(39\right)$

$\begin{array}{c}{P}_{0j}(k-1)=\underset{i=1}{\overset{M}{\mathrm{\&Sigma;}}}{\mathrm{\&mu;}}^{i|j}\left(k\right)\{P_i(k-1)\\ +\&lsqb;{\hat{X}}_i(k-1)-{\hat{X}}_{0j}(k-1)\&rsqb;{\&lsqb;{\hat{X}}_i(k-1)-{\hat{X}}_{0j}(k-1)\&rsqb;}^T\}\end{array}---\left(40\right)$

其中，P_i(k-1)分别表示模型i在上一时刻更新的估计值和协方差；

滤波

基于每个模型j使用UKF进行滤波，同时计算每个滤波器的观测似然函数

Λ^j(k)＝N(V^J(k)，S^j(k))(41)

V^I(k)为模型j在UKF滤波更新后的测量残差，S^j(k)是协方差；模型j在k时刻的概率计算为：

$c=\underset{i=1}{\overset{M}{\mathrm{\&Sigma;}}}{\mathrm{\&Lambda;}}^i\left(k\right){\stackrel{\&OverBar;}{c}}_i,{\mathrm{\&mu;}}^j\left(k\right)=\frac{1}{c}{\mathrm{\&Lambda;}}^j\left(k\right){\stackrel{\&OverBar;}{c}}_j---\left(42\right)$

联合

最后计算各个滤波器的状态值和协方差的联合估计：

$\hat{X}\left(k\right)=\underset{i=1}{\overset{M}{\mathrm{\&Sigma;}}}{\mathrm{\&mu;}}^i\left(k\right){\hat{X}}_i\left(k\right)---\left(43\right)$

$P\left(k\right)=\underset{i=1}{\overset{M}{\mathrm{\&Sigma;}}}{\mathrm{\&mu;}}^i\left(k\right)\{P_i\left(k\right)+\&lsqb;{\hat{X}}_i\left(k\right)-\hat{X}\left(k\right)\&rsqb;{\&lsqb;{\hat{X}}_i\left(k\right)-\hat{X}\left(k\right)\&rsqb;}^T\}---\left(44\right)$

以上为IMM算法的步骤；通过IMM对两个模型进行交互，可以有效对目标状态进行预测，将所预测的目标状态作为飞行器的控制输入。

所述的步骤S4中，

在飞行器控制模块中，使用IMM算法得到的目标状态 $X\left(k\right)=\&lsqb;\begin{array}{cccccc}x& y& \stackrel{\&CenterDot;}{x}& \stackrel{\&CenterDot;}{y}& \stackrel{\&CenterDot;\&CenterDot;}{x}& \stackrel{\&CenterDot;\&CenterDot;}{y}\end{array}\&rsqb;$ 作为输入；接下来分情况讨论：

在飞行器与目标的相对距离大于阈值κ_d且都小于阈值κ_a时，使用正常模式跟踪；正常模式下控制器飞行器根据IMM得到的目标位置估计设计速度环PID控制器：

在飞行器与目标的相对距离时，若或大于κ_a时，进一步判断该加速度方向，若加速度方向是远离飞行器时，判断无法跟踪，飞行器减速悬停等待地面站重新给定跟踪目标；若加速度方向是接近飞行器，进入悬停模式；在悬停模式中，飞行器悬停于原地，只通过改变飞行器的偏航角和云台角度来对目标进行跟踪；

飞行器所要偏转的偏航角应与云台期望水平偏角相等，考虑飞行器具有

云台增稳系统，可以不考虑飞行器改变偏航角时对云台造成的影响；

Ε(ψ)＝E(α)(46)

在飞行器与目标的相对距离d＜κ_d时，进入悬停模式；若云台俯仰角β＞κ_β，不改变飞行器高度；当κ_β＞β≥-90°时，提升飞行器的高度使视野变大，同时将飞行器偏航角向目标运动方向变化；飞行器提升的速度：

v_h＝v_ytanθ_H(47)

飞行器偏航角速度：

${\mathrm{\&omega;}}_{\mathrm{\&psi;}}=\frac{\mathrm{\&pi;}d}{2v}---\left(48\right)$

这样的分情况控制能够有效解决飞行器由于自身动力学特性而无法快速反向机动带来的跟踪困难。

显然，本发明的上述实施例仅仅是为清楚地说明本发明所作的举例，而并非是对本发明的实施方式的限定。对于所属领域的普通技术人员来说，在上述说明的基础上还可以做出其它不同形式的变化或变动。这里无需也无法对所有的实施方式予以穷举。凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明权利要求的保护范围之内。

Claims (10)

1.一种旋翼无人机移动目标自主跟踪装置，其特征在于，包括飞行器和地面站，所述的飞行器包括飞行器控制器，与飞行器控制器连接的飞行器螺旋桨电机、云台电机，所述的云台电机连接云台摄像头；

所述的飞行器和地面站之间设有无线图像传输模块和无线数据传输模块，飞行器与地面站通过无线图像传输模块和无线数据传输模块连接；

所述的地面站包括图像跟踪模块、云台控制模块、目标位置估算模块、飞行器控制模块。

2.根据权利要求1所述的一种旋翼无人机移动目标自主跟踪装置，其特征在于：所述的飞行器上设有气压计。

3.根据权利要求2所述的一种旋翼无人机移动目标自主跟踪装置，其特征在于：所述的飞行器上设有磁力计。

4.根据权利要求3所述的一种旋翼无人机移动目标自主跟踪装置，其特征在于：所述的飞行器上设有惯性测量单元。

5.根据权利要求4所述的一种旋翼无人机移动目标自主跟踪装置，其特征在于：所述的云台摄像头设于飞行器机体下方；所述的气压计、磁力计、惯性测量单元均与无线数据传输模块连接。

6.利用权利要求1至5任一所述的旋翼无人机移动目标自主跟踪装置的控制方法，其特征在于，包括以下步骤：

S1.云台摄像头将图像信号输送到无线图像传输模块，无线图像传输模块再输送到图像跟踪模块，图像跟踪模块采用struck算法，得到的目标图像坐标发送给云台控制模块和目标位置估算模块；

S2.云台控制模块在获得图像跟踪模块发送的图像坐标后，为目标建立运动模型，得到下一时刻的估计坐标，提供给指令发送模块控制云台的转动；

S3.目标位置估算模块接收到目标图像坐标后结合机载传感器的数据计算目标与飞行器的相对位置，将经过IMM算法得到估计的目标状态，发送给飞行器控制模块；

S4.飞行器控制模块根据目标状态决定控制策略从而改变自己的姿态角进行移动。

7.根据权利要求6所述的旋翼无人机移动目标自主跟踪装置的控制方法，其特征在于：所述的步骤S1中，

Struck算法使用结构输出的支持向量机算法，将样本描述为区域和变换的组合也即(x,y)，其中y是待求的目标区域变化函数，将目标位置变化函数与分类结合在一起可以得到一个区分函数：

这个区分函数的作用是给定坐标区域x和平面变换y，可以分别计算原坐标区域和变换后的坐标区域的haar特征值并比较两个区域之间的相似度并评分；好处是将这个区分函数用于预测目标可以同时把分类问题和判定相似度的问题合并成一个参数最优化的问题：

其中是目标上一帧的位置，γ是所有可能的变化组成的空间；

通过把区分函数约束成带有核函数的形式：

F(x,y)＝〈w,Φ(x,y)〉(3)

可以将具有一系列样本{(x₁,y₁)…(x_n,y_n)}的跟踪问题简化成可以使用支持

向量机模型求解的形式：

$\underset{w}{min}\frac{1}{2}\left|\right|w|{|}^2+C\underset{i=1}{\overset{n}{\&Sigma;}}{\mathrm{\&xi;}}_i---\left(4\right)$

$\begin{array}{ccc}s.t.& \&ForAll;i:{\mathrm{\&xi;}}_i\&GreaterEqual;0& \&ForAll;i,\&ForAll;y\&NotEqual;y_i:<w,{\mathrm{\&delta;\&Phi;}}_i\left(y\right)>\&GreaterEqual;\mathrm{\&Delta;}(y_i,y)-{\mathrm{\&xi;}}_i\end{array}$

whereδΦ_i(y)＝Φ(x_i,y_i)-Φ(x_i,y)

损失函数可以定义为区域变换后的重叠度：

$\mathrm{\&Delta;}(y,\stackrel{\&OverBar;}{y})=1-s_{P_t}^O(y,\stackrel{\&OverBar;}{y})---\left(5\right)$

使用拉格朗日算子将约束和最优化问题结合起来，可以得到式子：

$\begin{array}{c}\underset{\mathrm{\&alpha;}}{\max }\underset{i,\stackrel{\&OverBar;}{y}\&NotEqual;y_i}{\&Sigma;}\mathrm{\&Delta;}(y,y_i){\mathrm{\&alpha;}}_i^y-\frac{1}{2}\underset{\begin{array}{c}i,y\&NotEqual;y_i\\ j,\stackrel{\&OverBar;}{y}\&NotEqual;y_j\end{array}}{\&Sigma;}{\mathrm{\&alpha;}}_i^y{\mathrm{\&alpha;}}_j^{\stackrel{\&OverBar;}{y}}<{\mathrm{\&delta;\&Phi;}}_i\left(y\right),{\mathrm{\&delta;\&Phi;}}_j\left(\stackrel{\&OverBar;}{y}\right)>\\ \begin{array}{ccc}s.t.& \&ForAll;i,\&ForAll;y\&NotEqual;y_i:{\mathrm{\&alpha;}}_i^y\&GreaterEqual;0& \&ForAll;i:\underset{y\&NotEqual;y_i}{\&Sigma;}{\mathrm{\&alpha;}}_i^y\&le;C\end{array}\end{array}---\left(7\right)$

定义变量

${\mathrm{\&beta;}}_i^y=\left\{\begin{array}{cc}-{\mathrm{\&alpha;}}_i^y& \begin{array}{cc}if& y\&NotEqual;y_i\end{array}\\ \underset{\stackrel{\&OverBar;}{y}\&NotEqual;y_j}{\&Sigma;}{\mathrm{\&alpha;}}_i^{\stackrel{\&OverBar;}{y}}& otherwise\end{array}\right.---\left(8\right)$

可以将问题简化成：

$\begin{array}{c}\underset{\mathrm{\&beta;}}{\max }-\underset{i,y}{\&Sigma;}\mathrm{\&Delta;}(y,y_i){\mathrm{\&beta;}}_i^y-\frac{1}{2}\underset{i,y,j,\stackrel{\&OverBar;}{y}}{\&Sigma;}{\mathrm{\&beta;}}_i^y{\mathrm{\&beta;}}_j^{\stackrel{\&OverBar;}{y}}<\mathrm{\&Phi;}(x_i,y),\mathrm{\&Phi;}(x_j,\stackrel{\&OverBar;}{y})>\\ \begin{array}{ccc}s.t.& \&ForAll;i,\&ForAll;y:{\mathrm{\&beta;}}_i^y\&le;\mathrm{\&delta;}(y,y_i)C& \&ForAll;i:\underset{y}{\&Sigma;}{\mathrm{\&beta;}}_i^y=0\end{array}\end{array}---\left(9\right)$

在算法过程中，使用SMO算法求解这个最优化问题，每次为当前样本选择一个最好的正样本和负样本，然后将有效的样本加入支持向量，同时调整已有支持向量的系数，舍弃调整后不符合约束的支持向量，过程中一直保持有效支持向量的数目不超过100，超过的话根据公式：

$\begin{array}{c}\left|\right|\mathrm{\&Delta;}w|{|}^2={\mathrm{\&beta;}}_r^{y^2}\{<\mathrm{\&Phi;}(x_r,y),\mathrm{\&Phi;}(x_i,y)>+<\mathrm{\&Phi;}(x_r,y_r),\mathrm{\&Phi;}(x_r,y_r)>\\ -2<\mathrm{\&Phi;}(x_r,y),\mathrm{\&Phi;}(x_r,y_r)>\}\end{array}---\left(10\right)$

去掉负支持向量(x_r,y)；

根据正支持向量得到的样本重新在它们周围进行采样，从众多样本中选择使区分函数的值最大的一个区域作为跟踪结果。

8.根据权利要求6所述的旋翼无人机移动目标自主跟踪装置的控制方法，其特征在于：所述的步骤S2中，

在得到目标图像坐标后，基于帧与帧的间隔时间较短，可以认为目标的坐标在图像上的运动是在同一直线上的变加速运动；利用这样的假设，可以为目标坐标建立一个运动模型；记目标在t时刻的坐标为(u(t),v(t))，可以分别对横坐标u(t)和纵坐标v(t)建立一个CA模型，也即：

$\left[\begin{array}{c}u\left(t\right)\\ \stackrel{\&CenterDot;}{u}\left(t\right)\\ \stackrel{\&CenterDot;\&CenterDot;}{u}\left(t\right)\end{array}\right]=\left[\begin{array}{ccc}1& \mathrm{\&Delta;}t& \frac{1}{2}{\mathrm{\&Delta;t}}^2\\ 0& 1& \mathrm{\&Delta;}t\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}u\left(t\right)\\ \stackrel{\&CenterDot;}{u}\left(t\right)\\ \stackrel{\&CenterDot;\&CenterDot;}{u}\left(t\right)\end{array}\right]---\left(11\right)$

$\left[\begin{array}{c}v\left(t\right)\\ \stackrel{\&CenterDot;}{v}\left(t\right)\\ \stackrel{\&CenterDot;\&CenterDot;}{v}\left(t\right)\end{array}\right]=\left[\begin{array}{ccc}1& \mathrm{\&Delta;}t& \frac{1}{2}{\mathrm{\&Delta;t}}^2\\ 0& 1& \mathrm{\&Delta;}t\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}v\left(t\right)\\ \stackrel{\&CenterDot;}{v}\left(t\right)\\ \stackrel{\&CenterDot;\&CenterDot;}{v}\left(t\right)\end{array}\right]---\left(12\right)$

同时基于前四帧的图像坐标可以差分得到目标坐标的前一时刻的速度和加速度：

$\stackrel{\&CenterDot;}{u}\left(t\right)=\frac{1}{4}\left(u\right(t-1)-u(t-3)+u(t-2)-u(t-4\left)\right)---\left(13\right)$

$\stackrel{\&CenterDot;\&CenterDot;}{u}\left(t\right)=\frac{1}{4}\left(u\right(t-1)-u(t-3)-u(t-2)+u(t-4\left)\right)---\left(14\right)$

$\stackrel{\&CenterDot;}{v}\left(t\right)=\frac{1}{4}\left(v\right(t-1)-v(t-3)+v(t-2)-v(t-4\left)\right)---\left(15\right)$

$\stackrel{\&CenterDot;\&CenterDot;}{v}\left(t\right)=\frac{1}{4}\left(v\right(t-1)-v(t-3)-v(t-2)+v(t-4\left)\right)---\left(16\right)$

将式(14-16)代入CA模型用以预测移动目标下一时刻的坐标，这个预测的坐标能够结合目标在图像的运动趋势和云台的控制性能，可使云台在跟踪过程中的运动更为平滑，从而避免由于开环控制所带来的云台控制超调振荡的情况；通过预测的坐标计算云台需要偏转的偏航角和俯仰角：

$E\left(\mathrm{\&alpha;}\right)=\frac{\hat{v}}{f_W}{\mathrm{\&theta;}}_W---\left(17\right)$

$E\left(\mathrm{\&beta;}\right)=\frac{\hat{u}}{f_H}{\mathrm{\&theta;}}_H---\left(18\right)$

其中f_H、f_W是图像高度和宽度，θ_H和θ_W是摄像头纵向和横向成像视角。

9.根据权利要求6所述的旋翼无人机移动目标自主跟踪装置的控制方法，其特征在于：所述的步骤S3中，

基于目标在世界坐标系下的运动高度不变的假设，可以将目标运动模型简化到二维平面；

∠TPT′和∠NPT′可通过摄像头成像原理计算得到：

$\&angle;{\mathrm{TPT}}^{\&prime;}=\frac{u}{f_H}{\mathrm{\&theta;}}_H---\left(19\right)$

$\&angle;{\mathrm{NPT}}^{\&prime;}=\frac{v}{f_W}{\mathrm{\&theta;}}_W---\left(20\right)$

其中，f_H、f_w分别为图像高度和宽度，θ_H和θ_w分别为摄像头纵向和横向成像视角；OP是通过机载气压计或超声波传感器估计的飞行器的高度，∠OPM可以通过云台俯仰角获得；由三角关系可得

${\mathrm{PT}}^{,}=\frac{OP}{\cos {\mathrm{OPT}}^{,}}---\left(21\right)$

OT'＝OPtanOPT'(22)

TM＝OP(tanOPT'-tan0PM)(23)

T'N＝T'Mcos0PM(24)

$T^{,}T=T^{,}N\frac{u}{v}---\left(25\right)$

$\&angle;{\mathrm{TOT}}^{\&prime;}={\tan }^{-1}\frac{T^{,}T}{{\mathrm{OT}}^{,}}---\left(26\right)$

$OT=\sqrt{{\mathrm{OT}}^{,2}+T^{,}{T}^2}---\left(27\right)$

由此，可以得到目标相对于飞行器的坐标为：

d_x＝OTsin(∠MOy+∠TOM)(28)

d_y＝OTcos(∠MOy+∠TOM)(29)

因为得到的目标位置是通过几何换算得到的，传感器精度带来的观测误差会影响飞行器的跟踪，同时基于目标当前状态的跟踪会带来较大的超调，所以需要对目标状态进行滤波和预测；跟踪的目标状态可能是静止、弱机动、强机动，无法用一个确定的模型来描述，因此在对目标状态进行估计的时候需要考虑模型的改变；使用匀速直线运动CV模型和当前统计模型作为系统模型集使用交互式多模型算法IMM算法去预测目标状态；记采样间隔为T，由于x方向和y方向的预测是等价的，以x方向为例说明目标状态的预测；

CV模型也即匀速直线运动模型，设目标离散运动模型和观测模型为：

X(k+1)＝ΦX(k)+Γ(k)W(k)(30)

Z(k)＝C(k)X(k)+V(k)(31)

其中状态量Φ是状态转移矩阵，Γ是过程噪声分布矩阵，W(k)和V(k)是均值为零相互独立且不相关的高斯白噪声，Z(k)是观测向量，C(k)是测量函数；

其状态转移矩阵为：

$\mathrm{\&Phi;}=\left[\begin{array}{ccc}I& TI& 0\\ 0& I& 0\\ 0& 0& I\end{array}\right]---\left(32\right)$

过程噪声分布矩阵为：

$\mathrm{\&Gamma;}\left(k\right)=\left[\begin{array}{c}TI\\ I\\ 0\end{array}\right]---\left(33\right)$

当前统计模型是一种非零均值时间相关模型，它假设当前时刻加速度均值为前一时刻加速度的估计值，可以用来对机动目标进行建模；当前统计模型认为目标当前下一时刻的加速度只能在当前加速度的领域内，即假定加速度a(k)满足：

$a\left(k\right)=\tilde{a}\left(k\right)+\stackrel{\&OverBar;}{a}\left(k\right)---\left(34\right)$

其中，是零均值的一阶马尔可夫过程，是加速度的均值；

当前统计模型的离散状态方程为：

$X(k+1)=\mathrm{\&Phi;}X\left(k\right)+U\left(k\right)\stackrel{\&OverBar;}{a}\left(k\right)+W\left(k\right)---\left(35\right)$

其中，

$\mathrm{\&Phi;}=\left[\begin{array}{ccc}I& TI& \frac{e^{-\mathrm{\&alpha;}T}+\mathrm{\&alpha;}T-1}{{\mathrm{\&alpha;}}^2}I\\ 0& I& \frac{1-e^{-\mathrm{\&alpha;}T}}{\mathrm{\&alpha;}}I\\ 0& 0& e^{-\mathrm{\&alpha;}T}I\end{array}\right]---\left(36\right)$

$U\left(k\right)=\left[\begin{array}{c}\frac{-\mathrm{\&alpha;}T+{\mathrm{\&alpha;}}^2\frac{T}{2}+(1-e^{-\mathrm{\&alpha;}T})}{{\mathrm{\&alpha;}}^2}\\ \frac{\mathrm{\&alpha;}T-(1-e^{-\mathrm{\&alpha;}T})}{\mathrm{\&alpha;}}\\ 1-e^{-\mathrm{\&alpha;}T}\end{array}\right]---\left(37\right)$

IMM算法通过马尔可夫链描述模型集中的M个模型的切换，它的步骤归纳如下：

1.交互

计算模型i和模型j的混合概率μ^i|j：

${\stackrel{\&OverBar;}{c}}_j=\underset{i=1}{\overset{M}{\&Sigma;}}{p}_{ij}{\mathrm{\&mu;}}^i\&lsqb;k-1\&rsqb;,{\mathrm{\&mu;}}^{i|j}=\frac{1}{{\stackrel{\&OverBar;}{c}}_j}\underset{i=1}{\overset{M}{\&Sigma;}}{p}_{ij}{\mathrm{\&mu;}}^i\&lsqb;k-1\&rsqb;---\left(38\right)$

p_ij为下一时刻从模型i切换到模型j的概率，μⁱ[k-1]为模型i在k-1

时刻的概率，表示归一化系数；

每个滤波器的混合输入计算为：

${\hat{X}}_{0j}(k-1)=\underset{i=1}{\overset{M}{\&Sigma;}}{\mathrm{\&mu;}}^{i|j}\left(k\right){\hat{X}}_i(k-1)---\left(39\right)$

$\begin{array}{c}{P}_{0j}(k-1)=\underset{i=1}{\overset{M}{\&Sigma;}}{\mathrm{\&mu;}}^{i|j}\left(k\right)\{P_i(k-1)\\ +\&lsqb;{\hat{X}}_i(k-1)-{\hat{X}}_{0j}(k-1)\&rsqb;{\&lsqb;{\hat{X}}_i(k-1)-{\hat{X}}_{0j}(k-1)\&rsqb;}^T\}\end{array}---\left(40\right)$

其中，P_i(k-1)分别表示模型i在上一时刻更新的估计值和协方差；

2.滤波

基于每个模型j使用UKF进行滤波，同时计算每个滤波器的观测似然函数

Λ^j(k)＝N(V^J(k),S^j(k))(41)

V^J(k)为模型j在UKF滤波更新后的测量残差，S^j(k)是协方差；模型j在k时刻的概率计算为：

$c=\underset{i=1}{\overset{M}{\&Sigma;}}{\mathrm{\&Lambda;}}^i\left(k\right){\stackrel{\&OverBar;}{c}}_i,{\mathrm{\&mu;}}^j\left(k\right)=\frac{1}{c}{\mathrm{\&Lambda;}}^j\left(k\right){\stackrel{\&OverBar;}{c}}_j---\left(42\right)$

3.联合

最后计算各个滤波器的状态值和协方差的联合估计：

$\hat{X}\left(k\right)=\underset{i=1}{\overset{M}{\&Sigma;}}{\mathrm{\&mu;}}^i\left(k\right){\hat{X}}_i\left(k\right)---\left(43\right)$

$P\left(k\right)=\underset{i=1}{\overset{M}{\&Sigma;}}{\mathrm{\&mu;}}^i\left(k\right)\{P_i\left(k\right)+\&lsqb;{\hat{X}}_i\left(k\right)-\hat{X}\left(k\right)\&rsqb;{\&lsqb;{\hat{X}}_i\left(k\right)-\hat{X}\left(k\right)\&rsqb;}^T\}---\left(44\right)$

以上为IMM算法的步骤；通过IMM对两个模型进行交互，可以有效对目标状态进行预测，将所预测的目标状态作为飞行器的控制输入。

10.根据权利要求6所述的旋翼无人机移动目标自主跟踪装置的控制方法，其特征在于：所述的步骤S4中，

在飞行器控制模块中，使用IMM算法得到的目标状态 $X\left(k\right)=\&lsqb;\begin{array}{cccccc}x& y& \stackrel{\&CenterDot;}{x}& \stackrel{\&CenterDot;}{y}& \stackrel{\&CenterDot;\&CenterDot;}{x}& \stackrel{\&CenterDot;\&CenterDot;}{y}\end{array}\&rsqb;$ 作为输入；接下来分情况讨论：

a)在飞行器与目标的相对距离大于阈值κ_d且都小于阈值κ_a时，使用正常模式跟踪；正常模式下控制器飞行器根据IMM得到的目标位置估计设计速度环PID控制器：

b)在飞行器与目标的相对距离时，若或大于κ_a时，进一步判断该加速度方向，若加速度方向是远离飞行器时，判断无法跟踪，飞行器减速悬停等待地面站重新给定跟踪目标；若加速度方向是接近飞行器，进入悬停模式；在悬停模式中，飞行器悬停于原地，只通过改变飞行器的偏航角和云台角度来对目标进行跟踪；

飞行器所要偏转的偏航角应与云台期望水平偏角相等，考虑飞行器具有云台增稳系统，可以不考虑飞行器改变偏航角时对云台造成的影响；

Ε(ψ)＝E(α)(46)

c)在飞行器与目标的相对距离d＜κ_d时，进入悬停模式；若云台俯仰角β＞κ_β，不改变飞行器高度；当κ_β＞β≥-90°时，提升飞行器的高度使视野变大，同时将飞行器偏航角向目标运动方向变化；飞行器提升的速度：

v_h＝v_ytanθ_H(47)

飞行器偏航角速度：

${\mathrm{\&omega;}}_{\mathrm{\&psi;}}=\frac{\mathrm{\&pi;}d}{2v}---\left(48\right)$

这样的分情况控制能够有效解决飞行器由于自身动力学特性而无法快速反向机动带来的跟踪困难。