笔者参加了2023年电赛，E题思路原理非常清晰简单。但在实现过程中，会出现非常多的问题。

舵机问题：出现大角度抖动，舵机最小步进幅度过大，舵机齿轮存在空程差导致左右移动幅度不同，精度低，IDE提供的舵机速度控制函数失效，PID不灵敏。

Openmv问题：摄像头分辨率不够，光线和背景亮度变化导致无法稳定识别黑框，激光点的高亮度导致无法识别其颜色，红色激光点落入黑胶布区域时无法被识别，鱼眼镜头采集图片的畸变误差不易校正，openmv内置的STM32芯片内存不够无法进行大量计算。

物理问题：屏幕坐标系和云台坐标系不重合，不仅有中心点的位置偏差，还可能存在坐标轴旋转偏差。

        大部分问题都找到了解决方法，这也是我写这篇博客分享设计过程经验的初衷。但还有小部分问题有待解决，这需要增加更多的硬件资源来实现。相比于本方案只使用openmv的控制程序，添加外设大大增加了开发流程的复杂性和难度。因此本文暂且不做介绍。鉴于笔者水平有限，文中可能有不妥和错误的地方，如果诸位有用openmv解决遗留问题的思路，还请不吝斧正，谢谢。

效果展示视频在B站：

云台跟踪激光点

1.题目功能分析

        本文上篇先介绍云台追踪方法，这部分较为简单，题目要求如下，设计过程中如有增减修改将会特殊说明。

        追踪算法思路很简单，将摄像头与跟踪激光笔一同固定在旋转舵机云台上，激光笔的指向与镜头的指向就会一同随舵机转动，跟踪光点像素坐标是固定的。这时如果屏幕上识别到了另外的激光点就是目标点。计算出两个点分别在x和y方向的像素差，驱动舵机向像素差减小的方向移动一个最小步进。然后再次进行拍照识别，不断的重复上述驱动过程，直到x和y方向的像素差小于设定值。随着目标点的不断移动，该跟踪过程可以一直持续。在实现过程中却遇到以下问题。

2.代码实现中的问题与解决方法

问题一：红色和绿色激光笔光点在openmv的机器视觉中LAB阈值相近，只能读出光点位置，无法分辨颜色。

        在白色背景上，红色和绿色激光笔的光点在openmv的机器视觉中显示的亮度极高，接近纯白色即LAB阈值中L接近100。因此无法通过区分色块阈值以分辨出追踪点和目标点。只能通过L值读出光点位置（光点当然比背景白色亮很多，这很好区分）。

解决方法：在设计中，我们将摄像头与跟踪激光笔一同固定在旋转舵机云台上，这样激光笔的指向与镜头的指向就会一同随舵机转动，其像素坐标是固定的，测量可得。在跟踪过程中，在该像素坐标上的激光点就是跟踪激光点，另一个不在该像素坐标上的激光点就是目标点。 在该追踪设计方案中，由于采用像素坐标位置区分跟踪点和目标点，因此可以直接使用灰度阈值进行识别，排除更多干扰的同时还能节省内存。

问题二：在实际操作中，由于摆放重装以及手工固定不牢固等原因，每次追踪激光笔的初始坐标并不完全固定。

解决方法：进行识别校正，当同时识别到的两个光点距离较远时，分别计算两个光点与事先测得的追踪坐标的距离，将距离近的点坐标重新赋值给预设红点坐标。

问题三：红色激光笔不够亮，在黑色胶带上无法被识别，因此屏幕中的激光点数量将不再固定是2个，存在只识别到1个的可能性。

        当激光点落入黑色胶带区域时，无论红色还是绿色，其亮度都会变低很多。市面常见的红激光笔光点落在黑色胶带上时，虽然人眼可以分辨，但在openmv中已经无法识别了。好在绿色激光笔在黑色胶带上可勉强识别。注：我们使用的黑色胶带是市面上常见的3M电工胶带，更换不同的黑胶带会不会有不同的效果，尚没有进行尝试。

解决方法：更换更加高亮度的红色激光笔。由于openmv可以勉强识别绿色激光笔（在黑胶带上），为了节省时间和成本，本文直接调换了两只激光笔的功用， 使用红色激光笔追踪绿色激光笔。这与题目要求有所不一致，但并不妨碍我们对跟踪过程的设计。在视频展示中，能够看到，无论是在白色底纸上，还是在黑色胶带上，红色激光点都能完美的跟踪绿色激光点。

