システム / 制御 / 情報,Vol. 64, No. 2, pp , 移動体の自己位置推定技術 :SLAM 特集号解説 SLAM を応用した複数ドローンによる協調的屋内空間探索土屋武司 * 1. はじめに近年, ドローンとよばれている小型無人航空機の多方面での活

(1)

||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||

「移動体の自己位置推定技術： SLAM ^」特集号 ^解 ^説

SLAM を応用した複数ドローンによる協調的屋内空間探索

土屋　武司*

||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||

1. はじめに

近年，「ドローン」とよばれている小型無人航空機の多方面での活用が注目を集めている．とくに，複数のプロペラを用いて飛行するマルチコプタへの注目度が高い．垂直離着陸可能で，ホバリング（空中静止）可能なドローンにカメラやLIDARセンサを搭載させ，環境を計測する試みも始まっている．そのためには自己位置の正確な推定が重要である．屋外ではGPSなどの衛星測位システムを用いることができるが，屋内では利用できない．これまで著者らは室内を飛行するドローンの自己位置推定に関する研究を行ってきた．

この問題に対しては，SLAM(Simultaneous Localiza- tion and Mapping)とよばれる手法が有効と考えられる．

SLAMとは，カメラやLIDARといったセンサを用いて自己位置推定と周辺の環境地図作成を同時に行う手法である．ドローンは地上移動ロボットに比べ移動の自由度が大きく，車輪などの駆動装置の回転数から移動距離を求めるWheel odometryとよばれる手段が適用できない．

また，ドローンは飛行制御のためにジャイロ・加速度センサからなる慣性計測装置(IMU，Inertial Measurement Unit)を搭載しているが，搭載重量，サイズ，電力に制限があることから小型で精度が高くないMEMSセンサなどが搭載の限界である．これが地上移動ロボットに比べ，ドローンのSLAMを難しくしている要因である．そこで，ドローンの機上で，実時間でSLAMを成立させるためにはアルゴリズムに工夫が必要である．本稿はこれまで著者らが行ってきた，ドローンへの実装を目的にしたSLAMアルゴリズムを紹介する．

一方，著者の研究室は，宇宙航空研究開発機構(JAXA) と共同で，災害が起きた屋内環境に複数のドローンを投入させて自律的にかつ協調的に内部環境を探索させるようなシステムの研究開発を行っている（第1図）[1]．多数のドローンを一度に使用することで効率的に広範囲の屋内を探索することができる．そこで，ドローンの自己位置と自機の周辺環境探索はSLAMによって確立していることを前提とし，そのうえで，探索情報を集約する基地局を地上に設置し，ドローンとの通信を確立しつつ効率的な各ドローンの経路計画を定める手法を研究して

∗ 東京大学大学院工学系研究科航空宇宙工学専攻 Key Words: drone, unmanned aerial system, SLAM.

いる．本稿は，複雑な探索環境においてドローンに冗長な動きや配置の偏りがないように，各ドローンの探索目標を適切に定める協調的探索アルゴリズムの研究を紹介する．

なお，本研究では探索する環境は高さ方向に変化しないと仮定し，ドローンはレーザ光を水平走査して障害物までの方位と距離を得るLIDARと，ジャイロ・加速度センサからなるIMUを搭載していることを前提とする．

探索環境の明暗に依存する画像センサは使用しない．

第1図複数ドローンによる屋内探索イメージ[1]

2. LIDAR SLAM のドローンへの実装

2.1 Hector SLAM^{アルゴリズム}

SLAMの解法には多くの手法が提案されてきた．初期には純粋な確率論に従う手法が提案され，一例として拡張カルマンフィルタ(EKF，Extended Kalman Filter) を用いたEKF-SLAMなどが挙げられる．しかし，これらの手法では環境中の特徴点を抽出する必要があり，また観測する特徴点の数が増えるにつれて計算負荷が高くなるという問題があった．現在はセンサの小型化が進んだことを受け，LIDARより得られる点群データを用いるスキャンマッチングとよばれる手法が提案されている．

その一つがKohlbrecherらによって提案されたHector SLAMである[2]．このアルゴリズムは計算負荷が比較的低いことから，本研究のSLAMアルゴリズムの一部として採用している．

Hector SLAM は，LIDAR から得られた周囲の障害物までの距離情報を含んだスキャン点群をもとに

