JP5800613B2

JP5800613B2 - 移動体の位置・姿勢推定システム

Info

Publication number: JP5800613B2
Application number: JP2011151757A
Authority: JP
Inventors: 高斉松本; 正木　良三; 良三正木; 修一槙
Original assignee: Hitachi Industrial Equipment Systems Co Ltd
Current assignee: Hitachi Industrial Equipment Systems Co Ltd
Priority date: 2011-07-08
Filing date: 2011-07-08
Publication date: 2015-10-28
Anticipated expiration: 2031-07-08
Also published as: JP2013020345A

Description

本発明は、自らの位置・姿勢を検知できる装置を搭載した移動体の移動経路における位置・姿勢を推定するシステムに関する。特に、本発明は、ユーザから指定された移動経路に沿って自律的に走行する自律移動ロボットの移動経路における位置・姿勢を推定するシステムに関する。さらに本発明は、例えば、荷役作業を行う搬送システム、物品の生産に用いられる生産システム、あるいは防犯・警備などのセキュリティシステムなどの各種サービスにおいて使用される自律移動ロボット又は移動体の位置・姿勢推定システムに関する。特に、本発明は、初期移動開始時の自律移動ロボットの位置・姿勢を推定するシステムに関する。

本発明は、自律移動ロボットを実施例として説明するものであるが、自律移動を行わないロボットにも適用できるものであり、さらには、カメラやＧＰＳを実装した自動車のカーナビ等の移動体への適用も容易なものであり、発明の技術的範囲としては含まれる。

従来から、予め施設されたレールや磁気テープ等規定の移動経路に沿って移動する移動ロボットは多く開発されていたが、近年になって、規定の移動経路は持たずに、コンピュータ内で設定された移動経路に沿って自律的に移動するロボットが開発されている。このような自律移動ロボットにおいて、自律的な移動を実現するためには、ロボットの現在位置と姿勢を認識する自己位置・姿勢推定機能が不可欠であり、例えば自律移動ロボットが周囲状態を検知し、そのデータを基に自己位置を推定しながら、かつ、同時に地図を生成する方法が提案されている。この方法はＳＬＡＭ（ＳｉｍｕｌｔａｎｅｏｕｓＬｏｃａｌｉｚａｔｉｏｎａｎｄＭａｐｐｉｎｇ）とよばれる技術で、ロボットが地図を生成しながら自己位置を推定していくことができるので、自律的に移動する特徴を有している。

このような自律移動ロボットに関して、例えば、特許文献１（特開２００５−３３２２０４号公報）には、ＧＰＳ（ＧｌｏｂａｌＰｏｓｉｔｉｏｎｉｎｇＳｙｓｔｅｍ）などの自己位置検知手段と、周囲の物体との距離と方向を検知する物体検知手段と、それらの検知データを基に移動する方向の環境地図を生成する機能を備えた移動制御装置が知られている。この特許文献１の課題は、ＧＰＳ衛星からの電波を受信していなくても目標経路に沿って移動体を正確に移動させ、目標経路上の予期しない障害物を回避するように制御するものである。そして、その解決手段は、予め設定された目標経路情報に従って自律移動する移動体に、その移動を制御する移動制御装置が設けられている。移動制御装置は、移動体の現在位置及び方位を検知する自己位置検知手段と、移動体とその周囲に存在する物体との距離を検知するＡＬＲ（ＡｒｅａＬａｓｅｒＲａｄｅｒ）と、自己位置検知手段及びＡＬＲのデータに基づいて移動体の進路を決定するとともに、当該進路に沿って移動体を移動させるように移動体を制御する制御手段とを備えている。制御手段は、移動体の移動に伴い物体の存在を考慮した移動体周囲の環境地図を移動方向に累積的に生成し、目標経路情報及び環境地図に基づいて物体に非干渉となる移動体の進路を決定する。

また、特許文献２（特開２００４−２７６１６８号公報）の移動ロボットは、移動センサと認識手段により、物体間の相対姿勢で表わされる地図情報とロボットの姿勢の同時推定を行うことで、新規地図情報を作成していく方法が示されている。この特許文献２の課題は、精度の高い移動ロボット用地図を作成することができる地図作成システムの提供である。その解決手段のステップでは、まず、移動ロボットにより新規地図情報を作成する。次に、既存地図情報を読み込んで、既存地図のもつ相対姿勢を１つずつ取り出す。そして、同じ相対姿勢が新規地図情報にもあるかを調べる。新規地図情報にも同じ相対姿勢があれば、新規地図情報と既存地図情報が共通に持つ相対姿勢を確率的に融合する。新規地図情報に同じ相対姿勢がなければ、その相対姿勢を新規地図情報に追加する。その後、新規地図情報の相対姿勢集合の中にループを構成するものがある場合には、そのずれを解消するループ解決処理を行う。

さらに、特許文献３（特開２００７−９４７４３号公報）においては、マップデータ生成部と位置推定部が自律移動型ロボット、あるいは、サーバ装置に配置されていることが示されている。この特許文献２の課題は、ユーザが任意に選択した位置情報を容易に教示することができ、かつ、この教示された位置情報に基いて移動先を指定することで、指定された移動先に自律移動する自律移動型ロボットとそのシステムを提供するものである。その解決手段としては、ユーザからの位置情報及び移動先情報の入力を受ける情報入力部と、情報入力部から入力された位置情報と位置推定部で推定した位置とを関連付けてテーブルに記憶する位置情報記憶部とを備え、情報入力部に対して移動先情報の入力があると、移動経路計画部が、入力された移動先情報と位置情報記憶部に記憶されているテーブルとを対応させ、ロボットの移動空間内の障害物情報であるマップデータを参照して、位置推定部で推定された位置からの移動経路を求め自律移動する。

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

しかし、特許文献１に示すＧＰＳを利用する方法では、衛星からの信号に基づいて自己位置を推定することから、信号が受信困難な屋内での環境下では利用できないため、自走可能なエリアが制限されることから、ＧＰＳ衛星からの電波を受信していなくても目標経路に沿って移動体を制御する技術が開示されている。この点に関し、特許文献２、３においても、想定される動作領域を自走しながら障害物までの距離を距離センサで測定することから、ＧＰＳ信号を受信できない屋内においても自己位置を推定することができる。

荷役作業を行う搬送システム、物品の生産に用いられる生産システム、あるいは防犯・警備などのセキュリティシステムにおいては、想定される動作領域内を作業用の自律移動ロボットを自律移動させる場合、動作領域内のマップデータを記憶しておき、自律移動ロボットに備えた距離センサなどによって得られた探索データとマップデータとをマッチングさせて、動作領域内において自律移動ロボットの位置を判定することによって、予め設定された移動経路に沿った自律移動ロボットを自律移動させている。このような、自律移動ロボットにおいては、自律移動の開始時に動作領域内における現在位置を認識しなければ予め設定された移動経路に沿った自律移動が不可能であるが、この際、想定される動作領域全体をマッチングの探索範囲とした場合、位置・姿勢推定までの時間がかかり、高速応答の走行制御を行うことができないという課題を有していた。本発明において、「マッチング」或いは「マッチング処理」との用語の意味は、移動体の検知手段により得られた移動体周辺の幾何形状データ（障害物の壁等を外延とする幾何形状データ）と、予め格納している地図データとをマッチングさせ、所定の閾値以上の整合度合いがある位置と姿勢を探索することを意味する。

