JP2017170553A

JP2017170553A - 制御方法

Info

Publication number: JP2017170553A
Application number: JP2016058370A
Authority: JP
Inventors: 中村　仁彦; Yoshihiko Nakamura; 仁彦中村; 渉高野; Wataru Takano; 高橋　太郎; Taro Takahashi; 太郎高橋
Original assignee: University of Tokyo NUC; Toyota Motor Corp
Current assignee: University of Tokyo NUC; Toyota Motor Corp
Priority date: 2016-03-23
Filing date: 2016-03-23
Publication date: 2017-09-28
Anticipated expiration: 2036-03-23
Also published as: JP6567998B2

Abstract

【課題】統計モデル構築時と可動部の制御時で、可動部と操作対象物の位置関係が異なる場合でも、可動部を適正に制御すること。【解決手段】動作する可動部の位置又は速度情報と該可動部に対する力情報とに基づき構築した統計モデルと、時刻ｔの可動部の位置又は速度情報と力情報と、に基づいて、時刻ｔ＋１の目標位置情報又は目標速度情報をサンプリングし、時刻ｔ＋１の目標位置情報、目標速度情報及び目標加速度情報に基づいて、逆動力学演算を行って、時刻ｔ＋１の可動部の目標力情報を算出し、時刻ｔの位置情報及び力情報と統計モデルとに基づいて時刻ｔ＋１の目標位置情報及び目標力情報が統計モデルから生成される確率を算出し、時刻ｔ＋１の目標位置情報及び目標力情報と確率とに基づいて時刻ｔ＋１における目標位置情報及び目標力情報の期待値を算出し、該期待値に基づいて可動部を制御する。【選択図】図５

Description

本発明は、統計モデルを用いて可動部を制御する制御方法に関する。

動作する可動部の位置情報（ロボットの関節角度など）に基づき構築した統計モデル（隠れマルコフモデルなど）を用いて、可動部を制御する制御方法が知られている（例えば、特許文献１参照）。

特開２００４−３３０３６１号公報

上記制御方法においては、位置情報に基づいて統計モデルを構築している。このため、統計モデル構築時と、実際の可動部の制御時とで、例えば、その可動部と操作対象物との位置関係が変化した場合に、可動部と操作対象物との間に過大な力が発生する、あるいは可動部が操作対象物に接触できない虞がある。
本発明は、かかる課題を解決するためになされたものであり、統計モデル構築時と可動部の制御時で、可動部と操作対象物の位置関係が異なる場合でも、可動部と操作対象物との間に過大な力が発生する、あるいは可動部が操作対象物に接触できない状況を抑制できる制御方法を提供することを主たる目的とする。

上記目的を達成するための本発明の一態様は、
動作する可動部の位置又は速度情報と該可動部に対する力情報とに基づき構築したモデルであって、各ノード間の遷移確率が設定され該各ノードは前記位置又は速度情報と力情報の分布を有する統計モデルと、時刻ｔ（ｔは自然数）における前記可動部の位置又は速度情報と力情報と、に基づいて、時刻ｔ＋１における前記可動部の目標位置情報又は目標速度情報をサンプリングするステップと、
前記サンプリングした目標位置情報又は目標速度情報に基づいて、時刻ｔ＋１における前記可動部の目標位置情報又は目標速度情報と、目標加速度情報と、を算出するステップと、
前記算出した時刻ｔ＋１における目標位置情報、目標速度情報及び目標加速度情報に基づいて、逆動力学演算を行って、時刻ｔ＋１における前記可動部の目標力情報を算出するステップと、
前記時刻ｔにおける位置情報及び力情報と、前記統計モデルと、に基づいて、前記算出した時刻ｔ＋１における目標位置情報及び目標力情報が前記統計モデルから生成される確率を算出するステップと、
前記算出した時刻ｔ＋１における目標位置情報及び目標力情報と、前記算出した確率と、に基づいて、前記時刻ｔ＋１における目標位置情報及び目標力情報の期待値を算出するステップと、
前記算出した目標位置情報及び目標力情報の期待値に基づいて、前記可動部を制御するステップと、
を含む、ことを特徴とする制御方法
である。
この一態様によれば、可動部の位置指令値と力指令値の整合性を取ることができるため、統計モデル構築時と可動部制御時で、可動部と操作対象物の位置関係が異なる場合などでも、可動部と操作対象物との間で、過大な力が発生する、あるいは可動部が操作対象物に接触できない状況を抑制できる。

本発明によれば、統計モデル構築時と可動部の制御時で、可動部と操作対象物の位置関係が異なる場合でも、可動部と操作対象物との間に過大な力が発生する、あるいは可動部が操作対象物に接触できない状況を抑制できる制御方法を提供できる。

