JP2020017173A - 移動体 - Google Patents

Abstract

【課題】撮影画像を用いた位置推定が不適切である場合に、測距結果を用いた位置推定に切り替えても、位置精度が向上しないことがあるという問題があったが、複数の位置推定を用いて適切に現在位置を取得することができる移動体を提供する。【解決手段】移動体１は、測距センサである第１のセンサ１１と、イメージセンサである第２のセンサ１２と、測距結果を用いて第１の推定位置と第１の信頼度とを取得する第１の位置推定部１３と、撮影画像を用いて第２の推定位置と第２の信頼度とを取得する第２の位置推定部１４と、第１及び第２の推定位置と、第１及び第２の信頼度とを用いて、移動体１の現在位置を取得する現在位置取得部１５と、移動体１を移動させる移動機構１６と、現在位置を用いて移動機構１６を制御する移動制御部１７とを備える。構成により、第１及び第２の信頼度を比較し、精度の高い現在位置の取得を実現する。【選択図】図１

Description

本発明は、センサによって取得された情報を用いて現在位置を取得する移動体に関する。

従来、複数の教示画像と、撮影装置によって取得された実画像との比較結果に基づいた走行制御と、周囲の物体までの距離及び方向の検出結果に基づいた走行制御とを、所定の条件に応じて切り替える移動体が知られている（特許文献１参照）。

その特許文献１では、例えば、撮像画像と撮像用ランドマークのパターンマッチングに成功した特徴点の数があらかじめ決められた閾値以下となった場合や、認識した撮像用ランドマークの数が複数であり、どのランドマークの計測結果を使用すべきか判定できない場合、撮像した画像から求めた進行方向からの角度のずれ量があらかじめ決められた範囲を超えた場合に、画像の比較結果に基づいた走行制御から、距離等の検出結果に基づいた走行制御に切り替えられることが例示されている。したがって、特許文献１に記載された従来例では、２個の走行制御のうち、一方の走行制御に関する所定の条件が満たされた場合に、他方の走行制御に切り替えることになる。

特開２０１０−１４０２４６号公報

しかしながら、従来の移動体において、一方の走行制御に関する所定の条件が満たされた場合に、他方の走行制御に切り替えたとしても、切り替え後に適切な走行制御を行うことができるとは限らないという問題があった。例えば、撮像画像と撮像用ランドマークのパターンマッチングに成功した特徴点の数があらかじめ決められた閾値以下となった場合に、距離等の検出結果に基づいた走行制御に切り替えたとしても、周囲に計測対象となる物体の存在しない場所に移動体が位置しているときには、距離等の検出結果に基づいた適切な走行制御を行うことができなくなる。

本発明は、上記課題を解決するためになされたものであり、複数の位置推定を用いて適切に現在位置を取得することができる移動体を提供することを目的とする。

上記目的を達成するため、本発明による移動体は、自律的に移動する移動体であって、移動体の周囲の情報を取得する第１のセンサと、移動体の周囲の情報であって、第１のセンサによって取得される情報とは異なる情報を取得する第２のセンサと、第１のセンサによって取得された情報を用いて移動体の位置を推定することによって、第１の推定位置と、第１の推定位置に対応する第１の信頼度とを取得する第１の位置推定部と、第２のセンサによって取得された情報を用いて移動体の位置を推定することによって、第２の推定位置と、第２の推定位置に対応する第２の信頼度とを取得する第２の位置推定部と、第１及び第２の推定位置と、第１及び第２の信頼度とを用いて、移動体の現在位置を取得する現在位置取得部と、移動体を移動させる移動機構と、現在位置を用いて移動機構を制御する移動制御部と、を備えたものである。
このような構成により、第１及び第２の信頼度を用いて移動体の現在位置を取得できるため、例えば、両信頼度を比較して現在位置の取得を行うことができるようになる。その結果、例えば、第１の信頼度は高くないが、第２の信頼度は第１の信頼度よりもさらに低いような状況においては、第１の推定位置を用いて現在位置を取得するようにすることができるようになる。

また、本発明による移動体では、第１のセンサは、複数方向に関して周囲の物体までの距離を測定する測距センサであり、第１の位置推定部は、第１のセンサによって測定された距離、及び移動体の移動領域における障害物の位置を示す第１の地図を用いて第１の推定位置を取得し、第２のセンサは、移動体の周囲の撮影画像を取得するイメージセンサであり、第２の位置推定部は、第２のセンサによって取得された撮影画像において特定した特徴点、及び移動体の移動領域における特徴点の位置を示す第２の地図を用いて第２の推定位置を取得してもよい。
このような構成により、測距結果を用いた位置推定と撮影結果を用いた位置推定とを行うことができ、それらの推定結果を用いて現在位置を取得することができるようになる。通常、測距結果を用いた位置推定は、周囲の物体までの距離が遠い場合には精度が低くなるのに対して、周囲の明るさの変化にはロバストであるという特徴がある。一方、撮影結果を用いた位置推定は、周囲の物体までの距離が遠い場合でも精度の高い位置推定を行うことができるのに対して、周囲の明るさの変化には弱いという特徴がある。このように、両者は相補的であるため、両者を用いて位置推定を行うことは、より高い精度の現在位置の取得に有効である。

また、本発明による移動体では、第２の位置推定部は、特徴点ごとに、特徴点の適切さの程度を示す適切度を取得し、適切度を用いて、複数のブロックに分割された撮影画像のブロックごとにブロック信頼度を算出し、ブロック信頼度の高いブロックを用いて第２の推定位置を取得し、第２の推定位置の取得で用いたブロックのブロック信頼度を用いて第２の信頼度を取得してもよい。
このような構成により、撮影画像のうち、適切なブロックの箇所を用いて位置推定を行うことができ、撮影画像を用いた精度の高い位置推定を実現することができるようになる。上記のように、撮影結果を用いた位置推定は通常、明るさの変化に弱いが、明るさが変化したとしても、撮影画像のすべての領域を位置推定に用いることができないわけではない。したがって、このようにすることで、撮影画像に含まれる特徴点のうち、所定の条件を満たすブロックの特徴点を用いて位置推定を行うことは有効である。

