JP2006234453A - 自己位置標定用ランドマーク位置の登録方法 - Google Patents

Abstract

【課題】 ランドマーク位置を手作業により測定する必要がなく、簡単にかつ短時間で前記ランドマークの三次元位置を測定して登録することが可能で、任意にランドマークの設置を行うことができる自己位置標定用ランドマーク位置の登録方法を提供する。
【解決手段】 移動体の走行環境内に複数配置されたランドマークの三次元位置を自動登録する自己位置標定標用ランドマーク位置の登録方法において、前記移動体を基準座標系の原点から複数のランドマークが認識できる初期位置Ｐ_０に移動させ、該初期位置から任意の距離移動させたときの移動状態量と該移動体から撮像したランドマーク画像のずれ量とを取得した後、前記移動状態量と前記ランドマーク画像のずれ量から前記初期位置を基準としたランドマークの相対三次元位置を算出し、該相対三次元位置を、基準座標系を基準とした三次元位置に座標変換して該基準座標系における三次元位置座標を算出する。
【選択図】 図１

Description

本発明は、自律走行する移動体の自己位置標定技術において、移動体位置の標定に用いられるランドマークが移動体走行環境内に複数配置された状態にて、基準座標系を基準としたランドマークの三次元位置を自動的に登録する自己位置標定用ランドマーク位置の登録方法に関する。

従来より、自律的に行動する移動体に関する技術は種々提案されており、またこのような移動体は各種工業にて広く利用されている。さらに近年においては、一般家庭や各種施設等においても移動体の導入が要望されており、あらゆる動作環境に対応できる移動体が必要とされている。
移動体を円滑に自律走行させるには、移動体の自己位置を正確に把握させる必要がある。そこで、移動体の主要な技術の一つとして、移動体の自己位置標定に関する技術が種々提案されている。

移動体の自己位置を検出する方法として、走行環境内に離散的にランドマークを配置し、このランドマークを特徴量として移動体に認識させ、該認識したランドマークの幾何学的な位置関係から相対的に移動体の現在位置を標定する方法がある。
例えば特許文献１（特開平８−１７８６５４号公報）では、移動体に搭載された視覚センサにより既知の特定目標の相対方位角を検出させ、予め記憶させた特定目標の絶対位置から自己位置と移動方位を演算により求めて自己位置を標定する方法が開示されている。

また、特許文献２（特開２００２−２１３９２０号公報）には、絶対空間において、床面から略同一の高さに設けた発光色の異なる３つのＬＥＤを、移動体に搭載したＣＣＤカメラによって撮像し、この撮像画像をもとに絶対空間の所定位置を原点とした絶対座標系における移動体の絶対座標を求める方法を開示している。
しかし、これらの方法では、広い範囲の天井のランドマークマップが必要であったり、視野範囲ごとに蛍光灯やマーカなどのランドマークを配置してその識別を行う必要があり、さらに移動体の自己位置標定を行うため、基準となる座標系に対するランドマークの三次元位置を事前に正確に測定しておく必要がある。
これらは非常に手間のかかる面倒な作業であるとともに、例えばロボットのような移動体を一般家庭に導入する場合、ランドマークの設置だけで長時間を費やす結果が生じてしまい、ユーザに非常な負担を強いることとなる。

特開２００４−１２４２９号公報 特開２００２−２１３９２０号公報 特開平１１−２５０３６４号公報

従って、本発明は上記従来技術の問題点に鑑み、移動体の走行環境内に複数配置されたランドマークの三次元位置を自動登録する方法において、ランドマーク位置を手作業により測定する必要がなく、簡単にかつ短時間で前記ランドマークの三次元位置を測定して登録することが可能で、任意にランドマークの設置を行うことができる自己位置標定用ランドマーク位置の自動登録方法を提供することを目的とする。

そこで、本発明はかかる課題を解決するために、移動体の走行環境内に複数配置されたランドマークの三次元位置を自動登録する自己位置標定標用ランドマーク位置の登録方法において、
前記複数のランドマークを認識できる初期位置に設置した移動体を、任意の距離移動させたときの移動状態量と該移動体から撮像したランドマーク画像のずれ量とを取得する移動情報取得ステップと、
前記移動量と前記ランドマーク画像のずれ量から前記初期位置を基準とした前記ランドマークの相対三次元位置を算出する相対三次元位置算出ステップと、
前記相対三次元位置を、基準座標系を基準とした三次元位置に座標変換して該基準座標系におけるランドマークの三次元位置座標を算出する三次元位置算出ステップと、を備えたことを特徴とする。

