JP7238999B2 - 位置検出装置及び移動体制御システム - Google Patents

Description

本開示は、位置検出装置及び移動体制御システムに関する。

近年、工場等の製造現場において、無人搬送車（Automated guided vehicle、ＡＧＶ）及び荷物運搬ロボット等の自律移動型の移動体が利用されている。自律移動型の移動体には、壁、人、及び他の移動体等の障害物と衝突することを回避するため、カメラ又はＬＩＤＡＲ（Light Detection and Ranging）等のセンサが搭載され、これを用いて周囲の環境を監視することが行われている。一方で、工場等の製造現場では、光、粉塵、汚れがある環境においても高精度に機能するセンサとして、ロバスト性の高いレーダを利用する取組みも始まってきている。しかしながら、センサによる測定結果に含まれるノイズが原因で、障害物の位置を精度良く測定できない問題があった。そこで、センサによる測定結果からノイズを除去する必要がある。

例えば、特許文献１は、対象測距点から予め設定された基準距離内に、過去において取得された測距点が存在するか否かを判定し、基準距離内に過去において取得された対象測距点が存在しない場合に、該対象測距点がノイズ点の可能性が高いと判定することを特徴とするノイズ除去方法を開示している。

特開２０１６－１６１３４０号公報

しかしながら、従来技術においては、ＡＧＶ及び荷物運搬ロボット等の自律移動型の移動体が移動しながら測距を行った場合、移動体に対して壁等の障害物が相対的に移動するため、壁等の障害物がノイズ点の可能性が高いと判定される問題があった。そのため、壁等の障害物の検出結果がノイズとして除去され、精度良く周囲の物体の位置を測定できないという課題があった。

本発明の目的は、以上の課題を解決し、精度良く移動体の周囲の物体の位置を測定できる位置検出装置及び移動体制御システムを提供することにある。

本開示の一態様に係る位置検出装置は、
移動体に搭載されて物体の位置を検出する位置検出装置であって、
電磁波を送信し、前記移動体の周囲の物体からの反射波を受信し、受信した反射波に基づいて、前記電磁波を反射した反射点の位置を所定の周期ごとに測定するセンサ部と、
前記センサ部によって測定された前記周期ごとの前記反射点の位置を示す位置データを、前記周期の間隔のフレームとして記憶する記憶部と、
前記位置検出装置の動きを示す移動情報に基づいて探索範囲を決定する探索範囲決定部と、
前記決定された探索範囲に基づいて前記位置データに含まれるノイズを識別し、識別されたノイズを前記位置データから除去し、ノイズが除去された位置データを前記物体の位置を示す位置データとするノイズ識別除去部とを備える。

本開示の一態様に係る移動体制御システムは、
上記の態様の位置検出装置を備える移動体制御システムであって、
前記物体の位置を示す位置データを受信し、受信された位置データに基づいて前記移動体の動作を制御する制御部を備える。

本開示に係る位置検出装置及び移動体制御システムによれば、精度良く移動体の周囲の物体の位置を測定することができる。

実施形態に係る移動体制御システムの全体構成例を示すブロック図である。 従来のノイズ識別除去処理を説明するための模式図である。 図１の移動体制御システムの構成例を示すブロック図である。 図３のレーダ装置の構成例を示すブロック図である。 図４の送信アンテナの構成例を示す模式図である。 図３の位置検出装置によって実行される反射位置測定及び探索範囲決定処理を示すフローチャートである。 図３の位置検出装置によって実行されるノイズ識別除去処理を示すフローチャートである。 図７のサブルーチンであって、ノイズ識別処理を示すフローチャートである。 変形例１に係る移動体制御システムの構成例を示すブロック図である。 変形例２に係る移動体制御システムの構成例を示すブロック図である。

以下、添付の図面を参照して本開示に係る移動体制御システムの実施形態を説明する。なお、以下の実施形態において、同一又は同様の構成要素については同一の符号を付している。

（適用例）
図１は、本開示に係る移動体制御システム１００の適用例を説明するための模式図である。移動体制御システム１００は、移動体１４０と、位置検出装置１２０とを備える。移動体１４０は、例えば、ＡＧＶ及び荷物運搬ロボット等の自律移動型の移動体である。位置検出装置１２０は、移動体１４０に取り付けられている。位置検出装置１２０は、例えば、レーダ装置１５０を用いて、移動体１４０の周囲の障害物１１０の位置を検出する。レーダ装置１５０は、本開示の「センサ部」の一例である。障害物１１０は、例えば壁、棚、床、人、及び他の移動体等の物体である。

レーダ装置１５０は、例えば３次元電磁波レーダであり、電磁波ビームを周囲の３次元空間内で走査させて位置検出を行う。例えば、電磁波ビームは、周囲の３次元空間内を一定の周期Ｔで繰り返し走査する。したがって、レーダ装置１５０は、図２に示すように、周期Ｔごとに、電磁波ビームの走査範囲における位置検出の結果を示すデータ（以下、「フレーム」という。）を得ることができる。

レーダ装置１５０による位置測定の結果を示す位置信号にはノイズが含まれるため、位置検出装置１２０のノイズ識別除去部１２５は、ノイズ除去処理を行う。ノイズ除去後の位置信号は移動体１４０の制御部１４２に入力される。これにより、移動体１４０の制御部１４２は、ノイズ除去後の位置信号に基づいて、例えば移動体１４０が壁等の障害物１１０に衝突しないように、車輪及びステアリング等の駆動部を制御することができる。

