日本ロボット学会誌 Vol. 31896

No. 9, pp.896〜906, 2013

学術・技術論文

大規模な三次元環境地図と RGB-D カメラを用いた
移動ロボットの広域位置同定
鄭

龍

振∗

倉 爪

亮∗

岩 下

友

美∗

長谷川

勉∗

Global Localization for Mobile Robot using Large-scale 3D Environmental Map and
RGB-D Camera
Jeong Yongjin∗ , Ryo Kurazume∗ , Yumi Iwashita∗ and Tsutomu Hasegawa∗
We proposed a global positioning technique in 3D environment using 3D geometrical map and a RGB-D camera
based on a ND (Normal Distributions) voxel matching. Firstly, a 3D geometrical map represented by point-cloud isconverted to ND voxels, and eigen ellipses are extracted. Meanwhile, ND voxels are also created from a range image
captured by a RGB-D camera, and eigen ellipses and seven representative points are calculated in each ND voxel.
For global localization, point-plane and plane-plane correspondences are tested and an optimum global position isdetermined using a particle filter. Experimental results show that the proposed technique is robust for the similarity
in a 3D map and converges more stably than a standard maximum likelihood method using a beam model.
Key Words: 3D Global Localization, 3D Environmental Map, Occupancy Grid, Particle Filter, Normal Distributions
Transformation

多く提案されている [6]．しかし，近年，三次元レーザスキャナ

1. は じ め にの普及により，高精度な三次元環境地図がロボット用途としても移動ロボットにとって正確な位置同定機能は，効率的な作業遂利用可能になりつつある [7]〜[12]．一方，Kinect（Microsoft）

行のためには欠くことができない基本技術である．これまでにや Xtion（ASUSTek），SR4000（Swiss Ranger）など RGB-D（1）オドメトリや IMU など，ロボットに搭載した内界センサ

カメラと呼ばれる低価格の距離センサが相次いで発売され，リアルタイムで面状の三次元距離データが手軽に得られるように情報を用いた手法（2）ランドマークの発見や観測など，ロボットに搭載した外界なった．
そこで本論文では，事前にレーザ計測により得られた大規模センサ情報を用いた手法（3）ロボット外に設置されたセンサによりロボットの動きを観な三次元環境地図 [13] を用いた，RGB-D カメラを搭載したロボットによる大域的位置同定および局所的位置追跡手法を提案す測する手法 [1] [2]などが提案されてきた [3] [4]．一方，ロボットの位置同定問題はる．従来，このような三次元情報を用いた位置同定は，環境地図

以下の二つに大別できる [5]．

と観測データに含まれる点群を用いた ICP（Iterative Closest（a）位置に関する事前知識を用いない大域的位置同定（b）位置に関する事前知識を用いる局所的位置追跡

Point）法 [7] [8] や，観測データ内の点群やボクセルと環境地図
内の面やボクセルの分布を比較する手法 [14] [15] などが提案さ本論文では，（2）外界センサを用いた手法のうち，特にロボットれている．しかしこれらの手法は一般に計算量が多く，特に位に搭載した三次元距離センサからの大量の観測情報と大規模な置に関する事前知識が利用できない大域的位置同定においては，三次元環境地図の照合により，広域環境において上記（a）（b）

大規模な地図に直接適用するには問題がある．
この問題に対し，本手法では，まず点群データで表されたの位置同定問題を効率的に扱う手法を提案する．
従来，二次元レーザレンジファインダや超音波センサなどの三次元環境地図に NDT（Normal Distributions Transforma-

距離センサをロボットに搭載し，計測された距離データとあらかtion）[16] を適用し，点の分布を三次元正規分布で表現した ND

じめ与えられた二次元地図との比較から位置を同定する手法がボクセル（Normal Distributions Voxels）を生成する．次に各ボクセルにおいて，計算された三次元正規分布から最小固有値

原 2012 年 4 月 11 日∗
九州大学∗
Kyushu University
 本論文は有用性で評価されました．

JRSJ Vol. 31 No. 9

方向を法線とする代表平面（Eigen Ellipse）を抽出し，これらを地図データとして登録する．また位置同定時には，RGB-D
カメラにより得られる点群から同様に ND ボクセルを生成し，—68—

Nov., 2013

大規模な三次元環境地図と RGB-D カメラを用いた移動ロボットの広域位置同定

897

各ボクセルで代表的な 7 点および代表平面を抽出して，これをSLAM を開発している．Wülfing ら [21] は，RGB-D カメラを

計測データとする．その後，点–平面間距離や平面法線方向の比搭載したロボットから得られる距離データと，三次元環境モデ

較により地図データと計測データの一致度を計算し，パーティ

ルから合成して得られる仮想的な距離データを比較して，ICP

クルフィルタにより移動ロボットの自己位置を同定する．

法により高速に位置同定を行う手法を提案した．また，事前にNDT は，ICP 法に代わる複数距離データの高速な位置合わ

レーザスキャナにより得られた三次元環境地図を用い，その断せ手法として提案されたものであるが，本手法は，NDT のもう

面とロボットに搭載した 2D レーザレンジファインダによる計一つの特徴である局所特徴の効率的な格納性能に着目し，NDT

測値とを比較して，三次元位置同定を行う手法も提案されていを局所特徴を用いた大規模三次元データ間の大まか，かつ高速

る [11] [12]．

な位置合わせに利用するものである．広域かつ詳細な三次元空

また，三次元ボクセル同士を直接比較して三次元位置同定を間データの利用や大量のデータが取得できるセンサの開発によ行う手法も提案されている．Olson ら [14] は，ステレオカメラ

