JP2021163277A - 位置検出システム - Google Patents

Abstract

【課題】自律移動体の行動を制約することなく自律移動体の位置を検出する。【解決手段】自律移動体の現在位置を検出する位置検出システム１００は、自律移動体の外部に設けられ自律移動体の位置を検出する外部位置検出部２２１と、自律移動体に搭載され自律航法により自律移動体の位置を検出する自己位置検出部１２１と、外部位置検出部２２１により検出された自律移動体の位置に基づいて自己位置検出部１２１により検出された自律移動体の位置を補正することで自律移動体の現在位置を算出する現在位置算出部１２３とを備える。【選択図】図３

Description

本発明は、自律移動体の現在位置を検出する位置検出システムに関する。

この種の装置として、従来、自律航法により算出した現在位置を校正するようにした装置が知られている（例えば、特許文献１参照）。特許文献１記載の装置では、予め定められた位置に設置された光ビーコンの光信号を受光する位置で移動ロボットを停止させ、壁面からの反射光の入射角に基づいて光ビーコンから移動ロボットまでの離間距離を検出することで、移動ロボット自身が検出する現在位置を較正する。

特開２００７-１０１４９２号公報

しかしながら上記特許文献１記載の装置では、現在位置を較正するために移動ロボットを停止させる必要があるため、移動ロボットの行動に対する制約が大きい。

本発明の一態様は、自律移動体の現在位置を検出する位置検出システムであって、自律移動体の外部に設けられ、自律移動体の位置を検出する第１位置検出部と、自律移動体に搭載され、自律航法により自律移動体の位置を検出する第２位置検出部と、第１位置検出部により検出された自律移動体の位置に基づいて第２位置検出部により検出された自律移動体の位置を補正することで、自律移動体の現在位置を算出する現在位置算出部と、を備える。

本発明によれば、自律移動体の行動を制約することなく自律移動体の位置を検出することができる。

本発明の実施形態に係る位置検出システムが適用される自律移動体の一例を模式的に示す概略図。 本発明の実施形態に係る位置検出システムの一例を模式的に示す概略図。 本発明の実施形態に係る位置検出システムの全体構成を示すブロック図。 図１のＡＧＶの進行方向について説明するための図。 図１のＡＧＶの回転角度について説明するための図。 図１のＡＧＶの進行方向を示すセンサ出力値の一例を示す図。 図３のスケール変換部により変換された不連続データの一例を示す図。 図３の連続／不連続変換部により変換された連続データの一例を示す図。 図３のノイズ除去部によりノイズを除去された連続データの一例を示す図。 図３の三角関数変換部により変換された連続データの一例を示す図。 図３の連続／不連続変換部により変換された不連続データの一例を示す図。 図３のスケール変換部により変換された、センサ出力値に相当する不連続データの一例を示す図。 ローパスフィルタ処理による誤差の一例を示す図。 本発明の実施形態に係る位置検出システムにより実行されるノイズ除去処理の一例を示すフローチャート。 図３の現在位置算出部による重み付けについて説明するための図。 図３の現在位置算出部による外部位置の補正について説明するための図。 本発明の実施形態に係る位置検出システムにより実行される現在位置算出処理の一例を示すフローチャート。

以下、図１〜図１６を参照して本発明の実施形態について説明する。図１は、本発明の実施形態に係る位置検出システムが適用される自律移動体の一例を模式的に示す概略図であり、工場内を自律移動する無人搬送車（Automated Guided Vehicle。以下、ＡＧＶ）１を示す。図１に示すように、ＡＧＶ１は、工場内を予め定められた経路ＲＴに沿って自律移動する。

ＡＧＶ１には、走行用モータや車輪の回転数を検出する回転センサおよびヨー方向の角速度を検出するジャイロセンサなどのセンサ群１ａと、ＡＧＶ１の動作を制御するコントローラ１０とが搭載される。コントローラ１０は、センサ群１ａからの信号に基づいてＡＧＶ１の現在位置を検出し、検出された現在位置に基づいて経路ＲＴ上を自律走行するようにＡＧＶ１の動作を制御する。

ＡＧＶ１の現在位置は、例えば、走行用モータや車輪の回転数に基づくＡＧＶ１の移動距離と、ヨー方向の角速度の積分値に基づくＡＧＶ１の進行方向とに基づいて、自律航法により検出される。一般的な精度のセンサ群１ａを用いる場合、ＡＧＶ１による現在位置の検出精度は、±１０ｍｍ程度となる。

