JP7186414B2 - 移動物体の速度検出システム、速度検出装置及びそのプログラム - Google Patents

Description

本発明は、移動物体の速度検出システム、速度検出装置及びそのプログラムに係り、更に詳しくは、観測装置により観測された誤差のある移動物体の位置情報から、カルマンフィルタを利用して移動物体の速度を推定する移動物体の速度検出システム、速度検出装置及びそのプログラムに関する。

自律移動ロボットが人共存環境で安全、安心な活動を行うためには、高度な環境認識及び状態推定と、それに基づく行動決定及び制御技術とが不可欠であり、特に、自律移動ロボット周囲の人間の存在やその軌跡、移動速度等の推定技術が重要となる。例えば、自律移動ロボットに、周囲環境の位置情報を取得するレーザレンジファインダ（ＬＲＦ）等の観測装置を設け、当該位置情報に基づいて、自律移動ロボットの周囲を移動する歩行者等の状況を把握し、当該状況に応じて自律移動ロボットの動作制御を行うことが求められる。

ところで、前記観測装置での観測値は、様々な要因があって観測誤差が生じ得る。そこで、自律移動ロボットの周囲に存在する移動物体の速度推定をより正確に行うには、カルマンフィルタを用い、前記観測値を修正して移動物体の現在の位置情報や速度情報を推定する手法が用いられる（例えば、特許文献１参照）。

特開２０１１－１９６７４９号公報

しかしながら、カルマンフィルタを利用した速度推定では、その対象を歩行者とした場合に次の問題が生じる。つまり、歩行時における人間の体幹部分の速度は、前後だけでなく左右に周期的に変動する特性があり、観測装置での観測値から生成された人間の軌跡を時間微分することで算出された速度ベクトルには、大きな推定誤差を生じ得ることになる。また、自律移動ロボットにＬＲＦを搭載したときに、ＬＲＦから得られる点群情報には、自律移動ロボット自身の移動による振動等に起因するノイズ等の観測誤差が生じ、真値に相当する位置情報の推定が難しい。これら誤差を解消するためには、カルマンゲインを調整する必要があるが、当該調整には、複雑なモデルの適用が必要となり容易でなく、これまで、このようなモデルは出現していない。また、仮に、複雑な当該モデルがあっても、例えば、人混みの中で、人間への衝突を回避する素早い軌道調整が求められる場合等、位相遅れを極力少なくするための処理の応答性を重視した場合には、自律移動ロボットへの適用が難しい。総じて、従来技術では、複雑に動作する移動物体の現在位置の推定精度と、位相遅れを抑制する応答性とがトレードオフの関係となっており、それらのバランスを取るための手法は存在しない。

そこで、本発明者らが鋭意研究及び実験を行った結果、カルマンゲインの大きさを左右する要素である状態の予測誤差に関するシステム雑音分散と、予め設定した窓長における観測値に基づく歩行者の移動距離との間に、一定の関係性があることを見出した。そこで、当該関係性を利用してカルマンフィルタによる位置推定処理を行うと、応答性を損ねずに、移動物体の速度推定の精度を向上できることが実証された。

本発明は、本発明者らの知見に基づいて案出されたものであり、その目的は、移動物体の周期振動による推定誤差を抑制することで、移動物体の位置推定精度を向上するとともに、負担の少ない演算処理により、位相遅れを抑制して応答性を高めることのできる移動物体の速度検出システム、速度検出装置及びそのプログラムを提供することにある。

前記目的を達成するため、本発明は、主として、移動物体の位置情報を観測可能な観測装置と、当該観測装置での観測値に基づき、前記移動物体の速度情報を検出する速度検出装置とを備えた速度検出システムにおいて、前記速度検出装置は、所定の演算により、前記観測値から前記移動物体の位置状態を推定した上で前記速度情報を推定する速度算出手段を備え、前記速度算出手段は、前記観測値からカルマンフィルタを利用して前記移動物体の推定位置情報を求める位置情報推定部と、前記推定位置情報の経時変化から前記移動物体の推定速度情報を求める速度情報推定部とを備え、前記位置情報推定部では、前記観測値から、予め設定された位置推定用の窓長における前記移動物体の移動距離を求め、当該移動距離に応じてカルマンゲインが決定される、という構成を採っている。

なお、本特許請求の範囲及び本明細書において、「窓長」とは、カルマンフィルタによる位置推定や速度推定の演算タイミングとなる各ステップのうち、現在の演算処理がなされる現時点でのステップから過去に遡るステップ数を意味する。

本発明によれば、本発明者らが知見した前述の関係性を利用することで、観測値による移動物体の現在位置の推定精度を向上させるためのカルマンゲインの調整を簡易に行うことができ、位置推定精度に対応する移動物体の速度推定精度の向上と応答時間の遅延抑制との両立を図ることができる。

