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 439af03..fdc1134 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" @@ -280,3 +280,61 @@ ・src/pc/steering/pursuit_control.py: PursuitControl クラス ・src/pc/gui/main_window.py: 制御手法の切替 UI + + +8. Theil-Sen PD 制御 (Theil-Sen PD Control) +------------------------------------------------------------------------ + + 8-1. 概要 + + 行中心点に Theil-Sen 直線近似を適用し，画像下端での位置偏差と + 直線の傾きから PD 制御で操舵量を算出する方式． + 2点パシュートの Theil-Sen フィッティングと PD 制御の微分項を + 組み合わせたハイブリッド手法である． + GUI で PD 制御・2点パシュートと切り替えて使用可能． + + 8-2. 特徴 + + ・Theil-Sen 推定による外れ値耐性: 行中心点の外れ値に強い + ・傾き（slope）を直接利用: 多項式の微分計算が不要 + ・D 項による振動抑制: 偏差の変化率を見て急な操舵変化を抑制 + ・傾きベースの速度制御: 直線の傾きでカーブ度合いを判定 + + 8-3. アルゴリズム + + (1) 二値画像の最大連結領域を抽出する + (2) 各行の左右端から中心 x 座標（row_centers）を求める + (3) 有効な行の (y, x) 座標に Theil-Sen 直線近似を適用する + x = slope × y + intercept + (4) 画像下端での位置偏差を算出する + bottom_x = slope × (画像高さ - 1) + intercept + position_error = (画像中心x - bottom_x) / 画像中心x + (5) P 項 + Heading 項で操舵量の基礎値を算出する + error = Kp × position_error + Kh × slope + (6) D 項（微分項）を加算する + derivative = (error - error_prev) / dt + steer = error + Kd × derivative + (7) レートリミッターで急な操舵変化を制限する + (8) 速度を算出する + throttle = max_throttle - speed_k × |slope| + + 8-4. 2点パシュートとの関係 + + 2点パシュートの操舵式 K_near × near_err + K_far × far_err は， + Theil-Sen 直線上の 2 点から計算されるため， + Kp × position_error + Kh × slope と数学的に等価である． + Theil-Sen PD はこれに D 項（微分項）を追加した拡張である． + + 8-5. パラメータ一覧（GUI で調整可能） + + ・kp（デフォルト: 0.5）: 位置偏差ゲイン + ・kh（デフォルト: 0.3）: 傾き（Theil-Sen slope）ゲイン + ・kd（デフォルト: 0.1）: 微分ゲイン + ・max_steer_rate（デフォルト: 0.1）: 1フレームあたりの最大操舵変化量 + ・max_throttle（デフォルト: 0.4）: 直線での最大速度 + ・speed_k（デフォルト: 2.0）: 傾きベースの減速係数 + + 8-6. 実装ファイル + + ・src/pc/steering/ts_pd_control.py: TsPdControl クラス + ・src/pc/gui/main_window.py: 制御手法の切替 UI diff --git "a/docs/04_ENV/ENV_04_\343\203\207\343\202\243\343\203\254\343\202\257\343\203\210\343\203\252\346\247\213\346\210\220.txt" "b/docs/04_ENV/ENV_04_\343\203\207\343\202\243\343\203\254\343\202\257\343\203\210\343\203\252\346\247\213\346\210\220.txt" index 9f72519..178535d 100644 --- "a/docs/04_ENV/ENV_04_\343\203\207\343\202\243\343\203\254\343\202\257\343\203\210\343\203\252\346\247\213\346\210\220.txt" +++ "b/docs/04_ENV/ENV_04_\343\203\207\343\202\243\343\203\254\343\202\257\343\203\210\343\203\252\346\247\213\346\210\220.txt" @@ -58,6 +58,7 @@ │ ├── base.py 共通インターフェース │ ├── pd_control.py PD 制御の実装 │ ├── pursuit_control.py 2点パシュート制御の実装 + │ ├── ts_pd_control.py Theil-Sen PD 制御の実装 │ ├── param_store.py プリセット保存・読み込み │ └── auto_params.py パラメータ自動保存・復元 └── vision/ 画像処理 diff --git a/src/pc/gui/main_window.py b/src/pc/gui/main_window.py index 5c19934..a822003 100644 --- a/src/pc/gui/main_window.py +++ b/src/pc/gui/main_window.py @@ -30,9 +30,11 @@ load_detect_params, load_overlay, load_pursuit, + load_ts_pd, save_control, save_overlay, save_pursuit, + save_ts_pd, ) from pc.steering.base import SteeringBase from pc.steering.pd_control import PdControl, PdParams @@ -40,6 +42,10 @@ PursuitControl, PursuitParams, ) +from pc.steering.ts_pd_control import ( + TsPdControl, + TsPdParams, +) from pc.vision.fitting import theil_sen_fit from pc.vision.line_detector import ( ImageParams, @@ -92,8 +98,13 @@ params=pursuit_params, image_params=image_params, ) + ts_pd_params = load_ts_pd() + self._ts_pd_control = TsPdControl( + params=ts_pd_params, + image_params=image_params, + ) - # 現在の制御手法（"pd" or "pursuit"） + # 現在の制御手法（"pd", "pursuit", "ts_pd"） self._steering_method: str = last_steering # 最新フレームの保持（自動操縦で使用） @@ -205,6 +216,7 @@ self._control_panel = ControlParamPanel( self._pd_control.params, self._pursuit_control.params, + self._ts_pd_control.params, self._steering_method, ) self._control_panel.pd_params_changed.connect( @@ -213,6 +225,9 @@ self._control_panel.pursuit_params_changed.connect( self._on_pursuit_params_changed, ) + self._control_panel.ts_pd_params_changed.connect( + self._on_ts_pd_params_changed, + ) self._control_panel.steering_method_changed.connect( self._on_steering_method_changed, ) @@ -245,6 +260,8 @@ """現在選択中の制御クラスを返す""" if self._steering_method == "pursuit": return self._pursuit_control + if self._steering_method == "ts_pd": + return self._ts_pd_control return self._pd_control def _setup_timers(self) -> None: @@ -262,9 +279,10 @@ def _on_image_params_changed( self, ip: ImageParams, ) -> None: - """画像処理パラメータの変更を両制御クラスに反映する""" + """画像処理パラメータの変更を全制御クラスに反映する""" self._pd_control.image_params = ip self._pursuit_control.image_params = ip + self._ts_pd_control.image_params = ip def _on_method_changed(self, method: str) -> None: """検出手法の変更に合わせて制御設定を保存する""" @@ -288,6 +306,13 @@ self._pursuit_control.params = p save_pursuit(p) + def _on_ts_pd_params_changed( + self, p: TsPdParams, + ) -> None: + """Theil-Sen PD パラメータの変更を反映して保存する""" + self._ts_pd_control.params = p + save_ts_pd(p) + def _on_overlay_flags_changed(self) -> None: """オーバーレイフラグの変更を保存する""" save_overlay(self._overlay_panel.get_flags()) @@ -443,6 +468,49 @@ far_x = slope * far_y + intercept return ((near_x, near_y), (far_x, far_y)) + def _calc_ts_pd_line_preview( + self, + ) -> ( + tuple[tuple[float, float], tuple[float, float]] + | None + ): + """Theil-Sen PD の近似直線を表示用に算出する + + 直線の上端と下端の 2 点を返す + + Returns: + ((bottom_x, bottom_y), (top_x, top_y)) または None + """ + if self._is_auto: + fit = self._ts_pd_control.last_fit_line + if fit is None: + return None + slope, intercept = fit + r = self._last_detect_result + if r is None or r.row_centers is None: + return None + h = len(r.row_centers) + else: + r = self._last_detect_result + if r is None or not r.detected: + return None + if r.row_centers is None: + return None + centers = r.row_centers + valid = ~np.isnan(centers) + ys = np.where(valid)[0].astype(float) + xs = centers[valid] + if len(ys) < 2: + return None + slope, intercept = theil_sen_fit(ys, xs) + h = len(centers) + + bottom_y = float(h - 1) + top_y = 0.0 + bottom_x = slope * bottom_y + intercept + top_x = slope * top_y + intercept + return ((bottom_x, bottom_y), (top_x, top_y)) + def _display_frame(self, frame: np.ndarray) -> None: """NumPy 配列の画像を QLabel に表示する @@ -464,6 +532,10 @@ pursuit_pts = ( self._calc_pursuit_points_preview() ) + elif self._steering_method == "ts_pd": + pursuit_pts = ( + self._calc_ts_pd_line_preview() + ) bgr = draw_overlay( bgr, self._last_detect_result, self._overlay_panel.get_flags(), diff --git a/src/pc/gui/panels/collapsible_group_box.py b/src/pc/gui/panels/collapsible_group_box.py index d465e6a..2b9e026 100644 --- a/src/pc/gui/panels/collapsible_group_box.py +++ b/src/pc/gui/panels/collapsible_group_box.py @@ -35,6 +35,16 @@ if layout is None: return _set_layout_visible(layout, on) + if on: + self._on_expanded() + + + def _on_expanded(self) -> None: + """展開後のフックポイント + + サブクラスでオーバーライドし，展開後に + ウィジェットの表示状態を再適用する + """ def _set_layout_visible( diff --git a/src/pc/gui/panels/control_param_panel.py b/src/pc/gui/panels/control_param_panel.py index 9c824a0..bddedfe 100644 --- a/src/pc/gui/panels/control_param_panel.py +++ b/src/pc/gui/panels/control_param_panel.py @@ -1,6 +1,6 @@ """ control_param_panel -PD / 2点パシュート制御パラメータ調整 UI パネル +PD / 2点パシュート / Theil-Sen PD 制御パラメータ調整 UI パネル """ from PySide6.QtCore import Signal @@ -24,36 +24,46 @@ from pc.gui.panels.image_param_panel import _create_preset_ui from pc.steering.param_store import ( PdPreset, + TsPdPreset, add_pd_preset, + add_ts_pd_preset, delete_pd_preset, + delete_ts_pd_preset, load_pd_presets, + load_ts_pd_presets, ) from pc.steering.pd_control import PdParams from pc.steering.pursuit_control import PursuitParams +from pc.steering.ts_pd_control import TsPdParams class ControlParamPanel(CollapsibleGroupBox): - """PD / 2点パシュート制御パラメータ調整 UI""" + """PD / 2点パシュート / Theil-Sen PD 制御パラメータ調整 UI""" # PD パラメータが変更されたときに emit する pd_params_changed = Signal(object) # Pursuit パラメータが変更されたときに emit する pursuit_params_changed = Signal(object) - # 制御手法が変更されたときに emit する（"pd" or "pursuit"） + # Theil-Sen PD パラメータが変更されたときに emit する + ts_pd_params_changed = Signal(object) + # 制御手法が変更されたときに emit する steering_method_changed = Signal(str) def __init__( self, pd_params: PdParams, pursuit_params: PursuitParams, + ts_pd_params: TsPdParams, steering_method: str = "pd", ) -> None: super().__init__("制御パラメータ") self._pd_params = pd_params self._pursuit_params = pursuit_params + self._ts_pd_params = ts_pd_params self._initial_steering_method = steering_method self._auto_save_enabled = False self._pd_presets: list[PdPreset] = [] + self._ts_pd_presets: list[TsPdPreset] = [] self._setup_ui() self._auto_save_enabled = True @@ -66,6 +76,14 @@ """現在の Pursuit パラメータを返す""" return self._pursuit_params + def get_ts_pd_params(self) -> TsPdParams: + """現在の Theil-Sen PD パラメータを返す""" + return self._ts_pd_params + + def _on_expanded(self) -> None: + """展開後に制御手法に応じた表示切替を再適用する""" + self._on_steering_method_changed() + def _setup_ui(self) -> None: """UI を構築する""" layout = QVBoxLayout() @@ -77,6 +95,9 @@ self._steering_combo.addItem( "2点パシュート", "pursuit", ) + self._steering_combo.addItem( + "Theil-Sen PD", "ts_pd", + ) idx = self._steering_combo.findData( self._initial_steering_method, ) @@ -193,16 +214,100 @@ self._spin_pursuit_speed_k, ] - # --- プリセット管理 --- + # --- Theil-Sen PD パラメータ --- + self._ts_pd_param_form = QFormLayout() + layout.addLayout(self._ts_pd_param_form) + + tp = self._ts_pd_params + + self._spin_ts_kp = _create_spin( + tp.kp, 0.0, 0.05, + ) + self._ts_pd_param_form.addRow( + "Kp (位置):", self._spin_ts_kp, + ) + + self._spin_ts_kh = _create_spin( + tp.kh, 0.0, 0.05, + ) + self._ts_pd_param_form.addRow( + "Kh (傾き):", self._spin_ts_kh, + ) + + self._spin_ts_kd = _create_spin( + tp.kd, 0.0, 0.05, + ) + self._ts_pd_param_form.addRow( + "Kd (微分):", self._spin_ts_kd, + ) + + self._spin_ts_steer_rate = _create_spin( + tp.max_steer_rate, 0.01, 0.01, + ) + self._ts_pd_param_form.addRow( + "操舵制限:", self._spin_ts_steer_rate, + ) + + self._spin_ts_throttle = _create_spin( + tp.max_throttle, 0.0, 0.05, + ) + self._ts_pd_param_form.addRow( + "最大速度:", self._spin_ts_throttle, + ) + + self._spin_ts_speed_k = _create_spin( + tp.speed_k, 0.0, 0.1, + ) + self._ts_pd_param_form.addRow( + "減速係数:", self._spin_ts_speed_k, + ) + + # Theil-Sen PD 固有ウィジェットリスト（表示切替用） + self._ts_pd_widgets: list[QWidget] = [ + self._spin_ts_kp, + self._spin_ts_kh, + self._spin_ts_kd, + self._spin_ts_steer_rate, + self._spin_ts_throttle, + self._spin_ts_speed_k, + ] + + # --- プリセット管理（PD 制御）--- + self._pd_preset_container = QWidget() + pd_preset_layout = QVBoxLayout( + self._pd_preset_container, + ) + pd_preset_layout.setContentsMargins(0, 0, 0, 0) self._pd_preset_combo, self._pd_preset_memo = ( _create_preset_ui( - layout, + pd_preset_layout, self._on_load_pd_preset, self._on_save_pd_preset, self._on_delete_pd_preset, self._on_pd_preset_selected, ) ) + layout.addWidget(self._pd_preset_container) + + # --- プリセット管理（Theil-Sen PD）--- + self._ts_pd_preset_container = QWidget() + ts_pd_preset_layout = QVBoxLayout( + self._ts_pd_preset_container, + ) + ts_pd_preset_layout.setContentsMargins( + 0, 0, 0, 0, + ) + ( + self._ts_pd_preset_combo, + self._ts_pd_preset_memo, + ) = _create_preset_ui( + ts_pd_preset_layout, + self._on_load_ts_pd_preset, + self._on_save_ts_pd_preset, + self._on_delete_ts_pd_preset, + self._on_ts_pd_preset_selected, + ) + layout.addWidget(self._ts_pd_preset_container) # コールバック接続 self._steering_combo.currentIndexChanged.connect( @@ -218,8 +323,13 @@ spin.valueChanged.connect( self._on_pursuit_changed, ) + for spin in self._ts_pd_widgets: + spin.valueChanged.connect( + self._on_ts_pd_changed, + ) self._refresh_pd_presets() + self._refresh_ts_pd_presets() # 初期表示の更新 self._on_steering_method_changed() @@ -255,10 +365,27 @@ if self._auto_save_enabled: self.pursuit_params_changed.emit(p) + def _on_ts_pd_changed(self) -> None: + """Theil-Sen PD パラメータの変更を反映する""" + p = self._ts_pd_params + p.kp = self._spin_ts_kp.value() + p.kh = self._spin_ts_kh.value() + p.kd = self._spin_ts_kd.value() + p.max_steer_rate = ( + self._spin_ts_steer_rate.value() + ) + p.max_throttle = self._spin_ts_throttle.value() + p.speed_k = self._spin_ts_speed_k.value() + + if self._auto_save_enabled: + self.ts_pd_params_changed.emit(p) + def _on_steering_method_changed(self) -> None: """制御手法の変更を反映する""" method = self._steering_combo.currentData() is_pd = method == "pd" + is_pursuit = method == "pursuit" + is_ts_pd = method == "ts_pd" # PD 固有ウィジェットの表示切替 for w in self._pd_widgets: @@ -267,7 +394,7 @@ if label: label.setVisible(is_pd) - # 共通ウィジェット（操舵制限/最大速度/減速係数） + # PD 共通ウィジェット（操舵制限/最大速度/減速係数） for w in [ self._spin_max_steer_rate, self._spin_max_throttle, @@ -280,12 +407,25 @@ # Pursuit ウィジェットの表示切替 for w in self._pursuit_widgets: - w.setVisible(not is_pd) + w.setVisible(is_pursuit) label = ( self._pursuit_param_form.labelForField(w) ) if label: - label.setVisible(not is_pd) + label.setVisible(is_pursuit) + + # Theil-Sen PD ウィジェットの表示切替 + for w in self._ts_pd_widgets: + w.setVisible(is_ts_pd) + label = ( + self._ts_pd_param_form.labelForField(w) + ) + if label: + label.setVisible(is_ts_pd) + + # プリセット UI の表示切替 + self._pd_preset_container.setVisible(is_pd) + self._ts_pd_preset_container.setVisible(is_ts_pd) if self._auto_save_enabled: self.steering_method_changed.emit(method) @@ -376,6 +516,94 @@ delete_pd_preset(idx) self._refresh_pd_presets() + # ── Theil-Sen PD プリセット管理 ───────────────────── + + def _refresh_ts_pd_presets(self) -> None: + """Theil-Sen PD プリセット一覧を更新する""" + self._ts_pd_presets = load_ts_pd_presets() + self._ts_pd_preset_combo.clear() + for p in self._ts_pd_presets: + self._ts_pd_preset_combo.addItem(p.title) + self._ts_pd_preset_memo.setText("") + + def _on_ts_pd_preset_selected(self) -> None: + """Theil-Sen PD プリセット選択時にメモを表示する""" + idx = self._ts_pd_preset_combo.currentIndex() + if 0 <= idx < len(self._ts_pd_presets): + self._ts_pd_preset_memo.setText( + self._ts_pd_presets[idx].memo, + ) + else: + self._ts_pd_preset_memo.setText("") + + def _on_load_ts_pd_preset(self) -> None: + """Theil-Sen PD プリセットを読み込む""" + idx = self._ts_pd_preset_combo.currentIndex() + if idx < 0 or idx >= len(self._ts_pd_presets): + return + self._auto_save_enabled = False + try: + p = self._ts_pd_presets[idx].params + self._spin_ts_kp.setValue(p.kp) + self._spin_ts_kh.setValue(p.kh) + self._spin_ts_kd.setValue(p.kd) + self._spin_ts_steer_rate.setValue( + p.max_steer_rate, + ) + self._spin_ts_throttle.setValue( + p.max_throttle, + ) + self._spin_ts_speed_k.setValue(p.speed_k) + self._ts_pd_params = p + finally: + self._auto_save_enabled = True + self.ts_pd_params_changed.emit(p) + + def _on_save_ts_pd_preset(self) -> None: + """Theil-Sen PD プリセットを保存する""" + title, ok = QInputDialog.getText( + self, "TS-PD プリセット保存", "タイトル:", + ) + if not ok or not title.strip(): + return + memo, ok = QInputDialog.getText( + self, "TS-PD プリセット保存", "メモ:", + ) + if not ok: + return + p = self._ts_pd_params + add_ts_pd_preset(TsPdPreset( + title=title.strip(), + memo=memo.strip(), + params=TsPdParams( + kp=p.kp, kh=p.kh, kd=p.kd, + max_steer_rate=p.max_steer_rate, + max_throttle=p.max_throttle, + speed_k=p.speed_k, + ), + )) + self._refresh_ts_pd_presets() + self._ts_pd_preset_combo.setCurrentIndex( + self._ts_pd_preset_combo.count() - 1, + ) + + def _on_delete_ts_pd_preset(self) -> None: + """Theil-Sen PD プリセットを削除する""" + idx = self._ts_pd_preset_combo.currentIndex() + if idx < 0 or idx >= len(self._ts_pd_presets): + return + title = self._ts_pd_presets[idx].title + reply = QMessageBox.question( + self, "削除確認", + f"「{title}」を削除しますか？", + QMessageBox.StandardButton.Yes + | QMessageBox.StandardButton.No, + QMessageBox.StandardButton.No, + ) + if reply == QMessageBox.StandardButton.Yes: + delete_ts_pd_preset(idx) + self._refresh_ts_pd_presets() + def _create_spin( value: float, min_val: float, step: float, diff --git a/src/pc/steering/auto_params.py b/src/pc/steering/auto_params.py index d4f666d..5ee974b 100644 --- a/src/pc/steering/auto_params.py +++ b/src/pc/steering/auto_params.py @@ -7,6 +7,7 @@ params/ ├── control.json PD 制御 + 最後に使用した手法 ├── pursuit.json パシュート制御パラメータ + ├── ts_pd.json Theil-Sen PD 制御パラメータ ├── overlay.json オーバーレイ表示フラグ ├── detect_current.json 現行手法の画像処理パラメータ ├── detect_blackhat.json 案A の画像処理パラメータ @@ -19,6 +20,7 @@ from common.json_utils import PARAMS_DIR, read_json, write_json from pc.steering.pd_control import PdParams from pc.steering.pursuit_control import PursuitParams +from pc.steering.ts_pd_control import TsPdParams from pc.vision.line_detector import ImageParams from pc.vision.overlay import OverlayFlags @@ -28,6 +30,9 @@ # パシュート制御パラメータファイル _PURSUIT_FILE = PARAMS_DIR / "pursuit.json" +# Theil-Sen PD 制御パラメータファイル +_TS_PD_FILE = PARAMS_DIR / "ts_pd.json" + # オーバーレイ表示フラグファイル _OVERLAY_FILE = PARAMS_DIR / "overlay.json" @@ -108,6 +113,33 @@ return PursuitParams(**filtered) +def save_ts_pd(params: TsPdParams) -> None: + """Theil-Sen PD 制御パラメータを保存する + + Args: + params: Theil-Sen PD 制御パラメータ + """ + write_json(_TS_PD_FILE, asdict(params)) + + +def load_ts_pd() -> TsPdParams: + """Theil-Sen PD 制御パラメータを読み込む + + Returns: + Theil-Sen PD 制御パラメータ（ファイルがない場合はデフォルト） + """ + if not _TS_PD_FILE.exists(): + return TsPdParams() + + data = read_json(_TS_PD_FILE) + known = TsPdParams.__dataclass_fields__ + filtered = { + k: v for k, v in data.items() + if k in known + } + return TsPdParams(**filtered) + + def save_overlay(flags: OverlayFlags) -> None: """オーバーレイ表示フラグを保存する diff --git a/src/pc/steering/param_store.py b/src/pc/steering/param_store.py index 5a9cc58..8c88b98 100644 --- a/src/pc/steering/param_store.py +++ b/src/pc/steering/param_store.py @@ -1,16 +1,18 @@ """ param_store パラメータプリセットの保存・読み込みを管理するモジュール -画像処理パラメータと PD 制御パラメータを独立して管理する +画像処理・PD 制御・Theil-Sen PD 制御パラメータを独立して管理する """ from dataclasses import asdict, dataclass from common.json_utils import PARAMS_DIR, read_json, write_json from pc.steering.pd_control import PdParams +from pc.steering.ts_pd_control import TsPdParams from pc.vision.line_detector import ImageParams _PD_FILE = PARAMS_DIR / "presets_pd.json" +_TS_PD_FILE = PARAMS_DIR / "presets_ts_pd.json" _IMAGE_FILE = PARAMS_DIR / "presets_image.json" @@ -58,6 +60,51 @@ ) +# ── Theil-Sen PD 制御プリセット ──────────────── + + +@dataclass +class TsPdPreset: + """Theil-Sen PD 制御パラメータのプリセット + + Attributes: + title: プリセットのタイトル + memo: メモ + params: Theil-Sen PD 制御パラメータ + """ + + title: str + memo: str + params: TsPdParams + + +def load_ts_pd_presets() -> list[TsPdPreset]: + """Theil-Sen PD 制御プリセット一覧を読み込む""" + return _load_presets( + _TS_PD_FILE, TsPdPreset, + "params", TsPdParams, + ) + + +def add_ts_pd_preset(preset: TsPdPreset) -> None: + """Theil-Sen PD 制御プリセットを追加する""" + presets = load_ts_pd_presets() + presets.append(preset) + _save_presets( + _TS_PD_FILE, presets, "params", + ) + + +def delete_ts_pd_preset(index: int) -> None: + """Theil-Sen PD 制御プリセットを削除する""" + presets = load_ts_pd_presets() + if 0 <= index < len(presets): + presets.pop(index) + _save_presets( + _TS_PD_FILE, presets, "params", + ) + + # ── 画像処理プリセット ──────────────────────── diff --git a/src/pc/steering/ts_pd_control.py b/src/pc/steering/ts_pd_control.py new file mode 100644 index 0000000..e940694 --- /dev/null +++ b/src/pc/steering/ts_pd_control.py @@ -0,0 +1,177 @@ +""" +ts_pd_control +Theil-Sen 直線近似による PD 制御モジュール +行中心点に Theil-Sen 直線をフィッティングし， +位置偏差・傾き・微分項から操舵量を算出する +""" + +import time +from dataclasses import dataclass + +import numpy as np + +from common import config +from pc.steering.base import SteeringBase, SteeringOutput +from pc.vision.fitting import theil_sen_fit +from pc.vision.line_detector import ( + ImageParams, + detect_line, + reset_valley_tracker, +) + + +@dataclass +class TsPdParams: + """Theil-Sen PD 制御のパラメータ + + Attributes: + kp: 位置偏差ゲイン + kh: 傾き（Theil-Sen slope）ゲイン + kd: 微分ゲイン + max_steer_rate: 1フレームあたりの最大操舵変化量 + max_throttle: 直線での最大速度 + speed_k: 傾きベースの減速係数 + """ + kp: float = 0.5 + kh: float = 0.3 + kd: float = 0.1 + max_steer_rate: float = 0.1 + max_throttle: float = 0.4 + speed_k: float = 2.0 + + +class TsPdControl(SteeringBase): + """Theil-Sen 直線近似による PD 制御クラス + + 行中心点から Theil-Sen 直線近似を行い， + 画像下端での位置偏差と直線の傾きから PD 制御で操舵量を計算する + """ + + def __init__( + self, + params: TsPdParams | None = None, + image_params: ImageParams | None = None, + ) -> None: + self.params: TsPdParams = ( + params or TsPdParams() + ) + self.image_params: ImageParams = ( + image_params or ImageParams() + ) + self._prev_error: float = 0.0 + self._prev_time: float = 0.0 + self._prev_steer: float = 0.0 + self._last_result = None + self._last_fit_line: ( + tuple[float, float] | None + ) = None + + def compute( + self, frame: np.ndarray, + ) -> SteeringOutput: + """カメラ画像から Theil-Sen PD 制御で操舵量を計算する + + Args: + frame: グレースケールのカメラ画像 + + Returns: + 計算された操舵量 + """ + p = self.params + + # 線検出 + result = detect_line(frame, self.image_params) + self._last_result = result + + if not result.detected or result.row_centers is None: + self._last_fit_line = None + return SteeringOutput( + throttle=0.0, steer=0.0, + ) + + centers = result.row_centers + + # 有効な点（NaN でない行）を抽出 + valid = ~np.isnan(centers) + ys = np.where(valid)[0].astype(float) + xs = centers[valid] + + if len(ys) < 2: + self._last_fit_line = None + return SteeringOutput( + throttle=0.0, steer=0.0, + ) + + # Theil-Sen 直線近似: x = slope * y + intercept + slope, intercept = theil_sen_fit(ys, xs) + self._last_fit_line = (slope, intercept) + + center_x = config.FRAME_WIDTH / 2.0 + h = len(centers) + + # 画像下端での位置偏差 + bottom_x = slope * (h - 1) + intercept + position_error = (center_x - bottom_x) / center_x + + # 操舵量: P 項（位置偏差）+ Heading 項（傾き） + error = p.kp * position_error + p.kh * slope + + # 時間差分の計算 + now = time.time() + dt = ( + now - self._prev_time + if self._prev_time > 0 + else 0.033 + ) + dt = max(dt, 0.001) + + # D 項（微分項） + derivative = (error - self._prev_error) / dt + steer = error + p.kd * derivative + + # 操舵量のクランプ + 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 + + # 速度制御（傾きベース: 傾きが大きい → カーブ → 減速） + throttle = p.max_throttle - p.speed_k * abs(slope) + throttle = max(0.0, throttle) + + # 状態の更新 + self._prev_error = error + self._prev_time = now + self._prev_steer = steer + + return SteeringOutput( + throttle=throttle, steer=steer, + ) + + def reset(self) -> None: + """内部状態をリセットする""" + self._prev_error = 0.0 + self._prev_time = 0.0 + self._prev_steer = 0.0 + self._last_result = None + self._last_fit_line = None + reset_valley_tracker() + + @property + def last_detect_result(self): + """直近の線検出結果を取得する""" + return self._last_result + + @property + def last_fit_line( + self, + ) -> tuple[float, float] | None: + """直近の Theil-Sen 直線近似結果を取得する + + Returns: + (slope, intercept) または None + """ + return self._last_fit_line