特異点付近での TCP 減速とその対処

要点#

特異点 (singularity) では TCP 速度 $v$ を出すのに必要な関節速度が $1/r$ で発散するので、関節の速度上限を守る限り TCP は必ず減速する。対処は「減速を受け入れる (速度スケーリング)」「直線を諦める (DLS / PTP)」「特異点を通らない (経由点)」の 3 系統。

数字・式#

関節速度と TCP 速度の関係 (ヤコビアン $J$):

$\dot{q}=J^{-1}v,\Vert \dot{q}\Vert \le \Vert J^{-1}\Vert \Vert v\Vert$

垂直多関節・SCARA 型の根元 (base yaw) 特異点では、TCP が回転軸から距離 $r$ にあるとき

${\dot{\theta }}_{\text{base}}=\frac{v_{\perp }}{r}$

($v_{\perp }$ は軸まわりの接線方向速度成分)。関節上限 ${\omega }_{\max }$ を守ると TCP 速度は

$v_{\perp }\le {\omega }_{\max }r$

に制限される。例: ${\omega }_{\max }=2.5$ rad/s なら $r=0.30$ m で 0.75 m/s、$r=0.05$ m で 0.125 m/s、$r=0.02$ m で 0.05 m/s。

可操作度 (Yoshikawa 1985):

$w=\sqrt{\det (J{J}^{\top })}$

$w\to 0$ が特異点。閾値 $w_0$ (単位系に依存、m と rad の混合で $10^{-2}$ 程度) で切替判定に使う。

減衰最小二乗法 (DLS、Nakamura & Hanafusa 1986 / Wampler 1986):

\lambda^2 = \begin{cases} 0 & w \ge w_0 \\ \lambda_{\max}^2 \left(1 - \dfrac{w}{w_0}\right)^2 & w < w_0 \end{cases}$$ $\lambda_{\max}$ は $0.05 \sim 0.1$ (m・rad 系) が典型。$\lambda > 0$ の間は $J\dot q \ne v$、つまり TCP は指令直線から外れる (誤差は有界)。 ## 対処法 5 種 | 方式 | 何をするか | 直線精度 | 関節速度 | 主な採用例 | |---|---|---|---|---| | 1. 速度スケーリング | 経路上の $\max |dq_j/ds|$ から $v \le \min_j \omega_{\max,j} / |dq_j/ds|$ でセグメント速度を落とす | 保つ | 上限内 | 産業用制御器の LIN 命令 (KUKA / FANUC の特異点近傍オーバーライド低減)、TOTP / TOPP-RA | | 2. DLS (減衰逆行列) | 上式で $\dot q$ を有界化。TCP は直線から外れる | 有界誤差 | 上限内 | 研究系 IK、MoveIt Servo (`lower_singularity_threshold` / `hard_stop_singularity_threshold`)、ROS 2 `kdl_kinematics_plugin` | | 3. PTP で通過 | 特異点をまたぐ区間だけ関節空間補間に切替 (LIN → PTP) | 捨てる | 上限内 | 産業用ロボットの標準運用。ティーチングで「特異点は PTP で通す」 | | 4. 経由点で回避 | 経路計画側で特異領域 ($r < r_{\min}$) を通らない (via point、回り込み) | 保つ | 上限内 | オフラインプランナ全般、パレタイザの段取り点 (radial stage) | | 5. タスク優先度 | 姿勢 (tool yaw 等) の拘束を $w$ に応じて弱め、位置だけ追従 (重み付き最小二乗 / null-space) | 位置のみ保つ | 上限内 | 冗長腕、人型 | MoveIt Servo の実装 (2 閾値): 条件数 $\kappa(J)$ が `lower_singularity_threshold` (既定 17) を超えると速度を $\kappa$ に比例して線形に落とし、`hard_stop_singularity_threshold` (既定 30) で停止する。停止直前の減速を滑らかにするための 2 段構成。 ## 選び方 - 手動ジョグ (人がスティックで動かす): 1 か 2。直線精度より「止まらない」ことが優先なら 2 (または 1 に速度下限を付ける = 実質 2 と同じ挙動)。 - 自動搬送: 4 が第一選択。経路を切る段階で $r_{\min}$ の外側に経由点を置く。またぐ必要があれば 3。 - 1 の副作用: 特異点脱出後に上限が戻ると再加速する (脈動)。上限の回復を rate 制限すると脈動は消えるが、ジョグ全体が遅いままになる。 ## 減速の目安 (根元特異点) | $r$ [m] | $v_\perp$ 上限 [m/s] ($\omega_{\max}=2.5$) | |---|---| | 0.30 | 0.75 | | 0.15 | 0.375 | | 0.05 | 0.125 | | 0.02 | 0.05 | ## 関連 - [/docs/control/task-space-ruckig](/docs/control/task-space-ruckig) — 直交 OTG と関節制限の整合 - [/docs/control/topp-ra](/docs/control/topp-ra) — 経路上の速度スケーリングの一般形 - [/docs/robotics/palletizer-arm-analytic-ik](/docs/robotics/palletizer-arm-analytic-ik) — 解析 IK と根元特異点 - [/docs/robotics/ik-nearest-feasible-projection](/docs/robotics/ik-nearest-feasible-projection) — 可動域外目標の射影