本発明の実施形態１に係るロボットアームの概略的な構成を示す図である。本発明の実施形態１に係るロボットアーム及び制御装置の概略的なシステム構成を示すブロック図である。隠れマルコフモデルの一例を示す図である。各関節部の目標軌道生成およびロボットアーム制御を示す図である。本発明の実施形態１に係る制御方法のフローを示すフローチャートである。

以下、図面を参照して本発明の実施の形態について説明する。
実施形態１
図１は、本発明の実施形態１に係るロボットアームの概略的な構成を示す図である。本実施形態１に係る制御装置は、例えば、多関節型のロボットアーム（可動部の一具体例）２を制御する。

ロボットアーム２は、複数のリンク２１と、各リンク２１を回動可能に連結する関節部（手首関節、肘関節、肩関節など）２２と、その先端に設けられ操作対象を操作するエンドエフェクタ２３と、を有する。

図２は、本発明の実施形態１に係るロボットアーム及び制御装置の概略的なシステム構成を示すブロック図である。各関節部２２には、各関節部２２の関節角度（位置情報の一具体例）を検出するエンコーダなどの角度センサ２４と、各関節部２２を駆動するサーボモータなどのアクチュエータ２５と、各関節部２２の操作力を検出する力センサ２６と、が設けられている。

力センサ２６は、例えば、各関節部２２の関節トルク（力情報の一具体例）を検出するトルクセンサなどである。各関節部２２には、減速機構が設けられている。エンドエフェクタ２３は、例えば、物体を把持、接触などして物体に操作力を加える。エンドエフェクタ２３には、エンドエフェクタ２３を駆動するアクチュエータ２５と、エンドエフェクタ２３の操作力を検出する力センサ２６と、が設けられている。

制御装置１は、例えば、各関節部２２の角度センサ２４からの角度情報（関節角度など）と、力センサ２６からの操作力と、に基づいて、各関節部２２及びエンドエフェクタ２３のアクチュエータ２５を制御することで、ロボットアーム２をフィードバック制御する。

なお、制御装置１は、例えば、演算処理等と行うＣＰＵ（Central Processing Unit）１ａ、ＣＰＵ１ａによって実行される演算プログラム、制御プログラム等が記憶されたＲＯＭ（Read Only Memory）やＲＡＭ（Random Access Memory）からなるメモリ１ｂ、外部と信号の入出力を行うインターフェイス部（Ｉ／Ｆ）１ｃ、などからなるマイクロコンピュータを中心にして、それぞれ、ハードウェア構成されている。ＣＰＵ１ａ、メモリ１ｂ、及びインターフェイス部１ｃは、データバス１ｄなどを介して相互に接続されている。

本実施形態１に係る制御装置１は、例えば、隠れマルコフモデル（Hidden Markov Model：ＨＭＭ）などの統計モデルを用いて、人の動作を模倣学習することで、ロボットアーム２によるテーブル拭きのような力を加えて行う物理タスクを実行する。

ところで、ロボットアームが上記のような物理タスクを実行するために、制御装置は、ロボットアームの位置情報（関節角度や手先位置）に加えて、ロボットアームの力情報（関節トルクや手先反力）についても学習する必要がある。この場合、制御装置が単にロボットアームの位置情報と同様に、力情報を学習するだけでは次のような問題が生じる。例えば、その学習時と制御時とでそのロボットアームと操作対象物との位置関係が変化した場合（テーブルの高さが変化した場合など）に、ロボットアームと操作対象物との間に過大な力が発生する、あるいはロボットアームが操作対象物に接触できない虞がある。

これに対し、本実施形態１に係る制御装置１は、隠れマルコフモデルによるロボットアーム２の目標軌道生成時に、後述の動力学演算式を用いて力学的整合性を確保する。

制御装置１は、予め用意されたデータに基づいて学習を行い隠れマルコフモデルを構築する学習処理と、学習処理で構築した隠れマルコフモデルを用いてロボットアーム２を制御する実行処理と、を実行する。

（学習処理）
制御装置１は、学習処理において、例えば、予め用意されたロボットアーム２の各関節部２２の関節角度および関節トルクの時系列データ（シミュレーションデータなど）を用いて学習を行い、Baum-Welch法によりleft-right隠れマルコフモデルを構築する（図３）。

なお、制御装置１は、実際にロボットアーム２を遠隔的に動作させ、その際に角度センサ２４及び力センサ２６により検出されたロボットアーム２の各関節部２２の関節角度および関節トルクの時系列データを用いて学習を行い、隠れマルコフモデルを構築してもよい。