当识别到两个点的时候，若二者距离较远，被视为没有追上，比较二者与预设追踪点坐标的距离，距离远的为目标点，距离近的为追踪点，此时将追踪点的坐标重新赋值给预设追踪点坐标。一般而言，开机复位功能基本能保证最初摄像模块可以识别到两个激光点完成校正。

当只识别到一个点的时候，识别到的点必为绿点即目标点，此时启用追踪点预设坐标。由于只识别到一个点的情况必然发生在矫正预设坐标之后，因此预设坐标已经被重新赋值校正过，精度很高，可以保证直接追踪的效果。

当识别到三个点的时候，程序灯光报警，清空识别到的点坐标，短时间暂停后重新识别。

问题四：控制舵机速度函数不生效，追踪过快会刹不住车出现大范围摆动调整。

解决方法：详细理解掌握星瞳科技提供的PID较为复杂，因此本设计改用PWM频率控制舵机运动，手动添加延时。并根据距离远近调整延时量以保证在2秒之内追上目标。

遗留问题：

1.常见舵机步进幅度为1度，换算到一米之外的幕布上距离约为16mm,精度不够高。

以下为追踪部分全部代码：

import sensor, image, time,math
from pyb import Pin
#from pid import PID
from pyb import Servo
from pyb import LED


#红追绿，由于红色激光照到黑胶布上，红色激光无法识别。而绿色的可以，
#因此我们使用绿色作为移动目标，红色作为跟踪。

#本舵机使用红色激光笔，去追踪绿设激光点。
#舵机结构红色激光笔与镜头一体，同时运动

#红色中心点初始位置为 170，110，这个根据自己激光笔和摄像头固定的位置测量得到，
#有可能存在误差
red_x=170
red_y=110


pan_servo=Servo(1)
tilt_servo=Servo(2)

#p_in0 = Pin('P0', Pin.IN, Pin.PULL_UP)#设置p_in为输入引脚，并开启上拉电阻，复位用
p_in1 = Pin('P1', Pin.IN, Pin.PULL_UP)#设置p_in为输入引脚，并开启上拉电阻，停止光标移动
p_in2 = Pin('P2', Pin.IN, Pin.PULL_UP)#设置p_in为输入引脚，并开启上拉电阻，重启光标移动
#如果单纯实现追踪，可忽略以上设置。
pan_servo.calibration(500,2500,500)
tilt_servo.calibration(500,2500,500)
time.sleep_ms(300)

gray_threshold = (0, 191)#灰度阈值根据实际情况可微调，排除最亮的点即可，后续程序有取反
green_threshold  = (100, 78, -32, 91, -60, 99)#此处LAB阈值红绿通用，不改程序名称了
sensor.reset() # Initialize the camera sensor.
#sensor.set_pixformat(sensor.RGB565) # use RGB565.
sensor.set_pixformat(sensor.GRAYSCALE) # use GRAYSCALE.
sensor.set_framesize(sensor.QVGA) # use QVGA for speed.
sensor.skip_frames(10) # Let new settings take affect.
sensor.set_auto_whitebal(False) # turn this off.
clock = time.clock() # Tracks FPS.


while(True):

    # 但由于安装不够稳固，每次安装会有少许变化，需要在程序中进行修正。red_x,red_y
    # 跟踪过程实际上是用修正后的红色坐标去追踪绿色目标

    ss=[[0,0],[0,0],[0,0]] #存储识别到的亮点坐标，实际上只需要两个，防止溢出多开一个空间
    min_size=400
    for i in range (1000):
        if p_in1.value()==0:  #暂停mov_pwm
            while(True):
                if p_in2.value()==0:  #恢复mov_pwm
                    break
        j=0
        r=[900,900]  #初始化两个亮点到标准绿点的距离为一个比较大的值
        img = sensor.snapshot()
        #blobs = img.find_blobs([green_threshold],x_stride=1,y_stride=1)
        blobs = img.find_blobs([gray_threshold],x_stride=1,y_stride=1,invert=True)
        if i > 10 :
            for blob in blobs:
                #尽量过滤掉一些杂散的blobs
                if (blob[2]*blob[3] < min_size) and (blob[2]>2) and  (blob[3]>2) and (blob[2]<2*blob[3]) and (blob[3]<2*blob[2]):
