概要
本稿では、リアルタイム対応の安全バブル・ディテクタ（Safety Bubble Detector）を実現するためのアーキテクチャについて説明します。具体的には、モジュール・ソリューションの開発、データ帯域幅の広いアプリケーションの最適化（30fpsで検出を実行）、地面に近い物体を正確に検知するためのマルチスレッド・アプリケーション／アルゴリズムの設計に関する課題などについて述べます。
はじめに
現在、ロボットは様々な業界で広く使用されるようになってきています。それにより、効率と生産性の向上が実現されていることは間違いありません。但し、ロボットを活用するためには、その付近にいる人とアセットの安全性を確保する必要があります。そのためには、ロボットに衝突検出機能と停止機能を実装しなければなりません。これに関連し、本稿では安全バブル・ディテクタの話題を取り上げます。同ディテクタは、指定された安全ゾーン内に存在する物体や人を検出できるように設計されます。
アナログ・デバイセズは、Time Of Flight（TOF）に対応する評価用プラットフォーム「EVAL-ADTF3175D-NXZ」を提供しています。本稿では、同プラットフォームを使用して実現した安全バブル・ディテクタのアプリケーションを紹介します。その実装には、75°の視野（FoV：Field of View）を備えるTOFモジュール「ADTF3175D」を使用しています。実際のアプリケーションにおいて、より広いFoVをカバーしたい場合には同モジュール（TOFセンサー）をいくつか組み合わせることになります。例えば、270°のFoVをカバーするには、4台を使用するといった具合です。安全バブル・ディテクタの検出アルゴリズムは、EVAL-ADTF3175D-NXZが備えるプロセッサ「i.MX 8M Plus」上で実行されます。このアルゴリズムにより、センサーで取得した深度画像を基に、安全バブルの半径内に存在するあらゆる物体を検出します。また、ロボットへの組み込みを容易にするために、安全バブル・ディテクタのアプリケーションは、ROS（Robot Operating System）のフレームワークを使用して実装されています。更に、本稿で紹介するアルゴリズムは、このプラットフォーム上で30fpsのフレーム・レートを達成できるよう高度に最適化されています。
安全バブル・ディテクタは、無人搬送車（AGV：Automated Guided Vehicle）や自律移動ロボット（AMR：Autonomous Mobile Robot）の基本的なコンポーネントとして機能します。通常、安全ゾーンは、AGV/AMRの周囲の仮想的な円形のエリアとして設定されます。図1では緑色の円として示されています。安全バブル・ディテクタは、あらゆるAGV/AMRシステムにとって不可欠な要素です。本稿で紹介する例では、EVAL-ADTF3175D-NXZを使用して同ディテクタを実現しています。図2に示したように4台のEVAL-ADTF3175D-NXZを水平方向に配置し、各TOFモジュールが互いに67.5°の角度を成すようにしています。この構成によって死角が減り、278°のFoVが実現されています。
TOFモジュールとホスト・システムの間の通信には、ROSのパブリッシャ‐サブスクライバ型（出版‐購読型）のモデルを採用しています。すなわち、通信を円滑に行えるようにするために、図3に示すような構成をとっています。通信用のプロトコルとしてはEthernet over USBを使用し、データの完全性と高い通信速度を確保しています。
先述したように、このシステムでは高度なアルゴリズムを使用することにより、安全バブルの半径内の物体を検出します。ROSのトピックとして検出フラグが送信されたら、ホスト・マシンはすべてのモジュールのトピックをサブスクライブします。それにより、すべての検出結果を統合することが可能になります。また、TOFモジュールは詳細な分析を行えるようにするために、深度画像、IR（赤外線）画像、出力画像をパブリッシュします。ROSは、パブリッシュされたトピックを可視化するためのいくつかのツールを備えています。その例の1つが、効果的な可視化ツールであるrvizです。このアプリケーションは、高度な構成が行えるように設計されています。例えば、パラメータの値をROSのノードに引き渡すことで、カメラの位置、回転、その他の設定に使用する値を調整できるようになっています。
図4に示すように、このアプリケーションではマルチスレッドに対応するアーキテクチャを採用しています。それにより、入力、処理、出力という3つのスレッドが並列に実行されます。このように設計した目的は、レイテンシを最小限に抑え、処理用のブロックが、アクセス可能な最新のフレームに対応して継続的に動作できるようにすることです。入力用のスレッドはTOFモジュールから画像データを取得し、入力キューを更新します。一方、処理用のスレッドは入力キューを取得して、安全バブル・ディテクタの検出アルゴリズムを実行します。そして、検出フラグをパブリッシュし、出力を出力キューにプッシュします。出力用のスレッドは、出力キューを読み取って可視化のためのトピックをパブリッシュします。リアルタイム対応のシナリオでは、処理用のブロックのフレーム・レートが入力スレッドより低い場合、前のフレームは破棄されます。そして、レイテンシが最小の最新のフレームが優先されます。
ホストとTOFモジュールの間の通信は、ROSのパブリッシャ‐サブスクライバ型のモデルをベースとし、TCP/IPを使用して実行されます。ホストは、ROSのノード（TOFモジュール）からパブリッシュされた出力画像を結合し、合成後の出力をパブリッシュします。
ホストのマシンでは、NVIDIA®の「Jetson Xavier NX」を使用しています（図5）。そして、Ethernet over USBのプロトコルにより、4台のTOFモジュールに対する給電と通信を実現します。安全バブルの半径は、デフォルトの値で1mです。この値についてはROSのローンチ・ファイルで設定することができます。このエリア内で物体が検出されると、検出フラグがトリガされ、ROSのトピックを介してホストに送信されます。ホストのマシンは、各TOFモジュールによる物体の検出結果（トピック）をサブスクライブします。その結果は、単純な論理演算（OR）を使用して結合されます。いずれかのTOFモジュールが安全バブル内で物体を検出した場合には、物体が存在しているという結論が下されます。
TOFモジュールは、可視化を実現するために、取得した画像を上面図に変換します。そして、物体が安全バブルの内側にあるのか外側にあるのかに応じ、緑色か赤色のピクセルで表示を行います。その画像は、各TOFモジュールからROSのトピックとしてもパブリッシュされます。ホスト・マシンはそれらを結合し、1つの合成画像を生成します。パブリッシュされたすべての画像（トピック）を結合した画像は図6のようになります。
図6を見ると、左上の隅に正方形のボックスが描かれています。これは検出結果のステータスを表しています。この正方形が緑色であれば物体は検出されていないということです。赤色であれば物体が検出されたという意味になります（図7）。
これらの画像は、ROSのrvizというツールを使用することで可視化できます。また、NVIDIAのJetson Xavier NXの場合、HDMI®ケーブルでモニタに接続すれば、その出力を見られるようになっています。例えば、入力された画像の深度画像、点群、上面図といった可視化手法を使って分析を実施することが可能です。このような形で可視化技術を活用すれば、検出された物体とそれらの空間的な関係に関するより詳細な情報／知見が得られます（図8）。
SQAプロセスによる検証
開発したソフトウェアについては、安全性と品質を確認する必要があります。以下、標準的なソフトウェア品質保証（SQA：Software Quality Assurance）の手法に即した場合に必要な作業についてまとめます。
- ユニット・テスト：ROSは、複数のレベルのユニット・テストをサポートしています。
- ライブラリのユニット・テスト：ROSと依存関係を持たないライブラリのテストを実施します。
- ROSのノードのユニット・テスト：ノードのユニット・テストでは、ノードを起動し、その外部API（external application programming interface）のテストを実施します。ここで言う外部APIとは、パブリッシュされたトピック、サブスクライブされたトピック、サービスのことを指します。
- コード・カバレッジ：コード・カバレッジの分析は、ROSのパッケージの1つを使うことによって実施できます。この作業は、デッド・コードを削除し、ユニット・テストの品質を向上させることに役立ちます。
- ドキュメント化：ROSには、rosdoc_liteと呼ばれるパッケージが含まれています。これを使用すれば、ソース・ファイルを基にdoxygen形式のドキュメントを得ることができます。
- コードの整形：Clang-Formatを使用することにより、コードの整形（フォーマッティング）を実施できます。また、Clang-Tidyを使用すれば、ROSでガイドラインとして提示されているコーディング・スタイルに準拠することが容易になります。
本稿で紹介した安全バブル・ディテクタを稼働させれば、様々な形状、色、サイズの物体を確実に検出することができます。例えば、太さが5mmのケーブルなども検出可能です。
アルゴリズムの実行に伴うレイテンシは、わずか30ミリ秒に抑えられています。そのため、リアルタイムの物体の検出と応答を保証できます。
ROSのフレームワークをインターフェースや可視化に広く活用すれば、アプリケーションの移植性が向上します。ROSを使用するあらゆるホスト・マシンとの互換性が得られるので、製品を市場に投入するまでの時間を短縮できます。
なお、TOFモジュールでは、透明な物体や反射率の低い物体に対する精度が低くなります。つまり、離れた位置にあるガラスの瓶やプラスチックのボールといった物体を検出しにくいということです。図9にその特性を表す一例を示しました。これは、本稿で紹介しているアルゴリズムにより、各種の物体がどこまで近づいたら検出されるのかということ（物体検出距離）を表しています（安全バブルの半径は100cmに設定）。Y軸には、テストの対象とした物体を列挙しています。例えば、いちばん下の「ガラスの瓶（12, 7）」は、高さが12cm、幅が7cmのガラスの瓶を表しています。カッコ内に1つの値しか書いていないものについては、その値が物体の半径または立方体の辺の長さを表します。最後に表1をご覧ください。これは、本稿で紹介した安全バブル・ディテクタの概略仕様をまとめたものです。
| 指標 | 値 | 備考 |
| 検出に伴うレイテンシ | 30ミリ秒 | 画像の分解能：512×512 |
| 検出エリア | 円形／四角形のエリアを設定可能 | デフォルト：半径1mの円形エリア |
| FoV | 75° | センサーが1台の場合 |
まとめ
EVAL-ADTF3175D-NXZは、TOFモジュールとしてADTF3175Dを活用したプラットフォームです。本稿で紹介したように、同プラットフォームを使用して構成した安全バブル・ディテクタは多くの長所を備えています。例えば、i.MX 8M Plusのプラットフォームに対して高度に最適化されており、30fpsの滑らかな動作が得られます。また、マルチスレッドに対応することで、レイテンシを最小限に抑えて迅速かつ応答性の高い動作を確保しています。更に、SQAの手法を導入することで、開発したソフトウェアの安全性を確保し、品質に関する基準を満たしています。
参考資料
「インダストリアル・ビジョン技術」 Analog Devices
「Analog Devices ToF（アナログ・デバイセズ ToF）」GitHub, Inc.
「Analog Devices 3DToF ADTF31xx（アナログ・デバイセズ 3DToF対応のADTF31xx）」GitHub, Inc.
「Analog Devices 3DToF Safety Bubble Detector（アナログ・デバイセズ 3DToF対応の安全バブル・ディテクタ）」GitHub, Inc.
「Analog Devices 3DToF Floor Detector（アナログ・デバイセズ 3DToF対応のフロア・ディテクタ）」GitHub, Inc.
「Analog Devices 3DToF Image Stitching（アナログ・デバイセズ 3DToF対応の画像スティッチング）」GitHub, Inc.
謝辞
ADI-TOF SDKチームのサポートに感謝します。
