A
@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))
利用此段程序计算标签移动距离，标签运动时会产生俯仰角和偏航角导致像素宽度或高度出现偏移，利用矩阵进行校正透视投影方法减小误差