自作ロボットカーをナビケーションする

ナビケーション 未分類

外観

ロボットカーの構成部品

PCとロボットカーのインタフェース

システム構成仕様書

============================================================
 自作ロボット「My_Navi」システム構成仕様書 (v1.1)
============================================================

作成日: 2025年12月14日
概要: M5Atom Matrix, UnitRoller, RPLidarを使用した差動二輪駆動ロボットの自律移動システム

------------------------------------------------------------
【1. ハードウェア仕様 (URDFより算出)】

| 項目 | 値 | 備考 |
| :--- | :--- | :--- |
| 駆動方式 | 差動二輪駆動 | 前輪駆動または後輪駆動 |
| タイヤ径 | 0.07 m (70mm) | 半径 0.035m |
| トレッド幅 | 0.1635 m (163.5mm) | 左右タイヤ中心間の距離 |
| 機体サイズ | 160 x 185 x 140 mm | (長さ x 幅 x 高さ) |
| LiDAR位置 | X: -0.025m, Z: 0.130m | ロボット中心より2.5cm後方、高さ13cm |
| MCU | M5Atom Matrix | ESP32-PICO-D4 |
| 通信 | Wi-Fi (MQTT) | Broker: 192.168.0.110 |

------------------------------------------------------------
【2. ソフトウェア構成一覧】

PC側（ROS 2）とロボット側（ESP32）が MQTTブローカーを介して連携する分散構成。

A. ロボット側 (Firmware)
   - ファイル名: My_Navi_Lidar_Rev.ino
   - 役割: ロボット制御ファームウェア
     [Core 0] LiDARデータ収集・解析（180度回転補正済み）
     [Core 1] MQTT通信、モーター制御、オドメトリ送信
   - 安全機能: LEDステータス表示、通信切断時の再接続

B. PC側 (ROS 2 Control & Nodes)
   - My_Navi.py (Manual):
       ジョイスティック手動操縦。Joy入力をMQTT(RPM)に変換。
       ※右モーター指令の符号反転処理を含む。
   
   - mqtt_to_odom.py (Bridge):
       足回り情報のROS化。MQTT(Pos) -> ROS(/odom)。
   
   - mqtt_to_scan.py (Bridge):
       LiDAR情報のROS化。MQTT(Scan) -> ROS(/scan)。
   
   - cmd_vel_to_mqtt.py (Bridge):
       Nav2指令のMQTT化。ROS(/cmd_vel) -> MQTT(RPM)。
   
   - my_navigation_launch.py (Launch):
       ナビゲーション起動。既存の地図を読み込み自律移動を行う。
   
   - my_slam_launch.py (Launch):
       地図作成(SLAM)起動。未知の環境を探索して地図を作成する。

C. 設定・定義ファイル
   - my_robot.urdf.xacro (Model): ロボット形状定義
   - nav2_params.yaml (Config): Nav2パラメータ
   - mapper_params_...yaml (Config): SLAM Toolboxパラメータ

------------------------------------------------------------
【3. 重要ロジック・特記事項（注意点）】

1. モーター回転方向の「二重反転」処理
   - PC側(Python): 右モーター指令値を「-1倍」して送信。
   - マイコン側(Arduino): 受信した左右両方の指令値を「-1倍」して出力。
   - 結果: 右モーターは正転、左モーターは逆転（配線都合による補正）。

2. LiDARの取り付け向き補正
   - 物理的に180度反転（後ろ向き）に取り付け。
   - マイコン側で `angle += 180.0` の補正済み。
   - ROS側(URDF)では回転させないこと。

3. ESP32 マルチコア並列処理
   - Core 0: LiDAR処理 / Core 1: 通信・制御
   - 排他制御(bufferMutex)によりデータ破損を防止。

============================================================

ハードウェア仕様 (URDF定義)

項目値備考
駆動方式差動二輪駆動 (Differential Drive)前輪駆動または後輪駆動
タイヤ径0.07 m (70mm)半径 0.035m
トレッド幅0.1635 m (163.5mm)左右タイヤ中心間の距離
機体サイズ160 x 185 x 140 mm(長さ x 幅 x 高さ)
LiDAR位置X: -0.025m, Z: 0.130mロボット中心より2.5cm後方、高さ13cm
MCUM5Atom MatrixESP32-PICO-D4
通信Wi-Fi (MQTT)Broker: 192.168.0.110

ソフトウェア構成一覧

A. ロボット側 (Firmware)

ファイル名分類役割・概要依存ライブラリ
My_Navi_Lidar_Rev.inoMainロボット制御ファームウェア
– Core 0: LiDARデータ収集・解析（180度回転補正済み）
– Core 1: MQTT通信、モーター制御、オドメトリ送信
– 安全機能: LEDステータス表示、通信切断時の再接続		M5Atom, WiFi, PubSubClient, ArduinoJson, unit_rolleri2c

B. PC側 (ROS 2 Control & Nodes)

