全方向移動機の経路追従

要点: 横に動ける機体では、pure pursuit の先読みが曲がりで内側を通る原因になる。横ずれ $e$ を横方向の速度 $-k_{e}e$ で直接消し、向きは $\omega ={\theta }^{\prime }(s)\dot{s}$ を前向き項にする。

制御則 (毎周期)#

射影    s* = argmin |p − c(s)|          窓の中だけ探し、単調に進める
基準点  s_r = s* + v·(τ + dt/2)         τ = 指令が効き始めるまでの遅れ
横ずれ  e   = (p − c(s*))·n̂(s*)         符号つき
速度    v_cmd = v·t̂(s_r) − clamp(k_e·e, ±v_lat_max)·n̂(s_r)
向き    ω_cmd = θ'(s_r)·v + k_ψ·wrap(θ(s_r) − ψ)
速さ    v = min(速度上限の包絡線(s_r), 加速の上限, 終端まで止まれる速さ)

閉ループは $\dot{e}=-k_{e}e$ になり、時定数は $1/k_e$。

pure pursuit が内側を通る理由#

先読み 0.3 m・最小曲率半径 0.15 m の経路では、運動学だけの閉ループで計画からのずれが最大 102 mm になった。横ずれを直接消す方式は同じ経路で 0.2 mm (詳細: RC27 BR の経路計画と追従)。その代わり pure pursuit は短い道を通るので、所要時間は 3.9% 短く出る。

向きを θ(t) でなく θ(s) で持つ理由#

• 機体が遅れると回転も同じだけ遅れる。回転が間に合わないことが起きない
• $\omega ={\theta }^{\prime }(s)\dot{s}$、$\alpha ={\theta }^{\prime \prime }(s){\dot{s}}^2+{\theta }^{\prime }(s)\ddot{s}$ なので、回転の上限が $(s,\dot{s})$ の制約になる。TOPP-RA にそのまま入る
• θ(s) の補間は PCHIP (形状保存 3 次エルミート) にする。線形補間は ω が段になり、素の 3 次スプラインは節点の間で行き過ぎる

スワーブで追加で要るもの#

• 旋回中心が車輪に乗る特異点: 輪 i の接地速度 $w_i=(v_x-\omega y_i,v_y+\omega x_i)$。$|w_i|\to 0$ でステア角が無限に速く回る。見る量は ${\min }_i|w_i|/{\max }_i|w_i|$ (0〜1)。追従の補正でこの値は計画より下がる (例: 計画 0.109 → 走行中 0.021) ので、計画側と実行時の 2 段で下限を持つ
• 低速での方向保持: 速度が小さいとき指令方向が数値誤差で回り、ステア目標が巻き取り限界まで回る。一定速度を下回ったら方向を保持する
• 終端: 下限速度を出し続けると止まらない。残り数 cm で経路追従をやめ、位置と向きの寄せに切り替える (許容差の中に一定時間入ったら到着)

関連#

• TOPP-RA
• 運動学的特異点の扱い
• RC27 BR の経路計画と追従