基于云台相机的无人机对地面目标跟踪控制方法及系统与流程

本发明涉及目标跟踪技术领域，具体涉及一种基于云台相机的无人机对地面目标跟踪控制方法及系统。

背景技术：

基于机载传感器实现对地面目标的持久鲁棒跟踪是无人机执行对地观测任务的基本形式之一，也是后续目标行为/意图分析和态势生成的基础。传统方法大多采用“识别-定位-控制”策略，即在识别目标后，结合机载传感器内外参数定位目标在空间中的位置，而后基于定位结果设计跟踪控制律。但是在目标定位环节中，定位误差通常较大，且无人机时刻处于高速运动中，导致固定翼无人机跟踪效果较差，难以做到长时间的鲁棒跟踪。因此，以固定翼无人机和机载视觉传感器为对象，如何设计无需目标位置解算的视觉伺服跟踪控制律，实现“识别-控制”一体化，是实现无人机对地面目标持久鲁棒跟踪的关键问题。

现有研究大多需要对目标的三维空间位置进行解算，即先对机载传感器的参数进行标定，再利用图像检测算法获得目标在图像上的特征点坐标，最后将特征点坐标转化为目标相对无人机的三维空间坐标，或是再结合gps信息进行控制器的设计。例如：morgan等通过估计无人机相对目标的位置，可实现固定翼无人机对目标信息的获取，定位及连续监视。florent等提出一种视觉伺服控制算法，利用惯性测量单元和嵌入式相机，通过让固定翼无人机在三维空间上的运动轨迹收敛到圆周，从而实现相机固联状态下无人机对静止目标的跟踪。pietro等[3]使用带云台相机的固定翼无人机，通过让云台朝向与无人机飞行方向的夹角趋于90度，进而利用基于图像的视觉伺服方法实现无人机对地面静止目标的盘旋跟踪；进一步，他们又验证了目标以很慢的速度沿直线运动时无人机跟踪效果的有效性。quintero等使用带云台相机的固定翼无人机，实现对未知运动的地面车辆的跟踪，即无人机在保持与地面目标足够近的同时，还需保证目标在相机视场内。其中，车辆的位置信息通过gps接收器获得，再通过解算获得无人机的空间位置信息，从而实现对无人机和云台的控制。

目前使用的固定翼无人机对地面目标的跟踪控制方法主要存在以下两个缺点：

(1)在通过二维的目标特征点坐标解算无人机相对目标的三维空间坐标时，需结合机载传感器内外参数进行定位。然而在目标定位环节中，定位误差通常较大，且无人机时刻处于高速运动中，导致固定翼无人机跟踪效果较差，难以做到长时间的鲁棒跟踪。例如，morgan等在实现固定翼无人机对目标的持续监视时，就是先定位无人机相对目标的位置，进而设计控制器实现无人机对目标的跟踪。florent等通过获得无人机相对目标的三维空间位置，进而让该位置点收敛到恒定高度平面与圆锥面的交界处，即圆周上，从而实现无人机绕静止目标盘旋飞行。此外，quintero等在进行无人机的跟踪设计时，还用到gps接收器获得目标的绝对位置信息，这在gps拒止环境下将无法完成。

(2)在不解算目标位置的前提下，只能实现固定翼无人机对静止或运动速度很慢的目标进行跟踪。目前关于不解算目标位置，直接利用视觉伺服方法实现带云台相机的固定翼无人机对地面目标的跟踪研究还很少。pietro等成功实现了该条件下的无人机跟踪控制，然而关于云台偏航角的控制是让其保持在90度，即在俯视图中云台相机朝向与无人机飞行方向垂直。因此，当目标运动速度较快时，由于无人机无法快速调整飞行方向，且云台需要优先保持与无人机的夹角，容易导致目标脱离相机视场。

