跟踪算法设计

ex ey d height 1.6115 x arcsin pt w dy height 1.2450 arcsin pt h
1.2 目标跟踪导引分析
若要使飞行器对目标进行跟踪，需要一个合适的导引规律，还需要将导引规 律分解为无人机各个轴向上的动作。
y 目标(ex,ey) x 图像平面（地平面）
表 向上速度为 0， 如此仅剩 x, 这两个自由度， 其中 x 表示飞行器的前进速度，
示飞行器的前进方向的变化率， 分别设计控制方法对这两个变量进行控制就可以 达到 (ex , ey ) 0 的控制效果。 将上述分析应用于式错误!未找到引用源。，可得：
ey x (m ey ) sin yO ex (m ex ) cos xO
首先，需要研究图像采集设备的成像规律，根据规律确定成像平面上的像 素距离与实际距离的关系。 在本系统中图像采集设备是一台运动相机，相机符合 针孔模型。 此处忽略相机镜头畸变产生的模型变化，因为跟踪控制的整个反馈回 路能够抑制镜头畸变产生的相对位置误差， 且本系统对跟踪精度的要求并不很高。 图 0-1 是针孔模型的成像示意图， 针孔模型的核心是光学透视原理， 在此就不赘 述。再利用针孔模型解算目标位置时，需要对相机的参数进行标定，下面介绍一 下标定过程。
ex dis tan ey dis tan
其中， d x、d y、w、h 是已知量， tan 距离，将在下文介绍。
cx
2

