k210的简单PID巡线

目录

1.前言

2.简介

3.代码讲解

1.调用自带的库文件

2.将I/O18和 I/O19分别注册为UART1_TX和UART1_RX功能并设置串口

3.向STM32F103单片机发送数据包

4.基本初始化

5.主程序代码

1.寻找色块部分

2.巡线部分 

3.侦测关键点部分

 4.完整代码

---

1.前言

        最近在复刻电赛送药小车和跟随小车，发现巡线部分使用灰度循迹已经不能很好的完成了，于是开始琢磨摄像头循迹，一开始发现ccd摄像头非常不错，它由一个 1x128 的光电二极管阵列、相关的电荷放大电路以及一个内部像素数据保功能组成。

ccd可以直接返回偏差值，价格也合适，准备入手时却发现它只能寻白底黑线，那只能含泪pass。

于是目光就放回了openmv和k210，在一番比较下选择了性价比较高的k210，于是就有了一下的文章。

                                    （使用的是Maixpy k210 Dock板，自行安装了一个补光灯） 

2.简介

        本文章主要讲述使用k210完成简单的PID寻线，通过调用 image 模块中现有查找色块方法，得到偏移量和关键点（如十字路口，起停线等）的数据，返回数据给STM32f103单片机进行巡线，本片只涉及k210的代码，后续会更新和stm32f103通讯的代码。

3.代码讲解

1.调用自带的库文件

import sensor, image,lcd
from machine import UART #串口库函数
from fpioa_manager import fm # GPIO重定向函数

2.将I/O18和 I/O19分别注册为UART1_TX和UART1_RX功能并设置串口

fm.register(18, fm.fpioa.UART1_TX, force=True)
fm.register(19, fm.fpioa.UART1_RX, force=True)
uart_A = UART(UART.UART1, 115200, 8, 0, 1, timeout=1000, read_buf_len=4096)

3.向STM32F103单片机发送数据包

def sending_data(x,y,z):
    global uart;
    FH = bytearray([0x2C,0x12,x,y,z,0x5B])
    uart_A.write(FH);

此数据包的格式为2个帧头，3个数据和一个帧尾，STM32F103接收K210数据包的代码以后会更新。

4.基本初始化

green_threshold = ((0, 190))           #黑色
roi1            = [0,100,320,16]       #巡线敏感区
roi2            = [0,180,320,8]        #关键点敏感区
expectedValue   = 160                  #巡线位置期望
err             = 0                    #本次误差
old_err         = 0                    #上次误差
Kp              = 0.046                #PID比例系数
Kd              = 0                    #PID微分系数
Speed           = 0                    #期望速度
Speed_left      = 0                    #左轮速度
Speed_right     = 0                    #右轮速度
Flag            = 0                    #用于关键点标志

lcd.init()
sensor.reset()
sensor.set_pixformat(sensor.GRAYSCALE)
sensor.set_framesize(sensor.QVGA)# 320x240
sensor.skip_frames(time = 3000 )#跳过3000张图片
sensor.set_auto_gain(False) # must be turned off for color tracking
sensor.set_auto_whitebal(False) # must be turned off for color tracking
sensor.run(1)

        其中的 sensor.set_pixformat(sensor.GRAYSCALE)  是将像素模式设置为灰度灰度模式对于只有两种颜色的地图可以起到提高帧数，过滤其它颜色的影响，提高了识别的正确率。

       其中的 Kp 和 Kd  是巡线PID的参数没有使用到积分参数，Kd设置为0的原因是因为仅仅调节Kp时巡线就已经很稳定了，如果你的小车仅靠Kp无法达到稳定或者出现了高频振荡，请自行调节Kd。

       其中的 Speed，Speed_left和Speed_right 分别为当偏差量为0时的期望速度，计算后发送给STM32F103的左轮速度，计算后发送给STM32F103的右轮速度。

        其中的 Flag 是用于当检测到如十字路口，起停线等的标志。

5.主程序代码

1.寻找色块部分

while True:
    img=sensor.snapshot()
    statistics1 = img.find_blobs([green_threshold],roi=roi1,area_threshold=200,merge=True)
    statistics2 =img.find_blobs([green_threshold],roi=roi2,area_threshold=120,merge=True,margin=120)
    if statistics1:
        for b in statistics1:
            tmp=img.draw_rectangle(b[0:4])
            tmp=img.draw_cross(b[5], b[6])
            c=img.get_pixel(b[5], b[6])

            #PID计算

            actualValue=b[5]
            err=actualValue-expectedValue
            Speed_left = Speed - (Kp*err+Kd*(err-old_err))
            Speed_right = Speed + (Kp*err+Kd*(err-old_err))
            old_err= err
            print("Speed_left,Speed_right")
            print(int(Speed_left),int(Speed_right))
    if statistics2:
        for b in statistics2:
            tmp=img.draw_rectangle(b[0:4])
            tmp=img.draw_cross(b[5], b[6])
            c=img.get_pixel(b[5], b[6])
            if b[2] >50:
                Flag = 1
    sending_data(int(Speed_left),int(Speed_right),Flag)

        通过find_blobs函数可以找到色块，其中的[green_threshold]寻找目标是颜色的阈值，阈值可以通过Maixpy IDE工具栏中的工具 -> 机器视觉 -> 阈值编辑器获得。

       其中的 roi1 和 roi2 分别是寻线部分和关键的感兴趣区，就是在要处理的整张图像中提取出的要处理的区域，寻找色块只在下图红框里分别进行。

        area_threshold 是面积阈值，如果色块被框起来的面积小于这个值，会被过滤掉，减少干扰提高巡线准确度。

        merge=True 将所有重叠的blob合并为一个。

       margin为边界，如果设置为1，那么两个blobs如果间距1一个像素点，也会被合并。此参数只在检测关键点寻找色块函数中运用，可以更明确的侦测出关键点，如Y型路口。

       statistics = img.find_blobs[ ] 对象返回的是多个blob的列表,列表类似与C语言的数组，一个blobs列表里包含很多blob对象，blobs对象就是色块，每个blobs对象包含一个色块的信息。b就是很多色块。可以用for循环把所有的色块找一遍。

