A*で経路を計画しEKFで自己位置推定する差動二輪ロボット

作るもの

このプロジェクトでは、車輪型ロボット向けの最小限で完結したナビゲーションスタックを、NumPyのファイル1つで構築します。差動二輪ロボットは、内壁2枚と箱1つがある10 m × 10 mの部屋の一隅から出発します。ロボットは占有格子地図上でA*を使い、反対側の隅までの経路を計画します。その経路にPure Pursuitコントローラで追従し、拡張カルマンフィルタ（EKF）で自身の姿勢を推定します。フィルタは、滑らかだがドリフトする車輪オドメトリと、ノイズは大きいがドリフトしない位置観測を融合します。

コントローラは真の姿勢ではなく、フィルタの推定値に基づいて操舵します。実機のロボットもこの方法で動くしかありません。シミュレーションでは、推定値を評価できるよう真の姿勢を別に保持しています。このページの実行例では、ロボットは69.8秒でゴールに到達します。走行全体を通じてEKFの誤差は真値から0.151 m以内に収まります。一方、オドメトリ単独の推定は、最終的に実際の位置から5.6 mずれます。

差動二輪、グリッド上の経路計画、センサとノイズの各レッスンで、構成要素を一つずつ扱っています。式はカルマンフィルタ早見表にまとめてあります。

必要なもの

• Python 3.10以降とNumPy（pip install numpy）。当サイトではNumPy 2.5.4を使い、Apple Silicon搭載のMac miniで実行しました。
• ほかには何も要りません。シミュレータもプロットライブラリも使わず、シードを固定していない乱数もありません。
• 設計を読み、実行し、バリエーションを試すのに約3時間。

設計

地図、膨張処理、A*

部屋は0.1 mのセルを100 × 100並べたグリッドで、grid[row, col]で参照します（row = y / 0.1、col = x / 0.1）。ワールド座標系はREP 103に従います。xは右、yは上、zは床面から外向き、角度はx軸から反時計回りです。ロボットは半径0.20 mの円盤です。すべての障害物をこの半径だけ膨らませるとロボットを点として扱えるので、A*はグリッドをそのまま使って計画できます。この膨張させたグリッドは、円盤型ロボットのコンフィギュレーション空間です。コントローラが操舵に使う推定値は0.1 m以上ずれることがあるため、さらに0.15 mの余裕を加えます。この余裕がないと計画経路が膨張後の壁に接し、推定誤差だけで実際のロボットが壁にぶつかることがあります。この現象は「試してみること」の節で確認できます。

Aは8近傍の移動を使い、直進のコストを1、斜め移動のコストを√2とします。ヒューリスティックはオクタイル距離で、障害物のないグリッド上での8近傍最短経路の正確なコストです。過大評価しないため、Aはグリッド上の最短経路を返します。斜め移動は、その間をすり抜ける2つのセルのどちらかが占有されていれば禁止されます。そのため経路が壁の角を横切ることはありません。この地図では、A*は10,000セルのうち4,304セルを展開し、長さ20.15 mの経路を返します。

Pure Pursuit

Pure Pursuitは、経路上でロボットの0.5 m先にある点を選び、その点に到達する円弧に沿って走行します。その点がロボット座標系（x前方、y左）で(x_r, y_r)にあるとき、円弧の曲率はκ = 2 y_r / (x_r² + y_r²)です。角速度はω = v κです。前進速度は直線区間で0.5 m/sとし、旋回中は1 / (1 + 1.5 |κ|)倍に下げます。前方注視距離を長くすると経路は滑らかになりますが、角を大きく内側にショートカットします。短くすると経路に密着して追従しますが、振動しやすくなります。この関数は直前の最近傍点から40点先までしか探索しません。そのため、近くを通る経路の後半部分にロボットが飛び移ることはありません。

運動、ノイズ、EKF

状態は(x, y, θ)です。50 msの各ステップで、コントローラは(v, ω)を、ホイールベース0.30 mでの左右の車輪移動量d_lとd_rに変換します。運動モデルには中点法を使います。ロボットは旋回の中間時点の方位に沿って(d_l + d_r) / 2だけ進み、方位は(d_r - d_l) / 0.30だけ変化します。