り，大規模データの高速処理はロボット分野でも喫緊の課題であから得られる距離データから占有ボクセル地図を作成し，既存る．本論文は特にレーザ計測された数千万点もの大規模な三次の高さ地図と比較して，最尤推定法により最も形状の一致する

元環境地図と，一度に 30 万点の距離データが得られる RGB-D

位置を求める手法を提案した．Ryde ら [22] も多重解像度占有

カメラを用い，NDT の性質を巧みに利用することで，実用的ボクセル地図を用いた同様の手法を提案している．
一方，Biber らにより提案された NDT [16] は，一般的な ICP

な処理時間で実行可能なロボットの新たな三次元位置同定手法

法で最も計算コストの高い最近傍点探索を行わずに，二つの距を提案する．
なお，通常，NDT では，参照点群（位置合わせ先の点群，本離データを詳細に位置合わせする手法である．ICP 法に比べて研究では環境地図）と対象点群（位置合わせされる点群，本研究

精度は同程度か若干劣るものの，高速かつより離れた初期位置かでは RGB-D カメラから得られた点群）に対し，まず参照点群

らでも収束する特徴がある [23]．Takeuchi ら [24] [25] は ND ボに NDT を適用し，得られた正規分布に対象点群のすべての点

クセルの大きさを収束に従って変化させることで，広範囲な収束を投影して尤度を求め，その総和が最小となる相対位置をニュー

性能と高精度を両立する手法を提案している．また Magnusson

トン法などで決定する [16] [17]．これに対し，提案手法は環境

らは坑道内を走行するロボットに対する，三次元 NTD を用いが多くの平面で構成されているとの仮定のもと，通常，多くのた三次元位置同定手法を提案している．Kaminade ら [17] は，平面から構成される人工環境により適した手法として，1）対象

高解像度二次元 NDT を用いた地図表現に対する，広範囲な収点群にも NDT を適用して少数の代表点で表現する，2）参照，

束計算手法を提案している．Magnusson ら [26] は，MD ボク

対象点群の NDT から代表平面を計算し，点・平面間距離や平セルを固有値をもとに線や面に分類し，得られたヒストグラム

面同士の法線の差を尤度として用いる，などの改良を加えたもを比較することでループ検出を行う手法を提案した．
これに対し，提案手法は NDT（あるいは ND ボクセル）をのである．
本論文では，第 2 章で関連研究について述べた後，第 3 章で本来の目的である距離データ間の詳細な位置合わせに用いるの提案手法の詳細を示す．また第 4 章で 70 × 35 × 3 [m] の屋内ではなく，NDT のもう一つの特徴である局所特徴を効率よく環境に対し，群ロボットシステムによって構築された詳細な環

表現，格納できる性質に着目し，NDT を局所特徴を用いた二境地図 [13] と RGB-D カメラ（Kinect, Microsoft）を用いて，つの距離データ間の大まかな位置合わせに利用するものである．

提案手法の評価を行った結果を示す．

提案手法により大まかに位置合わせされた距離データは，すでに ND ボクセルの計算は終了していることから，提案手法で得

2. 関 連 研 究られた位置を初期位置として通常の NDT と同様に詳細な位置

レーザスキャナや超音波センサを用いたロボットの二次元（位合わせを行うことも可能である．

である．特に，最尤推定法や ICP（Iterative Closest Point）法

なお，大域的位置同定には，Grid を用いた手法 [27] や Particle filter を用いた手法 [5] [18] などが提案されているが，その多くは二次元地図を対象としたものである．一方，Kummerle
ら [28] は，2.5 次元地図である Multi-level Surface Map を用を用いた二次元スキャンマッチングや，パーティクルフィルタをいた三次元大域的位置同定を提案していが，センサ情報から直用いた位置同定手法は多く提案されている [18] [19]．Konolige

接推定される自由度数は，位置 2 自由度，姿勢 1 自由度である．

ら [20] は，ある方向に沿った計測距離と，その方向で計測点に三次元点群を用いて移動ロボットの位置姿勢 6 自由度を推定す

最も近い地図データとの距離を比較し，その差の総和が最も小る方法としては，Nüchter らの 6D-SMAM [8] が有名であるが，さくなる位置を計測位置として最尤推定する基本的な手法を提これは局所的位置追跡のために逐次的に ICP を用いる手法であ案した．

り，後述のように計算時間の問題から大域的位置同定への適用

置 2 自由度，姿勢 1 自由度）の位置同定法では，ロボットがレー
ザ光や超音波を地面と平行に投射し，得られた距離データとあらかじめ得られている平面地図データを比較する手法が一般的

一方，近年，三次元レーザスキャナや RGB-D カメラの普及により手軽に三次元データが得られるようになったことから，三次元データを用いた三次元（位置 3 自由度，姿勢 3 自由度）のは困難である．

3. 三次元環境地図と RGB-D カメラを用いた位置同定

位置同定法も提案されている．Nüchter ら [7] [8] は，レーザ計
測された連続的な点群データに逐次的に ICP 法を適用した 6D

日本ロボット学会誌 31 巻 9 号

本章では，提案する三次元環境地図と RGB-D カメラを用いた移動ロボットの位置同定手法の詳細を示す．

—69—2013 年 11 月

鄭

898

龍

振

倉

爪

亮

岩

友

美

長谷川

勉

(a) Point-cloud data of elevator hall(indicated by circle in Fig. 2)

Fig. 1 Concept of NDT and ND voxels [16](a) Corridor

