(57)【特許請求の範囲】
【請求項１】
動作する可動部の位置又は速度情報と該可動部に対する力情報とに基づき構築したモデルであって、各ノード間の遷移確率が設定され、該各ノードは前記位置又は速度情報と力情報の分布を有する統計モデルと、時刻ｔ（ｔは自然数）における前記可動部の位置又は速度情報と力情報と、に基づいて、時刻ｔ＋１における前記可動部の目標位置情報又は目標速度情報をサンプリングするステップと、
前記サンプリングした目標位置情報又は目標速度情報に基づいて、時刻ｔ＋１における前記可動部の目標位置情報又は目標速度情報と、目標加速度情報と、を算出するステップと、
前記算出した時刻ｔ＋１における目標位置情報、目標速度情報及び目標加速度情報に基づいて、逆動力学演算を行って、時刻ｔ＋１における前記可動部の目標力情報を算出するステップと、
前記時刻ｔにおける位置情報及び力情報と、前記統計モデルと、に基づいて、前記算出した時刻ｔ＋１における目標位置情報及び目標力情報が前記統計モデルから生成される確率を算出するステップと、
前記算出した時刻ｔ＋１における目標位置情報及び目標力情報と、前記算出した確率と、に基づいて、前記時刻ｔ＋１における目標位置情報及び目標力情報の期待値を算出するステップと、
前記算出した目標位置情報及び目標力情報の期待値に基づいて、前記可動部を制御するステップと、
を含む、ことを特徴とする制御方法。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、統計モデルを用いて可動部を制御する制御方法に関する。
【背景技術】
【０００２】
動作する可動部の位置情報（ロボットの関節角度など）に基づき構築した統計モデル（隠れマルコフモデルなど）を用いて、可動部を制御する制御方法が知られている（例えば、特許文献１参照）。
【先行技術文献】
【特許文献】
【０００３】
【特許文献１】特開２００４−３３０３６１号公報
【発明の概要】
【発明が解決しようとする課題】
【０００４】
上記制御方法においては、位置情報に基づいて統計モデルを構築している。このため、統計モデル構築時と、実際の可動部の制御時とで、例えば、その可動部と操作対象物との位置関係が変化した場合に、可動部と操作対象物との間に過大な力が発生する、あるいは可動部が操作対象物に接触できない虞がある。
本発明は、かかる課題を解決するためになされたものであり、統計モデル構築時と可動部の制御時で、可動部と操作対象物の位置関係が異なる場合でも、可動部と操作対象物との間に過大な力が発生する、あるいは可動部が操作対象物に接触できない状況を抑制できる制御方法を提供することを主たる目的とする。
【課題を解決するための手段】
【０００５】
上記目的を達成するための本発明の一態様は、
動作する可動部の位置又は速度情報と該可動部に対する力情報とに基づき構築したモデルであって、各ノード間の遷移確率が設定され該各ノードは前記位置又は速度情報と力情報の分布を有する統計モデルと、時刻ｔ（ｔは自然数）における前記可動部の位置又は速度情報と力情報と、に基づいて、時刻ｔ＋１における前記可動部の目標位置情報又は目標速度情報をサンプリングするステップと、
前記サンプリングした目標位置情報又は目標速度情報に基づいて、時刻ｔ＋１における前記可動部の目標位置情報又は目標速度情報と、目標加速度情報と、を算出するステップと、
前記算出した時刻ｔ＋１における目標位置情報、目標速度情報及び目標加速度情報に基づいて、逆動力学演算を行って、時刻ｔ＋１における前記可動部の目標力情報を算出するステップと、
前記時刻ｔにおける位置情報及び力情報と、前記統計モデルと、に基づいて、前記算出した時刻ｔ＋１における目標位置情報及び目標力情報が前記統計モデルから生成される確率を算出するステップと、
前記算出した時刻ｔ＋１における目標位置情報及び目標力情報と、前記算出した確率と、に基づいて、前記時刻ｔ＋１における目標位置情報及び目標力情報の期待値を算出するステップと、
前記算出した目標位置情報及び目標力情報の期待値に基づいて、前記可動部を制御するステップと、
を含む、ことを特徴とする制御方法
である。
この一態様によれば、可動部の位置指令値と力指令値の整合性を取ることができるため、統計モデル構築時と可動部制御時で、可動部と操作対象物の位置関係が異なる場合などでも、可動部と操作対象物との間で、過大な力が発生する、あるいは可動部が操作対象物に接触できない状況を抑制できる。
【発明の効果】
【０００６】
本発明によれば、統計モデル構築時と可動部の制御時で、可動部と操作対象物の位置関係が異なる場合でも、可動部と操作対象物との間に過大な力が発生する、あるいは可動部が操作対象物に接触できない状況を抑制できる制御方法を提供できる。
【図面の簡単な説明】
【０００７】
【図１】本発明の実施形態１に係るロボットアームの概略的な構成を示す図である。
【図２】本発明の実施形態１に係るロボットアーム及び制御装置の概略的なシステム構成を示すブロック図である。
【図３】隠れマルコフモデルの一例を示す図である。
【図４】各関節部の目標軌道生成およびロボットアーム制御を示す図である。
【図５】本発明の実施形態１に係る制御方法のフローを示すフローチャートである。
【発明を実施するための形態】
【０００８】
以下、図面を参照して本発明の実施の形態について説明する。
実施形態１
図１は、本発明の実施形態１に係るロボットアームの概略的な構成を示す図である。本実施形態１に係る制御装置は、例えば、多関節型のロボットアーム（可動部の一具体例）２を制御する。
【０００９】
ロボットアーム２は、複数のリンク２１と、各リンク２１を回動可能に連結する関節部（手首関節、肘関節、肩関節など）２２と、その先端に設けられ操作対象を操作するエンドエフェクタ２３と、を有する。
【００１０】
図２は、本発明の実施形態１に係るロボットアーム及び制御装置の概略的なシステム構成を示すブロック図である。各関節部２２には、各関節部２２の関節角度（位置情報の一具体例）を検出するエンコーダなどの角度センサ２４と、各関節部２２を駆動するサーボモータなどのアクチュエータ２５と、各関節部２２の操作力を検出する力センサ２６と、が設けられている。
【００１１】
力センサ２６は、例えば、各関節部２２の関節トルク（力情報の一具体例）を検出するトルクセンサなどである。各関節部２２には、減速機構が設けられている。エンドエフェクタ２３は、例えば、物体を把持、接触などして物体に操作力を加える。エンドエフェクタ２３には、エンドエフェクタ２３を駆動するアクチュエータ２５と、エンドエフェクタ２３の操作力を検出する力センサ２６と、が設けられている。
【００１２】
制御装置１は、例えば、各関節部２２の角度センサ２４からの角度情報（関節角度など）と、力センサ２６からの操作力と、に基づいて、各関節部２２及びエンドエフェクタ２３のアクチュエータ２５を制御することで、ロボットアーム２をフィードバック制御する。
【００１３】
なお、制御装置１は、例えば、演算処理等と行うＣＰＵ（Central Processing Unit）１ａ、ＣＰＵ１ａによって実行される演算プログラム、制御プログラム等が記憶されたＲＯＭ（Read Only Memory）やＲＡＭ（Random Access Memory）からなるメモリ１ｂ、外部と信号の入出力を行うインターフェイス部（Ｉ／Ｆ）１ｃ、などからなるマイクロコンピュータを中心にして、それぞれ、ハードウェア構成されている。ＣＰＵ１ａ、メモリ１ｂ、及びインターフェイス部１ｃは、データバス１ｄなどを介して相互に接続されている。
【００１４】
本実施形態１に係る制御装置１は、例えば、隠れマルコフモデル（Hidden Markov Model：ＨＭＭ）などの統計モデルを用いて、人の動作を模倣学習することで、ロボットアーム２によるテーブル拭きのような力を加えて行う物理タスクを実行する。
【００１５】
ところで、ロボットアームが上記のような物理タスクを実行するために、制御装置は、ロボットアームの位置情報（関節角度や手先位置）に加えて、ロボットアームの力情報（関節トルクや手先反力）についても学習する必要がある。この場合、制御装置が単にロボットアームの位置情報と同様に、力情報を学習するだけでは次のような問題が生じる。例えば、その学習時と制御時とでそのロボットアームと操作対象物との位置関係が変化した場合（テーブルの高さが変化した場合など）に、ロボットアームと操作対象物との間に過大な力が発生する、あるいはロボットアームが操作対象物に接触できない虞がある。
【００１６】
これに対し、本実施形態１に係る制御装置１は、隠れマルコフモデルによるロボットアーム２の目標軌道生成時に、後述の動力学演算式を用いて力学的整合性を確保する。
【００１７】
制御装置１は、予め用意されたデータに基づいて学習を行い隠れマルコフモデルを構築する学習処理と、学習処理で構築した隠れマルコフモデルを用いてロボットアーム２を制御する実行処理と、を実行する。
【００１８】
（学習処理）
制御装置１は、学習処理において、例えば、予め用意されたロボットアーム２の各関節部２２の関節角度および関節トルクの時系列データ（シミュレーションデータなど）を用いて学習を行い、Baum-Welch法によりleft-right隠れマルコフモデルを構築する（図３）。
【００１９】
なお、制御装置１は、実際にロボットアーム２を遠隔的に動作させ、その際に角度センサ２４及び力センサ２６により検出されたロボットアーム２の各関節部２２の関節角度および関節トルクの時系列データを用いて学習を行い、隠れマルコフモデルを構築してもよい。
【００２０】
図３に示す如く、隠れマルコフモデルにおいて、各ノードｑと、ノードｑ間を遷移する遷移確率ａが設定される。各ノードｑは、各関節部２２の関節角度θおよび関節トルクτの分布を有している。制御装置１は、上記各関節部２２の関節角度および関節トルクの時系列データを用いてノード間の遷移確率ａと、各関節部２２の関節角度θおよび関節トルクτの分布を表すパラメータと、を学習した隠れマルコフモデルを構築する。
【００２１】
このように、学習処理において、ロボットアーム２の関節角度と関節トルクを使用して隠れマルコフモデルを構築する。これにより、ロボットアーム２の関節角度と関節トルクの相関関係も含めて時系列データを保存でき、ロボットアーム２が操作対象物の接触動作を行うための位置指令と力指令の両者を同時に生成できる。また、そのデータの情報量を圧縮しつつ、ロボットアーム２の動作認識等を行うこともできる。
【００２２】
（実行処理）
制御装置１は、本実行処理において、図４に示す如く、各関節部２２の目標軌道生成と、ロボットアーム制御と、を同時に実行する。さらに、制御装置１は、このロボットアーム制御時において、力センサ２６及び角度センサ２４により検出された現在時刻ｔのセンサ値に基づいて、次瞬間である時刻ｔ＋１のロボットアーム２の目標位置情報θｐｌａｎ及び目標力情報τｐｌａｎを決定し、これら目標位置情報θｐｌａｎ及び目標力情報τｐｌａｎに基づいてロボットアーム２のコンプライアンス制御を実行する。これにより、ロボットアーム２の現在の状態から大きく離れた指令値が生成され、ロボットアーム２が急激に動作するような状況を回避できる。さらに、ロボットアーム２の位置指令及び力指令を同時に厳密に満たせない場合でも、上記目標位置情報及び目標力情報を用いて、ロボットアーム２の位置及び力を予め設定したパラメータに従ったバランス状態で適正に制御できる。
【００２３】
制御装置１は、所定時間毎に、以下の[prediction]（１）〜（８）および[resampling]を繰り返して関節部２２の目標軌道を生成する。
【００２４】
[prediction]
（１）制御装置１は、まず、現在時刻ｔにおける隠れマルコフモデルのノードｑｔと、角度センサ２４及び力センサ２６により検出された現在時刻ｔにおける関節角度θｔ及び関節トルクτｔと、を保持する。
【００２５】
（２）制御装置１は、隠れマルコフモデルのノード間の遷移確率ａを用いて、次瞬間である時刻ｔ＋１におけるノードｑｔ＋１をサンプリングする。
【００２６】
（３）制御装置１は、サンプリングしたノードｑｔ＋１が有する関節角度の分布（ノードｑｔ＋１における出力確率）に基づいて、時刻ｔ＋１における関節部２２の目標関節角度（目標位置情報の一具体例）θｔ＋１ｒｅｆをサンプリングする。
このように、関節部２２の目標軌道生成時に関節部２２の関節角度び関節トルクを含む隠れマルコフモデルと、角度センサ２４及び力センサ２６により検出された現在時刻ｔにおける関節角度及び関節トルクと、に基づいて、目標関節角度をサンプリングしている。
【００２７】
（４）制御装置１は、サンプリングした目標関節角度θｔ＋１ｒｅｆを１階微分して関節部２２の目標関節角速度（目標速度情報の一具体例）θ（ドット）ｔ＋１ｒｅｆを算出し、目標関節角度θｔ＋１ｒｅｆを２階微分して関節部２２の目標関節角加速度（目標加速度情報の一具体例）θ（２ドット）ｔ＋１ｒｅｆを算出する。なお、上記のようにθの上部に１つのドット記号を付したもの（１階微分値）をθ（ドット）と表記し、θの上部に２つのドット記号を付したもの（２階微分値）をθ（２ドット）と表記し、以降、他のパラメータについても同様に表記する。
【００２８】
（５）制御装置１は、算出した目標関節角加速度θ（２ドット）ｔ＋１ｒｅｆを下記（式１）を用いて修正する。下記式１において、Ｋｐ及びＫｄは、予め設定される係数行列である。
この修正により、制御安定性をより高めることができる。
【数１】
0006567998.tif 000002
なお、本実施形態１において、制御装置１は、処理の高速化を図るために、上記(５)による目標関節角加速度θ（２ドット）ｔ＋１ｒｅｆの修正を行わなくても良い。
【００２９】
（６）制御装置１は、算出した目標関節角度θｔ＋１ｒｅｆ、目標関節角速度θ（ドット）ｔ＋１ｒｅｆ、および修正した目標関節角加速度θ（２ドット）ｔ＋１ｒｅｆと、目標反力Ｆｔ＋１ｒｅｆとを実現する、目標関節トルクτｔ＋１を算出する。
なお、上記目標反力Ｆｔ＋１ｒｅｆは、ロボットアーム２の操作内容に基づいてユーザにより設定される。例えば、ロボットアーム２を操作対象物に強く接触させる場合は、目標反力１０[Ｎ]に設定し、軽く接触させる場合は、１[Ｎ]に設定する。
制御装置１は、算出した目標関節角度θｔ＋１ｒｅｆ、目標関節角速度θ（ドット）ｔ＋１ｒｅｆ、および修正した目標関節角加速度θ（２ドット）ｔ＋１ｒｅｆと、目標反力Ｆｔ＋１ｒｅｆと、に基づいて、下記逆動力学演算式（運動方程式）（式２）を用いて関節部２２の目標関節トルク（目標力情報の一具体例）τｔ＋１を算出する。なお、下記（式２）において、Ｍは慣性行列であり、Ｃは遠心及びコリオリ項であり、Ｇは重力項である。Jは、関節角速度をロボットアーム２（可動部）の速度に関係付けるヤコビ行列である。
このように、下記逆動力学演算式を用いて目標関節トルクτｔ＋１を算出することで、隠れマルコフモデルによる関節部２２の目標軌道生成時において、ロボットアーム２の力学的な整合性を確保することができる。
【数２】
0006567998.tif 000003
【００３０】
（７）制御装置１は、角度センサ２４及び力センサ２６により検出された現在時刻ｔでの関節角度θｔ及び関節トルクτｔと、隠れマルコフモデルと、に基づいて、目標関節角度θｔ＋１ｒｅｆ、および目標関節トルクτｔ＋１が隠れマルコフモデルから生成される（ｑｔ＋１、θｔ＋１ｒｅｆ、τｔ＋１に到達する）確率Ｐ（θｔ＋１ｒｅｆ、τｔ＋１｜θｔ、τｔ、λ）を算出する。このように、角度センサ２４及び力センサ２６により検出された現在時刻ｔの関節角度及び関節トルク（センサ値）と、隠れマルコフモデルと、から、その目標関節角度θｔ＋１ｒｅｆおよび目標関節トルクτｔ＋１になる確率（尤度）を算出する。
制御装置１は、パーティクル毎に上記確率Ｐを算出する。ここで、制御装置１は、各パーティクルの確率Ｐの総和が１にするため、次の修正処理を行う。例えば、制御装置１は、各パーティクルの確率の総和を算出し、各パーティクルの確率を算出した総和で除算する。
【００３１】
（８）制御装置１は、目標関節角度θｔ＋１ｒｅｆ、目標関節トルクτｔ＋１および修正した確率に基づいて下記（式３）を用いて、次瞬間の指令値である時刻ｔ＋１における目標関節角度の推定値θ（ハット）ｔ＋１ｒｅｆ及び目標関節トルクの推定値τ（ハット）ｔ＋１を、パーティクル毎の関節角度及び関節トルクの期待値として算出する。なお、θ及びτの上部にハット記号を付したものを、夫々、θ（ハット）及びτ（ハット）と表記し、以降、他のパラメータについても同様に表記する。
このように、上記（７）で算出した確率による重みづけ平均により次瞬間の指令値を算出する。
【数３】
0006567998.tif 000004
【００３２】
制御装置１は、上記（８）において算出した目標関節角度の推定値θ（ハット）ｔ＋１ｒｅｆ及び目標関節トルクの推定値τ（ハット）ｔ＋１と、角度センサ２４により検出された各関節部２２の関節角度θと、に基づいて、各関節部２２のコンプライアンス制御を行う。制御装置１は、上記（８）において算出した目標関節角度の推定値θ（ハット）ｔ＋１ｒｅｆ及び目標関節トルクの推定値τ（ハット）ｔ＋１を、夫々、角度指令θｐｌａｎ、及びトルク指令τｐｌａｎとして、下記（式４）を用いて、トルク指令τｒｅｆを算出する。なお、下記（式４）において、Ｋ及びＤは予め設定される係数行列である。
【数４】
0006567998.tif 000005
【００３３】
制御装置１は、算出したトルク指令τｒｅｆを各関節部２２のアクチュエータ２５に送信することで、各アクチュエータ２５を制御する。
さらに、制御装置１は、力センサ２６により検出された関節トルクτが、算出したトルク指令τｒｅｆに追従するように、関節部２２のアクチュエータ２５に対して、フィードバック制御を行う。
【００３４】
以上のように、目標軌道生成時において、関節部２２の関節角度及び関節トルクを含む隠れマルコフモデルと、角度センサ２４及び力センサ２６により検出された関節角度及び関節トルクに基づいて、目標関節角度をサンプリングする。サンプリングした目標関節角度に基づいて目標関節角速度及び目標関節角加速度を算出し、目標関節角度と、算出した目標関節角速度および目標関節角加速度と、目標反力と、に基づいて、逆動力学演算を行い目標関節トルクを算出して、ロボットアーム２の力学的な整合性を確保する。現在時刻ｔの角度センサ値及びトルクセンサ値と、隠れマルコフモデルと、から、その目標関節角度および目標関節トルクになる確率を算出する。算出した確率による重みづけ平均により時刻ｔ＋１の目標関節角度及び目標関節トルクの期待値を算出し、この目標関節角度及び目標関節トルクの期待値を用いてロボットアーム２の関節部２２を制御する。
【００３５】
これにより、ロボットアーム２の位置指令値と力指令値の整合性を取ることができる。したがって、隠れマルコフモデル構築時とロボットアーム制御時で、例えば、操作対象物の大きさ、形状が異なる場合や、ロボットアーム２と操作対象物の位置関係（テーブルの高さが異なるなど）が異なる場合などでも、ロボットアーム２と操作対象物との間で、過大な力が発生する、あるいはロボットアームが操作対象物に接触できない状況を抑制できる。
【００３６】
[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）によってコンピュータに供給されてもよい。一時的なコンピュータ可読媒体の例は、電気信号、光信号、及び電磁波を含む。一時的なコンピュータ可読媒体は、電線及び光ファイバ等の有線通信路、又は無線通信路を介して、プログラムをコンピュータに供給できる。
【符号の説明】
【００７８】
１  制御装置、２  ロボットアーム、２１  リンク、２２  関節部、２３  エンドエフェクタ、２４  角度センサ、２５  アクチュエータ、２６  力センサ
【図１】
0006567998.tif 000006
【図２】
0006567998.tif 000007
【図３】
0006567998.tif 000008
【図４】
0006567998.tif 000009
【図５】
0006567998.tif 000010