第１実施形態に係る速度検出システムの全体構成を表したブロック図。 変形例に係る前記速度検出システムの全体構成を表したブロック図。 第２実施形態に係る速度検出システムの全体構成を表したブロック図。

以下、本発明の実施形態について図面を参照しながら説明する。
（第１実施形態）

図１には、第１実施形態に係る速度検出システムの全体構成を表したブロック図が示されている。この図において、前記速度検出システム１０は、図示省略した自律移動ロボットの周囲に存在する移動物体としての歩行者の存在及びその経時的な位置変化を検出し、当該歩行者の速度を推定するシステムとなっている。

この速度検出システム１０は、自律移動ロボットに搭載されてその周囲の歩行者の位置情報を観測可能な観測装置として機能する測域センサ１１と、測域センサ１１での観測値に基づき、歩行者の速度情報を検出する速度検出装置として機能する演算処理装置１２とを備えている。

前記測域センサ１１は、特に限定されるものではないが、自律移動ロボットからその周囲へのレーザ光の照射による人間を含む物体の反射状態に基づいて、自律移動ロボットの周囲の物体の各表面部分の位置情報を検出する公知のレーザレンジファインダが用いられる。つまり、この測域センサ１１では、人間等の表面部分を構成する点群における各点の平面視での２次元位置座標が検出され、その検出結果から公知の手法を利用して歩行者の仮想体幹の位置が特定され、当該仮想体幹における位置情報を観測値とするようになっている。ここで、時系列データでの仮想体幹の位置情報の変位が所定の閾値（例えば、２０ｃｍ）未満である場合には、同一の歩行者であるとし、同一のＩＤが割り当てられる。そして、当該各歩行者について、後述するように、演算処理装置１２で速度情報が求められることになる。

なお、本発明における観測装置としては、前述のレーザレンジファインダやその他の測域センサ１１に限定されるものではなく、歩行者の位置情報を検出できる限りにおいて、様々なセンサや装置類を採用することができる。

前記演算処理装置１２は、ＣＰＵ、メモリ、ハードディスク等の記憶装置等からなるコンピュータによって構成されており、当該コンピュータを以下の各手段として機能させるためのプログラムがインストールされている。

この演算処理装置１２は、所定の演算により、測域センサ１１の観測値から歩行者の正確な位置状態を推定した上でその速度情報を推定する速度算出手段１４と、速度算出手段１４での処理のための前提条件を設定する条件設定手段１５とを備えている。

なお、演算処理装置１２は、自律移動ロボットに搭載されているか、自律移動ロボットと離れた場所に配置されているかは問わず、また、速度算出手段１４と条件設定手段１５がそれぞれ別の場所に配置されていても良い。

本実施形態における位置情報としては、直交２軸方向の座標成分からなる位置ベクトルＰが用いられ、同速度情報としては、移動方向に相当する進行方向角度と、速度の大きさに相当する進行方向速度との２成分からなる速度ベクトルＶが用いられる。

前記速度算出手段１４は、測域センサ１１の観測値からカルマンフィルタを利用して歩行者の推定位置情報を求める位置情報推定部１７と、前記推定位置情報の経時変化から歩行者の推定速度情報を求める速度情報推定部１８とを備えている。

前記位置情報推定部１７では、位置推定の演算タイミングとなるステップ毎に、観測値を修正した歩行者の位置情報が時系列で推定され、現時点のステップｋでの位置情報が、その１つ前の直前ステップｋ－１での演算結果を利用して推定される。ここでは、後述する演算に利用する現時点のステップｋから過去に遡るステップ数ｎを意味する窓長として、位置推定用の窓長である第１の窓長ｎ１が用いられ、この第１の窓長ｎ１は、条件設定手段１５で予め所定値に設定される。

この位置情報推定部１７は、現時点の１つ前のステップである直前ステップｋ－１での演算結果を利用して現時点の予測値を算出する予測フェーズ演算機能２０と、予測フェーズ演算機能２０での予測値を修正して現時点での推定値を算出する修正フェーズ演算機能２１とを有している。

前記予測フェーズ演算機能２０では、予め記憶された関係式により、現時点のステップｋでの予測値として事前に推定される歩行者の位置ベクトルＰである事前推定位置ベクトルＰ^－（ｋ）と、ステップｋでの事前誤差分散σ^２－（ｋ）とが算出される。この事前誤差分散σ_ｐ ^２－（ｋ）は、カルマンフィルタにおける修正前の状態での予測誤差分散である。

すなわち、事前推定位置ベクトルＰ^－（ｋ）は、直前ステップｋ－１において、後述する修正フェーズ演算機能２１により求められた歩行者の事後推定位置ベクトルＰ^＋（ｋ－１）と、後述する速度情報推定部１８で求められた直前ステップｋ－１における歩行者の推定速度ベクトルＶ（ｋ－１）とから、次式（１）の通りに求められる。ここで、ΔＴは、１ステップ間の経過時間（例えば、０．０２５（ｓｅｃ））である。