図２は、従来のノイズ識別除去処理を説明するための模式図である。図２において、フレーム１ａは、時刻ｔにおける位置検出の結果を示している。フレーム１ｂは、時刻ｔ－Ｔにおける位置検出の結果を示し、フレーム１ｃは、時刻ｔ－２Ｔにおける位置検出の結果を示している。フレーム１ａ～１ｃにおける丸印で示した検出点は、レーダ装置によって検出された反射点の座標データを示している。以下で説明するノイズ識別除去処理には、フレーム１ａ～１ｃ以外にも多数のフレームを用いるが、図２においては図示していない。

図２において、従来のノイズ識別処理技術は、フレーム１ａ～１ｃ等の複数のフレームを比較し、複数のフレームのうちの所定数以上のフレームで同一の反射点の位置をノイズでないと判断する一方、所定数未満のフレームで同一の反射点の位置をノイズと判断する。これにより、図２のフレーム２で示したようなノイズ除去後の位置データが得られる。

しかしながら、従来技術では、移動体が移動している場合、移動体に搭載された位置検出装置も移動するため、位置検出装置に対して壁等の障害物が相対的に移動する。これにより、壁等の障害物がノイズと判断されて除去され、精度良く周囲の物体の位置を測定できないという課題があった。

そこで、本開示に係る図１の位置検出装置１２０は、移動体１４０又は位置検出装置１２０の速度及び移動距離等の移動情報を取得し、移動情報に応じてフレーム間における同一の反射点の探索範囲を変更する。例えば、移動体１４０が＋ｘ方向に移動している場合、位置検出装置１２０は、フレーム間における同一の反射点の探索範囲を－ｘ方向に広げる。これにより、壁等の障害物がノイズと判断されて除去されることを防止することができ、精度良く移動体の周囲の物体の位置を測定することができる。

（構成例）
図３は、本開示の実施形態に係る移動体制御システム１００の構成例を示すブロック図である。移動体制御システム１００は、移動体１４０と、位置検出装置１２０とを備える。

［位置検出装置］
図３において、位置検出装置１２０は、レーダ装置１５０と、移動体１４０の移動距離を算出する移動情報解析部１２２と、探索範囲決定部１２３と、メモリ１２４と、ノイズ識別除去部１２５と、通信インタフェース（以下、「通信Ｉ／Ｆ」という。）１２７とを備える。

レーダ装置１５０は、移動体１４０の周囲の障害物１１０の位置を測定し、測定結果を示す位置信号を出力する。

移動情報解析部１２２は、移動体１４０から受信した速度信号に基づいて、所定の期間における移動体１４０の移動方向及び移動距離を算出する。探索範囲決定部１２３は、移動情報解析部１２２によって算出された移動方向及び移動距離に基づいて、同一の反射点を探索する探索範囲を決定する。

メモリ１２４は、レーダ装置１５０からの位置信号が示すデータと、探索範囲決定部１２３によって決定された探索範囲を示すデータとを関連付けて記録する記録媒体である。メモリ１２４は、例えばＲＡＭ（Random Access Memory）で構成される。あるいは、メモリ１２４は、フラッシュメモリ、ＳＳＤ（Solid State Drive）などの半導体メモリ装置、ハードディスク等の磁気記憶装置で構成されてもよい。

ノイズ識別除去部１２５は、メモリ１２４に格納された複数のフレームの反射位置データと、探索範囲を示すデータとを用いて、探索範囲内で、複数のフレームのうちの所定数以上のフレームで同一の反射点の位置をノイズでないと判断する一方、所定数未満のフレームで同一の反射点の位置をノイズと判断する。次に、ノイズ識別除去部１２５は、識別されたノイズを反射位置データから除去し、ノイズ除去後の位置信号を出力する。

移動情報解析部１２２、探索範囲決定部１２３、及びノイズ識別除去部１２５は、例えばＣＰＵ（Central Processing Unit）、ＲＡＭ、ＲＯＭ（Read Only Memory）等を含む情報処理回路により構成される。移動情報解析部１２２、探索範囲決定部１２３、及びノイズ識別除去部１２５は、例えば、ＲＡＭに展開されたプログラムをＣＰＵにより解釈及び実行することにより、前述の処理を行う。

通信Ｉ／Ｆ１２７は、位置検出装置１２０と移動体１４０との通信接続を可能とするためのインタフェース回路を含む。通信Ｉ／Ｆ１２７は、例えば、ＩＥＥＥ８０２．３、ＩＥＥＥ８０２．１１又はＷｉ－Ｆｉ、ＬＴＥ、３Ｇ、４Ｇ、５Ｇ等の規格に従って通信を行う。通信Ｉ／Ｆ１２７は、ＵＳＢ(Universal Serial Bus)、ＨＤＭＩ(High Definition Multimedia Interface)、ＩＥＥＥ１３９４、Ｂｌｕｅｔｏｏｔｈ等の規格に従って通信を行うインタフェースであってもよい。

［移動体］
図３において、移動体１４０は、通信Ｉ／Ｆ１４１と、制御部１４２と、車輪駆動部１４３と、速度検出部１４４とを備える。

移動体１４０の通信Ｉ／Ｆ１４１は、移動体１４０と位置検出装置１２０との通信接続を可能とするためのインタフェース回路を含むものであり、位置検出装置１２０の通信Ｉ／Ｆ１２７と同様の構成を有する。

