日本ロボット学会誌 Vol. 22 No. 3, pp.343〜352, 2004
343
学術・技術論文
全方位カメラとデッドレコニング機能を有するサッカーロボット
のロバストな自己位置同定手法
倉 爪
松 岡
亮∗1
毅∗2
本 村
山 本 元
暁∗1
司∗1
山 口 祐 規∗1
長谷川
勉∗1
Robust Positioning Method using Omni-directional Camera and Dead Reckoning for
Soccer Robots
Ryo Kurazume∗1 , Akira Motomura∗1, Yuuki Yamaguchi∗1 ,
Takeshi Matsuoka∗2 , Motoji Yamamoto∗1 and Tsutomu Hasegawa∗1
In this decade, a number of various robot contests represented by ”RoboCup” have been held in various parts of
the world. In some of these contests, such as RoboCup middle-size robot league, fully automated mobile robots are
used to compete for their mobility and autonomy. These robots have to plan optimum moving strategy automatically without any human intervention after the game starts. One of the fundamental and crucial faculties for these
autonomous robots is the ability of accurate positioning of enemy and themselves.
In this paper, a robust positioning system is proposed for soccer robots equipped with an omni-directional camera
and wheels. The robot identifies its own position by observing some landmarks placed around the field through
an omni-directional camera, and by counting rotation of wheels. To achieve the robust positioning, the proposed
method utilizes (1) LMedS method for making correct correspondence between observed directions and landmarks,
(2) elimination of miss-correspondence of landmarks using a median filter, (3) position calculation considering the
properties of measurement error distributions using nonlinear maximum likelihood method, and (4) data fusion of
positions obtained by observing landmarks and dead reckoning using kalman filter.
Key Words: RoboCup, Positioning, Landmark, LMedS Method, Median Filter, Kalman Filter, Most Likelihood
Method
置を同定する手法が採用されている [2] [3]．しかし，実際には
1. は じ め に
観客や他のロボットが複数存在する環境でランドマークを誤対
近年，ロボカップに代表されるロボットコンテストが世界各
応なく安定して認識するのは困難であり，同定位置の精度，信
地で盛んに開催されている [1]．これらのコンテストの中には，
頼性はそれほど高くない．特に全方位カメラを用いた場合，全
ロボカップ中型リーグなど，競技開始後はオペレータはまった
方位画像は対象までの距離が遠くなると画像解像度が急激に低
く介入せず，おのおののロボットが自己，周囲のロボット，あ
下するため，誤対応を生じやすくなる．また画像処理のために
るいはボールの位置を測定し，自律的に判断し行動するものも
ある程度の時間が必要であることから，ランドマークを観測す
含まれている．このようなロボットにおいて，適切な行動戦略
る自己位置同定は，より高速で信頼性の高いデッドレコニング
の立案と実行のためには，正確な自己位置同定の実現は最も基
情報と併用されるのが一般的である．
全方位画像とデッドレコニング情報を併用して自己位置同定，
本的かつ重要な問題である．
通常，これらのコンテストにおいては，位置が既知のランド
さらには環境マップの作成を行う手法として，辻ら [5] は最小二
マークが競技フィールドの周囲に置かれ，移動ロボットは搭載
乗法と誤差楕円の変化を用いた誤対応検出手法を提案した．こ
したカメラからの画像を用いてランドマークを認識し，自己位
の手法はランドマークが未知でも位置同定を継続して行うこと
ができるが，ランドマークの位置推定時に観測位置と方位の誤
原稿受付 2002 年 6 月 7 日
九州大学
∗2
福岡大学
∗1
Kyushu University
∗2
Fukuoka University
∗1
日本ロボット学会誌 22 巻 3 号
差，あるいは観測方位と自己位置の相関を考慮していない．ま
た，位置同定に使用するランドマークは，長時間続けて精度よ
く観測されたエッジのみを用いるため，信頼性は高いもののサッ
カーロボットのように高速で移動する場合には適さない．一方，
—63—
2004 年 4 月
344
倉 爪
亮
本 村
暁
山 口 祐 規
松 岡
毅
山 本 元 司
長谷川
勉
サッカーロボットでは，ランドマークは既知かつ識別可能であ
周囲のランドマークを観測し，その観測方位を記録する．
り，全方位画像から自己位置が次々と計算されるので，多少誤対
（2）次に得られた観測方位とあらかじめ保持している環境地図
応が存在してもデッドレコニング情報との併用やカルマンフィ
を比較し，LMedS 法を用いて観測方位と地図上のランド
ルタを用いた滑らか拘束などにより，実用上十分な精度の自己
マークの対応付けを行う．
（3）メディアンフィルタを用い，誤って測定された観測方位を
位置同定が可能である．
また Nakamura ら [4] は，他のロボットの方位が観測可能で
あり，さらに各ロボットの大まかな位置情報をすべてのロボッ
除去する．
（4）残った観測方位に対して，観測誤差を考慮した非線形最尤推
定法（Nonlinear MLM）を適用し，自己位置を計算する．
トが共有していると仮定し，観測の結果，構成される三角形の
成立条件から，誤対応を検出する手法を提案している．しかし， （5）上記の手順で得られた自己位置と，車輪から得られたデッ
それぞれの観測に含まれる観測誤差の確率的な振る舞いについ
ドレコニング情報をカルマンフィルタにより融合する．
本論文の構成は以下のようになる．まず第 2 章，第 3 章，第
ては言及していない．
そこで本論文では，Fig. 1 に示すような全方位カメラとデッ
ドレコニング用受動車輪を有するロボカップ用サッカーロボッ
4 章では，観測されたランドマーク方位とランドマーク番号の
LMedS 法を用いた対応付け手法，全方位画像の観測誤差分布
トに対して，全方位画像に含まれる誤観測情報を LMedS 法，メ
を考慮した非線形最尤推定法による自己位置同定手法，および
ディアンフィルタによりあらかじめ自動的に除外し，観測誤差
メディアンフィルタを用いた誤観測情報の除去手法を提案する．
分布を考慮した非線形最尤推定法およびカルマンフィルタを用
さらに第 5 章では，提案した手法により得られた自己位置情報
いて，高精度かつ連続的に自己位置を同定する手法を提案する．
とデッドレコニング情報をカルマンフィルタにより融合する手
提案する手法のフローチャートを Fig. 2 に示す．本手法は，以
法を示す．第 6 章では，提案した手法をロボカップ用サッカー
下の手順からなる．
ロボットに実装し，位置同定実験を行った結果を示す．
（1）まず全方位カメラを用い，ポールやゴールなどフィールド
2. 観測されたランドマーク方位とランドマーク番号の
LMedS 法を用いた対応付け手法
まず，全方位カメラ等で複数のランドマーク方位を同時に観
測したとき，その方位情報と観測されたランドマークの番号を
対応付ける手法を示す．
ロボカップ中型機リーグでは，フィールドの四隅に置かれた
ポールや敵，味方のゴールがそれぞれ特定のパターンで色付け
されており，どのポールやゴールが観測されたかは画像から決
定できる．しかしより一般的には，視覚センサからランドマー
Fig. 1 A soccer robot with an omni-directional camera and dead
reckoning mechanism
クの方位は観測されるが，そのランドマークが地図上のどの番
号のランドマークに対応するかは，未知である場合も考えられ
る．これには，フィールド境界を表すポールが同じ色パターン
を有し識別不可能である場合だけではなく，例えば味方のロボッ
トを観測してその方位が得られたが，そのロボットがチーム内
のどのロボットかは分からない場合なども含まれる．このよう
な状況では，まず観測された複数のランドマークの方位情報と，
観測時点のランドマークの位置と番号を記した地図から，方位
情報とランドマーク番号の対応関係を決定しなければならない．
しかし観測された方位にはランドマークと見間違った別の対象
物を観測した情報も含まれる可能性があり，これらを対応付け
の過程で適切に取り除く必要がある．
そこで本論文ではこの問題に対し，非線形最適化手法の一手
法である LMedS 法（The Least Median of Squaresx）を適用
することで，観測誤差にロバストな対応付け手法を提案する．
まず，Fig. 3 に示すように，環境に位置が既知の M 個のラ
ンドマークが存在し，またある時刻で N 個のランドマークが観
測され，それぞれ方位 φi , i = 1〜N が与えられたとする．本論
文で提案する対応付け手法は以下のようなものである．
（1）まず，M 個のランドマークからランダムに三つのランド
マークを選択する．
Fig. 2 A flow chart of proposed positioning system
JRSJ Vol. 22 No. 3
（2）観測された N 個の方位から同様に三つの方位を抜き出し，
—64—
Apr., 2004
全方位カメラとデッドレコニング機能を有するサッカーロボットのロバストな自己位置同定手法
345
さて，上述した手法において，（1）で三つのランドマークを
選択する代わりに，三つの観測方位を選択する手法も考えられ
る．すなわち，
（1）
まず，N 個の観測方位からランダムに三つの観測方位を選
択する．
（2） M 個のランドマークから同様に三つのランドマークを抜
き出し，これらと（1）で選択した観測方位から Pk を計算
する．
というものである．そこで両者の計算量を比較してみる．
まず一般に，あるデータ群に含まれる例外値の割合を とし，
全データから F 個のデータをサンプリングして何らかの推定値
Fig. 3 Positioning in the case where correspondance of landmark number and observed direction is unknown
を計算したとき，q 回のサンプリングで少なくとも 1 回のサン
プリングには例外値が含まれない確率は
これらと（1）で選択したランドマークの設置位置から，付
P = 1 − {1 − (1 − )F }q
録 A に示す方法でロボットの位置姿勢 Pk を計算する．
（3）残りの N − 3 個の観測方位 φi に対して，（2）で計算され
（2）
で与えられる．また，Fig. 3 の問題設定として，N 個の観測方
たロボットの位置姿勢 Pk から他の M − 3 個のランドマー
位には，Nf 個の観測に失敗したデータが含まれているとし，ま
クの推定観測方位 φj , j = 1〜M − 3 を計算し，実際の観
た M 個のランドマークのうち，正確に測定されなかったラン
測方位 φi に最も近い推定観測方位を有するランドマーク
ドマーク数を D とする．さらに（3），（4）で必要な計算量 S
を選択する．このとき，実際の観測方位と選択されたラン
とする．
ドマークの推定観測方位の二乗残差 Ek,i = (δφi )2 を計算
まず，（1）で M 個のランドマークからランダムに三つのラ
ンドマークを選択した場合を考える．このとき F = 3 であり，
する．
（4）（3）をすべての観測方位に対して行い，得られた二乗残差
Ek,i , i = 1〜N − 3 の中間値 Ek,med = medi Ek,i を求
め，この逆数をロボットの位置姿勢 Pk の計算の確からし
と定義する．
さ RELk = E 1
k,med
（5）（2）〜（4）をすべての観測方位の組み合わせに対して行
い，確からしさが最大となるロボットの位置姿勢 Pmax =
maxRELk Pk を計算する．
（6）（1）〜（5）を確からしさの最大値がある閾値を超えるまで
繰り返す．
不適切な選択（例外値）を行う割合は =
適な組み合わせを決定する手法
Pmax = max medEk,i Pk
RELk
となる．また式
（2）より P が適当な確率を超えるのに必要な繰り返し回数を q1
とする．さらに（2）〜（5）で必要な計算量は
N C3
× S である
から，（1）〜（6）で必要な全計算量は
C1 =N C3 × S × q1
（3）
となる．
一方，（1）
で N 個の観測方位からランダムに三つの観測方
位を選択した場合を考える．このとき =
本手法はすなわち，観測残差の中間値を最小にするように最
D
M
Nf
N
であり，また式
（2）より P が適当な確率を超えるのに必要な繰り返し回数を q2
とする．さらに（2）
〜（5）で必要な計算量は M C3 × S である
〜（6）で必要な全計算量は
から，（1）
（1）
C2 =M C3 × S × q2
である．もし（1）で切り出されたランドマークの番号と，（2）
で選択された観測方位が正しく組み合わされている場合には，
（4）
となる．
対する二乗残差は大きな値となる．したがって，単純にすべて
一例として第 6 章で示す実験結果（M = 8, N = 6, Nf =
1, D = 3）に対してこれらの値を計算すると，P = 0.99 とした
場合，q1 = 17 回，q2 = 6 回，全計算量はそれぞれ C1 = 340S
回，C2 = 336S 回となり，両者にはそれほど大きな差がないこ
の二乗残差の和を計算の確からしさとすると，大きな値を有す
とが分かる．
（3）で計算される二乗残差の大部分は小さな値になるはずであ
る．しかし，中には間違った対象物を観測した場合など，観測
方位自体に大きな誤りが含まれている場合も考えられ，これに
る少数の二乗残差の影響を強く受け，正しい組み合わせが求め
3. 最尤推定を用いた位置計算法
られない場合が考えられる．これに対し，このような場合でも
大部分の観測が正しい場合には，二乗残差の中間値は小さな値
上述した手法を用いて，方位情報とランドマーク番号の対応
になるはずである．そこで，このように観測残差の中間値を最
付けを行った後，観測されたすべての方位情報を用いて自己位
小にする観測方位とランドマークの組み合わせを求めることで，
置を再計算する．
例えば間違った対象物の観測データなど，大きな観測誤差を含
一般に，付録 A に示すように，全方位画像から少なくとも三
むデータの影響を受けることなく，正しい組み合わせを見つけ
つのランドマーク A，B，C の方向が観測されれば，ロボット
ることができる．
の位置を決定することができる．しかし実際には多くのランド
日本ロボット学会誌 22 巻 3 号
—65—
2004 年 4 月
倉 爪
346
亮
本 村
暁
山 口 祐 規
松 岡
毅
山 本 元 司
長谷川
勉
と書ける．ただし，
dΨ = (dψ1 , dψ2 , · · · , dψn )T
A=
− x1d−x̃
2
−1
− x2d−x̃
2
2
yn −ỹ
d2
n
− xnd2−x̃
−1
..
.
−1
..
.
1
..
.
n
dX = (dx dy dθ)T
ψ̃1 + θ̃ −
ỹ
tan−1 xy11 −
−x̃
ỹ
tan−1 xy22 −
−x̃
ψ̃2 + θ̃ −
L=
..
.
Fig. 4 Positioning in the case where several landmarks are observed
マークの方位が観測され，これらを適切に用いて誤観測情報の
y1 −ỹ
d2
1
y2 −ỹ
d2
2
ψ̃n + θ̃ − tan−1
yn −ỹ
xn −x̃
（9）
（10）
（11）
（12）
である．ここで，式（8）左辺の二乗平均が，
除去や自己位置計算を精度よく行わなければならない．
2
2
2
ΣΨ = diag(σψ
, σψ
, · · · , σψ
)
n
1
1
そこでまず，すべての観測は正しく行われているが，それら
（13）
の方位角にはガウス分布に従う観測誤差が含まれていると仮定
2
はランドマーク i の方位角 ψi
で与えられるとする．ただし σψ
i
し，線形化した繰り返し最尤推定法により複数観測誤差の分布
の観測誤差の二乗平均値（分散）であり，これを既知であると
特性を考慮した位置計算手法を示す [6] [7]．
仮定する．よって，観測誤差 dΨ を最も適切に解に配分するに
まず，Fig. 4 に示すように，位置が既知のランドマーク i が，
は，残差方程式
ロボットから見て方位 ψi に観測されたと仮定する．このとき，
観測方程式は，
ψi + θ = tan−1
yi − y
xi − x
こで，この現在考えているロボットの位置や，観測されたラン
ドマークの方位に誤差が含まれていると考え，x = x̃ + dx，
φi = ψ̃i + dψi などを代入し，Taylor 展開すると，
VT Σ−1
Ψ V
（15）
を最小にする dX を求めればよい．そこで式（15）を dX で微
分すると，
∂
∂
VT Σ−1
(AdX − L)T Σ−1
Ψ V =
Ψ (AdX − L) = 0
∂dX
∂dX
（16）
ψ̃i + dψi + θ̃ + dθ
yi − ỹ
1
dx
=
ỹ 2
2
(x
i − x̃)
1 + ( xyii −
)
−x̃
より，
−1
yi − ỹ
dy + tan−1
ỹ 2 x − x̃
x
i
i − x̃
1 + ( xyii −
)
−x̃
1
xi − x̃
yi − ỹ
yi − ỹ
dx −
dy + tan−1
=
d2i
d2i
xi − x̃
（14）
に対して，
（5）
となる．ただし，(x, y, θ) はロボットの位置，姿勢である．こ
+
V = AdX − L
−1 T
dX = (AT Σ−1
A Σ−1
Ψ A)
Ψ L
（6）
（17）
となる．
これらのことから，n 個のランドマークが観測された場合には，
=
(xi − x̃)2 + (yi − ỹ)2 で あ り，
（1）ま ず 三 つ の ラ ン ド マ ー ク を 選 び ，式（A.13），（A.14），
∂
tan−1 A は ，tan B = A と し て 両辺 を 偏 微 分し た 式 ，
∂x
（A.16）を解いて，ロボットの仮の位置 X = (x, y, θ) を
1
1
∂B
∂A
= ∂A
から， ∂B
= cos2 B ∂A
= 1+tan
2 B ∂x を
∂x
∂x
∂x
cos2 B ∂x
求める．
用いた．さらに式（6）を整理すると，
（2）x̃ = x などとして，観測されたすべてのランドマークに対
yi − ỹ
xi − x̃
−1 yi − ỹ
し，式（17）を解いて dX を求める．
dψi =
dx
−
dy
−
dθ
−
+
θ̃
−
tan
ψ̃
i
d2i
d2i
xi − x̃
（3） X ← X + dX を代入する．
（7）
（4）（2），（3）を dX が十分小さくなるまで繰り返す．
と な る ．た だ し ，di
の手順により，最適なロボットの位置姿勢が求められる．
となる．
さて，ランドマークが n 個観測された場合には，それぞれに
4. メディアンフィルタを用いた不適切な観測情報の除去
対して式（7）が成り立つから，これを行列の形で整理すると，
前章で示した非線形最適化の手法は，観測誤差がガウス分布
式（7）は，
dΨ = AdX − L
JRSJ Vol. 22 No. 3
に従うことを仮定していた．しかし実際には，ランドマークと
（8）
—66—
Apr., 2004
全方位カメラとデッドレコニング機能を有するサッカーロボットのロバストな自己位置同定手法
347
似た色の服を着た観客をランドマークとして観測してしまうな
を用いて両者を融合することを試みる．
ど，少なくとも自己位置同定時間内ではガウス分布に従わない
ま ず 時 刻 k で の ロ ボット の 推 定 位 置 ，速 度 を Xk
(x(k∆), ẋ(k∆))T とする．また状態方程式は
誤差も多く存在する．そこでランドマークの観測情報に前章で
=
示した非線形最尤推定法を適用する前に，外乱（outlier）に対し
てロバストであるといわれているメディアンフィルタを用いて，
観測情報に含まれる外乱をあらかじめ取り除くことを考える．
Xk+1 = AXk + BUk
（19）
Yk = CXk + w
（20）
出力方程式は
本論文で提案するメディアンフィルタを用いた外乱除去の手
法を以下に示す．
（1）まず，観測されたランドマークから三つのランドマーク
を選び，式（A.13），（A.14）を解いて位置の候補点 pi =
とする．ただし w は観測ノイズである．ここで
(x, y, θ)T を求める．
で行い，N 個の候補点を得る．後述するようにロボカップ
中型リーグでは最大 8 個のランドマークが観測できるので，
1
0
A=
（2）これを観測されたランドマークに対しすべての組み合わせ
∆
1
（21）
であり，等速直線運動を仮定すると，
候補点の数は最大 Nmax =8C3 = 56 個である．
Uk = 0
（22）
（3）すべての候補点の平均位置と分散行列を計算し，分散行列
である．また，デッドレコニングからは速度の情報が得られ，全
の固有ベクトル s1 , s2 を求める．
（4）すべての候補点を二つの固有ベクトルの方向 s1 , s2 にそれ
ぞれ射影し，ベクトル方向を基準に降順（あるいは昇順）に
方位画像からは位置の情報が得られるとすると，デッドレコニ
ングに対しては，
並べ替えて順位付ける．すなわち，各候補点は二つの固有ベ
C = (0 1)T
Yk = (0 ẋk,wheel )T
クトルの方向それぞれに対して順位 R1 (i), R2 (i) = 1〜N
を持つ．
（23）
であり，全方位画像に対しては，
（5）次式で各候補点の得点 R(i) を計算する．
C = (1 0)T
Yk = (xk,image 0)T
R(i) =| R1 (i) − N/2 | + | R2 (i) − N/2 | （18）
これは中間の順位への近さを示す．
（24）
となる．
（6）得点 R(i) の最も少ない候補点を中間値候補点とする．
ここで（5）で得られる得点として，例えば最も単純な二つの順
よって本システムにカルマンフィルタを適用すると，
位の単純和 R(i) = R1 (i) + R2 (i) なども考えられるが，単純和
（1）適当な観測ノイズ Σw ，初期位置誤差 ΣX を決める．
では，それぞれの方向で偏った順位を持つものが最小の得点を
（2）デッドレコニングの場合には式（23）を，全方位画像のと
きには式（24）を設定する．
有する場合も想定されるため，適当ではない．
次に得られた中間値候補点を基に，ガウス状の誤差を有する
と思われるランドマーク観測情報だけを選択し，前章で示した
T
−1
で，P を計算する．
（3） P = (ΣX −1
k + C Σw C)
（4） X−
k+1 = AXk を計算する．
−1
T
(Yk+1 − CX−
非線形最尤推定法を用いた同定位置の最適化を行う．このため， （5） Xk+1 = X−
k+1 + PC Σw
k+1 ) で，Xk+1
を計算する．
まず
（1）すべての候補点に対して中間値候補点からの距離を計算し， （6） ΣX k+1 = APAT + BΣU BT を求める．
（7）（2）〜（6）を繰り返す．
これが閾値以下であるものを抽出する．
（2）抽出された観測点の導出に使用されたランドマーク点をカ
ウントし，一定回数（α ×
3
C ，n
n n 3
は観測された全ラン
となる．ただし，画像から得られる位置の誤差が大きいときに
は，Σw = diag(σx2 , σẋ2 ) のうち，σx2 を大きくすればよい．
ドマーク数，α は誤観測の予想割合）以上使用されたラン
6. サッカーロボットを用いた位置同定実験
ドマークのみを選択する．
Fig. 5 に示すロボカップ 2002 中型機部門のフィールドとラ
（3）得られたランドマークの観測情報だけを用いて式（A.13），
（A.14），（A.16）等を求め，非線形最尤推定法による繰り
ンドマーク（L0 〜L7 ）に対して，提案したアルゴリズムの性能
評価実験を行った．使用したロボットは，九州大学で製作した
返し計算により最適解を求める．
5. カルマンフィルタを用いたデッドレコニング情報との
融合
ロボカップ用サッカーロボット（Fig. 6）である [8]〜[10]．こ
のロボットは，全方位カメラ，画像処理ボード（日立製，IP5005），ボードコンピュータ（Celeron 700 [MHz]），無線 LAN
ロボットの位置が車輪の回転角を測定する，いわゆるデッド
を搭載し，ロボット下部には駆動輪と独立にデッドレコニング
レコニングでも測定されている場合を考える．このとき，デッ
用の受動車輪を装備している．以下の実験では，ロボットがお
ドレコニングから得られる位置と，全方位画像から得られる位
よそ（5,000 [mm]，4,000 [mm]，180 [deg.]）の位置で静止した
置を適切に融合する必要があるが，ここではカルマンフィルタ
状態で行った．
日本ロボット学会誌 22 巻 3 号
—67—
2004 年 4 月
348
倉 爪
亮
本 村
暁
山 口 祐 規
松 岡
毅
山 本 元 司
長谷川
勉
Table 2 Procedure of correspondance calculation
Table 3 20 candidate points
Fig. 5 Field and landmark arrangement for middle-size robot
league at RoboCup 2002
Fig. 6 Soccer robot for middle-size robot league
Table 1 Direction of landmarks
Table 4 Comparison of positioning results
6. 1 ランドマークの対応付け
まず，全方位画像から複数のランドマークの方向が観測され
たとき，第 2 章で提案した手法を用いて，観測方位とランドマー
示す合計 20 個の候補点を計算する．これら候補点の単純平均値，
ク番号の対応付けを決定する実験を行った．実際にはランドマー
中間値候補点，および最尤推定位置を Figs. 7，8，Table 4 に
クに描かれた色模様によりランドマークの対応付けは可能であ
示す．ただし，Fig. 7 中の円は中間値候補点から半径 1,000 [mm]
るが，ここではより一般的な場合として，画像情報からでは対応
の位置を示している．これに含まれる候補点は，Table 3 の右端
付けができないものと仮定した．計測データの一例を Table 1
の “N” で示された 12 点であり，これらの候補点を算出するの
に示す．この場合，設置されている 8 個のランドマーク中，計測
に用いられたランドマークの参照回数を Table 5 に示す．これ
されたランドマークは L1 , L2 , L3 , L4 , L6 , L7 の六つであり，そ
より，4 回 (=α ×
れぞれの観測方位を O1 〜O6 とした．すなわち，正しい対応付
ドマークは L1 , L2 , L3 , L6 , L7 の 5 個であるから，これら 5 個
けは，(L1 , O1 ),(L2 , O2 ), (L3 , O3 ),(L4 , O4 ), (L6 , O5 ),(L7 , O6 )
のランドマークの観測情報を用いて，最尤推定法を用いた繰り
3
C ,n
n n 3
= 6,α = 0.4) 以上参照されたラン
である．ただし敵ゴールポスト端であるランドマーク L3 に対
返し計算により最終位置を計算した．実際には Fig. 9 に示すよ
しては，実際には誤った対象物を計測していたため，L3 の測定
うに，4 回の繰り返し計算で誤差が 0.01 [mm] 以下に収束した．
方位は不正確である．
これらの結果から，Fig. 7 より単純な平均を用いた場合には，
対応付けの過程を Table 2 に示す．これより 4 回目の繰り
返しで正しい対応付けを発見していることが分かる．
観測に失敗したランドマーク L4 の影響で実際とは大きく異な
る位置に同定されているが，提案した手法では，この計測に失
6. 2 位置同定実験
敗したランドマークは自動的に除外され，より正確な位置に同
次に求められたランドマークと観測データの対応付けを用い，
定されていることが分かる．
第 3 章で提案したメディアンフィルタと非線形最尤推定法によ
まず，Table 1 に示した七つのランドマークから，Table 3 に
JRSJ Vol. 22 No. 3
また，Fig. 10 に示す位置にロボットを置き，同様の実験を
行った結果を Table 6 に示す．これらの実験から，フィール
りより正確な位置同定を行った．
ド内ではほぼ 200 [mm] 以下の精度で位置同定が可能であるこ
—68—
Apr., 2004
全方位カメラとデッドレコニング機能を有するサッカーロボットのロバストな自己位置同定手法
349
Table 5 Referenced number of each landmarks
Fig. 7 Candidate points and average, median, and optimum positions
Fig. 10 Robot’s positions for experiments
Table 6 Calculatd positions
Fig. 8 Candidate points and average, median, and optimum positions (magnified)
ルマンフィルタによって融合した結果を示す．
Figs. 11〜13 に，デッドレコニングによる位置同定結果，全
方位画像による自己位置，およびそれらをカルマンフィルタに
より融合した結果を示す．ただし実験では，位置，速度の誤差分
散 σx2 = σẋ2 = 1.0，姿勢，角速度の誤差分散 σθ2 = σθ̇2 = 0.1 と
した．実験の結果，デッドレコニングだけを用いた場合には，ロ
ボットがフィールド内にいるにもかかわらず，自己位置がフィー
ルドの外へ出てしまう場合があったが，30 [ms] ごとに更新され
る全方位画像からの自己位置情報を用いることで，より正確な
Fig. 9 Convergence error
自己位置同定が実現できることが確かめられた．
7. お わ り に
とが分かった．またカメラ画像の取得から自己位置同定までに
必要な処理時間は，Celeron 700 [MHz] で 30 [ms] 以内であり，
本論文ではロボカップ用のサッカーロボットに対して，全方
ボールの大きさが 210 [mm] であることから，本システムは精
位画像と車輪からのデッドレコニング情報を元に，正確な自己
度，計算時間ともに中型機リーグのサッカーロボットに対する
位置同定を行う手法を示した．本手法は，（1）LMedS 法を用
位置同定システムとして十分な性能を有することが確認できた．
いたランドマークの対応付け，（2）メディアンフィルタによる
6. 3 カルマンフィルタによる位置情報の融合
誤観測情報の除去，（3）観測誤差を考慮した非線形最尤推定法
次に全方位画像からの位置情報とデッドレコニング情報をカ
による全方位画像からの最適自己位置同定，（4）デッドレコニ
日本ロボット学会誌 22 巻 3 号
—69—
2004 年 4 月
350
倉 爪
亮
本 村
暁
山 口 祐 規
松 岡
毅
山 本 元 司
長谷川
勉
提案した手法は，ロボカップ用ロボットだけでなく，清掃ロ
ボットや探査ロボットなど他の多くの移動ロボットにも適用可
能である．
本論文で提案した手法の問題点として，必ず三つ以上のラン
ドマークが観測されなければならないことが挙げられる．しか
し試合中は敵や味方のロボットによりランドマークが遮蔽され，
この条件が満たされない場合も考えられる．提案した手法では，
ランドマークが十分観測できない間はデッドレコニング情報の
みで位置同定を行うが，この場合は位置同定誤差の蓄積が大き
な問題となる．この問題に対する我々の解決手法については，次
報で報告する予定である．
謝
辞
本研究は，21 世紀 COE プログラム「システム情報
科学での社会基盤システム形成」の一環として実施された．
参 考
文 献
[ 1 ] 中村：“実機ロボットリーグの現状と今後の課題”，日本ロボット学会
誌，vol.20, no.1, pp.11–14, 2002.
[ 2 ] C.F. Marques and P.U. Lima: “A Localization Method for a
Soccer Robot Using a Vision-Based Omni-Directional Sensor,”
In RoboCup2000: Robot Soccer World Cup IV, pp.96–107,
2001.
[ 3 ] F.M. Marchese and D.G. Sorrenti: “Omni-directional Vision
with a Multi-part Mirror,” In RoboCup2000: Robot Soccer
World Cup IV, pp.178–188, 2001.
[ 4 ] T. Nakamura, M. Oohara, A. Ebina, M. Imai, T. Ogasawara
and H. Ishiguro: “Real-Time Estimation Spatial Configuration Between Multiple Robots by Triangle and Enumeration
Constraint,” In RoboCup2000: Robot Soccer World Cup IV,
pp.219–228, 2001.
[ 5 ] 辻，八木，谷内田：“全方位視覚センサを用いたロバストな環境マップ
生成と自己位置推定”，日本ロボット学会誌，vol.19, no.1, pp.59–67,
2001.
[ 6 ] R. Kurazume and S. Hirose: “Study on Cooperative Positioning System—Optimum Moving Strategies for CPS-III—,”
Proc. IEEE Int. Conf. on Robotics and Automation, vol.4,
pp.2896–2903, 1998.
[ 7 ] 倉爪，広瀬，岩崎，長田，指田：“協調ポジショニングシステムの研
究—CPS-III を用いた自動清掃ロボットシステムの開発—”，日本ロ
ボット学会誌，vol.16, no.7, pp.934–941, 1998.
[ 8 ] 井上，山口，杉本，松岡，牛見，山本：“自律型サッカーロボットの
開発—第一報 ハードウェアおよびソフトウェア—”，第 20 回計測自
動制御学会九州支部学術講演会予稿集，pp.41–42, 2001.
[ 9 ] 山口，井上，杉本，松岡，桐木，長谷川：“自律型サッカーロボット
の開発—第二報 全方位ビジョンとデッドレコニングの組み合わせに
よる自己位置同定—”，第 20 回計測自動制御学会九州支部学術講演
会予稿集，pp.13–14, 2001.
[10] 杉本，荒岡，榊，井上，山口，山本，長谷川：“自律型サッカーロボッ
トの開発—第三報 通信負荷分散を目的とした複数ロボット間の情報
巡回型通信方式—”，第 20 回計測自動制御学会九州支部学術講演会
予稿集，pp.99–100, 2001.
[11] 米谷，森：測量学．丸善，1979.
Fig. 11 Calculated positions (X-Y coordinates）
Fig. 12 Calculated positions (X coordinates）
付録 A.
Fig. 13 Calculated positions (Y coordinates）
三つのランドマークの観測による位置計算法
Fig. 14 に示すように，位置が既知のランドマーク A，B，C
が観測されたときに，ロボット X の位置を計算する手法を示す
ング情報とのカルマンフィルタを用いた融合からなる．また提
（Cassini の解法）[11]．ただしここでは，全方位画像から各ラン
案した手法を，ロボカップ 2002 中型機部門で使用予定のサッ
ドマーク間の角度 φ1 , φ2 が測定されたとする．
カーロボットに実装し，位置同定実験を行った結果，提案した
ABX, および CBX を通る円 O,O を考え，B から O,O の
中心を通る線を引き，O,O との交点を D,E とする．このと
手法の有効性が確認された．
JRSJ Vol. 22 No. 3
—70—
Apr., 2004
全方位カメラとデッドレコニング機能を有するサッカーロボットのロバストな自己位置同定手法
351
Fig. 14 Positioning by measuring the directions of landmarks
A, B, and C
き， BAD = 90◦ ， BCE = 90◦ ， BXA =
Fig. 15 Calculation of robot orientation
BDA = φ1 ，
¯ を B,D を結ぶ
BXC = BEC = φ2 である．また，tan BD
線の方位角とすると，
¯
yB − yD = (xB − xD ) tan BD
xE = xC + (yC − yB ) cot φ2
（A.9）
yE = yC − (xC − xB ) cot φ2
（A.10）
（A.1）
¯
yA − yD = (xA − xD ) tan AD
となる．
¯ + (xA − xB ) tan AD
¯
= (xB − xD ) tan AD
，
さて，ここで X を B,D から求めることを考えると，式（A.4）
（A.2）
（A.5）と同様に，
¯
yB − yD − (xB − xD ) tan BX
（A.11）
¯
¯
tan DX − tan BX
¯
yB − yD − (xB − xD ) tan BX
¯
tan DX
y = yD +
¯
¯
tan DX − tan BX
（A.12）
であるから，両式を引くことで，
x = xD +
¯ − tan AD)
¯
yB − yA = (xB − xD )(tan BD
¯
−(xA − xB ) tan AD
（A.3）
となる．よって，D の位置は
となる．ここで，BX ⊥ DX であり，また D,X,E は一直線上
¯
yA − yB − (xA − xB ) tan AD
（A.4）
¯
¯
tan BD − tan AD
¯
yA − yB − (xA − xB ) tan AD
¯
tan BD
= yB +
¯
¯
tan BD − tan AD
（A.5）
¯ tan DE
¯ = −1，tan DX
¯ = tan DE
¯ とな
にあるから，tan BX
xD = xB +
yD
¯ =
と求められる．ここで，tan AB
yA −yB
xA −xB
る．これらを代入して整理すると，
であり，AD ⊥ AB
¯ tan AB
¯ = −1 である．さらに
から tan AD
◦
¯
BD = 90 − φ1 より，
¯ −
ABD = AB
¯ + φ1 )
= − cot(AB
¯ tan φ1
1 − tan AB
=−
¯
tan AB + tan φ1
（A.6）
であるから，これらを式（A.4），
（A.5）に代入して整理すると，
¯
yA − yB − (xA − xB ) tan AD
¯
¯
tan BD − tan AD
（A.7）
= xA + (yA − yB ) cot φ1
¯
yA − yB − (xA − xB ) tan AD
¯
tan AD
= yB +
¯ − tan BD
¯
tan BD
xD = xB +
= yA − (xA − xB ) cot φ1
となる．同様に，E についても
日本ロボット学会誌 22 巻 3 号
（A.14）
となる．ただし，
¯ = tan(AB
¯ + φ1 − 90◦ )
tan BD
yD
¯
yB − yD + (xB − xD ) cot DE
（A.13）
¯ + cot DE
¯
tan DE
¯
yB − yD + (xB − xD ) cot DE
¯
tan DE
y = yD +
¯ + cot DE
¯
tan DE
x = xD +
¯ = yD − yE
tan DE
x D − yE
(yA − (xA − xB ) tan φ1 ) − (yA − (xC − xB ) tan φ2 )
=
(xA + (yA − yB ) tan φ1 ) − (xA + (yC − yB ) tan φ2 )
−(xA − xB ) tan φ1 + (xC − xB ) tan φ2
=
(yA − yB ) tan φ1 − (yC − yB ) tan φ2
（A.15）
である．またロボットの方位 θ は，Fig. 15 に示すようにラン
ドマーク A（または B,C）の観測された方向を ψA とすると，
ψA + θ = tan−1
（A.8）
yA − y
xA − x
（A.16）
で求められる．
—71—
2004 年 4 月
352
倉 爪
倉爪
亮
本 村
暁
山 口 祐 規
松 岡
山 本 元 司
本村
亮（Ryo Kurazume）
1967 年 2 月 4 日生．1991 年東京工業大学機械物理
工学専攻修士課程修了．同年（株）富士通研究所入
社，1995 年東京工業大学機械宇宙学科助手，2000
年スタンフォード大学客員研究員，同年東京大学生
産技術研究所博士研究員，2002 年より九州大学シ
ステム情報科学研究院助教授，現在に至る．群ロ
ボット，歩行機械，レーザ計測の研究に従事．博士（工学）．
（日本ロボット学会正会員）
山口祐規（Yuuki Yamaguchi）
1978 年 9 月 25 日生．2001 年九州大学電気情報
工学科卒業．2003 年九州大学大学院システム情報
科学府修士課程修了（知能システム学専攻）．同年，
九州日本電気ソフトウェア株式会社入社，現在に至
る．在学中は協調自律分散型ロボットの視覚誘導の
研究に従事．
山本元司 （Motoji Yamamoto）
1962 年 7 月 10 日生．1990 年九州大学大学院工学
研究科博士課程修了（生産機械工学）
．日本学術振興
会特別研究員（1988〜1990 年）．1990 年九州大学
工学部講師，1992 年同助教授，現在に至る．ワイヤ
懸垂機構，移動ロボットなどの研究に従事．IEEE，
日本機械学会，計測自動制御学会の会員．
（日本ロボット学会正会員）
JRSJ Vol. 22 No. 3
毅
—72—
長谷川
勉
暁（Akira Motomura）
1979 年 12 月 26 日生．2002 年九州大学電気情報
工学科卒業．現在，同大学大学院システム情報科学
府修士課程（知能システム学専攻）に在籍中．協調
自律分散型ロボットの研究に興味を持つ．
（日本ロボット学会学生会員）
松岡
毅（Takeshi Matsuoka）
1971 年 6 月 10 日生．1994 年九州大学情報工学科
卒業．1996 年九州大学大学院工学研究科修士課程
修了．1999 年九州大学大学院システム情報科学研
究科博士後期課程修了．同年，福岡大学工学部講師．
2002 年同学部電気工学科助教授，現在に至る．知能
ロボットの研究に従事．（日本ロボット学会正会員）
長谷川勉（Tsutomu Hasegawa）
1950 年 2 月 18 日生．1973 年東京工業大学電子物
理工学科卒業．同年電子技術総合研究所勤務．1992
年より九州大学工学部情報工学科教授．現在同大学
大学院システム情報科学研究院教授．知能ロボット
の研究に従事．工学博士．計測自動制御学会，電気
学会，日本機械学会などの会員．
（日本ロボット学会正会員）
Apr., 2004
© Copyright 2024 Paperzz