(2)

第2図 Hector SLAMにおけるスキャンマッチング SLAMの問題を解くSLAM subsystemと，これを補

償する形でIMUから得られる加速度・角速度の情報を用いるNavigation subsystemから構成される．SLAM subsystemはHector SLAMの中心となるアルゴリズムである．環境は高さ方向に変化しないという前提のもと，二次元的な占有格子地図によって環境を表現する．

占有格子地図とは，環境を一定の大きさの格子に分割し，各格子に壁といった障害物が存在する確率を格納した地図である．これを確率の大きさに応じたグレースケールで表示すると，部屋の間取り図のような画像が得られ，視覚的にも解りやすい表現方法といえる．SLAM subsystemでは，新たにLIDARから得られたスキャン点群が既存の占有格子地図と最も合致する座標変換を最適化計算手法の一つであるガウス・ニュートン法によって求めることで，占有格子地図に対する相対的な自己位置推定を行う．この手法は一般的にスキャンマッチング (Scan matching)とよばれる．その概略を第2図に示す．

第2図(a)が占有格子地図，第2図(b)がLIDARから得られる点群であり，この二つからスキャンマッチングによって求められた最適な座標変換が第2図(c)である．

原理的にはスキャンマッチングのみで自己位置推定は可能であるが，ドローンを安定して飛行させるためには数十Hz以上の高い更新周波数が必要である[3]．

LIDARから得られる点群は約数百点におよび，ドロー

ンに搭載する演算装置で，高い周期で座標変換を繰り返し計算して点群を更新することは困難である．そこで，SLAM subsystemは低更新周波数に抑え，その間をIMUから得られる慣性情報を用いて高更新周波数で補完するNavigation subsystemを適用した．これらの二つのsubsystemから得られる推定情報は，拡張カルマ

ンフィルタにより統合される．

2.2 拡張カルマンフィルタの実装

カルマンフィルタは誤差のある観測値を用いて動的システムの状態量を推定するための手法であり，線形システムに対して適用可能なものである．これを非線形システムに対しても適用できるようにしたのが拡張カルマンフィルタである．これは非線形システムの各時刻における状態量まわりのヤコビアンを計算することによって局所的にシステムを逐次線形近似してカルマンフィルタに適用することによって実現される．

航空機の運動を表現するために用いられる座標系に準じた二つの座標系，E系(Earth Frame)とB系(Body Frame)を定義する．E系は静止（慣性）座標系であり，

xy平面を水平面内に，z軸を鉛直下向きに取る．ここで，

さらにx軸を北向き，y軸を東向きとするNED座標系とよばれる座標系が航空機の運動表現でよく用いられるが，Hector SLAMにおいては原理的に方位を決定する手段がない．そこで，E系のx軸方向は機体の最初の向きに一致させることとする．B系は機体固定座標系（動座標系）であり，機体前方向にx軸，機体右方向にy軸，

機体下方向にz軸を定義する．

以上のもとで，本手法におけるシステム状態方程式を表す．まず，E系で表した機体の位置ベクトルr≡ (x,y,z)^T∈R³^{，速度ベクトル}v≡(vx,vy,vz)^T∈R³^，B系へのクォータニオンq˜∈R⁴^{から状態量}x≡(r^T,v^T,q˜^T)^T∈ R¹⁰^{を構成する．また，}IMUから得られるB系で表した機体の3軸加速度a∈R³^，3軸角速度w∈R³^で入力 u≡(aIMUT,wIMUT)^T∈R⁶を構成する．このとき，微小時刻間隔∆tで離散化された状態方程式は，

(3)

f(x,u)≡







r+v∆t+1

2(˜q^∗aIMUq+˜ g)∆t² v+ (˜q^∗aIMUq˜+g)

˜ q+1

2qw˜ IMU∆t





 (1)

x(t+∆t) =f(x(t),u(t)) (2) と定義される．ここで，gはE系での重力加速度ベクトルである．これがNavigation subsystemにおける状態量xの更新式である．

また SLAM subsystem からは，二次元座標 (x,y) とヨー角（方位角）ψが得られる．これを観測値z≡ (x,y,ψ)^T∈R³^{とすると，状態量}xの間に以下の関係がある．

h(x)≡





x y