移動体１４０の制御部１４２は、例えばＣＰＵ、ＲＡＭ、ＲＯＭ等を含む情報処理回路により構成され、移動体１４０全体の制御を行う。例えば、制御部１４２は、ＲＡＭに展開されたプログラムをＣＰＵにより解釈及び実行することにより、車輪駆動部１４３を制御する。制御部１４２は、位置検出装置１２０から受信したノイズ除去後の位置信号に基づいて、例えば移動体１４０が障害物１１０に衝突しないように、車輪駆動部１４３を制御する。

移動体１４０の車輪駆動部１４３は、制御部１４２による制御に従って車輪を駆動する。例えば、車輪駆動部１４３は、移動体１４０が障害物１１０に衝突しないように、車輪を制御して移動体１４０の速度及び移動方向等を制御する。

移動体１４０の速度検出部１４４は、移動体１４０の速度を検出する。速度検出部１４４は、例えば、制御部１４２による車輪駆動部１４３の制御情報に基づいて、移動体１４０の速度を検出する。あるいは、速度検出部１４４は、移動体１４０の速度を検出する速度センサであってもよい。速度検出部１４４は、本開示の「移動情報検出部」の一例である。

［レーダ］
図４は、図３のレーダ装置１５０の構成例を示すブロック図である。図４において、レーダ装置１５０は、識別情報（以下、「ＩＤ情報」という。）を含む信号（以下、「ＩＤ信号」という。）を生成するＩＤ信号発生器１５２と、ＩＤ信号に従って無線搬送波を周波数変調（Frequency Modulation、ＦＭ）することにより無線信号を出力する変調送信回路１５３と、無線信号を送信する送信アンテナ１５１と、送信アンテナ１５１の指向性を制御する指向性制御部１５４とを備える。

図５は、図４の送信アンテナ１５１の構成例を示す模式図である。送信アンテナ１５１は、例えば、複数のアンテナ１５１ａを所定の間隔で２次元に配列した２次元アレイアンテナである。指向性制御部１５４は、各アンテナ１５１ａから送信される各無線信号の位相をそれぞれ制御して、送信アンテナ１５１から送信される送信ビームの指向性を制御する。指向性制御部１５４は、無線信号の位相だけでなく、振幅を制御してもよい。例えば、指向性制御部１５４は、送信ビームが所定の３次元空間内を一定の周期Ｔで走査するように、送信ビームの指向性を制御する。言い換えれば、指向性制御部１５４は、１／Ｔ［ｆｐｓ］のフレームレートで送信ビームを走査する。

図４において、レーダ装置１５０は、受信アンテナ１５５をさらに備える。レーダ装置１５０の送信アンテナ１５１から送信された無線信号は、障害物１１０によって反射され、受信アンテナ１５５によって受信される。

レーダ装置１５０は、受信信号を低雑音増幅する低雑音増幅器１５６と、ミキサ１５７とをさらに備える。ミキサ１５７は、増幅された受信信号と、変調送信回路１５３からの無線搬送波とを混合し、混合後の信号を出力する。ミキサ１５７から出力された混合後の信号は、ローパスフィルタ（以下、「ＬＰＦ」という。）１５８に入力され、不要な高周波成分が取り除かれる。このようにして、ＩＤ情報を含むベースバンド信号が得られる。

ＬＰＦ１５８から出力されたベースバンド信号は、ＡＤ変換器１５９に入力され、デジタル信号に変換される。ＡＤ変換器１５９から出力されたデジタル信号は、反射位置測定部１６０に入力される。

反射位置測定部１６０は、レーダ装置１５０の指向性制御部１５４からの指向性を示す信号に基づいて、送信アンテナ１５１から送信された電磁波の反射位置、すなわち障害物１１０の位置を測定する。測定方法の詳細については後述する。以上のようにして、レーダ装置１５０は、測定結果を示す位置信号を出力する。

指向性制御部１５４及び反射位置測定部１６０は、例えばＣＰＵ、ＲＡＭ、ＲＯＭ等を含む情報処理回路により構成される。指向性制御部１５４及び反射位置測定部１６０は、例えば、ＲＡＭに展開されたプログラムをＣＰＵにより解釈及び実行することにより、前述の指向性制御及び位置測定を行う。

（動作例）
図６は、図３の位置検出装置１２０によって実行される反射位置測定及び探索範囲決定処理を示すフローチャートである。図６は、位置検出装置１２０のレーダ装置１５０、移動情報解析部１２２、及び探索範囲決定部１２３によって行われる前段処理を例示する。位置検出装置１２０は、図６のフローを所定の周期Ｔで繰り返し行う。すなわち、位置検出装置１２０は、まずｉを初期化し（Ｓ１１）、周期Ｔごとにｉをインクリメントさせて（Ｓ１８）、以下のステップＳ１２～Ｓ１７の動作を行う。

（ステップＳ１２）
図６において、レーダ装置１５０は、電磁波を送受信することによって、電磁波を反射した反射点の位置を測定する（Ｓ１２）。ステップＳ１２は、例えば以下の手順で行われる。
（１）受信したデジタル信号に含まれるＩＤ情報を取得する。
（２）受信したデジタル信号に基づいて、受信アンテナ１５５が当該ＩＤ情報を含む反射波を受信した時刻ｔ１を検出する。
（３）指向性制御部１５４から、当該ＩＤ情報を含む送信ビームが送信された方向についての情報を取得する。
（４）指向性制御部１５４から、当該ＩＤ情報を含む送信ビームが送信された時刻ｔ２についての情報を取得する。
（５）送信アンテナ１５１から、（３）で取得された方向に、距離（ｔ１－ｔ２）×ｃ／２だけ離れた点を、反射点の位置として測定する。ここで、ｃは、光速である。

