JP4672175B2

JP4672175B2 - 位置検出装置、位置検出方法、及び位置検出プログラム

Info

Publication number: JP4672175B2
Application number: JP2001124943A
Authority: JP
Inventors: 孝明永井; 慎一松永; 義秋坂上
Original assignee: Honda Motor Co Ltd
Current assignee: Honda Motor Co Ltd
Priority date: 2000-05-26
Filing date: 2001-04-23
Publication date: 2011-04-20
Anticipated expiration: 2021-04-23
Also published as: EP1158309A3; US7239339B2; US20010055063A1; EP1158309B1; EP1158309A2; JP2002048513A

Description

【０００１】
【発明の属する技術分野】
本発明は、距離画像を用いてロボットや自動車が移動した際の自己の位置を検出する位置検出装置、位置検出方法、及び位置検出プログラムに関する。特に、人間型の脚式移動ロボットの自己位置検出に関する。
【０００２】
【従来の技術及び発明が解決しようとする課題】
従来の車両の位置検出には、ＧＰＳやビーコンから発せられた情報を受信して、この情報を解析して自己の位置を検出するものがある。ＧＰＳやビーコンを使用した位置検出は、ＧＰＳやビーコンが発した電磁波を受信しなければならないために、電磁波の送信装置と自己に備えられた受信装置との間に電磁波を遮断する物体がある場合、自己の位置検出を行えなくなるという問題がある。特に屋内のみを移動するロボットなどは全く利用することができない。このため、車両に車輪の回転数を検出する距離センサ及び方位センサを備え、これらのセンサの出力値に基づき、場合によっては道路地図データをマッチングすることによって、自己の位置を検出する位置検出装置がある。例えば、特開平９−２４３３８９号公報に記載されている技術が挙げられる。
【０００３】
しかしながら、この位置検出装置は、内部に詳細な道路地図データを備える必要があるとともに、距離センサや方位センサの出力値には誤差が含まれるために、距離センサや方位センサの出力値のみによる位置検出が長時間継続すると、検出誤差が累積されてしまい、検出された車両位置が実際の車両位置から大きくずれてしまうという問題がある。また、特に人間型の脚式移動ロボットが屋内を自由に移動する際においては、壁や柱との相対位置をロボット自身が得る必要があるが、従来の位置検出装置では、周囲の環境との相対位置関係を得ることが難しいために自律移動が困難になるという問題がある。
【０００４】
本発明は、このような事情に鑑みてなされたもので、人間型脚式移動ロボットや自動車が自律的に移動する際に、周囲環境の画像を用いて自己の位置検出を簡単に行うことができる位置検出装置、位置検出方法、及び位置検出プログラムを提供することを目的とする。
【０００５】
【課題を解決するための手段】
本願発明の一態様は、移動体の自己位置を検出する位置検出装置であって、前記位置検出装置は、前記移動体の前方視野の画像を取得する画像取得手段と、前記画像取得手段と同一の視野を有し、前記画像取得手段が画像を取得するのと同時に距離画像を取得する距離画像取得手段と、少なくとも連続する２フレームの画像からそれぞれ特徴点を抽出する特徴点抽出手段と、前記特徴点抽出手段によって抽出された特徴点の２フレーム間の位置の変位量を前記距離画像に基づいて算出し、当該変位量に基づいて各特徴点が静止物体であるか移動物体であるか判断し、前記静止物体であると判断された前記特徴点を基準特徴点として選択する基準特徴点選択手段と、前記距離画像から前記基準特徴点に対応する距離を取得し、前記基準特徴点と自己位置との相対的位置の前記２フレーム間の移動量を算出し、前記移動量に基づいて現在の自己位置を検出する位置検出手段とを備えたことを特徴とする。
この発明によれば、連続する２フレームの画像から同一の静止物体を抽出し、この静止物体の変位を求め、この変位量から自己の移動量を求めるようにしたため、精度良く自己位置の検出を行うことができるという効果が得られる。また、静止物体の抽出を自己で行うようにしたため、予め静止物体の位置が定義されたマップ等を備える必要がなく、構成を簡単にすることが可能となる。さらに、道路地図データ等のマップを備える必要がないために、構成を簡単にすることができるとともに、未知の場所へ移動することが可能となり移動体の行動範囲の制限をなくすことができるという効果も得られる。
【０００６】
本願発明の一態様は、移動体の自己位置を検出する位置検出装置であって、前記位置検出装置は、前記移動体の前方視野内の画像を取得する画像取得手段と、前記移動体の移動に当たって基準となる基準特徴点を前記画像取得手段によって得られた画像に基づいて決定する基準点決定手段と、自己の移動制御と前記基準点の観測量を拡張カルマンフィルタに代入して自己の位置を検出する位置検出手段とを備えたことを特徴とする。
この発明によれば、自己の移動制御と前記基準点の観測量を拡張カルマンフィルタに代入して自己の位置を検出する位置検出手段を備えたため、より正確な自己位置の検出が可能になるという効果が得られる。
【０００７】
本願発明の一態様は、移動体の自己位置を検出する位置検出装置であって、前記位置検出装置は、前記移動体の前方視野の画像を取得する画像取得手段と、前記画像取得手段と同一の視野を有し、前記画像取得手段が画像を取得するのと同時に距離画像を取得する距離画像取得手段と、得られた画像からそれぞれ特徴点を抽出する特徴点抽出手段と、予め記憶された物体情報と抽出された特徴点とを比較して、相関が高い特徴点を既知と特徴点と見なして自己位置を算出するための基準特徴点とする基準特徴点選択手段とを備えたことを特徴とする。
この発明によれば、予め記憶された物体情報と抽出された特徴点とを比較して、相関が高い特徴点を既知と特徴点と見なして自己位置を算出するための基準特徴点とするようにしたため、より正確に自己の位置を検出することができるという効果が得られる。
【０００８】
本願発明の一態様は、前記特徴点選択手段は、既知と見なされた特徴点が存在する画像中の未知の特徴点と既知の特徴点との相対関係を求め、該未知の特徴点を既知の特徴点として記憶することにより前記物体情報を更新することを特徴する。
この発明によれば、既知と見なされた特徴点が存在する画像中の未知の特徴点と既知の特徴点との相対関係を求め、該未知の特徴点を既知の特徴点として記憶することにより前記物体情報を更新するようにしたため、物体情報の更新を自動的に行うことができるという効果が得られる。
【０００９】
本願発明の一態様は、移動体の自己位置を検出する位置検出装置であって、前記位置検出装置は、前記移動体の前方視野の画像を取得する画像取得手段と、前記画像中の特徴点群を抽出する特徴点群抽出手段と、予め画像取得手段で得られた画像における特徴点群と当該特徴点群を得た位置を対応させて複数記憶しておき、新たに得られた画像の特徴点群と予め記憶された特徴点群の相関を算出して、自己の位置を算出する自己位置検出手段とを備えたことを特徴とする。
この発明によれば、予め画像取得手段で得られた画像における特徴点群と当該特徴点群を得た位置を対応させて複数記憶しておき、新たに得られた画像の特徴点群と予め記憶された特徴点群の相関を算出して、自己の位置を算出するようにしたため、幾何学的にロボットの位置が得られない場合であっても過去の位置検出結果に基づいて自己の位置を検出することができるという効果が得られる。
【００１０】
本願発明の一態様は、移動体の自己位置を検出する位置検出方法であって、前記位置検出方法は、前記移動体の前方視野の画像を取得する画像取得過程と、前記画像と同一の視野を有し、前記画像を取得するのと同時に距離画像を取得する距離画像取得過程と、少なくとも連続する２フレームの画像からそれぞれ特徴点を抽出する特徴点抽出過程と、前記特徴点抽出過程において抽出された特徴点の２フレーム間の位置の変位量を前記距離画像に基づいて算出し、当該変位量に基づいて各特徴点が静止物体であるか移動物体であるか判断し、前記静止物体であると判断された前記特徴点を基準特徴点として選択する基準特徴点選択過程と、前記距離画像から前記基準特徴点に対応する距離を取得し、前記基準特徴点と自己位置との相対的位置の前記２フレーム間の移動量を算出し、前記移動量に基づいて現在の自己位置を検出する位置検出過程とを有することを特徴とする。
この発明によれば、連続する２フレームの画像から同一の静止物体を抽出し、この静止物体の変位を求め、この変位量から自己の移動量を求めるようにしたため、精度良く自己位置の検出を行うことができるという効果が得られる。また、静止物体の抽出を自己で行うようにしたため、予め静止物体の位置が定義されたマップ等を備える必要がなく、構成を簡単にすることが可能となる。さらに、道路地図データ等のマップを備える必要がないために、構成を簡単にすることができるとともに、未知の場所へ移動することが可能となり移動体の行動範囲の制限をなくすことができるという効果も得られる。
【００１１】
本願発明の一態様は、移動体の自己位置を検出する位置検出方法であって、前記位置検出方法は、前記移動体の前方視野内の画像を取得する画像取得過程と、前記移動体の移動に当たって基準となる基準特徴点を前記画像に基づいて決定する基準点決定過程と、自己の移動制御と前記基準点の観測量を拡張カルマンフィルタに代入して自己の位置を検出する位置検出過程とを有することを特徴とする。
この発明によれば、自己の移動制御と前記基準点の観測量を拡張カルマンフィルタに代入して自己の位置を検出する位置検出手段を備えたため、より正確な自己位置の検出が可能になるという効果が得られる。
【００１２】
本願発明の一態様は、移動体の自己位置を検出する位置検出方法であって、前記位置検出方法は、前記移動体の前方視野の画像を取得する画像取得過程と、前記画像と同一の視野を有し、前記画像を取得するのと同時に距離画像を取得する距離画像取得過程と、得られた画像からそれぞれ特徴点を抽出する特徴点抽出過程と、予め記憶された物体情報と抽出された特徴点とを比較して、相関が高い特徴点を既知と特徴点と見なして自己位置を算出するための基準特徴点とする基準特徴点選択過程とを有することを特徴とする。
この発明によれば、予め記憶された物体情報と抽出された特徴点とを比較して、相関が高い特徴点を既知と特徴点と見なして自己位置を算出するための基準特徴点とするようにしたため、より正確に自己の位置を検出することができるという効果が得られる。
【００１３】
本願発明の一態様は、前記特徴点選択過程は、既知と見なされた特徴点が存在する画像中の未知の特徴点と既知の特徴点との相対関係を求め、該未知の特徴点を既知の特徴点として記憶することにより前記物体情報を更新することを特徴する。
この発明によれば、既知と見なされた特徴点が存在する画像中の未知の特徴点と既知の特徴点との相対関係を求め、該未知の特徴点を既知の特徴点として記憶することにより前記物体情報を更新するようにしたため、物体情報の更新を自動的に行うことができるという効果が得られる。
【００１４】
本願発明の一態様は、移動体の自己位置を検出する位置検出方法であって、前記位置検出方法は、前記移動体の前方視野の画像を取得する画像取得過程と、前記画像中の特徴点群を抽出する特徴点群抽出過程と、予め画像取得過程で得られた画像における特徴点群と当該特徴点群を得た位置を対応させて複数記憶しておき、新たに得られた画像の特徴点群と予め記憶された特徴点群の相関を算出して、自己の位置を算出する自己位置検出過程とを有することを特徴とする。
この発明によれば、予め画像取得過程で得られた画像における特徴点群と当該特徴点群を得た位置を対応させて複数記憶しておき、新たに得られた画像の特徴点群と予め記憶された特徴点群の相関を算出して、自己の位置を算出するようにしたため、幾何学的にロボットの位置が得られない場合であっても過去の位置検出結果に基づいて自己の位置を検出することができるという効果が得られる。
【００１５】
本願発明の一態様は、移動体の自己位置を検出する位置検出プログラムであって、前記位置検出プログラムは、前記移動体の前方視野の画像を取得する画像取得処理と、前記画像と同一の視野を有し、前記画像を取得するのと同時に距離画像を取得する距離画像取得処理と、少なくとも連続する２フレームの画像からそれぞれ特徴点を抽出する特徴点抽出処理と、前記特徴点抽出処理において抽出された特徴点の２フレーム間の位置の変位量を前記距離画像に基づいて算出し、当該変位量に基づいて各特徴点が静止物体であるか移動物体であるか判断し、前記静止物体であると判断された前記特徴点を基準特徴点として選択する基準特徴点選択処理と、前記距離画像から前記基準特徴点に対応する距離を取得し、前記基準特徴点と自己位置との相対的位置の前記２フレーム間の移動量を算出し、前記移動量に基づいて現在の自己位置を検出する位置検出処理とをコンピュータに行わせることを特徴とする。
この発明によれば、連続する２フレームの画像から同一の静止物体を抽出し、この静止物体の変位を求め、この変位量から自己の移動量を求めるようにしたため、精度良く自己位置の検出を行うことができるという効果が得られる。また、静止物体の抽出を自己で行うようにしたため、予め静止物体の位置が定義されたマップ等を備える必要がなく、構成を簡単にすることが可能となる。さらに、道路地図データ等のマップを備える必要がないために、構成を簡単にすることができるとともに、未知の場所へ移動することが可能となり移動体の行動範囲の制限をなくすことができるという効果も得られる。
【００１６】
本願発明の一態様は、移動体の自己位置を検出する位置検出プログラムであって、前記位置検出プログラムは、前記移動体の前方視野内の画像を取得する画像取得処理と、前記移動体の移動に当たって基準となる基準特徴点を前記画像に基づいて決定する基準点決定処理と、自己の移動制御と前記基準点の観測量を拡張カルマンフィルタに代入して自己の位置を検出する位置検出処理とをコンピュータに行わせることを特徴とする。
この発明によれば、自己の移動制御と前記基準点の観測量を拡張カルマンフィルタに代入して自己の位置を検出する位置検出手段を備えたため、より正確な自己位置の検出が可能になるという効果が得られる。
【００１７】
本願発明の一態様は、移動体の自己位置を検出するための位置検出プログラムであって、前記位置検出プログラムは、前記移動体の前方視野の画像を取得する画像取得処理と、前記画像と同一の視野を有し、前記画像を取得するのと同時に距離画像を取得する距離画像取得処理と、得られた画像からそれぞれ特徴点を抽出する特徴点抽出処理と、予め記憶された物体情報と抽出された特徴点とを比較して、相関が高い特徴点を既知と特徴点と見なして自己位置を算出するための基準特徴点とする基準特徴点選択処理とをコンピュータに行わせることを特徴とする。
この発明によれば、予め記憶された物体情報と抽出された特徴点とを比較して、相関が高い特徴点を既知と特徴点と見なして自己位置を算出するための基準特徴点とするようにしたため、より正確に自己の位置を検出することができるという効果が得られる。
【００１８】
本願発明の一態様は、前記特徴点選択処理は、既知と見なされた特徴点が存在する画像中の未知の特徴点と既知の特徴点との相対関係を求め、該未知の特徴点を既知の特徴点として記憶することにより前記物体情報を更新することを特徴する。
この発明によれば、既知と見なされた特徴点が存在する画像中の未知の特徴点と既知の特徴点との相対関係を求め、該未知の特徴点を既知の特徴点として記憶することにより前記物体情報を更新するようにしたため、物体情報の更新を自動的に行うことができるという効果が得られる。
【００１９】
本願発明の一態様は、移動体の自己位置を検出するための位置検出プログラムであって、前記位置検出プログラムは、前記移動体の前方視野の画像を取得する画像取得処理と、前記画像中の特徴点群を抽出する特徴点群抽出処理と、予め画像取得処理で得られた画像における特徴点群と当該特徴点群を得た位置を対応させて複数記憶しておき、新たに得られた画像の特徴点群と予め記憶された特徴点群の相関を算出して、自己の位置を算出する自己位置検出処理とをコンピュータに行わせることを特徴とする。
この発明によれば、予め画像取得処理で得られた画像における特徴点群と当該特徴点群を得た位置を対応させて複数記憶しておき、新たに得られた画像の特徴点群と予め記憶された特徴点群の相関を算出して、自己の位置を算出するようにしたため、幾何学的にロボットの位置が得られない場合であっても過去の位置検出結果に基づいて自己の位置を検出することができるという効果が得られる。
【００２０】
【発明の実施の形態】
＜第１の実施形態＞
以下、本発明の第一の実施形態による位置検出装置を図面を参照して説明する。図１は同実施形態の構成を示すブロック図である。ここでは、図１に示す位置検出装置は屋内を移動する自律走行のロボットに備えられているものとして説明する。この図において、符号１は、ロボットが移動する際の移動方向の視野内に存在する物体を撮像する２台のカメラであり、所定の間隔で設置され、互いの視野角は一致している。符号２は、カメラ１において得られた画像の１フレーム分をそれぞれ記憶する距離画像記憶部であり、２フレーム分の画像メモリからなる。符号３は、画像記憶部２に記憶されている２フレームの画像から距離画像を生成する距離画像生成部である。符号４は、距離画像生成部３において生成された距離画像を記憶する距離画像記憶部である。符号５は、画像記憶部２または距離画像記憶部４に記憶されている画像から特徴点を抽出する特徴点抽出部である。符号６は、特徴点抽出部５における特徴点抽出結果に基づいて自己の位置の検出を行う位置検出部である。符号７は、位置検出部６による位置検出結果を参照してロボットの移動を制御する移動制御部である。
【００２１】
ここで、以下の説明において用いる座標系を定義する。ロボットが初期状態であるときの前方の距離方向をＸ軸、ロボットの左右方向をＹ軸、鉛直方向をＺ軸とし、これらの３軸は互いに直交している。また、以下でいう距離とは、カメラ１から各物体までの直線距離である。したがって、距離画像データは、カメラ１の視野における物体表面の測定点の３次元座標値の集合である。
【００２２】
次に、図面を参照して、図１に示す位置検出装置の動作を説明する。図２は、特徴点抽出部５及び位置検出部６が画像記憶部２に記憶されている輝度画像と、距離画像記憶部４に記憶されている距離画像とから、特徴点を連続時間でトラッキングし、その位置情報を基に自己の位置検出を行う動作を示すフローチャートである。図２のフローチャートに示す各処理は、カメラ１によって画像を撮像する度に繰り返し実行される。
【００２３】
まず、２台のカメラ１は、ステレオカメラとして動作し、各カメラで輝度画像を取得し、画像記憶部２へそれぞれ格納する。続いて、画像記憶部２に輝度画像が記憶されたのを受けて距離画像生成部３は、２つのカメラ１で取得した２枚の輝度画像からそれぞれの画像の画素の対応点を求めることによって距離画像を生成し、距離画像記憶部４へ格納する。ここでは、少ない画素でより精度の良い距離画像を得るために画素間を補間するいわゆるサブピクセルを用いて距離画像を作成する。なおここでは、一例として各画素の輝度画像及び距離画像を２５６階調で表現するものとする。通常の画像の画素は、センサの視野内における物体表面の輝度を表現したものであるが、距離画像の画素は、センサの視野内における物体表面までの距離を２５６階調で表現したものである。したがって、距離画像中の画素の位置によってカメラ位置からの３次元空間に存在する物体表面上の１点へのＸＹＺ座標を特定することができる。なお、より高精度な距離精度が必要な場合には、カメラに換えてレーザレーダ、ミリ波レーダや超音波を用いたレーダを併用するなどして精度を向上させることもできる。また、カメラ１は赤外線カメラでもよい。さらに、距離データを２５６階調で表現せずに、距離センサの出力値をそのまま使用するようにしてもよい。カメラ１と距離画像生成部３は、この動作を繰り返し実行する。一方、特徴点抽出部５は、画像記憶部２に記憶されている画像を読み出す（ステップＳ１）。
【００２４】
次に、特徴点抽出部５は、読み出した輝度画像から特徴点を抽出して、この特徴点を特徴点抽出部５内に保存する（ステップＳ２）。ここでいう特徴点とは、画像処理で用いられる不変特徴量に相当し、画像中に現れている像を特徴付ける点のことである。この特徴点の抽出は、例えば輝度画像に対してエッジ検出処理を施し、検出されたエッジを構成する点を選択することによって行う。なお、特徴点抽出部５は、距離画像記憶部４に記憶されている距離画像を読み出し、この距離画像から特徴点を抽出するようにしてもよい。特徴点は、連続して取り込まれた２枚の輝度画像を用いて作成される。ここでは、それぞれの時刻を「ｔ」と「（ｔ＋１）」と表記する。また、特徴点抽出においては、左右いずれか一方のカメラ１から得られた輝度画像を用いており、時刻（ｔ＋１）の入力画像から抽出した特徴点の集合と、入力画像の直前の時刻ｔの画像から抽出した特徴点の集合の２種類の特徴点が保存される。この特徴点は、画像上の座標値が保存される。
【００２５】
次に、これらの２つの異なる時刻の特徴点の対応点を求めるために、位置検出部６は、特徴点抽出部５内に保存されている入力画像の特徴点を１つ読み出す（ステップＳ３）。続いて、位置検出部６は、特徴点抽出部５内に保存されている直前の画像の特徴点を１つ読み出す（ステップＳ４）。ここで、直前の時刻ｔとは、現在説明しているステップＳ１〜Ｓ１７が前回行われた時刻であり、２つの異なる時刻ｔと（ｔ＋１）との対応点は、ステップＳ１〜Ｓ１７が２回以上実行されることで得られる。よって初めてステップＳ１〜Ｓ１７が実行されるときは、例外処理としてステップＳ３まで実行してステップＳ１へ戻ることになる。
【００２６】
次に、位置検出部６は、２つの特徴点の確からしさＣAを算出し時刻ｔで保存された特徴点とパターンマッチングを行う（ステップＳ５）。ここで、図３を参照して、２つの特徴点の確からしさＣAを算出する処理の詳細を説明する。まず、特徴点を抽出した入力画像に対して、特徴点の周囲のｍ画素×ｎ画素（ただし、ｍ、ｎは２以上の自然数）を選択する。そして、直前画像の特徴点の周囲のｍ画素×ｎ画素（ただし、ｍ、ｎは２以上の自然数）を選択する。このとき、２つの特徴点の周囲の画素の選択する大きさ（ｍ×ｎ）は任意に決定してよいが、２つの選択する画素数は同一でなければならない。
【００２７】
この選択動作によって、２５６階調の輝度値（０〜２５５の整数）を有したｍ×ｎ個の輝度値の集合、すなわちｇ１＝｛ｇ１₁，ｇ１₂，・・・，ｇ１_mn｝、ｇ２＝｛ｇ２₁，ｇ２₂，・・・，ｇ２_mn｝が得られる。そして、得られた２つの輝度値の集合のうち、対応する画素毎の輝度値の減算を行い、その減算結果を加算することによって、確からしさＣAを算出する。すなわち、
ＣA＝｜（ｇ１₁−ｇ２₁）｜＋｜（ｇ１₂−ｇ２₂）｜＋…＋｜（ｇ１_mn−ｇ２_mn）｜によって算出される。この演算によれば、２つの輝度値集合が全く同一であれば確からしさＣAは「０」となるはずである。したがって、この演算によって算出されたＣAが小さいほど２つの特徴点が同一である可能性が高くなる。
【００２８】
次に、位置検出部６は、算出した確からしさＣAとしきい値ＴCを比較して（ステップＳ６）、ＣAがＴCより小さい場合、連続時間の特徴点移動量としての座標値から変位ベクトルｄａを算出して、位置検出部６内に保存する（ステップＳ７）。一方、ＣAがＴCより小さい場合、ステップＳ８へ進む。
【００２９】
次に、位置検出部６は、直前の画像の特徴点の全てについて処理したか否かを判定し（ステップＳ８）、処理していなければステップＳ４へ戻り処理を繰り返す。一方、直前の画像の特徴点の全てについて処理が終了した場合、位置検出部６は、入力画像の特徴点の全てについて処理したか否かを判定し（ステップＳ９）、処理していなければステップＳ３に戻り処理を繰り返す。一方、入力画像の特徴点の全てについて処理が終了した場合、ステップＳ１０へ進む。
【００３０】
このように、ステップＳ３〜Ｓ９の処理によって、入力画像（時刻ｔ＋１）の特徴点と直前画像（時刻ｔ）の特徴点との対応付けがされ、さらに、対応付けされた特徴点の変位ベクトルｄａが算出されて保存された状態となる。ただし、変位ベクトルｄａが算出されるのは、２つの特徴点の確からしさが所定のしきい値Ｔcを超えている場合のみであるために、算出される変位ベクトルｄａの個数は処理の度に異なる。
ここでは、直前の画像の特徴点の全てについて処理を行ったが、計算処理の負荷を低減するために、必要とする移動量に合わせ、求める特徴点の近傍のみを計算して求めるようにしてもよい。
【００３１】
次に、位置検出部６は、保存されている変位ベクトルｄａを読み出し、平均変位ベクトルＴｄを算出する（ステップＳ１０）。
次に、位置検出部６は、保存されている変位ベクトルｄａを１つずつ読み出す（ステップＳ１１）。
そして、位置検出部６は、｜ベクトルＴｄ−ベクトルｄａ｜を算出してこの値が、所定のしきい値Ａより小さいか否かを判定する（ステップＳ１２）。この判定によって、算出された変位ベクトルｄａの各々が平均変位ベクトルＴｄとどの程度の差を持っているかが判定される。
【００３２】
この判定の結果、算出した差（｜ベクトルＴｄ−ベクトルｄａ｜）がしきい値Ａより小さい場合、位置検出部６は、該当する変位ベクトルｄａを算出するときに用いられた特徴点を静止物体であると判断する（ステップＳ１３）。一方、しきい値Ａより大きい場合、位置検出部６は、該当する変位ベクトルｄａを算出するときに用いられた特徴点を移動物体と判断する（ステップＳ１４）。この処理は、算出された全ての変位ベクトルｄａに対して行われ（ステップＳ１５）、これによって、特徴点の各々について、静止物体であるか移動物体であるかの判断が行われる。
【００３３】
次に、位置検出部６は、静止物体であると判断された特徴点のうち、確からしさＣAが上位の特徴点を抽出する（ステップＳ１６）。ここでいう確からしさＣAが上位とは、ＣAの値が小さいほど上位であることを意味する。この抽出処理によって抽出される特徴点は、入力画像（時刻ｔ＋１）の特徴点と対になる直前の画像（時刻ｔ）の特徴点であり、これらの特徴点が自己位置算出の基準特徴点となる。
【００３４】
次に、位置検出部６は、抽出した各々の基準特徴点と距離画像記憶部４に記憶された距離画像との対応点より特徴点の距離を求め、その特徴点とロボットの相対的位置の移動量に基づいて、直前の画像を取得した時点から現時点の画像（入力画像）を取得した時点までにロボットが移動した量を算出する（ステップＳ１７）。ステップＳ１７の処理が終了しロボットの位置が決定されると再びステップＳ１へ戻り、記憶されている前回対応がとれた特徴量に対して同様の処理を繰り返す。
【００３５】
ここで、図４を参照して、自己位置を算出する原理を説明する。ここでの座標系は、ロボットが初期状態であるときに定義される座標系であり、ロボットの前方方向をＸ軸、このＸ軸に直交する方向をＹ軸とする。図４において、ａ、ｂはそれぞれ建造物内の柱などの任意の特徴点であり、前述した処理によって抽出される。ロボットが画像を取得することによって得られる情報は、図４に示す特徴点ａの相対的位置（ｘ_a，ｙ_a）及び特徴点ｂの相対的位置（ｘ_b，ｙ_b）である。
ここでのφはｚ軸回りのロボットの回転角で反時計回りを正とする。
【００３６】
ここで、ａ，ｂの絶対位置は既知であり、ａ（ａ_x，ａ_y）、ｂ（ｂ_x，ｂ_y）と仮定すると、ａｂの相対的座標値（ｘ_a，ｙ_a），（ｘ_b，ｙ_b）を用いてロボットの絶対位置（ｘ_t，ｙ_t）はａ，ｂ点を中心とした半径ｌ₁＝√（ｘ_a ²＋ｙ_a ²），ｌ₂＝√（ｘ_b ²＋ｙ_b ²）の円の交点となる。原点（０，０）からロボット位置（ｘ_t，ｙ_t）までのベクトルをＰ→（以下の説明におけるベクトル表現を「Ｐ→」とし、図面においては、Ｐの頭に→を付ける）とし、原点からのａ，ｂまでのベクトルをａ→，ｂ→、ロボットの位置（ｘ_t，ｙ_t）からａ，ｂそれぞれの点へのベクトルをｌ₁→，ｌ₂→とすると、ロボットの回転角φが０の場合、Ｐ→＝（ａ→）−（ｌ₁→），Ｐ→＝（ｂ→）−（ｌ₂→）の関係が成り立つ。また、ロボットの回転角φが０でない場合は、ベクトルｌ₁→，ｌ₂→をそれぞれ（−φ）だけ回転させたベクトルが成立する。また、ロボットのＺ軸方向の回転角φを考慮すると、ａに関していえば、
ｔａｎ^-1（ａ_y／ａ_x）−ｔａｎ^-1（ｙ_a／ｘ_a）＝φ
と決定される。また、ｂに関していえば、
ｔａｎ^-1（ｂ_y／ｂ_x）−ｔａｎ^-1（ｙ_b／ｘ_b）＝φ
と決定される。
【００３７】
これによって、ロボットの回転角φとロボットの位置（ｘ_t，ｙ_t）を求めることができる。上記の説明においては、ａ，ｂを既知として仮定したが、ここでは、未知の物体も扱うので、ａ，ｂの絶対位置は求められていないので、時刻ｔにおいて、ロボットの位置を基にａ，ｂの位置を決定し、時刻ｔ＋１において得られたａ，ｂの相対位置の変化量ｄａを基に、ロボット位置の変化量（ロボットの移動量）を求め、既知の絶対位置座標にこの変化量を加算することで、時刻ｔ＋１のロボット位置を求めることができる。もちろんａ，ｂが既知である場合はａ，ｂの相対位置を基にロボット位置を容易に決定できる。
【００３８】
算出された位置は、移動制御部７へ通知され、移動制御部７においては、この位置に基づいて、ロボットの移動制御が行われる。また、移動制御部７は、位置検出をする必要がある場合に、特徴点抽出部５に対して位置検出指示を出力するようにして、この位置検出指示を受けて、特徴点抽出部５は図２に示す処理を開始するようにしてもよい。このようにすることによって、例えば、図１に示す位置検出装置を自動車に備え、トンネル等へ進入した時点で画像による位置検出を行うようにすれば、ＧＰＳによる位置検出ができなくなった場合であっても位置検出を継続することができる。
【００３９】
このように、連続して取り込まれる画像から同一の静止物体を連続してトラッキングすることで、この静止物体の刻々と変化する位置の変位を求め、この変位量から自己の移動量を刻々と求めるようにしたため、精度良く自己位置の検出を行うことができる。ここでは、連続する２枚の画像でトラッキング処理を行う例を説明したが、信頼性や精度向上のために２枚以上の画像から特徴点の移動経歴を求め、静止物体を抽出しトラッキングするようにしてもよい。
このように、ターゲットとなる特徴点の位置が既知の場合は、ロボットの絶対的位置を決定でき、また、未知の場合は時刻ｔにおけるターゲットの相対的位置とロボット位置（ｘ_t，ｙ_t）によりターゲットの位置を決定し、そのターゲット位置の時刻ｔ＋１の位置の変化量をもとにロボット位置（ｘ_t+1，ｙ_t+1）を求めることができる。また、連続時間Ｋだけ同じターゲットをトラッキングし続けた場合は、同様にして、（ｘ_t+k，ｙ_t+k）のｋ時間の変化量を基に時刻ｔ＋ｋの位置を検出することができる。
【００４０】
なお、図１に示す距離画像生成部３は、超音波や電磁波を用いたレーダ等に置き換えてもよい。このとき、レーダの計測視野を図１に示すカメラ１と同一になるように設定する。また、レーダ等を用いて距離画像を得る場合は、輝度画像を得るカメラ１は１台のみを備えていればよい。
【００４１】
また、移動体が移動している場合、特徴点に近づくに従いフレームから特徴点が外れてしまうことがあり、位置の検出ができなくなってしまうため、複数の特徴点全てについて図２に示す処理を行うことによって、移動量を算出する特徴点を次々に切り替えていくようにする。ただし、この場合、移動体から離れるにしたがって、計測距離の誤差が移動体から近い場合に比べて大きくなるため、フレーム内に存在する特徴点のうち距離が短い特徴点を優先的に選択して移動量の算出を行うようにすれば、移動の精度が悪化することを防止することができる。また、移動体の回転（自転）によって、フレームから１度外れてしまった物体は位置を記憶しておき、同じ物体が再びフレーム内に戻ってきたときに同一物体としてトラッキングするようにしてもよい。
【００４２】
＜第２の実施形態＞
次に、従来技術のように詳細な地図を持たずに、最小限の地図情報に基づいて自己の位置を検出する第２の実施形態を図５〜９を参照して説明する。図５は、他の実施形態の構成を示すブロック図である。図５に示すブロック図が図１に示すブロック図と異なる点は、新たに物体情報記憶部８を設けた点である。この物体情報記憶部８は、特徴量として予め既知の複数の静止物体の特徴点の集合と、その静止物体の絶対位置が対応付けられて物体の地図として記憶されているものであり、特徴点抽出部５、及び位置検出部６によって参照される。ここでいう既知の静止物体とは、部屋の中の柱や植木、ロッカーなどの家具などである。また、どのような位置の物体を記憶しておくかは特に制限はないが、移動体が移動する際に行動を変化させる位置、すなわち曲がり角、ドアの前等の付近に存在する特徴物を優先的に記憶するようにしておけば、効率良くかつ精度良く移動体の制御が可能となる。
【００４３】
次に、図６を参照して、図５に示す位置検出装置の動作を説明する。ここで、カメラ１、画像記憶部２、距離画像生成部３、距離画像記憶部４の動作は、前述した実施形態における動作と同様なので説明を省略する。図６のフローチャートに示す各処理は、カメラ１によって画像を撮像する度に繰り返し実行される。
まず、特徴点抽出部５は、画像記憶部２に記憶されている画像を読み出す（ステップＳ２１）。そして、特徴点抽出部５は、読み出した輝度画像から特徴点を抽出して、この特徴点を特徴点抽出部５内に保存する（ステップＳ２２）。
【００４４】
ここでいう特徴点とは、画像処理で用いられる不変特徴量に相当し、画像中に現れている像を特徴付ける点のことである。また、特徴量とは、物体形状を構成する複数の特徴点群のことである。この特徴点の抽出は、例えば画像に対してエッジ検出処理を施し、検出されたエッジを構成する特徴点を複数選択することによって行う。なお、特徴点抽出部５は、距離画像記憶部４に記憶されている距離画像を読み出し、この距離画像から特徴点を抽出するようにしてもよい。特徴点は、時刻ｔの画像（直前の画像）から抽出した特徴点の集合が保存され、この特徴点は、画像上の座標値が保存される。
【００４５】
次に、特徴点抽出部５は、内部に保存されている特徴点を１つずつ読み出し、物体情報記憶部８に記憶されている複数の物体情報の特徴点群（特徴量）とパターンマッチングすることによって比較する（ステップＳ２３）。この比較処理は、前述した実施形態の処理（図２に示す処理）と同様の処理によって行う。またここでいう物体情報とは、予め得られた画像から静止物体の特徴点を抽出しておき、この特徴点とこの静止物体が存在する位置の絶対座標からなる情報である。ここで、用いるパターンマッチング手段としては単純なテンプレートマッチング法を利用してもよい。
【００４６】
続いて、特徴点抽出部５は、ステップＳ２２で得られた特徴点と一致するまたは確からしさが大きい、すなわち相関が高い物体情報が物体情報記憶部８に存在したか、すなわち既知の物体があったか否かを判定する（ステップＳ２４）。この結果、既知の物体が存在した場合、位置検出部６は、この既知の静止物体と自己の相対位置から自己の絶対位置を求める（ステップＳ２５）。この絶対位置は、既知の物体の絶対座標と、距離画像記憶部４に記憶された距離画像から得られる既知の物体と自己の相対位置関係とから算出する。
【００４７】
次に、ステップＳ２４において、既知の物体が存在しなかった場合、特徴点抽出部５は、ステップＳ２２において抽出した特徴点が移動物体であるか静止物体であるかを判定する（ステップＳ２６）。判定の結果得られた静止物体は直前に求められた自己の位置情報を基に自己との相対的位置関係よりそれぞれ絶対位置を求める（ステップＳ２７）。この判定処理は、図２に示すステップＳ４〜Ｓ１３，Ｓ１４の処理によって行う。そして、この判定処理の結果得られた静止物体は直前の処理（時刻ｔ）においてもトラッキングできていたかを判定する（ステップＳ２８）。この判定は、直前に物体情報記憶部８に記憶された特徴量と今回の特徴量をパタンパターンマッチングして一致量を比較し判定する。また、直前にステップＳ３０で物体情報記憶部８に記憶された特徴量の絶対位置とステップＳ２７で求めた位置とを比較し、近傍であった場合トラッキングできたと判定を加えてより精度を上げることもできる。この判定の結果、直前の処理においてもトラッキングできた特徴点である場合、位置検出部６は、静止物体と自己との相対位置から自己の絶対位置を求める（ステップＳ２９）。
【００４８】
一方、ステップＳ２８において、特徴点がトラッキングできておらず新規のものである場合、特徴点抽出部５は、新規の静止物体の特徴点と直前のステップＳ２５またはＳ２９で求められた時刻ｔの自己の絶対位置を基に新規の特徴点の絶対位置を決定し物体情報記憶部８に記憶する（ステップＳ３０）。この移動量の算出は、図２に示すステップＳ３〜Ｓ１７の処理によって行う。
【００４９】
前述したステップＳ２８での判定において、近傍か否かに判断は距離のしきい値を定め、直前の位置と今回の位置がこの距離内にあるか否かによって判断する。しかし、画像によっては得られる距離にばらつきやノイズが大きい場合は、統計的処理を行ってもよい。これは、連続時間だけある特徴量がトラッキングできていたとすると、その特徴量の画像上の位置に対する距離画像によって得られる自己に対する相対的位置（ｘ，ｙ）のばらつきの分散値を刻々計算し、分散値が収束した時点で、分散の範囲内にあるものを同一の物体とし、分散外にあるものを別物体と判定するようにしてもよい。この分散値はステレオカメラの特性や物体によっても異なるため、予め設定された分散既定値を用いてもよいし、物体情報記憶部８に物体毎に設定し記憶された既定値を用いてもよい。また、ステレオカメラの特性として距離の２乗で精度が落ちることから分散値を距離の２乗で変化させるなどの関数を用いてもよい。
【００５０】
ここで、ステップＳ２５、Ｓ２９における絶対位置の算出方法を図７を参照して説明する。図７において、符号ＡＲは、行動エリアを示しており、予めこの行動エリアＡＲ内の原点Ｇが決定されている。符号Ｒは、ロボットであり、符号Ｋ４は、既知の静止物体であり、符号Ｕ５は、未知の物体である。図７においては、簡単にするためにロボットＲの回転角φは無視している。このような状態であるときに、絶対位置が既知である物体Ｋ４の絶対座標値（ｘ₁，ｙ₁）と、この既知物体Ｋ４とロボットＲに相対位置（ｘ₂、ｙ₂）に基づいて、ロボットＲの絶対位置（Ｒ_x，Ｒ_y）は、（Ｒ_x，Ｒ_y）＝（ｘ₁＋ｘ₂，ｙ₁−ｙ₂）によって自己の絶対位置を求める。また、同様に未知の物体Ｕ５の絶対位置の座標値（ｘ₄，ｙ₄）は、（ｘ₄，ｙ₄）＝（ｘ₁＋ｘ₂＋ｘ₃，ｙ₁−ｙ₂＋ｙ₃）によって求める。図８に示すように、既知の静止物体Ｋと未知の静止物体Ｕがカメラによって同時に撮像された場合に、未知の物体Ｕの絶対座標を計算によって求めておくようにしたため、ロボットＲが符号Ｒ’の位置まで移動した場合に既知物体Ｋを撮像することができず未知の物体Ｕのみが撮像された場合でも、予め計算しておいた絶対座標を利用することが可能となる。
【００５１】
なお、ステップＳ２８の判定は、抽出された特徴量が複数の場合は、全てに対して行われ、ステップＳ２９においては全ての特徴量との相対位置から自己の位置を求め、平均値によって自己の絶対位置を求めるようにする。
このように、既知の物体、未知の物体、未知の物体のうち計算によって位置が既知になった物体の状況に応じて、自己の絶対位置を求めるようにしたため、移動制御部７は精度良く移動制御を行うことが可能となる。
【００５２】
次に、図９を参照して、具体例を挙げて動作を説明する。ここでは、ロボットは、Ａ地点からＢ地点へ移動し、さらにＣ地点へ移動する場合の動作を例にして説明する。まずＡ地点において、ステップＳ２３の比較処理の結果、絶対座標が既知である物体Ｋ１、Ｋ２、Ｋ３が存在することを認識する。そして、位置検出部６は、物体Ｋ１、Ｋ２、Ｋ３の３つの物体のうち、物体Ｋ３は距離が長いため、物体Ｋ１、Ｋ２の絶対位置の座標値から自己の絶対位置を求める（ステップＳ２５）。このとき、位置検出部６は、未知物体Ｕ１、Ｕ２の絶対位置の座標値を計算によって求めて、内部記憶しておく（ステップＳ３０）。そして、ロボットは、絶対位置が既知となった未知物体Ｕ１、Ｕ２の位置に基づいて自己位置を求めながら（ステップＳ２９）移動し、さらに未知物体Ｕ１、Ｕ２が撮像できない位置へ到達した場合に、既知物体Ｋ３に基づいて自己位置を求めながら（ステップＳ２５）、Ｂ地点まで移動し、Ｂ地点の絶対位置を求める。これによって、未知物体Ｕ１、Ｕ２に関する位置情報と物体情報が更新されることになる。
【００５３】
次に、ロボットは、方向転換し、Ｃ地点方向へ向く。このとき、カメラ１には、未知の物体Ｕ３、Ｕ４が撮像される。ただし、この場合、９０°向きを変えたため、未知の物体Ｕ３、Ｕ４が撮像された画像には、既知の物体が撮像されない場合もある。このような時には、未知物体Ｕ３、Ｕ４に基づいて移動量を求め、Ｂ地点の絶対座標値に求めた移動量を加算することによって自己位置を求める（ステップＳ２９）。そして、Ｃ地点まで移動する途中で既知物体が撮像された場合、この既知物体に基づいて自己の絶対位置を求め、移動量によって求めた絶対位置の誤差をリセットする。
【００５４】
このように、未知の物体の位置に基づいて、自己の絶対位置を求める場合、距離画像から得られる相対位置の誤差が累積されるが、既知の物体が撮像された場合に、自己の位置をこの既知の物体から求めるようにしたため、累積した誤差をリセットすることが可能となり、精度の良い位置検出が可能となる。
次に、もう一度同じエリアにおいて移動を行う際は、地点Ａに移動した場合、前回未知物体Ｕ１、Ｕ２は地図位置情報と物体情報が内部に記憶されているため既知物体として使われる。また、未知物体Ｕ３、Ｕ４も同様である。また、前回既知の物体が今回存在することが認識できない場合は、その物体が移動物体である可能性があるため、物体情報として移動物体である可能性があること記憶して検出対象から外すなどの処理を加えてもよい。
【００５５】
ところで、画像から得られる距離情報にはばらつきが大きいことがあり、上記の方法によってロボットの位置を求めるのは困難である場合がある。そのため、ロボットの位置は、抽出した基準特徴点に基づき、カルマンフィルタを使用して算出するようにしてもよい。このカルマンフィルタを用いることによって、距離情報のばらつきなどを考慮してロボットの位置を算出することができる。
【００５６】
ここで、カルマンフィルタを用いた場合のロボット位置算出の処理について説明する。ここで用いるカルマンフィルタは拡張カルマンフィルタであり、状態方程式を（１）式に示す。ここで、ロボットの状態を表現する変数をｘ，ｙ，φ，Ｔの４つで表現し、
【数１】

