JP7082365B2 - パラレルリンクロボット - Google Patents

Description

本発明は、産業用ロボットの１つであるパラレルリンクロボットに関するものである。

パラレルリンクロボットの機構は、例えばロボットを固定する基台に固定されたモータ装置と、モータ装置の可動部に連結されたアーム部を有するリンク機構と、リンク機構に連結されエンドエフェクタなどの作用部材が取り付けられて直交三軸方向へ移動可能な移動部（トラベリング部）で構成されている。

パラレルリンクロボットは、直列に関節機構を配置したものと比較して、アーム部を繋ぐ関節毎にモータを設ける必要がなく、各アーム部と各モータを振り回す必要もないため軽量にできるという利点がある。また、パラレルリンクロボットは、アーム部を三角錐の構造にすることで非常に剛性が高くすることができ、エンドエフェクタを非常に高速で動かすことができる。

特開２００７－２７２１１６号公報 特開２０１５－１４２９５２号公報

このような構成を有するパラレルリンクロボットを機構部品等の組立てに使用する場合、エンドエフェクタにおいて押付力など、力を制御する使い方が望まれている。そこで特許文献１、特許文献２に開示されているように、エンドエフェクタに力センサを取り付けてエンドエフェクタに加わる反力を用いてフィードバック制御を行う方法が公知である。

しかしながらこの方法では、パラレルリンクロボットのエンドエフェクタ付近に力センサを取り付けなければならないので、エンドエフェクタの重量が増加すると共に形状が制限されてしまい用途が限定されてしまうという課題があった。そしてアーム部が障害物に当たっても力センサではこれを検出しないので、人と協調作業をする工程にパラレルリンクロボットを配置することはできなかった。そこで発明者は、パラレルリンクロボットの各モータと一体的にトルクセンサを配置して、エンドエフェクタの力の制御を行う開発を行っている。

本発明は上記のような問題点に鑑みなされたもので、構造的な制限を取り払い、さらにエンドエフェクタの力の制御を高精度に行うことを可能にするパラレルリンクロボットを提供するのが目的である。

上記の目的を達成するために、本発明のパラレルリンクロボットは、
少なくとも３ つ以上の複数のモータ部と、各モータ部の回転位置を検出して回転位置検出値を出力する回転位置検出部と、各モータ部の出力軸のねじりトルクを検出してトルク検出値を出力するトルク検出部と、を有して基台に固定される複数のモータ装置に、エンドエフェクタを取り付ける移動部が複数のリンク機構によって並列に連結されたパラレルリンクロボットであって、
各モータ装置を制御する制御手段は、
移動部に発生させる力の目標値と、各モータ部の各回転位置検出値と、を入力として、各モータ部へ指令する各トルク指令値を演算して出力する指令値演算手段と、
各トルク指令値から各トルク検出値をそれぞれ減算してトルク偏差を出力するトルク値減算手段と、
トルク偏差を入力として各モータ部へ制御指令を出力するモータ制御手段と、
を含んで構成されている。

そして、指令値演算手段が、トルク指令値を、各モータ部の回転位置座標変数の微分を移動部の位置座標変数の微分に変換するヤコビ行列の転置行列を力の目標値に乗算して出力するように構成されている。

また、ヤコビ行列が、各モータ部の現在の各回転位置から所定距離の１／２離間した前後の２つの位置に対応したそれぞれの移動部の位置の差を所定距離で除算して得られるように構成されている。

また、指令値演算手段が、移動部の移動先の目標値と、各モータ部の各回転位置検出値と、を入力として各回転位置指令値を演算して出力し、
制御手段は、
各回転位置指令値から各回転位置検出値をそれぞれ減算して回転位置偏差を出力する回転位置値減算手段と、
トルク値減算手段からの出力と、回転位置値減算手段からの出力とを切換える制御切換え手段と、を含み、
モータ制御手段は、制御切換え手段によって切換えられた回転位置偏差若しくはトルク偏差のいずれかに基づいて各モータ部へ制御指令を出力するように構成されている。

また、指令値演算手段が、制御切換え手段による切換え状態に応じて、速度制限値をモータ制御手段へ出力し、
モータ制御手段は、速度制限値に基づいて各モータ部へ制御指令を出力するように構成されている。