また、事前誤差分散σ_ｐ ^２－（ｋ）は、次式（２）の通り、直前ステップｋ－１時に、後述する修正フェーズ演算機能２１で求められた修正後の状態の予測誤差分散である事後誤差分散σ_ｐ ^２＋（ｋ－１）と、次式（３）により求められるシステム雑音分散σ_ε ^２（ｋ）とを加算することで求められる。このシステム雑音分散σ_ε ^２（ｋ）は、カルマンフィルタにおける状態の予測誤差に対応した状態方程式の雑音分散である。

上式（３）は、本発明者らの実験研究の結果により見出した関係式である。ここで、Δｌは、ステップ数Ｎ_１である第１の窓長ｎ１の範囲における最初のステップｋ－（Ｎ_１－１）での歩行者位置と現時点のステップｋでの歩行者位置の直線距離である移動距離を表す。当該移動距離は、最初のステップｋ－（Ｎ_１－１）での測域センサ１１での観測値と、現時点のステップｋでの測域センサ１１での観測値とから演算で求められる。また、上式（３）のａ，ｂは、第１の窓長ｎ１の大きさに対応して予め決定される正の一定値である。

換言すれば、システム雑音分散σ_ε ^２（ｋ）と、観測値における第１の窓長ｎ１での歩行者の移動距離とが比例関係にあり、当該移動距離に応じて一義的に決定される。また、上式（３）は、条件設定手段１５で設定された第１の窓長ｎ１の大きさに応じて、予め記憶されたａ，ｂの異なる数式が選択される。

前記修正フェーズ演算機能２１では、予め記憶された次の関係式により、予測フェーズ演算機能２０で求めた現時点のステップｋにおける各値から、当該現時点のステップｋにおける修正値となる歩行者の事後推定位置ベクトルＰ^＋（ｋ）と、ステップｋにおけるカルマンフィルタ上の事後誤差分散σ_ｐ ^２＋（ｋ）とが求められる。

すなわち、ステップｋにおける事後推定位置ベクトルＰ^＋（ｋ）は、次式（４）の通り、予測フェーズ演算機能２０で求めたステップｋにおける事前推定位置ベクトルＰ^－（ｋ）と、ステップｋにおけるカルマンゲインｇ（ｋ）と、現時点のステップｋでの測域センサ１１の観測値である観測位置ベクトルＰ_ｏ（ｋ）とにより算出される。

また、ステップｋにおける事後誤差分散σ_ｐ ^２＋（ｋ）は、次式（５）の通り、予測フェーズ演算機能２０で求めたステップｋにおける事前誤差分散σ_ｐ ^２－（ｋ）と、ステップｋにおけるカルマンゲインｇ（ｋ）とにより算出される。これらの式で用いられるカルマンゲインｇ（ｋ）は、０から１の間の値を採り、次式（６）により求められる。すなわち、カルマンゲインｇ（ｋ）は、ステップｋにおける事前誤差分散σ_ｐ ^２－（ｋ）と、カルマンフィルタ上の観測方程式の雑音分散となる観測雑音分散σ_ω ^２とにより算出される。

なお、上式（６）における観測雑音分散σ_ω ^２は、測域センサ１１の計測誤差に対応し、測域センサ１１の性能や仕様等に対応して予め設定された一定値となっているが、本発明はこれに限定されるものではなく、ロバスト性を向上させるように、観測時点での環境条件等に応じて可変にすることも可能である。例えば、路面の状況や光量によって、観測誤差が変化すると考えられることから、測域センサ１１で得られる環境の点群情報の分散をリアルタイムで計算し、当該計算結果により、ステップ毎に観測雑音分散σ_ω ^２の値を変化可能に設定することもできる。

上式（３）によれば、観測値により求めた第１の窓長ｎ１間の歩行者の移動距離Δｌとシステム雑音分散σ_ε ^２（ｋ）とは比例関係にあり、上式（２）、（６）の通り、システム雑音分散σ_ε ^２（ｋ）の増減に伴って、カルマンゲインｇ（ｋ）が増減する。従って、カルマンゲインｇ（ｋ）は、観測値により求めた第１の窓長ｎ１間の歩行者の移動距離Δｌに応じて決定され、移動距離Δｌの増減に伴って増減することになる。

このようにして位置情報推定部１７で求めた事後推定位置ベクトルＰ^＋（ｋ）は、推定値である推定位置情報として、ステップ毎に時系列で記憶され、次の速度情報推定部１８での演算に利用される。

前記速度情報推定部１８では、次のようにして、現時点となるステップｋにおける推定速度ベクトルＶ（ｋ）が推定位置情報として演算で求められる。ここでは、速度推定用の窓長となる第２の窓長ｎ２が用いられ、この第２の窓長ｎ２は、後述するように、条件設定手段１５で予め所定値に設定される。