図３に示す如く、隠れマルコフモデルにおいて、各ノードｑと、ノードｑ間を遷移する遷移確率ａが設定される。各ノードｑは、各関節部２２の関節角度θおよび関節トルクτの分布を有している。制御装置１は、上記各関節部２２の関節角度および関節トルクの時系列データを用いてノード間の遷移確率ａと、各関節部２２の関節角度θおよび関節トルクτの分布を表すパラメータと、を学習した隠れマルコフモデルを構築する。

このように、学習処理において、ロボットアーム２の関節角度と関節トルクを使用して隠れマルコフモデルを構築する。これにより、ロボットアーム２の関節角度と関節トルクの相関関係も含めて時系列データを保存でき、ロボットアーム２が操作対象物の接触動作を行うための位置指令と力指令の両者を同時に生成できる。また、そのデータの情報量を圧縮しつつ、ロボットアーム２の動作認識等を行うこともできる。

（実行処理）
制御装置１は、本実行処理において、図４に示す如く、各関節部２２の目標軌道生成と、ロボットアーム制御と、を同時に実行する。さらに、制御装置１は、このロボットアーム制御時において、力センサ２６及び角度センサ２４により検出された現在時刻ｔのセンサ値に基づいて、次瞬間である時刻ｔ＋１のロボットアーム２の目標位置情報θ_ｐｌａｎ及び目標力情報τ_ｐｌａｎを決定し、これら目標位置情報θ_ｐｌａｎ及び目標力情報τ_ｐｌａｎに基づいてロボットアーム２のコンプライアンス制御を実行する。これにより、ロボットアーム２の現在の状態から大きく離れた指令値が生成され、ロボットアーム２が急激に動作するような状況を回避できる。さらに、ロボットアーム２の位置指令及び力指令を同時に厳密に満たせない場合でも、上記目標位置情報及び目標力情報を用いて、ロボットアーム２の位置及び力を予め設定したパラメータに従ったバランス状態で適正に制御できる。

制御装置１は、所定時間毎に、以下の[prediction]（１）〜（８）および[resampling]を繰り返して関節部２２の目標軌道を生成する。

[prediction]
（１）制御装置１は、まず、現在時刻ｔにおける隠れマルコフモデルのノードｑ_ｔと、角度センサ２４及び力センサ２６により検出された現在時刻ｔにおける関節角度θ_ｔ及び関節トルクτ_ｔと、（パーティクル）を保持する。

（２）制御装置１は、隠れマルコフモデルのノード間の遷移確率ａを用いて、次瞬間である時刻ｔ＋１におけるノードｑ_ｔ＋１をサンプリングする。

（３）制御装置１は、サンプリングしたノードｑ_ｔ＋１が有する関節角度の分布（ノードｑ_ｔ＋１における出力確率）に基づいて、時刻ｔ＋１における関節部２２の目標関節角度（目標位置情報の一具体例）θ_ｔ＋１ ^ｒｅｆをサンプリングする。
このように、関節部２２の目標軌道生成時に関節部２２の関節角度び関節トルクを含む隠れマルコフモデルと、角度センサ２４及び力センサ２６により検出された現在時刻ｔにおける関節角度及び関節トルクと、に基づいて、目標関節角度をサンプリングしている。

（４）制御装置１は、サンプリングした目標関節角度θ_ｔ＋１ ^ｒｅｆを１階微分して関節部２２の目標関節角速度（目標速度情報の一具体例）θ（ドット）_ｔ＋１ ^ｒｅｆを算出し、目標関節角度θ_ｔ＋１ ^ｒｅｆを２階微分して関節部２２の目標関節角加速度（目標加速度情報の一具体例）θ（２ドット）_ｔ＋１ ^ｒｅｆを算出する。なお、上記のようにθの上部に１つのドット記号を付したもの（１階微分値）をθ（ドット）と表記し、θの上部に２つのドット記号を付したもの（２階微分値）をθ（２ドット）と表記し、以降、他のパラメータについても同様に表記する。

（５）制御装置１は、算出した目標関節角加速度θ（２ドット）_ｔ＋１ ^ｒｅｆを下記（式１）を用いて修正する。下記式１において、Ｋ_ｐ及びＫ_ｄは、予め設定される係数行列である。
この修正により、制御安定性をより高めることができる。

なお、本実施形態１において、制御装置１は、処理の高速化を図るために、上記(５)による目標関節角加速度θ（２ドット）_ｔ＋１ ^ｒｅｆの修正を行わなくても良い。

