機能追加: Vision+EKFベースの内部ロボット状態推定システムを実装 #902
Merged
Conversation
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
SSL Vision入力に対してExtended Kalman Filter (EKF)を適用した 高精度ロボット状態推定システムを新規実装し、従来の外部トラッカー 依存を削減してマルチセンサー融合による自律的な状態推定を実現。 ## 主要な新機能 ### 新規ファイル - robot_tracker.hpp/cpp: EKFベースロボットトラッカー実装 - RobotTracker: 基本EKFトラッカー（敵ロボット用） - FriendlyRobotTracker: 拡張EKF（味方、オドメトリ統合） - RobotTrackerManager: 複数ロボット管理 - RobotPhysicsModel: 物理制約・状態遷移モデル ### アーキテクチャ変更 - VisionDataProcessor: EKFトラッカー統合、重複防止ロジック追加 - WorldModelDataProvider: 半面コート機能削除、外部トラッカー比較ログ - CMakeLists.txt: Eigenライブラリ依存追加 ## 技術詳細 ### 状態推定方式 - 6次元状態ベクトル: [x, y, theta, vx, vy, omega] - マハラノビス距離による外れ値検出 - 角度正規化とイノベーション計算 - 物理制約（最大速度・角速度制限）適用 ### センサー融合 - SSL Vision: 位置・角度観測 - オドメトリ: バイアス補正付き位置推定（味方のみ） - マウスセンサー: 速度観測（味方のみ） - ロボットコマンド: 予測モデル統合 ### 品質管理 - 追跡信頼度による動的管理 - モーター電流・温度・エラー状態評価 - 古いトラッカー自動削除機能 ## 修正内容 ### 重複トラッカー問題解決 Vision検出で同一ロボットIDに対して複数EKFトラッカーが 作成される問題を解決。チーム色判定により適切な トラッカーのみ生成するよう修正。 ### 座標変換システム簡素化 半面コート練習モード機能を削除し、フルコート座標系に 統一することでコードベース簡素化とバグ削減を実現。 ## 期待効果 - 内部トラッカー精度: 外部トラッカーとの角度差±5度以内 - リアルタイム性能: 60Hz Vision更新に対応 - 自律性向上: 外部トラッカー依存度削減 - 堅牢性: マルチセンサー冗長性による信頼性向上 🤖 Generated with [Claude Code](https://claude.ai/code) Co-Authored-By: Claude <noreply@anthropic.com>
trackerAddressのポート番号を21010から標準の11010に戻して SSL通信設定を正常化。 🤖 Generated with [Claude Code](https://claude.ai/code) Co-Authored-By: Claude <noreply@anthropic.com>
EKFシステムが実戦投入され正常稼働していることを確認し、 Vision データを robot_info_ 配列に直接更新していた レガシー互換性維持コードを削除しました。 変更内容: - vision_data_processor.cpp: 黄・青チーム処理の互換性コード削除 - EKFフィルタリング後データのみ updateRobotInfoWithEKFData() で統合 - データ一貫性向上とメンテナンス性改善 🤖 Generated with [Claude Code](https://claude.ai/code) Co-Authored-By: Claude <noreply@anthropic.com>
互換性コード削除により vision_detected フラグが設定されなくなり、 EKFフィルタリング済みデータが robot_info_ 配列に反映されない 問題を修正しました。 修正内容: - visionDetectionCallback開始時に全ロボットのvision_detectedフラグをリセット - 黄・青チームロボット処理でVision検出時にフラグを適切に設定 - updateRobotInfoWithEKFData()の条件分岐が正常に動作するよう復旧 これにより Vision=ACTIVE, Tracker=INACTIVE 状態でも ロボットの位置・角度が正常に更新されます。 🤖 Generated with [Claude Code](https://claude.ai/code) Co-Authored-By: Claude <noreply@anthropic.com>
EKFの6次元状態ベクトル [x, y, theta, vx, vy, omega] と 8次元拡張状態ベクトル [x, y, theta, vx, vy, omega, bias_x, bias_y] の マジックナンバー的なインデックスアクセスをenumに置き換えました。 変更内容: - StateIndex enum追加: 基本6次元状態ベクトル用 - ExtendedStateIndex enum追加: 8次元拡張状態ベクトル用 - 全40箇所以上のインデックスアクセスをenumに変更 - 状態遷移行列F、制御入力行列B、観測行列Hすべて対応 - 物理制約関数、Vision/オドメトリ/マウスセンサ観測すべて対応 効果: - コードの可読性大幅向上 - インデックスミスによるバグの予防 - 状態ベクトル構造の自己文書化 🤖 Generated with [Claude Code](https://claude.ai/code) Co-Authored-By: Claude <noreply@anthropic.com>
idx()ヘルパー関数とnormalizeAngle()標準化による一貫性向上 - idx(StateIndex::X)によるenumベースの型安全アクセス導入 - static_cast冗長記法から簡潔な表記への40+箇所変更 - normalizeAngle()関数使用による角度正規化の標準化 技術的改善： - robot_tracker.hpp: constexprヘルパー関数idx()追加 - robot_tracker.cpp: 全状態ベクトルアクセスをenum化 - 手動atan2正規化からnormalizeAngle()関数に統一 - crane_geometry/geometry_operations.hppインクルード追加 保守性向上によりEKFシステムの理解と修正が大幅に容易化 🤖 Generated with [Claude Code](https://claude.ai/code) Co-Authored-By: Claude <noreply@anthropic.com>
EKFトラッカーの信頼度をRobotInfoメッセージで公開し検出判定に統合 - RobotInfo.msgにinternal_tracker_detectedフィールド追加 - MIN_TRACKING_CONFIDENCE(0.2)以上のEKFトラッカーでtrue設定 - detected論理和にinternal_tracker_detectedを組み込み 実装詳細： - world_model_data_provider.cpp: 初期化と統合論理追加 - vision_data_processor.cpp: EKF信頼度に基づく検出フラグ設定 - 3つの検出ソース（Vision、フィードバック、内部トラッカー）独立管理 これによりVision+フィードバック+EKFトラッカーの包括的な ロボット検出状態管理が実現され、システム可観測性が向上 🤖 Generated with [Claude Code](https://claude.ai/code) Co-Authored-By: Claude <noreply@anthropic.com>
if-else文からIIFE（即座実行関数）パターンに変更してコード品質向上 - team_index変数の一時的な-1初期化を排除 - ラムダ式による条件分岐でロジックをより明確に表現 - 関数型プログラミングスタイルによる可読性改善 技術詳細： - vision_data_processor.cpp:updateRobotInfoWithEKFData()内 - チーム色に基づくteam_index計算ロジックの改善 - C++11ラムダ式の適切な活用例 小規模な改善だが、コードの意図がより明確に表現される 🤖 Generated with [Claude Code](https://claude.ai/code) Co-Authored-By: Claude <noreply@anthropic.com>
|
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
Add this suggestion to a batch that can be applied as a single commit.
This suggestion is invalid because no changes were made to the code.
Suggestions cannot be applied while the pull request is closed.
Suggestions cannot be applied while viewing a subset of changes.
Only one suggestion per line can be applied in a batch.
Add this suggestion to a batch that can be applied as a single commit.
Applying suggestions on deleted lines is not supported.
You must change the existing code in this line in order to create a valid suggestion.
Outdated suggestions cannot be applied.
This suggestion has been applied or marked resolved.
Suggestions cannot be applied from pending reviews.
Suggestions cannot be applied on multi-line comments.
Suggestions cannot be applied while the pull request is queued to merge.
Suggestion cannot be applied right now. Please check back later.
概要
SSL Visionに対してExtended Kalman Filter (EKF)を適用した高精度ロボット状態推定システムを新規実装。外部トラッカー依存を削減し、マルチセンサー融合による自律的な状態推定を実現。
主要な変更
🆕 新機能
🔧 アーキテクチャ改善