近年、自律移動ロボットの普及が急速に進んでいます。従来は工場内など限定された環境で使用されていた自律移動ロボットですが、世界的な労働者人口不足などの影響から、宅配、飲食店の配膳、建物内警備などの分野への進出が始まっています。これらのロボットは「人や物にぶつからない」というミクロな判断と、「地図上での現在位置と進行方向」というマクロな情報を常にアップデートする必要があります。
しかし、人や物が多い場所では、不意の接触や、レーダー情報の混乱などにより、ロボットが現在位置を見失うことがあります。このとき、どのようにして現在位置を再取得するのかが、ロボット運用の検討課題の一つとなっています。
参考：経済産業省「自動配送ロボットを活用した新たな配送サービスについて」

位置再取得の手法

ロボットが自己位置を見失ったときの再取得手法として次のような技術が用いられています。

• パッシブ測距センサ（LiDAR等）
• 電磁波（UWB:Ultra Wide Band, BLE（Bluetooth Low Energy））
• 人工衛星（RTK-GPS）
• ２次元画像マーカー （AprilTag）

本コラムでは、導入が容易な「２次元画像マーカー（AprilTag)」を用いた自己位置再取得手法（TagSLAM）を紹介します。

SLAM（Simultaneous Location And Mapping）

現在、ロボットの自己位置推定の手法としてレーザー光で距離を測るLiDAR技術を用いて、予め与えられた地図と照合するLiDAR SLAMが広く使用されています。GPSが届かない屋内でも使用が可能であり、装置がコンパクトで汎用性が高いなどの利点があります。一方で、「特徴の少ない場所では自己位置を推定できない」「短時間に大きな移動をすると自己位置を見失う」といった特徴もあります。

マトリクス型２次元コード

ロボットの移動範囲内に貼付された画像マーカー（ランドマーク、タグ）から自己位置を推定する手法がTagSLAMです。マーカーには、紙面に白黒の点が印刷された「マトリクス型２次元コード」が使用されます。
2次元コードと言えば、任意の情報を埋め込むことが出来るQRコード（QRコード(R)は、株式会社デンソーウェーブの登録商標です）が日常生活で広く用いられています。AprilTagはオープンソースでサポートされるタグで、設定できる情報量は少ないのですが検出精度はQRコードより高いとされています。AR markerは仮想現実で用いられるタグで、位置推定にはあまり用いられていません。

TagSLAMではQRコードとAprilTagのどちらでも用いることができますが、検出精度の高さからAprilTagが好まれる傾向にあるようです。画像マーカーはプリンタで印刷したもので十分な精度が得られ、作成や変更が容易、かつカメラ側も2K程度の解像度で実用に耐えうるため導入のハードルが低いというメリットがあります。

AprilTag

AprilTagは"25h9", "36h11"などの「ファミリー」で分類されます。"25", "36"などの数字はビット数を表し、25なら5x5マトリクス、36なら6x6マトリクスです。同じ大きさのタグであれば、"25h"の方が"36h"よりも遠方からの認識が可能ですが、設定できる情報量は小さくなります。

"h"の後の数字はハミング距離で、大きな値の方がエラー耐性が大きく、設定できる情報量は小さくなります。
例えば"36h11"ファミリーのAprilTagでは、6x6マトリクスに587個のユニークIDが設定されています。

ファミリー名	ユニークIDの数
16h5	30
25h9	35
36h11	587
Circle21h7	38
Cirlce49h12	65,698
Custom48h12	42,211
Standard41h12	2,115
Standard52h13	48,714

QRコードはカスタム文字列を格納できるのに対し、AprilTagはIDしか識別できません。すなわち、QRコードならマーカー内に設置場所の情報を直接書き込むことができますが、AprilTag単独では座標を表すことができないのです。このためAprilTagを自己位置推定に用いる際は、システム側に用意したIDと座標を紐づけたテーブルに、発見したマーカーと座標を紐づけるシステムが必要です。

位置推定

マーカーから距離を求めるのはどのような仕組みなのでしょうか？