ステップＳ１２において反射点の位置を測定する際には、ノイズを反射点と誤認して検出することを防止するため、例えばＣＦＡＲ（Constant False Alarm Rate）処理等の閾値処理が行われてもよい。また、反射点は、１つの物標に対して複数検出されるため、検出点間の距離に基づいて、検出点群を物標ごとの塊としてまとめるクラスタリング処理が実施されてもよい。

ステップＳ１２において、レーダ装置１５０の指向性制御部１５４は、ビームが所定の３次元空間内を走査するように送信アンテナ１５１の指向性を制御する。

（ステップＳ１３）
次に、位置検出装置１２０は、レーダ装置１５０から、ステップＳ１２において測定された反射点の位置を示す位置信号を受信し、位置信号が示す位置データをメモリ１２４に格納する（Ｓ１３）。

ステップＳ１３において、位置検出装置１２０は、レーダ装置１５０の反射位置測定部１６０によってｉ番目の周期において検出された全ての反射点の位置を、ｉフレーム目の反射点の位置としてメモリ１２４に格納する。例えば、ｋフレーム目（ｉ＝ｋ）においてレーダ装置１５０の反射位置測定部１６０がα個の反射点の位置を測定した場合、反射位置測定部１６０は、フレーム番号ｋと関連付けて、α個の反射点の位置の相対座標をメモリ１２４に格納する。

ここで、「相対座標」とは、レーダ装置１５０に対する反射点の相対的な位置である。相対座標は、例えばレーダ装置１５０の送信アンテナ１５１の位置を原点とする３次元極座標（ｒ，θ，φ）で表される。相対座標は、３次元直交座標（ｘ，ｙ，ｚ）、及び円筒座標（ｒ，θ，ｚ）等で表されてもよい。上記のようにｋフレーム目（ｉ＝ｋ）においてレーダ装置１５０の反射位置測定部１６０がα個の反射点の位置Ｐ_ｋ１～Ｐ_ｋαを測定した場合、α個の反射点の位置の相対座標［Ｐ_ｋ１（ｒ_ｋ１，θ_ｋ１，φ_ｋ１）、Ｐ_ｋ２（ｒ_ｋ２，θ_ｋ２，φ_ｋ２）、…、Ｐ_ｋα（ｒ_ｋα，θ_ｋα，φ_ｋα）］が、フレーム番号ｋと関連付けられてメモリ１２４に記憶される。

（ステップＳ１４）
位置検出装置１２０の移動情報解析部１２２は、移動体１４０の速度検出部１４４から、通信Ｉ／Ｆ１４１，１２７を介して、検出された移動体１４０の速度を示す速度信号を受信する（Ｓ１４）。

（ステップＳ１５）
位置検出装置１２０の移動情報解析部１２２は、受信した速度信号に基づいて、移動体１４０が前フレームから現フレームまでに移動した移動距離及び移動方向を算出する（Ｓ１５）。

（ステップＳ１６）
位置検出装置１２０の探索範囲決定部１２３は、ステップＳ１５で得られた移動距離及び移動方向に基づいて、同一の反射点を探索する探索範囲を決定する（Ｓ１６）。ここで、位置検出装置１２０は、異なるフレーム間における電磁波の反射点が同一の反射物上の同一の点によるものと識別した場合、両反射点が同一の反射点であると判断する。

例えば、移動体１４０が前フレーム（ｋ－１番目のフレーム）から現フレーム（ｋ番目のフレーム）までの間に移動しておらず、ステップＳ１５で得られた移動距離が０である場合、探索範囲決定部１２３は、探索範囲を微小な半径ΔＲの球の内部とする。すなわち、この場合、後述のノイズ識別除去部１２５は、前フレームの反射点の位置Ｐ_{（ｋ－１）α}と同一の反射点Ｐ_ｋαを現フレームにおいて探索する際に、探索範囲をＰ_{（ｋ－１）α}を中心とする半径ΔＲの球の内部とする。この球の外部にある点は、前フレームの反射点の位置Ｐ_{（ｋ－１）α}と同一の反射点の候補とはならず、ノイズとして除去される。

これに対して、移動体１４０が前フレーム（ｋ－１番目のフレーム）から現フレーム（ｋ番目のフレーム）までの間に移動した場合、探索範囲決定部１２３は、探索範囲を半径ΔＲの球から変更する。例えば、移動体１４０が前フレームから現フレームまでの間に＋ｘ方向に移動した場合、移動体１４０から見ると、同一の物体は－ｘ方向に相対的に移動する。したがって、この場合、例えば、探索範囲決定部１２３は、探索範囲を当該球から－ｘ方向に広げた範囲とし、又は当該球を－ｘ方向に引き伸ばした範囲とする。

（ステップＳ１７）
次に、探索範囲決定部１２３は、ステップＳ１６において決定された探索範囲を、ｉフレーム目の探索範囲として、メモリ１２４に格納する（Ｓ１７）。

図７は、図３の位置検出装置１２０によって実行されるノイズ識別除去処理を示すフローチャートである。

