3.6.1-霍夫變換檢測圓形 STM32串口通信 openmv+STM32串口通信

**非常詳細的視頻和文字教程，講解常見的openmv教程包括 巡線、物體識別、圓環(huán)識別、閾值自動獲取等。非常適合學(xué)習(xí)openmv、K210、K230等項目
視頻合集鏈接在

3.6.1-霍夫變換檢測圓形

圓形檢測和卡爾曼濾波

# 導(dǎo)入硬件操作庫和數(shù)學(xué)計算庫
import sensor, image, time   # OpenMV攝像頭操作庫
from ulab import numpy as np  # 嵌入式版numpy庫

"""
====================== 全局參數(shù)配置 ======================
"""
# 霍夫圓檢測參數(shù)
HOUGH_THRESHOLD = 2000   # 邊緣檢測敏感度（2000-4000典型值，值越大需要更明顯的邊緣）
MIN_RADIUS = 10          # 目標最小識別半徑（根據(jù)實際目標尺寸調(diào)整）
MAX_RADIUS = 50          # 目標最大識別半徑（建議設(shè)為畫面寬度的1/4）

# 卡爾曼濾波參數(shù)
TS = 1/60                # 時間步長（基于60FPS幀率計算）

"""
====================== 攝像頭初始化 ======================
"""
sensor.reset()                      # 復(fù)位攝像頭硬件
sensor.set_pixformat(sensor.RGB565) # 設(shè)置像素格式為RGB565（每個像素16位）
sensor.set_framesize(sensor.QQVGA)  # 設(shè)置分辨率160x120（平衡處理速度與精度）
sensor.set_vflip(True)              # 垂直鏡像（根據(jù)攝像頭安裝方向調(diào)整）
sensor.set_hmirror(True)            # 水平鏡像
sensor.set_auto_gain(False)         # 關(guān)閉自動增益（防止亮度突變影響檢測）
sensor.set_auto_whitebal(False)     # 關(guān)閉自動白平衡（保持色彩一致性）
clock = time.clock()                # 創(chuàng)建幀率計時器

"""
====================== 卡爾曼濾波器實現(xiàn) ======================
"""
class KalmanFilter:
    def __init__(self, initial_state):
        """
        卡爾曼濾波器初始化
        :param initial_state: 初始狀態(tài)向量 [x, y, w, h, dx, dy]
        """
        # 狀態(tài)轉(zhuǎn)移矩陣（描述物理運動模型）
        self.A = np.array([
            [1, 0, 0, 0, TS, 0],  # x位置 = 前次x + 速度x*Δt
            [0, 1, 0, 0, 0, TS],  # y位置 = 前次y + 速度y*Δt 
            [0, 0, 1, 0, 0, 0],   # 寬度保持不變（假設(shè)目標尺寸穩(wěn)定）
            [0, 0, 0, 1, 0, 0],   # 高度保持不變
            [0, 0, 0, 0, 1, 0],   # x方向速度保持不變（勻速模型）
            [0, 0, 0, 0, 0, 1]    # y方向速度保持不變
        ])
        
        self.C = np.eye(6)         # 觀測矩陣（直接觀測位置和尺寸）
        self.Q = np.diag([1e-6]*6) # 過程噪聲（系統(tǒng)不確定性，值越小濾波越敏感）
        self.R = np.diag([1e-6]*6) # 觀測噪聲（測量誤差，值越小越信任傳感器數(shù)據(jù)）
        
        # 初始化狀態(tài)
        self.x_hat = initial_state  # 狀態(tài)估計向量 [x, y, w, h, dx, dy]
        self.p = np.diag([10]*6)    # 誤差協(xié)方差矩陣（初始不確定性較大）

    def update(self, Z):
        """ 
        卡爾曼濾波更新步驟
        :param Z: 觀測值向量 [x, y, w, h, dx, dy]
        :return: 更新后的狀態(tài)估計
        """
        # ---------- 預(yù)測階段 ----------
        x_hat_minus = np.dot(self.A, self.x_hat)            # 狀態(tài)預(yù)測
        p_minus = np.dot(self.A, np.dot(self.p, self.A.T)) + self.Q  # 協(xié)方差預(yù)測

        # ---------- 更新階段 ----------
        S = np.dot(self.C, np.dot(p_minus, self.C.T)) + self.R  # 新息協(xié)方差
        S_inv = np.linalg.inv(S + 1e-4*np.eye(6))          # 正則化逆矩陣（防止奇異）
        K = np.dot(np.dot(p_minus, self.C.T), S_inv)       # 卡爾曼增益計算
        innovation = Z - np.dot(self.C, x_hat_minus)       # 測量殘差
        self.x_hat = x_hat_minus + np.dot(K, innovation)   # 狀態(tài)修正
        self.p = np.dot((np.eye(6) - np.dot(K, self.C)), p_minus)  # 協(xié)方差更新
        return self.x_hat