また、本発明による移動体では、現在位置取得部は、第１及び第２の信頼度のうち、高い方の信頼度に対応する推定位置を移動体の現在位置としてもよい。
このような構成により、上記従来例のように、一方の位置推定が不適切である場合に、自動的に他方の位置推定の結果を用いるようなことを回避することができ、より精度の高い現在位置の取得を実現することができる。

また、本発明による移動体では、現在位置取得部は、第１及び第２の信頼度を用いて、第１及び第２の推定位置から移動体の現在位置を合成してもよい。
このような構成により、例えば、第１の信頼度が高い場合には、第１の推定位置に近い現在位置を取得したり、第１及び第２の信頼度が同程度である場合には、第１及び第２の推定位置の間の現在位置を取得したりすることができる。

本発明による移動体によれば、複数の位置推定を用いて適切に現在位置を取得することができるようになる。

本発明の実施の形態による移動体の構成を示すブロック図 同実施の形態による移動体の動作を示すフローチャート 同実施の形態における移動体が有する測距センサの一例を示す上面図 同実施の形態における移動体が有するイメージセンサの一例を示す上面図 同実施の形態における移動体が有する２個の測距センサの一例を示す上面図 同実施の形態における移動体が有する２個のイメージセンサの一例を示す上面図 同実施の形態における測距の一例を示す模式図 同実施の形態における撮影画像のブロックの一例を示す模式図

以下、本発明による移動体について、実施の形態を用いて説明する。なお、以下の実施の形態において、同じ符号を付した構成要素及びステップは同一または相当するものであり、再度の説明を省略することがある。本実施の形態による移動体は、２個のセンサを用いて取得された第１及び第２の推定位置と、それらにそれぞれ対応する第１及び第２の信頼度とを用いて現在位置を取得するものである。

図１は、本実施の形態による移動体１の構成を示すブロック図である。本実施の形態による移動体１は、自律的に移動するものであり、第１のセンサ１１と、第２のセンサ１２と、第１の位置推定部１３と、第２の位置推定部１４と、現在位置取得部１５と、移動機構１６と、移動制御部１７とを備える。なお、移動体１が自律的に移動するとは、移動体１がユーザ等から受け付ける操作指示に応じて移動するのではなく、自らの判断によって目的地に移動することであってもよい。その目的地は、例えば、手動で決められたものであってもよく、または、自動的に決定されたものであってもよい。また、その目的地までの移動は、例えば、移動経路に沿って行われてもよく、または、そうでなくてもよい。また、自らの判断によって目的地に移動するとは、例えば、進行方向、移動や停止などを移動体１が自ら判断することによって、目的地まで移動することであってもよい。また、例えば、移動体１が、障害物に衝突しないように移動することであってもよい。移動体１は、例えば、台車であってもよく、移動するロボットであってもよい。ロボットは、例えば、エンターテインメントロボットであってもよく、監視ロボットであってもよく、搬送ロボットであってもよく、清掃ロボットであってもよく、動画や静止画を撮影するロボットであってもよく、その他のロボットであってもよい。

第１及び第２のセンサ１１，１２は、移動体１の周囲の情報を取得する。第１及び第２のセンサ１１，１２は、それぞれ独立して、複数方向に関して周囲の物体までの距離を測定する測距センサであってもよく、移動体１の周囲の撮影画像を取得するイメージセンサであってもよい。第１及び第２のセンサ１１，１２が測距センサである場合には、取得対象の情報は距離になり、第１及び第２のセンサ１１，１２がイメージセンサである場合には、取得対象の情報は撮影画像になる。なお、第２のセンサ１２は、第１のセンサ１１によって取得される情報とは異なる情報を取得する。第１及び第２のセンサ１１，１２によって取得される情報が異なるとは、例えば、（１）第１及び第２のセンサ１１，１２の種類が異なることであってもよく、（２）第１及び第２のセンサ１１，１２の情報の取得方向が異なることであってもよい。以下、それぞれの場合について説明する。

（１）第１及び第２のセンサ１１，１２の種類が異なる場合
この場合には、例えば、第１のセンサ１１は測距センサであり、第２のセンサ１２はイメージセンサであってもよい。図３Ａ，図３Ｂは、移動体１の上面図であり、それぞれ測距センサである第１のセンサ１１による測距と、イメージセンサである第２のセンサ１２による撮影とを示す図である。なお、図３Ａ，図３Ｂでは、説明の便宜上、それぞれ別の図面に測距センサとイメージセンサを示しているが、実際には、１個の移動体１に、測距センサとイメージセンサとが装着されていることになる。測距センサである第１のセンサ１１の測距の向きと、イメージセンサである第２のセンサ１２の撮影の向きとは、例えば、図３Ａ，図３Ｂで示されるように、同じであってもよく、または、異なっていてもよい。第１のセンサ１１の測距の向きと第２のセンサ１２の撮影の向きとが同じである場合に、その向きは移動体１の進行方向であってもよい。進行方向の測距結果を用いて衝突回避などの制御を行うことができ、また、進行方向の撮影画像はより変化の小さいものになるため、位置推定に用いるのに好適だからである。