図１に示すように、経路ＲＴ上には、ＡＧＶ１と移載装置との間で搬送品の受け渡しが行われ、ＡＧＶ１の通過位置や停止位置が厳密に管理される管理エリアＡＲ１と、それ以外の非管理エリアＡＲ２とが定められる。管理エリアＡＲ１におけるＡＧＶ１の現在位置に対しては、±１ｍｍ程度の検出精度が要求される。

そこで、本実施形態では、ＡＧＶ１の自律走行を制限することなく、簡易な構成で、管理エリアＡＲ１におけるＡＧＶ１の現在位置を精度よく検出できるよう、以下のように位置検出システムを構成する。

図２は、本発明の実施形態に係る位置検出システム１００の一例を模式的に示す概略図である。図２に示すように、位置検出システム１００では、管理エリアＡＲ１に複数台のカメラ２が設置され、モーションキャプチャにより±１ｍｍ程度の検出精度でＡＧＶ１の位置が外部から検出される。

図３は、本発明の実施形態に係る位置検出システム１００の全体構成を示すブロック図である。図３に示すように、位置検出システム１００は、ＡＧＶ１に搭載されたコントローラ１０と、モーションキャプチャ用のコントローラ２０とを有する。コントローラ１０にはセンサ群１ａが接続され、コントローラ２０には複数台のカメラ２が接続される。コントローラ１０とコントローラ２０とは、工場内の無線ネットワーク３を介して通信可能に構成される。

ＡＧＶ１に搭載されたコントローラ１０は、通信部１１、ＣＰＵ１２、ＲＯＭ，ＲＡＭなどのメモリ１３、およびＩ／Ｏその他の周辺回路などを有するコンピュータを含んで構成される。ＣＰＵ１２は、センサ信号処理部１２０と、自己位置検出部１２１と、速度検出部１２２と、現在位置算出部１２３として機能する。

モーションキャプチャ用のコントローラ２０は、通信部２１、ＣＰＵ２２、ＲＯＭ，ＲＡＭなどのメモリ２３、およびＩ／Ｏその他の周辺回路などを有するコンピュータを含んで構成される。ＣＰＵ２２は、エリア判定部２２０と、外部位置検出部２２１として機能する。

センサ信号処理部１２０は、機能的構成として、スケール変換部１２４、連続／不連続変換部１２５と、ノイズ除去部１２６と、三角関数変換部１２７とを有し、センサ群１ａからの信号を処理する。具体的には、センサ信号処理部１２０は、ＡＧＶ１の進行方向Ａの連続的な変化に伴い周期的に増加する、進行方向Ａと基準方向Ｒとのなす角度αを示すセンサ出力値の不連続データからノイズを除去する。

図４Ａは、ＡＧＶ１の進行方向Ａについて説明するための図であり、予め定められたＸＹ座標空間におけるＡＧＶ１の絶対的な進行方向Ａを示す。図４Ａに示すように、進行方向Ａは、進行方向Ａと、予め定められた基準方向Ｒ（例えば、東向き）とのなす角度αで表される。北向きの進行方向Ａと基準方向Ｒとのなす角度αは、０°〜１８０°で表され、南向きの進行方向Ａと基準方向Ｒとのなす角度αは、０°〜−１８０°で表される。基準方向Ｒと逆向きの進行方向Ａと、基準方向Ｒとのなす角度αは、１８０°および−１８０°として２通りに表される。

図４Ｂは、ＡＧＶ１の回転角度θについて説明するための図である。図４Ｂに示すように、回転角度θは、基準移動方向Ａ０（例えば、ＡＧＶ１の起動時の進行方向Ａ）に対するＡＧＶ１の相対的な回転角度である。例えば、基準移動方向Ａ０から左回りに１回転半したときの回転角度θは、５４０°（１×３６０°＋１８０°）として表される。

図５は、ＡＧＶ１の進行方向Ａを示すセンサ出力値の一例を示す図であり、ＡＧＶ１を西向きの基準移動方向Ａ０から左回りに連続的に複数回、回転させたときの、進行方向Ａと、東向きの基準方向Ｒとのなす角度αを示すセンサ出力値の変化を示す。

図５に示すように、ＡＧＶ１が１回転して回転角度θが０°から３６０°まで連続的に増加すると、角度αのセンサ出力値が−１８０°から１８０°まで連続的に増加する。さらにＡＧＶ１が１回転して回転角度θが３６０°から７２０°まで連続的に増加すると、角度αのセンサ出力値が再び−１８０°から１８０°まで連続的に増加する。

