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

Abstract

(57)【要約】 【課題】 人間型脚式移動ロボットや自動車が自律的に
移動する際の自己の位置検出を簡単に行うことができる
位置検出装置を提供することを目的とする。 【解決手段】 移動体の自己位置を検出する位置検出装
置であって、移動体の前方視野の輝度画像を取得する輝
度画像取得手段と、輝度画像取得手段と同一の視野を有
し、輝度画像取得手段が輝度画像を取得するのと同時に
距離画像を取得する距離画像取得手段と、少なくとも連
続する２フレームの輝度画像からそれぞれ特徴点を抽出
する特徴点抽出手段と、特徴点抽出手段によって抽出さ
れた特徴点の２フレーム間の位置の変位量を距離画像に
基づいて算出し、当該変位量から自己位置を算出するた
めの基準特徴点を選択する基準特徴点選択手段とを備え
たことを特徴とする。

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を算出する処
理を示す説明図である。

【図４】 ロボットの自己位置を求める場合の座標系を
示す説明図である。

【図５】 本発明の第２の実施形態の構成を示すブロッ
ク図である。

【図６】 図５に示す特徴点抽出部５及び位置検出部６
の動作を示すフローチャートである。

【図７】 自己位置算出方法を示す説明図である。

【図８】 自己位置算出方法を示す説明図である。

【図９】 自己位置算出方法を示す説明図である。

【図１０】 カルマンフィルタの状態モデルを示す説明
図である。

【図１１】 本発明の第３の実施形態の構成を示すブロ
ック図である。

【図１２】 図１１に示す位置検出装置の動作を示すフ
ローチャートである。

【図１３】 第３の実施形態における自己位置検出の原
理を説明する説明図である。

【図１４】 第３の実施形態における自己位置検出の原
理を説明する説明図である。

【図１５】 第３の実施形態における自己位置検出の原
理を説明する説明図である。

【符号の説明】

１・・・カメラ、 ２・・・画像記憶部、 ３・・・距離画像生成部、 ４・・・距離画像記憶部、 ５・・・特徴点抽出部、 ６・・・位置検出部、 ７・・・移動制御部、 ８・・・物体情報記憶部。

