for t in np.linspace(0, 1, 10):
    if is_collision(p + t * (q - p)):
        return True

経路計画（Path Planning：ロボットが障害物を避けて動く経路を求める処理）の衝突判定でよく見る形です。この 10 を、何を根拠に決めましたか。

私が公開している RRT（Rapidly-exploring Random Trees：ランダムに木を伸ばして経路を探す経路計画アルゴリズム）の実装には、_DEVIDED_DISTANCE = 0.05 という定数が書いてあります。1本の辺を 0.05 m 刻みで分割して点判定する、という意味です。この値を検算したところ、いちばん深く食い込んだ辺では、実際に判定へ使われていた刻みが 0.0625 m でした。1.25倍です。そして、その差のぶんだけ経路は壁に食い込んでいました。最大 2.14 mm。

本記事では、ステップ幅と刻み幅の上限を閉じた式で出し、それが実測を正しく縛っているかを確かめます。最後は、自分の環境の数値を入れるだけで判定できる関数まで落とします。

本記事で分かること

ステップ幅 Δ（木を1回伸ばす距離）の上限を決める式と、その実測での確認
衝突判定の刻み幅 d の上限を、「どこまでのめり込みなら許すか」から逆算する式
コードに書いた d と、実際に判定に使われる刻み幅がズレる条件（今回いちばん効いた話）
自分の実装のパラメータが妥当かを判定する関数（コードあり）

想定読者は、RRT や PRM（Probabilistic Roadmap：あらかじめ点を撒いて道路網を作る経路計画手法）を実装したことはあるものの、衝突判定まわりのパラメータを暫定値のまま運用している人です。RRT 本体の実装は既存記事に譲り、ここはパラメータの決め方だけを扱います。

1. 結論：守るのは不等式2本

先に答えを置きます。

決めるもの	守る条件	破ったときに起きること
ステップ幅 Δ	Δ < T（環境で最も薄い障害物の、膨張後の厚み）	1辺で障害物をまたぐ。正解が「経路なし」の環境でも「見つけた」と返す
刻み幅 d	d_eff < 2r（ロボットの直径）	障害物を貫通する経路が通る
刻み幅 d	d_eff ≤ 2√(2ρε − ε²)	貫通はしないが、許容量 ε を超えて食い込む

d_eff は「コードに書いた d」ではなく実際に判定へ使われた刻み幅、r はロボットを円とみなしたときの半径、ρ は膨張半径（障害物の半径 + ロボット半径）のうち最小のもの、ε は許容できるめり込み量です。膨張（inflation：障害物をロボットの半径ぶん太らせて、ロボットを点として扱えるようにする前処理）した後の寸法で考えるところが肝心です。

Δ と d は別のパラメータで、縛られる上限も別です。式にすると次の2本になります。

壁の最薄部の厚み（円を半径ぶんの間隔で並べた壁の場合）:

T = 2\sqrt{(R + r)^2 - (R/2)^2}

刻み幅 d_eff で見逃しうる、めり込み量の上限:

\varepsilon_{\max} = \rho - \sqrt{\rho^2 - (d_{\rm eff}/2)^2} \;\simeq\; \frac{d_{\rm eff}^{\,2}}{8\rho}

近似のほうは、d_eff = 0.05・ρ = 0.25 で誤差 0.25%（厳密 1.2531 mm に対して 1.2500 mm）。分子が d_eff の2乗なので、刻みを半分にすると、見逃せるめり込みは約1/4になります。

決める順番は次のとおりです。

図1: パラメータを決める順番。最後の検算まで含めて1セット

以降の実測は、2次元の平面上に円の障害物を置き、ロボットを半径 r = 0.05 の円とみなして行いました。乱数シードは固定してあります（Python 3.10.9 / numpy 1.26.4）。

2. 判定しているのは点、通るのは線分

刻み幅という論点が生まれる理由は、RRT の構造にあります。