そこで本発明は、上記課題に着目してなされたものであり、移動体のマッチング処理に要する時間を節約するために、新規な概念である「距離和データ」の比較照合により移動体の位置・姿勢の推定を行うものである。さらには、この距離和データの比較照合を、移動体のマッチング処理の前段階として実施して、マッチング処理に要する時間を大幅に削減しようとするものである。特に、自律移動ロボットにおいては、自律移動開始時に動作領域内における現在位置と姿勢を確定するための位置・姿勢の推定に係る時間を可及的に短縮して高速応答の走行制御を可能とする自律移動ロボットの初期位置・姿勢推定システムを提供することを目的とする。なお、本発明において「位置・姿勢の推定」との用語は、「位置及び姿勢の推定」或いは「位置又は姿勢の推定」の意味で用いている。

本発明の移動体の位置・姿勢推定システムは、移動体と、この移動体の移動機構制御部と、移動体周囲の障害物までの距離を角度ごとに検知する距離センサとを備え、さらに、前記移動体が走行する動作領域における障害物の配置形状の地図データを予め格納しており、
前記距離センサにより検知された障害物表面までの角度ごとの距離を距離データとして記録し、前記距離センサから前記障害物表面までの角度ごとの距離を総和して距離和データを算出し、
予め、多数の既知の位置・姿勢における障害物表面までの距離データから得られる距離和データ群を格納しておき、
前記算出した距離和データと前記予め格納してある距離和データ群の距離和データとを比較照合して該距離和データ群の中から近似した距離和データを検索して移動体の位置・姿勢を推定することを特徴とする。

さらに、本発明の移動体の位置・姿勢推定システムは、移動体と、この移動体の移動機構制御部と、移動体周囲の障害物までの距離を角度ごとに検知する距離センサとを備え、さらに、前記移動体が走行する動作領域における障害物の配置形状の地図データを予め格納しており、
前記距離センサにより検知された障害物表面までの角度ごとの距離を距離データとして記録し、該検知された障害物表面を外延とする幾何形状データを演算により得、該幾何形状データにおける前記距離センサから前記障害物表面までの角度ごとの距離を総和して距離和データを算出し、
予め、多数の既知の位置・姿勢における障害物表面までの距離データから得られる距離和データ群を格納しておき、
前記算出した距離和データと前記予め格納してある距離和データ群の距離和データとを比較照合して該距離和データ群の中から近似した距離和データを検索し、その検索結果に基づいて前記幾何形状データと前記地図データとのマッチング処理を行うことを特徴とする。

さらに、本発明の移動体の位置・姿勢推定システムは、前記距離センサの検知結果から得られた距離和データと前記予め記憶された距離和データ群のなかの距離和データとの比較照合する際に、移動体の過去の走行履歴において、該走行履歴内で移動体が取り得る位置・姿勢での距離和データ群との比較照合を行い、その照合によって近似した距離和データが存在する場合には、その位置・姿勢の近傍において移動体の位置・姿勢を推定するマッチング処理を行うことを特徴とする。

さらに、本発明の移動体の位置・姿勢推定システムは、前記距離センサの検知結果から得られた距離和データと前記予め記憶された距離和データ群のなかの距離和データとの比較照合する際に、移動体の経路グラフデータを基に該経路内で移動体が取り得る位置・姿勢での距離和データ群との比較照合を行い、その照合によって近似した距離和データが存在する場合には、その位置・姿勢の近傍において移動体の位置・姿勢を推定するマッチング処理を行うことを特徴とする。

さらに、本発明の移動体の位置・姿勢推定システムは、前記距離センサの検知結果から得られた距離和データと前記予め記憶された距離和データ群のなかの距離和データとの比較照合する際に、移動体の通路データを基に該通路内で移動体が取り得る位置・姿勢での距離和データ群との比較照合を行い、その照合によって近似した距離和データが存在する場合には、その位置・姿勢の近傍において移動体の位置・姿勢を推定するマッチング処理を行うことを特徴とする。

さらに、本発明の移動体の位置・姿勢推定システムは、前記距離センサの検知結果から得られた距離和データと前記予め記憶された距離和データ群のなかの距離和データとの比較照合する際に、移動体の動作領域の地図データ内で移動体が取り得る位置・姿勢での距離和データ群との比較照合を行い、その照合によって近似した距離和データが存在する場合には、その位置・姿勢の近傍において移動体の位置・姿勢を推定するマッチング処理を行うことを特徴とする。

さらに、本発明の移動体の位置・姿勢推定システムは、前記算出した距離和データと前記予め格納してある距離和データ群の距離和データとを比較照合して該距離和データ群の中から近似した距離和データを検索し、その検索結果に基づいて前記幾何形状データと前記地図データとのマッチング処理を行う際に、マッチング処理を行う探索条件を複数用意しておき、そのマッチング処理の度合いを段階的に広げていくことを特徴とする。

さらに、本発明の移動体の位置・姿勢推定システムは、前記第１のマッチング処理の探索条件が、移動体の過去の走行履歴内で移動体が取り得る位置・姿勢でのマッチング処理であり、第２のマッチング処理の探索条件が、移動体の経路内で移動体が取り得る位置・姿勢でのマッチング処理であり、最終のマッチング処理の探索条件が、移動体の通路内で移動体が取り得る位置・姿勢でのマッチング処理であることを特徴とする。

さらに、本発明の移動体の位置・姿勢推定システムは、前記距離センサの検知結果から得られた距離和データと前記予め記憶された距離和データ群のなかの距離和データとの比較照合し、又は幾何形状データと地図データをマッチング処理する際に、近似した複数の距離和データの位置・姿勢が推定された場合または地図データと一致度が高い複数の幾何形状データが推定された場合、ディスプレイ上に複数の移動体の位置・姿勢とそれに対応した複数の幾何形状データとを表示し、ユーザが表示された複数の位置・姿勢の中から一つの位置・姿勢を選択可能としたことを特徴とする。

本発明においては、新規な距離和データの比較照合を行うことによって、移動体の位置・姿勢の推定において、簡便な推定シシテムを提供することが可能なものである。また、移動体の位置・姿勢の推定において、幾何形状データと地図データのマッチング処理を行う場合であっても、事前に距離和データの比較照合を行うことによって、効率的なマッチング処理を行うことが可能となる。

本発明においては、特に、自律移動ロボットの自律移動時において、距離センサの検知結果から得られた幾何形状データの距離和データを算出し、これと地図データ上で自律移動ロボットが取りうる位置・姿勢での距離和地図データにおける距離和データ群とを比較照合して、近似するその距離和データを検索し、その結果に応じて自律移動ロボットの位置・姿勢を推定する探索範囲を限定してマッチング処理を行うので、幾何形状データとのマッチングにより動作領域全体の地図データを探索する場合に比べ、演算処理を軽減でき、位置姿勢の推定に伴う時間を可及的に短縮して高速応答の走行制御が可能となる。