cy
2
dx w dy h
cx
2
、 tan
cy
2
是待标定的系数， dis 是成像
由于 L、w 均为已知量，可以通过标定获得多组 disi 和 li ，最后对所有的
ey x pt dis (m ey ) sin z cos pt yO ex y pt dis (m ex ) cos z cos pt xO
在此方程中， x, y 项为无人机水平方向上的带来的速度， pt , pt 是云台机动
dx w ex W
可得：
ex
最终可以得到距离求取公式
dx tan cx dis w 2
dx tan cx dis w 2 其中 d x 为目标的像素距离， w 为图像像素宽度。 ex
同理，定长标定工具沿成像平面 y 轴放置，使用同样的方法，可以获得类似 的公式。最终，根据图像采集、目标识别结果获得目标的位置公式如下：
在实际控制过程中，只要跟踪算法得当， sin 将会是一个很小的值，因此 只要控制飞行器的 x 和 两个自由度，使 x yO ， (m ex ) cos xO 即可跟踪 实现跟踪目标的功能。换句话说，只需要控制飞机前进的速度与目标相同，方向 角的变化率与观察被跟踪目标视线角的变化率相同，就可以跟踪目标。
项是无人机在 yaw 方向上旋转带来的速度，z 项是无人机高度变 带来的速度，
化带来的速度， xO , yO 项是目标机动带来的速度。值得注意的是，此处计算时， 假定图像平面与地平面重合， 所以有些运动的分量并未考虑到这一点，但是由于 整个系统有闭环，所以并不影响最终的效果。另外，因为双轴云台可以隔离飞行 器的 pitch 和 roll 运动，所以飞行器机动带来的 、 并不会影响目标在图像平面 的位置和速度。 从方程可以看到，一共有 2 个方程， x, y, pt , pt , , z, xO , yO 共 8 个未知数， 无法解出。在这 8 个量中， x, y, pt , pt , , z, 是飞行器和云台的自由度，属于可控 变量； xO , yO 是被跟踪目标的速度，可以通过图像分析结合飞行器状态求得。如 此，现有 6 个控制量两个变量。 为了简便地实现控制算法，需要将这 6 个自由度退化为 2 个。在大部分的跟 踪任务中， 希望目标在视野中的大小不变， 且飞行器和目标保持相对静止， 因此， 可以锁定 pt , pt , z 这三个自由度，保证飞行器定高、云台角固定的情况下使用剩 余的 x, y , 三个自由度跟踪目标。这样，在控制效果良好的情况下，飞行器将和 目标相对于大地拥有相同的速度。 借鉴导弹的制导的方法， 目标的追踪问题可以近似看做在二维平面内的制导 问题。 不过与制导问题不同的是，制导算法的侧重点是将一个 0 矢量通过导弹的 执行系统变为一个与目标相同的速度矢量； 而本系统的侧重点是如何使飞行器的 速度矢量与目标矢量保持一致。这一点在借鉴制导方式时要加以留意。 常用的导弹导引规律有纯追踪法，平行接近法，三点法和比例导引法。在自 寻的导弹上，比例导引法较为常见。它是指当导弹飞向目标时，导弹的速度矢量 方向的变化率与目标视线的变化律成比例[29]。 借鉴这种思想，可以将旋翼飞行器模拟为一个导弹。将 y 值保持为 0， y 方
的图像。从左至右依次是在 60cm、120cm、180cm 的距离下使用相机拍摄 60cm 长的标定工具的图像。由于篇幅有限，这里仅放置了 3 幅图像，余下的 21 幅图
像与这三幅图像的拍摄距离变化规律相同，在此就不列出。
图 0-2 标定用系列图
设在 img x 中，相机全幅的尺寸在实际中是 Wi H i （单位：米） ，定长标定工 具在图像平面中的长度用像素表示为 li 。因为照相机的成像可以视为针孔模型， 故可得：
tan(
cy
2
)
图 0-3 相机 x 轴参数标定图
1.1.2 成像几何关系分析
还有一个未知量 dis ，即拍摄距离是未知量，需要通过一些方法将其求取出 来。
机体平面y
飞行器 云台角
pt
机体平面x
云 台 平面 机 相
直线距离dis
高度height
图
水平距离m pt 地平面 期望控制角γ 地平面x 图像平面x
跟踪算法设计
1.1 目标位置的三维重建
如果要控制飞行器跟踪一个目标， 那就必须获得这个目标相对于飞行器的位 置和速度。 本课题采用的传感器是图像采集设备，它只能提供目标在成像平面上 的二维信息。所以，需要使用三维重建技术，根据图像和其他信息，将目标的位 置和速度解算出来。
1.1.1 相机成像规律和参数标定
tan(
cx
2
)求平均数即可获得一个参考值。标定结果如图 0-3 所示，可以计算得出
tan(
cx
2
)
w L 2560 0.6 1.6115 。 k 953.1429
h L 1920 0.6 1.2450 。 k 925.3012
同理， 可以通过类似的标定和计算方式得到图像平面 y 方向上待标定系数值
dis
height arcsin pt
在本系统的应用环境中， 相机的成像对象为被跟踪的对象，大部分情况下在 地面附近。 由于飞行器离地高度远大于被跟踪对象的高度，所以可以忽略被跟踪 对象的高度。那么也就是说，图像平面一定在地平面附近。考虑到飞行器的离地 高度较高，可以把图像平面近似当做地平面处理，这样可以简化一些计算。这种 处理方法一定会带来相对位置误差， 但是因为目标识别不会因为这个误差导致识 别错误， 相机镜头的畸变带来的位置误差要高于这种近似带来的位置误差，跟踪
面y 平 像
目标点
地平面y
图 0-4 飞行器、成像设备、目标三维关பைடு நூலகம்图
如图 0-4 所示，飞行器有一个机体平面，机体平面 x 轴向前，y 轴与 x 轴和 竖直方向垂直， 定义成右手系。 这种定义方法与飞行器控制器的定义方法相一致， 便于双方进行通信。在飞行器下方挂载一个云台，该云台可以接收控制指令，在 俯仰方向变动一定的角度，称为云台角 pt 。图像采集设备为一个运动相机，相 机平面与机体平面的夹角和云台角 pt 互为补角。根据机载的超声波传感器和气 压传感器， 可以测出飞行器的离地高度，因为这个高度远大于相机和飞行器的高 度差， 所以直接取这个高度作为相机平面与地平面的垂直距离， 简称高度 height 。 根据相机的成像特点， 相机成像的中心点必定在相机平面的垂线方向，因此定义 相机平面到图像平面的距离为 dis ，称为直线距离。 由于飞行器的飞行高度和云台角度都是已知量，可以求得距离：
镜头 定长标定工具 距离dis
视角
c
2
成像平面1 成像平面2
图 0-1 针孔模型示意图
设相机全幅的尺寸用为 w h（单位： 像素） ， 定长的标定工具长为 L（单位： 米） 。现有标定的一系列图像 imgi , i 1, 2,3, 机在 dis1 , dis2 , dis3 ,
, n ，如图 0-2 所示，他们分别是相 , dis n 距离下拍摄定长标定工具沿成像平面 x 轴放置而形成
控制的整个反馈回路又能够抑制相对位置误差， 且本系统对跟踪精度的要求并不 很高，因此这么处理是有益无害的。 在近似处理后， 可以得到， 相机平面中心点在地平面上的投影与图像平面中 心点的距离 m ， 称为水平距离， 这个距离可以代表图像平面中心点所代表的位置 与飞行器在地面上垂直投影的距离。而通过近似，也可以将图像平面中被追踪目 标的位置直接转化为被追踪目标与飞行器在实际中的相对位置。 通过上述讨论，最终可得，根据图像采集、目标识别结果获得目标相对于图 像中心点的位置公式如下：
水平距离m
γ x
y
z 机上坐标系
图 0-5 飞行器、成像平面、目标二维关系图
图 0-5 是跟踪过程中无人机、图像平面、被跟踪目标的几何关系示意图。上 一节已经求出目标相对于图像平面中心点的相对位置 (ex , e y ) 。 若要使飞行器跟踪 目标，那就需要尽量满足这两个条件：
(ex , ey ) 0 (ex , ey ) 0
li L w Wi
由几何关系可得：
tan
cx
2