本発明によれば、移動体を任意の距離移動させるだけで自動的に複数のランドマークの三次元位置を登録できるため、簡単にかつ短時間で登録処理を行うことが可能となる。これにより、任意の位置にランドマークを設置することができるようになるため、移動体の導入が非常に簡便となり一般ユーザにも手軽に行えるようになる。
尚、前記移動状態量は、移動体が具備するオドメトリにより検出することが好ましく、これにより得られる移動状態量は、該移動体の移動距離及び姿勢方位を表す情報であると良い。
また、本発明において、任意位置への移動体の移動を複数回行い、前記ランドマークの相対三次元位置を繰り返し求め、これにより得られた複数の相対三次元位置の平均値を採用することが好ましい。

また、前記移動情報取得ステップにて、前記移動体を直線経路に沿って移動させるようにしたことを特徴とする。これにより、廊下等の狭隘部においても移動体を円滑に移動させ、ランドマーク位置を登録することが可能となる。
さらに、前記移動情報取得ステップにて、前記移動体から撮像したランドマーク画像の移動軌跡が、２以上の直線成分を含むように該移動体を移動させることを特徴とする。
これは、移動体の進行方向延長線上にランドマークが存在する場合、移動体の移動によるランドマーク画像の画素位置の変化が小さくなり、ランドマーク画像位置の取りこぼしや三次元位置精度の劣化の懸念されるが、本発明によれば、前記ランドマークの移動軌跡が２以上の直線成分を含むように移動させることにより、確実にランドマーク画像の画素変化を抽出でき、精度の高い移動状態量情報を検出することが可能となる。
さらにまた、前記移動情報取得ステップにて、前記移動体を直線経路と折曲部からなるＬ字型経路に沿って移動させるようにしたことを特徴とする。このように、Ｌ字型経路上を移動させることにより、前記ランドマークの画素変化量が大きくなり、正確なランドマーク位置を検出することが可能となる。

また、前記移動情報取得ステップにて、前記移動体が折曲部を有する経路上を移動する場合に、折曲部通過時にはランドマーク画像を取得しないようにしたことを特徴とする。
前記移動体の旋回時には、処理速度の関係からランドマーク画像を鮮明に取り込むことができないため、ランドマーク位置の三次元位置の誤差が大きくなってしまう。従って、本発明のように旋回時にランドマーク画像を取り込まないようにすることにより、より精度の高い三次元位置を測定できるようになる。

以上記載のごとく本発明によれば、、移動体を任意の距離移動させるだけで自動的に複数のランドマークの三次元位置を登録できるため、簡単にかつ短時間で登録処理を行うことが可能となり、延いては、任意の位置にランドマークを設置できるようになり、移動体の導入が非常に簡便で一般ユーザにも手軽に行えるようになる。
また、移動体を直線経路に沿って移動させることにより、廊下等の狭隘部においても移動体を円滑に移動させ、ランドマーク位置を登録することが可能となる。
さらに、前記移動情報取得ステップにて、前記移動体から撮像したランドマーク画像の移動軌跡が、２以上の直線成分を含むように移動させることにより、確実にランドマーク画像の画素変化を抽出でき、精度の高い移動状態量情報を検出することが可能となる。
さらにまた、前記移動体をＬ字型経路に沿って移動させることにより、前記ランドマークの画素変化量が大きくなり、正確なランドマーク位置を検出することが可能となる。
また、前記移動体の折曲部通過時にはランドマーク画像を取得しないようにすることにより、より精度の高い三次元位置を測定できるようになる。

以下、図面を参照して本発明の好適な実施例を例示的に詳しく説明する。但しこの実施例に記載されている構成部品の寸法、材質、形状、その相対的配置等は特に特定的な記載がない限りは、この発明の範囲をそれに限定する趣旨ではなく、単なる説明例に過ぎない。
本実施例に示す移動体の走行空間としては、一般家庭、各種施設、工場内等が挙げられるが、特に前記移動体は一般家庭内にてユーザの生活を補助、支援、介護するロボットであることが好適である。