（ステップＳ２１）
図７において、まず、図３のノイズ識別除去部１２５には、判断に用いる比較フレーム数Ｎと、判断閾値Ｍとが入力される（Ｓ２１）。ここで、Ｎは２以上の整数であり、ＭはＮ以下の自然数である。比較フレーム数Ｎ及び判断閾値Ｍは、予め定められてもよい。

（ステップＳ２２）
次に、ノイズ識別除去部１２５は、直近Ｎ個のフレームの反射位置データをメモリ１２４から読み込む。例えば、比較フレーム数Ｎが５に設定されている場合、ノイズ識別除去部１２５は、直近５フレームの反射位置データ、例えばｋ－４，ｋ－３，ｋ－２，ｋ－１，及びｋ番目のフレームの各反射位置データをメモリ１２４から読み込む。

（ステップＳ２３）
ノイズ識別除去部１２５は、ステップＳ２２においてメモリ１２４から読み込まれたＮ個のフレームの反射位置データを比較し、探索範囲内において、同一の反射点を検出する（Ｓ２３）。

例えば、ｋ－４番目のフレームにα_ｋ－４個の反射位置データが記録されている場合、ノイズ識別除去部１２５は、ｋ－３，ｋ－２，ｋ－１，及びｋ番目の各フレームの反射位置データのうち、ｋ－４番目のフレームのα_ｋ－４個の反射位置データと同一の反射点によるものを検出する。このとき、ｋ－４番目のフレームの反射位置データとｋ－３番目のフレームの反射位置データとを比較する際には、ステップＳ１７においてｋ－３番目のフレームの探索範囲としてメモリ１２４に格納された探索範囲データを用いる。また、ｋ－４番目のフレームの反射位置データとｋ－２番目のフレームの反射位置データとを比較する際には、ｋ－３番目のフレームに対応する探索範囲データとｋ－２番目のフレームに対応する探索範囲データとを用いる。例えば、両者を足し合わせた範囲を探索範囲とする。

続いて、ノイズ識別除去部１２５は、ｋ－３番目のフレームのα_ｋ－３個の反射位置データを基準として、ｋ－４，ｋ－２，ｋ－１，及びｋ番目の各フレームに同一の反射点があるかを探索して検出する。すなわち、ノイズ識別除去部１２５は、ｋ－４，ｋ－２，ｋ－１，及びｋ番目の各フレームの反射位置データのうち、ｋ－３番目のフレームのα_ｋ－３個の反射位置データと同一の反射物によるものを検出する。その後も同様にして、ノイズ識別除去部１２５は、ｋ－２，ｋ－１，及びｋ番目の各フレームの反射位置データを基準として、他のフレームにおける同一の反射点を検出する。

ノイズ識別除去部１２５が、異なるフレーム間における電磁波の反射点データが同一の反射点によるものと識別するには、例えば以下の（１）～（３）の方法が用いられる。これらの方法は単独で用いられてもよいし、組み合わせて用いられてもよい。
（１）反射点データに含まれる反射波強度情報が近い反射点同士を同一の反射点によるものと識別する。
（２）反射点データに含まれる速度情報が近い反射点同士を同一の反射点によるものと識別する。
（３）反射点データに含まれる距離、方位等の位置情報が近い反射点同士を同一の反射点によるものと識別する。

（ステップＳ２４）
次に、ノイズ識別除去部１２５は、ステップＳ１２３で検出されたＮ個のフレーム間における同一の反射点を示すデータがノイズであるか否かを識別する（Ｓ２４）。

図８は、図７のサブルーチンであって、ステップＳ２４のノイズ識別処理を示すフローチャートである。図８において、ステップＳ２２においてメモリ１２４から読み込まれたＮ個のフレーム内の全ての反射点Ｐｊについてノイズ識別を行うため、ノイズ識別除去部１２５は、ｊを初期化し（Ｓ２４１）、全ての反射点Ｐｊについてのノイズ識別を完了するまで後続のステップＳ２４２～Ｓ２４６を繰り返す（Ｓ２４７，Ｓ２４８）。

図８において、まず、ノイズ識別除去部１２５は、ステップＳ２２においてメモリ１２４から読み込まれたＮ個のフレーム内の反射点から、反射点Ｐｊを選択する（Ｓ２４２）。

次に、ノイズ識別除去部１２５は、ステップＳ２２でメモリ１２４から読み込まれたＮ個のフレーム内の反射点のうち、ステップＳ２４２で選択された反射点Ｐｊと同一の反射点を示す位置データの検出数Ｃｊを計数する（Ｓ２４３）。検出数Ｃｊの計数には、図７のステップＳ２３の検出結果を利用する。

次に、ノイズ識別除去部１２５は、検出数Ｃｊが判断閾値Ｍ以上であるか否かを判断する（Ｓ２４４）。比較フレーム数Ｎが５である前述の例の場合、例えばＭは４に設定され、ノイズ識別除去部１２５は、検出数Ｃｊが４以上であるか否かを判断する。検出数Ｃｊが判断閾値Ｍ以上である場合（Ｓ２４４でＹｅｓ）、ノイズ識別除去部１２５は、当該Ｃｊ個の反射点がノイズでないと識別する（Ｓ２４５）。ステップＳ２４４において検出数Ｃｊが判断閾値Ｍ未満である場合（Ｓ２４４でＮｏ）、ノイズ識別除去部１２５は、当該Ｃｊ個の反射点がノイズであると識別する（Ｓ２４６）。ステップＳ２４５及びステップＳ２４６の次は、Ｓ２４７に進む。

