← ノート一覧へ
Notes

ロボットAIエミュレータ(wip)

１．SLAMとは

Simultaneous Localization and Mapping（自己位置推定と地図作成の同時実行）
ロボットが未知空間を移動しながら、「自分が今どこにいるか」と「周囲の地図」を同時に構築する技術。

PCでエミュレートする方法はいくつかある。ここではPLATEAUの空間データとの組み合わせで利用する目的で調査する。

２．シミュレーター比較

シミュレーターWindows対応難易度SLAMPLATEAU連携今の段階での優先度
Webots◎ネイティブ★☆☆○△今すぐ使える
DARPA SubT + Gazebo△Docker経由★★★◎△Ubuntu後に本命
NVIDIA Isaac Sim○★★☆◎◎将来（高性能PC必要）
Unity Robotics Hub◎★★☆△◎比較的容易にスタートできる
AI Habitat△Linux推奨★★☆○△将来
CoppeliaSim○★★☆○△必要なら
MuJoCo○★★★△✗脚型ロボット専門
AirSim○★★☆○△ドローン専門

３．Webotsを動かす

１）ダウンロードと起動

https://cyberbotics.com

→ 右上「Download Webots」→ Windows を選択

ファイル名: webots-R2023b_setup.exe（約 1.5GB）

Fileメニューからサンプルワールドをオープン。検索窓で、lidarと打って出たワールドが次図。lidarが周辺の物体を計測しながら動く。

slam（測地）サンプル

４．テストエリア

    約30m×20mの区画とする
                    ［非常口B］
                        │
                ┌────────┤
                │ 煙区域         │
                │        ● 要救護者
                └───┬────┘
                        │
［待機所］───────＋──────［非常口A］
  5体配置               │
                        ■ がれき・通行止め
                        │
                   ［店舗区画］

入れる要素

  • 幅4m程度の主通路
  • T字路または十字路
  • 非常口2か所
  • 通行止め1か所
  • 煙・危険区域1か所
  • 要救護者1人
  • 5体の待機場所

壁、床、がれきは、最初はすべて直方体

MaruBase.proto　：共通装備

装備初期実験での用途
左右2輪前進、後退、旋回
距離センサー壁・がれきとの衝突回避
GPS実験中の現在位置取得
CompassまたはInertialUnit向きの取得
Emitter他のロボットへの送信
Receiver指令・報告の受信
LED役割や状態の表示

フォルダ構成
TokyoStationMarubots/
│
├─ worlds/
│ └─ tokyo_station_minimum.wbt
│
├─ protos/
│ └─ MaruBase.proto
│
├─ controllers/
│ ├─ maru_controller/
│ │ └─ maru_controller.py
│ │
│ └─ disaster_supervisor/
│ └─ disaster_supervisor.py
│
└─ data/
　├─ route_graph.json
　└─ scenarios.json

実装順序

第1段階：1体（SEARCH)を動かす

開始地点 → 指定地点 → 停止

①NewProject作成（先ほどのフォルダ構成に従い、TokyoStationMarubots）World名はtokyo_station_minimum）

②ToolsメニューからScene Treeを表示

③RectangleArenaをダブルクリック　floorSizeを30mx20mにする

④floorTileSizeを1m,1mにする

⑤全体を見えるように、RectangleArenaで右クリック　Move Viewpoint to Object　に

⑥エリアオブジェクト追加

一度webotsを閉じ、tokyo_station_minimum.wbtをテキストエディタで開き、ボックスなどを直接指定して追記する。

# ------------------------------------------------------------
# Tokyo Station minimum underground test area
# ENU coordinate system:
# X = east/west, Y = north/south, Z = height
# ------------------------------------------------------------

# 共通壁材
DEF WALL_MATERIAL PBRAppearance {
  baseColor 0.68 0.72 0.76
  roughness 0.8
  metalness 0
}

# 北西側壁1
Solid {
  translation -11.25 2 0.75
  children [
    Shape {
      appearance USE WALL_MATERIAL
      geometry Box {
        size 5.5 0.2 1.5
      }
    }
  ]
  name "wall_north_west_1"
  boundingObject Box {
    size 5.5 0.2 1.5
  }
}