このように、角度αのセンサ出力値は、回転角度θの連続的な変化に伴い周期的に増加するため、ＡＧＶ１が１回転して回転角度θが連続的に３６０°変化する度に１８０°および−１８０°の２値間で不連続となる。センサ信号処理部１２０は、このような角度αのセンサ出力値の不連続データからノイズを除去する。

スケール変換部１２４は、不連続データのデータ範囲のスケール変換を行う。具体的には、図５に示す角度αのセンサ出力値の−１８０°から１８０°までのデータ範囲が−πからπとなるように、角度αのセンサ出力値の不連続データに所定値ａ（＝π／１８０°）を乗算する。図６は、スケール変換部１２４により所定値ａを乗算してスケール変換された不連続データａαを示す。

連続／不連続変換部１２５は、スケール変換部１２４により所定値ａを乗算してスケール変換された不連続データを正弦関数および余弦関数により連続データに変換する。具体的には、図６に示すような不連続データａαを、図７に示すようなｓｉｎａαおよびｃｏｓａαで表される連続データに変換する。

ノイズ除去部１２６は、連続／不連続変換部１２５により変換された連続データｓｉｎａα，ｃｏｓａαからノイズを除去する。例えば、カットオフ周波数をωとするローパスフィルタを、サンプル時間をΔｔ（＝ｔ_n−ｔ_n-1）として双一次変換により離散化した下式(i)を用いて、ノイズを除去する。図８は、ノイズ除去部１２６によりノイズを除去された連続データｓｉｎａα（実線）、ｃｏｓａα（破線）を示す。

但し、 ｙ＝ｓｉｎｘ
ｘ＝ａα

三角関数変換部１２７は、ノイズ除去部１２６によりノイズを除去された連続データｓｉｎａα，ｃｏｓａαを正接関数により変換する。具体的には、図８に示すような連続データｓｉｎａα，ｃｏｓａαを、図９に示すような連続データｔａｎａα（＝ｓｉｎａα／ｃｏｓａα）に変換する。

連続／不連続変換部１２５は、三角関数変換部１２７により変換された連続データｔａｎａαを、逆正接関数により、最小値から最大値までのデータ範囲が−πからπまでの不連続データａαに変換する。図１０は、連続／不連続変換部１２５により変換された不連続データａαを示す。

スケール変換部１２４は、連続／不連続変換部１２５により変換された不連続データａαを所定値ａで除算して、最小値から最大値までのデータ範囲が−１８０°から１８０°までの不連続データαにスケール変換する。図１１は、スケール変換部１２４により所定値ａを除算してスケール変換された、角度αのセンサ出力値に相当する不連続データを実線で示す。

図５に示すような不連続データから直接、ローパスフィルタ処理などによりノイズを除去すると、図１１に破線で示すように、センサ出力値が不連続となる回転角度θ（３６０°，７２０°．１０８０°）において処理結果がセンサ出力値から乖離する。これは、式(i)のようなローパスフィルタ処理では、センサ出力値の前回値ｙ_n-1が今回値ｙ_nに反映されることに起因する。

連続／不連続変換部１２５により不連続データを連続データに変換してからノイズ除去部１２６によりノイズを除去することで、図１１に実線で示すように、センサ出力値から乖離することなく、不連続データからノイズを除去することができる。

図１２は、ローパスフィルタ処理による誤差の一例を示す図であり、センサ信号処理部１２０により連続データに変換してからノイズを除去した場合の誤差を実線で示し、不連続データから直接、ノイズを除去した場合の誤差を破線で示す。図１２に示すように、不連続データから直接、ノイズを除去した場合には、センサ出力値が不連続となる回転角度θにおいて１８０°程度の誤差が生じ、誤差のオーバシュートが生じる。一方、センサ信号処理部１２０により連続データに変換してからノイズを除去した場合には、センサ出力値の前回値ｙ_n-1を使用するローパスフィルタ処理に起因する１５°程度の誤差が安定して生じるのみで、誤差のオーバシュートは生じない。

図１３は、本発明の実施形態に係る位置検出システム１００により実行されるノイズ除去処理の一例を示すフローチャートであり、予めメモリ１３に記憶されたプログラムに従い、コントローラ１０のＣＰＵ１２で実行される処理の一例を示す。図１３のフローチャートに示す処理は、例えば、ＡＧＶ１およびコントローラ１０が起動されると開始される。