（２）第１及び第２のセンサ１１，１２の情報の取得方向が異なる場合
この場合には、例えば、第１及び第２のセンサ１１，１２の両方が測距センサであってもよく、またはイメージセンサであってもよい。図３Ｃは、測距センサである第１及び第２のセンサ１１，１２による測距を示す上面図である。図３Ｄは、イメージセンサである第１及び第２のセンサ１１，１２による撮影を示す上面図である。図３Ｃ，図３Ｄで示されるように、第１及び第２のセンサ１１，１２の両方が同じ種類のセンサである場合には、情報の取得方向が異なっているものとする。なお、情報の取得方向が異なっているとは、第１及び第２のセンサ１１，１２が取得する情報に、異なる方向の情報が少なくとも含まれていることを意味していると考えてもよい。すなわち、第１及び第２のセンサ１１，１２が取得する情報の少なくとも一部は、同じ方向であってもよい。なお、図３Ｃ，図３Ｄでは、第１及び第２のセンサ１１，１２の情報の取得方向が１８０度異なる場合について示しているが、そうでなくてもよい。第１及び第２のセンサ１１，１２の情報の取得方向は、例えば、９０度異なっていてもよい。

なお、第１及び第２のセンサ１１，１２は、種類が異なると共に、情報の取得方向も異なっていてもよい。また、図３Ａ〜図３Ｄにおいて、移動体１の上面視における輪郭形状が台形である場合について示しているが、これは移動体１の向きを分かりやすくするためのものであり、移動体１の上面視における輪郭形状は、例えば、矩形状であってもよく、円形状であってもよく、多角形状であってもよく、特に限定されるものではない。本実施の形態では、第１のセンサ１１が測距センサであり、第２のセンサ１２がイメージセンサである場合について主に説明する。

測距センサは、例えば、レーザセンサや、超音波センサ、マイクロ波を用いた距離センサ、ステレオカメラによって撮影されたステレオ画像を用いた距離センサなどであってもよい。レーザセンサは、レーザレンジセンサ（レーザレンジスキャナ）であってもよい。なお、それらの測距センサについてはすでに公知であり、それらの説明を省略する。本実施の形態では、測距センサがレーザレンジセンサである場合について主に説明する。また、第１のセンサ１１は、１個のレーザレンジセンサを有していてもよく、または、２個以上のレーザレンジセンサを有していてもよい。後者の場合には、２個以上のレーザレンジセンサによって、全方向がカバーされてもよい。また、測距センサが超音波センサや、マイクロ波を用いた距離センサなどである場合に、測距センサの測距方向を回転させることによって複数方向の距離を測定してもよく、複数方向ごとに配置された複数の測距センサを用いて複数方向の距離を測定してもよい。複数方向の距離を測定するとは、例えば、あらかじめ決められた角度範囲や全周囲（３６０度）について、あらかじめ決められた角度間隔で複数方向の距離を測定することであってもよい。その角度間隔は、例えば、１度間隔や２度間隔、５度間隔などのように一定であってもよい。測距センサから得られる情報は、例えば、移動体１のある向きを基準とした複数の方位角のそれぞれに関する周辺の物体までの距離であってもよい。その距離を用いることによって、移動体１のローカル座標系において、移動体１の周囲にどのような物体が存在するのかを知ることができるようになる。

イメージセンサは、例えば、ＣＣＤイメージセンサや、ＣＭＯＳイメージセンサなどであってもよい。イメージセンサは、例えば、イメージセンサ上に結像させるためのレンズ等の光学系を含んでいてもよい。また、イメージセンサは、単眼であってもよく、双眼（ステレオカメラ）であってもよい。本実施の形態では、イメージセンサが単眼である場合について主に説明する。イメージセンサは、通常、動画を撮影するもの、すなわち、連続した画像フレームを取得するものである。移動している移動体１の現在位置を取得するために撮影画像を取得することから、そのフレームレートは、移動速度に対して十分大きいものであることが好適である。例えば、フレームレートは、約３０ｆｐｓなどであってもよい。

なお、第１及び第２のセンサ１１，１２による情報の取得は、同期して行われることが好適である。第１及び第２の位置推定部１３，１４によって、同時点の位置がそれぞれ推定されることが好適だからである。

第１の位置推定部１３は、第１のセンサ１１によって取得された情報を用いて移動体１の位置を推定することによって、第１の推定位置と、その第１の推定位置に対応する第１の信頼度とを取得する。なお、上記のとおり、本実施の形態では、第１のセンサ１１が測距センサである場合について説明するため、第１の位置推定部１３は、第１のセンサ１１によって測定された距離、及び移動体１の移動領域における障害物の位置を示す第１の地図を用いて第１の推定位置を取得するものとする。測定された距離は、厳密には、測定された各方向の距離である。また、第１の地図は、第１の位置推定部１３が有する図示しない記録媒体において記憶されているものとする。その障害物は、測距センサによる距離の測定対象となる壁や設備等の物体であってもよい。第１の推定位置は、通常、ワールド座標系における位置である。そのような測距結果を用いた自己位置推定は、測距結果を用いたＳＬＡＭ（Simultaneous Localization and Mapping）においてすでに知られているため、その詳細な説明を省略する。なお、第１の位置推定部１３は、環境地図である第１の地図の作成を行ってもよく、または行わなくてもよい。前者の場合には、第１の位置推定部１３は、測距結果を用いたＳＬＡＭと同様のものであってもよく、後者の場合には、第１の位置推定部１３は、既存の環境地図である第１の地図を用いて自己位置推定を行うものであってもよい。

第１の信頼度は、第１の推定位置の信頼できる程度、すなわち確からしさを示すものであり、例えば、測定結果と第１の地図とのマッチングの程度に応じて取得されてもよく、測定結果の精度に応じて取得されてもよく、その両方であってもよく、その他の方法によって取得されてもよい。したがって、第１の信頼度は、例えば、第１のセンサ１１による測定結果と、移動体１が第１の推定位置に存在するとした場合における第１の地図の示す障害物までの距離とのマッチングの程度が高いほど大きい値となるものであってもよい。この場合には、第１の信頼度が次のように算出されてもよい。
第１の信頼度＝（ΣＦ（Ｌ_i））／Ｎ１

