【K230 CanMV】K230云台舵机跟踪识别色块 PID+滤波算法

本项目是通过K230进行图像识别追踪色块进行PID电控舵机，实现色块跟踪的功能。

一、图像采集与色块识别

 # 初始化摄像头
    sensor = Sensor(width = DETECT_WIDTH, height = DETECT_HEIGHT)
    # 传感器复位
    sensor.reset()
    # 开启镜像
    sensor.set_hmirror(True)#False
    # sensor vflip
    # sensor.set_vflip(False)
    # 设置图像一帧的大小
    sensor.set_framesize(width = DETECT_WIDTH, height = DETECT_HEIGHT)
    # 设置图像输出格式为彩色的RGB565
    sensor.set_pixformat(Sensor.RGB565)

    # 使用IDE显示图像
    Display.init(Display.VIRT, width = DETECT_WIDTH, height = DETECT_HEIGHT, fps = 100)

    # 初始化媒体管理器
    MediaManager.init()
    # 摄像头传感器开启运行
    sensor.run()

在开始时，创建了一个 Sensor 对象，指定图像分辨率为 DETECT_WIDTH 和 DETECT_HEIGHT。然后，调用 sensor.reset() 方法重置摄像头传感器，以确保它从初始状态开始工作。接着，启用了水平镜像（sensor.set_hmirror(True)），使得图像在水平方向上翻转，适用于需要调整摄像头方向的情况。垂直翻转的功能（sensor.set_vflip(False)）接下来，设置了图像帧的大小为指定的分辨率，确保每帧图像的尺寸与摄像头采集的图像一致。图像格式被设置为 RGB565，这是一种常用的彩色图像格式。

随后，使用 Display.init() 方法初始化了图像显示，指定分辨率和帧率。MediaManager.init() 被调用来初始化媒体管理器，确保图像数据能正确传输和管理。最后，调用 sensor.run() 启动摄像头传感器开始图像采集和处理。

二、图像滤波

        # 拍摄一张图片
        img = sensor.snapshot()
        fps.tick()
        # 查找图像中满足红色阈值的区域
        blobs = img.find_blobs([red_threshold], pixels_threshold=200, area_threshold=200, merge=True)

        # 如果找到了至少一个blob
        if blobs:
            # 找到最大的blob
            largest_blob = max(blobs, key=lambda b: b.pixels())

            # 画框
            img.draw_rectangle(largest_blob.rect(), color=(255, 0, 0))
            # 在框内画十字，标记中心点
            img.draw_cross(largest_blob.cx(), largest_blob.cy(), color=(255, 0, 0))

            # 计算相对于屏幕中心的X轴和Y轴的偏移量
            x_offset = largest_blob.cx() - img.width() // 2
            y_offset = largest_blob.cy() - img.height() // 2
            w_offset = largest_blob.w();
            h_offset = largest_blob.h();

#            x_offset, y_offset, w_offset, h_offset = ewma_filter(x_offset, y_offset, w_offset, h_offset)

            # 屏幕显示位置信息和像素大小，包含正负号