図１は本実施例に係るランドマーク位置の自動登録アルゴリズムを示すフロー、図２はランドマーク配置の一例を示す図、図３はランドマーク位置の登録時における走行方法の具体例を示す図、図４はランドマーク位置標定の原理を示す図、図５は本実施例に係る移動体の概略構成図、図６〜図８はランドマーク位置登録処理における具体的な操作方法を示すフローである。
まず、図５を参照して本実施例に適用できる移動体の一例につき説明する。尚、本実施例では以下に示す移動体に限定されるものではなく、移動手段と移動状態量検出手段（オドメトリ）と撮像手段とを有する装置であれば何れの構成であっても良い。
図５に示されるように、本実施例１に係る移動体１０は、頭部１１と胴部１２と台車部１３からなる。前記台車部１３に装備された車輪１４は、駆動制御装置１５の指令に基づき制御される車輪モータ１６により操舵、走行可能であり、該車輪１４の回転量を検出する回転計測装置１７により移動体１０の移動距離、姿勢方位変化量からなる移動状態量を表すオドメトリ情報を検出可能としている。

前記頭部１１には、前記移動体１０の上方空間を広視野範囲で撮像する全方位カメラ１８と、該移動体１０の前方空間を狭視野範囲で撮像する前方カメラ１９と、これらのカメラからの撮像画像を基に各種画像処理を行なう画像処理装置２０と、天井方向に光を放射するＬＥＤ照明２１と、該ＬＥＤ照明２１の点滅等を制御する照明制御装置２２が設けられている。
前記全方位カメラ１８は公知のカメラであって、例えば双曲面ミラーとカメラから構成され、周囲360°から双曲面に届く光をミラーで反射して丸画像を生成し、該丸画像を画像処理することにより、移動体の周囲360°に亘る全方位の動画像が取得できる。尚、前記全方位カメラ１８は、このような構成のカメラに限定されるものではなく、例えば魚眼レンズのような広角カメラであれば何れでも良い。

前記前方カメラ１９は、狭視野範囲内にて歪みの少ない画像を撮像可能で、移動体１０の周囲に存在する移動対象物、特に人物を認識可能であることが好ましい。尚、本実施例の自己位置標定に際しては、前記前方カメラ１９を具備しない構成とすることもできる。
前記画像処理装置２０は、前記全方位カメラ１８等で取得した撮像画像に対して、二値化処理、差分処理等の画像の基本的な処理を行なうプログラムが格納されており、本実施例では移動体１０から撮像されたランドマーク画像の画素位置変化量の検出等を行う。
前記ＬＥＤ照明２１は、可視光領域若しくは赤外領域の波長の光を発するＬＥＤが好適であり、前記照明制御装置２２により発光間隔等が制御される。尚、前記ＬＥＤ照明２１及び照明制御装置２２は具備しない構成とすることもできる。

また、前記胴部１２には、前記頭部１１と該胴部１２を回動自在に連結する頭部傾倒モータ２３と、該頭部傾倒モータ２３の動作を制御するとともに、該頭部傾動モータ２３の動作を検出することにより頭部１１の姿勢角、姿勢方位を検出する頭部傾倒制御装置２４と、該頭部傾倒制御装置２４、前記駆動制御装置１５、前記回転計測装置１７、及び前記画像処理装置２０に接続された制御装置２５が設けられている。
前記制御部２５は、自己位置標定部、記憶部等を具備している。

図２に移動体１０の走行環境に配置されたランドマークの一例を示す。図２に示されるように、該走行環境は天井を有する部屋３０であり、ｘｙｚ直交座標系（基準座標系）が定められている。ｘｙ面は、部屋３０の床と同一平面を有し、ｚ軸は該床面に垂直な高さ方向と規定される。前記部屋３０内における移動体１０の位置は、座標（ｘ，ｙ）によって表現され、原点（Ｘ_０、Ｙ_０、Ｚ_０）を有する。このとき、前記原点は例えば移動体１０の充電ステーションとすることが好ましい。
前記屋内３０には、天井面若しくは壁面上方に添付された、またはポール等で空間上方に設置された、ランドマークとしてマーカ３１が複数設けられている。該マーカ３１は、少なくとも３個以上設けるものとする。

