差動二輪駆動の運動学とオドメトリ

研究用プラットフォームからロボット掃除機まで、屋内の移動ロボットの多くは、独立に駆動される2つの車輪と、バランスをとるための1つ以上の受動キャスタで走行します。この構成を差動二輪駆動と呼びます。安価で、その場で旋回でき、運動学は1行に収まります。このレッスンは、レッスン2で2次元の座標系と回転を、レッスン6でノイズのあるセンサを学んだ読者を対象としています。車輪速度と機体の運動を相互に変換し、車輪エンコーダの読みを積分して姿勢の推定値（オドメトリ）を求めます。さらに、2つの小さな較正誤差によって、部屋を3周する間にこの推定値が1 m近くずれていく様子を確かめます。

ロボットとその座標系

ボディ座標系base_linkを、2つの駆動輪を結ぶ車軸の中点に置きます。REP 103に従い、xは前方、yはロボットの左方向、zは上方向を向きます。方位thetaは、ワールド座標系のx軸から反時計回りに測った機体x軸の角度です。幾何形状は2つの数で表せます。車輪半径rと、2つの車輪の接地点間の距離であるトレッド（track width）bです。

車輪速度から機体の運動へ、そしてその逆

どちらの車輪も滑らなければ、各車輪の接地点は対地速度で動きます。左はv_l = r * omega_l、右はv_r = r * omega_rで、omegaはrad/s単位の車輪角速度です。機体の前進速度は両者の平均、ヨーレートは両者の差をトレッドで割った値です。

• v = (v_r + v_l) / 2
• omega = (v_r - v_l) / b

逆に解くと、指令した機体速度に対してモータコントローラが必要とする車輪速度が得られます。v_l = v - omega * b / 2、v_r = v + omega * b / 2です。組(v, omega)は機体のツイストで、ROS 2のgeometry_msgs/msg/Twist指令がlinear.xとangular.zフィールドで運ぶ量と同じです。

車輪速度が異なると、ロボットは車軸の延長線上にある瞬間回転中心と呼ばれる点のまわりを、半径v / omegaの円を描いて走ります。両輪の速度が等しいとこの点は無限遠になり、ロボットは直進します。大きさが等しく向きが逆なら、この点はbase_linkに来て、ロボットはその場で旋回します。次のコードでは車輪半径33 mm、トレッド160 mmとしています。小型の屋内ロボットを想定したきりのよい値です。

import numpy as np

WHEEL_RADIUS = 0.033   # m
TRACK_WIDTH = 0.160    # m, distance between the wheel contact points

def wheels_to_twist(omega_l: float, omega_r: float, r: float = WHEEL_RADIUS, b: float = TRACK_WIDTH):
    """Wheel angular speeds (rad/s) to body linear speed v (m/s) and yaw rate w (rad/s)."""
    v_l, v_r = r * omega_l, r * omega_r
    return (v_r + v_l) / 2.0, (v_r - v_l) / b

def twist_to_wheels(v: float, w: float, r: float = WHEEL_RADIUS, b: float = TRACK_WIDTH):
    """Body twist to the wheel angular speeds that produce it."""
    return (v - w * b / 2.0) / r, (v + w * b / 2.0) / r

for v_cmd, w_cmd in [(0.2, 0.0), (0.0, 1.0), (0.2, 0.5)]:
    wl, wr = twist_to_wheels(v_cmd, w_cmd)
    v, w = wheels_to_twist(wl, wr)
    radius = v / w if w != 0 else float("inf")
    print(f"v={v_cmd:.2f} m/s w={w_cmd:.2f} rad/s -> wheels L {wl:6.2f} R {wr:6.2f} rad/s "
          f"-> back to v={v:.2f} w={w:.2f}, turn radius {radius:.3f} m")

v=0.20 m/s w=0.00 rad/s -> wheels L   6.06 R   6.06 rad/s -> back to v=0.20 w=0.00, turn radius inf m
v=0.00 m/s w=1.00 rad/s -> wheels L  -2.42 R   2.42 rad/s -> back to v=0.00 w=1.00, turn radius 0.000 m
v=0.20 m/s w=0.50 rad/s -> wheels L   4.85 R   7.27 rad/s -> back to v=0.20 w=0.50, turn radius 0.400 m

往復の変換は厳密に一致します。2つの線形方程式なので当然の結果です。1 rad/sでその場旋回するのに必要なのは、各車輪で逆向きに2.42 rad/sだけです。0.2 m/sで走りながら0.5 rad/sで旋回すると、旋回半径は0.4 mになります。