シミュレーション上のロボットには2種類のオドメトリ誤差があります。エンコーダは各車輪の移動量に、平均ゼロで走行距離の平方根あたり0.01 m（0.01 m/√m）のノイズを加えて報告します。そのため分散は距離とともに増えます。また、実際の右車輪はソフトウェアの想定より0.5%大きくなっています。これは摩耗したタイヤや空気圧の低いタイヤのような系統誤差です。EKFは1つ目の誤差を知っていますが、2つ目は知りません。実機のロボットでも通常はこの状況です。

予測。 フィルタは計測した車輪移動量を使い、同じ運動モデルで平均を移動させます。共分散はP ← F P Fᵀ + G M Gᵀで伝播させます。Fは状態に関する運動のヤコビ行列です。Gは2つの車輪移動量に関するヤコビ行列です。Mはこのステップの車輪ノイズの対角共分散行列です。こうすると、ノイズは物理的に入り込む場所である車輪で加えられます。そしてヤコビ行列が、それをx、y、θの正しい相関付き不確かさに変換します。

更新。 0.5秒ごとに位置観測が届きます。真のxとyに、各軸0.1 mのガウスノイズを加えたものです。実機では、超広帯域無線のビーコンシステム、モーションキャプチャシステム、固定マーカーを捉えるカメラなどから得られます。観測モデルはz = H x、H = [[1, 0, 0], [0, 1, 0]]の線形モデルなので、更新は通常のカルマンフィルタの更新になります。位置観測は方位を計測しません。それでもフィルタは、予測ステップで蓄積された位置と方位の相関を通じて方位も補正します。

2つの座標系、2つの推定値。 移動ロボットの座標系に関するROSの規約であるREP 105が、まさにこの区別を定めています。オドメトリのみの姿勢は、ROSでodom座標系と呼ぶものです。連続で滑らかですが、際限なくドリフトし得ます。EKFで融合した姿勢はmap座標系に対応します。ドリフトしませんが、位置観測のたびに少し跳びます。違いが分かるよう、プログラムは両方を追跡します。

フィルタは正直か。 各ステップで、EKFの2 × 2位置共分散のもとでの真の位置誤差の二乗マハラノビス距離を計算します。フィルタの共分散が正しければ、この値は約95%のステップで5.99（自由度2のカイ二乗分布の95%点）を下回ります。95%を大きく下回れば、フィルタは自信過剰です。大きく上回れば、慎重すぎます。

コード

これをastar_ekf.pyとして保存します。地図、プランナ、ロボットモデル、フィルタ、コントローラ、閉ループをまとめた1つのブロックです。

# astar_ekf.py: plan with A*, follow with pure pursuit, localise with an EKF.
import heapq
import math

import numpy as np

rng = np.random.default_rng(7)

# ---------- 1. Map: a 10 m x 10 m room on a 0.1 m grid ----------
RES = 0.1                                  # metres per cell
N = 100                                    # cells per side
grid = np.zeros((N, N), dtype=bool)        # grid[row, col], row = y / RES, col = x / RES
grid[0, :] = grid[-1, :] = grid[:, 0] = grid[:, -1] = True     # outer walls
grid[0:65, 30:33] = True                   # wall from the bottom, gap at the top
grid[35:100, 60:63] = True                 # wall from the top, gap at the bottom
grid[70:80, 75:90] = True                  # a box near the goal

ROBOT_RADIUS = 0.20                        # m
SAFETY_MARGIN = 0.15                       # m, extra room for localisation error


def inflate(occ: np.ndarray, radius_m: float) -> np.ndarray:
    """Grow obstacles by the robot radius so the planner can treat the robot as a point."""
    r = math.ceil(radius_m / RES)
    out = occ.copy()
    rows, cols = np.nonzero(occ)
    for dr in range(-r, r + 1):
        for dc in range(-r, r + 1):
            if dr * dr + dc * dc <= r * r:
                rr = np.clip(rows + dr, 0, N - 1)
                cc = np.clip(cols + dc, 0, N - 1)
                out[rr, cc] = True
    return out