下(b) Point-cloud data(b) Eigen ellipses of corridor(c) Top view of point-cloud data(c) Eigen ellipses of elevator hall converted
from point-cloud data (a)Fig. 3 Point-cloud data and eigen ellipses(d) Floor map
Fig. 2 Point-cloud data of a 3D map measured by a LRF

3. 1 地図データ
地図データには，群ロボットシステム [13] によってあらかじめ構築された大規模な三次元環境地図を用いる．ただし，この環境地図は大量の点群データから構成されており，位置同定時の計算量削減のために，事前に Fig. 1 に表す ND ボクセル化を行
Fig. 4 Floor map extracted by ND voxels

い，分散値が最も小さい方向を法線方向とする代表平面（Eigen

Ellipses）を抽出しておく．ND ボクセル [16] とは，空間を格子状に分割し，それぞれの格子内に含まれる観測点群の位置に

また Fig. 4 に表すように，代表平面の法線ベクトルが重力対して分散行列を求め，それを固有値分解することで得られる方向と一致し，かつ中心の高さが事前に指定した範囲の ND ボ

分散値および軸の方向により点群を三次元正規分布で表現する

クセルを床面として抽出し，二次元平面地図に変換することで，手法である．なおボクセルは一般に空間を分割した小領域を示

ロボットの移動可能領域を事前に抽出しておく．これは後述す

すが，本論文ではそのうち計測点が存在するものをボクセル，

るパーティクルフィルタの処理において，パーティクルの初期

さらに複数の計測点が存在し正規分布が計算されたものを ND分布を決定するのに利用される．

ボクセルと定義する．今回実験に使用した環境地図（70 × 35
× 3 [m]，Fig. 2）はおよそ 4 千万点の点データからなり，一辺800 [mm] の ND ボクセルを用いた場合，およそ 5 万ボクセルとなった．得られた Eigen Ellipses を Fig. 3 に示す．

JRSJ Vol. 31 No. 9

なお，離散化の影響を低減するため，提案手法では Biber
ら [16] の手法を三次元に拡張し，Fig. 5 に表すように各格子を半分ずつ重複するように配置（オーバーラップ化）し，一つの点が八つのボクセルに含まれるようにしている．

—70—

Nov., 2013

大規模な三次元環境地図と RGB-D カメラを用いた移動ロボットの広域位置同定

899

Fig. 8 Seven representative points

Fig. 5 Overlapped ND voxels

Fig. 6 Mobile robot equipped with RGB-D cameras (Kinect)[29]

Voxel size 3,200 [mm](90 voxels)

Voxel size 1,600 [mm](304 voxels)

Voxel size 800 [mm](1,141 voxels)

Voxel size 400 [mm](4,327 voxels)

Voxel size 200 [mm](17,720 voxels)
Fig. 9 Voxel resolution pyramid of eigen ellipses in elevator hall(indicated by circle in Fig. 2)(a) Photo of elevator hall (indicated by circle in Fig. 2)

3. 3 マルチレベル ND ボクセル
上述のように，計測データはオーバーラップ化された ND ボ
クセル，代表平面，および七つの代表点により表現される．このとき，ボクセルの大きさが小さいほど複雑な形状を細かく表現できるが，ボクセルの総数が増加し位置同定の計算量も増加する．
一方，大きなボクセルを用いると，ボクセル総数が減少し高速な(b) Point-cloud data of elevator hall

計算が可能になるが，位置同定精度は低下する．そこで Fig. 9

Fig. 7 Point-cloud data measured by a RGB-D cameraに表すように，異なる大きさのボクセルを用いて ND ボクセル化を行い，位置同定の状況に応じて解像度を変更する．具体的

3. 2 計測データには，後述の実験では，パーティクル数が初期状態の 72,000〜

本論文では，Fig. 6 に示すような RGB-D カメラ（Kinect,

5,000 個まで収束した時点で，解像度を 1,600〜800 [mm] に変Microsoft）を搭載した移動ロボットに対して位置同定を行う．
RGB-D カメラから一回の計測で得られる 30 万点の三次元点群
データの一例を Fig. 7 に表す．
この点群データに対しても，同様に ND ボクセルを計算する．
その後，得られた三次元正規分布に対して，Fig. 8 に示すよう
pに，各軸方向に半径 −2 loge r の球面上の点をだ円上に射影更した．

した点（付録参照）を合計六つ抽出し，正規分布の中心点（重示す手順に従い尤度計算を行う．

3. 4 パーティクルフィルタを用いた位置同定・追跡
上述したように ND ボクセルで表された地図データと計測
データを用い，パーティクルフィルタ [30] により大域的位置同定と局所的位置追跡を行う．ただし，各パーティクルはそれぞれ候補となるロボットの位置 t，姿勢 R を保持し，Fig. 10 に心点）と合わせて七つの点を計測データとする．ただしここで

まず Fig. 11 に示すように，計測データ中の各ボクセルを位

は r = 0.5 とした．また地図データと同様に代表平面も求めて置姿勢候補で座標変換し，地図データである各ボクセルの代表

おく．なお計測データに対しても，地図データと同様にボクセ

平面と計測データである代表点の距離および代表平面の法線のルをオーバーラップ化して，離散化の影響を低減する．

角度差を計算する．

日本ロボット学会誌 31 巻 9 号—71—

2013 年 11 月

鄭

900

龍

振

倉

爪

亮

岩

下

友

美

長谷川

勉

最終的に，7 個の代表点に対して評価値の和を計算して，計測データのボクセル i の評価値 δi とする．

δi =

7
X

（7）

γik

k=1

