import numpy as np
import cv2 as cv
import random as rng
from collections import deque
import board_demo.ros_robot_controller_sdk as rrc
import os
import time
from Fuzzy_pid import FuzzyPID # 导入模糊PID类
# 解决Qt平台插件问题
os.environ["QT_QPA_PLATFORM"] = "xcb"
# 初始化硬件控制板
board = rrc.Board()
board.enable_reception()
print("硬件控制板初始化完成")
# 设置随机种子
rng.seed(12345)
# 舵机控制类
class ServoController:
def __init__(self):
self.current_pos = [500, 500]
self.horizontal_range = (100, 900)
self.vertical_range = (200, 800)
self.laser_offset_x = 0
self.laser_offset_y = 0
print(f"舵机控制初始化: 水平范围={self.horizontal_range}, 垂直范围={self.vertical_range}")
print(f"激光偏移量: X={self.laser_offset_x}, Y={self.laser_offset_y}")
def set_position(self, horizontal, vertical, move_time=500):
target_h = int(horizontal + self.laser_offset_x)
target_v = int(vertical + self.laser_offset_y)
print(f"\n[舵机设置] 原始目标: H={horizontal:.2f}, V={vertical:.2f}")
target_h = max(self.horizontal_range[0], min(self.horizontal_range[1], target_h))
target_v = max(self.vertical_range[0], min(self.vertical_range[1], target_v))
if target_h != horizontal + self.laser_offset_x or target_v != vertical + self.laser_offset_y:
print(f"位置限制生效: H={target_h}, V={target_v}")
self.current_pos = [target_h, target_v]
board.bus_servo_set_position(0.5, [[1, target_h], [2, target_v]])
print(f"设置舵机位置: H={target_h}, V={target_v}, 移动时间={move_time}ms")
return target_h, target_v
def get_position(self):
return self.current_pos
def main():
# 打开摄像头
cap = cv.VideoCapture(0)
if not cap.isOpened():
print("无法打开摄像头")
return
# 设置分辨率
cap.set(cv.CAP_PROP_FRAME_WIDTH, 640)
cap.set(cv.CAP_PROP_FRAME_HEIGHT, 480)
print(f"摄像头设置: 宽度={640}, 高度={480}")
# 创建边缘检测对象
ed = cv.ximgproc.createEdgeDrawing()
EDParams = cv.ximgproc_EdgeDrawing_Params()
EDParams.MinPathLength = 120
EDParams.PFmode = True
EDParams.MinLineLength = 30
EDParams.NFAValidation = True
ed.setParams(EDParams)
print(f"边缘检测参数: MinPathLength={EDParams.MinPathLength}, MinLineLength={EDParams.MinLineLength}")
center_queue = deque(maxlen=100)
cv.namedWindow("Detected Ellipses", cv.WINDOW_NORMAL)
# 初始化模糊PID控制器 - 分别用于X轴和Y轴
# 水平控制 (X轴)
fuzzy_pid_x = FuzzyPID(
base_kp=0.2, kp_min=0.01, kp_max=3,
base_ki=0.000, ki_min=0.000, ki_max=0.1,
base_kd=1, kd_min=0.0, kd_max=10,
output_min=-10, output_max=10,
e_min=-316, e_max=316, # 假设图像宽度640,中心偏差范围±320
ec_min=-100, ec_max=100
)
# 垂直控制 (Y轴)
fuzzy_pid_y = FuzzyPID(
base_kp=0.5, kp_min=0.1, kp_max=2.0,
base_ki=0.000, ki_min=0.000, ki_max=0.02,
base_kd=0.1, kd_min=0.0, kd_max=0.5,
output_min=-10, output_max=10,
e_min=-240, e_max=240, # 假设图像高度480,中心偏差范围±240
ec_min=-10, ec_max=10
)
servo = ServoController()
# 获取图像中心
ret, frame = cap.read()
if not ret:
print("无法读取摄像头图像")
return
image_height, image_width = frame.shape[:2]
center_x, center_y = image_width // 2, image_height // 2
print(f"图像中心: X={center_x}, Y={center_y}")
# 帧率计算变量
frame_count = 0
start_time = time.time()
while True:
ret, frame = cap.read()
if not ret:
print("无法读取摄像头图像")
break
frame_count += 1
# 图像处理
gray = cv.cvtColor(frame, cv.COLOR_BGR2GRAY)
blurred = cv.GaussianBlur(gray, (5, 5), 0)
ed.detectEdges(blurred)
ellipses = ed.detectEllipses()
# 绘制图像中心点
cv.circle(frame, (center_x, center_y), 5, (0, 255, 255), -1)
cv.line(frame, (center_x - 15, center_y), (center_x + 15, center_y), (0, 255, 255), 1)
cv.line(frame, (center_x, center_y - 15), (center_x, center_y + 15), (0, 255, 255), 1)
cv.putText(frame, "X", (image_width - 20, center_y - 10),
cv.FONT_HERSHEY_SIMPLEX, 0.5, (0, 255, 255), 1)
cv.putText(frame, "Y", (center_x + 10, 20),
cv.FONT_HERSHEY_SIMPLEX, 0.5, (0, 255, 255), 1)
ellipse_detected = False
if ellipses is not None:
print(f"\n检测到 {len(ellipses)} 个椭圆")
for i, ellipse in enumerate(ellipses):
ellipse_data = np.array(ellipse).flatten()
if len(ellipse_data) < 5:
continue
center = (int(ellipse_data[0]), int(ellipse_data[1]))
axis1 = ellipse_data[2]
axis2 = ellipse_data[3]
angle = ellipse_data[4]
color = (0, 0, 255) # 红色-椭圆
circle_ratio = abs(axis1 - axis2) / max(axis1, axis2)
if circle_ratio < 0.1:
color = (0, 255, 0) # 绿色-正圆
print(f"椭圆 {i+1}: 中心({center[0]},{center[1]}), 轴长({axis1:.2f},{axis2:.2f}), "
f"角度={angle:.2f}°, 圆度={circle_ratio:.4f}")
cv.ellipse(frame, center, (int(axis1), int(axis2)), angle, 0, 360, color, 2)
cv.circle(frame, center, 3, (255, 0, 0), -1)
center_queue.append(center)
ellipse_detected = True
average_center = None
if len(center_queue) > 5:
mean_center = np.mean(center_queue, axis=0)
std_deviation = np.std(center_queue, axis=0)
print(f"\n圆心队列: 数量={len(center_queue)}, 平均值=({mean_center[0]:.2f},{mean_center[1]:.2f}), "
f"标准差=({std_deviation[0]:.2f},{std_deviation[1]:.2f})")
filtered_centers = []
for center in center_queue:
distance = np.linalg.norm(np.array(center) - mean_center)
if distance <= 2 * np.linalg.norm(std_deviation):
filtered_centers.append(center)
if filtered_centers:
mean_centers = np.mean(filtered_centers, axis=0)
average_center = (int(mean_centers[0]), int(mean_centers[1]))
cv.circle(frame, average_center, 6, (255, 0, 0), 2)
# 计算偏差
deviation_x = average_center[0] - center_x
deviation_y = average_center[1] - center_y
print(f"圆心偏差: dx={deviation_x}, dy={deviation_y}")
# 绘制偏差线
cv.arrowedLine(frame, (center_x, center_y), average_center, (0, 255, 255), 2)
deviation_text = f"dx: {deviation_x:.1f}, dy: {deviation_y:.1f}"
cv.putText(frame, deviation_text, (10, 30),
cv.FONT_HERSHEY_SIMPLEX, 0.7, (0, 255, 255), 2)
# 模糊PID控制
if ellipse_detected:
current_h, current_v = servo.get_position()
print(f"当前舵机位置: H={current_h}, V={current_v}")
# 调用模糊PID控制器
u_x = fuzzy_pid_x.FuzzyPIDcontroller(0, deviation_x)
u_y = fuzzy_pid_y.FuzzyPIDcontroller(0, deviation_y)
print(f"模糊PID输出: u_x={u_x:.2f}, u_y={u_y:.2f}")
# 更新舵机位置
new_h = current_h - u_x * 0.30
new_v = current_v - u_y * 0.3401
print(f"计算新位置: H={new_h:.2f}, V={new_v:.2f}")
# 设置舵机位置
servo.set_position(new_h, new_v, move_time=500)
pid_text = f"Fuzzy PID: ux={u_x:.1f}, uy={u_y:.1f}"
cv.putText(frame, pid_text, (10, 60),
cv.FONT_HERSHEY_SIMPLEX, 0.7, (0, 255, 0), 2)
# 检查是否达到目标位置
if abs(deviation_x) < 10 and abs(deviation_y) < 10:
target_text = "Target Reached!"
cv.putText(frame, target_text, (image_width // 2 - 100, 30),
cv.FONT_HERSHEY_SIMPLEX, 0.9, (0, 255, 0), 2)
# 计算并显示帧率
elapsed_time = time.time() - start_time
if elapsed_time > 1:
fps = frame_count / elapsed_time
frame_count = 0
start_time = time.time()
print(f"当前帧率: {fps:.1f} FPS")
if 'fps' in locals():
fps_text = f"FPS: {fps:.1f}"
cv.putText(frame, fps_text, (10, image_height - 10),
cv.FONT_HERSHEY_SIMPLEX, 0.6, (0, 255, 255), 1)
# 显示图像
cv.imshow("Detected Ellipses", frame)
# 退出处理
if cv.waitKey(1) & 0xFF == ord('q'):
board.bus_servo_set_position(1, [[1, 500], [2, 500]])
print("退出程序: 舵机归中")
break
# 释放资源
cap.release()
cv.destroyAllWindows()
print("程序结束")
if __name__ == '__main__':
main()怎么减小曝光值