（６）制御装置１は、算出した目標関節角度θ_ｔ＋１ ^ｒｅｆ、目標関節角速度θ（ドット）_ｔ＋１ ^ｒｅｆ、および修正した目標関節角加速度θ（２ドット）_ｔ＋１ ^ｒｅｆと、目標反力Ｆ_ｔ＋１ ^ｒｅｆとを実現する、目標関節トルクτ_ｔ＋１を算出する。
なお、上記目標反力Ｆ_ｔ＋１ ^ｒｅｆは、ロボットアーム２の操作内容に基づいてユーザにより設定される。例えば、ロボットアーム２を操作対象物に強く接触させる場合は、目標反力１０[Ｎ]に設定し、軽く接触させる場合は、１[Ｎ]に設定する。
制御装置１は、算出した目標関節角度θ_ｔ＋１ ^ｒｅｆ、目標関節角速度θ（ドット）_ｔ＋１ ^ｒｅｆ、および修正した目標関節角加速度θ（２ドット）_ｔ＋１ ^ｒｅｆと、目標反力Ｆ_ｔ＋１ ^ｒｅｆと、に基づいて、下記逆動力学演算式（運動方程式）（式２）を用いて関節部２２の目標関節トルク（目標力情報の一具体例）τ_ｔ＋１を算出する。なお、下記（式２）において、Ｍは慣性行列であり、Ｃは遠心及びコリオリ項であり、Ｇは重力項である。Jは、関節角速度をロボットアーム２（可動部）の速度に関係付けるヤコビ行列である。
このように、下記逆動力学演算式を用いて目標関節トルクτ_ｔ＋１を算出することで、隠れマルコフモデルによる関節部２２の目標軌道生成時において、ロボットアーム２の力学的な整合性を確保することができる。

（７）制御装置１は、角度センサ２４及び力センサ２６により検出された現在時刻ｔでの関節角度θ_ｔ及び関節トルクτ_ｔと、隠れマルコフモデルと、に基づいて、目標関節角度θ_ｔ＋１ ^ｒｅｆ、および目標関節トルクτ_ｔ＋１が隠れマルコフモデルから生成される（ｑ_ｔ＋１、θ_ｔ＋１ ^ｒｅｆ、τ_ｔ＋１に到達する）確率Ｐ（θ_ｔ＋１ ^ｒｅｆ、τ_ｔ＋１｜θ_ｔ、τ_ｔ、λ）を算出する。このように、角度センサ２４及び力センサ２６により検出された現在時刻ｔの関節角度及び関節トルク（センサ値）と、隠れマルコフモデルと、から、その目標関節角度θ_ｔ＋１ ^ｒｅｆおよび目標関節トルクτ_ｔ＋１になる確率（尤度）を算出する。
制御装置１は、パーティクル毎に上記確率Ｐを算出する。ここで、制御装置１は、各パーティクルの確率Ｐの総和が１にするため、次の修正処理を行う。例えば、制御装置１は、各パーティクルの確率の総和を算出し、各パーティクルの確率を算出した総和で除算する。

（８）制御装置１は、目標関節角度θ_ｔ＋１ ^ｒｅｆ、目標関節トルクτ_ｔ＋１および修正した確率に基づいて下記（式３）を用いて、次瞬間の指令値である時刻ｔ＋１における目標関節角度の推定値θ（ハット）_ｔ＋１ ^ｒｅｆ及び目標関節トルクの推定値τ（ハット）_ｔ＋１を、パーティクル毎の関節角度及び関節トルクの期待値として算出する。なお、θ及びτの上部にハット記号を付したものを、夫々、θ（ハット）及びτ（ハット）と表記し、以降、他のパラメータについても同様に表記する。
このように、上記（７）で算出した確率による重みづけ平均により次瞬間の指令値を算出する。

制御装置１は、上記（８）において算出した目標関節角度の推定値θ（ハット）_ｔ＋１ ^ｒｅｆ及び目標関節トルクの推定値τ（ハット）_ｔ＋１と、角度センサ２４により検出された各関節部２２の関節角度θと、に基づいて、各関節部２２のコンプライアンス制御を行う。制御装置１は、上記（８）において算出した目標関節角度の推定値θ（ハット）_ｔ＋１ ^ｒｅｆ及び目標関節トルクの推定値τ（ハット）_ｔ＋１を、夫々、角度指令θ_ｐｌａｎ、及びトルク指令τ_ｐｌａｎとして、下記（式４）を用いて、トルク指令τ_ｒｅｆを算出する。なお、下記（式４）において、Ｋ及びＤは予め設定される係数行列である。