最後に，計測データのすべてのボクセルの和を求めて，パー
ティクルのスコア λ とする．

λ=

N
X

（8）

δi

i=1

Fig. 10 Flow of likelihood calculation

ただし N は計測データのボクセル数である．
なお，式（6）では，距離評価値 αik→m と角度評価値 βi→mの積で，観測された代表点 Sik の評価値 γik を計算しているが，これはそれぞれの評価値を観測値に対する条件付き確率とみなし，代表点ごとに尤度を求めたものである．一方，式（7），（ 8）は，各代表点で得られた尤度の和として，全代表点および全ボ
クセルの一致度を求めたものである．

Fig. 11 Evaluation of distance and orientation

なお，必要なパーティクル数は，KLD サンプリング法 [5] に

具体的には，Sik = (Sikx , Siky , Sikz )T を計測データのボクより，収束状況に応じて 1,000〜5,000 個の範囲で適応的に変更

T

セル i における代表点 k (k = 1 ∼ 7)，Ni = (Nix , Niy , Niz )する．ただし初回のパーティクルフィルタの計算のみ，ランダを代表平面の法線ベクトルとすると，位置姿勢変換後の代表点

ムにサンプリングされた 1,000 箇所の位置において，72 とおり

S̃ik ，代表平面の法線ベクトル Ñi はそれぞれに方向を変えた 72,000 個のパーティクルを使用した．

S̃ik = RSik + t（1）Ñi = RNi

（2）

4. 位 置 推 定 実 験Fig. 6 のロボットを用い，Fig. 4 に表す 70 × 35 × 3 [m] の屋
内廊下において，ロボットが移動しながら距離データを計測し，となる．次に，変換後の代表点が含まれる地図データ中の 8 個（1）単一の距離データを用いた大域的位置同定実験のオーバーラップ化されたボクセル m (m = 1 ∼ 8) に対し，計（2）時系列距離データを用いた局所的位置追跡実験

測データの代表点と地図データの代表平面との距離 dik→m を（3）階段環境における局所的位置追跡実験

求め，正規分布に当てはめて代表点 S̃ik の距離評価値 αik→m

を行った．

とする．

なお，地図データは一辺 800 [mm] の ND ボクセル 51,395 個

αik→m = √

1
e
2πσd

2
−d2
ik→m /σdからなり，また RGB-D カメラからの 1 回の計測では 307,200 点（3）の距離データが得られ，これを一辺 1,600 [mm] および 800 [mm]

ここで σd は dik→m の分散値を表すパラメータである．また，Nm を地図データ中のボクセル m における代表平面の法線ベ
クトル，µm を同じく地図データ中のボクセル m の点群の平均位置とすると，のオーバーラップ化された ND ボクセル（平均ボクセル数はそれぞれ 121 個，670 個）に変換する．

4. 1 大域的位置同定実験
大域的位置同定実験では，RGB-D カメラから得られた距離データ 1 枚を与え，事前知識を用いずに Fig. 4 に表す屋内廊下

dik→m =| Nmx (Sikx − µmx ) + Nmy (Siky − µmy )+ Nmz (Sikz − µmz ) |で計測位置を推定した．
実験では，パーティクルフィルタの尤度計算には（4）（1）ビームモデルに基づく最尤推定法（付録 B）を用いた場合と求まる．

（2）提案する ND ボクセルの代表点および代表平面を用いた

また地図データのボクセル m と計測データのボクセル i の
代表平面の相対角度の差を評価値 βi→m とする．

βi→m =| Nmx Ñix + Nmy Ñiy + Nmz Ñiz |場合の 2 種類を用い，両者の大域的位置同定性能を比較した．なお，
ロボットは傾斜センサを搭載し，roll，pitch は既知であると仮（5）

定し，各パーティクルは位置（x, y, z ）と方向（yaw）の四つのこれより，代表点 Sik の評価値 γik を，オーバラップ化され情報を有するとした．
まず，ロボットを Fig. 12 の点線で示す経路に従い移動させ，たボクセルにおける距離評価値と相対角度評価値の積の最大値

経路上の 80 点からロボットが静止した状態で RGB-D カメラ

として求める．

γik = max αik→m βi→m1≤m≤8

JRSJ Vol. 31 No. 9（6）

により距離データを取得した．Fig. 12 にロボットに搭載した

RGB-D カメラで計測したカラー画像と距離画像，およびそれ—72—

Nov., 2013

大規模な三次元環境地図と RGB-D カメラを用いた移動ロボットの広域位置同定

901

Fig. 12 Measuring positions for global positioning
Table 1 Correctness of global positioning
σ, σd [mm]
100
300
500
800
1000
Proposed [%]
23.8
27.5
28.8
18.8
23.8
Beam model [%]
2.5
1.3
1.3
2.5
1.3
Average error [mm] 267.6 295.3 280.2 271.1 303.4
Average error [deg.]
1.7
2.0
2.2
2.5
2.3

ぞれの撮影位置を示す．このように，廊下は多くの地点で非常に類似したカラー画像，距離画像が得られ，知覚的見せかけ（Per-

ceptual aliasing）† の生じやすい，大域的位置同定が難しい環境である．
次に，得られた距離データを一辺 1,600 [mm] のオーバーラッ
プ ND ボクセルに変換し，パーティクルフィルタによりそれぞれの計測位置を独立に推定した．初期パーティクル数は 72,000であり，その後は 1,000〜5,000 までの範囲で適応的にパーティ
クル数を変更しながらパーティクルを 4 回更新し，最終的に最も尤度の高いパーティクルの位置情報を推定値とした．

