技術的概要：マニピュレータ搭載の連続体ロボット向けサンプリングベースのフォロウ・ザ・リーダー運動計画

1. 問題定式化

本論文は、完全に駆動された 6 自由度（6-DOF）のベース姿勢（$SE(3)$）を持つロボットマニピュレータに搭載された連続体ロボット（CR）における、フォロウ・ザ・リーダー（FTL）運動の運動計画問題を扱っている。

• 文脈: FTL は、ロボット本体がその先端の経路をトレースし、周囲の組織や構造体への横方向の力を最小化する重要な運動プリミティブである。蛇型ロボット、ステアブルニードル、同心管ロボットに対しては広範に研究されているが、腱駆動型 CR（TDCRs）に対する既存の手法は、通常、固定されたベースまたは 1 自由度（1-DOF）の直線挿入を仮定している。
• ギャップ: 実用的な展開では、CR を手術用アームや産業用検査ロボットなどの直列マニピュレータに搭載することが増えている。これにより、ロボットの内部構成（$Q$）と調整する必要がある 6-DOF のベース姿勢が導入される。
• 課題: 結合された構成空間（$K = SE(3) \times Q$）は高次元かつ非線形である。既存のアプローチは、しばしば単純化された運動学モデル（例：ピースワイズ定数曲率、PCC）と反復最適化（例：逆運動学ソルバー）に依存している。これらの手法は以下の欠点がある：
  • 初期値への感度と局所最適解への収束。
  • 計算コストのため、複雑で正確な前方モデル（例：コッセラットロッド、FEM、ニューラルネットワーク）を利用できない。
  • 解の完全性や先端追跡精度に関する形式的保証の欠如。

目的: 順序付けられた 3 次元経由点の集合 $W$ が与えられたとき、ロボット先端が経路を正確に追跡し、挿入された本体が経路からの偏差を最小化する連続的な運動計画を生成する。その際、CR 構成と 6-DOF ベース姿勢を同時に最適化する。

2. 手法

著者は、グローバル形状探索をローカル補間およびベース姿勢決定から解離させるサンプリングベースのフレームワークを提案する。中核的な革新は、任意の候補形状に対して 6-DOF ベース姿勢を一意に決定する閉形式の幾何学的構成であり、オンライン計画中の反復最適化の必要性を排除する。

A. オフラインライブラリ生成

• CR 構成空間 $Q$ を一様にサンプリングすることで、形状のライブラリをオフラインで事前計算する。
• 各サンプルに対して、一般的な前方モデル（FM）が離散化されたバックボーン点を計算する。
• これにより、複雑な運動学モデルの計算負荷をオフライン段階へシフトし、オンライン最適化には実用的でないモデルの使用を可能にする。
• クラスタリング: オンライン検索の複雑さを $O(N_{lib})$ から $O(\sqrt{N_{lib}})$ に削減するため、形状をペアワイズ類似性に基づいてクラスタリングする。実行中は、プランナがまずクラスタ中心を評価し、次に最良の一致するクラスタを網羅的に検索する。

B. オンラインベース姿勢アライメント（幾何学的構成）

各経由点 $w_i$ とライブラリからの候補形状 $S$ に対して、ベース姿勢 $T_b \in SE(3)$ は、形状をアクティブな経路に整合させる 3 つの逐次変換を通じて決定される：

1. 並進: 先端（$p_D$）が現在の経由点 $w_i$ と正確に一致するように形状を並進させる。
2. 先端周りの回転: アクティブな形状点から先端へのベクトルを経路方向に整合させるよう、$w_i$ 周りで形状を回転させる。
3. 軸回転: 3 番目の対応点（弧長比に基づく）を経路に整合させることで、残りの回転自由度（ツイスト）を解決する。

主要な特性: この構成は、選択された形状に関係なく、反復解法なしに設計上先端配置を正確に保証する（$p_D = w_i$）。

C. 形状評価と選択

• 変換された形状とアクティブな経路との偏差を、対称的な Chamfer 距離を用いて計算する。
• この偏差を最小化する形状が選択される。このプロセスはライブラリ全体で並列化可能である。
• 選択された構成 $(q, T_b)$ が、スパースな運動計画を形成する。