また、自律移動ロボットの初期位置・姿勢を推定するマッチング処理の際には、複数の探索条件を備えており、距離和データ群からの検索処理の結果に基づいて探索範囲を段階的に徐々に広げることにより、さらに演算処理の負担を軽減でき、位置姿勢の推定に伴う時間を可及的に短縮して高速応答の走行制御が可能となる。

特に、自律移動ロボットの運用においては、工場などで始業とともに自律移動ロボットの起動を開始して走らせるまでの時間短縮や、自律移動ロボットが脱線した状態から経路に復帰する際の時間短縮につながる。

しかしながら本発明は、勿論、作業開始時の自律移動ロボット１の初期動作としての位置・姿勢の推定時に限らず、走行経路の脱線状態からの復旧時、或いは自身の位置・姿勢を見失った際等の位置・姿勢の特定時に適用できることは勿論であり、それ以外にも、通常の位置・姿勢推定のマッチング処理としても適用可能なものである。従って、本発明において「自律移動ロボット（移動体）の初期位置・姿勢の推定」との用語は、「初期位置・姿勢」との意味を厳格に解釈されるべきものではない。

また、距離和データの検索の結果、或いはマッチング処理の結果、近似した候補或いは一定の閾値以上の一致度の高い候補が複数存在する場合には、ディスプレイに自律移動ロボットと距離センサの検知結果から得られた幾何形状データを複数表示させることで、ユーザが手入力などにより、自律移動ロボットの位置・姿勢を選択可能にすることにより、自律移動ロボットの初期位置・姿勢の特定を容易にすることができる。

本発明の実施例を示す自律移動ロボットの制御構成の概略を示すブロック図である。本発明の実施例のハードウェア及びソフトウェア構成をブロック図である。本発明の実施例の距離センサによる自律移動ロボットの位置・姿勢推定方法を示す動作状態図である。本発明の実施例において、自律移動ロボットの位置・姿勢推定処理における幾何形状データと地図データのマッチング方法を示す動作状態図である。本発明の実施例の自律移動ロボットの位置・姿勢推定処理を示すフローチャートである。本発明の実施例の自律移動ロボットの位置・姿勢推定処理の一つの手順を示す詳細なフローチャートである。本発明の実施例の自律移動ロボットの位置・姿勢推定処理の別の手順を示す詳細なフローチャートである。本発明の実施例において、地図データと共に複数の自律移動ロボットの位置・姿勢の候補を表示させた状態を示す平面図である。

以下、移動体を自律移動ロボットに限定して、添付図面を参照して本発明の実施例を説明する。図１は、自律移動ロボット１の制御構成の概略を示すブロック図である。自律移動ロボット１は、この自律移動ロボット１の位置・姿勢を判定して走行指令を出力するコントローラ部２、自律移動ロボット１と周囲にある障害物Ｂの該壁面との距離を検知する距離センサ３及び車輪４，４を備えている。また、コントローラ部２は、距離センサ３を制御する距離センサ制御部５を備えており、距離センサ３からの検知結果を受けて自律移動ロボット１の位置・姿勢を推定する推定部を２つ備えている。この２つの位置・姿勢推定部は通常位置・姿勢推定部６と初期位置・姿勢推定部７である。図に示すように障害物は自律移動ロボットの動作領域内に複数配置されており、本発明においては、地図データ上におけるそれら障害物を配置させた際の障害物の外周輪郭形状を障害物の配置形状と称して説明する。通常位置・姿勢推定部６は、従前技術による位置・姿勢の推定を行うものであり、本発明における主要な課題ではないので、詳細に説明することはしない。本発明においては、初期位置・姿勢推定部７を設けたことを特徴としており、この初期位置・姿勢推定部７は、位置姿勢候補表示・設定部８を備えている。本実施例の自律移動ロボット１は、通常位置・姿勢推定部６又は初期位置・姿勢推定部７により推定された位置・姿勢に基づいて自らが走行する領域の経路を設定する経路計画部９を設け、この経路計画部９で計画された経路に沿うように車輪４を駆動して自律移動ロボット１を自律移動させる移動機構制御部１０を設けている。

本実施例においては、発明の理解の容易さのために、通常位置・姿勢推定部６と初期位置・姿勢推定部７を別に構成しているが、当然のこととして、通常位置・姿勢推定部６と初期位置・姿勢推定部７を一つの位置・姿勢推定部として構成することは当業者にとって何らの考案力も要しない。また、通常の位置・姿勢の推定において本発明を適用することも困難性はない。

本実施例の自律移動ロボット１が、初期位置・姿勢推定部７により初期位置・姿勢を推定する際に使用するデータはデータ記憶部２５に記憶されている。このデータ記憶部の構成は、自律移動ロボット１自身の形状データを記憶するロボット形状データ記憶部１１、予め自律移動ロボット１の周辺にある障害物の壁面との距離の角度ごとの総和を演算して距離和地図データを記憶している距離和地図データ記憶部１２、自律移動ロボット１が走行する領域の地図を記憶した地図データ記憶部１３、自律移動ロボット１の走行履歴データを記憶する走行履歴記憶部１４、自律移動ロボット１が走行した経路（図３のＲ）の経路グラフデータを記憶する経路グラフデータ記憶部１５から構成されている。これらの記憶部１１乃至１５は、一つの記憶部から構成しても良いし、複数の記憶部から構成しても良い。

図２は、図１に示す自律移動ロボット１の機能構成を実現するためのハードウェア及びソフトウェア構成を示している。自律移動ロボット１の位置・姿勢推定コントローラ部２０には、プロセッサ２１とメモリ２２及び記憶装置２３を備えている。記憶装置２３は、プログラム記憶部２４とデータ記憶部２５から成っている。このプログラム記憶部２４には、オペレーティングシステム（ＯＳ）２４ａ、コントローラ初期化プログラム２４ｂ、距離センサ制御プログラム２４ｃ、通常位置・姿勢推定プログラム２４ｄ、経路計画プログラム２４ｅ、移動機構制御プログラム２４ｆ、初期位置・姿勢推定プログラム２４ｇ、位置・姿勢候補表示・設定プログラム２４ｈを備えている。一方、データ記憶部２５には、前述したように、ロボット形状データ２５ａ、距離和地図データ２５ｂ、地図データ２５ｃ、経路グラフデータ２５ｄ、走行履歴データ２５ｃを格納している。

本実施例の自律移動ロボット１は、以上のプログラム及びデータ構成を備えることにより、所期の目的が達成できるものである。自律移動ロボット１の位置・姿勢推定コントローラ部２０は、各プログラム２３ａ〜２３ｆを記憶装置２３から読み出し、プロセッサ２１によって各プログラム２３ａ〜２３ｈを実行し、距離センサ３からの情報に基づいて自らの位置と姿勢を推定し、それに基づいて前記車輪４の駆動を制御するモータ、エンコーダ、モータドライバなどの移動機構２６を制御する。その際には、各種のデータがメモリ２２に一時的に記憶され使用される。ここで、移動機構２６としては、前述の通り、車輪による移動機構を想定しているが、同様に動作領域内を移動する効果が得られるのであれば移動機構の方式は異なっていてもよい。例えば、無限軌道を備える車両、歩行可能な脚を備えるロボット、船舶（動作領域としては水面或いは水中を含むことになる）、航空機、飛行船（動作領域としては空中を含むことになる）など他の移動機構であってもよい。