在参考文献([3]pelitip,rosal,oriolog,etal.vision-basedloiteringoveratargetforafixed-winguav[j].ifacproceedingsvolumes,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中，多个旋转变换为：

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′点：

优选地，在步骤s33中，令z′p＝f，则可以求得系数k：

公式(9)中的上角标(3,·)表示矩阵的第三行；在求得k后，可进一步求出目标在理想相机图像平面中的特征点坐标：

优选地，在步骤s03中，通过特征点变化速度获得无人机的速度控制量，利用图像雅可比矩阵进行求解，公式可表示成：

其中t和ω分别表示无人机飞行的线速度和角速度，都是三维的向量；jv为图像雅可比矩阵，表示成：

其中z表示目标到相机的视深；

其中为使目标快速收敛到图像中心，采用指数收敛方式；目标到图像中心的误差为则误差的收敛速度可表示成：

其中λ是个2×2的正定矩阵。

优选地，在步骤s03中，将s与期望特征点s^*的差值作为输入，输入至伺服控制器中，实现特征点s变化到无人机速度变化、无人机偏航角变化和云台姿态角变化的映射；无人机速度变化包括无人机速度速率，所述无人机偏航角变化包括无人机偏航角速率，对应的得到过程为：

记理想相机状态下的机体坐标系为当前状态下的欧拉坐标系为则从到需经过以下变换：

a)从转换到

记中对应的目标线速度^cv和角速度^cω分别为：

则经过上述第5)步的逆变换，即可与保持一致；记下固定翼无人机的线速度和角速度分别为和则表示成：

b)从转换到

固定翼无人机在当前状态下，滚转角和俯仰角分别旋转φ和θ后转换成理想相机对应的状态，；当下对应的无人机角速率已知时，可以通过投影变换获得无人机的欧拉角速率；

记欧拉角速率为

其中ob-xyz表示惯性坐标系，此时三个姿态角均为0；绕z轴偏航ψ角后与坐标系重合；而后绕y1轴俯仰θ角后与坐标系重合；最后绕x2轴滚转φ角后与坐标系重合；考虑下的无人机角速度与^eω之间的投影关系，可以得到：

其中

由于理想相机状态下固定翼无人机的俯仰角和滚转角始终为0，因此对应的滚转角速率和俯仰角速率也始终为0，则偏航角速率的输入为：

再根据公式，可以通过下的速度得到下对应的速度：

又由于无人机以恒定的速度进行飞行，对应于s2状态下就是沿x轴的飞行速度为vt，而沿y轴和z轴的速度均为0；合公式(19)，可以得到下的无人机速度表示：

将公式(11)，(21)代入公式(12)中，可以得到：

进一步整理可得：

其中

再利用最小二乘法，可以求得最终的偏航角速率：

优选地，在云台相机模型中，云台通过调整θp和θt使目标趋于图像中心；以o1为相机光心所在位置，o2为图像中心，|o1o2|表示相机焦距，c为目标特征点；结合图像坐标系的构建，目标的坐标可表示为(|o2a|,-|o2b|)；

云台相机的偏航角θp变化将影响目标特征点的横坐标，而θt的变化将影响目标特征点的纵坐标；

记△θp和△θt为目标从当前位置到达图像中心时云台需要旋转的角度，结合云台姿态角符号的定义，则有

若期望图像上特征点坐标(u1,v1)指数收敛到图像中心，则有

其中

因此，云台姿态角的变化速率可表示成：

优选地，所述伺服控制器为：

本发明还公开了一种基于云台相机的无人机对地面目标跟踪控制系统，包括

特征点提取模块，用于进行目标检测与识别，提取目标的特征点；

特征点s提取模块，用于利用坐标系转换，将当前特征点映射到不受固定翼无人机姿态和云台姿态变化影响的图像平面上，从而获得对应的特征点s；

控制模块，用于将s与期望特征点s^*的差值作为输入，对无人机和云台系统进行伺服控制，获得无人机的速度va和期望的云台姿态θg；

调整模块，用于根据va和θg分别对无人机和云台进行姿态调整。

与现有技术相比，本发明的优点在于：

