移動体の移動速度推定装置及び移動体の制御装置

本発明は、移動体の移動速度を推定する装置と、移動体の制御装置とに関する。

移動ロボット等の移動体を、他の移動体等が共存する動作環境空間で移動させる技術が例えば特許文献1に提案されている。

特開2012−128585号公報

特許文献1に見られる如き技術では、移動体の将来位置を推定することが必要となる。そして、その推定のためには、正確な移動体の移動速度の推定値もしくは計測値が必要となる。

ここで、移動体の移動動作の形態によっては、移動体の単位時間当たりの移動距離(平均的な移動速度)がほぼ一定であっても、移動体の瞬間の移動速度(瞬時移動速度)は、平均的な移動速度を中心として振動する場合がある。

例えば、人もしくは脚式移動ロボット等の脚式移動体は、各脚の着地及び空中移動を繰り返しながら移動するために、該脚式移動体の上体又は基体は、該脚式移動体の左右方向あるいは上下方向あるいは前後方向の瞬時移動速度が周期的に振動するような形態で移動していく。

そして、このような移動体の瞬時移動速度を逐次推定もしくは計測しても、該瞬時移動速度は、移動体の実際の進行方向と異なる方向の速度成分を有したり、あるいは、該進行方向における平均的な移動速度からずれた速度となる。このため、該移動体の瞬時移動速度の推定値又は計測値から将来の位置を予測すると、誤った予測がなされてしまいやすい。

なお、移動体の瞬時移動速度の推定値又は計測値を平均化する処理を実行することで、移動体の移動速度を推定することも考えられる。しかるに、この場合、平均化する時間幅が短いと、瞬時移動速度の振動成分を十分に除去できない。また、平均化する時間幅が長いと、移動体の移動速度の推定が遅れてしまう。

本発明は、かかる背景に鑑みてなされたものであり、周期的な移動動作に伴う周期的な振動成分を含む速度で移動する移動体の移動速度を適切に推定することができる移動速度推定装置を提供することを目的とする。

またさらに、該移動速度推定装置を用いて移動体の移動を適切に制御し得る制御装置を提供することを目的とする。

本発明の移動体の移動速度推定装置は、周期的な移動動作に伴う周期的な振動成分を含む速度で移動する移動体の移動速度を推定する装置であって、 前記移動体の位置を逐次計測する位置計測部と、 該位置計測部により得られた前記移動体の位置の計測値の時系列から該移動体の瞬時移動速度を逐次推定する瞬時移動速度推定部と 前記移動体の周期的な移動動作に伴い該移動体の瞬時移動速度の時系列に含まれる前記周期的な振動成分の振動周期又は振動周波数を推定する振動成分情報推定部と、 前記瞬時移動速度推定部により得られた前記瞬時移動速度の推定値の時系列に、前記振動成分情報推定部により得られた前記振動周期又は振動周波数の推定値に対応する振動成分を除去する処理を少なくとも実行することにより得られる速度値を前記移動体の移動速度の推定値として逐次生成する速度推定値調整処理部とを備えることを特徴とする(第1発明)。

かかる本発明によれば、前記移動体の周期的な移動動作に伴い該移動体の瞬時移動速度に含まれる前記周期的な振動成分の振動周期又は振動周波数が、前記振動成分情報推定部により推定される。

そして、前記速度推定値調整処理部は、前記瞬時移動速度推定部により得られた前記瞬時移動速度の推定値の時系列に、前記振動成分情報推定部により得られた前記振動周期又は振動周波数の推定値に対応する振動成分を除去する処理を少なくとも実行することにより得られる速度値を前記移動体の移動速度の推定値として逐次生成する。

このため、前記速度推定値調整処理部により生成される前記移動体の移動速度の推定値は、前記移動体の周期的な移動動作に伴う振動成分が除去されたものとなる。ひいては、該移動体の移動速度の推定値が、その変動が抑制された形態で得られることとなる。

従って、本発明によれば、周期的な移動動作に伴う周期的な振動成分を含む速度で移動する移動体の移動速度を適切に推定することができる。

上記第1発明では、前記振動成分情報推定部は、前記移動体の瞬時移動速度の推定値の時系列を周波数領域に変換してなるスペクトル分布データにおいて、スペクトル強度がピーク値となる周波数から前記振動周期又は振動周波数を推定するように構成され得る(第2発明)。

これによれば、前記周期的な振動成分の振動周期又は振動周波数を、実データに基づいて推定することできる。従って、移動体の移動速度の推定値の信頼性を高めることができる。

また、前記第1発明では、前記振動成分情報推定部は、前記振動周期又は振動周波数の推定用のあらかじめ作成されたデータに基づいて前記振動周期又は振動周波数を推定するように構成することも可能である(第3発明)。

これによれば、振動成分情報推定部は、スペクトル分布データを求める処理を必要とせずに、前記振動周期又は振動周波数を推定できる。

上記第3発明では、前記データとして、前記移動体の移動速度と前記振動周期又は振動周波数との関係を表す相関データを採用し得る。この場合、前記振動成分情報推定部は、前記速度推定値調整処理部からフィードバックされる移動速度の推定値から、前記相関データに基づいて前記振動周期又は振動周波数の推定値を更新しつつ決定するように構成されていることが好ましい(第4発明)。

これによれば、前記移動体の移動中に、該移動体の実際の移動状態を反映させて、前記振動周期又は振動周波数の推定値を更新できる。このため、移動体の移動速度の推定値の信頼性を高めることができる。

上記第1〜第4発明では、前記速度推定値調整処理部は、前記振動周期又は振動周波数の推定値に基づいて構成されたバンドストップフィルタにより、前記振動周期又は振動周波数の推定値に対応する振動成分を除去する処理を実行するように構成されていることが好ましい(第5発明)。

これによれば、前記移動体の移動速度の推定値の、実際の移動速度の変化に対する追従性を高めることができる。

また、上記第1〜第5発明では、前記移動体が脚式移動体である場合には、前記速度推定値調整処理部は、前記移動体の周期的な移動動作に伴う振動成分として、該振動成分のうちの最も低周波の第1の振動周波数の振動成分と、該第1の振動周波数の2倍の周波数である第2の振動周波数の振動成分とを前記瞬時移動速度の推定値から除去する処理を実行するように構成されており、前記振動成分情報推定部は、前記第1の振動周波数と、該第1の振動周波数に対応する第1の振動周期と、前記第2の振動周波数と、該第2の振動周波数に対応する第2の振動周期とのうちの少なくともいずれか1つを推定するように構成されていることが好ましい(第6発明)。

すなわち、移動体が脚式移動体である場合、通常、前記移動体の周期的な移動動作に伴う振動成分は、基本波成分としての第1の振動周波数成分と、その2倍の周波数の第2の振動周波数成分とを多く含む。従って、第6発明によれば、前記速度推定値調整処理部は、移動体の移動速度の推定値を、その変動を効果的に抑制して生成することができる。また、前記第1の振動周波数と、該第1の振動周波数に対応する第1の振動周期と、前記第2の振動周波数と、該第2の振動周波数に対応する第2の振動周期との4つのパラメータのうちのいずれか1つを決めれば、残りの3つのパラメータの値も決まる。従って、振動成分情報推定部は、上記4つのパメータのうちの少なくとも1つの値を推定すれば済む。

上記第1〜第6発明では、前記速度推定値調整処理部は、前記瞬時移動速度の推定値から、前記振動周期又は振動周波数の推定値に対応する振動成分に加えて、該振動成分よりも高周波領域の成分を高周波成分除去フィルタにより除去する処理をさらに実行することによって、前記移動体の移動速度の推定値を逐次生成するように構成されていることが好ましい(第7発明)。