すなわち、現時点のステップｋから第２の窓長ｎ２となるＮ_２ステップ分遡った各ステップ（ｋ、ｋ－１、ｋ－２、・・・ｋ－（Ｎ_２－１））における各事後推定位置ベクトルＰ^＋（ｋ＊）から、最小二乗法により歩行者の軌跡を求め、窓長Ｎ_２の間の経過時間から、ステップｋにおける歩行者の推定速度ベクトルＶ（ｋ）が算出される。

次に、前記条件設定手段１５について説明する。

この条件設定手段１５では、予め被験者に行った実験歩行により取得した実験データから、前述の第１及び第２の窓長ｎ１，ｎ２の組み合わせ（以下、単に「窓長組み合わせ」と称する）の中で最適となるものを所定の複数パターンの中から選択するようになっている。

当該条件設定手段１５は、所定の歩行条件の下での実験歩行時における測域センサ１１の観測値から窓長組み合わせ毎に各ステップｋの推定位置情報及び推定速度情報を演算により求める速度算出部２３と、速度算出部２３で求めた推定位置情報及び推定速度情報に基づき、窓長組み合わせ毎にコスト計算を行うコスト計算部２４と、コスト計算部２４での計算結果により、実際の現場での速度算出手段１４による位置情報及び速度情報の演算に用いる窓長組み合わせを決定する窓長決定部２５とからなる。

前記速度算出部２３は、前述した速度算出手段１４と実質的に同一の構成となっており、詳細な説明を省略するが、ここでは、測域センサ１１の観測値からカルマンフィルタを利用して、実験歩行時の歩行者の各ステップｋの推定位置情報及び推定速度情報が求められる。なお、特に限定されるものではないが、リアルタイムの観測時に利用される速度算出手段１４での演算と実験データ処理時に用いられる速度算出部２３での演算は、同一のプログラムモジュール等で行っても良いし、それぞれ別配置されたプログラムモジュールやコンピュータで行っても良い。

ここで、実験歩行としては、所定の被験者に予め指定した目標軌道での歩行動作を一定の速度で行って貰い、測域センサ１１で所定の計測間隔毎に歩行時の位置情報を検出し、これら各位置情報での観測値が速度算出部２３に入力される。つまり、被験者には、スタート地点からゴール地点まで、予め指定された方向に沿って歩行して貰いながら、途中の所定位置で所定の方向転換を行って貰い、その歩行時における被験者の位置情報の観測値が速度算出部２３に入力される。

また、速度算出部２３での各種演算に使用される窓長組み合わせとしては、次を例示できる。すなわち、第１の窓長ｎ１として、０．２秒，０，４秒，０．６秒，０．８秒，１．０秒の５種類を用意し、第２の窓長ｎ２として、第１の窓長ｎ１と同一の５種類を用意し、これらを１つずつ組み合わせた合計２５パターンの窓長組み合わせが用意される。そして、速度算出部２３では、これら２５パターンの窓長組み合わせについて、それぞれの各ステップでの推定位置情報及び推定速度情報が、前述の速度算出手段１４の場合と同様の手法で算出される。

前記コスト計算部２４では、予め記憶された次式（７）のコスト式により、各パターンの窓長組み合わせ毎にコストＣの計算が行われる。このコストＣは、次の３つの要素について、予め指定された前述の歩行条件における真値に対する誤差を総合して数値化した指標である。つまり、当該コストＣは、歩行者の進行方向角度の誤差に対応する第１のコストＣ_１と、歩行者の進行方向速度の誤差に対応する第２のコストＣ_２と、歩行者の方向転換に要する応答時間の誤差に対応する第３のコストＣ_３とをそれぞれ定数（α、β、γ）倍した合計値となる。

前記第１のコストＣ_１は、歩行者の進行方向角度の第１誤差に関する標準偏差を全データにより標準化したものであり、上式（８）で求められる。つまり、ここでは、第１及び第２の窓長ｎ１，ｎ２を使って速度算出部２３で算出された推定速度ベクトルＶ（ｋ）の進行方向角度成分Ｖ_θ（ｋ）と当該成分の真値Ｖ_θＴ（ｋ）との差分が、各ステップで算出される。そして、ある窓長組み合わせについての実験時の全てのステップ（ｋ＝１～Ｋ）における当該差分の標準偏差が、当該差分についての全ての窓長組み合わせ（総数Ａ）に係る総データの標準偏差で除算されることで求められる。ここでの真値Ｖ_θＴ（ｋ）としては、実験歩行時の目標軌道が予め設定されていることから、当該目標軌道における各ステップでの進行方向角度となる。また、上記例では、上式（８）の分母となる進行方向角度成分に関する総データの標準偏差は、総数Ａが２５パターンとなる全ての窓長組み合わせにおける同データの総和の標準偏差となる。