木を伸ばすとき、多くの実装が判定するのは新しく作ったノード1点です。ところが実際にロボットが動くのは、親ノードから新しいノードまでの線分です。

図2: 両端が無衝突でも、その間が障害物に食い込むことがある。灰色が障害物、破線がロボット半径 r ぶん膨らませた輪郭。左は端点2点だけを判定するので当たらず「衝突なし」と返る。右は線分を6点に刻んだので、中央の点（×）で食い込みを検出できる

擬似コードには if CollisionFree(x_new) then としか書かれていません。x_new は点です。実装したときの衝突判定関数も、引数は点1つでした。

import numpy as np


def is_collision_point(point: np.ndarray, obstacles: np.ndarray, robot_radius: float) -> bool:
    """点が障害物に当たっているか

    パラメータ
        point: 判定したい位置 (x, y)
        obstacles: 各行が (中心x, 中心y, 半径) の障害物
        robot_radius: ロボットを円とみなしたときの半径

    戻り値
        当たっていれば True
    """
    distance = np.linalg.norm(obstacles[:, :2] - point, axis=1)

    return bool(np.any(distance <= obstacles[:, 2] + robot_radius))

この関数自体は正しく動きます。判定する場所が点しかない、というだけです。だから「点をどれだけ細かく並べるか」が設計変数になります。

3. 上限その1：ステップ幅 Δ と、壁の最薄部 T

端点しか判定しないなら、1辺の長さ Δ が障害物の厚みを超えた時点で、またげます。

半径 R の円を間隔 R で並べて壁を作り、ロボット半径 r ぶん膨らませます。膨らませた円（半径 ρ = R + r）が重なってできる帯のうち、いちばん薄いのは隣り合う円の中間、x = R/2 の位置です。

図3: 灰色が壁の円（R=0.20・間隔 R）、破線がロボット半径 r=0.05 ぶん膨らませた輪郭。赤い矢印が最薄部 T、青い線分が Δ=0.50 の1辺。両端は破線の外側にあるので「衝突なし」と判定される

三平方の定理でそのまま書けます。

T = 2\sqrt{(R + r)^2 - (R/2)^2}

R = 0.20、r = 0.05 なら T = 0.4583。この式が本当に壁の厚みを表しているか不安だったので、膨張円の和集合を縦に走査して厚みを直接測りました。R を 0.03 から 0.20 まで変えて、式と実測の差は 0.000001 以下でした。

def wall_min_thickness(wall_radius: float, robot_radius: float) -> float:
    """円を半径ぶんの間隔で並べた壁の、膨張後の最薄部の厚み T

    パラメータ
        wall_radius: 壁を構成する円の半径 R
        robot_radius: ロボットを円とみなしたときの半径 r

    戻り値
        最も薄い場所の厚み [m]
    """
    inflated = wall_radius + robot_radius       # 膨張半径 ρ = R + r

    return 2.0 * np.sqrt(inflated ** 2 - (wall_radius / 2.0) ** 2)

では、この T が本当にすり抜けの境目になっているか。左右いっぱいに壁で塞いだ環境（正解は「経路なし」）に素朴な RRT を入れて、Δ と R をそれぞれ振りました。各50試行です。

Δ（R = 0.20 固定・T = 0.4583）	0.15	0.25	0.35	0.45	0.50	0.60	0.80	1.00
「見つけた」と答えた回数	0/50	0/50	0/50	0/50	50/50	50/50	50/50	50/50

R（Δ = 0.25 固定・境目は R = 0.0815）	0.03	0.04	0.06	0.08	0.10	0.15	0.20
「見つけた」と答えた回数	50/50	50/50	50/50	6/50	0/50	0/50	0/50

図4: 正解が「経路なし」の環境で「経路を見つけた」と答えた割合。破線が理論しきい値。左は Δ を振ったもの、右は壁の円半径 R を振ったもの

