JP2016206100A - 移動体の移動速度推定装置及び移動体の制御装置 - Google Patents

Abstract

【課題】周期的な移動動作に伴う周期的な振動成分を含む速度で移動する移動体の移動速度を適切に推定する。【解決手段】移動体の周期的な移動動作に伴い移動体の瞬時移動速度に含まれる周期的な振動成分の振動周期又は振動周波数を振動成分情報推定部４により推定する。移動体の位置の計測値の時系列から推定した瞬時移動速度の時系列に、振動周期又は振動周波数の推定値に対応する振動成分を除去する処理を少なくとも実行することにより得られる速度値を移動体の移動速度の推定値として逐次生成する速度推定値調整処理部５を備える。【選択図】図２

Description

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

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

特開２０１２−１２８５８５号公報

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

本発明の一実施形態における移動体の動作環境空間を概略的に示す図。 第１実施形態における移動体運動推定装置の機能を示すブロック図。 図３Ａは移動体の瞬時移動速度の波形の一例を示すグラフ、図３Ｂは図３Ａの波形を周波数領域に変換して得られるスペクトル分布データを示すグラフ。 図２に示すバンドスップフィルタの特性の一例を示すグラフ。 図２に示すフィルタリング処理部に入力される瞬時移動速度の推定値の波形と、該フィルタリング処理部から出力される移動速度の推定値の波形とを例示するグラフ。 図６Ａは、図２に示すフィルタリング処理部に入力される位置の計測値に対応する経路と、該フィルタリング処理部から出力される位置の推定値に対応する経路とを例示するグラフ、図６Ｂは、図２に示すフィルタリング処理部に入力されるＸ軸方向の瞬時移動速度の推定値の波形と、該フィルタリング処理部から出力されるＸ軸方向の移動速度の推定値の波形とを例示するグラフ、図６Ｃは、図２に示すフィルタリング処理部に入力されるＹ軸方向の瞬時移動速度の推定値の波形と、該フィルタリング処理部から出力されるＹ軸方向の移動速度の推定値の波形とを例示するグラフ。 第２実施形態におけるバンドストップフィルタの特性の一例を示すグラフ。 第２実施形態におけるバンドストップフィルタの特性値の設定手法を説明するためのグラフ。 第２実施形態におけるバンドストップフィルタの構成例を示すブロック図。 第３実施形態における移動速度推定装置の機能を示すブロック図。 第４実施形態における移動速度推定装置の機能を示すブロック図。 移動体の制御装置の一実施形態（第５実施形態）における該制御装置の機能を示すブロック図。 図１２に示す移動可能領域推定部の処理を示すフローチャート。 図１３のＳＴＥＰ３の処理を示すフローチャート。 図１４のＳＴＥＰ１２の処理を示すフローチャート。 図１２に示す移動可能領域推定部の処理に関する説明図。 図１２に示す移動可能領域推定部の処理に関する説明図。 図１２に示す移動可能領域推定部の処理に関する説明図。 図１２に示す移動軌道制御部の処理に関する説明図。

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

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

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

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

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

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

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

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

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

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

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

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

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

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

瞬時移動速度推定部３は、各移動体Ａの位置が位置計測部２により計測される毎に、各移動体Ａの位置の今回の計測値（Ｘ(t)，Ｙ(t)）と、前回の計測値（Ｘ(t-Δt)，Ｙ(t-Δt)）との差（位置偏差）を、それらの計測値のサンプリング周期Δtにより除算することにより、各移動体Ａの瞬時移動速度（Ｖx(t)，Ｖy(t)）を推定する。

すなわち、次式（１ａ），（１ｂ）により、Ｘ軸方向の瞬時移動速度Ｖx(t)と、Ｙ軸方向の瞬時移動速度Ｖy(t)とが算出される。

Ｖx(t)＝(Ｘ(t)−Ｘ(t-Δt))／Δt …（１ａ）
Ｖy(t)＝(Ｙ(t)−Ｙ(t-Δt))／Δt …（１ｂ）

振動成分情報推定部４は、瞬時移動速度推定部３から、各移動体Ａの瞬時移動速度の推定値の一例の時系列データ（一定時間内における瞬時移動速度の推定値の経時変化を示す時系列データ）を事前に取得しておく。該時系列データは、移動体Ａの瞬時移動速度に含まれる振動成分についての情報を取得するために、移動体Ａの移動を試験的に行ったときに、前記位置計測部２及び瞬時移動速度推定部３の処理により得られたデータである。そして、振動成分情報推定部４は、各移動体Ａの瞬時移動速度の推定値の時系列データを、ＦＦＴ等のフーリエ変換処理により、周波数領域に変換することで、スペクトル分布データを生成する。

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

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

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

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

