K210学习笔记——三角函数下的目标追踪

各位正在读文章的朋友们你们好，本人为非专业学生，如有不对的地方，期待您的指正。

目标追踪的意思是：识别到目标物体，通过舵机转动，朝向目标物体.

实验器材：二自由度舵机云台加两个SG90舵机，K210。

实验分两步走：一是识别到目标物体，二是识别到色块后返回色块位置坐标，进而控制舵机转动指向目标物体。

一：识别目标色块——image.find_blobs函数：

识别颜色方块，在编程环境中，打开工具中的机器视觉、阈值管理器，在网上找一个颜色方块，或者在缓冲区内，选择最佳的颜色跟踪阈值，通过不断调整，使目标物体在二进制图像中呈现白色，其余为黑色。下面的一串数字即为目标物体特定的颜色阈值。

image.find_blobs(thresholds, roi=Auto, x_stride=2, y_stride=1, invert=False, area_threshold=10, pixels_threshold=10, merge=False, margin=0, threshold_cb=None, merge_cb=None)

image.find_blobs()函数用来识别目标色块。

thresholds为目标色块的颜色阈值。

roi为感兴趣区，我这里只有一个色块，所以不用理它。

x_stride和y_stride分别是查找的色块在x,y方向上的最小宽度的像素。结合实际摄像头到目标色块的距离，设置它的大小，可排除一些误差。

通过此函数，可以获取以下的值：

blobs[0] = 色块外框的X坐标，blobs[1] = 色块外框的Y坐标，blobs[2] = 色块外框的宽度，blobs[3] = 色块外框的高度，blobs[4] = 色块外框的像素数量，blobs[5] = 色块外框的中心点横坐标，blobs[6] = 色块外框的中心点纵坐标。

利用这些值，我们可以在显示屏上“圈住”目标色块、在旁边写单词，画中心叉等。

最重要的就是获取中心点横纵坐标(blobs[5]，blobs[6])，用它们来确定色块的位置。

舵机的转动——Servo函数：

舵机的转动是由PWM控制的，利用内部的定时器生成20ms为一个周期的脉冲，只有在此周期下，SG90舵机才能工作，另通过定时器控制高电平的占空比，函数是打包好的，直接就拿来用了。

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

比如输入angle = 0 则S1.duty(7.5)，对应上图 0度在20ms的工作脉冲中为7.5%的占空比。

总：

import sensor,image,lcd,time
import math
from machine import Timer,PWM
tim = Timer(Timer.TIMER0, Timer.CHANNEL0, mode=Timer.MODE_PWM)
S1 = PWM(tim, freq=50, duty=0, pin=17)//自由映射在17引脚
S2 = PWM(tim, freq=50, duty=0, pin=15)//自由映射在15引脚

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

lcd.init()
sensor.reset()
sensor.set_pixformat(sensor.RGB565)
sensor.set_framesize(sensor.QVGA)
sensor.skip_frames(time = 2000)
sensor.set_auto_exposure(1)
sensor.set_auto_gain(1)
sensor.set_vflip(1)
sensor.set_hmirror(1)
sensor.run(1)

red_threshold   = ((43, 66, 45, 107, 40, 1))

bfive=[];bsix=[]
while True:
    
    img=sensor.snapshot()
    blobs = img.find_blobs([red_threshold],x_strides=100,y_strides=100)
    if blobs:
        for b in blobs:
            tmp=img.draw_rectangle(b[0:4],color=(225,225,225))##在图像上绘制一个矩形。
            tmp=img.draw_string(b[0],(b[1]-10),"BOX",color=(0,0,255))
            tmp=img.draw_cross(b[5], b[6])##画十字交叉
            bfive.append(b[5]);bsix.append(b[6])
            if len(bfive)==50 and len(bsix)==50:
     

                hengzuobiao=max(bfive,key=bfive.count)
                zongzuobiao=max(bsix,key=bsix.count) 
                print("中心点横坐标为",hengzuobiao)
                print("中心点纵坐标为",zongzuobiao)
                x = hengzuobiao
                y = 240 - zongzuobiao ##相对于舵机云台
                b = 300               ##b为平台到图像之间的距离，假设为300(尚未搭建实验台)
                a = math.sqrt(b*b+(160-x)*(160-x))
                if x <= 160:
                    jiaoA = math.degrees(math.atan((160-x)/b))##角a为水平方向舵机偏转角度，b为舵机云台到图像的距离，为固定已知值
                    jiaoB = math.degrees(math.atan(y/a))      ##角c为垂直方向偏转角度
                else:
                    jiaoA = -math.degrees((math.atan((x-160)/b)))
                    jiaoB = math.degrees(math.atan(y/a))
                bfive=[];bsix=[]
                print("水平方向舵机应偏转角度为：",jiaoA,"度")
                print("垂直方向舵机应偏转角度为：",jiaoB,"度")
                
                Servo(S1,jiaoA)
                time.sleep(1)
                Servo(S2,jiaoB)
                
                time.sleep(1)
                Servo(S1,-jiaoA)
                Servo(S2,-jiaoB)##复位


    lcd.display(img)

math.atan()求出反三角正切值，math.degrees()将弧度制转化为角度值。

注意：

本实验因没有搭建实验台，有些数据是模拟的，若假设摄像截取的图片大小在真实平面中为320dm * 240dm,可参考上面代码，这样对应的坐标值就可以当作长度值用于计算。

垂直方向的舵机起并不是在X-Y平面上的，包括水平方向也是的，有一定高度。

舵机云台放置在正对拍摄面(X-Y)，居中位置.

K210返回的坐标值是左上角为原点，水平向右为X轴正方向，垂直向下为Y轴正方向。

实验现象：