前記第２のコストＣ_２は、歩行者の進行方向速度の第２誤差に関する標準偏差を全データにより標準化したものであり、上式（９）で求められる。つまり、ここでは、第１及び第２の窓長ｎ１，ｎ２を使って速度算出部２３で算出された推定速度ベクトルＶ（ｋ）の進行方向速度成分Ｖ_ｍ（ｋ）と当該成分の真値Ｖ_ｍＴ（ｋ）との差分が、各ステップで算出される。そして、ある窓長組み合わせについての実験時の全てのステップ（ｋ＝１～Ｋ）における当該差分の標準偏差が、当該差分についての全ての窓長組み合わせ（総数Ａ）に係る総データの標準偏差で除算されることで求められる。ここでの真値Ｖ_ｍＴ（ｋ）としては、前記目標軌道における平均速度が予め設定される。また、上記例では、上式（９）の分母となる進行方向速度成分に関する総データの標準偏差は、第１のコストＣ_１と同様に、２５パターン全ての窓長組み合わせにおける同データの総和の標準偏差となる。

前記第３のコストＣ_３は、方向転換時におけるシステムの応答時間の第３誤差に関する標準偏差を全データにより標準化したものであり、上式（１０）で求められる。つまり、ここでは、第１及び第２の窓長ｎ１，ｎ２を使って速度算出部２３で算出された推定速度ベクトルＶ（ｋ）に基づき、次のように応答時間が求められる。すなわち、ｍ番目の方向転換地点について、各ステップで算出された推定速度ベクトルＶ（ｋ）から、方向転換後の指定角度の許容範囲内に、演算による推定値が所定数連続で収まった最初の時刻が特定され、歩行者が曲がり始める既知の開始時刻から当該特定時刻までの経過時間が応答時間Ｔ（ｍ）とされる。そして、当該方向転換地点ｍ毎の応答時間Ｔ（ｍ）とその真値Ｔ_Ｔ（ｍ）との差分が算出され、全ての方向転換地点（ｍ＝１～Ｍ）における当該差分の標準偏差が、当該差分についての全ての窓長組み合わせ（総数Ａ）に係る総データの標準偏差で除算されることで求められる。ここでの真値Ｔ_Ｔ（ｍ）としては、各方向転換地点で設定された方向転換角度から、当該角度に応じた人間の方向転換動作に要する時間が予め設定される。また、上記例では、上式（１０）の分母となる応答時間に関する総データの標準偏差は、第１及び第２のコストＣ_１，Ｃ_２と同様に、２５パターン全ての窓長組み合わせにおける同データの総和の標準偏差となる。

以上により、第１のコストＣ_１は、歩行者の進行方向左右となる横方向への周期振動に対応するコストに相当し、第２のコストＣ_２は、歩行者の進行方向前後となる縦方向への周期振動に対応するコストに相当し、第３のコストＣ_３は、方向転換時におけるシステムの応答時間に対応するコストに相当する。

コスト計算上で前記各コストに重み付けをする前記定数（α、β、γ）は、自律移動ロボットの周囲の環境の状況に対応した環境パラメータとして予め任意の値に設定される。なお、特に限定されるものではないが、本実施形態において、α、β、γは、合計して１になるように設定される。

なお、前記環境パラメータは、可変とすることができ、次の状況に応じて任意の値を事前に入力可能な構成としても良い。

すなわち、第１の状況として、歩行者密度が大きい動的環境では、歩行者の速度情報の推定精度を高くし、且つ、方向転換時の応答遅れも小さくしなければ、自律移動ロボットの瞬時の行動計画ができない。そこで、この場合は、全てのコストＣ_１，Ｃ_２，Ｃ_３の重み付けを均一にし、α＝β＝γ＝１／３とされる。

また、第２の状況として、歩行者が実質的に停止している静的環境で、自律移動ロボットが歩行者に接触しながら移動する場合には、厳密な接触位置の検出のために、歩行者の周期振動による誤差を極力抑制する必要性が高いため、第１及び第２のコストＣ_１，Ｃ_２の重み付けを高くするために、α＝β＝１／２、γ＝０とされる。

更に、第３の状況として、子供が走り回っている状況等、自律移動ロボットの瞬時の判断が優先される場合には、歩行者の応答時間に関する誤差を極力抑制する必要性が高いため、第３のコストＣ_３の重み付けを高くするために、α＝β＝０、γ＝１とされる。

また、条件設定手段１５の変形例として、図２に示されるように、環境パラメータ設定部２８を条件設定手段１５に新たに設け、所定のモデルにより環境パラメータを自動的に決定可能な構成を採ることもできる。このモデルとしては、自律移動ロボットに搭載された測域センサ１１の観測範囲で正規化された歩行者の密度ｄ（０＜ｄ＜１）と、当該歩行者の正規化された運動量の合計ＭＶ（０＜ＭＶ＜１）とに基づき、各環境パラメータを特定するモデルを例示できる。