D. ローカル補間

展開用の連続軌道を生成するために：

• 半径対称性による事前整合: 連続する構成を、共通の参照方向へ回転させることで事前整合させる。これにより CR の半径対称性を活用し、向きの不連続性を最小化する。
• 滑らかな補間: 中間構成は、ジョイント状態を線形補間し、先端の向きには SLERP を使用して生成する。
• ベース姿勢の再構成: 各補間ステップにおいて、ベース姿勢は、希望する補間された先端姿勢に対する前方モデルの先端姿勢の逆を用いて構成される。これにより、補間ステップ全体を通じて正確な先端追跡が維持されることを保証する。

3. 理論的保証

本論文は、最適化ベースのアプローチでは通常欠けている形式的保証を確立する：

1. 解像度完全性: ライブラリサイズ $N_{lib} \to \infty$ となるとき、有効な計画（存在する場合）を見つける確率は 1 に近づく。これは形状偏差メトリックのリップシッツ連続性に依存する。
2. 構成による正確な先端追跡: 幾何学的アライメントにより、各離散ステップで先端が経由点に正確に位置することが保証される。
3. 漸近的な先端追跡収束: 補間ステップの数が増加するにつれて、経由点間の直線経路からの先端の最大偏差は消失する（$O(1/h^2)$）。

4. 実験結果

本手法は 3 つの設定で評価された：

A. PCC モデル対最適化ベースライン

• 設定: 3 つのクラス（C 字カーブ、S 字カーブ、ロボットカーブ）にわたる 120 の経路。L-BFGS-B 最適化ベースラインと比較。
• 結果:
  • 先端誤差: 提案手法では 0%（最適化では約 1.2%）。
  • 形状偏差: 線形探索変種は最適化ベースラインを上回った（例：C 字カーブで 1.25% 対 1.74%）。
  • 効率性: クラスタリング変種は、形状精度の最小限の損失で計算時間を 1 桁以上削減した（例：約 1.8 秒対約 40 秒）。

B. MuJoCo シミュレーション（疑似剛体モデル）

• 設定: 現実的な変形とセグメント結合をシミュレートするため、システム同定された PRB モデル（30 の剛体リンクと弾性ジョイント）を使用。
• 結果: 0% の先端誤差を達成。モデルの複雑さにより形状偏差はわずかに高かった（約 2.5–3.2%）が、競争力があり、複雑な前方モデルへのスケーラビリティを実証した。

C. ハードウェア実証

• 設定: 7-DOF の Franka マニピュレータに搭載された 3 セグメントの腱駆動型 CR。
• 結果: ロボットは協調的な FTL 運動を正常に実行した。
  • 先端誤差: 観測されたハードウェア誤差（平均 5.5%–9.1%）は、プランナ失敗ではなく、システム同定モデルと物理的現実（腱の伸び、摩擦、ヒステリシス）の間のギャップに起因するとされた。
  • 検証: ハードウェア結果は、プランナが質的に正しい FTL 運動を達成する滑らかで実行可能な軌道を生成することを確認した。

5. 意義と主張

本論文は、完全に駆動された $SE(3)$ ベースを持つ連続体ロボット向けに、形式的な計画保証を提供する最初の FTL 運動計画手法を提示すると主張する。

• 解離: 主要な貢献は、形状探索をベース姿勢決定から解離させることである。ベース姿勢を幾何学的に解くことで、高次元空間における反復最適化の局所最適解と計算ボトルネックを回避する。
• モデル非依存性: 重い計算をオフラインへシフトすることで、PCC、コッセラット、FEM、ニューラルネットワークなど、任意の前方モデルをサポートする。
• 実用性: 理論的計画とマニピュレータ搭載システムへの実用的展開の間のギャップを埋め、速度（クラスタリングによる）と精度の間の原理的なトレードオフを提供する。
• 安全性と予測可能性: 理論的保証（解像度完全性と正確な先端追跡）は、予測不可能なプランナの動作が許容されない安全上重要な要件に対応する。

著者は、重力効果を無視したベース姿勢非依存の前方モデルという仮定や、軌道全体の形状最適化の欠如といった限界を指摘し、これらを今後の研究の方向性として提案している。