───────────────────────────────────────────────────── フロントページの続き (72)発明者 坂上 義秋 埼玉県和光市中央１丁目４番１号 株式会 社本田技術研究所内 Ｆターム(参考） 2F065 AA04 AA11 BB15 CC00 CC11 DD00 DD07 FF01 FF05 FF09 FF61 JJ03 JJ05 JJ26 MM06 PP25 QQ00 QQ24 QQ25 QQ38 RR02 RR07 UU05 3C007 CS08 KS18 KS36 KT03 KT04 KT11 WA03 WA13 5B057 AA05 AA06 BA02 BA11 CA13 CB08 CB12 CB16 CB18 CC01 CE06 CH11 DA07 DB03 DB09 DC05 DC16 DC32 DC36 5L096 AA06 AA09 BA04 BA05 CA05 DA02 FA69 GA08 GA55 HA02

Claims (15)

【特許請求の範囲】
1. 【請求項１】 移動体の自己位置を検出する位置検出装
   置であって、 前記位置検出装置は、 前記移動体の前方視野の画像を取得する画像取得手段
   と、 前記画像取得手段と同一の視野を有し、前記画像取得手
   段が画像を取得するのと同時に距離画像を取得する距離
   画像取得手段と、 少なくとも連続する２フレームの画像からそれぞれ特徴
   点を抽出する特徴点抽出手段と、 前記特徴点抽出手段によって抽出された特徴点の２フレ
   ーム間の位置の変位量を前記距離画像に基づいて算出
   し、当該変位量から自己位置を算出するための基準特徴
   点を選択する基準特徴点選択手段と、 を備えたことを特徴とする位置検出装置。
2. 【請求項２】 移動体の自己位置を検出する位置検出装
   置であって、 前記位置検出装置は、 前記移動体の前方視野内の画像を取得する画像取得手段
   と、 前記移動体の移動に当たって基準となる基準特徴点を前
   記画像取得手段によって得られた画像に基づいて決定す
   る基準点決定手段と、 自己の移動制御と前記基準点の観測量を拡張カルマンフ
   ィルタに代入して自己の位置を検出する位置検出手段
   と、 を備えたことを特徴とする位置検出装置。
3. 【請求項３】 移動体の自己位置を検出する位置検出装
   置であって、 前記位置検出装置は、 前記移動体の前方視野の画像を取得する画像取得手段
   と、 前記画像取得手段と同一の視野を有し、前記画像取得手
   段が画像を取得するのと同時に距離画像を取得する距離
   画像取得手段と、 得られた画像からそれぞれ特徴点を抽出する特徴点抽出
   手段と、 予め記憶された物体情報と抽出された特徴点とを比較し
   て、相関が高い特徴点を既知と特徴点と見なして自己位
   置を算出するための基準特徴点とする基準特徴点選択手
   段と、 を備えたことを特徴とする位置検出装置。
4. 【請求項４】 前記特徴点選択手段は、 既知と見なされた特徴点が存在する画像中の未知の特徴
   点と既知の特徴点との相対関係を求め、該未知の特徴点
   を既知の特徴点として記憶することにより前記物体情報
   を更新することを特徴する請求項３に記載の位置検出装
   置。
5. 【請求項５】 移動体の自己位置を検出する位置検出装
   置であって、 前記位置検出装置は、 前記移動体の前方視野の画像を取得する画像取得手段
   と、 前記画像中の特徴点群を抽出する特徴点群抽出手段と、 予め画像取得手段で得られた画像における特徴点群と当
   該特徴点群を得た位置を対応させて複数記憶しておき、
   新たに得られた画像の特徴点群と予め記憶された特徴点
   群の相関を算出して、自己の位置を算出する自己位置検
   出手段と、 を備えたことを特徴とする位置検出装置。
6. 【請求項６】 移動体の自己位置を検出する位置検出方
   法であって、 前記位置検出方法は、 前記移動体の前方視野の画像を取得する画像取得過程
   と、 前記画像と同一の視野を有し、前記画像を取得するのと
   同時に距離画像を取得する距離画像取得過程と、 少なくとも連続する２フレームの画像からそれぞれ特徴
   点を抽出する特徴点抽出過程と、 前記特徴点抽出過程において抽出された特徴点の２フレ
   ーム間の位置の変位量を前記距離画像に基づいて算出
   し、当該変位量から自己位置を算出するための基準特徴
   点を選択する基準特徴点選択過程と、 を有することを特徴とする位置検出方法。
7. 【請求項７】 移動体の自己位置を検出する位置検出方
   法であって、 前記位置検出方法は、 前記移動体の前方視野内の画像を取得する画像取得過程
   と、 前記移動体の移動に当たって基準となる基準特徴点を前
   記画像に基づいて決定する基準点決定過程と、 自己の移動制御と前記基準点の観測量を拡張カルマンフ
   ィルタに代入して自己の位置を検出する位置検出過程
   と、 を有することを特徴とする位置検出方法。
8. 【請求項８】 移動体の自己位置を検出する位置検出方
   法であって、 前記位置検出方法は、 前記移動体の前方視野の画像を取得する画像取得過程
   と、 前記画像と同一の視野を有し、前記画像を取得するのと
   同時に距離画像を取得する距離画像取得過程と、 得られた画像からそれぞれ特徴点を抽出する特徴点抽出
   過程と、 予め記憶された物体情報と抽出された特徴点とを比較し
   て、相関が高い特徴点を既知と特徴点と見なして自己位
   置を算出するための基準特徴点とする基準特徴点選択過
   程と、 を有することを特徴とする位置検出方法。
9. 【請求項９】 前記特徴点選択過程は、 既知と見なされた特徴点が存在する画像中の未知の特徴
   点と既知の特徴点との相対関係を求め、該未知の特徴点
   を既知の特徴点として記憶することにより前記物体情報
   を更新することを特徴する請求項８に記載の位置検出方
   法。
10. 【請求項１０】 移動体の自己位置を検出する位置検出
    方法であって、 前記位置検出方法は、 前記移動体の前方視野の画像を取得する画像取得過程
    と、 前記画像中の特徴点群を抽出する特徴点群抽出過程と、 予め画像取得過程で得られた画像における特徴点群と当
    該特徴点群を得た位置を対応させて複数記憶しておき、
    新たに得られた画像の特徴点群と予め記憶された特徴点
    群の相関を算出して、自己の位置を算出する自己位置検
    出過程と、 を有することを特徴とする位置検出方法。
11. 【請求項１１】 移動体の自己位置を検出するための位
    置検出プログラムであって、 前記位置検出プログラムは、 前記移動体の前方視野の画像を取得する画像取得処理
    と、 前記画像と同一の視野を有し、前記画像を取得するのと
    同時に距離画像を取得する距離画像取得処理と、 少なくとも連続する２フレームの画像からそれぞれ特徴
    点を抽出する特徴点抽出処理と、 前記特徴点抽出処理において抽出された特徴点の２フレ
    ーム間の位置の変位量を前記距離画像に基づいて算出
    し、当該変位量から自己位置を算出するための基準特徴
    点を選択する基準特徴点選択処理と、 をコンピュータに行わせることを特徴とする位置検出プ
    ログラム。
12. 【請求項１２】 移動体の自己位置を検出するための位
    置検出プログラムであって、 前記位置検出プログラムは、 前記移動体の前方視野内の画像を取得する画像取得処理
    と、 前記移動体の移動に当たって基準となる基準特徴点を前
    記画像に基づいて決定する基準点決定処理と、 自己の移動制御と前記基準点の観測量を拡張カルマンフ
    ィルタに代入して自己の位置を検出する位置検出処理
    と、 をコンピュータに行わせることを特徴とする位置検出プ
    ログラム。
13. 【請求項１３】 移動体の自己位置を検出するための位
    置検出プログラムであって、 前記位置検出プログラムは、 前記移動体の前方視野の画像を取得する画像取得処理
    と、 前記画像と同一の視野を有し、前記画像を取得するのと
    同時に距離画像を取得する距離画像取得処理と、 得られた画像からそれぞれ特徴点を抽出する特徴点抽出
    処理と、 予め記憶された物体情報と抽出された特徴点とを比較し
    て、相関が高い特徴点を既知と特徴点と見なして自己位
    置を算出するための基準特徴点とする基準特徴点選択処
    理と、 をコンピュータに行わせることを特徴とする位置検出プ
    ログラム。
14. 【請求項１４】 前記特徴点選択処理は、 既知と見なされた特徴点が存在する画像中の未知の特徴
    点と既知の特徴点との相対関係を求め、該未知の特徴点
    を既知の特徴点として記憶することにより前記物体情報
    を更新することを特徴する請求項１３に記載の位置検出
    プログラム。
15. 【請求項１５】 移動体の自己位置を検出するための位
    置検出プログラムであって、 前記位置検出プログラムは、 前記移動体の前方視野の画像を取得する画像取得処理
    と、 前記画像中の特徴点群を抽出する特徴点群抽出処理と、 予め画像取得処理で得られた画像における特徴点群と当
    該特徴点群を得た位置を対応させて複数記憶しておき、
    新たに得られた画像の特徴点群と予め記憶された特徴点
    群の相関を算出して、自己の位置を算出する自己位置検
    出処理と、 をコンピュータに行わせることを特徴とする位置検出プ
    ログラム。