def astar(occ: np.ndarray, start: tuple[int, int], goal: tuple[int, int]) -> list[tuple[int, int]]:
    """8-connected A* with the octile-distance heuristic. Cells are (row, col)."""
    moves = [(-1, 0, 1.0), (1, 0, 1.0), (0, -1, 1.0), (0, 1, 1.0),
             (-1, -1, math.sqrt(2)), (-1, 1, math.sqrt(2)), (1, -1, math.sqrt(2)), (1, 1, math.sqrt(2))]

    def h(c: tuple[int, int]) -> float:
        dr, dc = abs(c[0] - goal[0]), abs(c[1] - goal[1])
        return (dr + dc) + (math.sqrt(2) - 2) * min(dr, dc)

    g = {start: 0.0}
    parent = {start: None}
    open_heap = [(h(start), 0.0, start)]
    closed = set()
    while open_heap:
        _, g_cur, cur = heapq.heappop(open_heap)
        if cur in closed:
            continue
        if cur == goal:
            path = [cur]
            while parent[path[-1]] is not None:
                path.append(parent[path[-1]])
            print(f"A*: expanded {len(closed)} cells, path cost {g_cur * RES:.2f} m")
            return path[::-1]
        closed.add(cur)
        for dr, dc, cost in moves:
            nb = (cur[0] + dr, cur[1] + dc)
            if not (0 <= nb[0] < N and 0 <= nb[1] < N) or occ[nb]:
                continue
            if dr and dc and (occ[cur[0] + dr, cur[1]] or occ[cur[0], cur[1] + dc]):
                continue                    # no cutting corners past an obstacle
            g_new = g_cur + cost
            if g_new < g.get(nb, math.inf):
                g[nb] = g_new
                parent[nb] = cur
                heapq.heappush(open_heap, (g_new + h(nb), g_new, nb))
    raise RuntimeError("no path")


def to_cell(x: float, y: float) -> tuple[int, int]:
    return int(y / RES), int(x / RES)


def to_xy(cell: tuple[int, int]) -> np.ndarray:
    return np.array([(cell[1] + 0.5) * RES, (cell[0] + 0.5) * RES])


start_xy, goal_xy = (1.0, 1.0), (9.0, 9.0)
occ = inflate(grid, ROBOT_RADIUS + SAFETY_MARGIN)
cells = astar(occ, to_cell(*start_xy), to_cell(*goal_xy))
path = np.array([to_xy(c) for c in cells])

# ---------- 2. Robot, sensors and the EKF ----------
DT = 0.05                  # control period (s)
WHEEL_BASE = 0.30          # m
V_MAX = 0.5                # m/s
LOOKAHEAD = 0.5            # m, pure pursuit
ODO_SIGMA = 0.01           # wheel distance noise, m per sqrt(m) travelled
RIGHT_WHEEL_SCALE = 1.005  # true right wheel is 0.5 % larger than the odometry assumes
FIX_SIGMA = 0.10           # position fix noise (m), 1 sigma per axis
FIX_EVERY = 10             # one fix every 10 steps = 0.5 s


def motion(state: np.ndarray, d_l: float, d_r: float) -> np.ndarray:
    """Midpoint integration of a differential drive for wheel travels d_l, d_r."""
    x, y, th = state
    ds, dth = (d_r + d_l) / 2, (d_r - d_l) / WHEEL_BASE
    return np.array([x + ds * math.cos(th + dth / 2), y + ds * math.sin(th + dth / 2), th + dth])


def ekf_predict(mu: np.ndarray, P: np.ndarray, d_l: float, d_r: float) -> tuple[np.ndarray, np.ndarray]:
    ds, dth = (d_r + d_l) / 2, (d_r - d_l) / WHEEL_BASE
    a = mu[2] + dth / 2
    F = np.array([[1, 0, -ds * math.sin(a)],
                  [0, 1, ds * math.cos(a)],
                  [0, 0, 1]])
    # Jacobian of the motion with respect to (d_l, d_r)
    G = np.array([[0.5 * math.cos(a) + ds * math.sin(a) / (2 * WHEEL_BASE), 0.5 * math.cos(a) - ds * math.sin(a) / (2 * WHEEL_BASE)],
                  [0.5 * math.sin(a) - ds * math.cos(a) / (2 * WHEEL_BASE), 0.5 * math.sin(a) + ds * math.cos(a) / (2 * WHEEL_BASE)],
                  [-1 / WHEEL_BASE, 1 / WHEEL_BASE]])
    M = np.diag([ODO_SIGMA ** 2 * abs(d_l) + 1e-6, ODO_SIGMA ** 2 * abs(d_r) + 1e-6])
    return motion(mu, d_l, d_r), F @ P @ F.T + G @ M @ G.T