前記窓長決定部２５では、コスト計算部２４でコストＣがそれぞれ計算された窓長組み合わせの中から、最も小さいコストＣの窓長組み合わせを選択し、当該選択された窓長組み合わせが、速度算出手段１４での演算に用いる窓長組み合わせとして決定される。

以上の構成の前記速度検出システム１０について、本発明者らは、高精度且つ低遅延で歩行者の速度を推定できるか否かについての実証実験を行った。

この実験では、１１名の被験者を歩行者とし、各歩行者が同一の目標軌道に沿って方向転換（ジグザグ歩行）を行った際に、歩行者の生データとの対比を行った。この際、各被験者には、事前に一定の速さで自然な歩行を行うように伝えた。また、この際の歩行条件としては、先ず、所定の基準方向から７５度傾いた方向で２ｍ移動した後、方向転換して基準方向に沿って２ｍ移動し、更に方向転換して基準方向から４５度傾いた方向で２ｍ移動する目標軌道を設定した。更に、測域センサ１１が搭載される自律移動ロボットの移動速度を０．４（ｍ／ｓ）とした。

上記実験の結果、前記速度検出システム１０で推定した速度成分の誤差は、測域センサ１１の観測値のみから最小二乗法によって求めた速度成分の誤差よりも大幅に改善したことが実証された。また、方向転換時の応答時間についても、歩行者が一歩踏み出すのに要する時間０．５秒よりも短時間である０．２７５秒に抑えることができた。また、進行方向角度の誤差に関する標準偏差が、５．６４度となり、自律移動ロボットが移動している際にも精度よく速度推定できたことが実証された。

以上により、前記速度検出システム１０によれば、移動する自律移動ロボットで取得した観測値から、体幹のブレが大きい人間の歩行時の推定速度を高精度且つ低遅延で推定することが可能となる。

次に、本発明の他の実施形態について説明する。なお、以下の説明において、前記第１実施形態と同一若しくは同等の構成部分については同一符号を用いるものとし、説明を省略若しくは簡略にする。
（第２実施形態）

本実施形態に係る速度検出システム３０は、図３に示されるように、第１の実施形態の速度検出システム１０における条件設定手段１５に対し、構成を変えた条件設定手段３２を採用したところに特徴を有する。この条件設定手段３２では、予め取得した実験データを用いずに実現場において、予め設定された複数パターンの窓長組み合わせ中から最適な窓長組み合わせを選択し、当該窓長組み合わせが、速度算出手段１４での処理の前提条件として設定される。

前記条件設定手段３２は、測域センサ１１での観測値をそのまま利用して最小二乗法で求められる歩行者の第１の速度情報を算出する第１の速度算出部３４と、測域センサ１１での観測値からカルマンフィルタを利用して歩行者の推定位置情報を求めた上で、当該推定位置情報から最小二乗法で求められる歩行者の第２の速度情報を算出する第２の速度算出部３５と、これら第１及び第２の速度算出部３４，３５から所定時間内での歩行者の歩行状態を推定する移動状態推定部３６と、移動状態推定部３６での推定結果により、速度算出手段１４での演算に用いる窓長組み合わせを決定する窓長決定部３７とからなる。

この条件設定手段３２では、所定の設定時間内で測域センサ１１により観測された観測値に基づいて、次の各処理が行われる。この設定時間としては、例えば、予め複数パターン用意した第１及び第２の窓長ｎ１，ｎ２の組み合わせである窓長組み合わせの中から、最大の窓長で想定される最長時間よりも長い時間とされる。

前記第１の速度算出部３４では、予め用意された第２の窓長ｎ２のバリエーションの中からデフォルト値として予め設定された１つの第２の窓長ｎ２を用い、測域センサ１１での観測値をそのまま使って最小二乗法による第１の速度情報を求めるようになっている。なお、例えば、第２の窓長ｎ２のデフォルト値として、予め用意された第２の窓長ｎ２の範囲の中間付近の値が用いられる。

前記第２の速度算出部３５では、予め用意された窓長組み合わせのパターンの中でデフォルト値として予め設定された１組の窓長組み合わせを用い、前記速度算出手段１４と同様にして、歩行者の推定位置情報及び推定速度情報が算出される。従って、第２の速度算出部３５は、速度算出手段１４と実質的に同一の構成となっており、ここでの詳細な説明を省略する。

前記移動状態推定部３６では、第１の速度算出部３４で求めた推定速度ベクトルと、第２の速度算出部３５で求めた推定速度ベクトルとの誤差（差分）の絶対値を前記設定時間で積分する。そして、予め記憶された数式により、当該演算結果に基づき歩行者の移動状態が推定される。具体的に、ここでは、歩行者の進行方向左右の振動に対応する値である横ぶれ値と、歩行者の進行方向前後の振動に対応する値である縦ぶれ値と、歩行者の方向転換の大きさに対応する値である方向転換値とが移動状態の指標として求められる。

