JP4753103B2 - 移動体位置の推定装置と推定方法及び推定プログラム - Google Patents

Description

この発明は、ロボット等の移動体の位置を推定するための装置と推定方法、推定プログラムに関する。

ロボットなどの移動体の制御では、その自己位置を正確に推定することが重要になる。そこで移動体に視覚センサとして全方位カメラや魚眼レンズカメラなどを搭載すると共に、カメラで見えるべきランドマークのマップを記憶し、カメラからの画像でマップを参照して自己位置を推定することが知られている。例えば非特許文献１では、カメラ画像からランドマークの候補を３個ランダムに抽出し、マップからもランダムにランドマークを３個抽出し、これらを１：１に対応させて、自己位置の候補とする。次にこの候補位置が正しいものとして、マップの他のランドマークに付いて推定した自己位置からの視野角を求め、カメラ画像の他のランドマークの候補の視野角との誤差を求める。そしてこの誤差が最小となる、ランドマークの候補とマップとの対応付けが正しいものとして、これに基づく自己位置を出力する。誤差の評価には例えば、カメラ画像のランドマークの候補とマップ上のランドマークとの視野角の誤差を２乗し、その中央値を用いる。なお中央値に限らず、誤差の積算値などを用いても良い。

しかしながら建屋内などの人工的な環境では、マップの作成後に新たに物品が持ち込まれることによりマップ上のランドマークが見えなくなる、あるいはランドマークと紛らわしい物が持ち込まれる、照明によりランドマークが見えにくくなるなどのことがある。これらによって、マップ上のランドマークが消失したり、あるいはカメラ画像からランドマークの候補以外のもの、言い換えると偽のランドマークを抽出することがある。ランドマークの消失や偽のランドマークによって、自己位置の推定は飛躍的に複雑になる。そこで確実にカメラ画像から認識できて、他の物と混同することがないマークを探索する必要がある。この点について、特許文献１（特開２００４−３４２７２号公報）は天井の蛍光灯をランドマークとすることを提案している。しかしながら広い室内で蛍光灯が多数ある場合、どの蛍光灯も同じ形状でかつ規則的に配置されているため、どの蛍光灯がカメラ画像で見えているのかの認識は容易ではない。
全方位カメラとデッドレコニング機能を有するサッカーロボットのロバストな自己位置同定手法，日本ロボット学会誌，Vol.22，No.3, pp.343-352,2004,4 特開２００４−３４２７２号公報

この発明の課題は、マップの作成後にランドマークの一部が隠され、あるいはランドマークと紛らわしいものが発生しても、移動体の位置を正確に推定できるようにすることにある。
この発明での追加の課題は、床と壁側との切れ目に広角カメラ画像で見えない箇所がある場合も、移動体の位置を推定できるようにすることにある。

この発明の移動体位置の推定装置は、 広角カメラとランドマークのマップを備え、広角カメラ画像から求めたランドマークをマップ上のランドマークと照合して移動体の自己位置を推定する装置において、前記マップに、床と床に垂直なオブジェクトとの間の切れ目を記憶すると共に、推定した自己位置の妥当性を評価するために、推定した自己位置に基づいてマップに記憶した切れ目を広角カメラ画像に投影した切れ目と、広角カメラ画像での切れ目、との誤差を評価するためのマッチング手段を設けると共に、前記マッチング手段では、前記投影した切れ目に対応する、広角カメラ画像での切れ目が無い際に、移動体の内界センサで求めた自己位置に基づいてマップに記憶した切れ目を広角カメラ画像に投影した切れ目と、前記推定した自己位置に基づいて投影した切れ目、との誤差を評価することを特徴とする。

この発明の移動体位置の推定方法は、広角カメラとランドマークのマップを用い、広角カメラ画像から求めたランドマークをマップ上のランドマークと照合して移動体の自己位置を推定する方法において、前記マップに床と床に垂直なオブジェクトとの間の切れ目を記憶し、推定した自己位置の妥当性を評価するために、推定した自己位置に基づいてマップに記憶した切れ目を広角カメラ画像に投影した切れ目と、広角カメラ画像での切れ目、との誤差を評価することにより、推定した自己位置の妥当性を評価し、かつ前記投影した切れ目に対応する、広角カメラ画像での切れ目が無い際に、移動体の内界センサで求めた自己位置に基づいて、マップに記憶した切れ目を広角カメラ画像に投影した切れ目と、前記推定した自己位置に基づいて投影した切れ目、との誤差を評価することを特徴とする。

