【嵌入式linux开发】基于旭日x3派的视觉循迹小车

效果

硬件

1、旭日x3派（烧录好系统镜像）
2、USB摄像头
3、TB6612
4、小车底盘（直流电机或直流减速电机）
5、电源

视觉循迹原理

x3派读取摄像头图像，转换成灰度图像，从灰度图像中选择第 120 行（图像的一个水平线），遍历第120行的全部320列，根据像素值小于或大于阈值，将相应的值（0 或 1）添加到 date 列表中。最后根据小于阈值的像素个数和它们的总和来判断黑色赛道的位置，以此调节左右电机的转速实现循迹。

代码

直接使用板子上的gpio控制了，比较随意。一般而言这种嵌入式linux的板子是跑算法和大数据的，底盘驱动一般用stm32或者arduino。

import Hobot.GPIO as GPIO
import time
import cv2

class EYE():
    def __init__(self):
        self.video = cv2.VideoCapture(8)  #打开索引为8的摄像头
        ret = self.video.isOpened()  #判断摄像头是否打开成功
        if ret:
            print("The video is opened.")
        else:
            print("No video.")

        codec = cv2.VideoWriter_fourcc( 'M', 'J', 'P', 'G' )   #设置参数
        self.video.set(cv2.CAP_PROP_FOURCC, codec)
        self.video.set(cv2.CAP_PROP_FPS, 30)
        self.video.set(cv2.CAP_PROP_FRAME_WIDTH, 672)
        self.video.set(cv2.CAP_PROP_FRAME_HEIGHT, 672)

        # 创建全屏窗口
        #cv2.namedWindow("Camera Feed", cv2.WND_PROP_FULLSCREEN)
        #cv2.setWindowProperty("Camera Feed", cv2.WND_PROP_FULLSCREEN, cv2.WINDOW_FULLSCREEN)

    def outmiss(self):
        _, img = self.video.read()  #从摄像头读取一帧图像
        img = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)  #将图像转为灰度
        img = img[120]   #选择图像的第120行，一共240行。

        date = []
        for i in range(320):    #遍历每一列，一共320列
            if img[i] <= 64:   #如果当前列的像素值小于等于 64，将 1 添加到 date 列表，表示该像素是感兴趣的。
                date.append(1)
            elif img[i] > 64:  #如果当前列的像素值大于 64，将 0 添加到 date 列表，表示该像素不感兴趣。
                date.append(0)

        n = 0   #用于计算感兴趣的像素数量。  
        sum = 0   #用于计算感兴趣像素的列索引总和。
        for i in range(320):
            if date[i] == 1:
                sum += i   #如果该列的像素是感兴趣的（即 date[i] 为 1），则更新 sum 和 n。
                n += 1
        if n >= 18:
            return sum / n - 159.5
        else:
            return None

    def off(self):

        self.video.release()

class CTRL():
    def __init__(self, in1, in2, in3, in4, pa, pb):
        GPIO.setmode(GPIO.BOARD)
        GPIO.setwarnings(False)

        GPIO.setup(in1, GPIO.OUT)
        GPIO.setup(in2, GPIO.OUT)
        GPIO.setup(in3, GPIO.OUT)
        GPIO.setup(in4, GPIO.OUT)

        self.in1 = in1
        self.in2 = in2
        self.in3 = in3
        self.in4 = in4

        self.PWMA = GPIO.PWM(pa, 48000)
        self.PWMB = GPIO.PWM(pb, 48000)

    def drive(self, FL, FR):
        if FL >= 0:
            GPIO.output(self.in3, GPIO.HIGH)
            GPIO.output(self.in4, GPIO.LOW)
        elif FL < 0:
            GPIO.output(self.in4, GPIO.HIGH)
            GPIO.output(self.in3, GPIO.LOW)

        if FR >= 0:
            GPIO.output(self.in1, GPIO.HIGH)
            GPIO.output(self.in2, GPIO.LOW)
        elif FR < 0:
            GPIO.output(self.in2, GPIO.HIGH)
            GPIO.output(self.in1, GPIO.LOW)

        self.PWMA.ChangeDutyCycle(abs(FR))
        self.PWMB.ChangeDutyCycle(abs(FL))
        self.PWMA.start(abs(FR))
        self.PWMB.start(abs(FL))

    def stop(self):
        GPIO.output(self.in1, GPIO.LOW)
        GPIO.output(self.in2, GPIO.LOW)
        GPIO.output(self.in3, GPIO.LOW)
        GPIO.output(self.in4, GPIO.LOW)

        self.PWMA.ChangeDutyCycle(0)
        self.PWMB.ChangeDutyCycle(0)
        self.PWMA.start(0)
        self.PWMB.start(0)

    def clean(self):
        self.PWMB.stop()
        self.PWMA.stop()
        GPIO.cleanup()
        
class PID():
    def __init__(self,KP,KI,KD):
        self.KP = KP
        self.KI = KI
        self.KD = KD
        self.p1 , self.p2 = 0 , 0#保留一个帧的误差
        self.i = 0#积累误差初值
        
    def naosu(self,miss):
        if miss != None:
            self.p1 , self.p2 = self.p2 , miss #替换缓存的误差
            self.i += miss
            if self.i > 1000:
                self.i -= 800
            if self.i < -1000:
                self.i += 800#积累误差的限制
            naosu = self.KP * miss + self.KI * self.i + self.KD * (self.p2 - self.p1)
            #按照公式输出
            return naosu
            
        elif miss == None:
        #摄像头读空时，根据上一帧的缓存误差正负，来判断现在应该原地左转还是右转
            if self.p2 >= 0:
                self.p1 , self.p2 = self.p2 , 1
                return "r"
                
            elif self.p2 < 0:
                self.p1 , self.p2 = self.p2 , -1
                return "l"

if __name__ == '__main__':
    try:
        Ctrl = CTRL(11, 13, 16, 15, 32, 33)  # 设置管脚
        Eye = EYE()  # 调用视觉模块
        Pid = PID(0.095,0.001,0.52)#调用PID，传入参数
        Ctrl.drive(25, 25)  # 小车的始发运动
        time.sleep(0.5)
        while True:
            ms = Eye.outmiss()  # 获取误差
            ns = Pid.naosu(ms)#获取修正值
            if ns == "r":#原地转弯的情况
                Ctrl.drive(20,-20)
            elif ns == "l":
                Ctrl.drive(-20,20)
            else:#限制修正值，保证不超过PWM上下限
                if ns > 18:
                    ns = 18
                if ns < -18:
                    ns = -18
                    
                Ctrl.drive(25+ns, 25-ns)  # 小车的始发运动
         
            # 添加代码来显示摄像头捕获的图像
            _, frame = Eye.video.read()
            cv2.imshow("Camera Feed", frame)
            time.sleep(0.2)
            if cv2.waitKey(1) & 0xFF == ord('q'):
                break
                
                
    finally:
        Ctrl.stop()
        Ctrl.clean()
        Eye.off()
        cv2.destroyAllWindows()