切り替わりは Δ = 0.45 と 0.50 の間。T = 0.4583 を挟んでいます。R を振ったほうは、T(R) = Δ を解くと R = 0.0815 で、R = 0.06（すり抜ける）と R = 0.10（すり抜けない）の間に入りました。また、境目の内側にあたる R = 0.08 だけが 6/50 という中途半端な値になりました。またげはするものの、またげる向きに木が伸びる確率は低い、という理屈と合います。境目の付近だけ確率的になる。式が当たっている証拠としては、ここが一番強いと思います。

この T の式は、円を等間隔に並べた壁に特化したものです。一般の環境では、膨張後の最も薄い断面の厚みを測ることになります（板や柵、テーブルの天板のような薄い障害物が候補）。厚みが読めない形状なら、次の §4 の 2r ルールが下限として使えます。

4. 上限その2：刻み幅 d と、許容できるめり込み

線分を刻んで判定すれば、またぎは消えます。別の環境で300試行ずつ測ったときは、端点のみ判定で 12〜24本（辺の総数は約11,900）あった取りこぼしが、線分補間を入れると 0本になりました（この300試行のデータは、後述する姉妹記事のために取ったものです）。ただし刻めば安全、で終わりにはなりません。どこまで細かくすれば足りるのかが残ります。

刻み幅 d_eff で点判定を並べたとき、隣り合う点の中間がいちばん手薄になります。障害物の中心が線分から距離 h にあり、両隣の点がどちらも膨張円の外（距離 ρ 以上）にあるなら、

\sqrt{h^2 + (d_{\rm eff}/2)^2} \ge \rho

なので h ≥ √(ρ² − (d_eff/2)²)。線分は最悪でも ρ − √(ρ² − (d_eff/2)²) だけしか食い込めません。これが見逃しの上限です。逆に解けば、許容できるめり込み ε から刻み幅の上限が出ます。

d_{\rm eff} \le 2\sqrt{2\rho\varepsilon - \varepsilon^2}

ρ = 0.25 で計算するとこうなります。

許容めり込み ε	10 mm	5 mm	1 mm	0.5 mm	0.1 mm
刻み幅の上限 d	0.1400 m	0.0995 m	0.0447 m	0.0316 m	0.0141 m

def max_penetration(inflated_radius: float, spacing: float) -> float:
    """刻み幅 spacing の点判定が見逃しうる、めり込み量の上限

    パラメータ
        inflated_radius: 膨張半径 ρ（障害物半径 + ロボット半径）のうち最小のもの
        spacing: 実際に判定に使われる刻み幅 [m]

    戻り値
        めり込み量の上限 [m]
    """
    if spacing >= 2.0 * inflated_radius:
        return inflated_radius                  # 障害物を丸ごとまたげる状態

    return inflated_radius - np.sqrt(inflated_radius ** 2 - (spacing / 2.0) ** 2)


def resolution_for(inflated_radius: float, allowed: float) -> float:
    """許容できるめり込み量から、刻み幅の上限を逆算する

    パラメータ
        inflated_radius: 膨張半径 ρ のうち最小のもの
        allowed: 許容できるめり込み量 ε [m]

    戻り値
        刻み幅の上限 [m]
    """
    return 2.0 * np.sqrt(2.0 * inflated_radius * allowed - allowed ** 2)

代償のほうも測りました。環境A（右に抜け道がある壁）で Δ = 0.25 に固定し、d だけを振って100試行ずつ。

刻み幅 d	1辺あたりの判定回数	取りこぼした辺	実測の最大めり込み	理論上限	平均計算時間
端点のみ	2	2 / 3,937	13.775 mm	33.494 mm	7.15 ms
0.25	2	1 / 3,936	12.592 mm	33.494 mm	12.30 ms
0.125	3	0	0.000 mm	7.939 mm	14.00 ms
0.05	6	0	0.000 mm	1.253 mm	19.40 ms
0.025	11	0	0.000 mm	0.313 mm	28.25 ms
0.0125	21	0	0.000 mm	0.078 mm	50.59 ms
0.005	51	0	0.000 mm	0.013 mm	98.25 ms

