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