#            wz = "x={}, y={}, w={}, h={}".format(x_offset, y_offset, largest_blob.w(), largest_blob.h())
            wz = "x={}, y={}, w={}, h={}".format(x_offset, y_offset, w_offset, h_offset)
            img.draw_string_advanced(0,0,32,wz)

            # 根据中心偏移量计算PWM的PID
            pid_lr_value = pid_lr.pid_calc(0,x_offset - camera_offset)
            pid_ud_value = pid_ud.pid_calc(0,y_offset)

            # 滤波并且输出实际的占空比
            duty_lr_value = input_to_duty_cycle(-lr_filter.update(pid_lr_value))
            duty_ud_value = input_to_duty_cycle(ud_filter.update(pid_ud_value))

            # 根据计算后的占空比控制舵机
            pwm_lr.duty(duty_lr_value)
            pwm_ud.duty(duty_ud_value)

            # zxc = "{}, {}".format(-pid_lr_value,pid_ud_value)
            # print(zxc)


        # 中心画十字
        img.draw_cross(img.width() // 2 + camera_offset, img.height() // 2, color=(0, 255, 0), size=10, thickness=3)
        # IDE显示图片
        Display.show_image(img)
        #输出帧率
        #print(fps.fps())

识别区域计算目标的偏移量，并根据偏移量使用PID控制算法调整舵机位置的功能。

首先，摄像头捕捉到一帧图像，并查找图像中符合红色阈值的区域（使用 img.find_blobs() 函数）。这些区域被称为“blob”，是图像中具有特定颜色特征的区域。如果找到符合条件的 blob，程序会从中选择最大的一个，即拥有最多像素的区域（通过 max(blobs, key=lambda b: b.pixels())）。然后，程序在该区域绘制一个矩形框，并标记其中心点。

接着，程序计算该目标相对于图像中心的偏移量，包括X轴和Y轴的偏移量以及目标的宽度和高度。这些偏移量是通过计算目标中心点与图像中心的差值得到的。然后，程序会在图像上显示这些偏移量和目标的大小信息。

通过计算X轴和Y轴的偏移量，程序将这些值输入到PID控制器中，以计算出舵机的调整值。PID控制器的输出值通过滤波器进行处理，以确保舵机控制更加平稳。最后，根据计算出的占空比，舵机的PWM信号被更新，从而控制舵机调整到正确的位置。

此外，程序还会在图像中心绘制一个绿色的十字，表示图像的中心点。最后，图像会通过显示器进行显示，并输出帧率（即每秒处理的图像帧数）。这段代码结合了目标识别、图像处理、PID控制和舵机驱动，完成了基于颜色跟踪和位置控制的任务。

三、控制舵机

    # 配置排针引脚号12，芯片引脚号为47的排针复用为PWM通道3输出
    pwm_io1 = FPIOA()
    pwm_io1.set_function(47, FPIOA.PWM3)
    pwm_ud = PWM(3, 50, 50, enable=True)  # 配置PWM3，默认频率50Hz,占空比50%

    # 配置排针引脚号32，芯片引脚号为46的排针复用为PWM通道2输出
    pwm_io2 = FPIOA()
    pwm_io2.set_function(46, FPIOA.PWM2)
    pwm_lr = PWM(2, 50, 50, enable=True)  # 配置PWM2，默认频率50Hz,占空比50%
    pwm_lr.duty(7.5)    #左右舵机旋转到中间
    pwm_ud.duty(7.7)    #上下舵机旋转到中间

简单的通过PWM来控制舵机即可，不多赘述

四、PID 控制

class PID:
    def __init__(self, kp, ki, kd, maxI, maxOut):
        #静态参数
        self.kp = kp
        self.ki = ki
        self.kd = kd
        #动态参数
        self.change_p = 0
        self.change_i = 0
        self.change_d = 0

        self.max_change_i = maxI    #积分限幅
        self.maxOutput = maxOut     #输出限幅

        self.error_sum = 0  #当前误差
        self.last_error = 0 #之前误差

    def change_zero(self):#PID变化累计的参数清零
        self.change_p = 0
        self.change_i = 0
        self.change_d = 0

    def pid_calc(self, reference, feedback):#reference=目标位置	feedback=当前位置
        self.last_error = self.error_sum
        self.error_sum = reference - feedback #获取新的误差

        #计算微分
        dout = (self.error_sum - self.last_error) * self.kd

        #计算比例
        pout = self.error_sum * self.kp

        #计算积分
        self.change_i += self.error_sum * self.ki

        #积分限幅
        if self.change_i > self.max_change_i :
            self.change_i = self.max_change_i
        elif self.change_i < -self.max_change_i:
            self.change_i = -self.max_change_i

        #计算输出
        self.output = pout + dout + self.change_i

        #输出限幅
        if self.output > self.maxOutput:
            self.output =   self.maxOutput
        elif self.output < -self.maxOutput:
            self.output = -self.maxOutput

        return self.output

在该类中，构造函数 __init__() 初始化了 PID 控制器的参数：

• kp、ki 和 kd 分别是比例、积分和微分的增益常数。
• maxI 是积分限幅，用于限制积分项的累计误差。
• maxOut 是输出限幅，防止输出值超过一定范围。

动态参数包括：

• change_p、change_i 和 change_d 用于保存各项的变化值，虽然它们并没有直接用于计算输出。
• error_sum 用于保存当前误差的积分值，即误差的累积。
• last_error 保存前一时刻的误差，用于计算微分。

change_zero() 方法用于清除所有的累计参数，即将 change_p、change_i 和 change_d 重置为零。

pid_calc() 方法是控制器的核心，它根据输入的目标位置 reference 和当前反馈值 feedback 计算出新的控制输出。具体计算过程如下：

1. 计算误差：通过 reference - feedback 获取当前误差，并将其累加到 error_sum 中。
2. 计算比例项 (P)：比例项是误差的比例，pout = error_sum * kp。
3. 计算积分项 (I)：积分项是误差的累积，change_i += error_sum * ki。积分值会受到 maxI 限幅的约束，避免积分项无限增长。
4. 计算微分项 (D)：微分项是当前误差与上一次误差的差，dout = (error_sum - last_error) * kd。
5. 计算输出：输出是三项（比例、积分、微分）的加权和。
6. 输出限幅：控制器的输出值会被 maxOut 限制，避免过大的输出值影响系统稳定性。

在之前的文章也有专门讲到PID，不懂的可以去看看。