σ, σd をそれぞれ 500 [mm] としたときの，経路上の 80 箇所における xyz 方向の位置誤差，および方位誤差の頻度分布をFig. 13 に示す．これより，誤差が小さい地点では，位置誤差500 [mm]，方位誤差 10◦ 以下に誤差が集中しており，以降の実験では推定位置が実際の計測位置から xyz 方向に 500 [mm]，かつ方向が 10◦ 以内のときに正解とすることとした．
次に様々な σ ，σd に対して，提案手法により推定された場合の正解率と，付録 B で示したビームモデルに基づく最尤推定法の正解率および正解した場合の平均誤差を Table 1 に示す．

Fig. 13 Error histograms for global positioning

定法ではボクセル間の距離のみを評価しているのに対し，提案手法ではボクセル内の点の分布（法線方向や分散値）を用いている

このように，提案手法により大局的位置同定の正答率が大幅ためであると考えられる．ただし，正解率は最高でも 28.8%にに向上することが分かる．これは，ビームモデルに基づく最尤推とどまっているが，ビームモデルに基づく最尤推定法との比較や知覚的見せかけの置きやすい廊下環境であること，次節で示†類似したセンサ入力のために環境認識が困難になること．

日本ロボット学会誌 31 巻 9 号

すように走行などにより時系列計測データが得られれば，正答—73—

2013 年 11 月

902

鄭

龍

振

倉

爪

亮

岩

下

友

美

長谷川

勉

率は向上することから，筆者らは妥当な結果と考える．
なお，ビームモデルに基づく最尤推定法に対しては，ボクセルの大きさを例えば 1,600 [mm] から 400 [mm] および 100 [mm]に変更しても，計算時間がそれぞれ 20 分超および 8 時間超となる一方で，推定精度には大きな変化はなかった．また比較のために，得られた距離データを一辺 100 [mm] のオーバーラッ
プ ND ボクセルに変換し，地図データ（一辺 800 [mm] のボクセル）中に計測データのボクセルが存在するかを調べ，存在したボクセル数に比例して尤度を与えた場合，正解率は 8.8%であった．
また計算時間（Intel(R) Xeon(R) CPU 2.67 GHz Quad core

4 GB メモリ）は，提案手法ではパーティクル 4 回の更新で 75.4
秒であり，1 パーティクル当たりの 1 回の尤度計算の処理時間
はビームモデルに基づく最尤推定法で平均 0.39 [ms]，提案手法
で平均 1.15 [ms] であった，提案手法の計算時間が大きいのは，提案手法ではオーバーラップ化された NDT を用いており，一つのパーティクルの尤度計算に 8 個のボクセルを評価するためと考えられる．なお，ND ボクセルのオーバーラップ化をしない場合，正解率は σd =500 [mm] の場合で 28.8%から 17.5%に低下した．

4. 2 局所的位置追跡実験
次に，前節の実験により得られた大局的位置を初期位置とし，その後，オドメトリの情報と計測された距離データを用いて，移動しながら位置を連続的に同定する実験を行った．
実験では，初期位置は Fig. 12 の 1 番の位置とし，前節の実験と同様に一辺 1,600 [mm] のオーバーラップ ND ボクセルにより大域的位置同定を行った後，一辺 800 [mm] の ND ボクセルを用いて移動しながらパーティクルフィルタを更新した．パー
ティクルの収束様子を Fig. 14 に表す．
また，提案手法とビームモデルに基づく最尤推定法法，およびオドメトリにより推定されたロボットの移動軌跡を Fig. 15
および Fig. 16 に示す．このように，特に移動開始直後では，提案手法はビームモデルに基づく最尤推定法に比べて安定に位置が推定できている．これは単純なボクセルの有無では局所特徴が表現できないため，異なる位置でも尤度に差が表れにくいが，提案手法は ND ボクセルを利用していることから，局所特徴に差が生じるためと考えられる．また z 方向（高さ）の推定値を比較すると，今回の実験では床面は平らであることから，提案
手法がより正確な推定を行っていることが分かる．
最後に，パーティクルフィルタの更新に必要な処理時間（In-tel(R) Xeon(R) CPU 2.67 GHz Quad core 4 GB メモリ）を示す．初期位置に対する大域的位置同定では，初回のみ 72,000
個のパーティクルを用い，4 回のパーティクルフィルタの更新を行った場合で 77.2 [s] が必要であった．一方，移動中の位置推定
では，ND ボクセルサイズが 800 [mm] の場合で平均 7.2 [s]（平均パーティクル数 1,379.3 個），1,600 [mm] の場合で平均 2.5 [s]（平均パーティクル数 1,385.2 個）であった．
なお一例として，RGB-D カメラ（Kinect, Microsoft）から得られる距離データ（307,200 点）2 枚に対して，Point Cloud
Library [31] で提供されている ICP 法を適用してレジストレー
ションを行った場合，最近傍点探索等の一連の更新手順 1 回にJRSJ Vol. 31 No. 9—74—

Fig. 14 Convergence of particles and estimated trajectory

Nov., 2013

大規模な三次元環境地図と RGB-D カメラを用いた移動ロボットの広域位置同定

903

Fig. 15 Estimated trajectory by the proposed method

Fig. 17 Point-cloud data of stairs measured by a RGB-D camera

つき平均で 118.5 [s] 必要であった．すなわち，上記の実験のよ
うに 72,000 個のパーティクルを利用した場合には，全パーティ
クルの更新 1 回につき単純計算で 2370 時間と，非現実的な計算時間が必要となる．