for b in statistics:

        返回色块的外框的x坐标（int），也可以通过b[0]来获得
        返回色块的外框的y坐标（int），也可以通过b[1]来获得
        返回色块的外框的宽度w（int），也可以通过b[2]来获得
        返回色块的外框的高度h（int），也可以通过b[3]来获得
        返回色块的像素数量（int），也可以通过b[4]来获得
        返回色块的外框的中心x坐标（int），也可以通过b[5]来获得
        返回色块的外框的中心y坐标（int），也可以通过b[6]来获得

        tmp=img.draw_rectangle(b[0:4])和tmp=img.draw_cross(b[5], b[6]) 分别为画出找到色块的外框和中心十字

2.巡线部分 

        由上我们可得b[5]为返回外框中心x的值，而且拍摄的画面为320x240，X方向的中间为160，所以线与摄像头中心的偏移量err，就为实际值actualValue 也就是b[5]减去期望expectedValue(160)。对应代码为：

actualValue=b[5]
err=actualValue-expectedValue

        得到了偏差值就可以进行PID计算了。

Speed_left = Speed - (Kp*err+Kd*(err-old_err))
Speed_right = Speed + (Kp*err+Kd*(err-old_err))

old_err= err

        当偏差小于零时说明小车向右偏，需要增加左轮的速度，减小右轮的速度，反之亦然。

3.侦测关键点部分

        当要经过十字路口，y型路口，起停线时，可以关注blob的列表中的b[2],其反回的值为返回色块的外框的宽度w。因为正常巡线时寻找到的色块外框宽度w的值是小于关键点部分的值。

                                         正常巡线

                                         r型路口

                                          起停线

                                          十字路口

        只需要判断是否大于正常色块外框宽度就可以侦测关键点。

if b[2] >50:
   Flag = 1

 4.完整代码

# Untitled - By: User - 周三 4月 19 2023

import sensor, image,lcd
from machine import UART #串口库函数
from fpioa_manager import fm # GPIO重定向函数

fm.register(18, fm.fpioa.UART1_TX, force=True)
fm.register(19, fm.fpioa.UART1_RX, force=True)
uart_A = UART(UART.UART1, 115200, 8, 0, 1, timeout=1000, read_buf_len=4096)

green_threshold = ((0, 190))           #黑色
roi1            = [0,100,320,16]       #巡线敏感区
roi2            = [0,180,320,8]        #关键点敏感区
expectedValue   = 160                  #巡线位置期望
err             = 0                    #本次误差
old_err         = 0                    #上次误差
Kp              = 0.046                #PID比例系数
Kd              = 0                    #PID微分系数
Speed           = 0                    #期望速度
Speed_left      = 0                    #左轮速度
Speed_right     = 0                    #右轮速度
Flag            = 0                    #用于关键点标志

def sending_data(x,y,z):
    global uart;
    FH = bytearray([0x2C,0x12,x,y,z,0x5B])
    uart_A.write(FH);

lcd.init()
sensor.reset()
sensor.set_pixformat(sensor.GRAYSCALE)
sensor.set_framesize(sensor.QVGA) # 320x240
sensor.skip_frames(time = 3000 )#跳过3000张图片
sensor.set_auto_gain(False) # must be turned off for color tracking
sensor.set_auto_whitebal(False) # must be turned off for color tracking
sensor.run(1)

while True:
    img=sensor.snapshot()
    statistics1 = img.find_blobs([green_threshold],roi=roi1,area_threshold=200,merge=True)
    statistics2 = img.find_blobs([green_threshold],roi=roi2,area_threshold=120,merge=True,margin=120)
    if statistics1:
        for b in statistics1:
            tmp=img.draw_rectangle(b[0:4])
            tmp=img.draw_cross(b[5], b[6])

            #PID计算

            actualValue=b[5]
            err=actualValue-expectedValue
            Speed_left = Speed - (Kp*err+Kd*(err-old_err))
            Speed_right = Speed + (Kp*err+Kd*(err-old_err))
            old_err= err
            print("Speed_left,Speed_right")
            print(int(Speed_left),int(Speed_right))
    if statistics2:
        for b in statistics2:
            tmp=img.draw_rectangle(b[0:4])
            tmp=img.draw_cross(b[5], b[6])
            if b[2] >50:
                Flag = 1
    sending_data(int(Speed_left),int(Speed_right),Flag)

如果报错说缺少相对应的库，请自行去MaixPy官网下载openmv api的固件，这篇文章的第三部分有教你如何安装固件。