また、本実施例においては、ロボットが自律移動することを想定しているが、人が搭乗してロボットを操縦するようになっていてもよいし、また、搭乗せずに遠隔からの通信によって人が操縦するようになっていてもよい。また、プロセッサ２１や各プログラム２３ａ〜２３ｈによる処理について、これと同様の効果が得られるのであれば、実装は異なっていてもよい。例えば、ＦＰＧＡ（ＦｉｅｌｄＰｒｏｇｒａｍｍａｂｌｅＧｒｉｄＡｒｒａｙ）やＣＰＬＤ（ＣｏｍｐｌｅｘＰｒｏｇｒａｍｍａｂｌｅＬｏｇｉｃＤｅｖｉｃｅ）などのプログラマブルなハードウェアで以上の処理を実現してもよい。また、プログラムやデータは、ＣＤ−ＲＯＭ等の記憶媒体から移してもよいし、ネットワーク経由で他の装置からダウンロードしてもよい。また、２７は例えば地図データ上での自律移動ロボット１の位置などを表示するディスプレイであり、２８は目的地の設定等の各種入力を行うキーボードやマウスなどの入力機器である。また、プロセッサや記憶装置、移動機構など、ロボットを構成する各デバイスは、ここでは有線の通信線により互いに通信することを想定しているが無線であってもよく、また、通信が可能であれば、位置・姿勢推定コントローラ２０、ディスプレイ２７、入力装置２８などの各デバイスが物理的に遠隔にあってもよい。また、以上のハードウェアやソフトウェアは、実施形態に応じて、取捨選択してもよい。

まず、距離センサ３による自律移動ロボット１から障害物Ｂの壁面までの距離の検知方法について図３を参照して説明する。距離センサ３は、レーザを物体に照射してから、照射したレーザが物体によって反射してセンサに返ってくるまでの時間を検知することで、センサから環境中の物体（障害物Ｂの壁面）までの距離の検知を行うレーザ照射部を備えており、このレーザ照射部を一定の回転角毎に回転（以下、スキャン）させながら検知することで、一定の角度範囲内にある物体までの角度ごとの距離の検知が可能である。このスキャン動作によってレーザがスキャン可能な平面（以下、スキャン面）上にある物体のまでの距離と方向が得られるようになっている。ここでは、距離センサ３がスキャンする角度範囲を１８０度とし、この角度範囲において０．５度毎にレーザを照射し、物体（障害物Ｂの壁面）までの距離を検知する。この場合、スキャンする角度範囲やレーザを照射する角度の刻み幅、距離の最大検知範囲などは異なっていてもよい。また、物体までの距離と角度の検知が可能なセンサであればセンサの方式は異なっていてもよい。例えばステレオカメラでもよいし、赤外線を面状に物体に照射することで画素毎の物体までの距離検知が可能なデプスカメラなどであってもよい。

ここで、図３では地図データ上の壁３０、障害物（Ｂ１〜Ｂ７）の配置及び自律移動ロボット１の位置は、絶対座標系（ｘ−ｙ静止座標系：図３のｂ）で表わされている。そして、その自律移動ロボット１の位置（ｘ、ｙ）における自律移動ロボット１の姿勢（角度θ）を合わせて自律移動ロボット１の位置・姿勢と呼ぶ。図３は、自律移動ロボット１が動作領域の中を移動する際の、位置・姿勢の推定手法の一例を動作領域の上部から見た動作状態図である。

図３の動作領域は壁面３０に囲まれた動作領域空間であり、当該空間内に配置された障害物Ｂ１，Ｂ２，Ｂ３・・・を避けながら予め設定した経路Ｒに沿って走行可能領域（つまり、通路又は作業位置）を移動する。ここで、障害物Ｂ１，Ｂ２，Ｂ３・・・は作業台、収納庫、あるいは、壁などを意味し、粗い実線の斜線部にて表示する。この実施例で用いる距離センサ３はレーザ距離センサとよばれるもので、自律移動ロボットの前方の所定高さに取り付けられている。この距離センサ３により、自律移動ロボット１の正面を中心に±９０°、つまり、１８０°の範囲で自律移動ロボット１から周囲の障害物Ｂ１，Ｂ２，Ｂ３・・・までの距離を検知できる。一例として、図３の動作領域空間の所定位置・所定姿勢（Ｐ１）にある自律移動ロボット１からの検知結果を密な実線の斜線部ａにて表示している。図３の場合には、図示の位置・姿勢での自律移動ロボット１から見て各障害物Ｂ１，Ｂ２，Ｂ３・・・の壁面から反射してきた反射光とその角度を検知し、距離データ（θごとのＬ）を得る。この距離データ（θごとのＬ）を基にして、自律移動ロボット１の位置（Ｐ１）を原点として、照射光を反射した各障害物Ｂ１，Ｂ２，Ｂ３・・・の壁面の位置を相対座標（Ｘ−Ｙ）として演算し、そのデータから斜線部分ａで示すような幾何形状データを取得する。ここで得られたデータは、上述の通り障害物Ｂ１，Ｂ２，Ｂ３に反射した線分の距離と距離センサ３からの障害物Ｂ１，Ｂ２，Ｂ３の反射壁面に対する角度を示し得るデータの集合であり、本発明においては、このデータ群を幾何形状データと称呼する。また、前述した距離データ（θごとのＬ）から、検知可能な各障害物Ｂ１，Ｂ２，Ｂ３の反射壁面に対する全ての角度における距離の和を算出する。以下、これを距離和データと呼ぶ。

図３においてａは、幾何形状データを目視可能に表現したものである。この幾何形状データは、図３の密な実線の斜線部ａに示したような図形データとして保有することも可能であるが、自律移動ロボット１からの＋９０度から−９０度までの各角度（θ）における距離データ（Ｌ）として保有しても良い。この幾何形状データを、自律移動ロボット１の走行可能な動作領域の全ての位置と全ての姿勢において地図データに対して展開させた距離和データ群（０〜ｎ）を距離和地図データ群Ｔ（ｔ０〜ｔｎ）として距離和地図データ記憶部１２に予め記憶しておくこととする。予め全ての位置と全ての姿勢における距離和地図データを記憶しておくことはせずに、必要な際に、必要な距離和地図データを計算して位置・姿勢の推定ステップで使用することは可能である。

以上のように構成される自律移動ロボット１は、例えば、初期状態において、距離センサ３からの信号に基づいて各角度における距離データＬを算出し、距離和地図データｔを生成するとともに、その距離和地図データｔを地図データ内の距離和地図データ群Ｔ（ｔ０〜ｔｎ）の距離和データと比較照合する。この照合の結果、近似する距離和データが検索された場合には、幾何形状データをマッチングにより、予め記憶してある距離和地図データＴ（ｔ０〜ｔｎ）と比較して、自律移動ロボット１の位置・姿勢を確認しながらコントローラ部２からの走行指令により初期位置・姿勢の推定を行い、自律的な走行を開始する。この自律移動ロボット１の初期位置・姿勢の推定と自律移動時における位置・姿勢の推定処理を図４の動作状態図及び図５のフローチャートを参照して説明する。

