JP6293652B2

JP6293652B2 - 状態予測システム

Info

Publication number: JP6293652B2
Application number: JP2014264272A
Authority: JP
Inventors: 総司射場
Original assignee: Honda Motor Co Ltd
Current assignee: Honda Motor Co Ltd
Priority date: 2014-12-26
Filing date: 2014-12-26
Publication date: 2018-03-14
Anticipated expiration: 2034-12-26
Also published as: US10166677B2; US20160184991A1; JP2016126387A

Description

本発明は、オブジェクトの状態を予測する技術に関する。

ロボットが人間と同じ環境で動作する場合、人間を含むオブジェクトの動きに応じてロボットの運動が適応的に変更される必要がある。そこで、オブジェクトの状態を予測する手法が提案されている（特許文献１〜４参照）。

特開２０１３−１４３１３４号公報特開２０１３−１９６６０１号公報特開２０１３−０５４７０３号公報特許第５４８９９６５号公報

しかし、オブジェクトの状態予測の条件によっては、予測状態が高頻度で変動するなど不安定になり、または当該予測精度が不安定になる可能性がある。

そこで、本発明は、オブジェクトの状態の予測結果の安定性の向上を図ることができるシステムを提供することを解決課題とする。

本発明の状態予測システムは、オブジェクトの現在状態に基づき、第１モデルにしたがって、未来における前記オブジェクトの状態の確率密度分布の遷移態様を表わす基準軌道を生成する第１軌道生成要素と、前記オブジェクトの現在状態および複数の候補状態に基づき、第２モデルにしたがって、前記オブジェクトの現在状態から前記複数の候補状態のそれぞれまでの状態の確率密度分布の遷移態様を表わす複数の候補軌道を生成する第２軌道生成要素と、前記第２軌道生成要素により生成された前記複数の候補軌道のそれぞれについて前記第１軌道生成要素により生成された前記基準軌道との近似度を評価し、第１指定期間にわたり前記近似度が指定順位以内である候補軌道である第１候補軌道、および、前記第１候補軌道として指定されていた最後の時点から第２指定期間経過前であるその他の候補軌道である第２候補軌道のうち少なくとも１つを前記オブジェクトの状態予測結果として指定する軌道指定要素と、を備えていることを特徴とする。

本発明の状態予測システムによれば、オブジェクトの現在状態に基づいて生成される基準軌道との近似度が第１指定期間にわたり指定順位以内であった少なくとも１つの候補軌道が「第１候補軌道」として指定される。このため、オブジェクトの状態がそれにしたがって遷移する蓋然性が一時的にのみ高かった候補軌道が第１候補軌道として指定されることが回避される。これにより、オブジェクトの状態予測結果としての当該指定結果の変化頻度が低減されうる。

第１候補軌道であった最後の時点から第２指定期間が経過前である少なくとも１つの候補軌道が「第２候補軌道」として指定される。オブジェクトの状態予測結果としての当該指定結果により、オブジェクトの状態が第２候補軌道にしたがって遷移する可能性が残されていることが顕在化される。これにより、第２候補軌道に指定されている候補軌道が再び第１候補軌道に指定された場合でも、オブジェクトの状態の予測結果が、それまでの予測結果から著しく乖離する事態が回避されうる。第１候補軌道が指定されない状態では、オブジェクトの状態予測結果としての当該指定結果により、オブジェクトの状態が第２候補軌道にしたがって遷移する可能性が最も高いものの、その可能性は第１候補軌道よりも低いことが顕在化される。これにより、その後、異なる候補軌道が第１候補軌道として指定された場合でも、オブジェクトの状態の予測結果が、それまでの予測結果から著しく乖離する事態が回避されうる。

よって、オブジェクトの状態の予測結果としての候補軌道の指定結果の安定性向上が図られる。

