ナビゲーション装置とその制御方法
【課題】高価で複雑なセンサを用いることなく、輝度分布の変化が大きい屋外で適用でき、磁気テープや光学式テープなどを用いずに容易に経路変更ができるナビゲーション装置とその制御方法を提供する。
【解決手段】移動可能な自律移動体１に搭載されたナビゲーション装置１０。自律移動体の移動方向の画像を一定時間間隔で撮影するカメラ１２と、記憶装置と演算装置を有しカメラの画像に基づき自律移動体を制御する制御装置１８とを備える。制御装置は、教示走行時において自律移動体が所定の走行経路を移動する際に所定のセンサ情報を記憶し、自律走行時において記憶したセンサ情報と現時点のセンサ情報とを比較して、自律移動体が前記走行経路を移動するように自律移動体を制御する。所定のセンサ情報は、少なくともカメラによる撮影画像と自律移動体に対するコマンドを含む。
【解決手段】移動可能な自律移動体１に搭載されたナビゲーション装置１０。自律移動体の移動方向の画像を一定時間間隔で撮影するカメラ１２と、記憶装置と演算装置を有しカメラの画像に基づき自律移動体を制御する制御装置１８とを備える。制御装置は、教示走行時において自律移動体が所定の走行経路を移動する際に所定のセンサ情報を記憶し、自律走行時において記憶したセンサ情報と現時点のセンサ情報とを比較して、自律移動体が前記走行経路を移動するように自律移動体を制御する。所定のセンサ情報は、少なくともカメラによる撮影画像と自律移動体に対するコマンドを含む。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、移動ロボットや自律搬送台車に適用するためのナビゲーション装置とその制御方法に関する。
【背景技術】
【０００２】
近年、様々な場所で移動ロボットが使われ始め、その需要も年々高まっている。その中でも特にＡＧＶ（無人搬送台車）の研究が盛んに行われている。
これら移動ロボットや無人搬送台車の移動手段として、磁気テープや光学式テープなどを用いたガイド式移動手段と、レーザーレンジファインダ（ＬＲＦ）やＲＴＫ−ＧＰＳなどを用いた自律式移動手段とが既に提案されている（例えば、特許文献１、非特許文献１〜３）。
また、本発明と関連する技術として、非特許文献４が開示されている。
【０００３】
特許文献１のシステムは、目視可能な軌条線、コマンドマーク内容の画像認識の情報解析によって走行方向の選択、走行速度等を制御する視覚誘導方式として、小型軽量、低床化した複数の搬送台車を確実、安定的に総合的に制御することを目的とする。
このため、特許文献１では、図７に示すように、被搬送物Ｐを載置する走行台車本体５１を走行駆動する走行駆動装置５２、搬送走行面上に敷設表示の軌条線５３及び載置セットした制御コードマーク片５４に表示のコマンドマーク夫々を視覚的に画像認識する視覚認識装置５５、軌条線５３に沿った走行方向を操舵する操舵装置５６、視覚認識装置５５で撮像・認識したコマンドコードで走行台車本体５１の速度、走行方向、停止・発進・後退等を制御する制御装置を備える。軌条線５３は粘着テープ体表面に目視可能に表示形成し、制御コードマーク片５４はコマンドスタート部５４ａ、コマンドコードを表示するコマンド部５４ｂ，５４ｃ、コマンドエンド部５４ｄ夫々を左右で対称表示し、制御内容を目視理解させる目視表示部５７を連設してある。
【０００４】
非特許文献１の自律式移動手段は、記録ランにおいてロボットを所定の経路に沿って移動させて複数の画像を記憶し、自動ランにおいて記憶した画像と外部から取得した実画像と比較し、その一致誤差（マッチングエラー）に応じてロボットの軌道を修正するものである。マッチングエラーの計算は、記憶画像をテンプレートとして実画像と輝度分布が一致する領域同士の相対位置誤差を算出している。
【０００５】
非特許文献２の自律式移動手段は、記録ランにおいてレーザーレンジファインダ（ＬＲＦ）を用いて外界を記憶し、自動ランにおいてレーザーレンジファインダによる外界データと記憶データとのマッチングエラーに応じてロボットの軌道を修正するものである。
【０００６】
非特許文献３の自律式移動手段は、画像情報を基にテンプレートマッチングにより、画像照合をし、誘導と自律移動を行うものである。
【０００７】
非特許文献４は、取得画像に対して特徴点抽出手段を開示している。
【０００８】
【特許文献１】特開２００１−１８８６１０号公報、「視覚誘導方式による無人搬送台車の制御方法及びこれを利用した無人搬送台車システム」
【０００９】
【非特許文献１】Ｙ． Ｍａｔｓｕｍｏｔｏ，Ｋ． Ｓａｋａｉ，Ｍ． Ｉｎａｂａ，Ｈ． Ｉｎｏｕｅ，“Ｖｉｅｗ−Ｂａｓｅｄ Ａｐｐｒｏａｃｈ ｔｏ Ｒｏｂｏｔ Ｎａｖｉｇａｔｉｏｎ”， Ｐｒｏｃｅｅｄｉｎｇｓ ｏｆ ２０００ ＩＥＥＥ／ＲＳＪ Ｉｎｔｅｒｎａｔｉｏｎａｌ Ｃｏｎｆｅｒｅｎｃｅ ｏｎ Ｉｎｔｅｌｌｉｇｅｎｔ Ｒｏｂｏｔｓ ａｎｄ Ｓｙｓｔｅｍｓ（ＩＲＯＳ’２０００），Ｆ−ＭＩ−６−３， ｐｐ．１７０２−１７０８，Ｔａｋａｍａｔｓｕ，Ｊａｐａｎ，Ｎｏｖ １ｓｔ−３ｒｄ， ２０００．
【非特許文献２】Ｙ． Ａｄａｃｈｉ， Ｈ． Ｓａｉｔｏ， Ｙ． Ｍａｔｓｕｍｏｔｏ， Ｔ． Ｏｇａｓａｗａｒａ，“Ｍｅｍｏｒｙ−Ｂａｓｅｄ Ｎａｖｉｇａｔｉｏｎ ｕｓｉｎｇ Ｄａｔａ Ｓｅｑｕｅｎｃｅ ｏｆ Ｌａｓｅｒ Ｒａｎｇｅ Ｆｉｎｄｅｒ”，Ｐｒｏｃｅｅｄｉｎｇｓ ｏｆ ２００３ ＩＥＥＥ Ｉｎｔｅｒｎａｔｉｏｎａｌ Ｓｙｍｐｏｓｉｕｍ ｏｎ Ｃｏｍｐｕｔａｔｉｏｎａｌ Ｉｎｔｅｌｌｉｇｅｎｃｅ ｉｎ Ｒｏｂｏｔｉｃｓ ａｎｄ Ａｕｔｏｍａｔｉｏｎ，ｐｐ．４７９−４８４，Ｋｏｂｅ，Ｊａｐａｎ，Ｊｕｌｙ １６−２０，２００３．
【非特許文献３】桂浩章，他、「季節や天候の変化にロバストな視覚認識に基づく移動ロボットの屋外ナビゲーション」、日本ロボット学会誌、Ｖｏｌ．２３， Ｎｏ．１， ｐｐ．７５〜８３，２００５
【非特許文献４】Ｊ，Ｓｈｉ ａｎｄ Ｃ．Ｔｏｍａｓｉ，“Ｇｏｏｄ Ｆｅａｔｕｒｅｓ ｔｏ Ｔｒａｃｋ”，１９９４ ＩＥＥＥ Ｃｏｎｆｅｒｅｎｃｅ ｏｎ Ｃｏｍｐｕｔｅｒ Ｖｉｓｉｏｎ ａｎｄ Ｐａｔｔｅｒｎ Ｒｅｃｏｇｎｉｔｉｏｎ （ＣＶＰＲ’９４），ｐｐ．５９３−６００，１９９４．
【発明の開示】
【発明が解決しようとする課題】
【００１０】
従来、屋内における移動ロボットや無人搬送台車の移動手段は、特許文献１のように、磁気テープや光学式テープなどを用いたガイド式移動手段が主流であった。
しかし、製品サイクルの短縮化から搬送経路の多様化が要求され、無人搬送台車における移動手段にも経路変更が容易な自律式移動手段が求められるようになった。
この自律式移動手段として、上述した非特許文献１〜３のナビゲーション手段が開示されている。
【００１１】
非特許文献１のナビゲーション手段は、マッチングエラーの計算に輝度分布を用いるため、画像上の輝度分布の変化が大きい屋外では適用できない。
非特許文献２，３のナビゲーション手段は、レーザーレンジファインダ（ＬＲＦ）やＲＴＫ−ＧＰＳなど高価で複雑なセンサを用いるため、装置が複雑化し高価となるため、用途が限定される問題点がある。
【００１２】
本発明はかかる問題点を解決するために創案されたものである。すなわち、本発明の目的は、レーザーレンジファインダ（ＬＲＦ）やＲＴＫ−ＧＰＳなど高価で複雑なセンサを用いることなく、輝度分布の変化が大きい屋外で適用でき、磁気テープや光学式テープなどを用いずに容易に経路変更ができるナビゲーション装置とその制御方法を提供することにある。
【課題を解決するための手段】
【００１３】
本発明によれば、移動可能な自律移動体に搭載されたナビゲーション装置であって、
自律移動体の移動方向の画像をリアルタイムに撮影するカメラと、
記憶装置と演算装置を有し前記カメラの画像に基づき自律移動体を制御する制御装置とを備え、
前記制御装置は、教示走行時において自律移動体が所定の走行経路を移動する際にカメラの画像と自律移動体に対するコマンドを含む所定のセンサ情報を記憶し、
自律走行時において記憶した前記センサ情報と現時点のセンサ情報とを比較して、自律移動体が前記走行経路を移動するように自律移動体を制御する、ことを特徴とするナビゲーション装置が提供される。
【００１４】
本発明の好ましい実施形態によれば、前記制御装置は、記憶画像と現時点の外部画像の特徴点を抽出する特徴点抽出手段と、前記特徴点同士のマッチング誤差を計算する誤差計算手段とを有する。
【００１５】
また、自律移動体の現在位置をリアルタイムに検出するＧＰＳ端末と、
自律移動体の移動方向をリアルタイムに検出する方位センサと自律移動体の駆動輪の回転角を計測するセンサとを備え、
前記制御装置は、前記マッチング誤差が所定の閾値より小さい地点まで、自律移動体を案内する復帰手段を有する。
【００１６】
また本発明によれば、移動可能な自律移動体に搭載され、
自律移動体の移動方向の画像を一定間隔で撮影するカメラと、
記憶装置と演算装置を有し前記カメラの画像に基づき自律移動体を制御する制御装置とを備えたナビゲーション装置の制御方法であって、
自律移動体を所定の走行経路に沿って移動させて、前記記憶装置にカメラの画像と自律移動体に対するコマンドを含む所定のセンサ情報を記憶する教示走行ステップと、
記憶した前記センサ情報と現時点のセンサ情報とを比較して、自律移動体が前記走行経路を移動するように自律移動体を制御する自律走行ステップとを有する、ことを特徴とするナビゲーション装置の制御方法が提供される。
【００１７】
本発明の好ましい実施形態によれば、前記教示走行ステップにおいて、所定の走行経路上において現在の外部画像Ｖと直前の記憶画像Ｖ（ｉ）（ｉは正の整数）とのマッチング誤差Ｅ（ｉ）を演算し、そのマッチング誤差Ｅ（ｉ）が所定の閾値ｅ１を上回った場合に新しい画像Ｖ（ｉ＋１）と自律移動体に対する新しいコマンドＭ（ｉ＋１）を記憶する。
【００１８】
前記自律走行ステップにおいて、所定の走行経路上において現在の外部画像Ｖと直前の記憶画像Ｖ（ｉ）（ｉは正の整数）とのマッチング誤差Ｅ（ｉ）と、外部画像Ｖと記憶画像Ｖ（ｉ）との視野の水平ずれｄｘ（ｉ）を算出し、その水平ずれを自律移動体のステアリングに反映して軌道修正する。
【００１９】
また、前記マッチング誤差Ｅ（ｉ）は、記憶画像Ｖ（ｉ）と現時点の外部画像Ｖの特徴点を抽出し、抽出した特徴点同士の距離の平均値が最小になるように、記憶画像Ｖ（ｉ）又は外部画像Ｖを相対的に移動させた際の最小平均距離であり、
前記水平ずれｄｘ（ｉ）は、前記最小平均距離の水平成分である。
【００２０】
また、前記マッチング誤差Ｅ（ｉ）は、記憶画像Ｖ（ｉ）と現時点の外部画像Ｖの特徴点に対し、記憶画像Ｖ（ｉ）中のある特徴点と、外部画像Ｖ中のすべての特徴点との距離を計算し、その中で最小のものを選択し、これをすべての特徴点に対して行い、それぞれの特徴点を１対１に対応させ、対応する２点間の距離が所定の閾値以下のものを小さい順に複数選択し、それらの距離の平均値を算出し、この平均値が最小になるように、記憶画像Ｖ（ｉ）又は外部画像Ｖを相対的に移動させた際の最小平均距離であり、
前記最小平均距離が所定の閾値以下のときに２つの画像のマッチングが取れたものと判断する。
【００２１】
また、前記マッチング誤差Ｅ（ｉ）が所定の閾値を超えるときにマッチングが取れないマッチングミスと判断し、既にマッチングが取れた走行経路上の最も近い位置及び方位までＧＰＳ端末と方位センサにより自律移動体を案内する。
【発明の効果】
【００２２】
上記本発明の装置及び方法によれば、教示走行時において自律移動体が所定の走行経路を移動する際にカメラの画像と自律移動体に対するコマンドを含む所定のセンサ情報を記憶し、自律走行時において記憶したセンサ情報と現時点のセンサ情報とを比較して、自律移動体が前記走行経路を移動するように自律移動体を制御するので、レーザーレンジファインダ（ＬＲＦ）やＲＴＫ−ＧＰＳなど高価で複雑なセンサを用いることなく自律移動体を制御することができ、かつ磁気テープや光学式テープなどを用いずに容易に経路変更ができる。
【００２３】
また、特徴点抽出手段により記憶画像と現時点の外部画像の特徴点を抽出し、誤差計算手段により特徴点同士のマッチング誤差を計算することにより、輝度分布の変化が大きい屋外に適用することができる。
【発明を実施するための最良の形態】
【００２４】
以下、本発明の好ましい実施形態を図面を参照して説明する。なお、各図において共通する部分には同一の符号を付し、重複した説明を省略する。
【００２５】
図１は、本発明のナビゲーション装置を備えた自律移動体の模式的平面図である。
この自律移動体１は、図示しない補助輪と、左右の独立した駆動輪２，３を有し、駆動輪２，３の同方向又は逆方向の回転により、直進（図で右方向）、右折、左折ができるようになっている。
【００２６】
本発明のナビゲーション装置１０は、自律移動体１の本体１ａに固定されたカメラ１２、ＧＰＳ端末１４、方位センサ１６及び制御装置１８を備える。
カメラ１２は、例えば安価なＵＳＢカメラ又はウエブカメラであり、自律移動体１の前方の画像をリアルタイムに撮影し制御装置１８に入力するようになっている。画像の視野角は、固定であるのが好ましいが可変であってもよい。画像は、少なくとも３０万画素（６４０×４８０ピクセル）以上のモノクロ画像であるのがよい。
ここで「リアルタイム」とは、走行経路上のある位置を通過する瞬間と撮影時との時間差がほとんど０（例えば１００ｍｓｅｃ以下）であることを意味する。なお、撮影自体は連続的ではなく、制御装置１８による後述する画像処理に要する時間より長い一定時間間隔であってもよい。
【００２７】
ＧＰＳ端末１４は、全地球測位システム（ＧＰＳ）の端末であり、その位置（緯度と経度）をリアルタイムに制御装置１８に入力する。ＧＰＳ端末の位置検出精度は、少なくとも数ｍ以内のものを使用する。
方位センサ１６は、自律移動体１の前方方位をリアルタイムに検出し制御装置１８に入力する。
制御装置１８は、記憶装置と演算装置を有するコンピュータであり、本発明によるナビゲーション方法を実施するためのプログラムが記憶装置にインストールされ、このプログラムにより自律移動体１を制御するようになっている。
制御装置１８は、記憶装置に記憶された記憶画像と現時点の外部画像の特徴点を抽出する特徴点抽出手段、特徴点同士のマッチング誤差を計算する誤差計算手段、及びマッチング誤差が所定の閾値より小さい地点まで、自律移動体を案内する復帰手段として機能する。
これらの特徴点抽出手段、誤差計算手段及び復帰手段は、コンピュータの記憶装置にインストールされたプログラムと演算装置により実行される。
【００２８】
図２は本発明によるナビゲーション方法を示す全体フロー図である。この図に示すように本発明のナビゲーション方法は、教示走行ステップＳ１と自律走行ステップＳ２とからなる。
教示走行ステップＳ１は、人間が自律移動体１を操作する走行であり、その際制御装置１８は、そのときの走行経路のセンサ情報を記憶する。自律移動体１の操作は、制御装置１８に無線又は有線で任意のモータコマンド（回転方向、回転速度、等）を入力して行うのがよい。
一方、自律走行ステップＳ２は、記憶したセンサ情報と現在のセンサ情報とを比較して制御することにより走行する走行である。
【００２９】
教示走行ステップＳ１においてどのようなセンサ情報を記憶させ、自律走行ステップＳ２においてどのような制御を行うかを以下に説明する。
【００３０】
（教示走行）
本発明において、記憶するセンサ情報は、カメラ１２による撮影画像、ＧＰＳ端末１４による位置情報、方位センサ１６による方位、及び駆動輪２，３に対するモータコマンド（回転方向、回転速度、等）である。以下、記憶装置に記憶された画像を「記憶画像」、カメラ１２による現在の撮影画像を「外部画像」と呼ぶ。
なお、本発明では屋外走行において、記憶する画像には明るさの変化に対してロバストな画像処理を行う必要がある。そこで、本発明では、取得画像に対して特徴点抽出を行い、抽出された点同士のマッチングをとる。特徴点抽出は色への依存度が低いため、多少色味が変わっても影響を受けることなく抽出できる利点がある。また画像全体が見えていないような場合でも、部分的な照合も可能である。
本発明では、画像特徴点の抽出・追跡手段として、非特許文献４に開示されている特徴点抽出（Ｇｏｏｄ Ｆｅａｔｕｒｅｓ ｔｏ Ｔｒａｃｋ）を用いた。以下、この方法を単に「特徴点抽出法」と呼ぶ。本発明では、マッチング可能な任意の特徴点抽出手法を用いることができ、有用である。
【００３１】
（教示データの作成）
図３は、本発明による教示走行ステップＳ１におけるセンサ情報の記憶方法を示す模式図である。
本発明で用いるセンサ情報は、画像情報を基に生成する。スタート地点の記憶画像をＶ（ｉ）（ｉ＝１、ｉは正の整数）として、現在の外部画像Ｖと前回記憶した記憶画像Ｖ（ｉ）とのマッチング誤差Ｅ（ｉ）を演算し、そのマッチング誤差Ｅ（ｉ）が所定の閾値ｅ１を上回った場合に新しい画像Ｖ（ｉ＋１）と新しいコマンドＭ（ｉ＋１）を記憶する。
これにより、自律移動体１の移動距離には依存せず、外部画像Ｖの特徴量の変化に依存した画像の記憶ができる。ここでのマッチング誤差Ｅ（ｉ）の算出方法は、後述する自律走行時の方法と同じである。この画像情報にＧＰＳ端末１４による位置情報Ｇ（ｉ）と、その際のモータコマンドＭ（ｉ）（直進、右折、左折など）を付加した時系列情報を教示データ（すなわちセンサ情報）とする。
【００３２】
（自律走行）
自律走行は主に画像とモータコマンドをベースとして行う。画像によるナビゲーションは、現在の外部画像と記憶した記憶画像との照合度（後述する）を計算し、そこから記憶画像と外部画像の視野のずれを算出する。そして、そのずれを自律移動体のステアリングに反映することで自律移動体の軌道を随時修正し、ナビゲーションを実現する。
【００３３】
（照合度の計算）
画像の照合は抽出した特徴点同士の距離の平均値が小さくなるように、片方の画像を上下左右に動かしながらマッチングをとる。このときの距離の平均値をマッチング誤差Ｅ（ｉ）として、この値が小さいほど照合度が大きいとする。
この方法は輝度値など屋外では不安定な特徴を用いないこと、特徴点ごとではなく特徴点全体を考慮した照合方法であることなどから屋外においても有用な方法である。
【００３４】
画像マッチングの方法は，大分して領域基準マッチングと特徴量基準マッチングの２つに分類される。
領域基準マッチングは特徴量付近の局所相関に基づく方法であり、画素情報（輝度値など）に大きく依存するため、屋外画像の場合に誤対応が多くなる。これに対し、特徴量基準マッチングは特徴量を広い領域で比較する方法である。この方法は他の画素情報に依存しない対応付けができるので、誤対応が少ない特徴がある。
以下、本発明における画像マッチングのアルゴリズムについて説明する．
【００３５】
図４は、本発明によるマッチング誤差Ｅ（ｉ）の計算方法を示す模式図である。
この図において、黒丸は記憶画像Ｖ（ｉ）の特徴点、白丸は現在の外部画像Ｖの特徴点を示す。記憶画像Ｖ（ｉ）と外部画像Ｖの特徴点抽出は、上述した非特許文献４の方法による。
次にこの図において、まず記憶画像Ｖ（ｉ）中のある特徴点Ｒ１と、外部画像Ｖ中のすべての特徴点との距離を計算し，その中で最小のものを選択する（Ａ）。これをすべての特徴点に対して行い、それぞれの特徴点を１対１の対応にする（Ｂ）。
次いで、１対１に対応した２点間の距離が所定の閾値以下のもの選択し、このとき条件を満たす特徴点の数をｎとして、それらの距離の総和をｎで割った平均値（平均距離）を算出する。
この計算を、記憶画像Ｖ（ｉ）又は外部画像Ｖを上下左右に平行移動しながら行い（Ｃ）、平均距離がもっとも小さいときを２つの画像のマッチングが取れたものとする（Ｄ）。またこのときの平均距離（最小平均距離）を２つの画像のマッチング誤差Ｅ（ｉ）とする。
この計算により２つの画像間における、マッチング誤差Ｅ（ｉ）とそのときの２画像間の横方向，縦方向のずれが算出される。ここでマッチング誤差Ｅ（ｉ）は平均距離の最小値（最小平均距離）なので、小さいほうが２つの画像は似ていると判断することができる。
【００３６】
（画像とモータコマンドによるナビゲーション）
図５は、本発明による自律走行ステップＳ２のフロー図である。自律走行ステップＳ２は、初期位置決定ステップＳ２１，Ｓ２２，Ｓ２３とナビゲーションステップＳ２４、Ｓ２５，Ｓ２６，Ｓ２７，Ｓ２８，Ｓ２９からなる。
まずナビゲーション開始時は、記憶画像の最初の画像（ｉ＝１，ｉは画像のシーケンス番号）と照合度を計算しマッチングがとれるまで探索する。マッチングがとれたらそこに付加されている行動（モータコマンド）を実行する。
【００３７】
すなわち、初期位置決定ステップＳ２１，Ｓ２２，Ｓ２３において、ステップＳ２１で外部画像Ｖを撮影し、外部画像Ｖの特徴点を抽出し、外部画像Ｖと初期画像Ｖ（１）とのマッチング誤差Ｅ（１）を計算する。次いで、ステップＳ２２で誤差Ｅ（１）が所定の閾値ｅ１より小さいか否かを判断し、ＮＯであれば初期位置ではないため、ステップＳ２１を再実行する。
ステップＳ２２でＹＥＳであれば、初期位置が決定され、ステップＳ２３でｉ＝１とし自律走行を開始し、ステップＳ２４でモータコマンドＭ（１）を実行する。
なお、初期位置までは、ＧＰＳ端末と方位センサにより自律移動体１を案内してもよい。また、初期位置の代わりに、既にマッチングが取れた走行経路上の任意の位置（ｉ＝ｋ）までＧＰＳ端末と方位センサにより自律移動体を案内し、その位置から自律走行を開始してもよい。
【００３８】
自律走行開始後のナビゲーションは、連続した２つの記憶画像Ｖ（ｉ）及びＶ（ｉ＋１）についてマッチング誤差Ｅ（ｉ）、Ｅ（ｉ＋１）を計算し、ｉ＋１の照合度がｉの照合度を上回った場合（Ｅ（ｉ＋１）＜Ｅ（ｉ））、次の画像へ近づいたと判断しｉの値を１増やし、そこに付加された行動、すなわちモータコマンドＭ（ｉ）を実行する。
また、次の画像へ近づくまでは、照合度の計算の際に算出した画像の横方向のずれｄｘ（ｉ）を式（１）で示すようにステアリング量ＳＴに反映することで目的の軌道に復帰する。ここでＧａｉｎは比例係数である。
ＳＴ＝Ｇａｉｎ×ｄｘ（ｉ）・・・（１）
【００３９】
すなわち、ナビゲーションステップＳ２４、Ｓ２５，Ｓ２６，Ｓ２７，Ｓ２８，Ｓ２９において、ステップＳ２５で外部画像Ｖを撮影し、外部画像Ｖの特徴点を抽出し、外部画像Ｖと画像Ｖ（ｉ）及び画像Ｖ（ｉ＋１）とのマッチング誤差Ｅ（ｉ）、Ｅ（ｉ＋１）、横方向のずれｄｘ（ｉ）を計算する。次いで、ステップＳ２６で誤差Ｅ（ｉ＋１）がＥ（ｉ）より小さいか否かを判断し、ＹＥＳであれば次の画像へ近づいたと判断しステップＳ２７でｉ＝ｉ＋１とし、ステップＳ２４でモータコマンドＭ（ｉ）を実行する。
ステップＳ２６でＮＯであれば、ステップＳ２８で誤差Ｅ（ｉ）とＥ（ｉ＋１）が所定の閾値ｅ２より小さいか否かを判断し、ＹＥＳであれば、ステアリング量ＳＴを制御して軌道に復帰する。
ステップＳ２８でＮＯの場合、所定の軌道から外れている可能性が高い。この場合、マッチングが取れないマッチングミスと判断し、既にマッチングが取れた走行経路上の最も近い位置（マッチング誤差が所定の閾値より小さい地点）及び方位までＧＰＳ端末１４と方位センサ１６により自律移動体を案内する。
【実施例】
【００４０】
（屋外走行実験）
上述した本発明の有効性を検証するために、屋外において画像情報のみを用いた直線走行実験を行った。この実験では、図１に示した自律移動体１にカメラ１２と制御装置１８からなるナビゲーション装置１０を搭載し、ＧＰＳ端末１４と方位センサ１６は省略した。
図６は、本発明の実施例を示す図であり、横軸は移動距離、縦軸はマッチング誤差、図中の黒丸と実線は、画像Ｖ（ｉ）とのマッチング誤差Ｅ（ｉ）、白丸と破線は画像Ｖ（ｉ＋１）とのマッチング誤差Ｅ（ｉ＋１）である。なお、両矢印の範囲において、ｉは左から順に１，２，３，４である。
この図から両矢印の各範囲において、範囲が切り替わる直前のみ、Ｅ（ｉ＋１）＜Ｅ（ｉ）となっていることがわかる。
すなわちマッチング誤差の計算により、予め取得した教示画像シーケンスに対する画像の遷移ができていることがわかる。従って各種パラメータの調整を行うことで長距離走行が可能になる。
【００４１】
この実験において、ナビゲーションの途中で自律移動体１が現在位置を見失うことがあった。
すなわち、この実験のように画像情報のみのナビゲーションでは、似たような画像系列が続いた場合や、特徴点の極端に少ない場所でナビゲーションが継続できない場合がある。そのような場合に、上述したＧＰＳ端末１４により記憶した位置情報と現在の位置情報を用いてナビゲーションに復帰することができる。
すなわちナビゲーションを失敗したと判断したら、現在の位置を基に教示データの中から、最も座標が近い画像を中心として、１０枚前後の記憶画像を選択する。そのすべての画像と現在の画像とのマッチング誤差を計算し、その中でも最も照合度が大きいものを現在位置と判断する。
さらに、前回照合がとれた画像との間に右左折がある場合とない場合とで行動選択を変える。まず右左折がない場合、ロボットは照合がとれた点から再び画像によるナビゲーションを開始する。右左折があった場合には、最後に照合がとれた場所へ駆動輪の回転データを逆算してナビゲーションを行うのがよい。
【００４２】
なお、本発明は上述した実施例及び実施形態に限定されず、本発明の要旨を逸脱しない範囲で種々変更できることは勿論である。
【図面の簡単な説明】
【００４３】
【図１】本発明のナビゲーション装置を備えた自律移動体の模式的平面図である。
【図２】本発明によるナビゲーション方法を示す全体フロー図である。
【図３】本発明による教示走行ステップにおけるセンサ情報の記憶方法を示す模式図である。
【図４】本発明によるマッチング誤差の計算方法を示す模式図である。
【図５】本発明による自律走行ステップのフロー図である。
【図６】本発明の実施例を示す図である。
【図７】特許文献１のシステムの模式図である。
【符号の説明】
【００４４】
１ 自律移動体、１ａ 本体、２，３ 駆動輪、
１０ ナビゲーション装置、１２ カメラ、
１４ ＧＰＳ端末、１６ 方位センサ、１８ 制御装置
【技術分野】
【０００１】
本発明は、移動ロボットや自律搬送台車に適用するためのナビゲーション装置とその制御方法に関する。
【背景技術】
【０００２】
近年、様々な場所で移動ロボットが使われ始め、その需要も年々高まっている。その中でも特にＡＧＶ（無人搬送台車）の研究が盛んに行われている。
これら移動ロボットや無人搬送台車の移動手段として、磁気テープや光学式テープなどを用いたガイド式移動手段と、レーザーレンジファインダ（ＬＲＦ）やＲＴＫ−ＧＰＳなどを用いた自律式移動手段とが既に提案されている（例えば、特許文献１、非特許文献１〜３）。
また、本発明と関連する技術として、非特許文献４が開示されている。
【０００３】
特許文献１のシステムは、目視可能な軌条線、コマンドマーク内容の画像認識の情報解析によって走行方向の選択、走行速度等を制御する視覚誘導方式として、小型軽量、低床化した複数の搬送台車を確実、安定的に総合的に制御することを目的とする。
このため、特許文献１では、図７に示すように、被搬送物Ｐを載置する走行台車本体５１を走行駆動する走行駆動装置５２、搬送走行面上に敷設表示の軌条線５３及び載置セットした制御コードマーク片５４に表示のコマンドマーク夫々を視覚的に画像認識する視覚認識装置５５、軌条線５３に沿った走行方向を操舵する操舵装置５６、視覚認識装置５５で撮像・認識したコマンドコードで走行台車本体５１の速度、走行方向、停止・発進・後退等を制御する制御装置を備える。軌条線５３は粘着テープ体表面に目視可能に表示形成し、制御コードマーク片５４はコマンドスタート部５４ａ、コマンドコードを表示するコマンド部５４ｂ，５４ｃ、コマンドエンド部５４ｄ夫々を左右で対称表示し、制御内容を目視理解させる目視表示部５７を連設してある。
【０００４】
非特許文献１の自律式移動手段は、記録ランにおいてロボットを所定の経路に沿って移動させて複数の画像を記憶し、自動ランにおいて記憶した画像と外部から取得した実画像と比較し、その一致誤差（マッチングエラー）に応じてロボットの軌道を修正するものである。マッチングエラーの計算は、記憶画像をテンプレートとして実画像と輝度分布が一致する領域同士の相対位置誤差を算出している。
【０００５】
非特許文献２の自律式移動手段は、記録ランにおいてレーザーレンジファインダ（ＬＲＦ）を用いて外界を記憶し、自動ランにおいてレーザーレンジファインダによる外界データと記憶データとのマッチングエラーに応じてロボットの軌道を修正するものである。
【０００６】
非特許文献３の自律式移動手段は、画像情報を基にテンプレートマッチングにより、画像照合をし、誘導と自律移動を行うものである。
【０００７】
非特許文献４は、取得画像に対して特徴点抽出手段を開示している。
【０００８】
【特許文献１】特開２００１−１８８６１０号公報、「視覚誘導方式による無人搬送台車の制御方法及びこれを利用した無人搬送台車システム」
【０００９】
【非特許文献１】Ｙ． Ｍａｔｓｕｍｏｔｏ，Ｋ． Ｓａｋａｉ，Ｍ． Ｉｎａｂａ，Ｈ． Ｉｎｏｕｅ，“Ｖｉｅｗ−Ｂａｓｅｄ Ａｐｐｒｏａｃｈ ｔｏ Ｒｏｂｏｔ Ｎａｖｉｇａｔｉｏｎ”， Ｐｒｏｃｅｅｄｉｎｇｓ ｏｆ ２０００ ＩＥＥＥ／ＲＳＪ Ｉｎｔｅｒｎａｔｉｏｎａｌ Ｃｏｎｆｅｒｅｎｃｅ ｏｎ Ｉｎｔｅｌｌｉｇｅｎｔ Ｒｏｂｏｔｓ ａｎｄ Ｓｙｓｔｅｍｓ（ＩＲＯＳ’２０００），Ｆ−ＭＩ−６−３， ｐｐ．１７０２−１７０８，Ｔａｋａｍａｔｓｕ，Ｊａｐａｎ，Ｎｏｖ １ｓｔ−３ｒｄ， ２０００．
【非特許文献２】Ｙ． Ａｄａｃｈｉ， Ｈ． Ｓａｉｔｏ， Ｙ． Ｍａｔｓｕｍｏｔｏ， Ｔ． Ｏｇａｓａｗａｒａ，“Ｍｅｍｏｒｙ−Ｂａｓｅｄ Ｎａｖｉｇａｔｉｏｎ ｕｓｉｎｇ Ｄａｔａ Ｓｅｑｕｅｎｃｅ ｏｆ Ｌａｓｅｒ Ｒａｎｇｅ Ｆｉｎｄｅｒ”，Ｐｒｏｃｅｅｄｉｎｇｓ ｏｆ ２００３ ＩＥＥＥ Ｉｎｔｅｒｎａｔｉｏｎａｌ Ｓｙｍｐｏｓｉｕｍ ｏｎ Ｃｏｍｐｕｔａｔｉｏｎａｌ Ｉｎｔｅｌｌｉｇｅｎｃｅ ｉｎ Ｒｏｂｏｔｉｃｓ ａｎｄ Ａｕｔｏｍａｔｉｏｎ，ｐｐ．４７９−４８４，Ｋｏｂｅ，Ｊａｐａｎ，Ｊｕｌｙ １６−２０，２００３．
【非特許文献３】桂浩章，他、「季節や天候の変化にロバストな視覚認識に基づく移動ロボットの屋外ナビゲーション」、日本ロボット学会誌、Ｖｏｌ．２３， Ｎｏ．１， ｐｐ．７５〜８３，２００５
【非特許文献４】Ｊ，Ｓｈｉ ａｎｄ Ｃ．Ｔｏｍａｓｉ，“Ｇｏｏｄ Ｆｅａｔｕｒｅｓ ｔｏ Ｔｒａｃｋ”，１９９４ ＩＥＥＥ Ｃｏｎｆｅｒｅｎｃｅ ｏｎ Ｃｏｍｐｕｔｅｒ Ｖｉｓｉｏｎ ａｎｄ Ｐａｔｔｅｒｎ Ｒｅｃｏｇｎｉｔｉｏｎ （ＣＶＰＲ’９４），ｐｐ．５９３−６００，１９９４．
【発明の開示】
【発明が解決しようとする課題】
【００１０】
従来、屋内における移動ロボットや無人搬送台車の移動手段は、特許文献１のように、磁気テープや光学式テープなどを用いたガイド式移動手段が主流であった。
しかし、製品サイクルの短縮化から搬送経路の多様化が要求され、無人搬送台車における移動手段にも経路変更が容易な自律式移動手段が求められるようになった。
この自律式移動手段として、上述した非特許文献１〜３のナビゲーション手段が開示されている。
【００１１】
非特許文献１のナビゲーション手段は、マッチングエラーの計算に輝度分布を用いるため、画像上の輝度分布の変化が大きい屋外では適用できない。
非特許文献２，３のナビゲーション手段は、レーザーレンジファインダ（ＬＲＦ）やＲＴＫ−ＧＰＳなど高価で複雑なセンサを用いるため、装置が複雑化し高価となるため、用途が限定される問題点がある。
【００１２】
本発明はかかる問題点を解決するために創案されたものである。すなわち、本発明の目的は、レーザーレンジファインダ（ＬＲＦ）やＲＴＫ−ＧＰＳなど高価で複雑なセンサを用いることなく、輝度分布の変化が大きい屋外で適用でき、磁気テープや光学式テープなどを用いずに容易に経路変更ができるナビゲーション装置とその制御方法を提供することにある。
【課題を解決するための手段】
【００１３】
本発明によれば、移動可能な自律移動体に搭載されたナビゲーション装置であって、
自律移動体の移動方向の画像をリアルタイムに撮影するカメラと、
記憶装置と演算装置を有し前記カメラの画像に基づき自律移動体を制御する制御装置とを備え、
前記制御装置は、教示走行時において自律移動体が所定の走行経路を移動する際にカメラの画像と自律移動体に対するコマンドを含む所定のセンサ情報を記憶し、
自律走行時において記憶した前記センサ情報と現時点のセンサ情報とを比較して、自律移動体が前記走行経路を移動するように自律移動体を制御する、ことを特徴とするナビゲーション装置が提供される。
【００１４】
本発明の好ましい実施形態によれば、前記制御装置は、記憶画像と現時点の外部画像の特徴点を抽出する特徴点抽出手段と、前記特徴点同士のマッチング誤差を計算する誤差計算手段とを有する。
【００１５】
また、自律移動体の現在位置をリアルタイムに検出するＧＰＳ端末と、
自律移動体の移動方向をリアルタイムに検出する方位センサと自律移動体の駆動輪の回転角を計測するセンサとを備え、
前記制御装置は、前記マッチング誤差が所定の閾値より小さい地点まで、自律移動体を案内する復帰手段を有する。
【００１６】
また本発明によれば、移動可能な自律移動体に搭載され、
自律移動体の移動方向の画像を一定間隔で撮影するカメラと、
記憶装置と演算装置を有し前記カメラの画像に基づき自律移動体を制御する制御装置とを備えたナビゲーション装置の制御方法であって、
自律移動体を所定の走行経路に沿って移動させて、前記記憶装置にカメラの画像と自律移動体に対するコマンドを含む所定のセンサ情報を記憶する教示走行ステップと、
記憶した前記センサ情報と現時点のセンサ情報とを比較して、自律移動体が前記走行経路を移動するように自律移動体を制御する自律走行ステップとを有する、ことを特徴とするナビゲーション装置の制御方法が提供される。
【００１７】
本発明の好ましい実施形態によれば、前記教示走行ステップにおいて、所定の走行経路上において現在の外部画像Ｖと直前の記憶画像Ｖ（ｉ）（ｉは正の整数）とのマッチング誤差Ｅ（ｉ）を演算し、そのマッチング誤差Ｅ（ｉ）が所定の閾値ｅ１を上回った場合に新しい画像Ｖ（ｉ＋１）と自律移動体に対する新しいコマンドＭ（ｉ＋１）を記憶する。
【００１８】
前記自律走行ステップにおいて、所定の走行経路上において現在の外部画像Ｖと直前の記憶画像Ｖ（ｉ）（ｉは正の整数）とのマッチング誤差Ｅ（ｉ）と、外部画像Ｖと記憶画像Ｖ（ｉ）との視野の水平ずれｄｘ（ｉ）を算出し、その水平ずれを自律移動体のステアリングに反映して軌道修正する。
【００１９】
また、前記マッチング誤差Ｅ（ｉ）は、記憶画像Ｖ（ｉ）と現時点の外部画像Ｖの特徴点を抽出し、抽出した特徴点同士の距離の平均値が最小になるように、記憶画像Ｖ（ｉ）又は外部画像Ｖを相対的に移動させた際の最小平均距離であり、
前記水平ずれｄｘ（ｉ）は、前記最小平均距離の水平成分である。
【００２０】
また、前記マッチング誤差Ｅ（ｉ）は、記憶画像Ｖ（ｉ）と現時点の外部画像Ｖの特徴点に対し、記憶画像Ｖ（ｉ）中のある特徴点と、外部画像Ｖ中のすべての特徴点との距離を計算し、その中で最小のものを選択し、これをすべての特徴点に対して行い、それぞれの特徴点を１対１に対応させ、対応する２点間の距離が所定の閾値以下のものを小さい順に複数選択し、それらの距離の平均値を算出し、この平均値が最小になるように、記憶画像Ｖ（ｉ）又は外部画像Ｖを相対的に移動させた際の最小平均距離であり、
前記最小平均距離が所定の閾値以下のときに２つの画像のマッチングが取れたものと判断する。
【００２１】
また、前記マッチング誤差Ｅ（ｉ）が所定の閾値を超えるときにマッチングが取れないマッチングミスと判断し、既にマッチングが取れた走行経路上の最も近い位置及び方位までＧＰＳ端末と方位センサにより自律移動体を案内する。
【発明の効果】
【００２２】
上記本発明の装置及び方法によれば、教示走行時において自律移動体が所定の走行経路を移動する際にカメラの画像と自律移動体に対するコマンドを含む所定のセンサ情報を記憶し、自律走行時において記憶したセンサ情報と現時点のセンサ情報とを比較して、自律移動体が前記走行経路を移動するように自律移動体を制御するので、レーザーレンジファインダ（ＬＲＦ）やＲＴＫ−ＧＰＳなど高価で複雑なセンサを用いることなく自律移動体を制御することができ、かつ磁気テープや光学式テープなどを用いずに容易に経路変更ができる。
【００２３】
また、特徴点抽出手段により記憶画像と現時点の外部画像の特徴点を抽出し、誤差計算手段により特徴点同士のマッチング誤差を計算することにより、輝度分布の変化が大きい屋外に適用することができる。
【発明を実施するための最良の形態】
【００２４】
以下、本発明の好ましい実施形態を図面を参照して説明する。なお、各図において共通する部分には同一の符号を付し、重複した説明を省略する。
【００２５】
図１は、本発明のナビゲーション装置を備えた自律移動体の模式的平面図である。
この自律移動体１は、図示しない補助輪と、左右の独立した駆動輪２，３を有し、駆動輪２，３の同方向又は逆方向の回転により、直進（図で右方向）、右折、左折ができるようになっている。
【００２６】
本発明のナビゲーション装置１０は、自律移動体１の本体１ａに固定されたカメラ１２、ＧＰＳ端末１４、方位センサ１６及び制御装置１８を備える。
カメラ１２は、例えば安価なＵＳＢカメラ又はウエブカメラであり、自律移動体１の前方の画像をリアルタイムに撮影し制御装置１８に入力するようになっている。画像の視野角は、固定であるのが好ましいが可変であってもよい。画像は、少なくとも３０万画素（６４０×４８０ピクセル）以上のモノクロ画像であるのがよい。
ここで「リアルタイム」とは、走行経路上のある位置を通過する瞬間と撮影時との時間差がほとんど０（例えば１００ｍｓｅｃ以下）であることを意味する。なお、撮影自体は連続的ではなく、制御装置１８による後述する画像処理に要する時間より長い一定時間間隔であってもよい。
【００２７】
ＧＰＳ端末１４は、全地球測位システム（ＧＰＳ）の端末であり、その位置（緯度と経度）をリアルタイムに制御装置１８に入力する。ＧＰＳ端末の位置検出精度は、少なくとも数ｍ以内のものを使用する。
方位センサ１６は、自律移動体１の前方方位をリアルタイムに検出し制御装置１８に入力する。
制御装置１８は、記憶装置と演算装置を有するコンピュータであり、本発明によるナビゲーション方法を実施するためのプログラムが記憶装置にインストールされ、このプログラムにより自律移動体１を制御するようになっている。
制御装置１８は、記憶装置に記憶された記憶画像と現時点の外部画像の特徴点を抽出する特徴点抽出手段、特徴点同士のマッチング誤差を計算する誤差計算手段、及びマッチング誤差が所定の閾値より小さい地点まで、自律移動体を案内する復帰手段として機能する。
これらの特徴点抽出手段、誤差計算手段及び復帰手段は、コンピュータの記憶装置にインストールされたプログラムと演算装置により実行される。
【００２８】
図２は本発明によるナビゲーション方法を示す全体フロー図である。この図に示すように本発明のナビゲーション方法は、教示走行ステップＳ１と自律走行ステップＳ２とからなる。
教示走行ステップＳ１は、人間が自律移動体１を操作する走行であり、その際制御装置１８は、そのときの走行経路のセンサ情報を記憶する。自律移動体１の操作は、制御装置１８に無線又は有線で任意のモータコマンド（回転方向、回転速度、等）を入力して行うのがよい。
一方、自律走行ステップＳ２は、記憶したセンサ情報と現在のセンサ情報とを比較して制御することにより走行する走行である。
【００２９】
教示走行ステップＳ１においてどのようなセンサ情報を記憶させ、自律走行ステップＳ２においてどのような制御を行うかを以下に説明する。
【００３０】
（教示走行）
本発明において、記憶するセンサ情報は、カメラ１２による撮影画像、ＧＰＳ端末１４による位置情報、方位センサ１６による方位、及び駆動輪２，３に対するモータコマンド（回転方向、回転速度、等）である。以下、記憶装置に記憶された画像を「記憶画像」、カメラ１２による現在の撮影画像を「外部画像」と呼ぶ。
なお、本発明では屋外走行において、記憶する画像には明るさの変化に対してロバストな画像処理を行う必要がある。そこで、本発明では、取得画像に対して特徴点抽出を行い、抽出された点同士のマッチングをとる。特徴点抽出は色への依存度が低いため、多少色味が変わっても影響を受けることなく抽出できる利点がある。また画像全体が見えていないような場合でも、部分的な照合も可能である。
本発明では、画像特徴点の抽出・追跡手段として、非特許文献４に開示されている特徴点抽出（Ｇｏｏｄ Ｆｅａｔｕｒｅｓ ｔｏ Ｔｒａｃｋ）を用いた。以下、この方法を単に「特徴点抽出法」と呼ぶ。本発明では、マッチング可能な任意の特徴点抽出手法を用いることができ、有用である。
【００３１】
（教示データの作成）
図３は、本発明による教示走行ステップＳ１におけるセンサ情報の記憶方法を示す模式図である。
本発明で用いるセンサ情報は、画像情報を基に生成する。スタート地点の記憶画像をＶ（ｉ）（ｉ＝１、ｉは正の整数）として、現在の外部画像Ｖと前回記憶した記憶画像Ｖ（ｉ）とのマッチング誤差Ｅ（ｉ）を演算し、そのマッチング誤差Ｅ（ｉ）が所定の閾値ｅ１を上回った場合に新しい画像Ｖ（ｉ＋１）と新しいコマンドＭ（ｉ＋１）を記憶する。
これにより、自律移動体１の移動距離には依存せず、外部画像Ｖの特徴量の変化に依存した画像の記憶ができる。ここでのマッチング誤差Ｅ（ｉ）の算出方法は、後述する自律走行時の方法と同じである。この画像情報にＧＰＳ端末１４による位置情報Ｇ（ｉ）と、その際のモータコマンドＭ（ｉ）（直進、右折、左折など）を付加した時系列情報を教示データ（すなわちセンサ情報）とする。
【００３２】
（自律走行）
自律走行は主に画像とモータコマンドをベースとして行う。画像によるナビゲーションは、現在の外部画像と記憶した記憶画像との照合度（後述する）を計算し、そこから記憶画像と外部画像の視野のずれを算出する。そして、そのずれを自律移動体のステアリングに反映することで自律移動体の軌道を随時修正し、ナビゲーションを実現する。
【００３３】
（照合度の計算）
画像の照合は抽出した特徴点同士の距離の平均値が小さくなるように、片方の画像を上下左右に動かしながらマッチングをとる。このときの距離の平均値をマッチング誤差Ｅ（ｉ）として、この値が小さいほど照合度が大きいとする。
この方法は輝度値など屋外では不安定な特徴を用いないこと、特徴点ごとではなく特徴点全体を考慮した照合方法であることなどから屋外においても有用な方法である。
【００３４】
画像マッチングの方法は，大分して領域基準マッチングと特徴量基準マッチングの２つに分類される。
領域基準マッチングは特徴量付近の局所相関に基づく方法であり、画素情報（輝度値など）に大きく依存するため、屋外画像の場合に誤対応が多くなる。これに対し、特徴量基準マッチングは特徴量を広い領域で比較する方法である。この方法は他の画素情報に依存しない対応付けができるので、誤対応が少ない特徴がある。
以下、本発明における画像マッチングのアルゴリズムについて説明する．
【００３５】
図４は、本発明によるマッチング誤差Ｅ（ｉ）の計算方法を示す模式図である。
この図において、黒丸は記憶画像Ｖ（ｉ）の特徴点、白丸は現在の外部画像Ｖの特徴点を示す。記憶画像Ｖ（ｉ）と外部画像Ｖの特徴点抽出は、上述した非特許文献４の方法による。
次にこの図において、まず記憶画像Ｖ（ｉ）中のある特徴点Ｒ１と、外部画像Ｖ中のすべての特徴点との距離を計算し，その中で最小のものを選択する（Ａ）。これをすべての特徴点に対して行い、それぞれの特徴点を１対１の対応にする（Ｂ）。
次いで、１対１に対応した２点間の距離が所定の閾値以下のもの選択し、このとき条件を満たす特徴点の数をｎとして、それらの距離の総和をｎで割った平均値（平均距離）を算出する。
この計算を、記憶画像Ｖ（ｉ）又は外部画像Ｖを上下左右に平行移動しながら行い（Ｃ）、平均距離がもっとも小さいときを２つの画像のマッチングが取れたものとする（Ｄ）。またこのときの平均距離（最小平均距離）を２つの画像のマッチング誤差Ｅ（ｉ）とする。
この計算により２つの画像間における、マッチング誤差Ｅ（ｉ）とそのときの２画像間の横方向，縦方向のずれが算出される。ここでマッチング誤差Ｅ（ｉ）は平均距離の最小値（最小平均距離）なので、小さいほうが２つの画像は似ていると判断することができる。
【００３６】
（画像とモータコマンドによるナビゲーション）
図５は、本発明による自律走行ステップＳ２のフロー図である。自律走行ステップＳ２は、初期位置決定ステップＳ２１，Ｓ２２，Ｓ２３とナビゲーションステップＳ２４、Ｓ２５，Ｓ２６，Ｓ２７，Ｓ２８，Ｓ２９からなる。
まずナビゲーション開始時は、記憶画像の最初の画像（ｉ＝１，ｉは画像のシーケンス番号）と照合度を計算しマッチングがとれるまで探索する。マッチングがとれたらそこに付加されている行動（モータコマンド）を実行する。
【００３７】
すなわち、初期位置決定ステップＳ２１，Ｓ２２，Ｓ２３において、ステップＳ２１で外部画像Ｖを撮影し、外部画像Ｖの特徴点を抽出し、外部画像Ｖと初期画像Ｖ（１）とのマッチング誤差Ｅ（１）を計算する。次いで、ステップＳ２２で誤差Ｅ（１）が所定の閾値ｅ１より小さいか否かを判断し、ＮＯであれば初期位置ではないため、ステップＳ２１を再実行する。
ステップＳ２２でＹＥＳであれば、初期位置が決定され、ステップＳ２３でｉ＝１とし自律走行を開始し、ステップＳ２４でモータコマンドＭ（１）を実行する。
なお、初期位置までは、ＧＰＳ端末と方位センサにより自律移動体１を案内してもよい。また、初期位置の代わりに、既にマッチングが取れた走行経路上の任意の位置（ｉ＝ｋ）までＧＰＳ端末と方位センサにより自律移動体を案内し、その位置から自律走行を開始してもよい。
【００３８】
自律走行開始後のナビゲーションは、連続した２つの記憶画像Ｖ（ｉ）及びＶ（ｉ＋１）についてマッチング誤差Ｅ（ｉ）、Ｅ（ｉ＋１）を計算し、ｉ＋１の照合度がｉの照合度を上回った場合（Ｅ（ｉ＋１）＜Ｅ（ｉ））、次の画像へ近づいたと判断しｉの値を１増やし、そこに付加された行動、すなわちモータコマンドＭ（ｉ）を実行する。
また、次の画像へ近づくまでは、照合度の計算の際に算出した画像の横方向のずれｄｘ（ｉ）を式（１）で示すようにステアリング量ＳＴに反映することで目的の軌道に復帰する。ここでＧａｉｎは比例係数である。
ＳＴ＝Ｇａｉｎ×ｄｘ（ｉ）・・・（１）
【００３９】
すなわち、ナビゲーションステップＳ２４、Ｓ２５，Ｓ２６，Ｓ２７，Ｓ２８，Ｓ２９において、ステップＳ２５で外部画像Ｖを撮影し、外部画像Ｖの特徴点を抽出し、外部画像Ｖと画像Ｖ（ｉ）及び画像Ｖ（ｉ＋１）とのマッチング誤差Ｅ（ｉ）、Ｅ（ｉ＋１）、横方向のずれｄｘ（ｉ）を計算する。次いで、ステップＳ２６で誤差Ｅ（ｉ＋１）がＥ（ｉ）より小さいか否かを判断し、ＹＥＳであれば次の画像へ近づいたと判断しステップＳ２７でｉ＝ｉ＋１とし、ステップＳ２４でモータコマンドＭ（ｉ）を実行する。
ステップＳ２６でＮＯであれば、ステップＳ２８で誤差Ｅ（ｉ）とＥ（ｉ＋１）が所定の閾値ｅ２より小さいか否かを判断し、ＹＥＳであれば、ステアリング量ＳＴを制御して軌道に復帰する。
ステップＳ２８でＮＯの場合、所定の軌道から外れている可能性が高い。この場合、マッチングが取れないマッチングミスと判断し、既にマッチングが取れた走行経路上の最も近い位置（マッチング誤差が所定の閾値より小さい地点）及び方位までＧＰＳ端末１４と方位センサ１６により自律移動体を案内する。
【実施例】
【００４０】
（屋外走行実験）
上述した本発明の有効性を検証するために、屋外において画像情報のみを用いた直線走行実験を行った。この実験では、図１に示した自律移動体１にカメラ１２と制御装置１８からなるナビゲーション装置１０を搭載し、ＧＰＳ端末１４と方位センサ１６は省略した。
図６は、本発明の実施例を示す図であり、横軸は移動距離、縦軸はマッチング誤差、図中の黒丸と実線は、画像Ｖ（ｉ）とのマッチング誤差Ｅ（ｉ）、白丸と破線は画像Ｖ（ｉ＋１）とのマッチング誤差Ｅ（ｉ＋１）である。なお、両矢印の範囲において、ｉは左から順に１，２，３，４である。
この図から両矢印の各範囲において、範囲が切り替わる直前のみ、Ｅ（ｉ＋１）＜Ｅ（ｉ）となっていることがわかる。
すなわちマッチング誤差の計算により、予め取得した教示画像シーケンスに対する画像の遷移ができていることがわかる。従って各種パラメータの調整を行うことで長距離走行が可能になる。
【００４１】
この実験において、ナビゲーションの途中で自律移動体１が現在位置を見失うことがあった。
すなわち、この実験のように画像情報のみのナビゲーションでは、似たような画像系列が続いた場合や、特徴点の極端に少ない場所でナビゲーションが継続できない場合がある。そのような場合に、上述したＧＰＳ端末１４により記憶した位置情報と現在の位置情報を用いてナビゲーションに復帰することができる。
すなわちナビゲーションを失敗したと判断したら、現在の位置を基に教示データの中から、最も座標が近い画像を中心として、１０枚前後の記憶画像を選択する。そのすべての画像と現在の画像とのマッチング誤差を計算し、その中でも最も照合度が大きいものを現在位置と判断する。
さらに、前回照合がとれた画像との間に右左折がある場合とない場合とで行動選択を変える。まず右左折がない場合、ロボットは照合がとれた点から再び画像によるナビゲーションを開始する。右左折があった場合には、最後に照合がとれた場所へ駆動輪の回転データを逆算してナビゲーションを行うのがよい。
【００４２】
なお、本発明は上述した実施例及び実施形態に限定されず、本発明の要旨を逸脱しない範囲で種々変更できることは勿論である。
【図面の簡単な説明】
【００４３】
【図１】本発明のナビゲーション装置を備えた自律移動体の模式的平面図である。
【図２】本発明によるナビゲーション方法を示す全体フロー図である。
【図３】本発明による教示走行ステップにおけるセンサ情報の記憶方法を示す模式図である。
【図４】本発明によるマッチング誤差の計算方法を示す模式図である。
【図５】本発明による自律走行ステップのフロー図である。
【図６】本発明の実施例を示す図である。
【図７】特許文献１のシステムの模式図である。
【符号の説明】
【００４４】
１ 自律移動体、１ａ 本体、２，３ 駆動輪、
１０ ナビゲーション装置、１２ カメラ、
１４ ＧＰＳ端末、１６ 方位センサ、１８ 制御装置
【特許請求の範囲】
【請求項１】
移動可能な自律移動体に搭載されたナビゲーション装置であって、
自律移動体の移動方向の画像をリアルタイムに撮影するカメラと、
記憶装置と演算装置を有し前記カメラの画像に基づき自律移動体を制御する制御装置とを備え、
前記制御装置は、教示走行時において自律移動体が所定の走行経路を移動する際にカメラの画像と自律移動体に対するコマンドを含む所定のセンサ情報を記憶し、
自律走行時において記憶した前記センサ情報と現時点のセンサ情報とを比較して、自律移動体が前記走行経路を移動するように自律移動体を制御する、ことを特徴とするナビゲーション装置。
【請求項２】
前記制御装置は、記憶画像と現時点の外部画像の特徴点を抽出する特徴点抽出手段と、前記特徴点同士のマッチング誤差を計算する誤差計算手段とを有する、ことを特徴とする請求項１に記載のナビゲーション装置。
【請求項３】
自律移動体の現在位置をリアルタイムに検出するＧＰＳ端末と、
自律移動体の移動方向をリアルタイムに検出する方位センサと自律移動体の駆動輪の回転角を計測するセンサとを備え、
前記制御装置は、前記マッチング誤差が所定の閾値より小さい地点まで、自律移動体を案内する復帰手段を有する、ことを特徴とする請求項２に記載のナビゲーション装置。
【請求項４】
移動可能な自律移動体に搭載され、
自律移動体の移動方向の画像を一定間隔で撮影するカメラと、
記憶装置と演算装置を有し前記カメラの画像に基づき自律移動体を制御する制御装置とを備えたナビゲーション装置の制御方法であって、
自律移動体を所定の走行経路に沿って移動させて、前記記憶装置にカメラの画像と自律移動体に対するコマンドを含む所定のセンサ情報を記憶する教示走行ステップと、
記憶した前記センサ情報と現時点のセンサ情報とを比較して、自律移動体が前記走行経路を移動するように自律移動体を制御する自律走行ステップとを有する、ことを特徴とするナビゲーション装置の制御方法。
【請求項５】
前記教示走行ステップにおいて、所定の走行経路上において現在の外部画像Ｖと直前の記憶画像Ｖ（ｉ）（ｉは正の整数）とのマッチング誤差Ｅ（ｉ）を演算し、そのマッチング誤差Ｅ（ｉ）が所定の閾値ｅ１を上回った場合に新しい画像Ｖ（ｉ＋１）と自律移動体に対する新しいコマンドＭ（ｉ＋１）を記憶する、ことを特徴とする請求項４に記載のナビゲーション装置の制御方法。
【請求項６】
前記自律走行ステップにおいて、所定の走行経路上において現在の外部画像Ｖと直前の記憶画像Ｖ（ｉ）（ｉは正の整数）とのマッチング誤差Ｅ（ｉ）と、外部画像Ｖと記憶画像Ｖ（ｉ）との視野の水平ずれｄｘ（ｉ）を算出し、その水平ずれを自律移動体のステアリングに反映して軌道修正する、ことを特徴とする請求項４に記載のナビゲーション装置の制御方法。
【請求項７】
前記マッチング誤差Ｅ（ｉ）は、記憶画像Ｖ（ｉ）と現時点の外部画像Ｖの特徴点を抽出し、抽出した特徴点同士の距離の平均値が最小になるように、記憶画像Ｖ（ｉ）又は外部画像Ｖを相対的に移動させた際の最小平均距離であり、
前記水平ずれｄｘ（ｉ）は、前記最小平均距離の水平成分である、ことを特徴とする請求項６に記載のナビゲーション装置の制御方法。
【請求項８】
前記マッチング誤差Ｅ（ｉ）は、記憶画像Ｖ（ｉ）と現時点の外部画像Ｖの特徴点に対し、記憶画像Ｖ（ｉ）中のある特徴点と、外部画像Ｖ中のすべての特徴点との距離を計算し、その中で最小のものを選択し、これをすべての特徴点に対して行い、それぞれの特徴点を１対１に対応させ、対応する２点間の距離が所定の閾値以下のものを小さい順に複数選択し、それらの距離の平均値を算出し、この平均値が最小になるように、記憶画像Ｖ（ｉ）又は外部画像Ｖを相対的に移動させた際の最小平均距離であり、
前記最小平均距離が所定の閾値以下のときに２つの画像のマッチングが取れたものと判断する、ことを特徴とする請求項６に記載のナビゲーション装置の制御方法。
【請求項９】
前記マッチング誤差Ｅ（ｉ）が所定の閾値を超えるときにマッチングが取れないマッチングミスと判断し、既にマッチングが取れた走行経路上の最も近い位置及び方位までＧＰＳ端末と方位センサにより自律移動体を案内する、ことを特徴とする請求項６に記載のナビゲーション装置の制御方法。
【請求項１】
移動可能な自律移動体に搭載されたナビゲーション装置であって、
自律移動体の移動方向の画像をリアルタイムに撮影するカメラと、
記憶装置と演算装置を有し前記カメラの画像に基づき自律移動体を制御する制御装置とを備え、
前記制御装置は、教示走行時において自律移動体が所定の走行経路を移動する際にカメラの画像と自律移動体に対するコマンドを含む所定のセンサ情報を記憶し、
自律走行時において記憶した前記センサ情報と現時点のセンサ情報とを比較して、自律移動体が前記走行経路を移動するように自律移動体を制御する、ことを特徴とするナビゲーション装置。
【請求項２】
前記制御装置は、記憶画像と現時点の外部画像の特徴点を抽出する特徴点抽出手段と、前記特徴点同士のマッチング誤差を計算する誤差計算手段とを有する、ことを特徴とする請求項１に記載のナビゲーション装置。
【請求項３】
自律移動体の現在位置をリアルタイムに検出するＧＰＳ端末と、
自律移動体の移動方向をリアルタイムに検出する方位センサと自律移動体の駆動輪の回転角を計測するセンサとを備え、
前記制御装置は、前記マッチング誤差が所定の閾値より小さい地点まで、自律移動体を案内する復帰手段を有する、ことを特徴とする請求項２に記載のナビゲーション装置。
【請求項４】
移動可能な自律移動体に搭載され、
自律移動体の移動方向の画像を一定間隔で撮影するカメラと、
記憶装置と演算装置を有し前記カメラの画像に基づき自律移動体を制御する制御装置とを備えたナビゲーション装置の制御方法であって、
自律移動体を所定の走行経路に沿って移動させて、前記記憶装置にカメラの画像と自律移動体に対するコマンドを含む所定のセンサ情報を記憶する教示走行ステップと、
記憶した前記センサ情報と現時点のセンサ情報とを比較して、自律移動体が前記走行経路を移動するように自律移動体を制御する自律走行ステップとを有する、ことを特徴とするナビゲーション装置の制御方法。
【請求項５】
前記教示走行ステップにおいて、所定の走行経路上において現在の外部画像Ｖと直前の記憶画像Ｖ（ｉ）（ｉは正の整数）とのマッチング誤差Ｅ（ｉ）を演算し、そのマッチング誤差Ｅ（ｉ）が所定の閾値ｅ１を上回った場合に新しい画像Ｖ（ｉ＋１）と自律移動体に対する新しいコマンドＭ（ｉ＋１）を記憶する、ことを特徴とする請求項４に記載のナビゲーション装置の制御方法。
【請求項６】
前記自律走行ステップにおいて、所定の走行経路上において現在の外部画像Ｖと直前の記憶画像Ｖ（ｉ）（ｉは正の整数）とのマッチング誤差Ｅ（ｉ）と、外部画像Ｖと記憶画像Ｖ（ｉ）との視野の水平ずれｄｘ（ｉ）を算出し、その水平ずれを自律移動体のステアリングに反映して軌道修正する、ことを特徴とする請求項４に記載のナビゲーション装置の制御方法。
【請求項７】
前記マッチング誤差Ｅ（ｉ）は、記憶画像Ｖ（ｉ）と現時点の外部画像Ｖの特徴点を抽出し、抽出した特徴点同士の距離の平均値が最小になるように、記憶画像Ｖ（ｉ）又は外部画像Ｖを相対的に移動させた際の最小平均距離であり、
前記水平ずれｄｘ（ｉ）は、前記最小平均距離の水平成分である、ことを特徴とする請求項６に記載のナビゲーション装置の制御方法。
【請求項８】
前記マッチング誤差Ｅ（ｉ）は、記憶画像Ｖ（ｉ）と現時点の外部画像Ｖの特徴点に対し、記憶画像Ｖ（ｉ）中のある特徴点と、外部画像Ｖ中のすべての特徴点との距離を計算し、その中で最小のものを選択し、これをすべての特徴点に対して行い、それぞれの特徴点を１対１に対応させ、対応する２点間の距離が所定の閾値以下のものを小さい順に複数選択し、それらの距離の平均値を算出し、この平均値が最小になるように、記憶画像Ｖ（ｉ）又は外部画像Ｖを相対的に移動させた際の最小平均距離であり、
前記最小平均距離が所定の閾値以下のときに２つの画像のマッチングが取れたものと判断する、ことを特徴とする請求項６に記載のナビゲーション装置の制御方法。
【請求項９】
前記マッチング誤差Ｅ（ｉ）が所定の閾値を超えるときにマッチングが取れないマッチングミスと判断し、既にマッチングが取れた走行経路上の最も近い位置及び方位までＧＰＳ端末と方位センサにより自律移動体を案内する、ことを特徴とする請求項６に記載のナビゲーション装置の制御方法。
【図１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【公開番号】特開２００８−１４６１９７（Ｐ２００８−１４６１９７Ａ）
【公開日】平成２０年６月２６日（２００８．６．２６）
【国際特許分類】
【出願番号】特願２００６−３３０３３８（Ｐ２００６−３３０３３８）
【出願日】平成１８年１２月７日（２００６．１２．７）
【出願人】（０００００００９９）株式会社ＩＨＩ (5,014)
【Ｆターム（参考）】
【公開日】平成２０年６月２６日（２００８．６．２６）
【国際特許分類】
【出願日】平成１８年１２月７日（２００６．１２．７）
【出願人】（０００００００９９）株式会社ＩＨＩ (5,014)
【Ｆターム（参考）】
[ Back to top ]