図１３に示すように、先ずステップＳ１で、センサ群１ａからのセンサ出力値αを読み込む。次いでステップＳ２で、ステップＳ１で読み込んだセンサ出力値αに所定値ａを乗算してスケール変換する。次いでステップＳ３で、ステップＳ２でスケール変換されたセンサ出力値ａαを正弦関数および余弦関数により連続データｓｉｎａα，ｃｏｓａαに変換する。次いでステップＳ４で、ステップＳ３で変換された連続データｓｉｎａα，ｃｏｓａαからローパスフィルタ処理によりノイズを除去する。

次いでステップＳ５で、ステップＳ４でノイズを除去された連続データｓｉｎａα，ｃｏｓａαを正接関数により連続データｔａｎａαに変換する。次いでステップＳ６で、ステップＳ５で変換された連続データｔａｎａαを逆正接関数によりセンサ出力値ａαに変換する。次いでステップＳ７で、ステップＳ６で変換されたセンサ出力値ａαを所定値ａで除算してスケール変換する。

このように、センサ出力値αの不連続データを連続データｓｉｎａα，ｃｏｓａαに変換してからノイズを除去することで、センサ出力値αが不連続となる回転角度θ付近でもオーバシュートを生じることなく、ローパスフィルタ処理を適用することができる（ステップＳ１〜Ｓ７）。

図３のコントローラ１０の自己位置検出部１２１および速度検出部１２２は、センサ信号処理部１２０によりノイズを除去されたセンサ群１ａのセンサ出力値に基づいてＡＧＶ１の位置（以下、自己位置）（Ｘ１，Ｙ１）および移動速度Ｖを検出する。例えば、走行用モータや車輪の回転数に基づくＡＧＶ１の移動距離と、ヨー方向の角速度の積分値に基づくＡＧＶ１の進行方向Ａとに基づいて、自律航法により自己位置（Ｘ１，Ｙ１）を検出する。また、走行用モータや車輪の回転数に基づいて移動速度Ｖを検出する。

自己位置検出部１２１により検出された自己位置（Ｘ１，Ｙ１）の情報は、通信部１１および無線ネットワーク３を介してコントローラ２０に送信されるとともに、検出時刻Ｔに関連付けてメモリ１３に記憶される。

コントローラ２０のエリア判定部２２０は、無線ネットワーク３および通信部２１を介してコントローラ１０から取得された自己位置（Ｘ１，Ｙ１）が、図１に示す管理エリアＡＲ１内であるか否かを判定する。

コントローラ２０の外部位置検出部２２１は、エリア判定部２２０により自己位置（Ｘ１，Ｙ１）が管理エリアＡＲ１内であると判定されると、カメラ２からの画像信号に基づいて、モーションキャプチャによりＡＧＶ１の位置（以下、外部位置）（Ｘ２，Ｙ２）を検出する。外部位置検出部２２１により検出された外部位置（Ｘ２，Ｙ２）の情報は、検出時刻Ｔの情報とともに、通信部２１および無線ネットワーク３を介してコントローラ１０に送信される。

現在位置算出部１２３は、無線ネットワーク３および通信部１１を介してコントローラ２０から取得された外部位置（Ｘ２，Ｙ２）に基づいて、自己位置検出部１２１により検出された自己位置（Ｘ１，Ｙ１）を補正する。すなわち、ＡＧＶ１側で自律航法により検出された自己位置（Ｘ１，Ｙ１）を、ＡＧＶ１の外部からモーションキャプチャにより検出された高精度な外部位置（Ｘ２，Ｙ２）に基づいて補正することで、ＡＧＶ１の現在位置（Ｘ，Ｙ）を算出する。

具体的には、現在位置算出部１２３は、速度検出部１２２により検出されたＡＧＶ１の移動速度Ｖに基づいて、自己位置（Ｘ１，Ｙ１）および外部位置（Ｘ２，Ｙ２）に重み付けし、重み付けされた位置に基づいて、ＡＧＶ１の現在位置（Ｘ，Ｙ）を算出する。より具体的には、重み値ｋを用いて下式(ii)，(iii)により現在位置（Ｘ，Ｙ）を算出する。
Ｘ＝（１−ｋ）Ｘ１＋ｋＸ２ ・・・(ii)
Ｙ＝（１−ｋ）Ｙ１＋ｋＹ２ ・・・(iii)