次に、ノイズ識別除去部１２５は、Ｎ個のフレームに含まれる全ての反射点についてステップＳ２４５又はステップＳ２４６の識別処理を行ったか否かを判断し（Ｓ２４７）、識別処理を行っていない場合は他の反射点を選択する（Ｓ２４８及びＳ２４２）。このようにして、全ての反射点についてステップＳ２４５又はステップＳ２４６の識別処理を行うまでステップＳ２４２～Ｓ２４６を繰り返す。なお、重複する識別処理は省略されてもよい。

（ステップＳ２５）
図８のステップＳ２４を終えた後、ノイズ識別除去部１２５は、図７のステップＳ２２においてメモリ１２４から読み込まれたＮ個のフレームの反射位置データから、ステップＳ２４においてノイズと識別された反射点を除去する（Ｓ２５）。

（作用・効果）
以上のようにして、移動体１４０に搭載された位置検出装置１２０は、移動体１４０が移動中であっても、ノイズ除去を行うことができる。この際、壁等の障害物をノイズと判断して除去することはない。したがって、精度良く周囲の物体の位置を測定することができる。

図３の位置検出装置１２０のノイズ識別除去部１２５から、ノイズ除去後の位置信号が出力される。ノイズ除去後の位置信号は、通信Ｉ／Ｆ１２７，１４１を介して移動体１４０の制御部１４２に入力される。これにより、移動体１４０の制御部１４２は、ノイズ除去後の位置信号に基づいて、例えば移動体１４０が障害物１１０に衝突しないように、車輪駆動部１４３を制御することができ、移動体１４０とその周囲の安全を確保することができる。

（変形例）
以上、本開示の実施形態を詳細に説明したが、前述までの説明はあらゆる点において本開示の例示に過ぎない。本開示の範囲を逸脱することなく種々の改良や変形を行うことができる。例えば、以下のような変更が可能である。なお、以下では、上記実施形態と同様の構成要素に関しては同様の符号を用い、上記実施形態と同様の点については、適宜説明を省略する。以下の変形例は適宜組み合わせることができる。

＜変形例１＞
図９Ａは、変形例１に係る移動体制御システム２００Ａの構成例を示すブロック図である。図９Ａの移動体制御システム２００Ａにおいては、図３の移動体制御システム１００の移動体１４０が速度検出部１４４を備えることに代えて、位置検出装置２２０Ａが速度検出部２２１Ａを備える。

すなわち、図３の移動体制御システム１００においては移動体１４０の速度検出部１４４が移動体１４０の速度を検出するのに対して、図９Ａの移動体制御システム２００Ａにおいては位置検出装置２２０Ａの速度検出部２２１Ａが位置検出装置２２０Ａの速度を検出する。

図９Ａにおいて、速度検出部２２１Ａは、レーダ装置１５０から受信した位置信号に基づいて、移動体２４０の速度を検出する。速度検出部２２１Ａは、レーダ装置１５０のＡＤ変換器１５９（図４参照）から出力されたデジタル信号に基づいて移動体２４０の速度を検出してもよい。

速度検出部２２１Ａは、例えば、移動体２４０の周囲にある静止物体の移動体２４０に対する相対位置を検出し、相対位置の時間的変化から位置検出装置２２０Ａの速度を検出する。移動体２４０が工場等の特定の敷地内で用いられる場合、電磁波を反射する金属膜等の反射材料を、随所に配置された所定の静止物体に取り付けてもよい。速度検出部２２１Ａは、このような反射材料の相対位置の時間的変化から位置検出装置２２０Ａの速度を検出することができる。あるいは、速度検出部２２１Ａは、レーダ装置１５０の受信アンテナ１５５（図４参照）が受信した受信信号の周波数の、送信信号の周波数に対するドップラーシフトを測定することにより、移動体２４０及び位置検出装置２２０Ａの周囲にある静止物体の相対速度を検出してもよい。静止物体の位置検出装置２２０Ａに対する相対速度がわかれば、位置検出装置２２０Ａの速度がわかる。

＜変形例２＞
図９Ｂは、変形例２に係る移動体制御システム２００Ｂの構成例を示すブロック図である。図９Ｂの移動体制御システム２００Ｂは、図９Ａの移動体制御システム２００Ａに比較して以下の点が異なる。すなわち、位置検出装置２２０Ｂは、速度検出部２２１Ａに代えて、速度検出部２２１Ｂを備える。

図９Ａの移動体制御システム２００Ａにおいては、速度検出部２２１Ａは、レーダ装置１５０から受信した位置信号等に基づいて位置検出装置２２０Ａの速度を検出するものである一方、図９Ｂの移動体制御システム２００Ｂにおいては、速度検出部２２１Ｂは、レーダ装置１５０からの信号に基づかずに位置検出装置２２０Ｂの速度を検出する。速度検出部２２１Ｂは、例えば汎用的な速度センサである。

＜変形例３＞
上記の実施形態においては、移動情報検出部の一例として、移動体１４０の速度検出部１４４について説明したが、本開示はこれに限定されない。例えば、移動情報検出部は、移動体１４０の角速度を検出するジャイロセンサ等の角速度検出部であってもよい。また、例えば、移動情報検出部は、移動体１４０の加速度を検出する加速度センサ等の加速度検出部であってもよい。また、例えば、移動情報検出部は、移動体１４０の移動距離を検出する距離センサ等の距離検出部であってもよい。

