JP5755715B2 - Robot control method - Google Patents
Robot control method Download PDFInfo
- Publication number
- JP5755715B2 JP5755715B2 JP2013260562A JP2013260562A JP5755715B2 JP 5755715 B2 JP5755715 B2 JP 5755715B2 JP 2013260562 A JP2013260562 A JP 2013260562A JP 2013260562 A JP2013260562 A JP 2013260562A JP 5755715 B2 JP5755715 B2 JP 5755715B2
- Authority
- JP
- Japan
- Prior art keywords
- joint
- arm body
- redundant
- angle
- robot
- 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.)
- Active
Links
- 238000000034 method Methods 0.000 title claims description 65
- 239000012636 effector Substances 0.000 claims description 44
- 238000004364 calculation method Methods 0.000 description 70
- 230000008859 change Effects 0.000 description 65
- 238000006073 displacement reaction Methods 0.000 description 37
- 230000036544 posture Effects 0.000 description 35
- 230000008569 process Effects 0.000 description 33
- 230000014509 gene expression Effects 0.000 description 26
- 239000011159 matrix material Substances 0.000 description 17
- 230000006870 function Effects 0.000 description 11
- 238000006243 chemical reaction Methods 0.000 description 9
- 238000010586 diagram Methods 0.000 description 7
- 230000007246 mechanism Effects 0.000 description 3
- 238000003825 pressing Methods 0.000 description 3
- 230000001131 transforming effect Effects 0.000 description 3
- 230000015572 biosynthetic process Effects 0.000 description 2
- 239000000470 constituent Substances 0.000 description 2
- 238000007796 conventional method Methods 0.000 description 2
- 230000007257 malfunction Effects 0.000 description 2
- 230000000877 morphologic effect Effects 0.000 description 2
- 230000004044 response Effects 0.000 description 2
- 230000009466 transformation Effects 0.000 description 2
- 241000270295 Serpentes Species 0.000 description 1
- 230000009471 action Effects 0.000 description 1
- 230000007423 decrease Effects 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 230000001788 irregular Effects 0.000 description 1
- 238000004088 simulation Methods 0.000 description 1
- 238000010977 unit operation Methods 0.000 description 1
- 238000003466 welding Methods 0.000 description 1
Images
Landscapes
- Manipulator (AREA)
Description
本発明は、ロボットの制御方法に関する。 The present invention relates to a robot control method.
７つ以上の複数の関節部を有する多関節ロボットの制御方法に関する従来技術として、たとえば特許文献１に開示されている。この従来技術では、冗長性を有する多関節ロボットに設定される設定部分が予め設定される固定位置に維持した状態で、ロボットを構成する複数の軸のうち、任意の１つの冗長関節部の微小移動が可能に設定される。操作者が冗長関節部を微小移動させた場合、ロボット制御装置は、設定部分が固定位置に維持されるように冗長関節部以外の残余の各関節部を微小移動させる。
For example,
特許文献１に開示される従来技術では、ヤコビ行列に基づいて、冗長関節部以外の残余の各関節部の移動量を決定する。ここで、ヤコビ行列は、設定部分の微小変化と、ロボット各関節部の微小変化との関係を示す関数である。従来技術のロボット制御装置は、設定部分の微小変化をゼロとし、ロボットの各関節部の微小変化を示す行列のうちで冗長設定関節部の微小変化を既知として、残余の各関節部の微小変化の解を求めることによって、設定部分を固定位置範囲に維持すべき、残余の各関節部の微小移動量を求める。
In the prior art disclosed in
従来技術では、複数の関節部のうちで任意の関節部を冗長関節部として選択可能に設定され、ヤコビ行列に基づいて、冗長関節部以外の残余の各関節部の微小移動量を求める。任意の関節部を冗長関節部として選択可能とすると、各関節部の移動量について、厳密解を求めることが困難であり、近似解を求めることになる。近似解を基に各関節部を決定すると、冗長設定関節部を変位移動させた場合に、目的とする固定位置に設定部分を正確に維持することができないという問題がある。 In the related art, an arbitrary joint part is set to be selectable as a redundant joint part among a plurality of joint parts, and the minute movement amount of each remaining joint part other than the redundant joint part is obtained based on the Jacobian matrix. If any joint part can be selected as a redundant joint part, it is difficult to obtain an exact solution for the movement amount of each joint part, and an approximate solution is obtained. If each joint portion is determined based on the approximate solution, there is a problem that the set portion cannot be accurately maintained at the target fixed position when the redundant setting joint portion is displaced.
設定部分のずれ量は、冗長関節部を高速移動または長距離移動する場合に顕著となる。設定部分がずれてしまうと、ロボット動作に不具合が生じる場合がある。また設定部分の位置ずれを防ぐために収束演算の回数を増やした場合には、各関節部の移動量を短時間で求めることができない。 The amount of deviation of the set part becomes conspicuous when the redundant joint part moves at a high speed or a long distance. If the setting part is shifted, a problem may occur in the robot operation. Further, when the number of times of convergence calculation is increased in order to prevent the position shift of the set portion, the movement amount of each joint portion cannot be obtained in a short time.
たとえば冗長性ロボットによって搬送対象物を搬送する場合、ロボットアームが障害物に衝突しないように位置教示を行う。位置教示時に、搬送対象物の位置姿勢を正確に維持したうえで、障害物近傍のアームを冗長関節部として微小移動させることができず、搬送不良が生じてしまうおそれがある。またたとえば冗長性を有するロボットについて、複数の関節部のうちから冗長関節部として選択する関節部を順次変更して、各冗長関節部について微小移動を繰返すと、設定部分のずれ方が複雑となり、設定部分の位置を前の状態に戻すことが困難となる。 For example, when a transport object is transported by a redundant robot, position teaching is performed so that the robot arm does not collide with an obstacle. At the time of position teaching, the position and orientation of the object to be transported can be accurately maintained, and the arm near the obstacle cannot be finely moved as a redundant joint, which may cause a transport failure. In addition, for example, for a robot having redundancy, if the joint part to be selected as the redundant joint part is sequentially changed from among a plurality of joint parts, and the minute movement is repeated for each redundant joint part, the setting part shift method becomes complicated, It becomes difficult to return the position of the setting portion to the previous state.
したがって本発明の目的は、設定部分を目的とする固定位置に正確に維持したうえで、ロボットの各関節部の角度を変更可能に、冗長ロボットを制御する制御方法を提供することである。 Accordingly, an object of the present invention is to provide a control method for controlling a redundant robot so that the angle of each joint portion of the robot can be changed while accurately maintaining the set portion at the intended fixed position.
また本発明は、互いに隣接する４つの関節部を有するロボットの制御方法であって、
操作手段に設けられた２つの入力手段であって、前記４つの関節部の内、前記ロボットの基端側に位置する２つの関節部の一方を選択するための入力手段、および前記ロボットの先端側に位置する２つの関節部の一方を選択するための入力手段の２つの入力手段のいずれかによって冗長関節部を１つ選択し、
前記ロボットの先端部に設定される設定部分の、前記ロボットの基端部に対する位置または姿勢の３自由度を固定しつつ、選択された前記冗長関節部を角変位させることを特徴とする。
また本発明は、前記ロボットは、前記４つの関節部によって連結される４つのアーム体および基台を有し、
第１アーム体の一端部は、第１関節部によって、第１アーム体の軸線と同軸の第１回転軸線まわりに回転自在に基台に連結され、
第２アーム体の一端部は、第２関節部によって、第１アーム体の軸線および第２アーム体の軸線に対して４５度で傾斜する第２回転軸線まわりに回転自在に、第１アーム体の他端部に連結され、
第３アーム体の一端部は、第３関節部によって、第２アーム体の軸線および第３アーム体の軸線に対して４５度で傾斜する第３回転軸線まわりに回転自在に、第２アーム体の他端部に連結され、
第２回転軸線と第３回転軸線とは平行であり、
第４アーム体の一端部は、第４関節部によって、第３アーム体の軸線および第４アーム体の軸線に対して４５度で傾斜する第４回転軸線まわりに回転自在に、第３アーム体の他端部に連結され、
第３回転軸線と第４回転軸線とは垂直であり、
前記設定部分は、第４アーム体の他端部に設定され、
前記ロボットの基端側に位置する２つの関節部の一方は、第１関節部であり、
前記ロボットの先端側に位置する２つの関節部の一方は、第４関節部であり、
冗長関節部を選択する工程では、前記第１関節部と前記第４関節部とだけが冗長関節部として選択可能に設定された操作手段を用いて冗長関節部が選択されることを特徴とする。
The present invention is also a method for controlling a robot having four joint portions adjacent to each other,
Two input means provided in the operation means, the input means for selecting one of the two joint portions located on the proximal end side of the robot among the four joint portions, and the tip of the robot redundant joint select one by one of the two input means of the input means for selecting one of the two joint portions located on the side,
The selected redundant joint portion is angularly displaced while fixing the three degrees of freedom of the position or posture of the setting portion set at the distal end portion of the robot with respect to the proximal end portion of the robot.
Further, in the present invention, the robot has four arm bodies and a base connected by the four joint portions,
One end portion of the first arm body is connected to the base by the first joint portion so as to be rotatable about a first rotation axis coaxial with the axis of the first arm body,
One end portion of the second arm body is rotatable by the second joint portion around a second rotation axis inclined at 45 degrees with respect to the axis of the first arm body and the axis of the second arm body. Connected to the other end of the
One end portion of the third arm body is rotatable by the third joint portion around the third rotation axis inclined at 45 degrees with respect to the axis of the second arm body and the axis of the third arm body. Connected to the other end of the
The second rotation axis and the third rotation axis are parallel,
One end portion of the fourth arm body is rotatable by the fourth joint portion so as to be rotatable around a fourth rotation axis inclined at 45 degrees with respect to the axis of the third arm body and the axis of the fourth arm body. Connected to the other end of the
The third rotation axis and the fourth rotation axis are vertical,
The setting portion is set at the other end of the fourth arm body,
One of the two joint portions located on the proximal end side of the robot is a first joint portion,
One of the two joint portions located on the tip side of the robot is a fourth joint portion,
In the step of selecting a redundant joint part, the redundant joint part is selected by using an operating means in which only the first joint part and the fourth joint part can be selected as redundant joint parts. .
また本発明は、７つのみの関節部を有し、冗長自由度が１であり、ロボットの先端部において隣接する３つの関節部の回転軸線が、一点で交差するロボットの制御方法であって、
操作手段に設けられた２つの入力手段であって、前記ロボットの基端部において隣接する４つの関節部の内、前記ロボットの基端側に位置する２つの関節部の一方を選択するための入力手段、および前記ロボットの先端側に位置する２つの関節部の一方を選択するための入力手段の２つの入力手段のいずれかによって前記４つの関節部のうちから１つの冗長関節部を選択し、
前記ロボットの先端部に設定される設定部分の、前記ロボットの基端部に対する位置および姿勢の６自由度を固定しつつ、選択された前記冗長関節部を角変位させることを特徴とする。
また本発明は、前記ロボットは、前記７つの関節部によって連結される７つのアーム体、基台およびエンドエフェクタを有し、
第１アーム体の一端部は、第１関節部によって、第１アーム体の軸線と同軸の第１回転軸線まわりに回転自在に基台に連結され、
第２アーム体の一端部は、第２関節部によって、第１アーム体の軸線および第２アーム体の軸線に対して４５度で傾斜する第２回転軸線まわりに回転自在に、第１アーム体の他端部に連結され、
第３アーム体の一端部は、第３関節部によって、第２アーム体の軸線および第３アーム体の軸線に対して４５度で傾斜する第３回転軸線まわりに回転自在に、第２アーム体の他端部に連結され、
第２回転軸線と第３回転軸線とは平行であり、
第４アーム体の一端部は、第４関節部によって、第３アーム体の軸線および第４アーム体の軸線に対して４５度で傾斜する第４回転軸線まわりに回転自在に、第３アーム体の他端部に連結され、
第３回転軸線と第４回転軸線とは垂直であり、
第５アーム体の一端部は、第５関節部によって、第４アーム体の軸線および第５アーム体の軸線と同軸の第５回転軸線まわりに回転自在に、第４アーム体の他端部に連結され、
第６アーム体の一端部は、第６関節部によって、第５アーム体の軸線および第６アーム体の軸線に対して４５度で傾斜する第６回転軸線まわりに回転自在に、第５アーム体の他端部に連結され、
エンドエフェクタは、第６アーム体の他端部に、第７関節部によって、第６アーム体の軸線と同軸の第７回転軸線５まわりに回転自在に連結され、
前記設定部分は、エンドエフェクタであり、
前記ロボットの基端側に位置する２つの関節部の一方は、第１関節部であり、
前記ロボットの先端側に位置する２つの関節部の一方は、第４関節部であり、
冗長関節部を選択する工程では、前記第１関節部と前記第４関節部とだけが冗長関節部として選択可能に設定された操作手段を用いて冗長関節部が選択されることを特徴とする。
Further, the present invention is a method for controlling a robot having only seven joints, having a redundancy degree of freedom of 1, and having rotation axes of three joints adjacent to each other at the tip of the robot intersecting at one point. ,
Two input means provided in the operating means for selecting one of the two joint portions located on the proximal end side of the robot among the four joint portions adjacent to each other at the proximal end portion of the robot input means, and selects one of the redundant joints from among the four joint portions by either of the two input means of the input means for selecting one of the two joint portions located on the distal end side of the robot ,
The selected redundant joint portion is angularly displaced while fixing the six degrees of freedom of the position and posture of the setting portion set at the distal end portion of the robot with respect to the proximal end portion of the robot.
Further, in the present invention, the robot has seven arm bodies, a base, and an end effector connected by the seven joint portions,
One end portion of the first arm body is connected to the base by the first joint portion so as to be rotatable about a first rotation axis coaxial with the axis of the first arm body,
One end portion of the second arm body is rotatable by the second joint portion around a second rotation axis inclined at 45 degrees with respect to the axis of the first arm body and the axis of the second arm body. Connected to the other end of the
One end portion of the third arm body is rotatable by the third joint portion around the third rotation axis inclined at 45 degrees with respect to the axis of the second arm body and the axis of the third arm body. Connected to the other end of the
The second rotation axis and the third rotation axis are parallel,
One end portion of the fourth arm body is rotatable by the fourth joint portion so as to be rotatable around a fourth rotation axis inclined at 45 degrees with respect to the axis of the third arm body and the axis of the fourth arm body. Connected to the other end of the
The third rotation axis and the fourth rotation axis are vertical,
One end portion of the fifth arm body can be rotated by the fifth joint portion around the axis of the fourth arm body and the fifth rotation axis coaxial with the axis of the fifth arm body, and to the other end portion of the fourth arm body. Concatenated,
One end portion of the sixth arm body is rotatable by the sixth joint portion around a sixth rotation axis inclined at 45 degrees with respect to the axis of the fifth arm body and the axis of the sixth arm body. Connected to the other end of the
The end effector is connected to the other end of the sixth arm body by a seventh joint so as to be rotatable about a
The setting portion is an end effector;
One of the two joint portions located on the proximal end side of the robot is a first joint portion,
One of the two joint portions located on the tip side of the robot is a fourth joint portion,
In the step of selecting a redundant joint part, the redundant joint part is selected by using an operating means in which only the first joint part and the fourth joint part can be selected as redundant joint parts. .
本発明に従えば、設定部分を目的とする固定位置に正確に維持したうえで、ロボットの各関節部の角度を変更可能に、冗長ロボットを制御することができる。 According to the present invention, it is possible to control the redundant robot so that the angle of each joint portion of the robot can be changed while the setting portion is accurately maintained at the intended fixed position.
図１は、本発明の第１実施形態である多関節ロボット３０の制御装置２を示すブロック図である。制御装置２によって制御される多関節ロボット３０は、４自由度を有する１軸冗長ロボットであり、４つのアーム体３〜６と、隣接する２つのアーム体を回転自在に連結する４つの関節部９〜１２と、基台１７とを有する。各アーム体３〜６は、それぞれ連結されて直列状に延びるアーム構成体１６を構成する。また基台１７は、アーム構成体１６の基端部に連結され、壁面または床面などの固定位置に予め固定される。
FIG. 1 is a block diagram showing the
各関節部９〜１２は、同軸関節部と傾斜関節部とのうちのいずれかである。同軸関節部は、隣接する２つのアーム体を、その各アーム体の軸線と同軸の回転軸線まわりに回転自在に連結する。傾斜関節部は、隣接する２つのアーム体のうちの一方を、各アーム体の軸線に対して傾斜する回転軸線まわりに円錐回転自在に連結する。 Each joint part 9-12 is either a coaxial joint part or an inclined joint part. The coaxial joint portion connects two adjacent arm bodies so as to be rotatable around a rotation axis coaxial with the axis of each arm body. The inclined joint portion connects one of two adjacent arm bodies so as to be conically rotatable about a rotation axis inclined with respect to the axis of each arm body.
各関節部９〜１２は、基台１７から第１〜第４関節部９〜１２の順で並ぶ。第１関節部９は、同軸関節部である。また第２関節部１０、第３関節部１１および第４関節部１２は、傾斜関節部である。また本発明において、用語「回転」とは、回転軸線まわりの３６０度以上の回転だけでなく、軸線まわりに３６０度以下の角度で角変位する状態も含むものとする。
The
第１アーム体３の一端部は、第１関節部９によって、第１アーム体３の軸線Ａ３と同軸の第１回転軸線Ａ９まわりに回転自在に基台１７に連結されている。また第２アーム体４の一端部は、第２関節部１０によって、第１アーム体３の軸線Ａ３および第２アーム体４の軸線Ａ４に対して所定の角度、本実施の形態では４５度で傾斜する第２回転軸線Ａ１０まわりに回転自在に、第１アーム体３の他端部に連結される。また第３アーム体５の一端部は、第３関節部１１によって、第２アーム体４の軸線Ａ４および第３アーム体５の軸線Ａ５に対して所定の角度、本実施の形態では４５度で傾斜する第３回転軸線Ａ１１まわりに回転自在に、第２アーム体４の他端部に連結される。
One end portion of the
第４アーム体６の一端部は、第４関節部１２によって、第３アーム体５の軸線Ａ５および第４アーム体６の軸線Ａ６に対して所定の角度、本実施の形態では４５度で傾斜する第４回転軸線Ａ１２まわりに回転自在に、第３アーム体５の他端部に連結される。第４アーム体６の他端部には、設定部分となる対象点Ｐｈが予め設定される。したがって対象点Ｐｈは、アーム構成体１６の先端部に配置され、基台１７は、アーム構成体１６の基端部に配置される。
One end portion of the
このように各関節部９〜１２は、隣接するアーム体を互いに回転可能に連結する。また各関節部９〜１２が角変位することで、アーム構成体１６は、図１に示すように、アーム体３〜６のそれぞれの軸線Ａ３〜Ａ６が同軸に配置されて一直線状に延びる状態に変形可能に構成される。各アーム体３〜６の軸線Ａ３〜Ａ６が一直線状に延びた状態では、第２回転軸線Ａ１０と第３回転軸線Ａ１１とは平行となり、第３回転軸線Ａ１１と第４回転軸線Ａ１２とは垂直となる。
Thus, each joint part 9-12 connects adjacent arm bodies so that rotation is mutually possible. Further, as the
多関節ロボット３０は、各関節部９〜１２を回転させることで、アーム構成体１６を蛇のように変形させ、対象点Ｐｈを任意の３次元位置に位置決め可能である。たとえば多関節ロボット３０は、設備等が複雑に入り組んで作業経路が複雑な場合や、他のロボットと近接して並列に配置されて動作範囲を大きくすることができない場合に好適に用いられる。
The
このような４つの関節部９〜１２を有する多関節ロボット３０は、４自由度を有するので、可動範囲内で対象点Ｐｈを予め定める３次元位置に位置合わせした状態で、第４アーム体６の取り得る姿勢が複数通り存在する。言換えると、多関節ロボット３０は、対象点Ｐｈを予め定める３次元位置に位置合わせした状態で、４つの関節部９〜１２のそれぞれの取り得る角度の組合せが複数通り存在する。
Since the articulated
各アーム体３〜６は、各アーム体３〜６をそれぞれ個別に回転駆動するモータを内蔵する。モータは、各アーム体３〜６を予め定める任意の角度に変位可能に構成される。
Each arm body 3-6 incorporates a motor that individually rotates and drives each arm body 3-6. The motor is configured to be able to displace each of the
各アーム体３〜６を対応する回転軸線Ａ９〜Ａ１２まわりに回転可能に連結するための、各関節部９〜１２の回転機構は、入力側部材と出力側部材とを備え、それらが相対的に回転可能に設けられる。入力側部材は、各関節部９〜１２によって連結する２つの部材の一方に連結され、出力側部材は、前記２つの部材のうち他方に連結される。モータからの回転力が入力側部材に与えられると、入力側部材と出力側部材とが相対的に回転する。これによって一方の部材と他方の部材とを相対的に回転させることができる。同軸関節部９では、その回転軸線Ａ９が、入力側部材および出力側部材に連結されるアーム体と一致する。傾斜関節部１０〜１２では、その回転軸線Ａ１０〜Ａ１２が、入力側部材および出力側部材にそれぞれ連結されるアーム体の軸線に対してそれぞれ傾斜する。
The rotation mechanism of each joint portion 9-12 for rotatably connecting each arm body 3-6 around the corresponding rotation axis A9-A12 includes an input side member and an output side member, which are relative to each other. Is rotatably provided. The input side member is connected to one of the two members connected by the
このような回転機構が設けられることによって、隣接する２つの部材を相対的に回転さ
せることができる。ここで、本発明における「関節部の角度」とは、関節部における入力側部材に対する出力側部材の角度を意味する。言い換えると、関節部によって連結される２つの部材のうち、入力側部材に連結される一方の部材に対する、出力側部材に連結される他方の部材の関節部の回転軸線まわりの角度を意味する。また本発明において「関節部の移動」とは、関節部の角度の角変位移動を示す。
By providing such a rotation mechanism, two adjacent members can be relatively rotated. Here, the “angle of the joint part” in the present invention means an angle of the output side member with respect to the input side member in the joint part. In other words, it means an angle around the rotation axis of the joint portion of the other member connected to the output side member with respect to one member connected to the input side member of the two members connected by the joint portion. Further, in the present invention, “movement of the joint portion” indicates angular displacement movement of the angle of the joint portion.
制御装置２は、入力部２０と、中央演算処理部１９と、出力部２２とを含んで構成される。入力部２０および出力部２２は、中央演算処理部１９に電気的に接続される。入力部２０は、操作者からの各種指令情報が入力され、入力された各種情報を中央演算処理部１９に与える。本実施の形態では、中央演算処理部１９と出力部２２とは、制御装置本体２１に設けられ、入力部２０は、制御装置本体２１とは別体に設けられる。具体的には、入力部２０は、操作者が多関節ロボットを直接制御するためのティーチペンダントによって実現され、制御装置本体２１であるロボットコントローラに対して、ケーブルによって電気的に接続される。
The
中央演算処理部１９は、予め記憶される動作プログラムに従って、入力部２０から与えられる各種情報に応答して、多関節ロボット３０を制御するために必要な演算を行う。本実施の形態では、中央演算処理部１９は、第４アーム体６に設定される対象点Ｐｈを予め定める目標位置に移動させるのに必要な、各関節部９〜１２の角度ｑ１〜ｑ４をそれぞれ演算する。中央演算処理部１９は、ＣＰＵなどの処理回路と、メモリなどの記憶回路とを含んで実現される。記憶回路には、動作プログラムとともに、演算に必要な計算式、アーム体の長さ、関節部の種類などを記憶する。
The
出力部２２は、中央演算処理部１９の演算結果を多関節ロボット３０に与える出力手段となる。本実施の形態では、出力部２２は、中央演算処理部１９の演算結果を多関節ロボット３０に設けられる電源回路に与える。電源回路は、中央演算処理部１９の演算結果に応じた動力を、関節部９〜１２毎に設けられるモータにそれぞれ個別に与える。動力が与えられた各モータは、中央演算処理部１９の演算結果に従って、対応する関節部を所定の角度に角変位させる。
The
また制御装置２は、第４アーム体６の対象点Ｐｈを予め指定される移動経路に沿って移動させるにあたって、回転すべき各関節部９〜１２の角度を演算する。この移動経路は、ロボット動作前に、操作者によって入力部２０から中央演算処理部１９に入力される。また多関節ロボット３０は、４つの関節部９〜１２を有することで、基台１７に対して、対象点Ｐｈを予め定める位置で固定した状態で、各関節部の取り得る角度の組合せが複数通り存在する。
Further, the
本実施形態の制御装置２は、ロボットの動作を教示するための教示動作を行うために用いられ、教示動作のための形態変更動作を行う。形態変更動作とは、対象点Ｐｈを予め定める固定条件で固定した状態で、第１〜第４関節部９〜１２の角度を変化させて、ロボットの形状および姿勢などの外形的形態を変更する動作である。本実施の形態では形態変更動作は、基台１７に設定されるベース座標に対する対象点Ｐｈの３次元位置を固定した状態で、第１〜第４関節部９〜１２の角度を変化させる動作となる。言換えると、形態変更動作では、アーム構成体１６のうちで直列方向一端部に設定される対象点Ｐｈが、アーム構成体１６の他端部に設定される座標系に従って求められる拘束条件で固定される。
The
形態変更動作を行うにあたって、制御装置２は、第１〜第４関節部９〜１２のうちから選択された１つの冗長関節部と、その冗長関節部の角度である冗長角度と、対象点Ｐｈを固定すべき３次元位置とが入力部２０を介して与えられる。中央演算処理部１９は、対象点Ｐｈを固定すべき固定状態に固定したうえで、冗長関節部が冗長角度となる場合の、残
余の関節部の角度を演算する。出力部２２は、中央演算処理部１９の演算結果を多関節ロボット３０に与える。演算結果が与えられた多関節ロボット３０は、冗長関節部を冗長角度とするとともに、残余の関節部の角度を演算結果に応じて位置合わせして、対象点Ｐｈを指定された固定状態とする。
In performing the shape changing operation, the
形態変更動作を行うことによって、操作者は、入力部２０を介して冗長関節部を選択するとともに、その冗長角度を入力することで、対象点Ｐｈを固定した状態で、アーム構成体１６の形態を変更させることができる。この場合、操作者は、中央演算処理部１９に形態変更動作を行わせて、多関節ロボット３０の形態を確認することによって、対象点Ｐｈを固定した状態で、障害物との衝突を回避するアーム構成体１６の形態を実現する各関節部９〜１２の角度を容易に決定することができる。操作者は、障害物との衝突を回避する各関節部９〜１２の角度ｑ１〜ｑ４を決定すると、その各関節部９〜１２の角度ｑ１〜ｑ４をロボットの移動動作を行う場合の情報として用いる。
By performing the form changing operation, the operator selects the redundant joint part via the
図２は、入力部２０を示す正面図である。入力部２０には、ロボット教示のための情報および指令が入力される操作部４１と、操作画面を表示する表示部４２とを含む。操作部４１には、各関節部操作スイッチ４３と、ツール移動スイッチ４４と、形態変更スイッチ４５とを含む。また操作部４１は、ロボットの動作プログラムを入力するための命令キー、カーソルキーおよびテンキーなどをさらに含む。
FIG. 2 is a front view showing the
関節部操作スイッチ４３は、各関節部９〜１２の角度ｑ１〜ｑ４を入力するためのスイッチであって、各関節部９〜１２について、プラス方向およびマイナス方向への予め定める角変位量の角変位指令が与えられる。ツール移動スイッチ４４は、ベース座標軸またはツール座標軸に従う対象点Ｐｈの移動指令が与えられる。ここで、ベース座標軸とは、基台１７に設定される座標であって、ツール座標軸とは、第４アーム体６に設定される座標である。また形態変更スイッチ４５は、対象点Ｐｈを予め定める位置に固定したまま、アーム構成体１６の形態を変化させるための形態変更指令が与えられる。
The joint
形態変更動作を行う場合、ツール移動スイッチ４４は、対象点Ｐｈを固定すべき位置の情報が入力される固定条件入力手段となる。また形態変更スイッチ４５が押下された状態で、関節部操作スイッチ４３によって１つの関節部の角変位量が指定されると、指定された関節部が冗長関節部として入力される。また、その角変位量に対応する角度が冗長関節部の角度として入力される。したがって形態変更スイッチ４５と関節部操作スイッチ４３とによって、冗長関節部が入力される冗長関節部入力手段と、冗長関節部の角度である冗長角度が入力される冗長角度入力手段とが構成される。
When performing the form changing operation, the
図３は、中央演算処理部１９の形態変更動作の手順を説明するためのフローチャートである。まずステップａ０で、中央演算処理部１９は、多関節ロボット３０を制御可能な状態で、形態変更スイッチ４５が押下されたことを判断すると、ステップａ１に進み、形態変更動作を開始する。
FIG. 3 is a flowchart for explaining the procedure of the form changing operation of the
ステップａ１では、中央演算処理部１９は、ツール移動スイッチ４４によって対象点Ｐｈの移動指令が与えられると、対象点Ｐｈを指定された位置に移動するための演算結果を出力部２２に与える。またツール移動スイッチ４４による移動指令がなければ、現時点の対象点Ｐｈの位置を維持させる。中央演算処理部１９は、関節部操作スイッチ４３が押下されたと判断すると、その時点の対象点Ｐｈの位置を固定すべき固定条件として決定し、ステップａ２に進む。
In step a <b> 1, when the movement command for the target point Ph is given by the
ステップａ２では、中央演算処理部１９は、関節部操作スイッチ４３によって指定された関節部を、冗長関節部として決定する。冗長関節部は、第１〜第４関節部９〜１２のい
ずれか１つとなる。また関節部操作スイッチ４３によって指定された角度を、冗長角度として決定する。本実施の形態では、関節部操作スイッチ４３のうち、プラス方向移動スイッチが押圧されると、関節部操作スイッチ４３が押下される前の状態から、予め定める角変位量で冗長関節部がプラス方向に軸線まわりに角変位した角度を冗長角度として決定する。またマイナス方向移動スイッチが押圧されると、関節部操作スイッチ４３が押下される前の状態から、予め定める角変位量で冗長関節部がマイナス方向に軸線まわりに角変位した角度を冗長角度として決定する。このように冗長関節部および冗長角度を決定するとステップａ３に進む。
In step a2, the
ステップａ３では、中央演算処理部１９は、ステップａ１で決定した対象点Ｐｈの３次元位置に、対象点Ｐｈが位置する場合における、第１〜第４関節部９〜１２の角度ｑ１〜ｑ４を求める。具体的には、第１〜第４関節部９〜１２のそれぞれの角度ｑ１〜ｑ４を変数として、対象点Ｐｈの３次元位置を求めるための順変換関係式と、ステップａ２で決定した冗長関節部の冗長角度と、ステップａ１で決定した対象点Ｐｈの３次元位置とに基づくことによって、第１〜第４関節部９〜１２の角度ｑ１〜ｑ４を解析的に求めることができる。
In step a3, the
順変換問題を解くことによって、対象点Ｐｈのｘ座標位置を表わすｘ座標式、対象点Ｐｈのｙ座標位置を表わすｙ座標式、対象点Ｐｈのｚ座標位置を表わすｚ座標式は、第１〜第４関節部９〜１２の各角度ｑ１〜ｑ４を変数とする関数で表わされる。順変換問題によるｘ座標式、ｙ座標式およびｚ座標式に、ステップａ１で求めた対象点Ｐｈの３次元位置を代入することで、それらの座標式は、第１〜第４関節部９〜１２の角度ｑ１〜ｑ４の数、すなわち４つの未知数を有する三元連立方程式となる。
By solving the forward conversion problem, the x coordinate expression representing the x coordinate position of the target point Ph, the y coordinate expression representing the y coordinate position of the target point Ph, and the z coordinate expression representing the z coordinate position of the target point Ph are the first -It represents with the function which makes each angle q1-q4 of the 4th joint parts 9-12 a variable. By substituting the three-dimensional position of the target point Ph obtained in step a1 into the x-coordinate expression, the y-coordinate expression, and the z-coordinate expression based on the forward conversion problem, these coordinate expressions are obtained from the first to fourth
また第１〜第４関節部９〜１２の各角度ｑ１〜ｑ４のうちの１つは、冗長角度としてステップａ２で決定しているので、冗長角度を上述した３元連立方程式に代入することによって、３つの未知数を有する三元連立方程式となる。この３元連立方程式を変形することで、３つの未知数をそれぞれ求める方程式を表わす計算式を導出することができる。この方程式に、対象点Ｐｈの座標、冗長関節部および冗長関節部の角度を代入することで、第１〜第４関節部９〜１２のうちで、冗長関節部以外の関節部の各角度をそれぞれ解析的に求めることができる。
In addition, since one of the angles q1 to q4 of the first to fourth
中央演算処理部１９は、冗長関節部の冗長角度と、対象点Ｐｈのｘ座標、ｙ座標、ｚ座標を代入することで、第１〜第４関節部９〜１２のうちで冗長関節部以外の各関節部の角度を解析的に求めることが可能な方程式を表わす計算プログラムを記憶している。中央演算処理部１９は、第１〜第４関節部９〜１２の計算プログラムにしたがって、第１〜第４関節部９〜１２のうちで、冗長関節部以外の各関節部の角度を解析的に演算し、ステップａ４に進む。冗長関節部の角度は、ステップａ２で既に決定しているので、演算することなく求めることができる。このように中央演算処理部１９は、第１〜第４関節部９〜１２の角度ｑ１〜ｑ４をそれぞれ演算する角度演算手段となる。
The
ステップａ４では、中央演算処理部１９は、求めた各関節部９〜１２の各角度ｑ１〜ｑ４が可動限界に到達していないことを判断するとステップａ５に進み、各角度ｑ１〜ｑ４のいずれかが可動限界に到達していること、または可動限界を超えていることを判断すると、ステップａ６に進む。たとえば中央演算処理部１９は、ステップａ３で求めた各関節部の角度が、解析的に求められる最大値または最小値となると、可動限界に到達したことを判断する。また各関節部の角度を求められない場合に可動限界を超えていると判断する。
In step a4, when the
ステップａ５では、ステップａ３で解析的に求めた第１〜第４関節部９〜１２の角度ｑ
１〜ｑ４を演算結果として、出力部２２に与える。出力部２２は、与えられた演算結果を多関節ロボット３０に与える。これによって多関節ロボット３０は、各関節部９〜１２が角変位し、対象点Ｐｈを固定位置に維持した状態で、冗長関節部を冗長角度とする。このとき、第１〜第４関節部９〜１２の角度ｑ１〜ｑ４は、収束演算などの近似法とは異なり、解析的な演算方法によって求められるので、冗長関節部の移動前と移動後とで、固定位置から対象点Ｐｈがずれることがない。このように演算結果を出力部２２に与えて、多関節ロボット３０を動作させると、ステップａ６に進む。
In step a5, the angle q of the first to fourth
1 to q4 are given to the
ステップａ６では、中央演算処理部１９は、形態変更動作を終了する終了条件を満足するか否かを判断する。たとえば形態変更解除スイッチが押下されることで、終了条件を満足することを判断してもよい。中央演算処理部１９は、形態変更動作の終了条件を満足していないと判断すると、ステップａ１に戻り、関節部操作スイッチ４３が再び押下されるのを待機する。この状態で、関節部操作スイッチ４３が再度押下されると、その指示に従ってステップａ１〜ａ５を順に行う。またステップａ６で終了条件を満足することを判断すると、ステップａ７に進み、ステップａ７で形態変更動作を終了する。
In step a6, the
このようにして、中央演算処理部１９が形態変更動作を行うことによって、操作者が、対象点Ｐｈの固定位置と、冗長関節部と、その冗長角度とを指定することによって、対象点Ｐｈの位置を確実に固定したまま、各関節部９〜１２を角変位させて、多関節ロボット３０の形態を変更させることができる。このとき各関節部９〜１２の角度ｑ１〜ｑ４を解析解として求めることができるので、収束演算を用いて解を求める場合に比べて短時間で各関節部９〜１２の角度を求めることができ、対象点Ｐｈの位置を固定したままロボットの形態を滑らかに変更することができる。
In this way, when the
また操作者は、多関節ロボット３０の形態を確認しながら、中央演算処理部１９に形態変更動作を行わせることができる。これによって、操作者は、対象点Ｐｈを固定位置に位置させることができる各関節部９〜１２の角度ｑ１〜ｑ４の複数の組合せのうちから、１つを容易に選択することができる。これによって操作者は、障害物に衝突することを回避した多関節ロボット３０の形態で、対象点Ｐｈを固定位置に配置させることができ、各関節部９〜１２の角度ｑ１〜ｑ４を求めることができる。
In addition, the operator can cause the
図４は、対象点Ｐｈの位置を予め定める位置に固定した状態で、第１関節部９を角変位させた多関節ロボット３０の状態を示す正面図である。第１関節部９の角度ｑ１をプラス方向に角変位するに伴って、多関節ロボット３０は、図４（１）〜図４（８）の順で変形する。また図５は、第１関節部９の角度ｑ１を冗長角度として設定した場合における第２〜第４関節部１０〜１２の角度ｑ２〜ｑ４を示すグラフである。図５（１）は、横軸に第１関節部９の角度ｑ１を示し、縦軸に第２関節部１０の角度ｑ２を示す。また図５（２）は、横軸に第１関節部９の角度ｑ１を示し、縦軸に第３関節部１１の角度ｑ３を示す。図５（３）は、横軸に第１関節部９の角度ｑ１を示し、縦軸に第４関節部１２の角度ｑ４を示す。
FIG. 4 is a front view showing a state of the articulated
詳細については後述するが、本実施の形態では、対象点Ｐｈを予め定める３次元位置に固定した状態に維持する、各関節部９〜１２の角度ｑ１〜ｑ４の解析解を決定することができる。これによって各関節部９〜１２の角度ｑ１〜ｑ４の近似解を決定する場合に比べて、対象点Ｐｈが固定位置からずれることを防ぐことができる。たとえばステップａ２において、関節部操作スイッチ４３のうち、プラス方向またはマイナス方向移動スイッチが１回押圧された場合に、冗長関節部が角変位する角変位量が大きく設定される場合であっても、対象点Ｐｈの固定状態を正確に維持することができる。同様に、ステップａ２において、冗長関節部が高速で移動する場合であっても、対象点Ｐｈの固定状態を正確に維持することができる。
Although details will be described later, in the present embodiment, it is possible to determine an analytical solution for the angles q1 to q4 of the
これによって形態変更動作によって、多関節ロボット３０の障害物回避形態の探索を行った場合、対象点Ｐｈの位置を正確に固定した状態で、各関節部９〜１２を角変位させることができる。また対象点Ｐｈを固定位置に正確に保ちつつ、障害物と各アーム体３〜６との干渉を回避することができるので、対象点Ｐｈの位置ずれに起因する多関節ロボットの動作不良を防ぐことができる。
As a result, when the obstacle avoidance form of the
また中央演算処理部１９は、各関節部９〜１２の角度ｑ１〜ｑ４の解析解を求めることに付随して、冗長関節部が可動限界に到達したか否かを判断することができる。ステップａ４に示すように、中央演算処理部１９は、冗長関節部が可動限界に到達することを判断すると、冗長関節部が可動限界に到達した状態の関節部の角度を演算する。また可動限界を超えるような冗長角度が入力されても、可動限界に到達した状態の関節部の角度を演算する。
In addition, the
これによって中央演算処理部１９が、機構的に移動不可能な移動指令値を、多関節ロボットに与えることを防ぐことができ、関節部の損傷を防ぐことができる。また中央演算処理部１９は、冗長関節部が可動限界に到達したか否かを精度よく判断することができるので、可動限界に近い領域まで冗長関節部を角変位させることができ、ロボットの可動範囲を広くすることができる。
As a result, the
また中央演算処理部１９は、冗長関節部が可動限界に到達する前に、冗長関節部が可動限界近傍の警告領域に到達したことを判断すると、その判断結果を入力部２０の表示部４２を介して報知してもよい。たとえば中央演算処理部１９は、演算によって決定される冗長関節部の可動限界角度よりも予め定める所定角度分手前の角度に、冗長角度が達した場合に、警告領域に到達したことを判断する。この場合、中央演算処理部１９は、警告範囲に到達したことを判断して報知する警告報知手段となる。
When the
たとえば図５の場合では、冗長関節部が第１関節部９であって、第３関節部１１の角度ｑ３が１８０度となった場合に可動限界に達する。この場合、中央演算処理部１９は、第３関節部１１の角度ｑ３が１７５度に達すると、警告領域に到達したことを判断し、判断結果を表示部４２に表示させる。これによって入力部２０を操作する操作者は、冗長関節部が可動限界に近づいていることを判断することができ、利便性を向上することができる。さらに中央演算処理部１９は、冗長関節部が警告範囲に入った場合、その関節部の角度が警告範囲を脱出する角変位方向を入力部２０の表示部４２に表示することが好ましい。
For example, in the case of FIG. 5, the movable limit is reached when the redundant joint is the first joint 9 and the angle q3 of the third joint 11 is 180 degrees. In this case, when the angle q3 of the third
本実施形態の制御装置２が制御する多関節ロボット３０は、傾斜関節部を含むので、形態変更動作における多関節ロボット３０の変形形態は変則的となる。この場合、操作者は、各関節部９〜１２の変形を直感的に予測することが困難である。本実施の形態では、中央演算処理部１９は、警告範囲および警告範囲を脱出するための角変位方向を報知するで、操作者は、警告範囲および脱出方向を容易に判断することができ、利便性を向上することができる。
Since the articulated
次に、ステップａ３における第１〜第４角度ｑ１〜ｑ４の演算工程の詳細について説明する。順変換問題に基づくことによって、第１〜第４関節部９〜１２のそれぞれの角度ｑ１〜ｑ４が決定されると、対象点Ｐｈのｘ座標位置ｐｈｘ、ｙ座標位置ｐｈｙ、ｚ座標位置ｐｈｚが一義的に決定される。すなわち対象点Ｐｈのｘ座標、ｙ座標、ｚ座標を求めるための式は、第１〜第４関節部９〜１２のそれぞれの角度ｑ１〜ｑ４を変数とする関数によって表わされ、本実施の形態では、以下に示す（１）式の関係を有する。
Next, details of the calculation process of the first to fourth angles q1 to q4 in step a3 will be described. When the respective angles q1 to q4 of the first to fourth
（１）式において第１関節部９の角度をｑ１とし、第２関節部１０の角度をｑ２とし、第３関節部１１の角度をｑ３とし、第４関節部１２の角度をｑ４とする。また第１関節部９から第２関節部１０までの第１アーム体３の距離をＬ１とし、第２関節部１０から第３関節部１１までの第２アーム体４の距離をＬ２とし、第３関節部１１から第４関節部１２までの第３アーム体５の距離をＬ３とし、第４関節部１２から対象点Ｐｈまでの距離をＬ４とする。またｓｉｎ（θ）はθの正弦関数（サイン関数）を示し、ｃｏｓ（θ）は、θの余弦関数（コサイン関数）を示す。
In the equation (1), the angle of the first
また（１）式において、対象点Ｐｈの座標ｐｈｘ，ｐｈｙ，ｐｈｚは、基台１７に設定されるベース座標軸に従って、第１アーム体３の代表点ｐ１を原点とした場合の座標である。ここでベース座標軸は、各アーム体の軸線が一直線状に配置された状態で、その直線と同軸に延びる軸をｚ軸とし、ｚ軸に垂直な平面に沿って延びてｚ軸を通過する軸をｘ軸およびｙ軸とする。ｘ軸およびｙ軸は、互いに直交する。また各関節部９〜１２の角度ｑ１〜ｑ４は、各アーム体３〜６の軸線が一直線状に延びた状態から関節部が角変位した角度とする。また第１アーム体３の代表点ｐ１は、第１アーム体の軸線Ａ１と、第２関節部１０の回転軸線Ａ１０との交点に設定される。
In the equation (1), the coordinates phx, phy, phz of the target point Ph are coordinates when the representative point p1 of the
対象点Ｐｈを固定すべき位置のｘ座標ｐｈｘ、ｙ座標ｐｈｙ、ｚ座標ｐｈｚの数値を（１）式に代入することによって、第１〜第４関節部９〜１２の４つの角度ｑ１〜ｑ４が未知数となる３元連立方程式となる。第１〜第４関節部９〜１２のうちの１つを冗長関節部として決定し、冗長関節部の冗長角度を（１）式に代入することによって、第１〜第４関節部９〜１２の４つの角度ｑ１〜ｑ４のうちの１つが既知となり、未知数が３つの３元連立方程式となる。これによって（１）式を連立して解くことによって、対象点Ｐｈを固定位置に位置すべき、各関節部９〜１２の角度ｑ１〜ｑ４を解析的に求めることができる。
By substituting numerical values of the x-coordinate phx, y-coordinate phy, and z-coordinate phz of the position where the target point Ph should be fixed into the equation (1), the four angles q1 to q4 of the first to fourth
たとえば第１関節部９を冗長関節部として決定し、第１関節部９を角変位すべき角度ｑ１を冗長角度として決定する。この場合、（１）式を変形することによって、以下の（２）式に示すような、第１関節部９と第４関節部１２の角度ｑ１，ｑ４とが未知数となる方程式に変形することができる。（２）式に、対象点Ｐｈのｚ座標ｐｈｚ、冗長関節部となる第１関節部９の角度ｑ１を代入することによって、第４関節部１２の角度ｑ４を求めることができる。得られた第４関節部１２の角度ｑ４は、対象点Ｐｈを固定位置に位置させ、かつ第１関節部９を冗長角度である第１角度ｑ１に角変位した場合における第４関節部１２の角度ｑ４を意味する。
For example, the first
（２）式を、以下の（３）式に変形し、第１関節部９の角度ｑ１を代入することによっ
て、第４関節部１２の角度ｑ４を求めることができる。
The angle q4 of the fourth
ここで、ａｒｃｃｏｓは、逆余弦関数（アークコサイン関数）を意味する。また（３）式におけるプラスマイナスの２つの符号を離散形態符号と称する。予め定める条件、たとえば計算前に設定されている第４関節部１２の角度ｑ４などに基づいて、２つの離散形態符号のいずれかを決定することで、対象点Ｐｈを固定位置に固定する場合における、第４関節部１２の角度ｑ４を演算することができる。
Here, arccos means an inverse cosine function (arc cosine function). In addition, two plus and minus codes in the expression (3) are referred to as discrete form codes. In the case where the target point Ph is fixed at a fixed position by determining one of the two discrete form codes based on a predetermined condition, for example, the angle q4 of the fourth
また（３）式において解が１つしかない場合、すなわちアークコサインの変数が、１または−１となる場合、第４関節部１２は特異点に位置する。具体的には、第４関節部１２の角度ｑ４が、０度または１８０度となると、特異点となる。またアークコサインの変数の絶対値が１を超えると、解が存在しない。このような判定を行うことで、中央演算処理部１９は、特異点でない解が存在するか、解が特異点となるか、解が存在しないかを判断することができる。
Further, when there is only one solution in the equation (3), that is, when the arc cosine variable is 1 or −1, the fourth
次に、（１）式を変形することによって、以下の（４）式に示すような、第３関節部１１と第４関節部１２との角度ｑ３，ｑ４が未知数となる方程式に変形することができる。（４）式に、（３）式に基づいて決定した第４関節部１２の角度ｑ４を代入することによって、第３関節部１１の角度ｑ３を求めることができる。得られた第３関節部１１の角度ｑ３は、対象点Ｐｈを固定位置に保ち、かつ第１関節部９を冗長角度である第１角度ｑ１に角変位した場合における第３関節部１１の角度ｑ３を意味する。
Next, by transforming equation (1), it is transformed into an equation in which the angles q3 and q4 between the third
次に、（１）式のうちのｐｈｘ、ｐｈｙ、ｐｈｚを求めるそれぞれの式のいずれか２つを連立させた式に、冗長関節部として設定される第１関節部９の角度ｑ１と、（３）式に基づいて求めた第４関節部１２の角度ｑ４と、（４）式に基づいて求めた第３関節部１１の角度ｑ３とを代入することによって、第２関節部１０の角度ｑ２における、ｃｏｓ（ｑ２）と、ｓｉｎ（ｑ２）を求めることができ、これらから第２関節部１０の角度ｑ２を求めることができる。
Next, an angle q1 of the first
このようにして、第１〜第４関節部９〜１２の角度ｑ１〜ｑ４をそれぞれ求めることができる。この場合、中央演算処理部１９は、上述した（３）式、（４）式、第２関節部１０の角度ｑ２を求める式をそれぞれ示す計算プログラムを記憶することで、第１関節部９の第１角度ｑ１が決定されることで、解が存在する場合、対象点Ｐｈを固定位置に保った場合における第２〜第４関節部１０〜１２の角度ｑ２〜ｑ４をそれぞれ求めることができる。
In this way, the angles q1 to q4 of the first to fourth
また中央演算処理部１９は、冗長関節部の冗長角度を正または負の方向に徐々に角変位させた場合に、冗長関節部の角度が取り得ることが可能な極大値または極小値となった場合、その角度を可動限界として判断することができる。たとえば冗長角度を大きくしてい
くと、冗長関節部以外の残余の関節部の角度の解を求めることができなくなる場合が生じる。この場合、中央演算処理部１９は、各関節部の角度の解を求めることができる冗長角度のうちで、最大値を可動限界として判断する。同様に冗長角度を小さくしていくと、冗長関節部以外の残余の関節部の角度の解を求めることができなくなる場合が生じる。この場合、中央演算処理部１９は、各関節部の角度の解を求めることができる冗長角度のうちで、最小値を可動限界として判断する。
Further, when the redundant angle of the redundant joint portion is gradually angularly displaced in the positive or negative direction, the
中央演算処理部１９は、上述するステップａ１およびステップａ２を終えた状態で、冗長関節部の可動限界を演算することができる。そして指定された冗長角度と、可動限界角度との偏差を求めることができる。この場合、その偏差が予め定める値以下となる場合に、可動限界近傍に設定される警告領域に、冗長関節部が到達したことを判断することができる。
The
また中央演算処理部１９は、現時点での冗長角度に対して、プラス方向に冗長角度を増加させた場合と、マイナス方向に冗長角度を増加させた場合とで、前記偏差の変化量を調べることで、偏差が大きくなる冗長角度の角変位方向を求めることができる。中央演算処理部１９は、偏差が大きくなる冗長角度の角変位方向を、冗長角度が可動限界から遠ざかって、警告領域から脱出する方向として判断することができる。
Further, the
また冗長関節部として、第１関節部以外の関節部が選択される場合には、同様にして（１）式を変形して連立方程式を解くことによって、解が存在する場合、冗長関節部以外の各関節部の角度をそれぞれ求めることができる。したがって中央演算処理部１９は、冗長関節部として選択された関節部に応じて、残余の関節部を求める計算プログラムをそれぞれ記憶することで、第１〜第４関節部９〜１２のいずれが冗長関節部として選択されたとしても、対象点Ｐｈを固定位置に維持しかつ、冗長関節部が冗長角度となる場合の、残余の関節部の角度をそれぞれ演算することができる。
If a joint other than the first joint is selected as the redundant joint, the equation (1) is similarly modified to solve the simultaneous equations. The angle of each joint part can be obtained. Therefore, the
図６は、対象点Ｐｈを固定した場合に多関節ロボット３０が取り得る状態の種類を説明するための１軸冗長ロボット６０を示す正面図である。図６に示す１軸冗長ロボットは、第１〜第４アーム体５０〜５３と、隣接するアーム体を連結する複数の関節部５４〜５７とを有する。各アーム体５０〜５３は、予め定めるｘ軸とｙ軸とを含むｘｙ平面上に配置される。第１アーム体５０の基端部５０ａは、固定部５８に第１関節部５４によって連結され、第１関節部５４によって固定部５８に対して、ｘｙ平面に垂直なｚ軸まわりに角変位可能となる。
FIG. 6 is a front view showing a single-axis
また第２アーム体５１の基端部５１ａは、第２関節部５５によって第１アーム体５０の先端部５０ｂに連結され、第２関節部５５によって第１アーム体５０の先端部５０ｂに対して、ｚ軸まわりに角変位可能となる。また第３アーム体５２の基端部５２ａは、第３関節部５６によって第２アーム体５１の先端部５１ｂに連結され、第３関節部５６によって第２アーム体５１の先端部５１ｂに対して、ｚ軸まわりに角変位可能となる。また第４アーム体５３の基端部５３ａは、第４関節部５７によって第３アーム体５２の先端部５２ｂに連結され、第４関節部５７によって第３アーム体５２の先端部５２ｂに対して、ｚ軸まわりに角変位可能となる。
The
したがって図６に示す１軸冗長ロボット６０は、第４アーム体５３をｘｙ平面内で移動可能であるとともに、第４アーム体５３をｚ軸まわりに回転可能な４関節平面ロボットとなる。
Therefore, the single-axis
このような１軸冗長ロボット６０に関して、第４アーム体５３の先端部５３ｂのｘ座標およびｙ座標と、第４アーム体５３のｚ軸まわりの姿勢とを固定する固定条件、すなわち
３つの自由度を拘束する固定条件を設定する。この固定条件で第４アーム５３を固定した場合における第１〜第４関節部５４〜５７の角度を考える。
With respect to such a one-axis
１軸冗長ロボット６０は、上述した固定条件で第４アーム体５３を固定した状態で、第１〜第４関節部５４〜５７の取り得る組合せが複数存在する。ここで、４つの関節部５４〜５７のうちの１つの角度を設定すれば、固定条件を満足する各関節部の角度を、逆変換により求めることができる。
The single-axis
４自由度を有する１軸冗長ロボット６０は、対象点Ｐｈの３つの自由度を拘束した固定状態では、各アーム体の長さと、第１関節部と第４関節部との間の距離とに応じて、３つの形態を取り得ることになる。
In the fixed state in which the three degrees of freedom of the target point Ph are constrained, the single-axis
第１の形態では回転揺動状態となる。この場合、１軸冗長ロボット６０は、図６（１）に示すように、第４アーム体５３を固定した状態で、第１アーム体５０が固定部５８に対して３６０度以上回転可能でかつ、第３アーム体５２が第４アーム体５３に対して３６０度以下揺動角変位可能となる。また第２アーム体５１は、第１アーム体５０に対して３６０度以上回転可能となり、第３アーム体５２は、第２アーム体５１に対して３６０度未満揺動角変位可能となる。
In the first form, a rotational swing state is established. In this case, as shown in FIG. 6A, the single-axis
第１関節部５４と第２関節部５５との間の距離をＬ１とし、第２関節部５５と第３関節部５６との間の距離をＬ２とし、第３関節部５６と第４関節部５７との間の距離をＬ３とし、第１関節部５４と第４関節部５７との間の距離をＬｗとすると、Ｌｗ＋Ｌ２＞Ｌ１＋Ｌ３、Ｌｗ＞Ｌ２、Ｌ１＞Ｌ３、Ｌｗ−Ｌ２＜Ｌ３−Ｌ１である場合に、回転揺動状態となる。
The distance between the first
また第２の形態では、２重揺動状態となる。この場合、１軸冗長ロボット６０は、図６（２）に示すように、第４アーム体５３を固定した状態で、第１アーム体５０が固定部５８に対して３６０度未満揺動角変位可能となり、第３アーム体５２が第４アーム体５３に対して３６０度未満揺動角変位可能となる。また第２アーム体５１は、第１アーム体５０に対して３６０度未満揺動角変位可能となり、第３アーム体５２は、第２アーム体５１に対して３６０度未満揺動角変位可能となる。
In the second embodiment, a double swing state is established. In this case, as shown in FIG. 6B, the single-axis
また第３の形態では、２重回転状態となる。この場合、１軸冗長ロボット６０は、図６（３）に示すように、第４アーム体５３を固定した状態で、第１アーム体５０が固定部５８に対して３６０度以上回転可能となり、第３アーム体５２が第４アーム体５３に対して３６０度以上回転可能となる。また第２アーム体５１は、第１アーム体５０に対して３６０度以上回転可能となり、第３アーム体５２は、第２アーム体５１に対して３６０度以上回転可能となる。
Moreover, in the 3rd form, it will be in a double rotation state. In this case, as shown in FIG. 6 (3), the single-axis
２重揺動状態および２重回転状態では、第１〜第４関節部５４〜５７のいずれか１つの角度を設定すれば、第４アーム体５３が固定状態となる場合に第１〜第４関節部５４〜５７が取り得る角度の組合せは、１つとなる。したがって第１〜第４関節部５４〜５７のうちから、１つの冗長関節部を設定し、その角度を指定することで、１軸冗長ロボットが取り得る範囲を全て計算することができる。これに対して回転揺動状態では、第１〜第４関節部５４〜５７のうちから１つの冗長関節部を設定しただけでは、１軸冗長ロボット６０が取り得る範囲を全て計算することができない。
In the double rocking state and the double rotation state, if any one angle of the first to fourth
図７は、１軸冗長ロボット６０が回転揺動状態となる場合において、各関節部が取り得る角度の組合せを示す図である。図７（１）は、１軸冗長ロボット６０が初期状態である場合を示し、第１アーム体５０が固定部５８に対して角変位する状態を、図７（２）〜図
７（５）に順に示す。
FIG. 7 is a diagram illustrating combinations of angles that can be taken by the joints when the single-axis
第４関節部５７を冗長関節部として選択する場合、図７（３）に示すように、第３アーム体５２を第４アーム体５３に対して一方向（図７では反時計方向）に角変位して、揺動限界角度に到達すると、それ以上、第３アーム体５２を第４アーム体５３に対して、一方向に角変位させることができなくなる。この場合、第１アーム体５０も、固定部５８に対して一方向にそれ以上角変位することができなくなる。揺動限界角度に到達した状態から、第３アーム体５２を第４アーム体５３に対して他方向（図７では時計方向）に角変位すると、第１アーム体５０も、固定部５８に対して他方向に角変位する。このように第４関節部を冗長関節部として選択すると、第１アーム体５０を固定部５８に対して３６０度回転させることができず、１軸冗長ロボット６０が取り得る範囲を全て計算することができない。これに対して、第１関節部５４を冗長関節部として選択する場合、図７（１）〜図７（５）に示すように、第１アーム体５０を固定部５８に対して一回転させることができ、１軸冗長ロボット６０が取り得る範囲を全て計算することができる。
When the fourth
したがって１軸冗長ロボット６０が、回転揺動状態となる場合、第１関節部５４と第２関節部５５とのうちのいずれか一方と、第３関節部５６と第４関節部５７とのいずれか一方との２つを冗長関節部として選択可能とすることによって、２つのうちいずれかが、回転揺動状態のうちで３６０度以上アーム体が回転する関節部を冗長関節部として選択可能となり、１軸冗長ロボット６０が取り得る範囲を全て計算することができる。
Therefore, when the single-axis
１軸冗長ロボット６０の各アーム体５０〜５３の長さが決定しているとしても、第１関節部５４と第４関節部５７との間の距離Ｌｗの長さが変化することによって、回転揺動状態となるか、２重揺動状態および２重回転状態となるかが変化する。上述したように、第１関節部５４と第２関節部５５とのうちのいずれか一方と、第３関節部５６と第４関節部５７とのいずれか一方との２つを、冗長関節部として選択可能とすることによって、仮に回転揺動状態となったとしても、１軸冗長ロボット６０が取り得る範囲を全て計算することができる。このことは、４つの関節部を有する１軸冗長ロボットに共通することである。したがって図１に示す多関節ロボット３０であっても同様となる。
Even if the lengths of the
図８は、図１に示す多関節ロボット３０において、第１関節部９を冗長関節部として、その角度ｑ１を順次角変位させた状態を示す正面図であり、図８（１）〜図８（３）の順に変化する。また図９は、図８において各関節部９〜１２の角度ｑ１〜ｑ４の時間変化を示すグラフである。図９（１）は、第１関節部９の角度ｑ１の時間変化を示し、図９（２）は、第２関節部１０の角度ｑ２の時間変化を示し、図９（３）は、第３関節部１１の角度ｑ３の時間変化を示し、図９（４）は、第４関節部１２の角度ｑ４の時間変化を示す。
FIG. 8 is a front view showing a state where the first
図８および図９には、対象点Ｐｈを固定位置に位置させた場合に、多関節ロボット３０が回転揺動状態となり、第１関節部９および第２関節部１０が揺動可能であって、第３関節部１１および第４関節部１２が回転可能である場合を示す。この場合、第１関節部９の角度ｑ１を予め定める時間毎に予め定める角変位量、順次変化させると、対象点Ｐｈを所定位置に位置させた状態で、第１関節部９の角変位とともに、第２〜第４関節部１０〜１２が角変位する。
8 and 9, when the target point Ph is located at a fixed position, the
図８（１）に示す初期状態から、第１関節部９の角度ｑ１を、たとえばマイナス方向に角変位すると、図８（２）〜図８（３）に順に示すように、多関節ロボット３０の形態が変化する。第１関節部９の角度ｑ１をマイナス方向に角変位させた場合に、図８（３）に示すように、第１関節部９が０度となることで極小値の可動限界となる。この場合、第４関節部１２の角度ｑ４が１８０度の状態で、第１アーム体３と第２アーム体４とが突っ張ってしまい、さらなる角変位が不可能な状態となる。
When the angle q1 of the first
図１０は、図１に示す多関節ロボット３０において、第４関節部１２を冗長関節部として、その角度ｑ４を順次角変位させた状態を示す正面図であり、図１０（１）〜図１０（５）の順に変化する。また図１１は、図１０において各関節部９〜１２の角度ｑ１〜ｑ４の時間変化を示すグラフである。図１１（１）は、第１関節部９の角度ｑ１の時間変化を示し、図１１（２）は、第２関節部１０の角度ｑ２の時間変化を示し、図１１（３）は、第３関節部１１の角度ｑ３の時間変化を示し、図１１（４）は、第４関節部１２の角度ｑ４の時間変化を示す。
FIG. 10 is a front view showing a state where the fourth
図１０および図１１には、対象点Ｐｈを固定位置に位置させた場合に、多関節ロボットが回転揺動状態となり、第１関節部９および第２関節部１０が揺動可能であって、第３関節部１１および第４関節部１２が回転可能である場合を示す。この場合、第４関節部１２を予め定める時間毎に予め定める角変位量、順次変化させると、対象点Ｐｈを所定位置に位置させた状態で、第４関節部１２の角変位とともに、第１〜第３関節部９〜１１が角変位する。
10 and 11, when the target point Ph is positioned at a fixed position, the articulated robot is in a rotational swinging state, and the first
図１１（１）に示す初期状態から、第４関節部１２の角度ｑ４を、たとえばプラス方向に角変位すると、図１１（２）〜図１１（５）に順に示すように、多関節ロボット３０の形態が変化する。第４関節部１２の角度ｑ４を指定してプラス方向に角変位させた場合には、第４関節部１２の角度ｑ４が１８０度以上となっても、第１アーム体３と第２アーム体４とが突っ張ることがなく、第１関節部９を冗長関節部として選択した場合に比べて、第４関節部１２のさらなる角変位が可能となる。
When the angle q4 of the fourth
このように、第４関節部１２を冗長関節部として選択することで、対象点Ｐｈを固定状態に維持したうえで、第１〜第４関節部９〜１２が取り得ることが可能な全ての角度の組合せを演算可能となる。実際には、対象点Ｐｈの固定位置によっては、第１または第２関節部を冗長関節部として選択したほうが、第１〜第４関節部９〜１２が取り得ることが可能な全ての角度の組合せを演算可能となる場合がある。
In this way, by selecting the fourth
したがって第１関節部９と第２関節部１０との２つのうちの１つと、第３関節部１１と第４関節部１２との２つのうちの１つとを冗長関節部として選択することで、対象点Ｐｈを固定状態に維持したうえで、第１〜第４関節部９〜１２が取り得ることが可能な全ての角度の組合せを演算可能となる。
Therefore, by selecting one of the two of the first
このことから本発明の第２実施形態として、図２に示す関節部操作スイッチ４３は、第１関節部９と第２関節部１０のいずれかと、第３関節部１１と第４関節部１２のいずれかとの２つが選択可能に配置される。このように４つではなく２つのスイッチが設けられるだけでも、第１〜第４関節部９〜１２が取り得ることが可能な全ての角度の組合せを演算可能となる。これによって操作者は、４つの関節部９〜１２の全てを冗長関節部として選択可能とする必要がなく、形態変更動作における入力部２０の操作性を向上することができる。また関節部操作スイッチ４３を、４つから２つのスイッチとすることで、形態変更動作に必要なスイッチを減らすことができ、操作性を向上するとともに、スイッチとして必要な部品点数を減らすことができる。
Therefore, as a second embodiment of the present invention, the joint
また中央演算処理部１９は、第１および第２関節部９，１０のいずれかが冗長関節部として選択された場合と、第３および第４関節部１１，１２のいずれかが冗長関節部として選択された場合とにおいてのみ各関節部を演算するだけでよく、各関節部の角度を演算するのに必要な計算プログラムを２つにすることができ、計算プログラムを４つ用意する場合に比べて、中央演算処理部１９に必要な記憶容量を小さくすることができる。また本実施の形態では、第１関節部９と、第４関節部１２とが冗長関節部として選択可能に設定さ
れる。このように第１関節部９と第４関節部１２とを冗長関節部として選択可能とすることによって、他の関節部を冗長関節部として選択する場合に比べて、各関節部の角度を演算するための計算プログラムを簡単化することができ、短時間に各関節部の角度を求めることができる。
Further, the
図１２は、中央演算処理部１９の他の形態変更動作の手順を説明するためのフローチャートである。まずステップｂ０で、中央演算処理部１９は、多関節ロボット３０を制御可能な状態で、形態変更スイッチ４５が押下されたことを判断すると、ステップｂ１に進み、形態変更動作を開始する。
FIG. 12 is a flowchart for explaining the procedure of another form changing operation of the
中央演算処理部１９は、ステップｂ１〜ｂ２で、上述したステップａ１〜ａ２と同様の同様の手順を順に行い、ステップｂ３に進む。ステップｂ３では、中央演算処理部１９は、上述するステップａ３と同様にして、第１〜第４関節部９〜１２の角度を求めるための計算プログラムにしたがって、第１〜第４関節部９〜１２のうちで、冗長関節部以外の各関節部の角度を解析的に求める。また冗長関節部の角度は、ステップｂ２で既に決定しているので、演算することなく求めることができる。このようにして第１〜第４関節部９〜１２の各角度ｑ１〜ｑ４を求めるとステップｂ４に進む。
The
ステップｂ４では、中央演算処理部１９は、ステップａ４と同様にして、ステップｂ３の演算結果に基づいて、ステップｂ２で決定された冗長関節部の角度が可動限界に到達していないことを判断すると、ステップｂ５に進む。またステップｂ２で決定された冗長関節部の角度が可動限界に到達または超えていることを判断するとステップｂ９に進む。
In step b4, when the
ステップｂ５では、現時点での冗長関節部の角度が可動限界に到達しているか否かを判断する。現時点での冗長関節部が可動限界に到達していないと判断するとステップｂ６に進み、現時点での冗長関節部が可動限界に到達していると判断するとステップｂ７に進む。 In step b5, it is determined whether or not the current angle of the redundant joint portion has reached the movable limit. If it is determined that the current redundant joint has not reached the movable limit, the process proceeds to step b6. If it is determined that the current redundant joint has reached the movable limit, the process proceeds to step b7.
ステップｂ６では、各関節部９〜１２において演算した角度が２とおり求められた場合、以前に、冗長関節部以外の各関節部の角度を求めるために用いた離散形態符号を用いて、２つの角度のうちから１つの角度を選択する。たとえば前回角度がプラスの離散形態符号を用いて決定される場合には、ステップｂ６では、プラスの離散形態符号を用いた場合に得られる角度を関節部が移動すべき角度として選択する。このように関節部が取り得る２とおりの角度のうちから１つの角度を選択するとステップｂ８に進む。 In step b6, when two angles calculated in each joint part 9-12 are obtained, two discrete form codes previously used for obtaining the angles of the joint parts other than the redundant joint part are used. One angle is selected from the angles. For example, when the previous angle is determined using a positive discrete form code, in step b6, the angle obtained when the positive discrete form code is used is selected as the angle at which the joint portion should move. When one angle is selected from the two angles that can be taken by the joint, the process proceeds to step b8.
ステップｂ７では、各関節部９〜１２において演算した角度が２とおり求められた場合、以前に、冗長関節部以外の各関節部の角度を求めるために用いた離散形態符号を反転した離散形態符号を用いて、２つの角度のうちから１つの角度を選択する。たとえば前回角度がプラスの離散形態符号を用いて決定される場合には、ステップｂ７では、マイナスの離散形態符号を用いた場合に得られる角度を関節部が移動すべき角度として選択する。このように関節部が取り得る２とおりの角度のうちから１つの角度を選択するとステップｂ８に進む。 In step b7, when two angles calculated in each joint part 9-12 are obtained, a discrete form code obtained by inverting the discrete form code used to obtain the angles of the joint parts other than the redundant joint part previously. Is used to select one angle from two angles. For example, when the previous angle is determined using a positive discrete form code, in step b7, the angle obtained when the negative discrete form code is used is selected as the angle at which the joint portion should move. When one angle is selected from the two angles that can be taken by the joint, the process proceeds to step b8.
ステップｂ８では、ステップｂ６またはステップｂ７で求めた第１〜第４関節部９〜１２の角度ｑ１〜ｑ４を演算結果として、出力部２２に与え、ステップｂ９に進む。出力部２２は、与えられた演算結果を多関節ロボット３０に与える。これによって多関節ロボット３０は、対象点Ｐｈの位置を維持した状態で、指定された冗長関節部が冗長角度となるように、各関節部９〜１２を角変位する。
In step b8, the angles q1 to q4 of the first to fourth
ステップｂ９では、中央演算処理部１９は、形態変更動作を終了する終了指令が与えら
れた否かを判断する。たとえば再び形態変更スイッチ４５が押下されることで、終了指令が与えられたことを判断してもよい。中央演算処理部１９は、形態変更動作を終了する指令が与えられていないと判断すると、ステップｂ１に戻り、関節部操作スイッチ４３が再び押下されるのを待機する。またステップｂ９で形態変更動作を終了する指令が与えられたことを判断すると、ステップｂ１０に進み、ステップｂ１０で形態変更動作を終了する。
In step b9, the
図１３は、中央演算処理部１９が図１２に示す形態動作の手順を行った場合における第１〜第４関節部９〜１２の角度ｑ１〜ｑ４を示すグラフである。図１３（１）は、第１関節部９の角度ｑ１の時間変化を示し、図１３（２）は、第２関節部１０の角度ｑ２の時間変化を示し、図１３（３）は、第３関節部１１の角度ｑ３の時間変化を示し、図１３（４）は、第４関節部１２の角度ｑ４の時間変化を示す。
FIG. 13 is a graph showing the angles q1 to q4 of the first to fourth
対象点Ｐｈの位置を決定した状態で、操作者が入力部２０を操作して、第１関節部９に対応するマイナス方向移動スイッチを一度押下する。この場合、中央演算処理部１９は、第１関節部９を冗長関節部として決定する。また中央演算処理部１９は、第１関節部９について、マイナス方向移動スイッチ押下前の角度から予め定める角変位量で、マイナス方向に角変位した位置を冗長角度として決定する。
With the position of the target point Ph determined, the operator operates the
図１２に示す形態動作の手順を行う場合、マイナス方向移動スイッチが順次押下されると、中央演算処理部１９は、ステップｂ１からステップｂ６、ステップｂ８を経由してステップｂ９に達する動作を行う。これによって中央演算処理部１９は、対象点Ｐｈの位置を保ちつつ、図１３（１）に示すように、第１関節部９の角度ｑ１を時間経過とともに小さくさせるとともに、図１３（２）〜（４）に示すように、第２関節部１０〜第４関節部１２の角度ｑ２〜ｑ４をそれぞれ変化させる。
When the procedure of the morphological operation shown in FIG. 12 is performed, when the minus direction movement switch is sequentially pressed, the
マイナス方向移動スイッチが押下され続けると、第１関節部９の角度ｑ１が可動限界となる角度（図１３では０度）に達する。この場合、中央演算処理部１９は、ステップｂ５〜ｂ８を経由せずに、ステップｂ９に達する動作を行う。これによって中央演算処理部１９は、関節部の角変位指令を演算せず、各関節部９〜１２の角変位を停止した状態で維持させる。可動限界に達した第１時期ｔ１を経過後、さらにマイナス方向移動スイッチが押下されても、各関節部９〜１２の角度は、可動限界に達した位置を維持する。
If the minus direction movement switch is continuously pressed, the angle q1 of the first
また第１時期ｔ１より後となる第２時期ｔ２で、第１関節部９に対応するプラス方向移動スイッチが押下されると、中央演算処理部１９は、第１関節部９について、プラス方向移動スイッチ押下前の角度から予め定める角変位量で、プラス方向に角変位した位置を冗長角度として決定する。これによって第１関節部９の角度ｑ１が可動限界から脱出する。第２時期ｔ２では、中央演算処理部１９は、ステップｂ１からステップｂ７、ステップｂ８を経由してステップｂ９に達する動作を行う。この場合、中央演算処理部１９は、第１時期ｔ１以前に、第４関節部１２の角度ｑ４を求めるために用いた離散形態符号を反転して、第４関節部１２の角度ｑ４を求める。
When the plus direction movement switch corresponding to the first
第２時期ｔ２より後で、プラス方向移動スイッチが押下されつづけると、中央演算処理部１９は、ステップｂ１からステップｂ６、ステップｂ８を経由してステップｂ９に達する動作を行う。この場合、中央演算処理部１９は、第２時期ｔ２で用いた離散形態符号を用いて、第４関節部１２の角度ｑ４を求める。これによって中央演算処理部１９は、対象点Ｐｈの位置を保ちつつ、図１３（１）に示すように、第１関節部９の角度ｑ１を時間経過とともに大きくさせるとともに、図１３（２）〜（４）に示すように、第２関節部１０〜第４関節部１２の角度ｑ２〜ｑ４をそれぞれ変化させる。
If the plus direction movement switch is continuously pressed after the second time t2, the
このように第１時期ｔ１より前と、第２時期より後とでは、各関節部９〜１２の角度ｑ１〜ｑ４を求めるのに用いた離散形態符号が異なる。したがって１つの冗長関節部を選択するだけで、対象点Ｐｈを固定状態に維持したうえで、第１〜第４関節部９〜１２が取り得ることが可能な全ての角度の組合せを演算可能となる。言換えると、多関節ロボット３０が回転揺動状態であって、揺動角変位する関節部が選択された場合であっても、第１〜第４関節部９〜１２が取り得ることが可能な全ての角度の組合せを演算可能となる。したがって操作者は、４つの関節部９〜１２のうちの１つを選択するだけでよく、全てを冗長関節部として選択可能とする必要がなく、形態変更動作における入力部２０の操作性を向上することができる。
As described above, the discrete form codes used for obtaining the angles q1 to q4 of the
また上述する形態変更動作を中央演算処理部１９が行う場合には、関節部操作スイッチ４３を、１つのスイッチとすることができる。この場合、形態変更動作に必要なスイッチを減らすことができ、スイッチとして必要な部品点数を減らすことができる。また各関節部の角度を演算するのに必要な計算プログラムを１つにすることができる。また第１関節部を冗長関節部として選択可能とすることによって、計算プログラムをより簡単化することができる。
When the
図１４は、形態変更動作における中央演算処理部１９の予測報知動作の手順を説明するためのフローチャートである。本実施の形態では、形態変更動作にあたって、中央演算処理部１９は、冗長関節部を決定した場合に、残余の関節部が角変位する方向を報知するとともに、各アーム体の移動量および移動方向を報知する予測報知動作を行うことができる。予測報知動作は、図３および図１２に示す形態変更動作のいずれにも併用して行うことができる。したがって中央演算処理部１９は、複数の関節部の角変位する方向を予測して報知する予測報知手段となる。
FIG. 14 is a flowchart for explaining the procedure of the prediction notification operation of the
まずステップｃ０で、中央演算処理部１９は、対象点Ｐｈの位置が決定された状態で、予測報知動作の動作指令が与えられたことを判断すると、ステップｃ１に進み、予測報知動作を開始する。ステップｃ１では、中央演算処理部１９は、移動すべき冗長角度の移動量を決定し、ステップｃ２に進む。以下、移動量は、移動方向も含むものとする。たとえば、たとえば関節部操作スイッチ４３のうち、プラス方向移動スイッチまたはマイナス方向移動スイッチが直前に押下されていると、同じ移動スイッチが次に押下された場合に冗長関節部が角変位する移動量と移動方向とを決定する。
First, in step c0, when the
ステップｃ２では、中央演算処理部１９は、対象点Ｐｈの移動量と、各関節部９〜１２の角度ｑ１〜ｑ４との関係を示す第１ヤコビ行列Ｊｈに基づいて、対象点Ｐｈが固定位置となる場合の各関節部９〜１２の角度ｑ１〜ｑ４の移動方向を決定する。具体的には、中央演算処理部１９は、第１ヤコビ行列Ｊｈを変形した行列式に、ステップｃ１で求めた移動量とを代入することによって、残余の角度の移動量を求めることができる。このようにして冗長関節部の移動量を決定した場合において、残余の角度の移動量を演算すると、ステップｃ３に進む。
In step c2, the
ステップｃ３では、中央演算処理部１９は、設定されるアーム体の移動量と、各関節部９〜１２の角度ｑ１〜ｑ４との関係を示す第２ヤコビ行列Ｊｅに基づいて、対象点Ｐｈが固定位置となる場合の各関節部９〜１２の角度ｑ１〜ｑ４の移動量とを決定する。具体的には、中央演算処理部１９は、第２ヤコビ行列Ｊｅに基づく行列式に、ステップｃ１およびｃ２で求めた各関節部９〜１２の角度ｑ１〜ｑ４を代入することによって、設定されるアーム体の移動量を求めることができる。このようにして冗長関節部の移動量を決定した場合において、設定されるアーム体の移動量を演算すると、ステップｃ４に進む。
In step c3, the
ステップｃ４では、中央演算処理部１９は、ステップｃ２およびステップｃ３で演算し
た演算結果を入力部２０の表示部４２によって報知し、ステップｃ１に戻る。中央演算処理部１９は、予め定める終了条件を満足するまで、ステップｃ１〜ｃ４を繰返し、終了条件を満足すると、予測報知動作を終了する。たとえば中央演算処理部１９は、形態変更動作を終了するとともに予測報知動作を終了する。
In step c4, the
このように予測報知動作を行うことによって、形態変更動作において、冗長関節部をプラスまたはマイナス方向に角変位した場合に、残余の関節部がプラスまたはマイナス方向のいずれに角変位するかを報知することができ、操作性を向上することができる。 By performing the prediction notification operation in this way, in the form change operation, when the redundant joint is angularly displaced in the plus or minus direction, the remaining joint is informed in the plus or minus direction. And operability can be improved.
同様に設定されるアーム体が、プラスまたはマイナス方向のいずれに角変位するかを報知することができ、操作性を向上することができる。たとえば設定されるアーム体は、第１〜第４アーム体３〜６の全てであってもよく、また操作者が注目したい任意のアーム体であってもよい。本実施の形態では、第３アーム体５と第４アーム体６との間の部分となる肘位置ｐ２の移動量を報知することによって、中央演算処理部１９は、多関節ロボット３０のおおよその形態を把握させることができる。
Similarly, it can be notified whether the arm body set in the same way is angularly displaced in the plus or minus direction, and the operability can be improved. For example, the arm bodies to be set may be all of the first to
操作者は、報知された情報を確認することで、各関節部および設定されるアーム体が動くであろう方向を容易に把握することができ、利便性を向上することができる。たとえば、形態変更動作を行って、障害物に衝突することを回避する形態に多関節ロボットを変形させる場合、ロボットの形態変化を予測することができるので、操作者は、ロボットが障害物に衝突することをより確実に防ぐことができる。 By checking the notified information, the operator can easily grasp the direction in which each joint part and the arm body to be set will move, and the convenience can be improved. For example, when deforming an articulated robot to a form that avoids colliding with an obstacle by performing a form changing operation, the robot can predict a change in the form of the robot, so that the operator can collide with the obstacle. Can be prevented more reliably.
また上述した予測報知動作における演算は、ヤコビ行列Ｊｈ，Ｊｅに基づいて行われる。したがって演算結果で有る各関節部９〜１２の移動量および設定されるアーム体の移動量は、解析解でなく近似解となる。しかしながら報知情報は、厳密な情報が要求されず、短時間および容易に演算されることが望まれる。本実施の形態に示すように、ヤコビ行列に基づいて、予測報知に用いる演算結果を求めることで、短時間でかつ容易に演算することができる。
Further, the calculation in the above-described prediction notification operation is performed based on the Jacobian matrices Jh and Je. Therefore, the amount of movement of each
次に、ステップｃ２における各関節部９〜１２の移動量演算工程の詳細について説明する。上述した（１）式に基づいて、対象点Ｐｈの移動量を求める第１ヤコビ行列Ｊｈは、次の（５）式によって表わされる。 Next, the detail of the movement amount calculation process of each joint part 9-12 in step c2 is demonstrated. The first Jacobian matrix Jh for obtaining the movement amount of the target point Ph based on the above-described equation (1) is represented by the following equation (5).
ここで、ｐｈｘ、ｐｈｙおよびｐｈｚは、（１）式で定義した関数であり、Ｊｈは、第１ヤコビ行列を示す。 Here, phx, phy, and phz are functions defined by equation (1), and Jh indicates the first Jacobian matrix.
また第４アーム体５と第３アーム体６との間の肘位置ｐ２を求める第２ヤコビ行列Ｊｅは、（５）式に示す第１ヤコビ行列Ｊｈのｐｈｘ、ｐｈｙおよびｐｈｚの関数に、Ｌ４＝ゼロを代入した行列によって表わされる。
Further, the second Jacobian matrix Je for obtaining the elbow position p2 between the
対象点Ｐｈの微小移動量Δｐｈと、各関節部の微小移動量Δｑとの間には、次の（６）式の関係を有する。 Between the minute movement amount Δph of the target point Ph and the minute movement amount Δq of each joint part, there is a relationship of the following equation (6).
形態変更動作では、対象点Ｐｈの位置が固定されるので、（６）式の左辺は、ゼロである。これによって（６）式は、次の（７）式となる。 In the form changing operation, the position of the target point Ph is fixed, so the left side of the equation (6) is zero. As a result, the expression (6) becomes the following expression (7).
ここで、Δｑ１は、第１関節部９の微小移動量Δｑ１である。この微小移動量Δｑ１は、ステップｃ１で決定される。たとえば関節部を移動する速度が予め決定されている場合、その速度に制御周期をかけた値となる。第１関節部９をプラス方向に移動させる場合にはΔｑ１が正となり、マイナス方向に移動させる場合には、Δｑ１が負となる。
Here, Δq1 is the minute movement amount Δq1 of the first
（７）式に、第１関節部９の微小移動量Δｑ１を代入することによって、第２〜第４関節部１０〜１２の微小移動量Δｑ２〜ｑ４を求めることができる。中央演算処理部１９は、（７）式を記憶する。ステップｃ２において、（７）式に冗長関節部の移動量Δｑ１を代入することによって、冗長関節部である第１関節部以外の各関節部１０〜１２の微小移動量Δｑ２〜Δｑ４を求めることができる。求めた各関節部の移動方向の正負を表示部４２に表示させることによって、操作者は、冗長関節部の移動方向を選択した場合に残余の関節部が移動する移動方向を把握することができる。
By substituting the minute movement amount Δq1 of the first
次に、ステップｃ３における各関節部９〜１２の移動量演算工程の詳細について説明する。肘位置ｐ２の微小移動量Δｐｅと、各関節部の微小移動量Δｑとには、次の（８）式に示す関係がある。 Next, details of the movement amount calculation step of each joint 9 to 12 in step c3 will be described. The minute movement amount Δpe at the elbow position p2 and the minute movement amount Δq of each joint have the relationship shown in the following equation (8).
ここで、ΔＰｅは肘位置ｐ２の微小移動量ΔＰｅであり、Ｊｅは、上述した第２ヤコビ行列Ｊｅである。（８）式に、（７）式で求めた各関節部１０〜１２の微小移動量Δｑ２〜ｑ４と、冗長関節部である第１関節部９の微小移動量Δｑ１を代入することによって、肘位置ｐ２の微小移動量ΔＰｅを求めることができる。
Here, ΔPe is the minute movement amount ΔPe at the elbow position p2, and Je is the above-described second Jacobian matrix Je. By substituting into the equation (8) the minute movement amounts Δq2 to q4 of the
中央演算処理部１９は、（８）式を記憶する。ステップｃ３において、（８）式に冗長関節部の移動量Δｑ１、ステップｃ１およびステップｃ２で求めた各関節部９〜１２の微小移動量Δｐ１〜Δｐ４を代入することによって、肘位置ｐ２の微小移動量ΔＰｅを求めることができる。求めた肘位置ｐ２の移動方向の正負を表示部４２に表示させることによって、操作者は、冗長関節部の移動方向を選択した場合に肘位置ｐ２が移動する移動方向たとえば基台１７を基準として、上下左右に移動する方向を把握することができる。
The
図１５は、本発明の第２実施形態である多関節ロボット１の制御装置２を示すブロック図である。図１５に示す多関節ロボット１および制御装置２は、図１に示す多関節ロボッ
ト３０および制御装置２と類似した構成を示し、類似した構成については同様の参照符号を付して、説明を省略する。
FIG. 15 is a block diagram showing the
図１５に示す多関節ロボット１は、７自由度を有する冗長ロボットであり、６つのアーム体３〜８と、隣接する２つのアーム体を回転自在に連結する７つの関節部９〜１５とを有する。各アーム体３〜８は、それぞれ連結されて直列状に延びるアーム構成体１６を構成する。また多関節ロボット１は、アーム構成体１６の基端部に連結される基台１７と、アーム構成体１６の先端部に連結されるエンドエフェクタ１８とを有する。
The
各関節部９〜１５は、同軸関節部と傾斜関節部とのうちのいずれかである。具体的には、第１関節部９、第５関節部１３および第７関節部１５は、同軸関節部であり、第２関節部１０、第３関節部１１、第４関節部１２および第６関節部１４は、傾斜関節部である。したがって第１〜第４アーム体３〜６および、第１〜第４関節部９〜１２は、図１に示す多関節ロボット３０と同様の構成を有する。
Each joint part 9-15 is either a coaxial joint part or an inclined joint part. Specifically, the first
第４アーム体６の一端部は、第４関節部１２によって、第３アーム体５の軸線Ａ５および第４アーム体６の軸線Ａ６に対して所定の角度、本実施の形態では４５度で傾斜する第４回転軸線Ａ１２まわりに回転自在に、第３アーム体５の他端部に連結される。第５アーム体７の一端部は、第５関節部１３によって、第４アーム体６の軸線Ａ６および第５アーム体７の軸線Ａ７と同軸の第５回転軸線Ａ１３まわりに回転自在に、第４アーム体６の他端部に連結される。
One end portion of the
アーム構成体１６の他端部である第６アーム体８の一端部は、第６関節部１４によって、第５アーム体７の軸線Ａ７および第６アーム体８の軸線Ａ８に対して所定の角度、本実施の形態では４５度で傾斜する第６回転軸線Ａ１４まわりに回転自在に、第５アーム体７の他端部に連結される。多関節ロボット１の先端部となるエンドエフェクタ１８は、第６アーム体８の他端部に、第７関節部１５によって、第６アーム体８の軸線Ａ８と同軸の第７回転軸線Ａ１５まわりに回転自在に連結されている。エンドエフェクタ１８は、多関節ロボット１のうちで、位置および姿勢を制御すべき設定部分となる。
One end portion of the
このように各関節部９〜１５は、アーム体を介して互いに角変位可能に連結される。また各関節部９〜１５が角変位することで、アーム構成体１６は、図１５に示すように、アーム体のそれぞれの軸線が同軸に配置されて一直線状に延びる状態に変形可能に構成される。各アーム体３〜６の軸線Ａ３〜Ａ６が一直線状に延びた状態では、第２回転軸線Ａ１０と第３回転軸線Ａ１１とは平行となり、第３回転軸線Ａ１１と第４回転軸線Ａ１２とは垂直となる。また第４アーム体６に設定される対象点Ｐｈは、アーム構成体１６の先端部に配置され、基台１７は、アーム構成体１６の他端部に配置される。
Thus, each joint part 9-15 is connected via an arm body so that angular displacement is mutually possible. Further, as the
また直列方向一端部から直列方向他端部に向かって並ぶ３つの関節部のそれぞれの回転軸線、すなわち第５関節部１３の回転軸線Ａ６と、第６関節部１４の回転軸線Ａ７と、第７関節部１５の回転軸線Ａ８とは、予め定められる一点となる３軸交点Ｐで交差する。このように第５〜第７関節部１３〜１５は、３軸交点Ｐを構成する３つの３軸交点形成関節部となる。
The rotation axes of the three joints arranged from one end in the series direction toward the other end in the series direction, that is, the rotation axis A6 of the fifth joint 13, the rotation axis A7 of the sixth joint 14, and the seventh It intersects with the rotation axis A8 of the joint portion 15 at a three-axis intersection P that is a predetermined point. As described above, the fifth to seventh
多関節ロボット１は７自由度を有し、各関節部９〜１５を回転させることで、エンドエフェクタ１８を任意の位置および姿勢に位置決め可能である。多関節ロボット１は、設備等が複雑に入り組んで作業経路が複雑な場合や、他のロボットと近接して並列に配置されて動作範囲を大きくすることができない場合に好適に用いられる。エンドエフェクタ１８は、たとえば溶接トーチおよびハンドリング装置などの手先装置であってもよい。各アーム体３〜８を回転させる回転機構などは、図１に示す多関節ロボット３０と同様の構成を
有する。また制御装置２についても、その構成は、図１に示す制御装置２と同様の構成を有する。
The articulated
中央演算処理部１９は、予め記憶される動作プログラムに従って、入力部２０から与えられる各種情報に応答して、多関節ロボット１を制御するために必要な演算を行う。本実施の形態では、中央演算処理部１９は、エンドエフェクタ１８を予め定める目標位置および目標姿勢に移動させるのに必要な、各関節部９〜１５の角度ｑ１〜ｑ７をそれぞれ演算する。
The
制御装置２は、エンドエフェクタ１８を予め指定される移動経路に沿って移動させるにあたって、回転すべき各関節部９〜１５の角度ｑ１〜ｑ７を演算する。この移動経路は、ロボット動作前に、操作者によって入力部２０から中央演算処理部１９に入力される。本発明の制御装置２に制御される多関節ロボット１は、７つの関節部９〜１５を有することで、冗長性を有する。すなわち、基台１７に対して、エンドエフェクタ１８を予め定める位置および姿勢で固定した状態で、各関節部９〜１５の取り得る角度ｑ１〜ｑ７の組合せが複数通り存在する。
The
本実施形態の制御装置２は、上述した形態変更動作を行うことができる。形態変更動作を行うにあたって、制御装置２は、複数の関節部９〜１５のうちから選択された冗長関節部と、その冗長関節部の冗長角度と、エンドエフェクタ１８を固定すべき位置および姿勢とが入力部２０を介して与えられる。中央演算処理部１９は、エンドエフェクタ１８を固定すべき固定状態に固定したうえで、冗長関節部が冗長角度となる場合の、残余の関節部の角度を演算する演算手段となる。出力部２２は、中央演算処理部１９の演算結果を多関節ロボットに与える。演算結果が与えられた多関節ロボットは、冗長関節部を冗長角度とするとともに、残余の関節部の角度を演算結果に応じて位置合わせして、エンドエフェクタ１８を指定された固定状態とする。
The
このように形態変更動作を行うことによって、エンドエフェクタ１８を固定した状態で、アーム構成体１６の形状および姿勢などの形態を変更させることができる。操作者は、ロボットを動作させながらロボットの形態を確認することによって、エンドエフェクタ１８を固定した状態で、障害物との衝突を回避するアーム構成体１６の形態を実現する各関節部９〜１５の角度を容易に決定することができる。
By performing the form changing operation in this manner, the form such as the shape and posture of the
図１６は、図１２に示す多関節ロボットを制御する制御装置２の入力部２０を示す正面図である。入力部２０の構成は、図２と同様の構成を有する。具体的には、入力部２０は、操作部４１と、表示部４２とを含む。操作部４１には、各関節部操作スイッチ４３と、ツール移動スイッチ４４と、形態変更スイッチ４５とを含む。また操作部４１は、ロボットの動作プログラムを入力するための命令キー、カーソルキーおよびテンキーなどをさらに含む。
FIG. 16 is a front view showing the
関節部操作スイッチ４３は、各関節部９〜１５の角度ｑ１〜ｑ７を入力するためのスイッチであって、各関節部９〜１５について、プラス方向およびマイナス方向への予め定める角変位量の角変位指令が与えられる。ツール移動スイッチ４４は、ベース座標軸またはツール座標軸に従ってエンドエフェクタ１８の移動指令が与えられる。また形態変更スイッチ４５は、対象点Ｐｈを予め定める位置に固定したまま、アーム構成体１６の形態を変化させるための形態変更指令が与えられる。
The joint
形態変更動作を行う場合、ツール移動スイッチ４４は、エンドエフェクタ１８を固定すべき位置および姿勢が入力される固定条件入力手段となる。また形態変更スイッチ４５と関節部操作スイッチ４３とによって、冗長関節部が入力される冗長関節部入力手段と、冗
長関節部の角度である冗長角度が入力される冗長角度入力手段とが構成される。
When performing the form changing operation, the
図１７は、第２実施形態における中央演算処理部１９の形態変更動作の手順を説明するためのフローチャートである。まずステップｄ０で、中央演算処理部１９は、多関節ロボット１を制御可能な状態で、形態変更スイッチ４５が押下されたことを判断すると、ステップｄ１に進み、形態変更動作を開始する。
FIG. 17 is a flowchart for explaining the procedure of the form changing operation of the
ステップｄ１では、中央演算処理部１９は、ツール移動スイッチ４４によってエンドエフェクタ１８の移動指令が与えられると、エンドエフェクタ１８を指定された位置および姿勢に移動するための演算結果を出力部２２に与える。またツール移動スイッチ４４による移動指令がなければ、現時点のエンドエフェクタ１８の位置および姿勢を維持させる。中央演算処理部１９は、関節部操作スイッチ４３が押下されたと判断すると、その時点のエンドエフェクタ１８の位置および姿勢を固定すべき固定条件として決定し、ステップｄ２に進む。
In step d1, when the movement command of the
ステップｄ２では、中央演算処理部１９は、関節部操作スイッチ４３によって指定された関節部を、冗長関節部として決定する。冗長関節部は、第１〜第４関節部９〜１２のいずれか１つとなる。また関節部操作スイッチ４３によって指定された角度を、冗長角度として決定する。このように冗長関節部および冗長角度を決定するとステップｄ３に進む。
In step d2, the
ステップｄ３では、中央演算処理部１９は、ステップｄ１で決定したエンドエフェクタ１８の位置および姿勢に基づいて、３軸交点Ｐの座標を演算する。３軸交点Ｐは、第５関節部１３、第６関節部１４、第７関節部１５の各回転軸線Ａ１３，Ａ１４，Ａ１５が一点で交わる点であるので、エンドエフェクタ１８の位置および姿勢が決定されると、一意的に求めることができる。このように中央演算処理部１９は、３軸交点Ｐを演算する３軸交点演算手段となり、３軸交点Ｐを求めるとステップｄ４に進む。
In step d3, the
ステップｄ４では、中央演算処理部１９は、ステップｄ３で演算した３軸交点Ｐの３次元位置に、３軸交点Ｐが位置する場合における、第１〜第４関節部９〜１２の角度ｑ１〜ｑ４を求める。具体的には、第１〜第４関節部９〜１２のそれぞれの角度ｑ１〜ｑ４を変数として、３軸交点Ｐの座標を表わす順変換関係式と、ステップｄ２で決定した冗長関節部の冗長角度と、ステップｄ３で演算した３軸交点Ｐの座標とに基づくことによって、第１〜第４関節部９〜１２の角度ｑ１〜ｑ４を解析的に求めることができる。第１〜第４関節部９〜１２の角度ｑ１〜ｑ４を解析的に求める場合、対象点Ｐｈの座標を３軸交点Ｐの座標に置き変えることで、上述するステップａ３と同様の方法を用いて求めることができる。
In step d4, the
したがって中央演算処理部１９は、冗長関節部の冗長角度と、３軸交点Ｐのｘ座標、ｙ座標、ｚ座標を代入することで、第１〜第４関節部９〜１２のうちで冗長関節部以外の各関節部の角度を解析的に求めることが可能な方程式を表わす計算プログラムを記憶している。中央演算処理部１９は、第１〜第４関節部９〜１２の計算プログラムにしたがって、第１〜第４関節部９〜１２のうちで、冗長関節部以外の各関節部の角度を解析的に演算し、ステップｄ５に進む。このように中央演算処理部１９は、注目する第１〜第４関節部９〜１２の角度ｑ１〜ｑ４をそれぞれ演算する注目関節部角度演算手段となる。
Therefore, the
ステップｄ５では、中央演算処理部１９は、ステップｄ２で決定された冗長関節部の冗長角度が可動限界に到達していないことを判断するとステップｄ６に進み、可動限界に到達しているか、または可動限界を超えているかを判断すると、ステップｄ８に進む。
In step d5, when the
ステップｄ６では、中央演算処理部１９は、エンドエフェクタ１８が固定位置および固
定姿勢となる、第５〜第７関節部１３〜１５の角度ｑ５〜ｑ７を求める。ステップｄ４で求めた第１〜第４関節部９〜１２の角度ｑ１〜ｑ４から、第４アーム体６の姿勢を求めることができる。第４アーム体６の姿勢が決定された状態では、エンドエフェクタ１８が予め定める位置および姿勢となる場合の第５〜第７関節部１３〜１５の角度ｑ５〜ｑ７は、一意に決定することができる。
In step d6, the
具体的には、中央演算処理部１９は、第１〜第４関節部９〜１２の角度ｑ１〜ｑ４の角度と、エンドエフェクタ１８の位置および姿勢と用いて、第５〜第７関節部１３〜１５の角度を解析的に求めることが可能な逆変換方程式を表わす第５〜第７関節部計算プログラムを記憶している。
Specifically, the
中央演算処理部１９は、第５〜第７関節部計算プログラムにしたがって、第５〜第７関節部１３〜１５の角度ｑ５〜ｑ７をそれぞれ求める。このように中央演算処理部１９は、第５〜第７関節部１３〜１５の角度ｑ５〜ｑ７をそれぞれ演算する交点形成関節部角度演算手段となり、各角度を求めるとステップｄ７に進む。
The
ステップｄ７では、ステップｄ４およびステップｄ６で得られた第１〜第７関節部９〜１５の角度ｑ１〜ｑ７を演算結果として、出力部２２に与える。出力部２２は、与えられた演算結果を多関節ロボット１に与える。これによって多関節ロボット１は、各関節部９〜１５が角変位し、エンドエフェクタ１８を固定位置および固定姿勢に維持した状態で、冗長関節部が冗長角度となる。このとき、第１〜第７関節部９〜１５の角度ｑ１〜ｑ７は、収束演算などの近似法とは異なり、解析的な演算方法によって求められるので、冗長関節部の移動前と移動後とで、固定位置および固定姿勢からエンドエフェクタ１８がずれることがない。このように演算結果を出力部２２に与えて、多関節ロボット１を動作させると、ステップｄ８に進む。
In step d7, the angles q1 to q7 of the first to seventh
ステップｄ８では、中央演算処理部１９は、形態変更動作を終了する終了条件を満足するか否かを判断する。たとえば形態変更解除スイッチが押下されることで、終了条件を満足することを判断してもよい。中央演算処理部１９は、終了条件を満足していないと判断すると、ステップｄ１に戻り、関節部操作スイッチ４３が押下されるのを待機する。この状態で、関節部操作スイッチ４３が再度押下されると、その指示に従ってステップｄ１〜ｄ８を繰返す。そしてステップａ７で形態変更動作を終了する指令が与えられたことを判断すると、ステップａ８に進み形態変更動作を終了する。
In step d8, the
このようにして、中央演算処理部１９が形態変更動作を行うことによって、エンドエフェクタ１８の位置および姿勢を固定したまま、各関節部９〜１５を角変位させて、多関節ロボット１の形態を変更させることができる。したがって第１実施形態と同様の効果を得ることができる。
In this way, the
また図１５に示す多関節ロボット１は、第５〜第７関節部１３〜１５のうちの１つが冗長関節部として選択されてしまうと、解析的に求めることができない。これに対して、本実施の形態では、第１〜第４関節部９〜１２のいずれかを冗長関節部として選択することによって、解析解を得ることができる。また７つの関節部９〜１５から１つを選択するよりも、４つの関節部９〜１２から１つを選択するほうが、操作性を向上することができる。
Further, the
また図１６に示すように、各関節部操作スイッチ４３を用いて、冗長関節部を決定する場合、形態変更スイッチ４５が押下された状態では、第１関節部〜第４関節部９〜１２の選択スイッチを有効とし、第５〜第７関節部１３〜１５の選択スイッチが無効とする。これによって冗長関節部としては、第１〜第４関節部９〜１２のいずれかについて選択可能
となり、誤動作を防ぐことができる。
As shown in FIG. 16, when the redundant joint portion is determined using each joint
また本実施の形態では、第５〜第７関節部１３〜１５で三軸交点を構成したが、これに限らない。たとえば第１〜第３関節部９〜１１で３軸交点を構成した場合であっても、同様に解析解を求めることができる。この場合には、第４〜第７関節部１２〜１５のうちから１つの冗長関節部が選択される。そしてエンドエフェクタ１８に設定される座標系から、第１関節部を見た座標系に反転した座標系に変換することによって、同様に解析的に求めることができる。
Moreover, in this Embodiment, although the triaxial intersection was comprised by the 5th-7th joint parts 13-15, it is not restricted to this. For example, even if the first to third
すなわち、アーム体を介して互いに角変位可能に連結されて直列に並ぶ７つの関節部を含み、７つの関節部９〜１５のうちで直列方向一端部から直列方向他端部に向かって並ぶ３つの関節部の回転軸線が一点で交差する３軸交点を形成する３つの交点形成関節部を有する７自由度の多関節ロボットにおいて、アーム構成体の一端部に対する他端部の位置および姿勢が固定条件として入力される場合に、３つの交点形成関節部以外となる４つの注目する関節部のうち１つが、冗長関節部として入力されることで、形態変更動作において各関節部９〜１５の角度を解析的に求めることができる。
That is, it includes seven joint portions that are connected to each other via an arm body so as to be angularly displaceable and are arranged in series, and among the seven
また上述したように、第２実施形態であっても第１および第２関節部９，１０のいずれか１つと、第３および第４関節部１１，１２のいずれか１つを冗長関節部として選択させるようにしてもよい。また図１２に示す形態変更動作を行わせてもよく、移動予測動作を行わせてもよい。移動予測動作と、形態変更動作とは、エンドエフェクタ１８を移動経路に沿って移動させて移動経路を教示する移動教示動作と併用して行ってもよい。
As described above, even in the second embodiment, any one of the first and second
図１８は、移動予測動作と、形態変更動作と、移動教示動作とを行う場合における中央演算処理部１９の動作手順を説明するためのフローチャートである。まずステップｅ０で、中央演算処理部１９は、多関節ロボット１を制御可能な状態で、入力部２０のツール移動スイッチ４４によって、エンドエフェクタ１８の移動指令が与えられると、ステップｅ１に進む。
FIG. 18 is a flowchart for explaining the operation procedure of the
ステップｅ１では、中央演算処理部１９は、ツール移動スイッチ４４に与えられた情報に従って、エンドエフェクタ１８を移動させる移動経路を演算する。エンドエフェクタ１８が指定された移動経路に従って移動するにあたって、各関節部の角度の組合せは、複数存在する場合がある。この場合、中央演算処理部１９は、予め定める規則にしたがって複数の角度の組合せから１つを選択する。たとえばエンドエフェクタ１８側の関節部の角変位が少なくなるような角度の組合せを選択する。中央演算処理部１９は、演算結果を出力部２２を介して多関節ロボット１に与え、ステップｅ２に進む。これによって多関節ロボット１は、エンドエフェクタ１８を指定された移動経路に沿って移動させる。
In step e1, the
ステップｅ２では、第１〜第４関節部９〜１２のいずれかが特異点近傍の警告範囲に到達するか否かを判断する。たとえば中央演算処理部１９は、第１〜第４関節部９〜１２の角度ｑ１〜ｑ４を、３軸交点位置に変換するヤコビ行列の行列式の絶対値が、予め定める設定値以下であると判定されると、第１〜第４関節部９〜１２の角度ｑ１〜ｑ４のいずれかが、特異点近傍の警告領域に到達したことを判断する。
In step e2, it is determined whether any of the first to fourth
ステップｅ２において、第１〜第４関節部９〜１２のいずれかが警告範囲に到達したことを判断するとステップｅ３に進み、そうでないとステップｅ６に進む。ステップｅ３では、図１４に示す予測報知動作を行う。図１９は、入力部２０の表示部４２に表示される表示画面を示す図である。たとえば図１９に示すように、中央演算処理部１９は、可動限界に近づいている冗長関節部を表示させる。また中央演算処理部１９は、可動限界に近づいている関節部が、特異点から脱出する角変位方向と、脱出する方向に移動したときの肘
位置ｐ２が動く方向を表示させる。このように予測情報を表示させると、ステップｅ４に進む。
If it is determined in step e2 that any of the first to fourth
ステップｅ４では、中央演算処理部１９は、形態変更指令が与えられたか否かを判断する。形態変更が与えられたと判断するとステップｅ５に進み、そうでないとステップｅ６に進む。ステップｅ５では、エンドエフェクタ１８の移動を停止して、図３または図１２に示す形態変更動作を開始し、形態変形動作が終了したことを判断するとステップｅ６に進む。
In step e4, the
ステップｅ６では、中央演算処理部１９は、移動動作を終了する終了条件を満足するか否かを判断する。中央演算処理部１９は、移動動作の終了条件を満足していないと判断すると、ステップｅ１に戻り、ツール移動スイッチ４４が再び押下されるのを待機する。この状態で、ツール移動スイッチ４４が再度押下されると、その指示に従ってステップｅ１１〜ｅ６を順に行う。またステップｅ６で終了条件を満足することを判断すると、ステップｅ７に進み、ステップｅ７で移動動作を終了する。
In step e6, the
これによって操作者は、移動動作を行う途中に、エンドエフェクタ１８の位置を固定したまま、ロボットの形態を変更して、冗長関節部を可動限界から離して、再び、移動指令を与えた方向にエンドエフェクタ１８を移動させることができる。このように移動動作に、中央演算処理部１９による予測動作と形態変更動作とを行わせることによって、利便性を向上することができる。
In this way, the operator changes the robot configuration while fixing the position of the
上述した本実施の形態は、発明の例示に過ぎず、発明の範囲内において構成を変更することができる。たとえば本実施の形態では、オンラインティーチング、すなわちロボットを動作させながら各関節部の角度を求めて、ロボットの動作計画の教示に用いたが、これに限らない。したがって中央演算処理部１９のうち、各関節部の角度を求めるプログラムを実行するコンピュータも本発明に属する。コンピュータに、ロボットの関節の種類、アーム体の長さなどの条件を入力した状態で、対象点Ｐｈおよびエンドエフェクタ１８を予め定める固定状態に保って、冗長関節部が冗長角度となる各関節部の角度を解析的に求めることができ、この解析結果をロボットに与えてもよい。すなわち本発明の方法をオフラインティーチングやロボットの動作シミュレーションに用いることも可能である。また本実施の形態では、（１）式に示す対象点の位置を求めるための関係式を変形して、連立方程式を解いた計算式に基づいて、各関節部の角度を求めたが、対象点の姿勢を求めるための関係式、対象点の位置の一部および姿勢の一部を求めるための関係式を変形して、連立方程式を解いた計算式に基づいて、各関節部の角度を求めてもよい。
The above-described embodiment is merely an example of the invention, and the configuration can be changed within the scope of the invention. For example, in the present embodiment, online teaching, that is, the angle of each joint portion is obtained while operating the robot, and is used for teaching the robot operation plan. However, the present invention is not limited to this. Therefore, the computer which executes the program which calculates | requires the angle of each joint part among the
また本実施の形態では、傾斜関節部と同軸関節部とを有する多関節ロボットを用いて説明したが、冗長性を有する他のロボットであっても同様に各関節部の角度を解析的に求めることができる。たとえば図６に示す４関節ロボットであっても同様に各関節部の角度を求めることができ、設定部分の固定についても、位置のほか姿勢が固定されてもよい。 In the present embodiment, the multi-joint robot having the tilt joint portion and the coaxial joint portion has been described. However, even in the case of another robot having redundancy, the angle of each joint portion is obtained analytically in the same manner. be able to. For example, even in the case of a four-joint robot shown in FIG. 6, the angles of the joint portions can be obtained in the same manner, and the posture of the setting portion may be fixed in addition to the position.
また本実施の形態では、１軸冗長ロボットについて説明したが、２軸以上の冗長性を有するロボットであっても同様に求めることができる。この場合、２つ以上の冗長関節部が選択されることで、解析的に各関節部の解を求めることができる。すなわち、注目する複数の関節部のうちで冗長関節部を除く残余の関節部の数が、設定部分を予め定める固定条件に従って拘束される自由度の数と一致するように、注目する複数の関節部の少なくとも１つが冗長関節部として入力されることで、連立方程式を解くことができ、解析的に各関節部の角度を求めることができる。 In the present embodiment, the description has been given of the single-axis redundant robot. However, even a robot having two or more axes of redundancy can be obtained similarly. In this case, the solution of each joint part can be analytically obtained by selecting two or more redundant joint parts. That is, the plurality of joints of interest so that the number of remaining joint portions excluding the redundant joint portion among the plurality of joints of interest matches the number of degrees of freedom in which the setting portion is constrained according to a predetermined fixed condition. By inputting at least one of the parts as a redundant joint part, simultaneous equations can be solved, and the angle of each joint part can be obtained analytically.
すなわち注目する複数（Ｎ）の関節部のうちで、入力された冗長関節部の数（ｎ）を除
いた数（Ｎ−ｎ）が、設定部分が拘束される自由度の数（Ｍ）と一致する必要がある。この場合、順変換関係式に含まれる変数を未知数とすると、設定部分が拘束される自由度の成分をそれぞれ解とする順変換関係式の数（Ｍ）と、順変換関係式に含まれる未知数の数（Ｎ−ｎ）とが等しくなる。これらの複数の順変換関係式を連立して解くことによって、各変数の値、すなわち残余の関節部の各角度を解析的に求めることができる。たとえば２軸冗長ロボットにおいて、オンラインティーチングを行う場合、２つの関節部を冗長関節部として決定し、そのうち第１冗長関節部を任意の角度に固定し、第２冗長関節部を変化させる。これによって、上述した演算方法と同様の演算方法を適用することができ、第２冗長角度を変化した場合に各関節部が取り得る角度の組合せの解析解を順次求めることができる。
That is, among a plurality of (N) joints of interest, the number (N−n) excluding the input number (n) of redundant joints is the number of degrees of freedom (M) in which the set part is constrained. Must match. In this case, if a variable included in the forward conversion relational expression is an unknown, the number of forward conversion relational expressions (M) each having a degree of freedom component to which the set part is constrained as a solution, and an unknown number included in the forward conversion relational expression Is equal to the number of (N−n). By solving these plurality of forward conversion relational expressions simultaneously, the value of each variable, that is, each angle of the remaining joint portion can be analytically determined. For example, when performing online teaching in a two-axis redundant robot, two joint portions are determined as redundant joint portions, of which the first redundant joint portion is fixed at an arbitrary angle, and the second redundant joint portion is changed. Accordingly, a calculation method similar to the calculation method described above can be applied, and analytical solutions of combinations of angles that can be taken by each joint portion when the second redundant angle is changed can be sequentially obtained.
本発明は、以下の実施の形態が可能である。
（１）アーム体を介して互いに角変位可能に連結されて直列に並ぶ複数の関節部を含み、複数の関節部のうちで、直列方向一端部の関節部に設定される設定部分を、予め定める位置および姿勢の少なくともいずれかを示す固定条件で固定した状態で、前記複数の関節部の取り得る角度の組合せが複数通り存在する多関節ロボットの制御装置であって、
前記設定部分を固定すべき固定条件が入力される固定条件入力手段と、
冗長関節部が入力される冗長関節部入力手段であって、前記複数の関節部のうちで冗長関節部を除く残余の関節部の数が、固定条件に従って設定部分が拘束される自由度の数と一致するように、複数の関節部の少なくとも１つが冗長関節部として入力される冗長関節部入力手段と、
冗長関節部の角度が冗長角度として入力される冗長角度入力手段と、
固定条件入力手段によって入力される固定条件と、冗長角度入力手段によって入力される冗長角度とに基づいて、複数の関節部の各角度を変数として設定部分の位置または姿勢を求めるための関係式を解いて、設定部分が固定条件で固定されるべき複数の関節部の角度をそれぞれ演算する角度演算手段と、
前記角度演算手段で演算された演算結果を多関節ロボットに与える出力手段とを備えることを特徴とする多関節ロボットの制御装置。
The following embodiments are possible for the present invention.
(1) It includes a plurality of joint portions that are connected to each other via an arm body so as to be angularly displaceable and arranged in series, and among the plurality of joint portions, a setting portion that is set to the joint portion at one end portion in the series direction is A controller for an articulated robot in which there are a plurality of combinations of angles that can be taken by the plurality of joints in a fixed state that indicates at least one of a predetermined position and posture,
A fixed condition input means for inputting a fixed condition for fixing the setting portion;
A redundant joint input means for inputting redundant joints, wherein the number of remaining joints excluding the redundant joints among the plurality of joints is the number of degrees of freedom in which the set part is constrained according to a fixed condition Redundant joint portion input means in which at least one of the plurality of joint portions is input as a redundant joint portion so as to match
Redundant angle input means for inputting the angle of the redundant joint as a redundant angle;
Based on the fixed condition input by the fixed condition input means and the redundant angle input by the redundant angle input means, a relational expression for obtaining the position or posture of the set portion using each angle of the plurality of joints as a variable is Unraveling, angle calculation means for calculating the angles of a plurality of joints, each of which should be fixed under fixed conditions,
A control apparatus for an articulated robot, comprising: output means for giving the result of calculation calculated by the angle calculating means to the articulated robot.
（２）固定条件として、直列方向他端部の関節部に対する設定部分の３つの自由度が拘束される条件が入力され、
冗長関節部入力手段は、前記複数の関節部のうちで、直列方向一端部から直列方向他端部に向かって並ぶ２つの関節部のうちの１つの関節部と、直列方向他端部から直列方向一端部に向かって並ぶ２つの関節部のうちの１つの関節部とを冗長関節部として選択可能に構成されることを特徴とする多関節ロボットの制御装置。
(2) As a fixed condition, a condition in which the three degrees of freedom of the set portion with respect to the joint portion at the other end in the series direction is constrained,
The redundant joint portion input means includes one joint portion of two joint portions arranged from one end portion in the series direction toward the other end portion in the series direction, and a series portion from the other end portion in the series direction. A control apparatus for an articulated robot, characterized in that one of the two joints arranged toward one end in the direction can be selected as a redundant joint.
（３）角度演算手段は、入力前の状態から冗長角度入力手段に入力された冗長角度に冗長関節部を角変位させる間に、冗長関節部が可動限界に到達することを判断した場合、その可動限界に到達した状態の関節部の角度を演算することを特徴とする多関節ロボット。 (3) When the angle calculation means determines that the redundant joint portion reaches the movable limit while angularly displacing the redundant joint portion from the state before the input to the redundant angle input to the redundant angle input means, An articulated robot characterized by calculating an angle of a joint part in a state where the movable limit is reached.
（４）角度演算手段は、冗長角度入力手段に入力された冗長角度に冗長関節部が角変位する間に、冗長関節部が可動限界に到達した状態から脱出することを判断した場合、冗長関節部以外の関節部の角度が取り得る２つの組合せのうち、可動限界に到達する前に取り得た角度の組合せと異なる角度の組合せを演算することを特徴とする多関節ロボットの制御装置。 (4) When the angle calculation means determines that the redundant joint portion escapes from the movable limit while the redundant joint portion is angularly displaced to the redundant angle input to the redundant angle input means, A control device for an articulated robot that calculates a combination of angles different from a combination of angles obtained before reaching a movable limit, out of two combinations that can be taken by angles of joints other than the joint.
（５）前記複数の関節部の角度変化を設定部分の位置変化に変換するヤコビ行列と、冗長角度入力手段に入力される冗長角度とに基づいて、設定部分を固定条件で拘束して、冗長関節部を冗長角度に角変位するときに、複数の関節部の角変位する方向を予測し、予測結果を報知する予測報知手段をさらに備えることを特徴とする多関節ロボットの制御装置。 (5) Based on the Jacobian matrix that converts the angle change of the plurality of joints into the position change of the setting part and the redundant angle input to the redundant angle input means, the setting part is constrained under a fixed condition, and redundant A control device for an articulated robot, further comprising: a prediction notification unit that predicts the direction of angular displacement of a plurality of joints when the joints are angularly displaced to a redundant angle, and notifies a prediction result.
（６）前記複数の関節部のいずれかが可動限界近傍に設定される警告範囲に到達したことを判断すると、判断結果を報知する警告報知手段をさらに備えることを特徴とする多関節ロボットの制御装置。 (6) Control of an articulated robot, further comprising warning notification means for notifying a determination result when it is determined that any one of the plurality of joints has reached a warning range set near the movable limit. apparatus.
（７）警告報知手段は、警告範囲に到達した関節部が、可動限界から遠ざかるような角変位方向を報知することを特徴とする多関節ロボットの制御装置。 (7) The control device for the articulated robot, wherein the warning notification means notifies the angular displacement direction in which the joint portion that has reached the warning range moves away from the movable limit.
（８）隣接する２つのアーム体を、各アーム体の軸線と同軸の回転軸線まわりに回転自在に連結する同軸関節部と、各アーム体の軸線に対して所定の角度を成して傾斜する回転軸線まわりに回転自在に連結する傾斜関節部とを含む多関節ロボットを制御することを特徴とする多関節ロボットの制御装置。 (8) A coaxial joint portion that connects two adjacent arm bodies so as to be rotatable about a rotation axis that is coaxial with the axis of each arm body, and is inclined at a predetermined angle with respect to the axis of each arm body. An articulated robot control device that controls an articulated robot including an inclined joint that is rotatably connected around a rotation axis.
（９）アーム体を介して互いに角変位可能に連結されて直列に並ぶ７つの関節部を含み、７つの関節部のうちで直列方向一端部から直列方向他端部に向かって並ぶ３つの関節部の回転軸線が一点で交差する３軸交点を形成する３つの交点形成関節部を有する７自由度の多関節ロボットの制御装置であって、
７つの関節部のうちで直列方向一端部の関節部に設定される設定部分を固定すべき位置および姿勢が固定条件として入力される固定条件入力手段と、
３つの交点形成関節部以外となる４つの基本関節部のうち１つが、冗長関節部として入力される冗長関節部入力手段と、
冗長関節部の角度が冗長角度として入力される冗長角度入力手段と、
設定部分が固定すべき位置および姿勢となる状態での３軸交点の固定位置を演算する３軸交点位置演算手段と、
３軸交点位置演算手段によって演算される３軸交点の位置と、冗長関節部入力手段によって入力される冗長角度とに基づいて、４つの基本関節部の各角度を変数として３軸交点の位置を求めるための関係式を解いて、３軸交点が固定位置に位置するような、４つの基本関節部の角度をそれぞれ演算する基本関節部角度演算手段と、
注目関節部角度演算手段によって演算された演算結果と、固定条件入力手段によって入力される設定部分の位置および姿勢とに基づいて、３つの交点形成関節部の角度をそれぞれ演算する交点形成関節部角度演算手段と、
前記各角度演算手段で演算された演算結果とを多関節ロボットに与える出力手段とを備えることを特徴とする多関節ロボットの制御装置。
(9) Three joints that are connected to each other via an arm body so as to be angularly displaceable and arranged in series, and among the seven joints, three joints arranged from one end in the series direction toward the other end in the series direction A control device for a multi-joint robot with 7 degrees of freedom having three intersection-forming joints that form three-axis intersections where the rotation axes of the parts intersect at one point,
A fixed condition input means for inputting a position and a posture to be fixed as a fixed condition among the seven joint parts to be set to the joint part at one end in the series direction;
Redundant joint input means in which one of the four basic joints other than the three intersection forming joints is input as a redundant joint;
Redundant angle input means for inputting the angle of the redundant joint as a redundant angle;
3-axis intersection position calculating means for calculating a fixed position of the 3-axis intersection in a state where the setting portion is to be fixed and in a posture;
Based on the position of the three-axis intersection calculated by the three-axis intersection position calculation means and the redundant angle input by the redundant joint input means, the position of the three-axis intersection is determined using each angle of the four basic joints as a variable. A basic joint angle calculating means for calculating the angles of the four basic joint parts so that the three-axis intersection is located at a fixed position by solving the relational expression for obtaining;
Intersection formation joint angle for calculating the angles of the three intersection formation joint sections based on the calculation result calculated by the target joint section angle calculation means and the position and orientation of the set portion input by the fixed condition input means Computing means;
An apparatus for controlling an articulated robot, comprising: output means for giving the result of computation calculated by each angle computing means to the articulated robot.
（１０）アーム体を介して互いに角変位可能に連結されて直列に並ぶ複数の関節部を含み、複数の関節部のうちで、直列方向一端部の関節部に設定される設定部分を、予め定める位置および姿勢の少なくともいずれかを示す固定条件で固定した状態で、前記注目する複数の関節部の取り得る角度の組合せが複数通り存在する多関節ロボットを制御する制御方法であって、
前記設定部分を固定すべき固定条件を決定する固定条件決定工程と、
冗長関節部を決定する冗長関節部決定工程であって、複数の関節部のうちで冗長関節部を除く残余の関節部の数が、固定条件に従って設定部分が拘束される自由度の数と一致するように、複数の関節部の少なくとも１つを冗長関節部として決定する冗長関節部決定工程と、
冗長関節部の角度を冗長角度として決定する冗長角度決定工程と、
固定条件決定工程によって決定した固定条件と、冗長角度決定工程によって決定した冗長角度とに基づいて、複数の関節部の各角度を変数として設定部分の位置または姿勢を求めるための関係式を解いて、設定部分が固定条件で固定されるべき、複数の関節部の角度をそれぞれ演算する角度演算工程と、
角度演算工程に従ってロボットを制御する制御工程とを備えることを特徴とする多関節ロボットの制御方法。
(10) A plurality of joint portions that are connected to each other via an arm body so as to be angularly displaceable and arranged in series, and among the plurality of joint portions, a setting portion that is set to the joint portion at one end portion in the series direction is A control method for controlling an articulated robot in which a plurality of combinations of angles that can be taken by the plurality of joints of interest are present in a fixed state that indicates at least one of a predetermined position and posture,
A fixing condition determining step for determining a fixing condition for fixing the setting portion;
A redundant joint part determining step for determining a redundant joint part, wherein the number of remaining joint parts excluding the redundant joint part among a plurality of joint parts matches the number of degrees of freedom in which the set part is constrained according to a fixed condition A redundant joint determination step for determining at least one of the plurality of joints as a redundant joint; and
A redundant angle determining step of determining the angle of the redundant joint as a redundant angle;
Based on the fixed condition determined by the fixed condition determining step and the redundant angle determined by the redundant angle determining step, solving the relational expression for obtaining the position or posture of the set part using each angle of the plurality of joints as a variable , An angle calculation step for calculating the angles of a plurality of joints, each of which should be fixed under a fixed condition;
And a control step of controlling the robot according to an angle calculation step.
（１１）アーム体を介して互いに角変位可能に連結されて直列に並ぶ複数の関節部を含み、複数の関節部のうちで、直列方向一端部の関節部に設定される設定部分を、予め定める位置および姿勢の少なくともいずれかを示す固定条件で固定した状態で、前記注目する複数の関節部の取り得る角度の組合せが複数通り存在する多関節ロボットにおける各関節部の角度を演算する演算方法であって、
前記設定部分を固定すべき固定条件を決定する固定条件決定工程と、
冗長関節部を決定する冗長関節部決定工程であって、複数の関節部のうちで冗長関節部を除く残余の関節部の数が、固定条件に従って設定部分が拘束される自由度の数と一致するように、複数の関節部の少なくとも１つを冗長関節部として決定する冗長関節部決定工程と、
冗長関節部の角度を冗長角度として決定する冗長角度決定工程と、
固定条件決定工程によって決定した固定条件と、冗長角度決定工程によって決定した冗長角度とに基づいて、複数の関節部の各角度を変数として設定部分の位置または姿勢を求めるための関係式を解いて、設定部分が固定条件で固定されるべき、複数の関節部の角度をそれぞれ演算する角度演算工程と備えることを特徴とする多関節ロボットの各関節部の角度の演算方法。
(11) A plurality of joint portions that are connected to each other via an arm body so as to be angularly displaceable and arranged in series, and among the plurality of joint portions, a setting portion that is set to the joint portion at one end portion in the series direction is A calculation method for calculating the angle of each joint in an articulated robot in which there are a plurality of combinations of angles that can be taken by the plurality of joints of interest in a fixed state that indicates at least one of a predetermined position and posture Because
A fixing condition determining step for determining a fixing condition for fixing the setting portion;
A redundant joint part determining step for determining a redundant joint part, wherein the number of remaining joint parts excluding the redundant joint part among a plurality of joint parts matches the number of degrees of freedom in which the set part is constrained according to a fixed condition A redundant joint determination step for determining at least one of the plurality of joints as a redundant joint; and
A redundant angle determining step of determining the angle of the redundant joint as a redundant angle;
Based on the fixed condition determined by the fixed condition determining step and the redundant angle determined by the redundant angle determining step, solving the relational expression for obtaining the position or posture of the set part using each angle of the plurality of joints as a variable An angle calculation step of calculating the angles of a plurality of joint portions, each of which should be fixed under a fixed condition, and an angle calculation method for each joint portion of the articulated robot.
１ 多関節ロボット
２ 制御装置
３ 第１アーム体
４ 第２アーム体
５ 第３アーム体
６ 第４アーム体
９ 第１関節部
１０ 第２関節部
１１ 第３関節部
１２ 第４関節部
１９ 中央演算処理部
２０ 入力部
２２ 出力部
ｐｈ 対象点
DESCRIPTION OF
Claims (4)
操作手段に設けられた２つの入力手段であって、前記４つの関節部の内、前記ロボットの基端側に位置する２つの関節部の一方を選択するための入力手段、および前記ロボットの先端側に位置する２つの関節部の一方を選択するための入力手段の２つの入力手段のいずれかによって冗長関節部を１つ選択し、
前記ロボットの先端部に設定される設定部分の、前記ロボットの基端部に対する位置または姿勢の３自由度を固定しつつ、選択された前記冗長関節部を角変位させることを特徴とする制御方法。 A method for controlling a robot having four joints adjacent to each other,
Two input means provided in the operation means, the input means for selecting one of the two joint portions located on the proximal end side of the robot among the four joint portions, and the tip of the robot redundant joint select one by one of the two input means of the input means for selecting one of the two joint portions located on the side,
A control method for angularly displacing the selected redundant joint portion while fixing three degrees of freedom of a position or posture of a setting portion set at the distal end portion of the robot with respect to a proximal end portion of the robot .
第１アーム体の一端部は、第１関節部によって、第１アーム体の軸線と同軸の第１回転軸線まわりに回転自在に基台に連結され、One end portion of the first arm body is connected to the base by the first joint portion so as to be rotatable about a first rotation axis coaxial with the axis of the first arm body,
第２アーム体の一端部は、第２関節部によって、第１アーム体の軸線および第２アーム体の軸線に対して４５度で傾斜する第２回転軸線まわりに回転自在に、第１アーム体の他端部に連結され、One end portion of the second arm body is rotatable by the second joint portion around a second rotation axis inclined at 45 degrees with respect to the axis of the first arm body and the axis of the second arm body. Connected to the other end of the
第３アーム体の一端部は、第３関節部によって、第２アーム体の軸線および第３アーム体の軸線に対して４５度で傾斜する第３回転軸線まわりに回転自在に、第２アーム体の他端部に連結され、One end portion of the third arm body is rotatable by the third joint portion around the third rotation axis inclined at 45 degrees with respect to the axis of the second arm body and the axis of the third arm body. Connected to the other end of the
第２回転軸線と第３回転軸線とは平行であり、The second rotation axis and the third rotation axis are parallel,
第４アーム体の一端部は、第４関節部によって、第３アーム体の軸線および第４アーム体の軸線に対して４５度で傾斜する第４回転軸線まわりに回転自在に、第３アーム体の他端部に連結され、One end portion of the fourth arm body is rotatable by the fourth joint portion so as to be rotatable around a fourth rotation axis inclined at 45 degrees with respect to the axis of the third arm body and the axis of the fourth arm body. Connected to the other end of the
第３回転軸線と第４回転軸線とは垂直であり、The third rotation axis and the fourth rotation axis are vertical,
前記設定部分は、第４アーム体の他端部に設定され、The setting portion is set at the other end of the fourth arm body,
前記ロボットの基端側に位置する２つの関節部の一方は、第１関節部であり、One of the two joint portions located on the proximal end side of the robot is a first joint portion,
前記ロボットの先端側に位置する２つの関節部の一方は、第４関節部であり、One of the two joint portions located on the tip side of the robot is a fourth joint portion,
冗長関節部を選択する工程では、前記第１関節部と前記第４関節部とだけが冗長関節部として選択可能に設定された操作手段を用いて冗長関節部が選択されることを特徴とする請求項１に記載の制御方法。In the step of selecting a redundant joint part, the redundant joint part is selected by using an operating means in which only the first joint part and the fourth joint part can be selected as redundant joint parts. The control method according to claim 1.
操作手段に設けられた２つの入力手段であって、前記ロボットの基端部において隣接する４つの関節部の内、前記ロボットの基端側に位置する２つの関節部の一方を選択するための入力手段、および前記ロボットの先端側に位置する２つの関節部の一方を選択するための入力手段の２つの入力手段のいずれかによって前記４つの関節部のうちから１つの冗長関節部を選択し、
前記ロボットの先端部に設定される設定部分の、前記ロボットの基端部に対する位置および姿勢の６自由度を固定しつつ、選択された前記冗長関節部を角変位させることを特徴とする制御方法。 A control method for a robot that has only seven joints, the degree of redundancy is 1, and the rotation axes of three joints adjacent at the tip of the robot intersect at one point,
Two input means provided in the operating means for selecting one of the two joint portions located on the proximal end side of the robot among the four joint portions adjacent to each other at the proximal end portion of the robot input means, and selects one of the redundant joints from among the four joint portions by either of the two input means of the input means for selecting one of the two joint portions located on the distal end side of the robot ,
A control method for angularly displacing the selected redundant joint portion while fixing the six degrees of freedom of the position and posture of the setting portion set at the distal end portion of the robot with respect to the proximal end portion of the robot .
第１アーム体の一端部は、第１関節部によって、第１アーム体の軸線と同軸の第１回転軸線まわりに回転自在に基台に連結され、One end portion of the first arm body is connected to the base by the first joint portion so as to be rotatable about a first rotation axis coaxial with the axis of the first arm body,
第２アーム体の一端部は、第２関節部によって、第１アーム体の軸線および第２アーム体の軸線に対して４５度で傾斜する第２回転軸線まわりに回転自在に、第１アーム体の他端部に連結され、One end portion of the second arm body is rotatable by the second joint portion around a second rotation axis inclined at 45 degrees with respect to the axis of the first arm body and the axis of the second arm body. Connected to the other end of the
第３アーム体の一端部は、第３関節部によって、第２アーム体の軸線および第３アーム体の軸線に対して４５度で傾斜する第３回転軸線まわりに回転自在に、第２アーム体の他端部に連結され、One end portion of the third arm body is rotatable by the third joint portion around the third rotation axis inclined at 45 degrees with respect to the axis of the second arm body and the axis of the third arm body. Connected to the other end of the
第２回転軸線と第３回転軸線とは平行であり、The second rotation axis and the third rotation axis are parallel,
第４アーム体の一端部は、第４関節部によって、第３アーム体の軸線および第４アーム体の軸線に対して４５度で傾斜する第４回転軸線まわりに回転自在に、第３アーム体の他端部に連結され、One end portion of the fourth arm body is rotatable by the fourth joint portion so as to be rotatable around a fourth rotation axis inclined at 45 degrees with respect to the axis of the third arm body and the axis of the fourth arm body. Connected to the other end of the
第３回転軸線と第４回転軸線とは垂直であり、The third rotation axis and the fourth rotation axis are vertical,
第５アーム体の一端部は、第５関節部によって、第４アーム体の軸線および第５アーム体の軸線と同軸の第５回転軸線まわりに回転自在に、第４アーム体の他端部に連結され、One end portion of the fifth arm body can be rotated by the fifth joint portion around the axis of the fourth arm body and the fifth rotation axis coaxial with the axis of the fifth arm body, and to the other end portion of the fourth arm body. Concatenated,
第６アーム体の一端部は、第６関節部によって、第５アーム体の軸線および第６アーム体の軸線に対して４５度で傾斜する第６回転軸線まわりに回転自在に、第５アーム体の他端部に連結され、One end portion of the sixth arm body is rotatable by the sixth joint portion around a sixth rotation axis inclined at 45 degrees with respect to the axis of the fifth arm body and the axis of the sixth arm body. Connected to the other end of the
エンドエフェクタは、第６アーム体の他端部に、第７関節部によって、第６アーム体の軸線と同軸の第７回転軸線５まわりに回転自在に連結され、The end effector is connected to the other end of the sixth arm body by a seventh joint so as to be rotatable about a seventh rotation axis 5 coaxial with the axis of the sixth arm body,
前記設定部分は、エンドエフェクタであり、The setting portion is an end effector;
前記ロボットの基端側に位置する２つの関節部の一方は、第１関節部であり、One of the two joint portions located on the proximal end side of the robot is a first joint portion,
前記ロボットの先端側に位置する２つの関節部の一方は、第４関節部であり、One of the two joint portions located on the tip side of the robot is a fourth joint portion,
冗長関節部を選択する工程では、前記第１関節部と前記第４関節部とだけが冗長関節部として選択可能に設定された操作手段を用いて冗長関節部が選択されることを特徴とする請求項３に記載の制御方法。In the step of selecting a redundant joint part, the redundant joint part is selected by using an operating means in which only the first joint part and the fourth joint part can be selected as redundant joint parts. The control method according to claim 3.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP2013260562A JP5755715B2 (en) | 2013-12-17 | 2013-12-17 | Robot control method |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP2013260562A JP5755715B2 (en) | 2013-12-17 | 2013-12-17 | Robot control method |
Related Parent Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| JP2012156985A Division JP2012192518A (en) | 2012-07-12 | 2012-07-12 | Device and method for controlling redundant robot having redundant joint |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| JP2014050960A JP2014050960A (en) | 2014-03-20 |
| JP5755715B2 true JP5755715B2 (en) | 2015-07-29 |
Family
ID=50609916
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| JP2013260562A Active JP5755715B2 (en) | 2013-12-17 | 2013-12-17 | Robot control method |
Country Status (1)
| Country | Link |
|---|---|
| JP (1) | JP5755715B2 (en) |
Families Citing this family (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP2018192551A (en) * | 2017-05-16 | 2018-12-06 | セイコーエプソン株式会社 | Control device, robot, and robot system |
| US11897146B2 (en) * | 2018-12-18 | 2024-02-13 | Canon Kabushiki Kaisha | Examination method for examining robot apparatus, control apparatus, and storage medium |
Family Cites Families (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JPS62251901A (en) * | 1986-04-25 | 1987-11-02 | Fanuc Ltd | Course controller for multiaxis robot |
| JP3379540B2 (en) * | 1991-10-31 | 2003-02-24 | 株式会社安川電機 | Arm control method for redundant arm robot |
| JPH05220681A (en) * | 1992-02-06 | 1993-08-31 | Mitsubishi Electric Corp | Robot and its control method |
| JP4647919B2 (en) * | 2004-02-18 | 2011-03-09 | 川崎重工業株式会社 | Control method and control apparatus |
-
2013
- 2013-12-17 JP JP2013260562A patent/JP5755715B2/en active Active
Also Published As
| Publication number | Publication date |
|---|---|
| JP2014050960A (en) | 2014-03-20 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| JP2007136590A (en) | Control device and control method for redundant robot having redundant joint | |
| EP3342561B1 (en) | Remote control robot system | |
| US9149931B2 (en) | Robot system, robot control device and method for controlling robot | |
| US9073211B2 (en) | Control system and teaching method for seven-axis articulated robot | |
| US8977392B2 (en) | Robot control device, robot control method, robot control program, and robot system | |
| EP2660013B1 (en) | Method of controlling seven-shaft multi-joint robot, control program, and robot control device | |
| JP2012192518A (en) | Device and method for controlling redundant robot having redundant joint | |
| JP5458769B2 (en) | Robot control device | |
| WO2015137162A1 (en) | Control device, robot system, and method for generating control data | |
| JP5755715B2 (en) | Robot control method | |
| KR101787865B1 (en) | Inverse kinematic solution for multi-joint link mechanism, and device for creating instructional data by using inverse kinematic solution | |
| JP2019093487A (en) | Robot control device and robot reverse conversion processing method | |
| JP2011224745A (en) | Robot teaching device and controller for the same, and program | |
| JPWO2018199035A1 (en) | Articulated robot and articulated robot system | |
| JP4230196B2 (en) | Positioning calculation method and positioning calculation apparatus | |
| JP2009050949A (en) | Method and apparatus for teaching track of robot arm | |
| JPH07129231A (en) | Noncontact point teaching device | |
| JP7352267B1 (en) | Articulated robot, control method for an articulated robot, robot system, and article manufacturing method | |
| JP2019093488A (en) | Robot control device and robot reverse conversion processing method | |
| JP4525325B2 (en) | Motion control method for super redundant robot | |
| WO2024048285A1 (en) | Articulated robot, articulated robot control method, robot system, and article manufacturing method | |
| JP7232704B2 (en) | ROBOT PROGRAM EVALUATION DEVICE, ROBOT PROGRAM EVALUATION METHOD AND ROBOT PROGRAM EVALUATION PROGRAM | |
| JP2014124733A (en) | Robot control device, and robot control method | |
| Chu et al. | Experimental analysis of augmented reality interfaces for robot programming by demonstration in manufacturing | |
| JP2021104551A (en) | Robot drive region simulation device, robot drive region simulation method and robot drive region simulation program |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| A621 | Written request for application examination |
Free format text: JAPANESE INTERMEDIATE CODE: A621 Effective date: 20131217 |
|
| A131 | Notification of reasons for refusal |
Free format text: JAPANESE INTERMEDIATE CODE: A131 Effective date: 20140924 |
|
| A977 | Report on retrieval |
Free format text: JAPANESE INTERMEDIATE CODE: A971007 Effective date: 20140925 |
|
| A521 | Request for written amendment filed |
Free format text: JAPANESE INTERMEDIATE CODE: A523 Effective date: 20141106 |
|
| 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: 20150526 |
|
| A61 | First payment of annual fees (during grant procedure) |
Free format text: JAPANESE INTERMEDIATE CODE: A61 Effective date: 20150527 |
|
| R150 | Certificate of patent or registration of utility model |
Ref document number: 5755715 Country of ref document: JP Free format text: JAPANESE INTERMEDIATE CODE: R150 |
|
| R250 | Receipt of annual fees |
Free format text: JAPANESE INTERMEDIATE CODE: R250 |
|
| R250 | Receipt of annual fees |
Free format text: JAPANESE INTERMEDIATE CODE: R250 |