制御装置１は、算出したトルク指令τ_ｒｅｆを各関節部２２のアクチュエータ２５に送信することで、各アクチュエータ２５を制御する。
さらに、制御装置１は、力センサ２６により検出された関節トルクτが、算出したトルク指令τ_ｒｅｆに追従するように、関節部２２のアクチュエータ２５に対して、フィードバック制御を行う。

以上のように、目標軌道生成時において、関節部２２の関節角度及び関節トルクを含む隠れマルコフモデルと、角度センサ２４及び力センサ２６により検出された関節角度及び関節トルクに基づいて、目標関節角度をサンプリングする。サンプリングした目標関節角度に基づいて目標関節角速度及び目標関節角加速度を算出し、目標関節角度と、算出した目標関節角速度および目標関節角加速度と、目標反力と、に基づいて、逆動力学演算を行い目標関節トルクを算出して、ロボットアーム２の力学的な整合性を確保する。現在時刻ｔの角度センサ値及びトルクセンサ値と、隠れマルコフモデルと、から、その目標関節角度および目標関節トルクになる確率を算出する。算出した確率による重みづけ平均により時刻ｔ＋１の目標関節角度及び目標関節トルクの期待値を算出し、この目標関節角度及び目標関節トルクの期待値を用いてロボットアーム２の関節部２２を制御する。

これにより、ロボットアーム２の位置指令値と力指令値の整合性を取ることができる。したがって、隠れマルコフモデル構築時とロボットアーム制御時で、例えば、操作対象物の大きさ、形状が異なる場合や、ロボットアーム２と操作対象物の位置関係（テーブルの高さが異なるなど）が異なる場合などでも、ロボットアーム２と操作対象物との間で、過大な力が発生する、あるいはロボットアームが操作対象物に接触できない状況を抑制できる。

[resampling]
制御装置１は、上記算出した確率Ｐを正規化して、確率密度分布ｐ_ｋを算出する。
制御装置１は、各パーティクルの確率Ｐとなるように、確率密度分布ｐ_ｋに従ってパーティクルをばらまき、時刻ｔ＋１でのパーティクルを更新する。これにより、確率の低いパーティクルを除去し、確率の高いパーティクルを残すことができる。
制御装置１は、選択したパーティクルｓのノードｑ_ｔ＋１を次のフレームのためにｑ_ｔにセットし、上記（１）に戻り処理を繰り返す。

図５は、本実施形態１に係る制御方法のフローを示すフローチャートである。なお、図５に示す制御処理は、例えば、所定時間毎に繰返し実行される。
制御装置１は、隠れマルコフモデルのノード間の遷移確率ａを用いて時刻ｔ＋１（次瞬間）におけるノードｑ_ｔ＋１をサンプリングする（ステップＳ１０１）。

制御装置１は、サンプリングしたノードｑ_ｔ＋１が有する関節角度の分布に基づいて、時刻ｔ＋１における目標関節角度θ_ｔ＋１ ^ｒｅｆをサンプリングする（ステップＳ１０２）。
制御装置１は、サンプリングした目標関節角度θ_ｔ＋１ ^ｒｅｆを１階微分して目標関節角速度θ（ドット）_ｔ＋１ ^ｒｅｆを算出し、目標関節角度θ_ｔ＋１ ^ｒｅｆを２階微分して目標関節角加速度θ（２ドット）_ｔ＋１ ^ｒｅｆを算出する（ステップＳ１０３）。

制御装置１は、算出した目標関節角加速度θ（２ドット）_ｔ＋１ ^ｒｅｆを上記（式１）を用いて修正する（ステップＳ１０４）。
制御装置１は、算出した目標関節角度θ_ｔ＋１ ^ｒｅｆ、目標関節角速度θ（ドット）_ｔ＋１ ^ｒｅｆ、および修正した目標関節角加速度θ（２ドット）_ｔ＋１ ^ｒｅｆと、目標反力Ｆ_ｔ＋１ ^ｒｅｆと、に基づいて、上記逆動力学演算式（式２）を用いて目標関節トルクτ_ｔ＋１を算出する（ステップＳ１０５）。

制御装置１は、角度センサ２４及び力センサ２６により検出された現在時刻ｔでの関節角度θ_ｔ及び関節トルクτ_ｔと、隠れマルコフモデルと、に基づいて、目標関節角度θ_ｔ＋１ ^ｒｅｆ、および目標関節トルクτ_ｔ＋１が隠れマルコフモデルから生成される確率Ｐ（θ_ｔ＋１ ^ｒｅｆ、τ_ｔ＋１｜θ_ｔ、τ_ｔ、λ）を算出する（ステップＳ１０６）。

