失敗しない IK は「到達可能集合への射影」で作る

要点: 到達可能集合は有限なので「どんな点でも届く IK」は物理的に存在しない。作れるのは「範囲外の入力でも解を返す IK」で、実体は 到達可能集合への射影 (最近傍の可達点を返す)。平面 2 関節なら制約境界ごとの 1 次元探索で厳密に解け、計算量は数 µs。

解析 IK が失敗する 5 分類#

いずれも「目標点が到達可能集合の外」なので、solver が正しく計算した結果である。

射影 IK の手順#

1. 解析解を出す。全制約を満たせば ok = true、そのまま返す。
2. 満たさない場合、次を解く。

$$\underset{q}{\min }\Vert \mathrm{FK}(q)-p_{\text{target}}{\Vert }^2\text{s.t.}{q}_{1,\min }\le q_1\le q_{1,\max },q_{2,\min }\le q_2\le q_{2,\max },q_2-q_1\le \alpha ,q_1-q_2\le \beta$$

2 次元 (q1, q2) の問題で、実行可能領域は 6 本の直線で囲まれた凸多角形。最小値は内点 (= 解析解、ここには無い) か境界上にある。6 本の境界線それぞれで 1 次元探索 (golden section、区間は多角形との交線) をして、最小距離の点を選ぶ。 base yaw は独立に clamp。OutOfReach は cos γ を ±1 に clamp すると、目標方向へ腕を伸ばし切った点 (半径方向の射影) になる。 3. 射影後の q と、FK で戻した実到達点 achieved を返す。ok = true にしつつ clamped = true、reason = "projected:<境界名>" で届かなかった事実を残す。

使い分け (射影を全部に適用しない理由)#

• teleop (jog): 射影を使う。上位は achieved を自分の target に採用すると、境界に沿って滑る。「押しても動かない」「戻される」感触が消える。
• 正確な到達が要る動作 (pick / place): strict を残す。射影距離 > 許容値 (例 5 mm) なら失敗として返す。射影で成功扱いにすると、届いていないのに把持動作へ進む。
• 障害物 (keepout) 判定は別問題。射影 IK は関節限界だけを扱う。

到達範囲そのものを広げる場合#

射影は範囲外を隠す手段であり、範囲は変わらない。範囲を広げるのは機構側 (関節限界、干渉) の話で、ソフトでは変えられない。

関連#

• パレタイザ型腕の解析 IK
• IK の検証順序