技术摘要：
本发明公开了一种基于云台相机的无人机对地面目标跟踪控制方法及系统，属于目标跟踪技术领域，用于解决目前目标跟踪效果差的技术问题，采用的技术方案为：首先进行目标检测与识别，提取目标的特征点；利用坐标系转换，将当前特征点映射到不受固定翼无人机姿态和云台姿  全部
背景技术：
基于机载传感器实现对地面目标的持久鲁棒跟踪是无人机执行对地观测任务的 基本形式之一，也是后续目标行为/意图分析和态势生成的基础。传统方法大多采用“识别- 定位-控制”策略，即在识别目标后，结合机载传感器内外参数定位目标在空间中的位置，而 后基于定位结果设计跟踪控制律。但是在目标定位环节中，定位误差通常较大，且无人机时 刻处于高速运动中，导致固定翼无人机跟踪效果较差，难以做到长时间的鲁棒跟踪。因此， 以固定翼无人机和机载视觉传感器为对象，如何设计无需目标位置解算的视觉伺服跟踪控 制律，实现“识别-控制”一体化，是实现无人机对地面目标持久鲁棒跟踪的关键问题。 现有研究大多需要对目标的三维空间位置进行解算，即先对机载传感器的参数进 行标定，再利用图像检测算法获得目标在图像上的特征点坐标，最后将特征点坐标转化为 目标相对无人机的三维空间坐标，或是再结合GPS信息进行控制器的设计。例如：Morgan等 通过估计无人机相对目标的位置，可实现固定翼无人机对目标信息的获取，定位及连续监 视。Florent等提出一种视觉伺服控制算法，利用惯性测量单元和嵌入式相机，通过让固定 翼无人机在三维空间上的运动轨迹收敛到圆周，从而实现相机固联状态下无人机对静止目 标的跟踪。Pietro等[3]使用带云台相机的固定翼无人机，通过让云台朝向与无人机飞行方 向的夹角趋于90度，进而利用基于图像的视觉伺服方法实现无人机对地面静止目标的盘旋 跟踪；进一步，他们又验证了目标以很慢的速度沿直线运动时无人机跟踪效果的有效性。 Quintero等使用带云台相机的固定翼无人机，实现对未知运动的地面车辆的跟踪，即无人 机在保持与地面目标足够近的同时，还需保证目标在相机视场内。其中，车辆的位置信息通 过GPS接收器获得，再通过解算获得无人机的空间位置信息，从而实现对无人机和云台的控 制。 目前使用的固定翼无人机对地面目标的跟踪控制方法主要存在以下两个缺点： (1)在通过二维的目标特征点坐标解算无人机相对目标的三维空间坐标时，需结 合机载传感器内外参数进行定位。然而在目标定位环节中，定位误差通常较大，且无人机时 刻处于高速运动中，导致固定翼无人机跟踪效果较差，难以做到长时间的鲁棒跟踪。例如， Morgan等在实现固定翼无人机对目标的持续监视时，就是先定位无人机相对目标的位置， 进而设计控制器实现无人机对目标的跟踪。Florent等通过获得无人机相对目标的三维空 间位置，进而让该位置点收敛到恒定高度平面与圆锥面的交界处，即圆周上，从而实现无人 机绕静止目标盘旋飞行。此外，Quintero等在进行无人机的跟踪设计时，还用到GPS接收器 获得目标的绝对位置信息，这在GPS拒止环境下将无法完成。 (2)在不解算目标位置的前提下，只能实现固定翼无人机对静止或运动速度很慢 的目标进行跟踪。目前关于不解算目标位置，直接利用视觉伺服方法实现带云台相机的固 7 CN 111596693 A 说　明　书 2/14 页 定翼无人机对地面目标的跟踪研究还很少。Pietro等成功实现了该条件下的无人机跟踪控 制，然而关于云台偏航角的控制是让其保持在90度，即在俯视图中云台相机朝向与无人机 飞行方向垂直。因此，当目标运动速度较快时，由于无人机无法快速调整飞行方向，且云台 需要优先保持与无人机的夹角，容易导致目标脱离相机视场。 在参考文献([3]Peliti  P,Rosa  L,Oriolo  G ,et  al.Vision-Based  Loitering  Over  a  Target  for  a  Fixed-Wing  UAV[J].Ifac  Proceedings  Volumes,2012,45(22): 51-57.)设计了固定翼无人机和云台的控制器，实现了固定翼无人机对地面目标的跟踪。该 控制器的设计思路是让云台与无人机飞行方向的夹角趋于90度，从而让无人机的飞行方向 与目标相对无人机的连线垂直，如图1所示，进而实现无人机绕地面静止目标的盘旋跟踪， 并验证了目标运动速度很慢情况下跟踪控制的有效性。然而，这种设计无法最大化云台的 性能，即无法让云台随着图像上特征点的位置变换而在限幅范围内自由偏转，这会导致当 目标运动速度过快时容易脱离相机视场，使得跟踪失败。
技术实现要素：
本发明要解决的技术问题就在于：针对现有技术存在的技术问题，本发明提供一 种跟踪精度高的基于云台相机的无人机对地面目标跟踪控制方法及系统。 为了解决上述技术问题，本发明采用的技术方案为： 一种基于云台相机的无人机对地面目标跟踪控制方法，包括步骤： S01、首先进行目标检测与识别，提取目标的特征点； S02、利用坐标系转换，将当前特征点映射到不受固定翼无人机姿态和云台姿态变 化影响的图像平面上，从而获得对应的特征点S； S03、将S与期望特征点S*的差值作为输入，对无人机和云台系统进行伺服控制，获 得无人机的速度Va和期望的云台姿态θg； S04、根据Va和θg分别对无人机和云台进行姿态调整。 优选地，在步骤S02中，不受固定翼无人机姿态和云台姿态变化影响的图像平面为 理想相机图像平面，将当前图像上的特征点坐标(u1 ,v1)映射到理想相机图像平面的坐标 (u2,v2)的具体过程为： S21、将云台相机光心与地面目标的连线记为lp，任取该线上一点P(xp,yp,zp)，对 应当前 下的点坐标，则根据三角形相似原理，有如下关系成立： 其中k为非正的常数；f为相机焦距； S22、通过多个旋转变换，将P点映射到理想相机下 中的P′(x′p,y′p,z′p)点； S23、根据点映射到理想相机下 中的P′(x′p,y′p,z′p)点，求得(u1,v1)映射到理想 相机图像平面的坐标(u2,v2)； 其中 为相机坐标系， 的原点oc位于相机光心，zc轴的方向与光轴朝向一致，yc 轴的方向垂直光轴向上，进而确定xc轴的方向使 为右手空间直角坐标系。 优选地，在步骤S22中，多个旋转变换为： 8 CN 111596693 A 说　明　书 3/14 页 1)令当前时刻的相机坐标系 对应的俯仰角θt变为-90°； 从 到 需要绕 逆时针旋转θa，对应的旋转矩阵为： 其中ob-xbybzb和 分别表示当前状态S0时的无人机机体坐标系 和相 机坐标系 表示云台俯仰角θt＝-90°时的相机坐标系 和 的 夹角大小为 其中 为机体坐标系， 的原点ob位于固定翼无人机的质 心处，xb轴的方向与机头朝向一致，yb轴的方向垂直机身向右，zb轴的方向垂直机体向下； 2)令当前时刻的相机坐标系 对应的偏航角θp变为 让 绕 轴逆时针旋转 后， 轴与ocxb轴重合；记变换后的坐标系为 则 对应的旋转矩阵为： 3)令固定翼无人机的滚转角为0°；当前时刻的相机坐标系 对应的无人机状态 为S0，此时的无人机姿态角为(偏航角ψ1,俯仰角θ1,滚转角φ1)，则转换为滚转角为0对应的 状态S1，绕 轴逆时针旋转φ1；对应的旋转矩阵为： 4)令固定翼无人机的俯仰角为0°；无人机的滚转角和俯仰角均为0时对应状态S2， 记该状态下的相机坐标系为 则使俯仰角为0，绕yb轴逆时针旋转θ1，也可绕 的y轴逆 时针旋转θ1，对应的旋转矩阵为： 5)令相机坐标系 的z轴与垂直机身方向的夹角为α；相机光轴朝向与机身垂直 方向保持夹角α；记变换后的相机坐标系为 对应的旋转矩阵为： 优选地，在步骤S22中，通过多个旋转变换，将目标在当前相机坐标系下的坐标P变 成理想相机坐标系下的P′点： 9 CN 111596693 A 说　明　书 4/14 页 优选地，在步骤S33中，令z′p＝f，则可以求得系数k： 公式(9)中的上角标(3,·)表示矩阵的第三行；在求得k后，可进一步求出目标在 理想相机图像平面中的特征点坐标： 优选地，在步骤S03中，通过特征点变化速度获得无人机的速度控制量，利用图像 雅可比矩阵进行求解，公式可表示成： 其中T和Ω分别表示无人机飞行的线速度和角速度，都是三维的向量；Jv为图像雅 可比矩阵，表示成： 其中z表示目标到相机的视深； 其中为使目标快速收敛到图像中心，采用指数收敛方式；目标到图像中心的误差 为 则误差的收敛速度可表示成： 其中λ是个2×2的正定矩阵。 优选地，在步骤S03中，将S与期望特征点S*的差值作为输入，输入至伺服控制器 中，实现特征点S变化到无人机速度变化、无人机偏航角变化和云台姿态角变化的映射；无 人机速度变化包括无人机速度速率，所述无人机偏航角变化包括无人机偏航角速率，对应 的得到过程为： 记理想相机状态下的机体坐标系为 当前状态下的欧拉坐标系为 则从 到 需经过以下变换： a)从 转换到 记 中对应的目标线速度cV和角速度cΩ分别为： 10 CN 111596693 A 说　明　书 5/14 页 则经过上述第5)步的逆变换， 即可与 保持一致；记 下固定翼无人机的线 速度和角速度分别为 和 则表示成： b)从 转换到 固定翼无人机在当前状态下，滚转角和俯仰角分别旋转φ和θ后转换成理想相机 对应的状态，；当 下对应的无人机角速率已知时，可以通过投影变换获得无人机的欧拉 角速率； 记欧拉角速率为 其中ob-xyz表示惯性坐标系，此时三个姿态角均为0；绕z轴偏航ψ角后与坐标系 重合；而后绕y1轴俯仰θ角后与坐标系 重合；最后绕x2轴滚转 φ角后与坐标系 重合；考虑 下的无人机角速度与eΩ之间的投影关系，可 以得到： 其中 由于理想相机状态下固定翼无人机的俯仰角和滚转角始终为0，因此对应的滚转 角速率和俯仰角速率也始终为0，则偏航角速率的输入为： 再根据公式，可以通过 下的速度得到 下对应的速度： 又由于无人机以恒定的速度进行飞行，对应于S2状态下就是沿x轴的飞行速度为 Vt，而沿y轴和z轴的速度均为0；合公式(19)，可以得到 下的无人机速度表示： 将公式(11)，(21)代入公式(12)中，可以得到： 11 CN 111596693 A 说　明　书 6/14 页 进一步整理可得： 其中 再利用最小二乘法，可以求得最终的偏航角速率： 优选地，在云台相机模型中，云台通过调整θp和θt使目标趋于图像中心；以O1为相 机光心所在位置，O2为图像中心，|O1O2|表示相机焦距，C为目标特征点；结合图像坐标系 的构建，目标的坐标可表示为(|O2A|,-|O2B|)； 云台相机的偏航角θp变化将影响目标特征点的横坐标，而θt的变化将影响目标特 征点的纵坐标； 记△θp和△θt为目标从当前位置到达图像中心时云台需要旋转的角度，结合云台 姿态角符号的定义，则有 若期望图像上特征点坐标(u1,v1)指数收敛到图像中心，则有 其中 因此，云台姿态角的变化速率可表示成： 优选地，所述伺服控制器为： 本发明还公开了一种基于云台相机的无人机对地面目标跟踪控制系统，包括 12 CN 111596693 A 说　明　书 7/14 页 特征点提取模块，用于进行目标检测与识别，提取目标的特征点； 特征点S提取模块，用于利用坐标系转换，将当前特征点映射到不受固定翼无人机 姿态和云台姿态变化影响的图像平面上，从而获得对应的特征点S； 控制模块，用于将S与期望特征点S*的差值作为输入，对无人机和云台系统进行伺 服控制，获得无人机的速度Va和期望的云台姿态θg； 调整模块，用于根据Va和θg分别对无人机和云台进行姿态调整。 与现有技术相比，本发明的优点在于： 本发明的基于云台相机的无人机对地面目标跟踪控制方法及系统，以固定翼无人 机和机载视觉传感器为对象，提出无需目标位置解算的视觉伺服目标跟踪控制器，实现“识 别-控制”一体化；利用基于图像的视觉伺服方法，通过图像雅可比矩阵直接构建特征点变 化速率和无人机速度控制之间的关系，进而将特征点坐标作为控制输入，直接获得系统的 控制输出；该方法无需使用GPS进行定位，也不需要通过相机参数解算目标相对无人机的位 置，从而避免了传统的“识别-定位-控制”方法在定位环节的误差，有助于提高无人机对地 面目标的跟踪精度，达到长久鲁棒的跟踪效果。 本发明的基于云台相机的无人机对地面目标跟踪控制方法及系统，提出了一种理 想相机模型，用于解决无人机姿态变化和云台姿态变化对图像上特征点坐标的影响，通过 将当前图像上的特征点转化到理想相机对应图像上的特征点，从而可将后者收敛到图像中 心的速度直接作为期望的特征点速率，并通过最小二乘法获得无人机的偏航角速率；进一 步，由于云台姿态的变化对特征点速率不产生影响，因此云台可以在限幅范围内自由偏转 以使目标处于图像中心，从而最大化云台的性能，保证在目标运动速度较快时无人机和云 台也能通过偏转实现对其的跟踪。 附图说明 图1为现有技术中的固定翼无人机绕地面静止目标盘旋跟踪俯视图。 图2为本发明的控制系统在实施例的框架图。 图3为本发明中无人机和云台系统的坐标系构建图。 图4为本发明中云台相机的偏航和俯仰运动图。 图5为本发明中云台姿态角的限幅示意图。 图6为相机坐标系通过旋转后使θt为-90°。 图7为相机坐标系通过旋转后使θp为0°。 图8为 中无人机角速率与欧拉角速率的变换图。 图9为云台相机成像与相应坐标之间的关系。 图10为本发明中系统控制架构的设计框图。