図１４は、現在位置算出部１２３による重み付けについて説明するための図であり、重み値ｋの一例を示す。図１４に示すように、ＡＧＶ１が停止または閾値ＶＬ（例えば、０．１ｍ／秒）以下の低速で移動している場合、重み値ｋは“１”に設定され、外部位置（Ｘ２，Ｙ２）が現在位置（Ｘ，Ｙ）としてそのまま採用される。一方、ＡＧＶ１が閾値ＶＨ（例えば、０．５ｍ／秒）以上の高速で移動している場合、重み値ｋは“０”に設定され、自己位置（Ｘ１，Ｙ１）が現在位置（Ｘ，Ｙ）としてそのまま採用される。ＡＧＶ１の移動速度Ｖが閾値ＶＬ〜ＶＨの範囲内の場合、重み値ｋは下式(iv)により設定される。
ｋ＝１−（Ｖ−ＶＬ）／（ＶＨ−ＶＬ） ・・・(iv)

外部位置（Ｘ２，Ｙ２）は、自己位置（Ｘ１，Ｙ１）よりも高精度で検出されるが、無線ネットワーク３を介して情報を送受信するときに例えば、０．３秒程度の遅延が発生するため、ＡＧＶ１の移動速度Ｖが高速になるほど実際の現在位置と一致しなくなる。この点を考慮して、現在位置算出部１２３は、無線ネットワーク３を介して取得された外部位置（Ｘ２，Ｙ２）を自律航法により補正する。

図１５は、外部位置（Ｘ２，Ｙ２）の自律航法による補正について説明するための図であり、時刻Ｔ４に取得された、時刻Ｔ２に検出された外部位置（Ｘ２（Ｔ２），Ｙ２（Ｔ２））を自律航法により補正する手順について説明する。図１５に示すように、現在位置算出部１２３は、先ず、取得された最新の外部位置（Ｘ２（Ｔ２），Ｙ２（Ｔ２））が検出された時刻Ｔ２から現在時刻Ｔ４までのＡＧＶ１の移動量Ｂ（Ｘｂ，Ｙｂ）を算出する。すなわち、時刻Ｔ２に検出された自己位置（Ｘ１（Ｔ２），Ｙ１（Ｔ２））と、時刻Ｔ４に検出された自己位置（Ｘ１（Ｔ４），Ｙ１（Ｔ４））とを用いて、下式(v)，(vi)により移動量Ｂ（Ｘｂ，Ｙｂ）を算出する。
Ｘｂ＝Ｘ１（Ｔ４）−Ｘ１（Ｔ２） ・・・(v)
Ｙｂ＝Ｙ１（Ｔ４）−Ｙ１（Ｔ２） ・・・(vi)

現在位置算出部１２３は、時刻Ｔ４に取得された、時刻Ｔ２に検出された外部位置（Ｘ２（Ｔ２），Ｙ２（Ｔ２））に、自律航法による移動量Ｂ（Ｘｂ，Ｙｂ）を加算して補正し、時刻Ｔ４における外部位置（Ｘ２（Ｔ４），Ｙ２（Ｔ４））とする。すなわち、下式(vii)，(viii)により時刻Ｔ４における外部位置（Ｘ２（Ｔ４），Ｙ２（Ｔ４））を算出する。
Ｘ２（Ｔ４）＝Ｘ２（Ｔ２）−Ｘｂ ・・・(vii)
Ｙ２（Ｔ４）＝Ｙ２（Ｔ２）−Ｙｂ ・・・(viii)

図１６は、本発明の実施形態に係る位置検出システム１００により実行される位置検出処理の一例を示すフローチャートであり、予めメモリ１３，２３に記憶されたプログラムに従い、コントローラ１０，２０のＣＰＵ１２，２２で実行される処理の一例を示す。図１６のフローチャートに示す処理は、例えば、ＡＧＶ１およびコントローラ１０，２０が起動されると開始される。

図１６に示すように、先ずステップＳ１０で、ノイズを除去されたセンサ群１ａのセンサ出力値を読み込む。次いでステップＳ１１で、ステップＳ１０で読み込んだセンサ出力値に基づいて自己位置（Ｘ１，Ｙ１）および移動速度Ｖを検出する。次いでステップＳ１２で、自己位置（Ｘ１，Ｙ１）が管理エリアＡＲ１内であるか否かを判定する。ステップＳ１２で肯定されるとステップＳ１３に進み、否定されると処理を終了する。