この発明の移動体位置の推定プログラムは、広角カメラからの画像とランドマークのマップとを参照し、広角カメラ画像から求めたランドマークをマップ上のランドマークと照合して移動体の自己位置を推定する、移動体位置の推定装置のためのプログラムにおいて、 前記推定装置を、広角カメラ画像からランドマークを抽出し、マップ上のランドマークと照合して移動体の自己位置を推定するための手段と、前記マップに記憶した床と床に垂直なオブジェクトとの間の切れ目を読み出すための手段と、推定した自己位置の妥当性を評価するために、推定した自己位置に基づいてマップに記憶した切れ目を広角カメラ画像に投影した切れ目と、広角カメラ画像での切れ目、との誤差を評価し、かつ前記投影した切れ目に対応する、広角カメラ画像での切れ目が無い際に、移動体の内界センサで求めた自己位置に基づいて、マップに記憶した切れ目を広角カメラ画像に投影した切れ目と、前記推定した自己位置に基づいて投影した切れ目、との誤差を評価するためのマッチング手段、として機能させるものである。

この発明では、前記マッチングでは、前記投影した切れ目に対応する、広角カメラ画像での切れ目が無い際に、移動体の内界センサで求めた自己位置に基づいてマップに記憶した切れ目を広角カメラ画像に投影した切れ目と、前記推定した自己位置に基づいて投影した切れ目、との誤差を評価する。

なおこの明細書において、移動体位置の推定に関する記載は、推定装置や推定方法、推定プログラムのいずれにも当てはまり、特に推定装置に関する記載は推定方法や推定プログラムにも当てはまり、推定方法に関する記載は推定装置や推定プログラムにも当てはまる。

この発明では、広角カメラ画像から抽出したランドマークをマップ上のランドマークと照合して、自己位置を推定する。ここに少なくとも３個のランドマークを広角カメラ画像とマップとから取り出して１：１に対応させると、自己位置の候補が求まる。推定した自己位置は、マップや広角カメラ画像から取り出すランドマークの組み合わせと、ランドマークの対応関係に依存し、これらが変わると別の自己位置が推定される。そこでランドマークの数が多い場合、自己位置の候補は極めて多数発生する。ランドマークは例えば床面に垂直なパターンであり、これは壁や壁に沿って配した家具、ロッカー、窓、ドアなどのオブジェクトのエッジなどである。しかしながらマップの作成後に別のオブジェクトが持ち込まれると、ランドマークが隠され、あるいはマップにないランドマークが発生することがある。ランドマークが多数あることと、一部のランドマークが見えず、また偽のランドマークが発生することにより、正しい自己位置を推定するまでの計算量が激増し、また誤った自己位置を出力する可能性が増している。

床と壁の境界などでは広角画像に切れ目が生じる。床に垂直なランドマークは広角カメラ画像では例えば半径方向の線分となるのに対し、切れ目は周方向のカーブや線分となる。床と壁の境界以外の箇所でも床に垂直なオブジェクトを置けば切れ目は発生するから、切れ目は多数存在し、しかもマップにない切れ目も存在し得る。そこで広角カメラ画像上の切れ目に対応する切れ目をマップに求めることには限界がある。しかし自己位置を推定した場合、推定した自己位置に基づいて、マップ上の切れ目が広角カメラ画像上でどのような切れ目に見えるはずかを求めることはできる。これをマップ上の切れ目を広角カメラ画像に投影すると言うと、投影した切れ目と広角カメラ画像での切れ目とをマッチングし、その誤差を評価することにより、自己位置の推定の妥当性を検証できる。そして自己位置の推定の妥当性を検証できれば、正しい自己位置を推定するまでの計算量を減らし、また見えないランドマークや偽のランドマークによる誤推定を減らすことができる。

自己位置推定の妥当性の検証はランドマークが多数ある場合でも簡単で、ランドマークの一部が見えなくなったり、あるいはランドマークと紛らわしいオブジェクトが持ち込まれたりした場合も実行できる。また移動体の周囲に、人間や他の移動体などのマップに位置を記載することが困難なものが発生しても、自己位置を推定できる。