図5: 左は刻み幅とめり込み量（実線が理論上限、四角が実測の最大値、×は実測がちょうど0だったもの）。右は1辺あたりの判定回数と計算時間。時間は判定回数にほぼ比例して増える

この表の読み方には注意が要ります。取りこぼしの本数は d = 0.125 の時点で 0 になりますが、そこで細かさが足りたわけではありません。d = 0.125 のとき、理論上はまだ 7.9 mm 食い込む余地があります。100試行では引かなかっただけです。取りこぼし件数がゼロかどうかで刻み幅を決めると、環境が変わった瞬間に破綻します。決めるべきは ε（許容めり込み）のほうです。

もう一つ、貫通に関しては下限がはっきりしています。線分が障害物そのものを横切るなら、その線分は必ず「障害物上の点を中心とする半径 r の円」を直径ぶん（= 2r）通過します。したがって d_eff < 2r なら、障害物を貫く経路は原理的に検出できます。刻み幅ごとにランダムな線分を20万本ずつ生成し、そのうち障害物を実際に貫いたものだけを数えました。

実効刻み d_eff	0.0999	0.0625	0.05	0.11	0.15
貫通しているのに見逃した本数	0 / 252	0 / 203	0 / 153	20 / 260	81 / 296

r = 0.05 なので 2r = 0.10。境目はここでした。貫通を消すだけなら d_eff < 2r、めり込みまで抑えたいなら ε から逆算する、と使い分けられます。

5. コードに書いた刻み幅は、判定に使われる刻み幅と一致しない

自分の実装を検算していて、いちばん驚いたのがここでした。

線分を刻むコードは、たいてい「分割数を出してから等分」します。

n_divided = max(int(np.ceil(np.linalg.norm(q - p) / resolution)), 1)

分割数が整数である以上、実際の刻みは辺の長さ ÷ 分割数 になります。書いた d とは一致しません。しかも丸め方でズレる向きが逆になります。

ceil（切り上げ）で分割 → d_eff ≤ d（細かくなる。安全側）
int / floor（切り捨て）で分割 → d_eff ≥ d（粗くなる。危険側）

私の実装は後者でした。

# 自作実装（rrt.py）より
n_devided = max(int(distance / self._DEVIDED_DISTANCE), 1)

_DEVIDED_DISTANCE = 0.05、ステップ幅は _MOVING_VALUE = 0.25 なので、5分割されるはずです。ところが最大めり込みを記録した辺を取り出して長さを見ると、0.24999999999999997 でした。正規化した方向ベクトルに 0.25 を掛けた結果で、浮動小数点の丸めによって 0.25 をわずかに下回っています。int(0.24999999999999997 / 0.05) は 4。

実効刻みは 0.0625 m、公称の1.25倍でした。めり込み上限は 1.569 mm から 2.456 mm へ広がります。

すり抜けの判定に使うべき値も d_eff です。塞がれた環境（R = 0.03・T = 0.1572）に Δ = 0.50 で入れ、d を振った結果がこれです。

書いた d	分割数	実効 d_eff	d_eff と T	「見つけた」回数
0.50	1	0.5000	T 以上	50/50
0.30	2	0.2500	T 以上	50/50
0.20	3	0.1667	T 以上	50/50
0.17	3	0.1667	T 以上	50/50
0.16	4	0.1250	T 未満	0/50
0.13	4	0.1250	T 未満	0/50
0.10	5	0.1000	T 未満	0/50

図6: 横軸がコードに書いた刻み幅、縦軸が実際に判定へ使われた刻み幅。階段状に変わる。赤の一点鎖線が壁の最薄部 T で、青い階段がそれを跨ぐ位置と、すり抜けの発生（赤い▽）がぴたりと一致している

