2023电赛E题 记实开发 已开源 附带源码+MATLAB算法仿真

 前言：2023电赛E题 在电赛期间本团队完成基础题1、2、3以及附加题1，现将开发日记以及源码分享给大家。

硬件：我们采用双K210，一个做红色激光控制 一个做绿色追踪 ，采用视觉加舵机控制 因此硬件需要2个K210 4个舵机即可。

软件：手撸算法，大家下载过去可以自行调参，使用的自制算法和PID，具体参数原理已在代码中标明。

                                          MATLAB仿真代码以及K210源码均在文末

正文：

一、 系统方案
本设计采用 K210 为主控，整体系统主要由主控与舵机组成。
1.1. 主控模块的论证与选择
方案一：选择 K210 芯片，搭载了双核 64 位 RISC-V 处理器和专门的 AI 加速器，具有高性能的计算能力；支持常见的图像处理任务，并且有多个 GPIO 接口、 UART、 I2C、 SPI 等常用接口，方便与其他外设进行连接和扩展，满足不同应用场景的需求。
方案二：选用OpenMV 芯片。OpenMV芯片使用MicroPython作为开发语言，提供了简洁的 API 和丰富的示例代码；支持多种图像处理算法；采用了优质的图像传感器，可以在低光条件下工作，并能够实时采集高质量的图像数据。
方案三：选用树莓派，树莓派搭载了 ARM 架构的处理器，性能较强，能够处理多种复杂的算法和任务；具有多个 GPIO 接口、 USB 接口、 HDMI 接口等常见接口；拥有庞大的开源社区支持，有大量的资源、文档和示例代码可供参考和使用。
  最终选择：综合考虑上述各项方案，我们选择方案一，因为 K210 芯片具备强大的图像处理和控制接口功能，可开发性强，成本低，适合该系统的需求。
1.2. 角点检测算法选择
方案一： Harris 角点检测算法，使用一个固定的窗口在图像上进行任意方向上的滑动，比较滑动前与滑动后两种情况，窗口中的像素灰度变化程度，如果存在任意方向上的滑动，都有着较大的灰度变化，那么我们可以认为该窗口中存在角点。

方案二： Shi-Tomasi 算法，是对 Harris 角点检测算法的升级版，是针对 R值的计算公式进行了优化和修改。由于 Harris 角点检测算法的稳定性和α 值有关，而α 是个经验值，需要我们根据自身经验去设置，不好设定最佳值。通过矩阵 M的较小特征值有关(1,2 )于是直接用较小的那个特征值作为分数。这样就不用调整α 值了。
    
   方案三： FAST 角点检测算法，速度最快在像素与其周围邻域内足够多的像
素点相差较大，则该像素可能是角点。
   方案选择：方案二相对于方案一在算法基础上具有突破，但考虑到我们具体
的功能以及速度要求，以及图片处理速度和精确度等方面综合考虑，最终我们选择方案三。根据 MATLAB 建模仿真分析，在满足题目中的要求的同时，准确度达到94%。综上所述，最终选择方案三FAST角点检测算法进行矩形识别与移动。

 1.3. 追踪控制算法选择
方案一： 反馈线性化控制算法
  首先，我们通过几何追踪法获取 K210 中心点与目标点的欧氏距离，并计算机械臂末端的坐标位置。然后，使用反馈线性化控制算法对机械臂的非线性动力学进行线性化处理。
方案二： 机械臂正逆解的算法
  机械臂正逆解的算法是基于角函数的方法。题目要求云台与激光笔在屏幕的1m 处，这使得我们可以使用绿色激光追踪红色激光非常方便。我们采用了几何追踪法来实现这一功能。通过获取 K210 中心点与目标点的欧氏距离后，进行坐标转化为舵机的转动角度从而实现追踪控制。
  方案选择：方案一相对于方案二尽管在精度上可能更加准确但在复杂程度远远大于方案二， 并且对算力要求较高，需要准确建模非线性动力学，否则线性化可能带来误差。 在具体的 K210 上效果不太理想.而机械臂正逆解算法适用于简单机械臂结构和任务，并可以通过几何追踪法实现目标的基本追踪。 因此我们最终选择了方案二。经过验证，几何算法通过测得两点的欧氏距离，补充给舵机实现追踪效果最佳。
二、 电路与程序设计
由于我们本次的主控板是用 k210， 为了保证我们设计的系统稳定性与集成度更高， 我们自主设计了 PCB 板并进行了打样， 所有的元器件都是依附于 k210进行运行。 依次我们设计了出来原理图， 简单明了， 所有功能都可以实现。
其原理图如下：
   

                           

   通过我们的电路设计以及程序， 实现了运动目标控制系统与自动追踪系统，其程序逻辑如下：

                                             

 在算法写入时， 我们根据实际情况， 在硬件的摄像头像素点上进行了测算，在设计要求 1m 的情况下， 摄像头可看到水平方向 70 度的视野夹角， 垂直方向上存在 45 度的视野夹角， 并且 1m 内为 320 个像素点， 整体绿色激光在摄像头的位置为(198， 118)的坐标位置， 并非在屏幕中心， 因此需要调整补偿量。 最终通过软硬结合， 在硬件上电路集成， 在软件算法上补短优化， 最终实现了预想功能