この第7発明によれば、移動体の周期的な移動動作に伴う振動成分に加えて、高周波領域のノイズ成分を移動体の移動速度の推定値から除去することができる。

このため、移動体の移動速度の推定値の信頼性をより一層高めることができる。

次に、本発明の移動体の制御装置は、上記した移動速度推定装置を利用する発明である。この移動体の制御装置は、動作環境空間を移動する制御対象の移動体の移動制御を行う制御装置であって、 前記動作環境空間に存在する各移動体の将来位置を予測する将来位置予測部と、 前記制御対象の移動体の移動可能領域を推定する移動可能領域推定部と、 前記各移動体の移動速度を推定可能に構成されると共に、前記位置計測部が、前記各移動体を含めて前記動作環境空間に存在する物体の位置を計測可能に構成された前記移動体の移動速度推定装置(前記第1〜第7発明のいずれかの移動速度推定装置)と、 前記制御対象の移動体の移動軌道を制御する移動軌道制御部とを備え、 前記将来位置予測部は、各移動体について前記移動速度推定装置により推定された移動速度を用いて該移動体の将来位置を予測するように構成されており、 前記移動可能領域推定部は、前記動作環境空間のうち、前記位置計測部の計測データから物体が存在しないことが認識される領域と、前記移動速度推定装置が移動速度を推定した各移動体の存在領域とを合わせた領域を移動可能領域として推定するように構成されており、 前記移動軌道制御部は、前記制御対象の移動体を移動させるとき、前記移動可能領域推定部により推定された移動可能領域以外の領域である移動不可領域と、前記将来位置予測部により予測された他の移動体の将来位置とに干渉しない領域にて、該制御対象の移動体を移動させるように該制御対象の移動体の移動軌道を制御するように構成されていることを特徴とする(第8発明)。

この第8発明によれば、前記制御対象の移動体を前記動作環境空間で移動させるとき、制御対象の移動体が、物体や他の移動体を干渉することがないよう該移動体を移動させることができる。そして、この場合、前記将来位置予測部は、各移動体について前記移動速度推定装置により推定された移動速度を用いて該移動体の将来位置を予測するように構成されているので、制御対象の移動体の他の移動体の将来位置を高い信頼性で予測することができる。ひいては、制御対象の移動体を他の移動体に干渉させないように移動させることを高い信頼性で実現できる。

上記第8発明では、前記移動可能領域推定部は、前記位置計測部の計測データの生成用のセンサと、いずれかの移動体との位置関係に応じて該計測データを生成することができない計測不能領域が存在するとき、該計測不能領域についての過去の計測データに基づいて、該計測不能領域を移動可能領域とするか否かを決定するように構成されていることが好ましい(第9発明)。

ここで、前記位置計測部の計測データの生成用のセンサと、いずれかの移動体との位置関係に応じて該計測データを生成することができない計測不能領域が存在する場合がある。

このような場合であっても、第9発明によれば、該計測不能領域についての過去の計測データに基づいて、該計測不能領域を移動可能領域とするか否かを決定することで、該計測不能領域を、移動可能領域として推定することも可能となる。このため、移動可能領域が必要以上に狭くなるのを防止することができる。ひいては、制御対象の移動体の移動軌道の設定自由度を高めることができる。

上記第9発明では、前記移動可能領域推定部は、例えば、前記計測不能領域についての過去の計測データのうちの最新の計測データによって、該計測不能領域を移動可能領域と推定し得る場合に、該計測不能領域を移動可能領域として決定するように構成され得る(第10発明)。

これによれば、前記計測不能領域を移動可能領域として決定する場合の信頼性を高めることができる。

本発明の一実施形態における移動体の動作環境空間を概略的に示す図。

第1実施形態における移動体運動推定装置の機能を示すブロック図。

図3Aは移動体の瞬時移動速度の波形の一例を示すグラフ、図3Bは図3Aの波形を周波数領域に変換して得られるスペクトル分布データを示すグラフ。

図2に示すバンドスップフィルタの特性の一例を示すグラフ。

図2に示すフィルタリング処理部に入力される瞬時移動速度の推定値の波形と、該フィルタリング処理部から出力される移動速度の推定値の波形とを例示するグラフ。

図6Aは、図2に示すフィルタリング処理部に入力される位置の計測値に対応する経路と、該フィルタリング処理部から出力される位置の推定値に対応する経路とを例示するグラフ、図6Bは、図2に示すフィルタリング処理部に入力されるX軸方向の瞬時移動速度の推定値の波形と、該フィルタリング処理部から出力されるX軸方向の移動速度の推定値の波形とを例示するグラフ、図6Cは、図2に示すフィルタリング処理部に入力されるY軸方向の瞬時移動速度の推定値の波形と、該フィルタリング処理部から出力されるY軸方向の移動速度の推定値の波形とを例示するグラフ。

第2実施形態におけるバンドストップフィルタの特性の一例を示すグラフ。

第2実施形態におけるバンドストップフィルタの特性値の設定手法を説明するためのグラフ。

第2実施形態におけるバンドストップフィルタの構成例を示すブロック図。

第3実施形態における移動速度推定装置の機能を示すブロック図。

第4実施形態における移動速度推定装置の機能を示すブロック図。

移動体の制御装置の一実施形態(第5実施形態)における該制御装置の機能を示すブロック図。

図12に示す移動可能領域推定部の処理を示すフローチャート。

図13のSTEP3の処理を示すフローチャート。

図14のSTEP12の処理を示すフローチャート。

図12に示す移動可能領域推定部の処理に関する説明図。

図12に示す移動可能領域推定部の処理に関する説明図。

図12に示す移動可能領域推定部の処理に関する説明図。

図12に示す移動軌道制御部の処理に関する説明図。

[第1実施形態] 本発明の第1実施形態を図1〜図6を参照して以下に説明する。図1に示すように、本実施形態では、移動速度の推定対象の移動体は、脚式移動体Aである。図1では、脚式移動体Aの例として、人A1と人型の移動ロボットA2とが模式的に図示されている。これらの移動体Aは、2つの脚のそれぞれの先端部(足部)の空中移動及びそれに続く着地を繰り返す移動動作(歩行動作又は走行動作)によって移動する。

これらの移動体Aが移動を行う動作環境空間は、各移動体A、あるいは、障害物等の物体の位置(動作環境空間における存在位置)を推定するためのインフラセンサ100が設置された空間である。

一例として、インフラセンサ100は、例えば動作環境空間の天井に付設された複数のレーザレンジファインダ等のレーザ式の測距センサにより構成される。この場合、各インフラセンサ100は、その下方正面側の計測対象領域内で、床面に向かって複数の方向にレーザビームを出力する。そして、インフラセンサ100は、レーザビームの反射信号を受信することで、該レーザビームの各出力方向に存在する物体(レーザビームの反射点)との間の距離の計測信号を出力する。

なお、インフラセンサ100は、レーザ式の測距センサに限られないことはもちろんである。例えば、レーザ式測距センサの代わりに、あるいは、レーザ式測距センサに加えて、レーダ方式もしくは超音波方式の測距センサ、あるいは、ステレオカメラ等をインフラセンサ100として利用することもできる。また、インフラセンサ100の配置箇所は、天井に限らず、任意に設定し得る。

図2に示す移動体運動推定装置1は、本発明の移動速度推定装置の一実施形態としての機能を有する装置である。この移動体運動推定装置1は、コンピュータ、あるいは、電子回路ユニット、あるいは、これらを組み合わせた装置により構成される。この場合、該移動体運動推定装置1は、相互に通信可能な複数のコンピュータあるいは複数の電子回路ユニットを含み得る。