周波数ｆ１は、定常移動状態の移動体Ａの歩行周期（２歩分の期間の時間幅）の振動成分に対応する周波数（例えば０．９Ｈｚ）、ｆ２，ｆ３は、それぞれｆ１の２倍、３倍の周波数である。ここで、脚式移動体である移動体Ａの移動動作では、該移動体Ａの進行方向（ここではＸ軸方向）における瞬時移動速度Ｖx(t)の時系列は、一般に、上記歩行周期の半分の周期（１歩分の期間の時間幅）の振動成分を多く含む。この半分の周期の振動成分の周波数が上記ｆ２である。

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

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

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

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

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

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

なお、以降の説明では、上記周波数ｆ１を第１振動周波数ｆ１、第１振動周波数ｆ１の２倍の周波数ｆ２を第２振動周波数ｆ２という。

フィルタリング処理部５は、バンドストップフィルタ１１によるフィルタリング処理と、カルマンフィルタ１２によるフィルタリング処理とを、瞬時移動速度推定部３から逐次入力される各移動体Ａの瞬時移動速度の推定値Ｖx(t)，Ｖy(t)と、位置計測部２から逐次入力される各移動体Ａの位置の計測値Ｘ(t)，Ｙ(t)とのそれぞれに施す。

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

さらに、フィルタリング処理部５は、第２振動周波数ｆ2よりも高周波域のノイズ成分（第１振動周波数ｆ1の３倍の周波数ｆ3を含む）をカルマンフィルタ１２により除去する。

このため、バンドストップフィルタ１１は、本実施形態では、第１振動周波数ｆ１の振動成分の除去用の第１バンドストップフィルタ１１ａ（図示省略）と、第２振動周波数ｆ２の振動成分の除去用の第２バンドストップフィルタ１１ｂ（図示省略）とにより構成される。

第１バンドストップフィルタ１１ａ及び第２バンドストップフィルタ１１ｂのそれぞれは、例えば、図４に示すような周波数通過特性で、除去対象の周波数ｆc（＝ｆ1又はｆ2）を除去するように構成される。この場合、除去対象の周波数ｆcでの振幅の減衰度合（入力強度に対する出力強度の減衰度合）がピークとなるように、第１バンドストップフィルタ１１ａ及び第２バンドストップフィルタ１１ｂの周波数通過特性が設定される。

このような特性は、各バンドストップフィルタ１１ａ，１１ｂの伝達関数Ｇ(S)を次式（２）により設定することで、実現される。

Ｇ(S)＝(Ｓ²＋ωc²)／(Ｓ²＋２・ζ・ωc・Ｓ＋ωc²) ……（２）

なお、ωc＝２・π・ｆc、ζは定数値である。

そして、各バンドストップフィルタ１１ａ，１１ｂをデジタルフィルタとして構成する場合の伝達関数Ｇ(Z)は、式（２）に基づいて、次式（３）により与えられる。

Ｇ(Z)＝(b0＋b1・Ｚ^-1＋b2・Ｚ^-2)／(a0＋a1・Ｚ^-1＋a2・Ｚ^-2) ……（３）
ただし、
a0＝ωc・Δt²＋４・ζ・ωc・Δt＋４
a1＝２・ωc²・Δt²−８
a2＝ωc・Δt²−４・ζ・ωc・Δt＋４
b0＝ωc・Δt²＋２・Δt
b1＝２・ωc²・Δt²
b2＝ωc・tΔt²−２・Δt

なお、Δtはサンプリング周期である。

フィルタリング処理部５は、上記の如く構成した第１バンドストップフィルタ１１ａ及び第２バンドストップフィルタ１１ｂのフィルタリング処理を、瞬時移動速度推定部３から逐次入力される各移動体Ａの瞬時移動速度の推定値（Ｖx(t)，Ｖy(t)）と、位置計測部２から逐次入力される各移動体Ａの位置の計測値（Ｘ(t)，Ｙ(t)）のそれぞれに施す。これにより、各移動体Ａの瞬時移動速度の推定値Ｖx(t)，Ｖy(t)及び位置の計測値Ｘ(t)，Ｙ(t)のそれぞれから第１振動周波数ｆ１の振動成分と、第２振動周波数ｆ２の振動成分とを除去してなるフィルタリング値Ｖx_f1(t)，Ｖy_f1(t)及びＸ_f1(t)，Ｙ_f1(t)が逐次求められる。