# 北西側壁2
Solid {
  translation -3.75 2 0.75
  children [
    Shape {
      appearance USE WALL_MATERIAL
      geometry Box {
        size 3.5 0.2 1.5
      }
    }
  ]
  name "wall_north_west_2"
  boundingObject Box {
    size 3.5 0.2 1.5
  }
}

# 北東側壁
Solid {
  translation 8 2 0.75
  children [
    Shape {
      appearance USE WALL_MATERIAL
      geometry Box {
        size 12 0.2 1.5
      }
    }
  ]
  name "wall_north_east"
  boundingObject Box {
    size 12 0.2 1.5
  }
}

# 南西側壁
Solid {
  translation -8 -2 0.75
  children [
    Shape {
      appearance USE WALL_MATERIAL
      geometry Box {
        size 12 0.2 1.5
      }
    }
  ]
  name "wall_south_west"
  boundingObject Box {
    size 12 0.2 1.5
  }
}

# 南東側壁
Solid {
  translation 8 -2 0.75
  children [
    Shape {
      appearance USE WALL_MATERIAL
      geometry Box {
        size 12 0.2 1.5
      }
    }
  ]
  name "wall_south_east"
  boundingObject Box {
    size 12 0.2 1.5
  }
}

# 北側縦通路・左壁
Solid {
  translation -2 6 0.75
  children [
    Shape {
      appearance USE WALL_MATERIAL
      geometry Box {
        size 0.2 8 1.5
      }
    }
  ]
  name "wall_upper_left"
  boundingObject Box {
    size 0.2 8 1.5
  }
}

# 北側縦通路・右壁
Solid {
  translation 2 6 0.75
  children [
    Shape {
      appearance USE WALL_MATERIAL
      geometry Box {
        size 0.2 8 1.5
      }
    }
  ]
  name "wall_upper_right"
  boundingObject Box {
    size 0.2 8 1.5
  }
}

# 南側縦通路・左壁
Solid {
  translation -2 -6 0.75
  children [
    Shape {
      appearance USE WALL_MATERIAL
      geometry Box {
        size 0.2 8 1.5
      }
    }
  ]
  name "wall_lower_left"
  boundingObject Box {
    size 0.2 8 1.5
  }
}

# 南側縦通路・右壁
Solid {
  translation 2 -6 0.75
  children [
    Shape {
      appearance USE WALL_MATERIAL
      geometry Box {
        size 0.2 8 1.5
      }
    }
  ]
  name "wall_lower_right"
  boundingObject Box {
    size 0.2 8 1.5
  }
}

# ------------------------------------------------------------
# 色付き区域
# ------------------------------------------------------------

# 煙区域
Transform {
  translation -9 6 0.02
  children [
    Shape {
      appearance PBRAppearance {
        baseColor 0.75 0.12 0.12
        emissiveColor 0.18 0.02 0.02
        transparency 0.35
        roughness 1
        metalness 0
      }
      geometry Box {
        size 6 6 0.03
      }
      castShadows FALSE
    }
  ]
}

# 共通安全区域材
DEF SAFE_AREA_MATERIAL PBRAppearance {
  baseColor 0.1 0.55 0.28
  emissiveColor 0.02 0.12 0.05
  transparency 0.15
  roughness 1
  metalness 0
}

# 待機所
Transform {
  translation -12 0 0.025
  children [
    Shape {
      appearance USE SAFE_AREA_MATERIAL
      geometry Box {
        size 3.5 3.2 0.01
      }
      castShadows FALSE
    }
  ]
}

# 非常口A
Transform {
  translation 13.2 0 0.025
  children [
    Shape {
      appearance USE SAFE_AREA_MATERIAL
      geometry Box {
        size 1.5 3 0.04
      }
      castShadows FALSE
    }
  ]
}

# 非常口B
Transform {
  translation 0 8.8 0.025
  children [
    Shape {
      appearance USE SAFE_AREA_MATERIAL
      geometry Box {
        size 3 1.5 0.04
      }
      castShadows FALSE
    }
  ]
}