移動体運動推定装置1には、インフラセンサ100の計測信号が入力される。そして、該移動体運動推定装置1は、実装されるプログラムあるいはハードウェア構成により実現される機能として、インフラセンサ100の計測信号に基づいて移動体Aの位置を逐次計測する位置計測部2と、該位置計測部2により得られた移動体Aの位置の計測値の時系列から該移動体Aの瞬時移動速度を逐次推定する瞬時移動速度推定部3と、移動体Aの移動動作に伴って該移動体Aの瞬時移動速度の時系列に含まれる周期的な振動成分の振動周波数(又は振動周期)を推定する振動成分情報推定部4と、位置計測部2により得られる移動体Aの位置の計測値と瞬時移動速度推定部3により得られる移動体Aの瞬時移動速度の推定値とのそれぞれに対して、周波数領域のフィルタリング処理を施すことによって、移動体Aの位置及び移動速度のそれぞれの推定値(フィルタリング値)を逐次生成するフィルタリング処理部5とを備える。

上記フィルタリング処理部5は、振動成分情報推定部4により得られた振動周波数(又は振動周期)の推定値に対応する周期的な振動成分を除去するフィルタリング処理を実行するバンドストップフィルタ11と、主に高周波領域のノイズ成分を除去するフィルタリング処理を実行するカルマンフィルタ12とから構成される。

なお、フィルタリング処理部5は、移動体Aの移動速度の推定値に対してフィルタリング処理を実行する機能を有することによって、本発明における速度推定値調整処理部としての機能を持つ。

以降、移動体運動推定装置1の各機能部の処理をより詳細に説明する。

位置計測部2は、各インフラセンサ100(本実施形態ではレーザ式測距センサ)毎に、所定のサンプリング周期で、各インフラセンサ100の計測信号から、該インフラセンサ100のレーザビームの各出力方向における測距データ(距離計測値を示すデータ)を取得する。そして、位置計測部2は、この測距データから背景差分等の手法によって、動作環境空間に存在する各移動体Aの存在位置と高さを計測する。

この場合、位置計測部2は、隣り合う複数のインフラセンサ100の計測対象領域が重なり合う領域に移動体Aが存在する場合には、これらのインフラセンサ100のそれぞれに対応する計測データを統合して、該移動体Aの存在位置と高さとを計測する。

なお、本実施形態では、移動体Aの存在位置は、例えば動作環境空間に対してあらかじめ設定されるグローバル座標系(例えば図1に示すXYZ座標系)における2次元水平面(XY座標平面)に投影して見た、該移動体Aの代表点(例えば重心点)の座標位置(X(t),Y(t))として表される。そして、移動体Aの物体の高さは、グローバル座標系のZ軸方向(鉛直方向)における該移動体Aの上端部の座標位置Z(t)として表される。

また、移動体Aの位置計測のより具体的な手法としては、例えば特開2011−253377号公報に本願出願人が提案した手法等を採用し得る。

補足すると、位置計測部2は、動作環境空間に存在する移動体Aに限らず、障害物等の物体の位置を計測することもできる。後述する第5実施形態では、位置計測部2により移動体以外の物体の計測も行われる。

瞬時移動速度推定部3は、各移動体Aの位置が位置計測部2により計測される毎に、各移動体Aの位置の今回の計測値(X(t),Y(t))と、前回の計測値(X(t-Δt),Y(t-Δt))との差(位置偏差)を、それらの計測値のサンプリング周期Δtにより除算することにより、各移動体Aの瞬時移動速度(Vx(t),Vy(t))を推定する。

すなわち、次式(1a),(1b)により、X軸方向の瞬時移動速度Vx(t)と、Y軸方向の瞬時移動速度Vy(t)とが算出される。

Vx(t)=(X(t)−X(t-Δt))/Δt …(1a) Vy(t)=(Y(t)−Y(t-Δt))/Δt …(1b) 振動成分情報推定部4は、瞬時移動速度推定部3から、各移動体Aの瞬時移動速度の推定値の一例の時系列データ(一定時間内における瞬時移動速度の推定値の経時変化を示す時系列データ)を事前に取得しておく。該時系列データは、移動体Aの瞬時移動速度に含まれる振動成分についての情報を取得するために、移動体Aの移動を試験的に行ったときに、前記位置計測部2及び瞬時移動速度推定部3の処理により得られたデータである。そして、振動成分情報推定部4は、各移動体Aの瞬時移動速度の推定値の時系列データを、FFT等のフーリエ変換処理により、周波数領域に変換することで、スペクトル分布データを生成する。

さらに、振動成分情報推定部4は、上記スペクトル分布データにおいて、スペクトル強度がピーク値となる周波数から、移動体Aの移動動作に伴って移動体Aの瞬時移動速度の時系列に含まれることとなる振動成分の振動周波数(又は振動周期)を推定する。

より具体的には、移動体A(例えば人A1)の静止状態から直進移動を行い、その後移動を停止した場合の瞬時移動速度の推定値の時系列データの一例として、図3Aに示す波形データ(時間領域での波形データ)が得られる。なお、図示例の波形データは、移動体AがX軸方向に直進移動を行った場合におけるX軸方向の瞬時移動速度Vx(t)の波形データである。

この波形データに見られるように、移動体Aの移動開始から一定時間経過した後の定常移動状態(単位時間当たりの進行方向移動量がほぼ一定となる状態)であっても、移動体Aの瞬時移動速度は短期的な変動を生じる。

この波形データを周波数領域に変換することで、図3Bに示す波形のスペクトル分布データが得られる。このスペクトル分布データにおいて、定常移動状態の移動体Aの瞬時移動速度の振動成分として、図3Bに示す周波数f1、f2、f3におけるスペクトル強度(振動振幅値)が比較的顕著な大きさのピーク値となる。

周波数f1は、定常移動状態の移動体Aの歩行周期(2歩分の期間の時間幅)の振動成分に対応する周波数(例えば0.9Hz)、f2,f3は、それぞれf1の2倍、3倍の周波数である。ここで、脚式移動体である移動体Aの移動動作では、該移動体Aの進行方向(ここではX軸方向)における瞬時移動速度Vx(t)の時系列は、一般に、上記歩行周期の半分の周期(1歩分の期間の時間幅)の振動成分を多く含む。この半分の周期の振動成分の周波数が上記f2である。

本実施形態では、振動成分情報推定部4は、上記スペクトル分布データにおいて、スペクトル強度(振動振幅値)がピーク値となる周波数から、定常移動状態の移動体Aの歩行周期に対応するとみなし得る周波数f1を推定する。例えば、振動成分情報推定部4は、所定の閾値以上の周波数域で、スペクトル強度が所定の閾値以上のピーク値となる周波数のうちの最も低い周波数を、定常移動状態の移動体Aの歩行周期に対応する周波数f1として推定する。

なお、周波数f1は、定常移動状態の移動体Aの移動動作に伴い該移動体Aの瞬時移動速度の時系列に含まれる振動成分の中で、最も低い周波数の振動成分(基本波成分)と言える。

そして、振動成分情報推定部4は、例えば、この周波数f1とその2倍の周波数f2とをフィルタリング処理部5での除去対象の振動成分の周波数として決定して、該周波数f1,f2(又はこれらに対応する振動周期)を図示しないメモリに記憶保持する。

以上のように、移動体Aの歩行周期に対応する周波数f1を推定し、さらに、フィルタリング処理部5での除去対象の振動成分の周波数f1,f2を決定する処理が、振動成分情報推定部4により事前に行われる。