図５において、自律移動プログラムを開始すると（ステップＳ１）、まず、コントローラ初期化プログラム２４ｂにより前回設定した目的地を初期化する（ステップＳ２）。ただし、この場合でも過去の走行履歴の記憶は一定期間しておく。次に、目的地を設定するか否かを適当な入力機器２８により入力すると、それを判定し（ステップＳ３）、目的地を設定しない場合（ステップＳ３にてＹＥＳ）、プログラムを終了する（ステップＳ４）。一方、目的地を設定する場合（ステップＳ３にてＮＯ）、ディスプレイ２７を見ながらキーボードやマウスなどの入力機器２８によって目的地を設定する（ステップＳ５）。目的地が設定されると、距離和地図データ記憶部１２から予め演算され格納されている距離和地図データ２５ｂを取得し（ステップＳ６）、さらに地図データ記憶部１３から動作領域の地図データ２５ｃを取得し（ステップＳ７）、経路グラフデータ記憶部１５から経路グラフデータ２５ｄを取得する（ステップＳ８）。

次に、自律移動ロボット１の距離センサ３を作動させ、自律移動ロボット１の前方１８０°の範囲内においてレーザ光を走査して照射し、その反射光の受信により、障害物Ｂ１，Ｂ２，Ｂ３・・・の照射光反射壁面までの距離、及びその障害物Ｂ１，Ｂ２，Ｂ３の照射光反射壁面の自距離センサ３に対する角度を検知し、そのデータを距離データとして取得する（ステップＳ９）。次に、この距離センサ３による距離データの取得が１回目の位置・姿勢の推定ステップであるかを判定する（ステップＳ１０）。つまり、１回目の位置・姿勢の推定ステップとは、初期位置・姿勢の推定ステップを意味し、例えば、１日の作業の開始時であってこれから作業開始で初めて電源を入れた後の状態、或いは、障害等で一旦電源等を落とした復旧動作の完了後の再立ち上げ時の状態等である。

このＳステップ１０で一回目の位置姿勢の推定（初期位置・姿勢の推定ステップ）である場合（ステップＳ１０にてＹＥＳ）、通常の自律移動時のように直前の自律移動ロボット１の位置・姿勢情報を持っていないので、距離センサ３で取得した自律移動ロボット１からの＋９０度から−９０度までの各角度における距離データにより生成した距離和データと、予め蓄積されている距離和地図データ群Ｔ内の距離和データとを比較照合し、近似した距離和データを検索し、その検索結果に基づいて自律移動ロボット１の初期位置・姿勢推定処理を実行する（ステップＳ１１）。ここまでにおいても、充分に効果のある自律移動ロボット１の初期位置・姿勢推定処理である。

このステップＳ１１の自律移動ロボット１の初期位置・姿勢推定処理でのマッチング過程において、距離センサ３で取得した距離和データと、予め蓄積されている距離和地図データ群内の距離和データとが近似している、或いは距離センサ３で取得した距離和データから演算した幾何形状データと地図データとが一定の閾値以上でマッチングすると判定された候補が複数存在する場合には、その複数の候補をディスプレイ２７上に表示させ、その複数の候補の中から一つの候補を選択して設定する（ステップＳ１２）ことも可能である。また、ステップＳ１０で一回目の位置姿勢の推定（初期位置・姿勢の推定ステップ）ではないと判定されると（ステップＳ１０でＮＯ）、自律移動ロボット１が自律移動プログラムによる自律移動中であると判定し、自律移動ロボット１の自律移動を継続して通常の位置・姿勢推定の処理をさせる（ステップＳ１４）。

ここで、図４を用いて、自律移動ロボット１の位置・姿勢推定の処理について説明する。今、自律移動ロボット１が、距離センサ３により自律移動ロボット１からの＋９０度から−９０度までの各角度（θ）における距離（Ｌ）を距離データ（Ｌ，θ）として取得したとする。この距離データから自律移動ロボット１自身を原点とした幾何形状データ（障害物の反射壁面を外延とした幾何形状を現すデータ）を得、この幾何形状データと地図データとの重なり具合を評価していき、最も重なり具合が一番大きくなるときの位置・姿勢を求める。この処理をマッチング処理と称する。図４の場合には取得した距離データから演算された幾何形状データを、地図データの上でマッチング処理をすると、位置・姿勢をＰ１としたとき（マッチング状態ｂ１）の方が位置・姿勢をＰ２としたとき（マッチング状態ｂ２）の方よりも重なり具合が大きくなっている。ここで、地図データは、動作領域全体に壁３０や障害物Ｂ１，Ｂ２，Ｂ３・・・、その障害物Ｂ１，Ｂ２，Ｂ３間の通路等から成り、幾何形状データは障害物の照射光の反射壁面が外延されるデータである。図４の引き出し線で示したマッピング図において、一つ飛びの黒色のドットの部分が幾何形状データｂと地図データの重なりを示している。そこで、自律移動ロボット１の位置・姿勢をＰ１と推定して以降の処理を行う。ただし、この通常の位置・姿勢推定の処理については本発明における主要な解決課題ではないので、これ以上の詳細な説明は行わない。勿論、通常の位置・姿勢推定の処理においても、本発明の距離和データの比較照合の技術は適用できるものであるが、ここでは詳細な説明しない。

ステップＳ１２において、自律移動ロボット１の位置・姿勢が推定されると次の目的地に向かった位置・姿勢へと移動するべく駆動制御される。自律移動ロボット１の自律移動時には、経路計画プログラム２４ｅにより、自律移動ロボット１を予め設定された経路に沿って目的地に到着するまで自律的に走行させるが、その際には推定した現在位置・姿勢から次の目的位置までの局所的経路計画を立てる（ステップＳ１６）。この局所的経路計画に沿って、自律移動ロボット１の車輪４の走行速度、及び、操舵角を移動機構制御部１０によって制御する（ステップＳ１５）。この時、自律移動ロボット１は目的地に到達したか否かの判断を行い（ステップＳ１３）、目的地に近づくように駆動制御される。つまり、走行しながら距離センサ３によって前方１８０°の範囲内の障害物Ｂ１，Ｂ２，Ｂ３・・・までの距離及びその障害物Ｂ１，Ｂ２，Ｂ３に対する角度を測定しながら走行し、その距離センサ３で得られた全角度における距離データから幾何形状データを生成し、これを地図データとマッチングさせて自律移動ロボット１の位置・姿勢を推定しながら走行する（ステップＳ９―ステップＳ１４）。すなわち、自律移動ロボット１は、地図データ上の自らの位置・姿勢を確認しながら自律移動し、目的地に到着することにより、自律移動ロボット１を停止させる（ステップＳ３、ステップＳ４）。

次に、図５のフローチャートにおけるステップＳ１１の一回目の位置・姿勢推定処理（初期位置・姿勢の推定ステップ）について図６のフローチャートを参照して一つの手法を詳細に説明する。一回目の位置・姿勢推定時とは、すなわち、１日の始めの自律移動ロボット１の起動時などの自律移動ロボット１の自律移動開始時の自律移動ロボット１の位置・姿勢を推定するときである。