そして、フィルタリング処理部５は、さらにこれらのフィルタリング値Ｖx_f1(t)，Ｖy_f1(t)及びＸ_f1(t)，Ｙ_f1(t)にカルマンフィルタ１２のフィルタリング処理を施す。この場合、カルマンフィルタ１２は、前記したように、第２振動周波数ｆ２よりも高周波域のノイズ成分を除去するように構成されている。このようなカルマンフィルタ１２は、公知の形態で構成し得る。

このカルマンフィルタ１２のフィルタリング処理によって、前記バンドストップフィルタ１１により生成したフィルタリング値Ｖx_f1(t)，Ｖy_f1(t)及びＸ_f1(t)，Ｙ_f1(t)から、高周波域のノイズ成分をさらに除去してなるフィルタリング値Ｖx_f2(t)，Ｖy_f2(t)及びＸ_f2(t)，Ｙ_f2(t)が逐次求められる。

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

かかる本実施形態では、移動体Ａの瞬時移動速度の推定値（Ｖx(t)，Ｖy(t)）と、位置の計測値（Ｘ(t)，Ｙ(t)）とのそれぞれに、前記バンドストップフィルタ１１及びカルマンフィルタ１２のフィルタリング処理を施すので、移動体Ａの移動動作に伴ってＶx(t)，Ｖy(t)，Ｘ(t)，Ｙ(t)の時系列に含まれる第１振動周波数ｆ１及び第２振動周波数ｆ２の振動成分と、高周波側のノイズ成分とが効果的に除去される。

このため、フィルタリング処理部５から出力される移動体Ａの移動速度の推定値Ｖx_f2(t)，Ｖy_f2(t)及び位置の推定値Ｘ_f2(t)，Ｙ_f2(t)、特に、移動速度の推定値Ｖx_f2(t)，Ｖy_f2(t)は、振動成分が効果的に抑制される。

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

また、例えば図６Ａに示すように８の字状の経路で移動体Ａ（ここでは人Ａ１）を移動させた場合、移動体Ａの位置の計測値（Ｘ(t), Ｙ(t)）の軌跡と、Ｘ軸方向の瞬時移動速度の推定値Ｖx(t)の波形と、Ｙ軸方向の瞬時移動速度の推定値Ｖy(t)の波形とは、それぞれ、図６Ａ、図６Ｂ、図６Ｃに実線のグラフで示すように、顕著な局所的もしくは短期的な変動を生じるものとなる。

これに対して、フィルタリング処理部５のフィルタリング処理後の移動体Ａの位置の推定値（Ｘ_f2(t), Ｙ_f2(t)）の軌跡と、移動体ＡのＸ軸方向の移動速度の推定値Ｖx_f2(t)の波形と、Ｙ軸方向の移動速度の推定値Ｖy_f2(t)の波形とは、それぞれ、図６Ａ、図６Ｂ、図６Ｃには破線のグラフで示すように、滑らかに変化するものとなる。

また、フィルタリング処理部５では、移動体Ａの移動動作に伴って移動体Ａの瞬時移動速度の時系列に含まれる第１振動周波数ｆ１の振動成分と、第２振動周波数ｆ２の振動成分とをバンドストップフィルタ１１により除去することで、図５、あるいは、図６Ｂ、図６Ｃを参照して判るように、瞬時移動速度の推定値Ｖx(t)，Ｖy(t)の変化（中心値の変化）に対して、高い追従性でフィルタリング処理後の移動速度の推定値Ｖx_f2(t)，Ｖy_f2(t)を追従させることができる。

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

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

すなわち、本実施形態では、第１バンドストップフィルタ１１ａ及び第２バンドストップフィルタ１１ｂのそれぞれは、例えば、図７に示すような特性で、除去対象の周波数ｆc（＝ｆ1又はｆ2）を中心とする所定の帯域幅Δｆ（ｆcHからｆcLまでの範囲の周波数幅）の振動成分を、移動体Ａの瞬時移動速度の推定値Ｖx(t)，Ｖy(t)及び位置の計測値Ｘ(t)，Ｙ(t)から除去するフィルタとして構成される。

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

一例として、例えば、図８に示すように、スペクトル分布データにおける除去対象の周波数ｆc（＝ｆ1又はｆ2）でのスペクトル強度から３ｄＢだけスペクトル強度が低くなるｆcの両側の周波数ｆcL，ｆcHが、上記帯域幅Δｆの下限値及び上限値として決定され得る。