前記窓長決定部３７では、横ぶれ値、縦ぶれ値、及び方向転換値と、最適となる窓長組み合わせとの関係が予め記憶されており、移動状態推定部３６で求めた各値に応じて、予め複数パターン用意した窓長組み合わせの中から最適なものが選択されるようになっている。

つまり、これら各値に対応する状況から、速度情報の推定誤差を少なくしつつ応答性への影響を考慮した最適な各窓長ｎ１，ｎ２が特定される。ここで、これら各値と窓長との関係性の概念としては、以下を例示できる。

横ぶれ値に関しては、横に大きく振れる歩行者であれば、カルマンゲインに影響する第１の窓長ｎ１を小さくし、速度計算に影響する第２の窓長ｎ２を予め用意した範囲での中程度の値にするのが良い。一方、横の振動が小さい歩行者であれば、第１の窓長ｎ１をある程度大きくても良く、第２の窓長ｎ２については、小さくなければ良い。

また、縦ぶれ値に関しては、進行方向速度が大きく振れる歩行者であれば、第１の窓長ｎ１を小さくし、第２の窓長ｎ２を大きくするのが良い。一方、進行方向速度の振れが小さい歩行者であれば、第１の窓長ｎ１を大きくしても良く、第２の窓長ｎ２をある程度小さくしても良い。

更に、方向転換値に関しては、頻繁に曲がる歩行者であれば、第１の窓長ｎ１を小さくし、第２の窓長ｎ２も小さくするのが良い。一方、頻繁に曲がらない歩行者であれば、第１の窓長ｎ１を大きくしても良く、第２の窓長ｎ２をある程度大きくしても良い。

以上の第２実施形態によれば、実現場において計測対象となる歩行者の状態を推定し、その結果により、当該状態にも起因する推定位置情報や推定速度情報の誤差を低減可能に、最適となる各窓長ｎ１，ｎ２を特定することができ、予め取得した実験データを不要とし、実現場における歩行者に適合した全ての処理を実現場で行うことができる。

なお、自律移動ロボットの周囲の場に合った各窓長ｎ１，ｎ２を探索するために、自律移動ロボットの前後左右に存在する歩行者の動作を総合して、場の歩行者の状態を推定し、当該推定により最適な窓長ｎ１，ｎ２を決定することも可能である。

また、前記各実施形態においては、人間である歩行者の速度推定を行っているが、本発明はこれに限らず、速度推定を行う対象として、他の自律移動ロボット及び自動車や自転車等の移動体、或いは動物等を含む全ての移動物体への適用が可能である。

更に、前記各実施形態においては、測域センサ１１を自律移動ロボットに搭載して移動させているが、本発明は、測域センサ１１等の観測装置が固定配置された状態でも適用可能である。

その他、本発明における装置各部の構成は図示構成例に限定されるものではなく、実質的に同様の作用を奏する限りにおいて、種々の変更が可能である。

１０ 速度検出システム
１１ 測域センサ（観測装置）
１２ 演算処理装置（速度検出装置）
１４ 速度算出手段
１５ 条件設定手段
１７ 位置情報推定部
１８ 速度情報推定部
２８ 環境パラメータ設定部
３０ 速度検出システム
３２ 条件設定手段
３４ 第１の速度算出部
３５ 第２の速度算出部
３６ 移動状態推定部
３７ 窓長決定部
Ｃ コスト
Ｃ１ 第１のコスト
Ｃ２ 第２のコスト
Ｃ３ 第３のコスト
Ｐ 位置ベクトル（位置情報）
Ｖ 速度ベクトル（速度情報）

Claims (11)

1. 移動物体の位置情報を観測可能な観測装置と、当該観測装置での観測値に基づき、前記移動物体の速度情報を検出する速度検出装置とを備えた速度検出システムにおいて、
   前記速度検出装置は、所定の演算により、前記観測値を修正した前記移動物体の正確な位置情報を推定した上で前記速度情報を推定する速度算出手段を備え、
   前記速度算出手段は、前記観測値からカルマンフィルタを利用して前記移動物体の推定位置情報を求める位置情報推定部と、前記推定位置情報の経時変化から前記移動物体の推定速度情報を求める速度情報推定部とを備え、
   前記位置情報推定部では、前記観測値から、予め設定された位置推定用の窓長における前記移動物体の移動距離を求め、当該移動距離に応じてカルマンゲインが決定されることを特徴とする移動物体の速度検出システム。