既に説明したように、自律移動ロボット１を自律移動プログラムによって自律移動させるためには、地図上での自律移動ロボット１の位置と姿勢が分からなければ自律移動ロボット１の自律移動プログラムを実行することができない。継続した自律移動プログラムの実行時には、今までの走行経路の近傍の位置・姿勢においてマッチング処理を行うことにより、比較的容易に位置・姿勢の推定は可能である。これに対して、作業開始時の自律移動ロボット１の１回目の位置・姿勢の推定時、走行経路の脱線状態からの復旧時、或いは自身の位置・姿勢を見失った際等の位置・姿勢の特定時には、参考となる直前の走行経路のデータはなく、マッチングさせる際には、マッチング対象である地図データに基づく幾何形状データは動作領域全体に亘り膨大であるために、自律移動ロボット１の位置・姿勢の推定が容易ではない。

そこで考え出された手法が本発明であり、新規な考え方に基づいた距離和データの比較照合により、予め得て格納してある地図データ上の距離和データ群の中から近似した距離和データを検索し、それにより自律移動ロボット１の初期位置・姿勢の推定を行おうとするものである。また、マッチング処理を用いる際にも、距離和データの近似した候補を比較照合して検索し、マッチング処理の探索範囲を順次広げていく手法である。つまり、自律移動ロボット１に搭載した距離センサ３で得られた幾何形状データから算出した距離和データを用いてマッチング処理の範囲を限定し、その限定された範囲内で地図データ上の自律移動ロボット１の位置・姿勢においてマッチング処理を行い、自律移動ロボット１の位置姿勢の推定を効率的に行うものである。つまり、距離センサ３で得られた幾何形状データを、地図上の全ての位置と姿勢における地図データと比較してマッチングさせるのでは処理に時間を要することになる。そこで、幾何形状データと地図データのマッチング処理において、マッチング処理の探索範囲の条件を複数設定しておき、距離和データの探索結果に従って順次マッチング処理の探索条件を広げる手法を採用した。以下、その手順の一つについて説明する。

図５の処理フローの初期位置姿勢推定（ステップＳ１１）においては、まず、動作領域の地図データ、経路グラフデータ、ロボット形状データより、通路データを生成する（ステップＳ１００）。次に、距離センサ３によって得られた幾何形状データの距離データから距離和データを算出する。この時、図３に示すように、距離センサ３によって前方１８０°の範囲内で距離センサ３からレーザ光を照射し、障害物Ｂ１，Ｂ２，Ｂ３・・の壁面で反射した反射光を受光することによって障害物Ｂ１，Ｂ２，Ｂ３・・の壁面までの距離及びその障害物Ｂ１，Ｂ２，Ｂ３に対する角度を測定して、距離センサ３で測定した全ての障害物Ｂ１，Ｂ２，Ｂ３・・の壁面までの全角度における距離の総和を演算する（ステップＳ１０１）。

次に、自律移動ロボット１の位置・姿勢のマッチングが取れる可能性の高さからいけば、マッチング処理の順番は、走行履歴でのマッチング処理であり、次いで、経路グラフデータの経路上でのマッチング処理、さらに通路データの通路内でのマッチング処理、最後に地図上全てでのマッチング処理である。

そこで、過去の走行履歴に含まれる位置・姿勢を基に距離和データの検索範囲を設定する（Ｓ１０７）。そこで、走行履歴に含まれる位置・姿勢を基にした距離和データ検索範囲において、距離和地図データ群の距離和データの中から近似した距離和データを検索する（Ｓ１２０）。この検索結果として近似した距離和データの位置・姿勢の候補が検索された場合には、その結果に応じて、幾何形状データと地図データのマッチング処理の探索範囲を設定する（Ｓ１２１）。そして、その位置・姿勢の候補の近傍でのマッチング処理を行い、自律移動ロボット１の位置・姿勢の推定を行う（Ｓ１０８）。すなわち、距離和地図データの中から過去の走行履歴に含まれる距離和データと、距離センサ３の測定結果から得られた距離和データとの一致度を比較照合し、その結果近似した距離和データが検索されれば、検索結果に基づいて幾何形状データと地図データとのマッチング処理を実施する。このマッチング処理において、予め設定した閾値以上の一致度が大きい解の候補があるかを判定する（ステップＳ１０９）。もし、閾値以上に一致する解の候補がある場合（ステップＳ１０９にてＹ）、一致度が所定の閾値より小さく相違が大きい解の候補を削除する（ステップＳ１０６）。これにより、自律移動ロボット１の初期位置での位置・姿勢の走行指令を出力するコントローラ部２に、自律移動ロボット１の初期位置・姿勢を入力する。

ステップＳ１０９にて閾値以上の解の候補がない場合（ステップＳ１０９にてＮ）、マッチング処理の探索条件の変更を行う。つまり、閾値以上の解の候補がない場合は、さらにマッチング処理の探索範囲を広げ、経路グラフデータに基づいて経路上で自律移動ロボット１が取り得る位置・姿勢を基に画像データの探索範囲を設定する（ステップＳ１１０）。次いで、経路上で自律移動ロボット１が取り得る位置・姿勢を基にした距離和データ検索範囲において、距離和地図データ群の距離和データの中から近似した距離和データを検索する（Ｓ１２２）。この検索結果として近似した距離和データの位置・姿勢の候補が検索された場合には、その結果に応じて、幾何形状データと地図データのマッチング処理の探索範囲を設定する（Ｓ１２３）。以後、同様な手順でのマッチング処理による位置・姿勢の推定（ステップＳ１１１）と、閾値以上の一致度が大きい解の候補があるか否かを判定する（ステップＳ１１２）。

ここでも閾値以上の一致度の高い解の候補がない場合（ステップＳ１１２にてＮ）には、さらに探索範囲を広げ、通路データに基づいて通路内で自律移動ロボット１が取り得る位置・姿勢を基に画像データの探索範囲を設定する（ステップＳ１１３）。次いで、通路内で自律移動ロボット１が取り得る位置・姿勢を基にした距離和データ検索範囲において、距離和地図データ群の距離和データの中から近似した距離和データを検索する（Ｓ１２４）。この検索結果として近似した距離和データの位置・姿勢の候補が検索された場合には、その結果に応じて、幾何形状データと地図データのマッチング処理の探索範囲を設定する（Ｓ１２５）。以後、同様な手順でのマッチング処理による位置・姿勢の推定（ステップＳ１１４）と、閾値以上の一致度が大きい解の候補があるか否かを判定する（ステップＳ１１５）。

一方、ステップＳ１０９にても、ステップＳ１１２にても、ステップＳ１１５にても閾値以上の一致度が大きい解の候補が無い場合（ステップＳ１１５にてＮ）には、初期位置推定失敗の警告を出力して（ステップＳ１１６）、図示していないが警告処理のステップに移る。なお、以上の手順において、Ｓ１０７―Ｓ１２０―Ｓ１２１―Ｓ１０８―Ｓ１０９で示した走行履歴データを探索範囲とした探索手順、Ｓ１１０―Ｓ１２２―Ｓ１２３―Ｓ１１１―Ｓ１１２で示した経路グラフデータを探索範囲とした探索手順、Ｓ１１３―Ｓ１２４―Ｓ１２５―Ｓ１１４―Ｓ１１５で示した通路データを探索範囲とした探索手順の一連の処理は、夫々距離和データを基にしてマッチング処理による位置・姿勢の推定を行っているものであるが、常に３つの処理操作が連続して行われるべきであるとする実施例ではなく、いずれか一つの探索範囲内での処理においても本発明は成立するものである。また、４つ目の探索範囲として地図データ全体上での距離和データの比較照合を行い、近似した距離和データを検索し、その近辺でのマッチング処理による位置・姿勢の推定を行ってもよい。勿論、この地図データ全体上での距離和データの比較照合後のマッチング処理のみを行う手順も十分に成立するものである。複数の探索範囲を順次連続して行う場合には、最後の探索範囲を「最終のマッチング処理の探索条件」と称する。

