JP4168441B2 - Transport device - Google Patents
Transport device Download PDFInfo
- Publication number
- JP4168441B2 JP4168441B2 JP2003275446A JP2003275446A JP4168441B2 JP 4168441 B2 JP4168441 B2 JP 4168441B2 JP 2003275446 A JP2003275446 A JP 2003275446A JP 2003275446 A JP2003275446 A JP 2003275446A JP 4168441 B2 JP4168441 B2 JP 4168441B2
- Authority
- JP
- Japan
- Prior art keywords
- force
- coordinate system
- axis
- force sensor
- manipulator
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Expired - Fee Related
Links
Images
Landscapes
- Manipulator (AREA)
Description
本発明は、重量物や長尺物をロボットマニピュレータと人間が協調して搬送する搬送装置に関する。 The present invention relates to a transport device that transports heavy and long objects in cooperation between a robot manipulator and a human.
搬送対象物の一方の端をマニピュレータが、他方の端を人間が把持して、人間の腕力による操作をマニピュレータがアシストするように、マニピュレータと人間が協調して対象物を搬送する装置が知られている。この搬送装置はマニピュレータ手先に並進力およびモーメントを検出するセンサを備えて、前記センサで人間が対象物に加えた操作力を検出して、人間の操作をアシストする方向にマニピュレータが動作するような動作指令を作成している（例えば、特許文献１）。
図３は従来の搬送装置の構成図である。図において、３１はマニピュレータであり、ハンド３２と操作者３３がワーク３４を把持し、操作者３３がワーク３４に加えた操作力を力センサ３５で検出してワーク３４を搬送する。特許文献１に記載の発明では、Ｘ軸並進方向は力センサのＦｘの値に基づいて移動量を計算する。また、Ｙ軸方向はＺ軸周りのトルク検出値Ｍｚで、Ｚ軸方向はＹ軸周りのトルク検出値Ｍｙで移動量を計算する。またＸ軸周りの回転方向はＸ軸周りのトルク検出値Ｍｘで、Ｙ軸周りはＭｙで、Ｚ軸周りはＭｚに基づいて移動量を計算している。
また、特許文献２および３には、Ｙ軸方向には並進運動が生じないように移動量の計算値を制限することで、操作者が操作しやすいようにした搬送装置の発明が開示されている。
このように、従来の多くの搬送装置は、１台のマニピュレータに搭載した力検出部で人間がワークに加えた力・トルクを検出し、操作力を演算してワークを協調搬送するものである。
一方、２台のマニピュレータに搭載した力検出部で人間がワークに加えた力・トルクを検出して１つの物体を搬送する装置の例は非特許文献１に開示されている。これは各マニピュレータの力検出部で検出された外力に応じて、それぞれに設定された仮想インピーダンスモデルに基づいてマニピュレータを制御する。
FIG. 3 is a configuration diagram of a conventional transport device. In the figure, reference numeral 31 denotes a manipulator, in which a hand 32 and an operator 33 grip a work 34, and an operation force applied to the work 34 by the operator 33 is detected by a force sensor 35 to convey the work 34. In the invention described in Patent Document 1, the amount of movement in the X-axis translation direction is calculated based on the value of Fx of the force sensor. Further, the amount of movement is calculated using the detected torque value Mz around the Z axis in the Y-axis direction and the detected torque value My around the Y-axis in the Z-axis direction. The rotation direction around the X axis is the torque detection value Mx around the X axis, My around the Y axis is My, and the movement around the Z axis is calculated based on Mz.
Further, Patent Documents 2 and 3 disclose inventions of a transport device that makes it easy for an operator to operate by limiting the calculated value of the movement amount so that translational movement does not occur in the Y-axis direction. Yes.
As described above, many conventional conveying devices detect force / torque applied to a workpiece by a force detection unit mounted on one manipulator, calculate an operation force, and cooperatively convey the workpiece. .
On the other hand, Non-Patent Document 1 discloses an example of an apparatus that detects a force / torque applied to a workpiece by a force detection unit mounted on two manipulators and conveys one object. This controls the manipulator based on the virtual impedance model set for each according to the external force detected by the force detector of each manipulator.
実際にマニピュレータの力を利用して人間をアシストして搬送するワークは重量物や長尺物が多く、マニピュレータ１台のみでワークの片端を把持して安定して搬送することが難しいことがある。ところが従来の搬送装置は、１台のマニピュレータが前提となっており、上記従来例をそのまま２台のマニピュレータに適用することができない。特に、人間と協調してワークを搬送するためにロボットを人間型の双腕マニピュレータ型に構成し、ワークの両端を人間とロボット双腕が向かい合って把持し作業を行う場合がある。このとき、長手方向（ロボットに向き合っている方向）には操作が容易であるが、その他の並進・回転方向に直接操作力を加えることが困難である。特にロボットと操作者との距離が遠くなればなるほどこれが困難になってくる。このため、上記１台のマニピュレータを用いた従来例をそのまま２台のマニピュレータに適用しただけでは搬送動作をすることができないという問題があった。また２台のマニピュレータそれぞれの力検出部で検出した外力に応じて制御する方法も、人間が意図する方向にそれぞれの力検出部に外力を加えることが可能であることが前提であるが、実際は例えば長手方向と直交した横方向などに操作力を加えても、それぞれの力検出部では長手方向の偶力が検出されるのみであり、この手法はすべての方向に適用できるものではなかった。本発明はこのような問題点に鑑みてなされたものであり、２台の並列に置かれたマニピュレータでワークを把持し、人間がワークに加えた操作力で、３次元空間上の任意の方向に搬送動作することができる搬送装置を提供することを目的とする。 There are many heavy and long workpieces that are actually conveyed by assisting humans using the force of the manipulator, and it may be difficult to stably convey the workpiece by gripping one end of the workpiece with only one manipulator. . However, the conventional transfer device is premised on one manipulator, and the above-described conventional example cannot be applied to two manipulators as they are. In particular, there is a case where a robot is configured as a human-type dual-arm manipulator type in order to transport a workpiece in cooperation with a human, and the work is performed by gripping both ends of the workpiece with the human and the robot dual-arm facing each other. At this time, the operation is easy in the longitudinal direction (the direction facing the robot), but it is difficult to directly apply the operation force to the other translation / rotation directions. This becomes more difficult as the distance between the robot and the operator increases. For this reason, there is a problem that the conventional operation using one manipulator cannot be transported by simply applying it to two manipulators. In addition, the control method according to the external force detected by the force detection unit of each of the two manipulators is also based on the premise that it is possible to apply an external force to each force detection unit in the direction intended by humans. For example, even if an operating force is applied in the lateral direction orthogonal to the longitudinal direction, only the longitudinal couple is detected in each force detection unit, and this method is not applicable to all directions. The present invention has been made in view of such problems, and grips a workpiece with two manipulators placed in parallel, and an operation force applied to the workpiece by a human being in any direction in a three-dimensional space. It is an object of the present invention to provide a transport apparatus capable of performing a transport operation.
上記問題を解決するため、本発明は、次のように構成したのである。
請求項１に記載の発明は、第１マニピュレータと、前記第１マニピュレータの先端に設けられた第１ハンドと、前記第１ハンドの根元に設けられた第１力センサと、前記第１マニピュレータに並列に設けられた第２マニピュレータと、前記第２マニピュレータの先端に設けられた第２ハンドと、前記第２ハンドの根元に設けられた第２力センサと、を備え、長尺ワークの一端を前記第１および第２ハンドが把持するとともに他端を操作者が把持し、前記操作者が前記長尺ワークに操作力を加えることによって前記長尺ワークを搬送する搬送装置であって、前記第１および第２ハンドの前記長尺ワーク把持位置をそれぞれ原点とする第１および第２座標系を設け、前記第１および第２座標系の間に制御中心座標系を設け、前記第１および第２力センサがそれぞれ検出した前記第１および第２座標系における操作力を、前記制御中心座標系における操作力に変換する操作力演算手段と、前記変換された操作力に応じて前記第１および第２マニピュレータを移動させる制御手段と、を備えた搬送装置において、前記操作力演算手段は、前記第１および第２座標系における操作力と前記制御中心座標系における操作力との釣り合い条件に基づいて操作力を変換することを特徴とするものである。
また、請求項２に記載の発明は、前記制御中心座標系は、前記第１および第２座標系の中心に設けられ、前記操作力演算手段は、前記操作力を次式
Ｆｘｃ ＝ （ＦｘＲ＋ＦｘＬ） 式（１）
Ｆｙｃ ＝ （ＦｘＲ−ＦｘＬ）＊（Ｌ１／Ｌ２） 式（２）
Ｆｚｃ ＝ −（ＭｙＲ＋ＭｙＬ）／Ｌ２ 式（３）
Ｍｘｃ ＝ （ＦｚＬ−ＦｚＲ）＊Ｌ１ 式（４）
Ｍｙｃ ＝ （ＭｙＲ＋ＭｙＬ） 式（５）
Ｍｚｃ ＝ （ＦｘＲ−ＦｘＬ）＊Ｌ１ 式（６）
（但し、Ｆｘｃ：前記制御中心座標系におけるＸ軸方向の力、Ｆｙｃ：前記制御中心座標系におけるＹ軸方向の力、Ｆｚｃ：前記制御中心座標系におけるＺ軸方向の力、Ｍｘｃ：前記制御中心座標系におけるＸ軸回りのモーメント、Ｍｙｃ：前記制御中心座標系におけるＹ軸回りのモーメント、Ｍｚｃ：前記制御中心座標系におけるＺ軸回りのモーメント、ＦｘＲ：前記第１力センサが検出した前記第１座標系におけるＸ軸方向の力、ＦｙＲ：前記第１力センサが検出した前記第１座標系におけるＹ軸方向の力、ＦｚＲ：前記第１力センサが検出した前記第１座標系におけるＺ軸方向の力、ＭｘＲ：前記第１力センサが検出した前記第１座標系におけるＸ軸回りのモーメント、ＭｙＲ：前記第１力センサが検出した前記第１座標系におけるＹ軸回りのモーメント、ＭｚＲ：前記第１力センサが検出した前記第１座標系におけるＺ軸回りのモーメント、ＦｘＬ：前記第２力センサが検出した前記第２座標系におけるＸ軸方向の力、ＦｙＬ：前記第２力センサが検出した前記第２座標系におけるＹ軸方向の力、ＦｚＬ：前記第２力センサが検出した前記第２座標系におけるＺ軸方向の力、ＭｘＬ：前記第２力センサが検出した前記第２座標系におけるＸ軸回りのモーメント、ＭｙＬ：前記第２力センサが検出した前記第２座標系におけるＹ軸回りのモーメント、ＭｚＬ：前記第２力センサが検出した前記第２座標系におけるＺ軸回りのモーメント、Ｌ１：前記第１または第２座標系の原点から前記制御中心座標系の原点までの長さ、Ｌ２：前記長尺ワークの長手方向長さ）に基づいて変換することを特徴とするものである。
In order to solve the above problem, the present invention is configured as follows.
According to the first aspect of the present invention, the first manipulator, the first hand provided at the tip of the first manipulator, the first force sensor provided at the root of the first hand, and the first manipulator A second manipulator provided in parallel; a second hand provided at the tip of the second manipulator; and a second force sensor provided at the base of the second hand, and one end of the long workpiece A transport device that grips the first and second hands while an operator grips the other end, and transports the long workpiece by the operator applying an operating force to the long workpiece. First and second coordinate systems having the long workpiece gripping positions of the first and second hands as origins are provided, respectively, and a control center coordinate system is provided between the first and second coordinate systems. Two forces Operating force calculating means for converting the operating force in the first and second coordinate systems detected by the sensor into operating force in the control center coordinate system, and the first and second in accordance with the converted operating force. And a control unit that moves the manipulator. The operation force calculation unit operates based on a balance condition between the operation force in the first and second coordinate systems and the operation force in the control center coordinate system. It is characterized by converting force.
In the invention according to claim 2, the control center coordinate system is provided at the center of the first and second coordinate systems, and the operation force calculation means calculates the operation force by the following equation:
Fxc = (FxR + FxL) Formula (1)
Fyc = (FxR−FxL) * (L1 / L2) Equation (2)
Fzc = − (MyR + MyL) / L2 Formula (3)
Mxc = (FzL-FzR) * L1 Formula (4)
Myc = (MyR + MyL) Formula (5)
Mzc = (FxR−FxL) * L1 Formula (6)
(Where Fxc: force in the X-axis direction in the control center coordinate system, Fyc: force in the Y-axis direction in the control center coordinate system, Fzc: force in the Z-axis direction in the control center coordinate system, Mxc: control center Moment about the X axis in the coordinate system, Myc: Moment about the Y axis in the control center coordinate system, Mzc: Moment about the Z axis in the control center coordinate system, FxR: The first detected by the first force sensor Force in the X-axis direction in the coordinate system, FyR: Force in the Y-axis direction in the first coordinate system detected by the first force sensor, FzR: Z-axis direction in the first coordinate system detected by the first force sensor Force, MxR: moment about the X axis in the first coordinate system detected by the first force sensor, MyR: Y in the first coordinate system detected by the first force sensor Moment about rotation, MzR: Moment about the Z axis in the first coordinate system detected by the first force sensor, FxL: Force in the X axis direction on the second coordinate system detected by the second force sensor, FyL: Force in the Y-axis direction in the second coordinate system detected by the second force sensor, FzL: Force in the Z-axis direction in the second coordinate system detected by the second force sensor, MxL: Force in the second force sensor The detected moment about the X axis in the second coordinate system, MyL: the moment about the Y axis in the second coordinate system detected by the second force sensor, MzL: the second coordinate detected by the second force sensor Based on the moment about the Z-axis in the system, L1: length from the origin of the first or second coordinate system to the origin of the control center coordinate system, L2: length in the longitudinal direction of the long workpiece) It is characterized in that.
請求項１および２に記載の発明によると、並列に配置された２台のマニピュレータで重
量物や長尺物を人間と協調して任意の方向に搬送することができる。
According to the first and second aspects of the invention, it is possible to transport a heavy object or a long object in an arbitrary direction in cooperation with a human by using two manipulators arranged in parallel.
以下、本発明の実施の形態について図を参照して説明する。 Hereinafter, embodiments of the present invention will be described with reference to the drawings.
図１は、本発明の実施例を示す搬送装置の構成図である。図において、１１は右マニピュレータ、１２は左マニピュレータで１３のマニピュレータ固定台に並列にそれぞれのマニピュレータが固定されている。１４は右マニピュレータの手先に装着されたハンド、１５は左マニピュレータの手先に装着されたハンドである。また１６、１７はそれぞれのマニピュレータのハンドの根元に装着された力センサであり、ハンドに加わる力・トルクを検出することができるようになっている。１９はロボットと操作を行う操作者１８が協調して搬送するワークであり、ハンド１４、１５によって把持固定されている。ロボットが２組の力検出部を備えており、ワーク１９を２つのハンド１４、１５で把持するように構成し、操作者１８がワーク１９に加えた操作力を２組の力センサの出力をもとに演算するようにした部分が本発明の特徴部分である。 FIG. 1 is a configuration diagram of a transport apparatus showing an embodiment of the present invention. In the figure, 11 is a right manipulator, 12 is a left manipulator, and each manipulator is fixed in parallel to 13 manipulator fixing bases. Reference numeral 14 denotes a hand attached to the hand of the right manipulator, and reference numeral 15 denotes a hand attached to the hand of the left manipulator. Reference numerals 16 and 17 denote force sensors attached to the bases of the hands of the respective manipulators, so that the force and torque applied to the hands can be detected. Reference numeral 19 denotes a workpiece which is conveyed in cooperation by an operator 18 who operates the robot and is held and fixed by the hands 14 and 15. The robot includes two sets of force detection units, and is configured to hold the workpiece 19 with the two hands 14 and 15, and the operation force applied to the workpiece 19 by the operator 18 is output from the two sets of force sensors. The part which is originally calculated is a characteristic part of the present invention.
次に、２組の力センサ１６，１７の出力から並進・回転方向の操作力を検出する具体的方法を説明する。右マニピュレータ１１のハンド１４の座標系ΣＲと左マニピュレータ１２のハンド１５の座標系ΣＬを図１に示すように設定する。また右ハンド１４と左ハンド１５の中心に制御中心座標系ΣＣを設定する。そして、右のマニピュレータに装着された力センサ１６によって検出される右ハンド１４に加わる力・トルクを｛FxR,FyR,FzR,MxR,MyR,MzR｝、左のマニピュレータに装着された力センサ１７によって検出される左ハンド１５に加わる力・トルクを｛FxL,FyL,FzL,MxL,MyL,MzL｝とする。また、左右ハンド１４，１５から制御中心座標系原点までの距離をＬ１、ワーク１９の長さをＬ２とする。 Next, a specific method for detecting the operation force in the translation / rotation direction from the outputs of the two sets of force sensors 16 and 17 will be described. The coordinate system ΣR of the hand 14 of the right manipulator 11 and the coordinate system ΣL of the hand 15 of the left manipulator 12 are set as shown in FIG. A control center coordinate system ΣC is set at the center of the right hand 14 and the left hand 15. The force / torque applied to the right hand 14 detected by the force sensor 16 attached to the right manipulator is {FxR, FyR, FzR, MxR, MyR, MzR}, and the force sensor 17 attached to the left manipulator. The detected force / torque applied to the left hand 15 is {FxL, FyL, FzL, MxL, MyL, MzL}. Further, the distance from the left and right hands 14 and 15 to the control center coordinate system origin is L1, and the length of the work 19 is L2.
このとき、ワーク１９が長尺物、つまりＬ１＜＜Ｌ２の時には、操作者１８が左右マニピュレータ１１，１２に対して長手方向（Ｘ並進方向やＹ軸周り回転方向）は操作力を加えることが容易であるが、Ｙ、Ｚ方向の並進力を与えることは困難である。また、同時に、図１のように２ヶ所でワーク１９を把持した場合は、Ｘ、Ｚ軸周りの回転トルクを与えることは困難である。そこで、並進・回転の操作力を２組の力センサの出力情報を用いて、制御中心座標系ΣＣ周りのモーメントの釣り合い条件に当てはめることによって、計算する。 At this time, when the work 19 is a long object, that is, when L1 << L2, the operator 18 can apply an operating force to the left and right manipulators 11 and 12 in the longitudinal direction (X translation direction or rotation direction around the Y axis). Although easy, it is difficult to provide translational forces in the Y and Z directions. At the same time, when the workpiece 19 is gripped at two places as shown in FIG. 1, it is difficult to apply rotational torque about the X and Z axes. Therefore, the translation / rotation operation force is calculated by applying the output information of the two sets of force sensors to the moment balance condition around the control center coordinate system ΣC.
Ｘ軸方向の並進力Fxcは、力センサ１６で求めた右ハンド１４に生じるＸ軸方向の並進力FxRと力センサ１７で求めた左ハンド１５に生じるＸ軸方向の並進力FxLの和と釣り合うから、下式で得られる。
Fxc=(FxR+FxL) （式１）
Ｙ軸方向の並進力Fycは、制御中心Ｃ回りのモーメントの釣り合いから、下式で得られる。
Fyc=(FxR-FxL)*(L1/L2) （式２）
Ｚ軸方向の並進力Fzcは、力センサ１６で求めた右ハンド１４に生じるＹ軸回りのモーメントMyRと、力センサ１７で求めた左ハンド１５に生じるＹ軸周りのモーメントMyLを、制御中心Ｃ回りのモーメントの釣り合い式に当てはめて、下式で得られる。
Fzc=-(MyR+MyL)/L2 （式３）
Ｘ軸回りのモーメントMxcは、力センサ１６で求めた右ハンド１４に生じるＺ軸方向の並進力FzRと、力センサ１７で求めた左ハンド１５に生じるＺ軸方向の並進力FzLによって制御中心Ｃ回りに生じるモーメントと釣り合うから、下式で得られる。
Mxc=(FzL-FzR)*L1 （式４）
Ｙ軸回りのモーメントMycは、力センサ１６で求めた右ハンド１４に生じるＹ軸回りのモーメントMyRと、力センサ１７で求めた左ハンド１５に生じるＹ軸周りのモーメントMyLの和と釣り合うから、下式で得られる。
Myc=(MyR+MyL) （式５）
Ｚ軸回りのモーメントMzcは、力センサ１６で求めた右ハンド１４に生じるＸ軸方向の並進力FxRと、力センサ１７で求めた左ハンド１５に生じるＸ軸方向の並進力FxLによって制御中心Ｃ回りに生じるモーメントと釣り合うから、下式で得られる。
Mzc=(FxR-FxL)*L1 （式６）
The translational force Fxc in the X-axis direction is balanced with the sum of the translational force FxR in the X-axis direction generated in the right hand 14 determined by the force sensor 16 and the translational force FxL in the X-axis direction generated in the left hand 15 determined by the force sensor 17. From this, the following formula is obtained.
Fxc = (FxR + FxL) (Formula 1)
The translational force Fyc in the Y-axis direction is obtained by the following equation from the balance of moments around the control center C.
Fyc = (FxR-FxL) * (L1 / L2) (Formula 2)
The translational force Fzc in the Z-axis direction is obtained by using the control center C to calculate the moment MyR about the Y axis generated in the right hand 14 determined by the force sensor 16 and the moment MyL about the Y axis generated in the left hand 15 determined by the force sensor 17. Applying to the balance formula of the moment of rotation, it is obtained by the following formula.
Fzc =-(MyR + MyL) / L2 (Formula 3)
The moment Mxc about the X-axis is determined by the control center C based on the translational force FzR in the Z-axis direction generated in the right hand 14 obtained by the force sensor 16 and the translational force FzL in the Z-axis direction produced in the left hand 15 obtained by the force sensor 17. Since it is balanced with the moment generated around, it is obtained by the following formula.
Mxc = (FzL-FzR) * L1 (Formula 4)
The moment Myc about the Y axis is balanced with the sum of the moment MyR about the Y axis generated in the right hand 14 obtained by the force sensor 16 and the moment MyL about the Y axis generated in the left hand 15 obtained by the force sensor 17. It is obtained by the following formula.
Myc = (MyR + MyL) (Formula 5)
The moment Mzc about the Z-axis is controlled by the translation center FxR in the X-axis direction generated in the right hand 14 determined by the force sensor 16 and the translational force FxL in the X-axis direction generated in the left hand 15 determined by the force sensor 17. Since it is balanced with the moment generated around, it is obtained by the following formula.
Mzc = (FxR-FxL) * L1 (Formula 6)
以上によって、操作者１８がワーク１９に対して加えた並進力・回転トルクが求まる。これらの並進力・回転トルクが、操作者１８がワーク１９を移動させようとする方向を示す操作力となる。次に、これらの並進力・回転トルクに基づいて左右それぞれのマニピュレータ１１，１２の移動量を算出する方法を、図２で引用して説明する。 As described above, the translational force / rotational torque applied to the workpiece 19 by the operator 18 is obtained. These translational force and rotational torque become the operating force indicating the direction in which the operator 18 tries to move the workpiece 19. Next, a method of calculating the movement amounts of the left and right manipulators 11 and 12 based on these translational forces and rotational torques will be described with reference to FIG.
図２は本発明の実施例を示す制御装置のブロック図である。図において、２１は２組の力検出部であり、図１の力センサ１６，１７に相当する。２２は操作力演算部であり、力検出部２１の出力を前述の式１ないし式６に従って、操作力に変換する。前記操作力は力制御部２３へ入力され、前記操作力に応じた直交移動量を算出する。また、これとは別に搬送装置に対して動作コマンドが入力されると、軌道生成部２４において、制御中心点ΣＣの軌道を生成し直交位置指令を出力する。ここで、軌道生成部２４の直交位置指令に力制御部２３の直交移動量を加えて得られた、制御中心点ΣＣの直交位置指令をXCrefとする。このとき、右マニピュレータ１１先端の位置指令を計算するためにΣＣの直交位置指令XCrefを右マニピュレータ位置姿勢変換部２５で右マニピュレータ１１のハンド１４の直交位置指令XRrefに変換する。そして右マニピュレータ逆運動学変換部２６で右マニピュレータ１１の各関節角度指令θRrefを出力する。また同時に、ΣＣの直交位置指令XCrefを左マニピュレータ位置姿勢変換部２７で左マニピュレータ１２のハンド１５の直交位置指令XLrefに変換する。そして、左マニピュレータ逆運動学変換部２８で左マニピュレータ１２の各関節角度指令θLrefを出力する。左マニピュレータ位置姿勢変換部２７では左マニピュレータ１２のハンド１５の位置姿勢が右マニピュレータのハンド１４と相対位置姿勢が常に変化しないように左マニピュレータ１２の位置姿勢を求める。このようにすることで、２組のマニピュレータで把持しているワーク１９を安定して把持し続けることが可能となる。ここでは右マニピュレータ１１を基準に位置姿勢を計算しているが、左マニピュレータ１２を基準に計算してもよい。出力された角度指令θRrefとθLrefを動作制御部２９に入力することによって、マニピュレータを制御する。
FIG. 2 is a block diagram of a control apparatus showing an embodiment of the present invention. In the figure, reference numeral 21 denotes two sets of force detection units, which correspond to the force sensors 16 and 17 in FIG. Reference numeral 22 denotes an operating force calculation unit that converts the output of the force detection unit 21 into an operating force according to the above-described equations 1 to 6. The operating force is input to the force control unit 23, and an orthogonal movement amount corresponding to the operating force is calculated. In addition, when an operation command is input to the transport apparatus, the trajectory generator 24 generates a trajectory of the control center point ΣC and outputs an orthogonal position command. Here, the orthogonal position command of the control center point ΣC obtained by adding the orthogonal movement amount of the force control unit 23 to the orthogonal position command of the trajectory generation unit 24 is defined as XCref. At this time, in order to calculate the position command of the tip of the right manipulator 11, the orthogonal position command XCref of ΣC is converted into the orthogonal position command XRref of the hand 14 of the right manipulator 11 by the right manipulator position / posture conversion unit 25. Then, the right manipulator inverse kinematics conversion unit 26 outputs each joint angle command θRref of the right manipulator 11. At the same time, the ΣC orthogonal position command XCref is converted into the orthogonal position command XLref of the hand 15 of the left manipulator 12 by the left manipulator position / orientation conversion unit 27. Then, the left manipulator inverse kinematics conversion unit 28 outputs each joint angle command θLref of the left manipulator 12. The left manipulator position / posture conversion unit 27 obtains the position / posture of the left manipulator 12 so that the position / posture of the hand 15 of the left manipulator 12 does not always change relative to the hand 14 of the right manipulator. By doing in this way, it becomes possible to continue holding | grip the workpiece | work 19 currently hold | gripped with two sets of manipulators stably. Here, the position and orientation are calculated based on the right manipulator 11, but may be calculated based on the left manipulator 12. The manipulator is controlled by inputting the output angle commands θRref and θLref to the
以上の構成によって、本発明の搬送装置は２組の並列に配置したマニピュレータで重量物や長尺物などワークを人間と協調して搬送させることが可能となる。上記実施例では、２組マニピュレータはマニピュレータ固定台１３に固定されているが、マニピュレータ固定台１３を移動台車としてもよい。移動台車に２組のマニピュレータを備える構成では、式１ないし式６で計算された操作力に応じて力制御部２３で計算される制御中心点ΣＣの移動量をマニピュレータと移動体の移動量に適度に分配することで、マニピュレータのみの動作範囲にとどまらず、広い範囲の搬送動作が可能となる。
また、本実施例では双腕マニピュレータで例示したが、双腕マニピュレータに限定するものではなく、２組の並列に配置された力検出部を有するものであればよい。
なおまた、本実施例では６軸（３並進力、３モーメント）の力センサを用いたが、３軸（Ｘ軸方向およびＺ軸方向の並進力、Ｙ軸回りのモーメント）の力センサが得られれば、それを用いてもよい。
With the above configuration, the transport apparatus of the present invention can transport workpieces such as heavy objects and long objects in cooperation with humans by using two sets of manipulators arranged in parallel. In the above embodiment, the two sets of manipulators are fixed to the manipulator fixing base 13, but the manipulator fixing base 13 may be a moving carriage. In the configuration in which the moving carriage is provided with two sets of manipulators, the movement amount of the control center point ΣC calculated by the force control unit 23 according to the operation force calculated by the equations 1 to 6 is used as the movement amount of the manipulator and the moving body. By appropriately distributing, not only the operation range of the manipulator but also a wide range of transfer operation is possible.
In the present embodiment, the dual-arm manipulator is exemplified. However, the present invention is not limited to the dual-arm manipulator, and any one having two sets of force detection units arranged in parallel may be used.
In this embodiment, a 6-axis (3 translational force, 3 moment) force sensor is used, but a 3-axis (X-axis direction and Z-axis direction translational force, Y-axis moment) force sensor is obtained. If you can, you may use it.
２組のマニピュレータと人間が協調してワークを搬送するロボットとして利用できる。 Two sets of manipulators and human beings can be used as robots that transport workpieces in cooperation.
１１ 右マニピュレータ
１２ 左マニピュレータ
１３ マニピュレータ固定台
１４ 右ハンド
１５ 左ハンド
１６ 力センサ
１７ 力センサ
１８ 操作者
１９ ワーク
２１ 力検出部
２２ 操作力演算部
２３ 力制御部
２４ 軌道生成部
２５ 右マニピュレータ位置姿勢変換部
２６ 右マニピュレータ逆運動学変換部
２７ 左マニピュレータ位置姿勢変換部
２８ 左マニピュレータ逆運動学変換部
２９ 動作制御部
３１ マニピュレータ
３２ ハンド
３３ 操作者
３４ ワーク
３５ 力センサ
11 Right Manipulator 12 Left Manipulator 13 Manipulator Fixing Base 14 Right Hand 15 Left Hand 16 Force Sensor 17 Force Sensor 18 Operator 19 Work 21 Force Detection Unit 22 Operation Force Calculation Unit 23 Force Control Unit 24 Trajectory Generation Unit 25 Right Manipulator Position / Orientation Conversion Unit 26 right manipulator inverse kinematics conversion unit 27 left manipulator position and orientation conversion unit 28 left manipulator inverse
Claims (2)
前記第１マニピュレータに並列に設けられた第２マニピュレータと、前記第２マニピュレータの先端に設けられた第２ハンドと、前記第２ハンドの根元に設けられた第２力センサと、を備え、
長尺ワークの一端を前記第１および第２ハンドが把持するとともに他端を操作者が把持し、前記操作者が前記長尺ワークに操作力を加えることによって前記長尺ワークを搬送する搬送装置であって、
前記第１および第２ハンドの前記長尺ワーク把持位置をそれぞれ原点とする第１および第２座標系を設け、前記第１および第２座標系の間に制御中心座標系を設け、前記第１および第２力センサがそれぞれ検出した前記第１および第２座標系における操作力を、前記制御中心座標系における操作力に変換する操作力演算手段と、
前記変換された操作力に応じて前記第１および第２マニピュレータを移動させる制御手段と、を備えた搬送装置において、
前記操作力演算手段は、前記第１および第２座標系における操作力と前記制御中心座標系における操作力との釣り合い条件に基づいて操作力を変換することを特徴とする搬送装置。 A first manipulator, a first hand provided at the tip of the first manipulator, a first force sensor provided at the base of the first hand,
A second manipulator provided in parallel to the first manipulator, a second hand provided at the tip of the second manipulator, and a second force sensor provided at the base of the second hand,
A transport device that transports the long workpiece by gripping one end of the long workpiece by the first and second hands, an operator gripping the other end, and the operator applying an operating force to the long workpiece. Because
First and second coordinate systems having the long workpiece gripping positions of the first and second hands as origins are provided, a control center coordinate system is provided between the first and second coordinate systems, and the first Operating force calculation means for converting the operating force in the first and second coordinate systems detected by the second force sensor and the operating force in the control center coordinate system, respectively,
A control device that moves the first and second manipulators according to the converted operating force ,
The transport device according to claim 1, wherein the operating force calculating means converts the operating force based on a balance condition between the operating force in the first and second coordinate systems and the operating force in the control center coordinate system .
前記操作力演算手段は、前記操作力を次式
Ｆｘｃ ＝ （ＦｘＲ＋ＦｘＬ） 式（１）
Ｆｙｃ ＝ （ＦｘＲ−ＦｘＬ）＊（Ｌ１／Ｌ２） 式（２）
Ｆｚｃ ＝ −（ＭｙＲ＋ＭｙＬ）／Ｌ２ 式（３）
Ｍｘｃ ＝ （ＦｚＬ−ＦｚＲ）＊Ｌ１ 式（４）
Ｍｙｃ ＝ （ＭｙＲ＋ＭｙＬ） 式（５）
Ｍｚｃ ＝ （ＦｘＲ−ＦｘＬ）＊Ｌ１ 式（６）
（但し、
Ｆｘｃ：前記制御中心座標系におけるＸ軸方向の力、
Ｆｙｃ：前記制御中心座標系におけるＹ軸方向の力、
Ｆｚｃ：前記制御中心座標系におけるＺ軸方向の力、
Ｍｘｃ：前記制御中心座標系におけるＸ軸回りのモーメント、
Ｍｙｃ：前記制御中心座標系におけるＹ軸回りのモーメント、
Ｍｚｃ：前記制御中心座標系におけるＺ軸回りのモーメント、
ＦｘＲ：前記第１力センサが検出した前記第１座標系におけるＸ軸方向の力、
ＦｙＲ：前記第１力センサが検出した前記第１座標系におけるＹ軸方向の力、
ＦｚＲ：前記第１力センサが検出した前記第１座標系におけるＺ軸方向の力、
ＭｘＲ：前記第１力センサが検出した前記第１座標系におけるＸ軸回りのモーメント、
ＭｙＲ：前記第１力センサが検出した前記第１座標系におけるＹ軸回りのモーメント、
ＭｚＲ：前記第１力センサが検出した前記第１座標系におけるＺ軸回りのモーメント、
ＦｘＬ：前記第２力センサが検出した前記第２座標系におけるＸ軸方向の力、
ＦｙＬ：前記第２力センサが検出した前記第２座標系におけるＹ軸方向の力、
ＦｚＬ：前記第２力センサが検出した前記第２座標系におけるＺ軸方向の力、
ＭｘＬ：前記第２力センサが検出した前記第２座標系におけるＸ軸回りのモーメント、
ＭｙＬ：前記第２力センサが検出した前記第２座標系におけるＹ軸回りのモーメント、
ＭｚＬ：前記第２力センサが検出した前記第２座標系におけるＺ軸回りのモーメント、
Ｌ１：前記第１または第２座標系の原点から前記制御中心座標系の原点までの長さ、
Ｌ２：前記長尺ワークの長手方向長さ）
に基づいて変換することを特徴とする請求項１記載の搬送装置。 The control center coordinate system is provided at the center of the first and second coordinate systems,
The operating force calculation means calculates the operating force as
Fxc = (FxR + FxL) Formula (1)
Fyc = (FxR−FxL) * (L1 / L2) Equation (2)
Fzc = − (MyR + MyL) / L2 Formula (3)
Mxc = (FzL-FzR) * L1 Formula (4)
Myc = (MyR + MyL) Formula (5)
Mzc = (FxR−FxL) * L1 Formula (6)
(However,
Fxc: force in the X-axis direction in the control center coordinate system,
Fyc: force in the Y-axis direction in the control center coordinate system,
Fzc: force in the Z-axis direction in the control center coordinate system,
Mxc: moment about the X axis in the control center coordinate system,
Myc: moment about the Y axis in the control center coordinate system,
Mzc: moment about the Z axis in the control center coordinate system,
FxR: force in the X-axis direction in the first coordinate system detected by the first force sensor,
FyR: force in the Y-axis direction in the first coordinate system detected by the first force sensor,
FzR: force in the Z-axis direction in the first coordinate system detected by the first force sensor,
MxR: moment about the X axis in the first coordinate system detected by the first force sensor;
MyR: moment about the Y axis in the first coordinate system detected by the first force sensor,
MzR: moment about the Z axis in the first coordinate system detected by the first force sensor;
FxL: force in the X-axis direction in the second coordinate system detected by the second force sensor,
FyL: force in the Y-axis direction in the second coordinate system detected by the second force sensor,
FzL: force in the Z-axis direction in the second coordinate system detected by the second force sensor,
MxL: moment about the X axis in the second coordinate system detected by the second force sensor;
MyL: moment about the Y axis in the second coordinate system detected by the second force sensor,
MzL: moment about the Z axis in the second coordinate system detected by the second force sensor;
L1: length from the origin of the first or second coordinate system to the origin of the control center coordinate system;
L2: length in the longitudinal direction of the long workpiece)
The transfer device according to claim 1, wherein conversion is performed based on
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP2003275446A JP4168441B2 (en) | 2003-07-16 | 2003-07-16 | Transport device |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP2003275446A JP4168441B2 (en) | 2003-07-16 | 2003-07-16 | Transport device |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| JP2005034960A JP2005034960A (en) | 2005-02-10 |
| JP4168441B2 true JP4168441B2 (en) | 2008-10-22 |
Family
ID=34212086
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| JP2003275446A Expired - Fee Related JP4168441B2 (en) | 2003-07-16 | 2003-07-16 | Transport device |
Country Status (1)
| Country | Link |
|---|---|
| JP (1) | JP4168441B2 (en) |
Cited By (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| DE102011109908B4 (en) * | 2010-08-17 | 2017-01-12 | Fanuc Corporation | Interactive human-robot system |
| US20210129321A1 (en) * | 2019-10-31 | 2021-05-06 | Seiko Epson Corporation | Control method and calculation device |
Families Citing this family (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| FR2983762B1 (en) | 2011-12-09 | 2014-01-10 | Commissariat Energie Atomique | METHOD FOR CONTROLLING A ROBOT AND STEERING SYSTEM USING SUCH A METHOD |
| CN104308841A (en) * | 2014-10-21 | 2015-01-28 | 泉州市微柏工业机器人研究院有限公司 | Heavy-load parallel manipulator |
| JP6577326B2 (en) * | 2015-10-16 | 2019-09-18 | ファナック株式会社 | Robot control apparatus, robot system, and method for controlling robot that carries objects in cooperation with human |
-
2003
- 2003-07-16 JP JP2003275446A patent/JP4168441B2/en not_active Expired - Fee Related
Cited By (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| DE102011109908B4 (en) * | 2010-08-17 | 2017-01-12 | Fanuc Corporation | Interactive human-robot system |
| US20210129321A1 (en) * | 2019-10-31 | 2021-05-06 | Seiko Epson Corporation | Control method and calculation device |
| US11518026B2 (en) * | 2019-10-31 | 2022-12-06 | Seiko Epson Corporation | Control method and calculation device |
Also Published As
| Publication number | Publication date |
|---|---|
| JP2005034960A (en) | 2005-02-10 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| JP4273335B2 (en) | Robot arm | |
| KR101549818B1 (en) | Robot hand and method of controlling robot hand | |
| JPS63241614A (en) | Robot device | |
| Karayiannidis et al. | In-hand manipulation using gravity and controlled slip | |
| JP6831530B2 (en) | Disturbance observer and robot control device | |
| KR101323217B1 (en) | Grasping force control system and method for a robotic hand | |
| JP2007098501A (en) | Robot system | |
| JP2010120124A (en) | Teaching system for robot arm and method thereof | |
| WO2017036520A1 (en) | System and method for generating a robot program with a hand-held teaching device | |
| JP4168441B2 (en) | Transport device | |
| Almusawi et al. | Online teaching of robotic arm by human–robot interaction: end effector force/torque sensing | |
| JP3884249B2 (en) | Teaching system for humanoid hand robot | |
| JP2009220184A (en) | Output torque limiting circuit of industrial robot | |
| JPH10100089A (en) | Grasp control method of articulated multi-finger hand and object grasping method | |
| Winkler et al. | Force-guided motions of a 6-dof industrial robot with a joint space approach | |
| Takubo et al. | Human-robot cooperative handling using virtual nonholonomic constraint in 3-D space | |
| Luo et al. | On-line adaptive control for minimizing slippage error while mobile platform and manipulator operate simultaneously for robotics mobile manipulation | |
| JPH1133952A (en) | Method for controlling robot, and method for correcting position and attitude of robot and held object | |
| Muthusamy et al. | Investigation and design of robotic assistance control system for cooperative manipulation | |
| JPS63276607A (en) | Coordinate transforming device for manipulator | |
| An et al. | Cooperative control of manipulator and human operator for direct teaching | |
| JP2718687B2 (en) | Control device for position and force of multi-degree-of-freedom work machine | |
| JP2019155523A (en) | Robot control device, robot control method, assembly method for article using robot control device, program, and recording medium | |
| JPH11345010A (en) | Controller for robot | |
| KR102624551B1 (en) | A method for task-space compliance control of robot manipulator without rotational displacement constraints |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| A621 | Written request for application examination |
Free format text: JAPANESE INTERMEDIATE CODE: A621 Effective date: 20060612 |
|
| A977 | Report on retrieval |
Free format text: JAPANESE INTERMEDIATE CODE: A971007 Effective date: 20071126 |
|
| A131 | Notification of reasons for refusal |
Free format text: JAPANESE INTERMEDIATE CODE: A131 Effective date: 20071204 |
|
| A521 | Written amendment |
Free format text: JAPANESE INTERMEDIATE CODE: A523 Effective date: 20080117 |
|
| TRDD | Decision of grant or rejection written | ||
| A01 | Written decision to grant a patent or to grant a registration (utility model) |
Free format text: JAPANESE INTERMEDIATE CODE: A01 Effective date: 20080714 |
|
| A01 | Written decision to grant a patent or to grant a registration (utility model) |
Free format text: JAPANESE INTERMEDIATE CODE: A01 |
|
| A61 | First payment of annual fees (during grant procedure) |
Free format text: JAPANESE INTERMEDIATE CODE: A61 Effective date: 20080727 |
|
| FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20110815 Year of fee payment: 3 |
|
| R150 | Certificate of patent or registration of utility model |
Free format text: JAPANESE INTERMEDIATE CODE: R150 |
|
| FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20120815 Year of fee payment: 4 |
|
| FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20130815 Year of fee payment: 5 |
|
| FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20140815 Year of fee payment: 6 |
|
| LAPS | Cancellation because of no payment of annual fees |