照明の具合や壁の付近に持ち込まれたオブジェクトなどにより、床と壁側などの切れ目が広角カメラ画像では見えなくなることがある。ところで一般に移動体は、走行車輪や足、走行モータの回転軸、ステアリング装置などに設けた内界センサにより、自己位置を推定している。内界センサにより求めた自己位置は精度が低いが、切れ目が広角カメラ画像から消失し、マップから投影した切れ目とのマッチングができない箇所で、広角カメラ画像の切れ目の代わりに、内界センサで求めた自己位置に基づいてマップ上の切れ目を広角カメラ画像に投影した切れ目を用いることができる。これによって、切れ目が見えにくいエリアで誤差が発散するのを防止でき、内界センサによる自己位置の信頼性に依存しながら、推定した自己位置の妥当性を評価できる。

実施例のロボットの自己位置認識部と走行系とを示すブロック図 実施例で用いた全方位カメラの構成を示す図 全方位カメラ画像での、ランドマークＭ1〜Ｍ3と床と壁の切れ目Ｎ1〜Ｎ3とを示す図 実施例の位置推定部のブロック図 実施例の自己位置推定方法のアルゴリズムを示すフローチャート 実施例の自己位置推定プログラムのブロック図 マップからの切れ目の投影（実線）とカメラ画像での切れ目（破線）及びデッドレコニング位置に基づく切れ目の投影（鎖線）とを示す図

符号の説明

２ 駆動輪 ３ キャスター車輪 ４ 走行モータ ５ ギアヘッド
７ エンコーダ ８ 走行指令発生部 ９ デッドレコニング部
１０ 位置記憶部 １２ マップ １３ ミラー １４ カメラ
１５ カメラ画像記憶部 １６ 位置推定部
２０ ランドマークマッチング部 ２２ ランドマーク誤差評価部
２４ 切れ目投影部 ２６ 切れ目投影部
２８ 切れ目マッチング部 ３０ 位置推定部
６０ 自己位置推定プログラム ６２ ランドマークマッチング命令
６３ 自己位置候補算出命令 ６４ ランドマーク誤差評価命令
６５，６６ 切れ目投影命令 ６７ 切れ目マッチング命令
６８ 自己位置推定命令 ７０ マップでの切れ目
７２ カメラ画像での切れ目 ７４ デッドレコニングからの切れ目
Ｍ ランドマーク Ｎ 切れ目

以下に本発明を実施するための最適実施例を示す。

図１〜図７に、移動体の自己位置の推定に関する実施例を示す。図１は実施例の自己位置の推定装置を組み込んだロボットの走行系を示し、２，２は一対の駆動輪で、走行モータ４とギアヘッド５とにより回転し、モータ４の回転数をエンコーダ７で監視し後述のデッドレコニング部９へ入力する。またデッドレコニング部９は、ギアヘッド５のギアの減速比や正転／逆転の種類も入力される。なお３，３は一対のキャスター車輪である。走行指令発生部８は、走行モータ４，４やギアヘッド５，５部を各々独立に制御することにより、駆動輪２，２の回転量と回転方向を独立に制御し、目標位置へロボットを移動させる。なお歩行型ロボットの場合、駆動輪２やキャスター車輪３に代えて関節の組み合わせからなる足を用いればよい。移動体の種類は、ロボットの他に、搬送台車や移載装置などでも良く、移動の機構自体は任意である。

エンコーダ７，７は内界センサの例で、それらの信号をデッドレコニング部９へ入力して移動距離を積算することにより、自己位置を求める。なお内界センサの信号で自己位置を求めることをデッドレコニングと言う。求めた位置は位置記憶部１０に記憶され、位置推定部１６でより正確な位置が推定される毎に更新され、位置記憶部１０のデータに基づいて走行指令発生部８は走行指令を発生する。