ファイル名分類役割・概要
My_Navi.pyManualジョイスティック手動操縦
– Joy入力をMQTT(RPM)に変換して送信
– 簡易オドメトリ計算と表示
– 重要: 右モーター指令の符号反転処理を含む
mqtt_to_odom.pyBridge足回り情報のROS化
– MQTT (Roller485/position) → ROS 2 (/odom)
– エンコーダ値から座標計算を行う
mqtt_to_scan.pyBridgeLiDAR情報のROS化
– MQTT (Roller485/scan) → ROS 2 (/scan)
– バイナリデータをLaserScan型に変換
cmd_vel_to_mqtt.pyBridgeNav2指令のMQTT化
– ROS 2 (/cmd_vel) → MQTT (Roller485/twin_rpm)
– 速度(m/s)を回転数(RPM)に変換
my_navigation_launch.pyLaunchナビゲーション起動
– 既存の地図を読み込み、自律移動を行うモード
– map_name 引数で地図切替が可能
my_slam_launch.pyLaunch地図作成 (SLAM) 起動
– 地図がない状態で走り、地図を作るモード
– SLAM Toolboxを起動し、作成された地図を保存可能にする

C. 設定・定義ファイル

ファイル名分類役割・概要
my_robot.urdf.xacroModelロボット形状定義
– タイヤ位置、LiDAR位置の物理パラメータ
nav2_params.yamlConfigNav2パラメータ
– コストマップ設定、プランナー設定、AMCL設定
mapper_params_...yamlConfigSLAM Toolboxパラメータ
– 地図作成アルゴリズムの設定

重要なロジック・特記事項（注意点）

============================================================
 My_Navi システム重要ロジック・特記事項 (Technical Notes)
============================================================
作成日: 2025年12月14日
対象システム: My_Navi (M5Atom + UnitRoller + RPLidar)

開発やメンテナンスを行う際、以下の3点はシステムの根幹に関わる「特殊な仕様」であるため、
変更する際は十分に注意すること。

------------------------------------------------------------
【1. モーター回転方向の「二重反転」処理】
ハードウェアの配線特性とソフトウェアの整合性を取るため、
PC側とマイコン側の2箇所で符号の反転処理が行われている。

A. PC側 (My_Navi.py)
   - 右モーターの指令値(Rrpm)のみ、符号を「-1倍」してMQTT送信している。
   - 理由: ジョイスティックの入力と実際のロボットの旋回方向を合わせるため。

B. マイコン側 (My_Navi_Lidar_Rev.ino)
   - 受信した左右両方のRPM指令(Lrpm, Rrpm)に対し、さらに「-1倍」してモーターへ出力している。
   - 理由: ユニットの配線または取り付け向きにより、プラス指令で「後退」してしまう現象を補正するため。

[結果]
最終的にモータードライバに渡る値は以下のようになる：
・左モーター = 指令値 × (-1)
・右モーター = 指令値 × (-1) × (-1) = 指令値

※ 今後配線を変更しない限り、この二重ロジックを維持する必要がある。

------------------------------------------------------------
【2. LiDARの取り付け向き補正】
LiDAR (RPLidar) は、ロボットの進行方向に対して物理的に「180度反転（後ろ向き）」に取り付けられている。

・補正処理の場所: マイコン側 (My_Navi_Lidar_Rev.ino)
・処理内容: 生データをパースする際、角度データに `angle += 180.0` を加算している。

[注意点]
ROS 2側 (URDFやLaunch) に届くデータは既に「正面が0度」に直されている。
したがって、URDFファイルでLiDARのジョイントを回転させる必要はない。
もしURDF側でも回転させてしまうと、補正が二重になり、前後逆の地図ができてしまうため注意。

------------------------------------------------------------
【3. ESP32 マルチコア並列処理】
処理落ちによるLiDARデータの欠損や通信遅延を防ぐため、ESP32のデュアルコアを活用している。

・Core 0 (裏方タスク):
   - LiDARからのシリアルデータ受信、パース処理、バッファリングを担当。
   - 重たい処理だが、通信や制御を阻害しない。

・Core 1 (メインタスク / Arduino `loop()`):
   - Wi-Fi/MQTT通信、モーター制御、LED制御を担当。
   - Core 0が用意したバッファを読み取って送信する。

[注意点]
プログラムを改造する際は、`bufferMutex` (セマフォ) による排他制御を壊さないようにすること。
Core 0とCore 1が同時にバッファへアクセスすると、データが破損したりフリーズする原因となる。

============================================================

IMUの効果

項目以前の状態現在の改善された状態
位置情報の主導権不安定なIMU積分や、ただの足し算「信頼できるタイヤの距離」 ＋ 「IMUの鋭い方向検知」 のハイブリッド。
通信トラブル耐性パケットが詰まると急停止・パニックパケットが詰まっても、嘘をついて（フィルタして）でも滑らかに振る舞う 強かさ。
地図との整合性回転するたびに世界が歪む回転しても世界（地図）が固定されたまま、ロボットだけが回る。