tan⁻¹(2(q1q2+q0q3)/(q02+q12+q22+q32))



 (3)

z=h(x) (4)

これを観測方程式として定義する．これらの状態方程式と観測方程式に拡張カルマンフィルタを適用し，状態量 xを推定していく．

2.3 ^{大規模環境における}SLAM^{実験（その１）}

ドローンに搭載したHector SLAMを基本とする提案手法の有効性を調べるため，大規模環境下における実験を実施した．実験環境は第3図に示すような縦20m，横 60m程度の建物内であり，中央の廊下を挟むように小部屋がいくつか存在する．これらの空間内でドローンを人が動かしてLIDARの点群データとIMUの加速度・角速度データを収集し，SLAMを用いて内部環境地図の推定を行う．なお，実験時点で，機上のオンボードPCで SLAMのリアルタイム処理を実現することはできておらず，データ収集のみを行い，オフラインでSLAM計算を行った．

第4図は生成された地図を示している．おおむね建物内の構造を知ることはできるが，提案手法は途中まではある程度正しい推定を行うことができたが，徐々に地図の蓄積誤差の影響が大きくなり，推定が破綻してしまうことが認められた．とくに長い廊下など，特徴の少ない空間を進むとき，徐々に地図が歪んでいく．また，同じ場所を再び通過したとき，壁の位置が不鮮明になる．このように，単一の占有格子地図を逐次更新する提案手法では，大規模環境においてうまく推定が働かなくなる場合があることがわかる．

2.4 ^{特徴点抽出を用いた}Loop closure

前節の実験から，提案手法による地図の歪みと地図推定の破綻を防ぐためには，再度訪れた場所の認識と，地図全体の歪みの修正が必要である．そこで，Loop closureとよばれる手法を導入する．ここではHierarchical SLAM[4]

第3図探索を行う建物（Google Mapより）

第4図提案するSLAMによる探索結果

の手法を参考として，グラフ構造をSLAMに適用する例を示す．

まず，環境地図をLocal mapとGlobal mapの二種類の地図に分割する．Local mapは複数個の占有格子地図から構成され，その占有確率は前節のSLAMによって推定される．個々のLocal mapは前節のSLAMが破綻しないサイズに制限される．したがって，ドローンはまず初めに単一のLocal mapのみを保持してこれを前節のSLAMにより更新するが，ある一定の距離を超えた場合には，それまで使用していたLocal mapの更新を停止し，新たに別のLocal mapを作成・更新し始めることとする．

これに対して，Global mapはグラフ構造から構成される．グラフはNodeとEdgeから構成され，各Nodeは個々のLocal mapとその原点位置に対応する．Edgeは各NodeのLocal map間の相対変位を表す．Node間で Edgeが発生するのは以下の2通りとする．(1) 前述のようにドローンが新たなLocal mapを作成し始めたとき，(2)画像処理的手法により異なるLocal map同士で同一の場所があることが判定された場合，である．これはGraph SLAMと考え方が似ているが，Graph SLAM ではNodeをロボットの位置に置いたPose-graphを考える．一方，本研究ではLocal mapをNodeに置いたグラフ構造を考えている．Pose graphはすべてのロボット位置を異なるNodeとして扱うことから非常に多くの Node が発生し得るのに対し，本手法においてはLocal mapの個数に限られ，計算負荷が抑えられている．

Local map間での同一箇所を検出する手法については，Local mapにおける占有格子地図を画像データと

(4)

第5図 Loop closureの適用結果

見なし，画像処理により特徴点を抽出するというアプローチを取る．特徴点の抽出手法としてORB[5]を使用する．得られた特徴点の中から類似したペアを探索する方法については，ハミング距離を類似度の判定に用いる

“BruteForce-Hamming”法を用いる．これらはオープンソースの画像処理ライブラリOpenCVによって比較的容易に実装することができる．

また，検出された特徴点ペアからLocal map間の相対的な座標変換を導出するにあたっては，特異値分解 (SVD，Singular Value Decomposition)を用いる．ただし特徴点ペアには間違った対応付けをされたもの(outlier) が含まれる可能性があるため，この除去を目的として RANSAC(Random Sample Consensus)[6]を併用する．

2.5 大規模環境におけるSLAM実験（その２）

