【Python/ロボティクス】差動二輪ロボットの簡易シミュレータを作ってみる

はじめに

この記事はロボティクスアドベントカレンダー1日目の記事です。

また、自分の勉強も兼ねて様々な言語、通信手法などを試しながらロボットシステムを作っていくシリーズの1つ目の記事です。
動作イメージとしては以下のようなものです。
これはシミュレータ、ビジュアライザー、プランナーの可視化の様子です。

ざっくりとした目標として

• 特定のフレームワークに依存しないソフトウェアの実装方法を学ぶ
• 複数の言語を学ぶ
• 複数の通信方法を学ぶ
• 依存の少ないものにする

としました。

そこで、まずは差動二輪のロボットのシミュレータを作ろうと思います。
このシリーズでは基本的にこのシミュレータに基づいて色々実装していこうと思います。

以下のリポジトリにアップしています。

今日の内容は、add_simple_simulatorブランチにあります。

環境

以下の環境で動作を確認しています。
また、Pythonのライブラリについてはプロジェクト内にpyproject.tomlファイルがあります。

項目	バージョン
Ubuntu	24.04
Python	3.12.3

シミュレータの大枠

ここでは、以下のクラスでシミュレータを構成していきます。

• Worldクラス
  • 環境、障害物の管理
• Robotクラス
  • ロボット本体。外部から速度指令を設定でき、外部からロボットの位置を取得できる。
• SimulatorVizualizerクラス
  • 障害物とロボットの位置（姿勢含む）を描画

以下の動画の白い画面の方の実装になります。
また、LiDARは次回後々追加します。

Worldクラス：環境

ここでは2次元、[size] x [size]の環境を作成することとします。
環境の情報はWorldクラスとしてまとめていきます。

メンバ変数のgridは、[size] x [size] の2次元配列であり、障害物の座標であれば1、そうでなければ0となっています。
静的な環境を想定していて、_generate_obstacles()関数内で障害物を生成しています。

world.py

import random

import numpy as np


class World:
    def __init__(self, size: int) -> None:
        self._size: int = size
        self._grid: np.ndarray = np.zeros((size, size))
        self._obstacles: set[tuple[int, int]] = set()
        self._generate_obstacles()

    def _generate_obstacles(
        self, wall_count: int = 10, wall_length_range: tuple[int, int] = (10, 30)
    ) -> None:
        for _ in range(wall_count):
            x = random.randint(0, self._size - 1)
            y = random.randint(0, self._size - 1)
            length = random.randint(*wall_length_range)
            horizontal = random.choice([True, False])
            for i in range(length):
                ox = x + i if horizontal else x
                oy = y if horizontal else y + i
                if 0 <= ox < self._size and 0 <= oy < self._size:
                    self._update_obstacle(ox, oy)

        # Fixed walls for structure
        for i in range(50):
            self._update_obstacle(25 + i, 50)
            self._update_obstacle(50, 25 + i)

        for i in range(self._size):
            self._update_obstacle(0, i)
            self._update_obstacle(i, 0)
            self._update_obstacle(self._size - 1, i)
            self._update_obstacle(i, self._size - 1)

    @property
    def grid(self) -> np.ndarray:
        return self._grid

    def _update_obstacle(self, x: int, y: int) -> None:
        if 0 <= x < self._size and 0 <= y < self._size:
            self._grid[x, y] = 1.0
            self._obstacles.add((x, y))

    def obstacles(self) -> set[tuple[int, int]]:
        return self._obstacles

    def is_obstacle(self, x: int, y: int) -> bool:
        return (x, y) in self._obstacles

Robotクラス：ロボット

ロボットを動かすにあたって様々な要因による誤差が発生しますが、まずは理想的な動きをするロボット（例：1m/sで1秒間前進したら本当に1m進むロボット）を作っていきましょう。

• set_velocity関数で速度を与えます
• update関数を実行すると微小時間に進むであろう距離を計算します
  • ここではX,Y座標それぞれについて移動ロボットの位置を計算、向きについては単純に加算しています
• _collides関数で、シミュレータの環境の障害物を確認し、障害物に接触しない場合は実際にロボットの位置を進めます

robot.py

import math

from kk_robotics.simulator import world


class Robot:
    def __init__(self, x: float = 10.0, y: float = 10.0, theta: float = 0.0) -> None:
        self._x = x
        self._y = y
        self._theta = theta
        self._v = 0.0
        self._omega = 0.0
        self._radius = 2.0

    def set_velocity(self, linear: float, angular: float) -> None:
        self._v = linear
        self._omega = angular

    def update(self, world: world.World, dt: float = 1.0) -> None:
        self._theta += self._omega * dt
        new_x = self._x + self._v * math.cos(self._theta) * dt
        new_y = self._y + self._v * math.sin(self._theta) * dt

        if not self._collides(new_x, new_y, world):
            self._x = new_x
            self._y = new_y
        else:
            self._v = 0.0

    def get_pose(self) -> tuple[float, float, float]:
        return self._x, self._y, self._theta

    def _collides(self, x: float, y: float, world: world.World) -> bool:
        r = int(math.ceil(self._radius))
        for dx in range(-r, r + 1):
            for dy in range(-r, r + 1):
                cx = int(x + dx)
                cy = int(y + dy)
                if world.is_obstacle(cx, cy):
                    return True

        return False

SimulatorVisualizerクラス：ビジュアライズ

