(57)【要約】【課題】低コスト化を図りながら屋外環境下における実用可に適するようにした。【解決手段】環境における面上を移動可能に構成されるとともに上記環境を表す情報を取得するための検出手段を有した移動体と、上記移動体が移動する上記面上に配設されるとともに局所環境情報を記憶した記憶手段を有し、かつ、上記移動体と非接触通信可能な複数の情報管理電子デバイスとを有し、上記移動体は、上記検出手段による検出結果と上記情報管理電子デバイスとの非接触通信により取得した局所環境情報とに基づいて自己位置推定を行う自己位置推定手段を有するようにした。【選択図】        図１
2006343881.tif 000002
【特許請求の範囲】
【請求項１】
環境における面上を移動可能に構成されるとともに前記環境を表す情報を取得するための検出手段を有した移動体と、
前記移動体が移動する前記面上に配設されるとともに局所環境情報を記憶した記憶手段を有し、かつ、前記移動体と非接触通信可能な複数の情報管理電子デバイスと
を有し、
前記移動体は、前記検出手段による検出結果と前記情報管理電子デバイスとの非接触通信により取得した局所環境情報とに基づいて自己位置推定を行う自己位置推定手段を有する
ことを特徴とする自己位置推定システム。
【請求項２】
請求項１に記載の自己位置推定システムにおいて、
前記情報管理電子デバイスは、前記面上の任意の位置に配置可能であり、かつ、前記記憶手段に記憶した前記局所環境情報は前記情報管理電子デバイスが配置された前記面上の環境の局所的な地形を表す地形情報である
ことを特徴とする自己位置推定システム。
【請求項３】
請求項１に記載の自己位置推定システムを用いた自己位置推定方法であって、
前記移動車両は、
出発点に配置された前記情報管理電子デバイスに記憶された局所環境情報を取得するとともにレンジ情報を取得して、前記取得した局所環境情報とレンジ情報とに基づいて自己位置推定を行って移動を開始し、
前記出発点における前記情報管理電子デバイスに引き続いて順次配置された前記情報管理電子デバイスの局所環境情報を順次取得するとともに当該取得に応じてレンジ情報を順次取得し、前記順次取得した局所環境情報とレンジ情報とに基づいてそれぞれ自己位置推定する
ことを特徴とする自己位置推定方法。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、自己位置推定システムおよび自己位置推定方法に関し、さらに詳細には、屋外環境下において自律的に移動する移動車や移動ロボットなどの移動体の位置を推定する際に用いて好適な自己位置推定システムおよび自己位置推定方法に関する。
【背景技術】
【０００２】
屋内や屋外の環境下において移動車や移動ロボットなどの移動体の自律的な移動、より具体的には、車両などの自動走行を実現するためには、移動体自身が自己の位置と姿勢とを認識することが必要である。
【０００３】
一般に、このような移動体自身による当該移動体の移動環境内における自己の位置と姿勢との認識を自己位置推定と称しており、これまでに自己位置推定の手法としては種々のものが提案されている。
【０００４】
例えば、屋外環境における自己位置推定の手法としては、数センチ単位の誤差で測定が可能なＲＴＫ−ＧＰＳ（Ｒｅａｌ  Ｔｉｍｅ  Ｋｉｎｅｍａｔｉｃｓ  Ｇｌｏｂａｌ  Ｐｏｓｉｔｉｏｎｉｎｇ  Ｓｙｓｔｅｍ）により移動体の位置を推定するとともに、高精度なジャイロセンサーにより移動体の姿勢を推定する手法が提案されている（非特許文献１参照）。
【０００５】
しかしながら、上記したようなＧＰＳを使用する手法によれば、移動体が移動する移動環境内に建物などの大型の遮蔽物が存在するような場合には、ＧＰＳの計測精度が低下する恐れがあるため、これが実用化へ向けての障碍になっているという問題点があった。
【０００６】
一方、移動体にカメラなどの視覚センサーやレンジセンサーを搭載しておき、これら視覚センサーやレンジセンサーによりランドマークを検出し、その検出結果に対して事前に与えられた地図情報をマッチングして、これにより自己位置を推定する手法も提案されている（非特許文献２参照）。
【０００７】
この非特許文献２に開示された手法においては、例えば、屋内環境下に存在する壁や柱などのような高さが一様な対象物を平面情報として扱うことにより、ランドマークの検出と地図情報とのマッチングを少ない計算量で短時間に行うことができるようになる。このため、非特許文献２に開示された手法は、屋内環境下における自己位置推定には適しているものと思われる。
【０００８】
しかしながら、非特許文献２に開示された手法を屋外環境下における自己位置推定に適用するにあたっては、屋外環境には柵、垣根、木や茂みなどの高さによって形が一様でない対象物が多数存在するので、３次元空間でのランドマークの検出と地図情報とのマッチングを行うためには多くの計算量が必要となってしまい、また、ランドマークとのマッチングをとるための詳細な３次元地図情報をあらかじめ取得していなければならないので、これらが実用化へ向けての障碍になっているという問題点があった。
【０００９】
さらに、自動車にレーザーレンジファインダーを搭載し、ビーコンを手がかりに屋外環境下で次第に地図生成を行なう手法も提案されている（非特許文献３参照）。
【００１０】
ここで、一般的な屋外環境においては、３次元的な形状が一様でない物体や自然対象物のような不定形な形状を含んだランドマークの利用を考慮しなければならない。このような場合において、事前に環境地図を持たせておき、マッチングを取ることでノイズ除去が可能になると考えられるが、走行環境の全ての地図を移動体に格納管理すると移動体のコストが増加するという問題点があった。
【００１１】
また、オンラインでサーバ等に格納された地図情報を参照するアプローチも考えられるが、高速で移動する際の処理の実時間性について問題が発生する可能性があるという問題点もあった。
【非特許文献１】Ｙ．Ｃｈｉｋａｙａｍａ，  Ｓ．Ｋｏｔａｎｉ  ａｎｄ  Ｈ．Ｍｏｒｉ，  “Ｐｏｓｉｔｉｏｎｉｎｇ  ｏｆ  Ｒｏｂｏｔｉｃ  Ｔｒａｖｅｌ  Ａｉｄ  ｗｉｔｈ  ＲＴＫ−ＧＰＳ”，  Ｐｒｏｃ．ｏｆ  ＲＯＢＯＭＥＣ  ’０１，Ｎｏ．１Ｐ１−Ｊ４，  ２００１（ｉｎ  Ｊａｐａｎｅｓｅ）
【非特許文献２】Ｓ．Ｓｏｕｍａｒｅ，  Ａ．Ｏｈｙａ  ａｎｄ  Ｓ．Ｙｕｔａ，  “Ｒｅａｌ−Ｔｉｍｅ  Ｏｂｓｔａｃｌｅ  Ａｖｏｉｄａｎｃｅ  ｂｙ  ａｎ  Ａｕｔｏｎｏｍｏｕｓ  Ｍｏｂｉｌｅ  Ｒｏｂｏｔ  ｕｓｉｎｇ  ａｎ  Ａｃｔｉｖｅ  Ｖｉｓｉｏｎ  Ｓｅｎｓｏｒ  ａｎｄ  ａ  Ｖｅｒｔｉｃａｌｌｙ  Ｅｍｉｔｔｅｄ  Ｌａｓｅｒ  Ｓｌｉｔ”，  Ｉｎｔｅｌｌｉｇｅｎｔ  Ａｕｔｏｎｏｍｏｕｓ  Ｓｙｓｔｅｍｓ  ７，  ２００２，  ｐｐ．３０１−３０８
【非特許文献３】Ｊ．Ｇｕｉｖａｎｔ，  Ｅ．Ｎｅｂｏｔ  ａｎｄ  Ｓ．Ｂａｉｋｅｒ，  “Ａｕｔｏｎｏｍｏｕｓ  Ｎａｖｉｇａｔｉｏｎ  ａｎｄ  Ｍａｐ  ｂｕｉｌｄｉｎｇ  Ｕｓｉｎｇ  Ｌａｓｅｒ  Ｒａｎｇｅ  Ｓｅｎｓｏｒｓ  ｉｎ  Ｏｕｔｄｏｏｒ  Ａｐｐｌｉｃａｔｉｏｎｓ”，  Ｊｏｕｒｎａｌ  ｏｆ  Ｒｏｂｏｔｉｃ  Ｓｙｓｔｅｍｓ，  Ｖｏｌ  １７，  Ｎｏ  １０，  ｐｐ５６５−５８３，  ２０００
【発明の開示】
【発明が解決しようとする課題】
【００１２】
本発明は、従来の技術の有する上記したような種々の問題点に鑑みてなされたものであり、その目的とするところは、低コスト化を図りながら屋外環境下における実用可に適した自己位置推定システムおよび自己位置推定方法を提供しようとするものである。
【課題を解決するための手段】
【００１３】
上記目的を達成するために、本発明は、予め自己位置推定に必要な環境情報を格納した局所無線通信可能な情報管理デバイスを環境内に分散配置し、移動体は必要時に情報管理デバイス環境から参照すべき目標物（ランドマーク）の情報を取得し、検出手段により取得した環境の情報とのマッチングを取ることで移動体の自己位置推定を行うようにしたものである。換言すれば、本発明は、環境内に配置された情報管理デバイスの情報と移動体が取得した情報とに基づいて、移動体の自己位置推定を行う手法を提案するものである。
【００１４】
より詳細には、情報管理デバイスには必要なランドマークとなり得る箇所の地形情報を書き込み、それを移動体の走行路面上に分散配置する。そして、移動体は移動中に局所通信により情報管理デバイスに書き込まれた情報を読み込みつつ、スキャンマッチングを行うことで自己位置推定を実現する。
【００１５】
即ち、本発明のうち請求項１に記載の発明は、環境における面上を移動可能に構成されるとともに上記環境を表す情報を取得するための検出手段を有した移動体と、上記移動体が移動する上記面上に配設されるとともに局所環境情報を記憶した記憶手段を有し、かつ、上記移動体と非接触通信可能な複数の情報管理電子デバイスとを有し、上記移動体は、上記検出手段による検出結果と上記情報管理電子デバイスとの非接触通信により取得した局所環境情報とに基づいて自己位置推定を行う自己位置推定手段を有するようにしたものである。
【００１６】
また、本発明のうち請求項２に記載の発明は、本発明のうち請求項１に記載の発明において、上記情報管理電子デバイスは、上記面上の任意の位置に配置可能であり、かつ、上記記憶手段に記憶した上記局所環境情報は上記情報管理電子デバイスが配置された上記面上の環境の局所的な地形を表す地形情報であるようにしたものである。
【００１７】
また、本発明のうち請求項３に記載の発明は、本発明のうち請求項１に発明を用いたものであって、上記移動車両は、出発点に配置された上記情報管理電子デバイスに記憶された局所環境情報を取得するとともにレンジ情報を取得して、上記取得した局所環境情報とレンジ情報とに基づいて自己位置推定を行って移動を開始し、上記出発点における上記情報管理電子デバイスに引き続いて順次配置された上記情報管理電子デバイスの局所環境情報を順次取得するとともに当該取得に応じてレンジ情報を順次取得し、上記順次取得した局所環境情報とレンジ情報とに基づいてそれぞれ自己位置推定するようにしたものである。
【発明の効果】
【００１８】
本発明は、以上説明したように構成されているので、低コスト化を図りながら屋外環境下における実用可に適した自己位置推定システムおよび自己位置推定方法を提供することができるという優れた効果を奏する。
【発明を実施するための最良の形態】
【００１９】
以下、添付の図面を参照しながら、本発明による自己位置推定システムおよび自己位置推定方法の実施の形態の一例を詳細に説明する。
【００２０】
まず、図１には、本発明による自己位置推定システムの実施の形態の一例を示す概念構成説明図が示されている。
【００２１】
この自己位置推定システム１０は、道路上を自律的に移動可能に構成された移動体たる移動車両２０と、移動車両２０が移動する道路上に配設されるとともに当該移動車両２０と非接触通信可能な複数の情報管理電子デバイスたるｎ個（ただし、ｎは正の整数である。）のＩＣタグとしてのＩＡ（Ｉｎｆｏｒｍａｔｉｏｎ  Ａｓｓｉｓｔａｎｔ）タグ３０１、３０２、３０３、・・・、３０ｎ−１、３０ｎとを有して構成されている。
【００２２】
なお、本明細書においては、ＩＡタグ３０１、３０２、３０３、・・・、３０ｎ−１、３０ｎを示す際に個々のＩＡタグをそれぞれ特定する必要がない場合や、ＩＡタグ３０１、３０２、３０３、・・・、３０ｎ−１、３０ｎにそれぞれ共通する説明を行う場合には、単にＩＡタグ３０と適宜に表記するものとする。
【００２３】
ここで、ＩＡタグ３０は、上記したように環境内に分散配置される非接触通信可能なメモリ付き電子デバイスであるが、非接触通信として赤外線通信などの局所無線通信を行うための通信手段と、情報を記録するためのメモリ（記憶手段）とを有して構成されている。そして、記憶手段には、レーザーレンジファインダー（ＬＲＦ：  Ｌａｓｅｒ  Ｒａｎｇｅ  Ｆｉｎｄｅｒ）２０６（後述する。）により外部の環境を表す情報として取得された目標物（ランドマーク）までの距離情報たるレンジ情報とマッチング（照合）するための情報として、局所的な環境を表す情報たる局所環境情報が記憶されている。
【００２４】
ＩＡタグ３０の記憶手段に記憶される局所環境情報には、環境の局所的な地形を表す情報たる地形情報（地形情報としては、例えば、ＩＡタグ３０から道路の縁石までの２次元的または３次元的な距離情報や道路幅を示す道路幅情報などがある。）が含まれる。
【００２５】
なお、ＩＡタグ３０には、上記した局所環境情報の他に、例えば、それぞれのＩＡタグ３０に固有のＩＤ番号、ＩＡタグ３０自身の座標値、次に局所環境情報を読み出すべきＩＡタグ３０のＩＤ番号、次に局所環境情報を読み出すべきＩＡタグ３０の座標値、移動車両２０の状態（例えば、移動車両２０は直線上を移動中、曲線上を移動中あるいは停止中などの状態である。）などがある。
【００２６】
このように、ＩＡタグ３０は、移動車両２０の自己位置推定に必要な情報を格納し、その情報を移動車両２０に提供することができるものであるが、こうしたＩＡタグ３０としては、本願出願人の出願に係る特開平８−１３８０１８号公報、特開平９−１７１５９９号公報および特開２００１−３２０７８１号公報などに開示されたタグと同様な構成のものを用いることができる。
【００２７】
次に、移動車両２０は、車体部２０２を備えており、車体部２０２の下部には、車輪２０４が配設されている。
【００２８】
また、車体部２０２には、外部の環境を表す情報を取得するための検出手段たるセンサーとして目標物までの距離情報たるレンジ情報を取得するためのレーザーレンジファインダー２０６と、道路に配設されたＩＡタグ３０と局所無線通信を行うためのアンテナ２０８と、移動車両２０の全体の動作の制御を行うための制御部２１０と、車輪２０４を駆動するめのモーター２１２とが配設されている。
【００２９】
より詳細には、レーザーレンジファインダー２０６は車体部２０２の進行方向前方側に配設され、また、アンテナ２０８は車体部２０２の下面側に配設されている。
【００３０】
そして、レーザーレンジファインダー２０６により得られたレンジ情報と、アンテナ２０８によるＩＡタグ３０との局所通信により得られた局所環境情報とは、制御部２１０に入力されることになる。
【００３１】
ここで、制御部２１０は、この移動車両２０の全体の動作の制御を行う中央処理装置（ＣＰＵ）２１０ａと、中央処理装置２１０ａが実行するプログラムやそのプログラムの処理に必要なデータなどを記憶するリード・オンリ・メモリ（ＲＯＭ）２１０ｂと、中央処理装置２１０ａのワーキング・エリアとしてのランダム・アクセス・メモリ（ＲＡＭ）２１０ｃとなどから構成されている。
【００３２】
そして、制御部２１０においては、レーザーレンジファインダー２０６により得られたレンジ情報とアンテナ２０８によるＩＡタグ３０との局所通信により得られた局所環境情報とに基づいてマッチングを行い、移動車両２０の位置を推定するとともにモーター２１２の駆動を制御して、移動車両２０の移動の方向を制御するものである。
【００３３】
以上の構成において、移動車両２０は、レーザーレンジファインダー２０６によりレンジ情報を取得するとともにＩＡタグ３０との間で局所通信を行って局所環境情報を取得し、当該レンジ情報と当該環境情報とに基づいてマッチングを行って移動車両２０の位置を推定するとともにモーター２１２により車輪２０４を駆動して、目的地に応じた方向へ適切に移動することになるものである。この局所通信は、移動車両２０の進行に応じて、ＩＡタグ３０１→ＩＡタグ３０２→ＩＡタグ３０３→・・・→ＩＡタグ３０ｎ−１→ＩＡタグ３０ｎの順序で順次に行われる。
【００３４】
即ち、ＩＡタグ３０からの信号をアンテナ２０８が受信し、その受信結果に応じて制御部２１０が移動車両２０の位置を推定するとともにモーター２１２を制御して、常に自己位置を推定しながら目的地へ向かうように車輪２０４の方向を定めて移動車両２０を移動させるものである。
【００３５】
従って、ＩＡタグ３０１、３０２、３０３、・・・、３０ｎ−１、３０ｎのそれぞれには、環境の局所的な情報たる局所環境情報を記憶させておけばよいので、各ＩＡタグ３０の記憶手段の記憶する情報量を少なくすることができるようになり、これによりＩＡタグ３０の製造コストの上昇を抑制することができる。
【００３６】
次に、自己位置推定システム１０における移動車両２０の自己位置推定方法の詳細について説明する。なお、この実施の形態においては、環境に存在する目標物（ランドマーク）として道路の縁石を用いた。
【００３７】
まず、自己位置推定システム１０における自己位置推定アルゴリズム、即ち、この自己位置推定システム１０を用いて移動車両２０の自己位置推定を実現するためのアルゴリズムについて説明する。
【００３８】
ここで、移動車両２０は、最初に出発点のＩＡタグ３０１内の局所環境情報を読み込むとともにレンジ情報を取得して、それらに基づいて自己位置推定を行って走行を開始すると、途中に設置されているＩＡタグ３０２、ＩＡタグ３０３、・・・、ＩＡタグ３０ｎ−１、ＩＡタグ３０ｎ内の局所環境情報を順次取得するとともに当該取得に応じてレンジ情報を順次取得して、それらに基づいてそれぞれ自己位置推定を行いながら目的地への到着を試みる。つまり、移動車両２０は大域的な座標空間で自己位置を推定するのではなく、認識したＩＡタグ３０を原点とする局所座標系における位置推定を行う。そして、次のＩＡタグ３０を読み込んだ際に新たなＩＡタグ３０の座標空間上で位置推定を行う。これを目的地まで繰り返すことにより、累積誤差をキャンセルしながら自己位置推定を行っていくことができる。
【００３９】
次に、図２には、自己位置推定システム１０における自己位置推定アルゴリズムを実施するために用いるキネマティクスモデルおよび座標系についての説明図が示されている。即ち、キネマティクスモデルおよび車体部２０２を中心とした座標系である車両座標系｛ｃ｝およびレーザーレンジファインダー２０６を中心とした座標系であるＬＲＦセンサー座標系｛ＬＲＦ｝は、図２に示すように設定することができる。
【００４０】
ここで、キネマティクスモデルは、前方左右の車輪２０４の中間に仮想的な車輪２０５があるのと等価であると考えることが可能であり、車両座標系の原点はＩＡタグ３０との通信を行うアンテナ２０８の前方中央とし、ＬＲＦセンサー座標系の原点はレーザーレンジファインダー２０６の中心とする。
【００４１】
なお、図２に示す寸法は、後述する実験において用いた移動車両２０における大きさを示すものである。
【００４２】
次に、ｉというＩＤ番号のついたＩＡタグ３０を中心とした座標系であるＩＡ座標系内での車両の位置ならびに姿勢を表すベクトルをＩＡｉＰとして、以下のように定義する。
【００４３】
ＩＡｉＰ＝［ｘ  ｙ  ｚ］Ｔ  
次に、移動車両２０の直進時における自己位置推定アルゴリズムを説明するが、図３には、移動車両２０の直進時の局所的なＩＡ座標系が示されている。各ＩＡタグ３０には、それぞれを特定するＩＤ番号およびＩＡ座標系における参照すべき直線までの距離情報Ｈを格納しておく。そして、以下のステップ１〜６に示すアルゴリズムを実行する。
【００４４】
ステップ１：ＩＡｉＰ＝［０  ０  ０］Ｔとし、ＩＡタグ３０から距離Ｈを取得する。
【００４５】
ステップ２：レーザーレンジファインダー２０６からレンジ情報を取得し、それを−π／２回転させて車両座標系空間に変換する。
【００４６】
ステップ３：ＩＡ座標系における参照すべき直線近傍（ＩＡｉＹ＝Ｈ±ΔＨ）のレンジ情報を抽出し、直線推定をし、この参照すべき直線と移動車両２０（車体部２０２）との傾きβと、参照すべき直線とレーザーレンジファインダー２０６の取り付け位置との垂直距離ｈとをそれぞれ求める。ここで、ΔＨは、レーザーレンジファインダー２０６で得られるレンジ情報たるセンシングデータの許容範囲を決める値である。
【００４７】
ステップ４：自己位置を以下の式（１）により更新する。
【数１】
2006343881.tif 000003
【００４８】
また、ｘ座標については、下記のようにサンプリング時間ΔＴごとに積分を行い更新する。
【数２】
2006343881.tif 000004
【００４９】
ステップ５：サンプリング時間ΔＴ後にレーザーレンジファインダー２０６からレンジ情報を取得し、−π／２回転させて車両座標系空間に変換する。
【００５０】
ステップ６：次のＩＡタグ３０を読み込めば上記ステップ１へ戻って新たな座標系をセットし、それ以外なら上記ステップ３へ戻って上記ステップ３〜６の処理を繰り返し行う。
【００５１】
次に、移動車両２０が交差点に位置するときにおける自己位置推定アルゴリズムを説明するが、図４には、移動車両２０の交差点通過時における局所的なＩＡ座標系が示されており、ここでは左折を例に説明する。各ＩＡタグ３０には、それぞれを特定するＩＤ番号、そのＩＡ座標系における参照すべき直線までの距離Ｈ１、Ｈ２および道路幅ｗ１、ｗ２を格納しておく。そして、以下のステップ１〜７に示すアルゴリズムを実行する。
【００５２】
ステップ１：ＩＡｉＰ＝［０  ０  ２／π］Ｔとし、ＩＡタグ３０から距離Ｈ１、Ｈ２を取得する。
【００５３】
ステップ２：レーザーレンジファインダー２０６からレンジ情報を取得し、道路幅ｗ１、ｗ２の小さいほうを閾値としてグループ化を行う。
【００５４】
ステップ３：グループ化されたデータ群から距離Ｈ１、Ｈ２近傍の点データ群を２つ取得し、それぞれを直線推定し、推定したそれぞれの直線（推定直線）と移動車両２０（車体部２０２）との傾きβ１、β２を求める。また、推定直線とレーザーレンジファインダー２０６の取り付け位置との垂直距離ｈ１、ｈ２をそれぞれ求める。
【００５５】
ステップ４：ｈ１、ｈ２のうち距離が近い方の直線を参照直線として選択し、ｈ１＞ｈ２ならば、β＝β２、ｈ＝ｈ２、Ｈ＝Ｈ２とし、それ以外なら、β＝β１、ｈ＝ｈ１、Ｈ＝Ｈ１とする。ここで、Δβは、推定直線と移動車両２０（車体部２０２）とがなす傾きの許容角度を表す。
【００５６】
ステップ５：自己位置を以下の式により更新する。
【数３】
2006343881.tif 000005
【００５７】
また、ｘ座標については、下記のようにサンプリング時間ΔＴごとに積分を行い更新する。
【数４】
2006343881.tif 000006
【００５８】
ステップ６：サンプリング時間ΔＴ後にレーザーレンジファインダー２０６からレンジ情報を取得し、道路幅ｗ１、ｗ２の小さいほうを閾値としてグループ化を行う。
【００５９】
ステップ７：次のＩＡタグ３０を読み込めば上記ステップ１へ戻って新たな座標系をセットし、それ以外なら上記ステップ３へ戻って上記ステップ３〜７の処理を繰り返し行う。
【００６０】
上記した移動車両２０の直進時における自己位置推定アルゴリズムと移動車両２０が交差点に位置するときにおける自己位置推定アルゴリズムとにより、走行環境での移動車両２０の自己位置推定が実現される。
【００６１】
なお、上記した各アルゴリズムにおいては、自己位置を求めるためにレーザーレンジファインダー２０６により得られたレンジ情報から縁石を抽出し、直線化できることが前提となっていた。
【００６２】
ここで、与えられた点の集合から直線を抽出する場合に、一般的に用いられるものとしては、最小２乗法による直線推定またはハフ変換による直線抽出アルゴリズムが考えられる。
【００６３】
ところで、屋外環境を想定した際には、レーザーレンジファインダー２０６により目標物として縁石を検出する場合でも、雑草または標識の柱や石などがグループ化されたデータに含まれてしまい、それがノイズとなり直線推定に影響を及ぼしてしまう恐れがある。一般的な直線推定手法としてハフ変換があるが、ハフ変換では直線を推定するのに計算量がかかり、走行しながら実時間処理を行うには適していないものと言える。そこで、本発明による自己位置推定システムにおいては、ハフ変換よりも処理時間が少なく、計測ノイズに強い直線推定アルゴリズムを用いるようにした。
【００６４】
即ち、Ｎ個のデータ点（ｘｉ，ｙｉ）（なお、ｉ＝１，・・・，Ｎとする。）を取得した際に、Ｍ個のパラメータを持つモデルに対する推定測定誤差が、ある確率的な分布を持つと仮定する。このときモデルξ（ｘ，ａ）のパラメータａ∈ＲＭを推定する最尤法の公式を導く際に、以下のように記述することができる。
【数５】
2006343881.tif 000007
【００６５】
ここで、関数ｆは推定測定誤差の確率密度関数である。この式（５）について対数をとることで積和記述を行うと式（６）のようになる。
【数６】
2006343881.tif 000008
【００６６】
パラメータａの推定には、式（６）をパラメータａについて最小化すればよい。ここで、式（６）をａにより偏微分したものは以下の式（７）のようになる。
【数７】
2006343881.tif 000009
【００６７】
さらに、誤差分布が正規分布の裾よりも広い分布関数である両側指数分布に従うと仮定すると、
【数８】
2006343881.tif 000010
【００６８】
が成立する。ここで、σｉは取得したレンジ情報の分散を表す係数である。
【００６９】
上記した式（７）（８）より、
【数９】
2006343881.tif 000011
【００７０】
ここで
【数１０】
2006343881.tif 000012
【００７１】
である。
【００７２】
また、取得したデータ点から推定する直線モデルξ（ｘ，ａ）は、以下のように表すことができる。
【数１１】
2006343881.tif 000013
【００７３】
これを用いて式（９）を展開すると、以下のように表すことができる。
【数１２】
2006343881.tif 000014
【００７４】
ここで、係数σｉが全て等しいとすると、
【数１３】
2006343881.tif 000015
【００７５】
が成り立つ。また、直線近似を考えると、
【数１４】
2006343881.tif 000016
【００７６】
といった誤差量積和を最小化しなければならない。これらの条件により、レーザーレンジファインダー２０６により取得したＮ個のデータを元に式（１４）で示される非線形方程式や式（１５）を満たすパラメータａ１、ａ２を二分法等の手法によって求めればよい。
【００７７】
これにより、レーザーレンジファインダー２０６により取得したレンジ情報を元に縁石のなす直線推定することが可能になり、移動車両２０の自己位置推定が実現可能になる。
【００７８】
次に、本願発明者によって行われた実験結果について説明するが、この実験においては、ＩＡタグ３０とアンテナ２０８との間の局所的な通信は赤外線通信により行い、通信距離は最大１５０［ｍｍ］であり、制御部２１０におけるデータの読み込み速度は４．８［ｋｂｐｓ］であり、書き込み速度は０．９６［ｋｂｐｓ］である。また、ＩＡタグ３０内に格納可能データ量は、６［ｂｙｔｅ］である。
【００７９】
また、ＩＡタグ３０、アンテナ２０８ならびに制御部２１０としては、移動車両２０が、最大１８［ｋｍ／ｈ］の状態で走行しても確実に読み込むことができるようにした。
【００８０】
なお、本実験においては、上記した移動車両２０の直進時における自己位置推定アルゴリズムと移動車両２０が交差点に位置するときにおける自己位置推定アルゴリズムとを用いるため、移動車両２０の走行環境に存在する縁石をレーザーレンジファインダー２０６による測定の目標物として、レーザーレンジファインダー２０６によりレンジ情報を取得した。
【００８１】
次に、移動車両２０およびその走行環境について説明すると、移動車両２０としては電動式車両を用いた。
【００８２】
また、レンジ情報を取得するためのセンサーたるレーザーレンジファインダー２０６は、車体部２０２の前方中央であって地面から８ｃｍの部位に設置した。
【００８３】
また、移動車両２０としては、ステアリング型（１ステアリング、１輪駆動）でアッカーマンステア方式に基づいたものを用いた。そして、移動車両２０の車軸にはローターリーエンコーダー（Ｒｏｔａｒｙ  Ｅｎｃｏｄｅｒ）が設置してあり、車輪回転速度（Ｖｅｒｏｃｉｔｙ：Ｖｃ）の測定が可能となされている（図２参照）。  
ＩＡタグ３０と通信を行うためのアンテナ２０８は、車体部２０８の下面に搭載している。これにより、走行中にＩＡタグ３０から局所環境情報を非接触通信により取得可能となされている。
【００８４】
また、走行環境としては、建物や木などに囲まれ、走行路面たる道路の両端に縁石のあるアスファルト道路を設定した。つまり、大型キャンパスなどに比較的多くみられる道路のような直進部と十字路やＴ字路のような交差点部で構成されているものとする。また、ここでは動的障害物が環境内に存在しないものとした。
【００８５】
次に、実験内容の詳細について説明するが、この実験は、電磁誘導線で直進、左折、直進からなるコースを予め設計し、その上にＩＡタグ３０を設置するようにした。移動車両２０は電磁誘導線上に沿いながら走行し、ＩＡタグ３０を読み込んだときに、格納されている情報に基づいて、ＩＡタグ３０を原点とするＩＡ座標空間において自己位置推定を行う。
【００８６】
なお、電磁誘導線による走行機能は移動車両２０に搭載されており、これを実軌道とし比較する。
【００８７】
図５には実験概要が示されており、図５において、実線は電磁誘導線を示し、破線は道路の縁石を示している。また、ＩＡタグ３０としては、ＩＡタグ３０１、ＩＡタグ３０２、ＩＡタグ３０３およびＩＡタグ３０４の４個配設されており、ＩＡタグ３０１に示された数字「１」とＩＡタグ３０２に示された数字「２」とＩＡタグ３０３に示された数字「３」とＩＡタグ３０４に示された数字「４」とは、それぞれＩＤ番号を示している。また、括弧内には、１番目のＩＡタグ３０１を原点とした世界座標（単位ｍ）の座標値を示している。
【００８８】
また、図６には、ＩＡタグ３０１、ＩＡタグ３０２、ＩＡタグ３０３およびＩＡタグ３０４にそれぞれ格納する情報が示されている。そして、直進時のレンジ情報を抽出するための基準値にΔＨ＝０．５［ｍ］を設定し、カーブ時にランドマークを切り替えるための基準値にΔβ＝π／９［ｒａｄ］を設定した。また、移動車両２０は、走行速度を５［ｋｍ／ｈ］として定速走行させた。
【００８９】
図７には、ＩＤ番号１、ＩＤ番号２ならびにＩＤ番号３のＩＡタグ３０をそれぞれ原点とするＩＡ座標系上で推定した移動車両２０の自己位置を、世界座標空間上に変換したものが示されている。この図７をみてもわかるように、直進時およびカーブ時においても検出した直線から直接測定できる状態変数においては、最大でも２０［ｃｍ］程度の誤差であった。これは、アンテナ２０８とＩＡタグ３０を読み込んだ位置のズレによって生じたものであると考えられる。
【００９０】
一方で、積分値によって求めたほうの状態変数つまり目標進行方向成分の誤差は、全ての場合において、当然ながら必ず誤差が蓄積されてしまう。しかし、目標進行方向の誤差が生じても移動車両２０は、早かれ遅かれ必ずＩＡタグ３０を通過するものであり、本発明による自己位置推定システム１０ではＩＡタグ３０を読み込んだ時点で常に新たなＩＡ座標空間にセットし直される。
【００９１】
そして、図８には、世界座標系に変換した自己位置を元にレンジ情報をつなぎ合わせたマップが示されている。これにより、本発明による自己位置推定システム１０は、各ＩＡタグ３０において位置、角度の補正を行うことが可能となっている。この結果、推定位置とレンジ情報とのデータを再構成したところ、実環境に近いマップの生成が実現された。これにより、位置推定が精度よく行われたことがわかる。
【００９２】
以上において説明したように、本発明においては、情報を分散配置して管理をすることにより大局通信網に負荷をかけずに，局所通信のみで並列的に移動車両２０の位置推定および制御を可能にすることができる。
【００９３】
なお、上記した実施の形態や実施例は、以下の（１）乃至（４）に示すように変形することができるものである。
【００９４】
（１）上記した実施の形態や実施例においては、ＩＡタグ３０が管理する情報として局所環境情報やＩＤ番号などを示したが、ＩＡタグ３０が管理する情報はこれらに限られるものではないことは勿論である。以下に、ＩＡタグ３０が管理することが好まし情報を列記すると、
・自身のＩＤ番号
・自身の世界座標での位置情報（座標値）
・次に見つけるべきＩＡタグ３０のＩＤ番号
・前のＩＡタグ３０を原点としたＩＡ座標系における位置情報（座標値）
・車両制御情報｛移動車両２０の状態を示す制御モード（直線軌道モード、曲線軌道追従モード）、停止、速度指令情報、分岐情報など｝
・環境の地形を表現する局所環境情報（２次元形状，３次元形状を含む）
・レーザーレンジファインダー２０６が地形の中のどこを参照すべきかを示す指示情報
などが挙げられる。
【００９５】
（２）上記した実施の形態において、環境内のどこの地形を参照して自己位置推定をすべきかの情報を複数内蔵しておき、移動車両のタイプおよびレーザーレンジファインダーなどのセンサーの性能に応じて、どこの特徴を検出して自己位置推定して走行すべきかの指示を行う指示手段を設けるようにしてもよい。こうした指示手段を設けることにより、指定された特徴が検出できない場合にも、他の複数の参照候補を利用することが可能になり、自己位置の推定を実現する確率をあげることができるようになる。また、複数の参照候補の中から幾つかの点を検出することにより、自己位置推定の精度を向上することもできるようになる。
【００９６】
（３）上記した実施の形態において、ＩＡタグ３０が情報の書き込み機能を備えるようにしてもよい。こうしたＩＡタグ３０の書き込み機能を利用することにより、移動車両２０がＩＡタグ３０上を通行した際に自己の選択した行動を書き込むことで、後続の移動車両２０に過去の通行情報の伝言板として利用することができるようになり、例えば、過去の通行情報（行動選択など）の統計情報を記録し、交通量に基づいて後続の移動車両２０の通行経路計画の変更などに用いることができる。
【００９７】
（４）上記した実施の形態ならびに上記した（１）乃至（３）に示す変形例は、適宜に組み合わせるようにしてもよい。
【産業上の利用可能性】
【００９８】
本発明は、屋外環境下において移動する移動車両や移動ロボットなどの移動体の位置を推定することができるので、屋外展示場などの移動用カートやゴルフ場におけるゴルフカートなどに利用することができる。
【図面の簡単な説明】
【００９９】
【図１】図１は、本発明による自己位置推定システムの実施の形態の一例を示す概念構成説明図である。
【図２】図２は、自己位置推定システムにおける自己位置推定アルゴリズムを実施するために用いるキネマティクスモデルおよび座標系についての説明図である。
【図３】図３は、移動車両の直進時の局所的なＩＡ座標系についての説明図である。
【図４】図４は、移動車両の交差点通過時における局所的なＩＡ座標系についての説明図である。
【図５】図５は、本願出願人による実験概要を示す説明図である。
【図６】図６は、本願出願人による実験においてＩＡタグにそれぞれ格納した情報を示す図表である。
【図７】図７は、ＩＡタグをそれぞれ原点とするＩＡ座標系上で推定した移動車両の自己位置を世界座標空間上に変換したものの説明図である。
【図８】図８は、世界座標系に変換した自己位置を元にレンジ情報をつなぎ合わせたマップの説明図である。
【符号の説明】
【０１００】
１０      自己位置推定システム
２０      移動車両
３０、３０１、３０２、３０３、・・・、３０ｎ−１、３０ｎ        ＩＡ（Ｉｎｆｏｒｍａｔｉｏｎ  Ａｓｓｉｓｔａｎｔ）タグ
２０２      車体部
２０４      車輪
２０６      レーザーレンジファインダー（ＬＲＦ：  Ｌａｓｅｒ  Ｒａｎｇｅ  Ｆｉｎｄｅｒ）
２０８      アンテナ
２１０      制御部
２１０ａ    中央処理装置（ＣＰＵ）
２１０ｂ    リード・オンリ・メモリ（ＲＯＭ）
２１０ｃ    ランダム・アクセス・メモリ（ＲＡＭ）
２１２      モーター
【図１】
2006343881.tif 000017
【図２】
2006343881.tif 000018
【図３】
2006343881.tif 000019
【図４】
2006343881.tif 000020
【図５】
2006343881.tif 000021
【図６】
2006343881.tif 000022
【図７】
2006343881.tif 000023
【図８】
2006343881.tif 000024