なお、「オブジェクト」には、ひとまとまりの物体のほか、当該物体の部分も含まれる。「状態」は一または複数のパラメータにより定義され、当該パラメータには、オブジェクトの位置、姿勢および速度などの動力学的パラメータ、オブジェクトの温度、色および輝度などの物理量、ならびにオブジェクトとしての人間などの動物の心理状態または生理状態を表現するパラメータなど、適当なセンサを用いて測定または検出可能なあらゆるパラメータが含まれる。「状態空間」は当該パラメータの次元数を有するヒルベルト空間によって定義される。たとえば、オブジェクトの状態がその３次元実空間における位置である場合、当該状態は３次元の状態空間におけるベクトルとして定義される。オブジェクトの状態がその３次元実空間における位置および姿勢である場合、当該状態は６次元の状態空間におけるベクトルとして定義される。

第１指定期間が経過する前に、それまで基準軌道に対する近似度が指定順位以内であった候補軌道に対応する候補状態が変化した場合、当該変化後の候補状態に応じて新たに生成された候補軌道が指定順位以内である期間の計測が０から再開されてもよい。第２指定期間が経過する前に、それまで第２候補軌道として指定されていた候補軌道に対応する候補状態が変化した場合、当該変化後の候補状態に応じて新たに生成された候補軌道から第２候補軌道の指定が解除されてもよい。

本発明の状態予測システムにおいて、前記軌道指定要素が、前記第１指定期間にわたり前記近似度が指定順位以内であり、かつ、前記近似度が閾値以上である候補軌道を前記第１候補軌道として指定することが好ましい。

当該構成の状態予測システムによれば、オブジェクトの状態の予測結果としての候補軌道の指定結果の安定性向上に加えて信頼度向上が図られる。

本発明の状態予測システムにおいて、前記軌道指定要素が、前記複数の候補状態のうち少なくとも１つの候補状態が前記オブジェクトと他の物体との相互作用を伴う状態である場合、前記オブジェクトが前記少なくとも１つの候補状態に遷移するまでの時間が短くなるほど、前記第１指定期間として長い期間を採用するまたは前記閾値として大きい値を採用することが好ましい。

当該構成の状態予測システムによれば、オブジェクトと他の物体とを相互作用させる観点から当該オブジェクトの状態予測が重要な段階において、当該予測結果のその他の段階に対する相対的な安定性向上が図られる。

本発明の状態予測システムにおいて、前記軌道指定要素が、前記オブジェクトの状態および前記複数の候補状態のそれぞれのうち少なくとも１つの変化量が大きいほど安定度を低く評価し、前記安定度が低いほど、前記第１指定期間として長い期間を採用するまたは前記閾値として大きい値を採用することが好ましい。

当該構成の状態予測システムによれば、オブジェクトの状態予測結果の精度が損なわれかねない状況において、当該オブジェクトの状態予測結果の安定性の向上が図られる。

本発明の状態予測システムにおいて、前記軌道指定要素が、今回の前記第１候補軌道に対して評価指数の今回値として指定の正値を付与し、今回の前記第１候補軌道を除く候補軌道に対して、前記評価指数の前回値に１未満の正値である減衰係数を乗じた結果を前記評価指数の今回値として付与し、前記評価指数の今回値が基準値以上である少なくとも１つの候補軌道を今回の前記第２候補軌道として指定することが好ましい。

当該構成の状態予測システムによれば、各候補軌道に対する評価指数の付与によって、オブジェクトの状態予測結果としての候補軌道の指定結果の安定性の向上が図られる。

本発明の一実施形態としての状態予測システムの構成説明図。本発明の一実施形態としての状態予測システムの機能説明図。軌道生成に関する説明図。本発明の実施例に関する説明図。基準軌道および候補軌道の生成例に関する説明図。基準軌道に対する各候補軌道の距離の変化態様に関する説明図。各候補軌道の評価指数の変化態様に関する説明図。

（構成）
図１に示されている状態予測システム１は、コンピュータ（ＣＰＵ，ＲＯＭ，ＲＡＭ，Ｉ／Ｏ回路等により構成されている。）により構成されている。当該コンピュータは、状態予測結果を用いて動作が制御され移動装置などの制御対象に搭載されていてもよく、当該制御対象に対して指令信号を付与するサーバを構成していてもよい。