ユニサイクルモデル

ワールド座標系では、姿勢(x, y, theta)は次のように変化します。

• dx/dt = v * cos(theta)
• dy/dt = v * sin(theta)
• dtheta/dt = omega

これがユニサイクルモデル（一輪車モデル）です。状態変数は3つですが入力は2つしかなく、ロボットを自身のy軸方向に動かす入力は存在しません。車輪は前後に転がりますが、横に滑ることはできません。位置ではなく速度に対するこの種の拘束を、非ホロノミック拘束と呼びます。縦列駐車に何度も切り返しが必要なのはこのためです。ロボットは平面内の任意の姿勢に到達できますが、そこへまっすぐ向かって移動することはできません。車輪型ロボットの経路計画器はこの拘束を守る必要があります。レッスン8のグリッド経路計画器が第一歩にすぎない理由の一つはここにあります。

オドメトリの積分

オドメトリとは、車輪エンコーダだけから求める姿勢の逐次推定値です。制御周期ごとに、前の周期から各車輪がどれだけ回転したかを読み取り、その増分をds（移動距離）とdtheta（方位の変化）に変換して姿勢に加えます。

最も単純な更新はオイラーステップです。現在の方位に沿ってdsだけ進み、次にdthetaを加えます。これはステップの間に方位が一定だったと仮定しており、ロボットが旋回している間はこの仮定が成り立ちません。代わりに、ステップの間に車輪速度が一定だったと仮定すると、ロボットは円弧を描いたことになり、その円弧は閉じた式で表せます。次のコードでは、0.2 m/s・0.5 rad/sで12 s走るロボットについて、両者を厳密解と比較します。

import numpy as np

def euler_step(pose, v, w, dt):
    x, y, th = pose
    return np.array([x + v * np.cos(th) * dt, y + v * np.sin(th) * dt, th + w * dt])

def arc_step(pose, v, w, dt):
    """Exact integration for constant v and w over dt: the robot follows a circular arc."""
    x, y, th = pose
    if abs(w) < 1e-9:
        return euler_step(pose, v, w, dt)
    th_new = th + w * dt
    return np.array([x + v / w * (np.sin(th_new) - np.sin(th)),
                     y - v / w * (np.cos(th_new) - np.cos(th)),
                     th_new])

v, w, t_end = 0.2, 0.5, 12.0          # 0.4 m turn radius, about 1 lap in 12.57 s
exact_end = arc_step(np.zeros(3), v, w, t_end)   # one big step is exact for constant v, w
print(f"exact pose at {t_end} s: x={exact_end[0]:.4f} y={exact_end[1]:.4f} th={exact_end[2]:.4f}")
for dt in (0.5, 0.1, 0.02):
    pe = np.zeros(3)
    pa = np.zeros(3)
    for _ in range(round(t_end / dt)):
        pe = euler_step(pe, v, w, dt)
        pa = arc_step(pa, v, w, dt)
    err_e = np.hypot(*(pe[:2] - exact_end[:2])) * 1000
    err_a = np.hypot(*(pa[:2] - exact_end[:2])) * 1000
    print(f"dt={dt:5.2f} s  Euler position error {err_e:7.2f} mm   arc error {err_a:.2e} mm")

exact pose at 12.0 s: x=-0.1118 y=0.0159 th=6.0000
dt= 0.50 s  Euler position error   14.12 mm   arc error 1.18e-13 mm
dt= 0.10 s  Euler position error    2.82 mm   arc error 5.43e-12 mm
dt= 0.02 s  Euler position error    0.56 mm   arc error 3.37e-11 mm

ステップを5分の1にすると、オイラー法の誤差も5分の1になります。1次の手法として予想どおりの挙動です。50 Hz（0.02 sのステップ）では1周後の誤差は約0.5 mmで、次の節で扱う誤差に比べれば小さな値です。円弧ステップは、各ステップの間に車輪速度が本当に一定であれば、丸め誤差の範囲で厳密です。追加のコストはsinとcosを1回ずつ計算するだけなので、使わない理由はほとんどありません。dthetaがほぼゼロのとき、円弧の式はほぼゼロで割ることになるため、コードは直線のステップに切り替えます。

較正誤差がドリフトになる仕組み