そして、振動成分情報推定部4は、移動体運動推定装置1が、実際の移動体Aの移動速度の推定処理等(詳しくは、フィルタリング処理部5の処理)を実行するときに、上記のように決定した周波数f1,f2(又はこれらに対応する振動周期)が、除去対象の振動成分の周波数を指示する除去対象指示情報としてフィルタリング処理部5に出力される。

補足すると、振動成分情報推定部4がフィルタリング処理部5に出力する除去対象指示情報は、該フィルタリング処理部5で後述のバンドストップフィルタ11の除去対象の振動成分の周波数(又は振動周期)を特定し得る情報であればよい。

なお、以降の説明では、上記周波数f1を第1振動周波数f1、第1振動周波数f1の2倍の周波数f2を第2振動周波数f2という。

フィルタリング処理部5は、バンドストップフィルタ11によるフィルタリング処理と、カルマンフィルタ12によるフィルタリング処理とを、瞬時移動速度推定部3から逐次入力される各移動体Aの瞬時移動速度の推定値Vx(t),Vy(t)と、位置計測部2から逐次入力される各移動体Aの位置の計測値X(t),Y(t)とのそれぞれに施す。

ここで、本実施形態のフィルタリング処理部5は、各移動体Aの瞬時移動速度及び位置から、振動成分情報推定部4の除去対象指示情報により示される上記第1振動周波数f1の振動成分と、第2振動周波数f2(=2・f1)の振動成分とをバンドストップフィルタ11により除去する。

さらに、フィルタリング処理部5は、第2振動周波数f2よりも高周波域のノイズ成分(第1振動周波数f1の3倍の周波数f3を含む)をカルマンフィルタ12により除去する。

このため、バンドストップフィルタ11は、本実施形態では、第1振動周波数f1の振動成分の除去用の第1バンドストップフィルタ11a(図示省略)と、第2振動周波数f2の振動成分の除去用の第2バンドストップフィルタ11b(図示省略)とにより構成される。

第1バンドストップフィルタ11a及び第2バンドストップフィルタ11bのそれぞれは、例えば、図4に示すような周波数通過特性で、除去対象の周波数fc(=f1又はf2)を除去するように構成される。この場合、除去対象の周波数fcでの振幅の減衰度合(入力強度に対する出力強度の減衰度合)がピークとなるように、第1バンドストップフィルタ11a及び第2バンドストップフィルタ11bの周波数通過特性が設定される。

このような特性は、各バンドストップフィルタ11a,11bの伝達関数G(S)を次式(2)により設定することで、実現される。

G(S)=(S²+ωc²)/(S²+2・ζ・ωc・S+ωc²) ……(2) なお、ωc=2・π・fc、ζは定数値である。

そして、各バンドストップフィルタ11a,11bをデジタルフィルタとして構成する場合の伝達関数G(Z)は、式(2)に基づいて、次式(3)により与えられる。

G(Z)=(b0+b1・Z^-1+b2・Z^-2)/(a0+a1・Z^-1+a2・Z^-2) ……(3) ただし、 a0=ωc・Δt²+4・ζ・ωc・Δt+4 a1=2・ωc²・Δt²−8 a2=ωc・Δt²−4・ζ・ωc・Δt+4 b0=ωc・Δt²+2・Δt b1=2・ωc²・Δt² b2=ωc・tΔt²−2・Δt なお、Δtはサンプリング周期である。

フィルタリング処理部5は、上記の如く構成した第1バンドストップフィルタ11a及び第2バンドストップフィルタ11bのフィルタリング処理を、瞬時移動速度推定部3から逐次入力される各移動体Aの瞬時移動速度の推定値(Vx(t),Vy(t))と、位置計測部2から逐次入力される各移動体Aの位置の計測値(X(t),Y(t))のそれぞれに施す。これにより、各移動体Aの瞬時移動速度の推定値Vx(t),Vy(t)及び位置の計測値X(t),Y(t)のそれぞれから第1振動周波数f1の振動成分と、第2振動周波数f2の振動成分とを除去してなるフィルタリング値Vx_f1(t),Vy_f1(t)及びX_f1(t),Y_f1(t)が逐次求められる。

そして、フィルタリング処理部5は、さらにこれらのフィルタリング値Vx_f1(t),Vy_f1(t)及びX_f1(t),Y_f1(t)にカルマンフィルタ12のフィルタリング処理を施す。この場合、カルマンフィルタ12は、前記したように、第2振動周波数f2よりも高周波域のノイズ成分を除去するように構成されている。このようなカルマンフィルタ12は、公知の形態で構成し得る。

このカルマンフィルタ12のフィルタリング処理によって、前記バンドストップフィルタ11により生成したフィルタリング値Vx_f1(t),Vy_f1(t)及びX_f1(t),Y_f1(t)から、高周波域のノイズ成分をさらに除去してなるフィルタリング値Vx_f2(t),Vy_f2(t)及びX_f2(t),Y_f2(t)が逐次求められる。

以上が、本実施形態における移動体運動推定装置1の各機能部の処理の詳細である。

かかる本実施形態では、移動体Aの瞬時移動速度の推定値(Vx(t),Vy(t))と、位置の計測値(X(t),Y(t))とのそれぞれに、前記バンドストップフィルタ11及びカルマンフィルタ12のフィルタリング処理を施すので、移動体Aの移動動作に伴ってVx(t),Vy(t),X(t),Y(t)の時系列に含まれる第1振動周波数f1及び第2振動周波数f2の振動成分と、高周波側のノイズ成分とが効果的に除去される。

このため、フィルタリング処理部5から出力される移動体Aの移動速度の推定値Vx_f2(t),Vy_f2(t)及び位置の推定値X_f2(t),Y_f2(t)、特に、移動速度の推定値Vx_f2(t),Vy_f2(t)は、振動成分が効果的に抑制される。

例えば、図5に示すように、移動体A(ここでは人A1)をX軸方向に直進させた場合、瞬時移動速度の推定値Vx(t)の波形(実線のグラフ)は、定常移動状態でも、短期的な変動が比較的大きく生じる波形となる。これに対して、フィルタリング処理部5のフィルタリング処理後の移動速度の推定値Vx_f2(t)の波形(破線のグラフ)は、滑らかなものとなる。特に、移動体Aの定常移動状態では、Vx_f2(t)がほぼ一定に維持される。

また、例えば図6Aに示すように8の字状の経路で移動体A(ここでは人A1)を移動させた場合、移動体Aの位置の計測値(X(t), Y(t))の軌跡と、X軸方向の瞬時移動速度の推定値Vx(t)の波形と、Y軸方向の瞬時移動速度の推定値Vy(t)の波形とは、それぞれ、図6A、図6B、図6Cに実線のグラフで示すように、顕著な局所的もしくは短期的な変動を生じるものとなる。

これに対して、フィルタリング処理部5のフィルタリング処理後の移動体Aの位置の推定値(X_f2(t), Y_f2(t))の軌跡と、移動体AのX軸方向の移動速度の推定値Vx_f2(t)の波形と、Y軸方向の移動速度の推定値Vy_f2(t)の波形とは、それぞれ、図6A、図6B、図6Cには破線のグラフで示すように、滑らかに変化するものとなる。

また、フィルタリング処理部5では、移動体Aの移動動作に伴って移動体Aの瞬時移動速度の時系列に含まれる第1振動周波数f1の振動成分と、第2振動周波数f2の振動成分とをバンドストップフィルタ11により除去することで、図5、あるいは、図6B、図6Cを参照して判るように、瞬時移動速度の推定値Vx(t),Vy(t)の変化(中心値の変化)に対して、高い追従性でフィルタリング処理後の移動速度の推定値Vx_f2(t),Vy_f2(t)を追従させることができる。