ここで、関数Ｆ（ｘ）は、ｘ＝０のときに１となり、ｘが正の大きい値になるほど小さくなる減少関数である。但し、Ｆ（ｘ）≧０とする。また、Ｌ_iは、ｉ番目の測距方向における測距結果と、第１の地図によって示される障害物のエッジまでの距離との差の絶対値である。また、Ｎ１はｉの総数、すなわち測距の総数であり、総和Σは、すべてのｉについて取られるものとする。Ｎ１によって規格化されているため、第１の信頼度は、０以上、１以下の実数となる。なお、Ｆ（ｘ）は、どのような関数であってもよいが、例えば、Ｆ（ｘ）＝ｅｘｐ（−α×ｘ）であってもよい。αは正の実数である。

また、レーザ等による測距結果は、距離が長くなるほど反射等の影響によって誤差が大きくなるため、第１の信頼度は、例えば、測距結果が大きいほど小さい値となるものであってもよい。この場合には、第１の信頼度が次のように算出されてもよい。
第１の信頼度＝（ΣＧ（Ｄ_i））／Ｎ１

ここで、関数Ｇ（ｘ）は、ｘ＝０のときに１となり、ｘが正の大きい値になるほど小さくなる減少関数である。但し、Ｇ（ｘ）≧０とする。また、Ｄ_iは、ｉ番目の測距方向における測距結果である。また、Ｎ１及び総和Σは、上記のとおりである。この場合にも、Ｎ１によって規格化されているため、第１の信頼度は、０以上、１以下の実数となる。また、Ｇ（ｘ）も、上記関数Ｆ（ｘ）と同様のものであってもよい。また、第１の信頼度は、例えば、第１のセンサ１１による測定結果と、第１の地図の示す障害物の位置とのマッチングの程度が高いほど大きい値となると共に、測距結果が大きいほど小さい値となるものであってもよい。

なお、第１の位置推定部１３は、第１の推定位置を一意に決定できない場合にも、第１の信頼度を低い値としてもよい。例えば、移動体１が長い廊下のように単調な環境に存在する場合には、第１のセンサ１１による測距結果を用いて、第１の推定位置を一意に決定できないこともありうる。そのような場合には、第１の信頼度は低い値とされてもよい。

第２の位置推定部１４は、第２のセンサ１２によって取得された情報を用いて移動体１の位置を推定することによって、第２の推定位置と、その第２の推定位置に対応する第２の信頼度とを取得する。なお、上記のとおり、本実施の形態では第２のセンサ１２がイメージセンサである場合について説明するため、第２の位置推定部１４は、第２のセンサ１２によって取得された撮影画像において特定した特徴点、及び移動体１の移動領域における特徴点の位置を示す第２の地図を用いて第２の推定位置を取得するものとする。なお、第２の地図は、第２の位置推定部１４が有する図示しない記録媒体において記憶されているものとする。第２の推定位置は、通常、ワールド座標系における位置である。そのような撮影画像を用いた自己位置推定は、ｖｉｓｕａｌ−ＳＬＡＭとしてすでに知られているため、その詳細な説明を省略する。なお、第２の位置推定部１４は、環境地図である第２の地図の作成を行ってもよく、または行わなくてもよい。前者の場合には、第２の位置推定部１４は、ｖｉｓｕａｌ−ＳＬＡＭと同様のものであってもよく、後者の場合には、第２の位置推定部１４は、既存の環境地図である第２の地図を用いて自己位置推定を行うものであってもよい。

ここで、第２の位置推定部１４が撮影画像において特定する特徴点について説明する。第２の位置推定部１４が、ＯＲＢ−ＳＬＡＭと同様の自己位置推定を行う場合には、特徴点として所定個数のＦＡＳＴキーポイントが用いられる。その所定個数のＦＡＳＴキーポイントは、撮影画像から取得されたＦＡＳＴキーポイントのうち、ハリスのコーナー尺度（Harris corner measure）の上位である所定個数が選ばれたものとなる。なお、ＦＡＳＴキーポイント、ハリスのコーナー尺度、ＯＲＢ−ＳＬＡＭにおける特徴点については、以下の文献１〜３をそれぞれ参照されたい。また、特徴点としては、例えば、ＳＩＦＴキーポイントや、ＳＵＲＦキーポイントなどを用いてもよい。それらのキーポイントについては、以下の文献４，５を参照されたい。また、ｖｉｓｕａｌ−ＳＬＡＭで用いられる他の特徴点が用いられてもよい。また、後述する特徴点の特徴記述子（feature descriptor）についても、例えば、以下の文献３〜５を参照されたい。本実施の形態では、第２の位置推定部１４がＯＲＢ−ＳＬＡＭと同様の手法によって自己位置推定を行う場合について主に説明する。ＯＲＢ−ＳＬＡＭについては、以下の文献６を参照されたい。

文献１：Edward Rosten, Tom Drummond, "Machine learning for high-speed corner detection", in 9th European Conference on Computer Vision, vol. 1, 2006, pp. 430-443.
文献２：C. Harris, M. Stephens, "A Combined Corner and Edge Detector", In Proceedings of the 4th Alvey Vision Conference (1988), pp. 147-151.
文献３：Ethan Rublee, Vincent Rabaud, Kurt Konolige, Gary R. Bradski, "ORB: An efficient alternative to SIFT or SURF", ICCV 2011, pp.2564-2571.
文献４：D. Lowe, "Distinctive image features from scale-invariant keypoints", Int. Journal of Computer Vision (2004), 60(2), pp.91-110.
文献５：H. Bay, A. Ess, T. Tuytelaars, L. Van Gool, "SURF: Speeded Up Robust Features", Computer Vision and Image Understanding (CVIU) (2008), 110(3), pp.346-359.
文献６：R. Mur-Artal, J.M. Montiel, J.D. Tardos, "ORB-SLAM: A Versatile and Accurate Monocular SLAM System", IEEE Transactions on Robotics 31 (2015), pp.1147-1163.

