@uzbd -- coding: utf-8 -- import sensor, image, time, math 初始化摄像头 sensor.reset() sensor.set_pixformat(sensor.RGB565) sensor.set_framesize(sensor.VGA) sensor.skip_frames(time=1000) sensor.set_auto_gain(False) sensor.set_auto_whitebal(False) 相机内参设置 f_x = (2.8 / 3.6736) * 640 f_y = (2.8 / 2.7384) * 480 c_x = 640 * 0.5 c_y = 480 * 0.5 系统状态变量 SETTING_BASE = 0 MONITORING = 1 MEASURING = 2 system_state = SETTING_BASE 位置信息 base_position = None # 存储基准位置(中心点) filtered_position = None stroke_mm = 0 base_samples = [] current_samples = [] INITIAL_SAMPLES = 20 CURRENT_SAMPLES = 20 运动检测参数 MOTION_THRESHOLD = 20 STABLE_FRAMES = 50 stable_count = 0 last_img = None roi = (0, 0, 0, 0) roi1 = (0, 0, 250, 480) # 运动检测的ROI区域 AprilTag参数 K1=14.98 TAG_SIZE_MM = 26 TAG_AREA_MM2 = TAG_SIZE_MM * TAG_SIZE_MM # 标签实际面积 x0, y0, x1, y1 = None, None, None, None 计算Open MV到TAG的距离 def Tag_OpenMV_distance(tx,ty,tz): Distance1 = K1 * math.sqrt(txtx+tyty+tz*tz) return Distance1 计算实际距离的函数 def calculate_real_distance(pixel_distance, k_value): return pixel_distance * k_value 计算二维空间欧氏距离 def euclidean_distance(pos1, pos2): if pos1 is None or pos2 is None: return 0.0 dx = pos1[0] - pos2[0] dy = pos1[1] - pos2[1] return math.sqrt(dxdx + dydy) 卡尔曼滤波器类 class KalmanFilter: def init(self, initial_value, process_variance=1, measurement_variance=0.5): """ 一维卡尔曼滤波器 :param initial_value: 初始估计值 :param process_variance: 过程噪声方差 (Q) 小值 (1e-6 到 1e-4): 状态变化缓慢，滤波器更"保守" 大值 (0.1 到 1.0): 状态变化快速，滤波器响应更快 :param measurement_variance: 测量噪声方差 (R) 小值 (0.01 到 0.1): 测量值可靠，滤波器更信任测量 大值 (1.0 到 10.0): 测量值噪声大，滤波器更信任预测 :estimate_variance: 初始估计方差 (P) 表示初始状态的不确定性 中等不确定性设为1 高不确定性可设10-100 """ self.state_estimate = initial_value self.estimate_variance = 20 # 初始估计方差 (P) self.process_variance = process_variance self.measurement_variance = measurement_variance def update(self, measurement): prior_estimate = self.state_estimate prior_variance = self.estimate_variance + self.process_variance kalman_gain = prior_variance / (prior_variance + self.measurement_variance) self.state_estimate = prior_estimate + kalman_gain * (measurement - prior_estimate) self.estimate_variance = (1 - kalman_gain) * prior_variance return self.state_estimate 创建卡尔曼滤波器实例 为四个角点创建滤波器 (每个角点有x,y坐标) corner_kalman = [] for i in range(4): # 四个角点 corner_kalman.append([ KalmanFilter(initial_value=320.0), # x坐标滤波器 KalmanFilter(initial_value=240.0)]) # y坐标滤波器 中心点卡尔曼滤波器 center_x_kalman = KalmanFilter(initial_value=320.0) center_y_kalman = KalmanFilter(initial_value=240.0) 边平均长度卡尔曼滤波器 edge_kalman = KalmanFilter(initial_value=50.0) 面积卡尔曼滤波器 area_kalman = KalmanFilter(initial_value=1000.0) 加权平均位置计算 def weighted_average_position(positions): if not positions: return None #n = len(positions) total_weight = 0 weighted_x = 0 weighted_y = 0 for i, pos in enumerate(positions): weight = i + 1 total_weight += weight weighted_x += pos[0] * weight weighted_y += pos[1] * weight return (weighted_x / total_weight, weighted_y / total_weight) 运动检测函数 def detect_motion(current_img): global last_img, stable_count if last_img is None: last_img = current_img.copy() return True diff = current_img.copy(roi=roi).difference(last_img.copy(roi=roi)) stats = diff.get_statistics() last_img = current_img.copy() avg_change = (stats.l_max() - stats.l_min()) / 2 if avg_change > MOTION_THRESHOLD: stable_count = 0 return True else: stable_count += 1 return False ================ 三重尺寸计算方法 ================ def calculate_edge_lengths(corners): """ 计算四条边的长度 """ edges = [] n = len(corners) for i in range(n): j = (i + 1) % n dx = corners[j][0] - corners[i][0] dy = corners[j][1] - corners[i][1] edges.append(math.sqrt(dxdx + dydy)) return edges def calculate_area(corners): """ 使用鞋带公式计算多边形面积 """ n = len(corners) area = 0.0 for i in range(n): j = (i + 1) % n area += corners[i][0] * corners[j][1] area -= corners[j][0] * corners[i][1] return abs(area) / 2.0 def filter_corners(corners): """ 对四个角点应用卡尔曼滤波 """ filtered_corners = [] for i in range(4): x = corner_kalman[i][0].update(corners[i][0]) y = corner_kalman[i][1].update(corners[i][1]) filtered_corners.append((x, y)) return filtered_corners def calculate_center(corners): """ 计算四个角点的中心 """ x_sum = sum(corner[0] for corner in corners) y_sum = sum(corner[1] for corner in corners) return (x_sum / 4, y_sum / 4) def calculate_combined_size(filtered_corners): """ 三重尺寸计算方法： 1. 计算四条边的平均长度 2. 使用鞋带公式计算面积 3. 从面积推导等效边长 4. 加权组合 (30% 边平均 + 20% 面积等效 + 50%TAG返回的像素宽度与高度的平均值) """ # 计算四条边长度 edges = calculate_edge_lengths(filtered_corners) avg_edge = sum(edges) / len(edges) # 计算面积 area = calculate_area(filtered_corners) # 计算等效边长 (假设为正方形) equiv_edge = math.sqrt(area) # 加权组合 combined_size = 0.3 * avg_edge + 0.2 * equiv_edge + 0.5 * tag.h-1#((tag.h+tag.w)/2) return combined_size, avg_edge, equiv_edge, area ================ 三重尺寸计算方法结束 ================ clock = time.clock() 主循环 while(True): clock.tick() img = sensor.snapshot().lens_corr(1.6) if system_state == SETTING_BASE: tags = img.find_apriltags(fx=f_x, fy=f_y, cx=c_x, cy=c_y, families=image.TAG36H11, tag_size=TAG_SIZE_MM) img.draw_rectangle(roi1, color=(255, 0, 0)) if tags: tag = tags[0] # 检查标签质心是否在ROI1区域内 tag_in_roi = (roi1[0] <= tag.cxf <= roi1[0] + roi1[2] and roi1[1] <= tag.cyf <= roi1[1] + roi1[3]) if not tag_in_roi: # 标签不在ROI1区域内，显示提示信息 img.draw_string(0, 10, "Move tag to ROI1 area !!!", color=(255, 255, 0)) continue # 跳过后续处理 tx,ty,tz=tag.x_translation,tag.y_translation,tag.z_translation x0 , y0 = tag.cxf , tag.cyf # 获取原始角点 raw_corners = tag.corners # 对角点进行卡尔曼滤波 filtered_corners = filter_corners(raw_corners) # 计算滤波后的中心点 center_x, center_y = calculate_center(filtered_corners) # 对TAG质心进行卡尔曼滤波 #filtered_center_x = center_x_kalman.update(tag.cxf) #filtered_center_y = center_y_kalman.update(tag.cyf) # 对中心点进行卡尔曼滤波 filtered_center_x = center_x_kalman.update(center_x) filtered_center_y = center_y_kalman.update(center_y) # 使用双重尺寸计算方法 #combined_size, avg_edge, equiv_edge, area = calculate_combined_size(filtered_corners) # 对平均边和面积进行卡尔曼滤波 #filtered_avg_edge = edge_kalman.update(avg_edge) #filtered_area = area_kalman.update(area) # 重新计算组合尺寸（使用滤波后的值） #combined_size = 0.7 * filtered_avg_edge + 0.3 * math.sqrt(filtered_area)#+0.4 *((tag.h+tag.w)/2) # 绘制标签轮廓和角点 for i in range(4): # 绘制滤波后的角点 img.draw_circle(int(filtered_corners[i][0]), int(filtered_corners[i][1]), 5, color=(0, 255, 255)) # 绘制原始角点 img.draw_circle(int(raw_corners[i][0]), int(raw_corners[i][1]), 3, color=(255, 0, 255)) # 绘制边 next_i = (i + 1) % 4 img.draw_line( int(filtered_corners[i][0]), int(filtered_corners[i][1]), int(filtered_corners[next_i][0]), int(filtered_corners[next_i][1]), color=(0, 255, 0) ) # 绘制滤波后的中心点 img.draw_cross(int(filtered_center_x), int(filtered_center_y), color=(0, 255, 0), size=10) # 显示尺寸信息 img.draw_string(0, 400, "ID:%d" % tag.id, color=(255, 0, 0)) #img.draw_string(0, 370, "AvgEdge:%.1f" % filtered_avg_edge, color=(255, 0, 0)) #img.draw_string(0, 390, "EquivEdge:%.1f" % math.sqrt(filtered_area), color=(0, 255, 0)) #img.draw_string(0, 380, "Combined:%.1f" % combined_size, color=(0, 255, 255)) #img.draw_string(0, 360, "Area:%.1f" % filtered_area, color=(255, 255, 0)) img.draw_string(0, 350, "H:%d" % tag.rect[3], color=(255, 255, 0)) img.draw_string(0, 340, "W:%d" % tag.rect[2], color=(255, 255, 0)) # 记录滤波后的中心位置 current_position = (filtered_center_x, filtered_center_y) base_samples.append(current_position) progress = len(base_samples) / INITIAL_SAMPLES * 100 img.draw_string(0, 10, "Setting base:%d%%" % progress, color=(255, 255, 0)) if len(base_samples) >= INITIAL_SAMPLES: # 计算加权平均位置 base_position = weighted_average_position(base_samples) print("Base position set: (%.1f, %.1f)" % base_position) img.draw_string(0, 420, "The base position is set!", color=(255, 0, 0)) system_state = MONITORING roi = (0, tag.rect[1]-20, 640, tag.rect[2]+40) # 运动检测的ROI区域 base_samples = [] current_samples = [] else: img.draw_string(0, 10, "Place piston at base position", color=(255, 0, 0)) elif system_state == MONITORING: motion_detected = detect_motion(img) status = "In motion" if motion_detected else "In stability (%d/%d)" % (stable_count, STABLE_FRAMES) img.draw_rectangle(roi, color=(0, 255, 0)) img.draw_string(0, 10, "State: %s" % status, color=(255, 255, 0)) if stable_count >= STABLE_FRAMES: print("活塞已停止，开始最终定位") img.draw_string(0, 20, "The piston has stopped and begins final positioning!", color=(255, 0, 0)) system_state = MEASURING elif system_state == MEASURING: tags = img.find_apriltags(fx=f_x, fy=f_y, cx=c_x, cy=c_y, families=image.TAG36H11, tag_size=TAG_SIZE_MM) if tags: tag = tags[0] x1 , y1 = tag.cxf , tag.cyf # 获取原始角点 raw_corners = tag.corners # 对角点进行卡尔曼滤波 filtered_corners = filter_corners(raw_corners) # 计算滤波后的中心点 center_x, center_y = calculate_center(filtered_corners) # 对中心点进行卡尔曼滤波 #filtered_center_x = center_x_kalman.update(tag.cxf) #filtered_center_y = center_y_kalman.update(tag.cyf) filtered_center_x = center_x_kalman.update(center_x) filtered_center_y = center_y_kalman.update(center_y) # 使用双重尺寸计算方法 combined_size, avg_edge, equiv_edge, area = calculate_combined_size(filtered_corners) # 对平均边和面积进行卡尔曼滤波 filtered_avg_edge = edge_kalman.update(avg_edge) filtered_area = area_kalman.update(area) # 重新计算组合尺寸（使用滤波后的值） combined_size = 0.3 * filtered_avg_edge + 0.2 * math.sqrt(filtered_area)+0.5 * tag.h-1#((tag.h+tag.w)/2) # 计算比例系数 k_value = TAG_SIZE_MM / combined_size # 绘制标签轮廓和角点 for i in range(4): # 绘制滤波后的角点 img.draw_circle(int(filtered_corners[i][0]), int(filtered_corners[i][1]), 5, color=(0, 255, 255)) # 绘制原始角点 img.draw_circle(int(raw_corners[i][0]), int(raw_corners[i][1]), 3, color=(255, 0, 255)) # 绘制边 next_i = (i + 1) % 4 img.draw_line( int(filtered_corners[i][0]), int(filtered_corners[i][1]), int(filtered_corners[next_i][0]), int(filtered_corners[next_i][1]), color=(0, 255, 0) ) # 绘制滤波后的中心点 img.draw_cross(int(filtered_center_x), int(filtered_center_y), color=(0, 255, 0), size=10) # 显示尺寸信息 img.draw_string(0, 400, "ID:%d" % tag.id, color=(255, 0, 0)) img.draw_string(0, 370, "AvgEdge:%.1f" % filtered_avg_edge, color=(255, 0, 0)) img.draw_string(0, 390, "EquivEdge:%.1f" % math.sqrt(filtered_area), color=(0, 255, 0)) img.draw_string(0, 380, "Combined:%.1f" % combined_size, color=(0, 255, 255)) img.draw_string(0, 360, "Area:%.1f" % filtered_area, color=(255, 255, 0)) img.draw_string(0, 350, "H:%d" % tag.rect[3], color=(255, 255, 0)) img.draw_string(0, 340, "W:%d" % tag.rect[2], color=(255, 255, 0)) # 记录滤波后的中心位置 current_position = (filtered_center_x, filtered_center_y) current_samples.append(current_position) # 保持采样列表长度 if len(current_samples) > CURRENT_SAMPLES: current_samples.pop(0) # 计算加权平均位置 if current_samples: avg_x, avg_y = weighted_average_position(current_samples) # 计算像素距离 pixel_distance = euclidean_distance((avg_x, avg_y), base_position) # 计算实际距离 stroke_mm = pixel_distance * k_value # 显示行程信息 text = "Distance:%.1fmm" % stroke_mm img.draw_string(0, 440, text, color=(0, 255, 0)) pos_text = "Pos:(%.1f,%.1f)" % (avg_x, avg_y) img.draw_string(0, 450, pos_text, color=(255, 255, 0)) k_text = "K:%.4f Size:%.1f" % (k_value, combined_size) img.draw_string(0, 470, k_text, color=(255, 255, 0)) # 显示旋转角度信息 Xrot_text = "XRot:%.1f°" % (math.degrees(tag.x_rotation)-180) img.draw_string(0, 230, Xrot_text, color=(150, 150, 255)) Yrot_text = "YRot:%.1f°" % math.degrees(tag.y_rotation) img.draw_string(0, 240, Yrot_text, color=(150, 150, 255)) Zrot_text = "ZRot:%.1f°" % math.degrees(tag.z_rotation) img.draw_string(0, 250, Zrot_text, color=(150, 150, 255)) print("%s | Combined:%.1fpx | K:%.4f" % (text, combined_size, k_value)) else: img.draw_string(0, 420, "Tag lost!", color=(255, 0, 0)) text = "Distance:%.1fmm" % stroke_mm img.draw_string(0, 440, text, color=(0, 255, 0)) if x0 is not None and y0 is not None: # 在基准点上方显示行程文本 text = "Distance:%.1fmm" % stroke_mm img.draw_string(int(x0)+3, int(y0)-10, text, color=(255, 255, 0)) # 在基准点上方画一条短竖线 img.draw_line(int(x0), int(y0+20), int(x0), int(y0-20), color=(255, 0, 0), thickness=1) # 如果当前测量位置存在，绘制线段和标记 if x1 is not None and y1 is not None: # 绘制从基准点到当前点的线段 img.draw_line(int(x0), int(y0), int(x1), int(y1), color=(255, 0, 0), thickness=1) # 在当前点上方画一条短竖线 img.draw_line(int(x1), int(y1+20), int(x1), int(y1-20), color=(255, 0, 0), thickness=1) img.draw_line(int(x1-20), int(y1-20), int(x1), int(y1), color=(255, 0, 0), thickness=1) img.draw_line(int(x1-20), int(y1+20), int(x1), int(y1), color=(255, 0, 0), thickness=1) # 显示系统状态 states = ["SETTING_BASE","MONITORING", "MEASURING"] img.draw_string(0, 430, "State:%s" % states[system_state], color=(255, 255, 255)) img.draw_string(0, 410, "FPS:%.1f" % clock.fps(), color=(255, 255, 255)) # 显示基准位置 if system_state == MEASURING and base_position is not None: base_text = "Base:(%.1f,%.1f)" % base_position img.draw_string(0, 460, base_text, color=(255, 255, 0)) ​​利用此段程序计算标签移动距离，标签运动时会产生俯仰角和偏航角导致像素宽度或高度出现偏移，利用矩阵进行校正透视投影方法减小误差