次に、幾何形状データと地図データのマッチング処理において、マッチング処理の探索範囲の条件を複数設定しておき、距離和データの探索結果に従って順次マッチング処理の探索条件を広げる別の手法を図７を用いて説明する。

図７の手順においても、まず、動作領域の地図データ、経路グラフデータ、ロボット形状データより、通路データを生成する（ステップＳ１００）。次に、距離センサ３によって得られた幾何形状データの距離データから距離和データを算出する。この時、図３に示すように、距離センサ３によって前方１８０°の範囲内で距離センサ３からレーザ光を照射し、障害物Ｂ１，Ｂ２，Ｂ３・・の壁面で反射した反射光を受光することによって障害物Ｂ１，Ｂ２，Ｂ３・・の壁面までの距離及びその障害物Ｂ１，Ｂ２，Ｂ３に対する角度を測定して、距離センサ３で測定した全ての障害物Ｂ１，Ｂ２，Ｂ３・・までの全角度における距離の総和を演算する（ステップＳ１０１）。

続いて、ステップＳ１０１で得られた距離和データと距離和地図データ記憶部１２に予め蓄積されている通路内の距離和地図データ群内の距離和データに一致する距離和データが存在するか否かを検索する（ステップＳ１０２）。なお、ここでは、走行履歴に含まれる位置・姿勢での距離和データの検索や、経路グラフ上での位置・姿勢での検索より広い範囲の検索範囲において行うのが良い。距離和地図データ記憶部１２には、動作領域内での地図上のあらゆる位置・姿勢（ｘ，ｙ，θ）での自律移動ロボットの幾何形状データと距離和データが記憶されている。従って、より一般化するには、可能であるならば、地図データ状における全ての位置・姿勢での距離和データを検索が望ましい。この距離和データの検索処理の結果により自律移動ロボット１の位置・姿勢候補を基に、複数の探索範囲からの１つの探索範囲を設定する。

ここで、距離和地図データは、仮定された地図上の位置・姿勢での距離センサからの幾何形状データに加えて距離の総和を記憶したものである。そして、ステップＳ１０２で、通路内において記憶された距離和地図データ群の中からステップＳ１０１で得られた距離和データと近似する距離和データを抽出し、この抽出された距離和データを基にして、地図データと幾何形状データのマッチング処理をすれば自律移動ロボット１の初期位置と姿勢を効率的に推定することができる。

まず、過去の走行履歴に含まれる位置・姿勢を基に探索範囲を設定し（ステップＳ１０７）、抽出された距離和データの中から過去の走行履歴に含まれる距離和データ基づいて、幾何形状データと地図データとのマッチング処理を実施する（ステップＳ１０８）。ここで、予め設定した閾値以上の一致度が大きい解の候補があるかを判定する（ステップＳ１０９）。閾値以上の一致度が大きい解の候補がない場合（ステップＳ１０９にてＮＯ）は、マッチング処理の探索範囲を広げ、経路グラフデータに基づいて経路上で自律移動ロボット１が取り得る位置・姿勢を基に画像データの探索範囲を設定する（ステップＳ１１０）。以後、同様な手順でのマッチング処理による位置・姿勢の推定（ステップＳ１１１）と、閾値以上の一致度が大きい解の候補があるかを判定する（ステップＳ１１２）。ここでも閾値以上の解の候補がない場合（ステップＳ１１２にてＮＯ）には、さらに探索範囲を広げ、通路データに基づいて通路内で自律移動ロボット１が取り得る位置・姿勢を基に画像データの探索範囲を設定（ステップＳ１１３）し、以後、同様な手順でのマッチング処理による位置・姿勢の推定（ステップＳ１１４）と、閾値以上の一致度が大きい解の候補があるかを判定する（ステップＳ１１５）。そして、ステップＳ１０９、ステップＳ１１２、ステップＳ１１５にて閾値以上の解の候補がある場合（夫々のステップにてＹＥＳ）には、ステップＳ１０６に戻り、一致度が閾値より小さく相違が大きい解の候補を削除し、自律移動ロボット１の位置・姿勢をコントローラ部２に入力する。一方、ステップＳ１０９にても、ステップＳ１１２にても、ステップＳ１１５にても閾値以上の一致度が大きい解の候補が無い場合（ステップＳ１１５にてＮＯ）には、初期位置推定失敗の警告を出力して（ステップＳ１１６）、図示していないが警告処理のステップに移る。

次に、閾値以上の一致度が大きい位置・姿勢の候補が複数存在する場合について説明する。例えば、図８に示すように、棚や収納庫などの障害物Ｂ１，Ｂ２，Ｂ３・・・が規則的に連続しており、距離センサの測定範囲が狭い場合においては、距離センサ３の測定結果から得られた幾何形状データは自律移動ロボット１の位置Ａ１と位置Ａ２とでは同じであるから、これらの位置Ａ１，Ａ２での距離和データ及び幾何形状データは同じであるため、距離和データを使って距離和データの一致する距離和地図データを検索したとしても、幾何形状データと地図データのマッチングを行って探索したとしても、自律移動ロボット１の位置・姿勢を特定する探索範囲を特定することはできない。図８に示す場合では、Ａ１とＡ２の２つの位置・姿勢が自律移動ロボット１のマッチング処理の探索結果であるとした判定がなされる。このような場合には、ディスプレイ２７に自律移動ロボット１と、距離センサ３の測定結果から得られた幾何形状データを地図上にＡ１及びＡ２として表示させ、ユーザが手入力などにより、自律移動ロボット１の正しい位置・姿勢としてＡ１又はＡ２を選択する、あるいは、自律移動ロボット１の進むべき方向を直接指示して自律移動ロボット１に走行指令を出力する。これにより、自律移動ロボット１は、初期位置・姿勢が特定されて、それ以降、指定された目的地の方向に自律移動することができる。

自律移動ロボット１の自律移動時においては、距離センサ３の測定結果から得られた幾何形状データによって距離和データの生成・更新を行うとともに、常に距離和地図データとのマッチングにより、自律移動ロボット１の位置を確認しながら目的地まで自律的に走行することができる。なお、ここではディスプレイ２７上に自律移動ロボットの位置・姿勢の候補を示すことを想定しているが、車両のカーナビゲーションシステムに表示したり、ロボットから離れた外部の基地局のディスプレイに表示してもよい。