三、 理论分析与计算
3.1. 角点检测算法分析仿真
3.1.1. FAST 算法角点检测的分析
  FAST 算法是一种高效的角点检测算法，它通过对像素点进行快速的灰度比较来确定角点。该算法的原理基于以下观察：角点处的像素与其周围的像素相比，灰度值会发生明显的变化。 FAST 算法利用这一特性，对每个像素点进行 16 个像素的比较，并根据一定的条件判断该像素是否为角点。由于篇幅有限显示出仿真图片，代码详细在附录

 

 3.1.2. Harris 角点检测算法
  Harris 角点检测算法是一种常用的角点检测方法。它通过计算图像局部区域的梯度来判断像素是否为角点。算法利用 Harris 响应函数对每个像素点进行评估，并根据响应值来确定角点。由于篇幅有限显示出仿真图片， 代码详细在附录

3.1.3. Shi-Tomasi 角点检测算法
  Shi-Tomasi 角点检测算法是对 Harris 角点检测算法的改进。它使用最小特征值代替 Harris 算法中的特征值计算，这样可以更好地选择稳定角点。相对于 Harris算法， Shi-Tomasi 算法在角点检测方面更加鲁棒。由于篇幅有限显示出仿真图片，
代码详细在附录
 

 3.1.4. 比较分析
将上述三种算法进行可视化的分析，将不同的参数进行绘制可得到以下三个折线图，如

   可知在 k210 进行 QVGA 分辨率的图片识别时，由于分辨率较小，所截取的像素点较小，在进行 Harris 角点检测算法时误差最小是的范围是 0.7-0.8，误差为 1，在进行 Shi-Tomasi 角点检测算法时误差最小是的范围是 0.7 左右，误差为 8，在进行 FAST 角点检测算法时误差最小是的范围是 7-9，误差为 1，而在8 是误差为 0。因此在小分辨率的情况下， FAST 角点检测算法时我们设备的最佳算法。

3.2. 追踪控制算法的实现(手撸算法开始)
  通过获取 K210 中心点与目标点的欧氏距离后，进行坐标转化为舵机的转动角度从而实现追踪控制。主要流程为：云台参数初始化，偏差点坐标获取，逆解计算，正解验证， 控制器设计追踪控制。

3.2.1. 偏差点坐标获取
首先进行当前坐标的获取。由于绿色激光的位置并不是在摄像头正中心， 也放不到正中心。但是绿色激光的发射端是和摄像头进行绑定的，在云台进行转动后，绿色激光随着云台上的摄像头一起转动，这就导致了绿色激光的位置是在摄像头所捕捉画面的固定位置，通过捕获绿色激光的像素位置得到当前位置，是一个固定的值（198， 118）。同样的道理进行画面的红色捕捉，得到所追踪的目标位置进行相减得到偏差坐标
3.2.2. 逆解计算与正解验证
使用逆解算法，将目标点的坐标（ xt, yt,）转化为对应的舵机角度（ theta1,theta2, ...）。可以采用几何追踪法或其他逆解算法，根据机械臂的几何结构和运动学模型来计算舵机角度。
  首先我们将偏差点的坐标（xt, yt,）转化为相机的水平视角与垂直视角，以相机的参数模拟舵机进行测试，在进行正解验证时发现所得到的误差较大，可能是相机的参数虽然和像素的偏差坐标相关性较大但和舵机的相关性较小甚至没有。所以我们在进行寻找中间参数关联两者时，应寻找到一个对两者的相关性都差不多的中间参数，以中间参数搭建起像素坐标与舵机角度的关系。但由于没有
满足要求的中间参数，我们只能进行比例变量尝试到合适的值，以增量式 PID 输出。对于我们的设备环境，水平与垂直方向的比例系数为 x， x。此方案的正解验证无差较小，满足题目的要求。

 

 

 MATLAB仿真代码：

