移動体の自己定位方法
【発明の詳細な説明】
発明の目的;
(産業上の利用分野)
この発明は移動ロボット等の移動体の自己定位方法に関し、特に出発地及び目的地が与えられたとき、移動体が内蔵している環境モデルより移動ルートを生成し、視覚センサからの入力情報と環境モデルとの対応づけにより移動体の位置,姿勢を計算するようにした自己定位方法に関する。
(従来の技術)
地上を動く移動体は、通常車輪の回転やステアリング角を検出して累積計算することにより、自己の移動距離や方向の変化を計測している。これはデッドレコニングと称され、移動体が自己の現在位置を知る上で基本となるものであるが、誤差が累積して行くという致命的な問題を有する。
このため、移動体は何らかの方法で環境を計測して現在位置を知り、上記デッドレコニングによる計測値を補正する必要がある。そのための手段としては、■環境(地上)側に移動体のための目印や灯台を設置する方法、■環境に既存している特徴点を目印に利用する方法がある。しかし、前者■の方法では特に目印や灯台を設置しなければならない煩雑さがあり、後者■の方法が望ましい。
(発明が解決しようとする課題)
後者■の方法を採用するためには視覚センサ等によって大量の情報を移動体に入力し、それを処理して位置確認に必要な情報を抽出する必要がある。このような自己定位法として、第25回 自動制御連合講演会で発表(昭和57年11月)された“カルマンフィルタによる移動ロボットの位置推定”(中村達也,上田 実)があるが、偏差に関する状態方程式と観測方程式とを用いており、この2つの方程式はモデル化されたものであり、現実のものとはギャップがあり過ぎて適用は困難である。また、第4回 日本ロボット学会学術講演会(昭和61年12月15〜17日)の“単眼自立ロボットのためのある位置決め問題”(杉原厚吉)は組合せのみに着眼しており、撮像手段の画像からの抽出については未解決であり、また組合せの爆発が起こる可能性がある。
この発明は上述のような事情よりなされたものであり、この発明の目的は、移動体が移動する環境をコンピュータグラフィックスによるモデルを用いて表現し、移動体の車輪に取付けたセンサの情報からカメラ視点の位置,姿勢を拘束してコンピュータグラフィックス手法を用いて予測情報を生成し、この生成された予測情報を利用して画像処理を行ない、誤差が累積しないようにした移動体の自己定位方法を提供することにある。
発明の構成;
(課題を解決するための手段)
この発明は移動ロボット等の移動体の自己定位方法に関するもので、この発明の上記目的は、予め障害物の位置を記憶させた環境モデルファイルを用意し、移動体が所定距離進んだとき、前記移動体に設けられた移動量検出手段の出力によりその地点を推定し、その推定地点からある誤差範囲の領域を設定し、その領域内の各点から前記移動体に設けられた撮像手段が捉えるであろう予測画像を前記環境モデルファイルからコンピュータグラフィックス手法を用いて各々生成し、生成された各予測画像上で前記環境モデルの障害物の特徴要素であるマーカを抽出しマーカの存在する領域を求め、この求められた領域を前記撮像手段からの入力画像中に当てはめてその領域からのみ実際のマーカの抽出を行ない、前記環境モデルのマーカと抽出されたマーカとを対応づけて自己定位を行なうことによって達成される。
(作用)
移動ロボットの機能としては、環境において自己の位置,姿勢を同定する機能が必須となる。ガスレートジャイロ、ロータリエンコーダ等のセンサを用いて位置,姿勢を同定する方法では誤差が累積するため適当な補正が必要となる。そのために、環境から画像情報を入力し、何等かの特徴をロボットに内蔵している環境モデルと照合することにより、自己の位置,姿勢を同定する方法が有効である。ここにおいて、ロボットが移動する環境として、(1)環境は既知であり多面体世界である、(2)床面は平坦であると仮定し、人工的な環境においては上記仮定(1)及び(2)は実用性を大きく制限するものではない。以上の仮定のもとで、この発明ではロボットが移動する環境を多面体で表現し、マーカ(たとえば垂直エッジ)を自己定位のための標識としている。
この発明では、ロボットが移動する空間についての知識を環境モデルとして内蔵し、位置,姿勢は3個のマーカから計算する。ここで、問題となるのは入力画像からのマーカの抽出と、抽出されたマーカと、環境モデル中のマーカとの対応づけであるが、この発明ではロボットに取付けたロータリエンコーダの出力によりカメラ視点の位置,姿勢をある範囲に拘束することでこれを解決している。つまり、視点の位置,姿勢をある範囲内に拘束することにより、入力画像中でマーカが存在する領域(以後、ウインドウとする)をコンピュータグラフィックスの手法で生成し、このウインドウを用いて入力画像を処理してマーカの抽出を行なう。この場合の特徴としてトップダウンの画像処理が挙げられる。ウインドウを用いることで、ウインドウ内には必ずマーカが1個存在すると仮定できるので、画像処理をトップダウンで行うことが可能となり、マーカの抽出が容易となるのである。他の特徴として簡単な対応づけがあり、ウインドウを生成した段階で環境モデルとの対応づけが行われている。各マーカに対するウインドウが重ならない場合には、抽出したマーカと環境モデル中のマーカとが一意に対応する。また、ウインドウが重なる場合には、抽出したマーカに対して環境モデル中のマーカが複数個対応する。抽出したマーカに対する複数個の対応候補については、検証により簡単に誤対応を防止するようになっている。
(実施例)
この発明では第1図に示すように環境情報の入力手段として1台のCCDカメラ(イメージセンサ)1を搭載し、車輪2に取付けたロータリエンコーダによって移動量を計測でき、自己定位と移動を繰返しながら目的地へ到達する移動ロボット3を考える。移動ロボット3は環境からCCDカメラ1を介して第2図に示すような画像情報を入力する。この場合、移動する床面は平坦で、移動は第3図に示すようにX,Y方向の並進成分とZ軸回りの回転成分だけとする。ロボット3が移動する環境は第4図R>図に示す如く既知であって多面体世界であり、第5図に示す如く出発地SPから目的地GPまでのルートを自動生成し、出発地SPにおいてロボット3は定位されている。ロボット3はルート上のサブゴールP1〜P5で自己定位を行ないながら目的地GPまで進み、障害物4,5,6を避けるようにしてルートを生成する。
尚、障害物とは、環境内に存在している物体のことを意味する。
ここで、自己定位の基本的な考え方を説明する。簡単のために第6図に示す如く自己定位と移動を繰返して1次元空間を出発地Sから目的地Gへ移動するロボットを想定し、移動中の任意の地点Pと任意の地点Qについて考える。先ず地点Pで自己定位する。自己定位の誤差によりロボットは実際には区間Spに存在している。そして、ロボットは地点Pから地点Qへ移動するが、ロボットは地点Pでの自己定位の誤差とロータリエンコーダで求めた移動量の計測誤差とで、区間Sq′に存在している可能性が大きい。地点Qで自己定位すると、自己定位の誤差によりロボットは実際には区間Sqに存在している。このように地点Qで区間Sq′を設定できれば視点の拘束(後述する)により区間Sqに自己定位ができ、ロータリエンコーダの計測誤差はクリアできる。よって、移動量の計測誤差が累積することはなく、自己定位の誤差で目的地Gへ到達できるのである。
次に視点の拘束について説明すると、第3図に示す如くロボット3の位置,姿勢をX,Y方向の並進成分x,yとZ軸回りの回転成分θとで(x,y,θ)と表現し、移動量をX,Y方向の並進成分dx,dyとZ軸回りの回転成分dθとで(dx,dy,dθ)と表現する。視点の拘束を行うために定位誤差,計測誤差を次のように定義する。先ずロボット3がある地点(x,y,θ)で自己定位した結果が(x′,y′,θ′)であったとき、各成分の定位誤差δx,δy,δθを次の(1)式で定義する。
そして、ロボット3がある地点から(dx,dy,dθ)だけ移動してロータリエンコーダから求まる移動量が(dx′,dy′,dθ′)であったとき、各成分の計測誤差Δx,Δy,Δθを次の(2)式で定義する。
第7図に示すようにロボット3が地点P(xp,yp,θp)で自己定位した後に(dx,dy,dθ)だけ移動し、地点Q(xq,yq,θq)へ到達したとする。地点Pでの自己定位結果とロータリエンコーダから求まる移動量(dx′,dy′,dθ′)により、ロボットは次の(3)式に示す範囲に存在することが、(1),(2)式より導かれる。
ここで、
である。
ここにおいて、ロボットは移動前の自己定位結果(xp′,yp′,θp′)と移動量(dx′,dy′,dθ′)を常時把握しているため、真の現在地(xq,yq,θq)を(3)式より制限すること、即ち視点がどの範囲にあるかを予測することができ、これを視点の拘束という。
次に、ウインドウ生成のアルゴリズムについて説明する。
先ず視点の位置,姿勢を、移動前の自己定位の結果及び移動量の計測値から前記(3)式を用いて拘束する。視点の位置,姿勢を第8図(A)の領域VAに示すように拘束した後に領域内を離散化し(実際には、例えば円VCに外接する正方形VAをθRも用いて求め、その正方形内の各点を周辺も含めて離散化する。この離散化される点の数は一定としているので正方形の大きさが変化すれば、各点のピッチが変わることになる。)、離散化した各地点で視点VPを設定し、その視点VPで環境がどのように見えるかを計算した予測画像をコンピュータグラフィックスの手法を用いて同図(B)のように各視点VP毎に生成する。なお、第8図(A)のWは世界(絶対)座標系を示し、Rはロボット座標系を示している。次に、各視点VPにおける予測画像中のマーカ(たとえば垂直エッジ)に注目してウインドウを形成する。環境モデルファイル中のマーカをL,予測画像上でのマーカをL′とすると、第8図(B)の予測画像例ではマーカL11′…L14′…L1n′とマーカL21′…L24′…L2n′とが存在し、マーカL11′…L14′…L1n′によって形成されるウインドウW1は同図(C)のようになる。マーカL21′…L24′…L2n′についても同様のウインドウW2(図示せず)を形成する。各マーカのウインドウが重なった場合には、第9図9図に示す如くそれらウインドウを統合した後区間を細分化して処理する。第9図はマーカL1′によって形成されるウインドウW1と、マーカL2′によって形成されるウインドウW2と、マーカL3′によって形成されるウインドウW3と、マーカL4′によって形成されるウインドウW4と、マーカL5′によって形成されるウインドウW5とが重なり、1つのウインドウWを形成する様子を示している。ウインドウから抽出されたマーカには、環境モデル中の複数個のマーカを第10図(A)の如く対応表で区間S1〜S5で対応づける。そして、実画像からのマーカの抽出順序を決めるためにウインドウのソーティングを行なう。ソーティング条件は高優先順位順に、イ)しきい値以上の大きさのマーカ、ロ)視点の位置,姿勢の変化に対して見え方の変化が大きいマーカ、ハ)距離が近いマーカ、ニ)画像の端にあるマーカとなる。
次にマーカの抽出について説明すると、生成したウインドウ個々に対して、マーカの抽出個数が所定の値になるまで下記の処理を行なう。
先ず第11図(A)の如くスムージングによってウインドウWi内の個々の画素に対してエッジ保存用平滑フィルタを2回作用させ、ウインドウWi内の個々の画素に対してSobel変換(エッジ強調処理)を行ない、Sobel変換のエッジ強度及びエッジ方向から同図(B)及び(C)の如く2値化を行なう。2値化の条件は、エッジ方向の場合には同図(C)のようにエッジ方向がある範囲以内(たとえば90゜±10゜)を“1"とし、エッジ強度の場合は同図(B)のようにエッジ強度がしきい値以上である強度を“1"とする。このようにして第11図(D)の如く2値化されたウインドウWi内の個々の画素に対して同図(E)のようなHough変換を行ない、ピーク点PPより同図(F)で示すような画像上での直線の方程式を求めてマーカMを抽出する。第10図(A)のような対応付リストをもとに、抽出したマーカを環境モデル中のマーカに同図(1)〜(4)のように対応づける。
一方、ロボットの位置,姿勢は、第12図に示す実画像から抽出したマーカA′,B′,C′より第13図に示す如くAB,BC間の角度α,βを計算し、次の(4)式を解くことで求まる。これに関しては後述する。
上述のように計算された位置,姿勢が正しいかどうかは、その位置,姿勢においてコンピュータグラフィックス画像を作成し、コンピュータグラフィックス画像上のマーカと入力画像から抽出したマーカとが一致するかどうかを調べれば分る。位置,姿勢の計算に用いたマーカA′,B′,C′は、カメラパラメータ、マーカ抽出時の誤差を持っているため、計算された位置,姿勢は第14図に示す斜線領域LAとなる。その時、検証されるマーカDが入力画像上に現れる範囲を計算することができる。よって、マーカDの検証はその範囲内でマーカが検出されているかどうかを調べればよい。ここで、マーカA′,B′,C′の抽出誤差は予め設定できないため、マーカA′,B′,C′の抽出誤差はマーカD′の検証が成功するか、又は一定の大きさになるまで1画素ずつ広げて行なう。すなわち、第14図に示すロボットの存在領域(誤差範囲)LAが広がって行き、かつマーカDの検証範囲も広がって行くことになる。
このようにして抽出したマーカからロボットの位置,姿勢の計算に用いる3個のマーカの組合せの全てについて上記検証を行ない、その中で誤差範囲を最小とする位置,姿勢を自己定位の結果とする。
第15図はこの発明の動作例を示しており、第1図に示す移動ロボット3は第5図に示す如く予め定められている出発地SPから目的地GPまでのルート及びサブゴール(P1,P2,…)に従って、障害物を避けながら移動し(ステップS1)、サブゴール(P1,P2,…)に来たか否かを常時判定しており(ステップS2)、サブゴールに達したときに一旦停止する(ステップS3)。この停止したサブゴールPiでは第16図に示すように、前回のサブゴールP(i-1)での誤差r(i-1),θ(i-1)に今回の移動によるロータリエンコーダの誤差eiを加えた領域内の各点から見えるであろう画像をコンピュータグラフィックス手法により作成し、環境モデルのマーカの存在する画像上の範囲であるウインドウWiを決定する(ステップS4)。次に、実際の画像において上述の如く求めたマーカ存在領域内からのみマーカが存在するか否かを判断し、マーカが存在すれば抽出する(ステップS5)。そして、抽出された3個以上のマーカが環境モデルのマーカのどれと対応するかを逐一調べて最終的な位置及び姿勢を決定し、同時にそのときの誤差ri,θiを記憶して次のサブゴールP(i-1)に備える(ステップS6)。上記動作を目的地GPまで繰返すことによってルート上を移動する(ステップS7)。
次に、第17図のフローチャートを参照して自己定位の方法を説明する。つまり、第15図のステップS4〜S6の詳細を説明する。
先ず、前回サブゴールP(i-1)の誤差r(i-1),θ(i-1)と今回サブゴールPiのロータリエンコーダの誤差eiの2つにより視点の領域を第16図のVAの如く決定し(ステップS10)、領域VA内を第8図(A)に示すように離散化して各視点VPを決める(ステップS11)。上記各視点VPについてその視点から見えるであろう予測画像を第8図(B)のようにコンピュータグラフィクスの手法により生成し、環境モデルのマーカの存在範囲であるウインドウWiを決める(ステップS12)。このとき姿勢誤差θ(i-1)も考慮し、次にマーカの存在範囲であるウインドウが重なり合うか否かを判定し(ステップS13)、重なり合う場合には各ウインドウを統合し(ステップS14)、第10図に示すようなマーカの候補リストを作成する(ステップS15)。次に、実際の画像(入力画像)より上記求めたウインドウ範囲からのみマーカを見つけて抽出し(ステップS16)、抽出したマーカが3個以上有るか否かを判断し(ステップS17)、2個以下の場合には処理不能となって停止する(ステップS18)。
上記ステップS17において抽出したマーカが3個以上有る場合には、抽出した全マーカより3個のマーカを選び(ステップS20)、当該3個のマーカを第10図(1)の最上部(L1′,L2′,L4′)のように環境モデルの候補リストのマーカと一旦対応づけ(ステップS21)、3個のマーカの抽出誤差を第14図の領域LAのように仮定する(ステップS22)。そして、次に第18図に示す如く自己定位の誤差範囲Q1〜Q8を計算する(ステップS23)。第18図における誤差範囲LAの9点Q0〜Q8は
で表わされ、1≦k≦8として誤差ri,θiは次のように求められる。
そして、誤差範囲LA内において検証を行なうマーカが入力画像中に存在する領域をコンピュータグラフィックスの手法を用いて計算する(ステップS24)。計算された領域内に検証するマーカが存在するか否かを調べ(ステップS25)、次に検証率がある割合以上(例えば80%以上)か否かを判定し(ステップS30)、ある割合よりも小さい場合には抽出誤差Deの画素を広げ(ステップS34)、Deが所定値(例えば3)よりも小さければステップS23にリターンする(ステップS35)。また、検証率がある割合以上の場合には誤差範囲の面積を計算し(ステップS31)、面積が最小であるか否かを判定し(ステップS32)、最小であれば面積及び誤差ri,θiを更新する(ステップS33)。第19図(A)はDe=1でウインドウw1にマーカD′が重ならないので検証失敗となる例を示し、同図(B)もDe=2でウインドウw2にマーカD′が重らないので検証失敗となる例を示し、同図(C)はDe=3でウインドウw3にマーカD′が重なり検証が成功した例を示している。この場合、検証率は100%となる。即ち、検証すべき領域が1つしかないときは0%か100%のいずれかとなる。そして、候補リストのマーカと全て対応づけが終了したか否かを判定し(ステップS36)、終了しておれば更に3個のマーカの選び方が全て終了したか否かを判定する(ステップS37)。ステップS36で全て対応づけが終了していない場合はステップS21にリターンする。
次に、第14図及び第18図で説明した自己定位の誤差範囲について説明すると、環境モデル20とイメージセンサ10との実際の関係は第20図のようになっているが、上下左右が逆になるので反対側位置10Aにイメージセンサ10があると考えて第21図のような構成を考える。その座標関係は第22図のようになり、Ocはレンズ中心座標を示している。そして、角度α,βの求め方を以下に説明する。イメージセンサ10上の入力画像A′,B′,C′は環境モデル20のA,B,Cと対応づけられる。画像A′はイメージセンサ10の何画素目に写っているかは分るので、第23図に示すような画像A′と光軸との距離l1は求まり(画素間隔は既知)、またレンズの焦点距離fは既知である。従って、α1=tan-1(l1/f)で角度α1が求まり、同様に角度α2も求まり、結局α=α1−α2で角度αが求まる。同様にして角度βも求まる。一方、第14図及び第24図に示すように3点A,B,Ocを通る円の中心をK1とすると、∠RAB=90゜で、線分▲■■▼の長さは環境モデル20より求まるので、sinα=▲■■▼/2r1から円の半径r1が求まる。環境モデル20よりA,Bの座標は既知であるので、半径r1が求まれば中心K1の座標(a1,b1)は求まる。同様にして3点B,C,Ocを通る円の中心K2の座標(a2,b2)も求まる。従って、前記(4)式から点Ocの座標(x,y)が求められる。これをQ0とする。
次に、姿勢θ(カメラ光軸とXwのなす角)の求め方について述べる。第25図に示すように、線分
と光軸のなす角α1は既に求められている。一方、A点の座標は環境モデル20から分っており、点Ocの座標も求まっているので、線分
と軸Xwのなす各θ′は演算で求められ、姿勢θはθ=θ′−α1となる。
次に、第19図で説明したようにイメージセンサ10の標本化の誤差である1画素を考慮する場合について述べる。第26図に示すように画像A′,B′をそれぞれ1画素広げたときの角度αmaxは前回と同様にして求められ、第27図に示すように画像A′,B′をそれぞれ1画素狭めたときの角度αminも前回と同様に求められる。同様に、角度βmax,βminも求められる。従って、点Oc(Q0)と同様にして点Q1〜Q8の座標(X,Y,θ)が(5)式の如く求められる。
ステップS17において、マーカが4つ以上あるときには今回使用した3つ以外のマーカが入力画像中に存在する領域を計算する。これはQ0(X0,Y0,θ0)〜Q8(X8,Y8,θ8)の各点から見える予測画像をコンピュータグラフィックス手法で計算し、今回使用した3つ以外のマーカが存在するウインドウD′を求める。そして、入力画像のウインドウD′内に実際にマーカが有るかどうかを調べ、検証率80%以上ならばOKとする。領域が複数あるとき、例えば5つの領域があってその内の4つ以上の領域でそれぞれ該当するマーカが有ったときは、4/5×100=80%となりOKとする。次に、点Q1〜Q8で囲まれる斜線部面積を計算し、最小ならその面積及び誤差ri,θiを記憶して更新する。もし検証率が80%未満であれば、第26図に示すようにイメージセンサの画素をもう1つ広げてαmax,βmaxを求め(α,βより2画素分広げたことになる)、同様に第27図に示すようにもう1画素減らしてαmin,βmin(α,βより2画素分狭めたことになる)を求めて、以下同様な処理を行なう。次に対応づけの組合せを変えて同様に行ない、全ての対応づけの組合せを終了すれば、次の組合せの3つのマーカを選んで同様の処理を行ない、結局Q1〜Q8で成る領域面積が最小となるもので自己定位することになる。そのときの誤差ri,θiが次回の誤差の基準となる。
発明の効果;
以上のようにこの発明によれば、マーカが存在しそうな領域について画像処理を行なうので効率が良く、領域を区切っているので画像からのマーカの抽出が安定する。また、余分な線を抽出するのを防いでいるので、抽出したマーカと環境モデル中のマーカとの対応づけの効率が良い。
【図面の簡単な説明】
第1図はこの発明の移動ロボットの外観構成図、第2図は撮像手段による環境の入力画像の一例を示す図、第3図R>図はロボットの座標系を説明するための図、第4図はロボットが移動する環境の一例を示す図、第5図は第4図の環境に対するルート生成の様子を示す図、第6図は自己定位を説明するための模式図、第7図は視点の拘束を説明するための図、第8図は視点領域の離散化及びウインドウの生成を説明するための図、第9図はウインドウの合成を説明するための図、第10図は抽出したマーカの対応づけを示す図、第11図はウインドウよりマーカの抽出を行なう様子を示すフロー図、第12図は入力画像中のマーカの例を示す図、第13図はロボットの位置,姿勢の計算原理を説明するための図、第14図は自己定位の誤差を説明するための図、第15図はこの発明のロボット移動の様子を示すフローチャート、第16図はサブゴールで自己停止しながら移動する様子を示す図、第17図はこの発明による自己定位の動作例を示すフローチャート、第18図はロボット位置,姿勢の誤差範囲を説明するための図、第19図は位置,姿勢についての検証の例を示す図、第20図〜第25図は位置,姿勢の誤差範囲の計算を説明するための図、第26図及び第27図は画像入力範囲の変更を説明するための図である。
1……CCDカメラ、2……車輪、3……移動ロボット、10……イメージセンサ、20……環境モデル。
発明の目的;
(産業上の利用分野)
この発明は移動ロボット等の移動体の自己定位方法に関し、特に出発地及び目的地が与えられたとき、移動体が内蔵している環境モデルより移動ルートを生成し、視覚センサからの入力情報と環境モデルとの対応づけにより移動体の位置,姿勢を計算するようにした自己定位方法に関する。
(従来の技術)
地上を動く移動体は、通常車輪の回転やステアリング角を検出して累積計算することにより、自己の移動距離や方向の変化を計測している。これはデッドレコニングと称され、移動体が自己の現在位置を知る上で基本となるものであるが、誤差が累積して行くという致命的な問題を有する。
このため、移動体は何らかの方法で環境を計測して現在位置を知り、上記デッドレコニングによる計測値を補正する必要がある。そのための手段としては、
(発明が解決しようとする課題)
後者
この発明は上述のような事情よりなされたものであり、この発明の目的は、移動体が移動する環境をコンピュータグラフィックスによるモデルを用いて表現し、移動体の車輪に取付けたセンサの情報からカメラ視点の位置,姿勢を拘束してコンピュータグラフィックス手法を用いて予測情報を生成し、この生成された予測情報を利用して画像処理を行ない、誤差が累積しないようにした移動体の自己定位方法を提供することにある。
発明の構成;
(課題を解決するための手段)
この発明は移動ロボット等の移動体の自己定位方法に関するもので、この発明の上記目的は、予め障害物の位置を記憶させた環境モデルファイルを用意し、移動体が所定距離進んだとき、前記移動体に設けられた移動量検出手段の出力によりその地点を推定し、その推定地点からある誤差範囲の領域を設定し、その領域内の各点から前記移動体に設けられた撮像手段が捉えるであろう予測画像を前記環境モデルファイルからコンピュータグラフィックス手法を用いて各々生成し、生成された各予測画像上で前記環境モデルの障害物の特徴要素であるマーカを抽出しマーカの存在する領域を求め、この求められた領域を前記撮像手段からの入力画像中に当てはめてその領域からのみ実際のマーカの抽出を行ない、前記環境モデルのマーカと抽出されたマーカとを対応づけて自己定位を行なうことによって達成される。
(作用)
移動ロボットの機能としては、環境において自己の位置,姿勢を同定する機能が必須となる。ガスレートジャイロ、ロータリエンコーダ等のセンサを用いて位置,姿勢を同定する方法では誤差が累積するため適当な補正が必要となる。そのために、環境から画像情報を入力し、何等かの特徴をロボットに内蔵している環境モデルと照合することにより、自己の位置,姿勢を同定する方法が有効である。ここにおいて、ロボットが移動する環境として、(1)環境は既知であり多面体世界である、(2)床面は平坦であると仮定し、人工的な環境においては上記仮定(1)及び(2)は実用性を大きく制限するものではない。以上の仮定のもとで、この発明ではロボットが移動する環境を多面体で表現し、マーカ(たとえば垂直エッジ)を自己定位のための標識としている。
この発明では、ロボットが移動する空間についての知識を環境モデルとして内蔵し、位置,姿勢は3個のマーカから計算する。ここで、問題となるのは入力画像からのマーカの抽出と、抽出されたマーカと、環境モデル中のマーカとの対応づけであるが、この発明ではロボットに取付けたロータリエンコーダの出力によりカメラ視点の位置,姿勢をある範囲に拘束することでこれを解決している。つまり、視点の位置,姿勢をある範囲内に拘束することにより、入力画像中でマーカが存在する領域(以後、ウインドウとする)をコンピュータグラフィックスの手法で生成し、このウインドウを用いて入力画像を処理してマーカの抽出を行なう。この場合の特徴としてトップダウンの画像処理が挙げられる。ウインドウを用いることで、ウインドウ内には必ずマーカが1個存在すると仮定できるので、画像処理をトップダウンで行うことが可能となり、マーカの抽出が容易となるのである。他の特徴として簡単な対応づけがあり、ウインドウを生成した段階で環境モデルとの対応づけが行われている。各マーカに対するウインドウが重ならない場合には、抽出したマーカと環境モデル中のマーカとが一意に対応する。また、ウインドウが重なる場合には、抽出したマーカに対して環境モデル中のマーカが複数個対応する。抽出したマーカに対する複数個の対応候補については、検証により簡単に誤対応を防止するようになっている。
(実施例)
この発明では第1図に示すように環境情報の入力手段として1台のCCDカメラ(イメージセンサ)1を搭載し、車輪2に取付けたロータリエンコーダによって移動量を計測でき、自己定位と移動を繰返しながら目的地へ到達する移動ロボット3を考える。移動ロボット3は環境からCCDカメラ1を介して第2図に示すような画像情報を入力する。この場合、移動する床面は平坦で、移動は第3図に示すようにX,Y方向の並進成分とZ軸回りの回転成分だけとする。ロボット3が移動する環境は第4図R>図に示す如く既知であって多面体世界であり、第5図に示す如く出発地SPから目的地GPまでのルートを自動生成し、出発地SPにおいてロボット3は定位されている。ロボット3はルート上のサブゴールP1〜P5で自己定位を行ないながら目的地GPまで進み、障害物4,5,6を避けるようにしてルートを生成する。
尚、障害物とは、環境内に存在している物体のことを意味する。
ここで、自己定位の基本的な考え方を説明する。簡単のために第6図に示す如く自己定位と移動を繰返して1次元空間を出発地Sから目的地Gへ移動するロボットを想定し、移動中の任意の地点Pと任意の地点Qについて考える。先ず地点Pで自己定位する。自己定位の誤差によりロボットは実際には区間Spに存在している。そして、ロボットは地点Pから地点Qへ移動するが、ロボットは地点Pでの自己定位の誤差とロータリエンコーダで求めた移動量の計測誤差とで、区間Sq′に存在している可能性が大きい。地点Qで自己定位すると、自己定位の誤差によりロボットは実際には区間Sqに存在している。このように地点Qで区間Sq′を設定できれば視点の拘束(後述する)により区間Sqに自己定位ができ、ロータリエンコーダの計測誤差はクリアできる。よって、移動量の計測誤差が累積することはなく、自己定位の誤差で目的地Gへ到達できるのである。
次に視点の拘束について説明すると、第3図に示す如くロボット3の位置,姿勢をX,Y方向の並進成分x,yとZ軸回りの回転成分θとで(x,y,θ)と表現し、移動量をX,Y方向の並進成分dx,dyとZ軸回りの回転成分dθとで(dx,dy,dθ)と表現する。視点の拘束を行うために定位誤差,計測誤差を次のように定義する。先ずロボット3がある地点(x,y,θ)で自己定位した結果が(x′,y′,θ′)であったとき、各成分の定位誤差δx,δy,δθを次の(1)式で定義する。
そして、ロボット3がある地点から(dx,dy,dθ)だけ移動してロータリエンコーダから求まる移動量が(dx′,dy′,dθ′)であったとき、各成分の計測誤差Δx,Δy,Δθを次の(2)式で定義する。
第7図に示すようにロボット3が地点P(xp,yp,θp)で自己定位した後に(dx,dy,dθ)だけ移動し、地点Q(xq,yq,θq)へ到達したとする。地点Pでの自己定位結果とロータリエンコーダから求まる移動量(dx′,dy′,dθ′)により、ロボットは次の(3)式に示す範囲に存在することが、(1),(2)式より導かれる。
ここで、
である。
ここにおいて、ロボットは移動前の自己定位結果(xp′,yp′,θp′)と移動量(dx′,dy′,dθ′)を常時把握しているため、真の現在地(xq,yq,θq)を(3)式より制限すること、即ち視点がどの範囲にあるかを予測することができ、これを視点の拘束という。
次に、ウインドウ生成のアルゴリズムについて説明する。
先ず視点の位置,姿勢を、移動前の自己定位の結果及び移動量の計測値から前記(3)式を用いて拘束する。視点の位置,姿勢を第8図(A)の領域VAに示すように拘束した後に領域内を離散化し(実際には、例えば円VCに外接する正方形VAをθRも用いて求め、その正方形内の各点を周辺も含めて離散化する。この離散化される点の数は一定としているので正方形の大きさが変化すれば、各点のピッチが変わることになる。)、離散化した各地点で視点VPを設定し、その視点VPで環境がどのように見えるかを計算した予測画像をコンピュータグラフィックスの手法を用いて同図(B)のように各視点VP毎に生成する。なお、第8図(A)のWは世界(絶対)座標系を示し、Rはロボット座標系を示している。次に、各視点VPにおける予測画像中のマーカ(たとえば垂直エッジ)に注目してウインドウを形成する。環境モデルファイル中のマーカをL,予測画像上でのマーカをL′とすると、第8図(B)の予測画像例ではマーカL11′…L14′…L1n′とマーカL21′…L24′…L2n′とが存在し、マーカL11′…L14′…L1n′によって形成されるウインドウW1は同図(C)のようになる。マーカL21′…L24′…L2n′についても同様のウインドウW2(図示せず)を形成する。各マーカのウインドウが重なった場合には、第9図9図に示す如くそれらウインドウを統合した後区間を細分化して処理する。第9図はマーカL1′によって形成されるウインドウW1と、マーカL2′によって形成されるウインドウW2と、マーカL3′によって形成されるウインドウW3と、マーカL4′によって形成されるウインドウW4と、マーカL5′によって形成されるウインドウW5とが重なり、1つのウインドウWを形成する様子を示している。ウインドウから抽出されたマーカには、環境モデル中の複数個のマーカを第10図(A)の如く対応表で区間S1〜S5で対応づける。そして、実画像からのマーカの抽出順序を決めるためにウインドウのソーティングを行なう。ソーティング条件は高優先順位順に、イ)しきい値以上の大きさのマーカ、ロ)視点の位置,姿勢の変化に対して見え方の変化が大きいマーカ、ハ)距離が近いマーカ、ニ)画像の端にあるマーカとなる。
次にマーカの抽出について説明すると、生成したウインドウ個々に対して、マーカの抽出個数が所定の値になるまで下記の処理を行なう。
先ず第11図(A)の如くスムージングによってウインドウWi内の個々の画素に対してエッジ保存用平滑フィルタを2回作用させ、ウインドウWi内の個々の画素に対してSobel変換(エッジ強調処理)を行ない、Sobel変換のエッジ強度及びエッジ方向から同図(B)及び(C)の如く2値化を行なう。2値化の条件は、エッジ方向の場合には同図(C)のようにエッジ方向がある範囲以内(たとえば90゜±10゜)を“1"とし、エッジ強度の場合は同図(B)のようにエッジ強度がしきい値以上である強度を“1"とする。このようにして第11図(D)の如く2値化されたウインドウWi内の個々の画素に対して同図(E)のようなHough変換を行ない、ピーク点PPより同図(F)で示すような画像上での直線の方程式を求めてマーカMを抽出する。第10図(A)のような対応付リストをもとに、抽出したマーカを環境モデル中のマーカに同図(1)〜(4)のように対応づける。
一方、ロボットの位置,姿勢は、第12図に示す実画像から抽出したマーカA′,B′,C′より第13図に示す如くAB,BC間の角度α,βを計算し、次の(4)式を解くことで求まる。これに関しては後述する。
上述のように計算された位置,姿勢が正しいかどうかは、その位置,姿勢においてコンピュータグラフィックス画像を作成し、コンピュータグラフィックス画像上のマーカと入力画像から抽出したマーカとが一致するかどうかを調べれば分る。位置,姿勢の計算に用いたマーカA′,B′,C′は、カメラパラメータ、マーカ抽出時の誤差を持っているため、計算された位置,姿勢は第14図に示す斜線領域LAとなる。その時、検証されるマーカDが入力画像上に現れる範囲を計算することができる。よって、マーカDの検証はその範囲内でマーカが検出されているかどうかを調べればよい。ここで、マーカA′,B′,C′の抽出誤差は予め設定できないため、マーカA′,B′,C′の抽出誤差はマーカD′の検証が成功するか、又は一定の大きさになるまで1画素ずつ広げて行なう。すなわち、第14図に示すロボットの存在領域(誤差範囲)LAが広がって行き、かつマーカDの検証範囲も広がって行くことになる。
このようにして抽出したマーカからロボットの位置,姿勢の計算に用いる3個のマーカの組合せの全てについて上記検証を行ない、その中で誤差範囲を最小とする位置,姿勢を自己定位の結果とする。
第15図はこの発明の動作例を示しており、第1図に示す移動ロボット3は第5図に示す如く予め定められている出発地SPから目的地GPまでのルート及びサブゴール(P1,P2,…)に従って、障害物を避けながら移動し(ステップS1)、サブゴール(P1,P2,…)に来たか否かを常時判定しており(ステップS2)、サブゴールに達したときに一旦停止する(ステップS3)。この停止したサブゴールPiでは第16図に示すように、前回のサブゴールP(i-1)での誤差r(i-1),θ(i-1)に今回の移動によるロータリエンコーダの誤差eiを加えた領域内の各点から見えるであろう画像をコンピュータグラフィックス手法により作成し、環境モデルのマーカの存在する画像上の範囲であるウインドウWiを決定する(ステップS4)。次に、実際の画像において上述の如く求めたマーカ存在領域内からのみマーカが存在するか否かを判断し、マーカが存在すれば抽出する(ステップS5)。そして、抽出された3個以上のマーカが環境モデルのマーカのどれと対応するかを逐一調べて最終的な位置及び姿勢を決定し、同時にそのときの誤差ri,θiを記憶して次のサブゴールP(i-1)に備える(ステップS6)。上記動作を目的地GPまで繰返すことによってルート上を移動する(ステップS7)。
次に、第17図のフローチャートを参照して自己定位の方法を説明する。つまり、第15図のステップS4〜S6の詳細を説明する。
先ず、前回サブゴールP(i-1)の誤差r(i-1),θ(i-1)と今回サブゴールPiのロータリエンコーダの誤差eiの2つにより視点の領域を第16図のVAの如く決定し(ステップS10)、領域VA内を第8図(A)に示すように離散化して各視点VPを決める(ステップS11)。上記各視点VPについてその視点から見えるであろう予測画像を第8図(B)のようにコンピュータグラフィクスの手法により生成し、環境モデルのマーカの存在範囲であるウインドウWiを決める(ステップS12)。このとき姿勢誤差θ(i-1)も考慮し、次にマーカの存在範囲であるウインドウが重なり合うか否かを判定し(ステップS13)、重なり合う場合には各ウインドウを統合し(ステップS14)、第10図に示すようなマーカの候補リストを作成する(ステップS15)。次に、実際の画像(入力画像)より上記求めたウインドウ範囲からのみマーカを見つけて抽出し(ステップS16)、抽出したマーカが3個以上有るか否かを判断し(ステップS17)、2個以下の場合には処理不能となって停止する(ステップS18)。
上記ステップS17において抽出したマーカが3個以上有る場合には、抽出した全マーカより3個のマーカを選び(ステップS20)、当該3個のマーカを第10図(1)の最上部(L1′,L2′,L4′)のように環境モデルの候補リストのマーカと一旦対応づけ(ステップS21)、3個のマーカの抽出誤差を第14図の領域LAのように仮定する(ステップS22)。そして、次に第18図に示す如く自己定位の誤差範囲Q1〜Q8を計算する(ステップS23)。第18図における誤差範囲LAの9点Q0〜Q8は
で表わされ、1≦k≦8として誤差ri,θiは次のように求められる。
そして、誤差範囲LA内において検証を行なうマーカが入力画像中に存在する領域をコンピュータグラフィックスの手法を用いて計算する(ステップS24)。計算された領域内に検証するマーカが存在するか否かを調べ(ステップS25)、次に検証率がある割合以上(例えば80%以上)か否かを判定し(ステップS30)、ある割合よりも小さい場合には抽出誤差Deの画素を広げ(ステップS34)、Deが所定値(例えば3)よりも小さければステップS23にリターンする(ステップS35)。また、検証率がある割合以上の場合には誤差範囲の面積を計算し(ステップS31)、面積が最小であるか否かを判定し(ステップS32)、最小であれば面積及び誤差ri,θiを更新する(ステップS33)。第19図(A)はDe=1でウインドウw1にマーカD′が重ならないので検証失敗となる例を示し、同図(B)もDe=2でウインドウw2にマーカD′が重らないので検証失敗となる例を示し、同図(C)はDe=3でウインドウw3にマーカD′が重なり検証が成功した例を示している。この場合、検証率は100%となる。即ち、検証すべき領域が1つしかないときは0%か100%のいずれかとなる。そして、候補リストのマーカと全て対応づけが終了したか否かを判定し(ステップS36)、終了しておれば更に3個のマーカの選び方が全て終了したか否かを判定する(ステップS37)。ステップS36で全て対応づけが終了していない場合はステップS21にリターンする。
次に、第14図及び第18図で説明した自己定位の誤差範囲について説明すると、環境モデル20とイメージセンサ10との実際の関係は第20図のようになっているが、上下左右が逆になるので反対側位置10Aにイメージセンサ10があると考えて第21図のような構成を考える。その座標関係は第22図のようになり、Ocはレンズ中心座標を示している。そして、角度α,βの求め方を以下に説明する。イメージセンサ10上の入力画像A′,B′,C′は環境モデル20のA,B,Cと対応づけられる。画像A′はイメージセンサ10の何画素目に写っているかは分るので、第23図に示すような画像A′と光軸との距離l1は求まり(画素間隔は既知)、またレンズの焦点距離fは既知である。従って、α1=tan-1(l1/f)で角度α1が求まり、同様に角度α2も求まり、結局α=α1−α2で角度αが求まる。同様にして角度βも求まる。一方、第14図及び第24図に示すように3点A,B,Ocを通る円の中心をK1とすると、∠RAB=90゜で、線分▲
次に、姿勢θ(カメラ光軸とXwのなす角)の求め方について述べる。第25図に示すように、線分
と光軸のなす角α1は既に求められている。一方、A点の座標は環境モデル20から分っており、点Ocの座標も求まっているので、線分
と軸Xwのなす各θ′は演算で求められ、姿勢θはθ=θ′−α1となる。
次に、第19図で説明したようにイメージセンサ10の標本化の誤差である1画素を考慮する場合について述べる。第26図に示すように画像A′,B′をそれぞれ1画素広げたときの角度αmaxは前回と同様にして求められ、第27図に示すように画像A′,B′をそれぞれ1画素狭めたときの角度αminも前回と同様に求められる。同様に、角度βmax,βminも求められる。従って、点Oc(Q0)と同様にして点Q1〜Q8の座標(X,Y,θ)が(5)式の如く求められる。
ステップS17において、マーカが4つ以上あるときには今回使用した3つ以外のマーカが入力画像中に存在する領域を計算する。これはQ0(X0,Y0,θ0)〜Q8(X8,Y8,θ8)の各点から見える予測画像をコンピュータグラフィックス手法で計算し、今回使用した3つ以外のマーカが存在するウインドウD′を求める。そして、入力画像のウインドウD′内に実際にマーカが有るかどうかを調べ、検証率80%以上ならばOKとする。領域が複数あるとき、例えば5つの領域があってその内の4つ以上の領域でそれぞれ該当するマーカが有ったときは、4/5×100=80%となりOKとする。次に、点Q1〜Q8で囲まれる斜線部面積を計算し、最小ならその面積及び誤差ri,θiを記憶して更新する。もし検証率が80%未満であれば、第26図に示すようにイメージセンサの画素をもう1つ広げてαmax,βmaxを求め(α,βより2画素分広げたことになる)、同様に第27図に示すようにもう1画素減らしてαmin,βmin(α,βより2画素分狭めたことになる)を求めて、以下同様な処理を行なう。次に対応づけの組合せを変えて同様に行ない、全ての対応づけの組合せを終了すれば、次の組合せの3つのマーカを選んで同様の処理を行ない、結局Q1〜Q8で成る領域面積が最小となるもので自己定位することになる。そのときの誤差ri,θiが次回の誤差の基準となる。
発明の効果;
以上のようにこの発明によれば、マーカが存在しそうな領域について画像処理を行なうので効率が良く、領域を区切っているので画像からのマーカの抽出が安定する。また、余分な線を抽出するのを防いでいるので、抽出したマーカと環境モデル中のマーカとの対応づけの効率が良い。
【図面の簡単な説明】
第1図はこの発明の移動ロボットの外観構成図、第2図は撮像手段による環境の入力画像の一例を示す図、第3図R>図はロボットの座標系を説明するための図、第4図はロボットが移動する環境の一例を示す図、第5図は第4図の環境に対するルート生成の様子を示す図、第6図は自己定位を説明するための模式図、第7図は視点の拘束を説明するための図、第8図は視点領域の離散化及びウインドウの生成を説明するための図、第9図はウインドウの合成を説明するための図、第10図は抽出したマーカの対応づけを示す図、第11図はウインドウよりマーカの抽出を行なう様子を示すフロー図、第12図は入力画像中のマーカの例を示す図、第13図はロボットの位置,姿勢の計算原理を説明するための図、第14図は自己定位の誤差を説明するための図、第15図はこの発明のロボット移動の様子を示すフローチャート、第16図はサブゴールで自己停止しながら移動する様子を示す図、第17図はこの発明による自己定位の動作例を示すフローチャート、第18図はロボット位置,姿勢の誤差範囲を説明するための図、第19図は位置,姿勢についての検証の例を示す図、第20図〜第25図は位置,姿勢の誤差範囲の計算を説明するための図、第26図及び第27図は画像入力範囲の変更を説明するための図である。
1……CCDカメラ、2……車輪、3……移動ロボット、10……イメージセンサ、20……環境モデル。
【特許請求の範囲】
【請求項1】予め障害物の位置を記憶させた環境モデルファイルを用意し、移動体が所定距離進んだとき、前記移動体に設けられた移動量検出手段の出力によりその地点を推定し、その推定地点からある誤差範囲の領域を設定し、その領域内の各点から前記移動体に設けられた撮像手段が捉えるであろう予測画像を前記環境モデルファイルからコンピュータグラフィックス手法を用いて各々生成し、生成された各予測画像上で前記環境モデルの障害物の特徴要素であるマーカを抽出しマーカの存在する領域を求め、この求められた領域を前記撮像手段からの入力画像中に当てはめてその領域からのみ実際のマーカの抽出を行ない、前記環境モデルのマーカと抽出されたマーカとを対応づけて自己定位を行なうようにしたことを特徴とする移動体の自己定位方法。
【請求項1】予め障害物の位置を記憶させた環境モデルファイルを用意し、移動体が所定距離進んだとき、前記移動体に設けられた移動量検出手段の出力によりその地点を推定し、その推定地点からある誤差範囲の領域を設定し、その領域内の各点から前記移動体に設けられた撮像手段が捉えるであろう予測画像を前記環境モデルファイルからコンピュータグラフィックス手法を用いて各々生成し、生成された各予測画像上で前記環境モデルの障害物の特徴要素であるマーカを抽出しマーカの存在する領域を求め、この求められた領域を前記撮像手段からの入力画像中に当てはめてその領域からのみ実際のマーカの抽出を行ない、前記環境モデルのマーカと抽出されたマーカとを対応づけて自己定位を行なうようにしたことを特徴とする移動体の自己定位方法。
【第1図】
【第2図】
【第3図】
【第4図】
【第5図】
【第6図】
【第7図】
【第9図】
【第10図】
【第12図】
【第18図】
【第23図】
【第8図】
【第13図】
【第16図】
【第20図】
【第21図】
【第24図】
【第26図】
【第27図】
【第11図】
【第14図】
【第15図】
【第17図】
【第19図】
【第22図】
【第25図】
【第2図】
【第3図】
【第4図】
【第5図】
【第6図】
【第7図】
【第9図】
【第10図】
【第12図】
【第18図】
【第23図】
【第8図】
【第13図】
【第16図】
【第20図】
【第21図】
【第24図】
【第26図】
【第27図】
【第11図】
【第14図】
【第15図】
【第17図】
【第19図】
【第22図】
【第25図】
【特許番号】第2839281号
【登録日】平成10年(1998)10月16日
【発行日】平成10年(1998)12月16日
【国際特許分類】
【出願番号】特願平1−81029
【出願日】平成1年(1989)3月31日
【公開番号】特開平2−259912
【公開日】平成2年(1990)10月22日
【審査請求日】平成8年(1996)3月1日
【新規性喪失の例外の表示】特許法第30条第1項適用申請有り 「自立移動ロボットの自己定位」日本ロボット学会第6回学術講演会予稿集 第531〜534頁,昭和63年10月20日発行
【出願人】(999999999)グローリー工業株式会社
【参考文献】
【文献】特開 昭60−217413(JP,A)
【登録日】平成10年(1998)10月16日
【発行日】平成10年(1998)12月16日
【国際特許分類】
【出願日】平成1年(1989)3月31日
【公開番号】特開平2−259912
【公開日】平成2年(1990)10月22日
【審査請求日】平成8年(1996)3月1日
【新規性喪失の例外の表示】特許法第30条第1項適用申請有り 「自立移動ロボットの自己定位」日本ロボット学会第6回学術講演会予稿集 第531〜534頁,昭和63年10月20日発行
【出願人】(999999999)グローリー工業株式会社
【参考文献】
【文献】特開 昭60−217413(JP,A)
[ Back to top ]