あるいは、例えば、ｆcL＝ｆc−Δｆ／２、ｆcH＝ｆc＋Δｆ／２というように、ｆcL，ｆcHを決定することも可能である。

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

また、フィルタリング処理部５は、第１バンドストップフィルタ１１ａ、第２バンドストップフィルタ１１ｂが、振動成分情報推定部４から与えられた振動成分情報により示される帯域幅Δｆ（ｆ1，ｆ2にそれぞれ対応する帯域幅Δｆ）での振動成分を除去するフィルタとなるように、それぞれのバンドストップフィルタ１１ａ，１１ｂの周波数通過特性を設定する。

このような第１バンドストップフィルタ１１ａ及び第２バンドストップフィルタ１１ｂのそれぞれは、例えば、図９に示すように、ローパスフィルタＬＰＦとハイパスフィルタＨＰＦとを組み合わせて構成することができる。

この場合、ローパスフィルタＬＰＦの伝達関数ＧL(S)と、ハイパスフィルタＨＰＦの伝達関数ＧH(S)とは、それぞれ例えば、次式（５ａ），（５ｂ）により設定できる。

ＧL(S)＝ωcL²／(Ｓ²＋２・ζ・ωcL・Ｓ＋ωcL²) ……（５ａ）
ＧH(S)＝Ｓ²／(Ｓ²＋２・ζ・ωcH・Ｓ＋ωcH²) ……（５ｂ）

なお、ωcL＝２・π・ｆcL、ωcH＝２・π・ｆcH、ζは定数値である。

本実施形態では、フィルタリング処理部５は、このように構成された第１バンドストップフィルタ１１ａ及び第２バンドストップフィルタ１１ｂのフィルタリング処理を、移動体Ａの瞬時移動速度の推定値Ｖx(t)，Ｖy(t)及び位置の計測値Ｘ(t)，Ｙ(t)に逐次施す。

これにより、Ｖx(t)，Ｖy(t)及びＸ(t)，Ｙ(t)の時系列から、第１振動周波数ｆ1を含む帯域幅Δｆ内の周波数の振動成分と、第２振動周波数ｆ2を含む帯域幅Δｆ内の周波数の振動成分とが除去される。

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

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

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

図１０を参照して、本実施形態では、振動成分情報推定部４には、位置計測部２から移動体Ａの高さ（Ｚ軸方向の位置）の計測値Ｚ(t)が逐次入力される。

ここで、人Ａ１等の脚式の移動体Ａは、移動体Ａの移動動作に該移動体Ａの上体又は基体が上下動（上下に振動）する。そして、該移動体Ａの高さの振動周波数は、移動体Ａの歩行周期に対応する前記第１振動周波数ｆ1の２倍の振動周波数（＝第２振動周波数ｆ2）に一致する。

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

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

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

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

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

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

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

ここで、例えば移動体Ａが人Ａ１である場合、歩行周期に対応する第１振動周波数ｆ１と、移動体Ａの進行方向の移動速度の大きさ｜Ｖ｜との間の関係は、近似的に次式（６）で表現できる。

ｆ1＝（２・｜Ｖ｜−２３９）／１８３ ……（６）

なお、ｆ1の単位は、[Ｈｚ]、｜Ｖ｜の単位は、[m／min]である。

そこで、振動成分情報推定部４は、フィードバックされた移動体Ａの移動速度の推定値Ｖx_f2(t)，Ｖy_f2(t)の大きさ｜Ｖ｜を算出し、この｜Ｖ｜の算出値から上記式（６）の演算を行うことで、移動体Ａの歩行周期に対応する第１振動周波数ｆ1を逐次算出する。そして、振動成分情報推定部４は、このように算出した第１振動周波数ｆ1及びその２倍の振動周波数ｆ2とを示す除去対象指示情報をフィルタリング処理部５に逐次出力する。

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

なお、振動成分情報推定部４は、フィルタリング処理部５から出力される移動体Ａの移動速度の推定値Ｖx_f2(t)，Ｖy_f2(t)が安定するまでは、第１振動周波数ｆ1及び第２振動周波数ｆ2の既定の暫定推定値を示す除去対象指示情報をフィルタリング処理部５に出力する。

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

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

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

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

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

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