マップ１２はロボットなどの移動体が移動可能な範囲のマップであり、移動可能範囲は室内や建屋内、あるいは屋外などである。実施例では床面に垂直なエッジをランドマークとし、ランドマークの位置と、床と壁との境界などの水平面と鉛直面との間の切れ目をマップに１２に記憶している。床面に垂直なエッジは例えば家具やロッカーなどの縁、ドアや窓などの鉛直方向の縁である。ランドマークの位置は(Ｘ，Ｙ)の水平面内位置で表すが、これにＺ方向（鉛直方向）座標を加えて、ランドマークの起点のＺ軸方向座標Ｚ0とランドマークの終点のＺ軸方向座標Ｚ1とを追加しても良い。切れ目は床と壁との間の切れ目に限らず、床面に対してオブジェクトが立ち上がる位置、即ち床面の端部で切れ目が発生する。また歩道と車道の境界や歩道と花壇との境界、歩道と側溝の境界でも切れ目発生する。マップ１２上の切れ目はカーブや線分で表され、高さが床面レベルの切れ目のみを用いても良く、より高い位置の切れ目、例えば床面から立ち上がった段差の上面と側面とのエッジも用いても良い。

マップ１２の作成は例えばマニュアルで行い、あるいは室内や建屋内もしくは建物の玄関付近などの移動体の移動空間に対し、既知の位置で撮像した広角カメラ画像を画像認識してマップ１２を作成しても良い。１３はミラー、１４はカメラで、これらを合わせて広角カメラという。広角カメラの出力画像は、カメラ画像記憶部１５に記憶され、位置推定部１６では位置記憶部１０のデータとマップ１２並びにカメラ画像とを用いて、移動体の自己位置を推定する。自己位置のデータは、例えば原点に対する(Ｘ，Ｙ)座標と移動体の向きの３成分である。

図２は床面に垂直なランドマークＭをカメラ１４で撮像している状態を示し、鉛直な線分からなるランドマークＭでの高さ位置が異なると、カメラ１４の視野中心から半径方向に沿った線分上に異なった位置で光が入射する。図３に、３つのランドマークＭ1〜Ｍ3と３つの切れ目Ｎ1〜Ｎ3の、広角カメラ画像での表れ方を模式的に示す。ｒは半径方向を、θは向きを表し、床面に垂直なランドマークＭ1〜Ｍ3では、向きは一定で半径方向に延びる線分が得られる。そして広角カメラ画像上で半径方向に延びるランドマークＭ1〜Ｍ3を検出できると、そのＸ，Ｙ座標が求まることは公知である。また切れ目Ｎ1〜Ｎ3は、例えば半径位置がほぼ一定の周方向のカーブとして表れる。

図４に位置推定部１６の構造を示す。ランドマークマッチング部２０はカメラ画像から抽出した３個のランドマークと、マップ１２から抽出した３個のランドマークとを１：１に対応させ、この対応に基づいて自己位置を推定する。推定した自己位置は、仮定したランドマーク間の対応関係に基づくものである。ランドマーク誤差評価部２２は自己位置の推定に用いなかったランドマークを用いて、推定した自己位置に基づく誤差を評価する。即ち自己位置を推定すると、マップ１２での他のランドマークが広角カメラ画像でどの向きに見えるかを推定でき、実際のカメラ画像との誤差を評価できる。なおランドマーク誤差評価部２２は設けなくても良い。

切れ目投影部２４はランドマークのマッチングで推定した自己位置に基づき、マップ１２上の切れ目を広角カメラ画像へと投影する。自己位置を推定すると、これに基づいてマップ１２上の切れ目が広角カメラ画像でどのように見えるかを推定でき、このことを切れ目の投影という。切れ目投影部２６は位置記憶部１０に記憶した位置、言い換えるとエンコーダ７を用いてデッドレコニングにより求めた自己位置に基づき、マップ１２上の切れ目が広角カメラ画像上でどのように見えるかを投影する。切れ目投影部２６は設けなくても良い。

切れ目マッチング部２８は切れ目投影部２４で投影した切れ目と、広角カメラ画像上の切れ目とをマッチングする。なお投影した切れ目から広角カメラ画像上で所定の距離内にカメラ画像上の切れ目が存在しない場合、投影部２４で投影した切れ目と、投影部２６で投影した切れ目とをマッチングし、言い換えると２つの切れ目間の距離を算出する。この距離は、切れ目の各部での誤差の加算値、切れ目の各部での誤差を適宜の関数で変換した物の和などとする。位置推定部３０は求めた誤差に基づいて、自己位置の推定値の妥当性を検証し、誤差が所定値未満、もしくは誤差が充分小さい推定位置を自己位置として出力する。これ以外の場合、マッチング部２０でマッチングする広角カメラ画像側のランドマークとマップ側のランドマークの組み合わせを変更し、マッチング部２８までの処理を繰り返す。