4. 3 階段環境における局所的位置追跡実験
最後に，Fig. 2 (d) に示す階段環境（図中の丸印）において
RGB-D カメラによる三次元位置追跡実験を行った．実験では，
RGB-D カメラ（Kinect, Microsoft）を光軸が地面と平行になるように手に持ち，下階から上階へ移動しながら，距離画像を
連続的に 35 枚撮影した．その後，得られた距離画像と，あらかじめ計測ロボット [13] により構築された三次元環境地図を元に，提案手法によりカメラの位置を追跡した．実験環境と三次
元環境地図を Fig. 17 に示す．三次元環境地図は 994 万点の点群からなり，これをあらかじめ一辺 800 [mm] の ND ボクセル

10,292 個に変換した．また前節と同様に，RGB-D カメラから得られる計測データは，一辺 1,600 [mm] および 800 [mm] のオーバーラップ化された ND ボクセルであり，初期位置の推定
時のみ一辺 1,600 [mm] の ND ボクセルを用い，その後の追跡では一辺 800 [mm] の ND ボクセルを用いた．
なお，初期位置は前節と同様に 72,000 個のパーティクルからなるパーティクルフィルタにより推定し，また RGB-D カメラを手に持って移動したことから，その後の追跡ではパーティク
ルの更新にオドメトリ情報は用いていない．

Fig. 18 に提案手法により推定された経路をビームモデルに基づく手法の結果とともに示す．これらより，二次元地図では追跡不可能な階段環境においても，提案手法により RGB-D カ
メラの三次元位置が推定できていることが確認できる．また，ビームモデルに基づく手法では，本実験では位置追跡に失敗しているが，これはオドメトリ情報が利用できず，三次元モデル
Fig. 16 Comparison of estimated trajectoriesとの照合による位置同定の性能の差が 4. 2 項の廊下環境よりも強く表れたためと考えられる．

日本ロボット学会誌 31 巻 9 号—75—

2013 年 11 月

鄭

904

龍

振

倉

爪

亮

岩

下

友

美

長谷川

勉

するとともに，ND ボクセルと代表点情報を用いた，より高精度な位置同定手法を実装，開発する予定である．
謝

辞本研究の一部は文部科学省科学研究費補助金基盤研

究（B）（課題番号 23360115）の支援を受けた．

参Fig. 18 Estimated trajectory by the proposed method andbeam model method

5. 結

論

本論文では，事前にレーザ計測により得られた数千万点もの大規模な三次元環境地図と，一度に 30 万点の距離データが得られる RGB-D カメラを用い，実用的な処理時間で実行可能な，
移動ロボットのための新たな位置同定手法を提案した．
本手法は，ND ボクセルの性質を巧みに利用し，点群で表された環境地図や計測データの ND ボクセル化，代表平面，代表点の抽出，およびパーティクルフィルタにより，大規模データに対しても高速な大域的位置同定および局所的位置追跡を実現するものである．手法の説明，および実験では RGB-D カメラを用いたが，提案手法の適用範囲は，特に RGB-D カメラに限定されるものではなく，レーザレンジファインダなど距離セン
サ全般に適用可能である．
また，レーザ計測された点群地図と RGB-D カメラを用いた屋内環境における位置同定実験の結果，壁，天井，柱など，多くが平面で構成される大規模環境において，従来手法に対する提案手法の同定精度，処理時間の優位性を確認した．しかし，例
えば乱雑な室内環境など，細かな形状変化が多くみられる環境
では，ボクセル表現を基本とした提案手法は位置同定性能が劣化することが予想される．
今後は，提案手法の適用可能な環境についてより詳細に検討

JRSJ Vol. 31 No. 9

—76—

考

文

献