マーカーの姿勢は、初期位置からの回転と平行移動で表すことができます。3次元空間での正方形の四隅の初期座標（X0,Y0,Z0）と、回転・平行移動後の座標（X,Y,Z）の関係は次式で表されます。（Rは回転行列、Tは並進行列）

次に、カメラの焦点と撮像面の距離をfとすると、実空間上にあるマーカーの四隅の3D座標（X,Y,Z）と、撮像面に投影されたスクリーン座標（x,y）には次のような関係が成り立ちます。（wは消去できる媒介変数）

最後に、スクリーン座標（x,y）を画像座標（i,j）に変換することで、実空間上にある物体のデジタル画像上の位置を得ることができます。この（i,j）が、デジタルカメラなどで撮影したときに得られる情報です。（kはスケール、cx, cyは画像の中心座標を表すパラメータで、カメラのキャリブレーションによって得られる）

以上の式は空間座標（X,Y,Z）から画像座標（i,j）を得るものですが、この逆変換を求めると、マーカーの姿勢行列T・Rを算出することができます。T・Rにより２次元のデジタル画像からマーカーの３次元的な位置と回転が取得できるという仕組みです。

これらの数値は、apriltag_rosなどのライブラリを使用すれば自動的に算出できます。しかし、パラメータのチューニングを行うのであれば、算出原理について理解しておく必要があるでしょう。
また、TagSLAMの推定精度はカメラのキャリブレーションに強く依存するため、キャリブレーションの質が精度向上の一つの目安になっています。

apriltag_ros

ROS2のapriltag_rosライブラリを用いたTagSLAMシミュレーションについて説明します。
apriltag_rosライブラリでは、カメラ画像と内部パラメータ（先述のk,cx,cy）を与えるだけでタグ認識が開始されます。カメラの視野内にAprilTagが写ると、ロボットの自己位置からTagまでの位置と回転が出力されます。

次に、検出したAprilTagのIDと、予め作成したテーブルを引き合わせて座標を取得します。このマーカー座標から距離と向きを逆算することで自己位置推定ができるというのが、TagSLAMの原理です。

実際の動作を見てみましょう。次の図はGazeboシミュレータ上に、ロボット、部屋、AprilTagを配置したものです。

ロボットにはLiDARセンサが設置されており、この距離情報から常に自己位置を推定しています。この推定された自己位置を可視化ツール「RViz」に表示したものが下図です。

位置を見失うという現象は、Gazeboシミュレータのロボットを動かすことで再現します。

(1)TagSLAMが働いていない場合

• ロボットを大きく移動する
• 自己位置を見失う
• ロボットを人の手で元の位置に戻すか、現在位置を与える
• 自律移動が再開する

(2)TagSLAMが働いている場合

• ロボットを大きく移動する
• 自己位置を見失う
• AprilTagが見える位置まで自律移動する
• 自己位置を推定し、現在位置を再取得する
• 自律移動が再開する

このように、自己位置を見失った場合の復旧を半自動で行えることがTagSLAMの利点です。

貼付位置

２次元マーカーの貼付位置は任意に設定できます。ある場所には「壁面」、別の場所では「天井」など柔軟に対応することができるのもTagSLAMの特徴です。ロボットが自己位置を見失う場所や状況に応じて、様々な貼付場所が考えられます。

まとめ

今回は２次元マーカーを用いたロボットの自己位置推定について詳解しました。
TagSLAMは、画像を用いた位置推定の手法で、ARマーカーライブラリの「ARToolKit」が公開された2004年頃から脚光を浴び始めました。その後、マーカーレスの研究が進み、Apple社の「ARKit」などではマーカーレスで位置推定を行うAPI（Application Programming Interface）も搭載されています。
しかし、混雑時や大規模移動時の自己位置回復が目的の場合には、ロバスト性（外的要因による変化を内部で阻止する仕組み）の高いマーカー方式に利点があると考えます。

関連サービスについて、詳細はこちらをご覧ください。
ROS/ROS2開発支援サービス

この記事の執筆者