本发明的基于云台相机的无人机对地面目标跟踪控制方法及系统，以固定翼无人机和机载视觉传感器为对象，提出无需目标位置解算的视觉伺服目标跟踪控制器，实现“识别-控制”一体化；利用基于图像的视觉伺服方法，通过图像雅可比矩阵直接构建特征点变化速率和无人机速度控制之间的关系，进而将特征点坐标作为控制输入，直接获得系统的控制输出；该方法无需使用gps进行定位，也不需要通过相机参数解算目标相对无人机的位置，从而避免了传统的“识别-定位-控制”方法在定位环节的误差，有助于提高无人机对地面目标的跟踪精度，达到长久鲁棒的跟踪效果。

本发明的基于云台相机的无人机对地面目标跟踪控制方法及系统，提出了一种理想相机模型，用于解决无人机姿态变化和云台姿态变化对图像上特征点坐标的影响，通过将当前图像上的特征点转化到理想相机对应图像上的特征点，从而可将后者收敛到图像中心的速度直接作为期望的特征点速率，并通过最小二乘法获得无人机的偏航角速率；进一步，由于云台姿态的变化对特征点速率不产生影响，因此云台可以在限幅范围内自由偏转以使目标处于图像中心，从而最大化云台的性能，保证在目标运动速度较快时无人机和云台也能通过偏转实现对其的跟踪。

附图说明

图1为现有技术中的固定翼无人机绕地面静止目标盘旋跟踪俯视图。

图2为本发明的控制系统在实施例的框架图。

图3为本发明中无人机和云台系统的坐标系构建图。

图4为本发明中云台相机的偏航和俯仰运动图。

图5为本发明中云台姿态角的限幅示意图。

图6为相机坐标系通过旋转后使θt为-90°。

图7为相机坐标系通过旋转后使θp为0°。

图8为中无人机角速率与欧拉角速率的变换图。

图9为云台相机成像与相应坐标之间的关系。

图10为本发明中系统控制架构的设计框图。

具体实施方式

以下结合说明书附图和具体实施例对本发明作进一步描述。

如图2所示，本实施例的基于云台相机的固定翼无人机对地面目标的视觉伺服跟踪控制方法，包括步骤：

s01、首先进行目标检测与识别，提取目标的特征点；

s02、利用坐标系转换，将当前特征点映射到不受固定翼无人机姿态和云台姿态变化影响的图像平面上，从而获得对应的特征点s；

s03、将s与期望特征点s^*(如取图像中点)的差值作为输入，对无人机和云台系统进行伺服控制，获得无人机的速度va和期望的云台姿态θg(包括云台偏航角和俯仰角)；

s04、根据va和θg分别对无人机和云台进行姿态调整。

通过上述步骤的循环，最终实现无人机对地面目标跟踪的同时，保持目标位于相机视场之内。其中为分析固定翼无人机的运动，根据实际情况对无人机的飞行状态做出一些合理的假设：固定翼无人机在对目标进行盘旋跟踪时，始终以恒定的速度vt进行飞行；同时，由于高度变化容易影响相机成像效果，故假设无人机的飞行高度h也保持不变。

借助现有的图像检测方法(例如yolov3等)，可获得目标特征点坐标(u1,v1)。而为了进行坐标系的转换，首先需构建所需的坐标系。各坐标系及其之间的相对关系如图3所示，包括以下四个坐标系：

(1)图像坐标系

的原点oi位于图像中心，xi轴平行于图像的宽，yi轴平行于图像的高。

(2)相机坐标系

的原点oc位于相机光心，zc轴的方向与光轴朝向一致，yc轴的方向垂直光轴向上，进而确定xc轴的方向使为右手空间直角坐标系。

(3)机体坐标系

的原点ob位于固定翼无人机的质心处，xb轴的方向与机头朝向一致，yb轴的方向垂直机身向右，zb轴的方向垂直机体向下。

(4)云台坐标系

的原点可认为与oc一致，都位于相机光心，yg轴沿相机光轴朝向，zg轴垂直相机光轴向上，xg轴的选取使为右手空间直角坐标系。

其中各参数以及定义如下：

表1：相关参数及其解释

