Obstacle-Avoidance Path Planning for a Basket-Carrying Robotic Arm Based on an Improved APF-RRT Algorithm

技術要約：改良型APF-RRTアルゴリズムに基づくバスケット搬送用ロボットアームの障害物回避経路計画

問題提起
本論文は、非構造的な農業環境における材料搬送用ロボットアームの経路計画という、極めて重要な課題に取り組んでいる。具体的には、複雑に、かつランダムに配置された障害物（例：作物の枝）や、高度に変化するバスケットのポーズが存在する中で、フィールドバスケットを自律的に把持・搬送するタスクを対象としている。従来のRapidly-exploring Random Tree (RRT) アルゴリズムは、目標指向性の弱さ、収束効率の低さ、および冗長で滑らかでない経路の生成により、高次元（6自由度）の関節空間において困難に直面している。さらに、標準的な人工ポテンシャル場（APF）法は、ターゲットが障害物の近くにある場合に、局所解へのトラップや目標到達不能の問題を引き起こす。これらの制限は、安全な農業搬送作業に求められるリアルタイムの応答性と運動の安定性を阻害している。

手法
著者らは、改良されたAPF法をノード拡張メカニズムに深く統合した、改良型RRTアルゴリズムを提案している。核となる技術要素は以下の通りである：

1. 多重場結合ノード拡張： 単なるランダムサンプリングに頼るのではなく、アルゴリズムは以下の3つの成分を組み合わせることで、ノード拡張のための合力ベクトルを算出する：
   • 目標引力： 引力ゲイン係数によって重み付けされた単位ベクトル。
   • 障害物斥力： 斥力ポテンシャル場の勾配に基づく。
   • ランダム探索： グローバルな被覆範囲を維持するための、ランダムサンプリング点への単位ベクトル。
2. べき乗則減衰斥力ゲイン： 目標到達不能問題（斥力が原因で、障害物付近のターゲットにアームが到達できない問題）を解決するため、著者らは標準的な指数関数的減衰を、斥力ゲイン係数（$k_r$）のべき乗則減衰モデルに置き換えた。これにより、ノードが目標に近づくにつれて、斥力がより高次のレート（$O(d_{goal}^\beta)$）で減少することを保証し、狭い空間での収束を可能にしている。
3. 3段階進行型脱出メカニズム： 引力と斥力が相殺される局所解に対処するため、アルゴリズムは失敗カウンター（$f$）と3段階の脱出戦略を採用している：
   • 通常モード： 標準的な拡張。
   • ソフト脱出： 軽微な停滞が検出された際、拡張方向にガウス分布に従うランダムな摂動を加える。
   • ハード脱出： 深い停滞が発生した場合、ポテンシャル場によるガイダンスを抑制し、複数の候補方向を生成して、衝突のない通過スパンが最も大きい方向を選択する。
4. ペナルティ重み付き距離指標： 最近傍探索が局所解に陥ったノードを繰り返し選択してしまうのを防ぐため、累積失敗数に基づくペナルティ項をユークリッド距離指標に追加している。
5. 適応型戦略：
   • 適応型ステップサイズ： 障害物および目標への近接度に基づいて拡張ステップサイズを動的に調整し、探索速度と衝突安全性のバランスをとる。
   • 動的目標バイアス確率： 累積的な進捗に基づき、区分的に滑らかな関数（3次エルミート補間とそれに続く線形成長）を用いて、広範な探索から指向的な収束へと移行させることで、目標構成のサンプリング確率を調整する。
6. 後処理： 初期経路は、冗長なノードを除去するためにマルチパス・グリーディ・プルーニング（貪欲な枝刈り）が行われ、続いて、ロボットの滑らかな運動を保証するために $C^2$ 連続性（曲率および角加速度の連続性）を確保する3次Bスプライン平滑化が行われる。

主な貢献

• アルゴリズムの統合： 農業用ロボットアームの高次元かつ障害物密度の高い関節空間に合わせて特別に設計された、改良型APFとRRTの斬新な融合。
• 目標到達不能問題の解決： べき乗則減衰斥力ゲイン係数の導入により、従来のAPF法に共通する目標到達不能のジレンマを効果的に排除した。
• 能動的な局所解脱出： ペナルティ重み付き距離指標と組み合わせた3段階進行型脱出メカニズムの開発により、ランダムツリーがデッドロック領域で停滞することなく、能動的に回避することを可能にした。
• エンジニアリング指向の最適化： 適応型ステップサイズおよび動的目標バイアス戦略に加え、厳格な後処理（プルーニングおよびBスプライン平滑化）を含めることで、生成された経路が衝突フリーであるだけでなく、計算効率が高く、運動学的に滑らかであることを保証している。

実験結果
提案されたアルゴリズムは、2Dおよび3D構成空間における比較シミュレーション、および6自由度Siasun GCR25-1800協働ロボットアームを用いた実機実験を通じて検証された。

• 2Dシミュレーション： 従来のRRTと比較して、改良型アルゴリズムは、疎な環境、狭い通路環境、および複雑で高密度な障害物環境において、平均反復回数を85.8%～89.7%削減し、計画時間を42.1%～56.4%短縮した。経路長は、RRTおよびRRT-Connectと比較して18.6%～19.6%短縮された。
• 3Dシミュレーション： 球体障害物を含む3D空間において、本アルゴリズムは平均17.1回の反復で計画を完了した（RRTおよびRRT*に対して99.3%の削減）。また、RRTと比較して経路長を31.4%短縮した。
• 6自由度ロボットアーム実験： バスケット搬送の関節空間計画において、提案アルゴリズムは、経路コストをRRTに対して38.9%、RRT-Connectに対して43.9%、RRT*に対して32.9%削減した。計画時間はベースラインと比較して10.1%～20.9%短縮された。
• 実機検証： 実機試験により、計画された軌道がすべての関節において、衝突のない滑らかでジッターのない運動をもたらすことが確認され、アルゴリズムの安定性とエンジニアリングへの適用可能性が実証された。

意義と主張
本論文は、提案されたアルゴリズムが、複雑な農業環境における計画の応答性と運動の安定性に関するエンジニアリング要件を効果的に満たしていると主張している。標準的なRRT（非効率性、冗長性）およびAPF（局所解、目標到達不能）の具体的な限界に対処することで、本手法は非構造的な農地における自動材料搬送のための堅牢なソリューションを提供する。著者らは、本アルゴリズムは（ラボでテストされたように）半構造的で比較的静的なシナリオでは優れた性能を示すものの、現在は事前にモデル化された静的障害物に依存しており、動的で予測不可能なフィールド条件（例：揺れる枝や移動する作業員）に対するオンライン再計画機能はまだ組み込まれていないと述べている。本研究は、実用的な農業オートメーションに向けた重要な一歩として位置づけられ、ロボットアームの搬送作業のための実現可能なアルゴリズム基盤を提供している。