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轴正方向。

实验现象: