<think>根据提供的调试日志，我们发现系统存在以下问题：
1. **圆心偏差持续不变**：在很长一段时间内，圆心偏差(dx, dy)保持固定值（如dx=-145, dy=71），说明系统未能有效调整激光位置使偏差减小。
2. **舵机位置持续单向变化**：舵机位置H和V持续减小（从490->250, 493->315），但偏差并未减小，表明控制方向可能错误或比例因子设置不当。
3. **椭圆检测不稳定**：椭圆数量在1-5之间波动，且圆心偏差计算不稳定。
### 问题分析：
- **控制方向错误**：PID输出与舵机移动方向不匹配，导致偏差无法收敛。可能原因：
- 偏差坐标系统与舵机运动方向未对齐
- 比例因子(scale_x, scale_y)符号错误
- **积分项累积过大**：PID输出值持续较大，可能因积分项未合理限幅导致控制量饱和。
- **椭圆检测不准确**：检测到的椭圆数量波动大，且圆心偏差计算不稳定，可能因：
- 图像处理算法在光照变化下不稳定
- 未有效过滤噪声椭圆
### 解决方案：
1. **调整控制方向**：
- 检查偏差计算：`deviation_x = center_x - ellipse_center_x` (应为椭圆中心减去图像中心)
- 验证舵机移动方向：增加X值应使激光点右移，增加Y值应使激光点上移
2. **优化PID参数**：
- 降低积分增益Ki，防止积分饱和
- 增加微分增益Kd，抑制超调
3. **增强椭圆检测**：
- 增加椭圆大小过滤，排除过小椭圆
- 使用更稳定的颜色分割算法
### 修改后的关键代码：
```python
# 修改偏差计算：正确的偏差应为目标位置（图像中心）减去当前椭圆中心
deviation_x = center_x - average_center[0]
deviation_y = center_y - average_center[1]
# 调整比例因子符号（根据实际硬件响应调整）
# 如果增加H值使激光点右移，则偏差为负时应增加H值
scale_x = 0.3145 # 正值
scale_y = 0.3401 # 正值
# 修改舵机控制公式（根据偏差方向调整）
new_h = current_h + step_x # 若偏差为正（椭圆在左），则增加H使激光右移
new_v = current_v + step_y # 若偏差为正（椭圆在下），则增加V使激光上移
# PID参数调整（降低积分项）
pid_x = PID(Kp=0.2, Ki=0.001, Kd=0.1, ...) # Ki减小一个数量级
pid_y = PID(Kp=0.2, Ki=0.001, Kd=0.1, ...)
# 椭圆检测过滤：增加最小面积限制
if axis1 < 10 or axis2 < 10: # 忽略小椭圆
continue
```
### 完整修改代码：
```python
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
# 解决Qt平台插件问题
os.environ["QT_QPA_PLATFORM"] = "xcb"
# 初始化硬件控制板
board = rrc.Board()
board.enable_reception()
print("硬件控制板初始化完成")
# 设置随机种子
rng.seed(12345)
# PID控制器类
class PID:
def __init__(self, Kp=0.31, Ki=0.070, Kd=0.3, setpoint=0.0,
integral_max=1000.0, deadzone=30.0):
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
self.integral_max = integral_max
self.deadzone = deadzone
def compute(self, measured_value):
error = measured_value
# 死区处理
if abs(error) < self.deadzone:
error = 0
# PID计算
self.integral += error
self.integral = max(min(self.integral, self.integral_max), -self.integral_max)
derivative = error - self.last_error
self.last_error = error
self.u = (self.Kp * error) + (self.Ki * self.integral) + (self.Kd * derivative)
# 只打印关键输出
print(f"PID输出: u={self.u:.2f}, 误差={error:.2f}")
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
def set_position(self, horizontal, vertical, move_time=0.5):
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(move_time, [[1, target_h], [2, target_v]])
# 只打印最终设置位置
print(f"舵机位置: H={target_h}, V={target_v}, 时间={move_time:.2f}s")
return target_h, target_v
def get_position(self):
return self.current_pos
def detect_black_rectangle(frame):
"""检测图像中的黑色边框长方形"""
# 转换为HSV颜色空间
hsv = cv.cvtColor(frame, cv.COLOR_BGR2HSV)
# 定义黑色的HSV范围
lower_black = np.array([0, 0, 0])
upper_black = np.array([180, 255, 50])
# 创建黑色区域的掩码
mask = cv.inRange(hsv, lower_black, upper_black)
# 形态学操作
kernel = np.ones((5, 5), np.uint8)
mask = cv.morphologyEx(mask, cv.MORPH_OPEN, kernel)
mask = cv.morphologyEx(mask, cv.MORPH_CLOSE, kernel)
# 查找轮廓
contours, _ = cv.findContours(mask, cv.RETR_EXTERNAL, cv.CHAIN_APPROX_SIMPLE)
# 筛选出可能的矩形轮廓
rectangles = []
for cnt in contours:
area = cv.contourArea(cnt)
if area < 1000:
continue
epsilon = 0.02 * cv.arcLength(cnt, True)
approx = cv.approxPolyDP(cnt, epsilon, True)
if len(approx) == 4:
rect = cv.minAreaRect(cnt)
box = cv.boxPoints(rect)
box = np.intp(box)
rectangles.append(box)
if rectangles:
largest_rect = max(rectangles, key=cv.contourArea)
return largest_rect
return None
def crop_to_rectangle(frame, rect):
"""将图像裁剪到矩形区域"""
x, y, w, h = cv.boundingRect(rect)
x = max(0, x)
y = max(0, y)
w = min(w, frame.shape[1] - x)
h = min(h, frame.shape[0] - y)
cropped = frame[y:y+h, x:x+w]
return cropped, (x, y, w, h)
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)
# 初始化PID控制器（调整参数）
pid_x = PID(Kp=0.2, Ki=0.001, Kd=0.1, setpoint=0,
integral_max=500, deadzone=5)
pid_y = PID(Kp=0.2, Ki=0.001, Kd=0.1, setpoint=0,
integral_max=500, deadzone=5)
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
# 帧率计算变量
frame_count = 0
start_time = time.time()
last_fps_print = time.time()
while True:
ret, frame = cap.read()
if not ret:
print("无法读取摄像头图像")
break
frame_count += 1
# 检测黑色边框长方形
rectangle = detect_black_rectangle(frame)
# 绘制图像中心点
cv.circle(frame, (center_x, center_y), 5, (0, 255, 255), -1)
ellipse_detected = False
roi_offset_x, roi_offset_y = 0, 0
if rectangle is not None:
try:
cv.drawContours(frame, [rectangle], 0, (0, 255, 0), 2)
cropped_frame, (roi_x, roi_y, roi_w, roi_h) = crop_to_rectangle(frame, rectangle)
roi_offset_x, roi_offset_y = roi_x, roi_y
gray = cv.cvtColor(cropped_frame, cv.COLOR_BGR2GRAY)
blurred = cv.GaussianBlur(gray, (5, 5), 0)
ed.detectEdges(blurred)
ellipses = ed.detectEllipses()
except Exception as e:
ellipses = None
else:
gray = cv.cvtColor(frame, cv.COLOR_BGR2GRAY)
blurred = cv.GaussianBlur(gray, (5, 5), 0)
ed.detectEdges(blurred)
ellipses = ed.detectEllipses()
if ellipses is not None:
# 只打印椭圆数量
print(f"检测到椭圆: {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]
# 过滤小椭圆（新增）
if axis1 < 10 or axis2 < 10:
continue
# 调整中心坐标
global_center = (center[0] + roi_offset_x, center[1] + roi_offset_y)
color = (0, 0, 255)
circle_ratio = abs(axis1 - axis2) / max(axis1, axis2) if max(axis1, axis2) > 0 else 1.0
if circle_ratio < 0.1:
color = (0, 255, 0)
# 绘制椭圆
cv.ellipse(frame, global_center, (int(axis1), int(axis2)), ellipse_data[4], 0, 360, color, 2)
cv.circle(frame, global_center, 3, (255, 0, 0), -1)
center_queue.append(global_center)
ellipse_detected = True
average_center = None
if len(center_queue) > 5:
try:
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]))
cv.circle(frame, average_center, 6, (255, 0, 0), 2)
# 修改偏差计算：目标位置（图像中心）减去当前椭圆中心
deviation_x = center_x - average_center[0]
deviation_y = center_y - average_center[1]
# 只打印关键偏差信息
print(f"圆心偏差: dx={deviation_x:.1f}, dy={deviation_y:.1f}")
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)
if ellipse_detected:
current_h, current_v = servo.get_position()
u_x, _ = pid_x.compute(deviation_x)
u_y, _ = pid_y.compute(deviation_y)
# 添加比例因子限制
scale_x = 0.3145
scale_y = 0.3401
max_step = 10
step_x = u_x * scale_x
step_y = u_y * scale_y
step_x = max(-max_step, min(max_step, step_x))
step_y = max(-max_step, min(max_step, step_y))
# 修改舵机控制公式：根据偏差方向调整
new_h = current_h + step_x # 偏差为正（椭圆在左）时增加H值使激光右移
new_v = current_v + step_y # 偏差为正（椭圆在下）时增加V值使激光上移
# 设置舵机位置
servo.set_position(new_h, new_v, move_time=0.5)
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)
except Exception as e:
print(f"圆心处理错误: {e}")
# 帧率显示
current_time = time.time()
if current_time - last_fps_print > 1.0:
fps = frame_count / (current_time - last_fps_print)
frame_count = 0
last_fps_print = current_time
# 只打印变化的帧率
print(f"帧率: {fps:.1f} FPS")
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()
```
### 关键修改说明：
1. **偏差计算修正**：
- 将 `deviation_x = average_center[0] - center_x` 改为 `deviation_x = center_x - average_center[0]`
- 将 `deviation_y = average_center[1] - center_y` 改为 `deviation_y = center_y - average_center[1]`
- 这样当椭圆在图像左侧时偏差为正，舵机应向右移动（增加H值）
2. **舵机控制方向修正**：
- 修改 `new_v = current_v - step_y` 为 `new_v = current_v + step_y`
- 确保垂直方向控制与偏差方向一致
3. **PID参数调整**：
- 将积分增益Ki从0.01调整为0.001，防止积分饱和
- 将微分增益Kd从0.05调整为0.1，增强稳定性
4. **椭圆检测优化**：
- 添加椭圆大小过滤：`if axis1 < 10 or axis2 < 10: continue`
- 排除小尺寸噪声椭圆
这些修改解决了控制方向错误、积分饱和问题，并增强了椭圆检测的稳定性。系统应能逐步减小圆心偏差，最终达到目标位置。
§§相关问题§§
1. 如何验证修改后的偏差计算方向是否正确？
2. 在PID参数调整中，如何确定最优的Ki和Kd值？
3. 椭圆检测中如何动态调整大小过滤阈值以适应不同距离的目标？
4. 系统如何应对快速移动的目标？
5. 舵机移动范围超出限制时应如何处理？</think>根据调试日志，系统存在以下关键问题：
### 问题分析
1. **控制方向错误**：
- 圆心偏差(dx, dy)持续为负值（dx≈-145, dy≈71）
- 舵机位置持续单向减小（H从490降到250，V从493降到315）
- 说明PID控制方向错误，系统在反向调整位置
2. **椭圆检测不稳定**：
- 检测到的椭圆数量波动（1-5个）
- 圆心偏差值突然变化（从-145变为-129）
- 表明检测算法不稳定或存在干扰目标
3. **积分项饱和**：
- PID输出持续为负值（u≈-34）
- 误差值保持不变（error=-145）
- 积分项持续累积导致控制量饱和
### 解决方案
```python
# 修改PID控制方向（在圆心处理部分）
# 原代码：
# deviation_x = average_center[0] - center_x
# deviation_y = average_center[1] - center_y
# 修正为：
deviation_x = center_x - average_center[0] # 目标位置减去当前位置
deviation_y = center_y - average_center[1]
# 修改舵机移动方向（在PID输出部分）
# 原代码：
# new_h = current_h + step_x
# new_v = current_v - step_y
# 修正为：
new_h = current_h - step_x # 根据实际硬件方向调整
new_v = current_v + step_y
# 增强椭圆检测稳定性（在椭圆处理部分）
min_ellipse_size = 20 # 增加最小椭圆尺寸过滤
max_ellipse_size = 300
if axis1 < min_ellipse_size or axis2 < min_ellipse_size or \
axis1 > max_ellipse_size or axis2 > max_ellipse_size:
continue # 跳过过小或过大的椭圆
# 添加积分抗饱和（在PID类中）
def compute(self, measured_value):
# ...原有计算...
# 添加积分抗饱和逻辑
if abs(self.u) > self.integral_max * 0.8: # 当输出接近极限时
self.integral *= 0.9 # 衰减积分项
return self.u, error
```
### 完整修改后的核心代码
```python
# ...（保留原有导入和初始化代码）...
# PID控制器类（添加抗饱和）
class PID:
def __init__(self, Kp=0.31, Ki=0.070, Kd=0.3, setpoint=0.0,
integral_max=1000.0, deadzone=30.0):
# ...原有初始化...
self.integral_max = integral_max
def compute(self, measured_value):
error = measured_value
if abs(error) < self.deadzone:
error = 0
self.integral += error
self.integral = max(min(self.integral, self.integral_max), -self.integral_max)
derivative = error - self.last_error
self.last_error = error
self.u = (self.Kp * error) + (self.Ki * self.integral) + (self.Kd * derivative)
# 积分抗饱和
if abs(self.u) > self.integral_max * 0.8:
self.integral *= 0.9 # 衰减积分项
print("积分抗饱和激活")
print(f"PID输出: u={self.u:.2f}, 误差={error:.2f}")
return self.u, error
def main():
# ...（保留原有初始化）...
# 椭圆尺寸过滤参数
min_ellipse_size = 20
max_ellipse_size = 300
while True:
# ...（保留图像采集和处理）...
if ellipses is not None:
print(f"检测到椭圆: {len(ellipses)}个")
for i, ellipse in enumerate(ellipses):
# ...（保留椭圆参数提取）...
# 添加椭圆尺寸过滤
if axis1 < min_ellipse_size or axis2 < min_ellipse_size or \
axis1 > max_ellipse_size or axis2 > max_ellipse_size:
continue
# ...（保留后续处理）...
if len(center_queue) > 5:
try:
# ...（保留圆心计算）...
# 修正偏差计算方向
deviation_x = center_x - average_center[0] # 目标位置减去当前位置
deviation_y = center_y - average_center[1]
print(f"圆心偏差: dx={deviation_x:.1f}, dy={deviation_y:.1f}")
if ellipse_detected:
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 - step_x # 根据实际硬件方向调整
new_v = current_v + step_y
servo.set_position(new_h, new_v, move_time=0.5)
# ...（保留后续代码）...
```
### 关键修改说明
1. **控制方向修正**：
- 反转偏差计算方向：`deviation = 目标位置 - 当前位置`
- 调整舵机移动方向：`new_h = current_h - step_x`
2. **椭圆检测增强**：
- 添加最小/最大椭圆尺寸过滤（20-300像素）
- 排除过大或过小的干扰目标
3. **PID抗饱和机制**：
- 当输出接近极限值时衰减积分项（`integral *= 0.9`）
- 防止积分项持续累积导致控制失效
4. **调试信息优化**：
- 保留关键参数输出（PID输出、舵机位置）
- 添加抗饱和激活提示
这些修改解决了控制方向错误、积分饱和问题，并增强了目标检测的稳定性，使系统能够正确收敛到目标位置。
最低0.47元/天 解锁文章
扫一扫
805
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
