今日继续学习树莓派4B 4G:(Raspberry Pi,简称RPi或RasPi)
本人所用树莓派4B 装载的系统与版本如下:
版本可用命令 (lsb_release -a) 查询:
Opencv 版本是4.5.1:
Python 版本3.7.3:
今日尝试搭建一台小车,外观自行设计、树莓派主控、自行挑选模块 ,完成简单的巡线动作,就开环控制了,这篇文章的使用就不闭环了......
巡线的思想就是在视频屏幕最下方1/4的ROI区域灰度识别黑色色块,计算与中点的偏差,与中点偏差的像素过大就原地左右转调整,其余识别到黑线在偏差范围内不大时就正常走直线
自评:原地左右转以及没使用PID闭环使得这个视觉巡线小车看起来不太智能,还可改进>>>
文章提供测试代码讲解,整体代码贴出、测试效果图、完整测试工程下载
目录
小车外观与使用模块:
搭个小车还是很快很简单的,不到半天就能搭好小车了......
主要部件:
主控: 树莓派4B 4G
电机: MG310 13线霍尔编码器 减速比 1:20 ( 4个 ) 购于 轮趣科技 淘宝店
电机控制板:幻尔 IIC四路电机扩展板 购于幻尔科技淘宝店
车轮: 购于 金色传说 淘宝店 (2.5mm轴)这个轮子摩擦力可以,价格还便宜!
车架: 使用 购于 星呗机器人淘宝店 的车架,贵~~
屏幕: 7英寸 HDML 显示屏,二手咸鱼购入
摄像头: 普通USB摄像头 (一般使用640*480像素模式),随便来个上网课的摄像头都行!
其余部件: 降压模块、6000mah电池、DC电池开关......
部分模块展示:
1、幻尔IIC四路电机控制板:
小车运动控制代码:
树莓派通过IIC与电机控制板通信,使电机控制板发PWM信号驱动电机运转:
这里我在Motor_control.py文件中加入了一些获取键盘值、字典的操作来简单控制小车
但在程序上获取键盘值每次都需要按一次回车,比较不方便......
Motor_control.py
这部分是电机控制代码:包含一些正反转,左右原地旋转的基本逻辑,这是为开环控制写的,因此四个轮子速度设定都一样,
import smbus
import time
import struct
# 设置I2C总线号,通常为1
I2C_BUS = 1
# 设置四路电机驱动模块的I2C地址
MOTOR_ADDR = 0x34
# 寄存器地址
ADC_BAT_ADDR = 0x00
MOTOR_TYPE_ADDR = 0x14 # 编码电机类型设置
MOTOR_ENCODER_POLARITY_ADDR = 0x15 # 设置编码方向极性,
# 如果发现电机转速根本不受控制,要么最快速度转动,要么停止。可以将此地址的值重新设置一下
# 范围0或1,默认0
MOTOR_FIXED_PWM_ADDR = 0x1F # 固定PWM控制,属于开环控制,范围(-100~100)
MOTOR_FIXED_SPEED_ADDR = 0x33 # 固定转速控制,属于闭环控制,
# 单位:脉冲数每10毫秒,范围(根据具体的编码电机来,受编码线数,电压大小,负载大小等影响,一般在±50左右)
MOTOR_ENCODER_TOTAL_ADDR = 0x3C # 4个编码电机各自的总脉冲值
# #如果已知电机每转一圈的脉冲数为U,又已知轮子的直径D,那么就可以通过脉冲计数的方式得知每个轮子行进的距离
# #比如读到电机1的脉冲总数为P,那么行进的距离为(P/U) * (3.14159*D)
# #对于不同的电机可以自行测试每圈的脉冲数U,可以手动旋转10圈读出脉冲数,然后取平均值得出
# 电机类型具体值
MOTOR_TYPE_WITHOUT_ENCODER = 0
MOTOR_TYPE_TT = 1
MOTOR_TYPE_N20 = 2 #MG310与N20相同
MOTOR_TYPE_JGB37_520_12V_110RPM = 3 # 磁环每转是44个脉冲 减速比:90 默认
# 电机类型及编码方向极性
MotorType = MOTOR_TYPE_N20
MotorEncoderPolarity = 1
bus = smbus.SMBus(I2C_BUS)
speed1 = [50, 50, 50, 50]
speed2 = [-50, -50, -50, -50]
speed3 = [0, 0, 0, 0]
speed4 = [-50, 50, 50, -50]
speed5 = [0, 0, 0, 0]
new_speed = 50
pwm1 = [50, 50, 50, 50]
pwm2 = [-100, -100, -100, -100]
pwm3 = [0, 0, 0, 0]
def Motor_Init(): # 电机初始化
bus.write_byte_data(MOTOR_ADDR, MOTOR_TYPE_ADDR, MotorType) # 设置电机类型
time.sleep(0.5)
bus.write_byte_data(MOTOR_ADDR, MOTOR_ENCODER_POLARITY_ADDR, MotorEncoderPolarity) # 设置编码极性
# 控制电机向前的代码
def move_forward():
print("向前移动")
global new_speed, speed5
speed5[0] = new_speed*(-1)
speed5[1] = new_speed
speed5[2] = new_speed
speed5[3] = new_speed*(-1)
bus.write_i2c_block_data(MOTOR_ADDR, MOTOR_FIXED_SPEED_ADDR, speed5)
# 控制电机向后的代码
def move_backward():
print("向后移动")
global new_speed, speed5
speed5[0] = new_speed
speed5[1] = new_speed*(-1)
speed5[2] = new_speed*(-1)
speed5[3] = new_speed
bus.write_i2c_block_data(MOTOR_ADDR, MOTOR_FIXED_SPEED_ADDR, speed5)
# 控制电机向左的代码
def move_left():
print("向左移动")
global new_speed, speed5
speed5[0] = new_speed
speed5[1] = new_speed*(-1)
speed5[2] = new_speed
speed5[3] = new_speed*(-1)
bus.write_i2c_block_data(MOTOR_ADDR, MOTOR_FIXED_SPEED_ADDR, speed5)
# 控制电机向右的代码
def move_right():
print("向右移动")
global new_speed, speed5
speed5[0] = new_speed * (-1)
speed5[1] = new_speed
speed5[2] = new_speed * (-1)
speed5[3] = new_speed
bus.write_i2c_block_data(MOTOR_ADDR, MOTOR_FIXED_SPEED_ADDR, speed5)
# 停止电机的代码
def stop_motor():
print("停止移动")
speed = [0, 0, 0, 0]
bus.write_i2c_block_data(MOTOR_ADDR, MOTOR_FIXED_SPEED_ADDR, speed)
def reduce_speed():
print("减速")
global new_speed
new_speed=new_speed-10
def add_speed():
print("加速")
global new_speed
new_speed=new_speed+10
direction_mapping = {
"W": move_forward,
"S": move_backward,
"A": move_left,
"D": move_right,
"X": stop_motor,
"Q": reduce_speed,
"E": add_speed
}
def main():
while True:
battery = bus.read_i2c_block_data(MOTOR_ADDR, ADC_BAT_ADDR)
print("V = {0}mV".format(battery[0] + (battery[1] << 8)))
Encode = struct.unpack('iiii', bytes(bus.read_i2c_block_data(MOTOR_ADDR, MOTOR_ENCODER_TOTAL_ADDR, 16)))
print("Encode1 = {0} Encode2 = {1} Encode3 = {2} Encode4 = {3}".format(Encode[0], Encode[1], Encode[2],
Encode[3]))
# PWM控制(注意:PWM控制是一个持续控制的过程,若有延时则会打断电机的运行)
# bus.write_i2c_block_data(MOTOR_ADDR, MOTOR_FIXED_PWM_ADDR,pwm1)
Motor_CLR=input("请输入运动方向(W前/S后/A左/D右/X停止): ").upper() # 转换为大写以忽略大小写差异
if Motor_CLR in direction_mapping:
direction_mapping[Motor_CLR]()
else:
print("无效输入,请输入正确的方向(W前/S后/A左/D右/X停止)")
if __name__ == "__main__":
Motor_Init()
main()
程序补足电机对向转动方向:
其中我们发现这里电机向前转的代码有些轮子的速度值被乘(-1)了,这是因为左右相邻电机对向安装的结果,使得它们旋转方向相反的,在程序上乘个(-1),使其方向调整过来
# 控制电机向前的代码 def move_forward(): print("向前移动") global new_speed, speed5 speed5[0] = new_speed*(-1) speed5[1] = new_speed speed5[2] = new_speed speed5[3] = new_speed*(-1) bus.write_i2c_block_data(MOTOR_ADDR, MOTOR_FIXED_SPEED_ADDR, speed5)
电机转动方向与传入速度数值正负关系 M1(左前轮) M2(左后轮) M3(右前轮) M4(右后轮) 前进 负 正 正 负 后退 正 负 负 正 左转 正 负 正 负 右转 负 正 负 正
Opencv识别黑线:
Gray_find_black.py
这里展示一下单独可以运行的Opencv识别黑线代码:
灰度图中识别黑色区域,最长的区域视作黑线,并标注出它的中点坐标:
Gray_find_black.py文件代码如下:
import time import numpy as np import cv2 ''' 标记颜色块中点程序,这里是黑线 ''' # 打开摄像头,0通常是默认摄像头的索引 cap = cv2.VideoCapture(0) # 设置目标分辨率 target_resolution = (640, 480) def find_line_midpoint(frame): # 转换到灰度图像 gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY) # 应用阈值处理来创建掩码,这里假设黑线的灰度值较低 _, mask = cv2.threshold(gray, 50, 255, cv2.THRESH_BINARY_INV) # 阈值可以根据实际情况调整 # 形态学操作,去除噪点 kernel = np.ones((3, 3), np.uint8) mask = cv2.morphologyEx(mask, cv2.MORPH_CLOSE, kernel, iterations=2) # 使用闭运算来填充小的孔洞 # 查找轮廓 contours, _ = cv2.findContours(mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE) # 假设最长的轮廓是黑线 max_area = 0 best_cnt = None for cnt in contours: area = cv2.contourArea(cnt) if area > max_area: max_area = area best_cnt = cnt # 如果找到轮廓,则计算中点 if best_cnt is not None: # 计算轮廓的边界矩形 x, y, w, h = cv2.boundingRect(best_cnt) # 绘制矩形框(注意:这里的y是矩形框在原图上的顶部坐标) cv2.rectangle(frame, (x, y), (x + w, y + h), (0, 0, 255), 2) # 使用蓝色绘制矩形框 M = cv2.moments(best_cnt) if M["m00"] != 0: cX = int(M["m10"] / M["m00"]) cY = int(M["m01"] / M["m00"]) cv2.circle(frame, (cX, cY), 5, (0, 255, 0), -1) # 标记中点 print("({},{})".format(cX, cY)) # 显示结果 cv2.imshow('Frame', frame) cv2.imshow('Mask', mask) while True: ret, frame = cap.read() if not ret: break find_line_midpoint(frame) if cv2.waitKey(1) & 0xFF == ord('q'): break # 释放资源和关闭所有窗口 cap.release() cv2.destroyAllWindows()
测试效果截图:
PC运行测试图:
小车实际运行测试图:
框出整个屏幕识别到的最大黑线区域的中心位置
Gray_find_black_roi.py
这个程序文件就是在之前的文件Gray_find_black.py 上对函数进行了优化
设置Roi 使寻迹识别黑色区域在 整个像素区域的下1/4 处
并添加了与中点进行比较的连线以及输出连线俩端点的差值
顺便添加了一些函数返回值,并在函数开头初始化:
detect #detect=0 没检测到黑线 , detect=1检测到黑线 dx #黑色块中心与Roi中心的x坐标差值 dy #黑色块中心与Roi中心的y坐标差值这个程序直接复制也能直接运行!
import cv2
import numpy as np
def find_line_midpoint_in_roi(frame):
#随便给这三个用到的需要返回的值赋初始值,防止返回报错!
detect=3 #detect=0没检测到黑线,detect=1检测到黑线
dx=3
dy=3
# 获取帧的高度和宽度
height, width = frame.shape[:2]
# 计算底部四分之一区域的高度
roi_height = height // 4
# 定义ROI区域(底部四分之一)
roi_y = height - roi_height
roi = frame[roi_y:height, 0:width]
# 转换ROI到灰度图像
gray_roi = cv2.cvtColor(roi, cv2.COLOR_BGR2GRAY)
# 应用阈值处理来创建掩码
_, mask = cv2.threshold(gray_roi, 50, 255, cv2.THRESH_BINARY_INV)
# 形态学操作,去除噪点(可选)
kernel = np.ones((3, 3), np.uint8)
mask = cv2.morphologyEx(mask, cv2.MORPH_CLOSE, kernel, iterations=2)
# 查找轮廓
contours, _ = cv2.findContours(mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
# 假设最长的轮廓是黑线
max_area = 0
best_cnt = None
for cnt in contours:
area = cv2.contourArea(cnt)
if area > max_area:
max_area = area
best_cnt = cnt
# 如果找到轮廓,则计算中点,并在原图上标记
# 如果找到轮廓,则计算中点,并使用矩形框在原图上框出识别到的黑色块
if best_cnt is not None:
detect=1
# 注意:轮廓点坐标是相对于mask的,但我们需要将其转换为原图的坐标
# 假设best_cnt是一个轮廓点集
# 计算轮廓的边界矩形(这一步是在ROI坐标系中进行的)
x, y, w, h = cv2.boundingRect(best_cnt)
# 将边界矩形的坐标从ROI坐标系转换为原图坐标系
# 注意:这里x坐标(即边界矩形的左边界)不需要修改,因为它已经是相对于原图的左边界
# 而y坐标(即边界矩形的下边界,因为ROI是从底部开始的)需要转换为原图的坐标
y_original = height - (y + h) # 将ROI中的下边界转换为原图的上边界
# 现在我们可以在原图上绘制矩形了
cv2.rectangle(frame, (x, y_original), (x + w, y_original + h), (0, 0, 255), 2) # 使用红色绘制矩形框
# 接下来计算并绘制轮廓的中心点
M = cv2.moments(best_cnt)
cX = int(M["m10"] / M["m00"]) # 轮廓中心的X坐标(相对于ROI左上角),无需修改
cY_roi = int(M["m01"] / M["m00"]) # 轮廓中心的Y坐标(相对于ROI底部)
cY = height - cY_roi # 将Y坐标从ROI底部转换为原图顶部
cv2.circle(frame, (cX, cY), 5, (0, 255, 0), -1) # 在原图上绘制圆心
# 打印中心点坐标(可选)
print("({},{})".format(cX, cY))
# 计算 ROI 的中心点
roi_mid_x = width // 2 # ROI 的 X 中心点(因为 ROI 总是从原图底部开始的全宽)
roi_mid_y = roi_y + roi_height // 2 # ROI 的 Y 中心点(注意要加上 ROI 的起始 Y 坐标)
# 绘制 ROI 中心点
cv2.circle(frame, (roi_mid_x, roi_mid_y), 5, (255, 0, 0), -1) # 使用蓝色绘制 ROI 中心点
# 计算并绘制连接线
cv2.line(frame, (roi_mid_x, roi_mid_y), (cX, cY), (0, 0, 255), 2) # 使用红色绘制连接线
# 计算并打印两个点的 X 和 Y 差值
dx = cX - roi_mid_x
dy = cY - roi_mid_y
# 创建包含差值的文本字符串
text = f"dx={dx}, dy={dy}"
#text_x 与 text_y 表示文本打印的位置
text_x = int((roi_mid_x + cX) / 2) # 线的中点 X 坐标(可能需要根据实际情况调整)
text_y = int((roi_mid_y + cY) / 2 - 10) # 线的中点 Y 坐标减去一些空间以容纳文本
cv2.putText(frame, text, (text_x, text_y), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 0, 255), 1, cv2.LINE_AA)
elif best_cnt is None: #没找到黑线
detect=0
# 返回处理后的帧(可选,也可以直接在原图上操作)
return frame,mask,dx,dy,detect
# 主循环,用于从摄像头读取帧并处理
cap = cv2.VideoCapture(0)
while True:
ret, frame = cap.read()
if not ret:
break
#使用下划_忽略接收最后俩个返回的dx,dy的差值
frame,mask,_,_,_= find_line_midpoint_in_roi(frame)
# 显示结果
cv2.imshow('Frame', frame)
cv2.imshow('Mask',mask)
# 按 'q' 键退出
if cv2.waitKey(1) & 0xFF == ord('q'):
break
cap.release()
cv2.destroyAllWindows()
测试效果截图:
PC端测试效果:
掩膜中白色部分就是识别到的黑色区域,而它只会将最大的黑色区域设置为识别到的黑色块
小车实际测试效果:
蓝色的点是屏幕ROI区域的中点,绿色的点是识别到的色块中点
一会和电机运动结合就是>50 就右转,<-50就左转,
这里巡线就不想使用左右侧 差速的思想了,直接原地停下左右调整就好了,也不会浪费时间的,差速总感觉容易冲出去
巡线代码:
本文写到这里我没有pid 闭环电机,就先这样开环能跑跑吧
这部分代码就是最终能控制电机并巡线的代码了,需要与Motor_control.py放在同一个文件夹
Line_inspection.py
这部分写的比较粗糙了,为了能巡线而巡线写的
速度写的比较低,然后添加了15ms 的
time.sleep()
结合循环的任务(貌似这个子函数是死循环,会卡死不进入主程序的while循环......)
实际运行也确实发现没有进入主程序的while循环显示摄像头效果,也许这里使用线程会好些
,在下部分文章实验在加入线程思想来写把......
def task(): pass def periodic_task(): while True: task() time.sleep(0.015) # 休眠15ms
import cv2
import Motor_control #导入自定义电机控制模块
import time
import numpy as np
def find_line_midpoint_in_roi(frame):
#随便给这三个用到的需要返回的值赋初始值,防止返回报错!
detect=3 #detect=0没检测到黑线,detect=1检测到黑线
dx=3
dy=3
# 获取帧的高度和宽度
height, width = frame.shape[:2]
# 计算底部四分之一区域的高度
roi_height = height // 4
# 定义ROI区域(底部四分之一)
roi_y = height - roi_height
roi = frame[roi_y:height, 0:width]
# 转换ROI到灰度图像
gray_roi = cv2.cvtColor(roi, cv2.COLOR_BGR2GRAY)
# 应用阈值处理来创建掩码
_, mask = cv2.threshold(gray_roi, 50, 255, cv2.THRESH_BINARY_INV)
# 形态学操作,去除噪点(可选)
kernel = np.ones((3, 3), np.uint8)
mask = cv2.morphologyEx(mask, cv2.MORPH_CLOSE, kernel, iterations=2)
# 查找轮廓
contours, _ = cv2.findContours(mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
# 假设最长的轮廓是黑线
max_area = 0
best_cnt = None
for cnt in contours:
area = cv2.contourArea(cnt)
if area > max_area:
max_area = area
best_cnt = cnt
# 如果找到轮廓,则计算中点,并在原图上标记
# 如果找到轮廓,则计算中点,并使用矩形框在原图上框出识别到的黑色块
if best_cnt is not None:
detect=1
# 注意:轮廓点坐标是相对于mask的,但我们需要将其转换为原图的坐标
# 假设best_cnt是一个轮廓点集
# 计算轮廓的边界矩形(这一步是在ROI坐标系中进行的)
x, y, w, h = cv2.boundingRect(best_cnt)
# 将边界矩形的坐标从ROI坐标系转换为原图坐标系
# 注意:这里x坐标(即边界矩形的左边界)不需要修改,因为它已经是相对于原图的左边界
# 而y坐标(即边界矩形的下边界,因为ROI是从底部开始的)需要转换为原图的坐标
y_original = height - (y + h) # 将ROI中的下边界转换为原图的上边界
# 现在我们可以在原图上绘制矩形了
cv2.rectangle(frame, (x, y_original), (x + w, y_original + h), (0, 0, 255), 2) # 使用红色绘制矩形框
# 接下来计算并绘制轮廓的中心点
M = cv2.moments(best_cnt)
cX = int(M["m10"] / M["m00"]) # 轮廓中心的X坐标(相对于ROI左上角),无需修改
cY_roi = int(M["m01"] / M["m00"]) # 轮廓中心的Y坐标(相对于ROI底部)
cY = height - cY_roi # 将Y坐标从ROI底部转换为原图顶部
cv2.circle(frame, (cX, cY), 5, (0, 255, 0), -1) # 在原图上绘制圆心
# 打印中心点坐标(可选)
print("({},{})".format(cX, cY))
# 计算 ROI 的中心点
roi_mid_x = width // 2 # ROI 的 X 中心点(因为 ROI 总是从原图底部开始的全宽)
roi_mid_y = roi_y + roi_height // 2 # ROI 的 Y 中心点(注意要加上 ROI 的起始 Y 坐标)
# 绘制 ROI 中心点
cv2.circle(frame, (roi_mid_x, roi_mid_y), 5, (255, 0, 0), -1) # 使用蓝色绘制 ROI 中心点
# 计算并绘制连接线
cv2.line(frame, (roi_mid_x, roi_mid_y), (cX, cY), (0, 0, 255), 2) # 使用红色绘制连接线
# 计算并打印两个点的 X 和 Y 差值
dx = cX - roi_mid_x
dy = cY - roi_mid_y
# 创建包含差值的文本字符串
text = f"dx={dx}, dy={dy}"
#text_x 与 text_y 表示文本打印的位置
text_x = int((roi_mid_x + cX) / 2) # 线的中点 X 坐标(可能需要根据实际情况调整)
text_y = int((roi_mid_y + cY) / 2 - 10) # 线的中点 Y 坐标减去一些空间以容纳文本
cv2.putText(frame, text, (text_x, text_y), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 0, 255), 1, cv2.LINE_AA)
elif best_cnt is None: #没找到黑线
detect=0
# 返回处理后的帧(可选,也可以直接在原图上操作)
return frame,mask,dx,dy,detect
# 用于从摄像头读取帧并处理
cap = cv2.VideoCapture(0)
def task():
# 这里放置你希望每15ms执行一次的任务
ret, frame = cap.read()
# 使用下划_忽略接收最后俩个返回的dx,dy的差值
frame, mask, dxx, _, detect = find_line_midpoint_in_roi(frame)
if detect == 0:
Motor_control.stop_motor()
else:
if (dxx > 55):
Motor_control.move_right()
elif (dxx < -55):
Motor_control.move_left()
else:
Motor_control.move_forward()
def periodic_task():
while True:
task()
time.sleep(0.015) # 休眠15ms
def main():
Motor_control.Motor_Init() #电机初始化
Motor_control.new_speed=15 #提醒一下可以这样初始化Motor_control模块中的变量
periodic_task()
while True:
# 这里放置你希望每15ms执行一次的任务
ret, frame = cap.read()
# 使用下划_忽略接收最后俩个返回的dx,dy的差值
frame, mask, _, _,_ = find_line_midpoint_in_roi(frame)
# 显示结果
cv2.imshow('Frame', frame)
cv2.imshow('Mask', mask)
# 按 'q' 键退出
if cv2.waitKey(1) & 0xFF == ord('q'):
break
if __name__ == "__main__":
main()
cap.release()
cv2.destroyAllWindows()
最终测试效果视频:
与预期相同,或者效果有些超预期?
反正能巡线了,因为程序原因,摄像头ROI区域识别的实际情况没有在屏幕上打印出来,下篇文章再优化吧......
小车电机开环运动与opencv摄像头巡线
测试自评改进:
后续会对ROI区域进行一些新增,用于识别弯道等更复杂的情况
或者尝试用画回归线连线的方式预测前方寻迹走向
之后优化目标是移除原地转向的逻辑,改为更丝滑的预测回归线后使用差速的方式巡线,
除非检测到类似于直角弯的情况再使用原地转向的运动代码
电源供电方案也有些许问题:树莓派、7寸屏幕、电机这三大块耗电有些大......
程序中的15ms延迟也有问题,影响了cv的窗口创建显示,影响观察识别效果,后续改成多线程程序试试.....