TOPP-RA

幾何経路 q(s) に対して速度・加速度・トルク制約を満たす時間最短パラメータ化 s(t) を求めるアルゴリズム (Pham & Stasse 2018)

要点: 幾何経路 $q (s)$ が与えられたとき、速度・加速度・トルク制約を満たす時間最短の $s (t)$ を計算する。到達可能集合を LP で解く方式で、TOPP-NI より特異点に強い (Pham & Stasse, IEEE T-RO 2018)。

入出力 #

項目	内容
入力	経路 $q (s), s \in [0, 1]$ (config space) + 制約群
出力	時間最短 $s (t)$ (経路上の進行速度)
計算量	$O (N \cdot M)$ (N = 離散点数、M = 制約数)
実装	`pip install toppra` (Python)、C++ 版もあり
論文	Pham & Stasse, "A New Approach to Time-Optimal Path Parameterization Based on Reachability Analysis", IEEE T-RO 2018

扱える制約 #

Velocity: $∣ \overset{q}{˙}_{i} ∣ \leq v_{i, m a x}$
Acceleration: $∣ \overset{q}{¨}_{i} ∣ \leq a_{i, m a x}$
Torque (動力学モデル込み): $∣ τ_{i} ∣ \leq τ_{i, m a x}$
2 階微分で書ける任意の制約 (力、接触、ジャーク等)

アルゴリズム #

位相平面 $(s, \overset{s}{˙}^{2})$ 上で:

経路を N 点に離散化
各点で前方到達可能集合 (forward reachable set) を LP で計算
後方到達可能集合も同様
両者の交わりが実行可能な $\overset{s}{˙}^{2}$ の範囲
その上限を辿ると時間最短の $\overset{s}{˙} (s)$
積分して $s (t)$ 、経路と合成して $q (t)$

TOPP-NI (Numerical Integration) は singular arc の処理で発散しやすい。RA 版は LP ベースで数値的にロバスト。

planner との分離 #

モーションプランナ (RRT / PRM / 可視グラフ) → 幾何経路 q(s)
                                                 ↓
                                            TOPP-RA
                                                 ↓
                                          時間最短軌道 q(t)
                                                 ↓
                                          軌道追従 (FF + PID)

planner はコリジョン回避と可到達性のみ担当し、速度・加速度・トルクの物理限界は TOPP-RA が担当する。

制約モデルの与え方 #

静的制約 (速度・加速度上限) → yaml 等で機構ごとに指定
動的制約 (トルク・接触力) → 動力学モデル + アクチュエータ性能
摩擦は詳細モデル化せず上限値を実測して与えることが多い (同定コスト回避)
経路上の姿勢 $θ (s)$ を別途定義することもできる (向きに制約がある機体)

Python 使用例 #

import toppra as ta
import toppra.constraint as constraint
import toppra.algorithm as algo
import numpy as np

path = ta.SplineInterpolator(np.linspace(0, 1, 5), waypoints)

vlim = np.array([[-vmax, vmax]] * dof)
alim = np.array([[-amax, amax]] * dof)
pc_vel = constraint.JointVelocityConstraint(vlim)
pc_acc = constraint.JointAccelerationConstraint(alim)

instance = algo.TOPPRA([pc_vel, pc_acc], path, solver_wrapper='seidel')
jnt_traj = instance.compute_trajectory()

ts = np.linspace(0, jnt_traj.duration, 100)
q, qd, qdd = jnt_traj(ts), jnt_traj(ts, 1), jnt_traj(ts, 2)

他手法との比較 #

アルゴリズム	手法	特徴
TOPP-NI	数値積分 (forward/backward)	古典的、singular arc に弱い
TOPP-RA	到達可能集合 + LP	ロバスト、標準的
Ruckig	オンライン jerk-limited	リアルタイム、jerk 制約向き、経路は入力しない
BVP shooting	最適制御	汎用だが計算コスト高い

Ruckig と TOPP-RA は用途が異なる。Ruckig は「今の状態から目標へ jerk-limited で行く」オンライン軌道生成、TOPP-RA は「与えられた幾何経路を時間最短で辿る」オフライン最適化。組み合わせも可能 (planner → TOPP-RA → 実行時に Ruckig で追従調整)。