def ekf_update(mu: np.ndarray, P: np.ndarray, z: np.ndarray) -> tuple[np.ndarray, np.ndarray]:
    H = np.array([[1.0, 0, 0], [0, 1.0, 0]])
    R = np.eye(2) * FIX_SIGMA ** 2
    S = H @ P @ H.T + R
    K = P @ H.T @ np.linalg.inv(S)
    mu = mu + K @ (z - H @ mu)
    P = (np.eye(3) - K @ H) @ P
    return mu, P


def pure_pursuit(pose: np.ndarray, path: np.ndarray, idx: int) -> tuple[float, float, int]:
    """Return (v, omega, nearest index) steering towards a point LOOKAHEAD metres along the path."""
    window = path[idx:idx + 40]
    idx = idx + int(np.argmin(np.linalg.norm(window - pose[:2], axis=1)))
    target = path[-1]
    for p in path[idx:]:
        if np.linalg.norm(p - pose[:2]) >= LOOKAHEAD:
            target = p
            break
    dx, dy = target - pose[:2]
    c, s = math.cos(pose[2]), math.sin(pose[2])
    x_r, y_r = c * dx + s * dy, -s * dx + c * dy          # target in the robot frame
    curvature = 2 * y_r / max(x_r ** 2 + y_r ** 2, 1e-9)
    v = V_MAX / (1 + 1.5 * abs(curvature))               # slow down in tight turns
    return v, v * curvature, idx


# ---------- 3. Closed loop: control on the EKF estimate ----------
start_heading = math.atan2(*(path[5] - path[0])[::-1])
truth = np.array([*start_xy, start_heading])
mu, P = truth.copy(), np.diag([0.01, 0.01, 0.01])
dead = truth.copy()                                       # odometry only, for comparison
idx, k = 0, 0
log = []
while np.linalg.norm(truth[:2] - goal_xy) > 0.15 and k < 4000:
    v, w, idx = pure_pursuit(mu, path, idx)
    d_l_cmd, d_r_cmd = (v - w * WHEEL_BASE / 2) * DT, (v + w * WHEEL_BASE / 2) * DT
    truth = motion(truth, d_l_cmd, d_r_cmd * RIGHT_WHEEL_SCALE)   # what the robot really did
    d_l_meas = d_l_cmd + rng.normal(0, ODO_SIGMA * math.sqrt(abs(d_l_cmd)))
    d_r_meas = d_r_cmd + rng.normal(0, ODO_SIGMA * math.sqrt(abs(d_r_cmd)))
    dead = motion(dead, d_l_meas, d_r_meas)
    mu, P = ekf_predict(mu, P, d_l_meas, d_r_meas)
    if k % FIX_EVERY == 0:
        z = truth[:2] + rng.normal(0, FIX_SIGMA, 2)
        mu, P = ekf_update(mu, P, z)
    k += 1
    in_2sigma = (truth[:2] - mu[:2]) @ np.linalg.inv(P[:2, :2]) @ (truth[:2] - mu[:2]) < 5.99
    log.append((k * DT, np.linalg.norm(truth[:2] - mu[:2]), np.linalg.norm(truth[:2] - dead[:2]), in_2sigma,
                *truth[:2]))