d = 0.20 と d = 0.17 は、書いた値が違うのに結果が同じ 50/50。分割数がどちらも3で、d_eff が同じ 0.1667 だからです。d = 0.16 で初めて4分割になり、d_eff が 0.1250 に落ちて T を下回り、すり抜けが消えました。判定を握っているのは d ではなく d_eff という予測が、そのまま当たっています。

書いた d だけで安全側を主張すると、d = 0.16 も d = 0.17 も T より細かいので、どちらも安全に見えます。実際には片方が全滅でした。

def effective_spacing(step: float, resolution: float) -> float:
    """線分を等分割したときに、実際に判定へ使われる刻み幅

    パラメータ
        step: 1辺の長さ（RRTのステップ幅 Δ）
        resolution: コードに書いた刻み幅 d

    戻り値
        実効刻み幅 d_eff [m]
    """
    n_divided = max(int(np.ceil(step / resolution)), 1)

    return step / n_divided

分割数の計算に int() を使っている実装は、刻み幅が最大2倍まで粗くなります。 辺の長さが刻み幅の整数倍をわずかに下回るとき（例：長さ 0.099 に対して刻み 0.05 なら1分割＝実効 0.099）が最悪です。切り上げに直すか、直せないなら d を半分に見積もってください。今回のように、長さ 0.25 のつもりの辺が 0.24999999999999997 になり、0.05 で割ると 4.999999999999999 を返す、という踏み方もします。

0.25 / 0.05 そのものは Python でも 5.0 を返します。問題は割られる側で、正規化した方向ベクトルに 0.25 を掛けて長さを測り直すと、丸めで 0.25 をわずかに下回ることがあります。定数どうしの割り算だけ確認しても見つかりません。

6. 狭隘通路での検算：成功率 20/20 の裏で、2.14 mm 食い込んでいた

式は出ました。実測がその内側に収まっているかを、別の環境で確かめます。

壁の途中に隙間を開け、その幅を 600 mm から 10 mm まで狭めます。ここのクリアランス実測も姉妹記事（Zenn）で取ったデータの再利用です。自作の RRT と RRT-Connect（始点側と終点側から2本の木を伸ばして繋ぐ改良版）に各20試行ずつ解かせ、返ってきた経路を線分と円の距離を解析的に解く別の物差しで測り直しました。刻み幅に依存しない検算です。

通路の隙間	RRT: 最小クリアランス / 干渉件数	RRT-Connect: 最小クリアランス / 干渉件数
600 mm	+0.012 mm / 0件	−0.866 mm / 1件
300 mm	+1.121 mm / 0件	+0.980 mm / 0件
100 mm	+2.756 mm / 0件	−0.222 mm / 1件
50 mm	+0.637 mm / 0件	−0.055 mm / 1件
20 mm	−1.841 mm / 5件	−0.784 mm / 3件
10 mm	−1.286 mm / 4件	−2.143 mm / 6件

クリアランス＝経路と障害物表面の余裕。負なら食い込んでいる。各20試行

図7: 左が最小クリアランス（赤い帯が、実効刻み 0.0625 m の判定が見逃しうる範囲）。右が「成功」と返ってきた20本のうち実際は干渉していた本数

成功率はどの隙間でも 20/20 のままです。 プランナは全部「解けました」と答えています。その裏で、隙間 20 mm 以下では20本中3〜6本が壁に食い込んでいました。最大 2.143 mm。丸め誤差の桁ではありません。

この 2.143 mm が、§4 の式の内側にあるかを見ます。膨張半径は ρ = 0.15 + 0.05 = 0.20。

公称刻み 0.05 m で計算した上限 → 1.569 mm（実測 2.143 mm がはみ出す）
実効刻み 0.0625 m で計算した上限 → 2.456 mm（実測は内側）

