(57)【要約】【課題】ＧＰＳを使用できずしかも地図を入手し難い場所であったとしても、高精度で移動体の自己位置を特定することができ、例えば、ビルで火災が発生した場合に、災害地点までの高精度マップを作成可能な移動体の自己位置特定方法を提供する。【解決手段】自走可能な移動体１の移動開始地点の慣性座標系も位置を決定して、これを中心にした局所慣性座標を定義する。移動体１に搭載したＣＣＤカメラ３の画像から第１の目標Ｌ１を選定し、第１の目標Ｌ１までの距離をレーザ測距装置４で測定し且つ角度センサ６で第１の目標Ｌ１の方向を特定して、第１の目標Ｌ１の局所慣性座標を求めた後、第１の目標Ｌ１を基準にして移動体１を移動させ、適宜位置で且つ第１の目標を基準にして特定した最後の自己位置でＣＣＤカメラ３の画像から第２の目標Ｌ２を選定し、以降、移動体１を移動させる毎に逐次更新した新しい目標を基準にして移動体１の自己位置を求める。【選択図】図１
2006031642.tif 000002
【特許請求の範囲】
【請求項１】
自走可能な移動体に画像取得手段，測距手段及び各種角度センサを搭載し、この移動体が移動を開始する地点の慣性座標系における位置を決定して、この移動開始地点を中心にした局所慣性座標を定義し、上記移動体に搭載した画像取得手段の画像から第１の目標を選定するのに続いて、この第１の目標までの距離を上記測距手段にて測定すると共に各種角度センサで局所慣性座標系における第１の目標の方向を特定して、上記距離及び方向から第１の目標の局所慣性座標を求めた後、この第１の目標を基準にして自己位置を特定しつつ上記移動体を移動させ、次いで、第１の目標から適宜距離だけ離れた位置で且つ第１の目標を基準にして特定した最後の自己位置において、上記移動体に搭載した画像取得手段の画像から第２の目標を選定するのに続いて、上記測距手段にて測定した第２の目標までの距離及び各種角度センサで特定した局所慣性座標系における第２の目標の方向から第２の目標の局所慣性座標を求め、以降、上記移動体を移動させる毎に逐次更新した新しい目標を基準にして移動体の自己位置を求めることを特徴とする移動体の自己位置特定方法。
【請求項２】
自走可能な移動体に複数個の目標体，測距手段及び各種角度センサを搭載し、この移動体が移動を開始する地点の慣性座標系における位置を決定して、この移動開始地点を中心にした局所慣性座標を定義すると共に、この位置に複数個の目標体のうちの１つを配置して第１の目標体とした後、上記移動体を移動させ、次いで、適宜停止位置において第１の目標体までの距離を上記測距手段にて測定すると共に各種角度センサで局所慣性座標系における第１の目標体の方向を特定して、上記距離及び方向から自己位置を特定するのに続いて、この位置に上記複数個の目標体のうちの１つを配置して第２の目標体とした後、上記移動体を移動させ、以降、上記移動体を移動させて停止させる毎に後方に逐次更新した新しい目標体を基準にして移動体の自己位置を求めることを特徴とする移動体の自己位置特定方法。
【請求項３】
目標体を色及び／又は形状で認識可能なマーカーとした請求項２に記載の移動体の自己位置特定方法。
【請求項４】
目標体を蛍光体とした請求項２に記載の移動体の自己位置特定方法。
【請求項５】
目標体を発光体とした請求項２に記載の移動体の自己位置特定方法。
【請求項６】
測距手段をレーザ測距装置とし、目標体をコーナリフレクタとした請求項２に記載の移動体の自己位置特定方法。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、ＧＰＳを使用することができずしかも地図を入手し難い場所、例えば、ビルや地下で火災等の災害が発生した場合において、当該災害地点に向かう消防救急員の手助けとなるマップを作成する際に用いるのに好適な移動体の自己位置特定方法に関するものである。
【背景技術】
【０００２】
従来、上記したような移動体の自己位置特定方法としては、例えば、デッドレコニングと呼ばれる周知な方法があり、この方法では、移動体に搭載した各種センサを用いて周囲を確認すると共に、移動体に連結した小車輪の回転数をカウントして移動距離を認識し、あらかじめ記憶してある地図を参照することで累積誤差をなくしつつ自己位置を特定するようにしている。
【発明の開示】
【発明が解決しようとする課題】
【０００３】
ところが、上記したデッドレコニングにおいて、地図を参照することが不可欠である以上、ビルや地下等の地図を入手し難い場所で用いることができないのは言うまでもなく、例え、内部構造がそれ程複雑でなく、地図を参照しなくても自己位置を認識できそうな場合であったとしても、走行面の状況によっては、移動体に連結した小車輪がスリップして移動距離が大幅にずれたり、移動体に搭載した各種センサのうちのジャイロがドリフトして方位が狂ったりする可能性があることから、自己位置を高精度で特定することができないという問題があり、この問題を解決することが従来の課題となっていた。
【０００４】
本発明は、上記した従来の課題に着目してなされたもので、ＧＰＳを使用することができずしかも地図を入手し難い場所であったとしても、高精度で移動体の自己位置を特定することができ、その結果、例えば、ビルや地下で火災等の災害が発生した場合において、必要とされる災害地点までの高精度マップを作成することが可能になる移動体の自己位置特定方法を提供することを目的としている。
【課題を解決するための手段】
【０００５】
本発明の請求項１の移動体の自己位置特定方法は、自走可能な移動体に画像取得手段，測距手段及び各種角度センサを搭載し、この移動体が移動を開始する地点の慣性座標系における位置を決定して、この移動開始地点を中心にした局所慣性座標を定義し、上記移動体に搭載した画像取得手段の画像から第１の目標を選定するのに続いて、この第１の目標までの距離を上記測距手段にて測定すると共に各種角度センサで局所慣性座標系における第１の目標の方向を特定して、上記距離及び方向から第１の目標の局所慣性座標を求めた後、この第１の目標を基準にして自己位置を特定しつつ上記移動体を移動させ、次いで、第１の目標から適宜距離だけ離れた位置で且つ第１の目標を基準にして特定した最後の自己位置において、上記移動体に搭載した画像取得手段の画像から第２の目標を選定するのに続いて、上記測距手段にて測定した第２の目標までの距離及び各種角度センサで特定した局所慣性座標系における第２の目標の方向から第２の目標の局所慣性座標を求め、以降、上記移動体を移動させる毎に逐次更新した新しい目標を基準にして移動体の自己位置を求める構成としたことを特徴としており、この移動体の自己位置特定方法の構成を前述した従来の課題を解決するための手段としている。
【０００６】
本発明の請求項２の移動体の自己位置特定方法は、自走可能な移動体に複数個の目標体，測距手段及び各種角度センサを搭載し、この移動体が移動を開始する地点の慣性座標系における位置を決定して、この移動開始地点を中心にした局所慣性座標を定義すると共に、この位置に複数個の目標体のうちの１つを配置して第１の目標体とした後、上記移動体を移動させ、次いで、適宜停止位置において第１の目標体までの距離を上記測距手段にて測定すると共に各種角度センサで局所慣性座標系における第１の目標体の方向を特定して、上記距離及び方向から自己位置を特定するのに続いて、この位置に上記複数個の目標体のうちの１つを配置して第２の目標体とした後、上記移動体を移動させ、以降、上記移動体を移動させて停止させる毎に後方に逐次更新した新しい目標体を基準にして移動体の自己位置を求める構成としたことを特徴としており、この移動体の自己位置特定方法の構成を前述した従来の課題を解決するための手段としている。
【発明の効果】
【０００７】
本発明の移動体の自己位置特定方法では、上記した構成としているので、ＧＰＳを使用することができない場所で且つ地図を入手困難な場所においても、高い精度をもって移動体の自己位置を特定することができ、したがって、例えば、ビルや地下で火災等の災害が生じた場合には、自己位置を特定させつつ移動体を移動させることで、消防救急員が渇望する災害地点までの高精度な情報地図を作成することが可能になるという非常に優れた効果がもたらされる。
【発明を実施するための最良の形態】
【０００８】
本発明の請求項１の移動体の自己位置特定方法は、例えば、移動体をビルに入り込ませるに際して、目標として選定した植木鉢や置物や消火器などの既存の物品や構造物を基準にして自己位置を特定するので、いわばパッシブタイプの自己位置特定方法である。
【０００９】
この場合、移動体に搭載したＣＣＤカメラ等の画像取得手段の画像からの目標の選定は、画像をモニターする人が行ってもよいし、コンピューターに行わせてもよい。
【００１０】
一方、本発明の請求項２の移動体の自己位置特定方法は、例えば、移動体をビルに入り込ませるに際して、移動体に搭載した複数個の目標体を停止するごとに配置し、この後方の目標体を基準にして自己位置を特定するので、いわばアクティブタイプの自己位置特定方法である。この移動体の自己位置特定方法では、後方の目標体を基準にして自己位置を特定する都合上、移動体に前方監視カメラを搭載したり、偵察ロボットを移動体の前方に先行させたりする必要がある。
【００１１】
上記目標体には、色及び／又は形状で認識可能なマーカーを採用することができ、視界が確保できる場合に適している。また、上記目標体には、マーカーの他に蛍光体や発光体を用いることができ、これらは視界が確保できない場合や視界不良の場合に適している。さらに、測距手段をレーザ測距装置とした場合には、目標体として反射性に優れたコーナリフレクタを採用することが望ましい。
【００１２】
本発明の移動体の自己位置特定方法において、測距手段には、上記したレーザ測距装置の他に、目標又は目標体を２台のＣＣＤカメラで方向を変えて撮影するステレオ測距装置や、レーザ光をコーナリフレクタに照射してその反射光を２台のＣＣＤカメラで方向を変えて撮影するアクティブ型ステレオ測距装置を採用することができるが、３次元レーザ測距装置を用いることが望ましい。
【００１３】
また、本発明の移動体の自己位置特定方法において、各種角度センサとしては、レーザ測距装置を支持するパンチルトジンバル（２軸ジンバル）や、傾斜センサや、太陽センサが用いられる。
【実施例】
【００１４】
以下、本発明を図面に基づいて説明する。
【００１５】
図１及び図２は、本発明の移動体の自己位置特定方法の一実施例を説明する図であって、この実施例では、ＧＰＳを使用することができずしかも地図を入手し難いビルで火災が発生した際に、移動体をこのビルに入り込ませて、自己位置を特定させつつ火災発生地点まで移動体を移動させる場合を例に挙げて説明する。
【００１６】
図１に示すように、自走可能な移動体１は、車体２と、画像取得手段としてのＣＣＤカメラ３と、測距手段としてのレーザ測距装置４と、車体２に立設したシャフト５の上端に位置してレーザ測距装置４を左右方向及び上下方向に回動可能に支持する角度センサとしてのパンチルトジンバル（２軸ジンバル）６と、同じく角度センサとしての傾斜センサ及び太陽センサ（いずれも図示せず）を備えており、ＣＣＤカメラ３はレーザ測距装置４に一体的に設けてある。
【００１７】
上記した移動体１を火災が生じたビルに入り込ませて、火災発生地点まで向かわせる場合、まず、図２に示すように、例えばビルの入口を移動開始地点Ｐ０とし、この移動開始地点Ｐ０の慣性座標系における位置（Ｘ０，Ｙ０，Ｚ０）を決定して、この移動開始地点Ｐ０を中心にした局所慣性座標（Ｘｉ，Ｙｉ，Ｚｉ）を定義する。
【００１８】
次いで、移動体１に搭載したＣＣＤカメラ３の画像から、植木鉢や置物や消火器などの目標と成り得る物（図示例では置物）を第１の目標Ｌ１として選定するのに続いて、この第１の目標Ｌ１までの距離Ｒ１を上記レーザ測距装置４にて測定すると共に各種角度センサ６で局所慣性座標系における第１の目標Ｌ１の方向を特定して、上記距離Ｒ１及び方向から第１の目標Ｌ１の局所慣性座標（Ｘｌ１，Ｙｌ１，Ｚｌ１）を求める。上記ＣＣＤカメラ３の画像からの第１の目標Ｌ１の選定は、画像をモニターするオペレータが行ってもよいし、コンピューターに行わせてもよい。
【００１９】
この後、第１の目標Ｌ１を基準にして自己位置を特定しつつ上記移動体１が移動すると、任意位置Ｐにおいては、局所慣性座標（Ｘ，Ｙ，Ｚ）が求められるが、第１の目標Ｌ１を捉え難くなる可能性が出てきた場合は、第１の目標Ｌ１を基準にして特定した最後の自己位置Ｐ１において、移動体１に搭載したＣＣＤカメラ３の画像から、植木鉢や置物や消火器などの目標と成り得る物（図示省略）を第２の目標Ｌ２として選定するのに続いて、上記レーザ測距装置４にて測定した第２の目標Ｌ２までの距離Ｒ２及び各種角度センサ６で特定した局所慣性座標系における第２の目標Ｌ２の方向から第２の目標Ｌ２の局所慣性座標（Ｘｌ２，Ｙｌ２，Ｚｌ２）を求め、この後、上記移動体１は第２の目標Ｌ２を基準にして自己位置を特定しつつ移動する。
【００２０】
以降、移動体１は、上記したようにして逐次更新した新しい目標Ｌを基準にして、常に自己位置を把握しつつ移動し、この移動体１が移動することで得た位置情報を収集すれば、ビル外部において火災地点までの高精度な情報地図が作成されることとなる。
【００２１】
ここで、図３に示すように、移動体１には車両座標系が用いられ、移動体１に搭載されるレーザ測距装置４や各種角度センサ６にはセンサ座標系が用いられ、このセンサ座標系のＸｓ軸方向に照射されたレーザ光により測定される第１の目標Ｌ１までの距離をＲ１とすると共に、車両座標系から局所慣性座標系への座標変換マトリックスをＣｖｉ、センサ座標系から車両座標系への座標変換マトリックスをＣｓｖとすると、第１の目標Ｌ１の局所慣性座標（Ｘｌ１，Ｙｌ１，Ｚｌ１）は、式１により得ることができる。なお、これらの座標変換マトリックスをＣｖｉ，Ｃｓｖを計算するために必要なセンサの角度は、表１のように計測される。
【００２２】
【数１】
2006031642.tif 000003
【００２３】
【表１】
2006031642.tif 000004
【００２４】
また、第１の目標Ｌ１を基準にして移動する移動体１の任意位置Ｐにおける局所慣性座標（Ｘ，Ｙ，Ｚ）は、式２により得ることができる。
【００２５】
【数２】
2006031642.tif 000005
但し、Ｒは、移動体１の任意位置Ｐから第１の目標Ｌ１までの距離であり、角度パラメータは任意位置Ｐで計測した値を使用する。
【００２６】
そして、ＣＣＤカメラ３の画像から第２の目標Ｌ２として選定する位置、すなわち、第１の目標Ｌ１を基準にして特定した最後の自己位置Ｐ１における局所慣性座標（Ｘ１，Ｙ１，Ｚ１）は、式３により得ることができる。
【００２７】
【数３】
2006031642.tif 000006
但し、Ｒｌ１は、移動体１の任意位置Ｐ１から第１の目標Ｌ１までの距離であり、第２の目標Ｌ２の局所慣性座標（Ｘｌ２，Ｙｌ２，Ｚｌ２）は、式４により得ることができる。
【００２８】
【数４】
2006031642.tif 000007
但し、Ｒ２は、移動体１の任意位置Ｐ１から更新した新しい第２の目標Ｌ２までの距離である。
【００２９】
上記と同様に、第２の目標Ｌ２を基準にして移動する移動体１の任意位置Ｐにおける局所慣性座標（Ｘ，Ｙ，Ｚ）は、式５により得ることができる。
【００３０】
【数５】
2006031642.tif 000008
但し、Ｒは、移動体１の任意位置Ｐから第２の目標Ｌ２までの距離である。
【００３１】
この式５は、さらに、以下の式６となる。
【００３２】
【数６】
2006031642.tif 000009
【００３３】
そして、ｋ番目の目標を基準にして移動する移動体１の任意位置Ｐにおける局所慣性座標（Ｘ，Ｙ，Ｚ）は、式７により得ることができる。
【００３４】
【数７】
2006031642.tif 000010
【００３５】
つまり、上記式７によって、移動体１の任意位置Ｐにおける自己位置を特定することができる。
【００３６】
図４は、本発明の移動体の自己位置特定方法の他の実施例を説明する図であって、図４に示すように、自走可能な移動体２１は、車体２２と、前方監視カメラ２３と、測距手段としてのレーザ測距装置２４と、各種角度センサ（図示せず）を備えており、車体２２は、複数個の目標体を搭載している。
【００３７】
この場合、測距手段をレーザ測距装置２４としている都合上、目標体に反射性に優れたコーナリフレクタ２９を採用している。なお、視界が確保できる場合には、目標体として色及び／又は形状で認識可能なマーカーを採用することができ、また、視界が確保できない場合や視界不良の場合には、目標体として蛍光体や発光体を用いることができる。
【００３８】
上記した移動体２１を火災が生じたビルに入り込ませて、火災発生地点まで向かわせる場合、まず、この移動体２１が移動を開始する地点の慣性座標系における位置を決定して、この移動開始地点を中心にした局所慣性座標を定義する。
【００３９】
次いで、この位置に複数個のコーナリフレクタ２９のうちの１つを配置して第１のコーナリフレクタ（第１の目標体）２９とした後、上記移動体２１を移動させる。
【００４０】
そして、適宜停止位置において、第１のコーナリフレクタ２９までの距離を上記レーザ測距装置２４にて測定すると共に各種角度センサで局所慣性座標系における第１のコーナリフレクタ２９の方向を特定して、上記距離及び方向から自己位置を特定するのに続いて、この位置に上記複数個のコーナリフレクタ２９のうちの１つを配置して第２のコーナリフレクタ（第２の目標体）２９とした後、移動体２１を再び移動させる。
【００４１】
以降、移動体２１を移動させて停止させる毎に、後方に逐次更新した新しいコーナリフレクタ２９を基準にして自己位置を把握し、この移動体１を移動停止させることで得た位置情報を収集すれば、ビル外部において火災地点までの高精度な情報地図が作成されることとなる。
【産業上の利用可能性】
【００４２】
本発明の移動体の自己位置特定方法は、ＧＰＳを使用することができずしかも地図を入手し難いビルにおいて、火災が発生した場合の消防救急活動に限らず、テロ集団等に占拠された場合の人質救出活動にも適用可能であり、その際、移動体に、情報収集カメラや集音マイクを搭載することが可能である。
【００４３】
また、上記した実施例では、移動体が２次元的に移動する場合を示したが、移動体が３次元的に移動する場合も同様に自己位置を把握することができるのは言うまでもない。
【００４４】
さらに、本発明の移動体の自己位置特定方法において、移動体の移動に際して、その地点までの経路情報を記憶させたＲＦＩＤ（Ｒａｄｉｏ  Ｆｒｅｑｕｅｎｃｙ  Ｉｄｅｔｉｆｉｃａｔｉｏｎ）タグを必要に応じて配置するように成すことも可能である。
【図面の簡単な説明】
【００４５】
【図１】本発明の一実施例による自己位置特定方法に用いる移動体が自己位置を特定している状況を簡略的に示す斜視説明図である。（実施例１）
【図２】図１における移動体の自己位置特定方法の概念説明図である。
【図３】本発明の移動体の自己位置特定方法の座標系を説明する図である。
【図４】本発明の他の実施例による自己位置特定方法に用いる移動体が自己位置を特定している状況を簡略的に示す斜視説明図である。（実施例２）
【符号の説明】
【００４６】
１，２１  移動体
３        ＣＣＤカメラ（画像取得手段）
４，２４  レーザ測距装置（測距手段）
６        パンチルトジンバル（角度センサ）
２９      コーナリフレクタ（第１の目標体，第２の目標体，…）
Ｌ１      第１の目標
Ｌ２      第２の目標
【図１】
2006031642.tif 000011
【図２】
2006031642.tif 000012
【図３】
2006031642.tif 000013
【図４】
2006031642.tif 000014