制御装置１は、目標関節角度θ_ｔ＋１ ^ｒｅｆ、目標関節トルクτ_ｔ＋１および修正した確率に基づいて上記（式３）を用いて、時刻ｔ＋１における目標関節角度の推定値θ（ハット）_ｔ＋１ ^ｒｅｆ及び目標関節トルクの推定値τ（ハット）_ｔ＋１（次瞬間の指令値）を、パーティクル毎の関節角度及び関節トルクの期待値として算出する（ステップＳ１０７）。

制御装置１は、上記（８）において算出した目標関節角度の推定値θ（ハット）_ｔ＋１ ^ｒｅｆ及び目標関節トルクの推定値τ（ハット）_ｔ＋１と、角度センサ２４により検出された各関節部の関節角度θと、に基づいて、各関節部２２のコンプライアンス制御を行う（ステップＳ１０８）。
制御装置１は、各パーティクルの確率Ｐとなるように、確率密度分布ｐ_ｋに従ってパーティクルをばらまき、時刻ｔ＋１でのパーティクルを更新する（ステップＳ１０９）。

以上、本実施形態１において、関節部２２の関節角度及び関節トルクを含む隠れマルコフモデルと、角度センサ２４及び力センサ２６により検出された関節角度及び関節トルクに基づいて、目標関節角度をサンプリングする。サンプリングした目標関節角度に基づいて目標関節角速度及び目標関節角加速度を算出し、目標関節角度と、算出した目標関節角速度および目標関節角加速度と、目標反力と、に基づいて、逆動力学演算を行い目標関節トルクを算出する。現在時刻ｔの角度センサ値及びトルクセンサ値と、隠れマルコフモデルと、から、その目標関節角度および目標関節トルクになる確率を算出する。算出した確率による重みづけ平均により時刻ｔ＋１の目標関節角度及び目標関節トルクの期待値を算出し、この目標関節角度及び目標関節トルクの期待値を用いてロボットアーム２の関節部２２を制御する。

これにより、ロボットアーム２の位置指令値と力指令値の整合性を取ることができる。したがって、隠れマルコフモデル構築時とロボットアーム制御時で、例えば、操作対象物の大きさ、形状が異なる場合や、ロボットアーム２と操作対象物の位置関係が異なる場合などでも、ロボットアーム２と操作対象物との間で、過大な力が発生する、あるいはロボットアームが操作対象物に接触できない状況を抑制できる。

実施形態２
本発明の実施形態２において、制御装置１は、エンドエフェクタ２３の手先位置（位置情報の一具体例）および、エンドエフェクタ２３の手先反力（力情報の一具体例）の時系列データに基づいて、隠れマルコフモデルを構築してもよい。この場合、制御装置１は、上記実施形態１における[prediction]（１）〜（８）および[resampling]の関節角度および関節トルクを、夫々、手先位置および手先反力に置き換えて同様の処理を行う。

（学習処理）
制御装置１は、学習処理において、例えば、予め用意されたエンドエフェクタ２３の手先位置および手先反力の時系列データを用いて学習を行い、Baum-Welch法によりleft-right隠れマルコフモデルを構築する。

（実行処理）
制御装置１は、所定時間毎に、以下の[prediction]（１）〜（８）および[resampling]を繰り返す。

[prediction]
（１）制御装置１は、まず、現在時刻ｔにおける隠れマルコフモデルのノードｑ_ｔと、センサにより検出された現在時刻ｔにおける手先位置及び手先反力と、（パーティクル）を保持する。

（２）制御装置１は、隠れマルコフモデルのノード間の遷移確率ａを用いて時刻ｔ＋１（次瞬間）におけるノードｑ_ｔ＋１をサンプリングする。

（３）制御装置１は、サンプリングしたノードｑ_ｔ＋１が有する手先位置の分布に基づいて、時刻ｔ＋１における目標手先位置（目標位置情報の一具体例）をサンプリングする。

（４）制御装置１は、サンプリングした目標手先位置にを１階微分して目標手先速度（目標速度情報の一具体例）を算出し、目標手先位置にを２階微分して目標手先加速度（目標加速度情報の一具体例）を算出する。

（５）制御装置１は、制御安定性を向上させるために、算出した目標手先加速度を修正する。本実施形態２において、制御装置１は、上記(５)による目標手先加速度の修正を行わなくても良い。

（６）制御装置１は、算出した目標手先位置、目標手先速度、および修正した目標手先加速度と、目標反力とを実現する、目標手先反力（目標力情報の一具体例）を算出する。