# 店舗区画
Transform {
  translation 0 -8.5 0.025
  children [
    Shape {
      appearance PBRAppearance {
        baseColor 0.24 0.36 0.58
        roughness 0.9
        metalness 0
      }
      geometry Box {
        size 3.5 2.5 0.04
      }
      castShadows FALSE
    }
  ]
}

# ------------------------------------------------------------
# 障害物と要救護者
# ------------------------------------------------------------

# がれき・通行止め
Solid {
  translation 0 -5 0.5
  children [
    Shape {
      appearance PBRAppearance {
        baseColor 0.55 0.25 0.08
        roughness 0.95
        metalness 0
      }
      geometry Box {
        size 3.6 1.2 1
      }
    }
  ]
  name "blocked_area"
  boundingObject Box {
    size 3.6 1.2 1
  }
}

# 要救護者の仮マーカー
Transform {
  translation -9 6 0.45
  children [
    Shape {
      appearance PBRAppearance {
        baseColor 0.8 0.05 0.65
        emissiveColor 0.25 0.01 0.18
        roughness 0.55
        metalness 0
      }
      geometry Sphere {
        radius 0.45
      }
    }
  ]
}

赤色・半透明 煙区域
紫色の球 要救護者
緑色 待機所・非常口
橙色 がれき・通行止め
青色 店舗区画
灰色 通路壁

⑦シミュレーションをストップしTimeを０に巻き戻し
⑧シーンにTurtleBot3Burgerを追加

シーンツリーで展開し、次の値に。
translation -12 0 0.02　（待機場所。床から少し0.02上）
rotation 0 0 1 0
name “search_maru”

⑨コントローラ作成（python）

Fileメニューから　New→Robot Controller（Pyrhonで）を選ぶ

Language：Python
Name：search_maru_controller

これにより自動的にcontrllerフォルダとファイルが作成される
D:\TokyoStationMarubots\
└─ controllers
└─ search_maru_controller
└─ search_maru_controller.py

テキストエディタでファイルが開く（serch_maru_controller.py )

"""探索丸：待機所から中央へ前進する最初の制御プログラム。"""

from controller import Robot


robot = Robot()
time_step = int(robot.getBasicTimeStep())

# TurtleBot3 Burgerの左右車輪
left_motor = robot.getDevice("left wheel motor")
right_motor = robot.getDevice("right wheel motor")

# 速度制御モードに切り替える
left_motor.setPosition(float("inf"))
right_motor.setPosition(float("inf"))

# 開始時は停止
left_motor.setVelocity(0.0)
right_motor.setVelocity(0.0)

start_time = robot.getTime()

while robot.step(time_step) != -1:
    elapsed = robot.getTime() - start_time

    # 約8秒間直進
    if elapsed < 8.0:
        left_motor.setVelocity(3.0)
        right_motor.setVelocity(3.0)
    else:
        # 8秒後に停止
        left_motor.setVelocity(0.0)
        right_motor.setVelocity(0.0)

TurtleBot3 Burgerの最大車輪速度はモデル上 6.67 rad/s なので、ここでは控えめに 3.0 rad/s としている

⑩コントローラの選択

シーンツリーで、このロボット（TurtleBot3Burger”serch_maru”)を開き、controllerで今作成したserch_maru_controllerを選択する

⑪Worldリロードして、プレイボタンをクリックする
待機場所から８秒まっすぐ動いて止まるロボットを観察できる（左マウスドラッグでカメラ斜め）

⑫回転を与える
次は時間ではなく、車輪の回転量で距離と90度旋回を制御する。

TurtleBot3 Burgerには、最初から左右の車輪センサー left wheel sensor、right wheel sensor が入っている。車輪半径は0.033m、左右輪の間隔は0.16m。PositionSensorは車輪の回転角をラジアンで取得できる。

待機所 (-12, 0)
    ↓ 東へ5m
煙区域入口 (-7, 0)
    ↓ 左90度
北へ6m
    ↓ 左90度
西へ2m
    ↓
要救護者 (-9, 6)

プログラム書き換え

"""探索丸：車輪エンコーダーを使って要救護者地点まで移動する。"""

import math
from controller import Robot


robot = Robot()
time_step = int(robot.getBasicTimeStep())