車輪オドメトリの実際の誤差の大きさを決めるのは、積分手法ではなくロボット自体です。車輪は滑らかな床で滑り、段差で跳ねます。これらはランダムな誤差です。同じくらい重要な系統誤差が2つあります。1つは、ソフトウェア上の車輪半径が実際の転がり半径と一致しないことです（タイヤは圧縮され、摩耗し、左右でも異なります）。もう1つは、ソフトウェア上のトレッドが接地面間の実効距離と一致しないことです。

次のコードでは、実際のロボットに一辺2 mの正方形を3周させ、各角ではその場で旋回させます。車輪の回転量は、真の幾何形状で完全な正方形を走るのに必要な値です。オドメトリはこの回転量を、車輪半径を0.9%大きく、トレッドを3.1%小さく見積もった状態で積分し、ロボットが最終的にどこにいると推定したかを出力します。

import numpy as np

R_TRUE, B_TRUE = 0.0330, 0.160      # what the robot really has
R_ODOM, B_ODOM = 0.0333, 0.155      # what the odometry code believes (+0.9 %, -3.1 %)
SIDE = 2.0                          # m, side of the square the robot drives

def odom_update(pose, d_phi_l, d_phi_r, r, b):
    """Integrate one pair of wheel rotation increments (rad) with the exact-arc formula."""
    x, y, th = pose
    ds_l, ds_r = r * d_phi_l, r * d_phi_r
    ds, dth = (ds_r + ds_l) / 2.0, (ds_r - ds_l) / b
    if abs(dth) < 1e-9:
        return np.array([x + ds * np.cos(th), y + ds * np.sin(th), th])
    rho = ds / dth
    return np.array([x + rho * (np.sin(th + dth) - np.sin(th)),
                     y - rho * (np.cos(th + dth) - np.cos(th)), th + dth])

# The real robot drives a perfect square: straight, turn on the spot 90 deg left, repeat.
# These are the wheel rotations that achieve that with the TRUE radius and track width.
straight = SIDE / R_TRUE
turn = (np.pi / 2) * (B_TRUE / 2) / R_TRUE
moves = [(straight / 2, straight / 2), (straight / 2, straight / 2), (-turn, turn)] * 4

pose = np.zeros(3)
path = [(0.0, 0.0)]
for lap in range(3):
    for d_l, d_r in moves:
        pose = odom_update(pose, d_l, d_r, R_ODOM, B_ODOM)
        if abs(d_l - d_r) < 1e-9:           # record after each half side
            path.append((round(float(pose[0]), 3), round(float(pose[1]), 3)))
    print(f"after lap {lap + 1}: odometry says x={pose[0]:+.3f} m y={pose[1]:+.3f} m "
          f"heading={np.rad2deg(pose[2]) - 360 * (lap + 1):+.1f} deg (truth: 0, 0, 0)")
print("odometry path (x, y):", path)

after lap 1: odometry says x=+0.279 m y=-0.229 m heading=+15.0 deg (truth: 0, 0, 0)
after lap 2: odometry says x=+0.607 m y=-0.378 m heading=+30.0 deg (truth: 0, 0, 0)
after lap 3: odometry says x=+0.963 m y=-0.437 m heading=+45.0 deg (truth: 0, 0, 0)
odometry path (x, y): [(0.0, 0.0), (1.009, 0.0), (2.018, 0.0), (1.952, 1.007), (1.886, 2.014), (0.886, 1.882), (-0.115, 1.751), (0.082, 0.761), (0.279, -0.229), (1.254, 0.032), (2.228, 0.293), (1.904, 1.249), (1.58, 2.204), (0.648, 1.818), (-0.285, 1.432), (0.161, 0.527), (0.607, -0.378), (1.481, 0.126), (2.355, 0.631), (1.795, 1.47), (1.235, 2.309), (0.434, 1.695), (-0.367, 1.081), (0.298, 0.322), (0.963, -0.437)]

3周後、ロボットは出発点に戻り、同じ向きを向いています。一方、オドメトリはロボットが1.06 m離れた位置にあり、45度回転していると報告します。