将来位置予測部５２は、移動体運動推定装置１により前記した如く逐次生成される各移動体Ａの移動速度の推定値Ｖx_f2(t)，Ｖy_f2(t)及び位置の推定値Ｘ_f2(t)，Ｙ_f2(t)とを用いて該移動体Ａの将来位置を予測する。

具体的には、将来位置予測部５２は、移動体Ａ毎に、例えば次式（１０ａ），（１０ｂ）により現在時刻ｔから任意の時間ΔＴ後の移動体Ａの将来位置（Ｘ(t＋ΔT)，Ｙ(t＋ΔT)）を予測する。

Ｘ(t＋ΔT)＝Ｘ_f2(t)＋ΔT・Ｖx_f2(t) ……（１０ａ）
Ｙ(t＋ΔT)＝Ｙ_f2(t)＋ΔT・Ｖy_f2(t) ……（１０ｂ）

この例では、移動体Ａがグローバル座標系のＸ軸方向及びＹ軸方向の現在の移動速度の推定値Ｖx_f2(t)，Ｖy_f2(t)により規定される速度ベクトルを維持して移動すると見なして、該移動体Ａの将来位置が予測される。

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

移動可能領域推定部５３は、前記インフラセンサ１００から得られる測距データを基に、障害物の存在領域（各移動体Ａの存在領域を除く）を推定し、該障害物の存在領域以外の領域を制御対象の移動体Ａ２の移動可能領域として推定する。

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

移動可能領域推定部５３は、各インフラセンサ１００毎に、ＳＴＥＰ１〜６の処理を実行する。

ＳＴＥＰ１において、移動可能領域推定部５３は、前記移動体運動推定装置１の位置計測部２から、インフラセンサ１００の各測距点の位置データ（グローバル座標系で見た位置の計測値。以降、計測位置データという）を取得する。

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

次いで、ＳＴＥＰ２において、移動可能領域推定部５３は、移動体運動推定装置１により推定された各移動体Ａの現在位置（Ｘ_f2(t)，Ｙ_f2(t)）を該移動体運動推定装置１から取得する。

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

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

移動体位置キャンセル処理では、移動可能領域推定部５３は、ＳＴＥＰ１１において、対象測距点が、移動体Ａの存在領域の点であるか否かをＯＮ、ＯＦＦで示すフラグＦ１の値と、該対象測距点に対応する背景点が、移動体Ａの存在領域の外側の領域のうち、インフラセンサ１００から見て移動体Ａにより遮られたオクルージョン領域の点であるか否かをＯＮ、ＯＦＦで示すフラグＦ２の値をそれぞれＯＦＦに初期化する。該オクルージョン領域はインフラセンサ１００による計測を行うことができない計測不能領域（死角領域）である。

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

例えば、図１６に例示するように、ある１つの測距点が、移動体Ａ上の点Ｐ1である場合、その測距点Ｐ1に対応する背景点は、図１６中の点Ｐ1bとなる。また、測距点が、背景部上の点Ｐ2である場合に、該測距点Ｐ2は、これに対応する背景点Ｐ2bに一致するものとなる。

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

次に、ＳＴＥＰ１２において、移動可能領域推定部５３は、対象測距点が、キャンセル対象点であるか否かを判定するキャンセル対象点判定処理を実行する。ここで、キャンセル対象点は、移動体Ａの存在領域内に位置する測距点（前記フラグＦ１がＯＮに設定されるべき測距点）である。該キャンセル対象点は、その位置データが後述する処理により計測位置データと異なる位置データに変更される測距点である。

上記キャンセル対象点判定処理は、図１５のフローチャートに示す如く実行される。このキャンセル対象点判定処理は、ＳＴＥＰ２で位置（Ｘ_f2(t)，Ｙ_f2(t)）を取得した全ての移動体Ａ（移動体運動推定装置１によって、移動速度の推定等が行われた全ての移動体）のそれぞれについて順番に行われる。従って、当該キャンセル対象点判定処理についての以下の説明では、移動体Ａは、上記全ての移動体Ａのうちの任意の１つの移動体Ａを意味する。

キャンセル対象点判定処理では、移動可能領域推定部５３は、ＳＴＥＰ２１において、移動体Ａの現在位置と、対象測距点との間の距離Ｌｐ（２次元平面（ＸＹ座標面）上での水平距離）を算出する。この距離Ｌｐは、該移動体ＡについてＳＴＥＰ２で取得した現在位置（Ｘ_f2(t)，Ｙ_f2(t)）の値と、対象測距点についてＳＴＥＰ１で取得した位置データのうちのＸ軸方向及びＹ軸方向の座標位置とから算出される。