# モーター
left_motor = robot.getDevice("left wheel motor")
right_motor = robot.getDevice("right wheel motor")

# 車輪位置センサー
left_sensor = robot.getDevice("left wheel sensor")
right_sensor = robot.getDevice("right wheel sensor")

left_sensor.enable(time_step)
right_sensor.enable(time_step)

# 速度制御モード
left_motor.setPosition(float("inf"))
right_motor.setPosition(float("inf"))

left_motor.setVelocity(0.0)
right_motor.setVelocity(0.0)

# TurtleBot3 Burgerの寸法
WHEEL_RADIUS = 0.033   # 車輪半径[m]
AXLE_LENGTH = 0.160   # 左右輪の間隔[m]

STRAIGHT_SPEED = 6.0  # 直進時の車輪速度[rad/s]
TURN_SPEED = 3.0      # 旋回時の車輪速度[rad/s]

# 移動工程
segments = [
    {
        "name": "煙区域入口まで東へ5m",
        "type": "straight",
        "value": 5.0,
    },
    {
        "name": "北方向へ左90度旋回",
        "type": "turn_left",
        "value": math.pi / 2,
    },
    {
        "name": "煙区域内を北へ6m",
        "type": "straight",
        "value": 6.0,
    },
    {
        "name": "要救護者方向へ左90度旋回",
        "type": "turn_left",
        "value": math.pi / 2,
    },
    {
        "name": "要救護者まで西へ2m",
        "type": "straight",
        "value": 2.0,
    },
]


def stop_robot():
    """左右の車輪を停止する。"""
    left_motor.setVelocity(0.0)
    right_motor.setVelocity(0.0)


# 最初のセンサー値が取得できるまで1ステップ進める
if robot.step(time_step) == -1:
    raise SystemExit

segment_index = 0
segment_started = False
mission_reported = False

start_left = 0.0
start_right = 0.0
target_rotation = 0.0
pause_until = 0.0

while robot.step(time_step) != -1:
    # 全工程終了
    if segment_index >= len(segments):
        stop_robot()

        if not mission_reported:
            print("===================================")
            print("VICTIM_FOUND")
            print("探索丸：要救護者地点に到着")
            print("想定座標：x=-9.0, y=6.0")
            print("===================================")
            mission_reported = True

        continue

    # 工程間の短い停止
    if robot.getTime() < pause_until:
        stop_robot()
        continue

    segment = segments[segment_index]

    # 新しい工程の開始
    if not segment_started:
        start_left = left_sensor.getValue()
        start_right = right_sensor.getValue()

        if segment["type"] == "straight":
            # 距離÷車輪半径＝必要な車輪回転角
            target_rotation = segment["value"] / WHEEL_RADIUS

            left_motor.setVelocity(STRAIGHT_SPEED)
            right_motor.setVelocity(STRAIGHT_SPEED)

        elif segment["type"] == "turn_left":
            # その場旋回で各車輪が進む円弧長
            wheel_arc = (AXLE_LENGTH / 2.0) * segment["value"]
            target_rotation = wheel_arc / WHEEL_RADIUS

            left_motor.setVelocity(-TURN_SPEED)
            right_motor.setVelocity(TURN_SPEED)

        print(f"開始：{segment['name']}")
        segment_started = True

    # この工程で左右輪が回転した角度
    left_progress = abs(left_sensor.getValue() - start_left)
    right_progress = abs(right_sensor.getValue() - start_right)

    average_progress = (left_progress + right_progress) / 2.0

    # 目標回転量へ到達
    if average_progress >= target_rotation:
        stop_robot()

        print(f"完了：{segment['name']}")

        segment_index += 1
        segment_started = False

        # 慣性を落ち着かせるため0.4秒停止
        pause_until = robot.getTime() + 0.4

モーターはsetPosition(float("inf"))にしてから速度を与えると、速度制御モードになる。今回のコードではPositionSensorの回転角から、走行距離と旋回角を判定している。実行後、コンソールにprint文が表示されている。

第2段階：2体(SEARCH,RELAY)で通信する

[SEARCH] → VICTIM_FOUND → [RELAY]

