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