以上のように、自律移動ロボット１の自律移動開始時においては、距離センサ３の測定結果から得られた距離和データと、予め記憶した距離和地図データ群内の距離和データとの検索処理結果に基づいて自律移動ロボット１の初期位置・姿勢を推定するシステムにおいて、マッチング対象となる距離和地図データの探索条件を段階的に徐々に広げるように探索することにより、最初から探索条件を動作領域全体の地図データを対象とする場合に比べ、演算処理を軽減でき、初期位置・姿勢の推定に伴う時間を可及的に短縮できるため、高速応答の走行制御が可能となる。また、マッチング処理の際に閾値以上の一致度の大きい候補が複数存在する場合には、ディスプレイ２７上に自律移動ロボット１と距離センサ３の測定結果から得られた幾何形状データを同時に複数表示させることで、ユーザが手入力などにより、自律移動ロボット１の位置・姿勢を入力することで自律移動ロボット１の初期位置・姿勢を特定し、目的地にまで自律移動ロボット１を自律的に走行させることができる。

以上の説明においては、距離和データは幾何形状データの角度ごとにおける距離の総和として説明したが、これはデータとしては単なる一つのデータであるために比較照合にかかる時間は短く、全体のマッチング処理に掛かる時間は大幅に削減できる。しかしながら、地図データによっては、比較照合の場合に近似する多くの距離和データの位置・姿勢候補が検索結果として得られることも考えられる。その場合には距離和データとして記憶するデータを、距離の総和と平均値という２つのデータ構成とすることが考えられる。これにより、距離和データの比較照合による一致度の精度が大きく向上する。さらに、距離の分布の一度を比較するとの観点で考えれば、特定の幾何形状データでの角度ごとの距離データのガウス分布から、ピークの距離データと、その分布のピークの半値の距離データ、１／３の距離データ、任意の割合の距離データ等の距離データに関係する複数の数値を記憶して、それを距離和データの比較照合の際に合わせて用いることでさらに検索の一致度を向上させることができる。

１自律移動ロボット（移動体）
２コントローラ部
３距離センサ
５距離センサ制御部
７初期位置姿勢推定部
８位置姿勢候補表示・設定部
９経路計画部
１０移動機構制御部
１１ロボット形状データ記憶部
１２距離和地図データ記憶部
１３地図データ記憶部
１４走行履歴記憶部
１５経路グラフデータ記憶部

Claims

移動体と、この移動体の移動機構制御部と、移動体周囲の障害物までの距離を角度ごとに検知する距離センサとを備え、さらに、前記移動体が走行する動作領域における障害物の配置形状の地図データを予め格納しており、
前記距離センサにより検知された障害物表面までの角度ごとの距離を距離データとして記録し、前記距離センサから前記障害物表面までの角度ごとの距離を総和して距離和データを算出し、
予め、多数の既知の位置・姿勢における障害物表面までの距離データから得られる距離和データ群を格納しておき、
前記算出した距離和データと前記予め格納してある距離和データ群の距離和データとを比較照合して該距離和データ群の中から近似した距離和データを検索して移動体の位置・姿勢を推定することを特徴とする移動体の位置・姿勢推定システム。
移動体と、この移動体の移動機構制御部と、移動体周囲の障害物までの距離を角度ごとに検知する距離センサとを備え、さらに、前記移動体が走行する動作領域における障害物の配置形状の地図データを予め格納しており、
前記距離センサにより検知された障害物表面までの角度ごとの距離を距離データとして記録し、該検知された障害物表面を外延とする幾何形状データを演算により得、該幾何形状データにおける前記距離センサから前記障害物表面までの角度ごとの距離を総和して距離和データを算出し、
予め、多数の既知の位置・姿勢における障害物表面までの距離データから得られる距離和データ群を格納しておき、
前記算出した距離和データと前記予め格納してある距離和データ群の距離和データとを比較照合して該距離和データ群の中から近似した距離和データを検索し、その検索結果に基づいて前記幾何形状データと前記地図データとのマッチング処理を行うことを特徴とする移動体の位置・姿勢推定システム。
前記距離センサの検知結果から得られた距離和データと前記予め記憶された距離和データ群のなかの距離和データとの比較照合する際に、移動体の過去の走行履歴において、該走行履歴内で移動体が取り得る位置・姿勢での距離和データ群との比較照合を行い、その照合によって近似した距離和データが存在する場合には、その位置・姿勢の近傍において移動体の位置・姿勢を推定するマッチング処理を行うことを特徴とする請求項２記載の移動体の位置・姿勢推定システム。
前記距離センサの検知結果から得られた距離和データと前記予め記憶された距離和データ群のなかの距離和データとの比較照合する際に、移動体の経路グラフデータを基に該経路内で移動体が取り得る位置・姿勢での距離和データ群との比較照合を行い、その照合によって近似した距離和データが存在する場合には、その位置・姿勢の近傍において移動体の位置・姿勢を推定するマッチング処理を行うことを特徴とする請求項２記載の移動体の位置・姿勢推定システム。
前記距離センサの検知結果から得られた距離和データと前記予め記憶された距離和データ群のなかの距離和データとの比較照合する際に、移動体の通路データを基に該通路内で移動体が取り得る位置・姿勢での距離和データ群との比較照合を行い、その照合によって近似した距離和データが存在する場合には、その位置・姿勢の近傍において移動体の位置・姿勢を推定するマッチング処理を行うことを特徴とする請求項２記載の移動体の位置・姿勢推定システム。
前記距離センサの検知結果から得られた距離和データと前記予め記憶された距離和データ群のなかの距離和データとの比較照合する際に、移動体の動作領域の地図データ内で移動体が取り得る位置・姿勢での距離和データ群との比較照合を行い、その照合によって近似した距離和データが存在する場合には、その位置・姿勢の近傍において移動体の位置・姿勢を推定するマッチング処理を行うことを特徴とする請求項２記載の移動体の位置・姿勢推定システム。
請求項２記載の移動体の位置・姿勢推定システムにおいて、
前記算出した距離和データと前記予め格納してある距離和データ群の距離和データとを比較照合して該距離和データ群の中から近似した距離和データを検索し、その検索結果に基づいて前記幾何形状データと前記地図データとのマッチング処理を行う際に、マッチング処理を行う探索条件を複数用意しておき、そのマッチング処理の度合いを段階的に広げていくことを特徴とする移動体の位置・姿勢推定システム。
前記第１のマッチング処理の探索条件が、移動体の過去の走行履歴内で移動体が取り得る位置・姿勢でのマッチング処理であり、第２のマッチング処理の探索条件が、移動体の経路内で移動体が取り得る位置・姿勢でのマッチング処理であり、最終のマッチング処理の探索条件が、移動体の通路内で移動体が取り得る位置・姿勢でのマッチング処理であることを特徴とする請求項７記載の移動体の位置・姿勢推定システム。
前記距離センサの検知結果から得られた距離和データと前記予め記憶された距離和データ群のなかの距離和データとの比較照合し、又は幾何形状データと地図データをマッチング処理する際に、近似した複数の距離和データの位置・姿勢が推定された場合または地図データと一致度が高い複数の幾何形状データが推定された場合、ディスプレイ上に複数の移動体の位置・姿勢とそれに対応した複数の幾何形状データとを表示し、ユーザが表示された複数の位置・姿勢の中から一つの位置・姿勢を選択可能としたことを特徴とする請求項１乃至８の内の一つの請求項記載の移動体の位置・姿勢推定システム。