次いで、ＳＴＥＰ２２において、移動可能領域推定部５３は、距離Ｌｐが、所定距離ｒｓよりも小さいか否かを判断する。この判断は、対象測距点が移動体Ａ上の点であるか否かを判断するための処理である。すなわち、本実施形態では、図１７に示すように、移動体運動推定装置１によって推定された移動体Ａの現在位置（Ｘ_f2(t)，Ｙ_f2(t)）から所定距離ｒｓ内の測距点（図１７の円Ｃ内の測距点）を、該移動体Ａ上の測距点とみなす。

この場合、本実施形態では、所定距離ｒｓは、移動体Ａが１つのインフラセンサ１００のみによって測距されたものである場合（１つのインフラセンサ１００の計測対象領域のうち、隣接する他のインフラセンサ１００の計測対象領域と重ならない領域に移動体Ａが存在する場合。以降、第１の場合という）と、移動体Ａが隣り合う２つインフラセンサ１００，１００により測距されたものである場合（隣合うインフラセンサ１００，１００の計測対象領域が重なり合う領域に移動体Ａが存在する場合。以降、第２の場合という）とで、異なる値に設定される。

第１の場合には、上記所定距離ｒｓは、移動体Ａの最大幅の１／２の長さよりも若干大きい（所定量だけ大きい）値に設定される。

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

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

このように、ＳＴＥＰ２２では、上記第１の場合と第２の場合とで異ならせた所定距離ｒｓの値を用いて、Ｌｐ＜ｒｓであるか否かが判断される。

ＳＴＥＰ２２の判断結果が否定的である場合には、移動可能領域推定部５３は、対象測距点がキャンセル対象点ではないとみなす。この場合には、移動可能領域推定部５３は、該対象測距点に対応する前記フラグＦ１，Ｆ２の値を変更することなく（Ｆ１＝ＯＦＦ、Ｆ２＝ＯＦＦに維持したまま）、キャンセル対象点判定処理を終了する。

また、ＳＴＥＰ２２の判断結果が肯定的である場合には、移動可能領域推定部５３は、対象測距点がキャンセル対象点であるとみなす。この場合には、移動可能領域推定部５３は、ＳＴＥＰ２３において、該対象測距点に対応する前記フラグＦ１の値をＯＮに設定する。

さらに、移動可能領域推定部５３は、対象測距点に対応する背景点が、移動体Ａの存在領域の外側にて、該移動体Ａにより遮られたオクルージョン領域の点であるか否かを判断するために、ＳＴＥＰ２４において、移動体Ａの現在位置と、対象測距点に対応する背景点との距離Ｌｃ（２次元平面（ＸＹ座標面）上での水平距離）を算出する。

この距離Ｌｃは、該移動体ＡについてＳＴＥＰ２で取得した現在位置（Ｘ_f2(t)，Ｙ_f2(t)）の値と、対象測距点に対応する背景点の位置データ（記憶装置にあらかじめ保存された位置データ）のうちのＸ軸方向及びＹ軸方向の座標位置とから算出される。

そして、移動可能領域推定部５３は、ＳＴＥＰ２５において、上記距離Ｌｃの算出値が、前記所定距離ｒｓ（ＳＴＥＰ２２の判断処理で使用した所定距離ｒｓ）よりも小さいか否かを判断する。

このＳＴＥＰ２５の判断結果が肯定的となる場合には、対象測距点（移動体Ａ上の測距点）に対応する背景点は、移動体Ａの存在領域内の点（例えば図１６における点Ｐ1b）である。この場合には、移動可能領域推定部５３は、対象測距点に対応する前記フラグＦ２の値を変更することなく（ＯＦＦに維持したまま）、キャンセル対象点判定処理を終了する。

また、ＳＴＥＰ２５の判断結果が否定的となる場合には、対象測定点（移動体Ａ上の測距点）に対応する背景点は、移動体Ａの存在領域の外側のオクルージョン領域内の点（例えば図１６における点Ｐ3b）である。この場合には、移動可能領域推定部５３は、ＳＴＥＰ２６において、前記フラグＦ２の値をＯＮに設定した後、キャンセル対象点判定処理を終了する。

図１４に戻って、移動可能領域推定部５３は、上記の如くキャンセル対象点判定処理を実行した後、ＳＴＥＰ１３において、フラグＦ１の値がＯＮであるか否かを判断する。