ステップＳ１３では、ＡＧＶ１側で取得された最新の外部位置（Ｘ２，Ｙ２）を読み込む。次いでステップＳ１４で、ステップＳ１３で読み込んだ外部位置（Ｘ２，Ｙ２）の検出時刻Ｔｐから現在時刻Ｔｃまでの移動量Ｂ（Ｘｂ，Ｙｂ）を自律航法により算出する。次いでステップＳ１５で、ステップＳ１４で算出された移動量Ｂ（Ｘｂ，Ｙｂ）により外部位置（Ｘ２，Ｙ２）を補正する。

次いでステップＳ１６で、ステップＳ１１で検出された移動速度Ｖに基づいて重み値ｋを算出する。次いでステップＳ１７で、ステップＳ１１で検出された自己位置（Ｘ１，Ｙ１）、ステップＳ１５で補正された外部位置（Ｘ２，Ｙ２）およびステップＳ１６で検出された重み値ｋに基づいて現在位置（Ｘ，Ｙ）を算出する。

このように、移動速度Ｖを考慮して、自律航法による自己位置をモーションキャプチャによる高精度な外部位置に基づいて補正することで、ＡＧＶ１の行動を制約することなく、ＡＧＶ１の位置を精度よく検出することができる（ステップＳ１０〜Ｓ１７）。また、無線ネットワーク３を介して取得した外部位置を自律航法により補正することで、ＡＧＶ１の位置を一層精度よく検出することができる（ステップＳ１４〜Ｓ１５）。なお、自律航法による外部位置の補正を行わず、ステップＳ１０〜Ｓ１３，Ｓ１６〜Ｓ１７のみを実行してもよい。

本発明の実施形態によれば以下のような作用効果を奏することができる。
（１）位置検出システム１００は、ＡＧＶ１の現在位置（Ｘ，Ｙ）を検出する。位置検出システム１００は、ＡＧＶ１の外部に設けられ、ＡＧＶ１の位置（Ｘ２，Ｙ２）を検出する外部位置検出部２２１（カメラ２および外部位置検出部２２１）と、ＡＧＶ１に搭載され、自律航法によりＡＧＶ１の位置（Ｘ１，Ｙ１）を検出する自己位置検出部１２１（センサ群１ａおよび自己位置検出部１２１）と、外部位置検出部２２１により検出されたＡＧＶ１の位置（Ｘ２，Ｙ２）に基づいて自己位置検出部１２１により検出されたＡＧＶ１の位置（Ｘ１，Ｙ１）を補正することで、ＡＧＶ１の現在位置（Ｘ，Ｙ）を算出する現在位置算出部１２３とを備える（図３）。

すなわち、ＡＧＶ１側で自律航法により検出された位置を、ＡＧＶ１の外部からモーションキャプチャにより検出された高精度な位置に基づいて補正することで、必要に応じて位置検出の精度を向上することができるため、簡易な構成でも精度よくＡＧＶ１の現在位置を検出することができる。

（２）現在位置算出部１２３は、ＡＧＶ１に設けられる（図３）。すなわち、ＡＧＶ１側で自律航法により検出された位置を、ＡＧＶ１の外部からモーションキャプチャにより検出された高精度な位置に基づいて補正するため、ＡＧＶ１の構成を簡易にすることができる。

（３）位置検出システム１００は、ＡＧＶ１の移動速度Ｖを検出する速度検出部１２２（センサ群１ａおよび速度検出部１２２）をさらに備える（図３）。現在位置算出部１２３は、速度検出部１２２により検出されたＡＧＶ１の移動速度Ｖに基づいて外部位置検出部２２１および自己位置検出部１２１によりそれぞれ検出されたＡＧＶ１の位置（Ｘ２，Ｙ２），（Ｘ１，Ｙ１）に重み付けし、重み付けされた位置（ｋＸ２，ｋＹ２），（（１−ｋ）Ｘ１，（１−ｋ）Ｙ１）に基づいて、ＡＧＶ１の現在位置（Ｘ，Ｙ）を算出する。これにより、ＡＧＶ１の移動速度Ｖに応じて精度よく現在位置を算出することができる。

（４）現在位置算出部１２３は、時刻Ｔｐに自己位置検出部１２１により検出されたＡＧＶ１の位置（Ｘ１（Ｔｐ），Ｙ１（Ｔｐ））に対する、時刻Ｔｐより後の現在時刻Ｔｃに自己位置検出部１２１により検出されたＡＧＶ１の位置（Ｘ１（Ｔｃ），Ｙ１（Ｔｃ））の相対位置（Ｘ１（Ｔｃ）−Ｘ１（Ｔｐ），Ｙ１（Ｔｃ）−Ｙ１（Ｔｐ））に、時刻Ｔｐに外部位置検出部２２１により検出されたＡＧＶ１の位置（Ｘ２（Ｔｐ），Ｙ２（Ｔｐ））を加算することで、時刻ＴｃにおけるＡＧＶ１の現在位置（Ｘ（Ｔｃ），Ｙ（Ｔｃ））を算出する。これにより、最新の高精度な位置情報を、さらに自律航法により補正することができる。