公称値のままだと、実測を説明できません。§5 の実効刻みを入れて初めて、式が実測を上から押さえます。式が合わないときは、まず自分がどの d を式に入れたかを疑う、というのが今回の教訓でした。

計算時間のほうも、隙間を狭めるほど伸びます（RRT の中央値で 6.33 ms → 29.58 ms）。ただし成功率は落ちません。成功率だけを見ていると、この壊れ方は見えません。 返ってきた経路を別の物差しで測り直さないかぎり、2.143 mm の食い込みはどこにも記録されずに終わります。

7. 自分の実装に当てはめる手順

ここまでを、そのまま持ち帰れる形にします。

環境で最も薄い障害物を探し、膨張後の厚み T を測る（板や柵、天板が候補。円を並べた壁なら §3 の式）
Δ < T を満たすステップ幅を選ぶ
許容できるめり込み ε を決める（要求クリアランスから逆算する。1 mm なのか 0.1 mm なのかは用途次第）
d ≤ 2√(2ρε − ε²) から刻み幅を決める。ρ は膨張半径のうち最小のもの
分割数の丸めを確認する。int() なら d を半分に見積もるか、ceil へ直す
別の物差しで検算する

判定は関数にできます。

def diagnose(step: float, resolution: float, min_thickness: float,
             min_inflated_radius: float, allowed: float) -> dict:
    """ステップ幅と刻み幅が、環境に対して妥当かを判定する

    パラメータ
        step: ステップ幅 Δ [m]
        resolution: 刻み幅 d [m]
        min_thickness: 環境で最も薄い障害物の、膨張後の厚み [m]
        min_inflated_radius: 膨張半径 ρ のうち最小のもの [m]
        allowed: 許容できるめり込み量 ε [m]

    戻り値
        判定結果と、根拠になった数値
    """
    actual = effective_spacing(step, resolution)
    penetration = max_penetration(min_inflated_radius, actual)

    return {
        "実効刻み幅": actual,
        "すり抜けない条件（実効刻み < 最薄部）": bool(actual < min_thickness),
        "めり込み上限": penetration,
        "要求クリアランスを満たすか": bool(penetration <= allowed),
        "刻み幅の上限": resolution_for(min_inflated_radius, allowed),
        "1辺あたりの点判定の回数": max(int(np.ceil(step / resolution)), 1) + 1,
    }

§5 で 50/50 すり抜けた条件を入れると、こう返ります。

>>> diagnose(step=0.50, resolution=0.17, min_thickness=0.1572,
...          min_inflated_radius=0.08, allowed=0.001)
{'実効刻み幅': 0.16666666666666666,
 'すり抜けない条件（実効刻み < 最薄部）': False,
 'めり込み上限': 0.08,
 '要求クリアランスを満たすか': False,
 '刻み幅の上限': 0.025219040425836985,
 '1辺あたりの点判定の回数': 4}

実測で全滅した条件を、走らせる前に弾けます。

最後の検算には、線分と円の最短距離を解析的に解く関数を使いました。刻み幅に依存しないので、審判として使えます。

def segment_to_circle_distance(p: np.ndarray, q: np.ndarray, centers: np.ndarray) -> np.ndarray:
    """線分 p-q と各円の中心との最短距離（刻み幅に依存しない）

    パラメータ
        p: 線分の始点
        q: 線分の終点
        centers: 各行が (中心x, 中心y) の円中心

    戻り値
        各円中心までの最短距離
    """
    d = q - p
    denom = float(d @ d)
    # 線分上で最も近い点のパラメータ t を [0, 1] に丸める（丸めないと無限直線の扱いになる）
    t = np.zeros(len(centers)) if denom == 0.0 else np.clip((centers - p) @ d / denom, 0.0, 1.0)
    closest = p + t[:, None] * d

    return np.linalg.norm(centers - closest, axis=1)