Webotsでは送信側にEmitter、受信側にReceiverを搭載し、同じチャンネルを設定する。双方向通信にする段階では、両方のロボットにEmitterとReceiverを搭載する。
シーンツリーで、serch_maruをコピーペースト。relay_maruを準備する。
translation -12 1.2 0
rotation 0 0 1 0
name “relay_maru”
controller “”

ワールドファイルへ通信機器（Relay)を追加

ワールドを保存して、次のファイルをメモ帳で開く。

D:\TokyoStationMarubots\worlds\tokyo_station_minimum.wbt

探索丸の記述を修正

探索丸の部分を検索。

name "search_maru"

現在、おおむね次の形。

TurtleBot3Burger {
  translation -12 0 0
  rotation 0 0 1 0
  name "search_maru"
  controller "search_maru_controller"
}

これを次に置き換える。

TurtleBot3Burger {
  translation -12 0 0
  rotation 0 0 1 0
  name "search_maru"
  controller "search_maru_controller"
  extensionSlot [
    Emitter {
      name "emitter"
      channel 1
      range -1
    }
  ]
}

range -1は、今回の基礎実験では通信距離を制限しない設定のため。

※TurtleBot3 BurgerのextensionSlotには、センサーや通信機器などのノードを追加できる。明示的に設定すると標準のLiDARは一時的に外れるが、現在の通信実験には影響なし。後でLiDARと通信機器を併設する。

通信丸の記述を修正

次に、

name "relay_maru"

を検索。

その部分を次の形にする。

TurtleBot3Burger {
  translation -12 1.2 0
  rotation 0 0 1 0
  name "relay_maru"
  controller "relay_maru_controller"
  extensionSlot [
    Receiver {
      name "receiver"
      channel 1
    }
  ]
}

保存。

Ctrl + S

通信丸のコントローラーを作る

File
→ New
→ New Robot Controller...
Language：Python
Name：relay_maru_controller

作成された、

D:\TokyoStationMarubots
└─ controllers
   └─ relay_maru_controller
      └─ relay_maru_controller.py

を開き、内容をすべて次に置き換え。

"""通信丸：探索丸から要救護者発見メッセージを受信する。"""

import json
from controller import Robot


robot = Robot()
time_step = int(robot.getBasicTimeStep())

# 通信丸は今回は移動せず、待機所で停止する
left_motor = robot.getDevice("left wheel motor")
right_motor = robot.getDevice("right wheel motor")

left_motor.setPosition(float("inf"))
right_motor.setPosition(float("inf"))

left_motor.setVelocity(0.0)
right_motor.setVelocity(0.0)

# 受信機
receiver = robot.getDevice("receiver")
receiver.enable(time_step)
receiver.setChannel(1)

print("===================================")
print("通信丸：受信待機を開始")
print("チャンネル：1")
print("===================================")

while robot.step(time_step) != -1:
    # 同じステップ内に複数届いた場合も、すべて処理する
    while receiver.getQueueLength() > 0:
        raw_message = receiver.getString()
        signal_strength = receiver.getSignalStrength()

        # 読み終わったパケットをキューから取り除く
        receiver.nextPacket()

        try:
            message = json.loads(raw_message)
        except json.JSONDecodeError:
            print(f"通信丸：JSONとして解析できないデータを受信：{raw_message}")
            continue

        message_type = message.get("type", "UNKNOWN")
        sender = message.get("sender", "UNKNOWN")

        print("-----------------------------------")
        print("通信丸：メッセージ受信")
        print(f"送信元：{sender}")
        print(f"種類　：{message_type}")
        print(f"信号強度：{signal_strength:.6f}")

        if message_type == "VICTIM_FOUND":
            position = message.get("position", [None, None])

            print("要救護者発見情報を受信しました")
            print(f"位置：x={position[0]}, y={position[1]}")
            print("次の処理：救護丸・運搬丸へ出動要請")

        print("-----------------------------------")

ReceiverのPython APIでは、getQueueLength()で未処理パケットを確認し、getString()で文字列を取得し、処理後にnextPacket()で次のパケットへ進める。

保存。

Ctrl + S

探索丸に送信処理を追加する