2.3節の実証実験に，提案したLoop closureを適用した場合の結果を示す．Local mapの形状は縦横20mの正方形とし，機体が5m以上移動した場合に新たに作成される設定とした．実験では，Local mapは32個生成された．あるLocal mapのNodeと一つ前のLocal map のNodeとの間にはEdgeが作られるが，さらに遠隔の Local map間において類似した特徴点がないかどうか ORBを用いて探索し，類似箇所が見つかった場合はそれらのNode間にもEdgeを付加した．

以上により求められた地図と第3図の衛星写真に重ね合わせたものを第5図に示す．第4図と比較すると，提案手法によって環境地図をより正確に構築できることが認められた．

3. 複数ドローンを用いた協調的探索

ここまでドローンが取得したデータから地図を構成する方法を解説してきたが，つぎに大規模な空間を複数のドローンが協調的に探索することを考え，各ドローンの経路計画を求める．そのために，複数のドローンに加え，地上に固定された一つの基地局を仮定する．ここで，各ドローンはLIDARとほかのドローンおよび基地局とデータ通信するための通信装置を搭載しており，すべてのドローンは画一的であるとする．また，基地局は各ドローンが得た探索情報を集約し，各ドローンの探索

経路計画を求め，それを伝達する．そのために，基地局と各ドローン間の通信は距離に応じて直接，あるいはドローンで中継するマルチホップ無線ネットワークにより結ばれる．また，各ドローンは基地局が立てた探索経路に従って正確に飛行すると仮定する．そのうえで，室内空間の探索を効率的に実施するドローンごとの探索経路をリアルタイムに求めるアルゴリズムを考える．なお，

効率的な探索経路とは探索時間が短い探索のことである [7]．

3.1 探索アルゴリズム

本研究の探索アルゴリズムはPeiらの手法[8]をもとに通信距離制約を考慮したアルゴリズムになっている．

ある時間に基地局はそれまでに判明している占有格子地図とドローン全機の位置配置を得ると，未探索領域の情報を収集する「探索ノード」と探索ノードと基地局との通信を接続する「中継ノード」をドローンの機数分だけ求める．そして，各ドローンにノードを割り振り，そのノード位置を目的地点とする経路を生成する．その後，

経路と目的地点は各ドローンに伝えられ，ドローンはその経路に沿って周囲を探索しながら移動する．全ドローンが割り振られた位置に到着した時点でアルゴリズムの１ステップが終了し，以降はこれを繰り返す．

探索ノードのもっとも一般的な決定法は，探索領域と未探索領域の境界を「フロンティア」とよび，フロンティアに対するヒューリスティック関数を定義して，この関数値に基づき探索ノードの位置を求める方法である．ここで，ドローンの現在位置から探索ノードの候補地点までの経路長から定義されるコスト関数，候補地点に到達した際に新たに得られる情報量の推定値から定義されるユーティリティ関数，複数のドローンの目標地点の偏りを防ぐために，候補地点間の距離で定義される関数の三つの関数を組み合わせて評価関数を定義し，最適な探索ノードを配置する．

また，中継ノードの配置法は全探索ノードと基地局の通信を接続する最小数の中継ノード配置を決定する問題であり，Steiner Minimum Tree with Minimum num- ber of Steiner Points and bounded edge length(SMT- MSP)[9]とよばれるネットワーク最適化問題として定義する．

各ドローンの目的地が決定された後，現在地から目的地までの移動経路が計算される．占有格子地図上における経路生成手法としてA^∗ Algorithmが一般的である．A^∗ Algorithmは最適な経路を生成することが保証されているものの，地図の解像度が小さくなるにつれて計算時間が増大する．そこで，距離変換および細線化処理を用いた経路生成手法(Distance Transform Based Path Generation)[10]を適用する．この手法により，A^∗ Algorithmとほぼ同等の準最適経路が少ない計算時間で生成される．

(5)

第6図複数ドローンによる屋内探索結果の一例

3.2 数値計算シミュレーション

本探索手法の数値計算結果を第6図に示す．探索範囲は約80m×60mとし，これを20cm四方で格子を構成す

る．ドローンの数は10機とし，各ドローンにはこれまで実験で用いてきたLIDARと同等な性能をもつLIDAR

(6)