#确保色块较小且较方（圆）
                    yuandian = blob
                    img.draw_rectangle(yuandian.rect(),color = (0,0,0)) # rect
                    img.draw_cross(yuandian.cx(), yuandian.cy()) # cx, cyj v
                    print(yuandian.cx(), yuandian.cy(),blob[2],blob[3])

                    if j>1:  #蓝色LED点亮，表示搜索到3个以上的目标，设备不能正常跟踪
                        LED(3).on()   #点亮蓝灯
                        time.sleep_ms(500)
                        #LED(3).off()
                        ss=[[0,0],[0,0],[0,0]]
                        break
                    else:    #逐个记录识别到的激光点
                        ss[j][0]=yuandian.cx()
                        ss[j][1]=yuandian.cy()
                    j=j+1

            if j==2: #只发现两个亮点
                LED(3).off()  #关闭蓝灯
                #计算两个亮点到初始红点的距离，谁近则谁就是追踪点，远的就是目标点
                r[0]=math.sqrt((ss[0][0]-red_x)*(ss[0][0]-red_x) + (ss[0][1]-red_y)*(ss[0][1]-red_y))
                r[1]=math.sqrt((ss[1][0]-red_x)*(ss[1][0]-red_x) + (ss[1][1]-red_y)*(ss[1][1]-red_y))

                #计算两个亮点之间的距离
                rr =math.sqrt((ss[0][0]-ss[1][0])*(ss[0][0]-ss[1][0]) + (ss[0][1]-ss[1][1])*(ss[0][1]-ss[1][1]))
                if rr>20:  #当两个亮点距离比较大的时候，动态的修正red_x，和red_y
#即将距离近的点坐标幅值给初始红坐标。
                    if (r[0]<r[1]) and (r[0]<30) :
                        red_x = ss[0][0]
                        red_y = ss[0][1]
                    if (r[1]<r[0]) and (r[1]<30) :
                        red_x = ss[1][0]
                        red_y = ss[1][1]

                if rr<6:  #如果两点之间的距离小于6个像素，则认为已经跟踪上了
                    LED(2).on()   #点亮绿灯
                else:
                    LED(2).off()   #熄灭绿灯
                    now_x=pan_servo.pulse_width()
                    now_y=tilt_servo.pulse_width()
                    dx = ss[0][0]-ss[1][0]
                    dy = ss[0][1]-ss[1][1]
                    if r[0]>r[1]:  # 再次根据远近判断红绿点，并确定距离方向。因为两点初始距离若小于等于20，则程序没有对预设的红点坐标进行动态校正。
                        dx=-dx
                        dy=-dy

                    if p_in1.value()==0:  #暂停mov_pwm
                        while(True):
                            if p_in2.value()==0:  #恢复mov_pwm
                                break

                    if dx>5:
                        pan_servo.pulse_width(now_x+10)  #最小步进是10，也许不同舵机有出入，可自行调整尝试。
                    if dx<-5:
                        pan_servo.pulse_width(now_x-10)
                    if dy>5:
                        tilt_servo.pulse_width(now_y+10)
                    if dy<-5:
                        tilt_servo.pulse_width(now_y-10)

                    if math.sqrt(dx*dx + dy*dy)>30 :
                        time.sleep_ms(10)
                    else:
                        time.sleep_ms(50)

            if j==1 and r[0]>6: #如果只发现一个亮点，则用标准红点red_x,red_y,去追踪这个点
                now_x=pan_servo.pulse_width()
                now_y=tilt_servo.pulse_width()
                dx = red_x-ss[0][0]
                dy = red_y-ss[0][1]

                if p_in1.value()==0:  #暂停mov_pwm
                    while(True):
                        if p_in2.value()==0:  #恢复mov_pwm
                            break

                if dx>5:
                    pan_servo.pulse_width(now_x+10)  #最小步进是10
                if dx<-5:
                    pan_servo.pulse_width(now_x-10)
                if dy>5:
                    tilt_servo.pulse_width(now_y+10)
                if dy<-5:
                    tilt_servo.pulse_width(now_y-10)

                if math.sqrt(dx*dx + dy*dy)>30 :  #根据距离不同设置休眠时间
                    time.sleep_ms(10)
                else:
                    time.sleep_ms(50)

    print("1111111111111111111111111111111111")

3. 硬件架构

        追踪云台由LD1501舵机、LM2596S降压稳压模块、Openmv4 Cam H7摄像模块、常见红绿激光笔、3M电工胶、面包板杜邦线、还有重量级烟灰缸硬纸壳等组成。舵机和摄像模块由于参数不同需单独供电并且共同接地减小干扰，否则舵机会高速乱摆对机器造成损坏。