log = np.array(log)
t, e_ekf, e_dead = log[:, 0], log[:, 1], log[:, 2]
occupied_xy = np.argwhere(grid)[:, ::-1] * RES + RES / 2               # centres of occupied cells
gap = min(np.min(np.linalg.norm(occupied_xy - p, axis=1)) for p in log[:, 4:6]) - RES / 2 - ROBOT_RADIUS
print(f"reached goal: {np.linalg.norm(truth[:2] - goal_xy) <= 0.15} in {t[-1]:.1f} s, {k} steps")
print(f"smallest gap between the robot's edge and an obstacle: {gap:.3f} m")
print(f"EKF position error:      mean {e_ekf.mean():.3f} m, rms {np.sqrt((e_ekf ** 2).mean()):.3f} m, max {e_ekf.max():.3f} m")
print(f"odometry-only error:     mean {e_dead.mean():.3f} m, rms {np.sqrt((e_dead ** 2).mean()):.3f} m, max {e_dead.max():.3f} m, final {e_dead[-1]:.3f} m")
print(f"truth inside EKF 95 % ellipse: {100 * log[:, 3].mean():.1f} % of steps")
print("planned path, every 6th cell (x,y in m):")
print(" ".join(f"{x:.2f},{y:.2f}" for x, y in [*path[::6], path[-1]]))
print("every 2 s: t (s), EKF error (m), odometry-only error (m), true x (m), true y (m)")
for i in range(39, len(t), 40):
    print(f"{t[i]:.1f} {e_ekf[i]:.3f} {e_dead[i]:.3f} {log[i, 4]:.2f} {log[i, 5]:.2f}")

閉ループはコードのセクション3にあるwhileループです。処理の順序が重要です。まずコントローラが推定値を読み取ります。真のロボットは、指令された車輪移動量で動きます（右車輪にはスケールを掛けます）。次にエンコーダが、指令移動量にノイズを加えた値を報告します。フィルタはそのエンコーダ値から予測し、10ステップごとに位置観測で更新します。真値は、位置観測の生成と推定値の評価にだけ使います。

実行

python -m pip install numpy
python astar_ekf.py

1秒もかからずに終わります。

テスト

以下は当サイトでの実行結果です。シードを7に固定しているので、同じ数値が得られるはずです。NumPyのバージョンが異なると乱数列が変わり、末尾の桁が変わることがあります。

A*: expanded 4304 cells, path cost 20.15 m
reached goal: True in 69.8 s, 1395 steps
smallest gap between the robot's edge and an obstacle: 0.082 m
EKF position error:      mean 0.051 m, rms 0.057 m, max 0.151 m
odometry-only error:     mean 1.922 m, rms 2.520 m, max 5.639 m, final 5.639 m
truth inside EKF 95 % ellipse: 95.3 % of steps
planned path, every 6th cell (x,y in m):
1.05,1.05 1.15,1.65 1.15,2.25 1.15,2.85 1.15,3.45 1.15,4.05 1.55,4.65 2.15,5.25 2.55,5.85 2.55,6.45 2.95,6.95 3.55,6.85 4.15,6.25 4.75,5.65 4.85,5.05 5.05,4.45 5.35,3.85 5.65,3.25 6.25,3.05 6.75,3.45 6.75,4.05 7.25,4.65 7.85,5.25 8.45,5.85 9.05,6.45 9.45,7.05 9.45,7.65 9.45,8.25 9.25,8.85 9.05,9.05
every 2 s: t (s), EKF error (m), odometry-only error (m), true x (m), true y (m)
2.0 0.049 0.019 1.15 1.77
4.0 0.048 0.032 1.20 2.58
6.0 0.068 0.033 1.20 3.37
8.0 0.053 0.063 1.22 4.03
10.0 0.027 0.103 1.39 4.47
12.0 0.098 0.227 1.84 5.14
14.0 0.146 0.396 2.26 5.72
16.0 0.054 0.573 2.49 6.34
18.0 0.073 0.695 2.66 6.74
20.0 0.053 0.792 2.92 6.96
22.0 0.046 0.865 3.19 7.03
24.0 0.075 0.911 3.52 6.96
26.0 0.063 0.950 3.95 6.67
28.0 0.034 1.028 4.45 6.11
30.0 0.049 1.121 4.73 5.66
32.0 0.072 1.204 4.88 5.06
34.0 0.091 1.376 5.08 4.61
36.0 0.066 1.640 5.37 4.15
38.0 0.021 1.872 5.53 3.59
40.0 0.012 2.072 5.71 3.25
42.0 0.024 2.254 5.96 3.09
44.0 0.049 2.417 6.24 3.05
46.0 0.022 2.525 6.49 3.14
48.0 0.036 2.567 6.67 3.36
50.0 0.059 2.537 6.75 3.71
52.0 0.029 2.514 6.83 4.17
54.0 0.106 2.698 7.14 4.65
56.0 0.054 3.072 7.63 5.18
58.0 0.030 3.672 8.31 5.79
60.0 0.078 4.194 8.81 6.35
62.0 0.046 4.685 9.24 6.89
64.0 0.104 4.935 9.37 7.27
66.0 0.061 5.266 9.39 8.01
68.0 0.048 5.522 9.35 8.53