次に、図１を参照して、前記移動体１０を用いたランドマーク位置登録の主要な処理を示すフローにつき説明する。
まず、移動体１０を自己位置標定の基準座標系の原点（Ｘ_０、Ｙ_０、Ｚ_０）に設置し（Ｓ１）、ランドマークとなるマーカ３１を移動体の走行環境内の任意の位置に複数設置する（Ｓ２）。前記マーカ３１は予め設置されているものであっても良い。
前記原点に設置された移動体１０を、移動体走行環境の中で複数のマーカ３１が前記移動体１０の全方位カメラにて検出できる初期位置Ｐ_０に移動させる（Ｓ３）。このとき、前記マーカ３１の全てが検出できる走行環境略中央位置に移動させることが好ましい。そして、初期位置Ｐ_０におけるオドメトリ値（Ｘ_{od_ref}，Ｙ_{od_ref}，Ｚ_{od_ref}）を取得し、登録する（Ｓ４）。
登録後に、初期位置Ｐ_０から移動体１０を任意の距離だけ移動させる（Ｓ５）。該移動体１０の位置登録時における走行においては、該移動体１０により得られるマーカの相対移動軌跡が、２以上の直線成分を有するように走行させることが好ましい」。この移動体の走行方法の具体的な方法については後述する。

このようにして得られた移動後のオドメトリ値と、初期位置Ｐ_０におけるオドメトリ値を比較して、移動状態量のオドメトリ値を取得する（Ｓ６）。該移動状態量とは、前記オドメトリ情報にて構成され、移動体の移動距離と姿勢方位角とからなる。
前記移動状態量のオドメトリ値を取得する際に、該オドメトリ値の変化に対するマーカ画像のずれ量を前記全方位カメラにより取得する（Ｓ７）。これは、画像中心からの方位角変化、即ち、マーカ画像の画素位置の変化より得られる。そして、前記オドメトリ値の変化に対するマーカ画像のずれ量から、マーカ３１の相対三次元位置を計算する（Ｓ８）。
このようにして、任意位置での移動体１０により得られるマーカ３１の相対三次元位置を計算する毎に走行完了か否かを判断し（Ｓ９）、完了していない場合には移動体を任意の距離だけ移動させ（Ｓ５）、上記と同様にしてマーカ３１の相対三次元位置を繰り返し求める。走行が完了したら、マーカ３１の相対三次元位置の平均を計算し、計算結果をマーカ３１の相対三次元位置として登録する。
前記相対三次元位置は、前記位置Ｐ_０を原点とした座標系として定義されているため、これを原点（Ｘ_０、Ｙ_０、Ｚ_０）を有する基準座標系に対する三次元位置とするため、前記位置Ｐ_０でのオドメトリ情報に基づいて前記相対三次元位置を座標変換し、基準座標系を基準としたマーカ３１の最終的な三次元座標位置を求める（Ｓ１１）。

上記した方法によれば、移動体１０を任意の距離移動させるだけで、オドメトリを用いて複数マーカ３１の三次元位置を自動的に登録できる。本実施例では、マーカ３１の三次元位置を事前に計測する必要がないため、任意の位置にマーカ３１を設置することができるようになり、移動体１０の導入が非常に簡便となり、また準備に際して時間がかかることなく一般ユーザにも手軽に導入することが可能となる。

ここで、前記移動体１０の登録時の走行方法の具体例を図３を参照して説明する。
具体的な移動体１０の走行方法としては、例えば図３（ａ）に示されるように、移動体１０を直線経路を往復移動させる方法が挙げられる。これは、廊下などの狭隘部においてマーカ３１の三次元位置を登録する際に有効に用いることができる。
また別の方法として、直線方向に所定距離移動させた後に右に約９０°旋回させてさらに所定距離だけ直線移動させる方法、直線方向に所定距離移動させた後に左に約９０°旋回させてさらに所定距離だけ直線移動させる方法、が挙げられる。直線経路の場合には、移動体１０の進行方向延長線上にマーカが存在する場合、移動体の移動によるマーカ画像の画素位置の変化が小さくなり、マーカ画像位置の取りこぼしや三次元位置精度の劣化が生じる場合がある。従って、本実施例のように直角方向へ曲がる経路を加えることにより、走行方向によって画像変化が少ないマーカ配置に対しても対応できるようになる。また右Ｌ字経路と左Ｌ字経路の両方を備えることが好ましく、これにより屋内環境の様々な家具などの設置環境に対しても対応できるようになる。