これだけでは実際に障害物がどこにあって、ロボットがどう動いているかわからないので、このシミュレータにビズアライズの機能を追加します。
なるべく依存を減らすことを目標にしているのでTkinterのCanvasに描画しましょう。

canvas.create_lineなどは、返り値にcanvas内で管理しているidを返していて、それを指定して位置を動かすことができます。canvas内の要素をdelete()で消すよりこの方法の方が処理が重くないようです。

vizualizer.py

import math
import tkinter as tk


class SimulatorVisualizer:
    def __init__(self, size: int, scale: float = 5.0) -> None:
        self._scale = scale
        self._size = size

        self._root = tk.Tk()
        self._root.title("2D Robot Simulator (Grid + LiDAR)")
        size_px = int(size * scale)
        self._canvas = tk.Canvas(self._root, width=size_px, height=size_px, bg="white")
        self._canvas.pack()

        self._robot_pose_canvas_id: int | None = None
        self._robot_heading_canvas_id: int | None = None
        self._sensor_line_ids: list[int] = []

        self._robot_radius = 2.0
        self._robot_pose = [0.0, 0.0, 0.0]

    def draw_world(self, obstacles: set[tuple[int, int]]) -> None:
        for x, y in obstacles:
            x0, y0 = self._convert_to_canvas_coodinate(x, y)
            x1, y1 = self._convert_to_canvas_coodinate(x + 1, y + 1)
            self._canvas.create_rectangle(x0, y1, x1, y0, fill="black", outline="")

        scale = int(self._scale)
        width = self._size * scale
        height = self._size * scale
        for x in range(0, width, scale):
            self._canvas.create_line(x, 0, x, height * self._scale, fill="light gray")
        for y in range(0, height, scale):
            self._canvas.create_line(0, y, width * self._scale, y, fill="light gray")

    def set_robot_pose(self, pose: tuple[float, float, float]) -> None:
        self._robot_pose = pose

    def update(self) -> None:
        self._draw_robot()
        self._root.update()

    def _convert_to_canvas_coodinate(self, x: float, y: float) -> tuple[float, float]:
        return x * self._scale, (self._size - y) * self._scale

    def _draw_robot(self) -> None:
        x = self._robot_pose[0]
        y = self._robot_pose[1]
        theta = self._robot_pose[2]

        xc, yc = self._convert_to_canvas_coodinate(x, y)
        r = self._robot_radius * self._scale

        if self._robot_pose_canvas_id is None:
            self._robot_pose_canvas_id = self._canvas.create_oval(
                xc - r, yc - r, xc + r, yc + r, fill="blue"
            )
        else:
            self._canvas.coords(self._robot_pose_canvas_id, xc - r, yc - r, xc + r, yc + r)

        hx = xc + r * math.cos(theta)
        hy = yc - r * math.sin(theta)
        if self._robot_heading_canvas_id is None:
            self._robot_heading_canvas_id = self._canvas.create_line(
                xc, yc, hx, hy, fill="white", width=2
            )
        else:
            self._canvas.coords(self._robot_heading_canvas_id, xc, yc, hx, hy)

外部向けAPIをまとめたクラスを作成する

ここまでで、シミュレータの骨格はできました。
ただこれだとこのシミュレータを使いたいとき、複数のクラスを使って複数の関数を実行しなければならなく、使う側にとっては手間になります。
そこでデザインパターンの1つであるFacadeパターンの考えに則って外部向けAPIをまとめたSimulatorクラスを作成しましょう。

simulator.py

import numpy as np

from kk_robotics.simulator import robot
from kk_robotics.simulator import vizualizer
from kk_robotics.simulator import world


class Simulator:
    def __init__(
        self,
        world_impl: world.World,
        robot_impl: robot.Robot,
        vizualizer_impl: vizualizer.SimulatorVisualizer,
    ) -> None:
        self._world = world_impl
        self._robot = robot_impl
        self._vizualizer = vizualizer_impl

        self._vizualizer.draw_world(world_impl.obstacles())

    def set_velocity(self, linear_velocity: float, angular_velocity: float) -> None:
        self._robot.set_velocity(linear_velocity, angular_velocity)

    def get_pose(self) -> tuple[float, float, float]:
        return self._robot.get_pose()

    def get_world(self) -> np.ndarray:
        return self._world.grid

    def update(self, dt: float = 0.1) -> None:
        self._robot.update(self._world, dt)
        self._vizualizer.set_robot_pose(self._robot.get_pose())
        self._vizualizer.update()

main文の例

このシミュレータを起動してロボットを動かすmain文の例です。

simulator_impl.set_velocity(0.5, 0.5)で適当な値を関数に渡すとロボットが動きます。

main.py

import math
import time

from kk_robotics.simulator import robot
from kk_robotics.simulator import simulator
from kk_robotics.simulator import vizualizer
from kk_robotics.simulator import world


def main() -> None:
    size = 100
    world_impl = world.World(size)
    robot_impl = robot.Robot(10, 10, math.pi / 2)
    vizualizer_impl = vizualizer.SimulatorVisualizer(size)
    simulator_impl = simulator.Simulator(world_impl, robot_impl, vizualizer_impl)

    for _ in range(10000):
        simulator_impl.set_velocity(0.5, 0.5)
        simulator_impl.update(0.5)
        time.sleep(0.05)


if __name__ == "__main__":
    main()

まとめ

簡単ではありますが、移動ロボットのシミュレータを作成しました。

まだまだ始まったばかり、ロボティクスアドベントカレンダーの投稿をお待ちしています！