状態予測システム１は、状態推定要素１０と、第１軌道生成要素１１と、第２軌道生成要素１２と、軌道指定要素１３と、を備えている。状態推定要素１０は、たとえばオブジェクトの様子を撮像するための撮像装置を備えていてもよく、オブジェクトの現在状態の検知結果が入力されるインターフェース回路または装置を備えていてもよい。状態推定要素１０は、状態予測システム１ではなく外部システムの構成要素であってもよい。

各要素１０〜１３は、後述する担当演算処理を実行するよう構成されている。各要素が担当演算処理を実行するように「構成されている」とは、要素を構成するＣＰＵ等の演算処理装置が、ＲＯＭ、ＲＡＭ等のメモリ又は記録媒体から必要な情報に加えてソフトウェアを読み出し、当該情報に対して当該ソフトウェアにしたがって演算処理を実行するように「プログラムされている」または「デザイン（設計）されている」ことを意味する。各構成要素が共通のプロセッサ（演算処理装置）により構成されてもよく、各構成要素が相互通信可能な複数のプロセッサにより構成されてもよい。

（機能）
まず、演算処理周期Δごとに計測される離散的な時刻を表わす指数ｋが「０」にリセットされる（図２／ＳＴＥＰ０１）。

状態推定要素１０により、オブジェクトの現在状態ｐ_0,kが時系列的に推定される（図２／ＳＴＥＰ０２）。当該推定には、たとえばカルマンフィルタが用いられ、現在状態ｐ_0,kが確率密度分布（正規分布）Ｎ（μ_0,k，Σ_0,k）により表現される。

第１軌道生成要素１１により、オブジェクトの現在状態ｐ_0,kに基づき、第１モデルにしたがって、未来におけるオブジェクトの状態の確率密度分布の遷移態様を表わす基準軌道Ｐ₀が生成される（図２／ＳＴＥＰ０４）。第１モデルとして、オブジェクトの状態空間における推定位置ｐ_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,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のそれぞれについて、第１軌道生成要素１１により生成された基準軌道Ｐ₀との近似度Ａ_iが評価される（図２／ＳＴＥＰ０８）。

具体的には、基準軌道分布Ｐ₀と各候補軌道Ｐ_iとの距離Ｄ_iの逆数が近似度Ａ_iとして計算される。当該距離Ｄ_iは、基準軌道分布Ｐ₀および各候補軌道Ｐ_iのそれぞれを構成する同時刻ｋにおける確率密度分布ｐ_0kおよびｐ_1kの距離Ｄ_ikの累積であり、関係式（０２）により表現される。