第２の信頼度は、第２の推定位置の信頼できる程度、すなわち確からしさを示すものであり、例えば、撮影画像において特定された特徴点と第２の地図に含まれる特徴点とのマッチングの程度に応じて取得されてもよく、撮影画像において特定された特徴点の精度に応じて取得されてもよく、その両方であってもよく、その他の方法によって取得されてもよい。したがって、第２の信頼度は、例えば、第２のセンサ１２によって取得された撮影画像において特定された特徴点の特徴記述子（特徴量）と、移動体１が第２の推定位置に存在するとした場合における、それらの特徴点に対応する第２の地図に含まれる特徴点の特徴記述子との類似度が大きいほど大きい値となるものであってもよい。その場合には、第２の信頼度が次のように算出されてもよい。
第２の信頼度＝（ΣＲ_i）／Ｎ２

ここで、Ｒ_iは、撮影画像において特定されたｉ番目の特徴点の特徴記述子と、その特徴点に対応する第２の地図に含まれる特徴点の特徴記述子との類似度であり、０から１までの値に規格化されていることが好適である。その類似度は、値が大きいほど、類似の程度が大きいものとする。また、撮影画像において特定されたｉ番目の特徴点に対応する特徴点が第２の地図に含まれない場合には、類似度は０であってもよい。また、Ｎ２は、ｉの総数、すなわち撮影画像において特定された特徴点の総数であり、総和Σは、すべてのｉについて取られるものとする。Ｎ２によって規格化されているため、第２の信頼度は、０以上、１以下の実数となる。なお、特徴記述子が二値ベクトルである場合（例えば、ＯＲＢ−ＳＬＡＭの場合）には、ハミング距離を用いて類似度を算出してもよい。特徴記述子が二値ベクトルではない場合（例えば、ＳＩＦＴキーポイントやＳＵＲＦキーポイントの場合）には、類似度をコサイン距離やユークリッド距離を用いて算出してもよい。

また、撮影画像における特徴点は、露出がオーバーになって白とびになっている場合や、露出がアンダーになって黒つぶれになっている場合には、適切に特定できないため、第２の信頼度は、例えば、特徴点の特定において用いられる評価値が大きいほど大きい値となるものであってもよい。なお、評価値の高い特徴点の候補が、特徴点として特定されることになるものとする。例えば、ＯＲＢ−ＳＬＡＭでは、ハリスのコーナー尺度の上位から所定個数のＦＡＳＴキーポイントが選ばれるため、その評価値は、ハリスのコーナー尺度であってもよい。すなわち、第２の信頼度は、撮影画像において特定された複数の特徴点に関するハリスのコーナー尺度の代表値であってもよい。代表値は、例えば、平均値や中央値、最大値等であってもよい。また、例えば、ＳＩＦＴによる特徴点（キーポイント）の特定では、ローコントラストのキーポイント候補を削除するため、キーポイント候補点の極値の大きさを計算し、その大きさが閾値未満の候補点を削除する。したがって、特定された複数の特徴点に関する極値の大きさの代表値を第２の信頼度として用いてもよい。他の特徴点についても同様である。また、第２の信頼度は、例えば、撮影画像において特定された特徴点の特徴記述子と、それらの特徴点に対応する第２の地図に含まれる特徴点の特徴記述子との類似度が大きいほど大きい値となると共に、特徴点の特定において用いられる評価値が大きいほど大きい値となるものであってもよい。

なお、第１及び第２の信頼度は、両者を適切に比較することができるように規格化されていることが好適である。すなわち、第１及び第２の信頼度が同程度の値であれば、第１及び第２の推定位置が同程度の信頼性となるようにそれぞれが規格化されていることが好適である。

現在位置取得部１５は、第１及び第２の推定位置と、第１及び第２の信頼度とを用いて、移動体１の現在位置を取得する。現在位置取得部１５は、例えば、第１及び第２の信頼度のうち、高い方の信頼度に対応する推定位置を移動体１の現在位置としてもよい。この場合には、現在位置取得部１５は、第１及び第２の信頼度を比較し、高いほうの信頼度に対応する推定位置を選択し、その選択した推定位置を移動体１の現在位置としてもよい。より具体的には、第１の信頼度のほうが第２の信頼度よりも高かった場合には、現在位置取得部１５は、第１の信頼度に対応する第１の推定位置を現在位置とすることになる。なお、この場合には、あらかじめ第１及び第２の推定位置と、第１及び第２の信頼度との両方が取得された状況において、その選択が行われてもよく、第１及び第２の信頼度のみを先に算出し、大きい方の信頼度に応じた推定位置を後から取得するようにしてもよい。このように、信頼度を用いて一方の推定位置を選択する場合であって、信頼度に応じた選択対象が切り替わった場合には、現在位置取得部１５によって取得された現在位置が突然に大きく変化することも考えられる。したがって、信頼度に応じた選択対象が切り替わった場合であって、選択対象が切り替わる前の現在位置と選択対象が切り替わった後の現在位置とに、あらかじめ決められた閾値以上の差が存在する場合には、切り替わる直前の現在位置から、新たに選択対象となった現在位置までをなめらかに繋ぐように現在位置を取得してもよい。なお、第１及び第２の信頼度が同じである場合には、現在位置取得部１５は、ランダムに選択した一方の推定位置を移動体１の現在位置としてもよく、または、第１及び第２の推定位置を合成した現在位置を取得してもよい。その合成については後述する。

