import numpy as np
import cv2 as cv
import random as rng
from collections import deque
import sys
import time
import signal
import threading
import board_demo.ros_robot_controller_sdk as rrc
import matplotlib.pyplot as plt
from matplotlib.backends.backend_agg import FigureCanvasAgg as FigureCanvas
from matplotlib.figure import Figure
# 初始化硬件控制板
board = rrc.Board()
board.enable_reception()
start = True
# 设置随机种子
rng.seed(12345)
# 增强型PID控制器类
class PID:
def __init__(self, Kp=0.31, Ki=0.070, Kd=0.3, setpoint=0.0, integral_max=100, deadzone=5):
self.Kp = Kp # 比例增益系数
self.Ki = Ki # 积分增益系数
self.Kd = Kd # 微分增益系数
self.setpoint = setpoint # 目标设定值
self.integral = 0.0 # 积分项累积值
self.derivative = 0.0 # 微分项(当前周期)
self.last_error = 0.0 # 上一次的误差值
self.u = 0.0 # PID输出控制量
self.integral_max = integral_max # 积分限幅
self.deadzone = deadzone # 死区阈值
def compute(self, measured_value):
# 1. 计算当前误差:设定值 - 测量值
error = self.setpoint - measured_value
# 2. 死区控制:小误差归零
if abs(error) < self.deadzone:
error = 0
# 3. 更新积分项(带限幅)
self.integral += error
self.integral = max(min(self.integral, self.integral_max), -self.integral_max)
# 4. 计算微分项(带低通滤波)
derivative = error - self.last_error
alpha = 0.2 # 滤波系数
self.derivative = alpha * derivative + (1 - alpha) * self.derivative
# 5. 保存当前误差
self.last_error = error
# 6. 计算PID输出
self.u = (self.Kp * error) + (self.Ki * self.integral) + (self.Kd * self.derivative)
return self.u, error
# 增强型舵机控制器
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 # 垂直激光偏差
self.max_step = 50 # 单步最大移动量
def set_position(self, horizontal, vertical, move_time=100):
"""设置舵机位置(带运动限制)"""
# 计算移动步长限制
delta_h = horizontal - self.current_pos[0]
delta_v = vertical - self.current_pos[1]
# 应用步长限制
if abs(delta_h) > self.max_step:
horizontal = self.current_pos[0] + np.sign(delta_h) * self.max_step
if abs(delta_v) > self.max_step:
vertical = self.current_pos[1] + np.sign(delta_v) * self.max_step
# 应用激光偏差补偿
target_h = int(horizontal + self.laser_offset_x)
target_v = int(vertical + self.laser_offset_y)
# 限制舵机位置在有效范围内
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))
# 更新当前位置
self.current_pos = [target_h, target_v]
# 发送控制指令
board.bus_servo_set_position(0.5, [[1, target_h], [2, target_v]])
print(f"Setting servos: H={target_h}, V={target_v}, Time={move_time}ms")
return target_h, target_v
def get_position(self):
return self.current_pos
# 图像预处理函数
def preprocess_image(frame):
"""应用CLAHE增强对比度并进行自适应阈值处理"""
# 转换到LAB颜色空间
lab = cv.cvtColor(frame, cv.COLOR_BGR2LAB)
l, a, b = cv.split(lab)
# 应用CLAHE
clahe = cv.createCLAHE(clipLimit=3.0, tileGridSize=(8, 8))
l = clahe.apply(l)
# 合并通道并转回BGR
lab = cv.merge((l, a, b))
enhanced = cv.cvtColor(lab, cv.COLOR_LAB2BGR)
# 转换为灰度图
gray = cv.cvtColor(enhanced, cv.COLOR_BGR2GRAY)
# 自适应阈值处理
binary = cv.adaptiveThreshold(
gray, 255, cv.ADAPTIVE_THRESH_GAUSSIAN_C,
cv.THRESH_BINARY_INV, 11, 2
)
# 形态学操作
kernel = cv.getStructuringElement(cv.MORPH_ELLIPSE, (5, 5))
processed = cv.morphologyEx(binary, cv.MORPH_CLOSE, kernel)
return processed
# 卡尔曼滤波器类
class KalmanFilter:
def __init__(self, process_noise=1e-5, measurement_noise=1e-1):
self.kf = cv.KalmanFilter(4, 2)
self.kf.transitionMatrix = np.array([
[1, 0, 1, 0],
[0, 1, 0, 1],
[0, 0, 1, 0],
[0, 0, 0, 1]
], dtype=np.float32)
self.kf.measurementMatrix = np.array([
[1, 0, 0, 0],
[0, 1, 0, 0]
], dtype=np.float32)
self.kf.processNoiseCov = np.eye(4, dtype=np.float32) * process_noise
self.kf.measurementNoiseCov = np.eye(2, dtype=np.float32) * measurement_noise
self.kf.errorCovPost = np.eye(4, dtype=np.float32)
def predict(self):
return self.kf.predict()
def correct(self, measurement):
measurement = np.array([[np.float32(measurement[0])], [np.float32(measurement[1])]])
return self.kf.correct(measurement)
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)
# 创建边缘检测对象
ed = cv.ximgproc.createEdgeDrawing()
# 设置边缘检测参数
EDParams = cv.ximgproc_EdgeDrawing_Params()
EDParams.MinPathLength = 160 # 最小边缘长度
EDParams.PFmode = True # 启用模式
EDParams.MinLineLength = 50 # 最小线段长度
EDParams.NFAValidation = True # 启用验证
ed.setParams(EDParams)
# 创建圆心位置队列
center_queue = deque(maxlen=100)
# 创建窗口
cv.namedWindow("Detected Ellipses", cv.WINDOW_NORMAL)
cv.namedWindow("Error Plot", cv.WINDOW_NORMAL)
# 初始化PID控制器
pid_x = PID(Kp=10, Ki=0.1, Kd=10.00, setpoint=0, deadzone=5)
pid_y = PID(Kp=10, Ki=0.1, Kd=10.00, setpoint=0, deadzone=5)
# 初始化舵机控制器
servo = ServoController()
# 初始化卡尔曼滤波器
kf = KalmanFilter()
# 状态机定义
STATES = {"SEARCH": 0, "TRACKING": 1, "LOST": 2}
current_state = STATES["SEARCH"]
lost_count = 0
scan_angle = 0
scan_direction = 1
# 获取图像中心坐标
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
# 初始化误差数据存储
max_data_points = 100 # 最大显示数据点数
x_errors = deque(maxlen=max_data_points)
y_errors = deque(maxlen=max_data_points)
frame_counts = deque(maxlen=max_data_points)
frame_counter = 0
# 初始化Matplotlib图表
fig = Figure(figsize=(5, 3), dpi=100)
canvas = FigureCanvas(fig)
ax = fig.add_subplot(111)
ax.set_title('实时控制误差曲线')
ax.set_xlabel('帧数')
ax.set_ylabel('误差(像素)')
ax.grid(True)
line_x, = ax.plot([], [], 'r-', label='X轴误差')
line_y, = ax.plot([], [], 'b-', label='Y轴误差')
ax.legend(loc='upper right')
ax.set_xlim(0, max_data_points)
ax.set_ylim(-100, 100)
# 主循环
while True:
frame_counter += 1
# 读取摄像头帧
ret, frame = cap.read()
if not ret:
print("无法读取摄像头图像")
break
# 图像预处理
processed = preprocess_image(frame)
# 应用高斯模糊
blurred = cv.GaussianBlur(processed, (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)
# 状态机处理
ellipse_detected = False
current_deviation_x = 0
current_deviation_y = 0
if ellipses is not None:
for i in range(len(ellipses)):
center = (int(ellipses[i][0][0]), int(ellipses[i][0][1]))
axes = (int(ellipses[i][0][2] + ellipses[i][0][3]),
int(ellipses[i][0][2] + ellipses[i][0][4]))
angle = ellipses[i][0][5]
# 根据椭圆类型设置颜色
color = (0, 0, 255) # 红色 - 椭圆
if ellipses[i][0][2] == 0:
color = (0, 255, 0) # 绿色 - 正圆
# 绘制椭圆
cv.ellipse(frame, center, axes, angle, 0, 360, color, 2, cv.LINE_AA)
cv.circle(frame, center, 3, (255, 0, 0), -1)
# 将圆心加入队列
center_queue.append(center)
ellipse_detected = True
current_deviation_x = center[0] - center_x
current_deviation_y = center[1] - center_y
# 状态转换逻辑
if ellipse_detected:
current_state = STATES["TRACKING"]
lost_count = 0
else:
if current_state == STATES["TRACKING"]:
lost_count += 1
if lost_count > 10: # 连续10帧丢失目标
current_state = STATES["LOST"]
lost_count = 0
elif current_state == STATES["SEARCH"]:
# 扫描模式:水平摆动
scan_angle += scan_direction * 10
if abs(scan_angle) > 300:
scan_direction *= -1
servo.set_position(500 + scan_angle, 500)
# 目标跟踪处理
average_center = None
if current_state == STATES["TRACKING"] and len(center_queue) > 5:
# 计算圆心平均值和标准差
mean_center = np.mean(center_queue, axis=0)
std_deviation = np.std(center_queue, axis=0)
# 过滤掉偏差过大的点
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]))
# 卡尔曼滤波更新
kf.correct(average_center)
predicted = kf.predict()
average_center = (int(predicted[0]), int(predicted[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
# 更新当前误差
current_deviation_x = deviation_x
current_deviation_y = 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控制计算
current_h, current_v = servo.get_position()
u_x, _ = pid_x.compute(deviation_x)
u_y, _ = pid_y.compute(deviation_y)
# 计算新舵机位置
new_h = current_h + u_x * 0.0420 # 水平控制
new_v = current_v - u_y * 0.0521 # 垂直控制
# 设置舵机位置
servo.set_position(new_h, new_v, 0.5)
# 显示PID控制信息
pid_text = f"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)
# 存储误差数据
x_errors.append(current_deviation_x)
y_errors.append(current_deviation_y)
frame_counts.append(frame_counter)
# 更新误差曲线
if frame_counter % 5 == 0: # 每5帧更新一次图表
# 清除旧数据
ax.clear()
# 设置图表属性
ax.set_title('实时控制误差曲线')
ax.set_xlabel('帧数')
ax.set_ylabel('误差(像素)')
ax.grid(True)
# 绘制误差曲线
ax.plot(frame_counts, x_errors, 'r-', label='X轴误差')
ax.plot(frame_counts, y_errors, 'b-', label='Y轴误差')
# 添加图例
ax.legend(loc='upper right')
# 设置坐标轴范围
ax.set_xlim(max(0, frame_counter - max_data_points), frame_counter + 5)
ax.set_ylim(-100, 100)
# 渲染图表到OpenCV图像
canvas.draw()
plot_img = np.frombuffer(canvas.tostring_rgb(), dtype=np.uint8)
plot_img = plot_img.reshape(canvas.get_width_height()[::-1] + (3,))
plot_img = cv.cvtColor(plot_img, cv.COLOR_RGB2BGR)
# 显示误差曲线
cv.imshow("Error Plot", plot_img)
# 显示状态信息
state_text = ["SEARCH", "TRACKING", "LOST"][current_state]
cv.putText(frame, f"State: {state_text}", (10, 90),
cv.FONT_HERSHEY_SIMPLEX, 0.7, (0, 255, 0), 2)
# 显示帧率
fps = cap.get(cv.CAP_PROP_FPS)
if fps > 0:
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(2, [[1, 500], [2, 500]])
break
# 释放资源
cap.release()
cv.destroyAllWindows()
if __name__ == '__main__':
main()
检查代码,舵机运动幅度过大,应该怎么调节参数
最新发布