(57)【特許請求の範囲】
【請求項１】
オブジェクトの現在状態に基づき、第１モデルにしたがって、未来における前記オブジェクトの状態の確率密度分布の遷移態様を表わす基準軌道を生成する第１軌道生成要素と、
前記オブジェクトの現在状態および複数の候補状態に基づき、第２モデルにしたがって、前記オブジェクトの現在状態から前記複数の候補状態のそれぞれまでの状態の確率密度分布の遷移態様を表わす複数の候補軌道を生成する第２軌道生成要素と、
前記第２軌道生成要素により生成された前記複数の候補軌道のそれぞれについて前記第１軌道生成要素により生成された前記基準軌道との近似度を評価し、第１指定期間にわたり前記近似度が指定順位以内である候補軌道である第１候補軌道、および、前記第１候補軌道として指定されていた最後の時点から第２指定期間経過前であるその他の候補軌道である第２候補軌道のうち少なくとも１つを前記オブジェクトの状態予測結果として指定する軌道指定要素と、を備えていることを特徴とする状態予測システム。
【請求項２】
請求項１記載の状態予測システムにおいて、
前記軌道指定要素が、前記第１指定期間にわたり前記近似度が指定順位以内であり、かつ、前記近似度が閾値以上である候補軌道を前記第１候補軌道として指定することを特徴とする状態予測システム。
【請求項３】
請求項１または２記載の状態予測システムにおいて、
前記軌道指定要素が、前記複数の候補状態のうち少なくとも１つの候補状態が前記オブジェクトと他の物体との相互作用を伴う状態である場合、前記オブジェクトが前記少なくとも１つの候補状態に遷移するまでの時間が短くなるほど、前記第１指定期間として長い期間を採用するまたは前記閾値として大きい値を採用することを特徴とする状態予測システム。
【請求項４】
請求項２または３記載の状態予測システムにおいて、
前記軌道指定要素が、前記オブジェクトの状態および前記複数の候補状態のそれぞれのうち少なくとも１つの変化量が大きいほど安定度を低く評価し、前記安定度が低いほど、前記第１指定期間として長い期間を採用するまたは前記閾値として大きい値を採用することを特徴とする状態予測システム。
【請求項５】
請求項１〜４のうちいずれか１つに記載の状態予測システムにおいて、
前記軌道指定要素が、今回の前記第１候補軌道に対して評価指数の今回値として指定の正値を付与し、今回の前記第１候補軌道を除く候補軌道に対して、前記評価指数の前回値に１未満の正値である減衰係数を乗じた結果を前記評価指数の今回値として付与し、前記評価指数の今回値が基準値以上である少なくとも１つの候補軌道を今回の前記第２候補軌道として指定することを特徴とする状態予測システム。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、オブジェクトの状態を予測する技術に関する。
【背景技術】
【０００２】
ロボットが人間と同じ環境で動作する場合、人間を含むオブジェクトの動きに応じてロボットの運動が適応的に変更される必要がある。そこで、オブジェクトの状態を予測する手法が提案されている（特許文献１〜４参照）。
【先行技術文献】
【特許文献】
【０００３】
【特許文献１】特開２０１３−１４３１３４号公報
【特許文献２】特開２０１３−１９６６０１号公報
【特許文献３】特開２０１３−０５４７０３号公報
【特許文献４】特許第５４８９９６５号公報
【発明の概要】
【発明が解決しようとする課題】
【０００４】
しかし、オブジェクトの状態予測の条件によっては、予測状態が高頻度で変動するなど不安定になり、または当該予測精度が不安定になる可能性がある。
【０００５】
そこで、本発明は、オブジェクトの状態の予測結果の安定性の向上を図ることができるシステムを提供することを解決課題とする。
【課題を解決するための手段】
【０００６】
本発明の状態予測システムは、オブジェクトの現在状態に基づき、第１モデルにしたがって、未来における前記オブジェクトの状態の確率密度分布の遷移態様を表わす基準軌道を生成する第１軌道生成要素と、前記オブジェクトの現在状態および複数の候補状態に基づき、第２モデルにしたがって、前記オブジェクトの現在状態から前記複数の候補状態のそれぞれまでの状態の確率密度分布の遷移態様を表わす複数の候補軌道を生成する第２軌道生成要素と、前記第２軌道生成要素により生成された前記複数の候補軌道のそれぞれについて前記第１軌道生成要素により生成された前記基準軌道との近似度を評価し、第１指定期間にわたり前記近似度が指定順位以内である候補軌道である第１候補軌道、および、前記第１候補軌道として指定されていた最後の時点から第２指定期間経過前であるその他の候補軌道である第２候補軌道のうち少なくとも１つを前記オブジェクトの状態予測結果として指定する軌道指定要素と、を備えていることを特徴とする。
【０００７】
本発明の状態予測システムによれば、オブジェクトの現在状態に基づいて生成される基準軌道との近似度が第１指定期間にわたり指定順位以内であった少なくとも１つの候補軌道が「第１候補軌道」として指定される。このため、オブジェクトの状態がそれにしたがって遷移する蓋然性が一時的にのみ高かった候補軌道が第１候補軌道として指定されることが回避される。これにより、オブジェクトの状態予測結果としての当該指定結果の変化頻度が低減されうる。
【０００８】
第１候補軌道であった最後の時点から第２指定期間が経過前である少なくとも１つの候補軌道が「第２候補軌道」として指定される。オブジェクトの状態予測結果としての当該指定結果により、オブジェクトの状態が第２候補軌道にしたがって遷移する可能性が残されていることが顕在化される。これにより、第２候補軌道に指定されている候補軌道が再び第１候補軌道に指定された場合でも、オブジェクトの状態の予測結果が、それまでの予測結果から著しく乖離する事態が回避されうる。第１候補軌道が指定されない状態では、オブジェクトの状態予測結果としての当該指定結果により、オブジェクトの状態が第２候補軌道にしたがって遷移する可能性が最も高いものの、その可能性は第１候補軌道よりも低いことが顕在化される。これにより、その後、異なる候補軌道が第１候補軌道として指定された場合でも、オブジェクトの状態の予測結果が、それまでの予測結果から著しく乖離する事態が回避されうる。
【０００９】
よって、オブジェクトの状態の予測結果としての候補軌道の指定結果の安定性向上が図られる。
【００１０】
なお、「オブジェクト」には、ひとまとまりの物体のほか、当該物体の部分も含まれる。「状態」は一または複数のパラメータにより定義され、当該パラメータには、オブジェクトの位置、姿勢および速度などの動力学的パラメータ、オブジェクトの温度、色および輝度などの物理量、ならびにオブジェクトとしての人間などの動物の心理状態または生理状態を表現するパラメータなど、適当なセンサを用いて測定または検出可能なあらゆるパラメータが含まれる。「状態空間」は当該パラメータの次元数を有するヒルベルト空間によって定義される。たとえば、オブジェクトの状態がその３次元実空間における位置である場合、当該状態は３次元の状態空間におけるベクトルとして定義される。オブジェクトの状態がその３次元実空間における位置および姿勢である場合、当該状態は６次元の状態空間におけるベクトルとして定義される。
【００１１】
第１指定期間が経過する前に、それまで基準軌道に対する近似度が指定順位以内であった候補軌道に対応する候補状態が変化した場合、当該変化後の候補状態に応じて新たに生成された候補軌道が指定順位以内である期間の計測が０から再開されてもよい。第２指定期間が経過する前に、それまで第２候補軌道として指定されていた候補軌道に対応する候補状態が変化した場合、当該変化後の候補状態に応じて新たに生成された候補軌道から第２候補軌道の指定が解除されてもよい。
【００１２】
本発明の状態予測システムにおいて、前記軌道指定要素が、前記第１指定期間にわたり前記近似度が指定順位以内であり、かつ、前記近似度が閾値以上である候補軌道を前記第１候補軌道として指定することが好ましい。
【００１３】
当該構成の状態予測システムによれば、オブジェクトの状態の予測結果としての候補軌道の指定結果の安定性向上に加えて信頼度向上が図られる。
【００１６】
本発明の状態予測システムにおいて、前記軌道指定要素が、前記複数の候補状態のうち少なくとも１つの候補状態が前記オブジェクトと他の物体との相互作用を伴う状態である場合、前記オブジェクトが前記少なくとも１つの候補状態に遷移するまでの時間が短くなるほど、前記第１指定期間として長い期間を採用するまたは前記閾値として大きい値を採用することが好ましい。
【００１７】
当該構成の状態予測システムによれば、オブジェクトと他の物体とを相互作用させる観点から当該オブジェクトの状態予測が重要な段階において、当該予測結果のその他の段階に対する相対的な安定性向上が図られる。
【００１８】
本発明の状態予測システムにおいて、前記軌道指定要素が、前記オブジェクトの状態および前記複数の候補状態のそれぞれのうち少なくとも１つの変化量が大きいほど安定度を低く評価し、前記安定度が低いほど、前記第１指定期間として長い期間を採用するまたは前記閾値として大きい値を採用することが好ましい。
【００１９】
当該構成の状態予測システムによれば、オブジェクトの状態予測結果の精度が損なわれかねない状況において、当該オブジェクトの状態予測結果の安定性の向上が図られる。
【００２０】
本発明の状態予測システムにおいて、前記軌道指定要素が、今回の前記第１候補軌道に対して評価指数の今回値として指定の正値を付与し、今回の前記第１候補軌道を除く候補軌道に対して、前記評価指数の前回値に１未満の正値である減衰係数を乗じた結果を前記評価指数の今回値として付与し、前記評価指数の今回値が基準値以上である少なくとも１つの候補軌道を今回の前記第２候補軌道として指定することが好ましい。
【００２１】
当該構成の状態予測システムによれば、各候補軌道に対する評価指数の付与によって、オブジェクトの状態予測結果としての候補軌道の指定結果の安定性の向上が図られる。
【図面の簡単な説明】
【００２２】
【図１】本発明の一実施形態としての状態予測システムの構成説明図。
【図２】本発明の一実施形態としての状態予測システムの機能説明図。
【図３】軌道生成に関する説明図。
【図４】本発明の実施例に関する説明図。
【図５】基準軌道および候補軌道の生成例に関する説明図。
【図６】基準軌道に対する各候補軌道の距離の変化態様に関する説明図。
【図７】各候補軌道の評価指数の変化態様に関する説明図。
【発明を実施するための形態】
【００２３】
（構成）
図１に示されている状態予測システム１は、コンピュータ（ＣＰＵ，ＲＯＭ，ＲＡＭ，Ｉ／Ｏ回路等により構成されている。）により構成されている。当該コンピュータは、状態予測結果を用いて動作が制御され移動装置などの制御対象に搭載されていてもよく、当該制御対象に対して指令信号を付与するサーバを構成していてもよい。
【００２４】
状態予測システム１は、状態推定要素１０と、第１軌道生成要素１１と、第２軌道生成要素１２と、軌道指定要素１３と、を備えている。状態推定要素１０は、たとえばオブジェクトの様子を撮像するための撮像装置を備えていてもよく、オブジェクトの現在状態の検知結果が入力されるインターフェース回路または装置を備えていてもよい。状態推定要素１０は、状態予測システム１ではなく外部システムの構成要素であってもよい。
【００２５】
各要素１０〜１３は、後述する担当演算処理を実行するよう構成されている。各要素が担当演算処理を実行するように「構成されている」とは、要素を構成するＣＰＵ等の演算処理装置が、ＲＯＭ、ＲＡＭ等のメモリ又は記録媒体から必要な情報に加えてソフトウェアを読み出し、当該情報に対して当該ソフトウェアにしたがって演算処理を実行するように「プログラムされている」または「デザイン（設計）されている」ことを意味する。各構成要素が共通のプロセッサ（演算処理装置）により構成されてもよく、各構成要素が相互通信可能な複数のプロセッサにより構成されてもよい。
【００２６】
（機能）
まず、演算処理周期Δごとに計測される離散的な時刻を表わす指数ｋが「０」にリセットされる（図２／ＳＴＥＰ０１）。
【００２７】
状態推定要素１０により、オブジェクトの現在状態ｐ0,kが時系列的に推定される（図２／ＳＴＥＰ０２）。当該推定には、たとえばカルマンフィルタが用いられ、現在状態ｐ0,kが確率密度分布（正規分布）Ｎ（μ0,k，Σ0,k）により表現される。
【００２８】
第１軌道生成要素１１により、オブジェクトの現在状態ｐ0,kに基づき、第１モデルにしたがって、未来におけるオブジェクトの状態の確率密度分布の遷移態様を表わす基準軌道Ｐ0が生成される（図２／ＳＴＥＰ０４）。第１モデルとして、オブジェクトの状態空間における推定位置ｐ0,k、推定速度ｖ0,k＝（ｐ0,k−ｐ0,k-1）／Δおよび推定加速度α0,k＝（ｖ0,k−ｖ0,k-1）／Δによりオブジェクトの状態の将来位置ｐ0,k+1等が定まる動力学的モデルが採用される。
【００２９】
これにより、図３に示されているように、状態空間におけるオブジェクトの予測位置ｐ0,k+1を基準として、その確率密度分布（正規分布）Ｎ（μ0,k+1，Σ0,k+1）の分散Σ0,k+1に応じた広がり（環状の実線参照）を有する基準軌道Ｐ0が生成される。状態空間においてオブジェクトの状態ｐ0,kは速度ｖ0,kで遷移する。
【００３０】
第２軌道生成要素１２により、オブジェクトの現在状態ｐ0,kおよび複数の候補状態ｐi,e（ｉ＝１，２，‥）に基づき、第２モデルにしたがって、オブジェクトの現在状態ｐ0,kから複数の候補状態ｐi,eのそれぞれまでの状態の確率密度分布の遷移態様を表わす複数の候補軌道Ｐi（またはアフォーダンス軌道）が生成される（図２／ＳＴＥＰ０６）。第２モデルとして、たとえばホビースプライン（Ｈｏｂｂｙ  Ｓｐｌｉｎｅ）法が用いられる。そのほかのスプライン法または軌道生成モデルが第２モデルとして採用されてもよい。
【００３１】
これにより、図３に示されているように、状態空間におけるオブジェクトの予測位置ｐi,k+1を基準として、その確率密度分布（正規分布）Ｎ（μi,k+1，Σi,k+1）の分散Σi,k+1に応じた広がり（環状の実線参照）を有する基準軌道Ｐiが生成される。状態空間においてオブジェクトの状態ｐi,kは速度ｖi,kで遷移する。
【００３２】
軌道指定要素１３により、第２軌道生成要素１２により生成された複数の候補軌道Ｐiのそれぞれについて、第１軌道生成要素１１により生成された基準軌道Ｐ0との近似度Ａiが評価される（図２／ＳＴＥＰ０８）。
【００３３】
具体的には、基準軌道分布Ｐ0と各候補軌道Ｐiとの距離Ｄiの逆数が近似度Ａiとして計算される。当該距離Ｄiは、基準軌道分布Ｐ0および各候補軌道Ｐiのそれぞれを構成する同時刻ｋにおける確率密度分布ｐ0kおよびｐ1kの距離Ｄikの累積であり、関係式（０２）により表現される。
【００３４】
Ｄi＝Ｄ（Ｐ0，Ｐi）＝Σk=1min(||0||,||i||)Ｄik，
Ｄik＝Ｄ（［ｐ0k，ｑ0k］，［ｐik，ｑik］）
＝ｃ（ｑ0k，ｑik）・Ｂ（ｐ0k，ｐik）‥（０２）。
【００３５】
ｃ（ｑ0k，ｑik）は、状態空間における確率密度分布ｐ0kの単位速度ベクトルｑ0k＝ｖ0k／｜ｖ0k｜および確率密度分布の単位速度ベクトルｑik＝ｖik／｜ｖik｜の内積ｑ0k・ｑikを用いて、関係式（０４）により表現される係数である。
【００３６】
ｃ（ｑ0k，ｑik）＝１．５−０．５ｑ0k・ｑik  ‥（０４）。
【００３７】
たとえば、両ベクトルが逆方向であれば内積ｑ0k・ｑikは「−１」になるので、係数ｃ（ｑ0k，ｑik）は「２」になり、両ベクトルが相互に垂直であれば内積ｑ0k・ｑikは「０」になるので、係数ｃ（ｑ0k，ｑik）は「１．５」になり、両ベクトルが同方向であれば内積ｑ0k・ｑikは「１」になるので、係数ｃ（ｑ0k，ｑik）は「１」になる。すなわち、両ベクトルの方向が近似するほど係数ｃ（ｑ0k，ｑik）が小さくなり、確率密度分布ｐ0kおよびｐ1kの距離Ｄikが短くなる。
【００３８】
なお、係数ｃ（ｑ0k，ｑik）として、関係式（０４）に代えて、平均値μNおよび標準偏差σNを有するラップド正規分布（Ｗｒａｐｐｅｄ  Ｎｏｒｍａｌ  Ｄｉｓｔｒｉｂｕｔｉｏｎ）ＷＮ（θk；μN，σN）を用いて、関係式（０５）により表現される係数が採用されてもよい。
【００３９】
ｃ（ｑ0k，ｑik）＝１／ＷＮ（θk；μN，σN），
θk≡ａｒｃｃｏｓ（ｑ0k・ｑik），
ＷＮ（θk；μN，σN）
≡｛１／（σN（２π）1/2）｝
×Σj=-∞+∞ｅｘｐ｛−（θk−μN−２πｊ）2／２σN2｝
＝（１／２π）ζ（（θk−μN）／２π，ｉσN2／２π），
ζ（θ，τ）≡Σn=-∞+∞ｅｘｐ（２ｎｉπθ）・ｅｘｐ（ｎ2ｉπτ）  ‥（０５）。
【００４０】
Ｂ（ｐ0k，ｐik）は、正規分布である確率密度分布ｐ0k＝Ｎ（μ0k，Σ0k）およびｐik＝Ｎ（μik，Σik）のバタチャラヤ（Ｂｈａｔｔａｃｈａｒｙｙａ）距離であり、関係式（０６）により表現される。
【００４１】
Ｂ（ｐ0k，ｐik）
＝（１／８）（μ0k−μik）TΣ-1（μ0k−μik）
＋（１／２）ｌｎ｛ｄｅｔ（（Σ0k＋Σik）／２）／（ｄｅｔΣ0k・ｄｅｔΣik）1/2｝‥（０６）。
【００４２】
関係式（０６）右辺第１項から、両確率密度分布の平均μ0kおよびμikの差が小さいほど、距離Ｂ（ｐ0k，ｐik）が短くなることがわかる。関係式（０６）右辺第２項から、両確率密度分布の広がりを表わす分散（共分散行列）Σ0kおよびΣikに応じて距離Ｂ（ｐ0k，ｐik）が変化することがわかる。なお、確率密度分布同士の距離として、マハロノビス（Ｍａｈａｌａｎｏｂｉｓ）距離、ＫＬ  Ｄｉｖｅｒｇｅｎｃｅまたはガウス分布同士の畳み込みなどが代替的に採用されてもよい。
【００４３】
これにより、各候補軌道Ｐiについて予測位置軌道Ｐ0に対する近似度Ａiが高い（距離Ｄiが短い）順に順位が定義されうる。
【００４４】
軌道指定要素１３により、複数の候補軌道Ｐiの中で第１指定要件を満たす候補軌道の有無が判定される（図２／ＳＴＥＰ１０）。「第１指定要件」として、（１）第１指定期間にわたり近似度Ａiが指定順位以内であること、および（２）第１指定期間にわたり近似度Ａiが閾値以上であること（距離Ｄiが基準距離以下であること）が採用されている。第１指定期間は、時刻を表わす指数ｋが指定回数（たとえば「２」）だけ更新される期間を表わす。指定順位としてはたとえば「第２位」が採用される。なお、要件（２）は省略されてもよい。
【００４５】
たとえば、今回の近似度Ａiが第ｎ位（たとえばｎ＝１，２）の候補軌道を表わす指数ｊ（＝１，２‥）がリストの第ｎ欄に記録され、今回および過去ｍ回（たとえばｍ＝２）にわたり連続で当該指数ｉがリストに記録されている指数ｊを有する候補軌道Ｐjが、第１指定期間にわたり近似度Ａiが指定順位以内であると判断される。
【００４６】
当該判定結果が肯定的である場合（図２／ＳＴＥＰ１０‥ＹＥＳ）、第１指定要件を満たす少なくとも１つの候補軌道が今回の「第１候補軌道」として指定される（図２／ＳＴＥＰ１２）。当該判定結果が否定的である場合（図２／ＳＴＥＰ１０‥ＮＯ）、いずれの候補軌道も今回の「第１候補軌道」として指定されない。
【００４７】
軌道指定要素１３により、今回の第１候補軌道を除く複数の候補軌道Ｐiの中で第２指定要件を満たす候補軌道の有無が判定される（図２／ＳＴＥＰ１４）。「第２指定要件」として、第１候補軌道に指定された最後の時点から第２指定期間が経過する前であることが採用されている。
【００４８】
たとえば、複数の候補軌道Ｐiのそれぞれに対して評価指数ｅiが付与され、当該評価指数ｅiが基準値ｅ0以上であるか否かに応じて、第２指定要件の充足性が判断される。具体的には、今回の第１候補軌道に指定された候補軌道Ｐjの評価指数ｅjが一定の正値（たとえば「１」）に設定される。各候補軌道Ｐiの前回評価指数ｅi,k-1に減衰係数γ（０＜γ＜１（たとえば「０．９９」））が乗じられることにより、各候補軌道Ｐiの今回評価指数ｅi,kが設定される。これにより、第１候補軌道として指定された最後の時点では評価指数ｅjが「１」であったものの、その後、減衰係数γが累積的に乗じられることにより評価指数ｅjが徐々に小さくなる。そして、評価指数ｅjが基準値ｅ0より小さい値（＝γb）となった場合、ｂ−１＜ｌｏｇγｅ0≦ｂという不等式を満たす整数ｂだけ指数ｋが更新される期間が第２指定期間に相当する。
【００４９】
当該判定結果が肯定的である場合（図２／ＳＴＥＰ１４‥ＹＥＳ）、第２指定要件を満たす少なくとも１つの候補軌道が今回の「第２候補軌道」として指定される（図２／ＳＴＥＰ１６）。当該判定結果が否定的である場合（図２／ＳＴＥＰ１４‥ＮＯ）、いずれの候補軌道も今回の「第２候補軌道」として指定されない。
【００５０】
続いて、オブジェクトの状態予測を終了するための条件（たとえば、オブジェクトの推定状態が複数の候補状態のうちいずれかに一致するまでの残り期間が指定期間以下になったこと）が満たされたか否かが判定される（図２／ＳＴＥＰ１８）。当該判定結果が否定的であれば（図２／ＳＴＥＰ１８‥ＮＯ）、指数ｋが１だけ増加されたうえで（図２／ＳＴＥＰ２０）、オブジェクトの状態推定以降の一連の処理が繰り返される（図２／ＳＴＥＰ０２〜ＳＴＥＰ１８参照）。当該判定結果が肯定的であれば（図２／ＳＴＥＰ１８‥ＹＥＳ）、前述の一連の処理が終了する。
【００５１】
第１候補軌道および第２候補軌道のそれぞれの今回指定の有無、今回の第１候補軌道として指定された候補軌道、および、今回の第２候補軌道として指定された候補軌道のそれぞれが、オブジェクトの状態予測結果として、予測状態システム１から外部システムに対して出力されてもよい。
【００５２】
（実施例）
図４に示されているように、ヒューマノイドロボット２１および人間２２がテーブル２０を挟んで向き合っている状態で、ロボット２１に搭載された予測状態システム１が、人間２２の右手をオブジェクトとしてその状態を予測する場合について説明する。ロボット２１の構成は、たとえば特許文献４に記載されているので、詳細な説明を省略する。
【００５３】
テーブル２０には、２つのコップ３１および３２、ならびに１つのトレイ３４が載せられている。（１）ロボット２１から見て右側のコップ３１、（２）左側のコップ３２の位置、トレイ３４の（３）右手前、（４）左手前、（５）右奥および（６）左奥のそれぞれにある４つの区画、トレイ３４の（７）右縁および（８）左縁のそれぞれの代表的な位置のそれぞれが候補位置（候補状態）ｘ1〜ｘ8のそれぞれとして定義されている。人間２２の右手の位置ｘ0は、たとえばロボット２１の頭部に搭載されている画像センサＳ1または胴体前下部に搭載されている赤外線センサＳ2の出力信号に基づき、状態推定要素１０によりオブジェクトの状態として時系列的に推定される。各候補位置ｘ1〜ｘ8も画像センサＳ1または赤外線センサＳ2の出力信号に基づき、状態推定要素１０により候補状態として時系列的に推定される。なお、各候補位置ｘ1〜ｘ8を推定するためのセンサ（たとえば画像センサＳ1または赤外線センサＳ2など）として、ロボット２１の外部に設置されたセンサが用いられてもよい。
【００５４】
図５には、第１軌道生成要素１１により人間２２の右手の推定位置ｘ0,kに基づいて生成された基準軌道Ｐ0、および、第２軌道生成要素１２により人間２２の右手の推定位置ｘ0,kおよび各候補位置ｘ1〜ｘ8に基づいて生成された複数の候補軌道Ｐ1〜Ｐ8が示されている（図２／ＳＴＥＰ０４，０６および図３参照）。
【００５５】
図６には、基準軌道Ｐ0と複数の候補軌道Ｐ1〜Ｐ8との距離Ｄ1〜Ｄ8のそれぞれの時系列が示されている（図２／ＳＴＥＰ０８および関係式（０２）〜（０６）参照）。図７には、当該距離Ｄ1〜Ｄ8の長短（またはその逆数である近似度距離Ａ1〜Ａ8の高低）に応じた、各候補軌道Ｐ1〜Ｐ8の評価指数ｅ1〜ｅ8の時系列が示されている。
【００５６】
期間ｔ0〜ｔ1では候補位置ｘ3（トレイ３４の右手前区画の位置）に対応する候補軌道Ｐ3の評価指数ｅ3が「１」であり、当該候補軌道Ｐ3が「第１候補軌道」として指定された。期間ｔ1〜ｔ2では、候補軌道Ｐ3の評価指数ｅ3が「１」から徐々に減少しているものの（第１指定条件が満たされていないことを意味する）、第２指定期間が経過する前（評価指数ｅ3が基準値ｅ0（＝０．４）以上）であるため、候補軌道Ｐ3が「第２候補軌道」に指定された。
【００５７】
時刻ｔ2において候補位置ｘ5（トレイ３４の右奥区画の位置）に対応する候補軌道Ｐ5の評価指数ｅ5が「１」になり、当該候補軌道Ｐ5が第１候補軌道として指定された。時刻ｔ3において候補軌道Ｐ3の評価指数ｅ3が基準値ｅ0（＝０．４）未満になり、その第２候補軌道の指定が解除された。その後、時刻ｔ4において候補軌道Ｐ3の評価指数ｅ3が再び「１」になり、候補軌道Ｐ3が第１候補軌道として指定された。
【００５８】
期間ｔ2〜ｔ4では候補軌道Ｐ5の評価指数ｅ3が「１」であり、当該候補軌道Ｐ5が「第１候補軌道」として指定された。期間ｔ4以降、候補軌道Ｐ5の評価指数ｅ5が「１」から徐々に減少し、第２指定期間が経過する前であるため、候補軌道Ｐ5が第２候補軌道に指定された。期間ｔ5以降、候補軌道Ｐ3の評価指数ｅ3が「１」から徐々に減少し、第２指定期間が経過する前であるため、候補軌道Ｐ3も候補軌道Ｐ5に加えて第２候補軌道に指定された。
【００５９】
上記の予測結果の遷移態様を表１にまとめて示す。
【００６０】
【表１】
0006293652.tif 000002
【００６１】
ロボット２１に搭載されている制御装置（図示略）は、状態予測システム１から出力された人間２２の手（オブジェクト）の位置の予測結果（候補軌道の指定結果（表１参照））を逐次受け取り、これに基づいてロボット２１の動作を制御する。
【００６２】
たとえば、第１候補軌道および第２候補軌道のいずれにも干渉しないようなハンドの位置姿勢軌道が生成され、これにしたがってロボット２１のアーム等の動作が制御される。第１候補軌道または第２候補軌道に対応する候補位置のそばにハンドを近づけるようにロボットのアーム等の動作が制御される。トレイ３４における候補位置ｘ3〜ｘ8のいずれかに向かう候補軌道Ｐ3〜Ｐ8のいずれかが第１候補軌道として指定されたことに応じて、ロボット２１がコップ３１または３２を把持するようにその動作が制御されてもよい。人間２２が手でコップ３１、３２を持った場合、当該コップ３１、３２を新たなオブジェクトとして未来における位置が予測されてもよい。
【００６３】
（効果）
前記状態予測システム１によれば、オブジェクトの現在状態に基づいて生成される基準軌道Ｐ0との近似度Ａi（＝１／Ｄi）が第１指定期間にわたり指定順位以内であった少なくとも１つの候補軌道Ｐiが「第１候補軌道」として指定される（図２／ＳＴＥＰ１０‥ＹＥＳ→ＳＴＥＰ１２および図７参照）。このため、オブジェクトの状態がそれにしたがって遷移する蓋然性が一時的にのみ高かった候補軌道が第１候補軌道として指定されることが回避される。これにより、オブジェクトの状態予測結果としての当該指定結果の変化頻度が低減されうる。第１指定要件に近似度Ａiが第１指定期間にわたり閾値以上であることが含まれているため、オブジェクトの状態予測精度の向上も図られている。
【００６４】
第１候補軌道であった最後の時点から第２指定期間が経過前である少なくとも１つの候補軌道が「第２候補軌道」として指定される（図２／ＳＴＥＰ１４‥ＹＥＳ→ＳＴＥＰ１６および図７参照）。オブジェクトの状態予測結果としての当該指定結果により、オブジェクトの状態が第２候補軌道にしたがって遷移する可能性が残されていることが顕在化される（図７／ｔ1〜ｔ3およびｔ5以降における候補軌道Ｐ3ならびにｔ4以降における候補軌道Ｐ5参照）。これにより、第２候補軌道に指定されていた候補軌道が再び第１候補軌道に指定された場合でも、オブジェクトの状態の予測結果が、それまでの予測結果から著しく乖離する事態が回避されうる（図７／時刻ｔ4における候補軌道Ｐ3参照）。第１候補軌道が指定されない状態では、オブジェクトの状態予測結果としての当該指定結果により、オブジェクトの状態が第２候補軌道にしたがって遷移する可能性が最も高いものの、その可能性は第１候補軌道よりも低いことが顕在化される（図７／ｔ1〜ｔ3およびｔ5以降における候補軌道Ｐ3ならびにｔ4以降における候補軌道Ｐ5参照）。これにより、その後、異なる候補軌道が第１候補軌道として指定された場合でも、オブジェクトの状態の予測結果が、それまでの予測結果から著しく乖離する事態が回避されうる。
【００６５】
よって、オブジェクトの状態の予測結果としての候補軌道の指定結果の安定性向上が図られる。
【００６６】
（本発明の他の実施形態）
状態予測システム１が車両に搭載され、当該車両の周辺に存在する他の車両または通行人などの周囲に存在するオブジェクトの状態または挙動が予測され、当該予測結果に基づいてオブジェクトとの接触が回避されるように車両の挙動が制御されてもよい。この場合、オブジェクトの候補状態として、たとえば道路における空きスペースに当該オブジェクトとしての他車両が存在する状態、または、通行人を対象とした催事場にオブジェクトが存在する状態が採用される。前記のように、状態予測システム１による予測対象となる状態には、オブジェクトとしての動物の心理状態および生理状態なども含まれていてもよい。
【００６７】
軌道指定要素１３により、オブジェクトの状態および複数の候補状態のそれぞれのうち少なくとも１つに応じて、第１指定期間、第２指定期間および閾値のうち少なくとも１つが変更されてもよい。たとえば、複数の候補状態のうち少なくとも１つの候補状態がオブジェクトと他の物体との相互作用を伴う状態である場合、オブジェクトが少なくとも１つの候補状態に遷移するまでの時間が短くなるほど、第１指定期間として長い期間が採用されてもよくまたは閾値として大きい値が採用されてもよい。「オブジェクトと他の物体との相互作用を伴う状態」とは、たとえば人間２２の手（オブジェクト）がカップ３１、３２またはトレイ３４（他の物体）と接触する状態を意味する。
【００６８】
当該構成によれば、オブジェクトと他の物体とを相互作用させる観点から当該オブジェクトの状態予測が重要な段階（オブジェクトが他の物体と相互作用する直前の段階）において、当該予測結果のその他の段階に対する相対的な安定性向上が図られる。
【００６９】
オブジェクトの状態および複数の候補状態のそれぞれのうち少なくとも１つの安定度が評価され、当該安定度が低いほど、第１指定期間として長い期間が採用されてもよくまたは閾値として大きい値が採用されてもよい。たとえば、人間２２の手または、コップ３１、３２もしくはトレイ３４の位置の単位時間当たりの変化量が大きいほど安定度が低く評価される。
【００７０】
当該構成によれば、オブジェクトの状態および候補状態の相対的な関係が大きく変化するため、前回候補軌道および今回候補軌道の形態が著しく異なるなど、オブジェクトの状態予測結果の精度が損なわれかねない状況において、当該オブジェクトの状態予測結果の安定性の向上が図られる。
【符号の説明】
【００７１】
１‥状態予測システム、１０‥状態推定要素、１１‥第１軌道生成要素、１２‥第２軌道生成要素、１３‥軌道指定要素。
【図１】
0006293652.tif 000003
【図２】
0006293652.tif 000004
【図３】
0006293652.tif 000005
【図４】
0006293652.tif 000006
【図５】
0006293652.tif 000007
【図６】
0006293652.tif 000008
【図７】
0006293652.tif 000009