また、現在位置取得部１５は、例えば、第１及び第２の信頼度を用いて、第１及び第２の推定位置から移動体１の現在位置を合成してもよい。現在位置取得部１５は、例えば、第１及び第２の信頼度を重みとして、第１及び第２の推定位置を合成してもよい。すなわち、現在位置取得部１５は、第１及び第２の信頼度を重みとして用いた第１及び第２の推定位置の重み付け加算によって現在位置を合成してもよい。具体的には、現在位置の座標値を（Ｘ１００，Ｙ１００）とした場合に、現在位置取得部１５は、次のように合成を行ってもよい。
Ｘ１００＝（Ｃ１×Ｘ１０１＋Ｃ２×Ｘ１０２）／（Ｃ１＋Ｃ２）
Ｙ１００＝（Ｃ１×Ｙ１０１＋Ｃ２×Ｙ１０２）／（Ｃ１＋Ｃ２）
ここで、第１及び第２の推定位置の座標値をそれぞれ（Ｘ１０１，Ｙ１０１）、（Ｘ１０２，Ｙ１０２）とし、第１及び第２の信頼度をそれぞれＣ１，Ｃ２としている。

また、現在位置取得部１５は、例えば、第１及び第２の信頼度が両方ともあらかじめ決められた閾値よりも大きい場合に、第１及び第２の推定位置の中点を現在位置としてもよい。なお、現在位置取得部１５は、例えば、第１及び第２の信頼度の少なくとも一方が閾値よりも小さい場合には、より大きい信頼度に対応する推定位置を現在位置としてもよい。また、現在位置取得部１５は、例えば、第１及び第２の信頼度が等しい場合には、第１及び第２の推定位置の中点を現在位置としてもよい。

移動機構１６は、移動体１を移動させる。移動機構１６は、例えば、移動体１を全方向に移動できるものであってもよく、または、そうでなくてもよい。全方向に移動できるとは、任意の方向に移動できることである。移動機構１６は、例えば、走行部（例えば、車輪など）と、その走行部を駆動する駆動手段（例えば、モータやエンジンなど）とを有していてもよい。また、移動機構１６は、車輪等の速度を取得できる機構、例えば、エンコーダ等を有していてもよい。なお、移動機構１６が、移動体１を全方向に移動できるものである場合には、その走行部は、全方向移動車輪（例えば、オムニホイール、メカナムホイールなど）であってもよい。この移動機構１６としては、公知のものを用いることができるため、その詳細な説明を省略する。

移動制御部１７は、現在位置取得部１５によって取得された現在位置を用いて移動機構１６を制御することによって、移動体１の移動を制御する。移動の制御は、移動体１の移動の向きや、移動の開始・停止などの制御であってもよい。例えば、移動経路が設定されている場合には、移動制御部１７は、移動体１がその移動経路に沿って移動するように、移動機構１６を制御してもよい。より具体的には、移動制御部１７は、現在位置取得部１５によって取得される現在位置が、その移動経路に沿ったものになるように、移動機構１６を制御してもよい。また、移動制御部１７は、地図を用いて、移動の制御を行ってもよい。移動制御部１７による移動機構１６の制御は公知であるため、その詳細な説明を省略する。

次に、移動体１の動作について図２のフローチャートを用いて説明する。
（ステップＳ１０１）現在位置取得部１５は、現在位置を取得するかどうか判断する。そして、現在位置を取得する場合には、ステップＳ１０２に進み、そうでない場合には、現在位置を取得すると判断するまで、ステップＳ１０１の処理を繰り返す。なお、現在位置取得部１５は、例えば、現在位置を取得すると定期的に判断してもよい。

（ステップＳ１０２）第１のセンサ１１は、移動体１の周囲の情報を取得する。その情報は、例えば、複数方向に関する測距結果である。

（ステップＳ１０３）第１の位置推定部１３は、第１のセンサ１１によって取得された情報を用いて、第１の推定位置及び第１の信頼度を取得する。

（ステップＳ１０４）第２のセンサ１２は、移動体１の周囲の情報を取得する。その情報の取得は、例えば、撮影画像の取得である。

（ステップＳ１０５）第２の位置推定部１４は、第２のセンサ１２によって取得された情報を用いて、第２の推定位置及び第２の信頼度を取得する。

（ステップＳ１０６）現在位置取得部１５は、第１の位置推定部１３によって取得された第１の推定位置及び第１の信頼度と、第２の位置推定部１４によって取得された第２の推定位置及び第２の信頼度とを用いて、現在位置を取得する。その現在位置は、例えば、第１及び第２の推定位置のいずれか一方であってもよく、または、第１及び第２の推定位置の合成結果であってもよい。そして、ステップＳ１０１に戻る。

なお、図２のフローチャートには含まれていないが、現在位置取得部１５によって取得された現在位置を用いた移動制御が、移動制御部１７によって行われるものとする。また、図２のフローチャートにおける処理の順序は一例であり、同様の結果を得られるのであれば、各ステップの順序を変更してもよい。前述のように、ステップＳ１０２，Ｓ１０４の処理は同期して行われることが好適であるため、ステップＳ１０２，Ｓ１０３の処理と、ステップＳ１０４，Ｓ１０５の処理とは並列的に行われてもよい。また、図２のフローチャートにおいて、電源オフや処理終了の割り込みにより処理は終了する。

次に、本実施の形態による移動体１の動作について、簡単な具体例を用いて説明する。この具体例では、移動体１が工場内において搬送ロボットとして用いられているものとする。また、現在位置取得部１５は、高い方の信頼度に対応する推定位置を現在位置として選択するものとする。また、第１の信頼度は、測定結果と第１の地図とのマッチングの程度に応じて取得され、第２の信頼度は、特定された特徴点と第２の地図に含まれる特徴点とのマッチングの程度に応じて取得されるものとする。