（５）ＡＧＶ１は、現在位置算出部１２３によるＡＧＶ１の現在位置（Ｘ，Ｙ）の算出が行われる管理エリアＡＲ１と、現在位置算出部１２３によるＡＧＶ１の現在位置（Ｘ，Ｙ）の算出が行われない非管理エリアＡＲ２とを移動する（図１、図２）。ＡＧＶ１の通過位置や停止位置が厳密に管理される管理エリアＡＲ１のみにモーションキャプチャ用のカメラ２などの高精度な外部位置センサが設置されるため、位置検出システム１００全体の構成を簡易にすることができる。

（６）ＡＧＶ１の回転角度θの連続的な変化に伴い周期的に増加するＡＧＶ１の進行方向Ａを示すセンサ出力値αの不連続データからノイズを除去するＣＰＵ１２は、不連続データの最小値から最大値までの範囲が２πとなるように、不連続データに所定値ａを乗算するスケール変換部１２４と、スケール変換部１２４により所定値ａを乗算された不連続データを正弦関数および余弦関数により連続データに変換する連続／不連続変換部１２５と、連続／不連続変換部１２５により変換された連続データからノイズを除去するノイズ除去部１２６と、ノイズ除去部１２６によりノイズを除去された連続データを正接関数により変換する三角関数変換部１２７とを備える（図３）。

連続／不連続変換部１２５は、三角関数変換部１２７により変換された連続データを、逆正接関数により、最小値から最大値までの範囲が２πの不連続データに変換する。スケール変換部１２４は、連続／不連続変換部１２５により変換された不連続データを所定値ａで除算する。

すなわち、不連続データを連続データに変換してからノイズを除去するため、状態の連続的な変化に伴い周期的に増加する状態量を示すセンサ出力値の不連続データであっても、オーバシュートを生じることなくノイズを除去することができる。また、三角関数などの汎用的で簡易な演算により不連続データから連続データへの変換を行うため、ＣＰＵ１２として、比較的、処理能力の低いＣＰＵを用いることができる。

（７）センサ出力値は、予め定められた基準移動方向Ａ０に対するＡＧＶ１の相対的な回転角度θであり、状態量は、予め定められたＸＹ座標空間におけるＡＧＶ１の絶対的な進行方向Ａを示すセンサ出力値αである。例えば、連続的な時間経過に伴い周期的に増加する時刻や、周回コースの連続走行に伴い周期的に増加するコース上のスタート地点からの距離などの不連続データについても、同様の手法によりオーバシュートを生じることなくノイズを除去することができる。

（８）ＡＧＶ１の回転角度θの連続的な変化に伴い周期的に増加するＡＧＶ１の進行方向Ａを示すセンサ出力値αの不連続データからノイズを除去するノイズ除去方法は、不連続データの最小値から最大値までの範囲が２πとなるように、不連続データに所定値ａを乗算するスケール変換ステップＳ２と、スケール変換ステップＳ２で所定値ａを乗算された不連続データを正弦関数および余弦関数により連続データに変換する連続／不連続変換ステップＳ３と、連続／不連続変換ステップＳ３で変換された連続データからノイズを除去するノイズ除去ステップＳ４と、ノイズ除去ステップＳ４でノイズを除去された連続データを正接関数により変換する三角関数変換ステップＳ５と、三角関数変換ステップＳ５で変換された連続データを、逆正接関数により、最小値から最大値までの範囲が２πの不連続データに変換する連続／不連続変換ステップＳ６と、連続／不連続変換ステップＳ６で変換された不連続データを所定値ａで除算するスケール変換ステップＳ７とを含む（図１３）。

すなわち、不連続データを連続データに変換してからノイズを除去するため、状態の連続的な変化に伴い周期的に増加する状態量を示すセンサ出力値の不連続データであっても、オーバシュートを生じることなくノイズを除去することができる。また、三角関数などの汎用的で簡易な演算により不連続データから連続データへの変換を行うため、比較的、処理能力の低いＣＰＵにも実装することができる。

上記実施形態は種々の形態に変形することができる。以下、変形例について説明する。上記実施形態では、工場内を自律移動するＡＧＶ１の位置を検出する例について説明したが、自律移動体は、このようなものに限らない。