と定義すると、ｘ_t，ｙ_tは、ｘｙ軸上のｘ，ｙ値、φ_tはｚ軸まわりの回転角、Ｔ_tは時間ステップｔから（ｔ＋１）で移動する距離である。
【数２】

（１）式において、右辺２番目の項は移動制御部７からの時刻ｔにおける移動制御変化量であり、３番目の項は時刻ｔにおけるロボット制御のシステムノイズを表す。ここで用いる座標系は図５と同様であり、また、図１０にロボット位置の状態を表す各パラメータの意味を示す。
また、（２）式は、特徴点ａの観測方程式であり、（３）式は、特徴点ｂの観測方程式であり、これらの式はロボットから見た特徴点ａ，ｂの相対的計測値を示すものである。この計測式は、図５に示す座標系に基づいて幾何学的に作成される。この計測量を表現する変数をｘ，ｙ，θの３つで表現し、
【数３】

となる。ここで、（２）、（３）式の右辺２項目は計測ノイズを表す。
【００５７】
上記の（１）〜（３）式は図２に示すステップＳ２５、Ｓ２９における処理に用いられるものであり、これらの式を画像が取得される度に演算を行うことによって、ロボットの回転角φ_t+1とロボットの位置（ｘ_t+1，ｙ_t+1）を求めることができる。この拡張カルマンフィルタを用いることによって、前述した幾何学的に算出するのに比べ、システムノイズ及び計測ノイズが考慮されているために精度良くロボットの位置検出を行うことができる。また、カルマンフィルタを用いたときは、ステップＳ２８の判断において特に明記していないが、カルマンフィルタ内で収束する特徴点に対する観測値の共分散を、その特徴点の分散値として用いることもできる。
【００５８】
なお、本実施形態においては対象となる物体の座標はｘ，ｙ平面で説明したが、高さ方向（ｚ方向）に拡張にして自己の位置を、高さを含めて決定するようにしてもよい。
【００５９】
＜第３の実施形態＞
次に、ロボットが移動する環境において、オクルージョンなどの要因で特徴物に対する相対位置を個別に決定できない場合に、複数の特徴点群とこれらの特徴点群が得られた場所の位置の情報を全て予め記憶しておき、この情報を参照しながら自己の位置を決定する第３の実施形態を説明する。図１１は、第３の実施形態における位置検出装置の構成を示すブロック図である。この図において、符号１はカメラであり、符号５は特徴点抽出部である。符号８は、物体情報記憶部である。符号９は、２つの特徴点の相関を求める相関処理部である。符号１０は、相関処理部９の処理結果に基づいて自己位置を決定する自己位置決定部である。図１１においては、得られた画像を記憶する画像記憶部２、及び移動制御部７は省略されている。また、第３の実施形態においては、距離画像を必要としないため、関係する構成要素は削除されている。
【００６０】
まず、図１３、１４を参照して自己位置を求める原理を簡単に説明する。図１３は、位置Ｍにおいてロボットの進行方向Ｆ→のシーンをカメラで撮影し、得られた画像から特徴点群を選択している図である。このとき、選択される特徴点群をＷとすると、特徴点群Ｗは位置Ｍと関連付けられ、物体情報記憶部８に記憶される。ここでの特徴点群は個々にオクルージョンなどにより相対位置を特定できないものであるため、特徴点全体に対し記憶した位置を関連付けることとなる。
【００６１】
図１４は、特徴点群を記憶する位置が９ヶ所（Ｍ１〜Ｍ９）ある時の例であり、ロボットが通過するであろう位置に予め設定されるものである。予め設定された各位置Ｍ１〜Ｍ９のそれぞれからロボットの進行方向Ｆ→の方向に向けられたカメラで撮像された画像の中から抽出した特徴点群をＷ１〜Ｗ９とすると、それぞれの位置（特徴点取得位置という）Ｍ１〜Ｍ９と特徴点群Ｗ１〜Ｗ９はそれぞれ対になるように対応付けられて物体情報記憶部８に予め記憶される。そして、ロボットが未知の位置を通過中に、進行方向Ｆ→の方向に向けられたカメラで撮像された画像の中から抽出された特徴点群と予め記憶された特徴点群Ｗ１〜Ｗ９との相関を求め、この相関が最も高い特徴点群Ｗｎ（ｎは１〜９のいずれか）を自己が通過中の位置と判断し、この特徴点群Ｗｎと対になる特徴点取得位置Ｍｎ（ｎは１〜９のいずれか）が自己位置となる。例えば、特徴点群Ｗ４とカメラで撮像された画像から抽出された特徴点群とが最も良く相関が取れるとき、この特徴点群Ｗ４を取得した位置Ｍ４を自己位置と決定する。
【００６２】
次に、図１２を参照して、図１１に示す位置検出装置が自己位置を求める具体的な動作を説明する。
まず、特徴点抽出部５は、カメラ１で撮像された画像を読み出す（ステップＳ４１）。そして、特徴点抽出部５は、読み出しだ画像から全ての特徴点群を抽出し、内部に保存する（ステップＳ４２）。一方、相関処理部９は、物体情報記憶部８に記憶されている特徴点群を１つ読み出す（ステップＳ４３）。続いて、相関処理部９は、入力画像（カメラ１で撮像した画像）の特徴点を特徴点抽出部５から１つ読み出す（ステップＳ４４）。さらに、相関処理部９は、ステップＳ４３で読み出した特徴点群から１つの特徴点を選択する（ステップＳ４５）。
【００６３】
次に、相関処理部９は、ステップＳ４４で読み出した特徴点とステップＳ４５で選択した特徴点との確からしさＣAを算出し（ステップＳ４６）、予め決められたしきい値Ｔcと比較する（ステップＳ４７）。この結果、ＣA＞ＴｃでなければステップＳ４９へ進む。一方、ＣA＞Ｔｃであれば、処理ループ毎に求められたＣAの和Ｓを求める（ステップＳ４８）。ただし、ＣAの和Ｓは、特徴点群毎に求めるため、確からしさＣAの和Ｓは特徴点群の数だけ得られることとなる。そして、相関処理部９は、ステップＳ４３で読み出した特徴点群を構成する特徴点の全てについて、ステップＳ４５〜Ｓ４８の処理を繰り返す（ステップＳ４９）。
【００６４】
次に、相関処理部９は、ステップＳ４２で保存された特徴点群を構成する特徴点の全てについて、ステップＳ４４〜Ｓ４８の処理を繰り返し（ステップＳ５０）、さらに、ステップＳ４２で保存された特徴点群の全てに対してステップＳ４３〜Ｓ４８の処理を繰り返す（ステップＳ５１）。ステップＳ４２〜Ｓ４８における処理は、入力画像から抽出した特徴点群を構成する特徴点と物体情報記憶部８に記憶されている特徴点とをそれぞれ総当たりで確からしさを求めている処理である。なお、ステップＳ４４〜Ｓ５０の処理において、画像ピクセルの近いものを選択して確からしさを求めることで処理負荷を低減するようにしてもよい。
【００６５】
次に、自己位置決定部１０は、ステップＳ４８で求めた確からしさＣAの和Ｓが大きい特徴点群を選択する（ステップＳ５２）。そして、選択した特徴点群と対になる特徴点取得位置を物体情報記憶部８から読み出すことによって、自己の位置を求め（ステップＳ５３）、この位置を自己位置情報として出力する。
【００６６】
また、図１５に示すようにロボットのステレオカメラを構成する左右の２台のカメラを利用し、各カメラ毎に予め記憶された特徴点群Ｗと比較し、相関を演算するようにしてもよい。このようにすることによって、同一の特徴点群Ｗに対するカメラの画像の相関度の大きさの違いによって、自己の位置を決定することができる。すなわち、特徴点群Ｗに対応した位置Ｍに対して各カメラで検出された相関値の差から位置Ｍの左右のオフセット方向を検出できる。このことにより、特徴点群を得る位置Ｍのうち左右方向の数を少なくすることができるため、予め画像を撮像する手間が大幅に削減できる。また、ステレオカメラを構成するカメラが２台以上である場合は、左右方向に並ぶカメラを選択して利用すればよい。
【００６７】
このように、ロボットの行動可能範囲における特徴点群とこれらの特徴点群を取得した位置を、ロボット内に予め記憶しておき、ロボットが行動を起こすときに得られた画像から抽出された特徴点群と相関が高い特徴点群を記憶された中から選択し、この選択された特徴点群が取得された位置を自己位置とするようにしたため、ロボットが決められた屋内で行動する場合などにおいて、容易に自己位置検出行うことが可能となる。
【００６８】
前述した図１３、１４の説明においては、各位置Ｍ１〜Ｍ９における画像撮像の方向を１方向として説明したが、各位置において複数の方向の画像を撮像し、得られた画像の全てについて特徴点群を抽出して、物体情報記憶部８に記憶しておけば、各位置におけるロボットの進行方向が異なっても同様の処理によって、自己位置を検出することが可能となる。また、ロボットは、自己に備えられたカメラのズーム機能を用いて、拡大・縮小させた画像から特徴点群を抽出するようにしてもよい。このようにすることによって、図１４に示す位置Ｍ２とＭ５の中間に位置するような場合の相関を取りやすくすることができる。さらに、ロボットは、カメラが備えられた頭部を左右上下に振った状態で得られた画像から特徴点群を抽出するようにしてもよい。このようにすることによって、記憶された特徴点群を取得した時の進行方向と異なる行動をしている場合であっても特徴点の相関を取りやすくすることが可能となる。
【００６９】
また、前述した３つの実施形態における位置検出処理を、ロボットが移動する環境に応じて適宜組み合わせたり、必要な処理を選択するようことによって自己位置を検出するようにしてもよい。
【００７０】
また、図２、図６、図１２に示す処理を実現するためのプログラムをコンピュータ読み取り可能な記録媒体に記録して、この記録媒体に記録されたプログラムをコンピュータシステムに読み込ませ、実行することにより位置検出処理を行ってもよい。なお、ここでいう「コンピュータシステム」とは、ＯＳや周辺機器等のハードウェアを含むものとする。また、「コンピュータシステム」は、ＷＷＷシステムを利用している場合であれば、ホームページ提供環境（あるいは表示環境）も含むものとする。また、「コンピュータ読み取り可能な記録媒体」とは、フレキシブルディスク、光磁気ディスク、ＲＯＭ、ＣＤ−ＲＯＭ等の可搬媒体、コンピュータシステムに内蔵されるハードディスク等の記憶装置のことをいう。さらに「コンピュータ読み取り可能な記録媒体」とは、インターネット等のネットワークや電話回線等の通信回線を介してプログラムが送信された場合のサーバやクライアントとなるコンピュータシステム内部の揮発性メモリ（ＲＡＭ）のように、一定時間プログラムを保持しているものも含むものとする。
【００７１】
また、上記プログラムは、このプログラムを記憶装置等に格納したコンピュータシステムから、伝送媒体を介して、あるいは、伝送媒体中の伝送波により他のコンピュータシステムに伝送されてもよい。ここで、プログラムを伝送する「伝送媒体」は、インターネット等のネットワーク（通信網）や電話回線等の通信回線（通信線）のように情報を伝送する機能を有する媒体のことをいう。また、上記プログラムは、前述した機能の一部を実現するためのものであっても良い。さらに、前述した機能をコンピュータシステムにすでに記録されているプログラムとの組み合わせで実現できるもの、いわゆる差分ファイル（差分プログラム）であっても良い。
【００７２】
【発明の効果】
以上説明したように、この発明によれば、連続する２フレームの画像から同一の静止物体を抽出し、この静止物体の変位を求め、この変位量から自己の移動量を求めるようにしたため、精度良く自己位置の検出を行うことができるという効果が得られる。また、静止物体の抽出を自己で行うようにしたため、予め静止物体の位置が定義されたマップ等を備える必要がなく、構成を簡単にすることが可能となる。さらに、道路地図データ等のマップを備える必要がないために、構成を簡単にすることができるとともに、未知の場所へ移動することが可能となり移動体の行動範囲の制限をなくすことができるという効果も得られので、特に人間型に脚式移動ロボットに好適である。
また、この発明によれば、自己の移動制御と前記基準点の観測量を拡張カルマンフィルタに代入して自己の位置を検出する位置検出手段を備えたため、より正確な自己位置の検出が可能になるという効果が得られる。
【００７３】
また、この発明によれば、予め記憶された物体情報と抽出された特徴点とを比較して、相関が高い特徴点を既知と特徴点と見なして自己位置を算出するための基準特徴点とするようにしたため、より正確に自己の位置を検出することができるという効果が得られる。
また、この発明によれば、既知と見なされた特徴点が存在する画像中の未知の特徴点と既知の特徴点との相対関係を求め、該未知の特徴点を既知の特徴点として記憶することにより前記物体情報を更新するようようにしたため、地図情報及び物体情報の更新を自動的に行うことができるという効果が得られる。
【図面の簡単な説明】
【図１】本発明の第１の実施形態の構成を示すブロック図である。
【図２】図１に示す特徴点抽出部５及び位置検出部６の動作を示すフローチャートである。
【図３】２つの特徴点の確からしさＣAを算出する処理を示す説明図である。
【図４】ロボットの自己位置を求める場合の座標系を示す説明図である。
【図５】本発明の第２の実施形態の構成を示すブロック図である。
【図６】図５に示す特徴点抽出部５及び位置検出部６の動作を示すフローチャートである。
【図７】自己位置算出方法を示す説明図である。
【図８】自己位置算出方法を示す説明図である。
【図９】自己位置算出方法を示す説明図である。
【図１０】カルマンフィルタの状態モデルを示す説明図である。
【図１１】本発明の第３の実施形態の構成を示すブロック図である。
【図１２】図１１に示す位置検出装置の動作を示すフローチャートである。
【図１３】第３の実施形態における自己位置検出の原理を説明する説明図である。
【図１４】第３の実施形態における自己位置検出の原理を説明する説明図である。
【図１５】第３の実施形態における自己位置検出の原理を説明する説明図である。
【符号の説明】
１・・・カメラ、
２・・・画像記憶部、
３・・・距離画像生成部、
４・・・距離画像記憶部、
５・・・特徴点抽出部、
６・・・位置検出部、
７・・・移動制御部、
８・・・物体情報記憶部。

