JPH04195403A - Locus control method for articulated arm robot - Google Patents
Locus control method for articulated arm robotInfo
- Publication number
- JPH04195403A JPH04195403A JP32578190A JP32578190A JPH04195403A JP H04195403 A JPH04195403 A JP H04195403A JP 32578190 A JP32578190 A JP 32578190A JP 32578190 A JP32578190 A JP 32578190A JP H04195403 A JPH04195403 A JP H04195403A
- Authority
- JP
- Japan
- Prior art keywords
- matrix
- angular velocity
- redundancy
- regular
- determinant
- 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.)
- Granted
Links
- 238000000034 method Methods 0.000 title claims abstract description 15
- 239000011159 matrix material Substances 0.000 claims abstract description 58
- 230000007246 mechanism Effects 0.000 abstract description 6
- 238000004364 calculation method Methods 0.000 description 25
- 230000008901 benefit Effects 0.000 description 7
- 238000010586 diagram Methods 0.000 description 3
- 230000008569 process Effects 0.000 description 3
- 230000008859 change Effects 0.000 description 2
- 238000011156 evaluation Methods 0.000 description 2
- 230000002349 favourable effect Effects 0.000 description 2
- 230000001788 irregular Effects 0.000 description 2
- 238000005070 sampling Methods 0.000 description 2
- 101000932768 Conus catus Alpha-conotoxin CIC Proteins 0.000 description 1
- 238000009795 derivation Methods 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 238000007689 inspection Methods 0.000 description 1
- 239000012528 membrane Substances 0.000 description 1
- 230000008439 repair process Effects 0.000 description 1
Abstract
Description
【発明の詳細な説明】
〈産業上の利用分野〉
本発明は、原子カプラント内の点検補修ロボット等とし
て使用される多関節形アームロボットの軌跡制御方法に
関する。DETAILED DESCRIPTION OF THE INVENTION <Industrial Application Field> The present invention relates to a method for controlling the trajectory of an articulated arm robot used as an inspection and repair robot in an atomic couplant.
〈従来の技術〉
従来、冗長度を有する多関節形アームロボットの軌跡制
御を行うには、非正則なりコピ行列を数値解法で解かな
１ブればならず、これをリアルタイムで実行するには高
価な計算機が必要であった。<Prior art> Conventionally, in order to control the trajectory of an articulated arm robot with redundancy, it was necessary to solve a non-regular copy matrix using a numerical method. An expensive calculator was required.
そこで、煩雑な数値解法を省略する方法としてヤコビ行
列の冗長度を仮想的に殺して正則行列化して求める方法
が知られている。Therefore, as a method of omitting the complicated numerical solution method, there is a known method of virtually eliminating the redundancy of the Jacobian matrix and converting it into a regular matrix.
その−例について、第２図、第３図、第５図、第６図を
参照して説明する。Examples thereof will be explained with reference to FIGS. 2, 3, 5, and 6.
第２図、第３図は先端にハントを有する３自由度の平面
アームロボットの一例を示すものである。両回に示すよ
うに、三つの関節角θ１゜θ１．θ２を制御することに
より、ｘ−ｙ平面内におけるアーム先端位置、つまりハ
ンドの位置を任意に移動させることができる。しかし、
平面上で、ハンドの位置のみを制御する場合には、２自
由度あれば充分であり、ｌ軸冗長である。FIGS. 2 and 3 show an example of a three-degree-of-freedom planar arm robot having a handle at the tip. As shown in both times, the three joint angles θ1°θ1. By controlling θ2, the arm tip position in the xy plane, that is, the position of the hand, can be arbitrarily moved. but,
When controlling only the position of the hand on a plane, two degrees of freedom are sufficient and the l-axis is redundant.
ここで、ハンドの位置、各関節角は次の（１） （２）
式のように表すものとする。Here, the position of the hand and each joint angle are as follows (1) (2)
It shall be expressed as in the formula.
ハンドの位置ｐ−（夛）・・・（１）
この様に表すと、ｌ！！Ｓ角θがら、ハンドの位置ｐは
解析的に次の（３）式のように求められる。Hand position p-(夛)...(1) Expressed like this, l! ! From the S angle θ, the position p of the hand is analytically determined as shown in the following equation (3).
（３）式の両辺を時間で微分して速度の関係式として示
すと、次の（４）式の様に表される。When both sides of equation (3) are differentiated with respect to time and expressed as a velocity relational expression, it is expressed as the following equation (4).
＝ Ｊ（０）・め ・・・（４）
ここで、Ｊ（θ）はヤコビ行列であり、３軸の場合、次
の（５）式のように表示される。= J(0)・me (4) Here, J(θ) is a Jacobian matrix, and in the case of three axes, it is displayed as shown in the following equation (5).
一方、制御装置としては、第５図に示すように、操縦桿
２０、各関節角速度演算部３０及びサーボ機構４０とか
ら構成されている。この各関節角速度演算部３０はヤコ
ビ行列発生器３１、小行列式演算機３２及び逆ヤコビ演
算器３３とから構成されている。On the other hand, as shown in FIG. 5, the control device is composed of a control stick 20, each joint angular velocity calculating section 30, and a servo mechanism 40. Each joint angular velocity calculation unit 30 is composed of a Jacobian matrix generator 31, a small determinant calculation unit 32, and an inverse Jacobian calculation unit 33.
ところで、上記制御装置で実際に制御できるのは、多関
節形アー１２０ボットの関節角０或いは関節角速度めで
あるから、上記（５）式を関節角速度のについて解く必
要がある。つまり、ハンド速度トから関節角速度のへの
変換をしなければならない。By the way, since what the above control device can actually control is the joint angle of 0 or the joint angular velocity of the multi-jointed robot 120, it is necessary to solve the above equation (5) for the joint angular velocity. In other words, it is necessary to convert the hand speed to the joint angular velocity.
もし、上記（５）式のヤコビ行列が正則行列、即ち冗長
度がない行列であれば、ｄｅｔＪ（０）ｙｏであるから
、Ｊ（０）の逆行列Ｊ−’（θ）が求められ、（６）式
のように関節角速度のが求まる。If the Jacobian matrix in equation (5) above is a regular matrix, that is, a matrix with no redundancy, then detJ(0)yo, so the inverse matrix J-'(θ) of J(0) can be found, The joint angular velocity can be found as shown in equation (6).
め−Ｊ−１（θ）・わ ・・・（６）しかし、冗長度
がある場合は、ヤコビ行列は２×３であり、正則ではな
い。Me-J-1(θ)・W (6) However, if there is redundancy, the Jacobian matrix is 2×3 and is not regular.
また、Ｊ（０）が正則でなくとも、擬似逆行列Ｊ”（θ
）を用いれば、上記（６）式の形に解けるが、擬似逆行
列を求めるのは計算量が膨大となり問題である。Moreover, even if J(0) is not regular, the pseudo-inverse matrix J”(θ
), it can be solved in the form of equation (6) above, but finding the pseudo-inverse matrix requires an enormous amount of calculation, which is problematic.
そこで、Ｊ（θ）から冗長列を一時的に取り除いて正則
化した行列をＪ、（θ）とする。Therefore, a matrix obtained by temporarily removing redundant columns from J(θ) and regularizing it is designated as J,(θ).
例えば、（５）式のＪ（θ）であれば、−列が冗長であ
るので、次の（７） （８） （９１式に示すの三つの
正則行列Ｊ、（θ）が定義できる。但し、ｓ＝１〜３と
する。For example, in the case of J(θ) in equation (5), the − column is redundant, so three regular matrices J and (θ) shown in the following equations (7), (8), and (91) can be defined. However, s=1 to 3.
このＪ、（θ）の逆行列Ｊ、−’（θ）を用いれば、ｄ
ｅｔＪｓ（ｅ）≠０の条件のもとで、次の（１０） ａ
υａカ式に示すように関節角速度のについて解くことが
出来る。If we use the inverse matrix J,−'(θ) of J,(θ), then d
Under the condition that etJs(e)≠0, the following (10) a
It is possible to solve for the joint angular velocity as shown in the equation υa.
但し、除かれた冗長項に対する関節角速度６、は０とす
る。However, the joint angular velocity 6 for the removed redundant term is set to 0.
この場合、どのＪ、（の）を用いるがは、その行列式の
絶対値１ｄｅｔＪｓ（θ）１の最も大きなものを使用す
るとすれば、ｄｅｔＪｓｃθ）−〇の特異点を回避でき
る。In this case, no matter which J (of) is used, if the one with the largest absolute value 1detJs(θ)1 of the determinant is used, the singularity of detJscθ)−0 can be avoided.
そこで、このような導出過程を基にして、従来の制御袋
ｒでは多関節形アームロボットを制御するようにしてい
る。Therefore, based on such a derivation process, the conventional control bag r controls an articulated arm robot.
即ち、オペレータＩＯが操縦桿２０を操作してハンド速
度わが指令値として制御装置に与えられると、各関節角
速度演算部３０では、第６図に示すフローチャートに従
って、関節角速度めを演算する。That is, when the operator IO operates the control stick 20 and provides the hand speed command value to the control device, each joint angular velocity calculation unit 30 calculates the joint angular velocity according to the flowchart shown in FIG.
先ず、ヤコビ行列演算器３１は各サンプリング時点のハ
ンド速度かと関節角θとから、ヤコビ行列式Ｊ（θ）を
演算する。First, the Jacobian matrix calculator 31 calculates the Jacobian determinant J(θ) from the hand speed and joint angle θ at each sampling time.
次いで、小行列式演算器３２はヤコビ行列式Ｊｌ）から
冗長列を取り除いた正則行列Ｊｓ（θ）と、その行列式
ｄｅｔＪ、（θ）〜ｄｅｔＪ ２（０）を計算する。Next, the small determinant calculator 32 calculates a regular matrix Js(θ) obtained by removing redundant columns from the Jacobian determinant Jl) and its determinant detJ, (θ) to detJ2(0).
引き続き、逆ヤコビ演算器３３は正則行列Ｊ、（θ）の
うち、その行列式の絶対値１ｄｅｔＪ＋（θ）〜ｄｅｔ
Ｊｓ（θ）ｌの最も大きなものを最適正則行列Ｊ、１．
とじ、その逆行列Ｊ ｍａｘ−１を計算する。Subsequently, the inverse Jacobian calculator 33 calculates the absolute value 1detJ+(θ)~det of the determinant of the regular matrix J, (θ).
The largest one of Js(θ)l is the optimal regular matrix J, 1.
The inverse matrix J max-1 is calculated.
そして、め”’ Ｊ ｍａｒ”” Ｉ’より、各関節角
速度Ｊ、−Ｊ、を求める。Then, the angular velocities J, -J of each joint are determined from the equation I'.
尚、除かれた関節角速度θ８は０とする。Note that the removed joint angular velocity θ8 is set to 0.
このように演算された各関節角速度ｂ１〜ｂ２が目標値
としてサーボ機構４０に出力され、多関節形アームロボ
ットが制御される。The thus calculated joint angular velocities b1 to b2 are output to the servo mechanism 40 as target values, and the multi-joint arm robot is controlled.
〈発明が解決しようとする課題〉
上述した従来の制御方法では、多関節形アームロボット
の冗長度を仮想的に殺して正則化しているので、非正則
なりコピ行列を数値解決する必要がなく、高価な計算機
が不要であるが、しかし、冗長度を殺しているので多関
節形アームロボットの姿勢を好ましい方向に変えていく
ことは出来なかった。<Problems to be Solved by the Invention> In the conventional control method described above, the redundancy of the articulated arm robot is virtually eliminated and regularized, so there is no need to numerically solve the irregular copy matrix. This method does not require an expensive computer, but because it eliminates redundancy, it is not possible to change the posture of the articulated arm robot in a favorable direction.
そこで、冗長度を生かした制御を行うには、擬似逆行列
Ｊ”（θ）を用いた場合、次の０３式に基礎を置くこと
ができる。Therefore, in order to perform control that takes advantage of redundancy, the following equation 03 can be used as the basis when a pseudo-inverse matrix J'' (θ) is used.
め−Ｊ”（θ）・わ＋（１−Ｊ”（θ）・ＪＣｅ））Ｃ
・・・０３
ここで、ベクトルＣは任意に取れるので、例えば、多関
節形アームロボットの姿勢が好ましい方向に動くように
ベクトルＣをコントロールすれば、目的とする軌跡を保
ちつつ、冗長度を利用して多関節形アームロボットの姿
勢を変えることが出来る。Me-J"(θ)・W+(1-J"(θ)・JCe))C
...03 Here, since the vector C can be taken arbitrarily, for example, if the vector C is controlled so that the posture of the multi-jointed arm robot moves in a preferable direction, it is possible to maintain the desired trajectory and utilize the redundancy. The posture of the articulated arm robot can be changed by
ところが、この場合も、擬似逆行列を求める演算が必要
となり、その演算量が膨大となる欠点がある。However, in this case as well, a calculation is required to obtain a pseudo-inverse matrix, which has the disadvantage that the amount of calculation is enormous.
このように従来では、非正則なりコピ行列から冗長度を
仮想的に除いて正則化した行列を利用して制御すると、
冗長度を生かした制御が出来ず、また、冗長度を生かし
た制御をするために擬似逆行列を用いた演算を採用する
と、演算量が膨大となり、高価な計算機が必要になって
いた。In this way, conventionally, when controlling using a regularized matrix by virtually removing redundancy from an irregular copy matrix,
Control that takes advantage of redundancy is not possible, and if an operation using a pseudo-inverse matrix is used to perform control that takes advantage of redundancy, the amount of calculation becomes enormous and an expensive computer is required.
本発明は、上記従来技術に鑑みてなされたものであり、
高価な計算機を必要とせず、しかも、冗長度を生かして
多関節形アームロボットの軌跡を制御できる方法を提供
することを目的とするものである。The present invention has been made in view of the above-mentioned prior art,
The purpose of this invention is to provide a method that does not require an expensive computer and can control the trajectory of an articulated arm robot by taking advantage of redundancy.
〈課題を解決するための手段〉
斯かる目的を達成する本発明の構成はｎ軸の冗長度を有
するｎ軸の多関節形アームロボットの軌跡制御において
、冗長度を含めたヤコビ行列Ｊ（θ）を求めると共に該
ヤコビ行列から第Ｓ軸（Ｓ＝１〜ｎ）を冗長として除い
たｎ個の正則行列Ｊ、（θ）を求めて、これらの正則行
列Ｊ。<Means for Solving the Problems> The configuration of the present invention to achieve the above object is to control the trajectory of an n-axis articulated arm robot having redundancy of n-axes, by calculating the Jacobian matrix J(θ ) and remove the S-th axis (S=1 to n) from the Jacobian matrix as redundant to obtain n regular matrices J and (θ).
（０）のうちから、行列式の絶対値ｔｄｅｔＪｓ（θ）
１の最も大きなものを最適正則行列Ｊ、４．とじて選び
、その逆行列Ｊ ｔｍａｘ−１をハンド速度すに掛けて
主関節角速度のを求め、更に正則行列の行列式ｄｅｔＪ
ｓ（θ）から定義される冗長速度ｄに任意の定数ｋを掛
けて前記主関節角速度のに加えて関節角目標速度の。と
じて出力することを特徴とする。(0), the absolute value of the determinant tdetJs(θ)
1 is the optimal regular matrix J,4. The main joint angular velocity is obtained by multiplying the hand speed by its inverse matrix J tmax-1, and then the determinant of the regular matrix detJ
Multiply the redundant velocity d defined from s(θ) by an arbitrary constant k to obtain the target joint angular velocity in addition to the main joint angular velocity. It is characterized in that it is output in a closed format.
〈作用〉
本発明では、冗長度を生かした制御を行うために前述し
た０３式を利用するのであるが、擬似逆行列Ｊ’（θ）
に代えて、Ｊ、（０）の逆行列Ｊ、−１（θ）に冗長自
由変分のθベクトルを加えたＪ５′−１（θ）を代入す
る。<Operation> In the present invention, the above-mentioned formula 03 is used to perform control that takes advantage of redundancy, and the pseudo-inverse matrix J'(θ)
Instead, J5'-1(θ), which is the inverse matrix J,−1(θ) of J,(0) plus the θ vector of the redundant free variation, is substituted.
例えば、前述した（７）〜（９）式のＪ、（θ）に対し
てＪｓ−’（θ）は次のように求められる。For example, with respect to J and (θ) in equations (7) to (9) described above, Js-'(θ) is obtained as follows.
・・・θ４ ・・・α９ ・・・ｑｅ 従って、Ｊ、ｌ−ｌは次式のように定義される。...θ4 ...α9 ...qe Therefore, J and l-l are defined as in the following equation.
・・・（１７１
・・・０日
・・・α９
但し、上記αでａｎ ｔｓ式は、第１行、第２行、第３
行に０ベクトルをそれぞれ付加したものである。...(171 ...0 days...α9 However, in the above α, the an ts expression is
A 0 vector is added to each row.
このＪ、’−’（ａ）をＪ”（の）に代えて、前述した
０３式に代入すると、次のようになる。If this J, '-' (a) is replaced with J'' (of) and substituted into the above-mentioned formula 03, the following is obtained.
め＝Ｊ、’ ＝（ｅ）・わ＋（ｌ−Ｊ、’−’（の）・
Ｊ（０））Ｃ・・（イ）
さて、Ｊ、’ −’（の）は第Ｓ軸変数ｔ、に対する行
ベクトルを０ベクトルとしたものであるが、この乙、を
（ｎ＋］）行に、Ｊ（０）についてもｂ５に関する第８
行の列ベクトル（、ｎ＋１）に移動させても、−膜性は
失わない。Me=J,'=(e)・wa+(l-J,'-'(of)・
J(0))C...(a) Now, J, '-'() is the row vector for the S-axis variable t, which is the 0 vector, but this B is the (n+]) row Also, for J(0), the eighth
Even if it is moved to the row/column vector (, n+1), the -membrane property is not lost.
例えば、
Ｊ（の）” ［（１＋、ｆｉ、、−ａ 、、 （１
、＋＋］−（２１）但し、８１はｎ次元の列ベクトル、
ｂ、はｎ次元の行ベクトル、ｄａｔ（ｎ＋１）はＪ ａ
ｌ＋（θ）＝［ａ ｌ・・・ａ、ｌの行列式の値である
。For example, J(of)'' [(1+, fi,, -a,, (1
, ++] - (21) However, 81 is an n-dimensional column vector,
b is an n-dimensional row vector, dat(n+1) is J a
l+(θ)=[a l... is the value of the determinant of a, l.
■
Ｊ、。／−１（の）・Ｊ（の）＝□×
ｄｅｊ（ｒ＋＋２）
・・・（２３）
もともと、Ｊ、’ −’（θ）はＪ７゜、（θ） ＝＝
（（１、・・・Ｇ、］の逆行列でり、次のように変形さ
れる。■ J. /-1(of)・J(of)=□× dej(r++2) ...(23) Originally, J, '-'(θ) is J7°, (θ) ==
(It is the inverse matrix of (1,...G,] and is transformed as follows.
Ｊ＠＋＋’−’（、の）・Ｊ（の） ＝ −−ｘｄｅ
ｃ（ロト１）
ここで、［’ａ＋＋Ｔ）＋（１＝ ｌ〜ｎ）は、行列Ｊ
（θ）から第１列を除いたＪｌ（の）に対し、第１列に
ａ。、を入れた行列Ｊ、′（の）の行列式の値である。J@++'-'(,'s)・J('s) = −-xde
c (Lotto 1) Here, ['a++T)+(1=l~n) is the matrix J
For Jl (of), which is obtained by removing the first column from (θ), a is in the first column. , is the value of the determinant of the matrix J,′ (of).
即ち、
Ｊ、（の）＝！ａｚｃｚ、・・０゜１．Ｑ、。５．・・
・Ｃい１．１・・・（２５）
Ｊ ＋ ’ ｌ） ：ｆ （１１＋ （１□ ＋
゛＋ ｃ Ｉ−ｌ＋ ａ 角＋ｌ＋ ｃ ＋＋
＋＋ ＋＋＋ｃｉ、］ −・・（２６）
ａ−−＋Ｔｏ＋＝ｄｅｔＪ１′（のＬ−（−１）’−’
ｄｅｔＪ ｌ（’４９）＝（−１）”−’ｄｅｔ ｉ
−（２７）従って、
」
Ｊ、。１′ 柑（の）・Ｊ（の）＝□×ｄａｔ（ｎ＋１
）
１−Ｊ 、、、’−’（（１：）＞・Ｊ（の）−−−ｘ
ｄａｔ（ｎ＋１）
（１−Ｊ、や＃ −ｌ（の）・、３（ｏ））・Ｃよって
、めは次のようになる。That is, J, (of)=! azcz,...0゜1. Q. 5.・・・
・C1.1...(25) J + 'l) :f (11+ (1□ +
゛+ c I-l+ a Angle+l+ c ++
++ +++ci,] -...(26) a--+To+=detJ1'(L-(-1)'-'
detJ l('49)=(-1)"-'det i
-(27) Therefore, ``J,. 1' Kan(no)・J(no)=□×dat(n+1
) 1-J ,,,'-'((1:)>・J(of)---x
dat(n+1) (1-J, #-l(of)・,3(o))・C Therefore, the number becomes as follows.
このように、ｓ＝ｎ＋ｌとして上記式を導Ｌｚたが、Ｓ
を１〜ｎとしても、はぼ同様な結果となる。平面３軸の
場合は、次のようになる。In this way, the above equation was derived by setting s=n+l, but S
Even if the value is set to 1 to n, almost the same result will be obtained. In the case of a plane with three axes, it is as follows.
め−Ｊ 、’ −’（ａ）・わ十−動一・ｄ ・・（
３３）ｅｔｌ
＝Ｊ、’−’ｌ）・ｐ−コ・ｄ ・・・（３４）：Ｊ、
″（０）・ｐ十回・ｄ ・・・（３５）以上から、Ｊ
、’−’（θ）を用いた場合、ｄの定数倍の速度ベクＩ
・ルをのに加えても、出力であるかは同一である。Me-J,'-'(a)・Wa-j-d-1・d...(
33) etl = J,'-'l)・p-co・d...(34):J,
″(0)・p ten times・d ... (35) From the above, J
, '-'(θ), the velocity vector I is a constant times d.
・Even if you add ru to , the output is the same.
即ち、ｄの定数倍と［７て、ロボ・ソト姿勢が好ましい
姿勢をとるように、めに加算することが出来る。That is, d can be multiplied by a constant [7] and can be added so that the robot/soto posture takes a preferable posture.
この定数の決め方は、何らかの評価関数■（θ）を定義
して決めれば良い。例えば、■（０）として、ＪＳ（θ
）の可操作度Ｗ（θ）が大きい方向となるようにすれば
、特異点を回避し、且つ、ロボットの姿勢が好ましい方
向に動くことになる。This constant can be determined by defining some kind of evaluation function (θ). For example, as ■(0), JS(θ
), the singularity can be avoided and the robot's posture can move in a preferable direction.
例えば、３軸の場合、（３３）〜（３５）式のうち、ｄ
ｅｔＪ＋〜ｄｅｔＪｓの絶対値が最も大きいものの式を
用いてめを求めるようにすると良い。For example, in the case of three axes, in equations (33) to (35), d
It is preferable to find the value using the formula with the largest absolute value of etJ+ to detJs.
また、定数Ｃ１は関節角速度のリミ・ソト値を越えず、
且つＷ（θ）が増加する方向に符号をとると良い。In addition, the constant C1 does not exceed the Limi-Soto value of the joint angular velocity,
In addition, it is preferable to take the sign in the direction in which W(θ) increases.
〈実施例〉
以下、本発明について、図面に示す実施例を参照して詳
細に説明する。<Examples> The present invention will be described in detail below with reference to examples shown in the drawings.
第１図に本発明の一実施例に使用する制御装置を示す。FIG. 1 shows a control device used in an embodiment of the present invention.
同図に示す実施例は、第２図に示す３自由度の平面アー
ムロボットの制御に示すものである。同図に示すように
、平面上でノ＼ンドの位置を制御する場合、２自由度あ
れば充分であり、Ｉ軸冗長である。The embodiment shown in the figure is for controlling a planar arm robot having three degrees of freedom shown in FIG. As shown in the figure, when controlling the position of a node on a plane, two degrees of freedom are sufficient, and the I-axis is redundant.
ここで、制御装置は、ハンド指令速度指令操縦桿２０と
、各関節角速度演算部３０と、サーボ機構４０とから構
成されている。各関１節角速度演算部３０は、ヤコビ行
列演算部３１、小行列式演算部３２、逆ヤコビ演算部３
３、冗長速度演算部３４及び関節目標速度演算部３５と
から構成されている。Here, the control device includes a hand command speed command control stick 20, each joint angular velocity calculating section 30, and a servo mechanism 40. Each joint 1-node angular velocity calculation section 30 includes a Jacobian matrix calculation section 31, a minor determinant calculation section 32, and an inverse Jacobian calculation section 3.
3. It is composed of a redundant speed calculating section 34 and a joint target speed calculating section 35.
従って、オペレータ１０が操縦桿２０を操作してハンド
速度わを指令値として制御装置に与えると、各関節角速
度演算部３０では、第６図に示すフローチャートに従−
て関節角目標速度の。を演算する。Therefore, when the operator 10 operates the control stick 20 and gives the hand speed W as a command value to the control device, each joint angular velocity calculation unit 30 operates the control stick 20 to provide the hand speed W as a command value.
joint angle target velocity. Calculate.
先ず、ヤコビ行列演算器３１は各サンプリング時点のハ
ンド速度わと関節角Ｏとから、ヤコビ行列式Ｊ（θ）を
演算する。First, the Jacobian matrix calculator 31 calculates the Jacobian determinant J(θ) from the hand speed and armpit joint angle O at each sampling time.
次いで、小行列式演算器３２はヤコビ行列式Ｊ（θ）か
ら冗長列を取り除いた正則行列Ｊ、（θ）〜Ｊ、（６）
の行列式を計算する。Next, the small determinant calculator 32 generates a regular matrix J, (θ) to J, (6) by removing redundant columns from the Jacobian determinant J(θ).
Compute the determinant of.
引き続き、逆ヤコビ演算器３３はｄｅｔＪ＋（ｅ？）〜
ｄｅｔＪｚ（θ）の絶対値のうち最も大きなものを最適
正則行列Ｊ、１．とじ、その逆行列Ｊ ｍａｘ−１を計
算する。Subsequently, the inverse Jacobian calculator 33 detJ+(e?)~
The largest absolute value of detJz(θ) is determined as the optimal regular matrix J, 1. The inverse matrix J max-1 is calculated.
そして、め＝Ｊｓａｚ−１わより、主関節角速度のを求
める。Then, from Me=Jsaz-1, the main joint angular velocity is determined.
尚、除かれた関節角速度すは０とする。Note that the removed joint angular velocity is assumed to be 0.
更に、本発明では、冗長速度演算器３４が、前述した（
３６）式に示す冗長速度ｄ ＝［ｄｅｔｌ −ｄｅｔ２
ｄｅｔ３］を求め、この冗長速度ｄの任意の定数ｋを
掛けて速度ベクトル６、（＝に−ｄ）を演算する。Furthermore, in the present invention, the redundant speed calculator 34 has the above-mentioned (
36) Redundant speed d shown in formula = [detl −det2
det3] and multiplying this redundant speed d by an arbitrary constant k to calculate a speed vector 6, (= to -d).
定数にの正負と大きさは、評価関数■（θ）により求め
る。The sign and magnitude of the constant are determined by the evaluation function ■(θ).
引き続き、関節目標速度演算器３５は、この速度ベクト
ルめ、に主関節角速度のを加えて、関節角目標速度の。Subsequently, the joint target velocity calculator 35 adds the main joint angular velocity to this velocity vector to obtain the joint angular target velocity.
（−の＋の、）を演算する。Calculate (-, +,).
但し、関節角目標速度の。が速度リミット値を越えない
ように調整する。However, the joint angle target velocity. Adjust so that the speed does not exceed the speed limit value.
このように演算された関節角目標速度の。が目標値とし
てサーボ機構４０に出力され、多関節形アームロボット
が制御される。of the joint angle target velocity calculated in this way. is output as a target value to the servo mechanism 40, and the articulated arm robot is controlled.
このように上記実施例では、冗長速度ｄに任意の定数ｋ
を掛けて、ｌ！Ｉ節角速度のに加えているので、この任
意の定数ｋを調整することにより、目的とする軌跡を得
ながら冗長度を生かして、多関節形アームロボットの姿
勢を好ましい方向に変えることが可能である。In this way, in the above embodiment, an arbitrary constant k is set for the redundant speed d.
Multiply by l! By adjusting this arbitrary constant k, it is possible to take advantage of the redundancy and change the posture of the articulated arm robot in a favorable direction while obtaining the desired trajectory. be.
また、擬似逆行列を使用しないので、演算量が膨大とな
ることはない。Furthermore, since a pseudo-inverse matrix is not used, the amount of calculation does not become enormous.
尚、本発明では、冗長度が１の場合に限っているが、こ
れは、冗長度としては、実用上Ｉで充分であり、２以旧
の冗長度を持つロボットは、その構造、強度上から作ら
れることは殆ど無いためである。In addition, in the present invention, the redundancy level is limited to 1, but this is because a redundancy level of I is sufficient for practical purposes, and a robot with a redundancy level of 2 or older may have a redundancy level of 1 or higher due to its structure and strength. This is because it is rarely made from.
従って、ヤコビ行列Ｊ（θ）は、制御入力をｎ次元とす
ると、（ｎ＋］）軸のロボットに対するものとなり、そ
の行列のサイズはｎｘ（１＋１）となる。即ち、平面３
軸の例で言えば、２Ｘ３のサイズの行列となる。Therefore, when the control input is n-dimensional, the Jacobian matrix J(θ) is for the (n+])-axis robot, and the size of the matrix is nx(1+1). That is, plane 3
In the case of the axis, it would be a matrix of size 2×3.
〈発明の効果〉
以上、実施例を参照して具１体的に説明したように、本
発明は冗長度を有する多関節形アームロボットの制卸に
おいて、全て予め数式解が求められたＪ（ｅ）、Ｊ、（
６９）、ｄｅＬＪ、（［）等を制御に用いるので、擬似
逆行列を使用する数値解法に比較し、演算量が少なく、
高価な計算機が不要となる。また、冗長度を生かした制
御ができるので、特異点回避がリアルタイムで可能とな
り、操作性、制訳性が格段に向上する。<Effects of the Invention> As specifically explained above with reference to the embodiments, the present invention is applicable to controlling an articulated arm robot with redundancy by using J( e), J, (
69), deLJ, ([), etc. are used for control, so the amount of calculation is small compared to numerical solution methods that use pseudo-inverse matrices,
No need for expensive calculators. Furthermore, since control can be performed that takes advantage of redundancy, singularities can be avoided in real time, and operability and translation efficiency are greatly improved.
第１図は本発明の一実施例に使用する制御装置の構成図
、第２図は本発明が適用される多関節形アームロボット
の一例を示す外観図、第３図は第２図に示す多関節形ア
ームロボットをモデル化して示す説明図、第４図は第１
図に示す制御装置の各軸速度指令演算部の演算過程を示
すフローチャート、第５図は従来の制御装置の構成図、
第６図は従来の制御装置の各軸速度指令演算部の演算過
程を示すフローチャートである。
図面中、
１０はオペレータ、
２０は操縦枠、
３０は各関節角速度演算部、
３】はヤコビ行列演算部、
３２は小行列式演算部、
３３は逆ヤコビ演算部、
３４は冗長速度演算部、
３５は！！１節目標速度演算部、
４０はサーボ機構である。
特 許 出 願 人
工業技術院長 杉油 賢
第１図
第２図
第３図
第４図
第５図Fig. 1 is a configuration diagram of a control device used in an embodiment of the present invention, Fig. 2 is an external view showing an example of an articulated arm robot to which the present invention is applied, and Fig. 3 is shown in Fig. 2. An explanatory diagram showing a model of an articulated arm robot, Figure 4 is the first
A flowchart showing the calculation process of each axis speed command calculation unit of the control device shown in the figure, FIG. 5 is a configuration diagram of a conventional control device,
FIG. 6 is a flowchart showing the calculation process of each axis speed command calculation section of the conventional control device. In the drawing, 10 is an operator, 20 is a control frame, 30 is each joint angular velocity calculation unit, 3 is a Jacobian matrix calculation unit, 32 is a small determinant calculation unit, 33 is an inverse Jacobian calculation unit, 34 is a redundant velocity calculation unit, 35 is! ! 1-section target speed calculation section; 40 is a servo mechanism; Patent application Director of the Agency for Industrial Science and Technology Ken Sugiyu Figure 1 Figure 2 Figure 3 Figure 4 Figure 5
Claims (1)
御において、冗長度を含めたヤコビ行列を求めると共に
該ヤコビ行列から冗長度を除去した正則行列を全て求め
、これらの正則行列のうちから、行列式の絶対値の最も
大きなものを最適正則行列として選び、該最適正則行列
の逆行列をハンド速度に掛けて主関節角速度を演算し、
更に正則行列の行列式から定義される冗長速度に任意の
定数を掛けて前記主関節角速度に加えて関節角目標速度
として出力することを特徴とする多関節形アームロボッ
トの軌跡制御方法。In trajectory control of an articulated arm robot with redundancy in one axis, a Jacobian matrix including redundancy is obtained, and all regular matrices are obtained by removing redundancy from the Jacobian matrix, and from among these regular matrices, The determinant with the largest absolute value is selected as the optimal regular matrix, and the hand speed is multiplied by the inverse matrix of the optimal regular matrix to calculate the main joint angular velocity,
A trajectory control method for an articulated arm robot, further comprising: multiplying a redundant velocity defined from a determinant of a regular matrix by an arbitrary constant and outputting the result as a joint angle target velocity in addition to the main joint angular velocity.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP2325781A JPH0816845B2 (en) | 1990-11-29 | 1990-11-29 | Trajectory control method of articulated arm robot |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP2325781A JPH0816845B2 (en) | 1990-11-29 | 1990-11-29 | Trajectory control method of articulated arm robot |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| JPH04195403A true JPH04195403A (en) | 1992-07-15 |
| JPH0816845B2 JPH0816845B2 (en) | 1996-02-21 |
Family
ID=18180541
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| JP2325781A Expired - Lifetime JPH0816845B2 (en) | 1990-11-29 | 1990-11-29 | Trajectory control method of articulated arm robot |
Country Status (1)
| Country | Link |
|---|---|
| JP (1) | JPH0816845B2 (en) |
Cited By (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| EP0771621A2 (en) * | 1995-10-11 | 1997-05-07 | Mitsubishi Jukogyo Kabushiki Kaisha | Elbow-position control way of redundant manipulator |
| US6540473B2 (en) * | 1996-10-18 | 2003-04-01 | Kabushiki Kaisha Yaskawa Denki | Robot vehicle for hot-line job |
| JP2009168626A (en) * | 2008-01-16 | 2009-07-30 | Canon Inc | Position attitude measurement device and method |
| CN108326844A (en) * | 2017-01-20 | 2018-07-27 | 香港理工大学深圳研究院 | The motion planning method and device of the operable degree optimization of redundancy mechanical arm |
-
1990
- 1990-11-29 JP JP2325781A patent/JPH0816845B2/en not_active Expired - Lifetime
Cited By (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| EP0771621A2 (en) * | 1995-10-11 | 1997-05-07 | Mitsubishi Jukogyo Kabushiki Kaisha | Elbow-position control way of redundant manipulator |
| EP0771621A3 (en) * | 1995-10-11 | 1999-03-24 | Mitsubishi Jukogyo Kabushiki Kaisha | Elbow-position control way of redundant manipulator |
| US6540473B2 (en) * | 1996-10-18 | 2003-04-01 | Kabushiki Kaisha Yaskawa Denki | Robot vehicle for hot-line job |
| JP2009168626A (en) * | 2008-01-16 | 2009-07-30 | Canon Inc | Position attitude measurement device and method |
| CN108326844A (en) * | 2017-01-20 | 2018-07-27 | 香港理工大学深圳研究院 | The motion planning method and device of the operable degree optimization of redundancy mechanical arm |
Also Published As
| Publication number | Publication date |
|---|---|
| JPH0816845B2 (en) | 1996-02-21 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| Handlykken et al. | Control system analysis and synthesis for a six degree-of-freedom universal force-reflecting hand controller | |
| Walker et al. | Efficient dynamic computer simulation of robotic mechanisms | |
| JP5349478B2 (en) | Inverse kinematics | |
| Zhang et al. | Stepsize range and optimal value for Taylor–Zhang discretization formula applied to zeroing neurodynamics illustrated via future equality-constrained quadratic programming | |
| Lee et al. | Resolved motion adaptive control for mechanical manipulators | |
| Mata et al. | Serial-robot dynamics algorithms for moderately large numbers of joints | |
| JP2874238B2 (en) | Control method of articulated robot | |
| Shin et al. | Compliant control of robotic manipulators with resolved acceleration | |
| Lenarčič | An efficient numerical approach for calculating the inverse kinematics for robot manipulators | |
| Kirćanski et al. | A new program package for the generation of efficient manipulator kinematic and dynamic equations in symbolic form | |
| Ghorbel et al. | A reduced model for constrained rigid bodies with application to parallel robots | |
| Li et al. | Robust task-space tracking for free-floating space manipulators by cerebellar model articulation controller | |
| Li et al. | Coordinated motion generation and real-time grasping force control for multifingered manipulation | |
| Ma et al. | Discrete-time practical robotic control for human–robot interaction with state constraint and sensorless force estimation | |
| JPH04195403A (en) | Locus control method for articulated arm robot | |
| Haviland et al. | Manipulator differential kinematics: Part i: Kinematics, velocity, and applications | |
| Namiki et al. | Optimal grasping using visual and tactile feedback | |
| Nguyen et al. | Performance of pinching motions of two multi-DOF robotic fingers with soft-tips | |
| Wu et al. | Discrete gradient-zeroing neural dynamics for future Moore–Penrose inverse with application to tracking control of manipulator | |
| Balestrino et al. | An algorithmic approach to coordinate transformation for robotic manipulators | |
| Hayati et al. | Dual arm coordination and control | |
| Mussa-Ivadi et al. | Solving kinematic redundancy with impedance control: a class of integrable pseudoinverses | |
| Orin et al. | A real-time computer architecture for inverse kinematics | |
| Zhou et al. | Joint configuration conservation and joint limit avoidance of redundant manipulators | |
| JPS59220806A (en) | Controlling method of industrial robot |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| EXPY | Cancellation because of completion of term |