[第2実施形態] 次に、本発明の第2実施形態を図7〜図9を参照して説明する。なお、本実施形態は、振動成分情報推定部4の処理、及びフィルタリング処理部5におけるバンドストップフィルタ11の処理だけが第1実施形態と相違する。このため、第1実施形態と同一の事項については説明を省略する。

本実施形態では、第1振動周波数f1及び第2振動周波数f2をそれぞれ第1バンドストップフィルタ11a、第2バンドストップフィルタ11bにより除去するにあたって、f1、f2のそれぞれの近辺の周波数の振動成分も併せて除去するように、第1バンドストップフィルタ11a、第2バンドストップフィルタ11bが構成される。

すなわち、本実施形態では、第1バンドストップフィルタ11a及び第2バンドストップフィルタ11bのそれぞれは、例えば、図7に示すような特性で、除去対象の周波数fc(=f1又はf2)を中心とする所定の帯域幅Δf(fcHからfcLまでの範囲の周波数幅)の振動成分を、移動体Aの瞬時移動速度の推定値Vx(t),Vy(t)及び位置の計測値X(t),Y(t)から除去するフィルタとして構成される。

この場合、除去対象の各周波数fc=f1又はf2に対応する帯域幅Δfの、低周波側の限界値(下限値)fcLと、高周波側の限界値(上限値)fcHとは、該帯域幅Δf内でのスペクトル強度(前記第1実施形態で説明したスペクトル分布データでのスペクトル強度)が、除去対象の周波数fcでのスペクル強度(ピーク値)に比較的近い大きさとなるように、振動成分情報推定部4により前記スペクトル分布データに基づいて、あらかじめ決定される。

一例として、例えば、図8に示すように、スペクトル分布データにおける除去対象の周波数fc(=f1又はf2)でのスペクトル強度から3dBだけスペクトル強度が低くなるfcの両側の周波数fcL,fcHが、上記帯域幅Δfの下限値及び上限値として決定され得る。

あるいは、例えば、fcL=fc−Δf/2、fcH=fc+Δf/2というように、fcL,fcHを決定することも可能である。

そして、振動成分情報推定部4は、このように決定した帯域幅Δfを示す情報を除去対象指示情報として、フィルタリング処理部5に出力する。この場合の除去対象指示情報は、例えば、除去対象の周波数fc(=f1又はf2)に対応する帯域幅Δfの下限値及び上限値の組、あるいは、該帯域幅Δfの大きさ(=上限値−下限値)と、該帯域幅Δfの中央値の周波数との組等により構成され得る。

また、フィルタリング処理部5は、第1バンドストップフィルタ11a、第2バンドストップフィルタ11bが、振動成分情報推定部4から与えられた振動成分情報により示される帯域幅Δf(f1,f2にそれぞれ対応する帯域幅Δf)での振動成分を除去するフィルタとなるように、それぞれのバンドストップフィルタ11a,11bの周波数通過特性を設定する。

このような第1バンドストップフィルタ11a及び第2バンドストップフィルタ11bのそれぞれは、例えば、図9に示すように、ローパスフィルタLPFとハイパスフィルタHPFとを組み合わせて構成することができる。

この場合、ローパスフィルタLPFの伝達関数GL(S)と、ハイパスフィルタHPFの伝達関数GH(S)とは、それぞれ例えば、次式(5a),(5b)により設定できる。

GL(S)=ωcL²/(S²+2・ζ・ωcL・S+ωcL²) ……(5a) GH(S)=S²/(S²+2・ζ・ωcH・S+ωcH²) ……(5b) なお、ωcL=2・π・fcL、ωcH=2・π・fcH、ζは定数値である。

本実施形態では、フィルタリング処理部5は、このように構成された第1バンドストップフィルタ11a及び第2バンドストップフィルタ11bのフィルタリング処理を、移動体Aの瞬時移動速度の推定値Vx(t),Vy(t)及び位置の計測値X(t),Y(t)に逐次施す。

これにより、Vx(t),Vy(t)及びX(t),Y(t)の時系列から、第1振動周波数f1を含む帯域幅Δf内の周波数の振動成分と、第2振動周波数f2を含む帯域幅Δf内の周波数の振動成分とが除去される。

本実施形態は、以上説明した事項以外は、前記第1実施形態と同じである。

かかる本実施形態においても、第1実施形態と同様の効果を奏することができる。また、移動体Aの実際の歩行周期に対応する周波数が、振動成分情報推定部4で決定された第1振動周波数f1から若干ずれても、移動体Aの実際の歩行周期に対応する周波数の振動成分と、その2倍の周波数の振動成分とを高い信頼性で除去することができる。

[第3実施形態] 次に、本発明の第3実施形態を図10を参照して説明する。なお、本実施形態は、振動成分情報推定部4の処理と、フィルタリング処理部5におけるバンドストップフィルタ11との処理だけが第1実施形態と相違する。このため、第1実施形態と同一の事項については説明を省略する。

図10を参照して、本実施形態では、振動成分情報推定部4には、位置計測部2から移動体Aの高さ(Z軸方向の位置)の計測値Z(t)が逐次入力される。

ここで、人A1等の脚式の移動体Aは、移動体Aの移動動作に該移動体Aの上体又は基体が上下動(上下に振動)する。そして、該移動体Aの高さの振動周波数は、移動体Aの歩行周期に対応する前記第1振動周波数f1の2倍の振動周波数(=第2振動周波数f2)に一致する。

そこで、本実施形態では、振動成分情報推定部4は、振動成分情報推定部4は、移動体Aの高さ(Z軸方向の位置)の計測値Z(t)の時系列から、一定時間毎に、移動体Aの高さの振動周波数(又は振動周期)の1/2の振動周波数を前記第1振動周波数f1として推定する。この場合、例えば、計測値Z(t)の時系列をフーリエ変換してなるスペクトル分布データから移動体Aの高さの振動周波数(又は振動周期)を推定できる。あるいは、例えば、計測値Z(t)がピーク値に達してから次にピーク値に達するまでの時間間隔を推定し、該時間間隔の推定値から、移動体Aの高さの振動周波数(又は振動周期)を推定することも可能である。

そして、振動成分情報推定部4は、上記の如く推定した第1振動周波数f1と、その2倍の振動周波数f2とを除去対象の振動成分の周波数として示す除去対象指示情報をフィルタリング処理部5に出力する。

なお、本実施形態では、移動体Aの移動開始直後で、移動体Aの高さの振動周波数(又は振動周期)が未だ推定されていない状態では、振動成分情報推定部4は、あらかじめ定められた除去対象指示情報(前記第1振動周波数f1及び第2振動周波数f2の暫定推定値等)を出力する。

本実施形態は、以上説明した事項以外は、前記第1実施形態と同じである。

かかる本実施形態においても、第1実施形態と同様の効果を奏することができる。また、バンドストップフィルタ11での除去対象の振動成分の周波数f1,f2は、移動体Aの実際の移動状態に整合するように定期的に更新されるので、移動体Aの移動状態の変化に合わせて、当該除去対象の振動成分の周波数を変化させることができる。

[第4実施形態] 次に、本発明の第4実施形態を説明する。なお、本実施形態は、振動成分情報推定部4の処理と、フィルタリング処理部5におけるバンドストップフィルタ11との処理だけが第1実施形態と相違する。このため、第1実施形態と同一の事項については説明を省略する。

図11を参照して、本実施形態では、振動成分情報推定部4には、フィルタリング処理部5から出力される移動体Aの移動速度の推定値Vx_f2(t),Vy_f2(t)が逐次フィードバックされる。そして、振動成分情報推定部4は、フィードバックされたVx_f2(t),Vy_f2(t)から、該移動体Aの移動速度の大きさと、歩行周期又はこれに対応する振動周波数(すなわち、前記第1振動周波数f1)との関係を示す相関データとしてあらかじめ作成された相関データを用いて、第1振動周波数f1(又はこれに対応する振動周期)を推定する。