Ｄ_i＝Ｄ（Ｐ₀，Ｐ_i）＝Σ_k=1 ^{min(||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−２πｊ）²／２σ_N ²｝
＝（１／２π）ζ（（θ_k−μ_N）／２π，ｉσ_N ²／２π），
ζ（θ，τ）≡Σ_n=-∞ ^+∞ｅｘｐ（２ｎｉπθ）・ｅｘｐ（ｎ²ｉπτ） ‥（０５）。

Ｂ（ｐ_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について予測位置軌道Ｐ₀に対する近似度Ａ_iが高い（距離Ｄ_iが短い）順に順位が定義されうる。

軌道指定要素１３により、複数の候補軌道Ｐ_iの中で第１指定要件を満たす候補軌道の有無が判定される（図２／ＳＴＥＰ１０）。「第１指定要件」として、（１）第１指定期間にわたり近似度Ａ_iが指定順位以内であること、および（２）第１指定期間にわたり近似度Ａ_iが閾値以上であること（距離Ｄ_iが基準距離以下であること）が採用されている。第１指定期間は、時刻を表わす指数ｋが指定回数（たとえば「２」）だけ更新される期間を表わす。指定順位としてはたとえば「第２位」が採用される。なお、要件（２）は省略されてもよい。

たとえば、今回の近似度Ａ_iが第ｎ位（たとえばｎ＝１，２）の候補軌道を表わす指数ｊ（＝１，２‥）がリストの第ｎ欄に記録され、今回および過去ｍ回（たとえばｍ＝２）にわたり連続で当該指数ｉがリストに記録されている指数ｊを有する候補軌道Ｐ_jが、第１指定期間にわたり近似度Ａ_iが指定順位以内であると判断される。

当該判定結果が肯定的である場合（図２／ＳＴＥＰ１０‥ＹＥＳ）、第１指定要件を満たす少なくとも１つの候補軌道が今回の「第１候補軌道」として指定される（図２／ＳＴＥＰ１２）。当該判定結果が否定的である場合（図２／ＳＴＥＰ１０‥ＮＯ）、いずれの候補軌道も今回の「第１候補軌道」として指定されない。

軌道指定要素１３により、今回の第１候補軌道を除く複数の候補軌道Ｐ_iの中で第２指定要件を満たす候補軌道の有無が判定される（図２／ＳＴＥＰ１４）。「第２指定要件」として、第１候補軌道に指定された最後の時点から第２指定期間が経過する前であることが採用されている。

たとえば、複数の候補軌道Ｐ_iのそれぞれに対して評価指数ｅ_iが付与され、当該評価指数ｅ_iが基準値ｅ₀以上であるか否かに応じて、第２指定要件の充足性が判断される。具体的には、今回の第１候補軌道に指定された候補軌道Ｐ_jの評価指数ｅ_jが一定の正値（たとえば「１」）に設定される。各候補軌道Ｐ_iの前回評価指数ｅ_i,k-1に減衰係数γ（０＜γ＜１（たとえば「０．９９」））が乗じられることにより、各候補軌道Ｐ_iの今回評価指数ｅ_i,kが設定される。これにより、第１候補軌道として指定された最後の時点では評価指数ｅ_jが「１」であったものの、その後、減衰係数γが累積的に乗じられることにより評価指数ｅ_jが徐々に小さくなる。そして、評価指数ｅ_jが基準値ｅ₀より小さい値（＝γ^b）となった場合、ｂ−１＜ｌｏｇ_γｅ₀≦ｂという不等式を満たす整数ｂだけ指数ｋが更新される期間が第２指定期間に相当する。

当該判定結果が肯定的である場合（図２／ＳＴＥＰ１４‥ＹＥＳ）、第２指定要件を満たす少なくとも１つの候補軌道が今回の「第２候補軌道」として指定される（図２／ＳＴＥＰ１６）。当該判定結果が否定的である場合（図２／ＳＴＥＰ１４‥ＮＯ）、いずれの候補軌道も今回の「第２候補軌道」として指定されない。

続いて、オブジェクトの状態予測を終了するための条件（たとえば、オブジェクトの推定状態が複数の候補状態のうちいずれかに一致するまでの残り期間が指定期間以下になったこと）が満たされたか否かが判定される（図２／ＳＴＥＰ１８）。当該判定結果が否定的であれば（図２／ＳＴＥＰ１８‥ＮＯ）、指数ｋが１だけ増加されたうえで（図２／ＳＴＥＰ２０）、オブジェクトの状態推定以降の一連の処理が繰り返される（図２／ＳＴＥＰ０２〜ＳＴＥＰ１８参照）。当該判定結果が肯定的であれば（図２／ＳＴＥＰ１８‥ＹＥＳ）、前述の一連の処理が終了する。

第１候補軌道および第２候補軌道のそれぞれの今回指定の有無、今回の第１候補軌道として指定された候補軌道、および、今回の第２候補軌道として指定された候補軌道のそれぞれが、オブジェクトの状態予測結果として、予測状態システム１から外部システムに対して出力されてもよい。

（実施例）
図４に示されているように、ヒューマノイドロボット２１および人間２２がテーブル２０を挟んで向き合っている状態で、ロボット２１に搭載された予測状態システム１が、人間２２の右手をオブジェクトとしてその状態を予測する場合について説明する。ロボット２１の構成は、たとえば特許文献４に記載されているので、詳細な説明を省略する。

テーブル２０には、２つのコップ３１および３２、ならびに１つのトレイ３４が載せられている。（１）ロボット２１から見て右側のコップ３１、（２）左側のコップ３２の位置、トレイ３４の（３）右手前、（４）左手前、（５）右奥および（６）左奥のそれぞれにある４つの区画、トレイ３４の（７）右縁および（８）左縁のそれぞれの代表的な位置のそれぞれが候補位置（候補状態）ｘ₁〜ｘ₈のそれぞれとして定義されている。人間２２の右手の位置ｘ₀は、たとえばロボット２１の頭部に搭載されている画像センサＳ₁または胴体前下部に搭載されている赤外線センサＳ₂の出力信号に基づき、状態推定要素１０によりオブジェクトの状態として時系列的に推定される。各候補位置ｘ₁〜ｘ₈も画像センサＳ₁または赤外線センサＳ₂の出力信号に基づき、状態推定要素１０により候補状態として時系列的に推定される。なお、各候補位置ｘ₁〜ｘ₈を推定するためのセンサ（たとえば画像センサＳ₁または赤外線センサＳ₂など）として、ロボット２１の外部に設置されたセンサが用いられてもよい。

図５には、第１軌道生成要素１１により人間２２の右手の推定位置ｘ_0,kに基づいて生成された基準軌道Ｐ₀、および、第２軌道生成要素１２により人間２２の右手の推定位置ｘ_0,kおよび各候補位置ｘ₁〜ｘ₈に基づいて生成された複数の候補軌道Ｐ₁〜Ｐ₈が示されている（図２／ＳＴＥＰ０４，０６および図３参照）。

図６には、基準軌道Ｐ₀と複数の候補軌道Ｐ₁〜Ｐ₈との距離Ｄ₁〜Ｄ₈のそれぞれの時系列が示されている（図２／ＳＴＥＰ０８および関係式（０２）〜（０６）参照）。図７には、当該距離Ｄ₁〜Ｄ₈の長短（またはその逆数である近似度距離Ａ₁〜Ａ₈の高低）に応じた、各候補軌道Ｐ₁〜Ｐ₈の評価指数ｅ₁〜ｅ₈の時系列が示されている。

期間ｔ₀〜ｔ₁では候補位置ｘ₃（トレイ３４の右手前区画の位置）に対応する候補軌道Ｐ₃の評価指数ｅ₃が「１」であり、当該候補軌道Ｐ₃が「第１候補軌道」として指定された。期間ｔ₁〜ｔ₂では、候補軌道Ｐ₃の評価指数ｅ₃が「１」から徐々に減少しているものの（第１指定条件が満たされていないことを意味する）、第２指定期間が経過する前（評価指数ｅ₃が基準値ｅ₀（＝０．４）以上）であるため、候補軌道Ｐ₃が「第２候補軌道」に指定された。

時刻ｔ₂において候補位置ｘ₅（トレイ３４の右奥区画の位置）に対応する候補軌道Ｐ₅の評価指数ｅ₅が「１」になり、当該候補軌道Ｐ₅が第１候補軌道として指定された。時刻ｔ₃において候補軌道Ｐ₃の評価指数ｅ₃が基準値ｅ₀（＝０．４）未満になり、その第２候補軌道の指定が解除された。その後、時刻ｔ₄において候補軌道Ｐ₃の評価指数ｅ₃が再び「１」になり、候補軌道Ｐ₃が第１候補軌道として指定された。

期間ｔ₂〜ｔ₄では候補軌道Ｐ₅の評価指数ｅ₃が「１」であり、当該候補軌道Ｐ₅が「第１候補軌道」として指定された。期間ｔ₄以降、候補軌道Ｐ₅の評価指数ｅ₅が「１」から徐々に減少し、第２指定期間が経過する前であるため、候補軌道Ｐ₅が第２候補軌道に指定された。期間ｔ₅以降、候補軌道Ｐ₃の評価指数ｅ₃が「１」から徐々に減少し、第２指定期間が経過する前であるため、候補軌道Ｐ₃も候補軌道Ｐ₅に加えて第２候補軌道に指定された。

上記の予測結果の遷移態様を表１にまとめて示す。

ロボット２１に搭載されている制御装置（図示略）は、状態予測システム１から出力された人間２２の手（オブジェクト）の位置の予測結果（候補軌道の指定結果（表１参照））を逐次受け取り、これに基づいてロボット２１の動作を制御する。

たとえば、第１候補軌道および第２候補軌道のいずれにも干渉しないようなハンドの位置姿勢軌道が生成され、これにしたがってロボット２１のアーム等の動作が制御される。第１候補軌道または第２候補軌道に対応する候補位置のそばにハンドを近づけるようにロボットのアーム等の動作が制御される。トレイ３４における候補位置ｘ₃〜ｘ₈のいずれかに向かう候補軌道Ｐ₃〜Ｐ₈のいずれかが第１候補軌道として指定されたことに応じて、ロボット２１がコップ３１または３２を把持するようにその動作が制御されてもよい。人間２２が手でコップ３１、３２を持った場合、当該コップ３１、３２を新たなオブジェクトとして未来における位置が予測されてもよい。

（効果）
前記状態予測システム１によれば、オブジェクトの現在状態に基づいて生成される基準軌道Ｐ₀との近似度Ａ_i（＝１／Ｄ_i）が第１指定期間にわたり指定順位以内であった少なくとも１つの候補軌道Ｐ_iが「第１候補軌道」として指定される（図２／ＳＴＥＰ１０‥ＹＥＳ→ＳＴＥＰ１２および図７参照）。このため、オブジェクトの状態がそれにしたがって遷移する蓋然性が一時的にのみ高かった候補軌道が第１候補軌道として指定されることが回避される。これにより、オブジェクトの状態予測結果としての当該指定結果の変化頻度が低減されうる。第１指定要件に近似度Ａ_iが第１指定期間にわたり閾値以上であることが含まれているため、オブジェクトの状態予測精度の向上も図られている。

第１候補軌道であった最後の時点から第２指定期間が経過前である少なくとも１つの候補軌道が「第２候補軌道」として指定される（図２／ＳＴＥＰ１４‥ＹＥＳ→ＳＴＥＰ１６および図７参照）。オブジェクトの状態予測結果としての当該指定結果により、オブジェクトの状態が第２候補軌道にしたがって遷移する可能性が残されていることが顕在化される（図７／ｔ₁〜ｔ₃およびｔ₅以降における候補軌道Ｐ₃ならびにｔ₄以降における候補軌道Ｐ₅参照）。これにより、第２候補軌道に指定されていた候補軌道が再び第１候補軌道に指定された場合でも、オブジェクトの状態の予測結果が、それまでの予測結果から著しく乖離する事態が回避されうる（図７／時刻ｔ₄における候補軌道Ｐ₃参照）。第１候補軌道が指定されない状態では、オブジェクトの状態予測結果としての当該指定結果により、オブジェクトの状態が第２候補軌道にしたがって遷移する可能性が最も高いものの、その可能性は第１候補軌道よりも低いことが顕在化される（図７／ｔ₁〜ｔ₃およびｔ₅以降における候補軌道Ｐ₃ならびにｔ₄以降における候補軌道Ｐ₅参照）。これにより、その後、異なる候補軌道が第１候補軌道として指定された場合でも、オブジェクトの状態の予測結果が、それまでの予測結果から著しく乖離する事態が回避されうる。

（本発明の他の実施形態）
状態予測システム１が車両に搭載され、当該車両の周辺に存在する他の車両または通行人などの周囲に存在するオブジェクトの状態または挙動が予測され、当該予測結果に基づいてオブジェクトとの接触が回避されるように車両の挙動が制御されてもよい。この場合、オブジェクトの候補状態として、たとえば道路における空きスペースに当該オブジェクトとしての他車両が存在する状態、または、通行人を対象とした催事場にオブジェクトが存在する状態が採用される。前記のように、状態予測システム１による予測対象となる状態には、オブジェクトとしての動物の心理状態および生理状態なども含まれていてもよい。

軌道指定要素１３により、オブジェクトの状態および複数の候補状態のそれぞれのうち少なくとも１つに応じて、第１指定期間、第２指定期間および閾値のうち少なくとも１つが変更されてもよい。たとえば、複数の候補状態のうち少なくとも１つの候補状態がオブジェクトと他の物体との相互作用を伴う状態である場合、オブジェクトが少なくとも１つの候補状態に遷移するまでの時間が短くなるほど、第１指定期間として長い期間が採用されてもよくまたは閾値として大きい値が採用されてもよい。「オブジェクトと他の物体との相互作用を伴う状態」とは、たとえば人間２２の手（オブジェクト）がカップ３１、３２またはトレイ３４（他の物体）と接触する状態を意味する。

当該構成によれば、オブジェクトと他の物体とを相互作用させる観点から当該オブジェクトの状態予測が重要な段階（オブジェクトが他の物体と相互作用する直前の段階）において、当該予測結果のその他の段階に対する相対的な安定性向上が図られる。

オブジェクトの状態および複数の候補状態のそれぞれのうち少なくとも１つの安定度が評価され、当該安定度が低いほど、第１指定期間として長い期間が採用されてもよくまたは閾値として大きい値が採用されてもよい。たとえば、人間２２の手または、コップ３１、３２もしくはトレイ３４の位置の単位時間当たりの変化量が大きいほど安定度が低く評価される。

当該構成によれば、オブジェクトの状態および候補状態の相対的な関係が大きく変化するため、前回候補軌道および今回候補軌道の形態が著しく異なるなど、オブジェクトの状態予測結果の精度が損なわれかねない状況において、当該オブジェクトの状態予測結果の安定性の向上が図られる。

１‥状態予測システム、１０‥状態推定要素、１１‥第１軌道生成要素、１２‥第２軌道生成要素、１３‥軌道指定要素。

Claims

オブジェクトの現在状態に基づき、第１モデルにしたがって、未来における前記オブジェクトの状態の確率密度分布の遷移態様を表わす基準軌道を生成する第１軌道生成要素と、
前記オブジェクトの現在状態および複数の候補状態に基づき、第２モデルにしたがって、前記オブジェクトの現在状態から前記複数の候補状態のそれぞれまでの状態の確率密度分布の遷移態様を表わす複数の候補軌道を生成する第２軌道生成要素と、
前記第２軌道生成要素により生成された前記複数の候補軌道のそれぞれについて前記第１軌道生成要素により生成された前記基準軌道との近似度を評価し、第１指定期間にわたり前記近似度が指定順位以内である候補軌道である第１候補軌道、および、前記第１候補軌道として指定されていた最後の時点から第２指定期間経過前であるその他の候補軌道である第２候補軌道のうち少なくとも１つを前記オブジェクトの状態予測結果として指定する軌道指定要素と、を備えていることを特徴とする状態予測システム。
請求項１記載の状態予測システムにおいて、
前記軌道指定要素が、前記第１指定期間にわたり前記近似度が指定順位以内であり、かつ、前記近似度が閾値以上である候補軌道を前記第１候補軌道として指定することを特徴とする状態予測システム。
請求項１または２記載の状態予測システムにおいて、
前記軌道指定要素が、前記複数の候補状態のうち少なくとも１つの候補状態が前記オブジェクトと他の物体との相互作用を伴う状態である場合、前記オブジェクトが前記少なくとも１つの候補状態に遷移するまでの時間が短くなるほど、前記第１指定期間として長い期間を採用するまたは前記閾値として大きい値を採用することを特徴とする状態予測システム。
請求項２または３記載の状態予測システムにおいて、
前記軌道指定要素が、前記オブジェクトの状態および前記複数の候補状態のそれぞれのうち少なくとも１つの変化量が大きいほど安定度を低く評価し、前記安定度が低いほど、前記第１指定期間として長い期間を採用するまたは前記閾値として大きい値を採用することを特徴とする状態予測システム。
請求項１〜４のうちいずれか１つに記載の状態予測システムにおいて、
前記軌道指定要素が、今回の前記第１候補軌道に対して評価指数の今回値として指定の正値を付与し、今回の前記第１候補軌道を除く候補軌道に対して、前記評価指数の前回値に１未満の正値である減衰係数を乗じた結果を前記評価指数の今回値として付与し、前記評価指数の今回値が基準値以上である少なくとも１つの候補軌道を今回の前記第２候補軌道として指定することを特徴とする状態予測システム。