この判断結果が否定的である場合（Ｆ１＝ＯＦＦである場合）には、移動可能領域推定部５３は、ＳＴＥＰ１４において、対象測距点についてＳＴＥＰ１で取得した今回の計測位置データをそのまま最新の位置データ（現在の演算処理周期で決定する位置データ）として記憶装置に保存する。

またＳＴＥＰ１３の判断結果が肯定的である場合（Ｆ１＝ＯＮである場合）には、移動可能領域推定部５３は、次に、ＳＴＥＰ１５において、フラグＦ２の値がＯＮであるか否かを判断する。

この判断結果が否定的である場合（Ｆ２＝ＯＦＦである場合）には、対象測距点に対応する背景点は、移動体Ａの存在領域内の点であるから、障害物が存在しない領域内の点とみなし得る。そこで、この場合には、移動可能領域推定部５３は、ＳＴＥＰ１６において、対象測距点の最新の位置データとして、該対象測距点に対応する背景点の位置データを保存する。

一方、ＳＴＥＰ１５の判断結果が肯定的である場合（Ｆ２＝ＯＮである場合）には、対象測距点に対応する背景点は、障害物が存在しない領域であるか否かは不明である。

そこで、この場合には、移動可能領域推定部５３は、ＳＴＥＰ１７において、対象測距点の最新の位置データとして、前回の演算処理周期にて、該対象測距点について決定した位置データ（前回位置データ）を保存する。

図１３に戻って、以上の如く移動体位置キャンセル処理を実行した後、移動可能領域推定部５３は、次にＳＴＥＰ４において、インフラセンサ１００の各測定点が、床面（ほぼ平坦な面）であるか否かを判別する処理（床面判別処理）を実行する。

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

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

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

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

以上説明したＳＴＥＰ１〜６の処理により、各インフラセンサ１００毎のグリッドデータが作成される。

次いで、移動可能領域推定部５３は、ＳＴＥＰ７において、各インフラセンサ１００毎のグリッドデータ統合してなる統合グリッドデータを作成する。

この場合、隣り合うインフラセンサ１００，１００の計測対象領域が重なり合う領域以外の領域（１つのインフラセンサ１００のみに対応する領域）では、前記ＳＴＥＰ６で設定されたグリッドデータがそのまま、統合グリッドデータとして設定される。

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

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

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

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

次に、移動軌道制御部５４は、上記の如く移動可能領域推定部５３が求めた移動可能領域と、前記将来位置予測部５２により予測される移動体Ａ（制御対象の移動体Ａ２以外の移動体）の将来位置とを用いて移動対象の移動体Ａ２の目標移動軌道を逐次設定する。

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

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

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

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

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

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

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

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

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

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

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

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

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

１…移動体運動推定装置（移動速度推定装置）、２…位置計測部、３…瞬時速度推定部、４…振動成分情報推定部、５…フィルタリング処理部（速度推定値調整処理部）、１１…バンドストップフィルタ、１２…カルマンフィルタ、５１…制御装置、５２…将来位置予測部、５３…移動可能領域推定部、５４…移動軌制御部、Ａ，Ａ１，Ａ２…移動体。

Claims (10)

1. 周期的な移動動作に伴う周期的な振動成分を含む速度で移動する移動体の移動速度を推定する装置であって、
   前記移動体の位置を逐次計測する位置計測部と、
   該位置計測部により得られた前記移動体の位置の計測値の時系列から該移動体の瞬時移動速度を逐次推定する瞬時移動速度推定部と
   前記移動体の周期的な移動動作に伴い該移動体の瞬時移動速度の時系列に含まれる前記周期的な振動成分の振動周期又は振動周波数を推定する振動成分情報推定部と、
   前記瞬時移動速度推定部により得られた前記瞬時移動速度の推定値の時系列に、前記振動成分情報推定部により得られた前記振動周期又は振動周波数の推定値に対応する振動成分を除去する処理を少なくとも実行することにより得られる速度値を前記移動体の移動速度の推定値として逐次生成する速度推定値調整処理部とを備えることを特徴とする移動体の移動速度推定装置。
2. 請求項１記載の移動体の移動速度推定装置において、
   前記振動成分情報推定部は、前記移動体の瞬時移動速度の推定値の時系列を周波数領域に変換してなるスペクトル分布データにおいて、スペクトル強度がピーク値となる周波数から前記振動周期又は振動周波数を推定するように構成されていることを特徴とする移動体の移動速度推定装置。