（７）制御装置１は、センサにより検出された現在時刻ｔでの手先位置及び手先反力と、隠れマルコフモデルと、に基づいて、目標手先位置および目標手先反力が隠れマルコフモデルから生成される確率Ｐを算出する。
制御装置１は、パーティクル毎に上記確率Ｐを算出する。ここで、制御装置１は、各パーティクルの確率Ｐの総和が１にするため、次の修正処理を行う。例えば、制御装置１は、各パーティクルの確率の総和を算出し、各パーティクルの確率を算出した総和で除算する。
制御装置１は、算出した確率Ｐを正規化して、確率密度分布ｐ_ｋを算出する。

（８）制御装置１は、目標手先位置、目標手先反力および修正した確率に基づいて、時刻ｔ＋１における目標手先位置の推定値及び目標手先反力の推定値（次瞬間の指令値）を、パーティクル毎の手先位置及び手先反力の期待値として算出する。
制御装置１は、上記（８）において算出した目標手先位置の推定値及び目標手先反力の推定値と、センサにより検出された手先位置と、に基づいて、各関節部２２のコンプライアンス制御を行う。

[resampling]
制御装置１は、各パーティクルの確率Ｐとなるように、確率密度分布ｐ_ｋに従ってパーティクルをばらまき、時刻ｔ＋１でのパーティクルを更新する。

実施形態３
本発明の実施形態３において、制御装置１は、ロボットアーム２の各関節部２２の関節角度および関節トルクと、エンドエフェクタ２３の手先反力と、の時系列データに基づいて隠れマルコフモデルを構築する。

この場合、制御装置１は、上記実施形態１の[prediction]（３）において、サンプリングしたノードｑ_ｔ＋１が有する関節角度の分布に基づいて、時刻ｔ＋１における目標関節角度θ_ｔ＋１ ^ｒｅｆ及び目標反力をサンプリングする。

そして、制御装置１は、上記[prediction]（３）において、算出した目標関節角度θ_ｔ＋１ ^ｒｅｆ、目標関節角速度θ（ドット）_ｔ＋１ ^ｒｅｆ、および修正した目標関節角加速度θ（２ドット）_ｔ＋１ ^ｒｅｆと、サンプリングした目標反力Ｆ_ｔ＋１と、に基づいて、上記運動方程式（逆動力学演算式）（式３）を用いて目標関節トルクτ_ｔ＋１を算出する。
これにより、隠れマルコフモデルに手先反力を含めることで、[prediction]の過程で自動的に目標反力を設定することができる。

本実施形態３において、上記[prediction]（３）及び（６）以外の他の処理は、上記実施形態１と同一であるため、詳細な説明は省略する。

実施形態４
本発明の実施形態４において、制御装置１は、エンドエフェクタ２３の手先速度（速度情報の一具体例）および、エンドエフェクタ２３の手先反力、の時系列データに基づいて、隠れマルコフモデルを構築する。

（学習処理）
制御装置１は、学習処理において、例えば、予め用意されたエンドエフェクタ２３の手先速度および手先反力の時系列データを用いて学習を行い、Baum-Welch法によりleft-right隠れマルコフモデルを構築する。

（実行処理）
[prediction]
（１）制御装置１は、まず、現在時刻ｔにおける隠れマルコフモデルのノードｑ_ｔと、センサにより検出された現在時刻ｔにおける手先速度及び手先反力と、（パーティクル）を保持する。

（３）制御装置１は、サンプリングしたノードｑ_ｔ＋１が有する手先速度の分布に基づいて、時刻ｔ＋１における目標手先速度をサンプリングする。

（４）制御装置１は、サンプリングした目標手先速度を１階積分して目標手先位置を算出し、目標手先速度を１階微分して目標手先加速度を算出する。
上記（１）乃至（４）以外の処理は、上記実施形態２と同一であるため、詳細な説明は省略する。

実施形態５
本発明の実施形態５において、制御装置１は、ロボットアーム２の各関節部２２の関節角速度および関節トルクの時系列データに基づいて、隠れマルコフモデルを構築する。

（学習処理）
制御装置１は、学習処理において、例えば、予め用意されたロボットアーム２の各関節部２２の関節角速度および関節トルクの時系列データを用いて学習を行い、Baum-Welch法によりleft-right隠れマルコフモデルを構築する。

（実行処理）
[prediction]
（１）制御装置１は、まず、現在時刻ｔにおける隠れマルコフモデルのノードｑ_ｔと、センサにより検出された現在時刻ｔにおける関節角速度及び関節トルクと、（パーティクル）を保持する。