由于云台需要利用自身的偏转让目标处于图像中心，因此需要对云台的偏转进行分析。如图4所示，表示云台的偏航角速率，表示云台的俯仰角速率。通过这两个维度的变化，云台可调整相机的视角而使目标位于视场之内。此外，由于云台机械特性的限制，云台的偏转角都有限幅。如图5所示，其中偏航角俯仰角

由于假设无人机的飞行速度和高度恒定，因此可以结合unicycle模型对“无人机+云台”系统进行分析。系统的模型可以表示为：

在进行控制器的设计前，先对“理想相机”概念进行描述：(1)该状态下的无人机质心与当前时刻的质心一致；(2)相机相对无人机的姿态保持恒定；(3)无人机的俯仰角和滚转角为0。定义“理想相机”的目的，是为了让图像上目标特征点坐标不受云台姿态变化及无人机俯仰角和滚转角变化的影响，从而实现目标特征点变化到无人机偏航角变化的映射。

因此，为获得期望的特征点变化速率，需要将当前图像上的特征点坐标(u1,v1)映射到“理想相机”状态下的坐标(u2,v2)。将光心与地面目标的连线记为lp，任取该线上一点p(xp,yp,zp)(对应当前下的点坐标)，则根据三角形相似原理，有如下关系成立：

其中k为非正的常数。接下来，通过以下5个旋转变换，将p点映射到“理想相机”下中的p′(x′p,y′p,z′p)点。

(1)令当前时刻的相机坐标系对应的θt变为-90°。如图6所示，ob-xbybzb和分别表示当前状态s0时的无人机机体坐标系和相机坐标系表示云台俯仰角θt＝-90°时的相机坐标系和的夹角大小为

从到需要绕逆时针旋转θa，对应的旋转矩阵为：

(2)令当前时刻的相机坐标系对应的θp变为0°。如图7所示，由于因此让绕轴逆时针旋转后，轴就能与ocxb轴重合。记变换后的坐标系为则对应的旋转矩阵为：

(3)令固定翼无人机的滚转角为0°。由于当前时刻的相机坐标系对应的无人机状态为s0，若此时的无人机姿态角为(ψ1,θ1,φ1)，则要转换为滚转角为0对应的状态s1，需要绕轴逆时针旋转φ1。对应的旋转矩阵为：

(4)令固定翼无人机的俯仰角为0°。无人机的滚转角和俯仰角均为0时对应状态s2，记该状态下的相机坐标系为则要使俯仰角为0，需要绕yb轴逆时针旋转θ1，也可近似认为绕的y轴逆时针旋转θ1，对应的旋转矩阵为：

(5)令相机坐标系的z轴与垂直机身方向的夹角为α。根据“理想相机”的定义，相机光轴朝向还要与机身垂直方向保持夹角α。记变换后的相机坐标系为对应的旋转矩阵为：

因此通过以上变换，可以将目标在当前相机坐标系下的坐标p变成“理想相机”坐标系下的p′点：

令z′p＝f，则可以求得系数k：

公式(9)中的上角标(3,·)表示矩阵的第三行。在求得k后，可进一步求出目标在“理想相机”图像平面中的特征点坐标：

为使目标快速收敛到图像中心，可采用指数收敛方式。目标到图像中心的误差为则误差的收敛速度可表示成：

其中λ是个2×2的正定矩阵。

要通过特征点变化速度获得无人机的速度控制量，可以利用图像雅可比矩阵进行求解。

公式可表示成：

其中t和ω分别表示无人机飞行的线速度和角速度，都是三维的向量；jv为图像雅可比矩阵，表示成：

其中z表示目标到相机的视深。

记“理想相机”状态下的机体坐标系为当前状态下的欧拉坐标系为则从到需经过以下变换：

(1)从转换到

记中对应的目标线速度和角速度分别为：

则经过上述第(5)步的逆变换，即可与保持一致。记下固定翼无人机的线速度和角速度分别为和则它们可表示成：

(2)从转换到

固定翼无人机在当前状态下，滚转角和俯仰角分别旋转φ和θ后转换成“理想相机”对应的状态，转换过程如图8所示。当下对应的无人机角速率已知时，可以通过投影变换获得无人机的欧拉角速率。

