(57)【特許請求の範囲】
【請求項１】
基体に関節を介して旋回可能に連結されたリンクと、前記基体に対して互いに反対方向に前記リンクを引っ張る駆動力を発生して、駆動力の差により前記関節にトルクを付与すると共に、駆動力の和により前記関節に剛性を付与する一対のアクチュエータと、を備え、前記関節の剛性を目標剛性にすると共に前記関節の関節角度を目標軌道に追従させるのに必要な前記各アクチュエータの各駆動力指令値を求めて、前記各アクチュエータに発生させる駆動力が前記各駆動力指令値となるように、前記各アクチュエータを制御するロボット装置の制御方法において、
前記リンクの逆動力学に基づき、前記目標剛性、前記目標軌道、前記目標軌道の角速度、前記目標軌道の角加速度を用いて、前記関節に必要なトルクを示すトルク指令値を算出するトルク指令値算出工程と、
前記トルク指令値算出工程で算出した前記トルク指令値と前記目標剛性とを用いて求められる前記各駆動力指令値が０以上の値を取り得るか否かを判断する判断工程と、
前記判断工程で前記各駆動力指令値のうち少なくとも一方が負の値を取り得ると判断した場合、前記目標剛性を増加する変更、前記目標軌道の角加速度を低減する変更のうち少なくとも一方の変更をして、前記トルク指令値算出工程に戻る変更工程と、
前記判断工程で前記各駆動力指令値が０以上の値を取り得ると判断した場合、前記目標剛性及び前記トルク指令値を用いて、前記各駆動力指令値を算出する駆動力指令値算出工程と、
前記関節角度と前記目標軌道との差分を補償する補正トルク指令値を算出する補正トルク指令値算出工程と、を備え、
前記駆動力指令値算出工程では、前記各駆動力指令値の算出に用いる前記トルク指令値として、前記トルク指令値算出工程で求めた前記トルク指令値に前記補正トルク指令値算出工程で求めた前記補正トルク指令値を加算補正した結果を用いることを特徴とするロボット装置の制御方法。
【請求項２】
基体に関節を介して旋回可能に連結されたリンクと、前記基体に対して互いに反対方向に前記リンクを引っ張る駆動力を発生して、駆動力の差により前記関節にトルクを付与すると共に、駆動力の和により前記関節に剛性を付与する一対のアクチュエータと、を備え、前記関節の剛性を目標剛性にすると共に前記関節の関節角度を目標軌道に追従させるのに必要な前記各アクチュエータの各駆動力指令値を求めて、前記各アクチュエータに発生させる駆動力が前記各駆動力指令値となるように、前記各アクチュエータを制御するロボット装置の制御方法において、
前記リンクの逆動力学に基づき、前記目標剛性、前記目標軌道、前記目標軌道の角速度、前記目標軌道の角加速度を用いて、前記関節に必要なトルクを示すトルク指令値を算出するトルク指令値算出工程と、
前記関節角度と前記目標軌道との差分を補償する補正トルク指令値を算出する補正トルク指令値算出工程と、
前記トルク指令値算出工程で求めた前記トルク指令値に前記補正トルク指令値算出工程で求めた前記補正トルク指令値を加算補正して、補正後のトルク指令値を取得し、前記目標剛性及び前記補正後のトルク指令値に基づき、前記各駆動力指令値を算出する駆動力指令値算出工程と、を備え、
前記駆動力指令値算出工程における算出の結果、前記各駆動力指令値のうち少なくとも１つが前記リンクを押し出す駆動力を発生する値となった場合、前記目標剛性を増加する変更、及び前記目標軌道の角加速度を低減する変更のうち少なくとも一方を実行して、再び、前記トルク指令値算出工程、前記補正トルク指令値算出工程及び前記駆動力指令値算出工程を実行することを特徴とするロボット装置の制御方法。
【請求項３】
前記リンクが第１リンクであり、
前記関節が第１関節であり、
前記ロボット装置が、前記第１リンクに対して第２関節を介して旋回可能に連結された第２リンクを更に有していることを特徴とする請求項１又は２に記載のロボット装置の制御方法。
【請求項４】
前記ロボット装置が、前記第１リンクに対して前記第２リンクを互いに反対方向に駆動する一対の一関節駆動型のアクチュエータと、
前記第１リンクと前記第２リンクとを同時に互いに反対方向に駆動する一対の二関節駆動型のアクチュエータと、を更に有することを特徴とする請求項３に記載のロボット装置の制御方法。
【請求項５】
基体に関節を介して旋回可能に連結されたリンクと、
前記基体に対して互いに反対方向に前記リンクを引っ張る駆動力を発生して、駆動力の差により前記関節にトルクを付与すると共に、駆動力の和により前記関節に剛性を付与する一対のアクチュエータと、
前記関節の剛性を目標剛性にすると共に前記関節の関節角度を目標軌道に追従させるのに必要な前記各アクチュエータの各駆動力指令値を求めて、前記各アクチュエータに発生させる駆動力が前記各駆動力指令値となるように、前記各アクチュエータを制御する制御部と、を備え、
前記制御部は、
前記リンクの逆動力学に基づき、前記目標剛性、前記目標軌道、前記目標軌道の角速度、前記目標軌道の角加速度を用いて、前記関節に必要なトルクを示すトルク指令値を算出するトルク指令値算出処理と、
前記トルク指令値算出処理で算出した前記トルク指令値と前記目標剛性とを用いて求められる前記各駆動力指令値が０以上の値を取り得るか否かを判断する判断処理と、
前記判断処理で前記各駆動力指令値のうち少なくとも一方が負の値を取り得ると判断した場合、前記目標剛性を増加する変更、前記目標軌道の角加速度を低減する変更のうち少なくとも一方の変更をして、前記トルク指令値算出処理に戻る変更処理と、
前記判断処理で前記各駆動力指令値が０以上の値を取り得ると判断した場合、前記目標剛性及び前記トルク指令値を用いて、前記各駆動力指令値を算出する駆動力指令値算出処理と、
前記関節角度と前記目標軌道との差分を補償する補正トルク指令値を算出する補正トルク指令値算出処理と、を実行し、
前記駆動力指令値算出処理では、前記各駆動力指令値の算出に用いる前記トルク指令値として、前記トルク指令値算出処理で求めた前記トルク指令値に前記補正トルク指令値算出処理で求めた前記補正トルク指令値を加算補正した結果を用いることを特徴とするロボット装置。
【請求項６】
基体に関節を介して旋回可能に連結されたリンクと、
前記基体に対して互いに反対方向に前記リンクを引っ張る駆動力を発生して、駆動力の差により前記関節にトルクを付与すると共に、駆動力の和により前記関節に剛性を付与する一対のアクチュエータと、
前記関節の剛性を目標剛性にすると共に前記関節の関節角度を目標軌道に追従させるのに必要な前記各アクチュエータの各駆動力指令値を求めて、前記各アクチュエータに発生させる駆動力が前記各駆動力指令値となるように、前記各アクチュエータを制御する制御部と、を備え、
前記制御部は、
前記リンクの逆動力学に基づき、前記目標剛性、前記目標軌道、前記目標軌道の角速度、前記目標軌道の角加速度を用いて、前記関節に必要なトルクを示すトルク指令値を算出するトルク指令値算出処理と、
前記関節角度と前記目標軌道との差分を補償する補正トルク指令値を算出する補正トルク指令値算出処理と、
前記トルク指令値算出処理で求めた前記トルク指令値に前記補正トルク指令値算出処理で求めた前記補正トルク指令値を加算補正して、補正後のトルク指令値を取得し、前記目標剛性及び前記補正後のトルク指令値に基づき、前記各駆動力指令値を算出する駆動力指令値算出処理と、を実行し、
前記駆動力指令値算出処理における算出の結果、前記各駆動力指令値のうち少なくとも１つが前記リンクを押し出す駆動力を発生する値となった場合、前記目標剛性を増加する変更、及び前記目標軌道の角加速度を低減する変更のうち少なくとも一方を実行して、再び、前記トルク指令値算出処理、前記補正トルク指令値算出処理及び前記駆動力指令値算出処理を実行することを特徴とするロボット装置。
【請求項７】
前記リンクが第１リンクであり、
前記関節が第１関節であり、
前記第１リンクに対して第２関節を介して旋回可能に連結された第２リンクを更に有していることを特徴とする請求項５又は６に記載のロボット装置。
【請求項８】
前記第１リンクに対して前記第２リンクを互いに反対方向に駆動する一対の一関節駆動型のアクチュエータと、
前記第１リンクと前記第２リンクとを同時に互いに反対方向に駆動する一対の二関節駆動型のアクチュエータと、を更に有することを特徴とする請求項７に記載のロボット装置。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、手先が対象物に柔軟に接触できるロボット装置の制御方法及びロボット装置に関するものである。好適には、作業ロボットや脚式移動ロボットに適用される。
【背景技術】
【０００２】
マニピュレータの制御方法において、手先が対象物に柔軟に接触できることが重要になってきている。これを産業ロボットに応用すれば、ロボット装置と人間との共同作業が実現でき、手先の柔軟性の方向を制御することにより部品の嵌め合い作業等が容易になる。また、これを脚式移動ロボットに応用すれば、地面に柔らかく接地することで胴体に対する衝撃を和らげることができ、段差を吸収することで不整地を安定に歩行することを可能となる。
【０００３】
手先の柔軟性の制御を実現するために、手先に力センサを装着するインピーダンス制御や、人工筋肉アクチュエータを用いる制御等が行われている。人間の筋はアクチュエータであると同時に、粘弾性可変の制御機構であることが知られている。人工筋肉の中でも、特にＭｃＫｉｂｂｅｎ型人工筋肉に代表される空気圧ゴム人工筋肉は、粘弾性特性が筋に類似している。そこで、マニピュレータに配置されている人工筋肉アクチュエータの柔らかさを制御することで、任意の手先の柔軟性で対象物に接触することができる。しかし、人工筋肉アクチュエータは、粘弾性特性に非線系性を有している、収縮方向のみに力を発生するため拮抗配置して制御を行う必要がある、などの理由で制御性に難があることが知られている。
【０００４】
これに対し、筋の粘弾性特性を含むマニピュレータのモデルと、修正値計算部を用いて関節角度と手先の柔軟性を同時制御するためのフィードフォワード入力を生成する技術が開示されている（特許文献１参照）。特許文献１では、モデルを用いて制御入力が与えられた時の関節角度と人工筋肉アクチュエータの粘弾性係数を出力し、関節角度を目標値と比較する。そして、関節角度と目標値との誤差を修正値計算部に逆伝播させ、フィードフォワード入力を修正する。修正されたフィードフォワード入力を再びモデルに与える操作を繰り返し、徐々にフィードフォワード入力を求めている。
【先行技術文献】
【特許文献】
【０００５】
【特許文献１】特許第３４３６３２０号公報
【発明の概要】
【発明が解決しようとする課題】
【０００６】
上記特許文献１では、フィードフォワード制御により関節角度と手先の柔軟性の同時制御を行っている。しかし、フィードフォワード制御入力は、目標値との誤差を繰り返し修正することによって求められているため、その生成に要する計算量は非常に大きい。そのため、関節角度や手先の柔軟性の目標値を変更する度に、再び繰り返し演算をする必要があり、目標値の変更に時間を要するという問題がある。
【０００７】
そこで、本発明は、簡便なフィードフォワード制御系により短時間でトルク指令値を導出し、関節の角度の制御と手先の柔軟性の制御の同時制御を実現するロボット制御装置の制御方法及びロボット装置を提供することを目的とするものである。
【課題を解決するための手段】
【０００８】
本発明は、基体に関節を介して旋回可能に連結されたリンクと、前記基体に対して互いに反対方向に前記リンクを引っ張る駆動力を発生して、駆動力の差により前記関節にトルクを付与すると共に、駆動力の和により前記関節に剛性を付与する一対のアクチュエータと、を備え、前記関節の剛性を目標剛性にすると共に前記関節の関節角度を目標軌道に追従させるのに必要な前記各アクチュエータの各駆動力指令値を求めて、前記各アクチュエータに発生させる駆動力が前記各駆動力指令値となるように、前記各アクチュエータを制御するロボット装置の制御方法において、前記リンクの逆動力学に基づき、前記目標剛性、前記目標軌道、前記目標軌道の角速度、前記目標軌道の角加速度を用いて、前記関節に必要なトルクを示すトルク指令値を算出するトルク指令値算出工程と、前記トルク指令値算出工程で算出した前記トルク指令値と前記目標剛性とを用いて求められる前記各駆動力指令値が０以上の値を取り得るか否かを判断する判断工程と、前記判断工程で前記各駆動力指令値のうち少なくとも一方が負の値を取り得ると判断した場合、前記目標剛性を増加する変更、前記目標軌道の角加速度を低減する変更のうち少なくとも一方の変更をして、前記トルク指令値算出工程に戻る変更工程と、前記判断工程で前記各駆動力指令値が０以上の値を取り得ると判断した場合、前記目標剛性及び前記トルク指令値を用いて、前記各駆動力指令値を算出する駆動力指令値算出工程と、前記関節の角度と前記目標軌道との差分を補償する補正トルク指令値を算出する補正トルク指令値算出工程と、を備え、前記駆動力指令値算出工程では、前記各駆動力指令値の算出に用いる前記トルク指令値として、前記トルク指令値算出工程で求めた前記トルク指令値に前記補正トルク指令値算出工程で求めた前記補正トルク指令値を加算補正した結果を用いることを特徴とする。
【発明の効果】
【０００９】
本発明によれば、簡便なフィードフォワード制御系により短時間でトルク指令値を導出し、関節の角度の制御と手先の柔軟性の制御の同時制御を実現することができる。
【図面の簡単な説明】
【００１０】
【図１】筋の粘弾性モデルを示す図である。
【図２】本発明の第１実施形態に係るロボット装置の概略構成を示す説明図である。
【図３】本発明の第１実施形態における制御部のブロック線図である。
【図４】本発明の第１実施形態における制御入力の導出方法を示すフローチャートである。
【図５】本発明の第１実施形態におけるシミュレーション結果を示す図である。
【図６】本発明の第１実施形態におけるシミュレーション結果を示す図である。
【図７】本発明の第２実施形態における制御部のブロック線図である。
【図８】本発明の第２実施形態におけるシミュレーション結果を示す図である。
【図９】本発明の第３実施形態に係るロボット装置の概略構成を示す説明図である。
【図１０】本発明の第３実施形態におけるスティフネス楕円を説明する図である。
【図１１】本発明の第３実施形態における制御部のブロック線図である。
【図１２】本発明の第３実施形態における制御部のフィードフォワード制御のブロック線図である。
【図１３】本発明の第３実施形態における軌道設計を示す図である。
【図１４】本発明の第３実施形態におけるシミュレーション結果を示す図である。
【図１５】本発明の第３実施形態におけるシミュレーション結果を示す図である。
【図１６】本発明の第３実施形態におけるシミュレーション結果を示す図である。
【図１７】本発明の第３実施形態におけるシミュレーション結果を示す図である。
【発明を実施するための形態】
【００１１】
以下、本発明を実施するための形態を、図面を参照しながら詳細に説明する。
【００１２】
［第１実施形態］
図１は、本発明の第１実施形態に係るロボット装置に適用されるアクチュエータの粘弾性モデルを示す図である。本第１実施形態では、ロボット装置として、空気圧式の人工筋肉アクチュエータを用いたマニピュレータの関節角度と関節の剛性の同時制御を例として説明する。
【００１３】
（１）モデリング
人工筋肉アクチュエータは、筋の粘弾性と呼ばれる特性と類似する特性を有するアクチュエータである。人工筋肉アクチュエータは、図１に示すように、力発生要素と弾性要素と粘性要素を用いてモデル化される。ここで、力発生要素の収縮力をｕ、収縮方向を正とする筋の収縮量をｘとする。収縮速度は、以下の式となる。
【００１４】
【数１】
0005930753.tif 000002
【００１５】
また、ｋを弾性力定数、ｂを粘性力定数、Ｆを筋収縮力とする。このとき、筋の粘弾性特性は、
【００１６】
【数２】
0005930753.tif 000003
とモデル化され、筋収縮力の弾性力、粘性力は、力発生要素の収縮力ｕに比例するという非線形要素を有する点が特徴的である。
【００１７】
次に、本第１実施形態のロボット装置を図２に示す。図２に示すロボット装置１００は、いわゆる１リンクマニピュレータであり、リンク１０１と、一対のアクチュエータｅ１，ｆ１からなる駆動部１２０と、基体であるベース部材１０３及びベース部材１０３から延びる棒状のアーム部材１０４とを備えている。リンク１０１は、アーム部材１０４に関節１０５を介して旋回可能に連結されている。
【００１８】
なお、本第１実施形態では、この基体として、図２に示すように、ベース部材１０３及びアーム部材１０４で構成される場合について説明するが、これに限定するものではない。例えば、基体が、脚式移動ロボットの胴体や作業ロボットの台座部、リンク１０１に連結される別のリンク等であってもよい。
【００１９】
各アクチュエータｅ１，ｆ１は、一関節筋アクチュエータであり、一端が基体であるベース部材１０３に接続され、他端がリンク１０１の基端１０１ａに接続されて、駆動力（収縮力）の差によりリンク１０１を揺動させるように拮抗配置されている。つまり、アクチュエータｅ１，ｆ１は、リンク１０１を挟んでリンク１０１の両側に対称配置されている。
【００２０】
また、ロボット装置１００は、各アクチュエータｅ１，ｆ１の駆動力を各駆動力指令値により設定して、リンク１０１の動作を制御する制御部１５０を備えている。
【００２１】
各アクチュエータｅ１，ｆ１は、図１に示す空気圧式の人工筋肉アクチュエータである。人工筋肉アクチュエータは収縮方向にのみ力を発生するため、関節１０５を任意の角度に位置決めするために、図２に示すように拮抗して配置されている。つまり、各アクチュエータｅ１，ｆ１は、ベース部材に対して互いに反対方向にリンク１０１を引っ張る駆動力を発生する。各アクチュエータｅ１，ｆ１は、人工筋肉からなる一関節筋アクチュエータであるので、収縮することにより駆動力が発生する。したがって、アクチュエータｅ１，ｆ１の収縮力が駆動力となる。
【００２２】
拮抗配置されたアクチュエータｅ１，ｆ１の力発生要素の駆動力（収縮力）をそれぞれｕｅ１，ｕｆ１、アーム部材１０４に対するリンク１０１の角度、つまり関節１０５の角度をθ、リンク１０１の慣性モーメントをＩとする。また、リンク１０１のモーメントアーム径、つまりリンク１０１の旋回中心点とリンク１０１におけるアクチュエータｅ１，ｆ１の接続点との長さをｒとする。運動方程式は、以下の式（２）となる。
【００２３】
【数３】
0005930753.tif 000004
【００２４】
式（２）の右辺第一項における収縮力ｕｅ１，ｕｆ１の差が関節１０５に回転トルクを与え、右辺第二、第三項における収縮力ｕｅ１，ｕｆ１の和が関節１０５に対する剛性および粘性を変動させることがわかる。つまり、アクチュエータｅ１，ｆ１は、駆動力の差により関節１０５にトルクを付与すると共に、駆動力の和により関節１０５に剛性を付与する。
【００２５】
（２）制御系設計
制御部１５０は、トルク指令値算出処理１５１、判断処理１５２、変更処理１５３及び駆動力指令値算出処理１５５を実行する。本第１実施形態では、駆動力指令値ｕｅ１，ｕｆ１を算出する駆動力指令値算出処理１５５の実行にフィードフォワード制御入力を与えることにより、関節１０５の角度を目標軌道に追従させ、同時に関節１０５の剛性を制御することを目的としている。関節１０５の目標軌道をｒａ、目標剛性をｒｓとする。制御部１５０は、関節１０５の剛性を目標剛性ｒｓにすると共に関節１０５の角度θを目標軌道ｒａに追従させるのに必要な各アクチュエータｅ１，ｆ１の各駆動力指令値ｕｅ１，ｕｆ１を求める。そして、制御部１５０は、各アクチュエータｅ１，ｆ１に発生させる駆動力が駆動力指令値ｕｅ１，ｕｆ１となるように、各アクチュエータｅ１，ｆ１を制御する。
【００２６】
（２．１）フィードフォワード制御系設計
本節では、各アクチュエータｅ１，ｆ１に与えるフィードフォワード入力の導出方法について述べる。まず、以下のようにモーメントアーム径ｒ倍される駆動力指令値ｕｅ１，ｕｆ１の差をＴ１、駆動力指令値ｕｅ１，ｕｆ１の和（合計値）を示す関節剛性指令値をＵ１とする。
【００２７】
（ｕｆ１−ｕｅ１）ｒ＝Ｔ１          （３）
ｕｆ１＋ｕｅ１＝Ｕ１                （４）
【００２８】
これより式（２）は、
【００２９】
【数４】
0005930753.tif 000005
と表され、Ｔ１が関節１０５にトルクを与え、Ｕ１が関節１０５に対する剛性および粘性を増減させることがわかる。ここで、Ｕ１は関節１０５の目標剛性がｒｓであるため、
Ｕ１＝ｒｓ／ｋｒ２          （６）
と決定される。
【００３０】
目標軌道ｒａの角速度、角加速度をそれぞれ、
【００３１】
【数５】
0005930753.tif 000006
あるいは、ｒａ’，ｒａ’’と表す。
【００３２】
関節１０５の角度θが目標軌道ｒａに追従するためのフィードフォワードトルク（トルク指令値）Ｔ１は、リンク１０１の逆動力学により、以下の式（７）で求まる。
【００３３】
【数６】
0005930753.tif 000007
【００３４】
そこで、トルク指令値Ｔ１と駆動力指令値の和である関節剛性指令値Ｕ１とを同時に満たす駆動力指令値を求めるには、式（３）と式（４）をｕｅ１，ｕｆ１について解き、
【００３５】
【数７】
0005930753.tif 000008
と決定すればよい。しかし、人工筋肉アクチュエータｅ１，ｆ１は収縮方向にのみ力を発生するため、
ｕｆ１＞０，ｕｅ１＞０          （９）
という条件を同時に満たさなければならない。式（８）と式（９）より
Ｔ１／ｒ＜Ｕ１，Ｔ１／ｒ＞−Ｕ１          （１０）
という条件が得られる。つまりトルク指令値Ｔ１が、
｜Ｔ１｜＜Ｕ１ｒ          （１１）
という条件を満たす必要がある。
【００３６】
図３に本第１実施形態における制御部１５０のブロック線図を示す。制御部１５０は、トルク指令値算出処理１５１として、リンク１０１の逆動力学に基づき、目標剛性ｒｓ、目標軌道ｒａ、目標軌道の角速度ｒａ’、目標軌道の角加速度ｒａ’’を用いて、関節１０５に必要なトルクを示すトルク指令値Ｔ１を算出する。つまり、制御部１５０は、目標剛性ｒｓから式（６）に従って関節剛性指令値Ｕ１を算出する。次に制御部１５０は、関節剛性指令値Ｕ１、目標軌道ｒａ、目標軌道の角速度ｒａ’、目標軌道の角加速度ｒａ’’を用いて、式（７）に従ってトルク指令値Ｔ１を算出する（トルク指令値算出工程）。
【００３７】
制御部１５０は、判断処理１５２として、トルク指令値算出処理１５１で算出したトルク指令値Ｔ１と目標剛性ｒｓとを用いて求められる各駆動力指令値が０以上の値を取り得るか否かを判断する（判断工程）。より具体的に説明すると、制御部１５０は、式（９）を満たす条件として、式（１１）を満たすか否かを判断する。
【００３８】
制御部１５０は、駆動力指令値算出処理１５５として、判断処理１５２で各駆動力指令値が０以上の値を取り得ると判断した場合、目標剛性ｒｓ及びトルク指令値Ｔ１を用いて、各駆動力指令値ｕｅ１，ｕｆ１を算出する（駆動力指令値算出工程）。
【００３９】
つまり、制御部１５０は、式（１１）を満たすと判断した場合、式（８）により各アクチュエータｅ１，ｆ１に対するフィードフォワード入力、即ち駆動力指令値ｕｅ１，ｕｆ１を求める。これにより、関節１０５の剛性が目標剛性ｒｓとなると同時に、関節１０５にはトルク指令値Ｔ１に対応するトルクが印加され、関節１０５の角度θが目標軌道ｒａに追従する。
【００４０】
制御部１５０は、変更処理１５３として、判断処理１５２で各駆動力指令値のうち少なくとも一方が負の値を取り得ると判断した場合、目標剛性ｒｓを増加する変更、目標軌道の角加速度ｒａ’’を低減する変更のうち少なくとも一方の変更をする（変更工程）。そして、制御部１５０は、変更処理１５３を実行した後、トルク指令値算出処理１５１の実行に戻る（変更工程）。
【００４１】
つまり、制御部１５０は、式（１１）の条件を満たさない場合は、目標軌道の角加速度ｒａ’’の最大値を下げる、もしくは、関節１０５の目標剛性ｒｓを高くするという変更を行い、トルク指令値Ｔ１の導出と式（１１）の確認を再び行う。
【００４２】
ここで、図３中、破線で囲んだＫＦＦＷが式（７）を示している。また、Ｃｈｅｃｋで示す判断処理１５２のブロックは、式（１１）に示したフィードフォワード入力の成立条件を確認するブロックである。
【００４３】
また、Ｃｏｒｒｅｃｔｉｏｎで示す変更処理１５３のブロックは、トルク指令値Ｔ１の導出を再び行うようにするブロックである。Ｃｏｒｒｅｃｔｉｏｎブロックが行う処理を図４のフローチャートに示す。
【００４４】
制御部１５０は、まず、フィードフォワード入力の生成を中止し（Ｓ１）、目標軌道ｒａの変更が可能か否かを判断する（Ｓ２）。ここで、関節剛性指令値Ｕ１を変更する変更幅をξＵ，目標軌道の角加速度ｒａ’’を変更する変更幅をξａとする。
【００４５】
制御部１５０は、目標軌道ｒａの変更が可能と判断した場合（Ｓ２：Ｙｅｓ）、角加速度ｒａ’’から変更幅ξａを減じて角加速度ｒａ’’を変更する（Ｓ４）。次に、制御部１５０は、この角加速度ｒａ’’の変更により目標軌道ｒａを再生成する（Ｓ５）。そして、制御部１５０は、フィードフォワード入力の再計算を行う（Ｓ６）。
【００４６】
制御部１５０は、目標軌道ｒａの変更が可能ではないと判断した場合（Ｓ２：Ｎｏ）、関節剛性指令値Ｕ１から変更幅を増加させて関節剛性指令値Ｕ１を変更する（Ｓ７）。そして、制御部１５０は、フィードフォワード入力の再計算を行う（Ｓ６）。
【００４７】
変更幅ξＵもしくはξａを例えば０．５Ｕ１や０．５ｒａ’’というような大きな値に設定すれば、式（７）に示したフィードフォワード入力の導出と図４に示した処理を数回試行することで、短時間でフィードフォワード入力を生成することが可能である。もしくは、式（１１）の右辺と左辺を同一図上に図示し、成立条件を確認しながらξＵもしくはξａの調整を行うことで、短時間でフィードフォワード入力を生成することが可能である。
【００４８】
なお、本第１実施形態では、関節剛性指令値Ｕ１、即ち目標剛性ｒｓ及び角加速度ｒａ’’のいずれか一方を変更するようにしたが、両方を変更するようにしてもよい。
【００４９】
以上の導出は、筋の弾性力がθ＝０ｄｅｇからの角度に比例して発生することを仮定していた。任意の角度を筋の弾性力発生の基準となる角度（以下、中立角度と記す）としてフィードフォワード制御入力を生成するためには、θｃを中立角度とすると、式（７）に代わり、
【００５０】
【数８】
0005930753.tif 000009
とすることで求まる。
【００５１】
（２．２）軌道設計
次に、関節１０５の目標軌道ｒａ、目標剛性ｒｓの設計方法を示す。本第１実施形態では、目標軌道ｒａは図５（ａ）に示すような、加速区間−等速区間−減速区間を有する軌道を用いる。ここで、ｒａＳ，ｒａＦはそれぞれ、軌道の初期角度、目標角度である。また、ｔａ，ｔｂ，ｔｆｉｎはそれぞれ、等速区間の開始時間、等速区間の終了時間、位置決め終了時間である。さらに、目標剛性ｒｓは図５（ｂ）に示すように、加減速区間で高剛性ｒｓｍａｘをとなり、等速間で低剛性ｒｓｍｉｎとなるように設定する。剛性を可変させることにより、マニピュレータは駆動開始時は筋の弾性力により加速され、等速区間では低剛性となることによりほぼ慣性力で駆動される。これにより、少ない筋の収縮力で駆動することが可能となる。
【００５２】
本第１実施形態では、加速区間−等速区間−減速区間を有する軌道を用いたが、例えばミニマムジャーク軌道のような等速区間を伴わない軌道であっても構わない。また、駆動の効率を考慮しない場合は、目標剛性を一定としても構わない。
【００５３】
（３）シミュレーション
前節の制御系を用いるシミュレーションを行った。リンク１０１の慣性モーメントをＩ＝８．３×１０−２ｋｇｍ２、モーメントアーム径をｒ＝０．１ｍ、弾性、粘性定数をｋ＝２５，ｂ＝３とする。目標軌道ｒａは、初期角度ｒａＳ＝−２０ｄｅｇ、目標角度ｒａＦ＝２０ｄｅｇ、等速区間の開始時間ｔａ＝０．４秒、等速区間の終了時間ｔｂ＝０．６秒、位置決め終了時間ｔｆｉｎ＝１秒とする。関節の目標剛性はｒｓｍａｘ＝２．３ｋｒ２Ｎｍ／ｒａｄとし、ｒｓｍｉｎ＝０．８ｋｒ２Ｎｍ／ｒａｄとする。ここで、ｋ，ｒは定数であるので、収縮力の和Ｕ１を、加減速区間でＵ１＝２．３、等速区間で、Ｕ１＝０．８となるように制御すればよい。ここで、Ｕ１は図４に示したフローにより、式（１１）に示した条件を満たすように設計されている。
【００５４】
図６（ａ）に関節１０５の角度θと目標軌道ｒａを、それぞれ実線、破線で、図６（ｂ）に駆動力指令値ｕｅ１，ｕｆ１の応答をそれぞれ実線、破線で示す。さらに、図６（ｃ）に駆動力指令値ｕｅ１，ｕｆ１の差を実線で、和を破線で、Ｔ１／ｒの応答を一点鎖線で示す。
【００５５】
図６（ａ）より実線で示す関節角度は破線で示す軌道と重なっており、関節は目標軌道に追従していることがわかる。図６（ｂ）より、駆動力指令値ｕｅ１，ｕｆ１は、フィードフォワード制御系の制御入力Ｔ１が式（８）と式（１１）に示した条件で配分されて求められるため、人工筋肉の収縮力は正の値のみを取るという特性を満たしていることがわかる。
【００５６】
図６（ｃ）において、実線で示すｕｆ１−ｕｅ１は、一点鎖線で示すＴ１／ｒと重なっており、式（３）の条件を満たしている。また、破線で示すｕｆ１＋ｕｅ１＝Ｕ１は加減速区間で２．３、等速区間で０．８となっており、関節の剛性は目標値に制御されていることがわかる。本第１実施形態の手法により、目標軌道への追従を実現しながら、自在に関節の剛性を変化させるフィードフォワード入力（トルク指令値）を簡便に生成可能であることがわかる。
【００５７】
［第２実施形態］
次に、本発明の第２実施形態に係るロボット装置について説明する。本第２実施形態では、制御部の制御動作が上記第１実施形態の制御部の制御動作と異なるものである。なお、ロボット装置本体の構成について、上記第１実施形態で説明した図１のロボット装置本体の構成と同様の部分については、同一符号を付して詳細な説明を省略する。
【００５８】
本第２実施形態では、上記第１実施形態で示したフィードフォワード制御系とフィードバック制御系を併合する２自由度制御系を導出する。フィードバック制御系は、フィードフォワード制御系と同様に、目標軌道への追従と関節剛性を同時に制御可能とするように導出する。フィードフォワード制御系のみでは、リンク１０１の慣性モーメントなどのパラメータに同定誤差が生ずると制御性能が低下する。しかし、フィードバック制御系との２自由度制御系を構成することにより、モデル誤差が存在していても目標軌道への追従と関節角度の同時制御が可能であることを示す。
【００５９】
（１）モデリング
本第２実施形態で用いるモデルは上記第１実施形態と同じとする。
【００６０】
（２）制御系設計
フィードフォワード制御系の導出は上記第１実施形態と同様である。本第２実施形態では、式（７）により導出される関節トルクをＴＦＦＷ１と記す。
【００６１】
図７に本第２実施形態に係る制御部１５０Ａのブロック線図を示す。図７では、図３で示したフィードフォワード制御系の破線で囲まれた部分を、ＫＦＦＷ１として示している。
【００６２】
制御部１５０Ａは、フィードフォワード制御系ＫＦＦＷ１の他、更に、フィードバック制御系である、補正トルク指令値算出処理１５４としてのＰＩＤ制御処理ＫＰＩＤ１を実行する機能を有している。制御部１５０Ａは、ＰＩＤ制御処理ＫＰＩＤ１として、関節１０５の角度θと目標軌道ｒａとの差分を補償する補正トルク指令値としての制御入力トルクＴＦＢ１を算出する（補正トルク指令値算出工程）。
【００６３】
ここで、式（２）より、
（ｕｆ１−ｕｅ１）ｒ＝ＴＦＦＷ１＋ＴＦＢ１＝Ｔ′１          （１３）
を満たすように駆動力指令値ｕｅ１，ｕｆ１を決定すれば、関節１０５にフィードフォワード制御トルクＴＦＦＷ１とフィードバック制御トルクＴＦＢ１との和であるＴ′１を与えることができる。トルク指令値Ｔ′１は、フィードフォワード制御系ＫＦＦＷ１のトルク指令値算出処理１５１（図３参照）で求めたトルク指令値ＴＦＦＷ１にＰＩＤ制御処理ＫＰＩＤ１で求めた制御入力トルクＴＦＢ１を加算補正した演算結果である。
【００６４】
したがって、本第２実施形態では、駆動力指令値算出処理１５５を実行する際に、各駆動力指令値の算出に用いるトルク指令値として、トルク指令値Ｔ′１を用いる。
【００６５】
ところで、上記第１実施形態と同様に、同時に関節１０５の剛性に関する条件
Ｕ１＝ｕｆ１＋ｕｅ１＝ｒｓ／ｋｒ２          （１４）
を満たすように、駆動力指令値ｕｅ１，ｕｆ１を決定する必要がある。
【００６６】
トルク指令値Ｔ′１と関節剛性指令値Ｕ１を同時に満たすには、式（１３）と式（１４）を駆動力指令値ｕｅ１，ｕｆ１について解き、
【００６７】
【数９】
0005930753.tif 000010
と決定すればよい。しかし、人工筋肉アクチュエータｅ１，ｆ１は収縮方向にのみ力を発生するため、上記第１実施形態と同様に、Ｔ′１が、
−Ｕ１ｒ＜Ｔ′１＜Ｕ１ｒ          （１６）
という条件を満たす必要がある。これにより、関節１０５の剛性が目標剛性ｒｓとなると同時に、トルク指令値Ｔ′１により目標角度ｒａに位置決めされる。式（１６）を実現するには、制御部１５０Ａは、図７のブロック線図に示すように、
｜Ｔ′１｜＜Ｕ１ｒ          （１７）
と制御入力を制約する制約処理１５６を実行すればよい。もしくは、ＰＩＤ制御処理ＫＰＩＤ１のゲインを変動させ、式（１６）の範囲に制御入力の大きさを収めるなどの方法がある。
【００６８】
しかし、フィードバック制御系のゲインが高い場合には、それによる関節１０５の剛性が支配的になってしまう。
【００６９】
そこで、制御部１５０Ａは、図７に示すように、接触ゲインＧｔを導入し、接触が発生した場合は、Ｇｔ＝０とすることでフィードバック系を遮断する。これにより、手先が人や物に接触したことを検知するなどの任意の時刻で、速やかにリンク１０１（関節１０５）の剛性を目標剛性ｒｓへと切り替えることができる。
【００７０】
本第２実施形態では、ＰＩＤ制御処理ＫＰＩＤ１に用いる伝達関数を、
【００７１】
【数１０】
0005930753.tif 000011
としている。
【００７２】
（３）シミュレーション
前節で導出した制御系を用いてシミュレーションを行った。リンク１０１のパラメータと目標軌道ｒａは上記第１実施形態と同様とする。本第２実施形態では、モデルの同定誤差に対する検証を行うため、リンク１０１の慣性モーメントをＩ′＝１．０５Ｉとし、フィードフォワード入力生成時のモデルに対して誤差を有するモデルを用いてシミュレーションを行った。
【００７３】
図８（ａ）に関節１０５の角度θと目標軌道ｒａを、それぞれ実線、破線で、図８（ｂ）に駆動力指令値ｕｅ１，ｕｆ１の応答をそれぞれ実線、破線で示す。さらに、図８（ｃ）に駆動力指令値ｕｅ１，ｕｆ１の差を実線で、和を破線で、Ｔ′１／ｒの応答を一点鎖線で示す。また、図８（ｄ）にトルク指令値ＴＦＦＷ１，ＴＦＢ１をそれぞれ実線、破線で示す。
【００７４】
図８（ａ）より、実線で示す関節１０５の角度θは破線で示す目標軌道ｒａと重なっており、関節１０５は目標軌道ｒａに追従していることがわかる。フィードフォワード制御系のみでは、モデル誤差の存在により軌道に追従することはできない。しかし、図８（ｄ）の破線で示すように、フィードバック制御入力がモデル誤差を補償しているため、追従することを可能としている。図８（ｂ）より、駆動力指令値ｕｅ１，ｕｆ１は２自由度制御系の制御入力Ｔ′１が、式（１５）と式（１７）に示した条件で配分されているため、人工筋肉の収縮力は正の値のみを取るという特性を満たしていることがわかる。図８（ｃ）において、実線で示すｕｆ１−ｕｅ１は、一点鎖線で示すＴ′１／ｒと重なっており、式（１３）の条件を満たしている。また、破線で示すｕｆ１＋ｕｅ１＝Ｕ１は、上記第１実施形態と同様に、加減速区間で２．３、等速区間で０．８となっており、関節１０５の剛性は目標剛性ｒｓに制御されていることがわかる。本第２実施形態で示した２自由度制御系により、モデルに同定誤差などが存在しても、目標軌道ｒａへの追従を実現しながら、自在に関節１０５の剛性を変化させることが可能であることがわかる。
【００７５】
［第３実施形態］
次に、本発明の第３実施形態に係るロボット装置について詳細に説明する。図９は、本発明の第３実施形態に係るロボット装置の概略構成を示す説明図である。本第３実施形態では、人工筋肉アクチュエータを用いた３対６筋を有する２リンクマニピュレータとしてのロボット装置２００の手先剛性の制御を例として説明する。
【００７６】
（１）モデリング
図９に示すロボット装置２００は、プーリ２０３と、プーリ２０３に対して旋回可能に第１関節２１１を介して連結された第１リンク２０１と、第１リンク２０１に対して旋回可能に第２関節２１２を介して連結された第２リンク２０２とを備えている。
【００７７】
第１リンク２０１は、長手部材からなり、その基端２０１ａがプーリ２０３にｘ−ｙ直交座標系の平面（以下、「作業平面」という）内で旋回可能に支持されている。第２リンク２０２は、長手部材からなり、その基端２０２ａが第１リンク２０１の先端２０１ｂに作業平面内で旋回可能に支持されている。
【００７８】
第２リンク２０２の先端（以下、「リンク先端」という）２０２ｂには、不図示のエンドエフェクタ（例えばハンド）が設けられている。つまり、第１リンク２０１は、第１関節２１１と第２関節２１２との間に配置され、第１関節２１１で旋回可能に支持されており、第２リンク２０２は、第２関節２１２で旋回可能に支持されている。
【００７９】
なお、本第２実施形態では、第１リンク２０１に対しては、プーリ２０３が基体となり、第２リンク２０２に対しては、第１リンク２０１が基体となる。このプーリ２０３は、例えばロボット胴体等に設けられている。
【００８０】
ロボット装置２００は、一対の第１アクチュエータｅ１，ｆ１と、一対の第２アクチュエータｅ２，ｆ２と、一対の第３アクチュエータｅ３，ｆ３と、を備えている。各第１アクチュエータｅ１，ｆ１は、一端がプーリ２０３に接続され、他端が第１リンク２０１の長手方向中央部に接続され、駆動力の差により第１リンク２０１を旋回させるように拮抗配置されている。
【００８１】
また、各第２アクチュエータｅ２，ｆ２は、一端が第１リンク２０１の長手方向中央部に接続され、他端が第２リンク２０２の基端２０２ａに接続され、駆動力の差により第２リンク２０２を旋回させるように拮抗配置されている。また、各第３アクチュエータｅ３，ｆ３は、一端がプーリ２０３に接続され、他端が第２リンク２０２の基端２０２ａに接続され、駆動力の差により第１リンク２０１及び第２リンク２０２を旋回させるように拮抗配置されている。つまり、第１アクチュエータｅ１，ｆ１は、第１リンク２０１を挟んで第１リンク２０１の両側に対称配置されている。また、第２アクチュエータｅ２，ｆ２は、第１リンク２０１を挟んで第１リンク２０１の両側に対称配置されている。また、第３アクチュエータｅ３，ｆ３は、第１リンク２０１を挟んで第１リンク２０１の両側に対称配置されている。
【００８２】
また、ロボット装置２００は、各アクチュエータｅ１，ｆ１，ｅ２，ｆ２，ｅ３，ｆ３の駆動力を各駆動力指令値により設定して、リンク２０１，２０２の動作を制御する制御部２５０を備えている。
【００８３】
第１アクチュエータｅ１，ｆ１は、第１リンク２０１を駆動する第１の一関節駆動アクチュエータである。第２アクチュエータｅ２，ｆ２は、第２リンク２０２を駆動する第２の一関節駆動アクチュエータである。また、第３アクチュエータｅ３，ｆ３は、第１リンク２０１と第２リンク２０２を同時に駆動する二関節同時駆動アクチュエータである。人の上腕部や下肢大腿部には、二関節筋とよばれる二関節同時駆動アクチュエータが存在することが知られている。人の四肢の筋配列は複雑だが、実効筋概念が導入され、３対６筋を有する２リンクモデルが提示されている。
【００８４】
各アクチュエータｅ１，ｆ１，ｅ２，ｆ２，ｅ３，ｆ３は、図１に示す筋の粘弾性特性を有する人工筋肉アクチュエータである。人工筋肉アクチュエータは、筋の粘弾性と呼ばれる特性と類似する特性を有するアクチュエータである。筋は図１に示すように、力発生要素と弾性要素と粘性要素を用いてモデル化される。
【００８５】
図９のアクチュエータｅ１，ｆ１，ｅ２，ｆ２，ｅ３，ｆ３について、ｕｅｎ，ｕｆｎ（ｎ＝１，２，３）を力発生要素の駆動力を発生させる駆動力指令値とする。また、ｋｅｎ，ｋｆｎ，ｂｅｎ，ｂｆｎ（ｎ＝１，２，３）を人工筋肉アクチュエータの弾性力定数、粘性力定数とする。第１，第２リンク２０１，２０２の角度、つまり関節の角度をθ１，θ２、第１，第２リンク２０１，２０２の慣性モーメントをＩ１，Ｉ２、第１，第２リンク２０１，２０２の長さを２×ｌ１，２×ｌ２、第１，第２リンク２０１，２０２の質量をｍ１，ｍ２とする。
【００８６】
モーメントアーム径、つまり第１リンク２０１の旋回中心点とプーリ２０３におけるアクチュエータｅ１，ｆ１の接続点との長さ、及び第２リンク２０２の旋回中心点と第２リンク２０２におけるアクチュエータｅ２，ｆ２の接続点との長さをｒとする。
【００８７】
本第３実施形態では、各筋の弾性力定数，粘性力定数をいずれもｋ，ｂとすると、運動方程式は、以下の式（１９）及び式（２０）となる。
【００８８】
【数１１】
0005930753.tif 000012
【００８９】
【数１２】
0005930753.tif 000013
となる。
【００９０】
（２）制御系設計
上記第１及び第２実施形態では、関節の角度と剛性の同時制御を行ったが、本第３実施形態においても、２自由度制御系を用いて関節の角度と剛性の同時制御を行う。しかし、２リンクマニピュレータでは、手先が外界と直に接触するので、手先の剛性を制御することが重要である。手先の剛性は、図１０に示すようなスティフネス楕円によって表される。この楕円は、各方向に対しての剛性の分布を示し、手先と楕円の距離が離れるほど剛性が高いことを表している。
【００９１】
図１１に本第３実施形態の制御部２５０のブロック線図を示す。ＫＦＦＷで示すブロックがフィードフォワード制御系であり、設計方法を以下に示す。
【００９２】
（２．１）フィードフォワード制御系
上記第１実施形態と同様に、収縮力の差をＴｎ、和をＵｎとおくと、
（ｕｆｎ−ｕｅｎ）ｒ＝ＴＦＦＷｎ＋ＴＦＢｎ＝Ｔｎ，ｎ＝１，２，３      （２１）
ｕｆｎ＋ｕｅｎ＝Ｕｎ，ｎ＝１，２，３          （２２）
と表せる。ここで、ＴＦＦＷｎ，ＴＦＢｎは、それぞれフィードフォワード制御系、フィードバック制御系により与えられるトルク指令値である。本節では、ＴＦＢｎ＝０としてフィードフォワード制御入力の導出方法を示す。
【００９３】
式（１９）及び式（２０）をθｖ＝［θ１  θ２］Ｔ，Ｔｖ＝［Ｔ１＋Ｔ３  Ｔ２＋Ｔ３］Ｔと定義して行列表記すると、
【００９４】
【数１３】
0005930753.tif 000014
となる。ここで、Ｋｖ，Ｃｖはそれぞれ筋の粘弾性による剛性行列、減衰行列であり、
【００９５】
【数１４】
0005930753.tif 000015
である。また、θｃｖは、関節トルクＴ１，Ｔ２が作用しないときの筋の弾性力による中立姿勢角度θｃ１，θｃ２からなるベクトルであり、
θｃｖ＝［θｃ１  θｃ２］Ｔ          （２５）
となる。
【００９６】
二関節同時駆動アクチュエータは、駆動力指令値ｕｅ３，ｕｆ３に差を与えることによって、一関節駆動アクチュエータと同様に関節にトルクを与えることは可能である。しかし、一つのリンクを複数のフィードバック制御系が制御することは冗長であり、フィードバック制御系の設計が複雑になってしまう。そこで、本第３実施形態では、二関節同時駆動アクチュエータｅ３，ｆ３は関節に剛性を与えるために用いることとし、収縮力ｕｅ３，ｕｆ３の差Ｔ３を、Ｔ３＝０（式（２６））とする。これにより、Ｔ１，Ｔ２が関節にトルクを与え、Ｕ１，Ｕ２，Ｕ３が関節に対する剛性および粘性を増減させることがわかる。本第３実施形態では、手先のスティフネス楕円を考慮してＵ１，Ｕ２，Ｕ３を決定するが、詳細は２．３節で示す。
【００９７】
制御部２５０は、トルク指令値算出処理２５１、判断処理２５２、変更処理２５３、補正トルク指令値算出処理２５４及び駆動力指令値算出処理２５５を実行する。本第３実施形態では、制御部２５０は、関節２１１，２１２の剛性を目標剛性にすると共に関節２１１，２１２の角度を目標軌道に追従させるのに必要な各アクチュエータｅｎ，ｆｎの各駆動力指令値ｕｅｎ，ｕｆｎを求める。そして、制御部２５０は、各アクチュエータｅｎ，ｆｎに発生させる駆動力が駆動力指令値ｕｅｎ，ｕｆｎとなるように、各アクチュエータｅｎ，ｆｎを制御する。
【００９８】
関節角度θｎ（ｎ＝１，２）に対する目標軌道をｒａｎ（ｎ＝１，２）とし、
ｒａｖ＝［ｒａ１  ｒａ２］Ｔ          （２７）
と定義すると、関節角度が目標軌道に追従するためのフィードフォワード入力ＴＦＦＷ＝［ＴＦＦＷ１  ＴＦＦＷ２］Ｔは、目標軌道の角速度、角加速度をそれぞれ、
【００９９】
【数１５】
0005930753.tif 000016
と表すと、逆動力学により、
【０１００】
【数１６】
0005930753.tif 000017
と求まる。次に、上記第１実施形態と同様に、収縮方向のみに力を発生するという条件、
｜ＴＦＦＷｎ｜＜Ｕｎｒ，ｎ＝１，２          （２９）
の成立を確認する。
【０１０１】
（２．２）２自由度制御系
図１１に示すフィードバック制御系である、補正トルク指令値算出処理２５４としてのＰＩＤ制御処理ＫＰＩＤ１，ＫＰＩＤ２を行う。制御部２５０は、ＰＩＤ制御処理ＫＰＩＤ１，ＫＰＩＤ２として、関節角度θ１，θ２と目標軌道ｒａ１，ｒａ２との差分を補償するための制御入力トルクＴＦＢ１，ＴＦＢ２を算出する。
【０１０２】
そして、制御入力Ｔｎ（ｎ＝１，２）に対して上記第２実施形態と同様に、
｜Ｔｎ｜＜Ｕｎｒ，ｎ＝１，２          （３０）
と制約する。収縮力ｕｆｎ，ｕｅｎ（ｎ＝１，２）は、
【０１０３】
【数１７】
0005930753.tif 000018
となる。また、Ｔ３＝０であるため、図１１のブロック線図に示すように、収縮力ｕｅ３，ｕｆ３は、
ｕｆ３＝ｕｅ３＝Ｕ３／２          （３２）
となる。
【０１０４】
フィードフォワード制御のブロック線図を図１２に示す。制御部２５０は、トルク指令値算出処理２５１として、リンク２０１，２０２の逆動力学に基づき、目標剛性、目標軌道、目標軌道の角速度、目標軌道の角加速度を用いて、関節２１１，２１２に必要なトルクを示すトルク指令値ＴＦＦＷを算出する。つまり、制御部２５０は、目標剛性から関節剛性指令値Ｕｎを算出する。次に制御部２５０は、関節剛性指令値、目標軌道、目標軌道の角速度、目標軌道の角加速度を用いて、式（２８）に従ってトルク指令値ＴＦＦＷを算出する（トルク指令値算出工程）。
【０１０５】
制御部２５０は、判断処理２５２として、トルク指令値算出処理２５１で算出したトルク指令値ＴＦＦＷと目標剛性とを用いて求められる各駆動力指令値ｕｅｎ，ｕｆｎが０以上の値を取り得るか否かを判断する（判断工程）。
【０１０６】
まず、ＴＦＢｎ＝０としてフィードフォワード制御のみを行う場合について説明する。つまり、Ｔｎ＝ＴＦＦＷｎである。
【０１０７】
制御部２５０は、駆動力指令値算出処理２５５（図１１）として、判断処理２５２で各駆動力指令値が０以上の値を取り得ると判断した場合、目標剛性及びトルク指令値Ｔｎを用いて、各駆動力指令値ｕｅｎ，ｕｆｎを算出する（駆動力指令値算出工程）。
【０１０８】
つまり、制御部２５０は、式（２９）を満たすと判断した場合、式（３１）により駆動力指令値ｕｅｎ，ｕｆｎを求める。これにより、関節２１１，２１２の剛性が目標剛性となると同時に、関節２１１，２１２にはトルク指令値Ｔ１，Ｔ２に対応するトルクが印加され、関節２１１，２１２の角度θ１，θ２が目標軌道に追従する。
【０１０９】
制御部２５０は、変更処理２５３として、判断処理２５２で各駆動力指令値のうち少なくとも一方が負の値を取り得ると判断した場合、目標剛性を増加する変更、目標軌道の角加速度を低減する変更のうち少なくとも一方の変更をする（変更工程）。そして、制御部２５０は、変更処理２５３を実行した後、トルク指令値算出処理２５１の実行に戻る（変更工程）。
【０１１０】
つまり、制御部２５０は、式（２９）の条件を満たさない場合は、上記第１実施形態と同様、図４に示したフローチャートを用いて、目標剛性、目標軌道の角加速度
【０１１１】
【数１８】
0005930753.tif 000019
を変更する。
【０１１２】
また、フィードフォワード制御と共にフィードバック制御も行う場合は、制御部２５０は、ＰＩＤ制御処理ＫＰＩＤ１，ＫＰＩＤ２を実行する（補正トルク指令値算出工程）。
【０１１３】
そして、制御部２５０は、トルク指令値Ｔｎとして、フィードフォワード入力ＴＦＦＷｎに制御入力トルクＴＦＢｎを加算補正した結果を用いる。つまり、制御部２５０は、式（２１）の演算を行う。その際、制御部２５０は、上記第２実施形態と同様、制約処理２５６として式（３０）で制約し、収縮力ｕｆｎ，ｕｅｎを求める。
【０１１４】
（２．３）手先剛性制御
上記第１実施形態の１リンクマニュピレータでは、関節の剛性と手先の剛性は同一であった。３対６筋を有する２リンクマニュピレータでは、収縮力の和である関節剛性指令値Ｕ１，Ｕ２，Ｕ３を制御することにより手先の剛性が求まる。例えば、関節剛性指令値Ｕ１，Ｕ２，Ｕ３を
Ｕ１＝Ｕ２＝Ｕ３          （３３）
と制御すると、スティフネス楕円の長軸は第１関節２１１と手先を結ぶ方向を向くことが知られている。本第３実施形態では、新たに手先のスティフネス楕円の長短軸がｘ−ｙ軸と平行になる関節剛性指令値Ｕ１，Ｕ２，Ｕ３を求める。
【０１１５】
ｘ−ｙ軸に平行な微小な外力ΔＦｘ，ΔＦｙによるリンク先端の微小な変位をΔｘｔ，Δｙｔとする。また、微小な外力にΔＦｘ，ΔＦｙよる、リンクの微小な回転角度をΔθ１，Δθ２とする。微小な回転が起こると、筋の粘弾性を有するアクチュエータは、筋の弾性力によりリンクに微小なトルクΔＴｐ１，ΔＴｐ２を発生させ、以下のような式で表せる。
ΔＴｐ１＝｛Ｕ１Δθ１＋Ｕ３（Δθ１＋Δθ２）｝ｋｒ２          （３４）
ΔＴｐ２＝｛Ｕ２Δθ２＋Ｕ３（Δθ１＋Δθ２）｝ｋｒ２          （３５）
【０１１６】
これを行列で表記し、微小な角度とトルクの関係は２．１節で用いた剛性行列Ｋｖを用いて、
【０１１７】
【数１９】
0005930753.tif 000020
となる。ここで、ヤコビ行列Ｊを導入する。
【０１１８】
【数２０】
0005930753.tif 000021
【０１１９】
これより、微小手先変位と微小外力との関係をコンプライアンス行列ＪＳで表すことができる。
【０１２０】
【数２１】
0005930753.tif 000022
【０１２１】
コンプライアンス行列の逆行列が剛性を表すので、スティフネス楕円がｘ−ｙ軸に平行であるという条件は、
Ｊｓ（１，２）＝Ｊｓ（２，１）＝０          （３９）
となる。この方程式を収縮力の和Ｕ３について解くと、
【０１２２】
【数２２】
0005930753.tif 000023
となる。
【０１２３】
（２．４）軌道設計
本第３実施形態では、目標軌道をマニピュレータの手先が図１３（ａ）の実線に示すようにｘ軸に平行で、図中の矢印方向に駆動するものとする。さらに上記第１実施形態と同様に加速区間−等速区間−減速区間を有するものとする。そして、関節角度に対する目標軌道ｒａ１，ｒａ２は、手先軌道から逆運動学により求める。
【０１２４】
つぎに、目標剛性は、図１３（ａ）の破線に示すように、常にｘ−ｙ軸に平行な手先スティフネス楕円となるように設定する。図１３（ａ）のスティフネス楕円は、ｔｆｉｎ／１０秒毎に図示している。また、上記第１実施形態と同様に、加減速区間で高剛性となり、等速間で低剛性となるように設定する。加減速区間での一関節アクチュエータの収縮力の和をＵ１ｍａｘ，Ｕ２ｍａｘ、等速区間での一関節アクチュエータの収縮力の和をＵ１ｍｉｎ，Ｕ２ｍｉｎとする。二関節同時駆動アクチュエータの収縮力の和Ｕ３は、加減速区間ではＵ１ｍａｘ，Ｕ２ｍａｘを用いて、等速間ではＵ１ｍｉｎ，Ｕ２ｍｉｎを用いて、式（４０）により導出する。図１３（ｂ）に
Ｕ１ｍａｘ＝３，Ｕ２ｍａｘ＝１．７１，Ｕ１ｍｉｎ＝１，Ｕ２ｍｉｎ＝０．５７    （４１）
として求められたＵ３を示す。
【０１２５】
本第３実施形態では、加速区間−等速区間−減速区間を有する軌道を用いたが、例えばミニマムジャーク軌道のような等速区間を伴わない軌道であっても構わない。また、駆動の効率を考慮しない場合は、目標剛性を一定としても構わない。
【０１２６】
（３）シミュレーション
前節の制御系を用いたシミュレーションを行う。第１リンク２０１と第２リンク２０２の物理パラメータは同一とする。リンク長さを０．２ｍ、リンクの慣性モーメントをＩ１＝Ｉ２＝１．３×１０−３ｋｇｍ２、モーメントアーム径を０．０５ｍ、弾性、粘性定数をｋ＝１２，ｂ＝０．００３とする。目標軌道は、等速区間の開始時間ｔａ＝０．２７４７秒、等速区間の終了時間ｔｂ＝０．４７４６秒、位置決め終了時間ｔｆｉｎ＝０．７５秒とする。また、関節トルクＴ１，Ｔ２が作用しないときの弾性力による中立姿勢角度θｃ１，θｃ２は、手先軌道の中間地点における関節角度とし、θｃ１＝２９．７ｄｅｇ、θｃ２＝１２０．７ｄｅｇとする。手先のスティフネス楕円がｘ−ｙ軸と水平になるように、各人工筋肉アクチュエータの収縮力の和Ｕ１，Ｕ２，Ｕ３を制御する。さらに、本第３実施形態では、上記第２実施形態と同様に、２自由度制御系のモデルの同定誤差に対する検証を行うため、フィードフォワード入力の生成では慣性モーメントをＩ１，Ｉ２とする。シミュレーションではリンクの慣性モーメントをＩ１′＝１．１Ｉ１，Ｉ２′＝１．１Ｉ２とする。
【０１２７】
図１４（ａ）及び図１４（ｂ）に第１、第２リンクの関節角度θ１，θ２を実線で、目標軌道ｒａ１，ｒａ２を破線で示す。図１５（ａ）〜図１５（ｃ）に収縮力ｕｆ１，ｕｆ２，ｕｆ３の応答を実線で、収縮力ｕｅ１，ｕｅ２，ｕｅ３の応答を破線で示す。さらに、図１６（ａ）〜図１６（ｃ）に収縮力ｕｅｎ，ｕｆｎ（ｎ＝１，２，３）の差を実線で、和を破線で、Ｔｎ／ｒ（ｎ＝１，２）の応答を一点鎖線で示す。また、図１７（ａ）にＴＦＦＷ１，ＴＦＢ１を、図１７（ｂ）にＴＦＦＷ２，ＴＦＢ２をそれぞれ実線、破線で示す。
【０１２８】
図１４（ａ）及び図１４（ｂ）より実線で示す関節角度は破線で示す軌道と重なっており、リンクは目標軌道に追従していることがわかる。上記第２実施形態と同様、図１７（ａ）及び図１７（ｂ）の破線で示すように、フィードバック制御入力がモデル誤差を補償しているためである。図１５（ａ）より、収縮力ｕｅ１，ｕｆ１は２自由度制御系の制御入力Ｔ１を、式（３０）と式（３１）に示した条件で配分されているため、人工筋肉の収縮力は正の値を取るという特性を満たしていることがわかる。図１５（ｂ）より、収縮力ｕｅ２，ｕｆ２も同様に正の値を取っていることがわかる。図１５（ｃ）より、収縮力ｕｅ３，ｕｆ３はＵ３／２である。
【０１２９】
図１６（ａ）において、実線で示すｕｆ１−ｕｅ１は、一点鎖線で示すＴ１／ｒと重なっており、式（２１）の条件を満たしている。また、破線で示すｕｆ１＋ｕｅ１＝Ｕ１は２．４節で設定したとおり、加減速区間でＵ１ｍａｘをとり、等速間ではＵ１ｍｉｎとなっていることがわかる。同様に、図１６（ｂ）において、実線で示すｕｆ２−ｕｅ２は、一点鎖線で示すＴ２／ｒと重なっており、式（２１）の条件を満たしている。また、破線で示すｕｆ２＋ｕｅ２＝Ｕ２は加減速区間でＵ２ｍａｘをとり、等速間ではＵ２ｍｉｎとなっていることがわかる。さらに、図１６（ｃ）において、実線で示すｕｆ３−ｕｅ３は０であり、式（２６）の条件を満たしている。また、破線で示すｕｆ３＋ｕｅ３＝Ｕ３は図１３（ｂ）に示した目標値と一致していることがわかる。
【０１３０】
各拮抗対の剛性が目標値に制御されていることから、手先のスティフネス楕円は目標軌道において、ｘ−ｙ軸と平行になるように制御されていることがわかる。本第３実施形態の２自由度制御系により、モデルに同定誤差などが存在する場合でも、目標軌道への追従を実現しながら、３対６筋の弾性により手先の剛性を制御することが可能であることがわかる。
【０１３１】
なお、本発明は、以上説明した実施形態に限定されるものではなく、多くの変形が本発明の技術的思想内で当分野において通常の知識を有する者により可能である。
【符号の説明】
【０１３２】
１００…ロボット装置、１０１…リンク、１０５…関節、１５０…制御部、１５１…トルク指令値算出処理、１５２…判断処理、１５３…変更処理、１５５…駆動力指令値算出処理
【図１】
0005930753.tif 000024
【図２】
0005930753.tif 000025
【図３】
0005930753.tif 000026
【図４】
0005930753.tif 000027
【図５】
0005930753.tif 000028
【図６】
0005930753.tif 000029
【図７】
0005930753.tif 000030
【図８】
0005930753.tif 000031
【図９】
0005930753.tif 000032
【図１０】
0005930753.tif 000033
【図１１】
0005930753.tif 000034
【図１２】
0005930753.tif 000035
【図１３】
0005930753.tif 000036
【図１４】
0005930753.tif 000037
【図１５】
0005930753.tif 000038
【図１６】
0005930753.tif 000039
【図１７】
0005930753.tif 000040