281 lines
9.4 KiB
Python
281 lines
9.4 KiB
Python
import cv2
|
||
import numpy as np
|
||
import motor
|
||
import time
|
||
import signal
|
||
import sys
|
||
|
||
# 信号处理函数，用于捕获Ctrl+C
|
||
def signal_handler(sig, frame):
|
||
print("\n接收到中断信号，正在停止电机...")
|
||
motor.stop()
|
||
print("电机已停止，程序退出")
|
||
sys.exit(0)
|
||
|
||
# 注册信号处理函数
|
||
signal.signal(signal.SIGINT, signal_handler)
|
||
|
||
print("开始运行摄像头程序...")
|
||
print(f"OpenCV版本: {cv2.__version__}")
|
||
|
||
# 尝试打开摄像头
|
||
cap = cv2.VideoCapture(0)
|
||
|
||
if not cap.isOpened():
|
||
print("无法打开摄像头！")
|
||
exit()
|
||
|
||
print("摄像头打开成功")
|
||
|
||
# 设置摄像头参数
|
||
cap.set(3, 320)
|
||
cap.set(4, 240)
|
||
cap.set(cv2.CAP_PROP_BUFFERSIZE, 1)
|
||
|
||
print(f"摄像头分辨率: {cap.get(3)}x{cap.get(4)}")
|
||
|
||
# 初始化稳定阶段
|
||
print("初始化摄像头...")
|
||
success_count = 0
|
||
for i in range(10):
|
||
ret, frame = cap.read()
|
||
if not ret:
|
||
print(f"第{i+1}帧读取失败")
|
||
continue
|
||
success_count += 1
|
||
print(f"第{i+1}帧读取成功")
|
||
time.sleep(0.1)
|
||
print(f"摄像头初始化完成，成功读取{success_count}帧")
|
||
|
||
# 读取初始帧
|
||
ret, prev_frame = cap.read()
|
||
if not ret:
|
||
print("无法读取初始帧！")
|
||
cap.release()
|
||
exit()
|
||
|
||
print("初始帧读取成功")
|
||
prev_gray = cv2.cvtColor(prev_frame, cv2.COLOR_BGR2GRAY)
|
||
prev_gray = cv2.GaussianBlur(prev_gray, (5,5), 0)
|
||
print("初始帧处理完成")
|
||
|
||
# 记录上一帧目标的位置和面积
|
||
prev_cx = None
|
||
prev_cy = None
|
||
prev_area = None
|
||
# 目标跟踪状态
|
||
tracking_target = None
|
||
|
||
while True:
|
||
ret, frame = cap.read()
|
||
if not ret:
|
||
break
|
||
|
||
gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
|
||
gray = cv2.GaussianBlur(gray, (5,5), 0)
|
||
|
||
# 帧差
|
||
diff = cv2.absdiff(prev_gray, gray)
|
||
diff = cv2.convertScaleAbs(diff, alpha=2.5)
|
||
|
||
# 应用形态学操作，去除噪声
|
||
kernel = cv2.getStructuringElement(cv2.MORPH_ELLIPSE, (5, 5))
|
||
diff = cv2.morphologyEx(diff, cv2.MORPH_OPEN, kernel)
|
||
diff = cv2.morphologyEx(diff, cv2.MORPH_CLOSE, kernel)
|
||
|
||
_, thresh = cv2.threshold(diff, 30, 255, cv2.THRESH_BINARY)
|
||
|
||
contours, _ = cv2.findContours(thresh, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
|
||
|
||
# ===== 核心：目标检测和跟踪 =====
|
||
target = None
|
||
max_area = 0
|
||
|
||
# 找到所有符合条件的轮廓
|
||
valid_contours = []
|
||
for cnt in contours:
|
||
area = cv2.contourArea(cnt)
|
||
|
||
if area < 75: # 过滤噪声，增大阈值
|
||
continue
|
||
|
||
if area > 5000: # 过滤大面积假目标（如整个画面），减小阈值
|
||
continue
|
||
|
||
# 计算轮廓的宽高比，过滤不符合垃圾特征的目标
|
||
x, y, w, h = cv2.boundingRect(cnt)
|
||
aspect_ratio = float(w) / h if h > 0 else 0
|
||
if aspect_ratio > 3 or aspect_ratio < 0.3: # 过滤过于狭长的目标
|
||
continue
|
||
|
||
valid_contours.append((area, cnt, x, y, w, h))
|
||
|
||
# 如果有有效的轮廓
|
||
if valid_contours:
|
||
# 如果正在跟踪目标，优先选择与上一目标位置接近的轮廓
|
||
if tracking_target is not None and prev_cx is not None and prev_cy is not None:
|
||
best_match = None
|
||
min_distance = float('inf')
|
||
|
||
search_radius = 80 # 目标锁定区域半径
|
||
|
||
for area, cnt, x, y, w, h in valid_contours:
|
||
cx = x + w // 2
|
||
cy = y + h // 2
|
||
# 计算与上一目标的距离
|
||
distance = ((cx - prev_cx) ** 2 + (cy - prev_cy) ** 2) ** 0.5
|
||
|
||
# 只考虑在锁定区域内的目标
|
||
if distance > search_radius:
|
||
continue
|
||
|
||
# 如果距离小于阈值，认为是同一个目标
|
||
if distance < 50: # 距离阈值
|
||
if distance < min_distance:
|
||
min_distance = distance
|
||
best_match = (area, cnt, x, y, w, h)
|
||
|
||
# 如果找到匹配的目标
|
||
if best_match:
|
||
area, cnt, x, y, w, h = best_match
|
||
target = cnt
|
||
max_area = area
|
||
else:
|
||
# 如果没有找到匹配的目标，选择最大的轮廓
|
||
valid_contours.sort(key=lambda x: x[0], reverse=True)
|
||
area, cnt, x, y, w, h = valid_contours[0]
|
||
target = cnt
|
||
max_area = area
|
||
# 开始跟踪新目标
|
||
tracking_target = True
|
||
else:
|
||
# 如果没有正在跟踪的目标，选择最大的轮廓
|
||
valid_contours.sort(key=lambda x: x[0], reverse=True)
|
||
area, cnt, x, y, w, h = valid_contours[0]
|
||
target = cnt
|
||
max_area = area
|
||
# 开始跟踪新目标
|
||
tracking_target = True
|
||
|
||
# ===== 只处理一个目标 =====
|
||
if target is not None:
|
||
x, y, w, h = cv2.boundingRect(target)
|
||
cx = x + w // 2
|
||
cy = y + h // 2
|
||
|
||
# 目标位置平滑处理
|
||
alpha = 0.6
|
||
if prev_cx is not None and prev_cy is not None:
|
||
cx = int(alpha * prev_cx + (1 - alpha) * cx)
|
||
cy = int(alpha * prev_cy + (1 - alpha) * cy)
|
||
|
||
# 绘制画面中心
|
||
center_x = 90 # 画面中心x坐标
|
||
center_y = 120 # 画面中心y坐标
|
||
cv2.circle(frame, (center_x, center_y), 5, (255, 0, 0), -1)
|
||
|
||
# 绘制目标矩形和中心
|
||
cv2.rectangle(frame, (x, y), (x+w, y+h), (0, 255, 0), 2)
|
||
cv2.circle(frame, (cx, cy), 5, (0, 0, 255), -1)
|
||
|
||
print("唯一目标:", cx, cy, "area:", max_area)
|
||
|
||
# 检测目标是否突然跳变
|
||
target_jumped = False
|
||
if prev_cx is not None and prev_cy is not None and prev_area is not None:
|
||
# 计算位置变化
|
||
dx = abs(cx - prev_cx)
|
||
dy = abs(cy - prev_cy)
|
||
# 计算面积变化比例
|
||
area_ratio = max_area / prev_area if prev_area > 0 else 0
|
||
|
||
# 如果位置变化过大或面积变化过大，认为目标跳变
|
||
if dx > 100 or dy > 100 or area_ratio < 0.3 or area_ratio > 3:
|
||
target_jumped = True
|
||
print("目标突然跳变，可能已离开视野范围")
|
||
# 重置跟踪状态
|
||
tracking_target = None
|
||
|
||
# 控制逻辑：根据x轴和y轴坐标控制垃圾桶移动
|
||
center_x = 90 # 画面中心x坐标
|
||
center_y = 120 # 画面中心y坐标
|
||
threshold = 15 # 阈值范围
|
||
|
||
if target_jumped:
|
||
# 目标跳变，停止移动
|
||
print("目标跳变，停止移动")
|
||
motor.stop()
|
||
else:
|
||
# 计算目标与中心的偏差
|
||
dx = cx - center_x
|
||
dy = cy - center_y
|
||
|
||
# 确定移动方向
|
||
if abs(dx) < threshold and abs(dy) < threshold:
|
||
# 目标在中心附近，停止移动
|
||
print("目标在中心，停止移动")
|
||
motor.stop()
|
||
elif abs(dx) < threshold:
|
||
# 只在y轴方向有偏差
|
||
if dy > threshold:
|
||
# 目标在下侧，垃圾桶向右移动
|
||
print("目标在下侧，向右移动")
|
||
motor.move_right(speed=1.0)
|
||
else:
|
||
# 目标在上侧，垃圾桶向左移动
|
||
print("目标在上侧，向左移动")
|
||
motor.move_left(speed=1.0)
|
||
elif abs(dy) < threshold:
|
||
# 只在x轴方向有偏差
|
||
if dx > threshold:
|
||
# 目标在右侧，垃圾桶向后移动
|
||
print("目标在右侧，向后移动")
|
||
motor.backward(speed=1.0)
|
||
else:
|
||
# 目标在左侧，垃圾桶向前移动
|
||
print("目标在左侧，向前移动")
|
||
motor.forward(speed=1.0)
|
||
else:
|
||
# 在x轴和y轴方向都有偏差，使用斜向移动
|
||
if dx > threshold and dy > threshold:
|
||
# 目标在右下侧，垃圾桶向右后移动
|
||
print("目标在右下侧，向右后移动")
|
||
motor.move_right_backward(speed=1.0)
|
||
elif dx > threshold and dy < -threshold:
|
||
# 目标在右上侧，垃圾桶向左后移动
|
||
print("目标在右上侧，向左后移动")
|
||
motor.move_left_backward(speed=1.0)
|
||
elif dx < -threshold and dy > threshold:
|
||
# 目标在左下侧，垃圾桶向右前移动
|
||
print("目标在左下侧，向右前移动")
|
||
motor.move_right_forward(speed=1.0)
|
||
else:
|
||
# 目标在左上侧，垃圾桶向左前移动
|
||
print("目标在左上侧，向左前移动")
|
||
motor.move_left_forward(speed=1.0)
|
||
|
||
# 更新上一帧的目标信息
|
||
prev_cx = cx
|
||
prev_cy = cy
|
||
prev_area = max_area
|
||
else:
|
||
# 没有检测到目标，停止移动
|
||
print("未检测到目标，停止移动")
|
||
motor.stop()
|
||
# 重置上一帧的目标信息
|
||
prev_cx = None
|
||
prev_cy = None
|
||
prev_area = None
|
||
# 重置跟踪状态
|
||
tracking_target = None
|
||
|
||
cv2.imshow("tracking", frame)
|
||
|
||
prev_gray = gray.copy()
|
||
|
||
if cv2.waitKey(1) & 0xFF == 27:
|
||
break
|
||
|
||
cap.release()
|
||
cv2.destroyAllWindows()
|
||
motor.stop() |