図５に自己位置の推定アルゴリズムを示す。全方位カメラ画像を入力し、カメラ画像からランドマークの候補を例えば３個抽出する。抽出はランダムに行っても良く、あるいは位置記憶部１０のデータに基づき、自己の現在位置から見えるはずのランドマークを抽出しても良い。マップから同様にランドマークの候補を３個抽出し、１：１に対応付ける。この対応はランダムでも良く、あるいは位置記憶部１０のデータ上でもっともらしい対応を優先しても良い。広角カメラ画像とマップとで３個のランドマークを対応付けると、自己位置の候補が１つ求まる。そして例えば、他の候補と他のランドマークとを用いて自己位置の推定誤差を評価する。なおこのステップは省略しても良い。

次に求めた自己位置の候補に基づき、マップ上の切れ目を広角カメラ画像に投影する。なお床面に垂直なランドマークはその床面上の１点のデータを与えるが、切れ目は床面上の直線やカーブのデータを与え、より情報量が豊富である。従って切れ目のマッチングにより、誤差をより正確に評価できる。次にデッドレコニングで求めた自己位置に基づき、マップ上の切れ目を広角カメラ画像に投影する。この処理は省略しても良く、また実行する場合でも、自己位置の推定値に基づいて投影した切れ目と、広角カメラ画像とが対応しないエリアのみに対して、実行しても良い。

次にマップ上の切れ目を推定した自己位置に基づいて投影したものと、広角カメラ画像の切れ目との誤差を評価する。自己位置の推定値に基づいて投影した切れ目に対応するものが、広角カメラ画像に無いエリアでは、デッドレコニングにより求めた位置に基づいてマップ上の切れ目を投影したものと、ランドマークにより推定した自己位置によりマップ上の切れ目を投影したものとを比較する。そしてこれらの誤差を求め、許容範囲内の誤差であれば、推定位置を正しい自己位置として出力し、位置記憶部１０のデータを更新する。

図６に自己位置推定プログラム６０を示すと、ランドマークマッチング命令６２は広角カメラ画像のランドマークの候補と、マップ上のランドマークとを例えば３個ずつ抽出して仮に対応付ける。自己位置候補算出命令６３は上記の対応に基づいて、自己位置の候補を算出する。ランドマーク誤差評価命令６４は、算出した自己位置に基づいて、マップの他のランドマークと広角カメラ画像での他のランドマークの候補との間の誤差を評価する。ランドマーク誤差評価命令６４は設けなくても良い。

切れ目投影命令６５は推定した自己位置の候補に基づき、マップ上の切れ目を広角カメラ画像へ投影する。切れ目投影命令６６はデッドレコニングにより求めた位置に基づき、マップ上の切れ目を広角カメラ画像へ投影し、命令６６は設けなくても良い。切れ目マッチング命令６７は切れ目間のマッチングを行い、誤差を評価する。自己位置推定命令６８は、切れ目マッチング命令６７で評価した切れ目のマッチング誤差に基づき、推定した自己位置の妥当性を評価し、妥当な場合にはその位置を出力し、妥当でない場合、命令６２〜６７の処理を繰り返す。なお切れ目は一般に複数有り、例えば室内であれば数本程度の切れ目が存在する。そこでマッチングは各切れ目について行い、誤差は個々の切れ目の誤差の加算値もしくは個々の切れ目の誤差を統計化した統計量などである。

図７に切れ目のマッチングの例を示す。７０は自己位置の推定とマップとに基づく切れ目で、ランドマークから推定した自己位置に基づき、マップ上の切れ目を広角カメラ画像へ投影したものである。７２はカメラ画像での切れ目で、切れ目の一部が他のオブジェクトで隠されて、あるいは照明の具合などにより一部分が失われている。デッドレコニングで求めた自己位置に基づき、マップ上の切れ目を投影したものを、デッドレコニングからの切れ目７４として示す。また図７の各矢印の距離を例えば積算したものが、切れ目のマッチング誤差である。

