(57)【要約】【課題】リンクの負荷変動に対して関節角度の追従制御性能を維持すること。【解決手段】関節トルク演算部２３０は、関節角度θを目標関節角度ｒａにするのに必要な関節トルクＴ１を演算する。和算部２３１は、関節の目標剛性ｒｓに基づき、各一関節駆動アクチュエータに対する発生力指令値ｕｅ１，ｕｆ１の和を示す和算値Ｕ１を求める。弾性トルク演算部２３２は、関節に作用する各一関節駆動アクチュエータの弾性力による弾性トルクＴＰＣ１を求める。制約部２３４は、リンクのモーメントアーム径をｒとしたとき、｜Ｔ１＋ＴＰＣ１｜＜Ｕ１×ｒの制約条件を満たすように、関節トルクＴ１と弾性トルクＴＰＣ１との合計値を制約する。発生力演算部２３５は、制約部２３４にて制約された合計値（Ｔ１＋ＴＰＣ１）に基づき、各一関節駆動アクチュエータの発生力指令値ｕｅ１，ｕｆ１を演算する。【選択図】図４
2014200893.tif 000002
【特許請求の範囲】
【請求項１】
基部に対して第１関節で旋回可能に連結された第１リンクと、
引張力を発生し、前記第１リンクを前記第１関節まわりに旋回駆動するよう前記第１リンクの両側に配置された一対の一関節駆動アクチュエータと、
前記各一関節駆動アクチュエータに対する発生力指令値をそれぞれ求めて前記各一関節駆動アクチュエータを制御する制御部と、を備え、
前記制御部は、
前記第１関節の関節角度を目標関節角度にするのに必要な関節トルクを求める関節トルク演算処理と、
前記第１関節の目標剛性に基づき、前記各一関節駆動アクチュエータに対する発生力指令値の和を示す和算値を求める和算処理と、
前記第１関節に作用する前記各一関節駆動アクチュエータの弾性力による弾性トルクを求める弾性トルク演算処理と、
前記関節トルクをＴ１、前記和算値をＵ１、前記弾性トルクをＴＰＣ１、前記第１リンクのモーメントアーム径をｒとしたとき、｜Ｔ１＋ＴＰＣ１｜＜Ｕ１×ｒの制約条件を満たすように、前記関節トルクと前記弾性トルクとの合計値を制約する制約処理と、
前記制約処理にて制約された前記合計値に基づき、前記各一関節駆動アクチュエータの発生力指令値を求める発生力演算処理と、を実行することを特徴とするロボット装置。
【請求項２】
前記制御部は、
前記関節トルク演算処理として、前記関節角度と前記目標関節角度との差分に基づくフィードバック演算により、前記関節トルクを求めるフィードバック演算処理を実行することを特徴とする請求項１に記載のロボット装置。
【請求項３】
前記制御部は、
前記関節トルク演算処理として、
前記目標関節角度に基づくフィードフォワード演算により、前記第１関節に対するフィードフォワード制御トルクを求めるフィードフォワード演算処理と、
前記関節角度と前記目標関節角度との差分に基づくフィードバック演算により、フィードバック制御トルクを求めるフィードバック演算処理と、
前記フィードバック制御トルクに前記フィードフォワード制御トルクを加算して、前記関節トルクを求める加算処理と、を実行することを特徴とする請求項１に記載のロボット装置。
【請求項４】
前記制御部は、
前記フィードバック演算処理として、前記差分を補償するＰＤ制御を行うことを特徴とする請求項２又は３に記載のロボット装置。
【請求項５】
前記制御部は、
前記弾性トルク演算処理として、前記各一関節駆動アクチュエータの弾性定数をｋ、前記関節角度をθとしたとき、Ｕ１×ｋ×ｒ２×θを演算することにより、前記弾性トルクを求めることを特徴とする請求項１乃至４のいずれか１項に記載のロボット装置。
【請求項６】
前記第１リンクに対して第２関節で旋回可能に連結された第２リンクと、
引張力を発生し、前記第１リンク及び前記第２リンクを同時に前記第１関節及び前記第２関節まわりに旋回駆動するよう前記第１リンク及び前記第２リンクの両側に配置された一対の二関節同時駆動アクチュエータと、を備え、
前記制御部は、
前記各二関節同時駆動アクチュエータの弾性力により前記第１関節及び前記第２関節に付与されるトルクをキャンセルするように、前記各二関節同時駆動アクチュエータに対する、正の値となる制約を満たす発生力指令値を演算することを特徴とする請求項１乃至５のいずれか１項に記載のロボット装置。
【請求項７】
基部に対して第１関節で旋回可能に連結された第１リンクと、
引張力を発生し、前記第１リンクを前記第１関節まわりに旋回駆動するよう前記第１リンクの両側に配置された一対の一関節駆動アクチュエータと、
前記各一関節駆動アクチュエータに対する発生力指令値をそれぞれ求めて前記各一関節駆動アクチュエータを制御する制御部と、を有するロボット装置を制御するロボット制御方法において、
前記制御部が、前記第１関節の関節角度を目標関節角度にするのに必要な関節トルクを求める関節トルク演算ステップと、
前記第１関節の目標剛性に基づき、前記各一関節駆動アクチュエータに対する発生力指令値の和を示す和算値を求める和算ステップと、
前記第１関節に作用する前記各一関節駆動アクチュエータの弾性力による弾性トルクを求める弾性トルク演算ステップと、
前記関節トルクをＴ１、前記和算値をＵ１、前記弾性トルクをＴＰＣ１、前記第１リンクのモーメントアーム径をｒとしたとき、｜Ｔ１＋ＴＰＣ１｜＜Ｕ１×ｒの制約条件を満たすように、前記関節トルクと前記弾性トルクとの合計値を制約する制約ステップと、
前記制約ステップにて制約された前記合計値に基づき、前記各一関節駆動アクチュエータの発生力指令値を求める発生力演算ステップと、を備えたことを特徴とするロボット制御方法。
【請求項８】
コンピュータに、請求項１乃至６のいずれか１項に記載の各処理を実行させるためのプログラム。
【請求項９】
請求項８に記載のプログラムを記録したコンピュータ読み取り可能な記録媒体。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、リンクを旋回駆動する一対のアクチュエータを制御する制御部を備えたロボット装置、ロボット制御方法、プログラム及び記録媒体に関する。
【背景技術】
【０００２】
マニピュレータの制御方法において、手先が対象物に柔軟に接触できることが重要になってきている。これを産業ロボットに応用すれば、人間とロボットとの共同作業が実現したり、手先の柔軟性の方向を制御することにより部品のはめ合い作業等が容易になったりする。
【０００３】
また、これを脚式移動ロボットに応用すれば、地面に柔らかく接地することで胴体に対する衝撃を和らげたり、段差を吸収することで不整地を安定に歩行したりすることを可能とする。
【０００４】
手先の柔軟性の制御を実現するために、手先に力センサを装着するインピーダンス制御や、人工筋肉アクチュエータを用いる制御等が行われている。人間の筋はアクチュエータであると同時に、粘弾性可変の制御機構であることが知られている。人工筋肉アクチュエータの中でも、特にＭｃＫｉｂｂｅｎ型の人工筋肉アクチュエータに代表される空気圧式のゴム人工筋肉アクチュエータは、粘弾性特性が筋に類似している。そこで、マニピュレータに配置されている人工筋肉アクチュエータの柔らかさを制御することで、任意の手先の柔軟性で対象物に接触することができる。
【０００５】
非特許文献１においては、第１リンク及び第２リンクをそれぞれ駆動する人工筋肉アクチュエータに加え、第１リンクと第２リンクとを同時に駆動する二関節同時駆動アクチュエータを有する、３対６筋マニピュレータが提示されている。非特許文献１では、人工筋肉アクチュエータの弾性を等しくすると、第１関節と手先を結ぶ線上から手先に外力が加わった場合では、手先の外力方向と移動方向が一致するという剛性特性に特徴があることを検証している。ＭｃＫｉｂｂｅｎ型のアクチュエータは粘弾性特性に非線形性を有している、拮抗配置して制御を行う必要がある、という理由でモーションコントロールに用いることには難がある。
【０００６】
これに対し、特許文献１では、制御入力の飽和を用いる簡便なフィードバック制御系を導出し、手先の剛性制御に加えて関節角度の同時制御を行っている。
【０００７】
マニピュレータシステムは手先の質量によってダイナミクスが変化する。この変化を考慮しない制御系では、手先のペイロード把持などにより関節角度の制御性能が劣化してしまう。マニピュレータシステムにダイナミクスの変化が生じた際に、制御性能及び安定性を保つ制御手法として、受動性に基づく制御が知られている。
【先行技術文献】
【特許文献】
【０００８】
【特許文献１】特開２０１２−８６３５４号公報
【非特許文献】
【０００９】
【非特許文献１】一関節筋および二関節筋を含む筋座標系によるロボットアームの機能的特性、大島徹、藤川智彦、熊本水頼、精密工学会誌、Ｖｏｌ．６６，Ｎｏ．１，ｐｐ．１４１−１４６
【発明の概要】
【発明が解決しようとする課題】
【００１０】
しかしながら、上記特許文献１に示されているマニピュレータの制御では、アクチュエータの弾性要素による関節トルクを補償するため、フィードバック系に積分器を持たせている。そのため、ＰＤ制御等の受動性に基づく制御を導入することが難しく、ペイロード把持などにより手先質量が変動すると、関節角度の追従制御性能が劣化するという問題があった。
【００１１】
そこで、本発明は、リンクの負荷変動に対して関節角度の追従制御性能を維持することができるロボット装置、ロボット制御方法、プログラム及び記録媒体を提供することを目的とする。
【課題を解決するための手段】
【００１２】
本発明のロボット装置は、基部に対して第１関節で旋回可能に連結された第１リンクと、引張力を発生し、前記第１リンクを前記第１関節まわりに旋回駆動するよう前記第１リンクの両側に配置された一対の一関節駆動アクチュエータと、前記各一関節駆動アクチュエータに対する発生力指令値をそれぞれ求めて前記各一関節駆動アクチュエータを制御する制御部と、を備え、前記制御部は、前記第１関節の関節角度を目標関節角度にするのに必要な関節トルクを求める関節トルク演算処理と、前記第１関節の目標剛性に基づき、前記各一関節駆動アクチュエータに対する発生力指令値の和を示す和算値を求める和算処理と、前記第１関節に作用する前記各一関節駆動アクチュエータの弾性力による弾性トルクを求める弾性トルク演算処理と、前記関節トルクをＴ１、前記和算値をＵ１、前記弾性トルクをＴＰＣ１、前記第１リンクのモーメントアーム径をｒとしたとき、｜Ｔ１＋ＴＰＣ１｜＜Ｕ１×ｒの制約条件を満たすように、前記関節トルクと前記弾性トルクとの合計値を制約する制約処理と、前記制約処理にて制約された前記合計値に基づき、前記各一関節駆動アクチュエータの発生力指令値を求める発生力演算処理と、を実行することを特徴とする。
【発明の効果】
【００１３】
本発明によれば、アクチュエータの弾性要素による関節剛性を補償する制御により、受動性に基づく制御が可能となる。そのため、リンクに接触が生じていなければ、リンクの負荷変動などに対してロバスト安定性の高い軌道制御が実現され、接触時はアクチュエータの弾性要素による所望の剛性が実現される。これにより、ロバスト性の高い関節角度の軌道追従制御と手先の柔軟性の制御の同時制御を実現することができる。
【図面の簡単な説明】
【００１４】
【図１】アクチュエータの粘弾性モデルを示す模式図である。
【図２】第１実施形態におけるロボット装置を示す模式図である。
【図３】第１実施形態における制御装置の構成を示すブロック図である。
【図４】第１実施形態におけるＣＰＵの制御動作を説明するための機能ブロック線図である。
【図５】第１実施形態におけるＣＰＵの制御動作を説明するための機能ブロック線図である。
【図６】第１実施形態における軌道設計を示す図である。
【図７】第１実施形態におけるシミュレーション結果を示す図である。
【図８】第１実施形態におけるシミュレーション結果を示す図である。
【図９】第２実施形態におけるロボット装置を示す模式図である。
【図１０】第２実施形態におけるスティフネス楕円を説明する図である。
【図１１】第２実施形態における制御装置の構成を示すブロック図である。
【図１２】第２実施形態におけるＣＰＵの制御動作を説明するための機能ブロック線図である。
【図１３】図１２のフィードバック演算部のブロック線図である。
【図１４】第２実施形態におけるシミュレーション結果を示す図である。
【図１５】第２実施形態におけるシミュレーション結果を示す図である。
【図１６】第２実施形態におけるシミュレーション結果を示す図である。
【図１７】第２実施形態におけるシミュレーション結果を示す図である。
【発明を実施するための形態】
【００１５】
以下、本発明を実施するための形態を、図面を参照しながら詳細に説明する。
【００１６】
［第１実施形態］
第１実施形態では、人工筋肉アクチュエータを用いたマニピュレータの関節角度と剛性の同時制御について説明する。
【００１７】
（１）モデリング
本第１実施形態では、人工筋肉アクチュエータを用いたマニピュレータの制御を行う。人工筋肉アクチュエータとは、筋の粘弾性と呼ばれる特性と類似する特性を有するアクチュエータである。筋は図１に示すように、力発生要素と弾性要素と粘性要素とを用いてモデル化される。ここで、ｕを力発生要素の収縮力（引張力）、ｘを収縮方向（引張方向）を正とする筋の収縮量（引張量）とする。収縮速度は、以下の式となる。
【００１８】
【数１】
2014200893.tif 000003
【００１９】
また、ｋを弾性定数、ｂを粘性定数、Ｆを筋発生力とする。このとき、筋の粘弾性特性は、
【００２０】
【数２】
2014200893.tif 000004
とモデル化され、筋発生力の弾性力、粘性力は、力発生要素の発生力ｕと比例するという非線形要素を有する点が特徴的である。
【００２１】
次に、本第１実施形態に係るロボット装置を図２に示す。図２に示すロボット装置１００は、いわゆる１リンクマニピュレータ（以下、単に「マニピュレータ」という）１２０を有している。マニピュレータ１２０は、第１リンクであるリンク１０１、一対の一関節駆動アクチュエータであるアクチュエータｅ１，ｆ１を備えている。
【００２２】
リンク１０１は、基端が基部１０３に対して第１関節である関節Ｊ１で旋回可能に連結されている。
【００２３】
アクチュエータｅ１，ｆ１は、力発生要素、弾性要素及び粘性要素の並列接続でモデル化された、ＭｃＫｉｂｂｅｎ型の人工筋肉アクチュエータに代表される空気圧式のゴム人工筋肉アクチュエータである。
【００２４】
アクチュエータｅ１，ｆ１は、発生力として、引張力（収縮力）のみ発生する。関節Ｊ１を任意の関節角度に位置決めするためには、アクチュエータｅ１，ｆ１は、拮抗して配置される必要がある。そのため、アクチュエータｅ１，ｆ１は、リンク１０１を関節Ｊ１まわりに旋回駆動するようリンク１０１の両側に配置されている。アクチュエータｅ１，ｆ１の一端が、基部１０３に固定され、他端がリンク１０１に固定されている。
【００２５】
拮抗配置された各アクチュエータｅ１，ｆ１の力発生要素の発生力（収縮力）、即ち発生力指令値をそれぞれｕｅ１，ｕｆ１とする。また、リンク１０１の旋回角度、即ち関節Ｊ１の関節角度（第１関節角度）をθとする。また、リンク１０１の慣性モーメントをＩ、モーメントアーム径をｒとすると、運動方程式は、
【００２６】
【数３】
2014200893.tif 000005
となる。式（２）の右辺第一項における発生力（発生力指令値）ｕｅ１，ｕｆ１の差が関節に回転トルク（関節トルク）を与え、右辺第二、第三項における発生力ｕｅ１，ｕｆ１の和が関節Ｊ１に対する剛性及び粘性を変動させることがわかる。
【００２７】
（２）制御系設計
ロボット装置１００は、制御装置２００と、制御装置２００からの発生力指令値ｕｅ１，ｕｆ１に基づき、アクチュエータｅ１，ｆ１をそれぞれ駆動させる２つのドライバ３０１，３０２とを備えている。これらドライバ３０１，３０２は、アクチュエータｅ１，ｆ１に空気を送り込み、発生力指令値に応じた力をアクチュエータｅ１，ｆ１に発生させるものである。
【００２８】
制御装置２００は、各アクチュエータｅ１，ｆ１に対応する発生力指令値ｕｅ１，ｕｆ１を生成する。そして、制御装置２００は、各ドライバ３０１，３０２に各発生力指令値ｕｅ１，ｕｆ１を出力することで、各アクチュエータｅ１，ｆ１の発生力を制御する。
【００２９】
以下、制御装置２００の動作を説明する前に、制御装置２００の装置構成について説明する。図３は、制御装置２００の構成を示すブロック図である。
【００３０】
制御装置２００は、コンピュータで構成されており、制御部（演算部）としてのＣＰＵ（Central  Processing  Unit）２０１を有している。また制御装置２００は、ＲＯＭ（Read  Only  Memory）２０２及びＲＡＭ（Random  Access  Memory）２０３を有している。また、制御装置２００は、ＨＤＤ（Hard  Disk  Drive）２０４、記録ディスクドライブ２０５及び各種のインタフェース２０７，２０９，２１０を備えている。
【００３１】
ＣＰＵ２０１には、ＲＯＭ２０２、ＲＡＭ２０３、ＨＤＤ２０４、記録ディスクドライブ２０５及び各種のインタフェース２０７，２０９，２１０が、バス２０６を介して接続されている。ＲＯＭ２０２には、ＢＩＯＳ等の基本プログラムが格納されている。ＲＡＭ２０３は、ＣＰＵ２０１の演算処理結果を一時的に記憶する記憶装置である。
【００３２】
ＨＤＤ２０４は、ＣＰＵ２０１の演算処理結果である各種のデータ等を記憶する記憶部であると共に、ＣＰＵ２０１に、後述する各種演算処理を実行させるためのプログラム２２１を記録する記録媒体でもある。ＣＰＵ２０１は、ＨＤＤ２０４に記録されたプログラム２２１に基づいて各種演算処理を実行する。
【００３３】
記録ディスクドライブ２０５は、記録ディスク２２２に記録された各種データやプログラム等を読み出すことができる。
【００３４】
インタフェース２１０には、上述したドライバ３０１，３０２が接続されている。ＣＰＵ２０１は、バス２０６及びインタフェース２１０を介して各ドライバ３０１，３０２に発生力指令値ｕｅ１，ｕｆ１を出力する。これにより、各ドライバ３０１，３０２は、各発生力指令値ｕｅ１，ｕｆ１に対応する発生力を各アクチュエータｅ１，ｆ１に発生させる。
【００３５】
本第１実施形態では、ロボット装置１００は、図１では不図示の第１検出部である検出部４０１、及び接触センサ４０３を備えている。検出部４０１は、図１で関節Ｊ１に配置され、関節Ｊ１の関節角度θを検出する。接触センサ４０３は、リンク１０１に設けられ、リンク１０１に物体が接触したか否かを検知する。
【００３６】
インタフェース２０７には、検出部４０１が接続され、検出された関節角度θを示す信号が、インタフェース２０７及びバス２０６を介してＣＰＵ２０１に伝送される。インタフェース２０９には、接触センサ４０３が接続され、接触したか否かを示す信号が、インタフェース２０９及びバス２０６を介してＣＰＵ２０１に伝送される。
【００３７】
図４は、ＣＰＵ２０１の制御動作を説明するための機能ブロック線図である。以下、図４を用いてロボット制御方法について説明する。なお、図４では、マニピュレータ１２０は、符号Ｐで表している。ここで、制御装置２００のＣＰＵ（制御部）２０１は、ＨＤＤ２０４から読み出したプログラム２２１を実行することにより、図４に示す各部として機能する。
【００３８】
即ち、ＣＰＵ２０１は、プログラム２２１に基づき、関節トルク演算処理を実行する関節トルク演算部２３０、和算処理を実行する和算部２３１、弾性トルク演算処理を実行する弾性トルク演算部２３２として機能する。更に、ＣＰＵ２０１は、プログラム２２１に基づき、加算部２３３、制約処理を実行する制約部２３４、及び発生力演算処理を実行する発生力演算部２３５として機能する。
【００３９】
本第１実施形態では、関節Ｊ１を目標関節角度ｒａに位置決めし、同時に関節Ｊ１の剛性を制御する。受動性に基づくロバスト制御を行う場合、その制御は積分器を考慮して設計することは難しい。
【００４０】
そこで、本第１実施形態では、弾性力を補償するループを設け、手先に接触などが発生せず剛性制御を必要としない際は弾性力をキャンセルし、受動性に基づく制御系を設計することにより位置決め制御に対するロバスト性を向上させる。
【００４１】
（２．１）粘弾性力補償
関節Ｊ１の目標関節角度をｒａ、関節Ｊ１の目標剛性をｒｓとする。関節トルク演算部２３０は、関節トルク演算処理として、関節Ｊ１の関節角度θを目標関節角度ｒａにするのに必要な関節トルクＴ１を演算する（関節トルク演算ステップ）。
【００４２】
具体的には、関節トルク演算部２３０は、フィードフォワード演算処理を実行するフィードフォワード演算部２４１と、フィードバック演算処理を実行するフィードバック演算部２４２と、加算処理を実行する加算部２４３とを有する。
【００４３】
フィードフォワード演算部２４１は、目標関節角度ｒａに基づくフィードフォワード演算により、関節Ｊ１に対するフィードフォワード制御トルクＴＦＦＷ１を求める。フィードバック演算部２４２は、関節角度θと目標関節角度ｒａとの差分（θ−ｒａ）に基づくフィードバック演算により、フィードバック制御トルクＴＦＢ１を求める。より具体的には、フィードバック演算部２４２は、フィードバック演算処理として、差分（θ−ｒａ）を補償するＰＤ制御を行う。そして、加算部２４３は、フィードバック制御トルクＴＦＢ１にフィードフォワード制御トルクＴＦＦＷ１を加算して、関節トルクＴ１を求める。つまり、関節トルクをＴ１＝ＴＦＢ１＋ＴＦＦＷ１と定義する。フィードバック制御系であるフィードバック演算部２４２により用いられる伝達関数をＫＰａｓとする。
【００４４】
また、アクチュエータｅ１，ｆ１の弾性力を補償する制御入力（弾性トルク）をＴＰＣ１とすると、
【００４５】
【数４】
2014200893.tif 000006
となる。即ち、弾性トルク演算部２３２は、弾性トルク演算処理として、式（３）に基づき、関節Ｊ１に作用する弾性力による弾性トルクＴＰＣ１を演算する（弾性トルク演算ステップ）。
【００４６】
ここで、式（２）より、
【００４７】
【数５】
2014200893.tif 000007
を満たすように発生力指令値ｕｅ１，ｕｆ１を決定すれば、関節トルクＴ１と弾性トルクＴＰＣ１を与えることができる。
【００４８】
しかし、同時に関節Ｊ１の剛性に関する条件
【００４９】
【数６】
2014200893.tif 000008
を満たさなければならない。Ｕ１を発生力指令値ｕｅ１，ｕｆ１の和を示す和算値
【００５０】
【数７】
2014200893.tif 000009
と定義すると、
【００５１】
【数８】
2014200893.tif 000010
を満たすように、発生力指令値ｕｅ１，ｕｆ１を決定する必要がある。即ち、和算部２３１は、和算処理として、関節Ｊ１の目標剛性ｒｓに基づき、つまり式（７）の関係式に基づき、和算値Ｕ１を求める（和算ステップ）。
【００５２】
トルクの合計値（Ｔ１＋ＴＰＣ１）と収縮力の和算値Ｕ１を同時に満たすには、式（４）と式（６）をｕｅ１，ｕｆ１について解き、
【００５３】
【数９】
2014200893.tif 000011
と決定すればよい。しかし、人工筋肉アクチュエータは収縮方向（引張方向）にのみ力を発生するため、
【００５４】
【数１０】
2014200893.tif 000012
という条件を同時に満たさなければならない。式（８）と式（９）より
【００５５】
【数１１】
2014200893.tif 000013
という条件が得られる。つまりトルクの合計値Ｔ１＋ＴＰＣ１が、
【００５６】
【数１２】
2014200893.tif 000014
という条件を満たしていれば、マニピュレータ１２０は、関節Ｊ１の剛性が目標値ｒｓとなると同時に、トルクＴ１により目標角度ｒａに位置決めされる。
【００５７】
式（１１）を実現するには、
【００５８】
【数１３】
2014200893.tif 000015
と制御入力を制約すればよい。
【００５９】
即ち、加算部２３３は、関節トルクＴ１と弾性トルクＴＰＣ１とを合計した合計値（Ｔ１＋ＴＰＣ１）を生成する。次に、制約部２３４は、制約処理として、｜Ｔ１＋ＴＰＣ１｜＜Ｕ１×ｒの制約条件を満たすように、合計値（Ｔ１＋ＴＰＣ１）を制約する（制約ステップ）。次に、発生力演算部２３５は、制約部２３４にて制約されたトルクの合計値（Ｔ１＋ＴＰＣ１）に基づき、各アクチュエータｅ１，ｆ１の発生力指令値ｕｅ１，ｕｆ１を、式（８）を用いて演算する（発生力演算ステップ）。
【００６０】
ここで、図４に示すように、接触ゲインＧｔを導入し、接触が発生した場合は、Ｇｔ＝０とすることでフィードバック系および筋の弾性力の補償ループを遮断する。これにより、リンク１０１が人や物に接触したことを検知するなどの任意の時刻で、速やかにリンク１０１の剛性を目標剛性ｒｓへと切り替えることができる。
【００６１】
本第１実施形態では、筋の弾性力を補償しているためフィードバック演算部２４２に積分器が不要となるため、ＰＤ制御系を用いる。伝達関数ＫＰａｓは、
【００６２】
【数１４】
2014200893.tif 000016
としている。
【００６３】
ここで、図４に示す制御ブロックを図５に示す制御ブロックに等価変換することが可能である。式（１４）を発生力の和算値Ｕ１を用いて書き直すと、
【００６４】
【数１５】
2014200893.tif 000017
となる。即ち、制御部であるＣＰＵ２０１は、弾性トルク演算処理として、Ｕ１×ｋ×ｒ２×θを演算することにより、弾性トルクＴＰＣ１を求める。これを、式（８）に代入すると、
【００６５】
【数１６】
2014200893.tif 000018
となる。このとき、アクチュエータｅ１，ｆ１が収縮方向にのみ力を発生する条件は、
【００６６】
【数１７】
2014200893.tif 000019
となる。
【００６７】
制御入力（関節トルク）Ｔ１を、関節角度θをパラメータとして式（１６）に示したように上限値、下限値を制約し、さらに式（１５）に示したように分配する。これにより、マニピュレータ１２０はＰＤ制御系のような積分器を含まない制御系を用いて関節Ｊ１の剛性が目標値ｒｓとなると同時に、トルクＴ１により目標角度ｒａに位置決めされる。
【００６８】
なお、本第１実施形態では筋の弾性力を補償する制御系を示したが、関節角速度を制御ループに加えることで筋の粘性力を補償することも可能である。
【００６９】
（２．２）軌道設計
次に、関節Ｊ１の目標軌道ｒａ、目標剛性ｒｓの設計方法を示す。本第１実施形態では、目標軌道ｒａは図６（ａ）に示すような、加速区間−等速区間−減速区間を有する軌道を用いる。ここで、ｒａＳ，ｒａＦはそれぞれ、軌道の初期角度、目標角度である。また、ｔａ，ｔｂ，ｔｆｉｎはそれぞれ、等速区間の開始時間、等速区間の終了時間、位置決め終了時間である。さらに、目標剛性ｒｓは図６（ｂ）に示すように、加減速区間で高剛性ｒｓｍａｘとなり、等速間で低剛性ｒｓｍｉｎとなるように設定する。剛性を可変させることにより、マニピュレータ１２０は、駆動開始時では筋の弾性力により加速され、等速区間では低剛性となることによりほぼ慣性力で駆動される。これにより、少ない筋の発生力で駆動することが可能となる。
【００７０】
本第１実施形態では、加速区間−等速区間−減速区間を有する軌道を用いたが、例えばミニマムジャーク軌道のような等速区間を伴わない軌道であっても構わない。また、駆動の効率を考慮しない場合は、目標剛性を一定としても構わない。
【００７１】
（３）シミュレーション
前節の制御系を用いるシミュレーションを行う。リンク１０１の慣性モーメントをＩ＝８．３×１０−２［ｋｇｍ２］、モーメントアーム径をｒ＝０．１［ｍ］、弾性定数，粘性定数をｋ＝２５，ｂ＝３とする。目標軌道ｒａは、初期角度ｒａＳ＝０［ｄｅｇ］、目標角度ｒａＦ＝２０［ｄｅｇ］、等速区間の開始時間ｔａ＝０．３５［秒］、等速区間の終了時間ｔｂ＝０．４５［秒］、位置決め終了時間ｔｆｉｎ＝０．８［秒］とする。関節Ｊ１の目標剛性はｒｓｍａｘ＝２．３×ｋ×ｒ２［Ｎｍ／ｒａｄ］とし、ｒｓｍｉｎ＝０．８×ｋ×ｒ２［Ｎｍ／ｒａｄ］とする。ここで、ｋ，ｒは定数であるので、発生力の和Ｕ１を、加減速区間でＵ１＝２．３、等速区間で、Ｕ１＝０．８となるように制御すればよい。
【００７２】
図７（ａ）に関節角度θと目標角度ｒａをそれぞれ実線，破線で、図７（ｂ）に発生力指令値ｕｅ１，ｕｆ１の応答をそれぞれ実線，破線で示す。図７（ｃ）に発生力指令値ｕｅ１，ｕｆ１の差を実線で、和を破線で、Ｔ１／ｒ＋Ｕ１×ｋ×ｒ×θの応答を一点鎖線で示す。さらに、図７（ｄ）に関節トルクＴ１を実線で、そのうちフィードフォワード制御トルクＴＦＦＷ１を破線で、式（１６）に示した制御トルクの上限値、下限値を点線で示す。
【００７３】
図７（ａ）より、目標軌道ｒａと関節角度θは精度良く一致しており、関節Ｊ１は目標角度に位置決めされていることがわかる。図７（ｂ）より、発生力指令値ｕｅ１，ｕｆ１はフィードバック制御系の関節トルクＴ１を、式（１５）と式（１６）に示した条件で配分されているが、人工筋肉アクチュエータの収縮力（引張力）は正の値のみを取るという特性を満たしていることがわかる。図７（ｃ）において、実線で示すｕｆ１−ｕｅ１は、一点鎖線で示すＴ１／ｒ＋Ｕ１×ｋ×ｒ×θと重なっており、式（４）の条件を満たしている。また、破線で示すｕｆ１＋ｕｅ１＝Ｕ１は、０．３６［秒］以降は加減速区間で２．３、等速区間で０．８となっており、関節Ｊ１の剛性は目標値に制御されていることがわかる。図７（ｄ）より、慣性モーメントがノミナル値では関節トルクＴ１とフィードフォワード制御トルクＴＦＦＷ１は一致していることがわかる。
【００７４】
次に、制御系のロバスト性を検証するため、リンク１０１の慣性モーメントを１．２倍とするシミュレーションを行う。応答を図８（ａ）〜図８（ｄ）に示す。図８（ａ）より、慣性モーメントが変動しても受動性により関節角度θは目標軌道ｒａに精度良く追従していることがわかる。また、図８（ｄ）より、慣性モーメントの変動をフィードバック制御系が補償していることがわかる。フィードバック制御系の制御入力により、関節トルクＴ１は破線で示すトルク制限に達し、０．３［秒］前後でトルクが制約されている。これにより、アクチュエータｅ１，ｆ１は引張方向（収縮方向）のみに力を発生するという特性が保証され、図８（ｂ）より、発生力指令値ｕｅ１，ｕｆ１は正の値のみを取るという特性を満たしていることがわかる。このように、制御対象のパラメータが変動しても、図８（ｃ）において、破線で示すｕｆ１＋ｕｅ１＝Ｕ１は、０．３６［秒］以降は加減速区間で２．３、等速区間で０．８となっており、関節Ｊ１の剛性は目標値に制御されていることがわかる。
【００７５】
以上、本第１実施形態により、フィードバック制御系に積分器を含めることなく、高精度な軌道追従制御と、筋の粘弾性力による剛性制御の同時制御が実現できることがわかる。
【００７６】
［第２実施形態］
次に、本発明の第２実施形態に係るロボット装置について説明する。
【００７７】
（１）モデリング
図９は、第２実施形態におけるロボット装置を示す模式図であり、ロボット装置１００Ｂは、マニピュレータ１２０Ｂと、制御装置２００Ｂと、６つのドライバ３０１〜３０６を備えている。
【００７８】
マニピュレータ１２０Ｂは、３対６筋の２リンクマニピュレータである。マニピュレータ１２０Ｂは、基部１０３に対して関節（第１関節）Ｊ１で旋回可能に連結されたリンク（第１リンク）１０１と、リンク１０１に対して関節（第２関節）Ｊ２で旋回可能に連結されたリンク（第２リンク）１０２とを有する。
【００７９】
また、マニピュレータ１２０Ｂは、リンク１０１を関節Ｊ１まわりに旋回駆動するようリンク１０１の両側に配置された一対のアクチュエータｅ１，ｆ１を有する。また、マニピュレータ１２０Ｂは、リンク１０２を関節Ｊ２まわりに旋回駆動するようリンク１０２の両側に配置された一対のアクチュエータｅ２，ｆ２を有する。更に、マニピュレータ１２０Ｂは、リンク１０１，１０２を同時に関節Ｊ１，Ｊ２まわりに旋回駆動するようリンク１０１，１０２の両側に配置された一対のアクチュエータｅ３，ｆ３を有する。
【００８０】
一対のアクチュエータｅ３，ｆ３は、二関節同時駆動アクチュエータである。二関節同時駆動アクチュエータは、リンク１０１とリンク１０２を同時に駆動するように配置されているアクチュエータであり、人の上腕部や下肢大腿部には、二関節筋とよばれる二関節同時駆動アクチュエータが存在することが知られている。人の四肢の筋配列は複雑であるが、実効筋概念が導入され、図９に示すような３対６筋を有する２リンクモデルが提示されている。なお、アクチュエータｅ１，ｆ１の拮抗対とアクチュエータｅ２，ｆ２の拮抗対のように単一のリンクを駆動するアクチュエータを一関節駆動アクチュエータとよぶ。
【００８１】
アクチュエータｅ１，ｆ１，ｅ２，ｆ２，ｅ３，ｆ３は、上記第１実施形態で説明した図１に示した筋の粘弾性特性を有するアクチュエータである。即ち、アクチュエータｅ１，ｆ１，ｅ２，ｆ２，ｅ３，ｆ３は、力発生要素、弾性要素及び粘性要素の並列接続でモデル化され、引張力（収縮力）のみを発生する。本第２実施形態では、２リンクマニピュレータ１２０Ｂに対して関節角度と剛性の同時制御を行う。
【００８２】
制御装置２００Ｂは、各アクチュエータｅ１，ｆ１，ｅ２，ｆ２，ｅ３，ｆ３に対応する発生力指令値ｕｅ１，ｕｆ１，ｕｅ２，ｕｆ２，ｕｅ３，ｕｆ３を生成する。そして、制御装置２００Ｂは、各ドライバ３０１〜３０６に各発生力指令値ｕｅ１，ｕｆ１，ｕｅ２，ｕｆ２，ｕｅ３，ｕｆ３を出力することで、各アクチュエータｅ１，ｆ１，ｅ２，ｆ２，ｅ３，ｆ３の発生力を制御する。各ドライバ３０１〜３０６は、発生力指令値ｕｅ１，ｕｆ１，ｕｅ２，ｕｆ２，ｕｅ３，ｕｆ３に対応する発生力をアクチュエータｅ１，ｆ１，ｅ２，ｆ２，ｅ３，ｆ３に発生させる。
【００８３】
ｕｅｎ，ｕｆｎ（ｎ＝１，２，３）を力発生要素の発生力（発生力指令値）、ｋｅｎ，ｋｆｎ，ｂｅｎ，ｂｆｎ（ｎ＝１，２，３）をアクチュエータの弾性定数、粘性定数とする。リンク１０１，１０２の旋回角度（即ち、関節Ｊ１，Ｊ２の関節角度）をθ１，θ２とする。また、リンク１０１，１０２の慣性モーメントをＩ１，Ｉ２、リンク１０１，１０２の長さをｌ１，ｌ２、リンク１０１，１０２の質量をｍ１，ｍ２とする。モーメントアーム径をリンク１０１，１０２共にｒとする。また、エンドエフェクタの質量をｍｅとする。エンドエフェクタは、図９では省略されているが、リンク１０２の先端Ｅに設けられている。本第２実施形態では、各筋の弾性定数、粘性定数をいずれもｋ，ｂとすると、運動方程式は、
【００８４】
【数１８】
2014200893.tif 000020
となる。
【００８５】
（２）制御系設計
上記第１実施形態では関節Ｊ１の角度と剛性の同時制御を行ったが、本第２実施形態においても、関節Ｊ１，Ｊ２の角度と剛性の同時制御を行う。しかし、２リンクマニピュレータ１２０Ｂでは、手先（リンク１０２の先端）Ｅが外界と直に接触するので、手先Ｅの剛性を制御することが重要である。手先Ｅの剛性は、図１０に示すようなスティフネス楕円によって表される。この楕円は、各方向に対しての剛性の分布を示し、手先Ｅと楕円の距離が離れるほど剛性が高いことを表している。
【００８６】
図１１は、制御装置２００Ｂの構成を示すブロック図である。制御装置２００Ｂは、コンピュータで構成されており、上記第１実施形態と同様、制御部（演算部）としてのＣＰＵ２０１、ＲＯＭ２０２、ＲＡＭ２０３、ＨＤＤ２０４、記録ディスクドライブ２０５及び各種のインタフェース２０７〜２１０を備えている。
【００８７】
ＣＰＵ２０１には、ＲＯＭ２０２、ＲＡＭ２０３、ＨＤＤ２０４、記録ディスクドライブ２０５及び各種のインタフェース２０７〜２１０が、バス２０６を介して接続されている。ＣＰＵ２０１は、ＨＤＤ２０４に記録されたプログラム２２１Ｂに基づいて各種演算処理を実行する。
【００８８】
インタフェース２１０には、上述したドライバ３０１〜３０６が接続されている。ＣＰＵ２０１は、バス２０６及びインタフェース２１０を介して各ドライバ３０１〜３０６に発生力指令値ｕｅ１，ｕｆ１，ｕｅ２，ｕｆ２，ｕｅ３，ｕｆ３を出力する。各ドライバ３０１〜３０６は、入力した各発生力指令値に対応する発生力を各アクチュエータｅ１，ｆ１，ｅ２，ｆ２，ｅ３，ｆ３に発生させる。
【００８９】
本第２実施形態では、ロボット装置１００Ｂは、図９では不図示の第１検出部である検出部４０１、第２検出部である検出部４０２及び接触センサ４０３を備えている。検出部４０１は、図９で関節Ｊ１に配置され、関節Ｊ１の関節角度（第１関節角度）θ１を検出する。検出部４０２は、図９で関節Ｊ２に配置され、関節Ｊ２の関節角度（第２関節角度）θ２を検出する。接触センサ４０３は、リンク１０１又はリンク１０２に設けられ、リンク１０１又はリンク１０２に物体が接触したか否かを検知する。
【００９０】
インタフェース２０７には、検出部４０１が接続され、検出された関節角度θ１を示す信号が、インタフェース２０７及びバス２０６を介してＣＰＵ２０１に伝送される。インタフェース２０８には、検出部４０２が接続され、検出された関節角度θ２を示す信号が、インタフェース２０８及びバス２０６を介してＣＰＵ２０１に伝送される。インタフェース２０９には、接触センサ４０３が接続され、接触したか否かを示す信号が、インタフェース２０９及びバス２０６を介してＣＰＵ２０１に伝送される。
【００９１】
図１２は、ＣＰＵ２０１の制御動作を説明するための機能ブロック線図である。以下、図１２を用いてロボット制御方法について説明する。なお、図１２では、マニピュレータ１２０Ｂは、符号Ｐで表している。ここで、制御装置２００ＢのＣＰＵ（制御部）２０１は、ＨＤＤ２０４から読み出したプログラム２２１Ｂを実行することにより、図１２に示す各部として機能する。
【００９２】
即ち、ＣＰＵ２０１は、プログラム２２１Ｂに基づき、目標関節角度生成部５２０として機能する。また、ＣＰＵ２０１は、プログラム２２１Ｂに基づき、関節トルク演算処理を実行する関節トルク演算部５３０、及び和算処理を実行する和算部５３１として機能する。また、ＣＰＵ２０１は、プログラム２２１Ｂに基づき、弾性トルク演算処理、制約処理及び発生力演算処理を実行する発生力演算部５３２，５３３，５３４として機能する。
【００９３】
目標関節角度生成部５２０は、リンク１０２の先端Ｅの目標軌道に基づき、関節Ｊ１の目標関節角度（第１目標関節角度）ｒａ１及び関節Ｊ２の目標関節角度（第２目標関節角度）ｒａ２を生成する目標関節角度生成処理を実行する。
【００９４】
ここで、関節トルク演算部５３０は、上記第１実施形態の関節トルク演算部２３０と略同様の演算処理を実行する。また、和算部５３１は、上記第１実施形態の和算部２３１と略同様の演算処理を実行する。また、発生力演算部５３２，５３３は、上記第１実施形態の各部２３３，２３４，２３５と略同様の演算処理を実行する。関節トルク演算部５３０は、上記第１実施形態と同様、フィードフォワード演算部５４１と、フィードバック演算部５４２と、加算部５４３，５４４とを有している。
【００９５】
（２．１）粘弾性力補償
一関節駆動アクチュエータであるアクチュエータｅ１，ｆ１及びアクチュエータｅ２，ｆ２の制御方法は第１実施形態と略同様である。リンク１０１，１０２に対して、フィードバック制御系ＫＰａｓ及びフィードフォワード制御系ＫＦＦＷは、関節角度θｎ（ｎ＝１，２）と目標角度ｒａｎ（ｎ＝１，２）との誤差を補償するための関節トルク（制御入力トルク）Ｔｎとして、
【００９６】
【数１９】
2014200893.tif 000021
を算出する。上記第１実施形態と同様に、発生力指令値の和算値をＵｎとおくと、
【００９７】
【数２０】
2014200893.tif 000022
と表せる。上記第１実施形態と同様にこれを解くと、
【００９８】
【数２１】
2014200893.tif 000023
となる。そして、関節トルク（制御入力トルク）Ｔｎ（ｎ＝１，２）に対して、
【００９９】
【数２２】
2014200893.tif 000024
と制約する。
【０１００】
本第２実施形態では、一関節駆動アクチュエータに加えて、二関節同時駆動アクチュエータであるアクチュエータｅ３，ｆ３を用いて関節Ｊ１，Ｊ２に剛性を与える。アクチュエータｅ３，ｆ３の発生力指令値ｕｅ３，ｕｆ３の和算値をＵ３とする。さらに、アクチュエータｅ３，ｆ３の発生力指令値ｕｅ３，ｕｆ３に差を与えることによって、一関節駆動アクチュエータと同様に関節Ｊ１，Ｊ２にトルクを与えることが可能である。つまり、制御部であるＣＰＵ２０１（発生力演算部５３４）は、各アクチュエータｅ３，ｆ３に対する発生力指令値ｕｅ３，ｕｆ３の差分（ｕｆ３−ｕｅ３）に基づくトルクを各関節Ｊ１，Ｊ２に付与するように発生力指令値ｕｅ３，ｕｆ３を算出する。その際、接触などが発生せず手先Ｅに剛性を必要としない時間帯では、アクチュエータｅ３，ｆ３の関節トルクにより、弾性要素の発生力をキャンセルするように発生力指令値ｕｅ３，ｕｆ３を算出する。ここで、
【０１０１】
【数２３】
2014200893.tif 000025
とする。これを解くと発生力指令値ｕｅ３，ｕｆ３は、
【０１０２】
【数２４】
2014200893.tif 000026
となる。そして、
【０１０３】
【数２５】
2014200893.tif 000027
と制約する。
【０１０４】
（２．２）ロバスト制御
上記第１実施形態では受動性に基づく制御系としてＰＤ制御系を適用した。しかし、２リンクマニピュレータに対しては受動性に基づくロバスト制御手法が数多く提案されており、制御系に受動性に基づくロバスト安定性に加え、ロバスト性能を付与することができる。これらのロバスト制御を、前節の粘弾性補償を用いることにより二関節筋マニピュレータに対して適用することができる。本第２実施形態では、フィードバック演算部５４２は、図１３に示すフィードバック制御系として構成されている。ここで、ｖ，ａは補助的なベクトルであり、
【０１０５】
【数２６】
2014200893.tif 000028
である。ただし、
【０１０６】
【数２７】
2014200893.tif 000029
である。また、
【０１０７】
【数２８】
2014200893.tif 000030
とする。Ｙはリグレッサと呼ばれる行列であり、
【０１０８】
【数２９】
2014200893.tif 000031
とおく。これを用いて、
【０１０９】
【数３０】
2014200893.tif 000032
と定義する。このときｕｐを、
【０１１０】
【数３１】
2014200893.tif 000033
とすると、フィードバック制御トルクＴＦＢｎは、
【０１１１】
【数３２】
2014200893.tif 000034
と求まる。ここで、
【０１１２】
【数３３】
2014200893.tif 000035
の変動幅から求めることができる。ここで、
【０１１３】
【数３４】
2014200893.tif 000036
はリンク１０２とエンドエフェクタを一体の剛体とみなすときの重心までの距離と慣性モーメントである。また、Λ，Ｋ，εは設計パラメータである。
【０１１４】
（２．３）手先剛性制御
上記第１実施形態の１リンクマニュピレータ１２０では、関節Ｊ１の剛性と手先の剛性は同一であった。３対６筋を有する２リンクマニュピレータ１２０Ｂでは、発生力の和Ｕ１，Ｕ２，Ｕ３を制御することにより手先Ｅの剛性が求まる。例えば、発生力の和Ｕ１，Ｕ２，Ｕ３を、
【０１１５】
【数３５】
2014200893.tif 000037
と制御すると、スティフネス楕円の長軸は関節Ｊ１と手先Ｅを結ぶ方向を向くことが知られている。また、手先Ｅのスティフネス楕円の長短軸がｘ−ｙ軸と平行になる発生力の和Ｕ１，Ｕ２，Ｕ３を求めてもよい。
【０１１６】
（２．４）軌道設計
本第２実施形態では、目標軌道を手先Ｅがｙ軸に平行に駆動するものとする。さらに上記第１実施形態と同様に加速区間−等速区間−減速区間を有するものとする。そして、関節角度θ１，θ２に対する目標軌道ｒａ１，ｒａ２は、手先軌道から逆運動学により求める。
【０１１７】
つぎに、目標剛性は式（３５）を満たし、手先Ｅの目標位置においてスティフネス楕円の長軸が関節Ｊ１と手先Ｅを結ぶ方向を向くように設定する。また、上記第１実施形態と同様に、加減速区間で高剛性となり、等速間で低剛性となるように設定する。加減速区間での一関節アクチュエータの発生力の和をＵｎｍａｘ（ｎ＝１，２，３）、等速区間での一関節アクチュエータの発生力の和をＵｎｍｉｎ（ｎ＝１，２，３）とする。
【０１１８】
本第２実施形態では、加速区間−等速区間−減速区間を有する軌道を用いたが、例えばミニマムジャーク軌道のような等速区間を伴わない軌道であっても構わない。また、駆動の効率を考慮しない場合は、目標剛性を一定としても構わない。
【０１１９】
（３）シミュレーション
前節の制御系を用いたシミュレーションを行う。リンク１０１とリンク１０２の物理パラメータは同一とする。リンク１０１，１０２の長さを０．２［ｍ］、リンク１０１，１０２の慣性モーメントをＩ１＝Ｉ２＝１．３×１０−３［ｋｇｍ２］、モーメントアーム径を０．０５［ｍ］、弾性、粘性定数をｋ＝１２，ｂ＝０．００３とする。目標軌道は、等速区間の開始時間ｔａ＝０．２７４７［秒］、等速区間の終了時間ｔｂ＝０．４７４６［秒］、位置決め終了時間ｔｆｉｎ＝０．７５［秒］とする。また、手先Ｅのスティフネス楕円が関節Ｊ１と手先Ｅを結ぶ方向になるように、各アクチュエータの発生力の和Ｕ１，Ｕ２，Ｕ３を制御する。そこで、Ｕｎｍａｘ＝４，Ｕｎｍｉｎ＝２（ｎ＝１，２，３）とする。
【０１２０】
図１４（ａ）及び図１４（ｂ）にリンク１０１，１０２の関節角度θ１，θ２を実線で、目標軌道ｒａ１，ｒａ２を破線で示す。図１５（ａ）〜図１５（ｃ）に発生力指令値ｕｆ１，ｕｆ２，ｕｆ３の応答を実線で、発生力指令値ｕｅ１，ｕｅ２，ｕｅ３の応答を破線で示す。さらに、図１６に発生力指令値ｕｅｎ，ｕｆｎ（ｎ＝１，２，３）の和をそれぞれ実線、破線、一点鎖線で示す。
【０１２１】
図１４（ａ）及び図１４（ｂ）より実線で示す関節角度θ１，θ２は破線で示す軌道と重なっており、リンク１０１，１０２は目標軌道に追従していることがわかる。図１５（ａ）及び図１５（ｂ）に示すリンク１０１，１０２の応答より、発生力指令値ｕｅ１，ｕｆ１及び発生力指令値ｕｅ２，ｕｆ２は、それぞれ関節トルクＴ１，Ｔ２を、式（２１）に示した条件で配分されている。図１５（ｃ）より、発生力指令値ｕｅ３，ｕｆ３は弾性力を補償するためのトルクを式（２５）に示した条件で配分されている。
【０１２２】
図１６において、実線、破線、一点鎖線で示すｕｆｎ＋ｕｅｎ（ｎ＝１，２，３）は、いずれも加減速区間で４、等速区間で２となっていることがわかる。各拮抗対の剛性が目標剛性に制御されていることから、手先Ｅのスティフネス楕円は関節Ｊ１と手先Ｅを結ぶ方向に制御されていることがわかる。
【０１２３】
さらに、エンドエフェクタ質量を変動させるシミュレーションを行う。図１７（ａ）にエンドエフェクタ質量を１倍、２倍、４倍とする際のリンク１０２の位置決め誤差の応答をそれぞれ実線、破線、一点鎖線で示す。また、比較として図１７（ｂ）にフィードバック制御系にＰＤ制御系を用いるエンドエフェクタ変動シミュレーションの位置決め誤差応答を示す。フィードバック制御系にロバスト制御を用いると、エンドエフェクタ質量が変動しても軌道追従誤差に大きな劣化はない。しかし、ＰＤ制御系ではエンドエフェクタ質量が変動するにつれて軌道位置決め誤差が大きくなってゆくことがわかる。
【０１２４】
本第２実施形態の制御系により、目標軌道への追従を実現しながら、３対６筋の弾性により手先の剛性を制御することが可能であることがわかる。さらに、エンドエフェクタ質量などにパラメータ変動が発生する際も受動性に基づくロバスト性を有し、軌道追従制御性能の劣化を防ぐことができる。
【０１２５】
なお、本発明は、以上説明した実施形態に限定されるものではなく、多くの変形が本発明の技術的思想内で当分野において通常の知識を有する者により可能である。
【０１２６】
上記実施形態の各処理動作は具体的には制御装置２００，２００Ｂの制御部としてのＣＰＵ２０１により実行されるものである。従って上述した機能を実現するプログラムを記録した記録媒体を制御装置に供給し、制御装置のコンピュータ（ＣＰＵやＭＰＵ）が記録媒体に格納されたプログラムを読み出し実行することによって達成されるようにしてもよい。この場合、記録媒体から読み出されたプログラム自体が上述した実施形態の機能を実現することになり、プログラム自体及びそのプログラムを記録した記録媒体は本発明を構成することになる。
【０１２７】
また、上記実施形態では、コンピュータ読み取り可能な記録媒体がＨＤＤ２０４であり、ＨＤＤ２０４にプログラム２２１，２２１Ｂが格納される場合について説明したが、これに限定するものではない。プログラムは、コンピュータ読み取り可能な記録媒体であれば、いかなる記録媒体に記録されていてもよい。例えば、プログラムを供給するための記録媒体としては、図３，図１１に示すＲＯＭ２０２、記録ディスク２２２、不図示の外部記憶装置等を用いてもよい。具体例を挙げて説明すると、記録媒体として、フレキシブルディスク、ハードディスク、光ディスク、光磁気ディスク、ＣＤ−ＲＯＭ、ＣＤ−Ｒ、磁気テープ、書き換え可能な不揮発性のメモリ（例えばＵＳＢメモリ）、ＲＯＭ等を用いることができる。
【０１２８】
また、上記実施形態におけるプログラムを、ネットワークを介してダウンロードしてコンピュータにより実行するようにしてもよい。
【０１２９】
また、コンピュータが読み出したプログラムコードを実行することにより、上記実施形態の機能が実現されるだけに限定するものではない。そのプログラムコードの指示に基づき、コンピュータ上で稼働しているＯＳ（オペレーティングシステム）等が実際の処理の一部または全部を行い、その処理によって前述した実施形態の機能が実現される場合も含まれる。
【０１３０】
さらに、記録媒体から読み出されたプログラムコードが、コンピュータに挿入された機能拡張ボードやコンピュータに接続された機能拡張ユニットに備わるメモリに書き込まれてもよい。そのプログラムコードの指示に基づき、その機能拡張ボードや機能拡張ユニットに備わるＣＰＵ等が実際の処理の一部または全部を行い、その処理によって上記実施形態の機能が実現される場合も含まれる。
【０１３１】
また、上記実施形態では、コンピュータがＨＤＤ等の記録媒体に記録されたプログラムを実行することにより、図４、図５又は図１２に示す機能を果たす場合について説明したが、これに限定するものではない。図４、図５又は図１２に示す制御部の一部又は全部の機能をＡＳＩＣやＦＰＧＡ等の専用ＬＳＩで構成してもよい。なお、ＡＳＩＣはApplication  Specific  Integrated  Circuit、ＦＰＧＡはField-Programmable  Gate  Arrayの頭字語である。
【符号の説明】
【０１３２】
１００…ロボット装置、１０１…リンク（第１リンク）、１２０…マニピュレータ、２０１…ＣＰＵ（制御部）、ｅ１，ｆ１…アクチュエータ（一関節駆動アクチュエータ）
【図１】
2014200893.tif 000038
【図２】
2014200893.tif 000039
【図３】
2014200893.tif 000040
【図４】
2014200893.tif 000041
【図５】
2014200893.tif 000042
【図６】
2014200893.tif 000043
【図７】
2014200893.tif 000044
【図８】
2014200893.tif 000045
【図９】
2014200893.tif 000046
【図１０】
2014200893.tif 000047
【図１１】
2014200893.tif 000048
【図１２】
2014200893.tif 000049
【図１３】
2014200893.tif 000050
【図１４】
2014200893.tif 000051
【図１５】
2014200893.tif 000052
【図１６】
2014200893.tif 000053
【図１７】
2014200893.tif 000054