ここで、例えば移動体Aが人A1である場合、歩行周期に対応する第1振動周波数f1と、移動体Aの進行方向の移動速度の大きさ|V|との間の関係は、近似的に次式(6)で表現できる。

f1=(2・|V|−239)/183 ……(6) なお、f1の単位は、[Hz]、|V|の単位は、[m/min]である。

そこで、振動成分情報推定部4は、フィードバックされた移動体Aの移動速度の推定値Vx_f2(t),Vy_f2(t)の大きさ|V|を算出し、この|V|の算出値から上記式(6)の演算を行うことで、移動体Aの歩行周期に対応する第1振動周波数f1を逐次算出する。そして、振動成分情報推定部4は、このように算出した第1振動周波数f1及びその2倍の振動周波数f2とを示す除去対象指示情報をフィルタリング処理部5に逐次出力する。

この場合、フィルタリング処理部5のバンドストップフィルタ11は、第1実施形態と同様に、除去対象指示情報により規定される第1振動周波数f1の振動成分と、その2倍の周波数の第2振動周波数f2の振動成分とを除去対象として、フィルタリング処理を実行する。

なお、振動成分情報推定部4は、フィルタリング処理部5から出力される移動体Aの移動速度の推定値Vx_f2(t),Vy_f2(t)が安定するまでは、第1振動周波数f1及び第2振動周波数f2の既定の暫定推定値を示す除去対象指示情報をフィルタリング処理部5に出力する。

かかる本実施形態においても、第1実施形態と同様の効果を奏することができる。また、振動成分情報推定部4は、フーリエ変換処理等を実行することなく、移動体Aの歩行周期に対応する第1振動周波数f1を即座に求めることができるので、移動体Aの移動状態の変化に合わせた高い応答性で、バンドストップフィルタ11での除去対象の振動成分の周波数を変化させることができる。

[第5実施形態] 次に、本発明の移動体の制御装置の一実施形態を第5実施形態として説明する。本実施形態では、人、移動ロボット等の複数の移動体が移動を行う動作環境空間において、アクチュエータにより移動動作が行われる制御対象の移動体(例えば移動ロボット)の移動を制御する。

上記動作環境空間は、本実施形態では、図1に示したようにインフラセンサ100が設置された空間である。また、制御対象の移動体は、例えば図1に示した脚式移動ロボットA2である。ただし、制御対象の移動体は、脚式移動ロボットA2に限られず、例えば、車輪式の走行体等であってもよい。

図12は本実施形態における制御装置51を示している。この制御装置51は、コンピュータ、あるいは電子回路ユニット、あるいはこれらを組み合わせた装置により構成される。この場合、制御装置51は、相互に連携して動作可能な複数のコンピュータ、あるいは、複数の電子回路ユニットを含み得る。

この制御装置51には、前記インフラセンサ100の計測信号が入力される。そして、該制御装置51は、実装されるプログラムあるいはハードウェア構成により実現される機能として、動作環境空間に存在する各移動体の移動速度等を推定する機能部たる前記移動体運動推定装置1と、各移動体の将来位置を予測する将来位置予測部52と、動作環境空間において制御対象の移動体A2を移動させ得る移動可能領域を推定する移動可能領域推定部53と、障害物もしくは他の移動体と干渉しないように該移動体A2の移動軌道を制御する移動軌道制御部54とを備える。

なお、移動体運動推定装置1は、前記第1〜第4実施形態のいずれの形態のものであってよい。

将来位置予測部52は、移動体運動推定装置1により前記した如く逐次生成される各移動体Aの移動速度の推定値Vx_f2(t),Vy_f2(t)及び位置の推定値X_f2(t),Y_f2(t)とを用いて該移動体Aの将来位置を予測する。

具体的には、将来位置予測部52は、移動体A毎に、例えば次式(10a),(10b)により現在時刻tから任意の時間ΔT後の移動体Aの将来位置(X(t+ΔT),Y(t+ΔT))を予測する。

X(t+ΔT)=X_f2(t)+ΔT・Vx_f2(t) ……(10a) Y(t+ΔT)=Y_f2(t)+ΔT・Vy_f2(t) ……(10b) この例では、移動体Aがグローバル座標系のX軸方向及びY軸方向の現在の移動速度の推定値Vx_f2(t),Vy_f2(t)により規定される速度ベクトルを維持して移動すると見なして、該移動体Aの将来位置が予測される。

なお、移動体Aの将来位置は、例えば、現在時刻以前の一定期間における該移動体Aの位置又は移動速度の履歴を反映させて予測してもよい。例えば、移動体Aの現在時刻以後の移動経路を、現在時刻以前の一定期間における移動経路の曲率に整合させて湾曲させるように、移動体Aの将来位置を予測してもよい。あるいは、現在時刻以前の一定期間における移動体Aの移動加速度又は減速度に合わせて、現在時刻以後の移動体Aの加速もしくは減速が行われるように、移動体Aの将来位置を予測してもよい。

移動可能領域推定部53は、前記インフラセンサ100から得られる測距データを基に、障害物の存在領域(各移動体Aの存在領域を除く)を推定し、該障害物の存在領域以外の領域を制御対象の移動体A2の移動可能領域として推定する。

該移動可能領域推定部53の処理は、次のように実行される。移動可能領域推定部53は、図13のフローチャートに示す処理を所定の演算処理周期で実行する。なお、以降の説明では、各インフラセンサ100(本実施形態では、前記レーザ式測距センサ)のレーザビームの各出力方向で測距データが得られる点(レーザビームの反射点)を測距点という。各測定点は、レーザビームの各出力方向に対応づけられた点である。

移動可能領域推定部53は、各インフラセンサ100毎に、STEP1〜6の処理を実行する。

STEP1において、移動可能領域推定部53は、前記移動体運動推定装置1の位置計測部2から、インフラセンサ100の各測距点の位置データ(グローバル座標系で見た位置の計測値。以降、計測位置データという)を取得する。

この場合、計測位置データは、前記位置計測部2において、例えば次のように生成される。すなわち、インフラセンサ100の出力により示される各測定点における距離計測値と、該測定点に対応するレーザビームの出力方向とから、インフラセンサ100に対して設定されたセンサ座標系で見た該測定点の位置が求められる。そして、この位置を、センサ座標系からグローバル座標系に座標変換することによって、グローバル座標系における各測定点の計測位置データが得られる。

次いで、STEP2において、移動可能領域推定部53は、移動体運動推定装置1により推定された各移動体Aの現在位置(X_f2(t),Y_f2(t))を該移動体運動推定装置1から取得する。

次いで、STEP3において、移動可能領域推定部53は、移動体Aの存在領域を障害物の存在領域から除外するための移動体位置キャンセル処理を実行する。

この移動体位置キャンセル処理は、各インフラセンサ100の全ての測距点のうち、正常に測距データが得られた各測定点毎に、図14のフローチャートに示す如く実行される。以降、この処理の対象とする個々の測定点を対象測距点という。

移動体位置キャンセル処理では、移動可能領域推定部53は、STEP11において、対象測距点が、移動体Aの存在領域の点であるか否かをON、OFFで示すフラグF1の値と、該対象測距点に対応する背景点が、移動体Aの存在領域の外側の領域のうち、インフラセンサ100から見て移動体Aにより遮られたオクルージョン領域の点であるか否かをON、OFFで示すフラグF2の値をそれぞれOFFに初期化する。該オクルージョン領域はインフラセンサ100による計測を行うことができない計測不能領域(死角領域)である。