さらに好ましくは、前記右Ｌ字移動、前記左Ｌ字移動において、移動方向を直角に切り替える際に、この旋回動作中には画像を取り込まないようにすると良い。これは、旋回時には、処理速度の関係からマーカ画像を鮮明に取り込むことができず、旋回時のマーカ画像データを用いてマーカの三次元座標位置を計算すると、誤差が大きくなるという場合がある。従って、旋回時にはマーカの三次元位置を計算することを回避することにより、より精度の高い三次元位置を測定できるようになる。

図４にランドマーク（マーカ）位置の標定原理について示す。図４に示されるように、ランドマークの三次元座標は、カメラから見たランドマークの方位とカメラの進行距離を、ある直線状に置き直すことで求めている。図４（ａ）はランドマーク登録時のカメラとランドマークの関係を示し、初期位置からｄ_１、ｄ_２の距離を移動したときのカメラから見たランドマーク方位を、θ_０、θ_１、θ_２とする。そして、計測データを、下記数式により進行距離と方位の関係に直す。
（数式）
tan（θ_０）＝ｂ／ａ ⇒ ０＝tan（θ_０）−ａ
tan（θ_１）＝ｂ／（ａ＋ｄ_１） ⇒ ｄ_１＝tan（θ_１）−ａ
tan（θ_２）＝ｂ／（ａ＋ｄ_２） ⇒ ｄ_２＝tan（θ_２）−ａ

そして、進行距離ｄと方位１／tan（θ）を図４（ｂ）のグラフにプロットし、近似直線を求めると、傾きと切片がランドマーク座標を示す値となる。
ｘ、ｙ座標は初期位置からａ、ｂだけ並行移動した場所となり、ｚ座標は天井画像中のランドマーク位置から仰角φを求めると、tan（φ）＝ｚ／√（ａ＾２＋ｂ＾２）から導き出すことができる。

次に、ランドマーク登録処理における具体的な操作方法について図６〜図８を参照して説明する。
ユーザは、移動体１０に対してランドマーク登録の指示をだす（Ｓ２１）。これは、移動体１０に搭載されたキーボード又はスイッチ等により指示を入力しても良いし、音声により入力するようにしても良い。ユーザからの指示により、移動体１０は「ランドマーク登録モードに入ります。」（Ｓ２２）と出力する。移動体１０からの出力は、該移動体１０に搭載された画面に文字表示しても良いし、スピーカにより音声出力しても良い。
移動体１０は、「走行方法を教えて下さい。」（Ｓ２３）、「右Ｌ字ですか？左Ｌ字ですか？それともまっすぐですか？」（Ｓ２４）と出力し、ユーザに対して走行方法の指示を仰ぐ。ユーザが、“右Ｌ字”（Ｓ２５）、“左Ｌ字”（Ｓ２６）、“まっすぐ”（Ｓ２７）の何れかを選択したら、移動体１０は誤入力防止のために「“***”ですね。」（Ｓ２８）と確認を行い、ユーザから“いいえ”と入力されたら「ランドマーク登録を取りやめます。」（Ｓ２９）と出力し、ランドマーク登録処理を中止する。

一方、ユーザから“はい”と入力されたら、「エリアの中心で“***”に動けるところまで押して移動してください。ランドマーク位置に着いたら登録と言って下さい。全ての登録が終わったら終わりと言って下さい。」（Ｓ３０）とユーザに処理の流れを教示する。ユーザは移動体１０を手押し移動し（Ｓ３１）、“登録”（Ｓ３２）と入力する。
移動体１０は、「ここでランドマーク登録ですね？」（Ｓ３３）と確認を行い、ユーザから、“いいえ”と入力があった場合には、再度「エリアの中心で“***”に動けるところまで押して移動してください。ランドマーク位置に着いたら・・・」（Ｓ３０）、とユーザに指示を出し、上記した（Ｓ３１）〜（Ｓ３３）までのステップを繰り返し行う。
前記ランドマーク登録の確認（Ｓ３３）の際に“はい”と入力があった場合には、ランドマーク登録モードに切り換え、「ランドマーク登録モード、この位置から１ｍ“***”移動します。よろしいですか？」（Ｓ３４）と確認を行った後、ユーザから“はい”と入力されたら、「頭を真上にしています。」（Ｓ３６）と移動体の状態を伝え、「移動します。」（Ｓ３７）と行動予告を行うとともに「登録中です。」（Ｓ３８）と現在位置におけるランドマーク位置の登録を行った後に、指示された移動を行い（Ｓ３７）、移動後に「登録中です。」（Ｓ３８）と伝えるとともに移動後のランドマーク位置の登録を行う。
尚、移動体１０が「ランドマーク登録モード、この位置から・・・」（Ｓ３４）と出力した際に、ユーザが“いいえ”を選択した場合には、「移動を終了します。」（Ｓ３５）と出力して登録処理を終了する。