本発明のパラレルリンクロボットによれば、移動部に発生させる力の目標値と、各モータ部の各回転位置検出値とを入力として、各モータ部へ指令する各トルク指令値を演算して出力する指令値演算手段を設け、各トルク検出値を減算してフィードバック制御を行っていることから、エンドエフェクタに係る構造的な制限を取り払い、さらにエンドエフェクタの力の制御を高精度に行うことができる。

本発明の実施形態に係るパラレルリンクロボットの全体斜視図 本発明の実施形態に係るパラレルリンクロボットの機構部斜視図 本発明の実施形態に係るパラレルリンクロボットの制御ブロック図 本発明の実施形態に係るパラレルリンクロボットの機構部側面図

以下、本発明のパラレルリンクロボットを実施形態にて、図面を基に詳細な説明を行う。図１は本発明の実施形態に係るパラレルリンクロボット１８を斜め上側から見た斜視図であり、図２は斜め下側から見た斜視図である。パラレルリンクロボット１８は、基台１に取り付けられた機構部と制御手段１００とで構成されている。図１では機構部材と制御手段１００との電気的接続は便宜上３本の線で描かれているが、実際には多芯のケーブル等によって複数の信号線の送受信や電源供給が行われている。

まずパラレルリンクロボット１８のメカニカルな部分から説明する。基台１は機構部を支持固定する平板状の金属部材であって、フレーム等を組んだ構造物に固定される。基台１の下面側には、３つのモータ装置１１ａ～１１ｃが、基台１の下側平面のある一点を中心として、１２０度の等間隔でかつ半径方向に同一寸法で放射状に、モータ部ブラケット１３ａ～１３ｃによって固定配置されている。

モータ装置１１ａ～１１ｃは、モータ部２ａ～２ｃと、出力軸１２ａ～１２ｃのねじりトルクを検出してトルク信号を出力するトルク検出部４ａ～４ｃと、モータ部２ａ～２ｃの反負荷側にあって回転位置を検出して回転位置信号を出力する回転位置検出部３ａ～３ｃと、を有して構成されている。なおこのモータ部２ａ～２ｃとは、例えばモータと減速機を組み合わせたものである。そしてモータ部２ａ～２ｃに含まれるモータは例えば交流サーボモータである。またモータ部２ａ～２ｃに含まれる減速機は、正逆両方向に細かく回ってトルク制御を行うことから、例えばバックラッシュが基本的にない波動減速機を用いることが好ましい。

回転位置検出部３ａ～３ｃはいわゆる光学式エンコーダであって、モータ部２ａ～２ｃの回転位置を検出して回転位置検出値を出力するものである。本実施形態では反射型光学式であるが、透過式であってもよいし、光学式に限らず磁力その他の方式によるものであっても良い。

トルク検出部４ａ～４ｃは、モータ部２ａ～２ｃからの出力軸１２ａ～１２ｃに弾性変形をする起歪部を設け、その起歪部に歪みゲージを配置し、出力軸１２ａ～１２ｃが変形した際の歪み量を電気信号に変換してねじりトルクを得てトルク検出値として出力するものである。このモータ装置１１ａ～１１ｃの出力軸１２ａ～１２ｃに継続的な負荷が掛かることから、軸自体が時間に伴うクリープ変形を起こすことがあり、磁歪方式や光学的に歪みを検出する方式では正確なトルク値が得られない場合がある。磁歪方式や光学的に歪みを検出する方式と比較して歪みゲージを用いて検出を行うと、温度変化とクリープ変形に対する自己補償特性を付与することができるので、高精度なねじりトルク値が得られる。

モータ装置１１ａ～１１ｃの出力軸１２ａ～１２ｃにはそれぞれ棒状の第１アーム１５ａ～１５ｃが連結されている。第１アーム１５ａ～１５ｃの腕方向の長さはそれぞれ同一寸法で構成され、第１アーム１５ａ～１５ｃの一端が接続部５ａ～５ｃで出力軸１２ａ～１２ｃに規制された角度範囲で回転できるように接続されている。そして第１アーム１５ａ～１５ｃの他端側には関節６ａ～６ｃ及び関節７ａ～７ｃを介して略平行に２本並んだ棒状の第２アーム１６ａ～１６ｃがそれぞれ連結されている。第２アーム１６ａ～１６ｃの腕の長さもそれぞれ同一寸法で構成されている。

