diff --git "a/docs/03_TECH/TECH_01_\346\223\215\350\210\265\351\207\217\350\250\210\347\256\227\344\273\225\346\247\230.txt" "b/docs/03_TECH/TECH_01_\346\223\215\350\210\265\351\207\217\350\250\210\347\256\227\344\273\225\346\247\230.txt" index d3d77e5..d462206 100644 --- "a/docs/03_TECH/TECH_01_\346\223\215\350\210\265\351\207\217\350\250\210\347\256\227\344\273\225\346\247\230.txt" +++ "b/docs/03_TECH/TECH_01_\346\223\215\350\210\265\351\207\217\350\250\210\347\256\227\344\273\225\346\247\230.txt" @@ -197,6 +197,7 @@ 3. 案B: 二重正規化型 ・背景ブラーカーネルサイズ（bg_blur_ksize）: 101 + ・固定閾値（global_thresh）: 0（0 で無効，適応的閾値との AND） ・適応的閾値ブロックサイズ（adaptive_block）: 51 ・適応的閾値定数 C（adaptive_c）: 10 ・等方クロージングサイズ（iso_close_size）: 15 @@ -230,3 +231,49 @@ タイトル・メモ付きで JSON ファイルに保存できる． ・GUI のコンボボックスで保存済みパラメータを選択・読み込み可能． ・保存ファイル: pd_params.json（.gitignore に登録済み） + + +7. 2点パシュート制御 (Two-Point Pursuit Control) +------------------------------------------------------------------------ + + 7-1. 概要 + + PD 制御の代替として，多項式曲線上の近い点と遠い点の2箇所を + 目標点として操舵量を計算する方式．微分・曲率の計算が不要で， + パラメータ調整が直感的である．GUI でPD 制御と切り替えて使用可能 + + 7-2. アルゴリズム + + (1) 多項式曲線上の2点の y 座標を決定する + near_y = 画像高さ × near_ratio（手前側） + far_y = 画像高さ × far_ratio（奥側） + (2) 各点の x 座標を多項式から取得する + (3) 各点の偏差を計算する + near_err = (画像中心x - near_x) / 画像中心x + far_err = (画像中心x - far_x) / 画像中心x + (4) 操舵量を算出する + steer = K_near × near_err + K_far × far_err + (5) レートリミッターで急な操舵変化を制限する + (6) 速度を算出する + curve = |near_x - far_x| / 画像中心x + throttle = max_throttle - speed_k × curve + + 7-3. PD 制御との比較 + + ・PD 制御: 画像下端の1点で微分・曲率を計算 → 不安定になりやすい + ・2点パシュート: 2点の位置だけで判断 → 直感的で安定 + + 7-4. パラメータ一覧（GUI で調整可能） + + ・near_ratio（デフォルト: 0.8）: 近い目標点の位置（0.0=上端，1.0=下端） + ・far_ratio（デフォルト: 0.3）: 遠い目標点の位置 + ・k_near（デフォルト: 0.5）: 近い目標点の操舵ゲイン + ・k_far（デフォルト: 0.3）: 遠い目標点の操舵ゲイン + ・max_steer_rate（デフォルト: 0.1）: 1フレームあたりの最大操舵変化量 + ・max_throttle（デフォルト: 0.4）: 直線での最大速度 + ・speed_k（デフォルト: 2.0）: カーブ減速係数 + + 7-5. 実装ファイル + + ・src/pc/steering/pursuit_control.py: PursuitControl クラス + ・src/pc/gui/main_window.py: 制御手法の切替 UI diff --git a/src/pc/steering/auto_params.py b/src/pc/steering/auto_params.py index fb3167d..4376097 100644 --- a/src/pc/steering/auto_params.py +++ b/src/pc/steering/auto_params.py @@ -34,6 +34,7 @@ "blackhat": "detect_blackhat.json", "dual_norm": "detect_dual_norm.json", "robust": "detect_robust.json", + "valley": "detect_valley.json", } diff --git a/src/pc/steering/pursuit_control.py b/src/pc/steering/pursuit_control.py new file mode 100644 index 0000000..a592972 --- /dev/null +++ b/src/pc/steering/pursuit_control.py @@ -0,0 +1,131 @@ +""" +pursuit_control +2点パシュートによる操舵量計算モジュール +多項式曲線上の近距離・遠距離の2点から操舵量と速度を算出する +""" + +from dataclasses import dataclass + +import numpy as np + +from common import config +from pc.steering.base import SteeringBase, SteeringOutput +from pc.vision.line_detector import ( + ImageParams, + detect_line, + reset_valley_tracker, +) + + +@dataclass +class PursuitParams: + """2点パシュート制御のパラメータ + + Attributes: + near_ratio: 近い目標点の位置（0.0=上端，1.0=下端） + far_ratio: 遠い目標点の位置（0.0=上端，1.0=下端） + k_near: 近い目標点の操舵ゲイン + k_far: 遠い目標点の操舵ゲイン + max_steer_rate: 1フレームあたりの最大操舵変化量 + max_throttle: 直線での最大速度 + speed_k: カーブ減速係数（2点の差に対する係数） + """ + near_ratio: float = 0.8 + far_ratio: float = 0.3 + k_near: float = 0.5 + k_far: float = 0.3 + max_steer_rate: float = 0.1 + max_throttle: float = 0.4 + speed_k: float = 2.0 + + +class PursuitControl(SteeringBase): + """2点パシュートによる操舵量計算クラス + + 多項式曲線上の近い点と遠い点の2箇所のx座標偏差から + 操舵量を計算する．微分・曲率の計算が不要で直感的 + """ + + def __init__( + self, + params: PursuitParams | None = None, + image_params: ImageParams | None = None, + ) -> None: + self.params: PursuitParams = ( + params or PursuitParams() + ) + self.image_params: ImageParams = ( + image_params or ImageParams() + ) + self._prev_steer: float = 0.0 + self._last_result = None + + def compute( + self, frame: np.ndarray, + ) -> SteeringOutput: + """カメラ画像から2点パシュートで操舵量を計算する + + Args: + frame: グレースケールのカメラ画像 + + Returns: + 計算された操舵量 + """ + p = self.params + + # 線検出 + result = detect_line(frame, self.image_params) + self._last_result = result + + if not result.detected or result.poly_coeffs is None: + return SteeringOutput( + throttle=0.0, steer=0.0, + ) + + poly = np.poly1d(result.poly_coeffs) + center_x = config.FRAME_WIDTH / 2.0 + h = float(config.FRAME_HEIGHT) + + # 2点の y 座標を計算 + near_y = h * p.near_ratio + far_y = h * p.far_ratio + + # 2点の x 座標を多項式から取得 + near_x = poly(near_y) + far_x = poly(far_y) + + # 各点の偏差（正: 線が左にある → 右に曲がる） + near_err = (center_x - near_x) / center_x + far_err = (center_x - far_x) / center_x + + # 操舵量 + steer = p.k_near * near_err + p.k_far * far_err + steer = max(-1.0, min(1.0, steer)) + + # レートリミッター + delta = steer - self._prev_steer + max_delta = p.max_steer_rate + delta = max(-max_delta, min(max_delta, delta)) + steer = self._prev_steer + delta + + # 速度制御（2点の x 差でカーブ度合いを判定） + curve = abs(near_x - far_x) / center_x + throttle = p.max_throttle - p.speed_k * curve + throttle = max(0.0, throttle) + + self._prev_steer = steer + + return SteeringOutput( + throttle=throttle, steer=steer, + ) + + def reset(self) -> None: + """内部状態をリセットする""" + self._prev_steer = 0.0 + self._last_result = None + reset_valley_tracker() + + @property + def last_detect_result(self): + """直近の線検出結果を取得する""" + return self._last_result diff --git a/src/pc/vision/line_detector.py b/src/pc/vision/line_detector.py index 85d77d4..c7a7efd 100644 --- a/src/pc/vision/line_detector.py +++ b/src/pc/vision/line_detector.py @@ -44,11 +44,15 @@ close_height: クロージングの高さ blackhat_ksize: Black-hat のカーネルサイズ bg_blur_ksize: 背景除算のブラーカーネルサイズ + global_thresh: 固定閾値（0 で無効，適応的閾値との AND） adaptive_block: 適応的閾値のブロックサイズ adaptive_c: 適応的閾値の定数 C iso_close_size: 等方クロージングのカーネルサイズ dist_thresh: 距離変換の閾値 min_line_width: 行ごと中心抽出の最小線幅 + stage_close_small: 段階クロージング第1段のサイズ + stage_min_area: 孤立除去の最小面積（0 で無効） + stage_close_large: 段階クロージング第2段のサイズ（0 で無効） ransac_thresh: RANSAC の外れ値判定閾値 ransac_iter: RANSAC の反復回数 width_near: 画像下端での期待線幅（px，0 で無効） @@ -81,6 +85,7 @@ # 案B: 背景除算 bg_blur_ksize: int = 101 + global_thresh: int = 0 # 固定閾値（0 で無効） # 案B/C: 適応的閾値 adaptive_block: int = 51 @@ -91,6 +96,11 @@ dist_thresh: float = 3.0 min_line_width: int = 3 + # 案B: 段階クロージング + stage_close_small: int = 5 # 第1段: 小クロージングサイズ + stage_min_area: int = 0 # 孤立除去の最小面積（0 で無効） + stage_close_large: int = 0 # 第2段: 大クロージングサイズ（0 で無効） + # 案C: RANSAC ransac_thresh: float = 5.0 ransac_iter: int = 50 @@ -301,10 +311,28 @@ block, params.adaptive_c, ) - # 等方クロージング + 距離変換マスク + 幅フィルタ - binary = _apply_iso_closing( - binary, params.iso_close_size, - ) + # 固定閾値との AND（有効時のみ） + if params.global_thresh > 0: + _, global_mask = cv2.threshold( + normalized, params.global_thresh, + 255, cv2.THRESH_BINARY_INV, + ) + binary = cv2.bitwise_and(binary, global_mask) + + # 段階クロージング or 等方クロージング + if params.stage_min_area > 0: + binary = _apply_staged_closing( + binary, + params.stage_close_small, + params.stage_min_area, + params.stage_close_large, + ) + else: + binary = _apply_iso_closing( + binary, params.iso_close_size, + ) + + # 距離変換マスク + 幅フィルタ binary = _apply_dist_mask( binary, params.dist_thresh, ) @@ -404,6 +432,46 @@ ) +def _apply_staged_closing( + binary: np.ndarray, + small_size: int, + min_area: int, + large_size: int, +) -> np.ndarray: + """段階クロージング: 小穴埋め → 孤立除去 → 大穴埋め + + Args: + binary: 二値画像 + small_size: 第1段クロージングのカーネルサイズ + min_area: 孤立領域除去の最小面積（0 で無効） + large_size: 第2段クロージングのカーネルサイズ（0 で無効） + + Returns: + 処理後の二値画像 + """ + # 第1段: 小さいクロージングで近接ピクセルをつなぐ + result = _apply_iso_closing(binary, small_size) + + # 孤立領域の除去 + if min_area > 0: + contours, _ = cv2.findContours( + result, cv2.RETR_EXTERNAL, + cv2.CHAIN_APPROX_SIMPLE, + ) + mask = np.zeros_like(result) + for cnt in contours: + if cv2.contourArea(cnt) >= min_area: + cv2.drawContours( + mask, [cnt], -1, 255, -1, + ) + result = mask + + # 第2段: 大きいクロージングで中抜けを埋める + result = _apply_iso_closing(result, large_size) + + return result + + def _apply_width_filter( binary: np.ndarray, width_near: int,