[ 1 ] 羽田芳朗，高瀬國克：“屋内大域位置同定システム（igps）を用いた
複数台移動ロボットのナビゲーションの研究”，第 6 回ロボティクス
シンポジア予稿集，pp.354–359, 2001.
[ 2 ] 倉爪亮，村上剛司，木室義彦，家永貴史，馬場伸一，殷中翔：“ロボットタウンの共通プラットフォーム技術のメカニズム”，日本ロボット
学会誌，vol.26, no.5, pp.415–419, 2008.
[ 3 ] G.N. DeSouza and A.C. Kak: “Vision for mobile robot navigation: A survey,” IEEE Trans. on Pattern analysis and machine
intelligence, vol.24, no.2, pp.237–267, 2002.
[ 4 ] S. Thrun: Robotic mapping: A survey, CMU-CS-02-111, 2002.
[ 5 ] D. Fox: “Adapting the sample size in particle ﬁlters throughkld-sampling,” International Journal of Robotics Research,
vol.22, no.12, pp.985–1004, 2003.
[ 6 ] J.-A. Meyer and D. Filliat: “Map-based navigation in mobile
robots:: Ii. a review of map-learning and path-planning strategies,” Cognitive Systems Research, vol.4, no.4, pp.283–317,
2003.
[ 7 ] A. Nüchter, H. Surmann, K. Lingemann, J. Hertzberg and S.
Thrun: “6D slam with an application in autonomous mine
mapping,” Proc. of the 2004 IEEE International Conferenceon Robotics and Automation, pp.1998–2003, 2004.
[ 8 ] A. Nüchter, K. Lingemann, J. Hertzberg and H. Surmann: “6Dslam—3D mapping outdoor environments,” J. Field Robot.,vol.24, no.8–9, pp.699–722, 2007.
[ 9 ] 倉爪亮，戸畑享大，村上剛司，長谷川勉：“CPS SLAM の研究-大規模建造物の高精度三次元幾何形状レーザ計測システム”，日本ロボット学会誌，vol.25, no.8, pp.1234–1242, 2007.
[10] Y. Tobata, R. Kurazume, Y. Noda, K. Lingemann, Y. Iwashita
and T. Hasegawa: “Laser-based geometrical modeling oflarge-scale architectural structures using co-operative multiple
robots,” Autonomous Robot, vol.32, no.1, pp.46–62, 2011.
[11] S. Kagami, R. Hanai, N. Hatao and M. Inaba: “Outdoor 3D
map generation based on planar feature for autonomous vehicle
navigation in urban environment,” Proc. of the 2010 IEEE/RSJ
International Conference on Intelligent Robots and Systems,
pp.1526–1531, 2010.
[12] T. Suzuki, M. Kitamura, Y. Amano and T. Hashizume: “6DOF localization for a mobile robot using outdoor 3d voxel
maps,” Proc. of the IEEE/RSJ International conference on Intelligent Robots and Systems, pp.5737–5743, 2010.
[13] 鄭龍振，岩下友美，倉爪亮：“CPS-SLAM の研究—計測精度の向上とトンネル出来形計測システムの構築—”，日本ロボット学会誌，vol.30,
no.2, pp.180–187, 2012.
[14] C.F. Olson and L.H. Matthies: “Maximum likelihood rover localization by matching range maps,” Proc. of the IEEE International Conference on Robotics and Automation, pp.272–277,
1998.
[15] D. Hähnel and W. Burgard: “Probabilistic matching for 3D
scan registration,” Proc. of the VDI-Conference Robotik 2002
(Robotik), 2002.
[16] P. Biber and W. Straber: “The normal distributions transform:a new approach to laser scanmatching,” Proc. of the 2003
IEEE/RSJ International Conference on Intelligent Robots andSystems, pp.2743–2748, 2003.
[17] T. Kaminade, T. Takubo, Y. Mae and T. Arai: “The generation of environmental map based on a ndt grid mapping—
proposal of convergence calculation corresponding to high resolution grid—,” IEEE International Conference on Robotics
and Automation 2008, pp.1874–1879, 2008.

Nov., 2013

大規模な三次元環境地図と RGB-D カメラを用いた移動ロボットの広域位置同定[18] F. Dellaert, D. Fox, W. Burgard and S. Thrun: “Monte carlo
localization for mobile robots,” Proc. of the IEEE International
Conference on Robotics and Automation, vol.2, pp.1322 –1328,
1999.
[19] S. Thrun, W. Burgard and D. Fox: Probabilistic Robotics. MIT
Press, 2005.
[20] K. Konolige and K. Chou: “Markov localization using correlation,” Proc. of the International Joint Conference on AI (IJCAI), 1999.
[21] J. Wülﬁng, J. Hertzberg, K. Lingemann, A. Nü chter, T. Wiemann and S. Stiene: “Towards real time robot 6D localization
in a polygonal indoor map based on 3D tof camera data,” Proc.
of the 5th IFAC Symposium on Intelligent Autonomous Vehicles (IAV ’10), 2010.
[22] J. Ryde and H. Hu: “3D mapping with multi-resolution occupied voxel lists,” Autonomous Robots, vol.28, no.2, pp.169–
185, 2010.
[23] M. Magnusson, A. Nüchter, C. Lörken, A.J. Lilienthal and
J. Hertzberg: “Evaluation of 3D registration reliability andspeed—a comparison of icp and ndt,” Proc. of the IEEE International Conference on Robotics and Automation, pp.3907–
3912, 2009.
[24] E. Takeuchi and T. Tsubouchi: “A 3-D scan matching using
improved 3-D normal distributions transform for mobile robotic
mapping,” Proc. of the IEEE/RSJ International conference onIntelligent Robots and Systems, pp.3068–3073, 2006.
[25] 竹内栄二朗，坪内孝司：“移動ロボットによる環境地図生成のための3 次元化 ndt を用いた高速 3 次元スキャンマッチング”，第 11 回 ロ
ボティクスシンポジア予稿集，pp.252–257, 2006.
[26] M. Magnusson, H. Andreasson, A. Nuchter and A.J. Lilienthal: “Automatic appearance-based loop detection from 3D
laser data using the normal distributions transform,” Journalof Field Robotics, vol.26, no.11–12, pp.892–914, 2009.
[27] W. Burgard, D. Fox, D. Hennig and T. Schmidt: “Estimating
the absolute position of a mobile robot using position probability grids,” Proc. of the Thirteenth National Conference on
Artiﬁcial Intelligence, pp.896–901, 1996.
[28] R. Kümmerle, R. Triebel, P. Pfaﬀ and W. Burgard: “Monte
carlo localization in outdoor terrains using multilevel surface
maps,” Journal of Field Robotics, vol.25, no.6–7, pp.364–359,
2008.
[29] 鄭龍振，石橋正教，倉爪亮，岩下友美，長谷川勉：“4 台の kinect を搭載した全方向計測ロボットによる環境計測”，第 29 回日本ロボット学会学術講演会講演予稿集 DVD-ROM，1O3–4, 2011.
[30] 一般社団法人日本ロボット学会編：ロボットテクノロジー. オーム社，
2011.
[31] Point cloud library, http://pointclouds.org/.

付録 A.

一方，半径

√

905

−2loge r の球の表面上の 1 点を p とすると，上式は以下のように表せる．

(x − µ)T Σ−1 (x − µ) = pT p

（A.4）