ここで、上記「背景点」について補足すると、本実施形態の説明では、固定配置された設置物以外の障害物(適宜、移動され得る障害物)と、移動体とが存在しない状態の動作環境空間における床面と、これに固定配置された設置物とを合わせたものを背景部と称する。そして、各測距点に対応する出力方向のレーザビームの中心線が上記背景部に突き当たる点を、該測距点に対応する背景点と称する。インフラセンサ100のレーザビームの各出力方向において、該インフラセンサ100と背景部との間に物体が存在しない場合には、該出力方向に対応する測距点は、これに対応する背景点に一致するものとなる。

例えば、図16に例示するように、ある1つの測距点が、移動体A上の点P1である場合、その測距点P1に対応する背景点は、図16中の点P1bとなる。また、測距点が、背景部上の点P2である場合に、該測距点P2は、これに対応する背景点P2bに一致するものとなる。

本実施形態では各インフラセンサ100のレーザビームの各出力方向に対応する背景点の位置データ(グローバル座標系で見た位置)があらかじめ計測されている。そして、その各背景点の位置データが制御装置51あるいは該制御装置51と通信可能な外部のサーバの記憶装置にあらかじめ保存されている。

次に、STEP12において、移動可能領域推定部53は、対象測距点が、キャンセル対象点であるか否かを判定するキャンセル対象点判定処理を実行する。ここで、キャンセル対象点は、移動体Aの存在領域内に位置する測距点(前記フラグF1がONに設定されるべき測距点)である。該キャンセル対象点は、その位置データが後述する処理により計測位置データと異なる位置データに変更される測距点である。

上記キャンセル対象点判定処理は、図15のフローチャートに示す如く実行される。このキャンセル対象点判定処理は、STEP2で位置(X_f2(t),Y_f2(t))を取得した全ての移動体A(移動体運動推定装置1によって、移動速度の推定等が行われた全ての移動体)のそれぞれについて順番に行われる。従って、当該キャンセル対象点判定処理についての以下の説明では、移動体Aは、上記全ての移動体Aのうちの任意の1つの移動体Aを意味する。

キャンセル対象点判定処理では、移動可能領域推定部53は、STEP21において、移動体Aの現在位置と、対象測距点との間の距離Lp(2次元平面(XY座標面)上での水平距離)を算出する。この距離Lpは、該移動体AについてSTEP2で取得した現在位置(X_f2(t),Y_f2(t))の値と、対象測距点についてSTEP1で取得した位置データのうちのX軸方向及びY軸方向の座標位置とから算出される。

次いで、STEP22において、移動可能領域推定部53は、距離Lpが、所定距離rsよりも小さいか否かを判断する。この判断は、対象測距点が移動体A上の点であるか否かを判断するための処理である。すなわち、本実施形態では、図17に示すように、移動体運動推定装置1によって推定された移動体Aの現在位置(X_f2(t),Y_f2(t))から所定距離rs内の測距点(図17の円C内の測距点)を、該移動体A上の測距点とみなす。

この場合、本実施形態では、所定距離rsは、移動体Aが1つのインフラセンサ100のみによって測距されたものである場合(1つのインフラセンサ100の計測対象領域のうち、隣接する他のインフラセンサ100の計測対象領域と重ならない領域に移動体Aが存在する場合。以降、第1の場合という)と、移動体Aが隣り合う2つインフラセンサ100,100により測距されたものである場合(隣合うインフラセンサ100,100の計測対象領域が重なり合う領域に移動体Aが存在する場合。以降、第2の場合という)とで、異なる値に設定される。

第1の場合には、上記所定距離rsは、移動体Aの最大幅の1/2の長さよりも若干大きい(所定量だけ大きい)値に設定される。

一方、第2の場合には、隣り合うインフラセンサ100,100の計測タイミングのずれに起因して、これらのインフラセンサ100,100の計測データを統合して得られる移動体Aの位置の推定値の誤差が、第1の場合よりも大きくなる場合がある。

このため、第2の場合には、上記所定距離rsは、第1の場合よりも若干大きい値に設定される。

このように、STEP22では、上記第1の場合と第2の場合とで異ならせた所定距離rsの値を用いて、Lp

STEP22の判断結果が否定的である場合には、移動可能領域推定部53は、対象測距点がキャンセル対象点ではないとみなす。この場合には、移動可能領域推定部53は、該対象測距点に対応する前記フラグF1,F2の値を変更することなく(F1=OFF、F2=OFFに維持したまま)、キャンセル対象点判定処理を終了する。

また、STEP22の判断結果が肯定的である場合には、移動可能領域推定部53は、対象測距点がキャンセル対象点であるとみなす。この場合には、移動可能領域推定部53は、STEP23において、該対象測距点に対応する前記フラグF1の値をONに設定する。

さらに、移動可能領域推定部53は、対象測距点に対応する背景点が、移動体Aの存在領域の外側にて、該移動体Aにより遮られたオクルージョン領域の点であるか否かを判断するために、STEP24において、移動体Aの現在位置と、対象測距点に対応する背景点との距離Lc(2次元平面(XY座標面)上での水平距離)を算出する。

この距離Lcは、該移動体AについてSTEP2で取得した現在位置(X_f2(t),Y_f2(t))の値と、対象測距点に対応する背景点の位置データ(記憶装置にあらかじめ保存された位置データ)のうちのX軸方向及びY軸方向の座標位置とから算出される。

そして、移動可能領域推定部53は、STEP25において、上記距離Lcの算出値が、前記所定距離rs(STEP22の判断処理で使用した所定距離rs)よりも小さいか否かを判断する。

このSTEP25の判断結果が肯定的となる場合には、対象測距点(移動体A上の測距点)に対応する背景点は、移動体Aの存在領域内の点(例えば図16における点P1b)である。この場合には、移動可能領域推定部53は、対象測距点に対応する前記フラグF2の値を変更することなく(OFFに維持したまま)、キャンセル対象点判定処理を終了する。

また、STEP25の判断結果が否定的となる場合には、対象測定点(移動体A上の測距点)に対応する背景点は、移動体Aの存在領域の外側のオクルージョン領域内の点(例えば図16における点P3b)である。この場合には、移動可能領域推定部53は、STEP26において、前記フラグF2の値をONに設定した後、キャンセル対象点判定処理を終了する。

図14に戻って、移動可能領域推定部53は、上記の如くキャンセル対象点判定処理を実行した後、STEP13において、フラグF1の値がONであるか否かを判断する。

この判断結果が否定的である場合(F1=OFFである場合)には、移動可能領域推定部53は、STEP14において、対象測距点についてSTEP1で取得した今回の計測位置データをそのまま最新の位置データ(現在の演算処理周期で決定する位置データ)として記憶装置に保存する。

またSTEP13の判断結果が肯定的である場合(F1=ONである場合)には、移動可能領域推定部53は、次に、STEP15において、フラグF2の値がONであるか否かを判断する。

この判断結果が否定的である場合(F2=OFFである場合)には、対象測距点に対応する背景点は、移動体Aの存在領域内の点であるから、障害物が存在しない領域内の点とみなし得る。そこで、この場合には、移動可能領域推定部53は、STEP16において、対象測距点の最新の位置データとして、該対象測距点に対応する背景点の位置データを保存する。

一方、STEP15の判断結果が肯定的である場合(F2=ONである場合)には、対象測距点に対応する背景点は、障害物が存在しない領域であるか否かは不明である。

そこで、この場合には、移動可能領域推定部53は、STEP17において、対象測距点の最新の位置データとして、前回の演算処理周期にて、該対象測距点について決定した位置データ(前回位置データ)を保存する。