记欧拉角速率为

在图8中，ob-xyz表示惯性坐标系，此时三个姿态角均为0。绕z轴偏航ψ角后与坐标系重合；而后绕y1轴俯仰θ角后与坐标系重合；最后绕x2轴滚转φ角后与坐标系重合。考虑下的无人机角速度与^eω之间的投影关系，可以得到:

其中

由于“理想相机”状态下固定翼无人机的俯仰角和滚转角始终为0，因此对应的滚转角速率和俯仰角速率也始终为0，则偏航角速率的输入为：

再根据公式，可以通过下的速度得到下对应的速度：

又由于无人机以恒定的速度进行飞行，对应于s2状态下就是沿x轴的飞行速度为vt，而沿y轴和z轴的速度均为0。结合公式(19)，可以得到下的无人机速度表示：

将公式(11)，(21)代入公式(12)中，可以得到：

进一步整理可得：

其中

再利用最小二乘法，可以求得最终的偏航角速率：

接下来，对云台的控制进行分析。在云台相机模型中，云台通过调整θp和θt使目标趋于图像中心。如图9所示，o1为相机光心所在位置，o2为图像中心，|o1o2|表示相机焦距，c为目标特征点。结合图像坐标系的构建，目标的坐标可表示为(|o2a|,-|o2b|)。分析可知，云台相机的偏航角θp变化将影响目标特征点的横坐标，而θt的变化将影响目标特征点的纵坐标。记△θp和△θt为目标从当前位置到达图像中心时云台需要旋转的角度。结合图5中云台姿态角符号的定义，则有

若期望图像上特征点坐标(u1,v1)指数收敛到图像中心，则有

其中因此，云台姿态角的变化速率可表示成：

综上，“无人机+云台”系统的控制器为：

本发明还公开了一种基于云台相机的无人机对地面目标跟踪控制系统，包括

特征点提取模块，用于进行目标检测与识别，提取目标的特征点；

特征点s提取模块，用于利用坐标系转换，将当前特征点映射到不受固定翼无人机姿态和云台姿态变化影响的图像平面上，从而获得对应的特征点s；

控制模块，用于将s与期望特征点s^*的差值作为输入，对无人机和云台系统进行伺服控制，获得无人机的速度va和期望的云台姿态θg；

调整模块，用于根据va和θg分别对无人机和云台进行姿态调整。

本发明的基于云台相机的无人机对地面目标跟踪控制方法及系统，以固定翼无人机和机载视觉传感器为对象，提出无需目标位置解算的视觉伺服目标跟踪控制器，实现“识别-控制”一体化；利用基于图像的视觉伺服方法，通过图像雅可比矩阵直接构建特征点变化速率和无人机速度控制之间的关系，进而将特征点坐标作为控制输入，直接获得系统的控制输出；该方法无需使用gps进行定位，也不需要通过相机参数解算目标相对无人机的位置，从而避免了传统的“识别-定位-控制”方法在定位环节的误差，有助于提高无人机对地面目标的跟踪精度，达到长久鲁棒的跟踪效果。

本发明的基于云台相机的无人机对地面目标跟踪控制方法及系统，提出了一种理想相机模型，用于解决无人机姿态变化和云台姿态变化对图像上特征点坐标的影响，通过将当前图像上的特征点转化到理想相机对应图像上的特征点，从而可将后者收敛到图像中心的速度直接作为期望的特征点速率，并通过最小二乘法获得无人机的偏航角速率；进一步，由于云台姿态的变化对特征点速率不产生影响，因此云台可以在限幅范围内自由偏转以使目标处于图像中心，从而最大化云台的性能，保证在目标运动速度较快时无人机和云台也能通过偏转实现对其的跟踪。

以上仅是本发明的优选实施方式，本发明的保护范围并不仅局限于上述实施例，凡属于本发明思路下的技术方案均属于本发明的保护范围。应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理前提下的若干改进和润饰，应视为本发明的保护范围。