そして第２アーム１６ａ～１６ｃは、関節６ａ～６ｃ及び関節７ａ～７ｃを介して移動部１４に連結されている。これらの関節６ａ～６ｃ及び関節７ａ～７ｃにより、第２アーム１６ａ～１６ｃと移動部１４は、揺動可能及び規制された角度範囲で回転可能となっている。なお接続部５ａ～５ｃ、関節６ａ～６ｃ及び関節７ａ～７ｃは、例えばボールやベアリング等を有したジョイントである。

移動部１４は直交三軸方向に移動可能な板状の部材であって、移動部１４の中心から１２０度の角度間隔の箇所に、関節６ａ～６ｃ及び関節７ａ～７ｃを介して第２アーム１６ａ～１６ｃが接続される。ゆえに、これらの接続部５ａ～５ｃ、関節６ａ～６ｃ、関節７ａ～７ｃ、第１アーム１５ａ～１５ｃ、第２アーム１６ａ～１６ｃは、モータ装置１１ａ～１１ｃと移動部１４を並列に連結するリンク機構１７である。移動部１４には様々なエンドエフェクタ（不図示）が取り付けられる。本実施形態ではこの移動部１４に力センサは取り付けを行わない構成であって、エンドエフェクタの形状や取り付け位置に対する制約が少ない。しかも移動部１４及びエンドエフェクタ全体部の重量を軽くすることができて、これらを高速で駆動することが可能である。

次いで図３を参照してこのパラレルリンクロボット１８の制御について説明する。パラレルリンクロボット１８の制御は制御手段１００によって行われる。制御手段１００は、指令値演算手段１０６と、トルク値減算手段１０４ａ～１０４ｃと、回転位置値減算手段１０５ａ～１０５ｃと、制御切換え手段１０３ａ～１０３ｃと、モータ制御手段１０２ａ～１０２ｃと、駆動手段１０１ａ～１０１ｃとを含んで構成されている。

指令値演算手段１０６は、各回転位置検出部３ａ～３ｃから出力された回転位置検出値θ_ａ、θ_ｂ、θ_ｃと、移動部１４の移動先の目標位置Ｐ_ｒｅｆと、移動部１４に発生させたい力の目標値Ｆ_ｒｅｆと、を入力として、各モータ部２ａ～２ｃを制御するための指令値を算出する演算を行う。演算の詳細については後述する。

指令値演算手段１０６は、各モータ部２ａ～２ｃを制御するための指令値として、各トルク指令値τ_{ａ ｒｅｆ}、τ_{ｂ ｒｅｆ}、τ_{ｃ ｒｅｆ}、各回転位置指令値θ_{ａ ｒｅｆ}、θ_{ｂ ｒｅｆ}、θ_{ｃ ｒｅｆ}、各速度制限値ｖ_{ａ ｌｉｍｉｔ}、ｖ_{ｂ ｌｉｍｉｔ}、ｖ_{ｃ ｌｉｍｉｔ}、の信号を出力する。以下モータ装置１１ａ～１１ｃの制御は同様なので、モータ装置１１ａに関連した部分を代表して説明する。

トルク値減算手段１０４ａは、指令値演算手段１０６より出力されたトルク指令値τ_{ａ ｒｅｆ}からトルク検出値τ_ａを減算してトルク偏差を出力する。一方、回転位置値減算手段１０５ａは、指令値演算手段１０６より出力された回転位置指令値θ_{ａ ｒｅｆ}から回転位置検出値θ_ａを減算して回転位置偏差を出力する。

制御切換え手段１０３ａは、トルク値減算手段１０４ａから出力されたトルク偏差と、回転位置値減算手段１０５ａから出力された回転位置偏差とを入力として、いずれかを選択するように切換えて出力する。そしてこの切換えは、制御切換え手段１０３ａ～１０３ｃで同時に行われ、制御切換え手段１０３ａ～１０３ｃはトルク偏差若しくは回転位置偏差のいずれかに一斉に切換えて出力する。