上記実施形態では、管理エリアＡＲ１に複数台のカメラ２を設置してモーションキャプチャにより外部位置を検出する例を説明したが、自律移動体の外部に設けられて自律移動体の位置を検出する第１位置検出部は、このようなものに限らない。

上記実施形態では、速度検出部１２２がＡＧＶ１側でセンサ群１ａの出力値に基づいてＡＧＶ１の移動速度Ｖを検出する例を説明したが、自律移動体の移動速度を検出する速度検出部は、このようなものに限らない。ＡＧＶ１の外部に設けられるものでもよく、例えば、カメラ２からの画像信号に基づいてＡＧＶ１の移動速度Ｖを検出してもよい。

上記実施形態では、ＡＧＶ１の回転角度θの連続的な変化に伴い周期的に増加するＡＧＶ１の進行方向Ａを示すセンサ出力値αの不連続データからノイズを除去する例を説明したが、状態の連続的な変化に伴い周期的に増加する状態量を示す出力値の不連続データはこのようなものに限らない。例えば、連続的な時間経過に伴い周期的に増加する時刻の不連続データからノイズを除去してもよく、周回コースの連続走行に伴い周期的に増加するコース上のスタート地点からの距離の不連続データからノイズを除去してもよい。

上記実施形態では、具体的な式(i)を示してローパスフィルタ処理によりノイズを除去する例を説明したが、ノイズを除去する処理はセンサ出力値の前回値を使用するものであればよく、例示した処理に限らない。

以上の説明はあくまで一例であり、本発明の特徴を損なわない限り、上述した実施形態および変形例により本発明が限定されるものではない。上記実施形態と変形例の１つまたは複数を任意に組み合わせることも可能であり、変形例同士を組み合わせることも可能である。

１ ＡＧＶ（無人搬送車）、１ａ センサ群、２ カメラ、３ 無線ネットワーク、１０ ＡＧＶ側コントローラ、１１，２１ 通信部、１２，２２ ＣＰＵ、１３，２３ メモリ、１００ 位置検出システム、１２０ センサ信号処理部、１２１ 自己位置検出部、１２２ 速度検出部、１２３ 現在位置算出部、１２４ スケール変換部、１２５ 連続／不連続変換部、１２６ ノイズ除去部、１２７ 三角関数変換部、２２０ エリア判定部、２２１ 外部位置検出部、ＡＲ１ 管理エリア、ＡＲ２ 非管理エリア

Claims (5)

1. 自律移動体の現在位置を検出する位置検出システムであって、
   前記自律移動体の外部に設けられ、前記自律移動体の位置を検出する第１位置検出部と、
   前記自律移動体に搭載され、自律航法により前記自律移動体の位置を検出する第２位置検出部と、
   前記第１位置検出部により検出された前記自律移動体の位置に基づいて前記第２位置検出部により検出された前記自律移動体の位置を補正することで、前記自律移動体の現在位置を算出する現在位置算出部と、を備えることを特徴とする位置検出システム。
2. 請求項１に記載の位置検出システムにおいて、
   前記現在位置算出部は、前記自律移動体に設けられることを特徴とする位置検出システム。
3. 請求項１または２に記載の位置検出システムにおいて、
   前記自律移動体の移動速度を検出する速度検出部をさらに備え、
   前記現在位置算出部は、前記速度検出部により検出された前記自律移動体の移動速度に基づいて前記第１位置検出部および前記第２位置検出部によりそれぞれ検出された前記自律移動体の位置に重み付けし、重み付けされた位置に基づいて、前記自律移動体の現在位置を算出することを特徴とする位置検出システム。
4. 請求項１〜３のいずれか１項に記載の位置検出システムにおいて、
   前記現在位置算出部は、第１時刻に前記第２位置検出部により検出された前記自律移動体の位置に対する、前記第１時刻より後の第２時刻に前記第２位置検出部により検出された前記自律移動体の位置の相対位置に、前記第１時刻に前記第１位置検出部により検出された前記自律移動体の位置を加算することで、前記第２時刻における前記自律移動体の現在位置を算出することを特徴とする位置検出システム。
5. 請求項１または２に記載の位置検出システムにおいて、
   前記自律移動体は、前記現在位置算出部による前記自律移動体の現在位置の算出が行われる第１エリアと、前記現在位置算出部による前記自律移動体の現在位置の算出が行われない第２エリアと、を移動することを特徴とする位置検出システム。

Images