ロボットはゴールに到達し、車体の縁とすべての障害物との間に少なくとも0.082 mの間隔を保ちます。EKFの位置誤差は平均0.051 mで、0.151 mを超えることはありません。オドメトリ単独でも最初の6秒は良好で、実はフィルタより精度が高いくらいです。数メートルの範囲なら、車輪のほうが0.1 mの位置観測より正確だからです。その後は方位誤差が増え、それにつれて位置誤差も増え、ゴールでは5.6 mに達します。真値は95.3%のステップでEKFの95%楕円の内側に入っています。フィルタは車輪径の誤差をモデル化していませんが、共分散は実際の誤差を妥当に表しています。頻繁な位置観測が、このバイアスの蓄積を防いでいるためです。

ロボットがゴールに到達しない場合は、まずA*の行を確認してください。経路コストが20 mを大きく超える場合は、地図か膨張半径が変わっています。最小間隔が負の場合は、ロボットのフットプリントがどこかで障害物と重なったことを意味します。

実機で動かす場合

このプログラムの各部分には、ハードウェア上に直接対応するものがあります。そして、それぞれに固有の問題があります。

• エンコーダ。 得られるのはメートルではなくティック数です。車輪移動量はticks / ticks_per_rev × 2π × 車輪半径です。計測した距離の直線走行と、計測した回数のその場旋回を行い、車輪半径とホイールベースを較正してください。ここでモデル化した0.5%の誤差は、較正後に残る種類の誤差です。
• 車輪のスリップ。 滑りやすい床や急加速時には、ロボットが動かないまま車輪だけが回ります。EKFにはそれを知る手段がありません。加速度を制限し、急な操作中はフィルタのMを大きくしてください。
• ジャイロ。 多くの移動ロボットは慣性計測装置（IMU）のヨーレートジャイロを追加します。オドメトリがドリフトする原因は方位誤差だからです。ジャイロの角速度は、予測ステップの2つ目の入力として扱うか、θの観測として扱います。
• 位置観測は遅れて届く。 実際の位置観測のタイムスタンプは、現在の姿勢より古くなります。過去の状態をバッファしておき正しい時刻で更新を適用するか、ロボットの速度に比べて遅延を小さく保ってください。
• 地図は事前にわからない。 ここではグリッドが与えられています。実機では地図は同時自己位置推定と地図作成（SLAM）から得られ、「位置観測」はSLAMの姿勢そのものであることが多いです。

以下のブロックは、シリアルポート経由でモータコントローラからエンコーダのカウントを読み取り、それに対してEKFの予測ステップを実行します。実機のロボット、pyserialパッケージ、一定周期でE <left_ticks> <right_ticks>を出力するファームウェアが必要です。このプロトコルは一例であり、標準ではありません。定数はお使いのロボットに合わせて置き換えるためのプレースホルダです。

# needs-hardware
import math

import numpy as np
import serial  # pyserial

TICKS_PER_REV = 2048          # encoder counts per wheel revolution, from your motor's datasheet
WHEEL_RADIUS = 0.035          # m, measure it
WHEEL_BASE = 0.30             # m, calibrate it
ODO_SIGMA = 0.01


def motion(state, d_l, d_r):
    x, y, th = state
    ds, dth = (d_r + d_l) / 2, (d_r - d_l) / WHEEL_BASE
    return np.array([x + ds * math.cos(th + dth / 2), y + ds * math.sin(th + dth / 2), th + dth])


