JPH07132474A - Manipulator control device - Google Patents
Manipulator control deviceInfo
- Publication number
- JPH07132474A JPH07132474A JP27400393A JP27400393A JPH07132474A JP H07132474 A JPH07132474 A JP H07132474A JP 27400393 A JP27400393 A JP 27400393A JP 27400393 A JP27400393 A JP 27400393A JP H07132474 A JPH07132474 A JP H07132474A
- Authority
- JP
- Japan
- Prior art keywords
- joint
- manipulator
- redundant
- freedom
- angle
- 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.)
- Withdrawn
Links
Landscapes
- Numerical Control (AREA)
- Manipulator (AREA)
Abstract
Description
【0001】[0001]
【産業上の利用分野】本発明はマニピュレータ制御装置
に関し、特にロール回転軸とピッチ回転軸とが交互に結
合した7自由度の関節アームを有するマニピュレータの
制御装置に関するものである。BACKGROUND OF THE INVENTION 1. Field of the Invention The present invention relates to a manipulator control device, and more particularly to a manipulator control device having a 7-degree-of-freedom articulated arm in which roll rotating shafts and pitch rotating shafts are alternately coupled.
【0002】産業用に用いられているロボットの多く
は、最大でも6自由度のマニピュレータが主である。こ
れは、手先によって対象物の位置決めをするに際して6
自由度あれば充分であるからである。Most of the robots used for industrial purposes are mainly manipulators having a maximum of 6 degrees of freedom. This is useful when positioning an object with your hand.
This is because the degree of freedom is sufficient.
【0003】一方、人間の腕は7自由度を有する。人間
はこれによって、例えば手先で対象物を位置決めしなが
ら同時に肘の位置を制御して障害物をかわしたりするこ
とが出来る。On the other hand, the human arm has seven degrees of freedom. With this, a human can, for example, position an object with a hand and simultaneously control the position of the elbow to avoid an obstacle.
【0004】従って、人間の腕の様に冗長な7自由度を
有するロボット・マニピュレータが必要となっている。Therefore, there is a need for a robot manipulator having redundant 7 degrees of freedom like a human arm.
【0005】[0005]
【従来の技術】従来では、冗長マニピュレータを実時間
で動かすため、手先の位置・姿勢指令値と冗長自由度を
制御すための指令値から逆座標変換式を用いて関節角度
に変換することにより行われていた。2. Description of the Related Art Conventionally, since a redundant manipulator is moved in real time, it is necessary to convert a position / orientation command value of a hand and a command value for controlling redundant degrees of freedom into a joint angle by using an inverse coordinate conversion formula. It was done.
【0006】以下これについて図9及び図10を用いて
概略的に説明する。This will be schematically described below with reference to FIGS. 9 and 10.
【0007】図9はマニピュレータの構成を示す図であ
る。マニピュレータ10はロール回転軸とピッチ回転軸
が交互に結合したRPRPRPR型回転関節(7自由
度)とアーム11〜17と関節11a〜17aとを有
し、アーム17の先端には、手首30が固定されてい
る。また、各関節の回転方向はθ1〜θ7で示してい
る。FIG. 9 is a diagram showing the structure of the manipulator. The manipulator 10 has an RPRPRPR type rotary joint (7 degrees of freedom) in which a roll rotation axis and a pitch rotation axis are alternately coupled, arms 11 to 17 and joints 11a to 17a, and a wrist 30 is fixed to a tip of the arm 17. Has been done. The rotation directions of the joints are indicated by θ1 to θ7.
【0008】この機構では、人間の腕と同様に手先を固
定した状態で手首(第6関節)より根元側のアーム部分
を動かして肘関節(第4関節)の位置を変えることが出
来る。尚、6自由度の場合には第3関節13aがなく7
自由度のような働きは出来ない。With this mechanism, like the human arm, the position of the elbow joint (fourth joint) can be changed by moving the arm portion on the base side of the wrist (sixth joint) in a state where the hand is fixed. In the case of 6 degrees of freedom, there is no third joint 13a and 7
You cannot work like a degree of freedom.
【0009】ベース10aの上端には回転関節である第
1関節11aの下側が固定されており、第1関節11a
の上側には第1アーム11の下端が固定されている。第
1アーム11の上端には屈曲関節である第2関節12a
の一方が固定されており、第2関節12aの他方には第
2アーム12の下端が固定されている。さらに、第2ア
ーム12の上端には回転関節13aの下側が固定されて
おり、第3関節13aの上側には第3アーム12の下端
が固定されている。第3アームは途中で90°屈曲して
いる。第3アーム13の他端には屈曲関節である第4関
節14aの一方が固定されている。第4関節14aの他
方には第4アーム14が固定されている。第4アーム1
4も途中で90°屈曲している。The lower side of the first joint 11a, which is a rotary joint, is fixed to the upper end of the base 10a.
The lower end of the first arm 11 is fixed to the upper side of the. A second joint 12a, which is a bending joint, is provided at the upper end of the first arm 11.
One is fixed, and the lower end of the second arm 12 is fixed to the other of the second joint 12a. Further, the lower side of the rotary joint 13a is fixed to the upper end of the second arm 12, and the lower end of the third arm 12 is fixed to the upper side of the third joint 13a. The third arm is bent 90 ° on the way. One of the fourth joints 14a, which is a bending joint, is fixed to the other end of the third arm 13. The fourth arm 14 is fixed to the other side of the fourth joint 14a. 4th arm 1
4 also bends 90 ° in the middle.
【0010】第4アーム14の上端には回転関節である
第5関節15aの下側が固定されている。第5関節15
aの上側には第5アームの下端が固定されている。第5
アームの上端には屈曲関節である第6関節16aの一方
が固定されており、第6関節16aの他方には第6アー
ム16の下端が固定されている。第6アーム16の上端
には回転関節である第7関節17aの下側が固定されて
おり、第7関節17aの上側には第7アーム17の下端
が固定されている。第7アーム17の先端には手先30
が固定されている。即ち、第7関節17aは手先の回転
を制御する。The lower side of the fifth joint 15a, which is a rotary joint, is fixed to the upper end of the fourth arm 14. Fifth joint 15
The lower end of the fifth arm is fixed to the upper side of a. Fifth
One of the sixth joints 16a, which is a bending joint, is fixed to the upper end of the arm, and the lower end of the sixth arm 16 is fixed to the other of the sixth joints 16a. The lower side of the seventh joint 17a, which is a rotary joint, is fixed to the upper end of the sixth arm 16, and the lower end of the seventh arm 17 is fixed to the upper side of the seventh joint 17a. The tip of the seventh arm 17 has a fingertip 30.
Is fixed. That is, the seventh joint 17a controls the rotation of the hand.
【0011】尚、次の表1には関節角θi に対するリン
ク間距離di とねじれ角αi とリンク長ai との対応関
係が示されている。The following Table 1 shows the correspondence between the link distance d i , the twist angle α i , and the link length a i with respect to the joint angle θ i .
【0012】[0012]
【表1】 [Table 1]
【0013】ここで、マニピュレータ10の各関節11
a〜17aのうち、一個の関節を冗長自由度制御用関節
として割り当てる。ここでは、第3関節13aを冗長自
由度制御用関節とする。それ以外の6個の関節11a,
12a,14a,15a,16a,17aを手先運動制
御用関節として割り当てる。この6個の手先運動制御用
関節11a,12a,14a,15a,16a,17a
によって6自由度を持つ手先の運動が決まる。Here, each joint 11 of the manipulator 10
One of the joints a to 17a is assigned as a joint for redundant degree of freedom control. Here, the third joint 13a is a redundant degree of freedom control joint. 6 other joints 11a,
12a, 14a, 15a, 16a, and 17a are assigned as joints for controlling hand movements. These six hand movement control joints 11a, 12a, 14a, 15a, 16a, 17a
Determines the movement of the hand with 6 degrees of freedom.
【0014】そして図10に示すように、以下の制御を
行う。Then, as shown in FIG. 10, the following control is performed.
【0015】[ステップS21]:現在の関節角θ1 〜
θ7 を検出する。[Step S21]: Current joint angle θ 1 ~
Detect θ 7 .
【0016】[ステップS22]:現在の手先位置・姿
勢を計算する。[Step S22]: The current hand position / posture is calculated.
【0017】この様に手先位置・姿勢が決まると、肩
(第2関節)、肘(第4関節)及び手首(第6関節)の
成す三角形は一意に決まる。この三角形の肘位置での角
度がθ 4 であり、次の式(1)によって示すことが出来
る。When the hand position / posture is determined in this way, the shoulder
(2nd joint), elbow (4th joint) and wrist (6th joint)
The triangle formed is uniquely determined. The angle at the elbow position of this triangle
Degree is θ FourAnd can be expressed by the following equation (1).
It
【0018】[0018]
【数1】 [Equation 1]
【0019】この様に肘位置での角度θ4 を求めたの
ち、次に、肩(第2関節)位置と手首(第6関節)位置
の座標変換関係からθ1 ,θ2 ,θ3 の関係式を次式の
ように求める。After obtaining the angle θ 4 at the elbow position in this manner, next, from the coordinate conversion relationship between the shoulder (second joint) position and the wrist (sixth joint) position, θ 1 , θ 2 , and θ 3 are calculated. The relational expression is calculated as follows.
【0020】[0020]
【数2】 [Equation 2]
【0021】上記の数式(2a)〜(2c)にはθ5 〜
θ7 は含まれていないが、θ4 が既に求められているの
で結局未知数はθ1 〜θ3 の三つである。In the above equations (2a) to (2c), θ 5 to
Although θ 7 is not included, since θ 4 has already been obtained, the unknowns are three, θ 1 to θ 3 .
【0022】しかしながら、これらの関係式(2a)〜
(2c)は余弦定理関係を満たすので実質的には二つの
関係式が存在することとなり、任意に取り得る変数が存
在しこの関係式(2a)〜(2c)だけでは関節角度θ
1 〜θ3 を求めることは出来ない。このθ1 〜θ3 がア
ームの冗長性を表している。However, these relational expressions (2a)-
Since (2c) satisfies the cosine theorem relation, there are substantially two relational expressions, and there are variables that can be arbitrarily taken, and the joint angle θ can be obtained only by these relational expressions (2a) to (2c).
It is not possible to obtain 1 to θ 3 . This θ 1 to θ 3 represents the redundancy of the arm.
【0023】[ステップS23]:第3関節をパラメー
タ化する。上記のようにパラメータが冗長であるので、
ここで第3関節の角度θ3 をパラメータとする。これは
上述したように6自由度の場合には第3関節13aがな
くそれでも6自由度あれば一定の動きについてはマニピ
ュレータとして充分であるからである。[Step S23]: The third joint is parameterized. Since the parameters are redundant as above,
Here, the angle θ 3 of the third joint is used as a parameter. This is because, as described above, in the case of 6 degrees of freedom, there is no third joint 13a, and even if it has 6 degrees of freedom, it is sufficient as a manipulator for constant movement.
【0024】[ステップS24]:第3関節を固定し逆
座標変換式(逆運動学方程式)から各関節角を計算す
る。[Step S24]: The third joint is fixed and each joint angle is calculated from the inverse coordinate transformation formula (inverse kinematic equation).
【0025】上記の様に関節角θ3 をパラメータとして
値を固定すると式(2c)より次のように関節角θ1 が
求められる。When the value is fixed using the joint angle θ 3 as a parameter as described above, the joint angle θ 1 is obtained from the equation (2c) as follows.
【0026】[0026]
【数3】 [Equation 3]
【0027】更に第2関節の関節角度θ2 は次の様にな
る。Further, the joint angle θ 2 of the second joint is as follows.
【0028】[0028]
【数4】 [Equation 4]
【0029】そして更に、肘位置にある座標系から見た
手先の位置・姿勢の関係から関節角θ5 及びθ6 をそれ
ぞれ次式の様にして求めることができる。Further, the joint angles θ 5 and θ 6 can be calculated by the following equations from the relationship between the position and the posture of the hand viewed from the coordinate system at the elbow position.
【0030】[0030]
【数5】 [Equation 5]
【0031】[0031]
【数6】 [Equation 6]
【0032】更には手首位置にある座標系から見た手先
の位置・姿勢の関係から関節角θ7を次式の様に求める
ことができる。Furthermore, the joint angle θ 7 can be calculated from the relationship between the position and posture of the hand as seen from the coordinate system at the wrist position as in the following equation.
【0033】[0033]
【数7】 [Equation 7]
【0034】ここで、上記の式(5)〜(7)における
ベクトル成分「a」ついて簡単に説明すると、まず図9
に示した7自由度マニピュレータの各関節11a〜17
aにおける変換行列は次の式のように表される。Here, the vector component "a" in the above equations (5) to (7) will be briefly described. First, FIG.
Each joint 11a to 17 of the 7-DOF manipulator shown in FIG.
The conversion matrix in a is represented by the following equation.
【0035】[0035]
【数8】 [Equation 8]
【0036】従って、上記の様に肘位置(第4関節)に
ある座標系からみた手先位置・姿勢(第7関節)の座標
系は、次式の様に表される。Therefore, the coordinate system of the hand position / posture (seventh joint) viewed from the coordinate system at the elbow position (fourth joint) as described above is expressed by the following equation.
【0037】[0037]
【数9】 [Equation 9]
【0038】ここで、上記の座標変換行列4T7 を次の
式の様に置き換える。Here, the above coordinate transformation matrix 4 T 7 is replaced by the following equation.
【0039】[0039]
【数10】 [Equation 10]
【0040】従って、この場合、第4関節の座標からみ
た第7関節の座標系のz軸の方向は次の式の様になる。Therefore, in this case, the direction of the z-axis of the coordinate system of the seventh joint as seen from the coordinates of the fourth joint is as in the following equation.
【0041】[0041]
【数11】 [Equation 11]
【0042】この様なベクトルのx,y,z軸方向の成
分が求められると上記の式(5)及び(6)における4
a7X〜4a7Zが求められ、これを代入する事によって関
節角度θ5 及びθ6 が求められる。[0042] 4 in x of such vector, y, the components of the z-axis direction is determined above equation (5) and (6)
a 7X to 4 a 7Z are obtained, and the joint angles θ 5 and θ 6 are obtained by substituting these.
【0043】同様にして式(7)における6a7X及び6a
7Zも求められ、関節角度θ7 を求めることが出来る。Similarly, 6 a 7X and 6 a in equation (7)
7Z is also obtained, and the joint angle θ 7 can be obtained.
【0044】[ステップS25]:アームを動かす。こ
の様にして求められた各関節角に対して制御系が各アー
ムを動かすことによってステップS27で与えられた手
先位置・姿勢を実現することが出来る。[Step S25]: The arm is moved. The control system moves each arm with respect to each joint angle thus obtained, whereby the hand position / posture given in step S27 can be realized.
【0045】このように、7自由度の各関節角θ1 〜θ
7 を与えると座標変換式によって手先の位置・姿勢が求
まるのに対し、この座標変換式が非線型(不可逆)であ
るために手先の位置・姿勢を与えても各関節角度θ1 〜
θ7 は一意には決まらない。従って、上記の様にそのう
ちのθ3 のみを固定のパラメータとして演算すれば、そ
の他の関節角θ1 ,θ2 ,θ4 ,θ5 ,θ6 ,θ7 が得
られることを意味している。In this way, each joint angle θ 1 to θ of 7 degrees of freedom is
If 7 is given, the position / orientation of the hand can be obtained by the coordinate conversion formula, but since this coordinate conversion formula is non-linear (irreversible), even if the position / orientation of the hand is given, each joint angle θ 1 ~
θ 7 cannot be uniquely determined. Therefore, if only θ 3 among them is calculated as a fixed parameter as described above, the other joint angles θ 1 , θ 2 , θ 4 , θ 5 , θ 6 , θ 7 are obtained. .
【0046】換言すれば、7自由度以上のマニピュレー
タでは、上記の様な手先の位置・姿勢の値から関節角度
への逆変換に任意性が生じ、同一の手先の位置・姿勢を
取る関節角度はある制約条件を満たしながら無限に存在
する。冗長マニピュレータは、この関節角度の任意性を
利用してマニピュレータの機構的制限(特異点や関節リ
ミット等)の回避や腕全体での障害物回避を行うために
用いられている。In other words, in a manipulator having 7 or more degrees of freedom, there is arbitrariness in the inverse conversion from the position / orientation value of the hand as described above into the joint angle, and the joint angle having the same hand / position is obtained. Exists infinitely while satisfying certain constraints. Redundant manipulators are used to avoid mechanical limitations (such as singular points and joint limits) of manipulators and avoid obstacles in the entire arm by utilizing the arbitrariness of the joint angles.
【0047】[0047]
【発明が解決しようとする課題】しかしながら上記の様
な従来の制御装置では、冗長自由度の制御指令値を一つ
の変数、例えば第3関節の角度変数のθ3 で表していた
ため、冗長自由度が使えない手先位置・姿勢領域(図5
の網掛領域A)が存在し、冗長自由度を充分に活用する
事ができなかった。。However, in the conventional control device as described above, since the control command value of the redundant degree of freedom is represented by one variable, for example, the angle variable θ 3 of the third joint, the redundant degree of freedom is reduced. Hand position / posture area (Fig. 5)
The shaded area A) exists, and the redundancy degree of freedom could not be fully utilized. .
【0048】これを計算機シミュレーションにより確か
めると、図8(a)に示す様に、肘位置(肩位置、肘位
置及び手首位置からなる三角形が鉛直平面となす角度)
を動かしながら肘が延びた状態から折り畳まった状態に
なる手先位置軌道を与えたときに、冗長制御パラメータ
をθ3 のみを用いて行うと、時刻0.5〜0.83秒の
間では関節角度θ1 及びθ2 が図示の如く求まらず、肘
位置を動かすことが出来ない事が示されている。When this is confirmed by computer simulation, as shown in FIG. 8A, the elbow position (the angle formed by the triangle consisting of the shoulder position, the elbow position and the wrist position and the vertical plane).
When given the hand position trajectory in a state in which the elbow is Tsu folded from the state extending while moving the, when the redundancy control parameters using only theta 3, in between times from 0.5 to 0.83 seconds joints The angles θ 1 and θ 2 are not found as shown, indicating that the elbow position cannot be moved.
【0049】従って本発明は、回転関節より成る7自由
度の関節アームの関節角度を各軸制御部により制御する
マニピュレータ制御装置において、どの様な手先位置・
姿勢領域についても肘位置を動かすことが出来るように
することを目的とする。Therefore, according to the present invention, in the manipulator control device for controlling the joint angle of the joint arm having seven degrees of freedom, which is composed of the rotary joint, by each axis control section, what kind of hand position
The purpose is to be able to move the elbow position in the posture area as well.
【0050】[0050]
【課題を解決するための手段】上記の目的を達成するた
め、本発明に係るマニピュレータ制御装置においては、
図1に原理的に示すように、与えられた手先の位置・姿
勢の指令値に基づいて該位置・姿勢が冗長パラメータの
指令値の領域に在るか否かにより制御パラメータを決定
し該制御パラメータを用いて各関節角度を実時間で逆座
標変換する逆座標変換部3を備えたことを特徴としてい
る。In order to achieve the above object, in the manipulator control device according to the present invention,
As shown in principle in FIG. 1, the control parameter is determined based on whether or not the position / orientation is in the region of the command value of the redundant parameter based on the given command value of the position / orientation of the hand. It is characterized by including an inverse coordinate transformation unit 3 for performing inverse coordinate transformation of each joint angle in real time using parameters.
【0051】また上記の逆座標変換部3は、与えられた
手先の位置・姿勢の指令値に基づいて台座原点に対する
作業座標平面上の手首位置を求め、該手首位置が該手首
位置と3軸を通る直線との距離より大きいか小さいかに
応じて第1関節又は第3関節の角度を該冗長パラメータ
として各関節角度を逆座標変換することができる。Further, the inverse coordinate transformation unit 3 obtains the wrist position on the work coordinate plane with respect to the origin of the pedestal on the basis of the given command value of the position / orientation of the hand, and the wrist position is triaxial with the wrist position. Each joint angle can be inversely transformed by using the angle of the first joint or the third joint as the redundant parameter depending on whether it is larger or smaller than the distance from the straight line passing through.
【0052】[0052]
【作用】上記の様に従来のマニピュレータ制御装置では
一つの冗長制御パラメータのみを用いて制御を行ってい
たため冗長自由度が使えない手先位置・姿勢領域が存在
していた。As described above, in the conventional manipulator control device, since the control is performed using only one redundant control parameter, there is a hand position / posture area in which the redundant degree of freedom cannot be used.
【0053】そこで本発明では図1に示す逆座標変換部
3において図2に示すような処理を実時間で行う。Therefore, in the present invention, the inverse coordinate transformation unit 3 shown in FIG. 1 performs the processing shown in FIG. 2 in real time.
【0054】即ち、手先の位置・姿勢の指令位置を与え
た後、この位置・姿勢が冗長制御パラメータφi の領域
にあるか否かを判定する(ステップS1)。That is, after the command position of the position / orientation of the hand is given, it is determined whether or not this position / orientation is in the region of the redundant control parameter φ i (step S1).
【0055】そして、この冗長制御パラメータφi の領
域にあることが判ったときには制御パラメータをそのパ
ラメータφi とし(ステップS3)、このφi をパラメ
ータとして逆座標変換をφi の指令値に基づいて実行す
る(ステップS4)ことにより各関節角度θを求めるこ
とが出来る。[0055] Then, the control parameter when it is found in the region of the redundancy control parameter phi i and the parameter phi i (step S3), and based on an inverse coordinate transformation to the phi i as a parameter to the command value of phi i By executing the above (step S4), each joint angle θ can be obtained.
【0056】またステップS1において位置・姿勢が冗
長制御パラメータφi の領域にないと判定されたときに
は、この“i”を変更して別の冗長制御パラメータφi
に対して位置・姿勢がその領域にあるか否かを判定す
る。(ステップS2)。If it is determined in step S1 that the position / orientation is not within the area of the redundant control parameter φ i , this "i" is changed to another redundant control parameter φ i.
It is determined whether or not the position / orientation is within that area. (Step S2).
【0057】例えばマニピュレータが7自由度の場合に
はステップS1における冗長制御パラメータφi は図9
に示した関節角度θ1 及びθ3 となる。For example, when the manipulator has 7 degrees of freedom, the redundancy control parameter φ i in step S1 is as shown in FIG.
The joint angles θ 1 and θ 3 shown in FIG.
【0058】そしてこれらの制御パラメータθ1 又はθ
3 を選択するにあたっては、与えられた位置・姿勢の指
令値に基づいて求められる台座原点に対する作業平面上
の手首位置がこの手首位置と3軸を通る直線との距離よ
り大きいか小さいかに対応して決められる。Then, these control parameters θ 1 or θ
In selecting 3 , whether the wrist position on the work plane with respect to the origin of the pedestal, which is obtained based on the given position / orientation command value, is larger or smaller than the distance between this wrist position and the straight line passing through the three axes Can be decided.
【0059】従ってθ1 をパラメータとする領域の場合
にはそのθ1 の値を固定値として逆座標変換をすれば各
関節角度θを求めることが出来、同様にθ3 の領域の場
合にはθ3 を固定値として逆座標変換を行うことにより
各関節角度θを得ることが出来る。Therefore, in the case of a region having θ 1 as a parameter, each joint angle θ can be obtained by performing inverse coordinate transformation with the value of θ 1 being a fixed value. Similarly, in the case of the region of θ 3 , Each joint angle θ can be obtained by performing inverse coordinate transformation with θ 3 as a fixed value.
【0060】この様に本発明では、冗長マニピュレータ
を余分な(冗長な)関節角度をパラメータ化する事によ
り逆座標変換を6自由度の場合に縮退させ、その際、冗
長自由度を動かすためのパラメータを手先の位置・姿勢
により切り換えることにより手先の位置・姿勢に制限さ
れずにマニピュレータの冗長自由度を制御することが出
来る。As described above, in the present invention, the redundant manipulator degenerates the inverse coordinate transformation in the case of 6 degrees of freedom by parameterizing the extra (redundant) joint angle, and at that time, the redundant degree of freedom is moved. By switching the parameter according to the position / orientation of the hand, the redundant degree of freedom of the manipulator can be controlled without being limited to the position / orientation of the hand.
【0061】[0061]
【実施例】以下、本発明の一実施例を図面に基づいて説
明する。図2にマニピュレータの制御系のハードウェア
のブロック図を示す。制御系はワークステーション20
0、制御装置20及び制御されるマニピュレータ10か
ら構成され実時間で処理される。なお、マニピュレータ
10は7自由度であり、7個の関節を有するが、図2で
は説明を簡単にするために、1個の関節を駆動するモー
タのみを示し、制御系も1個の関節を制御する要素のみ
を図示している。DETAILED DESCRIPTION OF THE PREFERRED EMBODIMENTS An embodiment of the present invention will be described below with reference to the drawings. FIG. 2 shows a block diagram of the hardware of the control system of the manipulator. Control system is workstation 20
0, a control device 20, and a controlled manipulator 10 for processing in real time. Note that the manipulator 10 has seven degrees of freedom and has seven joints, but in FIG. 2, only a motor that drives one joint is shown in FIG. 2 for simplification of description, and the control system also has one joint. Only the controlling elements are shown.
【0062】制御装置20はボード計算機21,22を
中心に構成されている。ボード計算機21,22は32
ビットのRISCを使用している。ボード計算機21,
22には、バス21aを介して、A/D変換器23、D
IO(入出力インターフェース)24、D/A変換器2
5、カウンタ26、A/D変換器27が接続されてい
る。The controller 20 is mainly composed of the board computers 21 and 22. 32 on board calculators 21 and 22
Bit RISC is used. Board calculator 21,
22 to the A / D converters 23 and D via the bus 21a.
IO (input / output interface) 24, D / A converter 2
5, a counter 26, and an A / D converter 27 are connected.
【0063】ワークステーション200とボード計算機
21,22は通信回線201によって結合されており、
ワークステーション200からはボード計算機21,2
2にマニピュレータ10の動作プログラム及び必要なデ
ータが送られる。ボード計算機21,22ではこの動作
プログラムに従ってマニピュレータ10を制御する。The workstation 200 and the board computers 21 and 22 are connected by a communication line 201,
From the workstation 200, board computer 21,2
The operation program of the manipulator 10 and the necessary data are sent to 2. The board computers 21 and 22 control the manipulator 10 according to this operation program.
【0064】ボード計算機21はマニピュレータ10の
手先30の位置、速度の制御及び各関節角の位置、速
度、加速度を計算する。また、後述する冗長関節角の制
御等の全体の制御を行う。各関節角の位置、速度、加速
度及び冗長関節角のデータはボード計算機22に送られ
る。ボード計算機22ではこれらのデータを受け取り、
マニピュレータ10の各関節の位置、速度及び加速度を
制御する。The board computer 21 controls the position and speed of the hand 30 of the manipulator 10 and calculates the position, speed and acceleration of each joint angle. In addition, overall control such as control of redundant joint angles described below is performed. The position, velocity, acceleration, and redundant joint angle data of each joint angle are sent to the board computer 22. The board computer 22 receives these data,
The position, velocity and acceleration of each joint of the manipulator 10 are controlled.
【0065】A/D変換器23はマニピュレータ10の
各関節の力検出器からの出力をディジタル値に変換す
る。DIO24はマニピュレータ10とボード計算機2
1,22との間の入出力信号の授受を行うインタフェー
スである。D/A変換器25はボード計算機22からの
各関節の速度指令等を受け取り、モータドライバ28に
送り、モータドライバ28はこの指令に従ってモータ1
0Aを駆動する。The A / D converter 23 converts the output from the force detector of each joint of the manipulator 10 into a digital value. The DIO 24 is a manipulator 10 and a board computer 2.
This is an interface for exchanging input / output signals with the I / Os 1 and 22. The D / A converter 25 receives the speed command of each joint from the board computer 22 and sends it to the motor driver 28, and the motor driver 28 follows the command to the motor 1
Drive 0A.
【0066】モータ10Aにはエンコーダ10Bが同軸
に結合されており、モータ10Aの回転位置をフィード
バックする。すなわち、エンコーダ10Bはモータの回
転に応じてパルスを発生する。このパルスはカウンタ2
6でカウントされ、ボード計算機21,22はこのカウ
ンタ26を読み取ることにより、モータ10Aの回転位
置を認識できる。また、このパルスはF/V(周波数/
電圧)変換器29によって電圧に変換され、A/D変換
器27に送られる。An encoder 10B is coaxially coupled to the motor 10A and feeds back the rotational position of the motor 10A. That is, the encoder 10B generates a pulse according to the rotation of the motor. This pulse is counter 2
It is counted by 6, and the board computers 21 and 22 can recognize the rotational position of the motor 10A by reading the counter 26. In addition, this pulse is F / V (frequency /
The voltage is converted into a voltage by the voltage converter 29 and sent to the A / D converter 27.
【0067】そして、A/D変換器27によって、ディ
ジタル値に変換される。ボード計算機21,22はこの
ディジタル値を読み取ることにより、モータ10Aの速
度、言い換えれば各関節の速度を認識することができ
る。Then, it is converted into a digital value by the A / D converter 27. By reading this digital value, the board computers 21 and 22 can recognize the speed of the motor 10A, in other words, the speed of each joint.
【0068】勿論、図2では関節は1個のみで示したの
で、A/D変換器23、DIO24、D/A変換器2
5、カウンタ26、A/D変換器27等は関節の数に応
じた数のみ必要である。ただし、DIO24等は共通に
使用するように構成することもできる。また、関節を駆
動するモータ10Aも実際には7個必要であり、モータ
ドライバ28、F/V変換器29についても同様であ
る。Of course, since only one joint is shown in FIG. 2, the A / D converter 23, the DIO 24, and the D / A converter 2 are shown.
5, the counter 26, the A / D converter 27, and the like are required only in the number corresponding to the number of joints. However, the DIO 24 and the like may be configured to be commonly used. Further, seven motors 10A for driving the joints are actually required, and the motor driver 28 and the F / V converter 29 are also the same.
【0069】また、図1に示したアーム1は図3のマニ
ピュレータ10に対応し、各軸制御部2は図3のモータ
ドライバ28及びモータ10aに対応し、更に逆座標変
換部3は制御装置20に対応している。The arm 1 shown in FIG. 1 corresponds to the manipulator 10 shown in FIG. 3, each axis control section 2 corresponds to the motor driver 28 and the motor 10a shown in FIG. 3, and the inverse coordinate conversion section 3 is a control device. Corresponds to 20.
【0070】以下、上記の実施例の動作を説明する。こ
の動作は実時間で行われる。尚、本発明の実施例では上
述した式(1)〜(11)は本発明においても同様にし
て用いられるものであるので、本発明の実施例の動作を
説明する図4のフローチャートは概略的に示されてい
る。The operation of the above embodiment will be described below. This operation is performed in real time. In the embodiment of the present invention, the above equations (1) to (11) are similarly used in the present invention. Therefore, the flowchart of FIG. 4 for explaining the operation of the embodiment of the present invention is schematic. Is shown in.
【0071】まず図4において、手先の位置・姿勢の指
令値xが与えられると、この指令値xより手首位置(0x
5,0y5,0z5 )を求める(ステップS11)。First, in FIG. 4, when the command value x of the position / orientation of the hand is given, the wrist position ( 0 x
5 , 0 y 5 , 0 z 5 ) is obtained (step S11).
【0072】そしてこの手首位置から式(1)に示した
様に肩と肘と手首でなす三角形の肘位置での角度θ4 を
求めることができ、更に肩位置と手首位置の座標変換関
係から式(2a)〜(2c)に示す様に関節角度θ1 〜
θ3 の関係式を求めることができる。From this wrist position, the angle θ 4 at the elbow position of the triangle formed by the shoulders, elbows and wrists can be obtained as shown in equation (1), and further from the coordinate conversion relationship between the shoulder position and the wrist position. As shown in equations (2a) to (2c), the joint angle θ 1 to
The relational expression of θ 3 can be obtained.
【0073】そしてこの3つの関係式においては上述し
たように未知数がθ1 〜θ3 の3つあるのに対し実質上
の関係式は余弦定理関係により2つとなるのでパラメー
タがアームの冗長性を表していることになる。In the three relational expressions, as described above, there are three unknowns θ 1 to θ 3, whereas in practice there are two relational expressions due to the cosine theorem relation, and therefore the parameter is arm redundancy. It represents.
【0074】ここで式(2c)を変形すると次式の様に
なる。When the equation (2c) is modified, the following equation is obtained.
【0075】[0075]
【数12】 [Equation 12]
【0076】また、手先の位置・姿勢を決めれば上記の
様に0x5,0y5 及びθ4 が決まるから、式(12)はθ1
とθ3 の関数とみなすことができ、手先を固定した状態
での肘の動きに対応するので、θ1 又はθ3 により肘を
制御出来ることが判る。[0076] Further, as described above be determined the position and attitude of the end from 0 x 5, 0 y 5, and theta 4 is determined, Equation (12) is theta 1
It can be regarded as a function of θ 3 and θ 3 and corresponds to the movement of the elbow with the hand fixed, so it can be seen that the elbow can be controlled by θ 1 or θ 3 .
【0077】そこで、θ1 とθ3 のどちらを冗長制御パ
ラメータとするかは、以下の様に手先の位置・姿勢によ
って選択する。Therefore, which of θ 1 and θ 3 is used as the redundant control parameter is selected according to the position and orientation of the hand as follows.
【0078】式(12)における左辺の√(0x5 2+
0y5 2)は、図6(a)に示すように原点と手首30の位
置の作業座標平面(xy平面)上での距離であり、式
(12)のr(1−cosθ4 )+L2sinθ4 は同図
(b)に示すようにθ1 〜θ3 の3軸を通る直線との距
離であるので、肘が相対的に延びている時は次式の様に
なる。√ ( 0 x 5 2 + on the left side of equation (12)
0 y 5 2 ) is the distance between the origin and the position of the wrist 30 on the work coordinate plane (xy plane) as shown in FIG. 6A, and r (1-cos θ 4 ) + L in the equation (12). Since 2 sin θ 4 is a distance from a straight line passing through the three axes of θ 1 to θ 3 as shown in FIG. 7B, when the elbow is relatively extended, the following equation is obtained.
【0079】[0079]
【数13】 [Equation 13]
【0080】また、相対的に折り畳まっているときは次
式のようになる。When relatively folded, the following equation is obtained.
【0081】[0081]
【数14】 [Equation 14]
【0082】ここで、θ1 を制御パラメータとすると、
式(12)から、θ3 は次式のようになる。Here, when θ 1 is a control parameter,
From the equation (12), θ 3 is given by the following equation.
【0083】[0083]
【数15】 [Equation 15]
【0084】しかし、手先の位置が式(13)で示され
る関係の場合には、θ1 は次式のようになる。However, in the case where the position of the hand has the relationship represented by the equation (13), θ 1 is given by the following equation.
【0085】[0085]
【数16】 [Equation 16]
【0086】従ってθ1 は図5の領域Bのみ可能であ
り、網掛領域Aについては制限され、θ1 で連続して冗
長自由度を制御することは出来ない。この不連続性は図
7(c)に示す通りである。Therefore, θ 1 can be applied only to the area B in FIG. 5, and the shaded area A is limited, and the redundant degree of freedom cannot be continuously controlled with θ 1 . This discontinuity is as shown in FIG.
【0087】そこで、手先位置が式(13)に示す関係
の時には次式によりθ3 を冗長制御パラメータとする。Therefore, when the hand position has the relationship shown in the equation (13), θ 3 is set as the redundant control parameter by the following equation.
【0088】[0088]
【数17】 [Equation 17]
【0089】これは図7(a)に示す状態となり、θ3
は連続していることが判る。This is the state shown in FIG. 7A, and θ 3
It turns out that is continuous.
【0090】逆に手先位置・姿勢が式(14)に示す状
態の時には、θ3 を冗長制御パラメータとすると次式の
ように制約され、θ3 で制御することは出来ない(図7
(c)参照)。On the contrary, when the hand position / posture is in the state shown in the equation (14), if θ 3 is the redundant control parameter, it is restricted as in the following equation, and it is not possible to control with θ 3 (FIG. 7).
(See (c)).
【0091】[0091]
【数18】 [Equation 18]
【0092】従って、図4のフローチャートに戻って手
先の位置・姿勢が式(13)の状態にあるか又は式(1
4)の状態にあるかを判定し(ステップS12)、式
(13)の関係にある場合にはθ1 を制御パラメータと
して逆座標変換式によりθ2 ,θ3 ,θ5 ,θ6 ,θ7
を計算する(ステップS13)。Therefore, returning to the flowchart of FIG. 4, the position / orientation of the hand is in the state of the formula (13) or the formula (1)
Determines whether the state of 4) (step S12), the formula (13 theta 2 by the inverse coordinate conversion formula as control parameter theta 1 if the relationship of), θ 3, θ 5, θ 6, θ 7
Is calculated (step S13).
【0093】また、式(14)に示す関係のときにはθ
3 で制御すればよいので、このθ3をパラメータとした
逆座標変換式によりθ1 ,θ2 ,θ5 ,θ6 ,θ7 を計
算して全ての関節角度θ1 〜θ7 が得られることとな
る。Further, in the case of the relation shown in the equation (14), θ
Since may be controlled at 3, theta 1 by inverse coordinate conversion formula that the theta 3 as a parameter, θ 2, θ 5, θ 6, all joints by calculating the theta 7 angle theta 1 through? 7 is obtained It will be.
【0094】尚、θ1 ,θ2 ,θ5 〜θ7 の角度につい
ては上述した式(3)〜(7)によって求められること
になる。The angles θ 1 , θ 2 , and θ 5 to θ 7 can be obtained by the above equations (3) to (7).
【0095】また、図7(b)に示したグラフは図5に
おける斜線部分とそれ以外の部分との境界線の上を手首
が動く場合の軌跡を示している。The graph shown in FIG. 7B shows the locus when the wrist moves on the boundary line between the shaded portion and the other portion in FIG.
【0096】次に、上記の様に冗長制御パラメータをθ
1 とθ3 との間で切り換えて上手く制御出来るかを計算
機シュミレーションにより確かめた図が図8(b)に示
されている。Next, the redundancy control parameter is set to θ as described above.
FIG. 8B shows a diagram obtained by confirming by computer simulation whether it is possible to control by switching between 1 and θ 3 .
【0097】これによれば、時刻0.5秒のところでパ
ラメータをθ3 からθ1 へ切り換えた時には連続して肘
位置を動かすことができることが示されており、同図
(a)に示す従来例の様に時刻0.5〜0.83秒の間
でθ1 及びθ2 が求まらず肘位置を動かすことができな
いという状態が無くなっていることが判る。According to this, it is shown that the elbow position can be continuously moved when the parameter is switched from θ 3 to θ 1 at the time of 0.5 seconds, and the conventional method shown in FIG. As shown in the example, it can be seen that the state in which θ 1 and θ 2 are not obtained during the time 0.5 to 0.83 seconds and the elbow position cannot be moved has disappeared.
【0098】[0098]
【発明の効果】以上説明したように、本発明に係るマニ
ピュレータ制御装置によれば、与えられた手先の位置・
姿勢の指令値が冗長パラメータの指令値の領域にあるか
否かにより制御パラメータを決定しこの制御パラメータ
を用いて各関節角度を逆座標変換するように構成したの
で、手先の位置・姿勢に制限されずにマニピュレータの
冗長自由度をオフラインまたはオンラインにより制御す
ることが可能となる。As described above, according to the manipulator control device of the present invention, the position of a given hand is
The control parameter is determined depending on whether or not the command value of the posture is within the range of the command value of the redundant parameter, and each joint angle is inversely transformed using this control parameter. Instead, it becomes possible to control the redundant degree of freedom of the manipulator offline or online.
【図1】本発明に係るマニピュレータ制御装置の構成原
理を示したブロック図である。FIG. 1 is a block diagram showing a configuration principle of a manipulator control device according to the present invention.
【図2】本発明に係るマニピュレータ制御装置の動作原
理を示したフローチャート図である。FIG. 2 is a flowchart showing the operating principle of the manipulator control device according to the present invention.
【図3】本発明に係るマニピュレータ制御装置の実施例
を示したブロック図である。FIG. 3 is a block diagram showing an embodiment of a manipulator control device according to the present invention.
【図4】本発明に係るマニピュレータ制御装置に用いら
れる逆座標変換部の実施例を示したフローチャート図で
ある。FIG. 4 is a flowchart showing an embodiment of an inverse coordinate transformation unit used in the manipulator control device according to the present invention.
【図5】本発明に係るマニピュレータ制御装置によって
肘を動かすことができる領域を説明するための図であ
る。FIG. 5 is a diagram for explaining a region where an elbow can be moved by the manipulator control device according to the present invention.
【図6】本発明に係るマニピュレータ制御装置において
肘の伸び縮みを説明するための図である。FIG. 6 is a diagram for explaining expansion and contraction of an elbow in the manipulator control device according to the present invention.
【図7】本発明に係るマニピュレータ制御装置における
第1関節の角度θ1 と第3関節の角度θ3 との関係を示
したグラフ図である。7 is a graph showing the relationship between the angle of the first joint of the manipulator control device theta 1 and the angle theta 3 of the third joint according to the present invention.
【図8】従来例及び本発明におけるパラメータの相違を
説明するための計算機シミュレーション図である。FIG. 8 is a computer simulation diagram for explaining the difference in parameters between the conventional example and the present invention.
【図9】本発明に係るマニピュレータ制御装置における
7自由度のマニピュレータの一般的な構成を示した図で
ある。FIG. 9 is a diagram showing a general configuration of a seven-degree-of-freedom manipulator in the manipulator control device according to the present invention.
【図10】従来技術の動作を説明したフローチャート図
である。FIG. 10 is a flowchart illustrating the operation of the conventional technique.
1 アーム 2 各軸制御部 3 逆座標変換部 10a,11〜17 アーム 11a〜17a 関節 L0〜L3 リンク間距離 r リンク長 θ1 〜θ7 関節角度 30 手首 尚、図中、同一符号は同一または相当部分を示す。1 arm 2 each axis control unit 3 inverse coordinate conversion unit 10a, 11 to 17 arm 11a to 17a joint L 0 to L 3 link distance r link length θ 1 to θ 7 joint angle 30 wrist Indicates the same or corresponding part.
Claims (2)
ーム(1)の関節角度を各軸制御部(2)により制御す
るマニピュレータ制御装置において、 与えられた手先の位置・姿勢の指令値に基づいて該位置
・姿勢が冗長パラメータの指令値の領域に在るか否かに
より制御パラメータを決定し該制御パラメータを用いて
各関節角度を実時間で逆座標変換する逆座標変換部
(3)を備えたことを特徴とするマニピュレータ制御装
置。1. A manipulator control device for controlling a joint angle of a joint arm (1) having 7 degrees of freedom or more, which is composed of a rotary joint, by each axis control section (2). An inverse coordinate conversion unit (3) that determines a control parameter based on whether or not the position / orientation is in the region of the command value of the redundant parameter based on the above, and uses the control parameter to inversely convert each joint angle in real time A manipulator control device comprising:
手先の位置・姿勢の指令値に基づいて台座原点に対する
作業座標平面上の手首位置を求め、該手首位置が該手首
位置と3軸を通る直線との距離より大きいか小さいかに
応じて第1関節又は第3関節の角度を該冗長パラメータ
として各関節角度を逆座標変換することを特徴とした請
求項1に記載のマニピュレータ制御装置。2. The inverse coordinate conversion section (3) obtains a wrist position on a work coordinate plane with respect to the origin of the pedestal on the basis of the given command values of the position and orientation of the hand, and the wrist position is the wrist position. 2. The inverse coordinate transformation of each joint angle is performed by using the angle of the first joint or the third joint as the redundant parameter according to whether the distance is larger or smaller than the distance between a straight line passing through the three axes. Manipulator control device.
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
JP27400393A JPH07132474A (en) | 1993-11-02 | 1993-11-02 | Manipulator control device |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
JP27400393A JPH07132474A (en) | 1993-11-02 | 1993-11-02 | Manipulator control device |
Publications (1)
Publication Number | Publication Date |
---|---|
JPH07132474A true JPH07132474A (en) | 1995-05-23 |
Family
ID=17535599
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
JP27400393A Withdrawn JPH07132474A (en) | 1993-11-02 | 1993-11-02 | Manipulator control device |
Country Status (1)
Country | Link |
---|---|
JP (1) | JPH07132474A (en) |
Cited By (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN103252779A (en) * | 2012-02-16 | 2013-08-21 | 精工爱普生株式会社 | Robot control device, robot control method, robot control program, and robot system |
US8761932B2 (en) | 2000-12-28 | 2014-06-24 | C.H.I. Development Mgmt. Ltd. Xxiv, Llc | Automation equipment control system |
JP2017170571A (en) * | 2016-03-24 | 2017-09-28 | セイコーエプソン株式会社 | Robot, robot control apparatus, and robot system |
-
1993
- 1993-11-02 JP JP27400393A patent/JPH07132474A/en not_active Withdrawn
Cited By (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US8761932B2 (en) | 2000-12-28 | 2014-06-24 | C.H.I. Development Mgmt. Ltd. Xxiv, Llc | Automation equipment control system |
CN103252779A (en) * | 2012-02-16 | 2013-08-21 | 精工爱普生株式会社 | Robot control device, robot control method, robot control program, and robot system |
US8977392B2 (en) | 2012-02-16 | 2015-03-10 | Seiko Epson Corporation | Robot control device, robot control method, robot control program, and robot system |
JP2017170571A (en) * | 2016-03-24 | 2017-09-28 | セイコーエプソン株式会社 | Robot, robot control apparatus, and robot system |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
JP3207728B2 (en) | Control method of redundant manipulator | |
JP4273335B2 (en) | Robot arm | |
JPS591180A (en) | Manipulator executing motion of path controlled | |
KR20130066689A (en) | Control device and teaching method for seven-shaft multi-joint robot | |
CN111775145A (en) | Control system of series-parallel robot | |
JP7144754B2 (en) | Articulated robots and articulated robot systems | |
CN111791234A (en) | Anti-collision control algorithm for working positions of multiple robots in narrow space | |
JPH06143172A (en) | Control system for redundancy manipulator | |
JPS6346844B2 (en) | ||
JPH07132474A (en) | Manipulator control device | |
JPH0693209B2 (en) | Robot's circular interpolation attitude control device | |
JP2019093487A (en) | Robot control device and robot reverse conversion processing method | |
JP4230196B2 (en) | Positioning calculation method and positioning calculation apparatus | |
JP3206765B2 (en) | Control method of master-slave manipulator | |
JP4647919B2 (en) | Control method and control apparatus | |
JP2676721B2 (en) | Control device for articulated robot | |
JPH11239988A (en) | A singular point avoiding method in direct teaching of articulated robot | |
JPS63289606A (en) | Redundancy freedom type robot | |
JPH0468646B2 (en) | ||
JP2019093486A (en) | Robot controller and robot inverse transformation processing method | |
JP2019093488A (en) | Robot control device and robot reverse conversion processing method | |
KR102624551B1 (en) | A method for task-space compliance control of robot manipulator without rotational displacement constraints | |
JPH10244481A (en) | Method and device for preparing robot operating program | |
JPS6024603A (en) | Controlling method of robot | |
JPH06226667A (en) | Seven-shaft mainpulator control method |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
A300 | Withdrawal of application because of no request for examination |
Free format text: JAPANESE INTERMEDIATE CODE: A300 Effective date: 20010130 |