# 初始化卡爾曼濾波器（初始狀態(tài)：[中心x, 中心y, 寬度, 高度, x速度, y速度]）
kf = KalmanFilter(np.array([80, 60, 30, 30, 2, 2]))  # 初始位置設(shè)為畫面中心

"""
====================== 主處理循環(huán) ======================
"""
while True:
    clock.tick()  # 開始幀計時
    img = sensor.snapshot().lens_corr(1.8)  # 捕獲圖像并進行鏡頭畸變校正（1.8為校正強度）
    # ===== 在畫幅中心繪制小圓環(huán)標志 =====
    img.draw_circle(80, 60, 5, color=(0, 0, 0), thickness=1)  # 黑色小圓環(huán)，半徑5像素
    
    # ===== 階段1：全局圓形檢測 =====
    circles = img.find_circles(
        threshold=HOUGH_THRESHOLD,  # 邊緣檢測閾值
        x_margin=10,    # 圓心x坐標允許誤差（像素）
        y_margin=10,    # 圓心y坐標允許誤差
        r_margin=10,    # 半徑允許誤差
        r_min=MIN_RADIUS,  # 物理約束最小半徑
        r_max=MAX_RADIUS   # 物理約束最大半徑
    )

    if circles:
        # ===== 階段2：有效圓篩選 =====
        valid_circles = []
        img_center_x = img.width() // 2   # 圖像中心x坐標（QQVGA為80）
        img_center_y = img.height() // 2  # 圖像中心y坐標（QQVGA為60）
        
        # 篩選位于畫面中心區(qū)域(±30像素)的圓
        for c in circles:
            dx = abs(c.x() - img_center_x)
            dy = abs(c.y() - img_center_y)
            if dx < 30 and dy < 30:  # 排除邊緣誤檢
                valid_circles.append(c)
        
        if valid_circles:
            # ===== 階段3：選擇最優(yōu)目標 =====
            target = max(valid_circles, key=lambda c: c.r())  # 優(yōu)先大尺寸目標
            x, y, r = target.x(), target.y(), target.r()
            
            # 繪制檢測結(jié)果（綠色實線圓）
            img.draw_circle(x, y, r, color=(0,255,0), thickness=2)
            
            # ===== 階段4：卡爾曼濾波更新 =====
            # 構(gòu)造觀測向量（假設(shè)瞬時速度為零）
            Z = np.array([x, y, 2*r, 2*r, 0, 0])  
            state = kf.update(Z)  # 執(zhí)行卡爾曼濾波
            
            # ===== 階段5：預(yù)測結(jié)果處理 =====
            pred_x = int(state[0])      # 預(yù)測x坐標（像素）
            pred_y = int(state[1])      # 預(yù)測y坐標
            pred_r = int(state[2]/2)    # 預(yù)測半徑（寬度/2）
            
            # 打印坐標信息（新增輸出）
            print("卡爾曼預(yù)測坐標: X={}, Y={}, R={}".format(pred_x, pred_y, pred_r))
            
            # 可視化預(yù)測結(jié)果
            img.draw_cross(pred_x, pred_y, color=(255,0,0), size=10)  # 紅色十字標記
            img.draw_circle(pred_x, pred_y, pred_r, color=(255,255,0))  # 黃色虛線預(yù)測圓

    # 輸出性能指標
    print("幀率: {:.1f}FPS".format(clock.fps()))  # 保留一位小數(shù)