def odometry_loop(port: str = "/dev/ttyACM0") -> None:
    mu, P = np.zeros(3), np.diag([0.01, 0.01, 0.01])
    last = None
    with serial.Serial(port, 115200, timeout=1) as link:
        while True:
            parts = link.readline().decode(errors="ignore").split()
            if len(parts) != 3 or parts[0] != "E":
                continue
            ticks = np.array([int(parts[1]), int(parts[2])])
            if last is not None:
                d_l, d_r = (ticks - last) / TICKS_PER_REV * 2 * math.pi * WHEEL_RADIUS
                ds, dth = (d_r + d_l) / 2, (d_r - d_l) / WHEEL_BASE
                a = mu[2] + dth / 2
                F = np.array([[1, 0, -ds * math.sin(a)], [0, 1, ds * math.cos(a)], [0, 0, 1]])
                G = np.array([[0.5 * math.cos(a) + ds * math.sin(a) / (2 * WHEEL_BASE), 0.5 * math.cos(a) - ds * math.sin(a) / (2 * WHEEL_BASE)],
                              [0.5 * math.sin(a) - ds * math.cos(a) / (2 * WHEEL_BASE), 0.5 * math.sin(a) + ds * math.cos(a) / (2 * WHEEL_BASE)],
                              [-1 / WHEEL_BASE, 1 / WHEEL_BASE]])
                M = np.diag([ODO_SIGMA ** 2 * abs(d_l) + 1e-6, ODO_SIGMA ** 2 * abs(d_r) + 1e-6])
                mu, P = motion(mu, d_l, d_r), F @ P @ F.T + G @ M @ G.T
                print(f"x={mu[0]:.3f} y={mu[1]:.3f} th={mu[2]:.3f} sigma_xy={math.sqrt(P[0, 0] + P[1, 1]):.3f}")
            last = ticks

多くのコントローラでは符号付きティックカウンタがラップアラウンドします。長時間の走行結果を信頼する前に、差分のオーバーフローを処理してください。

試してみること

1. 安全余裕をなくす。 SAFETY_MARGIN = 0.0とします。当サイトの実行では経路が短くなり（18.95 m）、最小間隔は-0.084 mになりました。ロボットが壁をかすめたのです。推定精度は悪化していません。計画に、推定が外れる余地がなかったのです。
2. 位置観測を減らす。 FIX_EVERY = 100（5秒に1回）とします。当サイトの実行ではEKFのRMS誤差が0.057 mから0.139 mに増え、余裕を残していてもロボットは障害物に接触しました（間隔-0.113 m）。
3. 完璧な車輪。 RIGHT_WHEEL_SCALE = 1.0とします。それでもオドメトリ単独では、ランダムノイズだけでゴール時点で3.7 mドリフトしました。EKFはほとんど変わりませんでした。
4. ノイズの大きい位置観測。 FIX_SIGMA = 0.30とします。EKFのRMS誤差は0.140 mに増えました。次に、余裕を大きくして間隔を正の値に戻してみてください。
5. 不正直なフィルタ。 ekf_predictの中だけでODO_SIGMA / 10を使い、車輪が実際より10倍正確だとフィルタに思い込ませます。当サイトの実行では、95%楕円内に入る割合が95.3%から88.7%に下がり、RMS誤差は0.066 mにしか増えませんでした。誤差はほとんど変わりませんが、フィルタは実際以上の確からしさを主張するようになります。
6. 経路を平滑化する。 グリッド上のA*経路はジグザグになります。前後の点を結ぶ直線上にあるグリッド点を取り除くか、膨張後のグリッドで見通し判定を使ってショートカットし、走行時間を比較してください。

まとめ

• 障害物をロボット半径と自己位置推定誤差のための余裕の分だけ膨張させ、点として経路を計画します。0.1 mのノイズを含む正しい推定値で壁に衝突せずに済むのは、この余裕のおかげです。
• 8近傍グリッド上でオクタイル距離のヒューリスティックを使うA*は、グリッド上の最短経路を返します。角をすり抜ける斜め移動は禁止します。
• Pure Pursuitは、一定距離先の点へ向かう円弧に沿って操舵します。曲率は2 y_r / d²です。
• ノイズを含む車輪オドメトリで予測し、位置観測で更新するEKFは、誤差を位置観測のノイズ程度に保ちます。ここでは70秒の走行中に誤差は0.151 m以内に収まり、オドメトリ単独では5.6 mドリフトしました。
• 誤差だけでなく、フィルタの一貫性も確認してください。真値は約95%のステップで95%楕円の内側に入るはずです。

参考: map座標系とodom座標系についてはREP 105、単位と座標軸の慣例についてはREP 103、および状態推定と動作計画のトピックハブ。