技術要約: SAP-CoPE

問題提起

混雑した屋内環境（病院、ショッピングモールなど）で動作する自律移動ロボットは、安全かつ効率的なナビゲーションを妨げる2つの主要な課題に直面しています。

1. 知覚の限界: 車載センサのみに依存すると、視野（FOV）が限定され、遮蔽（オクルージョン）が頻繁に発生するため、人間の位置や意図をリアルタイムで正確に検出し、予測することが困難になります。
2. 社会的適合性: 従来の経路計画アルゴリズムは、物理的な衝突回避を優先する一方で、人間の心理的なコンフォートゾーン（快適領域）を軽視しがちです。これにより、ロボットが個人のパーソナルスペースに侵入して不快感を与えたり、非効率な挙動（不要な停止など）を引き起こしたりすることがあります。

既存のインフラベースのシステムは、コスト効率の高いセンサを用いて正確な3D人体ポーズや向き（ヘディング）を推定する能力に欠けていることが多く、また社会的な計画手法は、人間による対称的な影響を仮定したり、全方向移動ロボットの特定の運動学的制約を無視したりすることが頻繁にあります。

手法

本論文では、協調的なインフラセンシングと社会的に配慮した動作計画を統合したフレームワークであるSAP-CoPE（Social-Aware Planning using Cooperative Pose Estimation with Infrastructure Sensor Nodes）を提案します。システムアーキテクチャは以下の3つのレイヤーで構成されます。

• インテリジェント・センサ・ノード (ISN): カメラ、LiDAR、およびエッジコンピューティングを備えた天井設置型のノードです。これらはローカルでの2D検出および3Dポーズ推定を行います。
• 中央レイヤー (CL): ISNからのデータを集約してグローバルな融合、マルチオブジェクト・トラッキング、遅延補償、および動作計画を行います。
• アプリケーション・レイヤー (AL): ロボット（本研究では自律走行型医療ベッド）に対して制御コマンドを実行します。

1. 協調的知覚モジュール

知覚システムは、新しい3D人体ポーズ推定アプローチを通じて、遮蔽と奥行きの曖昧さに対処します。

• センサ融合: 微調整されたYOLOv11を用いた2Dカメラ検出と、疎なLiDAR点群を組み合わせます。
• 不確実性を考慮した推定: カメラの投影行列から世界座標への不確実性の伝播を行う最適化問題を定式化します。骨の長さとその不確実性の事前知識を取り入れることで、**結合コヒーレンス（joint coherence）**を強制します。
• 確率的モデリング: 人体の関節を多変量ガウス分布としてモデル化します。最大尤度推定（MLE）問題を用いて、関節のコヒーレンス制約とオプションのLiDAR計測値を統合し、3Dポーズを精緻化します。
• 遅延補償: 通信および計算のレイテンシに対処するため、タイムスタンプに基づく遅延補償と、定速度・旋回率モデルを用いた運動予測を採用し、古いデータと現在のシステム状態を一致させます。

2. 社会的に配慮した動作計画

動作計画器は、人間のパーソナルスペースを尊重しつつ、ロボットの運動学的制約を遵守する軌道を生成するために、モデル予測制御（MPC）を利用します。

• パーソナルスペース (PS) フィールド: 心理学の研究（Hall, Sommer）に着想を得て、人間のパーソナルスペースを非対称なフィールドとしてモデル化しています。人間は前方からの妨害に対してより敏感であるという特性を反映し、フィールドは背後よりも前方のほうが広くなっています。これは、対称的な人工ポテンシャル場（APF）や社会力モデル（SFM）とは対照的です。
• MPCの定式化: 以下の3つの項を最小化する目的関数を使用します。
  1. 社会的コスト ($J_1$): 人間の非対称なPSフィールドに進入することに対するペナルティ。
  2. 追従誤差 ($J_2$): 参照軌道からの偏差。
  3. 制御努力 ($J_3$): 車輪速度およびステアリング角の参照値からの偏差。
• 運動学的制約: ロボットの全方向移動能力（前後輪の独立したステアリングおよび速度）を明示的にモデル化し、車輪のスリップを防止するための線形化された制約を含みます。

主な貢献

1. リアルタイム協調知覚: 天井設置型のISNを活用することで、遮蔽に強く、拡張された視野を持つ知覚フレームワークを実現しました。
2. 新規3D人体ポーズ検出: 幾何学的手法、センサ融合（画像 + 疎なLiDAR）、および関節コヒーレンス制約を組み合わせ、不確実性伝播を伴う正確な3Dポーズ推定を実現する手法を開発しました。
3. 社会的に配慮したMPCプランナー: 非対称なパーソナルスペース・フィールドをMPCフレームワークに統合したアルゴリズムです。ロボットの全方向運動学と人間の心理的コンフォートゾーンを考慮することで、混雑した環境でのスムーズなナビゲーションを可能にします。
4. 実世界での検証: ベースライン手法と比較して、ポーズ検出および人間とロボットの相互作用におけるシステムの有効性を実証する広範な実験を行いました。

実験結果

システムは、人間（参加者）の数が変化する（2人、4人、6人）廊下環境で評価されました。

• 知覚性能:
  • 協調的なCam+LiDARアプローチは、学習ベースのベースライン（3DNet, Metrabs）や単一ノードの手法を上回り、最も低い位置誤差（MAE: 0.0771 m）とヨー角誤差（MAE: 6.86°）を達成しました。
  • 遅延を考慮したパイプラインは、通信遅延（最大200 ms）の影響を大幅に軽減し、遅延補償のないベースラインと比較して、より高い再現率（Recall）と低い変位・角度誤差を維持しました。
• 動作計画性能:
  • クリアランス: 提案手法は、特に群衆密度が高まるにつれて、社会力モデル（SFM）やポテンシャル場を用いたMPC（MPC+PF）と比較して、人間との間でより大きく一貫したクリアランスを維持しました。
  • 効率性: 提案手法は、すべてのシナリオにおいて最短の移動時間を達成しました。対照的に、MPC+PF法は、保守的で対称的なフィールドによって不要な減速が発生するため、混雑したシナリオにおいて移動時間が長くなりました。
  • リアルタイムの実現可能性: プランナー（知覚を含む）の計算時間は、制御ループの周期（100 ms）を十分に下回っており（障害物数に応じて中央値 約11〜19 ms）、リアルタイム実行能力が確認されました。

意義と主張

著者らは、SAP-CoPEが社会的快適性、効率性、および安全性のバランスをとった、屋内自律システムのための展開可能なフレームワークを提供すると主張しています。

• インフラによる知覚を統合することで、システムは遮蔽や視野に関する車載センサの限界を克服しています。
• 非対称なパーソナルスペースモデルにより、ロボットは対称的なモデルよりも自然かつ効率的にナビゲートでき、混雑した群衆の中でよく見られる「立ち往生」する挙動を回避できます。
• 全方向の運動学的制約を明示的に考慮することで、生成される軌道が特定のロボットプラットフォームに対して物理的に実行可能であることを保証しています。
• モジュール式のアーキタテクチャは、標準的なハードウェアを使用する既存の屋内ロボットプラットフォームと互換性があり、ヘルスケア物流や公共サービスロボットへの展開の強い可能性を示唆しています。

本論文は、現在のシステムは効果的であるものの、今後の課題として、高度に動的な環境における堅牢性の向上、および学習ベースのアルゴリズムによる人間による意図予測の改善に焦点を当てるとしています。