モータ制御手段１０２ａは、制御切換え手段１０３ａ～１０３ｃで切換えられたトルク偏差若しくは回転位置偏差のいずれかを入力として、この偏差をゼロにする制御の演算を行う。これと同時にモータ制御手段１０２ａは、指令値演算手段１０６からの速度制限値ｖ_{ａ ｌｉｍｉｔ}を入力として、モータ部２ａの速度が速度制限値ｖ_{ａ ｌｉｍｉｔ}を越えないようにする制御の演算も行う。なお指令値演算手段１０６が出力するモータ部２ａの速度制限値ｖ_{ａ ｌｉｍｉｔ}は、制御切換え手段１０３ａ～１０３ｃの切換え状態に応じたものである。そしてモータ制御手段１０２ａは、駆動手段１０１ａへモータ部２ａを駆動する制御指令値を出力し、駆動手段１０１ａはモータ部２ａを電流制御により駆動する。前述したようにモータ装置１１ｂ～１１ｃの制御もモータ装置１１ａと同様である。

次いで図４を参照して、指令値演算手段１０６が各トルク指令値τ_{ａ ｒｅｆ}、τ_{ｂ ｒｅｆ}、τ_{ｃ ｒｅｆ}を算出する手法を説明する。

移動部１４に加わる外力をＦ＝（Ｆ_ｘ，Ｆ_ｙ，Ｆ_ｚ）^Ｔ、
モータ部２ａ～２ｃの駆動力（トルク）をτ＝（τ_ａ，τ_ｂ，τ_ｃ）^Ｔ、
各モータ部２ａ～２ｃの座標をθ＝（θ_ａ，θ_ｂ，θ_ｃ）^Ｔ、
各モータ部２ａ～２ｃの仮想変位をδθ＝（δθ_ａ，δθ_ｂ，δθ_ｃ）^Ｔ、と定義すると、各モータ部２ａ～２ｃの駆動力による仮想仕事δＷ_τは、式１にて表される。

そして移動部１４の各仮想変位をδｐ＝（δｘ，δｙ，δｚ）^Ｔと定義する。

すると移動部１４へ加わる外力による仮想仕事は、式２で表される。

ここでδＷ_τ＝－δＷ_ｆ であるから式１と式２から式３が導かれる。

一方、モータ部の回転位置座標系θ＝φの微分を移動部１４の位置座標系ｐの微分へ変換するヤコビ行列をＪ（φ）とすると、δｐ＝Ｊ（φ）δθであるから、これの転置を取って δｐ^Ｔ＝（Ｊ（φ）δθ）^Ｔ＝δθ^Ｔ・Ｊ（φ）^Ｔ、となり、式４が導かれる。

したがってトルク指令値τ_ｒｅｆは、各モータ部２ａ～２ｃの各回転位置座標変数の微分を移動部１４の位置座標変数の微分に変換するヤコビ行列Ｊ（φ）の転置行列を力の目標値Ｆ_ｒｅｆに乗算することで得ることができる。ゆえにトルク指令値τ_ｒｅｆは式５で表すことができる。

次にＪ（φ）^Ｔを求める方法について説明する。移動部１４の座標系は各モータ部２ａ～２ｃの座標系の関数ｆで表される。（ｘ，ｙ，ｚ）＝ｆ（θ_ａ，θ_ｂ，θ_ｃ）この関数ｆは幾何学的に設計値から算出される。移動部１４の座標系は、モータ装置１１ａ～１１ｃの出力軸１２ａ～１２ｃの軸線を含むＸＹ平面と、このＸＹ平面に直交して鉛直上方向を正とするＺ軸で定義されるものである。

各関節点Ｊ_ａ（ｘ_ａ，ｙ_ａ，ｚ_ａ）、Ｊ_ｂ（ｘ_ｂ，ｙ_ｂ，ｚ_ｂ）、Ｊ_ｃ（ｘ_ｃ，ｙ_ｃ，ｚ_ｃ）を中心とする半径Ｒ_ａ、Ｒ_ｂ、Ｒ_ｃの球の交点Ｑ（ｘ，ｙ，ｚ）は次の式６～８の３元連立方程式を解くことで求められる。なお本実施形態のパラレルリンクロボット１８において、３つのモータ装置１１ａ～１１ｃの出力軸１２ａ～１２ｃの軸線それぞれと、３つの第１アーム１５ａ～１５ｃの軸線の交点（モータ装置１１ａ～１１ｃの配置の中心）と、をＸＹ平面上に投影したそれぞれの距離は、移動部１４及び第２アーム１６ａ～１６ｃを連結している関節６ａ～６ｃの軸線それぞれと、移動部１４の中心点とをＸＹ平面上に投影したそれぞれの距離と全て同じ寸法である。もしこれらの距離を異なるものにした場合はその分のオフセットを幾何学的に行えば良い。