前記移動体１０により登録処理が正確に行われた場合は「“**個”のマーカが認識できました。」（Ｓ４１）と伝えるとともに、「**個目のマーカはＸ**Ｙ**Ｚ**です。」（Ｓ４３）と登録されたマーカの位置を伝える。ここで移動体１０はユーザに対して「登録しますか？」（Ｓ４４）と登録の要否を確認し、ユーザから“はい”と入力された場合には、「登録するランドマーク名を言って下さい。」（Ｓ５４）とユーザに問いかけ、ユーザから“****”（Ｓ５５）と入力されたら、「“****”でよろしいですか？」と確認を行い（Ｓ５５）、ユーザから“はい”と入力されたら「ランドマーク****、座標登録番号****を登録しました。」（Ｓ５８）と伝えるとともに、ランドマーク位置座標に関連づけてランドマーク名及び座標登録番号を登録する。ここで、ユーザから“いいえ”と入力されたら、「言い直して下さい。」（Ｓ５７）と伝え、再度ランドマーク名の登録処理を行う（Ｓ５５〜）。

一方、「登録しますか？」（Ｓ４４）と伝えた際に、ユーザから“いいえ”と入力された場合には、「登録をやり直しますか？」（Ｓ４５）と伝え、“はい”と入力されたら「走行方法を教えて下さい。」と出力し（Ｓ２３〜）、登録をやり直す。ユーザから“いいえ”と入力された場合には、「ランドマーク登録をキャンセルします。」（Ｓ４６）と表示し、ランドマーク登録モードを解除する。

また、前記移動体１０により登録処理が正確に行われなかった場合には、「ランドマーク登録に失敗しました。」（Ｓ４２）と出力し、「再度登録を試みますか？」（Ｓ５０）と問いかけ、“はい”が選択された場合には「ランドマーク登録モード、この位置から・・・」（Ｓ３４〜）とランドマーク登録処理を再度行う。“いいえ”が選択された場合には、「この位置での登録を中止します。」（Ｓ５１）と伝え、「まだ登録箇所がありますか？」（Ｓ５２）とユーザに問いかけ、“はい”が選択されたら再度走行方法を確認して登録処理を行う（Ｓ２３〜）。“いいえ”が選択されたら、「ランドマーク登録を終了します。」（Ｓ５３）と出力してランドマーク登録処理を終了する。
また、登録処理中にユーザから“おわり”（Ｓ４７）と入力された場合には、「ランドマーク登録を終了しますか？」（Ｓ４８）と確認し、“はい”と入力されたら「ランドマーク登録を終了します。」（Ｓ４９）と出力してランドマーク登録モードを解除する。ここで、“いいえ”と入力されたら、「走行方法を教えて下さい。」と出力して再度登録処理を行う（Ｓ２３〜）。

上記したランドマーク登録処理により取得したランドマーク位置座標を用いて、移動体１０の自己位置同定を行う方法につき以下に例示する。尚、自己位置同定方法については、以下の方法に限定されるものではない。
図９に、本実施例にて自動登録したランドマークを用いて自己位置同定を行う場合のアルゴリズムを示すフローを示す。同図に示されるように、まず、前記ランドマーク位置の自動登録により絶対座標系を基準としたランドマーク３１の２次元位置座標を取得し、前記画像処理装置２０の記憶部（不図示）に事前登録しておく（Ｓ６１）。
そして、前記移動体１０の概略の自己位置を予測し、探索範囲を設定する。この自己位置の予測には、例えば車輪１４の回転量と操舵角からなるオドメトリデータを前記回転計測装置１７により検出し、これを積分して自己位置と方位を検出する方法等を用いると良い。
このようにして自己位置を予測したら、前記移動体１０に搭載された前記全方位カメラ１８により天井画像（天井近傍の壁、空間を含む）を撮像する（Ｓ６２）。