Claims

移動体の自己位置を検出する位置検出装置であって、
前記位置検出装置は、
前記移動体の前方視野の画像を取得する画像取得手段と、
前記画像取得手段と同一の視野を有し、前記画像取得手段が画像を取得するのと同時に距離画像を取得する距離画像取得手段と、
少なくとも連続する２フレームの画像からそれぞれ特徴点を抽出する特徴点抽出手段と、
前記特徴点抽出手段によって抽出された特徴点の２フレーム間の位置の変位量を前記距離画像に基づいて算出し、当該変位量に基づいて各特徴点が静止物体であるか移動物体であるか判断し、前記静止物体であると判断された前記特徴点を基準特徴点として選択する基準特徴点選択手段と、
前記距離画像から前記基準特徴点に対応する距離を取得し、前記基準特徴点と自己位置との相対的位置の前記２フレーム間の移動量を算出し、前記移動量に基づいて現在の自己位置を検出する位置検出手段と、
を備えたことを特徴とする位置検出装置。
移動体の自己位置を検出する位置検出方法であって、
前記位置検出方法は、
前記移動体の前方視野の画像を取得する画像取得過程と、
前記画像と同一の視野を有し、前記画像を取得するのと同時に距離画像を取得する距離画像取得過程と、
少なくとも連続する２フレームの画像からそれぞれ特徴点を抽出する特徴点抽出過程と、
前記特徴点抽出過程において抽出された特徴点の２フレーム間の位置の変位量を前記距離画像に基づいて算出し、当該変位量に基づいて各特徴点が静止物体であるか移動物体であるか判断し、前記静止物体であると判断された前記特徴点を基準特徴点として選択する基準特徴点選択過程と、
前記距離画像から前記基準特徴点に対応する距離を取得し、前記基準特徴点と自己位置との相対的位置の前記２フレーム間の移動量を算出し、前記移動量に基づいて現在の自己位置を検出する位置検出過程と、
を有することを特徴とする位置検出方法。
移動体の自己位置を検出するための位置検出プログラムであって、
前記位置検出プログラムは、
前記移動体の前方視野の画像を取得する画像取得処理と、
前記画像と同一の視野を有し、前記画像を取得するのと同時に距離画像を取得する距離画像取得処理と、
少なくとも連続する２フレームの画像からそれぞれ特徴点を抽出する特徴点抽出処理と、
前記特徴点抽出処理において抽出された特徴点の２フレーム間の位置の変位量を前記距離画像に基づいて算出し、当該変位量に基づいて各特徴点が静止物体であるか移動物体であるか判断し、前記静止物体であると判断された前記特徴点を基準特徴点として選択する基準特徴点選択処理と、
前記距離画像から前記基準特徴点に対応する距離を取得し、前記基準特徴点と自己位置との相対的位置の前記２フレーム間の移動量を算出し、前記移動量に基づいて現在の自己位置を検出する位置検出処理と、
をコンピュータに行わせることを特徴とする位置検出プログラム。