まず、移動体１が、人通りの多い領域を移動しているとする。そのような状況において、第１のセンサ１１によって障害物までの距離が測定され、第１の位置推定部１３によって第１の推定位置及び第１の信頼度が取得される（ステップＳ１０１〜Ｓ１０３）。なお、その第１の信頼度は、低い値となる。第１の地図に含まれていない障害物（人間）が多く存在するからである。その後、第２のセンサ１２によって撮影画像が取得され、第２の位置推定部１４によって第２の推定位置及び第２の信頼度が取得される（ステップＳ１０４，Ｓ１０５）。撮影画像を用いた位置推定は新たな障害物に対してロバストであるため、第２の信頼度は、第１の信頼度ほどは低下しないことになる。その結果、第２の信頼度の方が、第１の信頼度よりも高かったとする。すると、現在位置取得部１５は、より高い値である第２の信頼度に対応する第２の推定位置を選択して、その第２の推定位置である現在位置を移動制御部１７に渡す（ステップＳ１０６）。

その後、人の出入りのために工場の扉が開けられ、その扉を介して太陽光が工場内に差し込んできたとする。また、ちょうどそのときに、第２のセンサ１２が、その扉を向いていたとする。すると、太陽光の影響によって第２のセンサ１２によって取得された撮影画像の一部が白とびになる。その結果、第２の信頼度が大きく低下し、第１の信頼度よりも小さくなったとする（ステップＳ１０４，Ｓ１０５）。すると、現在位置取得部１５は、第１の信頼度に応じた第１の推定位置を選択して現在位置として移動制御部１７に渡すように切り替える（ステップＳ１０６）。なお、前述のように、切り替え前の現在位置と、切り替え後の現在位置とに大きな差がある場合には、現在位置取得部１５は、それらをなめらかに繋ぐように、移動制御部１７に渡す現在位置を、徐々に変化させるようにしてもよい。

以上のように、本実施の形態による移動体１によれば、第１及び第２の信頼度を用いて移動体１の現在位置を取得するため、例えば、測距センサによる測定結果を用いた第１の推定位置に対応する第１の信頼度が、イメージセンサによる撮影画像を用いた第２の推定位置に対応する第２の信頼度よりも低い場合には、第２の推定位置を現在位置とすることによって、より精度の高い現在位置の取得を実現することができる。また、上記従来例のように、一方の位置推定が不適切である場合に、自動的に他方の位置推定の結果を用いるようにすることを回避することができる。その結果、現在位置の精度がより高くなることになる。また、現在位置取得部１５によって第１及び第２の推定位置の合成が行われる場合であって、第１及び第２の信頼度が両方とも高い場合には、合成結果の現在位置は、より精度の高いものになると考えられる。

なお、上記のように、測距センサの測定結果は、距離が遠くなるほど精度が低くなるため、遠い距離の測定結果は用いないようにしてもよい。例えば、図４で示されるように、測距センサである第１のセンサ１１によって取得された測定点までの長さが、あらかじめ決められた閾値Ｌよりも大きい場合には、その測定点を自己位置推定に用いず、測定点までの長さが閾値Ｌよりも小さい場合には、その測定点を自己位置推定に用いるようにしてもよい。なお、測定点までの距離が閾値Ｌと等しい場合には、その測定点を自己位置推定に用いるようにしてもよく、または、そうでなくてもよい。図４で示される状況では、測定点ａは、第１の位置推定部１３による自己位置推定に用いられることになり、測定点ｂは、第１の位置推定部１３による自己位置推定に用いられないことになる。

また、撮影画像の露出がオーバーになって白とびが起こったり、露出がアンダーになって黒つぶれが起こったりする場合でも、撮影画像の全体がそのようになることはあまりなく、部分的に白とびや黒つぶれの起こることが多い。そのため、第２の位置推定部１４は、特徴点ごとに、特徴点の適切さの程度を示す適切度を取得し、その適切度を用いて、複数のブロックに分割された撮影画像のブロックごとにブロック信頼度を算出し、ブロック信頼度の高いブロックを用いて第２の推定位置を取得し、第２の推定位置の取得で用いたブロックのブロック信頼度を用いて第２の信頼度を取得するようにしてもよい。例えば、図５で示されるように、撮影画像があらかじめ９個のブロック５１〜５９に分割されている場合には、第２の位置推定部１４は、ブロックごとにブロック信頼度を取得してもよい。そのブロック信頼度は、例えば、そのブロックに含まれる特徴点に対応する適切度の代表値であってもよい。代表値は、例えば、平均値や中央値、最大値等であってもよい。その適切度は、特定された特徴点の特徴記述子と、その特徴点に対応する第２の地図に含まれる特徴点の特徴記述子との類似度であってもよく（この場合には、撮影画像において特定された特徴点と第２の地図に含まれる特徴点とのマッチングをブロック単位で暫定的に行うことによって、特徴点の対応関係を見つけてもよい）、上記した評価値であってもよく、その他の特徴点の適切さの程度を示す値であってもよい。また、ブロック信頼度の高いブロックとは、例えば、ブロック信頼度があらかじめ決められた閾値を超えているブロックであってもよく、ブロック信頼度の高い方からあらかじめ決められた個数のブロックであってもよい。また、ブロック信頼度の高いブロックを用いて第２の推定位置を取得するとは、そのブロック信頼度の高いブロックに含まれる特徴点を用いて第２の推定位置を取得することである。また、第２の推定位置の取得で用いたブロックのブロック信頼度を用いて第２の信頼度を取得するとは、第２の推定位置の取得で用いたブロック、すなわちブロック信頼度の高いブロックに対応するブロック信頼度の代表値を、第２の信頼度とすることであってもよい。その代表値も、例えば、平均値や中央値、最大値等であってもよく、ブロックに含まれる特徴点の個数を重みとして用いた重み付け加算によって算出された値であってもよい。

また、上記実施の形態において、各処理または各機能は、単一の装置または単一のシステムによって集中処理されることによって実現されてもよく、または、複数の装置または複数のシステムによって分散処理されることによって実現されてもよい。