前記撮像した天井画像に対して、前記画像処理装置２０にて二値化処理等により前記ランドマーク候補点を抽出し、該抽出したランドマーク候補点の２次元の候補点座標を算出する（Ｓ６３）。さらに、前記予測した探索範囲内において、絶対座標系上の移動体走行領域内に複数の仮想点をランダムに設定し、該仮想点に移動体１０の自己位置が存在すると仮定し、夫々の仮想点にて得られる画像上の前記ランドマーク３１の２次元座標を、前記３次元の登録位置座標から計算する（Ｓ６４）。

図１０に、前記仮想点におけるランドマーク位置Ｌ_１、Ｌ_２、Ｌ_３、Ｌ_４、Ｌ_５と、前記天井画像から抽出したランドマーク候補点位置Ｍ_１、Ｍ_２、Ｍ_３を示す。
前記ランドマーク位置Ｌ_１、Ｌ_２、Ｌ_３、Ｌ_４、Ｌ_５の２次元座標値と、これに最も近いランドマーク候補点Ｍ_１、Ｍ_２、Ｍ_３の候補点座標値との距離を計算して（Ｓ６５）距離評価値を計算する（Ｓ６６）。すなわち、全ての仮定した自己位置に対して、前記距離評価値を計算し、最も距離が小さくなる位置を移動体１０の同定位置と推定する（Ｓ６７）。そして、前記制御装置２５に結果を出力する（Ｓ６８）。

本実施例に係るランドマーク位置の自動登録アルゴリズムを示すフローである。 ランドマーク配置の一例を示す図である。 ランドマーク位置の登録時の走行方法を示す具体例で、（ａ）は直線走行を示す図、（ｂ）は右Ｌ字走行を示す図、（ｃ）は左Ｌ字走行を示す図である。 ランドマーク位置標定の原理を示す図である。 本実施例に係る移動体の概略構成図である。 ランドマーク位置登録処理における具体的な操作方法を示すフロー（Ｉ）である。 ランドマーク位置登録処理における具体的な操作方法を示すフロー（II）である。 ランドマーク位置登録処理における具体的な操作方法を示すフロー（III）である。 本実施例にて自動登録したランドマーク位置を用いて自己位置同定を行う場合のアルゴリズムを示すフローである。 ランドマーク位置とランドマーク候補点の位置を夫々示す図である。

符号の説明

１０ 移動体
１１ 頭部
１４ 車輪
１５ 駆動制御装置
１６ 車輪モータ
１７ 回転計測装置
１８ 全方位カメラ
１９ 前方カメラ
２０ 画像処理装置
２１ ＬＥＤ照明
２２ 照明制御装置
２３ 頭部傾倒モータ
２４ 頭部傾倒制御装置
２５ 制御装置
３１ ランドマーク

Claims (5)

1. 移動体の走行環境内に複数配置されたランドマークの三次元位置を自動登録する自己位置標定標用ランドマーク位置の登録方法において、
   前記複数のランドマークを認識できる初期位置に設置した移動体を、任意の距離移動させたときの移動状態量と該移動体から撮像したランドマーク画像のずれ量とを取得する移動情報取得ステップと、
   前記移動状態量と前記ランドマーク画像のずれ量から前記初期位置を基準とした前記ランドマークの相対三次元位置を算出する相対三次元位置算出ステップと、
   前記相対三次元位置を、基準座標系を基準とした三次元位置に座標変換して該基準座標系におけるランドマークの三次元位置座標を算出する三次元位置算出ステップと、を備えたことを特徴とする自己位置標定用ランドマーク位置の登録方法。
2. 前記移動情報取得ステップにて、前記移動体を直線経路に沿って移動させるようにしたことを特徴とする請求項１載の自己位置標定用ランドマーク位置の登録方法。
3. 前記移動情報取得ステップにて、前記移動体から撮像したランドマーク画像の移動軌跡が、２以上の直線成分を含むように該移動体を移動させることを特徴とする請求項１記載の自己位置標定用ランドマーク位置の登録方法。
4. 前記移動情報取得ステップにて、前記移動体を直線経路と折曲部からなるＬ字型経路に沿って移動させるようにしたことを特徴とする請求項１若しくは３記載の自己位置標定用ランドマーク位置の登録方法。
5. 前記移動情報取得ステップにて、前記移動体が折曲部を有する経路上を移動する場合に、折曲部通過時にはランドマーク画像を取得しないようにしたことを特徴とする請求項１、３若しくは４の何れかに記載の自己位置標定用ランドマーク位置の登録方法。
   

Images