代码：
img = imread('x.jpg');
gray_img = rgb2gray(img);
% 不同的 N 值
Ns = 1:20;
% 存储不同 N 值下检测到的角点数量
num_corners = zeros(1, length(Ns));
% 可视化不同 N 值下的 FAST 角点检测结果和角点数量
figure('Position', [200, 200, 1200, 400]);
for i = 1:length(Ns)
N = Ns(i);
% 使用 detectFASTFeatures 函数进行 FAST 角点检测
corners = detectFASTFeatures(gray_img, 'MinContrast', 0.3);
% 选择最强的 N 个角点
strongest_corners = corners.selectStrongest(N);
% 统计检测到的角点数量
num_corners(i) = strongest_corners.Count;
% 显示检测结果
subplot(2, 10, i);
imshow(img);
hold on;
plot(strongest_corners);
title(['N = ', num2str(N), ', 角点数 = ', num2str(num_corners(i))]);
hold off;
end
sgtitle('FAST 角点检测结果及角点数可视化', 'FontSize', 16);
% 绘制 N 值与角点数量的可视化折线图
figure('Position', [400, 200, 800, 400]);
plot(Ns, num_corners, 'b-o', 'LineWidth', 2, 'MarkerSize', 8);
xlabel('N', 'FontSize', 14);
ylabel('角点数量', 'FontSize', 14);
title('FAST 角点检测中 N 值与角点数量的关系', 'FontSize', 16);
grid on;
ax = gca;
ax.FontSize = 12;
% 在图形上添加数据标签
for i = 1:length(Ns)
text(Ns(i), num_corners(i), num2str(num_corners(i)), 'FontSize', 12,
'VerticalAlignment', 'bottom', 'HorizontalAlignment', 'center');
end
% 调整折线图的横坐标范围和 y 轴坐标范围
xlim([min(Ns)-1, max(Ns)+1]);
ylim([0, max(num_corners)+5]); % 取值略大于角点数量的最大值

 K210绿光追踪代码：

import sensor,image,lcd,math

from machine import UART
import utime
from machine import Timer,PWM
lcd.init()
sensor.reset()
sensor.set_pixformat(sensor.RGB565)
sensor.set_framesize(sensor.QVGA)
sensor.set_hmirror(1)  # 如果图像是镜像的，请设置为1
sensor.set_vflip(1)    # 如果图像是上下翻转的，请设置为1
sensor.run(1)
#kx=0.291
#ky=0.116
kx=0.07
ky=0.07

angle_x=0
angle_y=0

flag=1

angle_now_x = 0
angle_now_y = 0

tim = Timer(Timer.TIMER0, Timer.CHANNEL0, mode=Timer.MODE_PWM)
tim2 = Timer(Timer.TIMER1, Timer.CHANNEL0, mode=Timer.MODE_PWM)
tim3 = Timer(Timer.TIMER2, Timer.CHANNEL0, mode=Timer.MODE_PWM)
S1 = PWM(tim, freq=50, duty=0, pin=15)
S2 = PWM(tim2, freq=50, duty=0, pin=12)
S3 = PWM(tim3, freq=50, duty=0, pin=11)
red_threshold = (67, 86, 10, 127, -21, 127) # (L Min, L Max, A Min, A Max, B Min, B Max)

#X_pid = PID(p=1.35, i=1.1, imax=40)#在线调试使用这个PID
#Y_pid = PID(p=1.3, i=0.9, imax=40)#在线调试使用这个PID
# 设置云台控制范围
#pan_servo_range = (-90, 90)   # 云台水平角度范围
#tilt_servo_range = (-45, 45)  # 云台垂直角度范围

def servo_range(servo,angle):
       global angle_x
       global angle_y

       if servo == S1:
         if angle<-10:
            angle=angle_x
         if angle >10:
            angle=angle_x
       if servo == S2:
         if angle<-10:
           angle=angle_y
         if angle >10:
           angle=angle_y

def Servo(servo,angle):
    servo_range(servo,angle)
    servo.duty((angle+90)/180*10+2.5)


Servo(S1,30)
Servo(S2,10)
utime.sleep_ms(2000)

while True:



    img = sensor.snapshot()
    blobs = img.find_blobs([red_threshold],threshold = 200)

    if blobs:
        for blob in blobs:
            img.draw_rectangle(blob.rect())
            img.draw_cross(blob.cx(), blob.cy())
            img.draw_string(blob.x() + 2, blob.y() + 2, "red")
            print("Detected red block at (x, y): ({}, {})".format(blob.cx(), blob.cy()))

            # 计算色块中心与图像中心的偏移
            img_center_x = 198
            img_center_y = 118
            offset_x = img_center_x - blob.cx()
            offset_y = img_center_y - blob.cy()

            print(blob.cx(),blob.cy())

            if math.fabs(offset_x) <=2 or math.fabs(offset_y)<=2 :
                flag=2

            else:
                flag=1
            if math.fabs(offset_x) <=5 or math.fabs(offset_y)<=5 :
                S3.duty(100)
            else:
                S3.duty(0)
            if flag==1:
             #将偏移转换为角度
                angle_x = offset_x * kx
                angle_y = offset_y * ky

                angle_now_x += angle_x
                angle_now_y += angle_y

                servo_range(S1,angle_now_x)
                servo_range(S2,angle_now_y)

                    ##angle_x_output=X_pid.get_pid(angle_x,1)/2
                    ##angle_y_output=Y_pid.get_pid(angle_y,1)
                    #x+=angle_x
                    #y+=angle_y
                print(angle_now_x,angle_now_y)
                Servo(S1,angle_now_x)
                Servo(S2,angle_now_y)
                utime.sleep_ms(10)




    lcd.display(img)
    # 添加500毫秒的延迟
    utime.sleep_ms(20)