これらの式７及び式８から式６を減算して整理してｘ＝Ａ＋Ｂｚ、ｙ＝Ｃ＋Ｄｚの形式で表し、これを式６に代入して整理することでｚに関する２次方程式が得られる。２次方程式を解くためｚの座標は２つ得られるが、各関節点は負の領域のみの移動であるから、負の座標の方を使用し、このｚから球の交点Ｑ（ｘ，ｙ，ｚ）を求めることができる。

指令値演算手段１０６は、現在のモータ部２ａ、２ｂ、２ｃの座標軸φ＝（φ_ａ、φ_ｂ、φ_ｃ）から所定距離δφの１／２、すなわちδφ／２離間した近傍点を以下のようなψ_１～ψ_６の６点で定義する。
ψ_１＝（φ_ａ＋δφ／２，φ_ｂ，φ_ｃ）、ψ_２＝（φ_ａ―δφ／２，φ_ｂ，φ_ｃ）、
ψ_３＝（φ_ａ，φ_ｂ＋δφ／２，φ_ｃ）、ψ_４＝（φ_ａ，φ_ｂ－δφ／２，φ_ｃ）、
ψ_５＝（φ_ａ，φ_ｂ，φ_ｃ＋δφ／２）、ψ_６＝（φ_ａ，φ_ｂ，φ_ｃ－δφ／２）。この６点それぞれに対応する移動部１４の位置Ｐ_ｉは、前述の球の交点を求める方法を用いて、Ｐ_ｉ＝（ｘ_ｉ、ｙ_ｉ、ｚ_ｉ）＝ｆ（ψ_ｉ） （ｉ＝１～６）で求めることができる。ゆえにＪ（φ）はδφ／２だけ離れた近傍２点でのｆ（ψ_ｉ）の差を所定距離δφで除算し、その各ｘ、ｙ、ｚ成分を演算することで求めることができる（式９）。但しｆ_ｘ（ψ_ｉ）、ｆ_ｙ（ψ_ｉ）、ｆ_ｚ（ψ_ｉ） （ｉ＝１～６）はそれぞれｆ（ψ_ｉ）のｘ、ｙ、ｚ成分である。

最終的には次式を計算することになる。

本実施形態においてはδφ＝１ｘ１０^－６程度の値を用いてＪ（φ）を求めている。この方法では現在位置の前後の位置から割り出しているため、精度の高い指令値を求めることができる。

ゆえに本発明の実施形態によれば、指令値演算手段１０６は、各回転位置検出部３ａ～３ｃから出力された回転位置検出値θ_ａ、θ_ｂ、θ_ｃと、移動部１４を移動させたい目標位置Ｐ_ｒｅｆと、移動部１４に発生させたい力の目標値Ｆ_ｒｅｆと、を入力として、上記式により演算を行って、各トルク指令値τ_{ａ ｒｅｆ}、τ_{ｂ ｒｅｆ}、τ_{ｃ ｒｅｆ}、各回転位置指令値θ_{ａ ｒｅｆ}、θ_{ｂ ｒｅｆ}、θ_{ｃ ｒｅｆ}、の信号を出力する。制御切換え手段１０３ａは、一例として移動部１４が目標位置Ｐ_ｒｅｆに到達するまでは各回転位置指令値θ_{ａ ｒｅｆ}、θ_{ｂ ｒｅｆ}、θ_{ｃ ｒｅｆ}、による位置制御を選択し、移動部１４が目標位置Ｐ_ｒｅｆに到達した時点で、各トルク指令値τ_{ａ ｒｅｆ}、τ_{ｂ ｒｅｆ}、τ_{ｃ ｒｅｆ}による力制御（トルク制御）を行うような切換えを行う。

また制御切換え手段１０３ａが、変形例として移動部１４が目標位置Ｐ_ｒｅｆに到達するまでの期間に位置制御とゼロの力を目標とした力制御との切換えを高速で頻繁に行うと、移動部１４が障害物に衝突したような場合にも瞬時に力を緩めることができる。このような制御方法でパラレルリンクロボット１８の各アーム、各リンク機構及び対象物の破損などを防止することができる。そして移動部１４が人間に当たった場合にも瞬時に力を緩めることができるので、人との協調作業を行うことも可能であり、パラレルリンクロボット１８の配置の自由度を高めることができる。