def audit_path(path: np.ndarray, obstacles: np.ndarray, robot_radius: float) -> dict:
    """返ってきた経路を、刻み幅に依存しない物差しで検算する

    パラメータ
        path: 各行が (x, y) の経路
        obstacles: 各行が (中心x, 中心y, 半径) の障害物
        robot_radius: ロボットの半径

    戻り値
        辺の総数・干渉していた辺数・最大めり込み量
    """
    worst = 0.0
    bad = 0
    for i in range(len(path) - 1):
        distance = segment_to_circle_distance(path[i], path[i + 1], obstacles[:, :2])
        depth = float(np.max(obstacles[:, 2] + robot_radius - distance))
        if depth > 0.0:
            bad += 1
            worst = max(worst, depth)

    return {"edges": len(path) - 1, "collided": bad, "max_depth": worst}

円で済まない形状なら、python-fcl（Flexible Collision Library の Python 版：3D形状どうしの干渉を判定するライブラリ）のように、形状どうしの距離を返してくれる仕組みに任せるほうが早いです。使い方は別記事にまとめてあります。

今回の実測は2次元・円の障害物に限っています。3次元でも、最も薄い断面の厚みと、膨張半径のうち最小のものを測り直せば同じ式が使えるはずですが、薄板を含む3次元環境での追試はまだやっていません。

まとめ

ステップ幅 Δ の上限は、環境で最も薄い障害物の膨張後の厚み T。円を並べた壁なら T = 2√((R+r)² − (R/2)²) で閉じた式になり、走査実測との差は 0.000001 以下だった
塞がれた環境（正解は「経路なし」）で確かめると、切り替わりは Δ = 0.45 と 0.50 の間。T = 0.4583 を挟んでいた。境目の直近（R = 0.08）だけ 6/50 という中途半端な値になったのも理屈と合う
刻み幅 d の上限は、許容めり込み ε から d ≤ 2√(2ρε − ε²) で逆算する。取りこぼし件数がゼロになる細かさで止めると、環境が変わった瞬間に破綻する
貫通だけなら d_eff < 2r（ロボット直径）で消える。実効刻み 0.0999 では見逃し 0/252、0.11 では 20/260 だった
コードに書いた d は、判定に使われる d_eff と一致しない。int() で分割すると最大2倍まで粗くなる。自作実装では 0.05 と書いた刻みが 0.0625 で動いていた
狭隘通路の実測では、成功率 20/20 のまま最大 2.143 mm 食い込んでいた。この値は公称刻みの上限（1.569 mm）を超え、実効刻みの上限（2.456 mm）の内側にあった

パラメータを勘で置いても、広い環境なら動きます。壊れるのは、障害物が薄いときと、通路がクリアランスと同じオーダーまで狭まったときだけです。動いているうちに上限を計算しておくほうが安上がりだと思います。

終わりに

本記事のきっかけは、自分の実装の定数を検算したら 1.25 倍ズレていた、という一点でした。式そのものは三平方の定理しか使っていません。厄介だったのは、式に入れる値のほうが実装によって変わることでした。

RRT 本体の実装と干渉判定の詳細は、既存の記事に書いています。

経路生成手法の自作記事まとめ：RRT系アルゴリズムの実装記事の一覧
経路生成 (RRT) を実装してみた Part1：本記事で「自作実装」と書いたコードの解説
干渉判定用ライブラリ python-fcl の使用方法：2Dの円で済まなくなったときの選択肢
なぜダイクストラは最短を保証できるのか？：グリッド系の探索と比べたい人向け

検証は、しきい値の総当たり、刻み幅の走査、最悪ケースの再現、モンテカルロまで一式を書いて回しました。記事中のコードはすべてヘッドレス（画面を開かずに実行すること。matplotlib.use("Agg") を指定）で動かし、-W all で実行時の警告が出ないことも確認しています（Python 3.10.9 / numpy 1.26.4 / matplotlib 3.7.0）。