（３）制御装置１は、サンプリングしたノードｑ_ｔ＋１が有する関節角速度の分布に基づいて、時刻ｔ＋１における目標関節角速度をサンプリングする。

（４）制御装置１は、サンプリングした目標関節角速度にを１階積分して目標関節角度を算出し、目標関節角速度にを１階微分して目標関節角加速度を算出する。
上記（１）乃至（４）以外の処理は、上記実施形態１と同一であるため、詳細な説明は省略する。

なお、本発明は上記実施の形態に限られたものではなく、趣旨を逸脱しない範囲で適宜変更することが可能である。
上記実施形態において、制御装置１は、隠れマルコフモデルを構築し、構築した隠れマルコフモデルを用いて制御を行っているが、これに限定されない。制御装置１は、各ノード間に遷移確率が設定され各ノードは位置又は速度情報と力情報の分布を有する、グラフィカルな統計モデルを構築し、この統計モデル用いて制御を行っても良い。制御装置１は、例えば、マルコフモデル、やＣＲＦ（Conditional Random Field）などのグラフィカルな統計モデルを構築してもよい。

上記実施形態において、制御装置１は、ロボットアーム２を制御しているが、これに限定されない。制御装置１は、例えば、複数の関節部を有するロボット脚部や、人の脚部に装着されその歩行を補助する歩行支援ロボットを制御してよく、複数の関節部を有するロボットの各部を制御できる。さらに、制御装置１は、ロボットの制御だけでなく、工作機械（力制御を行う加工機械における人の加工方法の模倣等）、車両などの自動運転（人のハンドル操作の模倣等）、モータによる可動部分を含む装置、などを制御してもよい。

本発明は、例えば、図５に示す処理を、ＣＰＵ１ａにコンピュータプログラムを実行させることにより実現することも可能である。

プログラムは、様々なタイプの非一時的なコンピュータ可読媒体（non-transitory computer readable medium）を用いて格納され、コンピュータに供給することができる。非一時的なコンピュータ可読媒体は、様々なタイプの実体のある記録媒体（tangible storage medium）を含む。非一時的なコンピュータ可読媒体の例は、磁気記録媒体（例えばフレキシブルディスク、磁気テープ、ハードディスクドライブ）、光磁気記録媒体（例えば光磁気ディスク）、ＣＤ−ＲＯＭ（Read Only Memory）、ＣＤ−Ｒ、ＣＤ−Ｒ／Ｗ、半導体メモリ（例えば、マスクＲＯＭ、ＰＲＯＭ（Programmable ROM）、ＥＰＲＯＭ（Erasable PROM）、フラッシュＲＯＭ、ＲＡＭ（random access memory））を含む。

プログラムは、様々なタイプの一時的なコンピュータ可読媒体（transitory computer readable medium）によってコンピュータに供給されてもよい。一時的なコンピュータ可読媒体の例は、電気信号、光信号、及び電磁波を含む。一時的なコンピュータ可読媒体は、電線及び光ファイバ等の有線通信路、又は無線通信路を介して、プログラムをコンピュータに供給できる。

１制御装置、２ロボットアーム、２１リンク、２２関節部、２３エンドエフェクタ、２４角度センサ、２５アクチュエータ、２６力センサ

Claims

動作する可動部の位置又は速度情報と該可動部に対する力情報とに基づき構築したモデルであって、各ノード間の遷移確率が設定され、該各ノードは前記位置又は速度情報と力情報の分布を有する統計モデルと、時刻ｔ（ｔは自然数）における前記可動部の位置又は速度情報と力情報と、に基づいて、時刻ｔ＋１における前記可動部の目標位置情報又は目標速度情報をサンプリングするステップと、
前記サンプリングした目標位置情報又は目標速度情報に基づいて、時刻ｔ＋１における前記可動部の目標位置情報又は目標速度情報と、目標加速度情報と、を算出するステップと、
前記算出した時刻ｔ＋１における目標位置情報、目標速度情報及び目標加速度情報に基づいて、逆動力学演算を行って、時刻ｔ＋１における前記可動部の目標力情報を算出するステップと、
前記時刻ｔにおける位置情報及び力情報と、前記統計モデルと、に基づいて、前記算出した時刻ｔ＋１における目標位置情報及び目標力情報が前記統計モデルから生成される確率を算出するステップと、
前記算出した時刻ｔ＋１における目標位置情報及び目標力情報と、前記算出した確率と、に基づいて、前記時刻ｔ＋１における目標位置情報及び目標力情報の期待値を算出するステップと、
前記算出した目標位置情報及び目標力情報の期待値に基づいて、前記可動部を制御するステップと、
を含む、ことを特徴とする制御方法。