这周在 AirSim 里做了一个基于 KCF 算法的地面（下方）物体追踪仿真，写一篇博客记录一下，以防后面忘记现在在干什么（

任务介绍

我希望使用无人机的底部摄像头拍摄下方物体，使用 KCF 算法检测该物体的位置，使用 PID 作为控制器，通过控制无人机的水平方向速度保持目标物体处于下方摄像头拍摄图像的中央。任务流程及控制器结构如下所示。

mission

KCF 算法

肯德基核滤波相关算法（Kernel Correlation Filter，KCF）基于核化的岭回归分类器使用循环移位得到的循环矩阵来采集正负样本，利用循环矩阵在傅里叶空间可对角化的性质，将矩阵的运算转化为元素的点乘，从而降低了运算量，使得算法满足实时性要求。同时，KCF使用多通道HOG特征代替单通道灰度特征，将特征扩展到多通道的非线性特征空间，达到了更高的鲁棒性和准确性。

KCF 核心公式

  • 核相关矩阵的初始向量

    $$ \mathbf k^{\mathbf{xx'}}=\exp\left(-\frac{1}{\sigma^2}\left(\Vert \mathbf x\Vert^2 + \Vert \mathbf x'\Vert^2-2\mathcal F^{-1}(\sum_c\hat{\mathbf x}_c^* \odot \hat{\mathbf x_c})\right) \right) $$

    其中 $\mathcal F$ 表示傅里叶变换，$\mathbf x’=\mathcal F(\mathbf x)$，$\odot$ 表示卷积

  • 核化后的岭回归分类器权值

    $$ \hat{\alpha}=\frac{\hat{\mathbf y}}{\hat{\mathbf k}^{\mathbf{xx}}+\lambda} $$

  • 快速检测

    $$ \hat{\mathbf f}(\mathbf z)= \hat{\mathbf k}^{\mathbf{xz}} \odot \hat\alpha $$

详细原理可参考这篇博客或论文原文。

代码实现

OpenCV 已经完成了 KCF 算法的实现，可以直接调用。我实现时参考了这篇博客中的代码，添加了选择框。

PID 控制器

将 PID 控制器进行了简单的封装。

'''
PID.py
'''

class PIDController:
    def __init__(self, kp = 1.0, ki=0.0, kd=0.0, setpoint=0):
        self.kp = kp
        self.ki = ki
        self.kd = kd
        self.setpoint = setpoint

        self.prev_error = 0
        self.integral = 0

    def update(self, current_value):
        error = self.setpoint - current_value
        self.integral += error
        derivative = error - self.prev_error

        output = self.kp * error + self.ki * self.integral + self.kd * derivative

        self.prev_error = error
        return output

AirSim 设置

更改 User/Documents/AirSim/settings，添加两架无人机。追踪机为 Drone1，被追踪机为 Drone2：

{
    "SettingsVersion": 1.2,
    "SimMode": "Multirotor",
    "ClockSpeed": 1,

    "Vehicles": {
		"Drone1": {
		  "VehicleType": "SimpleFlight",
		  "X": 0, "Y": 0, "Z": 0,
          "Yaw": 0
		},
		"Drone2": {
		  "VehicleType": "SimpleFlight",
		  "X": -2, "Y": 0, "Z": 0,
          "Yaw": 0
		}

    }
}

在 Python 主程序中，先控制两架无人机上升，使 Drone1 在 Drone2 上方的一定位置，随后会弹出openCV窗口（Drone1 底部摄像头），框选目标无人机，然后 Drone2 开始水平移动，Drone1 进行追踪。参考代码

追踪效果

目前使用简单的水平匀速移动与简单的 P 控制器验证了该方案的可行性。$x$ 方向上的速度如图所示，可见 Drone1 的跟踪确实可以收敛。

trace