＜変形例４＞
上記の実施形態においては、ＡＧＶ及び荷物運搬ロボット等の自律移動型の移動体１４０に適用される移動体制御システム１００について説明したが、本開示はこれに限定されない。例えば、移動体１４０は、有人の自動車等の車両であってもよい。例えば、位置検出装置１２０によって検出された自動車と壁、人、及び他の移動体等の障害物等との距離が小さく、自動車が障害物に衝突するおそれがある場合、自動車の制御部がブレーキやステアリングを制御して衝突を回避することができる。

＜変形例５＞
上記の実施形態においては、位置検出装置１２０の探索範囲決定部１２３は、ステップＳ１６で、時間的に隣接する２つのフレームを比較して、全フレームに対して探索範囲を決定するものであったが、本開示はこれに限定されない。例えば、探索範囲決定部１２３は、時間的に隣接する複数フレーム、例えば１０フレームに対して同一の探索範囲を適用してもよい。これにより、探索範囲を決定するための処理負荷を低減することができる。

＜変形例６＞
上記の実施形態においては、図４に示したように、送信アンテナ１５１と受信アンテナ１５５とを別個に備えたレーダ装置１５０について説明した。しかしながら、本開示はこれに限定されず、レーダ装置１５０は、無線信号の送信及び受信の両方を行う１つのアンテナを備えるものであってもよい。この場合、例えば、送受信を切り替える切替スイッチや、サーキュレータ等が利用される。

＜変形例７＞
上記の実施形態においては、送信ビームを走査する送信アンテナ１５１と、無線信号を受信する受信アンテナ１５５とを備えるレーダ装置１５０について説明した。しかしながら、本開示はこれに限定されない。例えば、受信アンテナ１５５は、図５に示した送信アンテナ１５１と同様に、複数のアンテナを所定の間隔で２次元に配列した２次元アレイアンテナであってもよい。受信アンテナ１５５は、複数のアンテナを一直線に配列した１次元アレイアンテナで構成されてもよい。

本変形例においては、図４の反射位置測定部１６０は、受信アンテナ１５５を構成する各アンテナの受信信号の振幅及び位相を独立に制御してビームフォーミングを行うことにより、無線信号の到来方向を測定する。これにより、レーダ装置１５０は、電磁波の反射位置、すなわち障害物１１０の位置を測定することができる。

＜変形例８＞
上記の実施形態においては、図５に示したように、送信アンテナ１５１が複数のアンテナ１５１ａを２次元に配列した２次元アレイアンテナで構成されることを説明した。しかしながら、本開示はこれに限定されない。例えば、送信アンテナ１５１は、複数のアンテナを一直線に配列した１次元アレイアンテナで構成されてもよい。また、送信アンテナ１５１は、１つのアンテナ素子で構成されてもよい。

＜変形例９＞
上記の実施形態においては、図５の送信アンテナ１５１の各アンテナ１５１ａから送信される無線信号の位相又は振幅を制御して送信ビームの指向性を制御する指向性制御部１５４について説明した。しかしながら、本開示はこれに限定されない。例えば、指向性制御部１５４は、送信アンテナ１５１を物理的に動かして送信ビームの走査を行うものであってもよい。

＜変形例１０＞
上記の実施形態においては、図４に示したように、ＩＤ信号に従って無線搬送波を周波数変調して送信するレーダ装置１５０について説明した。しかしながら、本開示はこれに限定されず、レーダ装置１５０に用いられる変調方式は、他の連続波方式及びパルス方式であってもよい。例えば、電磁波レーダは、パルス変調方式を用いたパルスレーダであってもよい。

＜変形例１１＞
上記の実施形態においては、図４に示したように、レーダ装置１５０を用いた位置検出装置１２０について説明した。レーダ装置１５０は、電波レーダを含む。しかしながら、本開示はこれに限定されず、例えば、位置検出装置１２０は、レーダ装置１５０に代えて、レーザ光を用いたＬＩＤＡＲをセンサ装置として備えてもよい。もっとも、電波レーダは、ＬＩＤＡＲに比べて安価であるため、上記の作用効果を比較的安価に実現することができ、移動体制御システム１００を導入するために必要なコストを低減できる利点を有する。

また、近年、工場等の製造現場では、光、粉塵、汚れがある環境においても高精度に機能するセンサとして、ロバスト性の高い電波レーダを利用する取組みも始まってきている。電波レーダは、このような製造現場環境においても、精度良く位置検出をすることができる利点を有する。

（付記）
以下、本開示に係る各種態様を付記する。

本開示の一態様は、
移動体（１４０）に搭載されて物体の位置を検出する位置検出装置（１２０，２２０Ａ，２２０Ｂ）であって、
電磁波を送信し、前記移動体（１４０）の周囲の物体からの反射波を受信し、受信した反射波に基づいて、前記電磁波を反射した反射点の位置を前記周期ごとに測定するセンサ部（１５０）と、
前記センサ部（１５０）によって測定された前記周期ごとの前記反射点の位置を示す位置データを、前記周期の間隔のフレームとして記憶する記憶部（１２４）と、
前記位置検出装置（１２０，２２０Ａ，２２０Ｂ）の動きを示す移動情報に基づいて探索範囲を決定する探索範囲決定部（１２３）と、
前記決定された探索範囲に基づいて前記位置データに含まれるノイズを識別し、識別されたノイズを前記位置データから除去し、ノイズが除去された位置データを前記物体の位置を示す位置データとするノイズ識別除去部（１２５）と
を備える位置検出装置（１２０，２２０Ａ，２２０Ｂ）である。