ここで共分散行列 Σ が正則行列であれば，固有値分解により固有ベクトル行列 V と対角行列である固有値行列 D に分解できる．

(x − µ)T (V DV T )−1 (x − µ) = pT p（A.5）(x−µ)T (V D−1/2 D−1/2 V T )(x−µ) = pT p （A.6）
よって V が正規直交行列であることに注意して，

D−1/2 V T (x − µ) = p（A.7）

x = V D1/2 p + µ

（A.8）

となる．

√
−2loge r の球の表面上の一つの点 p を
共分散行列 Σ で表されるだ円表面上の一つの点 x に変換す√√るもので，これを用いて ( −2loge r, 0, 0), (− −2loge r, 0, 0),√√√(0, −2loge r, 0), (0, − −2loge r, 0), (0, 0, −2loge r),√(0, 0, − −2loge r) の六つの点 p をだ円上に変換した点，およ
この変換は，半径びだ円中心点を代表点とする．

付録 B. ビームモデルに基づく最尤推定法
ボクセル化された地図データと距離センサからの計測データを比較し，ロボットの位置姿勢を同定する方法として，ビームモデルに基づく最尤推定法が広く用いられる．
まず，ロボットの位置 s からある方向の計測点までの距離をr，その計測方向で地図上の最も近い物体までの距離を r̄ としたとき，センサの観測モデルを確率 ps (r|r̄) で与える．ただし
通常，この ps (r|r̄) として，Fig. 19 のように r̄ より近い部分は偽陽性として小さな確率，遠い部分は誤検出として 0 に近い
確率，最大計測値を計測範囲外として一様分布，また r̄ の近傍
ではガウス状の確率分布を与える．r̄ は通常，光線追跡法により推定される．また，観測の独立性を仮定すると，ロボットの位置はベイズ定理より最尤推定法により求まる．

代表点についてNT ボクセル内の代表点の求め方を以下に示す．点群が，以下の 3 変数正規分布に従う確率密度で空間に存在しているとする．
«
„
1
T −1
−1/2
P (xs ) = (2π det(Σ))
exp − (xs − µ) Σ (xs − µ)2（A.1）

arg max p(s|r) = arg maxssYps (ri |r̄i )

（B.9）

i

ただし ri 等は i 番めの観測された距離値である．
本手法は，地図データのみならず，計測データをボクセル形

ただし，µ は点群の平均位置，Σ は分布の共分散行列である．
このとき，P (xs ) が平均位置 µ での r 倍になる (0 < r ≤ 1)
位置 x は次で与えられる．

„«
1exp − (x − µ)T Σ−1 (x − µ) = r
2

（A.2）

(x − µ)T Σ−1 (x − µ) = −2loge r（A.3）

日本ロボット学会誌 31 巻 9 号

Fig. 19 Probalility of sensor reading—77—

2013 年 11 月

鄭

906

龍

振

倉

爪

亮

岩

下

友

美

長谷川

勉

計測位置に近い地図データのボクセル中心を r̄ とすることで，式（B.9）により計測位置 s が求められる．
また，パーティクルフィルタで最尤推定値を求める場合には，それぞれのパーティクルの尤度 λ として

λ=Y

ps (ri |r̄i )（B.10）

iFig. 20 Beam model for voxel representation

を用いればよい．なお第 4 章で示した実験では，Fig. 19 を簡式で表した場合にも適用できる．すなわち，Fig. 20 に示すよ

略化した次式を用いた．

うに，計測位置を s，計測データをボクセル形式に変換し，得ps (ri |r̄i ) = √られるボクセル中心を ri とする．また地図データ中で s と riを結ぶ直線と交差するボクセルが存在する場合，そのうち最も
鄭龍振（Jeong Yongjin）（B.11）倉爪 亮（Ryo Kurazume）

2010 年韓国釜山大学工学部情報コンピュータ工学科卒業．2012 年九州大学大学院システム情報科学
府修士課程修了．在学中は群ロボット，レーザ計測の研究に従事．

岩下友美（Yumi Iwashita）
2007 年九州大学大学院システム情報科学府知能シ
ステム学専攻博士課程修了．日本学術振興会特別研究員，Imperial College London 客員研究員を経て，同年より九州大学大学院システム情報科学研究院助教，現在に至る．2011 年から NASA ジェット推進研究所客員研究員（2015 年まで）．コンピュー
タビジョンの研究に従事．博士（工学）．（日本ロボット学会正会員）

JRSJ Vol. 31 No. 9221e−(ri −r̄i ) /σ
2πσ1991 年東京工業大学機械物理工学専攻修士課程修了．同年（株）富士通研究所入社，1995 年東京工業大学機械宇宙学科助手，2000 年スタンフォード大学客員研究員，同年東京大学生産技術研究所博士
研究員，2002 年九州大学システム情報科学研究院
助教授，2007 年より同教授，現在に至る．日本ロボット学会論文賞，日本ロボット学会功労賞，日本機械学会ロボティ
クス・メカトロニクス部門学術業績賞等受賞．群ロボット，歩行機械，
レーザ計測の研究に従事．博士（工学）．（日本ロボット学会正会員）—78—

長谷川勉（Tsutomu Hasegawa）
1973 年東京工業大学電子物理工学科卒業．同年電子技術総合研究所勤務．1992 年より九州大学工学
部情報工学科教授．現在同大学大学院システム情報科学研究院教授．知能ロボットの研究に従事．計測
自動制御学会，電気学会，日本機械学会などの会員．
博士（工学）．
（日本ロボット学会正会員）

Nov., 2013