1、旭日x3派（烧录好系统镜像）
2、USB摄像头
3、TB6612
4、小车底盘（直流电机或直流减速电机）
视觉循迹原理
x3派读取摄像头图像，转换成灰度图像，从灰度图像中选择第 120 行（图像的一个水平线），遍历第120行的全部320列，根据像素值小于或大于阈值，将相应的值（0 或 1）添加到 date 列表中。最后根据小于阈值的像素个数和它们的总和来判断黑色赛道的位置，以此调节左右电机的转速实现循迹。
python代码
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()