2. 前記カルマンゲインは、カルマンフィルタにおける状態の予測誤差に対応した状態方程式の雑音分散であるシステム雑音分散に応じて変化し、このシステム雑音分散は、予め記憶された前記移動距離との関係式により求められることを特徴とする請求項１記載の移動物体の速度検出システム。
3. 前記関係式は、前記システム雑音分散と前記移動距離が比例関係となる数式であり、前記窓長の大きさに応じて異なる数式が選択されることを特徴とする請求項２記載の移動物体の速度検出システム。
4. 前記移動物体は歩行者であり、
   前記速度検出装置は、前記速度算出手段での処理のための前提条件を設定する条件設定手段を更に備え、
   前記速度情報推定部では、予め設定された速度推定用の窓長における前記移動物体の推定位置情報の変化に基づいて前記推定速度情報が求められ、
   前記条件設定手段では、所定の歩行者が所定条件に沿って行った実験歩行により取得したデータから、前記位置推定用と前記速度推定用の窓長組み合わせ毎に、予め記憶されたコスト式に基づき、前記条件で設定される真値との誤差に対応したコストを算出し、当該コストが最も小さい前記窓長組み合わせを利用して、前記速度算出手段での処理が行われることを特徴とする請求項１記載の移動物体の速度検出システム。
5. 前記コストは、前記歩行者の進行方向角度に対応する第１のコストと、前記歩行者の進行方向速度に対応する第２のコストと、前記歩行者の方向転換に要する応答時間に対応する第３のコストをそれぞれ定数倍した合計値として求められることを特徴とする請求項４記載の移動物体の速度検出システム。
6. 前記第１のコストは、前記進行方向角度についての第１誤差に関する標準偏差を、全ての窓長組み合わせにおける前記第１誤差の全データにより標準化したものであり、
   前記第２のコストは、前記進行方向速度についての第２誤差に関する標準偏差を、全ての窓長組み合わせにおける前記第２誤差の全データにより標準化したものであり、
   前記第３のコストは、前記応答時間についての第３誤差に関する標準偏差を、全ての窓長組み合わせにおける前記第３誤差の全データにより標準化したものであることを特徴とする請求項５記載の移動物体の速度検出システム。
7. 前記第１～第３のコストにそれぞれ乗じる定数は、前記観測装置の周囲の環境の状況に対応した環境パラメータとして予め任意の値に設定されることを特徴とする請求項５記載の移動物体の速度検出システム。
8. 前記条件設定手段には、所定のモデルにより前記環境パラメータを自動的に決定する環境パラメータ設定部を含むことを特徴とする請求項７記載の移動物体の速度検出システム。
9. 前記速度検出装置は、前記速度算出手段での処理のための前提条件を設定する条件設定手段を更に備え、
   前記速度情報推定部では、予め設定された速度推定用の窓長における前記移動物体の推定位置情報の変化に基づいて前記推定速度情報が求められ、
   前記条件設定手段は、前記観測値をそのまま利用して最小二乗法で求められる前記移動物体の第１の速度情報を算出する第１の速度算出部と、前記観測値からカルマンフィルタを利用して前記移動物体の推定位置情報を求めた上で、当該推定位置情報から最小二乗法で求められる前記移動物体の第２の速度情報を算出する第２の速度算出部と、これら第１及び第２の速度情報の差分に基づき前記移動物体の移動状態を推定する移動状態推定部と、当該移動状態推定部での推定結果により、前記速度算出手段での演算に用いる前記位置推定用と前記速度推定用の窓長の組み合わせを決定する窓長決定部とを備えたことを特徴とする請求項１記載の移動物体の速度検出システム。
10. 移動物体の位置情報を観測可能な観測装置での観測値に基づき、前記移動物体の速度情報を検出する速度検出装置において、
    所定の演算により、前記観測値を修正した前記移動物体の正確な位置情報を推定した上で前記速度情報を推定する速度算出手段を備え、
    前記速度算出手段は、前記観測値からカルマンフィルタを利用して前記移動物体の推定位置情報を求める位置情報推定部と、前記推定位置情報の経時変化から前記移動物体の推定速度情報を求める速度情報推定部とを備え、
    前記位置情報推定部では、前記観測値から、予め設定された位置推定用の窓長における前記移動物体の移動距離を求め、当該移動距離に応じてカルマンゲインが決定されることを特徴とする速度検出装置。
11. 移動物体の位置情報を観測可能な観測装置での観測値に基づき、前記移動物体の速度情報を検出する速度検出装置のプログラムにおいて、
    所定の演算により、前記観測値を修正した前記移動物体の正確な位置情報を推定した上で前記速度情報を推定する速度算出手段としてコンピュータを機能させ、
    前記速度算出手段は、前記観測値からカルマンフィルタを利用して前記移動物体の推定位置情報を求める位置情報推定部と、前記推定位置情報の経時変化から前記移動物体の推定速度情報を求める速度情報推定部として機能し、
    前記位置情報推定部では、前記観測値から、予め設定された位置推定用の窓長における前記移動物体の移動距離を求め、当該移動距離に応じてカルマンゲインが決定されることを特徴とする速度検出装置のプログラム。
    

Images