を搭載している．また，ドローンの移動速度は0.5m/sec 一定であると仮定した．ドローンと基地局間，またドローン同士の最大通信可能距離は20mとする．

第6図に示すように，10機のドローンがすべての領域を探索するのに866.19sec要したが，ドローンは効率的に分散して配置され，左下にある基地局と各ドローンとの間の通信はほぼ確保できているのがわかる．なお，本シミュレーションには，CPUがCore i7-3770 3.4 GHz， RAMメモリが16GBである計算機を利用し，経路探索に計22.26 secを費やした．基地局の計算機がこれと同等以上の性能を有すれば，リアルタイム性があることが確かめられた．

4. おわりに

本稿はドローンにLIDARを搭載し，屋内空間のようなGPSを利用できない領域を探索するためのSLAM技術を紹介した．ドローンは地上移動ロボットと比較して，

搭載物のサイズ，重量，電力に制限がある．また，3次元空間を移動するためにWheel odometryなどの情報も利用できず，ドローンはLIDARに加え，MEMSのジャイロ，加速度センサから構成される慣性航法装置を搭載するのみと仮定した．また1機のドローンが探索できる範囲は限られているが，複数機を協調的に飛行させることで広範囲を短時間に探索することができる．複数ドローンを用いた協調的探索のアルゴリズム研究も紹介した．

空中を飛翔するドローンは地上ロボットが到達でない範囲に移動することができる．今後，利活用が広がることが期待され，そのときSLAMが基盤技術になると思われる．

(2019年10月23日受付) 参考文献

[1] 久保,土屋: 複数MAV協調運用による複雑任務対応能力の研究;日本航空宇宙学会第51回飛行機シンポジウム (2013)

[2] S. Kohlbrecher, O. Stryk, J. Meyer and U. Klingauf:

A ﬂexible and scalable SLAM system with full 3D motion estimation;Proceedings of 2011 IEEE Inter- national Symposium on Safety and Rescue Robotics, pp. 155–160 (2011)

[3] 藤原: リアルタイムSLAMによる飛行ロボットの屋内空間探索;平成26年度東京大学大学院工学系研究科航空宇宙工学専攻修士論文.

[4] C. Estrada, J. Neira and J. D. Tardos: Hierarchical SLAM: real-time accurate mapping of large environ- ments; IEEE Transaction on Robotics, Vol. 21, Iss.

4, pp. 599–596 (2005)

[5] E. Rublee, V. Rabaud, K. Konolige and G. Bradski:

ORB: an eﬃcient alternative to SIFT or SURF;Proc.

IEEE International Conference on Computer Vision, pp. 2564–2571 (2011)

[6] M. A. Fischler and R. C. Bolles: Random sample consensus: A paradigm for model ﬁtting with appli- cations to image analysis and automated cartogra- phy; Communications of the ACM, Vol. 24, Iss. 6, pp. 381–395 (1981)

[7] K. Umeki, D. Kubo and T. Tsuchiya: Cooperative indoor space exploration by multiple micro aerial ve- hicles with connectivity constraints; Proc. the 2018 Asia-Pacific International Symposium on Aerospace Technology (APISAT 2018), pp. 2410–2424 (2018) [8] Y. Pei, M. W. Mutka and N. Xi: Connectivity

and bandwidth-aware real-time exploration in mobile robot networks;Wireless Communications and Mo- bile Computing, Vol. 13, No. 9, pp. 847–863 (2013) [9] X. Cheng, D. Du, L. Wang and B. Xu: Relay sen-

sor placement in wireless sensor networks; Wireless Networks, Vol. 14, No. 3, pp. 347–355 (2008) [10] 楳木: 複数MAVを用いた協調的屋内空間探索;平成30

年度東京大学大学院工学研究科航空宇宙工学専攻修士論文．

著者略歴

つち土屋^や　 ^たけ武司^し

2000年3月東京大学大学院工学系研究科航空宇宙工学専攻博士課程修了．同年4 月航空宇宙技術研究所（現，宇宙航空研究開発機構）研究員，2002年10月東京大学講師，2015年教授となり現在に至る．航空機力学，制御工学の研究に従事．日本航空宇宙学会フェロー，日本機械学会などの会員．

「移動体の自己位置推定技術： SLAM 」特集号 解 説

SLAM を応用した複数ドローンによる 協調的屋内空間探索

土屋 武司*