位置検出装置（１２０，２２０Ａ，２２０Ｂ）において、
前記ノイズ識別除去部（１２５）は、前記記憶部（１２４）に格納された複数のフレームに係る位置データを用いて、前記決定された探索範囲内で、同一の反射点の位置を含むフレームの数に基づいて、同一の反射点の位置がノイズであるか否かを判断するものであってもよい。

位置検出装置（１２０，２２０Ａ，２２０Ｂ）において、
前記ノイズ識別除去部（１２５）は、前記決定された探索範囲内で、前記複数のフレームのうちの所定数以上のフレームで同一の反射点の位置をノイズでないと判断する一方、前記所定数未満のフレームで同一の反射点の位置をノイズと判断するものであってもよい。

位置検出装置（２２０Ａ）は、前記反射波に基づいて前記移動情報を検出する移動情報検出部（２２１Ａ）をさらに備えるものであってもよい。

位置検出装置（２２０Ｂ）は、前記位置検出装置（２２０Ｂ）の動きに基づいて前記移動情報を検出する移動情報検出部（２２１Ｂ）をさらに備えるものであってもよい。

位置検出装置（１２０）は、前記移動体（１４０）の移動情報検出部（１４４）から前記移動体（１４０）の動きを示す移動体移動情報を受信し、受信された移動体移動情報を前記移動情報として用いるものであってもよい。

位置検出装置（１２０，２２０Ａ，２２０Ｂ）において、
前記移動情報は、速度であり、
前記探索範囲決定部（１２３）は、前記速度が０である場合に前記探索範囲を最小とし、前記速度が高いほど前記探索範囲を大きくするものであってもよい。

本開示の一態様は、
位置検出装置（１２０，２２０Ａ，２２０Ｂ）を備える移動体制御システム（１００，２００Ａ，２００Ｂ）であって、
前記物体の位置を示す位置データを受信し、受信された位置データに基づいて前記移動体（１４０）の動作を制御する制御部を備える移動体制御システム（１００，２００Ａ，２００Ｂ）である。

１００ 移動体制御システム
１２０ 位置検出装置
１２２ 移動情報解析部
１２３ 探索範囲決定部
１２４ メモリ
１２５ ノイズ識別除去部
１２７ 通信Ｉ／Ｆ
１４０ 移動体
１４２ 制御部
１４３ 車輪駆動部
１４４ 速度検出部
１５０ レーダ装置

Claims (8)

1. 移動体に搭載されて物体の位置を検出する位置検出装置であって、
   電磁波を送信し、前記移動体の周囲の物体からの反射波を受信し、受信した反射波に基づいて、前記電磁波を反射した反射点の位置を所定の周期ごとに測定するセンサ部と、
   前記センサ部によって測定された前記周期ごとの前記反射点の位置を示す位置データを、前記周期の間隔の複数のフレームとして記憶する記憶部と、
   前記位置検出装置の動きを示す移動情報に基づいて探索範囲を決定する探索範囲決定部と、
   前記決定された探索範囲内の前記位置データを前記複数のフレーム間で比較することで、前記位置データに含まれるノイズを識別し、識別されたノイズを前記位置データから除去し、ノイズが除去された位置データを前記物体の位置を示す位置データとするノイズ識別除去部と
   を備え、
   前記探索範囲決定部は、前記探索範囲を決定する処理において、
   前記動きがない場合に前記探索範囲を最小とし、
   前記動きがある場合、前記動きの大きさが大きいほど、前記動きの向きと反対の方向に前記探索範囲を大きくする、
   位置検出装置。
2. 前記ノイズ識別除去部は、前記記憶部に格納された複数のフレームに係る位置データを用いて、前記決定された探索範囲内で、同一の反射点の位置を含むフレームの数に基づいて、同一の反射点の位置がノイズであるか否かを判断する請求項１に記載の位置検出装置。
3. 前記ノイズ識別除去部は、前記決定された探索範囲内で、前記複数のフレームのうちの所定数以上のフレームで同一の反射点の位置をノイズでないと判断する一方、前記所定数未満のフレームで同一の反射点の位置をノイズと判断する請求項２に記載の位置検出装置。
4. 前記反射波に基づいて前記移動情報を検出する移動情報検出部をさらに備える請求項１～３のいずれかに記載の位置検出装置。
5. 前記移動体の動きに基づいて前記移動情報を検出する移動情報検出部をさらに備える請求項１～３のいずれかに記載の位置検出装置。
6. 前記移動体の移動情報検出部から前記移動体の動きを示す移動体移動情報を受信し、受信された移動体移動情報を前記移動情報として用いる請求項１～３のいずれかに記載の位置検出装置。
7. 前記移動情報は、速度であり、
   前記探索範囲決定部は、前記速度が０である場合に前記探索範囲を最小とし、前記速度が高いほど前記探索範囲を大きくする請求項１～６のいずれかに記載の位置検出装置。
8. 請求項１～７のいずれかに記載の位置検出装置を備える移動体制御システムであって、
   前記物体の位置を示す位置データを受信し、受信された位置データに基づいて前記移動体の動作を制御する制御部を備える移動体制御システム。

Images