―修士学位論文―
屋内用自律移動ロボットのための ライン特徴量の限定・修正を利用した
ＥＫＦ－ＳＬＡＭ
EKF-SLAM using limitation and
correction of line features for autonomous indoor mobile robots
平成 22 年度
三重大学大学院 工学研究科 博士前期課程 電気電子工学専攻
山口 敦由
目 目 目 目 次 次 次 次
第 第
第 第
1111章 章 章 章 緒 緒 緒 緒 言 言 言 言
1111
1.1研究背景...1
1.2研究目的...2
1.3研究の位置づけ...3
第 第 第 第
2222章 章 章 章 問題設定と定義 問題設定と定義 問題設定と定義 問題設定と定義
5555 2.1対象環境設定...52.2移動ロボットと座標系の定義…...6
2.3ＬＲＦと地図の変数...8
第 第 第 第
3333章 章 章 章 ＥＫＦ－ＳＬＡＭ ＥＫＦ－ＳＬＡＭ ＥＫＦ－ＳＬＡＭ ＥＫＦ－ＳＬＡＭ
11111111 3.1ＥＫＦ－ＳＬＡＭの構成...113.2状態遷移関数...12
3.3状態予測と状態更新...15
第 第 第 第
4444章 章 章 章 ライン生成 ライン生成 ライン生成 ライン生成
18181818 4.1ライン生成の構成...18
4.2物体検出...20
4.3ライン抽出...22
4.4限定アルゴリズム...25
4.5修正アルゴリズム...28
4.6ラインデータ結合...31
第 第 第 第
5555章 章 章 章 オフライン実機実験 オフライン実機実験 オフライン実機実験 オフライン実機実験
33333333 5.1実験内容と条件...335.2実験結果...35
第 第 第 第
6666章 章 章 章 結 結 結 結 言 言 言 言
44440000参考文献 参考文献 参考文献 参考文献
4444111 1謝 謝
謝 謝 辞 辞 辞 辞
4442422 2第１章 緒 言
第１章
緒 言
1.1 研究背景
昨今、製造業の発展に多大な貢献をしてきたロボットの活動環境は、より身近な生活の 場へと拡大している。その例としては、自動で部屋のゴミを吸引する掃除ロボットや、主 に企業などで巡回警備の手助けを行う警備ロボットなどが挙げられる。このように屋内環 境で移動しながら働くロボットが実用化されてきている。ここで、これら屋内移動ロボッ トの環境認識手法に着目する。環境認識とはロボット自身が周囲の環境形状、同時に環境 の中での自己位置を把握することを意味する。この環境認識を適切に行わせることは、移 動を前提とした、あらゆるタスクを実現させるための基礎となり非常に重要である。
現在の掃除ロボットや警備ロボットの環境認識の方法を挙げれば、接触センサにより物 体の配置を確認する接触認識や、建造物の設計図を事前知識として与え、その情報との対 応関係から位置を認識する場合が多い。しかし、接触は環境物体の破損の危険があり、厳 密な位置関係の把握は困難である。また事前に環境の地図情報を登録しても実際の環境は 備品などが存在し地図とは異なる可能性が高いため対応関係を必ずしも認識できない。ゆ えにロボット自身が未知環境を移動する場合、非接触で事前情報を何も与えず、センサ情 報から環境認識ができれば最も汎用性が高く、その実現が求められる。
第１章 緒 言
1.2 研究目的
未知環境で移動ロボットが自己位置推定と地図生成を同時に行うことは、矛盾する要求 でもある。なぜなら自己位置を推定するためには周囲環境の把握が必要であり、地図を作 成するためには自己位置を同定していなければならない。この移動ロボットの未知環境に お け る 認 識 問 題 と し て 移 動 ロ ボ ッ ト 工 学 に お い て 頻 繁 に 用 い ら れ る 言 葉 に SLAM
（Simultaneous Localization And Mapping）がある。SLAMとは文字通り“同時の自己
位置推定と地図生成”という前述の問題を表すとともに、問題解決のために各種センサを 用いた手法自体の名称としても用いられる。
そもそもSLAMが、移動ロボットの普遍的な問題となる理由としては、あらゆる“不確 かさ”が挙げられる。ロボットの移動には、車輪移動ロボットであれば床とのすべりやセ ンサによる測定の観測誤差、ロボットのモデル化誤差など、あらゆる不確かさが含まれる。
その不確かさは短距離移動においては問題にならない場合も多いが、長距離移動では移動 に伴い誤差が蓄積し、最終的に地図が作成できず自己位置推定も失敗に終わる。よって SLAMには、この不確かさの影響に対処できるような手法を考える必要がある。
そこで、不確かさの対処としてSLAM手法に適用されているものに状態推定フィルタが ある。状態推定フィルタとは入力されたデータに誤差が存在することを前提として、前後 にわたる誤差量を考え、その中で最適な解を出力するものである。これを移動ロボットに おいて適用することでSLAM問題を解決する。本研究では、この状態推定フィルタを用い たSLAM手法に対し、推定のパフォーマンスを十分に発揮するための付加的な手法を提案 し実験によってその有効性を確認する。
第１章 緒 言
1.3 研究の位置づけ
本研究で用いる SLAM アルゴリズムは、状態推定フィルタに拡張カルマンフィルタ
（EKF: Extended Kalman Filter）を用いて、内界・外界センサには車輪の回転角を検出
するロータリーエンコーダと対象物との距離・角度を計測するLaser Range Finder（以下 LRF）を使用する。
SLAM で用いられる状態推定フィルタは大別すると拡張カルマンフィルタとパーティク ルフィルタの二つがある。パーティクルフィルタが次元の指数オーダで計算量が増加する のに対し、拡張カルマンフィルタは次元の 2 乗オーダで計算ができ、さらに拡張カルマン フィルタの方がSLAMとしての実績を多く残していることが、それを用いる理由である[1]。 拡張カルマンフィルタを用いたSLAM手法を一般的にEKF-SLAMという[2]。また、使用 される外界センサはカメラ・ソナー・LRFの3種類に分けられる。SLAMにおいて最も重 要なことは物体との相対的な距離の把握であると考え、カメラ・ソナーよりも精度が高い LRFを使用する。
また重要な制約としてLRFで取得される計測情報は、距離と角度で指定される数百とい う点情報となりEKFでは何周期にもわたり、その計測点すべてを入力として扱うことは計 算負荷の面から困難である。そのため環境としてランドマークを設置し、環境の次元を任 意に決定する手法がある[3][4]。しかし、ランドマークとして設置することは本当の未知環 境とは言えず、好ましくない。よって、計測点から環境を表現する特徴を抽出する必要が ある。この特徴にはラインを使用する。理由としては、移動ロボットの実行環境としては 屋内環境を想定しているため壁など直線環境が多い。ゆえに環境の表現にはラインが最も 優れていると判断した。ラインを用いたSLAMは、ラインベースSLAMとも呼ばれ、研究 が盛んに行われている[5]。そこで、ラインベースSLAMの実用化に向けて困難となってい る課題を考え、その課題を解決するために提案する手法を以下に示していく。
本研究が、ラインベースSLAMにおいて注目する課題はライン特徴の“信頼性”である。
言いかえれば、抽出したラインが環境をどれだけ忠実に表現できているかを意味する。ラ イン特徴を扱うことは、計測点からラインに直すという誤差量を増加させているとも理解 でき、計測点自体のノイズやライン抽出方法によりラインそれぞれの信頼性には、特にば らつきがでると考えられる。このばらつきに対処するため他研究では、LRF 計測点になる べく沿った形でラインを抽出する方法を模索し研究がされてきた[6]。
しかし、基本的にLRFの計測は近距離であれば概ね高精度であり、遠距離になるにつれ て精度が低くなるという性質がある。そのため、点に沿ったラインが抽出できても計測点 自体のノイズが無視できない場合、そのラインの信頼性は低いことになる。その場合、信 頼性の低いラインを地図として採用することは、EKF における無駄な計算量の増加、精度 の劣化を引き起こすと考えられる。
それでもEKFでは、ある程度の不確かさには対処できるため推定自体は問題なく行える
第１章 緒 言
可能性はある。しかし、SLAMの処理が問題なく行われるために前提としているのは、LRF の精度がある程度保証されていることが必要となる。つまり、真値は計測できなくても測 定としてのノイズは正規分布で従う形で取得されなければならない。前述にセンサとして LRFを用いる理由として、高精度が挙げられた。しかし、実計測のあらゆる場面において、
それは必ずしも当てはまらないことは一般的にも言われている[1]。例えばレーザで計測し ているため、対象物の色による反射率違いや計測角度に影響され、偶発的現象として全く 異なる計測結果を取得することも実計測により確認されている。この場合、環境との誤差 が小さいラインと環境との誤差が大きいラインを全てSLAMシステムに入力してしまうこ とは、最終的に推定自体が失敗する危険がある。
そこで、本研究では主目的として、抽出したラインの“信頼性”を定量的に表現するた めに“品質”を式として定義し、状態推定フィルタに適用するラインを限定し、さらにそ のラインを用いて品質を基にラインを修正することを提案する。これにより、推定失敗の リスクを避け安定性の向上、修正による推定精度の向上を達成することが目的となる。
第2章 問題設定と定義
第２章
問題設定と定義
本章ではSLAMが扱う問題の設定や、使用するロボットの機構、座標系や変数などSLAM システムの説明に必要な前提条件となる事柄について説明する。
2.1 対象環境設定
単独／複数ロボット
ロボットの位置推定問題には、ロボットの数が大きく関係する。本研究では、最も研究 されている問題である単独ロボットの位置推定を扱う。複数ロボットの位置推定に関す る研究はロボット同士の通信による知識の共有など、研究としての興味深い主題を提供 するが、本研究では問題を簡単にするため扱わない。
静的／動的環境
位置推定の難易度に影響を与える要因は、環境が静的か動的かによって分けられる。本 研究では、静的環境を扱う。静的環境とは、変化するものがロボットの位置姿勢のみの 環境を指す。言いかえれば、ロボット以外の物体は永遠に動かないため推定問題も動的 に比べ簡単となる。しかし、静的にしか用いることできないSLAMシステムを目指して いるわけではなく、将来は動的環境への適用を考える。
第2章 問題設定と定義
2.2 移動ロボットと座標系の定義
移動ロボットの機構
移動ロボットは図2.1に示す独立操舵二輪ロボットMIUBOTを用いる。移動機構として 車輪を用いる理由としては、制御が比較的容易であることが挙げられる。MIUBOTの仕様 を表2.1に示す。
図2.1 MIUBOT
表2.1 MIUBOT仕様
走行方式 独立二輪操舵方式
CPUボード LX-2020(イノテック株式会社)
LRF UHG-08LX(北陽電機株式会社)
駆動装置 FHA-8C-50(ハーモニックドライブシステムズ)
減速比 50
エンコーダ分解能 2000[p/r]
動作速度 1[m/s]程度
車輪間距離 0.34[m]
車輪半径 0.07[m]
重量 17[kg]
第2章 問題設定と定義
ロボット座標系と変数
ロボットの座標系と変数を図2.2に示す。移動ロボットの位置姿勢はグローバル座標系の 原点OGから車軸中心であるローカル座標系の原点OLまでの相対的な位置関係である。
EKF-SLAM ではロボット変数行列Rを(2.1)式と定義する。x,y,θはグローバル座標系におけ
るロボットの座標位置と角度である。またv,ωは(2.2)(2.3)式で表現されるロボットの移動方 向における直進速度・角速度であり、vl,vrは左右車輪の直進速度、bは車軸半径である。
図2.2 ロボット座標系
（2.1）
（2.2）
（2.3）
[ x y v ]
TR = θ ω
2
l
r
v
v v +
=
b v v
r l2
= − ω
O
LO
Gx
y
θ ω ,
v
b
v
rv
l第2章 問題設定と定義
2.3 LRF と地図の変数
Laser Range Finder
の仕様
LRFは北陽電機株式会社の測域センサUHG-08LXを用いる。測定領域は2次元および、
図 2.3 のように右側から角度分解能ごとに測定可能領域内の距離を走査していき(2.4)式の ようにi番目の点piに関する距離diと角度αiを返し、処理周期ごとにこれを繰り返す。LRFの 仕様を表2.2に示す。なお、LRFは図2.2から確認できるようにMIUBOTの先頭部分に設 置している。単周期におけるLRFの計測環境と結果例を図2.4と図2.5に示す。なお、SLAM に使用する地図情報を生成する際には、LRF 情報の原点はローカル座標系の原点OL (車軸 中心)に変換して用いる。
（2.4）
図2.3 LRFと測定領域イメージ
表2.2 LRF(UHG-08LX)仕様
+135° -135°
測定可能領域 角度分解能
測定可能距離 30～11000 [mm] ※精度保障は8[m]まで
測定精度 100～1000 [mm] ：±30[mm]
1000～8000 [mm] ：距離の±3％
測定角度 270[deg]
角度分解能 0.36[deg]
処理周期 67[ms]
[
i i]
i
d
p = α
第2章 問題設定と定義
図2.4 LRF計測環境
図2.5 LRF計測点 1000 2000 3000 4000[mm]
第2章 問題設定と定義
地図表現
地図の表現は、LRF 計測点から抽出されるラインを用いる。グローバル座標系のライ ン地図パラメータを図2.6に示す。ラインパラメータは、各座標系の原点からラインまでの 垂直距離ρとX軸から垂直線までの角度φである。(2.5)式のようにラインを取得し、後に(2.6) 式のように各ラインについて地図Mのように保存される。mはラインの数を表す。
図2.6 ライン地図パラメータ
（2.5）
（2.6）
以降、ロボット変数Rと地図Mの両方を含んだ行列を(2.7)式のように状態xとする。SLAM では、この状態を推定することが目的となる。
（2.7）
Y
ρ ϕ
X [ ]
TL = ρ ϕ
[ L L L ]
TM =
1 2L
mM
TR
x = [ ]
第３章 ＥＫＦ－ＳＬＡＭ
第３章
ＥＫＦ－ＳＬＡＭ
本章では前章で定義した事柄を用いてEKF-SLAMの状態推定の遷移式を説明する。3.1
節でEKF-SLAMの構造を簡単に示し、3.2節で推定に必要な状態遷移関数の定義、3.3節
でEKFの推定の式を具体的に示す。
3.1 EKF-SLAM の構成
EKF-SLAM の構成を図 3.1 に示す。状態xොは状態xの予測、tは周期時間を表す。Line
generation（ライン生成）は計測点から地図の特徴であるラインを生成する処理であり、生
成方法など詳細は次章で説明する。ゆえに、以下では状態予測・推定について解説する。
図3.1 EKF-SLAMの構成
State prediction（状態予測）
内界センサであるエンコーダ情報から、前周期の状態xt-1を用いて位置姿勢を計算する ことで周期時間tの状態xොtを予測
State update（状態更新）
予測した状態xොtと外界センサLRFの観測情報であるライン地図を用いて状態xtを更新
Encoder
State update x ˆ
t−1
x
tLaser Range Finder
State prediction
EKF Line generation
x
t第３章 ＥＫＦ－ＳＬＡＭ
3.2 状態遷移関数
状態予測・更新の計算に必要な状態遷移モデルを定義する。
ロボット動作モデル
ロボット変数行列Rの動作遷移関数fRを(3.1)式に示す。動作ノイズwは(3.2)式のように直 線加速度ノイズαvと角加速度ノイズαωと定義し、動作ノイズがロボットの各変数に与える誤 差を表す動作ノイズ関数gRを(3.3)式に示す。
（3.1）
（3.2）
（3.3）
EKF の推定式に必要な(3.1)式の動作遷移関数fRをロボット変数Rで偏微分したヤコビ行 列FRを(3.4)式に、(3.3)式の動作ノイズ関数gRを(3.2)式の動作ノイズwで偏微分したヤコビ行 列GRを(3.5)式に示す。
（3.4）
∆ +
∆ +
∆ +
=
=
−−
−
−
−
t t
t t
t t t
t t t
t t t t t
t R
v T Tv y
Tv x
v y x x f
ω ω θ
θ θ
ω
θ
11 1
1 1
) sin(
) cos(
) (
[
v]
Tw = α α
ω
∆
∆
∆
∆
∆
= −
−
ω ω
α α α
θ α
θ α
T T T T T
g
v t v
t v
R
2
) 2 sin(
) 2 cos(
2 1 2
1 2
∆
∆
∆
∆
∆
−
∂ =
= ∂
−
−
−
−
1 0
0 0
0
0 1
0 0
0
0 1
0 0
0 ) sin(
) cos(
1 0
0 ) cos(
) sin(
0 1
1 1
1 1
T T
Tv
T Tv
R F f
t t
t
t t
k
t R R
θ θ
θ
θ
第３章 ＥＫＦ－ＳＬＡＭ
（3.5）
地図遷移モデル
地図遷移モデルに関して、実行環境は静的環境を想定しているため(3.6)式となる。ゆえ に、(3.1)式のロボットの動作遷移関数fRと(3.6)式より状態遷移関数f(xt)は(3.7)式となる。ま た動作ヤコビ行列Ftと動作ノイズヤコビ行列Gtは、地図とは関係がないため(3.8)(3.9)式のよ うな行列となる。(3.8)式より、Iは単位行列である。
（3.6）
（3.7）
（3.8）
（3.9）
計測モデル
エンコーダは左右の車輪の角速度を測定し、それぞれの車輪の速度vl,vrを計算するために 用いる。各車輪の速度と車体速度vと車体角速度ωの速度計測関数hEを(3.10)式に示し、hEの ヤコビ行列を(3.11)式に示す。
（3.10）
（3.11）
LRF 計測のライン情報において、グローバルライン地図とローカルライン地図との関係
を(3.12)式に示す。上付き文字のL,Gは、それぞれローカル座標系、グローバル座標系にお
けるラインパラメータを表す。ゆえにエンコーダとLRFの計測関数を(3.13)式に示す。
−1
= t
t M
M
=
I Ft FR
0 0
= 0
R t
G G
+
= −
=
ω ω b v
b v v
h v
r l E
−
∂ =
= ∂
b b R
HER hE
1 0 0 0
1 0 0 0
∆
∆
∆
∆
∆
∂ =
=∂ −
−
T T
T T
T
w G g
t t
R R
0
0 0 2
0 ) 2 sin(
0 ) 2 cos(
2 1
2
1 2
θ θ
=
t t R
t
M
R x f
f ( )
)
(
第３章 ＥＫＦ－ＳＬＡＭ
また、計測ヤコビ行列Hは要素ごとに分けると(3.14)式のように構成される。
（3.12）
（3.13）
（3.14）
(3.14)式より1行1列目の要素HERは(3.11)式に既に示した。また 1行2列目の要素が０で
あるのは、地図とエンコーダの測定に直接的な因果関係がないからである。2行1列目のロ ボットの変数と抽出したラインの関係を表すヤコビ行列HLRを(3.15)式に示し、(3.15)式のn 番目のラインHLRn について(3.16)式に示す。
（3.15）
（3.16）
最後に2行2列目の抽出したラインと地図の関係を表すヤコビ行列HLMを(3.17)式に示し、
(3.17)式のn番目のラインについて(3.18)式に、そのラインに対応するm番目の地図のライン
について(3.19)式に示す。
（3.17）
（3.18）
（3.19）
T L
E h
h h=[ ]
=
∂
= ∂
LM LR
ER
H H H x
H h 0
−
−
= −
0 0 1 0
0
0 0 0 sin
cos
mG mGn
H
LRϕ ϕ
T m LR LR
LR L
LR H H H
R
H h =[ 1 2 L ]
∂
= ∂
T m LM LM
LM L
LM H H H
R
H h =[ 1 2 … ]
∂
=∂
] 0 0
[ 1 Lmn m
n
LM H
H = L L
−
∂ =
= ∂
1 0
) cos(
) sin(
1
mG mGm n L
Lm
y x
M
H h ϕ ϕ
−
−
−
−
−
−
=
=
θ ϕ
ϕ ϕ
ρ
θ ϕ
ϕ ϕ
ρ
ϕ ρ ϕ ρ
G m
G m G
m G
m
G
G G
G
L m L m L L
L
y x
y x
h
) sin(
) cos(
) sin(
) cos(
1
1 1
1 1
1
M
M
第３章 ＥＫＦ－ＳＬＡＭ
3.3 状態予測と状態更新
State estimation（
状態予測）
状態予測では、状態xの予測と誤差共分散行列Pの予測の二つの計算が行われる。一つ目 の状態xの予測は、(3.7)式を用いて(3.20)式となる。
（3.20）
二つ目は、誤差共分散行列Pの計算である。誤差共分散行列Pとは、これまでの状態の予測・
更新から計算した推定の不確かさを表す行列である。計算式の前に、誤差共分散行列Pの要 素を(3.21)式に示す。この行列の大きさはሺr+2mሻ次正方行列となる。rはロボットの状態変数、
mは地図のラインの数である。
（3.21）
ここで、(3.21)式よりPRRはロボットの状態共分散のr次正方行列である。PRMはロボットと
地図の間の共分散であり、r行2m列の行列である。PMMは地図の共分散の2m次正方行列で ある。ロボットと地図の間の共分散行列PRMと地図の共分散行列PMMを(3.22)式と(3.23)式に 示す。
（3.22）
（3.23）
誤差共分散行列Pの予測の計算式を(3.24)式に示す。ここで(3.24)式において誤差共分散行列
Pの計算負荷の軽減のため、実際の計算を以下の(3.25)式のように行う[7]。
（3.24）
（3.25） )
f(x xˆt = t
=
MM T
RM RM RR
P P
P P P
[
R Rm]
RM P P
P K
= 1
=
mm T
m T m
m T
m
MM
P P
P
P P
P
P P
P P
K M O M M
K K
2 1
2 22
12
1 12
11
T t t T t t t
t
F P F G wG
P ˆ =
−1+
+
=
0 0
ˆ
R RT0
MM T
R T RM
RM R T R RR R t
wG G P
F P
P
F
F
P
P F
第３章 ＥＫＦ－ＳＬＡＭ
State update
（状態更新）
更新ステップではラインを用いることによって予測した状態の更新を行う。更新の方法 としては、(3.13)式の実際の計測h(xt)と予測の計測h(xොt)との差をカルマンゲインKtに掛け、
それを状態xොtに加える。状態更新の計算式を(3.26)式に示す。同時に(3.27)式に誤差行列Pの 更新もカルマンゲインによって更新される。カルマンゲインの計算式を(3.28)式に示す。
（3.26）
（3.27）
（3.28）
(3.26)式のカルマンゲインに掛ける計測の差{hሺxtሻ- h(xොt)}の詳細を改めて(3.29)式に示す。エ
ンコーダとライン地図の予測と実計測の差をとるが、ライン地図は全てのラインを比較す るわけではなく、ラインデータ結合で同じ物体のラインであると判定されたものだけ計算 に用いる。判定方法については、4.6節で説明する。地図のライン比較の数に伴いカルマン ゲインの算出する行列数も変化する。
(3.29)
(3.27)式により更新された誤差共分散行列Ptは、次の周期における状態予測(3.24)式の誤差
共分散行列Pの予測にPt-1として用いられる。また(3.28)式より観測ノイズQtは、LRFの計測 点から抽出したライン推定の誤差量を表し、更新におけるカルマンゲインに影響を与える。
Qtの行列式を(3.30)式に示し、i番目の要素Qiを(3.31)式に示す。
（3.30）
（3.31）
{
h(x ) h(x )}
K x
xt = ˆt + t t − ˆt
t t t t
t
P K H P
P = ˆ − ˆ ˆ
1ˆ +
−= P H (H P H Q ) K
t t tT t t tT t
=
m t
Q Q
Q 0
1
0
O
= si
Q ρ 0
−
=
−
L j L j L L r l
L j L j L L r l
t t
v v v
v
x h x h
ϕ ρ ϕ ρ
ϕ ρ ϕ ρ
ˆ ˆ ˆ ˆ ˆ ˆ
) ˆ ( )
(
11 1
1
M
M
第３章 ＥＫＦ－ＳＬＡＭ
(3.31)式の中のsρi,sφiは、それぞれ距離ρ・角度φの推定の誤差量である。この誤差の求め方に
ついて次に説明する。各ラインパラメータの推定の誤差量は、次章のライン生成において、
ラインパラメータの推定に用いる最小二乗法の過程により求めた(3.32)式の共分散行列σを 用いて算出する。この共分散行列は簡単に言えば、ラインを構成する点のばらつき度合い をx,y座標系について計算されたものである。これをラインパラメータに関する誤差量とし て数値的に表すため(3.32)式の固有値を(3.33)式により求める。(3.33)式の数値的な意味を図
3.2に示す。
（3.32）
（3.33）
図3.2 ラインの誤差量
図3.2 よりσminはローカル座標系の原点OLからラインにのびる垂直線に対して平行であり、
ラインに含まれる点の距離方向の標準偏差を表し、σmaxは垂直線に対して直角であり、ライ ンに含まれる点の角度方向の標準偏差を表す。よって(3.30)式の要素を(3.34)式のように計
算する。(3.34)式よりnはラインに含まれる点の数であり、ライン長に応じた誤差量を算出
する役割を果たす。
（3.34）
以上のように状態の予測・更新を毎周期にわたり計算することで自己位置と地図の推定 を達成する。
= 2 2
2 2
y xy
xy x
σ σ
σ σ σ
( )
2 22 2 2 2
2 max
min, 2 2 xy
y x y
x
σ σ σ σ
σ σ
+
−
= + m
O
L minσ σ
max
=
max min 2
min
0
0
σ σ σ
Qi n
第４章 ライン生成
第４章
ライン生成
本章では前章のEKF-SLAMの構成におけるLine generation(ライン生成)について説明 する。本研究の提案部分はライン生成の一端を担う限定・修正アルゴリズムである。4.1節 でライン生成の全体構成を簡単に示し、4.2 節で物体検出、4.3 節でライン抽出アルゴリズ ム、4.4節で限定アルゴリズム、4.5節で修正アルゴリズム、4.6節でラインデータ結合の説 明を行う。
4.1 ライン生成の構成
ライン生成の流れを図4.1に示す。以降で、各処理の詳細について解説する。
図4.1 ライン生成の流れ
Measurement points
Object detection Line extraction
Limitation Correction
Line data association
EKF
第４章 ライン生成
Object detection（物体検出）
連続する点群に物体の分割点を見つけることで同じ物体に属する点集合を検出
Line extraction（ライン抽出）
物体検出により判別した点集合に関して地図の特徴であるラインを抽出
Limitation（限定アルゴリズム）
抽出したラインがEKF-SLAMに必要か限定アルゴリズムによって判定
Correction（修正アルゴリズム）
屋内環境の幾何学的な条件を用いて各ラインの角度を修正
Line data association（ラインデータ結合）
現周期に取得したラインのデータを過去の物理的に同じラインのデータと結合
第４章 ライン生成
4.2 物体検出
LRF 計測点からライン抽出を行うためには、全ての計測点をどこまでが同じ物体に属す る点かを判別する。すなわち、物体間の分割点を決定する。決定方法としては、計測点列 の距離の変化をみる。LRFの情報取得の順番と同じようにLRFから向かって右側の点から 順番に見ていくことを考えたとき、分割点と考えられる点は著しい距離の変化が存在する。
このような距離の変化は、LRFの測定可能領域に物体がない際に点が途切れた場合や、LRF から見て奥の物体の手前に部分的に異なる物体が位置している場合が考えられる。よって、
その距離の変化を利用して連続する点の距離の変化を閾値Dmaxと比較し、その値より変化が 大きかった場合、分割点とする。すなわち(4.1)式ように、連続する点が閾値に収まれば同 じ物体に属する点と判定する。
（4.1）
ゆえに、閾値Dmaxをどのように設定するかが、物体検出で最も重要となる。閾値は実行環境 など様々な要因によって適切な値が変化するために一定の値に決定するのが難しい。そこ で本研究では、可変閾値を用いる[8]。LRFの位置を原点とした可変閾値Dmaxの設定方法を 図4.2に示す。
図4.2 可変閾値Dmaxの設定
{
i i}
ii
i
p D p p OBJ
p −
−1≤
max⇒ ,
−1∈
α
∆
λ p
i−1p
i−1
d
id
iD
max hp
iライン１
ライン２
第４章 ライン生成
図4.2より連続する２点pi-1,piの原点からの距離は、それぞれdi-1,diで、その間の角度は0αで ある。可変閾値Dmaxによる判定は連続する点pi-1,piの間の角度0αと点pi-1の距離di-1によって 行われる。閾値Dmaxは、前の点pi-1の点を通り原点と点pi-1を結んだ線分に対して偏差角度λ 傾けた仮想線(ライン１)と原点と点piを結んだ延長線(ライン２)との交点をpihとすると、交点 pihと点pi-1間の距離とする。この距離より遠い場合、すなわち図4.2のように点pi-1を中心と した円の外に点piが存在する場合、二つを分割点として決定する。ゆえに閾値Dmaxは(4.2) 式で得られる。
（4.2）
しかし、この閾値をそのまま適用すると物体との距離が近い場合、点間の距離がきわめて 近く閾値が適切に設定されにくいため、(4.3)式のように閾値に一定値kを加える。一定値kは
45[mm]程度で良く機能する。
（4.3） )
sin(
) sin(
1 1
max
λ α
α
∆
−
= ∆
−
= pih pi− di− D
k d
k p p D
i i h i
∆ +
−
= ∆
+
−
=
−
−
) sin(
) sin(
1 1 max
α λ
α
第４章 ライン生成
4.3 ライン抽出
物体検出で計測点から分割点を見つけたら、次に各々の分割点間の点集合から物体を表 現するラインを抽出する。ラインを抽出する方法としては、移動ロボット工学においてよ く用いられているSplit and Mergeを用いる[9]。この方法を用いる理由は、他の主なライ ン抽出アルゴリズムに比べて計算時間が短く、正確に環境をラインで表現できる傾向が強 いからである。Split and Mergeの処理手順をそれぞれ図4.3と図4.4に示す。
1.
1.
1.
1. SplitSplitSplitSplit（分割）（分割）（分割）（分割）
(a). 点集合の両端点にラインを引き、そのラインから最大垂直距離の点dmaxを探索
(b). 点dmaxまでの垂直距離を閾値Dthと比較し閾値より大きい場合、点dmaxで点集合を分割
(c). (a)と(b)の処理を閾値Dthより遠い点がなくなるまで繰り返し
(a) (b)
(c)
d max
D
thd max
D th
d max
D th
第４章 ライン生成
２２
２２.... MergeMergeMergeMerge（結合）（結合）（結合）（結合）
(a). Splitによるライン結果から連続するラインの幅が閾値Dthの中に収まるかを探索
(b). 閾値Dthに収まったときラインを結合
(a) (b)
図4.4 Merge処理の流れ
以上の処理により大まかなライン抽出作業が終了するが、この時点では分割点同士を単 純に繋げただけであるため、点集合に対するラインの引き方としては適当ではない。よっ て、最終的にラインをどのように引くのかを決定する。その方法には最小二乗法を用いる。
ラインから見たラインに属する一つの計測点൛xi,yiൟまでの垂直距離は、図4.5から(4.4)式の ように表現される。
図4.5 ラインと点の距離関係
D
thϕ O
G) sin( ϕ y
i{ x
i, y
i}
) cos( ϕ
x
iρ
D
iϕ 90 −
X
Y
第４章 ライン生成
（4.4）
最良なラインは全ての計測点においてラインからの距離の誤差の二乗の合計である(4.5)式 を最小化することである。
（4.5）
次にD0をラインパラメータߩ, ߮に関して偏微分し、それが 0 になるρො,φොを求める。すなわち 最良なラインパラメータは(4.6)式・(4.7)式のように求められる。ここで、xത,yതは全ての計測 点の平均値、σは計測点の標準偏差を表す。
（4.6）
（4.7）
ここまでのライン生成処理を適用した結果を図4.6に示す。図4.6より物体検出により五つ の物体に分けられると判別され、ライン抽出により計測点からラインを抽出し、環境形状 を表現できている。
) sin(
)
cos( ϕ ϕ
ρ
i ii
x y
D = − −
2 0 =
∑
DiD
−
= −2 2
2 2
arctan ˆ
x y
xy
σ σ ϕ σ
ˆ ) sin(
ˆ )
ˆ cos( ϕ ϕ
ρ = x + y
1000 2000 3000 4000[mm]
第４章 ライン生成
4.4 限定アルゴリズム
誤差量の指標：品質の定義
研究目的で説明した通り、各ラインに誤差量の指標を設定する。この指標を“品質q”と
呼び、(4.8)式で表す。これは (3.34)式の要素と同じであり、σminはラインに対するラインを
構成する点の垂直距離の標準偏差、nはラインを構成する点の数である。品質の高低をライ ンと点のイメージで表すと図4.7のようになる。つまり同じ点数から構成されるラインでも ばらつきが小さい方が、ばらつきが同じでもラインを構成する点数が多いほど、品質qは値 としては小さくなり品質が高いとする。この品質を用いることによりラインの限定と修正 を試みる。
（4.8）
図4.7 ライン品質
限定するラインの判定
抽出したラインを限定するか否かを決定する。その判定は(4.9)式の場合に限定を行う。
qminは、一回のライン抽出において最も高品質(値が最小)なラインの品質値である。そしてqi は、最も高品質なライン以外のラインである。ゆえに最も高品質なラインの値に対する他 のラインの比率3を計算する。この比率3を任意に決定する閾値qthと比較し、閾値より比率3 が大きい場合、全体地図に対する精度が低いと判断し、そのラインを削除し地図として採 用しない。この判定により環境によって判定基準であるqminが変化するため、環境の複雑さ の違いについても対応が可能である。
（4.9）
2 min
q
σ
n=
高品質 低品質
th
i
3 q
q
q = >
min
第４章 ライン生成
限定アルゴリズムを適用した例を示す。図4.8の各ラインに対して品質の計算結果を表4.1 に示す。品質の値は図 4.8 の番号付けされたラインと対応しており、品質の性質上、LRF
（原点）に近いラインは品質が高く遠いラインは品質が低く設定される。その中でも最も 高品質なラインの品質qminは表の結果のようにライン７となり、この結果では閾値qth=10で 実験したためライン７の品質値を10倍した値以下のラインに地図が限定される。ゆえにラ イン３とライン５が削除され図4.9のような結果となり、高品質ラインだけに限定すること が可能となる。
図4.8 ライン特徴の番号付け
表4.1 ライン品質
1000 2000 3000 4000[mm]
１ ２ ３ ５ ４
６
７
特徴 標準偏差 点の数 品質 (×10⁻³)
ライン1 67.1 169 2.4
ライン2 104 159 4.1
ライン3 114.5 27 157
ライン4 82.3 80 12.9
ライン5 107.3 31 111.6
ライン6 102.5 108 8.8
ライン7 70.7 177 2.3
σ
minn q
第４章 ライン生成
図4.9 ライン限定結果 1000
2000 3000 4000[mm]
１ ２ ４
６
７
第４章 ライン生成
4.5 修正アルゴリズム
分割点修正アルゴリズム
ライン抽出アルゴリズムとして性能が高いSplit and Mergeであるが欠点も存在する。そ の欠点とは、環境に対するラインの再現性に、ばらつきが発生しやすいことである。その 理由は常にラインから一番離れた点を探すというアルゴリズムの性質上、はずれ点に影響 されやすい。このことはEKF-SLAMの推定にも悪影響を及ぼす。よって、その欠点をカバ ーするようにラインの精度を向上させる手法を考える。
最初に行うことは、限定アルゴリズムと同様に抽出ラインの(4.8)式で定義した品質を計
算する。Split and Mergeの抽出ラインにおいて品質が低くなる原因として考えられるのは、
点集合の適切な分割点が選択されてないことが挙げられる。分割点修正アルゴリズムでは、
Split and Mergeで選択された分割点の近傍の点を仮の分割点とし、再度ラインを引き、品
質を計算、前のラインの品質より高かった場合、分割点の修正を行うことでラインの環境 の再現性を高める。図を用いて分割点修正アルゴリズムの流れを説明する。図4.10にSplit
and Mergeで形成されたラインのイメージを示す。点とラインの関係を再度説明すれば、
図のように分割点を共有して点集合が分けられ（楕円に囲われた集合）、それぞれの集合に 対して最小二乗法を用いてラインが引かれる。加えて、この時点での二つのラインの品質 qa1,qa2を計算しておく。
図4.10 計測点とライン抽出
ここでは説明の簡単のために分割点の両近傍の点を仮の分割点１と仮の分割点２とする。
そして、図4.11のように仮の分割点に従って再度ラインを引き、それぞれ分割点１のライ ンの品質qb1,qb2、分割点２のラインの品質qc1,qc2を計算し最初の品質との比較を行う。する と、(4.10)式のように分割点２のラインの品質qc1,qc2が両ラインについて最も高品質である と分かる。このように両ラインの品質がともに向上したとき、ラインの修正を行う。ゆえ に今回の例の場合、分割点２のラインを採用し分割点修正アルゴリズムを終了する。この アルゴリズムにより結果的に再現性の高いラインが得られる。実際のアルゴリズムでは、
最初に設定された分割点から一定距離の点を仮の分割点として修正を試みる。
分割点
1
q
a2
q
a第４章 ライン生成
図4.11 分割点修正アルゴリズムの流れ
（4.10）
分割点修正アルゴリズムを適用した例を図4.12と表4.2に示す。適用前と適用後のライ ン１・２の品質を比較すると、どちらのラインも品質値が低くなり、高品質ラインが得ら れている。よって精度向上に寄与している。
図4.12 分割点修正アルゴリズム結果
表4.2 ライン品質
ライン１ ライン２
適用前 適用後
1
q
a2
q
a分割点１
分割点２
1
q
b b2q
1
q
c c2q
2 2 2 1
1
1 a b c a b
c q q A3D q q q
q < <
< <
特徴 適用前 適用後
ライン１
26.07 9.54
ライン２
7.2 7
第４章 ライン生成
角度修正アルゴリズム
角度修正アルゴリズムでは、屋内環境における幾何学的な特性、すなわち部屋の隅が直 角であることや向かい合う壁が平行であるという特性を利用しラインを改善する。つまり 抽出したラインをある基準ラインに対する相対的な角度が0度・90度・180度・270度と 変更する。このアルゴリズムによりラインの周期間でのばらつきを抑え、次節において説 明するラインデータ結合が適切に処理を行えるようにする。このアルゴリズムにおいて重 要となるのは、基準ラインを決めることである。基準ラインに実際の環境とのずれが存在 するとライン全体にずれが生じてしまうため、最も環境の再現性の高いラインを基準とす る必要がある。よって基準ラインには最も高品質なラインを採用する。ゆえに、基準ライ ン以外のライン角度φmは、(4.11)式のように設定される。φref は基準ライン角度である。し かしながら、基準ラインに対して直角や平行関係に程遠い場合は、適用しない。
（4.11）
角度修正アルゴリズムを適用した例を図に示す。図4.13(a)の適用前の二つのラインが交 差する角度は鋭角であるが、図4.13(b)の適用後の二つの交差するラインの角度は直角とな り角の部分を正確にラインで表現できている。また、適用前後でラインの交差点が結合し ているのは現実環境として、あり得ないためである。
(a) (b) 図4.13 角度修正アルゴリズム適用前後
{
0,90,180,270}
m =
ϕ
ref +ϕ
4.6 ラインデータ結合
SLAMの状態更新では、(3.26) による計測情報h(xොt)におけるライン 前周期までに保存されたラインの中で に同じ物体を意味しているかを判定
タ結合である。この処理結果によりライン間の対応関係が決まり、前のラインデータ情報 は新しいラインデータ情報へと置き換わり、それ以外のラインは引き続き保存される。
判定の方法としてはマハラノビス距離を計算し、カイ二乗値と比較
距離とは、多変数間の相関関係を利用して計算する距離のことである。また、カイ二乗値 は観測値と予測値との差を表す値で、
に従うと仮定する[10]。マハラノビス距離について図を用いて以下に説明する。
図
図4.14よりラインパラメータの距離
パラメータをプロットしていき、平均からの一定の差である“距離”を考えた場合、マハ ラノビス距離は一般距離の円とは異なり計測したラインパラメータの相関関係を用いて計 算し楕円形をとる。今回は、
計測したラインにおいて総当たりで計算し、その距離の最小値が閾値より低かった場合、
そのラインの組み合わせを同じ物体のラインとして判定する。
前周期までに保存されたn
ス距離Di2は(4.12)式のように計算できる。
式のように定義する。また、
り、(4.14)式のように定義する。
第４章 ライン生成
ラインデータ結合
(3.26)式において実際の計測観測h(xt)におけるラインと
におけるラインとの差をとる。よって、ラインの比較を行うためには、
前周期までに保存されたラインの中で、どれが現周期において計測されたライン
に同じ物体を意味しているかを判定しなければならない。この処理を行うのがラインデー 処理結果によりライン間の対応関係が決まり、前のラインデータ情報 は新しいラインデータ情報へと置き換わり、それ以外のラインは引き続き保存される。
マハラノビス距離を計算し、カイ二乗値と比較する
距離とは、多変数間の相関関係を利用して計算する距離のことである。また、カイ二乗値 は観測値と予測値との差を表す値で、EKF－SLAM ではマハラノビス距離はカイ二乗分布
。マハラノビス距離について図を用いて以下に説明する。
図4.14 一般距離とマハラノビス距離
よりラインパラメータの距離ρと角度φの２次元パラメータ空間を考え、計測した パラメータをプロットしていき、平均からの一定の差である“距離”を考えた場合、マハ ラノビス距離は一般距離の円とは異なり計測したラインパラメータの相関関係を用いて計
、マハラノビス距離を前の周期までに保存されたラインと現在 計測したラインにおいて総当たりで計算し、その距離の最小値が閾値より低かった場合、
そのラインの組み合わせを同じ物体のラインとして判定する。
n番目のラインと現在計測したm番目のラインの間のマハラノビ 式のように計算できる。y
nmは各々のライン間の誤差行列であり、
式のように定義する。また、Pnmはライン間の計測の相関関係を表した誤差共分散行列であ 式のように定義する。
nm nm T nm
i y P y
D2 = −1
におけるラインと予測状態 ラインの比較を行うためには、
現周期において計測されたラインと物理的 この処理を行うのがラインデー 処理結果によりライン間の対応関係が決まり、前のラインデータ情報 は新しいラインデータ情報へと置き換わり、それ以外のラインは引き続き保存される。
する。マハラノビス 距離とは、多変数間の相関関係を利用して計算する距離のことである。また、カイ二乗値 ではマハラノビス距離はカイ二乗分布
。マハラノビス距離について図を用いて以下に説明する。
の２次元パラメータ空間を考え、計測した パラメータをプロットしていき、平均からの一定の差である“距離”を考えた場合、マハ ラノビス距離は一般距離の円とは異なり計測したラインパラメータの相関関係を用いて計 マハラノビス距離を前の周期までに保存されたラインと現在 計測したラインにおいて総当たりで計算し、その距離の最小値が閾値より低かった場合、
番目のラインの間のマハラノビ は各々のライン間の誤差行列であり、(4.13) はライン間の計測の相関関係を表した誤差共分散行列であ
（4.12）
第４章 ライン生成
（4.13）
（4.14）
(4.14)式における右辺の左項の要素は、第３章にて既に詳細を述べており計測関数h、誤差
共分散行列Pなどを用いて(4.15)(4.16)(4.17)(4.18)式のように求める。また、(4.14)式におけ る右辺の右項の要素であるヤコビ行列Ｊは各座標系の対応関係を表し、(4.19)式のように求 める。
（4.15）
（4.16）
（4.17）
（4.18）
（4.19）
(4.12)式で計算されたマハラノビス距離を用いて、保存されたラインと計測したラインが
物理的に同じ物体と判定するためにはカイ二乗検定(自由度２・信頼区間αのカイ二乗値との 比較)を行う。ゆえに(4.20)式のようにマハラノビス距離がカイ二乗値以下であった場合、比 較したラインを同じラインであると判定し、対応するライン間で現周期の計測h(xt)と予測し た計測h(xොt)の差をとることで状態予測の正確さを算出する。すなわちラインデータ結合処理 の結果は状態更新の(3.26)式に計算に用いられる。
（4.20）
G m G n
nm L L
y = −
T n T
nm m nm
nm H P H JQ J
P = +
R HnR hLn
∂
=∂
G m Ln
m L
H h
∂
=∂
[
nR m]
Ln
nm H H
x
H h =
∂
=∂
=
mm T Rm
Rm RR
m
P P
P P P
( ) ( )
+ − +
∂ =
= ∂
1 0
sin cos
1
mL t mL tL m G
m
y x
L
J L ϕ θ ϕ θ
2 , 2 2
< χ
αD
i第５章 オフライン実機実験
第５章 オフライン実機実験
5.1 実験内容と条件
実験内容は、LRFを搭載した独立操舵二輪移動ロボット MIUBOT が、ある環境におい て初期位置から目標地点まで移動する。その際に得られた車輪エンコーダ情報とLRF計測 情報を基にEKF-SLAMを行う。なお、提案手法であるライン限定・修正の有効性を確認す るために通常のEKF-SLAMも従来手法として行い比較する。
実験環境は、図5.1の俯瞰写真のように周囲を囲われた4[m]×4[m]の環境に中央に障害 物を配置した。MIUBOTの走行経路は図5.2に示すように左手前を初期位置とし、最初に 直進して左奥へ進み障害物を回り込むように右折するように経路を設定した。実験パラメ ータ条件は表5.1に示す。また、ラインデータ結合に用いるカイ二乗値の信頼区間は統計学 において一般的に用いられる有意水準0.05として値を設定した[10]。
図5.1 実験環境