さらに移動部１４には力センサを設けていないことから、軽量で高速な動作も可能で、移動部１４に取り付けられるエンドエフェクタの形状に制限が少なく、また力の目標値Ｆ_ｒｅｆはｘ、ｙ、ｚ方向に自由に設定できるため、ピックアンドプレイスのみならず、鉛直面の孔への部品の圧入、立体的な部品への接着剤や塗料の刷毛塗りなど力の制御を必要とする広い範囲の作業に適用可能なパラレルリンクロボットを提供できる。

以上、本発明に関する実施形態について説明したが、本発明は上記の実施形態に限定されることではなく、種々の変形が可能である。

本発明の活用例として、部品の圧入、接着剤や塗料の塗布など力の制御を必要とする装置への適用が可能である。

１ ：基台
２ａ、２ｂ、２ｃ ：モータ部
３ａ、３ｂ、３ｃ ：回転位置検出部
４ａ、４ｂ、４ｃ ：トルク検出部
５ａ、５ｂ、５ｃ ：接続部
６ａ、６ｂ、６ｃ ：関節
７ａ、７ｂ、７ｃ ：関節
１１ａ、１１ｂ、１１ｃ ：モータ装置
１２ａ、１２ｂ、１２ｃ ：出力軸
１３ａ、１３ｂ、１３ｃ ：モータ部ブラケット
１４ ：移動部
１５ａ、１５ｂ、１５ｃ ：第１アーム
１６ａ、１６ｂ、１６ｃ ：第２アーム
１７ ：リンク機構
１８ ：パラレルリンクロボット
１００ ：制御手段
１０１ａ、１０１ｂ、１０１ｃ ：駆動手段
１０２ａ、１０２ｂ、１０２ｃ ：モータ制御手段
１０３ａ、１０３ｂ、１０３ｃ ：制御切換え手段
１０４ａ、１０４ｂ、１０４ｃ ：トルク値減算手段
１０５ａ、１０５ｂ、１０５ｃ ：回転位置値減算手段
１０６ ：指令値演算手段

Claims (2)

1. 少なくとも３つ以上の複数のモータ部と、前記各モータ部の回転位置を検出して回転位置検出値を出力する回転位置検出部と、前記各モータ部の出力軸のねじりトルクを検出してトルク検出値を出力するトルク検出部と、を有して基台に固定される複数のモータ装置に、エンドエフェクタを取り付ける移動部が複数のリンク機構によって並列に連結されたパラレルリンクロボットであって、
   前記各モータ装置を制御する制御手段は、
   前記移動部に発生させる力の目標値と、前記各モータ部の前記各回転位置検出値と、を入力として、前記各モータ部へ指令する各トルク指令値を演算して出力する指令値演算手段と、
   前記各トルク指令値から前記各トルク検出値をそれぞれ減算してトルク偏差を出力するトルク値減算手段と、
   前記トルク偏差を入力として前記各モータ部へ制御指令を出力するモータ制御手段と、を含み、
   前記指令値演算手段が、前記移動部の移動先の目標値と、前記各モータ部の前記各回転位置検出値と、を入力として各回転位置指令値を演算して出力し、
   前記制御手段は、
   前記各回転位置指令値から前記各回転位置検出値をそれぞれ減算して回転位置偏差を出力する回転位置値減算手段と、
   前記トルク値減算手段からの出力と、前記回転位置値減算手段からの出力とを切換える制御切換え手段と、を含み、
   前記モータ制御手段は、前記制御切換え手段によって切換えられた前記回転位置偏差若しくは前記トルク偏差のいずれかに基づいて前記各モータ部へ制御指令を出力することを特徴とするパラレルリンクロボット。
2. 前記指令値演算手段が、前記制御切換え手段による切換え状態に応じて、速度制限値を前記モータ制御手段へ出力し、
   前記モータ制御手段は、前記速度制限値に基づいて前記各モータ部へ制御指令を出力することを特徴とする請求項１に記載のパラレルリンクロボット。
   

Images