特集 第 19 回ロボティクスシンポジア I 計 測 自 動 制 御 学 会 論 文 集Vol.51, No.5, 309/318（2015）
合同変換に不変な特徴量
(CIF) とキーポイント間の幾何学的拘束に基づいた
ロバストなスキャンマッチング法の提案
中
村
恭
之
∗・脇
田
翔
平
∗Robust Global Scan Matching Method Using
Congruence Transformation Invariant Feature Descriptors
and a Geometric Constraint between Keypoints
Takayuki
Nakamura
∗and Shohei
Wakita
∗This paper proposes a new global scan matching algorithm using the CIF descriptors and a geometric con-straint between keypoints. The CIF descriptor was proposed in our previous work. It is a feature decriptor that is invariant against a congruence transformation. In our previous work, our method was able to perform robust local scan matching using CIF decriptors, but was apt to fail global scan mathching where a large map is used as the reference scan. In this paper, in order to resolve this problem, we propose to use a geometric constraint between keypoints in addtion to the CIF decriptors for the global scan mathching task. Our method can perform global scan matching in a cluttered environment without using an initial alignment. Through experiment in real environment, we confirm the validity of our method by comparing the performance of our method and that of our previous method.
Key Words: CIF descriptor, geometric constraint, global scan matching, map updating, mobile robot
1.
は じ め に 自律型移動ロボット普及のためには，自動的な環境地図構 築技術が必要不可欠である．この環境地図構築法には，正確 な地図が高速かつロバストに構築でき，さらに，環境の動的 な変化に対応するために地図を正確に更新できることが望ま れる．地図を更新するためには，大域的に自己位置を推定す る技術が必要となる．大域的自己位置推定では，ロボットの 初期の位置姿勢を与えずに，地図全域でロボットの位置姿勢 を推定しなければならない．大域的自己位置推定に関しては， これまでにも，いくつかの方法が提案されているが，比較的長 い直線で構成される環境にしか適用できないもの1)，環境内 の少数の顕著な特徴点のみを用いるためにノイズやオクルー ジョンなどによる特徴点の欠落に弱いもの2)，処理の頑健性 を向上させるために処理するデータ量が多く，調整しなけれ ばならないパラメータが多いもの3)などが提案されてきた． われわれの研究グループでも，データの欠損や変化があっ ても環境中から不変な個所を探索し安定して地図を更新でき ∗ 和歌山大学システム工学部 和歌山市栄谷 930∗ Faculty of Systems Engineering, Wakayama University, 930 Sakaedani, Wakayama
（Received June 20, 2014） （Revised December 26, 2014）
るように，合同変換に不変なCIF特徴量(Congruence trans-formation Invariant Feature)を用いたスキャンマッチング 法を提案した4), 5)．CIF特徴量とは，2次元のスキャンデー タからキーポイントを抽出し，その周囲の測定値から特徴量を 算出するものである．入力スキャン，参照スキャンから共に キーポイントを抽出し，それらの点における特徴量を計算し て，特徴量を比較することで入力スキャン，参照スキャン内の 対応点を見つけることができる．この手法は，曲線分を多く 含むような一般的な環境で適用可能で，処理するデータ量が 少なく，調整しなければならないパラメータが少ないという 特徴をもつ．しかし，われわれの以前の研究例4), 5)では参照 スキャンとして大きな地図を設定した場合，キーポイントの 差別化ができず，正しい対応点が求められない場合があった． そこで，本論文では，CIF特徴量に加えて，キーポイント の幾何学的拘束を利用し，入力スキャン・参照スキャン間の 対応点を探索し，マッチングを行なう方法を提案する．提案 手法の有効性を証明するために，比較的大規模な地図を参照 スキャンとして与えた場合で実験を行ない，それらの結果に ついて報告する．
2.
関 連 研 究 大域的自己位置推定法に関する従来研究にはさまざまなもの が存在するが1), 2), 6)，ここでは以下の従来研究について7)∼10) TR 0005/15/5105–0309 c 2014 SICEその概要を述べる．
Konolige7)らは，高速な自己位置同定が可能な Correlation-based Markov Localization法と呼ばれる手法を提案した． この手法では，ロボットが移動する範囲のすべての位置姿勢 の状態を一定の間隔で量子化し，各位置姿勢についてスキャ ンデータの相関計算を行ない自己位置同定を行なう．この手 法は，すべての位置姿勢の尤度を計算するため，環境内を網 羅的に探索することが可能であるが，一方で，探索範囲が広 範囲に及ぶ場合には計算量が膨大となる． Olson8)らは，並列計算機を利用したり，位置姿勢の量子 化を階層的に行なうことで，Konoligeらの手法の高速化を行 なった． Dellaert9)らは，大域的スキャンマッチング法として最も 知られているMonte Carlo Localization (MCL)法と呼ばれ る方法を提案した．この手法では，パーティクルフィルタを 用いて多数の位置姿勢の候補を粒子で表現し，ロボットが移 動するにつれて尤度の低い位置姿勢の粒子が淘汰されること で計算量を抑え，高精度に位置姿勢を推定する手法を提案し ている．しかしながら，この手法でも，探索範囲が広範囲に 及ぶ場合には，広大な探索範囲の中で大量の粒子が必要とな るため計算量が膨大となる．また，初期状態で粒子をある程 度限定した範囲で散布しないと，正しい解を得ることが困難 となる． 阪東10)らは，2次元測域センサデータの空間周波数解析に 基づく大域的自己位置推定法を提案している．この手法では， 2次元測域センサから入力されるスキャンデータと，あらか じめ作成されている地図データを，一旦，2次元の画像情報 へ変換して，その変換されたデータに対して2次元FFTに より周波数解析することで，入力スキャンと地図との照合を 行なう．この手法では，その計算過程でデータを量子化する 処理が度々行なわれるため，それらの処理過程における量子 化間隔によって，推定される位置姿勢の精度の良し悪しが左 右する．また，この手法では，スキャンデータ全体を用いて 処理が行なわれるため，基本的に環境は静的であることが前 提である． 従来手法では，大きな地図を扱う場合，地図自体を表現する ためや頑健に求解するために広大な探索空間を用意する必要 があったり，効率的に探索を行なうために，量子化のための さまざまなパラメータをチューニングする必要がある．一般 的に，従来手法では頑健に求解するために冗長なデータ表現 で環境構造を記述し，その結果として探索空間が広大になっ てしまう．そこで，もし，コンパクトなデータ表現で環境構 造を一意的に記述できれば，広大な探索空間を用意しなくて も，効率的で頑健な探索が可能になると考えられる． そこで，本研究では，コンパクトなデータ表現で環境構造を 一意的に記述するために，合同変換に不変なCIF特徴量 (Con-gruence transformation Invariant Feature)とそのCIF特 徴量の組合せによる幾何学的な拘束を用いる．これにより， 提案手法は，処理するデータ量が少なく，調整しなければなら ないパラメータが少ないという特徴をもち，スキャンデータ の欠損や変化があっても環境中から不変な個所を探索し，安 定して大域的スキャンマッチングが可能となっている．
3.
提 案 手 法 Fig. 1に，提案手法の処理手順を示す．提案手法では，時 刻tにおけるスキャンデータScan(t)を入力スキャン，地図 のデータM apを参照スキャンとして入力し，参照スキャンの 座標系における入力スキャンの位置姿勢 Δ˜x, Δ˜y, Δ˜θ を求 める．本手法では，あらかじめ手順(1)∼ (3)によって，地図 データM apにおけるキーポイントを検出し，それらの各キー ポイントにおける特徴量を計算しておくものとする（注1）．Fig. 1 Overview of our global scanmatching method
3. 1 スキャンデータの整形 走査型レーザーレンジファインダで取得されるスキャンデー タには，ある地点を近距離から観測したスキャンデータと遠 距離から観測したスキャンデータとでは，その地点の近傍の スキャンデータの密度は異なるという特徴がある．このよう （注 1） 現在の実装では，以下の手順で地図データを生成している． （ 1 ） ロボットが環境中を走行しながら適当な間隔を開けてス キャンデータを記録 （ 2 ） オフラインで提案手法とは別の手法を使用してスキャン マッチングやループクロージングを行なって整合性のとれた自己 位置推定と地図を生成 （ 3 ） 記録されたスキャンデータすべてに対して，1 つのスキャ ンデータごとにキーポイントにおいて CIF を計算 （ 4 ） (2) で生成した地図の座標系上のおのおののキーポイント の位置と CIF 特徴量とを組合せてリスト状にして保存
な特徴のために，同じ地点を表わすスキャンデータにもかか わらず，観測地点が異なるとスキャンデータの点の間隔も異 なる．これが原因となって，ある地点を表わす何らかの特徴 量をスキャンデータに基づいて計算しても，実際には同一の 地点であるにもかかわらずそれら2つの特徴量は同一の値と ならないことが頻繁に起こると考えられる．後述するわれわ れの提案するCIF特徴量においても，走査型レーザーレンジ ファインダで得られるスキャンデータをそのまま用いると， 同様の問題が生じる． そこで，文献11)で提案されている手法を利用して，スキャ ンデータの密度がスキャンデータ内で一様になるように，ス キャンデータを整形する．Fig. 1内の(1)の処理，スキャン データを整形する処理はつぎのように行なう．まず，1つの スキャンデータを不連続点において分節化し複数の線分に分 割する．スキャンデータ間の間隔がδpmaxより大きなところ を不連続点とする（注2）．そして，各線分内のスキャンデータ 点間の距離が等間隔δp（注3）になるように(Fig. 2参照)，ス キャンデータ点の位置を修正する．整形処理の詳細について は，文献11)を参照していただきたい．
Fig. 2 Rearranging scan data
3. 2 キーポイントの抽出 本研究では，レーザーレンジファインダから得られたスキャ ンの連続する2点を結んだときに得られる折れ線において， 各線分の傾きが急激に変化する点をキーポイントとして検出 する．Fig. 1内の(2)の処理，スキャンデータからキーポイ ントを抽出する処理はつぎのように行なう．Fig. 3(a)よう に，スキャン点pi(i = 1, 2,· · · N)が得られたとき，そのス キャン点間pi, pi+αを結ぶ勾配方向ηiを算出する（注4）すべ てのスキャン点における勾配方向ηiは，レーザーレンジファ （注 2） 実験では δpmax= 0.5 m と設定している． （注 3） 実験では δp = 0.1 m と設定している．このパラメータ の値が，元のスキャンデータ点間の距離よりも小さければ，元の スキャンデータを補間するような点が生成され，逆に大きけれ ば，元のスキャンデータ内のスキャンデータ点が削除されること になる．現在のところ，大よそ 0.1 ∼ 0.2 m の幅の凹凸をスキャ ンデータから検出することを想定して，このパラメータの値を決 めている． （注 4） なお，スキャンを整形するときのパラメータ δp = 0.1 m と設定しているので，実際にキーポイントを検出する際には， α = 1 として線分の勾配を計算している． インダはスキャン点が順番に沿って得られるため，点を並び 変える必要がなく容易に求めることができる． ηi= arctan yi+α− yi xi+α− xi 求まった勾配方向ηiから点間の線分の勾配方向の変化率を求 める． ϑi= ηi+1− ηi 求まった勾配方向の変化率がある閾値threshを超えた点を キーポイントとする．また，キーポイントの抽出は，あくま で，スキャンを構成する点群の中から，つぎに計算するCIF 特徴量を計算する点を選択するために行なっているだけであ るので，抽出の際に設定する閾値threshは，厳密に選定し なくても良い．
Fig. 3 Definition of a keypoint
3. 3 CIF特徴量の算出
本研究では，合同変換に不変な特徴量(Congruence trans-formation Invariant Feature)略して，CIF特徴量を新たに 提案する．3. 2節で説明した手法により，得られたキーポイ ントpi∗を中心に，ある距離内に存在する周囲の点の配置か ら，CIF特徴量を算出する． Fig. 1内の(3)の処理，CIF特徴量の算出はつぎのように 行なう．まず，Fig. 3(b)ように，キーポイントpi∗を中心と した局所的な座標系Σp i∗ を設定する． キーポイントの前後pi−1, pi+1に位置する点とキーポイン トpi∗を結んでできる2つの線分の勾配方向を求める．求め られた2つの勾配方向の平均値ηi∗が，キーポイントpi∗に おける局所座標系の座標軸の勾配方向となる．このようにし て，キーポイントpi∗における局所座標系Σp i∗ に関して特徴 量を記述するために，キーポイントにおいて記述される特徴 量は，入力スキャンと参照スキャンの姿勢にかかわらず，合 同変換（回転・並進変換）に関して不変となる． キーポイントpi∗における特徴量は，キーポイントpi∗と pi∗の近傍に存在するスキャン点同士を結んでできる線分群 の局所座標系Σpi∗ における勾配方向に関するヒストグラム により記述する．このとき，2種類の線分を生成する．1つ の線分SPは，キーポイントpi∗を起点としてpi∗の近傍に
Fig. 4 Definition of segment SP and its histogram
Fig. 5 Definition of segment SD and its histogram 存在するスキャン点群を結んでできる線分であり，このよう にして生成される線分群は，キーポイントpi∗周りのスキャ ン点の分布を表わす．Fig. 4(a)に，線分SP が生成される ようすを示す． もう1つの線分SDは，起点となる点とn個先のスキャン 点を結び，かつ起点となる点を1つずつずらして生成できる 線分であり，このようにして生成される線分群は，キーポイン トpi∗周りに存在する線分の方向の分布を表わす．Fig. 5(a)
に，線分SDが生成されるようすを示す． キーポイントpi∗から距離d以内の範囲にあるスキャン点 のうち，近傍にあるM個の点をリストneighbor(i∗)に登録 する．リストneighbor(i∗)内の点をindex順にソートして おく．リストneighbor(i∗)内のスキャン点に関して線分SP の勾配方向λiSP を算出し，ηi∗との差をとることで，局所座 標系Σpi∗ における線分の勾配方向θiSPを求める．キーポイ ントpi∗の近傍に存在するすべてのθiSP を，−π ∼ πの範 囲の角度を8方向に量子化した各ビンに分類してヒストグラ ムHi∗SP を作成する．Fig. 4(b)に，ヒストグラムHi∗SP が生 成されるようすを示す．このヒストグラムHi∗SP の各ビンの 値はHi∗SP[k], (k = 1, 2· · · , 8)と表わせる． 同様にして，リストneighbor(i∗)内のスキャン点に関し て線分SDの勾配方向λiSDを算出し，ηi∗との差をとるこ とで，局所座標系Σpi∗ における線分の勾配方向θiSD を求 める．キーポイントpi∗の近傍に存在するすべてのθiSDを， −π ∼ πの範囲の角度を8方向に量子化した各ビンに分類し てヒストグラムHi∗SDを作成する．Fig. 5(b)に，ヒストグラ ムHi∗SDが生成されるようすを示す．このヒストグラムHi∗SD の各ビンの値はHi∗SD[k], (k = 1, 2· · · , 8)と表わせる． キーポイントpi∗における特徴量は，ヒストグラムHi∗SP と Hi∗SDの各ビンの頻度を要素とする16次元のベクトル hi∗= (HSP
i∗ [1] . . . Hi∗SP[8]Hi∗SD[1] . . . Hi∗SD[8])
によって特徴量を記述している． さらに，よりロバストに対応点探索するため，キーポイン トpi∗近傍の点の個数を，周囲M 個と，周囲2M個のそれ ぞれの近傍でCIF特徴量を計算して，最終的に，合計32次 元のベクトルをCIF特徴量とする．キーポイントpi∗にお いて，周囲の点M個から算出したCIF特徴量をhMi∗，周囲 の点2M 個から算出したCIF特徴量をh2Mi∗ と表わす．h2Mi∗
では，ヒストグラムのビン内のカウント数の合計が，hMi∗の ものに比べて2倍になっているので，これを正規化するため に，h2Mi∗ の各要素値を1/2にした後に，hMi∗とh2Mi∗ を合わ せて，次式で示すような，32次元のCIF特徴量hi∗を得る． hi∗= hMi∗[1], ..., hMi∗[16],h 2M i∗ [1] 2 , ..., h2Mi∗ [16] 2 この特徴ベクトルの前半16次元はキーポイント近傍の狭い 範囲の詳細な環境構造を表わし，後半16次元はキーポイン ト周辺の広い範囲の粗な環境構造を表わしている． 3. 4 CIF特徴量とキーポイント間の幾何学的拘束を用い た対応点探索 入力スキャン内のキーポイントpi∗, (i∗= 1∼ NKC)と 参照スキャン内のキーポイントqj∗, (j∗= 1∼ NKR)の対 応点を探索する方法(Fig. 1内の(4)の処理)について述べる． まず，入力スキャンのキーポイントから，複数の3点の組 合せを抽出する．抽出する3点のキーポイントの組合せを (ps, pt, pu)と表わす．ロボットから遠い位置に存在するキー ポイント周辺の点群は，もともとスキャン点に不連続点が発 生しやすく，環境の真の形状を正確に表わしていない可能性 がある．つまり，そのような位置に存在するキーポイントに おいて計算されるCIF特徴量も環境の真の形状を正確に表わ していない可能性がある．したがって，3点の組合せを選択 する際には，このようなキーポイントを含まないようにする 必要がある．ロボットから近い位置にあるキーポイントを含 めるためのパラメータをrLと表わすことにする．また，互 いに近接したキーポイント同士を選択して3点の組合せを作 ると，それらの3点におけるCIF特徴量は互いに類似してい るために，3点の組合せによって表わせる一意性が向上しな いことになる．このような小さな三角形を生成するような3 点の組合せを選択しないためのパラメータをdmin，逆に，非 常に大きな三角形を生成するような3点の組合せを選択しな いためのパラメータをdmaxと表わすことにする． これらのパラメータを用いて，ロボットからある長さrLよ り近い位置にあり，点間の距離がdmin以上dmax以下である ような3点を選択する（注5）．この条件を定式化すると以下の ようになる． （注 5） 実験では，rL= 8.0 m, dmin= 3.0 m, dmax= 10.0 m と 設定している．現在の実装では，これらの値は実験的に決めてい る．
d(ps, po) < rL, d(pt, po) < rL, d(pu, po) < rL ⎧ ⎪ ⎪ ⎨ ⎪ ⎪ ⎩ dmin< d(ps, pt) < dmax dmin< d(pt, pu) < dmax dmin< d(pu, ps) < dmax ここで，d(p1, p2)は，2点p1, p2間の距離を表わす関数で あるとする．poは，局所座標系の原点を表わす． この条件を満たす3点の組合せが，入力スキャン内のキー ポイントpi∗, (i∗ = 1∼ NKC)からNα個得られたとし て，それらの組合せを含むリストLCを生成する． LC:{ (ps1, pt1, pu1) , (ps2, pt2, pu2) , · · · (psNα, ptNα, puNα)} つぎに，参照スキャン内のキーポイントからqj∗, (j∗= 1∼ NKR)から，3点の組合せを複数抽出する． 抽出する3点のキーポイントの組合せを(ql, qm, qn)と表 わす．参照スキャン内のキーポイントから3点の組合せを抽 出する際には，入力スキャン内から選択した3点の組合せと ほぼ同じ配置にある3点を見つける（注6）．このように選択す るために，以下の条件を設定する（注7）． d(ps, pt)− δd < d(ql, qm) < d(ps, pt) + δd d(pt, pu)− δd < d(qm, qn) < d(pt, pu) + δd d(pu, ps)− δd < d(qn, ql) < d(pu, ps) + δd この条件を満たす3点の組合せが，参照スキャン内のキーポ イントqj∗, (j∗= 1∼ NKR)からNβ 個得られたとして， それらの組合せを含むリストLRを生成する． LR:{ (ql1, qm1, qn1) , (ql2, qm2, qn2) , · · ·qlNβ, qmNβ, qnNβ } 本研究では，入力スキャンと参照スキャンのキーポイント からそれぞれ3点を抽出し，3点のCIF特徴量を組合せた合 計96次元の特徴量を生成する．そして，この特徴量に基づ いて，最も類似した3点の組合せを求め，それぞれの組合せ に存在する3点同士を対応点とする． リストLC内のα番目の組合せの3点におけるCIF特徴 量を3つ並べて，96次元のベクトルHαを生成する．同様 に，リストLR内のβ番目の組合せの3点におけるCIF特 徴量を3つ並べて，96次元のベクトルHβを生成する．これ らのベクトルは，それぞれ次式のように表わせる． Hα=hs[1], ..., hs[32], ht[1], ..., ht[32], hu[1], ..., hu[32] （注 6） スキャンデータは，赤外線レーザを扇上に走査し，各走 査方向における物体までの距離を連続して出力したものである． これと同様に，各キーポイントにも，元のスキャン点の出力順を 考慮して，出力順序を情報として付加することができる．本研究 では，これを利用することで，3 点のキーポイントとして単なる 組合せではなくキーポイントの出現順も考慮した組合せを選択す ることができるため，鏡像関係にあるキーポイントの組合せ同士 を選択することはない． （注 7） なお，実験では δd = 0.1 m と設定している． Hβ= hl[1], ..., hl[32], hm[1], ..., hm[32], hn[1], ..., hn[32] これら2つのベクトルの類似度を表わすために，バタチャリ ア係数を用いる．バタチャリア係数は，ヒストグラムの一致 度を求める指標で，この値が1に近ければ一致度が高いこと になる．バタチャリア係数を利用して一致度を計算するため の準備として，96次元の特徴量ベクトルを次式に基づいて最 小値0，最大値1に正規化しておく．ここでは，Hαを正規 化して，H˜αを得る． ˜ Hα[k] = 96Hα[k] k=1 Hα[k] このようにして正規化された，H˜α とH˜β について以下 の式に従ってバチャタリア係数を計算して，入力スキャンの キーポイントから抽出された3点の組合せ(psα, ptα, puα) と参照スキャンのキーポイントから抽出された3点の組合せ (qlβ, qmβ, qnβ)間の一致度S (α, β)を求める． S (α, β) = 96 k=1 ˜ Hα[k] ˜Hβ[k] 3点の組合せ(psα, ptα, puα)と(qlβ, qmβ, qnβ)について， α = 1∼ Nα, β = 1∼ Nβ の中から，一致度が最大のものを 以下の式に従って探索することによって求める． (α∗, β∗) = arg max α arg max β S(α, β) このようにして，一致度が最大となる対応点の組， (psα∗, ptα∗, puα∗)と(qlβ∗, qmβ∗, qnβ∗)が得られる． 本手法は，特徴量の一意性を向上させるために，キーポイン ト間の幾何学的拘束を考慮してCIF特徴量の次元を拡張し， CIF特徴量の組合せで環境構造を表現している． 3. 5 キーポイントの対応点組を基にしたスキャンマッチ ングアルゴリズム ここでは，前節で述べた手法によって求められた入力スキャ ン内のキーポイントと参照スキャン内のキーポイントの対応 点組を基にしたスキャンマッチングアルゴリズムについて述 べる．本研究でのスキャンマッチングアルゴリズムは，以下 のような2段階の処理によって構築している． （1） 粗な張り合わせ処理(Fig. 1内の(5)の処理): 対応するキーポイント数組を用いると，入力スキャンと参照 スキャン間の相対的な位置姿勢の変化量の閉形式解を得る ことができる．この位置姿勢の変化量の推定の詳細につい ては，文献12), 13)を参照願いたい．前節で述べた対応点 探索法によって求めた3組の対応点を用いて，文献12), 13) に基づいて，位置姿勢の変化量を求める．求められた位置 姿勢の変化量を用いることで，入力スキャンと参照スキャ ンの粗な張り合わせが可能になる． （2） 詳細な張り合わせ処理(Fig. 1内の(6)の処理): 先述した粗な張り合わせ処理では，対応するキーポイント のデータだけを用いて位置姿勢の変化量を求めているため，
近似解しか求めることができない．そこで，キーポイント周 辺の複数のスキャン点も用いて，ICPアルゴリズム14), 15) によって，位置姿勢の変化量の詳細解を求める．キーポイ ント周辺のスキャン点の選択方法はつぎのようにする．対 応点の存在するキーポイントi∗のインデックス順に前後 oi∗個をICPアルゴリズムに使用する点の数（以後，有効 点数と呼ぶ）とする．この個数は，粗な張り合わせ処理で 使用した対応点の組[i∗, j∗(i∗)]の重みw˜j∗(i)を用いて次式 のように決定する．ここで，Oは，有効点数の総数である． oi∗= ˜wj∗(i)O なお，対応点の組の重みw˜j∗(i)は，次式で求めている．
wj∗(i∗)= 1Cj∗(i∗), w˜j∗(i)= w
j∗(i) 3 j∗(i)=1 wj∗(i) ここで，Cj∗は，対応点の組の一致度を表わし，次式によっ て計算されるものである． Cj∗= 32 k=1 (hi∗k − hj∗k )2 ICPアルゴリズムの初期値は，先述の粗な張り合わせ処理 で求められた推定量を用いる．これにより，ICPアルゴリ ズムの初期値問題を解決できる．また，対応点の求められ たキーポイント周辺のスキャン点のみを使用することによ り，入力・参照スキャン中で共通して観測されるスキャン 点を用いることができるため，ICPアルゴリズムが安定収 束して，繰り返し計算の回数を減らすことができる．
4.
検 証 実 験 SICK社のレーザーレンジファインダ（LMS100：視野270◦， 角度分解能0.25◦）とMobilerobots社の車輪型移動ロボット (P3-DX)を用いて和歌山大学A棟1階および5階でスキャ ンデータを取得して実験に使用した．実験で使用している計 算機は，標準的なノートPC(2.70 GHz Intel Corei7-2620M， 8 GB of RAM)である．スキャンデータを取得した大学建物 の床面積の外形は，大よそ55 m× 55 mの範囲である．まず 最初に，取得したスキャンデータを使用して，移動ロボット プログラミングツールキット(MRPT)16)を用いて，環境地 図を生成している．Fig. 6は，和歌山大学A棟1階(環境I) において取得したスキャンデータから生成した地図（注8）を示 す．この図内の数字は，対応点探索の実験を行なった地点の ID番号を表わしている．この図に示しているように，この 環境内のランダムに選択した26地点において入力スキャン を取得して，入力スキャン内のキーポイントと参照スキャン である地図内のキーポイントとの対応点組を求める実験を行 なった． （注 8） 地図は，整形されたスキャンデータ 21 個から生成し，地 図を構成しているスキャンデータ点数は 3630 個である．スキャ ンデータを整形するときのパラメータは，δp = 0.1 m としてい る．Fig. 6 Map of environment I (reference scan)
4. 1 CIF特徴量とキーポイントの幾何学的拘束による対 応点探索の検証 CIF特徴量を取得する際に必要なパラメータは，特徴点の 変化率の閾値thresh = 40 deg，特徴量の算出時のスキャン 点の個数M = 20とした． Fig. 7は，CIF特徴量のみを用いて対応点組を求めた結果 を表わしている（注9）．図内の右側にある枠内には，地点番号 4において取得された入力スキャンが示されている．この図 内の淡灰色の点はキーポイントを表わし，この図内では入力 スキャン内のキーポイントと参照スキャン内のキーポイント との対応点は，点線で結ばれている（注10）．この図を見てわ かるように，CIF特徴量のみに基づいて計算される類似度に よって対応点探索を行なうと，誤った対応点組が得られてい る．つまり，参照スキャンが地図データ全体を表わしている ような場合には，1個のキーポイントに関しては，類似した キーポイントが参照スキャン中に多く存在するため，CIF特 徴量のみに基づいて対応点を求めることは困難になる．
Fig. 7 Example result of finding corresponding points based on only CIF descriptors
Fig. 8は，CIF特徴量とキーポイントの幾何学的拘束を用 いて対応点組を求めた結果を表わしている（注11）．この実験 （注 9） 対応点探索の処理時間は 66 ms である． （注 10）なお，入力スキャンを構成しているスキャンデータ点数 は 338 個であり，入力スキャン内の不連続点数は 43 である．ま た，参照スキャン，入力スキャン内のキーポイント数はそれぞれ， 555，56 である．これらのキーポイントを抽出するのにかかった 処理時間はそれぞれ，1.39 ms，1.11 ms である． （注 11）対応点探索の処理時間は 22651 ms である．
Fig. 8 Example result of finding corresponding points by our method 結果においても，この図の右側枠内に示されているように，入 力スキャンデータは地点番号4において取得されたものを用 いている．この図内では，入力スキャン，地図データ内の3 点のキーポイントは，各点同士を一点鎖線の直線で結んだ3 角形として表わしている．また，入力スキャン内のキーポイ ントと参照スキャン内のキーポイントとの対応点は，点線で 結ばれている．この図に示すように，地図データ内に存在す る多数の3点のキーポイントの組合せの中から，入力スキャ ン内の3点のキーポイントの組合せに正しく対応するものを 求めることができている． 紙面の都合上，26地点のほかの地点で取得された入力ス キャンに関する結果を掲載できないので，その結果を要約す るとつぎのようになった．CIF特徴量のみのを用いるわれわ れの従来手法では，26地点中7地点のみで対応点探索が成功 (成功率は約27%)した．一方，CIF特徴量とキーポイント 間の幾何学的な拘束を用いる提案手法では，26地点中19地 点で対応点探索が成功(成功率は約73%)した．これにより， CIF特徴量に加えて，キーポイント間の幾何学的な拘束を利 用することの有効性が示せた． つぎに，CIF特徴量を取得する際に設定したパラメータ， 特徴点の変化率の閾値threshと特徴量の算出時のスキャン 点の個数M を変化させると対応点探索結果がどのように変 わるか調査した．この際，不連続点を多く含む入力スキャン を用いる場合と，少数の不連続点を含む入力スキャンを用い て対応点探索を行なう場合で，対応点探索の成否を比較して みた． Fig. 8では，Fig. 6で示した地図内の地点番号4において取 得された入力スキャンを用いた結果を示していたが，この入 力スキャン内には43個の不連続点が含まれており，この結果 が不連続点を多く含む入力スキャンを用いて対応点探索を行 なった結果の一例であった．Table 1は，パラメータthresh とM を20∼ 40の間で変化させて，不連続点を多く含む入 力スキャンを用いて対応点探索を行なった際の対応点探索の 成否(成功：○，失敗：×)を示している．不連続点を多く含 む入力スキャンを用いる場合，Mを小さく(CIF特徴量を計 算するために用いられるスキャンデータ点数が多く)すれば，
Table 1 Success ○/failure × of finding corresponding points by our method at point [4] in case of changing M and thresh
thresh = 20 thresh = 30 thresh = 40
M = 20 ○ ○ ○ M = 30 × ○ ○ M = 40 × × × threshを変化させても対応点探索に成功している． 少数の不連続点を含む入力スキャンを用いて thresh = 30 deg，M = 20と設定して対応点探索を行なった結果を Fig. 9に示す（注12）．この図における入力スキャンはFig. 6 で示した地図内の地点番号5において取得されたもので，こ の入力スキャン内には，307個スキャンデータ点，43個のキー ポイント，18個の不連続点が含まれている．
Fig. 9 Example result of finding corresponding points by our method
Table 2 Success ○/failure × of finding corresponding points by our method at point [5] in case of changing M and thresh
thresh = 20 thresh = 30 thresh = 40
M = 20 ○ ○ × M = 30 ○ ○ × M = 40 ○ ○ ○ Table 2は，パラメータthreshとM を20 ∼ 40の間 で変化させて，少数の不連続点を含む入力スキャンを用いて 対応点探索を行なった際の対応点探索の成否を示している． 少数の不連続点を含む入力スキャンを用いる場合，M を大 きくすれば，threshを変化させても対応点探索に成功して いる．ただし，この入力スキャンに対して，多くの不連続点 を含む入力スキャンで用いたパラメータthresh = 40 deg， M = 20と設定すると，対応点探索に失敗している．これは， thresh = 40 degと大きな値を設定すると，検出されるキー ポイント数が35個に減少し，探索対象となるキーポイント の候補が少なくなることが原因であると考えられる．このよ うに，入力スキャン内のキーポイント数や不連続点数に応じ （注 12）対応点探索の処理時間は 174643 ms である．
て，パラメータthresh, Mの値を設定することができれば， より対応点探索に成功する確率が高くなるのではないかと考 えられる．この問題については，今後の課題としたい． さらに，提案手法と従来手法のMCL法との性能比較を行 なった．両手法ともあらかじめ用意された地図を基に大域的 スキャンマッチングを行なうという点で同類である．Fig. 6 に示された同じ地図を用いて，この地図内の26地点におい てパーティクルフィルタを用いて大域的スキャンマッチング を行なった．なお，パーティクルフィルタを用いた大域的ス キャンマッチングは，移動ロボットプログラミングツールキッ ト(MRPT)16)で実装されているものを使用した．パーティ クルの総数は5000点，初期状態で地図全体にパーティクルを ばら撒き，提案手法と実験条件を同一にするためにロボット は初期位置から移動しないように設定した．その結果，26地 点中19地点でパーティクルフィルタを用いた大域的スキャン マッチングが成功した．ここでの実験条件下においては，提 案手法と従来手法のMCL法の成功率は同じであり，提案手 法は，MCL法と同程度の性能をもつことが示せた（注13）． 4. 2 詳細な張り合わせ処理の結果 キーポイントの対応点組を求めた後，世界座標系ΣW にお ける入力スキャンの位置姿勢 Δ˜x, Δ˜y, Δ˜θ は，提案手法の 手順(5), (6)によって求める． Fig. 10は，提案手法の手順(6)を実行した後に得られる入 力スキャンの詳細な張り合わせ結果を示している（注14）．提 案手法の詳細な張り合わせ処理では，移動ロボットプログラ ミングツールキット(MRPT)16)で実装されているICPアル ゴリズムを用いている．また，提案手法において詳細な張り 合わせ処理を行なう際に使用する，対応点の存在するキーポ イント周辺のスキャン点の総数は，O = 50として設定して， 実験を行なっている．この図において，淡灰色の点は参照ス キャンの点，濃灰色の点は入力スキャンの点を表わしている．
Fig. 10 Result of precise matching by our method （注 13）ただし，MCL 法は，本来はロボットが移動して新たな スキャンデータを取り込みながら自己位置推定を行なう手法であ るため，そのような条件下では提案手法が MCL 法と性能が同程 度になる保証はない． （注 14）すべての処理 (キーポイント検出，対応点探索，ICP) の 処理時間は 21872 ms である． 入力スキャンは，地図内の地点3において取得されたもので ある．この図に示されているように，提案手法により入力ス キャンが地図に対して正しく張り合わせることができている ことがわかる． Table 3は，Fig. 10に示した提案手法の詳細張り合わせ の精度を表わしている．この表において示されている正解の 位置姿勢の値は，入力スキャンを地図に対して手動で張り合 わせることによって得られたものである．この表で，“error” は真値と提案手法による推定値との差の絶対値を示してい る．この表に示されているように，入力スキャンの位置姿勢 Δ˜x, Δ˜y, Δ˜θ が正しく推定されていることがわかる．
Table 3 Result of global self-localization by our method in Fig. 9 Δx [m] Δy [m] Δθ [deg] ground truth −10.62 2.74 137.5 our method −10.54 2.71 137.4 error 0.08 0.03 0.1 4. 3 提案手法の頑健性の実験的検証結果 Fig. 11は，和歌山大学A棟5階(環境II)において取得し たスキャンデータから生成した地図（注15）を示す．この地図 で示された環境内のある地点（大学建物内のエレベータホー ル）で取得した入力スキャンを，この地図に張り合わせる実 験を行なった．なお，地図生成時にはエレベータの入口ドア が閉じられた状態であったが，入力スキャン取得時には入口 ドアが開けられた状態になった．このような状態が変化した 環境において取得された入力スキャン内のキーポイントに対 応する地図内のキーポイントが，提案手法により発見できる か検証した．その結果を，Fig. 12に示す（注16）．
Fig. 11 Map of environment II (reference scan) （注 15）地図は，整形されたスキャンデータ 33 個から生成し，地
図を構成しているスキャンデータ点数は 2395 個である．スキャ ンデータを整形するときのパラメータは，δp = 0.1 m としてい る．
Fig. 12 Result of finding corresponding points in environ-ment II by our method
この図において，黒の点群(図の左側)は参照スキャン，淡灰 色の点群（図の右側）は入力スキャン，灰色の点群はそれぞれの スキャンデータにおけるキーポイントを表わしている（注17）． 濃灰色の点群は，スキャンデータにおける不連続点を表わし ている． また，図の左・右側において点線の円で囲んだ部分は，そ れぞれ，地図作成時にエレベータの入口ドアが閉じられた状 態のスキャンデータ，入力スキャン取得時に入口ドアが開け られた状態のスキャンデータを表わしている．この図からわ かるように，提案手法は，地図作成時と環境構造が変化して いるような入力スキャンに対しても地図内の対応点を探索す ることができる． 紙面の都合上，ほかのスキャンデータに関する結果を掲載 していないが，スキャンデータの一部に新たなデータが加え られるような状況や，オクルージョンによってスキャンデー タの一部が隠されているような状況においても，提案手法は， 入力・参照スキャン内で環境の変化に不変なキーポイントの 対応点を探索することができる． Fig. 13は，Fig. 12で示した対応点に基づいて，提案手法に より詳細な張り合わせを行なった結果を表わしている（注18）．
Fig. 13 The result of precise alignment of reference (gray dots) and input (dark gray dots) scans based on pairs of the corresponding points in the environment II （注 17）なお，入力スキャンを構成しているスキャンデータ点数 は 361 個である．また，参照スキャン，入力スキャン内のキー ポイント数はそれぞれ，261，57 である．これらのキーポイント を抽出するのにかかった処理時間はそれぞれ，1.09 ms，0.90 ms である． （注 18）すべての処理 (キーポイント検出，対応点探索，ICP) の 処理時間は 1996 ms である． この図において，淡灰色の点群は参照スキャン，濃灰色の点 群は入力スキャンを表わしている． さらに，地図作成時と環境構造が変化しているような入力 スキャンに対して大域的スキャンマッチングが可能か否かに 関して，提案手法と従来手法のMCL法との性能比較を行なっ た．先述したものと同じ入力スキャン(地図生成時にはエレ ベータの入口ドアが閉じられた状態であったが，入力スキャン 取得時には入口ドアが開けられた状態になった．)を，Fig. 11 に示した地図に対して，MCL法を用いて張り合わせる実験 を行なった．なお，先と同様に，MCL法(パーティクルフィ ルタを用いた大域的スキャンマッチング)は，移動ロボット プログラミングツールキット(MRPT)16)で実装されている ものを使用し，パーティクルの総数は5000点，初期状態で 地図全体にパーティクルをばら撒くように設定した．Fig. 14 は，MCL法による結果を表わしている（注19）．この図を見れ ばわかるように，MCL法は，入力スキャンデータの一部に 地図内には保存されていない新たなデータが加えられるよう な状況では，大域的スキャンマッチングに失敗する．
Fig. 14 The result of precise alignment of reference and in-put (dark gray dots) scans using MCL method in the environment II
Table 4 Results of global self-localization by our method in Fig. 12 and MCL method in Fig. 13
Δx [m] Δy [m] Δθ [deg] ground truth −0.46 −0.64 −49.4 our method −0.51 −0.73 −49.8 error 0.05 0.09 0.4 MCL −18.01 −32.11 −215.2 error 17.55 31.47 165.8 Table 4は，環境IIにおいて地図作成時と環境構造が変 化しているような入力スキャンが取得される状況下において， 従来法のMCL法と提案手法による詳細な張り合わせ処理の 精度比較結果を表わしている．この表では，入力スキャンを （注 19）MCL 法の処理時間は 384088 ms である．
手動で回転・並進移動させて参照スキャンと位置合わせを行 ない，その際求められた回転・並進移動量を真値として記載 してある．この表で，“error”は真値と各手法の推定値との 差の絶対値を示している． これらの結果からわかるように，従来法のMCL法では失 敗するような状況において，すなわち，スキャンデータの一 部に新たなデータが加えられるような状況や，オクルージョ ンによってスキャンデータの一部が隠されているような状況 においても，提案手法により，参照スキャン(淡灰色の点群) と入力スキャン(濃灰色の点群)が大きくずれることなく，ま た，真値との差も少なく推定を行なうことが可能である．
5.
まとめと今後の課題 本研究では，入力・参照スキャン間で頑健に対応点を探索 するために，CIF特徴量とキーポイントの幾何学的拘束を用 いたスキャンマッチング法を提案した．CIF特徴量に加え， キーポイントの幾何学的拘束を考慮すると，スキャンデータ の一部に新たなデータが加えられるような状況や，オクルー ジョンによってスキャンデータの一部が隠されているような 状況においても，対応点探索に成功することを確認した．さ らに，ICPアルゴリズムの初期値として3組の対応点から計 算された位置姿勢の変化量を用いても，入力・参照スキャン を正しく張り合わせることが可能であることを確認した．ま た，その初期値から，複数の対応点の組の周辺に存在するス キャン点を用いてICPアルゴリズムを適用することによっ て，より真値に近い張り合わせ結果を得ることができること も確認した． 今後は，対応点探索の成功率をより高める方法，対応点探 索にかかる時間を軽減する方法，地図更新を行なう方法とこ れらを実施するための新たな地図表現方法などについて検討 する予定である． 謝辞 本研究の一部は科学研究費 (基 盤 研 究 (C) No. 26450366)の助成を受けたものである． 参 考 文 献1）J.S. Gutmann, T. Weigel and B. Nebel: Fast, Accurate, and Robust Self-Localization in Polygonal Environments, Proc. IROS’99, 1412/1419 (1999)
2）P. Jensfelt and S. Kristensen: Active Global Localization for a Mobile Robot Using Multiple Hypothesis Tracking, IEEE Trans. Robotics and Automation, 17-5, 748/760 (2001)
3）友納正裕：ユークリッド変換に不変な特徴量を用いた二次元大 域スキャンマッチング方式，日本ロボット学会誌，25-3, 66/77 (2007)
4）T. Nakamura and Y. Tashita: Congruence Transformation Invariant Feature Descriptor for Robust 2D Scan Match-ing, Proc. 2013 IEEE International Conference on Systems, Man, and Cybernetics (SMC), SYS-10 (2013)
5）中村，脇田：2D スキャンデータの合同変換に不変な特徴量 (CIF)を用いたスキャンマッチング，第 19 回ロボティクスシ ンポジア，6C3, 592/598 (2014)
6）M. Tomono: A scan matching method using Euclidean in-variant signature for global localization and map building,
Proc. ICRA’04, 866/871 (2004)
7）K. Konolige and K. Chou: Markov Localization Using Cor-relation, Proc. IJCAI 1999, 1154/1159 (1999)
8）E. Olson: Real-time Correlative Scan Matching, Proc. ICRA 2009, 4387/4393 (2009)
9）F. Dellaert, D. Fox, W. Burgard and S. Thrun: Monte Carlo Localization for Mobile Robots, Proc. ICRA 1999, 1322/1328 (1999)
10）S. Bando, Y. Hara and T. Tsubouchi: Global Localization of a Mobile Robot in Indoor Environment Using Spatial Frequency Analysis of 2D Range Data, Proc. ICMA 2013, 488/493 (2013)
11）G.A. Borges and M.J. Aldon: Line extraction in 2D range images for mobile robotics, Journal of Intelligent and Robotic Systems, 40-3, 267/297 (2004)
12）K. Lingemann, H. Surmann, A. Nuchter and J. Hertzberg: Indoor and outdoor localization for fast mobile robots, Proc. IROS’04, 2185/2190 (2004)
13）中村恭之：制約条件付き最小 2 乗法に基づく高速な局所型ス キャンマッチング法，日本ロボット学会誌，28-5, 648/657 (2010)
14）P.J. Besl and N.D. McKay: A Method for Registration of 3-D Shapes, IEEE Trans. Pattern Analysis and Machine Intelligence, 14-2, 239/256 (1992)
15）F. Lu and E. Milios: Robot Pose Estimation in Unknown Environments by Matching 2D Range Scans, Journal of Intelligent and Robotic Systems, 18-3, 249/275 (1997) 16）The Mobile Robot Programming Toolkit (MRPT):
http://www.mrpt.com ［著 者 紹 介］ 中 村 恭 之（正会員） 1996年大阪大学大学院工学研究科博士後期課程 修了．同年，日本学術振興会特別研究員．97 年奈 良先端科学技術大学院大学情報科学研究科助手， 2002年和歌山大学システム工学部情報通信システ ム学科助教授，2013 年同大学同学部同学科教授と なり現在に至る．この間，96 年から約 1 年間米国 ブラウン大学計算機科学科客員研究員．2007 年か ら約 1 年間米国カーネギーメロン大学ロボティク ス研究所客員研究員．ロボットビジョン，視覚情 報に基づくロボット学習システム，移動ロボット の自己位置同定法の研究に従事．博士（工学）．日 本ロボット学会会員． 脇 田 翔 平 2013年和歌山大学システム工学部情報通信シス テム学科卒業，同年和歌山大学システム工学研究 科博士前期課程入学，現在に至る．この間，移動 ロボットの自己位置同定法の研究に従事．
![Table 1 Success ○/failure × of finding corresponding points by our method at point [4] in case of changing M and thresh](https://thumb-ap.123doks.com/thumbv2/123deta/6770496.719852/7.892.487.783.163.233/table-success-failure-finding-corresponding-points-method-changing.webp)
