======================================================================== 操舵量計算仕様 (Steering Calculation Specification) ======================================================================== 1. 概要 (Overview) ------------------------------------------------------------------------ 1-0. 目的 カメラ画像から黒線の位置を検出し，PD 制御によって操舵量と速度を 算出する方法を定義する． 1-1. 基本方針 ・制御方式: PD 制御（I 項なしで開始） ・線検出: 二値化画像の白ピクセルに多項式フィッティングし， 位置偏差・傾き・曲率を算出する． ・操舵量: 位置偏差と傾きに基づく PD 制御で決定する． ・速度: 曲率に応じて動的に調整する（カーブ減速，直線加速）． ・操舵変化: レートリミッターで急激な操舵変化を抑制する． 2. 画像処理パイプライン (Image Processing Pipeline) ------------------------------------------------------------------------ 2-1. 処理フロー 1. カメラからフレームを取得する． 2. グレースケールに変換する． 3. CLAHE でコントラストを強調する（照明ムラの影響を低減）． 4. ガウシアンブラーを適用する（軽いノイズ除去）． 5. 固定閾値で二値化する（BINARY_INV）． 6. オープニングで孤立ノイズを除去する． 7. 横方向クロージングで反射による途切れを補間する． 8. 検出領域内の白ピクセルに2次多項式をフィッティングする． ■ CLAHE（コントラスト制限付き適応ヒストグラム均等化） 照明ムラや反射により黒線と灰色の床の差が小さくなる問題への 対策として，グレースケール変換後に CLAHE を適用する． 局所的にコントラストを強調し，黒線と背景の分離を改善する． ・clahe_clip: コントラスト増幅の上限（デフォルト: 2.0） ・clahe_grid: 局所領域の分割数（デフォルト: 8） ■ オープニング（収縮→膨張） 影や汚れにより発生する孤立した小さなノイズピクセルを除去する． 小さい楕円カーネルで収縮→膨張を行い，小さな塊を消す． ・open_size: カーネルサイズ（デフォルト: 5） ・効果: 線とつながっていない孤立ピクセルを除去 ■ 横方向クロージング（膨張→収縮） 光の反射により線が途切れる問題への対策として， 二値化後にモルフォロジーのクロージング処理を行う． 横長の楕円カーネルを使用することで，縦方向への影響を抑えつつ 線の途切れを左右から埋める． ・close_width: カーネルの横幅（デフォルト: 25） ・close_height: カーネルの高さ（デフォルト: 3） ・効果: 反射で欠けた部分を，左右の検出済みピクセルからつなぐ 2-2. 多項式フィッティング 検出領域（画像全体）内の白ピクセル座標に対して， 2次多項式 x = f(y) = ay² + by + c をフィッティングする． これにより線全体の形状を1つの式で表現し，以下の情報を得る． ・位置偏差: 画像下端での x 座標と画像中心の差 ・傾き（ヘディング）: 画像下端での dx/dy ・曲率: d²x/dy²（2次多項式では定数 = 2a） 3. 偏差の算出 (Error Calculation) ------------------------------------------------------------------------ 3-1. 位置偏差 多項式を画像下端で評価し，画像中心との差を正規化する． position_error = (image_center_x - f(y_bottom)) / image_center_x ・position_error > 0: 線が画像の左側にある ・position_error < 0: 線が画像の右側にある ・position_error = 0: 線が画像の中心にある 3-2. 傾き（ヘディング） 画像下端での多項式の1次微分値を傾きとする． heading = dx/dy |_(y=y_bottom) ・heading > 0: 先（画像上方）に向かって線が左に曲がっている ・heading < 0: 先に向かって線が右に曲がっている ・効果: カーブの方向を事前に把握し，先読み操舵できる 3-3. 曲率 多項式の2次微分値を曲率とする． curvature = d²x/dy² ・曲率が大きい: 急カーブ ・曲率が小さい: 緩やかなカーブまたは直線 ・用途: 速度制御に使用（急カーブで減速） 4. PD 制御 (PD Control) ------------------------------------------------------------------------ 4-1. 操舵量の計算 error = Kp * position_error + Kh * heading steer = error + Kd * (error - error_prev) / dt ・Kp: 位置偏差ゲイン．偏差に比例した操舵量を出力する． - 大きいほど応答が速いが，振動しやすい． ・Kh: 傾きゲイン．線の傾きに比例した先読み操舵を出力する． - カーブの方向を検知し，事前にステアリングを切る． ・Kd: 微分ゲイン．偏差の変化率に比例した操舵量を出力する． - 振動を抑制し，カーブへの追従を滑らかにする． ・error_prev: 前フレームの error． ・dt: 前フレームからの経過時間． 4-2. レートリミッター 1フレームあたりの操舵量の変化を max_steer_rate 以内に制限する． 感度を高く設定しても急激な操舵変化を防ぎ， コースアウトのリスクを低減する． delta = clamp(steer - steer_prev, -max_steer_rate, max_steer_rate) steer = steer_prev + delta 4-3. 操舵量の制限 計算結果を -1.0 ~ +1.0 の範囲にクランプする． steer = clamp(steer, -1.0, +1.0) 4-4. I 項について 初期段階では I 項を使用しない． 理由: カーブ中に偏差が蓄積し，カーブ出口でオーバーシュート （ワインドアップ）を引き起こすリスクがあるため． 直線走行で定常的にずれ続ける症状が確認された場合にのみ， 小さい Ki で追加を検討する． 5. 速度制御 (Speed Control) ------------------------------------------------------------------------ 5-1. 曲率連動方式 throttle = max_throttle - speed_k * |curvature| ・max_throttle: 直線での最大速度． ・speed_k: 減速係数．曲率が大きいほど減速する． ・|curvature| が大きい → 急カーブ → 減速 ・|curvature| が小さい → 直線 → 加速 多項式フィッティングにより曲率を直接算出できるため， カーブの手前で事前に減速できる． 6. パラメータ一覧 (Parameters) ------------------------------------------------------------------------ ■ 二値化パラメータ（GUI で調整可能） GUI のコンボボックスで検出手法を切り替えられる． 手法ごとに使用するパラメータが異なり， GUI では選択中の手法に関連するパラメータのみ表示される． 詳細は `TECH_04_線検出精度向上方針.txt` を参照する． 1. 現行手法（CLAHE + 固定閾値） ・画像解像度: 320x240 ・CLAHE 強度（clahe_clip）: 2.0 ・CLAHE 分割数（clahe_grid）: 8 ・ブラーカーネルサイズ（blur_size）: 5 ・二値化閾値（binary_thresh）: 80 ・オープニングサイズ（open_size）: 5 ・クロージング横幅（close_width）: 25 ・クロージング高さ（close_height）: 3 2. 案A: Black-hat 中心型 ・Black-hat カーネルサイズ（blackhat_ksize）: 45 ・二値化閾値（binary_thresh）: 80 ・等方クロージングサイズ（iso_close_size）: 15 ・距離変換閾値（dist_thresh）: 3.0 ・最小線幅（min_line_width）: 3 3. 案B: 二重正規化型 ・背景ブラーカーネルサイズ（bg_blur_ksize）: 101 ・固定閾値（global_thresh）: 0（0 で無効，適応的閾値との AND） ・適応的閾値ブロックサイズ（adaptive_block）: 51 ・適応的閾値定数 C（adaptive_c）: 10 ・等方クロージングサイズ（iso_close_size）: 15 ・距離変換閾値（dist_thresh）: 3.0 ・最小線幅（min_line_width）: 3 4. 案C: 最高ロバスト型 ・Black-hat カーネルサイズ（blackhat_ksize）: 45 ・適応的閾値ブロックサイズ（adaptive_block）: 51 ・適応的閾値定数 C（adaptive_c）: 10 ・等方クロージングサイズ（iso_close_size）: 15 ・距離変換閾値（dist_thresh）: 3.0 ・最小線幅（min_line_width）: 3 ・RANSAC 閾値（ransac_thresh）: 5.0 ■ PD 制御パラメータ（GUI で調整可能） ・Kp（位置偏差ゲイン）: 0.5 ・Kh（傾きゲイン）: 0.3 ・Kd（微分ゲイン）: 0.1 ・max_steer_rate（最大操舵変化量）: 0.1 ■ 速度制御パラメータ（GUI で調整可能） ・max_throttle（最大速度）: 0.4 ・speed_k（曲率減速係数）: 0.3 ■ パラメータ管理 ・全パラメータ（画像処理 + PD 制御 + 速度制御）を タイトル・メモ付きで JSON ファイルに保存できる． ・GUI のコンボボックスで保存済みパラメータを選択・読み込み可能． ・保存ファイル: params/ ディレクトリ配下の各 JSON ファイル （control.json，detect_*.json，pursuit.json，ts_pd.json 等） 7. 2点パシュート制御 (Two-Point Pursuit Control) ------------------------------------------------------------------------ 7-1. 概要 PD 制御の代替として，多項式曲線上の近い点と遠い点の2箇所を 目標点として操舵量を計算する方式．微分・曲率の計算が不要で， パラメータ調整が直感的である．GUI でPD 制御と切り替えて使用可能 7-2. アルゴリズム (1) 二値画像の最大連結領域を抽出する (2) 各行の左右端から中心 x 座標（row_centers）を求める (3) 近い行と遠い行の y 座標を決定する near_y = 画像高さ × near_ratio（手前側） far_y = 画像高さ × far_ratio（奥側） (4) row_centers から直接 x 座標を取得する （該当行が NaN の場合は近傍の有効な行を探索） (5) 各点の偏差を計算する 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/common/steering/pursuit_control.py: PursuitControl クラス ・src/common/steering/base.py: SteeringBase（線検出・レートリミッター・reset の共通処理） ・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/common/steering/ts_pd_control.py: TsPdControl クラス ・src/common/steering/base.py: SteeringBase（線検出・レートリミッター・reset の共通処理） ・src/pc/gui/main_window.py: 制御手法の切替 UI 9. コースアウト復帰 (Course-Out Recovery) ------------------------------------------------------------------------ 9-1. 概要 自動操縦中に黒線を一定時間検出できなかった場合に， 最後に検出した方向へ旋回しながら走行して復帰を試みる機能． 全制御手法に共通で適用される． 詳細は `TECH_05_コースアウト復帰仕様.txt` を参照する． 9-2. パラメータ一覧（GUI で調整可能） ・enabled（有効/無効）: True ・timeout_sec（判定時間）: 0.5 秒 ・steer_amount（操舵量）: 0.5 ・throttle（速度）: -0.3（負で後退，正で前進） 9-3. 実装ファイル ・src/common/steering/recovery.py: RecoveryParams，RecoveryController ・src/pi/main.py: Pi 側での復帰ロジックの統合 ・src/pc/gui/panels/recovery_panel.py: RecoveryPanel ・src/pc/gui/main_window.py: 復帰パラメータ管理・Pi への送信