ロボットおよびその制御装置
【課題】 関節の姿勢角度と剛性を独立に制御し、良好な安全性と優れた制御性能を共に達成することができる関節ロボットおよびその制御装置を提供することを目的とする。
【解決手段】 基部と、基部に設置された４つのモータと、４つのモータに各々取り付けられた４つの巻取装置と、基部の表面に軸中心線が垂直するように設置された支持柱と、支持柱の上端に取り付けられた球面ジョイントと、球面ジョイントに取り付けられた可動プレートと、可動プレートの底部に設置された４つの自在継手と、４つの前記巻取装置と４つの前記自在継手とを一対一に連結する非線形バネが組み込まれた４本のワイヤと、可動プレートの上面に軸中心線が垂直するように固定された出力軸とを備えた。
【解決手段】 基部と、基部に設置された４つのモータと、４つのモータに各々取り付けられた４つの巻取装置と、基部の表面に軸中心線が垂直するように設置された支持柱と、支持柱の上端に取り付けられた球面ジョイントと、球面ジョイントに取り付けられた可動プレートと、可動プレートの底部に設置された４つの自在継手と、４つの前記巻取装置と４つの前記自在継手とを一対一に連結する非線形バネが組み込まれた４本のワイヤと、可動プレートの上面に軸中心線が垂直するように固定された出力軸とを備えた。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、ワイヤと非線形バネを介して駆動剛性と３自由度姿勢を変えることができる関節ロボットおよびその制御装置に関する。
【背景技術】
【０００２】
最近、サービスロボットおよび人協調産業ロボットの研究は盛んに行われている。これらのロボットは人間と直接に接するため、安全性の保証は重要になっている。ロボット全体の安全性を保つため、ロボットの各関節は柔軟かつ軽量であることが要求される。そこで、従来技術は、センタロッドの周囲に圧縮コイルを配置すると共にワイヤ駆動を用いることにより、装置の柔軟化および軽量化を図っている（例えば、特許文献１参照）。
【先行技術文献】
【特許文献】
【０００３】
【特許文献１】特開平１１−７７５７７号公報（第５−８頁、図１）
【発明の概要】
【発明が解決しようとする課題】
【０００４】
従来は、装置の剛性は圧縮コイルのバネ係数によって決められる。バネ係数を小さくすると、剛性が低くなり安全性が良くなるがエンドエフェクターの位置制御精度が落ちる。一方、バネ係数を大きくすると、エンドエフェクターの位置制御精度を向上することができるが剛性が高くなり安全性が悪くなる。すなわち、安全性と制御性能を両立させることが困難であるという問題があった。
本発明はこのような問題点に鑑みてなされたものであり、ワイヤに非線形バネ要素を組み込み、姿勢のフィードバック制御に必要なトルク指令と剛性の目標指令とに応じてワイヤの張力指令を与えてワイヤの張力を制御し、関節の姿勢角度と剛性を目標指令に独立かつ正確に追従させ、良好な安全性と優れた制御性能を共に達成することができる関節ロボットおよびその制御装置を提供することを目的とする。
【課題を解決するための手段】
【０００５】
上記問題を解決するため、本発明は、次のように構成したのである。
請求項１に記載の発明は、基部と、前記基部に設置された４つのモータと、４つの前記モータに各々取り付けられた４つの巻取装置と、前記基部の表面に軸中心線が垂直するように設置された支持柱と、前記支持柱の上端に取り付けられた球面ジョイントと、前記球面ジョイントに取り付けられた可動プレートと、前記可動プレートの底部に設置された４つの自在継手と、４つの前記巻取装置と４つの前記自在継手とを一対一に連結する非線形バネが組み込まれた４本のワイヤと、前記可動プレートの上面に軸中心線が垂直するように固定された出力軸とを備え、前記球面ジョイントの球体部の球面中心点を原点Ｏとし、前記出力軸の軸中心線をＺ軸として前記可動プレートの上面から離れる方向をＺ軸の正方向とし、前記可動プレートと一緒に移動するような移動直交座標系ＸＹＺＯを設け、前記支持柱の軸中心線と前記基部の表面との交点を原点ｏとし、前記支持柱の軸中心線をｚ軸として前記基部の表面から離れる方向をｚ軸の正方向とし、前記出力軸の軸中心線が前記支持柱の軸中心線と一致した際にｘ軸およびｙ軸がそれぞれＸ軸およびＹ軸と平行となるように固定直交座標系ｘｙｚｏを設け、４つの前記巻取装置がｘｏｙ座標系の異なる象限にあるように４つの前記モータを配置し、４つの前記自在継手をＸＯＹ座標系における位相が各々４つの前記巻取装置の前記ｘｏｙ座標系における位相と同じ値になるように配置し、そして前記ｘｏｙ座標系の第１象限にある前記巻取装置と前記ＸＯＹ座標系の第２象限にある前記自在継手を、前記ｘｏｙ座標系の第２象限にある前記巻取装置と前記ＸＯＹ座標系の第１象限にある前記自在継手を、前記ｘｏｙ座標系の第３象限にある前記巻取装置と前記ＸＯＹ座標系の第４象限にある前記自在継手を、前記ｘｏｙ座標系の第４象限にある前記巻取装置と前記ＸＯＹ座標系の第３象限にある前記自在継手を４本の前記ワイヤで各々連結するものである。
また、請求項２に記載の発明は、基部と、固定子が前記基部の上方に固定され回転子の軸中心線が前記基部の表面に平行する３つの巻取モータおよび回転子の軸中心線が基部４２０の表面に垂直する１つの旋回モータと、３つの前記巻取モータに各々取り付けられた３つの巻取装置と、前記旋回モータの可動子に連結された旋回伝動軸と、前記旋回伝動軸の上端に取り付けられたユニバーサルジョイントと、可動プレートと、前記可動プレートの上面の中央に固定された支持台と、２つ以上のスラスト軸受と、３つの自在継手と、３つの前記巻取装置と３つの前記自在継手とを一対一に連結する非線形バネが組み込まれた３本のワイヤと、前記ユニバーサルジョイントに取り付けられた出力軸とを備え、前記旋回モータが前記基部の中央に設置され、３つの前記巻取モータが前記基部の上方の中心とする円周上の互いに等角度をなす３箇所に設置され、３つの前記自在継手が前記可動プレートの下面の中心とする円周上の互いに等角度をなす３箇所に設置され、前記出力軸と前記支持台との間に２つ以上のスラスト軸受によって拘束されるものである。
また、請求項３に記載の発明は、姿勢角度の目標指令と剛性の目標指令とを生成する目標指令生成部と、前記各モータの回転角度を検出するエンコーダーと、前記各ワイヤの張力を検出する張力センサーとを備え、各ワイヤの張力指令と前記各ワイヤの張力信号とに基づいて張力の制御を行いながら、前記姿勢角度の目標指令と前記剛性の目標指令および前記各モータの回転角度信号に基づいて前記各ワイヤの張力指令を生成し前記関節ロボットの姿勢角度および剛性を制御し、前記各モータの回転角度信号および前記各ワイヤの張力信号に基づいて姿勢角度の推定値を算出する姿勢角度推定部と、前記姿勢角度の目標指令と前記姿勢角度の推定値との偏差を入力しトルク指令を出力する姿勢角度制御部と、前記トルク指令と前記剛性の目標指令および前記姿勢角度の推定値に基づいて張力指令を算出する張力指令計算部とを備えたものである。
【発明の効果】
【０００６】
関節の自由度からみると冗長なモータと、モータと可動プレートとを連結して力を伝達する非線形バネを取り込んだワイヤとを有する関節ロボットを構成すると共に、姿勢のフィードバック制御に必要なトルク指令と剛性の目標指令とに応じてワイヤの張力指令を与えてワイヤの張力を制御する関節ロボットの制御装置を構築することによって、３自由度の姿勢角度を目標指令に正確に追従させながら、機械の剛性も制御することができる。また、剛性の目標指令を高速度変位時小さい値、高加減速かつ低速度変位時大きい値に与えることにより良好な安全性と優れた制御性能を両立させることができる。
【図面の簡単な説明】
【０００７】
【図１】本発明の第１実施例を示す関節ロボットの構成図
【図２】関節ロボットの制御装置を示すブロック線図
【図３】第２実施例を示す関節ロボットの構成図
【発明を実施するための形態】
【０００８】
以下、本発明の実施の形態について図を参照して説明する。
【実施例１】
【０００９】
図１は、本発明の第１実施例を示す関節ロボットの構成図である。図において、モータ１０１、１０２、１０３および１０４は、回転子の軸中心線が基部１２１の表面に平行するように固定子が基部１２１の上方に設置されている。巻取装置１０５、１０６、１０７および１０８は、それぞれモータ１０１、１０２、１０３および１０４の回転軸に連結され、各々モータの回転子と一緒に回転しながらワイヤの巻取りをする。ワイヤ１１１、１１２、１１３および１１４は、それぞれ非線形バネ１１５、１１６、１１７および１１８が組み込まれている。ここで、非線形バネとは、バネ定数が一定な値ではなく変位によって変わるものである。たとえば、ピッチを段階的に変化させたコイルばねを用いている。これに限らず、２つのモータと各々のモータにワイヤで連結された２つの静滑車の間に動滑車を組み合せた構成を取ることができる。支持柱１２２は、基部１２１の上面に軸中心線が垂直するように設置されている。可動プレート１２４は、上方の中央には出力軸１２５が垂直に固定されている。球面ジョイント１２３は、球体部が支持柱１２２の先端に、受部材が可動プレート１２４に繋げられている。支持柱１２２の軸中心線と基部１２１の上面との接点を原点ｏとし、支持柱１２２の中心線をｚ軸として基部１２１の上面から離れる方向をｚ軸の正方向とするように固定直交座標系ｘｙｚｏを設ける。また、球面ジョイント１２３の球体部の球面中心点を原点Ｏとし、出力軸１２５の軸中心線をＺ軸として可動プレート１２４の上面から離れる方向をＺ軸の正方向とし、初期位置においてＸ軸、Ｙ軸およびＺ軸がそれぞれｘ軸、ｙ軸およびｚ軸に平行となり、可動プレート１２４と一緒に移動するような移動直交座標系ＸＹＺＯを設ける。モータ１０１と巻取装置１０５が固定平面座標系ｘｏｙの第１象限に、モータ１０２と巻取装置１０６が固定平面座標系ｘｏｙの第２象限に、モータ１０３と巻取装置１０７が固定平面座標系ｘｏｙの第４象限に、モータ１０４と巻取装置１０８が固定平面座標系ｘｏｙの第３象限にあるように設置される。移動平面座標系ＸＯＹにおける位相がそれぞれ巻取装置１０５、１０６、１０７、１０８のワイヤ出入口ａ、ｂ、ｃ、ｄ点が固定平面座標系ｘｏｙにおける位相と同じである可動プレート１２４下面のＡ、Ｂ、Ｃ 、Ｄ点に自在継手１３１、１３２、１３３、１３４が設置される。また、巻取装置１０６と自在継手１３１の間、巻取装置１０５と自在継手１３２の間、巻取装置１０７と自在継手１３３の間および巻取装置１０８と自在継手１３４の間にそれぞれ非線形バネ１１５が取り込まれたワイヤ１１１、非線形バネ１１６が取り込まれたワイヤ１１２、非線形バネ１１７が取り込まれたワイヤ１１３および非線形バネ１１８が取り込まれたワイヤ１１４が取り付けられている。
図２は関節ロボットの制御装置を示すブロック線図である。図において、目標指令生成部２０１は、出力軸の姿勢角度の目標指令値α＊、β＊、γ＊および剛性の目標指令値η＊を生成する。エンコーダー１４１、１４２、１４３および１４４は、それぞれモータ１０１、１０２、１０３および１０４の回転角度θ１、θ２、θ３およびθ４を検出する。張力センサー１５１、１５２、１５３および１５４は、それぞれワイヤ１１１、１１２、１１３および１１４の張力Ｆ１、Ｆ２、Ｆ３およびＦ４を検出する。姿勢角度推定部２０４は、エンコーダー１４１、１４２、１４３および１４４の出力と張力センサー１５１、１５２、１５３および１５４の出力とを入力して姿勢角度α、βおよびγの推定値αｓ、βｓおよびγｓを出力する。姿勢角度制御部２０２は、それぞれ姿勢角度αの目標指令値α＊と推定値αｓとの偏差ｅα、姿勢角度βの目標指令値β＊と推定値βｓとの偏差ｅβおよび姿勢角度γの目標指令値γ＊と推定値γｓとの偏差ｅγを入力してトルク指令ＴＸｃ、ＴＹｃおよびＴＺｃを出力する。張力指令計算部２０４は、トルクＴＸｃ、ＴＹｃ、ＴＺｃ、剛性の目標指令値η＊および姿勢角度の推定値αｓ、βｓ、γｓをに基づいて張力指令Ｆ１ｃ、Ｆ２ｃ、Ｆ３ｃおよびＦ４ｃを計算する。張力制御部２１１、２１２、２１３および２１４は、それぞれワイヤ１１１の張力指令Ｆ１ｃと張力Ｆ１との偏差、ワイヤ１１２の張力指令Ｆ２ｃと張力Ｆ２との偏差、ワイヤ１１３の張力指令Ｆ３ｃと張力Ｆ３との偏差およびワイヤ１１４の張力指令Ｆ４ｃと張力Ｆ４との偏差を入力してモータ１０１、１０２、１０３および１０４の電流指令Ｉ１ｃ、Ｉ２ｃ、Ｉ３ｃおよびＩ４ｃを出力する。また、モータ駆動部２２１、２２２、２２３および２２４は、それぞれモータ１０１、１０２、１０３および１０４の電流指令Ｉ１ｃ、Ｉ２ｃ、Ｉ３ｃおよびＩ４ｃに基づいて各モータを駆動制御する。
【００１０】
以下、制御装置の各補償部の実現方法について説明する。
まず、姿勢角度推定部２０４の実現方法を説明する。
可動プレート１２４のＸ−Ｙ−Ｚオイラ−角（ここで姿勢角度と称す）をα、β、γとし、それに対応する姿勢変換行列Ｒは式（１）で表される。
【００１１】
【数１】
【００１２】
ただし、Ｒの要素は式（２）〜式（１０）で表される。
【００１３】
【数２】
【００１４】
ａ、ｂ、ｃ、ｄ点の絶対座標をそれぞれ（ｘａ，ｙａ，ｚａ）、（ｘｂ，ｙｂ，ｚｂ）、（ｘｃ，ｙｃ，ｚｃ）、（ｘｄ，ｙｄ，ｚｄ）とし、また、Ｏ点の絶対座標を（ｘＯ，ｙＯ，ｚＯ）とすると、ａ、ｂ、ｃ、ｄ点の相対座標は式（１１）〜式（１４）のように与えられる。
（Ｘａ，Ｙａ，Ｚａ）＝｛（ｘａ，ｙａ，ｚａ）−（ｘＯ，ｙＯ，ｚＯ）｝Ｒ−1 （１１）
（Ｘｂ，Ｙｂ，Ｚｂ）＝｛（ｘｂ，ｙｂ，ｚｂ）−（ｘＯ，ｙＯ，ｚＯ）｝Ｒ−1 （１２）
（Ｘｃ，Ｙｃ，Ｚｃ）＝｛（ｘｃ，ｙｃ，ｚｃ）−（ｘＯ，ｙＯ，ｚＯ）｝Ｒ−1 （１３）
（Ｘｄ，Ｙｄ，Ｚｄ）＝｛（ｘｄ，ｙｄ，ｚｄ）−（ｘＯ，ｙＯ，ｚＯ）｝Ｒ−1 （１４）
そして、Ａ、Ｂ、Ｃ、Ｄ点の相対座標をそれぞれ（ＸＡ，ＹＡ，ＺＡ）、（ＸＢ，ＹＢ，ＺＢ）、（ＸＣ，ＹＣ，ＺＣ）、（ＸＤ，ＹＤ，ＺＤ）とすると、ベクトル
【００１５】
【数３】
【００１６】
ただし、Ｉ、Ｊ、ＫはそれぞれＸ軸、Ｙ軸、Ｚ軸方向を向く単位ベクトルである。
従って、Ａ点とｂ点との間の距離Ｌ１、Ｂ点とａ点との間の距離Ｌ２、Ｃ点とｄ点との間の距離Ｌ３およびＤ点とｃ点との間の距離Ｌ４は式（１９）〜式（２２）のように与えられる。
【００１７】
【数４】
【００１８】
関節ロボットの機構は定まると、ａ、ｂ、ｃ、ｄ、Ｏ点の絶対座標とＡ、Ｂ、Ｃ、Ｄ点の相対座標は既知なものとなり、式（１）〜式（１４）より式（１９）〜式（２２）は姿勢角度α、β、γの関数となる。
一方、非線形バネが発生する力Ｆと対応する伸縮量σとの関係を式（５３）で表されるものとする。
Ｆ＝ｆ（σ） （２３）
また、巻取装置のプーリの半径をｒとすると、式（２４）〜式（２７）が成り立つ。
Ｌ１＝ｆ−１（Ｆ１）＋θ１ｒ （２４）
Ｌ２＝ｆ−１（Ｆ２）＋θ２ｒ （２５）
Ｌ３＝ｆ−１（Ｆ３）＋θ３ｒ （２６）
Ｌ４＝ｆ−１（Ｆ４）＋θ４ｒ （２７）
ただし、Ｆ１、Ｆ２、Ｆ３およびＦ４はそれぞれワイヤ１１１、１１２、１１３および１１４の張力、θ１、θ２、θ３およびθ４はそれぞれモータ１０１、１０２、１０３および１０４の回転角度（モータと巻取装置との間にギヤがあった場合にモータの回転角度にギア比を掛けた値）である。
張力センサー１５１、１５２、１５３および１５４が検出した張力Ｆ１、Ｆ２、Ｆ３およびＦ４の値と、エンコーダー１４１、１４２、１４３および１４４が検出した回転角度θ１、θ２、θ３およびθ４の値とを式（２４）〜式（２７）に代入し、Ｌ１、Ｌ２、Ｌ３およびＬ４を計算する。そして、Ｌ１、Ｌ２、Ｌ３およびＬ４の値を式（１９）〜式（２２）に代入して連立方程式を解くと、姿勢角度α、β、γを求めることができる。このように求めた姿勢角度α、βおよびγの値を姿勢角度α、βおよびγの推定値αｓ、βｓおよびγｓとする。
次に、姿勢角度制御部２０２の実現方法を説明する。
姿勢角度の指令値α＊、β＊およびγ＊からそれぞれ姿勢角度の推定値αｓ、βｓおよびγｓを差し引いて得た偏差ｅα、ｅβおよびｅγに各々ＰＩＤ補償を掛けてトルク指令ＴＸｃ、ＴＹｃおよびＴＺｃが得られる。
次に、張力指令計算部２０４の実現方法を説明する。
すべてのワイヤが常に完全に伸びている（ワイヤの張力が０以上である）ように制御されるとすると、各ワイヤが可動プレート１２４に働く力のベクトルは式（２８）〜式（３１）のように表される。
【００１９】
【数５】
【００２０】
また、Ａ、Ｂ、Ｃ、Ｄ点の相対座標を用いてベクトル
【００２１】
【数６】
【００２２】
が発生するトルクのベクトルは式（３６）〜式（３９）のように表される。
【００２３】
【数７】
【００２４】
従って、可動プレート１２４に働く総トルクＴのＸ、Ｙ、Ｚ方向の分量は式（４０）〜式（４２）のように表される。
【００２５】
【数８】
【００２６】
ただし、
【００２７】
【数９】
【００２８】
ＴＸ、ＴＹ、ＴＺの代わりにそれぞれＴＸｃ、ＴＹｃ、ＴＺｃを、Ｆ１、Ｆ２、Ｆ３、Ｆ４の代わりにそれぞれＦ１ｃ、Ｆ２ｃ、Ｆ３ｃ、Ｆ４ｃを式（４０）〜式（４２）に代入すると、式（５５）〜式（５７）となる。
【００２９】
【数１０】
【００３０】
姿勢角度α、βおよびγの代わりに姿勢角度推定部２０４で計算した姿勢角度の推定値αｓ、βｓおよびγｓを式（２）〜式（１０）に代入して姿勢変換行列Ｒの各要素を計算し、また、式（１１）〜式（１４）のようにａ、ｂ、ｃ、ｄ点の相対座標を計算する。ａ、ｂ、ｃ、ｄおよびＡ、Ｂ、Ｃ、Ｄ点の相対座標を、式（１９）〜式（２２）に代入して得たＬ１、Ｌ２、Ｌ３およびＬ４と一緒に式（４３）〜式（５４）に代入してｋＸ１〜４、ｋＹ１〜４およびｋＺ１〜４を計算する。そして、姿勢角度制御部２０２から入って来たトルク指令ＴＸｃ、ＴＹｃおよびＴＺｃを式（５５）〜式（５７）に代入すると、式（５５）〜式（５７）がＦ１ｃ、Ｆ２ｃ、Ｆ３ｃ、Ｆ４ｃの線形方程式となる。
一方、非線形バネは変形量が大きくなるほどばね定数が大きくなるものとすると、剛性が高く要求される場合に各ワイヤの張力を大きくすれば良い。それで、剛性の目標指令値η＊に対応する各ワイヤの張力の和Ｆｓの指令値Ｆｓ＊を式（５８）のように与える。
Ｆｓ＊＝ｇ（η＊） （５８）
ただし、ｇ（・）単調増加関数である。
一般的に、式（５５）〜式（５７）に式（５９）を加えて連立方程式を解くと、Ｆ１ｃ、Ｆ２ｃ、Ｆ３ｃ、Ｆ４ｃを一意的に決められる。
Ｆ１ｃ＋Ｆ２ｃ＋Ｆ３ｃ＋Ｆ４ｃ＝Ｆｓ＊ （５９）
ところが、式（５５）〜式（５７）と式（５９）の連立方程式が特異になることもある。その場合に、各変数Ｆ１ｃ、Ｆ２ｃ、Ｆ３ｃ、Ｆ４ｃの前に各々適当な重み係数ｋ１、ｋ２、ｋ３、ｋ４を掛け、式（５９）を式（６０）のように書き直せば良い。
ｋ１Ｆ１ｃ＋ｋ２Ｆ２ｃ＋ｋ３Ｆ３ｃ＋ｋ４Ｆ４ｃ＝Ｆｓ＊ （６０）
また、ある向きの安全性をより重視したい場合は、その向きの剛性を低くするため、その向きに近いワイヤに対応する重み係数を大きく設定すれば良い。
なお、連立方程式を解いて得た張力指令の値が負になった場合に、関数ｇ（・）を式（６１）のように修正を行う。
ｇ（η＊）＝ｇ（η＊）＋Ｆａ＊ （６１）
ただし、Ｆａ＊が適当な正の数である。式（５９）を連立方程式に取り込んだ場合は、Ｆａ＊を負になった変数の和の絶対値の２倍より少し大きい値に与えれば良い。また、修正後の連立方程式は依然負の解が存在する場合に、連立方程式の解がすべて正になるまで以上の修正方法で繰り返して行えば良い。
最後に、張力制御部の実現方法を説明する。
張力制御部にはＰＩ補償器を込みこめば良い。
【００３１】
以下、関節ロボットの動作原理について説明する。
一般的に、姿勢制御のメインループと較べ、張力制御マイナーループには位相遅れが小さいので、張力制御部のゲインを大きく上げられ、張力制御マイナー閉ループの応答特性が高い。すなわち、張力指令の通りにワイヤの張力が制御される。
一方、張力指令が式（５５）〜式（５７）に基づいて求められたので、式（５５）〜式（５７）と式（４０）〜式（４２）とを比較することで移動座標系の各軸まわりのトルクＴＸ、ＴＹ、ＴＺがそれらの指令ＴＸｃ、ＴＹｃ、ＴＺｃに追従することが分かる。すなわち、剛性制御は姿勢制御に影響を与えず、剛性と姿勢を独立かつ正確に制御できる。
また、剛性の制御性能に対する影響が大きく現れるのは出力軸１２５が高加減速かつ低速度で変位する時である。一方、安全性に配慮すべきは出力軸１２５が高速度変位する時である。よって、剛性の目標指令は高速度変位時小さい値、高加減速かつ低速度変位時大きい値に与えれば良い。
【００３２】
このように、関節の自由度からみると冗長なモータと、モータと可動プレートとを連結して力を伝達する非線形バネを取り込んだワイヤとを有する関節ロボットを構成すると共に、姿勢のフィードバック制御に必要なトルク指令と剛性の目標指令とに応じてワイヤの張力指令を与えてワイヤの張力を制御する関節ロボットの制御装置を構築することによって、３自由度の姿勢角度を目標指令に正確に追従させながら、機械の剛性も制御することができる。また、剛性の目標指令を高速度変位時小さい値、高加減速かつ低速度変位時大きい値に与えることにより良好な安全性と優れた制御性能を両立させることができる。
【実施例２】
【００３３】
図３は第２実施例を示す関節ロボットの構成図である。図において、旋回モータ３２０は、回転子の軸中心線が基部３２１の表面に垂直するように固定子が基部３２１の中央に設置されている。巻取モータ３０１、３０２、３０３は、固定子が基部３２１の上方の中心とする円周上の互いに等角度をなす３箇所に設置されている。巻取装置３０４、３０５、３０６は、巻取モータの回転子と一緒に回転しながらワイヤの巻取りをする。ワイヤ３１１、３１２、３１３は、それぞれ非線形バネ３１４、３１５、３１６が組み込まれている。旋回伝動軸３２２は、旋回モータ３２０の回転子に連結されている。可動プレート３２４は、上面の中央には円筒型支持台３２６が設置されている。ユニバーサルジョイント３２３は、旋回伝動軸３２２と出力軸３２５とを連結する。スラスト軸受３２７と３２８は、出力軸３２５を支持台３２６に拘束する。基部３２１の中心を原点ｏとし、旋回伝動軸３２２の中心線をｚ軸として基部３２１の上面から離れる方向をｚ軸の正方向とするように固定直交座標系ｘｙｚｏを設ける。また、ユニバーサルジョイントの中心を原点Ｏとし、出力軸４１９の軸中心線をＺ軸として可動プレート３２４の上面から離れる方向をＺ軸の正方向とし、可動プレート３２４の下面が基部３２１の上面と平行になった際にＸ軸およびＹ軸がそれぞれｘ軸およびｙ軸と平行になるように、可動プレート３２４と一緒に移動するような移動直交座標系ＸＹＺＯを設ける。移動平面座標系ＸＯＹにおける位相がそれぞれ巻取装置３０４、３０５、３０６のワイヤ出入口ａ、ｂ、ｃ点が固定平面座標系ｘｏｙにおける位相と同じであり可動プレート３２４下面の中心とする円周上の互いに等角度をなすＡ、Ｂ、Ｃ点に自在継手３１７、３１８、３１９が設置されている。また、巻取装置３０４と自在継手３１７の間、巻取装置３０５と自在継手３１８の間および巻取装置３０６と自在継手３１９の間にそれぞれ非線形バネ３１４が取り込まれたワイヤ３１１、非線形バネ３１５が取り込まれたワイヤ３１２および非線形バネ３１６が取り込まれたワイヤ３１３が取り付けられている。
【００３４】
以下、動作原理について説明する。
旋回伝動軸３２２が旋回モータ３２０の可動子に固定されたため、その先端に連結されたユニバーサルジョイント３２３の中心位置が変わらないので、ユニバーサルジョイント３２３に連結された出力軸３２５はユニバーサルジョイント３２３の中心点を中心に変位しかできない。すなわち、出力軸３２５の先端はユニバーサルジョイント３２３の中心点を中心に３自由度の球面運動をする。また、支持台３２６と出力軸３２５とはスラスト軸受３２７および３２８によってに拘束されたため、両者の相対運動は出力軸３２５の軸中心の回転運動しかない。よって、支持台３２６および支持台３２６が固定された可動プレート３２４もユニバーサルジョイント３２３の中心点を中心に変位しかできない。
スラスト軸受の摩擦力が十分小さい場合は出力軸３２５と支持台３２６とのＺ軸まわりの回転運動は互いに影響しない。出力軸３２５のＸ軸およびＹ軸方向の姿勢は可動プレート３２４の姿勢に定められるが、Ｚ軸方向の姿勢は完全に旋回モータ３２０の回転角度に決められる。すなわち、３本のワイヤを用いて出力軸３２５の２自由度の運動を操作する。そのため、Ｘ軸およびＹ軸方向の姿勢と剛性を独立に調整できる。一方、Ｚ軸まわりの剛性を調整できないが、Ｚ軸中心の回転の最大半径が小さいため大きなトルクを出力しないので、外部のものに大きな被害を与えない。
【００３５】
以上のように、３本のワイヤを用いて出力トルクが大きくなる方向の姿勢と剛性を独立に調整するため、関節ロボットに良好な安全性と優れた制御性能を両立させることができる。
第１実施例と較べ、本実施例の技術を用いた場合は、構造が若干複雑になるが、可動範囲が広くなる。
【符号の説明】
【００３６】
１００ 関節ロボット
１０１、１０２、１０３、１０４ モータ
１０５、１０６、１０７、１０８ 巻取装置
１１１、１１２、１１３、１１４、３１１、３１２、３１３ ワイヤ
１１５、１１６、１１７、１１８、３１５、３１６、３１７ 非線形バネ
１３１、１３２、１３３、１３４、３１７、３１８、３１９ 自在継手
１２１、３２１ 基部
１２２ 支持柱
１２３ 球面ジョイント
１２４、３２４ 可動プレート
１２５、３２５ 出力軸
１４１、１４２、１４３、１４４ エンコーダー
１５１、１５２、１５３、１５４ 張力センサー
２０１ 目標指令生成部
２０２ 姿勢角度制御部
２０３ 張力指令計算部
２０４ 姿勢角度推定部
２１１、２１２、２１３、２１４ 張力制御部
２２１、２２２、２２３、２２４ モータ駆動部
３０１、３０２、３０３ 巻取モータ
３０４、３０５、３０６ 巻取装置
３２０ 旋回モータ
３２２ 旋回伝動軸
３２３ ユニバーサルジョイント
３２７、３２８ スラスト軸受
【技術分野】
【０００１】
本発明は、ワイヤと非線形バネを介して駆動剛性と３自由度姿勢を変えることができる関節ロボットおよびその制御装置に関する。
【背景技術】
【０００２】
最近、サービスロボットおよび人協調産業ロボットの研究は盛んに行われている。これらのロボットは人間と直接に接するため、安全性の保証は重要になっている。ロボット全体の安全性を保つため、ロボットの各関節は柔軟かつ軽量であることが要求される。そこで、従来技術は、センタロッドの周囲に圧縮コイルを配置すると共にワイヤ駆動を用いることにより、装置の柔軟化および軽量化を図っている（例えば、特許文献１参照）。
【先行技術文献】
【特許文献】
【０００３】
【特許文献１】特開平１１−７７５７７号公報（第５−８頁、図１）
【発明の概要】
【発明が解決しようとする課題】
【０００４】
従来は、装置の剛性は圧縮コイルのバネ係数によって決められる。バネ係数を小さくすると、剛性が低くなり安全性が良くなるがエンドエフェクターの位置制御精度が落ちる。一方、バネ係数を大きくすると、エンドエフェクターの位置制御精度を向上することができるが剛性が高くなり安全性が悪くなる。すなわち、安全性と制御性能を両立させることが困難であるという問題があった。
本発明はこのような問題点に鑑みてなされたものであり、ワイヤに非線形バネ要素を組み込み、姿勢のフィードバック制御に必要なトルク指令と剛性の目標指令とに応じてワイヤの張力指令を与えてワイヤの張力を制御し、関節の姿勢角度と剛性を目標指令に独立かつ正確に追従させ、良好な安全性と優れた制御性能を共に達成することができる関節ロボットおよびその制御装置を提供することを目的とする。
【課題を解決するための手段】
【０００５】
上記問題を解決するため、本発明は、次のように構成したのである。
請求項１に記載の発明は、基部と、前記基部に設置された４つのモータと、４つの前記モータに各々取り付けられた４つの巻取装置と、前記基部の表面に軸中心線が垂直するように設置された支持柱と、前記支持柱の上端に取り付けられた球面ジョイントと、前記球面ジョイントに取り付けられた可動プレートと、前記可動プレートの底部に設置された４つの自在継手と、４つの前記巻取装置と４つの前記自在継手とを一対一に連結する非線形バネが組み込まれた４本のワイヤと、前記可動プレートの上面に軸中心線が垂直するように固定された出力軸とを備え、前記球面ジョイントの球体部の球面中心点を原点Ｏとし、前記出力軸の軸中心線をＺ軸として前記可動プレートの上面から離れる方向をＺ軸の正方向とし、前記可動プレートと一緒に移動するような移動直交座標系ＸＹＺＯを設け、前記支持柱の軸中心線と前記基部の表面との交点を原点ｏとし、前記支持柱の軸中心線をｚ軸として前記基部の表面から離れる方向をｚ軸の正方向とし、前記出力軸の軸中心線が前記支持柱の軸中心線と一致した際にｘ軸およびｙ軸がそれぞれＸ軸およびＹ軸と平行となるように固定直交座標系ｘｙｚｏを設け、４つの前記巻取装置がｘｏｙ座標系の異なる象限にあるように４つの前記モータを配置し、４つの前記自在継手をＸＯＹ座標系における位相が各々４つの前記巻取装置の前記ｘｏｙ座標系における位相と同じ値になるように配置し、そして前記ｘｏｙ座標系の第１象限にある前記巻取装置と前記ＸＯＹ座標系の第２象限にある前記自在継手を、前記ｘｏｙ座標系の第２象限にある前記巻取装置と前記ＸＯＹ座標系の第１象限にある前記自在継手を、前記ｘｏｙ座標系の第３象限にある前記巻取装置と前記ＸＯＹ座標系の第４象限にある前記自在継手を、前記ｘｏｙ座標系の第４象限にある前記巻取装置と前記ＸＯＹ座標系の第３象限にある前記自在継手を４本の前記ワイヤで各々連結するものである。
また、請求項２に記載の発明は、基部と、固定子が前記基部の上方に固定され回転子の軸中心線が前記基部の表面に平行する３つの巻取モータおよび回転子の軸中心線が基部４２０の表面に垂直する１つの旋回モータと、３つの前記巻取モータに各々取り付けられた３つの巻取装置と、前記旋回モータの可動子に連結された旋回伝動軸と、前記旋回伝動軸の上端に取り付けられたユニバーサルジョイントと、可動プレートと、前記可動プレートの上面の中央に固定された支持台と、２つ以上のスラスト軸受と、３つの自在継手と、３つの前記巻取装置と３つの前記自在継手とを一対一に連結する非線形バネが組み込まれた３本のワイヤと、前記ユニバーサルジョイントに取り付けられた出力軸とを備え、前記旋回モータが前記基部の中央に設置され、３つの前記巻取モータが前記基部の上方の中心とする円周上の互いに等角度をなす３箇所に設置され、３つの前記自在継手が前記可動プレートの下面の中心とする円周上の互いに等角度をなす３箇所に設置され、前記出力軸と前記支持台との間に２つ以上のスラスト軸受によって拘束されるものである。
また、請求項３に記載の発明は、姿勢角度の目標指令と剛性の目標指令とを生成する目標指令生成部と、前記各モータの回転角度を検出するエンコーダーと、前記各ワイヤの張力を検出する張力センサーとを備え、各ワイヤの張力指令と前記各ワイヤの張力信号とに基づいて張力の制御を行いながら、前記姿勢角度の目標指令と前記剛性の目標指令および前記各モータの回転角度信号に基づいて前記各ワイヤの張力指令を生成し前記関節ロボットの姿勢角度および剛性を制御し、前記各モータの回転角度信号および前記各ワイヤの張力信号に基づいて姿勢角度の推定値を算出する姿勢角度推定部と、前記姿勢角度の目標指令と前記姿勢角度の推定値との偏差を入力しトルク指令を出力する姿勢角度制御部と、前記トルク指令と前記剛性の目標指令および前記姿勢角度の推定値に基づいて張力指令を算出する張力指令計算部とを備えたものである。
【発明の効果】
【０００６】
関節の自由度からみると冗長なモータと、モータと可動プレートとを連結して力を伝達する非線形バネを取り込んだワイヤとを有する関節ロボットを構成すると共に、姿勢のフィードバック制御に必要なトルク指令と剛性の目標指令とに応じてワイヤの張力指令を与えてワイヤの張力を制御する関節ロボットの制御装置を構築することによって、３自由度の姿勢角度を目標指令に正確に追従させながら、機械の剛性も制御することができる。また、剛性の目標指令を高速度変位時小さい値、高加減速かつ低速度変位時大きい値に与えることにより良好な安全性と優れた制御性能を両立させることができる。
【図面の簡単な説明】
【０００７】
【図１】本発明の第１実施例を示す関節ロボットの構成図
【図２】関節ロボットの制御装置を示すブロック線図
【図３】第２実施例を示す関節ロボットの構成図
【発明を実施するための形態】
【０００８】
以下、本発明の実施の形態について図を参照して説明する。
【実施例１】
【０００９】
図１は、本発明の第１実施例を示す関節ロボットの構成図である。図において、モータ１０１、１０２、１０３および１０４は、回転子の軸中心線が基部１２１の表面に平行するように固定子が基部１２１の上方に設置されている。巻取装置１０５、１０６、１０７および１０８は、それぞれモータ１０１、１０２、１０３および１０４の回転軸に連結され、各々モータの回転子と一緒に回転しながらワイヤの巻取りをする。ワイヤ１１１、１１２、１１３および１１４は、それぞれ非線形バネ１１５、１１６、１１７および１１８が組み込まれている。ここで、非線形バネとは、バネ定数が一定な値ではなく変位によって変わるものである。たとえば、ピッチを段階的に変化させたコイルばねを用いている。これに限らず、２つのモータと各々のモータにワイヤで連結された２つの静滑車の間に動滑車を組み合せた構成を取ることができる。支持柱１２２は、基部１２１の上面に軸中心線が垂直するように設置されている。可動プレート１２４は、上方の中央には出力軸１２５が垂直に固定されている。球面ジョイント１２３は、球体部が支持柱１２２の先端に、受部材が可動プレート１２４に繋げられている。支持柱１２２の軸中心線と基部１２１の上面との接点を原点ｏとし、支持柱１２２の中心線をｚ軸として基部１２１の上面から離れる方向をｚ軸の正方向とするように固定直交座標系ｘｙｚｏを設ける。また、球面ジョイント１２３の球体部の球面中心点を原点Ｏとし、出力軸１２５の軸中心線をＺ軸として可動プレート１２４の上面から離れる方向をＺ軸の正方向とし、初期位置においてＸ軸、Ｙ軸およびＺ軸がそれぞれｘ軸、ｙ軸およびｚ軸に平行となり、可動プレート１２４と一緒に移動するような移動直交座標系ＸＹＺＯを設ける。モータ１０１と巻取装置１０５が固定平面座標系ｘｏｙの第１象限に、モータ１０２と巻取装置１０６が固定平面座標系ｘｏｙの第２象限に、モータ１０３と巻取装置１０７が固定平面座標系ｘｏｙの第４象限に、モータ１０４と巻取装置１０８が固定平面座標系ｘｏｙの第３象限にあるように設置される。移動平面座標系ＸＯＹにおける位相がそれぞれ巻取装置１０５、１０６、１０７、１０８のワイヤ出入口ａ、ｂ、ｃ、ｄ点が固定平面座標系ｘｏｙにおける位相と同じである可動プレート１２４下面のＡ、Ｂ、Ｃ 、Ｄ点に自在継手１３１、１３２、１３３、１３４が設置される。また、巻取装置１０６と自在継手１３１の間、巻取装置１０５と自在継手１３２の間、巻取装置１０７と自在継手１３３の間および巻取装置１０８と自在継手１３４の間にそれぞれ非線形バネ１１５が取り込まれたワイヤ１１１、非線形バネ１１６が取り込まれたワイヤ１１２、非線形バネ１１７が取り込まれたワイヤ１１３および非線形バネ１１８が取り込まれたワイヤ１１４が取り付けられている。
図２は関節ロボットの制御装置を示すブロック線図である。図において、目標指令生成部２０１は、出力軸の姿勢角度の目標指令値α＊、β＊、γ＊および剛性の目標指令値η＊を生成する。エンコーダー１４１、１４２、１４３および１４４は、それぞれモータ１０１、１０２、１０３および１０４の回転角度θ１、θ２、θ３およびθ４を検出する。張力センサー１５１、１５２、１５３および１５４は、それぞれワイヤ１１１、１１２、１１３および１１４の張力Ｆ１、Ｆ２、Ｆ３およびＦ４を検出する。姿勢角度推定部２０４は、エンコーダー１４１、１４２、１４３および１４４の出力と張力センサー１５１、１５２、１５３および１５４の出力とを入力して姿勢角度α、βおよびγの推定値αｓ、βｓおよびγｓを出力する。姿勢角度制御部２０２は、それぞれ姿勢角度αの目標指令値α＊と推定値αｓとの偏差ｅα、姿勢角度βの目標指令値β＊と推定値βｓとの偏差ｅβおよび姿勢角度γの目標指令値γ＊と推定値γｓとの偏差ｅγを入力してトルク指令ＴＸｃ、ＴＹｃおよびＴＺｃを出力する。張力指令計算部２０４は、トルクＴＸｃ、ＴＹｃ、ＴＺｃ、剛性の目標指令値η＊および姿勢角度の推定値αｓ、βｓ、γｓをに基づいて張力指令Ｆ１ｃ、Ｆ２ｃ、Ｆ３ｃおよびＦ４ｃを計算する。張力制御部２１１、２１２、２１３および２１４は、それぞれワイヤ１１１の張力指令Ｆ１ｃと張力Ｆ１との偏差、ワイヤ１１２の張力指令Ｆ２ｃと張力Ｆ２との偏差、ワイヤ１１３の張力指令Ｆ３ｃと張力Ｆ３との偏差およびワイヤ１１４の張力指令Ｆ４ｃと張力Ｆ４との偏差を入力してモータ１０１、１０２、１０３および１０４の電流指令Ｉ１ｃ、Ｉ２ｃ、Ｉ３ｃおよびＩ４ｃを出力する。また、モータ駆動部２２１、２２２、２２３および２２４は、それぞれモータ１０１、１０２、１０３および１０４の電流指令Ｉ１ｃ、Ｉ２ｃ、Ｉ３ｃおよびＩ４ｃに基づいて各モータを駆動制御する。
【００１０】
以下、制御装置の各補償部の実現方法について説明する。
まず、姿勢角度推定部２０４の実現方法を説明する。
可動プレート１２４のＸ−Ｙ−Ｚオイラ−角（ここで姿勢角度と称す）をα、β、γとし、それに対応する姿勢変換行列Ｒは式（１）で表される。
【００１１】
【数１】
【００１２】
ただし、Ｒの要素は式（２）〜式（１０）で表される。
【００１３】
【数２】
【００１４】
ａ、ｂ、ｃ、ｄ点の絶対座標をそれぞれ（ｘａ，ｙａ，ｚａ）、（ｘｂ，ｙｂ，ｚｂ）、（ｘｃ，ｙｃ，ｚｃ）、（ｘｄ，ｙｄ，ｚｄ）とし、また、Ｏ点の絶対座標を（ｘＯ，ｙＯ，ｚＯ）とすると、ａ、ｂ、ｃ、ｄ点の相対座標は式（１１）〜式（１４）のように与えられる。
（Ｘａ，Ｙａ，Ｚａ）＝｛（ｘａ，ｙａ，ｚａ）−（ｘＯ，ｙＯ，ｚＯ）｝Ｒ−1 （１１）
（Ｘｂ，Ｙｂ，Ｚｂ）＝｛（ｘｂ，ｙｂ，ｚｂ）−（ｘＯ，ｙＯ，ｚＯ）｝Ｒ−1 （１２）
（Ｘｃ，Ｙｃ，Ｚｃ）＝｛（ｘｃ，ｙｃ，ｚｃ）−（ｘＯ，ｙＯ，ｚＯ）｝Ｒ−1 （１３）
（Ｘｄ，Ｙｄ，Ｚｄ）＝｛（ｘｄ，ｙｄ，ｚｄ）−（ｘＯ，ｙＯ，ｚＯ）｝Ｒ−1 （１４）
そして、Ａ、Ｂ、Ｃ、Ｄ点の相対座標をそれぞれ（ＸＡ，ＹＡ，ＺＡ）、（ＸＢ，ＹＢ，ＺＢ）、（ＸＣ，ＹＣ，ＺＣ）、（ＸＤ，ＹＤ，ＺＤ）とすると、ベクトル
【００１５】
【数３】
【００１６】
ただし、Ｉ、Ｊ、ＫはそれぞれＸ軸、Ｙ軸、Ｚ軸方向を向く単位ベクトルである。
従って、Ａ点とｂ点との間の距離Ｌ１、Ｂ点とａ点との間の距離Ｌ２、Ｃ点とｄ点との間の距離Ｌ３およびＤ点とｃ点との間の距離Ｌ４は式（１９）〜式（２２）のように与えられる。
【００１７】
【数４】
【００１８】
関節ロボットの機構は定まると、ａ、ｂ、ｃ、ｄ、Ｏ点の絶対座標とＡ、Ｂ、Ｃ、Ｄ点の相対座標は既知なものとなり、式（１）〜式（１４）より式（１９）〜式（２２）は姿勢角度α、β、γの関数となる。
一方、非線形バネが発生する力Ｆと対応する伸縮量σとの関係を式（５３）で表されるものとする。
Ｆ＝ｆ（σ） （２３）
また、巻取装置のプーリの半径をｒとすると、式（２４）〜式（２７）が成り立つ。
Ｌ１＝ｆ−１（Ｆ１）＋θ１ｒ （２４）
Ｌ２＝ｆ−１（Ｆ２）＋θ２ｒ （２５）
Ｌ３＝ｆ−１（Ｆ３）＋θ３ｒ （２６）
Ｌ４＝ｆ−１（Ｆ４）＋θ４ｒ （２７）
ただし、Ｆ１、Ｆ２、Ｆ３およびＦ４はそれぞれワイヤ１１１、１１２、１１３および１１４の張力、θ１、θ２、θ３およびθ４はそれぞれモータ１０１、１０２、１０３および１０４の回転角度（モータと巻取装置との間にギヤがあった場合にモータの回転角度にギア比を掛けた値）である。
張力センサー１５１、１５２、１５３および１５４が検出した張力Ｆ１、Ｆ２、Ｆ３およびＦ４の値と、エンコーダー１４１、１４２、１４３および１４４が検出した回転角度θ１、θ２、θ３およびθ４の値とを式（２４）〜式（２７）に代入し、Ｌ１、Ｌ２、Ｌ３およびＬ４を計算する。そして、Ｌ１、Ｌ２、Ｌ３およびＬ４の値を式（１９）〜式（２２）に代入して連立方程式を解くと、姿勢角度α、β、γを求めることができる。このように求めた姿勢角度α、βおよびγの値を姿勢角度α、βおよびγの推定値αｓ、βｓおよびγｓとする。
次に、姿勢角度制御部２０２の実現方法を説明する。
姿勢角度の指令値α＊、β＊およびγ＊からそれぞれ姿勢角度の推定値αｓ、βｓおよびγｓを差し引いて得た偏差ｅα、ｅβおよびｅγに各々ＰＩＤ補償を掛けてトルク指令ＴＸｃ、ＴＹｃおよびＴＺｃが得られる。
次に、張力指令計算部２０４の実現方法を説明する。
すべてのワイヤが常に完全に伸びている（ワイヤの張力が０以上である）ように制御されるとすると、各ワイヤが可動プレート１２４に働く力のベクトルは式（２８）〜式（３１）のように表される。
【００１９】
【数５】
【００２０】
また、Ａ、Ｂ、Ｃ、Ｄ点の相対座標を用いてベクトル
【００２１】
【数６】
【００２２】
が発生するトルクのベクトルは式（３６）〜式（３９）のように表される。
【００２３】
【数７】
【００２４】
従って、可動プレート１２４に働く総トルクＴのＸ、Ｙ、Ｚ方向の分量は式（４０）〜式（４２）のように表される。
【００２５】
【数８】
【００２６】
ただし、
【００２７】
【数９】
【００２８】
ＴＸ、ＴＹ、ＴＺの代わりにそれぞれＴＸｃ、ＴＹｃ、ＴＺｃを、Ｆ１、Ｆ２、Ｆ３、Ｆ４の代わりにそれぞれＦ１ｃ、Ｆ２ｃ、Ｆ３ｃ、Ｆ４ｃを式（４０）〜式（４２）に代入すると、式（５５）〜式（５７）となる。
【００２９】
【数１０】
【００３０】
姿勢角度α、βおよびγの代わりに姿勢角度推定部２０４で計算した姿勢角度の推定値αｓ、βｓおよびγｓを式（２）〜式（１０）に代入して姿勢変換行列Ｒの各要素を計算し、また、式（１１）〜式（１４）のようにａ、ｂ、ｃ、ｄ点の相対座標を計算する。ａ、ｂ、ｃ、ｄおよびＡ、Ｂ、Ｃ、Ｄ点の相対座標を、式（１９）〜式（２２）に代入して得たＬ１、Ｌ２、Ｌ３およびＬ４と一緒に式（４３）〜式（５４）に代入してｋＸ１〜４、ｋＹ１〜４およびｋＺ１〜４を計算する。そして、姿勢角度制御部２０２から入って来たトルク指令ＴＸｃ、ＴＹｃおよびＴＺｃを式（５５）〜式（５７）に代入すると、式（５５）〜式（５７）がＦ１ｃ、Ｆ２ｃ、Ｆ３ｃ、Ｆ４ｃの線形方程式となる。
一方、非線形バネは変形量が大きくなるほどばね定数が大きくなるものとすると、剛性が高く要求される場合に各ワイヤの張力を大きくすれば良い。それで、剛性の目標指令値η＊に対応する各ワイヤの張力の和Ｆｓの指令値Ｆｓ＊を式（５８）のように与える。
Ｆｓ＊＝ｇ（η＊） （５８）
ただし、ｇ（・）単調増加関数である。
一般的に、式（５５）〜式（５７）に式（５９）を加えて連立方程式を解くと、Ｆ１ｃ、Ｆ２ｃ、Ｆ３ｃ、Ｆ４ｃを一意的に決められる。
Ｆ１ｃ＋Ｆ２ｃ＋Ｆ３ｃ＋Ｆ４ｃ＝Ｆｓ＊ （５９）
ところが、式（５５）〜式（５７）と式（５９）の連立方程式が特異になることもある。その場合に、各変数Ｆ１ｃ、Ｆ２ｃ、Ｆ３ｃ、Ｆ４ｃの前に各々適当な重み係数ｋ１、ｋ２、ｋ３、ｋ４を掛け、式（５９）を式（６０）のように書き直せば良い。
ｋ１Ｆ１ｃ＋ｋ２Ｆ２ｃ＋ｋ３Ｆ３ｃ＋ｋ４Ｆ４ｃ＝Ｆｓ＊ （６０）
また、ある向きの安全性をより重視したい場合は、その向きの剛性を低くするため、その向きに近いワイヤに対応する重み係数を大きく設定すれば良い。
なお、連立方程式を解いて得た張力指令の値が負になった場合に、関数ｇ（・）を式（６１）のように修正を行う。
ｇ（η＊）＝ｇ（η＊）＋Ｆａ＊ （６１）
ただし、Ｆａ＊が適当な正の数である。式（５９）を連立方程式に取り込んだ場合は、Ｆａ＊を負になった変数の和の絶対値の２倍より少し大きい値に与えれば良い。また、修正後の連立方程式は依然負の解が存在する場合に、連立方程式の解がすべて正になるまで以上の修正方法で繰り返して行えば良い。
最後に、張力制御部の実現方法を説明する。
張力制御部にはＰＩ補償器を込みこめば良い。
【００３１】
以下、関節ロボットの動作原理について説明する。
一般的に、姿勢制御のメインループと較べ、張力制御マイナーループには位相遅れが小さいので、張力制御部のゲインを大きく上げられ、張力制御マイナー閉ループの応答特性が高い。すなわち、張力指令の通りにワイヤの張力が制御される。
一方、張力指令が式（５５）〜式（５７）に基づいて求められたので、式（５５）〜式（５７）と式（４０）〜式（４２）とを比較することで移動座標系の各軸まわりのトルクＴＸ、ＴＹ、ＴＺがそれらの指令ＴＸｃ、ＴＹｃ、ＴＺｃに追従することが分かる。すなわち、剛性制御は姿勢制御に影響を与えず、剛性と姿勢を独立かつ正確に制御できる。
また、剛性の制御性能に対する影響が大きく現れるのは出力軸１２５が高加減速かつ低速度で変位する時である。一方、安全性に配慮すべきは出力軸１２５が高速度変位する時である。よって、剛性の目標指令は高速度変位時小さい値、高加減速かつ低速度変位時大きい値に与えれば良い。
【００３２】
このように、関節の自由度からみると冗長なモータと、モータと可動プレートとを連結して力を伝達する非線形バネを取り込んだワイヤとを有する関節ロボットを構成すると共に、姿勢のフィードバック制御に必要なトルク指令と剛性の目標指令とに応じてワイヤの張力指令を与えてワイヤの張力を制御する関節ロボットの制御装置を構築することによって、３自由度の姿勢角度を目標指令に正確に追従させながら、機械の剛性も制御することができる。また、剛性の目標指令を高速度変位時小さい値、高加減速かつ低速度変位時大きい値に与えることにより良好な安全性と優れた制御性能を両立させることができる。
【実施例２】
【００３３】
図３は第２実施例を示す関節ロボットの構成図である。図において、旋回モータ３２０は、回転子の軸中心線が基部３２１の表面に垂直するように固定子が基部３２１の中央に設置されている。巻取モータ３０１、３０２、３０３は、固定子が基部３２１の上方の中心とする円周上の互いに等角度をなす３箇所に設置されている。巻取装置３０４、３０５、３０６は、巻取モータの回転子と一緒に回転しながらワイヤの巻取りをする。ワイヤ３１１、３１２、３１３は、それぞれ非線形バネ３１４、３１５、３１６が組み込まれている。旋回伝動軸３２２は、旋回モータ３２０の回転子に連結されている。可動プレート３２４は、上面の中央には円筒型支持台３２６が設置されている。ユニバーサルジョイント３２３は、旋回伝動軸３２２と出力軸３２５とを連結する。スラスト軸受３２７と３２８は、出力軸３２５を支持台３２６に拘束する。基部３２１の中心を原点ｏとし、旋回伝動軸３２２の中心線をｚ軸として基部３２１の上面から離れる方向をｚ軸の正方向とするように固定直交座標系ｘｙｚｏを設ける。また、ユニバーサルジョイントの中心を原点Ｏとし、出力軸４１９の軸中心線をＺ軸として可動プレート３２４の上面から離れる方向をＺ軸の正方向とし、可動プレート３２４の下面が基部３２１の上面と平行になった際にＸ軸およびＹ軸がそれぞれｘ軸およびｙ軸と平行になるように、可動プレート３２４と一緒に移動するような移動直交座標系ＸＹＺＯを設ける。移動平面座標系ＸＯＹにおける位相がそれぞれ巻取装置３０４、３０５、３０６のワイヤ出入口ａ、ｂ、ｃ点が固定平面座標系ｘｏｙにおける位相と同じであり可動プレート３２４下面の中心とする円周上の互いに等角度をなすＡ、Ｂ、Ｃ点に自在継手３１７、３１８、３１９が設置されている。また、巻取装置３０４と自在継手３１７の間、巻取装置３０５と自在継手３１８の間および巻取装置３０６と自在継手３１９の間にそれぞれ非線形バネ３１４が取り込まれたワイヤ３１１、非線形バネ３１５が取り込まれたワイヤ３１２および非線形バネ３１６が取り込まれたワイヤ３１３が取り付けられている。
【００３４】
以下、動作原理について説明する。
旋回伝動軸３２２が旋回モータ３２０の可動子に固定されたため、その先端に連結されたユニバーサルジョイント３２３の中心位置が変わらないので、ユニバーサルジョイント３２３に連結された出力軸３２５はユニバーサルジョイント３２３の中心点を中心に変位しかできない。すなわち、出力軸３２５の先端はユニバーサルジョイント３２３の中心点を中心に３自由度の球面運動をする。また、支持台３２６と出力軸３２５とはスラスト軸受３２７および３２８によってに拘束されたため、両者の相対運動は出力軸３２５の軸中心の回転運動しかない。よって、支持台３２６および支持台３２６が固定された可動プレート３２４もユニバーサルジョイント３２３の中心点を中心に変位しかできない。
スラスト軸受の摩擦力が十分小さい場合は出力軸３２５と支持台３２６とのＺ軸まわりの回転運動は互いに影響しない。出力軸３２５のＸ軸およびＹ軸方向の姿勢は可動プレート３２４の姿勢に定められるが、Ｚ軸方向の姿勢は完全に旋回モータ３２０の回転角度に決められる。すなわち、３本のワイヤを用いて出力軸３２５の２自由度の運動を操作する。そのため、Ｘ軸およびＹ軸方向の姿勢と剛性を独立に調整できる。一方、Ｚ軸まわりの剛性を調整できないが、Ｚ軸中心の回転の最大半径が小さいため大きなトルクを出力しないので、外部のものに大きな被害を与えない。
【００３５】
以上のように、３本のワイヤを用いて出力トルクが大きくなる方向の姿勢と剛性を独立に調整するため、関節ロボットに良好な安全性と優れた制御性能を両立させることができる。
第１実施例と較べ、本実施例の技術を用いた場合は、構造が若干複雑になるが、可動範囲が広くなる。
【符号の説明】
【００３６】
１００ 関節ロボット
１０１、１０２、１０３、１０４ モータ
１０５、１０６、１０７、１０８ 巻取装置
１１１、１１２、１１３、１１４、３１１、３１２、３１３ ワイヤ
１１５、１１６、１１７、１１８、３１５、３１６、３１７ 非線形バネ
１３１、１３２、１３３、１３４、３１７、３１８、３１９ 自在継手
１２１、３２１ 基部
１２２ 支持柱
１２３ 球面ジョイント
１２４、３２４ 可動プレート
１２５、３２５ 出力軸
１４１、１４２、１４３、１４４ エンコーダー
１５１、１５２、１５３、１５４ 張力センサー
２０１ 目標指令生成部
２０２ 姿勢角度制御部
２０３ 張力指令計算部
２０４ 姿勢角度推定部
２１１、２１２、２１３、２１４ 張力制御部
２２１、２２２、２２３、２２４ モータ駆動部
３０１、３０２、３０３ 巻取モータ
３０４、３０５、３０６ 巻取装置
３２０ 旋回モータ
３２２ 旋回伝動軸
３２３ ユニバーサルジョイント
３２７、３２８ スラスト軸受
【特許請求の範囲】
【請求項１】
基部と、前記基部に設置された４つのモータと、４つの前記モータに各々取り付けられた４つの巻取装置と、前記基部の表面に軸中心線が垂直するように設置された支持柱と、前記支持柱の上端に取り付けられた球面ジョイントと、前記球面ジョイントに取り付けられた可動プレートと、前記可動プレートの底部に設置された４つの自在継手と、４つの前記巻取装置と４つの前記自在継手とを一対一に連結する非線形バネが組み込まれた４本のワイヤと、前記可動プレートの上面に軸中心線が垂直するように固定された出力軸とを備え、 前記球面ジョイントの球体部の球面中心点を原点Ｏとし、前記出力軸の軸中心線をＺ軸として前記可動プレートの上面から離れる方向をＺ軸の正方向とし、前記可動プレートと一緒に移動するような移動直交座標系ＸＹＺＯを設け、
前記支持柱の軸中心線と前記基部の表面との交点を原点ｏとし、前記支持柱の軸中心線をｚ軸として前記基部の表面から離れる方向をｚ軸の正方向とし、前記出力軸の軸中心線が前記支持柱の軸中心線と一致した際にｘ軸およびｙ軸がそれぞれＸ軸およびＹ軸と平行となるように固定直交座標系ｘｙｚｏを設け、
４つの前記巻取装置がｘｏｙ座標系の異なる象限にあるように４つの前記モータを配置し、４つの前記自在継手をＸＯＹ座標系における位相が各々４つの前記巻取装置の前記ｘｏｙ座標系における位相と同じ値になるように配置し、そして前記ｘｏｙ座標系の第１象限にある前記巻取装置と前記ＸＯＹ座標系の第２象限にある前記自在継手を、前記ｘｏｙ座標系の第２象限にある前記巻取装置と前記ＸＯＹ座標系の第１象限にある前記自在継手を、前記ｘｏｙ座標系の第３象限にある前記巻取装置と前記ＸＯＹ座標系の第４象限にある前記自在継手を、前記ｘｏｙ座標系の第４象限にある前記巻取装置と前記ＸＯＹ座標系の第３象限にある前記自在継手を４本の前記ワイヤで各々連結することを特徴とするロボット。
【請求項２】
基部と、固定子が前記基部の上方に固定され回転子の軸中心線が前記基部の表面に平行する３つの巻取モータおよび回転子の軸中心線が基部４２０の表面に垂直する１つの旋回モータと、３つの前記巻取モータに各々取り付けられた３つの巻取装置と、前記旋回モータの可動子に連結された旋回伝動軸と、前記旋回伝動軸の上端に取り付けられたユニバーサルジョイントと、可動プレートと、前記可動プレートの上面の中央に固定された支持台と、２つ以上のスラスト軸受と、３つの自在継手と、３つの前記巻取装置と３つの前記自在継手とを一対一に連結する非線形バネが組み込まれた３本のワイヤと、前記ユニバーサルジョイントに取り付けられた出力軸とを備え、前記旋回モータが前記基部の中央に設置され、３つの前記巻取モータが前記基部の上方の中心とする円周上の互いに等角度をなす３箇所に設置され、３つの前記自在継手が前記可動プレートの下面の中心とする円周上の互いに等角度をなす３箇所に設置され、前記出力軸と前記支持台との間に２つ以上のスラスト軸受によって拘束されることを特徴とするロボット。
【請求項３】
姿勢角度の目標指令と剛性の目標指令とを生成する目標指令生成部と、前記各モータの回転角度を検出するエンコーダーと、前記各ワイヤの張力を検出する張力センサーとを備え、各ワイヤの張力指令と前記各ワイヤの張力信号とに基づいて張力の制御を行いながら、前記姿勢角度の目標指令と前記剛性の目標指令および前記各モータの回転角度信号に基づいて前記各ワイヤの張力指令を生成し前記関節ロボットの姿勢角度および剛性を制御し、
前記各モータの回転角度信号および前記各ワイヤの張力信号に基づいて姿勢角度の推定値を算出する姿勢角度推定部と、
前記姿勢角度の目標指令と前記姿勢角度の推定値との偏差を入力しトルク指令を出力する姿勢角度制御部と、
前記トルク指令と前記剛性の目標指令および前記姿勢角度の推定値に基づいて張力指令を算出する張力指令計算部とを備えたことを特徴とするロボットの制御装置。
【請求項１】
基部と、前記基部に設置された４つのモータと、４つの前記モータに各々取り付けられた４つの巻取装置と、前記基部の表面に軸中心線が垂直するように設置された支持柱と、前記支持柱の上端に取り付けられた球面ジョイントと、前記球面ジョイントに取り付けられた可動プレートと、前記可動プレートの底部に設置された４つの自在継手と、４つの前記巻取装置と４つの前記自在継手とを一対一に連結する非線形バネが組み込まれた４本のワイヤと、前記可動プレートの上面に軸中心線が垂直するように固定された出力軸とを備え、 前記球面ジョイントの球体部の球面中心点を原点Ｏとし、前記出力軸の軸中心線をＺ軸として前記可動プレートの上面から離れる方向をＺ軸の正方向とし、前記可動プレートと一緒に移動するような移動直交座標系ＸＹＺＯを設け、
前記支持柱の軸中心線と前記基部の表面との交点を原点ｏとし、前記支持柱の軸中心線をｚ軸として前記基部の表面から離れる方向をｚ軸の正方向とし、前記出力軸の軸中心線が前記支持柱の軸中心線と一致した際にｘ軸およびｙ軸がそれぞれＸ軸およびＹ軸と平行となるように固定直交座標系ｘｙｚｏを設け、
４つの前記巻取装置がｘｏｙ座標系の異なる象限にあるように４つの前記モータを配置し、４つの前記自在継手をＸＯＹ座標系における位相が各々４つの前記巻取装置の前記ｘｏｙ座標系における位相と同じ値になるように配置し、そして前記ｘｏｙ座標系の第１象限にある前記巻取装置と前記ＸＯＹ座標系の第２象限にある前記自在継手を、前記ｘｏｙ座標系の第２象限にある前記巻取装置と前記ＸＯＹ座標系の第１象限にある前記自在継手を、前記ｘｏｙ座標系の第３象限にある前記巻取装置と前記ＸＯＹ座標系の第４象限にある前記自在継手を、前記ｘｏｙ座標系の第４象限にある前記巻取装置と前記ＸＯＹ座標系の第３象限にある前記自在継手を４本の前記ワイヤで各々連結することを特徴とするロボット。
【請求項２】
基部と、固定子が前記基部の上方に固定され回転子の軸中心線が前記基部の表面に平行する３つの巻取モータおよび回転子の軸中心線が基部４２０の表面に垂直する１つの旋回モータと、３つの前記巻取モータに各々取り付けられた３つの巻取装置と、前記旋回モータの可動子に連結された旋回伝動軸と、前記旋回伝動軸の上端に取り付けられたユニバーサルジョイントと、可動プレートと、前記可動プレートの上面の中央に固定された支持台と、２つ以上のスラスト軸受と、３つの自在継手と、３つの前記巻取装置と３つの前記自在継手とを一対一に連結する非線形バネが組み込まれた３本のワイヤと、前記ユニバーサルジョイントに取り付けられた出力軸とを備え、前記旋回モータが前記基部の中央に設置され、３つの前記巻取モータが前記基部の上方の中心とする円周上の互いに等角度をなす３箇所に設置され、３つの前記自在継手が前記可動プレートの下面の中心とする円周上の互いに等角度をなす３箇所に設置され、前記出力軸と前記支持台との間に２つ以上のスラスト軸受によって拘束されることを特徴とするロボット。
【請求項３】
姿勢角度の目標指令と剛性の目標指令とを生成する目標指令生成部と、前記各モータの回転角度を検出するエンコーダーと、前記各ワイヤの張力を検出する張力センサーとを備え、各ワイヤの張力指令と前記各ワイヤの張力信号とに基づいて張力の制御を行いながら、前記姿勢角度の目標指令と前記剛性の目標指令および前記各モータの回転角度信号に基づいて前記各ワイヤの張力指令を生成し前記関節ロボットの姿勢角度および剛性を制御し、
前記各モータの回転角度信号および前記各ワイヤの張力信号に基づいて姿勢角度の推定値を算出する姿勢角度推定部と、
前記姿勢角度の目標指令と前記姿勢角度の推定値との偏差を入力しトルク指令を出力する姿勢角度制御部と、
前記トルク指令と前記剛性の目標指令および前記姿勢角度の推定値に基づいて張力指令を算出する張力指令計算部とを備えたことを特徴とするロボットの制御装置。
【図１】
【図２】
【図３】
【図２】
【図３】
【公開番号】特開２０１１−１０１９３８（Ｐ２０１１−１０１９３８Ａ）
【公開日】平成２３年５月２６日（２０１１．５．２６）
【国際特許分類】
【出願番号】特願２００９−２５８５５８（Ｐ２００９−２５８５５８）
【出願日】平成２１年１１月１２日（２００９．１１．１２）
【出願人】（０００００６６２２）株式会社安川電機 (2,482)
【Ｆターム（参考）】
【公開日】平成２３年５月２６日（２０１１．５．２６）
【国際特許分類】
【出願日】平成２１年１１月１２日（２００９．１１．１２）
【出願人】（０００００６６２２）株式会社安川電機 (2,482)
【Ｆターム（参考）】
[ Back to top ]