図13に戻って、以上の如く移動体位置キャンセル処理を実行した後、移動可能領域推定部53は、次にSTEP4において、インフラセンサ100の各測定点が、床面(ほぼ平坦な面)であるか否かを判別する処理(床面判別処理)を実行する。

この場合、各測距点の位置データのうちの高さデータ(グローバル座標系のZ軸方向の座標位置)が、床面の高さ位置に対応するものとしてあらかじめ定められた所定範囲内にある場合に、該測距点が床面上の点であると判断される。

次いで、移動可能領域推定部53は、STEP5においてノイズ成分を補償するノイズ処理を実行した後、STEP6において、グリッドデータを作成するグリッド変換処理を実行する。

ここで、グリッドというのは、動作環境空間の床(障害物を含めた床)のうちのインフラセンサ100の計測対象領域(詳しくは、水平な2次元平面(XY座標平面)に投影して見た計測対象領域)を、図18に示すように、複数の小面積の領域(図示例では方形領域)に仮想的に区分けした場合における各領域を意味する。そして、上記グリッドデータは、各グリッド(小面積の領域)が、床面の領域と、非床面の領域(障害物の存在領域)と、該床面の領域であるか、非床面の領域であるかの判別ができない未知の領域とのいずれの領域であるかを示す情報である。

この場合、本実施形態では、各グリッドは、その内部に、有効な(正常な測距データが得られた)測距点が存在しない場合には、未知の領域のグリッドとされる。また、各グリッドは、その内部に有効な測距点を持つ場合には、該グリッド内の測距点の全数のうち、床面上の測距点と判断された測距点の個数の割合が、所定割合(例えば100%)以上である場合に、床面の領域とされ、そうでない場合に、非床面の領域とされる。なお、床面の領域は、制御対象の移動体A2の移動可能領域である。

以上説明したSTEP1〜6の処理により、各インフラセンサ100毎のグリッドデータが作成される。

次いで、移動可能領域推定部53は、STEP7において、各インフラセンサ100毎のグリッドデータ統合してなる統合グリッドデータを作成する。

この場合、隣り合うインフラセンサ100,100の計測対象領域が重なり合う領域以外の領域(1つのインフラセンサ100のみに対応する領域)では、前記STEP6で設定されたグリッドデータがそのまま、統合グリッドデータとして設定される。

また、隣り合うインフラセンサ100,100の計測対象領域が重なり合う領域では、両方のインフラセンサ100,100に対応するグリッドデータが同じであるグリッドについては、該グリッドに当該同じグリッドデータが設定される。

また、両方のインフラセンサ100,100に対応するグリッドデーが異なるグリッドについては、一方のインフラセンサに対応するグリッドデータが床面の領域であることを示すと共に他方のインフラセンサに対応するグリッドデータが未知の領域であることを示スデータである場合に、当該グリッドが床面の領域とされ、これ以外の場合には、当該グリッドが非床面の領域とされる。

次いで、移動可能領域推定部53は、STEP8において、統合グリッドデータから制御対象の移動体A2の移動可能領域を抽出する。この場合、統合グリッドデータのうちの床面の領域とされたグリッドの全体が移動可能領域として抽出される。なお、移動可能領域以外の領域は、そこでの移動体A2の移動が禁止される移動不可領域となる。

以上が、移動可能領域推定部53の処理である。

次に、移動軌道制御部54は、上記の如く移動可能領域推定部53が求めた移動可能領域と、前記将来位置予測部52により予測される移動体A(制御対象の移動体A2以外の移動体)の将来位置とを用いて移動対象の移動体A2の目標移動軌道を逐次設定する。

この場合、制御対象の移動体A2の目標移動軌道は、移動可能領域でない移動不可領域と、他の移動体Aの将来位置とに抵触しない領域で制御対象の移動体A2を移動させるように設定される。

すなわち、目標移動軌道は、それに従って制御対象の移動体A2を移動させた場合に、その移動途中に、制御対象の移動体A2が移動不可領域の障害物と接触したり、あるいは、他の移動体Aと接触することがないよう生成される。

一例として、例えば図19に示すように、制御対象の移動体A2を目的地まで直線的に移動させた場合に、その移動途中で、他の移動体Aと接触することが予測された場合には、他の移動体Aの予測軌道上での該移動体Aの存在領域を迂回し、且つ、図外の移動不可領域を避けるように、目標移動軌道が作成される。なお、目標移動軌道は、他の移動体Aの移動状況等に応じて適宜修正される。そして、これに伴い、制御対象の移動体A2の目的地への到達時刻も適宜修正される。

そして、移動軌道制御部54は上記の如く設定した目標移動軌道を制御対象の移動体A2に指示する。このとき、制御対象の移動体A2は、指示された目標移動軌道で移動するように、自身の動作をアクチュエータを介して制御する。

補足すると、目標移動軌道のより具体的な生成手法としては、例えば、前記特許文献1に記載されている手法等を採用できる。

以上説明した第5実施形態によれば、制御対象の移動体A2を、他の移動体Aや、障害物と干渉させることなく所要の目的値に移動させることができる。この場合、各移動体Aの移動速度が、前記移動体運動推定装置1により前記した如く推定されるので、将来位置予測部52は、他の移動体Aの将来位置を高い信頼性で予測することができる。この結果、制御対象の移動体A2の目標移動軌道の信頼性を高めることができる。

また、移動可能領域推定部53の処理では、移動体Aがインフラセンサ100と床との間に存在することによって計測不能となるオクルージョン領域における測距点の位置データは、前回位置データとされる。このため、オクルージョン領域における測距点の位置データは、過去に計測された位置データの内の最新の位置データとされる。

そして、この場合、前回位置データが移動可能領域に該当するものである場合には、オクルージョン領域を移動可能領域として得ることができることとなる。このため、オクルージョン領域を移動可能領域として利用することが可能となり、移動可能領域が必要以上に狭くなるのを防止することができる。

なお、本発明は、以上説明した実施形態に限定されるものではない。以下に、いくつかの変形態様を説明しておく。

本発明における移動体は、脚式移動体以外の移動体であってもよい。移動体が脚式移動体でない場合であっても、該移動体の運動の仕方等によっては、該移動体の周期的な動作に伴って、該移動体の瞬時移動速度に周期的な振動成分を含まれる場合もある。例えば、車椅子をこれに搭乗している人が動かす場合には、該車椅子の移動速度が周期的な振動を呈する場合が多い。従って、このような場合に、本発明の移動速度推定装置を適用し得る。

前記実施形態では、X軸及びY軸の2軸方向における移動体の移動速度及び位置をフィルタリング処理部5を介して推定した。ただし、例えば移動体の位置の推定に関しては、フィルタリング処理部5によるフィルタリング処理を省略するようにしてもよい。

また、1軸方向の移動速度だけを推定したり、あるいは、3軸方向の(3次元的な)移動速度を推定することも可能である。例えば、人、脚式移動ロボット等の脚式移動体が階段を昇降するような場合に、3軸方向の移動速度を推定するようにしてもよい。

また、前記第1実施形態および第2実施形態では、例えば、移動体の移動中に、定期的に、瞬時移動速度の推定値の波形データを振動成分情報推定部4に入力して、第1振動周波数f1、及び第2振動周波数f2の推定値を適宜更新する(ひいては、除去対象指示情報を更新する)ようにしてもよい。

1…移動体運動推定装置(移動速度推定装置)、2…位置計測部、3…瞬時速度推定部、4…振動成分情報推定部、5…フィルタリング処理部(速度推定値調整処理部)、11…バンドストップフィルタ、12…カルマンフィルタ、51…制御装置、52…将来位置予測部、53…移動可能領域推定部、54…移動軌制御部、A,A1,A2…移動体。