また、上記実施の形態において、各構成要素間で行われる情報の受け渡しは、例えば、その情報の受け渡しを行う２個の構成要素が物理的に異なるものである場合には、一方の構成要素による情報の出力と、他方の構成要素による情報の受け付けとによって行われてもよく、または、その情報の受け渡しを行う２個の構成要素が物理的に同じものである場合には、一方の構成要素に対応する処理のフェーズから、他方の構成要素に対応する処理のフェーズに移ることによって行われてもよい。

また、上記実施の形態において、各構成要素が実行する処理に関係する情報、例えば、各構成要素が受け付けたり、取得したり、選択したり、生成したり、送信したり、受信したりした情報や、各構成要素が処理で用いる閾値や数式、アドレス等の情報等は、上記説明で明記していなくても、図示しない記録媒体において、一時的に、または長期にわたって保持されていてもよい。また、その図示しない記録媒体への情報の蓄積を、各構成要素、または、図示しない蓄積部が行ってもよい。また、その図示しない記録媒体からの情報の読み出しを、各構成要素、または、図示しない読み出し部が行ってもよい。

また、上記実施の形態において、各構成要素等で用いられる情報、例えば、各構成要素が処理で用いる閾値やアドレス、各種の設定値等の情報がユーザによって変更されてもよい場合には、上記説明で明記していなくても、ユーザが適宜、それらの情報を変更できるようにしてもよく、または、そうでなくてもよい。それらの情報をユーザが変更可能な場合には、その変更は、例えば、ユーザからの変更指示を受け付ける図示しない受付部と、その変更指示に応じて情報を変更する図示しない変更部とによって実現されてもよい。その図示しない受付部による変更指示の受け付けは、例えば、入力デバイスからの受け付けでもよく、通信回線を介して送信された情報の受信でもよく、所定の記録媒体から読み出された情報の受け付けでもよい。

また、上記実施の形態において、移動体１に含まれる２以上の構成要素が通信デバイスや入力デバイス等を有する場合に、２以上の構成要素が物理的に単一のデバイスを有してもよく、または、別々のデバイスを有してもよい。

また、上記実施の形態において、各構成要素は専用のハードウェアにより構成されてもよく、または、ソフトウェアにより実現可能な構成要素については、プログラムを実行することによって実現されてもよい。例えば、ハードディスクや半導体メモリ等の記録媒体に記録されたソフトウェア・プログラムをＣＰＵ等のプログラム実行部が読み出して実行することによって、各構成要素が実現されうる。その実行時に、プログラム実行部は、記憶部や記録媒体にアクセスしながらプログラムを実行してもよい。また、そのプログラムは、サーバなどからダウンロードされることによって実行されてもよく、所定の記録媒体（例えば、光ディスクや磁気ディスク、半導体メモリなど）に記録されたプログラムが読み出されることによって実行されてもよい。また、このプログラムは、プログラムプロダクトを構成するプログラムとして用いられてもよい。また、そのプログラムを実行するコンピュータは、単数であってもよく、複数であってもよい。すなわち、集中処理を行ってもよく、または分散処理を行ってもよい。

また、本発明は、以上の実施の形態に限定されることなく、種々の変更が可能であり、それらも本発明の範囲内に包含されるものであることは言うまでもない。

以上より、本発明による移動体によれば、精度の高い現在位置を取得できるという効果が得られ、例えば、搬送ロボット等の移動体として有用である。

１ 移動体
１１ 第１のセンサ
１２ 第２のセンサ
１３ 第１の位置推定部
１４ 第２の位置推定部
１５ 現在位置取得部
１６ 移動機構
１７ 移動制御部

Claims (5)

1. 自律的に移動する移動体であって、
   前記移動体の周囲の情報を取得する第１のセンサと、
   前記移動体の周囲の情報であって、前記第１のセンサによって取得される情報とは異なる情報を取得する第２のセンサと、
   前記第１のセンサによって取得された情報を用いて前記移動体の位置を推定することによって、第１の推定位置と、当該第１の推定位置に対応する第１の信頼度とを取得する第１の位置推定部と、
   前記第２のセンサによって取得された情報を用いて前記移動体の位置を推定することによって、第２の推定位置と、当該第２の推定位置に対応する第２の信頼度とを取得する第２の位置推定部と、
   前記第１及び第２の推定位置と、前記第１及び第２の信頼度とを用いて、前記移動体の現在位置を取得する現在位置取得部と、
   前記移動体を移動させる移動機構と、
   前記現在位置を用いて前記移動機構を制御する移動制御部と、を備えた移動体。
2. 前記第１のセンサは、複数方向に関して周囲の物体までの距離を測定する測距センサであり、
   前記第１の位置推定部は、前記第１のセンサによって測定された距離、及び前記移動体の移動領域における障害物の位置を示す第１の地図を用いて前記第１の推定位置を取得し、
   前記第２のセンサは、前記移動体の周囲の撮影画像を取得するイメージセンサであり、
   前記第２の位置推定部は、前記第２のセンサによって取得された撮影画像において特定した特徴点、及び前記移動体の移動領域における特徴点の位置を示す第２の地図を用いて前記第２の推定位置を取得する、請求項１記載の移動体。
3. 前記第２の位置推定部は、特徴点ごとに、特徴点の適切さの程度を示す適切度を取得し、当該適切度を用いて、複数のブロックに分割された撮影画像のブロックごとにブロック信頼度を算出し、前記ブロック信頼度の高いブロックを用いて前記第２の推定位置を取得し、前記第２の推定位置の取得で用いたブロックのブロック信頼度を用いて前記第２の信頼度を取得する、請求項２記載の移動体。
4. 前記現在位置取得部は、前記第１及び第２の信頼度のうち、高い方の信頼度に対応する推定位置を前記移動体の現在位置とする、請求項１から請求項３のいずれか記載の移動体。
5. 前記現在位置取得部は、前記第１及び第２の信頼度を用いて、前記第１及び第２の推定位置から前記移動体の現在位置を合成する、請求項１から請求項３のいずれか記載の移動体。

Images