A* で引いた経路を、そのままロボットに走らせてみます。前回までの2本で、差動二輪（左右の車輪の速度差で曲がる方式の車体）について、左右の車輪速から車体の動きを出す式（順運動学）と、その逆（逆運動学）を導きました。式は正しく、往復も閉じています。それでも、そのまま流すと最初の角で壁に当たります。 式の側は1文字も直さず、「いまどこを向いているか」を測って直しながら走らせると、同じロボットが 25 マスの経路を壁に一度も触れずに走り、ゴール誤差 7 cm で止まります。
| 走らせ方 | 何をするか | 結果 |
|---|---|---|
| 計画をそのまま流す（開ループ） | 「何秒回して何秒進むか」を式で先に決め、車輪速を順に流す | 最初の角で 45° 回す指令に対して 20.5° しか回らず、壁に当たってゴールの 16.3 m 手前で止まる |
| 測って直す（閉ループ） | 0.05 秒ごとに姿勢を測り、次の通過点を向くように車輪速を出し直す | 56.7 秒でゴール誤差 0.073 m、壁に触れたステップ 0 |
左が開ループ、右が閉ループの Husky を PyBullet のカメラで撮ったもの（5 倍速）。中央は上から見た地図で、赤が開ループ・青が閉ループの軌跡。左は最初の角で回りきれずに壁へ、右は隙間を抜けてゴール（赤い四角）に届く
開ループ（open loop：出した指令の結果を見ずに、決めたとおりに流すこと）と閉ループ（closed loop：結果を測って指令に反映すること）の違いだけです。式そのものは、前回の2行と「上限を超えたら左右を同じ比率で縮める」をそのまま使います。
先に断っておくと、この記事は A* そのものの解説ではありません。 A* が出した経路を、車体のあるロボットに実行させたときに何が起きるか、という「経路計画のあと」の話です。A* の仕組みを知りたい方は、経路生成②（下のリンク）のほうが目的に合います。
この記事は、9/9 の順運動学、9/16 の逆運動学に続く3本目で、移動ロボットの区切りです。前の2本を読んでいなくても、この記事のコードだけで動きます（A* も最小構成で載せます）。
動作環境と全体の流れ
Python 3.10.9
numpy 1.26.4
pybullet 3.2.7
matplotlib 3.7.0 ※図を描くときだけ
pip install numpy pybullet の1行で動きます。ROS も実機も要りません。PyBullet（物理シミュレータの Python 版。ロボットを実機なしで動かせる）には Husky という4輪の移動ロボットが最初から入っているので、機体を用意する必要もありません。
処理の流れは下図のとおりです。左の3つは走る前に1回だけやり、右の輪は走っている間ずっと回します。
図1：全体の流れ。前回までの式は右の輪の中の「翻訳役」に収まり、新しく足すのは「測る」と「向きのずれから v, ω を決める」の2つだけです
ファイルは3つです。
| ファイル | 役割 | 出どころ |
|---|---|---|
planner.py |
地図と A*、通過点への変換 | 経路生成②の再掲（最小構成） |
husky_sim.py |
PyBullet 上の Husky と壁 | 順運動学の回の Husky 実験を拡張 |
follow.py |
逆運動学の2行（前回）＋今回の追従 | 前回の関数をそのまま流用 |
経路を作る：経路生成②と同じマップで A*
地図は経路生成①②で使った 20×20 マスをそのまま使います。中央に縦の壁があり、1マスだけ隙間があります。右上にはぬかるみ帯（入ると3倍たいへんな地形）があります。
A*（エースター）は、「スタートからここまでの実際のコスト」と「ここからゴールまでの見積もり」を足した値が小さいマスから順に調べていく経路探索です。見積もりを足すぶん、ダイクストラ法より調べるマスが少なくて済みます。中身は②で図解しているので、ここでは動くコードを載せるだけにします。
import heapq # 優先度付きキュー（いつでも最小の要素を取り出せる入れ物）
import numpy as np
FREE, WALL, MUD = 0, 1, 2
WEIGHT = {FREE: 1.0, MUD: 3.0} # 入る先の地形の重み（ぬかるみは 3 倍たいへん）
CELL = 1.5 # 1マスの一辺 [m]（なぜ 1.5 かは本文で）
def make_sample_map() -> tuple[np.ndarray, tuple[int, int], tuple[int, int]]:
"""経路生成①②と同じ 20×20 のマップ。中央の壁に1マスの隙間、右上にぬかるみ帯。"""
grid = np.full((20, 20), FREE, dtype=int)
grid[0:20, 10] = WALL
grid[8, 10] = FREE
grid[3:9, 12:16] = MUD
return grid, (18, 1), (1, 18) # スタート（左下）・ゴール（右上）
def heuristic(a: tuple[int, int], b: tuple[int, int]) -> float:
"""オクタイル距離（斜め移動ありの格子で、まっすぐ行けたときの距離）。"""
dr, dc = abs(a[0] - b[0]), abs(a[1] - b[1])
return np.sqrt(2) * min(dr, dc) + abs(dr - dc)
def astar(grid: np.ndarray, start: tuple[int, int], goal: tuple[int, int],
cut_corners: bool = True) -> tuple[list[tuple[int, int]], float, int]:
"""8近傍の A*。経路（セルの列）・コスト・展開したセル数を返す。"""
rows, cols = grid.shape
moves = [(-1, 0), (1, 0), (0, -1), (0, 1), (-1, -1), (-1, 1), (1, -1), (1, 1)]
dist = {start: 0.0}
came_from: dict[tuple[int, int], tuple[int, int]] = {}
visited: set[tuple[int, int]] = set()
queue = [(heuristic(start, goal), 0.0, start)]
while queue:
_, g, cur = heapq.heappop(queue)
if cur in visited:
continue
visited.add(cur)
if cur == goal:
break
for dr, dc in moves:
nxt = (cur[0] + dr, cur[1] + dc)
if not (0 <= nxt[0] < rows and 0 <= nxt[1] < cols) or grid[nxt] == WALL:
continue
if dr and dc and not cut_corners: # 斜めに動くとき、両脇に壁があれば通さない
if grid[cur[0] + dr, cur[1]] == WALL or grid[cur[0], cur[1] + dc] == WALL:
continue
step = np.sqrt(2) if dr and dc else 1.0
new_g = g + step * WEIGHT[grid[nxt]]
if new_g < dist.get(nxt, np.inf):
dist[nxt] = new_g
came_from[nxt] = cur
heapq.heappush(queue, (new_g + heuristic(nxt, goal), new_g, nxt))
if goal not in came_from:
return [], np.inf, len(visited)
path = [goal]
while path[-1] != start:
path.append(came_from[path[-1]])
return path[::-1], dist[goal], len(visited)
cut_corners という引数が②には無かったものです。これが、経路計画と車体をつないで初めて出てきた問題の1つ目です。
つないで出た問題①：斜め移動が壁の角をかすめる
②と同じ条件（cut_corners=True）で解くと、②と同じ 23 セル・コスト 26.97・展開 113 が出ます。その経路は、壁の隙間 (8, 10) を 斜めに 通ります。(9, 9) → (8, 10) → (7, 11) です。
col: 9 10 11
row 7: . # ○ ← 隙間を抜けた先
row 8: . ○ . ← 隙間（壁の穴）
row 9: ○ # . ← 隙間の手前
↑ (9,9) から (8,10) へ斜めに入ると、(9,10) の壁の角をかすめる
図2：隙間を斜めに抜ける経路。点の大きさがゼロなら通れますが、Husky は 1.0 m × 0.69 m の箱です
A* は「マスの中心を結ぶ線」で経路を考えるので、点の大きさがゼロなら角をかすめても問題ありません。Husky には車体があります。斜めに入るとき、両脇のどちらかが壁なら通さない、という条件を足したのが cut_corners=False です。
def cell_to_xy(cell: tuple[int, int], rows: int = 20) -> np.ndarray:
"""セル (row, col) の中心を世界座標 [m] にする。row 0 が上（y が最大）、col 0 が左。"""
return np.array([(cell[1] + 0.5) * CELL, (rows - 1 - cell[0] + 0.5) * CELL])
def path_to_waypoints(path: list[tuple[int, int]]) -> np.ndarray:
"""セルの列を通過点の列にする。同じ向きに進んでいる途中の点は捨て、曲がる所だけ残す。"""
pts = [cell_to_xy(c) for c in path]
keep = [pts[0]]
for prev, cur, nxt in zip(pts[:-2], pts[1:-1], pts[2:]):
if not np.allclose(cur - prev, nxt - cur):
keep.append(cur)
keep.append(pts[-1])
return np.array(keep)
if __name__ == "__main__":
grid, start, goal = make_sample_map()
for cut in (True, False):
path, cost, n_explored = astar(grid, start, goal, cut_corners=cut)
print(f"角をかすめる={cut}: 通過セル={len(path)} コスト={cost:.2f} 展開={n_explored}")
waypoints = path_to_waypoints(path)
print(f"通過点 {len(waypoints)} 点:", waypoints.tolist())
角をかすめる=True: 通過セル=23 コスト=26.97 展開=113
角をかすめる=False: 通過セル=25 コスト=28.14 展開=142
通過点 8 点: [[2.25, 2.25], [14.25, 14.25], [14.25, 17.25], [17.25, 17.25], [17.25, 24.75], [18.75, 26.25], [26.25, 26.25], [27.75, 27.75]]
角をかすめない経路は 25 セル・コスト 28.14 で、②より 2 セル長くなります。これが車体のある経路の値段です。 25 セルのうち、曲がる所だけ残すと通過点は 8 点になります（直進の途中の点は、ロボットには要りません）。
つないで出た問題②：マスの大きさと車体の大きさ
CELL = 1.5 の理由です。最初は 1 マス 1.0 m で試しました。すると、壁の隣のマスの中心に正しく立っているだけで、車体が壁に食い込みました。 Husky の車体は縦 1.0 m・横 0.69 m なので、斜め 45° を向くと中心から角まで 0.60 m あります。マスの中心から壁面までは 0.5 m しかありません。
| 1 マスの一辺 | マス中心から壁面まで | Husky の中心から角まで | 判定 |
|---|---|---|---|
| 1.0 m | 0.50 m | 0.60 m（45° のとき） | 食い込む |
| 1.5 m | 0.75 m | 0.60 m | 0.15 m の余裕 |
経路計画の本では「障害物をロボットの半径ぶん膨らませておく」と教わります。この地図でそれをやると、1 マスしかない隙間が閉じます。今回は地図の形を保つため、マスのほうを 1.5 m にしました。地図は 30 m × 30 m、経路の長さは 42.2 m になります。
計画をそのまま流す（開ループ）：最初の角で壁に当たる
まず、前回の記事の方法をそのまま使います。通過点の列を「その場で回転 → 直進」の繰り返しにし、各区間の (v, ω, 秒数) を式で決め、車輪速に変換して順に流します。前回は理想の差動二輪でこれが誤差ゼロで届きました。
Husky を動かす側のコードです。順運動学の回の実験コードに、壁と接触の数え上げを足しました。
import numpy as np
import pybullet as p
import pybullet_data
from planner import WALL, CELL, cell_to_xy
R_WHEEL = 0.17775 # 車輪半径 [m]。husky.urdf（ロボットの形状・質量を書いたファイル）の値。本文参照
LEFT, RIGHT = [2, 4], [3, 5] # 前左・後左 / 前右・後右 の関節番号
SIM_HZ = 240 # PyBullet の既定の物理更新周波数
class HuskySim:
"""PyBullet 上の Husky と、マップの壁。"""
def __init__(self, grid: np.ndarray, start_xy: np.ndarray, start_yaw: float) -> None:
p.connect(p.DIRECT) # 画面を出さずに動かす
p.setAdditionalSearchPath(pybullet_data.getDataPath())
p.setGravity(0, 0, -9.81)
p.loadURDF("plane.urdf")
self.walls: list[int] = []
box = p.createCollisionShape(p.GEOM_BOX, halfExtents=[CELL / 2, CELL / 2, 0.25])
for r, c in zip(*np.where(grid == WALL)): # 壁のマスを 1 個ずつ箱にして置く
x, y = cell_to_xy((r, c), grid.shape[0])
self.walls.append(p.createMultiBody(0, box, basePosition=[x, y, 0.25]))
quat = p.getQuaternionFromEuler([0, 0, start_yaw]) # 向きを PyBullet の回転形式（クォータニオン）に
self.robot = p.loadURDF("husky/husky.urdf", [start_xy[0], start_xy[1], 0.1], quat)
for _ in range(SIM_HZ): # 着地して落ち着くまで 1 秒待つ
p.stepSimulation()
self.hits = 0 # 壁に触れていた物理ステップ数
def set_wheels(self, v_l: float, v_r: float) -> None:
"""左右の周速 [m/s] を車輪の角速度 [rad/s] にして、4 つのモータへ渡す。"""
for j in LEFT:
p.setJointMotorControl2(self.robot, j, p.VELOCITY_CONTROL,
targetVelocity=v_l / R_WHEEL, force=500)
for j in RIGHT:
p.setJointMotorControl2(self.robot, j, p.VELOCITY_CONTROL,
targetVelocity=v_r / R_WHEEL, force=500)
def step(self, seconds: float) -> None:
"""指定した秒数だけ物理を進める。その間に壁へ触れたステップを数える。"""
for _ in range(int(round(seconds * SIM_HZ))):
p.stepSimulation()
if any(p.getContactPoints(self.robot, w) for w in self.walls):
self.hits += 1
def pose(self) -> np.ndarray:
"""車体の姿勢 (x, y, θ) を返す。シミュレータの真値をそのまま使う（本文の「できないこと」参照）。"""
pos, quat = p.getBasePositionAndOrientation(self.robot)
return np.array([pos[0], pos[1], p.getEulerFromQuaternion(quat)[2]])
def close(self) -> None:
p.disconnect()
⚠️
husky.urdfを読み込むとb3Warning ... No inertial data for linkという警告が 10 行あまり出ます。Husky の URDF にセンサ等の質量が書かれていないためで、PyBullet が既定値で補います。走行には影響しないので、この記事ではそのままにしています。
車輪半径の落とし穴：URDF を読む
R_WHEEL = 0.17775 は、前回まで使っていた 0.1651 と違います。0.1651 は Husky のカタログの値（車輪の直径 330 mm）ですが、PyBullet に入っている husky.urdf の車輪は半径 0.17775 m で作られています。 この差は 7.7% で、0.8 m/s を指令すると 0.862 m/s で走ります。17 m 直進すると 1.3 m 行き過ぎる大きさです。
見つけたのは、直進だけの区間で位置が合わなかったからです。逆運動学の式は正しく、車輪の角速度も指令どおり出ているのに、走った距離が違う。残るのは「周速を角速度に換算する半径」しかありません。シミュレータのモデルの寸法は、カタログではなく URDF を読んで確かめる、というのが今回の教訓の1つです（pybullet_data.getDataPath() の下の husky/husky.urdf に radius="0.17775" と書いてあります）。同じファイルは PyBullet の GitHub（bullet3 リポジトリ）でも読めるので、手元にインストールする前に確かめたい方はこちらをどうぞ。
流してみる
import numpy as np
from planner import make_sample_map, astar, path_to_waypoints
from husky_sim import HuskySim
from follow import wheel_speeds, wrap, V_CRUISE
OMEGA_TURN = 1.0 # その場回転の角速度 [rad/s]（前回と同じ）
def plan_segments(wps: np.ndarray, yaw0: float) -> list[tuple[float, float, float]]:
"""通過点の列を、前回の「回転→直進」の繰り返し＝(v, omega, 秒) の一定入力の列にする。"""
segments, yaw = [], yaw0
for a, b in zip(wps[:-1], wps[1:]):
heading = np.arctan2(*(b - a)[::-1]) # 点 a から点 b へ向かう方向の角度（引数は dy, dx の順）
turn = wrap(heading - yaw)
if abs(turn) > 1e-9:
segments.append((0.0, OMEGA_TURN * np.sign(turn), abs(turn) / OMEGA_TURN))
segments.append((V_CRUISE, 0.0, np.hypot(*(b - a)) / V_CRUISE))
yaw = heading
return segments
grid, start, goal = make_sample_map()
path, _, _ = astar(grid, start, goal, cut_corners=False)
wps = path_to_waypoints(path)
yaw0 = np.arctan2(*(wps[1] - wps[0])[::-1]) # 最初の通過点を向く角度
segments = plan_segments(wps, yaw0)
print(f"区間数 {len(segments)} 合計 {sum(s[2] for s in segments):.2f} 秒")
sim = HuskySim(grid, wps[0], yaw0)
for i, (v, omega, seconds) in enumerate(segments, 1):
sim.set_wheels(*wheel_speeds(v, omega)) # 計画した車輪速を、決めた秒数だけ流す
sim.step(seconds)
x, y, th = sim.pose()
if i <= 3 or i == len(segments):
print(f"区間{i:>2}: v={v:.1f} ω={omega:+.1f} {seconds:5.2f}秒 → x={x:5.2f} y={y:5.2f}"
f" θ={np.degrees(th):5.1f}° 壁 {sim.hits}")
print(f"ゴールまで {np.hypot(x - wps[-1][0], y - wps[-1][1]):.2f} m")
sim.close()
wheel_speeds と wrap は次の節の follow.py にあります（前回の2行と、角度を -180°〜180° に畳む関数です）。先に follow.py を置いてから実行してください。
区間数 13 合計 59.05 秒
区間 1: v=0.8 ω=+0.0 21.21秒 → x=14.28 y=14.28 θ= 45.0° 壁 0
区間 2: v=0.0 ω=+1.0 0.79秒 → x=14.28 y=14.37 θ= 65.5° 壁 0
区間 3: v=0.8 ω=+0.0 3.75秒 → x=14.70 y=17.22 θ= 86.7° 壁 639
区間13: v=0.8 ω=+0.0 2.65秒 → x=15.09 y=17.50 θ= 90.0° 壁 7197
ゴールまで 16.29 m
区間 1 の直進 17 m は、計画 (14.25, 14.25) に対して (14.28, 14.28) と 4 cm で着いています。半径を直したので直進は合います。
区間 2 がその場回転です。45° 回す指令（ω = 1.0 rad/s を 0.785 秒）に対して、向きは 45.0° → 65.5° の 20.5° しか変わっていません。 順運動学の回で「Husky は式の予測の半分以下でしか回らない」と測ったとおりです。4輪が車体に固定されているので、曲がるにはタイヤが横に滑る必要があり、「車輪は横に滑らない」という式の前提が崩れています。
向きが 24.5° 足りないまま区間 3 で直進するので、壁（x = 15〜16.5 m）に向かって走り、区間 3 の途中で当たります。以降は壁に押し付けられたまま計画が流れ、ゴールの 16.3 m 手前で終わります。
図3：同じ経路（橙）を、開ループ（左・赤）と閉ループ（右・青）で走らせた Husky の軌跡。左は最初の角で回りきれず壁に当たって止まる。右は隙間を抜けてゴール（星）まで届く。灰色が壁、薄茶がぬかるみ帯
前回の記事で「Husky にはこの式は係数調整では合わない」と書きました。それは、開ループでは合わない、という意味でした。
向きを測って直す（閉ループ）：同じ式で 7 cm
開ループの計画は「45° 回すには 0.785 秒」と、回す時間を式で決めていました。式の前提が崩れると、この時間が間違います。
閉ループでは時間を決めません。0.05 秒ごとに「いまどこを向いているか」を測り、次の通過点との向きのずれから (v, ω) を決め直します。90° を向くまで回り続けるので、ロボットが式の半分でしか回らなくても、いずれ 90° に届きます。
import numpy as np
from planner import make_sample_map, astar, path_to_waypoints
from husky_sim import HuskySim
TREAD = 0.555 # 左右車輪の間隔 [m]（前回と同じ）
V_MAX = 1.0 # 車輪の周速の上限 [m/s]（前回と同じ）
V_CRUISE = 0.8 # 巡航の前進速度 [m/s]
K_OMEGA = 2.0 # 向きのずれ [rad] を旋回角速度 [rad/s] に変える係数（後述の比例制御のゲイン）
ANGLE_GATE = np.radians(30) # 向きがこれ以上ずれていたら、止まって回る
R_SWITCH = 0.5 # 通過点にここまで近づいたら次の点へ [m]
CTRL_HZ = 20 # 制御周期（0.05 秒ごとに姿勢を測って車輪速を出し直す）
# ---- 前回（逆運動学）の再掲：この 2 つは 1 文字も変えていない ----
def wheel_speeds(v: float, omega: float, tread: float = TREAD) -> tuple[float, float]:
"""前進速度 v と旋回角速度 omega から、左右の車輪の周速 [m/s] を求める（逆運動学の 2 行）。"""
return v - omega * tread / 2.0, v + omega * tread / 2.0
def limit_scale(v_l: float, v_r: float, v_max: float = V_MAX) -> tuple[float, float]:
"""上限を超えたら、左右を同じ比率で縮める（前回の (b) 比率保持）。"""
biggest = max(abs(v_l), abs(v_r))
if biggest <= v_max:
return v_l, v_r
scale = v_max / biggest
return v_l * scale, v_r * scale
ここまでが前回です。今回足すのは、次の2つです。
# ---- 今回の追加：姿勢を測って、次の通過点を向く ----
def wrap(angle: float) -> float:
"""角度を -π〜π に畳む（350° 右回りではなく 10° 左回りにする）。"""
return (angle + np.pi) % (2 * np.pi) - np.pi
def steer(pose: np.ndarray, target: np.ndarray) -> tuple[float, float]:
"""いまの姿勢から目標点を向いて進むための (v, omega) を返す。"""
dx, dy = target - pose[:2]
heading_err = wrap(np.arctan2(dy, dx) - pose[2]) # 目標の方向 − いまの向き
omega = K_OMEGA * heading_err # ずれが大きいほど強く回す
v = V_CRUISE if abs(heading_err) < ANGLE_GATE else 0.0 # 大きくずれていたら止まって回る
return v, omega
steer がやっていることは3行です。
- 目標点の方向と、いまの向きの差（向きのずれ）を出す
- ずれに比例した ω を出す（ずれ 90° なら 3.14 rad/s、ずれ 10° なら 0.35 rad/s）。これを比例制御（P制御：ずれの大きさに比例した操作量を返す、いちばん単純な制御）と呼びます
- ずれが 30° 未満なら巡航速度で進み、それ以上なら止まって回る
wrap は角度の畳み込みです。いまの向きが 170°、目標が -170° のとき、素直に引くと -340° になって右へ大回りします。畳むと +20° になり、左へ少し回るだけで済みます。
図4：開ループと閉ループの違いは「何秒回すか」を式で決めるかどうか。式は閉ループでも使いますが、決めるのは向きであって時間ではありません
通過点を順にたどる部分は、小さなクラスにします。
class Follower:
"""通過点の列を順にたどる。step() を制御周期ごとに呼ぶ。"""
def __init__(self, waypoints: np.ndarray) -> None:
self.wps = waypoints
self.i = 1 # 0 番は出発点なので、最初の目標は 1 番
self.done = False
def step(self, pose: np.ndarray) -> tuple[float, float]:
"""姿勢を受け取り、左右の車輪速 [m/s] を返す。"""
if self.done:
return 0.0, 0.0
target = self.wps[self.i]
last = self.i == len(self.wps) - 1
if np.hypot(*(target - pose[:2])) < (0.1 if last else R_SWITCH):
if last:
self.done = True # ゴールの 10 cm 以内で止まる
return 0.0, 0.0
self.i += 1
target = self.wps[self.i]
v, omega = steer(pose, target)
return limit_scale(*wheel_speeds(v, omega))
def run_husky() -> None:
grid, start, goal = make_sample_map()
path, _, _ = astar(grid, start, goal, cut_corners=False)
wps = path_to_waypoints(path)
yaw0 = np.arctan2(*(wps[1] - wps[0])[::-1]) # 最初の通過点を向く角度（引数は dy, dx の順）
sim = HuskySim(grid, wps[0], yaw0)
follower = Follower(wps)
t = 0.0
while not follower.done and t < 120.0:
v_l, v_r = follower.step(sim.pose()) # 測る → 決める
sim.set_wheels(v_l, v_r) # 渡す
sim.step(1.0 / CTRL_HZ) # 0.05 秒進める
t += 1.0 / CTRL_HZ
x, y, th = sim.pose()
print(f"到達={follower.done} {t:.1f} 秒 ゴール誤差 {np.hypot(x - wps[-1][0], y - wps[-1][1]):.3f} m"
f" 壁に触れたステップ {sim.hits}")
sim.close()
if __name__ == "__main__":
run_husky()
step の最後の1行を見てください。steer が出した (v, ω) を、前回の wheel_speeds に通し、前回の limit_scale に通しているだけです。ずれが 90° のとき ω = 3.14 rad/s なので車輪速は ±0.87 m/s、ずれが 180° なら ±1.74 m/s で上限 1.0 を超えます。そこは limit_scale が同じ比率で縮めます。
到達=True 56.7 秒 ゴール誤差 0.073 m 壁に触れたステップ 0
ゴール誤差 7.3 cm、壁に触れたステップ 0。 開ループで 16 m 手前だった同じロボット・同じ経路・同じ式です。
図5：最初の角（45° → 90°）での車体の向き。角の手前 1 m から測り直したもの。赤の開ループは 0.785 秒（薄赤の帯）で回すのをやめ、約 68° のまま直進して壁に当たる。青の閉ループは角の 0.5 m 手前（R_SWITCH）から回り始め、3.8 秒かけて 90° に届く
図5 が、この記事の要点そのものです。青は式の予測（0.785 秒）の 約 5 倍の時間をかけて 90° に届いています。時間は式で決めていないので、遅くても届きます。
理想の差動二輪でも、同じ追従器で走らせる
追従器が Husky 専用のものになっていないか確かめるため、前回の順運動学 step_exact（一定入力のあいだ車体は円弧を描く、という閉じた式）で動く理想の差動二輪にも、同じ Follower をつなぎました。sim.pose() の代わりに step_exact で姿勢を進めるだけで、follow.py は変えていません。
| 理想の差動二輪（前回の式） | PyBullet の Husky | |
|---|---|---|
| 開ループ | 誤差 0（前回のとおり） | 最初の角で壁・16.3 m 手前 |
| 閉ループ | 52.8 秒・ゴール誤差 0.082 m | 56.7 秒・ゴール誤差 0.073 m |
| 閉ループの経路からの最大ずれ | 0.33 m | 0.35 m |
| 壁に触れたステップ | 0 | 0 |
表1：同じ Follower を2つの車体につないだ結果。式どおりに回る理想の車体と、半分でしか回らない Husky で、閉ループの結果はほぼ同じ
図6：閉ループで走ったときの、経路（折れ線）からのずれ。ずれが立つのは角で、直進中はゼロに戻る。最大 0.35 m は隘路の余裕 0.41 m（隙間 1.5 m − 車幅 0.69 m の半分）の内側。隘路そのものを通っている間のずれは 0.12 m 以内でした
図6 で、Husky（青）は角のあとに 0.2〜0.25 m のゆるい山があります。角で回りきれずに外側へ膨らみ、次の通過点へ向かって斜めに戻っている形です。理想の車体（緑）は角でずれてから直線的に戻ります。どちらも「次の点を向く」だけの追従なので、線に乗り直す動きはしていません（後述）。
なぜ式が違っても着けるのか
開ループの式は、「45° 回すには 0.785 秒」というように 量を時間に翻訳していました。翻訳の係数（Husky なら「この ω を指令すると実際はいくつで回るか」）が違うと、答えがそのまま違います。しかも順運動学の回で測ったとおり、Husky の旋回角速度は予測 0.72 rad/s に対して 0.14〜0.39 rad/s の間を動き続けるので、係数を 1 つ直しても合いません。
閉ループの式は、向きを車輪速に翻訳する役に降りています。「左に回れ」という指令の向きさえ合っていれば、量が半分でも、次の 0.05 秒でまた測って「まだ足りない、左に回れ」と出し直します。係数が違うぶんは、回る時間が伸びる形で吸収されます。
図5 の青がその実物です。ω の指令は K_OMEGA × ずれ なので、ずれが縮むにつれて指令も小さくなり、なめらかに 90° へ寄ります。Husky が指令の半分でしか回らないことは、実質的に K_OMEGA が半分になったのと同じで、届くまでの時間が延びるだけです。
これは、ロボットアームで手先の位置を測りながら関節を少しずつ動かす微分逆運動学と同じ考え方です。運動学の式が完全でなくても、測って直す輪の中に入れれば使えます。
この記事の式でできないこと
点を向くだけなので、線には乗りません。 steer は「次の通過点の方向」しか見ていません。角で外側に膨らんだあと、経路の線に戻るのではなく、次の点へ斜めに向かいます。図6 の 0.25 m の山がそれです。今回の隘路は余裕 0.41 m なので通れましたが、余裕が 0.2 m の隘路なら当たります。線からのずれも見て戻す方法（純追跡法・Pure Pursuit や Stanley 法と呼ばれる追従則）は、この記事の外です。
姿勢は PyBullet から貰っています。 sim.pose() は getBasePositionAndOrientation で、シミュレータが知っている真の位置と向きです。実機にはこれがありません。「車輪の回転から積分して自分の位置を推定する」（オドメトリ：odometry）で代用したくなりますが、Husky ではそれが効きません。試しに sim.pose() の代わりに、前回の step_exact で指令 (v, ω) を積分した推定姿勢を Follower に渡すと、「到達=True」と表示されるのに、実際の Husky はゴールの 16.3 m 手前で壁に押し付けられていました（壁に触れたステップ 6891）。推定は「45° 回した」と思い、実物は 20° しか回っていないからです。閉ループは、測る手段が本物であるときにだけ効きます。実機なら、外部のカメラや LiDAR による自己位置推定が要ります。
ゴールの向きは合わせていません。 前回の記事は姿勢 (x, y, θ) の3つを狙いましたが、今回のゴールは点 (x, y) です。最後の通過点の 10 cm 以内に入ったら止めるだけで、θ は成り行きです。向きも合わせるなら、前回の「回転→直進→回転」の最後の回転を閉ループで足せば届きます。
地図は最初に1回だけ読んでいます。 走っている途中に障害物が現れても、経路を引き直しません。
まとめ
- A* の経路を車体のあるロボットに渡すと、経路計画の側にも問題が出ます。 斜め移動が壁の角をかすめる（23 → 25 セル）、マスの大きさが車体より小さい（1.0 → 1.5 m）の2つでした
- 計画をそのまま流す（開ループ）と、Husky は最初の角で 45° 指令に 20.5° しか回らず、壁に当たります。 式が悪いのではなく、「何秒回すか」を式で決めるやり方が、式の前提が崩れたロボットには使えません
- 0.05 秒ごとに姿勢を測り、次の通過点を向くように (v, ω) を出し直す（閉ループ）と、同じ式のまま 56.7 秒でゴール誤差 7.3 cm、壁接触 0 で着きます。 式は「向きを車輪速に翻訳する役」に降り、量のずれは時間で吸収されます
- 理想の差動二輪でも同じ追従器で 8.2 cm。追従器は車体を選びません
- シミュレータのモデルの寸法は URDF を読んで確かめます。Husky の車輪半径はカタログ 0.1651 m に対して URDF は 0.17775 m で、7.7% 違いました
- 閉ループが効くのは、姿勢を本物で測れるときだけです。車輪の積分で代用すると「到達=True」のまま 16 m 手前で止まります
まずこれだけやってみてください。 follow.py の R_SWITCH = 0.5 を 0.2 にして実行する。「角のぎりぎりまで行ってから曲がるほうが経路に忠実になるはず」と私は予想しましたが、Husky では最大ずれが 0.35 → 0.39 m に増え、壁に 292 ステップ触れます（理想の差動二輪では 0.33 → 0.13 m に減ります）。角に近づいてから回り始めると、半分でしか回らない Husky は外側へ大きく膨らむからです。次に K_OMEGA = 2.0 を 5.0 にすると、Husky は最大ずれ 0.26 m・54.1 秒に改善しますが、理想の車体では 0.38 m に悪化します。調整は式のモデルではなく、走らせる車体でやる、というのが2つの実験の共通点です。
これで、移動ロボットの3本（順運動学・逆運動学・経路をたどる）が閉じました。本シリーズの全体像は、まとめ記事から辿れます。
次回（9/30 公開予定）は4足ロボットに移り、脚1本＝3自由度の順運動学（股・膝・足首の角度から足先の位置を求める）から始めます。移動ロボットで「車輪の式」から始めたのと同じ順で、4足も「脚の式」から入ります。
役に立ったら いいね・ストック で応援いただけると、次回の励みになります。