既存の、

D:\TokyoStationMarubots
└─ controllers
   └─ search_maru_controller
      └─ search_maru_controller.py

を開く。

先頭にjsonを追加

現在、

import math
from controller import Robot

となっている部分を、次にする。

import json
import math
from controller import Robot

Emitterを取得する

モーターやセンサーを取得している部分の後へ、次を追加。

# 通信機器
emitter = robot.getDevice("emitter")
emitter.setChannel(1)

最後の到着処理を修正する

最後のprint文のところ。

if not mission_reported:
    print("===================================")
    print("VICTIM_FOUND")
    print("探索丸：要救護者地点に到着")
    print("想定座標：x=-9.0, y=6.0")
    print("===================================")
    mission_reported = True

これを次に置き換える。

if not mission_reported:
    message = {
        "sender": "SEARCH",
        "receiver": "RELAY",
        "type": "VICTIM_FOUND",
        "position": [-9.0, 6.0],
        "priority": 1,
        "simulation_time": round(robot.getTime(), 3),
    }

    message_text = json.dumps(message)

    emitter.send(message_text)

    print("===================================")
    print("VICTIM_FOUND")
    print("探索丸：要救護者地点に到着")
    print("想定座標：x=-9.0, y=6.0")
    print("探索丸：通信丸へ発見情報を送信")
    print(message_text)
    print("===================================")

    mission_reported = True

Webots R2025aのPython版Emitterは、send()へ文字列またはbytesを渡せます。今回はJSON文字列を送る。

保存。

Ctrl + S

ワールドを再読み込みする

シミュレーションを実行する

① 探索丸が待機所を出発
② 中央通路を移動
③ 北側へ旋回
④ 煙区域へ進入
⑤ 要救護者付近へ到着
⑥ 探索丸がVICTIM_FOUNDを送信
⑦ 通信丸が待機所で受信

成功時のConsole表示

探索丸側では、最後に次のように表示。

VICTIM_FOUND
探索丸：要救護者地点に到着
想定座標：x=-9.0, y=6.0
探索丸：通信丸へ発見情報を送信
{"sender": "SEARCH", "receiver": "RELAY", ...}

通信丸側では次が表示。

通信丸：メッセージ受信
送信元：SEARCH
種類　：VICTIM_FOUND
要救護者発見情報を受信しました
位置：x=-9.0, y=6.0
次の処理：救護丸・運搬丸へ出動要請

Webotsの標準radio通信は、壁による遮蔽を考慮せず、同じチャンネルのReceiverへ送信する。したがって、今回は純粋に「複数ロボット間の情報連携」が成立するかを確認できる。通信距離、曲がり角、壁による減衰は後の段階で追加する。

※「受信時の信号強度」は、探索丸から届いた電波が通信丸の位置でどの程度強いかを表す、Webots上の模擬値。

Webotsの標準設定では、送信機と受信機の距離との関係で、

信号強度 = 1 ÷ 距離²

として計算される。

例えば、

距離 1m  → 信号強度 1.0
距離 2m  → 信号強度 0.25
距離 5m  → 信号強度 0.04
距離10m  → 信号強度 0.01

今回表示された、

信号強度：0.022517

は、送信時の探索丸と通信丸の距離が約6.7mだったことに相当する。

第3段階：5体に増やす

共通機体を複製し、役割名だけ変更します。

GUIDE
RELAY
SEARCH
RESCUE
CARRIER

各ロボットが異なる目的地へ移動できれば、最初のマルチロボット実験が成立します。

第4段階：通行止めと経路変更

通路をノードとリンクで表します。

A ─ B ─ C
    │
    D ─ E

がれきが発生したら、

B-C 通行不可

に変更し、別経路を探索します。最初はDijkstra法またはA*法で十分です。

第5段階：通信丸の中継行動

通信丸は、探索丸から受け取った信号強度を監視します。Webots Receiverは、受信パケットについて信号強度と送信元方向を返せます。

信号強度が低下
↓
探索丸方向へ少し移動
↓
指令所との通信も確認
↓
両者と通信できる地点で停止

これで「通信丸らしい動き」が生まれます。

▲