注目すべきは方位の誤差です。その場旋回では両車輪が逆向きに同じ長さの円弧を進み、計算される角度は真の角度にR_ODOM / R_TRUEとB_TRUE / B_ODOMを掛けた値になります。ここではこの比は1.0091と1.0323の積、すなわち1.0417なので、90度の旋回はそれぞれ93.75度と読まれます。1周に4回旋回するので1周あたり15度となり、コードの出力と完全に一致します。半径の誤差もこの比に含まれるため、すべての距離を0.9%引き伸ばすだけでなく、方位の誤差にも寄与します。方位への寄与は、トレッドの誤差のほうが3倍以上大きくなります。方位誤差が危険なのは、その後に進む1 mごとに、わずかに違う方向へ進むことになるためです。そのため、方位誤差が引き起こす位置誤差は移動距離とともに増え続けます。

両方の値の較正は手間がかからず、新しいロボットでは必ず行う価値があります。既知の距離を直進させ、オドメトリを巻尺の測定値と比べて車輪半径を補正します。その場で既知の回数だけ1回転ずつ旋回させ、オドメトリの方位と比べてトレッドを補正します。左右の車輪がまったく同じであることはまれなので、旋回試験は両方向で行ってください。

ROS 2のオドメトリ：REP 105の座標系

ROSでは、関係する座標系にREP 105で標準の名前が付けられています。base_linkはロボットの機体に固定された座標系です。odomはワールドに固定された座標系で、その中でのロボットの姿勢はオドメトリから得られます。滑らかに動き、跳ぶことはありませんが、上のグラフのとおり際限なくドリフトします。mapもワールドに固定された座標系で、自己位置推定システム（地図とのスキャンマッチングや同時自己位置推定と地図作成（SLAM））が姿勢を長期的なドリフトのない状態に保ちます。その代償として、補正が届いたときに姿勢が突然跳びます。

座標系はmapからodom、odomからbase_linkへとつながる連鎖をなします。odomからbase_linkへの変換はオドメトリノードが担当し、通常は姿勢・ツイストとそれらの共分散を含むnav_msgs/msg/Odometryメッセージもパブリッシュします。自己位置推定ノードはmapからodomへの変換をパブリッシュします。これはmapからbase_linkへの変換を正しくするための補正そのものです。壁沿いの走行のように滑らかな局所的な動きを必要とするコントローラはodomで動作します。建物の反対側の部屋に到達する必要がある経路計画器はmapで動作します。レッスン10では、まさにこの連鎖をtf2でパブリッシュし、参照します。

演習

1. 3つ目のコードでR_ODOMをR_TRUEと等しくし、3周させてください。次に元に戻し、今度はB_ODOMをB_TRUEと等しくしてください。3周後のドリフトが大きいのはどちらの誤差が単独で残った場合か、またその理由を答えてください。
2. movesを変更してロボットが正方形を時計回りに走る（各角で右に旋回する）ようにし、最終的なオドメトリの姿勢を反時計回りの結果と比較してください。2つの結果を使って、較正走行で両方向を使うべき理由を説明してください。
3. ランダムな滑りを加えてください。シードを固定したrngを使い、各車輪の増分に1 + rng.normal(0, 0.01)を掛けます。3周の試行を20回行い、最終的な位置誤差の平均と標準偏差を報告してください。

まとめ

• 差動二輪駆動ロボットの機体ツイストはv = (v_r + v_l) / 2、omega = (v_r - v_l) / bで、車輪の対地速度は車輪半径と車輪角速度の積です。
• ユニサイクルモデルは状態が3つ、入力が2つです。ロボットは横方向に移動できません。これは非ホロノミック拘束です。
• オドメトリのオイラー積分は、ステップ幅に比例する誤差を持ちます。厳密な円弧ステップは、ステップの間に車輪速度が一定なら厳密で、追加コストはsinとcosを1回ずつ計算するだけです。
• オドメトリのドリフトを支配するのは、車輪半径とトレッドの系統誤差です。方位誤差は、移動距離に比例して位置誤差に成長します。
• REP 105はオドメトリをodom座標系に置きます。連続ですがドリフトします。自己位置推定システムがmapからodomへの変換をパブリッシュしてドリフトを除去します。

さらに読む

• Modern Robotics、Lynch・Park、Cambridge University Press、2017年（第1版改訂版、2019年プレプリント）：第13章で車輪型移動ロボットを扱い、ユニサイクルと差動二輪駆動の運動学、オドメトリを含みます。
• Probabilistic Robotics、Thrun・Burgard・Fox、MIT Press、2005年：第5章で、ノイズを含む速度動作モデルとオドメトリ動作モデルを導出しています。
• REP 105, Coordinate Frames for Mobile Platforms、Wim Meeussen：ROS 2で使われるbase_link、odom、mapの定義。