切れ目７０，７４の間の誤差は自己位置の推定誤差であり、デッドレコニングによる位置認識の信頼性が低いので、曖昧な誤差である。切れ目７０，７４は共にマップを元に投影したものであり、切れ目７０と切れ目７２が対応しないエリアでも、切れ目７０に対応する切れ目７４が存在する。そこで切れ目７０，７２が対応するエリアでは、図７の矢印の距離を加算して誤差を評価し、切れ目７０，７２が対応しないエリアでは、切れ目７０，７４間の誤差を加算して評価する。

なおマッチング誤差の評価は単純に誤差を積算しても良く、あるいは誤差を統計化して評価しても良い。また誤差が極端に大きいエリアにより、マッチング結果が左右されないように、誤差が大きくなると減衰する関数を定め、これを誤差に乗算してから加算しても良い。また誤差に上限を設けても良い。さらに切れ目７０，７４間の誤差は１よりも小さな重みを乗算して用いても良い。

実施例では以下の効果が得られる
(1) ランドマークの一部が隠れて見えない、もしくはランドマークと紛らわしい偽のマークが発生している場合にも、影響が小さい。
(2) 鉛直方向のランドマークはＸＹ平面での１点であるが、切れ目はＸＹ平面でのカーブや線分であり、より多くの情報量を備えている。そこで床の壁との切れ目を用いることにより、広角カメラ画像とマップとのランドマークの対応が正しいか否かを簡単に評価できる。
(3) 広角カメラ画像で切れ目の一部が失われている場合、デッドレコニングに基づいて投影した切れ目で補うことができる。

Claims (3)

1. 広角カメラとランドマークのマップを備え、広角カメラ画像から求めたランドマークをマップ上のランドマークと照合して移動体の自己位置を推定する装置において、
   前記マップに、床と床に垂直なオブジェクトとの間の切れ目を記憶すると共に、
   推定した自己位置の妥当性を評価するために、推定した自己位置に基づいてマップに記憶した切れ目を広角カメラ画像に投影した切れ目と、広角カメラ画像での切れ目、との誤差を評価するためのマッチング手段を設けると共に、
   前記マッチング手段では、前記投影した切れ目に対応する、広角カメラ画像での切れ目が無い際に、移動体の内界センサで求めた自己位置に基づいてマップに記憶した切れ目を広角カメラ画像に投影した切れ目と、前記推定した自己位置に基づいて投影した切れ目、との誤差を評価することを特徴とする、移動体位置の推定装置。
2. 広角カメラとランドマークのマップを用い、広角カメラ画像から求めたランドマークをマップ上のランドマークと照合して移動体の自己位置を推定する方法において、
   前記マップに床と床に垂直なオブジェクトとの間の切れ目を記憶し、
   推定した自己位置の妥当性を評価するために、推定した自己位置に基づいてマップに記憶した切れ目を広角カメラ画像に投影した切れ目と、広角カメラ画像での切れ目、との誤差を評価することにより、推定した自己位置の妥当性を評価し、
   かつ前記投影した切れ目に対応する、広角カメラ画像での切れ目が無い際に、移動体の内界センサで求めた自己位置に基づいて、マップに記憶した切れ目を広角カメラ画像に投影した切れ目と、前記推定した自己位置に基づいて投影した切れ目、との誤差を評価することを特徴とする、移動体位置の推定方法。
3. 広角カメラからの画像とランドマークのマップとを参照し、広角カメラ画像から求めたランドマークをマップ上のランドマークと照合して移動体の自己位置を推定する、移動体位置の推定装置のためのプログラムにおいて、
   前記推定装置を、
   広角カメラ画像からランドマークを抽出し、マップ上のランドマークと照合して移動体の自己位置を推定するための手段と、
   前記マップに記憶した床と床に垂直なオブジェクトとの間の切れ目を読み出すための手段と、
   推定した自己位置の妥当性を評価するために、推定した自己位置に基づいてマップに記憶した切れ目を広角カメラ画像に投影した切れ目と、広角カメラ画像での切れ目、との誤差を評価し、かつ前記投影した切れ目に対応する、広角カメラ画像での切れ目が無い際に、移動体の内界センサで求めた自己位置に基づいて、マップに記憶した切れ目を広角カメラ画像に投影した切れ目と、前記推定した自己位置に基づいて投影した切れ目、との誤差を評価するためのマッチング手段、として機能させるための、移動体位置の推定プログラム。

Images