3. 請求項１記載の移動体の移動速度推定装置において、
   前記振動成分情報推定部は、前記振動周期又は振動周波数の推定用のあらかじめ作成されたデータに基づいて前記振動周期又は振動周波数を推定するように構成されていることを特徴とする移動体の移動速度推定装置。
4. 請求項３記載の移動体の移動速度推定装置において、
   前記データは、前記移動体の移動速度と前記振動周期又は振動周波数との関係を表す相関データであり、
   前記振動成分情報推定部は、前記速度推定値調整処理部からフィードバックされる移動速度の推定値から、前記相関データに基づいて前記振動周期又は振動周波数の推定値を更新するように構成されていることを特徴とする移動体の移動速度推定装置。
5. 請求項１〜４のいずれか１項に記載の移動体の移動速度推定装置において、
   前記速度推定値調整処理部は、前記振動周期又は振動周波数の推定値に基づいて構成されたバンドストップフィルタにより、前記振動周期又は振動周波数の推定値に対応する振動成分を除去する処理を実行するように構成されていることを特徴とする移動体の移動速度推定装置。
6. 請求項１〜５のいずれか１項に記載の移動体の移動速度推定装置において、
   前記移動体は脚式移動体であり、
   前記速度推定値調整処理部は、前記移動体の周期的な移動動作に伴う振動成分として、該振動成分のうちの最も低周波の第１の振動周波数の振動成分と、該第１の振動周波数の２倍の周波数である第２の振動周波数の振動成分とを前記瞬時移動速度の推定値から除去する処理を実行するように構成されており、
   前記振動成分情報推定部は、前記第１の振動周波数と、該第１の振動周波数に対応する第１の振動周期と、前記第２の振動周波数と、該第２の振動周波数に対応する第２の振動周期とのうちの少なくともいずれか１つを推定するように構成されていることを特徴とする移動体の移動速度推定装置。
7. 請求項１〜６のいずれか１項に記載の移動体の移動速度推定装置において、
   前記速度推定値調整処理部は、前記瞬時移動速度の推定値から、前記振動周期又は振動周波数の推定値に対応する振動成分に加えて、該振動成分よりも高周波領域の成分を高周波成分除去フィルタにより除去する処理をさらに実行することによって、前記移動体の移動速度の推定値を逐次生成するように構成されていることを特徴とする移動体の移動速度推定装置。
8. 動作環境空間を移動する制御対象の移動体の移動制御を行う制御装置であって、
   前記動作環境空間に存在する各移動体の将来位置を予測する将来位置予測部と、
   前記制御対象の移動体の移動可能領域を推定する移動可能領域推定部と、
   前記各移動体の移動速度を推定可能に構成されると共に、前記位置計測部が、前記各移動体を含めて前記動作環境空間に存在する物体の位置を計測可能に構成された請求項１〜７のいずれか１項に記載の移動体の移動速度推定装置と、
   前記制御対象の移動体の移動軌道を制御する移動軌道制御部とを備え、
   前記将来位置予測部は、各移動体について前記移動速度推定装置により推定された移動速度を用いて該移動体の将来位置を予測するように構成されており、
   前記移動可能領域推定部は、前記動作環境空間のうち、前記位置計測部の計測データから物体が存在しないことが認識される領域と、前記移動速度推定装置が移動速度を推定した各移動体の存在領域とを合わせた領域を移動可能領域として推定するように構成されており、
   前記移動軌道制御部は、前記制御対象の移動体を移動させるとき、前記移動可能領域推定部により推定された移動可能領域以外の領域である移動不可領域と、前記将来位置予測部により予測された他の移動体の将来位置とに干渉しない領域にて、該制御対象の移動体を移動させるように該制御対象の移動体の移動軌道を制御するように構成されていることを特徴とする移動体の制御装置。
9. 請求項８記載の移動体の制御装置において、
   前記移動可能領域推定部は、前記位置計測部の計測データの生成用のセンサと、いずれかの移動体との位置関係に応じて該計測データを生成することができない計測不能領域が存在するとき、該計測不能領域についての過去の計測データに基づいて、該計測不能領域を移動可能領域とするか否かを決定するように構成されていることを特徴とする移動体の制御装置。
10. 請求項９記載の移動体の制御装置において、
    前記移動可能領域推定部は、前記計測不能領域についての過去の計測データのうちの最新の計測データによって、該計測不能領域を移動可能領域と推定し得る場合に、該計測不能領域を移動可能領域として決定するように構成されていることを特徴とする移動体の制御装置。